...
 
Commits (9)
......@@ -34,6 +34,7 @@ MbsAux * initMbsAux(MbsData *s)
nqv = s->nqv;
nqc = s->nqc;
#ifdef INVDYNARED
nqa = s->nqa;
#endif
......@@ -46,6 +47,14 @@ MbsAux * initMbsAux(MbsData *s)
mbs_aux->NRerr = s->NRerr;
mbs_aux->MAX_NR_ITER = 100;
// Added for the online coordinate partitioning
mbs_aux->use_det = 0;
mbs_aux->permu = 0;
mbs_aux->iter_part = 5;
mbs_aux->Jv_verbose = 0;
mbs_aux->online_part = 0;
mbs_aux->min_det = 1e-3;
if (Ncons>0)
{
mbs_aux->h = get_dvec_1(Ncons);
......
......@@ -15,6 +15,7 @@
#include "nrfct.h"
#include "mbs_project_interface.h"
#include "mbs_message.h"
#include "mbs_part.h"
double norm_vector(double *v, int n);
double norminf_vector(double *v, int n);
......@@ -28,6 +29,12 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
int nL,nC;
int err = 0;
double d;
double** mJv_0;
int* ind_c = NULL;
int* ind_u_des = NULL;
int part_condition;
MbsPart *mbs_part;
iter = 0;
mbs_aux->norm_h=1.0;
......@@ -36,6 +43,7 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
// Calcul des contraintes et de la Jacobienne
mbs_calc_hJ(s,mbs_aux);
// Norme des contraintes (en supposant que toutes les contraintes independantes sont au debut ???)
mbs_aux->norm_h = norminf_vector(mbs_aux->h,s->nhu);
......@@ -49,10 +57,120 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
mbs_aux->mJv[i][j] = -mbs_aux->Jac[s->hu[i]][s->qv[j]];
}
}
ind_u_des = get_ivec_0(0);
ind_c = get_ivec_0(s->nqc);
sort_ivec_0(s->qc + 1, ind_c, s->nqc);
// switch to index strating at 0 (instead of 1)
for (i = 0; i < s->nqc; i++) {
ind_c[i] -= 1;
}
// -------------------- ALEX start ------------------
mJv_0 = get_dmat_0(nL, nC);
sub_matrix(mbs_aux->mJv, nL + 1, 0, 0, mJv_0);
//Depending on the user choice, we look if a partitioning is needed
if (mbs_aux->use_det) {
part_condition = fabs(det_dmatrix_0(mJv_0, nL)) < mbs_aux->min_det; // Need of a new partitioning if the determinant of Jv is too small
}
else {
part_condition = iter > 0 && iter % mbs_aux->iter_part == 0; // Need of a new partitioning if the number of iteration on Newton-Raphson is too large
}
if (part_condition && mbs_aux->online_part){
mbs_part = mbs_new_part(s);
mbs_part->options->rowperm = 1;
mbs_part->options->verbose = 0;
if (mbs_aux->Jv_verbose) {
printf("mJv matrix before partitioning : \n");
print_dmat_0(mbs_aux->mJv, nC + 1, nL + 1);
}
s->DonePart = 0;
s->nqu = 0; // In order to use all the variable which are not driven
err = PART_coord_part(s, mbs_part, ind_u_des, 0, ind_c, s->nqc, 0); //Re-partition
if (err < 0) // Error management
{
mbs_msg(">>PART>>\n");
mbs_msg(">>PART>> ***** mbs_run_part : Coordinate partitioning interrupted : *****\n");
mbs_msg(">>PART>>\n");
//myproject_part = MBS_part;
s->DonePart = 0;
mbs_error_msg(" [%d] in mbs_run_part !! \n", -100 + err);
}
s->nqu = mbs_part->n_qu;
for (i = 0; i < mbs_part->n_qu; i++)
{
s->qu[i + 1] = mbs_part->ind_qu[i] + 1;
}
s->nqv = mbs_part->n_qv;
for (i = 0; i < mbs_part->n_qv; i++)
{
s->qv[i + 1] = mbs_part->ind_qv[i] + 1;
}
s->nhu = mbs_part->n_hu;
for (i = 0; i < mbs_part->n_hu; i++)
{
s->hu[i + 1] = mbs_part->ind_hu[i] + 1;
}
if (s->nqu + s->nqc > 0)
{
mbs_aux->iquc = get_ivec_1(s->nqu + s->nqc);
for (i = 1; i <= s->nqu; i++)
mbs_aux->iquc[i] = s->qu[i];
for (i = 1; i <= s->nqc; i++)
mbs_aux->iquc[i + s->nqu] = s->qc[i];
}
if (mbs_part->options->verbose)
{
mbs_msg(">>PART>> Partitionning result :\n");
mbs_msg("nqu : %d\n", mbs_part->n_qu);
mbs_msg("qu : "); print_ivec_0(s->qu + 1, mbs_part->n_qu);
mbs_msg("\n");
mbs_msg("nqv : %d\n", mbs_part->n_qv);
mbs_msg("qv : "); print_ivec_0(s->qv + 1, mbs_part->n_qv);
mbs_msg("\n");
mbs_msg("nhu : %d\n", mbs_part->n_hu);
mbs_msg("hu : "); print_ivec_0(s->hu + 1, mbs_part->n_hu);
mbs_msg("\n");
mbs_msg(">>PART>> ***** mbs_run_part end *****\n");
}
mbs_delete_part(mbs_part);
for (i = 1; i <= nL; i++)
{
for (j = 1; j <= nC; j++)
{
mbs_aux->mJv[i][j] = -mbs_aux->Jac[s->hu[i]][s->qv[j]];
}
}
if (mbs_aux->Jv_verbose) {
printf("mJv matrix after partitioning : \n");
print_dmat_0(mbs_aux->mJv, nC + 1, nL + 1);
}
mbs_aux->permu = 1;
return 12;
// -------------------- ALEX stop ------------------
}
// Decomposition LU de la matrice -Jv
err = ludcmp(mbs_aux->mJv,s->nqv,mbs_aux->ind_mJv,&d);
if (err < 0) // Error management
{
mbs_msg("\t \t >CLOSE GEO> Error in mbs_close_geo during LU decomposition of Jv matrix \n");
......@@ -67,6 +185,7 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
mbs_aux->mJv_h[i] = mbs_aux->h[s->hu[i]];
// mbs_aux->mJv_h[i][1] = mbs_aux->h[s->hu[i]];
}
lubksb(mbs_aux->mJv,s->nqv,mbs_aux->ind_mJv,mbs_aux->mJv_h);
// gaussj(mbs_aux->mJv, s->nqv, mbs_aux->mJv_h, 1);
......@@ -76,6 +195,8 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
s->q[s->qv[i]] += mbs_aux->mJv_h[i];
// s->q[s->qv[i]] += mbs_aux->mJv_h[i][1];
}
}
}
......
......@@ -77,6 +77,16 @@ MbsDirdyn* mbs_new_dirdyn_aux(MbsData* mbs_data, MbsAux* mbs_aux)
opts->compute_Qc[i] = 0;
}
/*---------------------------------------------------*/
/* Options related to online coordinate partitioning */
/*---------------------------------------------------*/
opts->use_det = 0;
opts->saveTimePart = 1;
opts->Jv_verbose = 1;
opts->iter_part = 5;
opts->det_precision = 1e-3;
/*------------------------------------------*/
/* Options related to numerical integration */
/*------------------------------------------*/
......@@ -233,6 +243,13 @@ void mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data)
// check (and print) warnings for users options of integrator
print_warnings_integrator(dd, dd->options->integrator);
//initialyze oline coordinate partitioning
dd->mbs_aux->online_part = 1;
dd->mbs_aux->use_det = dd->options->use_det;
dd->mbs_aux->min_det = dd->options->det_precision;
dd->mbs_aux->Jv_verbose = dd->options->Jv_verbose;
dd->mbs_aux->iter_part = dd->options->iter_part;
// real-time modules activated
#ifdef REAL_TIME
if (dd->options->realtime)
......@@ -526,8 +543,9 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data)
{
dd->loop_integrator(cur_t0, cur_tf, mbs_data, dd);
// Update times
cur_t0 = cur_tf;
cur_tf += opts->delta_t_wp;
cur_t0 = cur_tf;
cur_tf += opts->delta_t_wp;
// stop the simulation if 'flag_stop' set to 1
if (mbs_data->flag_stop)
......@@ -641,7 +659,17 @@ void mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsDirdy
{
int err = 0;
int i;
//----------------- Modif Alex Start -----------------
double *q_save, *qd_save;
q_save = get_dvec_1(s->njoint);
qd_save = get_dvec_1(s->njoint);
copy_dvec_1(s->q, q_save);
copy_dvec_1(s->qd, qd_save);
//----------------- Modif Alex End -----------------
s->tsim = tsim;
// Update state variables
......@@ -662,6 +690,19 @@ void mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsDirdy
else // Numerical solving of constraints and inv(M)
{
err = dirdynared(dd->mbs_aux,s);
//----------------- Modif Alex Start -----------------
if (dd->mbs_aux->permu) {
copy_dvec_1(q_save, s->q);
copy_dvec_1(qd_save, s->qd);
free_dvec_1(q_save);
free_dvec_1(qd_save);
return;
}
//----------------- Modif Alex End -----------------
}
if (err < 0) // Error management
......@@ -696,6 +737,12 @@ void mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsDirdy
dydt[i+2*s->nqu-1] = s->uxd[i];
}
//----------------- Modif Alex Start -----------------
free_dvec_1(q_save);
free_dvec_1(qd_save);
//----------------- Modif Alex End -----------------
}
void save_realtime_update(MbsDirdyn* dd, MbsData* mbs_data)
......
......@@ -42,6 +42,11 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)
{
// Geometric resolution
err = mbs_close_geo(s, mbs_aux);
if (mbs_aux->permu) {
return err;
}
if (err<0) // Error management
{
mbs_msg("\t \t in >DIRDYNARED> \n");
......@@ -247,8 +252,8 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)
for (j = 1; j <= nC; j++)
{
mbs_aux->Mr[i][j] = mbs_aux->Mruc[i][j]; // Muu
if (mbs_aux->Mr[i][j] > 1e-16)
has_a_line_of_zeros = 0;
if (mbs_aux->Mr[i][j] > 1e-16)
has_a_line_of_zeros = 0;
}
if (has_a_line_of_zeros) // Error management
{
......
......@@ -110,7 +110,7 @@ void PART_lutot(double** a, int nl, int nc, int rowperm, int* irk, int* ierr, in
}
// Non pathological case : the process ends at iter = nl or nc
if((iter == nl) || (iter == nc))
if((iter == nl-1) || (iter == nc-1)) //MODIF Alex before it was == nl and == nc
{
*ierr = 0;
PART_vec_perm(row_perm, ipl, *irk);
......@@ -428,7 +428,7 @@ void mbs_run_part(MbsPart* mbs_part, MbsData* mbs_data)
}
}
mbs_data->DonePart = 1;
}
......@@ -558,29 +558,26 @@ void mbs_run_part(MbsPart* mbs_part, MbsData* mbs_data)
//myproject_part = MBS_part;
if (1)
mbs_data->nqu = mbs_part->n_qu;
for (i = 0; i < mbs_part->n_qu; i++)
{
mbs_data->nqu = mbs_part->n_qu;
for (i = 0; i < mbs_part->n_qu; i++)
{
mbs_data->qu[i + 1] = mbs_part->ind_qu[i] + 1;
}
mbs_data->nqv = mbs_part->n_qv;
for (i = 0; i < mbs_part->n_qv; i++)
{
mbs_data->qv[i + 1] = mbs_part->ind_qv[i] + 1;
}
mbs_data->nhu = mbs_part->n_hu;
for (i = 0; i < mbs_part->n_hu; i++)
{
mbs_data->hu[i + 1] = mbs_part->ind_hu[i] + 1;
}
/*mbs_data->nhv = mbs_part->n_hv;
for(i=0; i<mbs_part->n_hv; i++)
{
mbs_data->hv[i+1] = mbs_part->ind_hv[i] +1;
}*/
mbs_data->qu[i + 1] = mbs_part->ind_qu[i] + 1;
}
mbs_data->nqv = mbs_part->n_qv;
for (i = 0; i < mbs_part->n_qv; i++)
{
mbs_data->qv[i + 1] = mbs_part->ind_qv[i] + 1;
}
mbs_data->nhu = mbs_part->n_hu;
for (i = 0; i < mbs_part->n_hu; i++)
{
mbs_data->hu[i + 1] = mbs_part->ind_hu[i] + 1;
}
/*mbs_data->nhv = mbs_part->n_hv;
for(i=0; i<mbs_part->n_hv; i++)
{
mbs_data->hv[i+1] = mbs_part->ind_hv[i] +1;
}*/
if (mbs_part->options->verbose)
......
......@@ -31,6 +31,8 @@ void initialize_rk4(MbsData *mbs_data, MbsDirdyn *mbs_dd) {
void loop_rk4(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
int i;
double h = tf - t0;
int count = 0;
FILE* file = NULL;
// Check remaining time
if (h > mbs_dd->dt) {
......@@ -41,8 +43,34 @@ void loop_rk4(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
if (mbs_dd->options->saveTimePart) {
file = fopen("Time_partitioning.txt", "w");
}
while (mbs_dd->tsim <= tf)
{
if (count == 1) {
if (mbs_dd->mbs_aux->use_det) {
mbs_dd->mbs_aux->iter_part = mbs_dd->mbs_aux->min_det / 10.0;
}
else {
mbs_dd->mbs_aux->iter_part = 2 * mbs_dd->mbs_aux->iter_part;
}
}
if (count >= 5 * mbs_data->njoint) {
printf("mJv matrix \n");
print_dmat_0(mbs_dd->mbs_aux->mJv, mbs_data->nhu+1, mbs_data->nqv+1);
if (mbs_dd->mbs_aux->use_det) {
mbs_msg("\t \t >CLOSE GEO> Error in loop_rk4 can't find a configuration in which the determinant of Jv is > %lf \n", mbs_dd->mbs_aux->min_det);
}
else {
mbs_msg("\t \t >CLOSE GEO> Error in loop_rk4 can't find a configuration in which the loop is close after %d \n", mbs_dd->mbs_aux->iter_part);
}
return;
}
user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
if (mbs_dd->options->flag_precise_dynamics) // Compute f' if asked
......@@ -50,17 +78,79 @@ void loop_rk4(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
}
rk4(mbs_dd->y, mbs_dd->yd, mbs_dd->nState, mbs_dd->tsim, h, mbs_dd->yout, mbs_fct_dirdyn, mbs_data, mbs_dd);
mbs_dd->tsim += h;
for (i = 0; i < mbs_dd->nState; i++)
{
mbs_dd->y[i] = mbs_dd->yout[i];
}
mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd); // next f'
if (!mbs_dd->mbs_aux->permu) {
rk4(mbs_dd->y, mbs_dd->yd, mbs_dd->nState, mbs_dd->tsim, h, mbs_dd->yout, mbs_fct_dirdyn, mbs_data, mbs_dd);
}
// -------------------- ALEX start ------------------
if (mbs_dd->mbs_aux->permu) {
if (mbs_dd->options->saveTimePart && file != NULL) {
fprintf(file, "%lf \n", mbs_dd->tsim);
}
for (i = 1; i <= mbs_data->nqu; i++)
{
mbs_dd->y[i - 1] = mbs_data->q[mbs_data->qu[i]];
mbs_dd->y[i - 1 + mbs_data->nqu] = mbs_data->qd[mbs_data->qu[i]];
}
for (i = 1; i <= mbs_data->Nux; i++)
{
mbs_dd->y[i - 1 + 2 * mbs_data->nqu] = mbs_data->ux[i];
}
for (i = 1; i <= mbs_data->nqu; i++)
{
mbs_dd->yd[i - 1] = mbs_data->qd[mbs_data->qu[i]];
mbs_dd->yd[i + mbs_data->nqu - 1] = mbs_data->qdd[mbs_data->qu[i]];
}
mbs_dd->mbs_aux->permu = 0;
count++;
}
//------------------ Alex stop ------------------------
else {
mbs_dd->tsim += h;
for (i = 0; i < mbs_dd->nState; i++)
{
mbs_dd->y[i] = mbs_dd->yout[i];
}
mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd); // next f'
// -------------------- ALEX start ------------------
if (mbs_dd->mbs_aux->permu) {
if (mbs_dd->options->saveTimePart && file != NULL) {
fprintf(file, "%lf \n", mbs_dd->tsim);
}
for (i = 1; i <= mbs_data->nqu; i++)
{
mbs_dd->y[i - 1] = mbs_data->q[mbs_data->qu[i]];
mbs_dd->y[i - 1 + mbs_data->nqu] = mbs_data->qd[mbs_data->qu[i]];
}
for (i = 1; i <= mbs_data->Nux; i++)
{
mbs_dd->y[i - 1 + 2 * mbs_data->nqu] = mbs_data->ux[i];
}
for (i = 1; i <= mbs_data->nqu; i++)
{
mbs_dd->yd[i - 1] = mbs_data->qd[mbs_data->qu[i]];
mbs_dd->yd[i + mbs_data->nqu - 1] = mbs_data->qdd[mbs_data->qu[i]];
}
mbs_dd->mbs_aux->permu = 0;
}
else {
count = 0;
mbs_dd->mbs_aux->iter_part = mbs_dd->options->iter_part;
mbs_dd->mbs_aux->min_det = mbs_dd->options->det_precision;
}
//------------------ Alex stop ------------------------
}
// save results if always asked or if waypoint reached
if ((!mbs_dd->options->flag_solout_wp) || (mbs_dd->options->flag_solout_wp && mbs_dd->tsim == tf)) {
save_realtime_update(mbs_dd, mbs_data);
......@@ -68,6 +158,10 @@ void loop_rk4(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
}
if (mbs_dd->options->saveTimePart && file != NULL) {
fclose(file);
}
}
void finish_rk4(MbsData *mbs_data, MbsDirdyn *dd) {
......
......@@ -25,6 +25,17 @@
int i;
double xh, hh, h6, *dym, *dyt, *yt;
//----------------- Modif Alex Start -----------------
double *q_save, *qd_save;
q_save = get_dvec_1(s->njoint);
qd_save = get_dvec_1(s->njoint);
copy_dvec_1(s->q, q_save);
copy_dvec_1(s->qd, qd_save);
//----------------- Modif Alex End -----------------
dym = &((MbsDirdynRK4 *)dd->integrator_struct)->dym[0];
dyt = &((MbsDirdynRK4 *)dd->integrator_struct)->dyt[0];
yt = &((MbsDirdynRK4 *)dd->integrator_struct)->yt[0];
......@@ -40,11 +51,39 @@
// Second step
(*derivs)(xh, yt, dyt, s, dd);
//------------ Modif Alex Start --------------
if (dd->mbs_aux->permu) {
copy_dvec_1(q_save, s->q);
copy_dvec_1(qd_save, s->qd);
free_dvec_1(q_save);
free_dvec_1(qd_save);
return;
}
//------------ Modif Alex End --------------
for (i = 0; i < n; i++)
yt[i] = y[i] + hh*dyt[i];
// Third step
(*derivs)(xh, yt, dym, s, dd);
//------------ Modif Alex Start --------------
if (dd->mbs_aux->permu) {
copy_dvec_1(q_save, s->q);
copy_dvec_1(qd_save, s->qd);
free_dvec_1(q_save);
free_dvec_1(qd_save);
return;
}
//------------ Modif Alex End --------------
for (i = 0; i < n; i++)
{
yt[i] = y[i] + h*dym[i];
......@@ -54,7 +93,22 @@
// Fourth step
(*derivs)(x + h, yt, dyt, s, dd);
//------------ Modif Alex Start --------------
if (dd->mbs_aux->permu) {
copy_dvec_1(q_save, s->q);
copy_dvec_1(qd_save, s->qd);
free_dvec_1(q_save);
free_dvec_1(qd_save);
return;
}
//------------ Modif Alex End --------------
// Accumulate increments with proper weights
for (i = 0; i < n; i++)
yout[i] = y[i] + h6*(dydx[i] + dyt[i] + 2.0*dym[i]);
free_dvec_1(q_save);
free_dvec_1(qd_save);
}
......@@ -26,6 +26,11 @@ typedef struct MbsAux_tag
double norm_h;
double NRerr;
int MAX_NR_ITER;
int use_det;
int iter_part;
int Jv_verbose;
int online_part;
double min_det;
double *h, **Jac;
double *huserc, **Juserc;
......@@ -53,6 +58,8 @@ typedef struct MbsAux_tag
double * bp;
// double **bp;
int permu; //equal to 1 if a coordiante permutation has been execute
MbsSensor psens[1]; // temporary sensor pointer
#elif defined SENSORKIN
......
......@@ -120,6 +120,33 @@ typedef struct MbsDirdynOptions
/*TO BE COMPLETED*/
int n_freeze; ///< number of time step when the jacobian is freezed (computed once at the start of the n_freeze time steps), default = 0;
/*---------------------------------------------------*/
/* Options related to online coordinate partitioning */
/*---------------------------------------------------*/
/** Determine if you use the determinant or the number of iterartion of Newton-Raphson as condtion on the repartitioning
* 1: Use the determinant
* 0: Use the number of iteration of Newton-Raphson
* default = 0
**/
int use_det;
/** Determine if time of partitioning should be save
* 1: time of partitioning is save
* 0: time of partitioning is not save
* default = 1
*/
int saveTimePart;
//minimum value of Jv's determinant, default = 1e-3
double det_precision;
//maximium number of Newton-Raphson iterration before re-partitioning, default = 5
int iter_part;
/** Determine if Jv before online partitioning and after should be display
* 1: Should dispaly Jv
* 0: Should not display Jv
* default = 1;
**/
int Jv_verbose;
} MbsDirdynOptions;
/**
......
......@@ -1108,3 +1108,85 @@ void scale_matrix(double** matrix, int* n, int* initzeros)
}
}
}
//---------------------------------------------------------------------------
// Function definition to obtain a sub matrix without ligne 1 and colum x
//---------------------------------------------------------------------------
void sub_matrix(double** a, int n, int x, int y, double** a1) {
int i, j;
int k = 0;
int m = 0;
for (i = 0; i < n; i++) {
for (j = 0; j < n; j++) {
if (i != x && j != y) {
a1[m][k] = a[i][j];
k++;
}
}
k = 0;
if (i != x) {
m++;
}
}
}
//---------------------------------------------------------------------------
// Function definition to compute the determinant of a matrix
//---------------------------------------------------------------------------
/*
* a : matrix
* n : matrix's size
*/
double det_dmatrix_0(double** a, int n) {
int i, j, k, index;
double ratio, swap, det = 1.0, p = 1.0;
for (i = 0; i < n - 1; i++)
{
index = i;
while (a[index][i] == 0.0) {
index++;
if (index == n) {
index = 0;
// the determinat of the matrix is zero
while (a[index][i] == 0.0 && index < i) {
index++;
}
// if all the colone is null det(a) = 0
if (index == i) {
return 0.0;
}
// if not nothing to do with this i
else {
continue;
}
}
}
if (index != i)
{
//loop for swaping the diagonal element row and index row
for (j = 0; j < n; j++)
{
swap = a[index][j];
a[index][j] = a[i][j];
a[i][j] = swap;
}
//determinant sign changes when we shift rows
//go through determinant properties
p = -p;
}
for (j = i + 1; j < n; j++)
{
ratio = a[j][i] / a[i][i];
for (k = i; k < n; k++)
{
a[j][k] = a[j][k] - ratio * a[i][k];
}
}
det *= a[i][i];
}
return p * det*a[n - 1][n - 1];
}
......@@ -517,6 +517,24 @@ void arrange_matrix(double** matrix, int* n, int* initzeros);
*/
void scale_matrix(double** matrix, int* n, int* initzeros);
/*! \brief function to obtain a sub matrix without line x and colum y
*
* \param[in,out] the matrix
* \param[in] the size of the matrix nxn
* \param[in] the number of the line to skip
* \param[in] the number of the colum to skip
* \param[in, out] sub-matrix
*/
void sub_matrix(double** a, int n, int x, int y, double** a1);
/*! \brief function to compute the determinant of a matrix
*
* \param[in,out] the matrix
* \param[in] the size of the matrix nxn
* \return the determinant of this matrix
*/
double det_dmatrix_0(double** a, int n);
/*
* Print values of mbs_data (used for debug)
*/
......