/*
* Initialize a 'Write_files' structure
*/
Write_files* init_write_files(int nstep, int njoint)
{
Write_files* write_files;
write_files = (Write_files*) malloc(sizeof(Write_files));
write_files->t = get_double_vec(nstep);
write_files->qq = get_double_tab(njoint, nstep);
write_files->out_vec = get_double_tab(NB_OUTPUT_VEC, nstep);
write_files->kount = 0;
return write_files;
}
void PART_calc_hJ(MBSdataStruct* MBSdata, double* h, double** J)
{
int i,j;
double* h_loopc = get_double_vec(MBSdata->Nloopc +1);
double** J_loopc = get_double_tab(MBSdata->Nloopc +1, MBSdata->njoint+1);
double* h_userc= get_double_vec(MBSdata->Nuserc +1);
double** J_userc = get_double_tab(MBSdata->Nuserc +1, MBSdata->njoint+1);
if(MBSdata->Nloopc)
{
cons_hJ(h_loopc, J_loopc, MBSdata, MBSdata->tsim);
for (i=0; i<MBSdata->Nloopc; i++)
{
h[i] = h_loopc[i+1];
for (j=0; j<MBSdata->njoint; j++)
{
J[i][j] =J_loopc[i+1][j+1];
}
}
}
if(MBSdata->Nuserc)
{
user_cons_hJ(h_userc, J_userc, MBSdata, MBSdata->tsim);
for (i=0; i<MBSdata->Nuserc; i++)
{
h[MBSdata->Nloopc+i] = h_userc[i+1];
for (j=0; j<MBSdata->njoint; j++)
{
J[MBSdata->Nloopc+i][j] =J_userc[i+1][j+1];
}
}
}
free_double_vec(h_loopc);
free_double_tab(J_loopc, MBSdata->Nloopc +1);
free_double_vec(h_userc);
free_double_tab(J_userc, MBSdata->Nuserc +1);
}
void PART_coord_part(MDS_gen_strct* mds_gen_strct, MBSdataStruct* MBSdata, PART_gen_strct* part_gen_strct, int* ind_u_des, int n_u_des, int* ind_c, int n_c, int first_partitioning , int* err)
{
int i, j, nl_JAC, nc_JAC, Real_Rank, prsnt,n_vu, ivu, irk, ierr;
double* q_rand, * q_safe;
double* h_;
double** Jac, **Jv;
int *ind_u_des_com, *ind_vu, *row_perm, *col_perm;
int *ind_aux, *ind_irk, *ind_u_nsort ,*ind_hv_nsort;
q_rand = get_double_vec(MBSdata->njoint);
q_safe = get_double_vec(MBSdata->njoint);
nl_JAC = MBSdata->Nloopc + MBSdata->Nuserc;
nc_JAC = MBSdata->njoint;
h_ = get_double_vec(nl_JAC);
Jac = get_double_tab(nl_JAC, nc_JAC);
ind_u_des_com = get_int_vec(n_u_des + n_c);
n_vu = MBSdata->njoint - (n_u_des + n_c);
ind_vu = get_int_vec(n_vu);
*err = 0;
// Calcul du rang réel de la matrice Jacobienne
for(i=0; i<MBSdata->njoint; i++)
{
q_rand[i] = frand();
}
copy_double_vec(&(MBSdata->q[1]), q_safe, MBSdata->njoint);
copy_double_vec(q_rand, &(MBSdata->q[1]), MBSdata->njoint);
PART_calc_hJ(MBSdata, h_, Jac);
Real_Rank = rank_double_tab(Jac, nl_JAC, nc_JAC); // calcul du rang reel de Jac pour q random
copy_double_vec(q_safe, &(MBSdata->q[1]), MBSdata->njoint);
// Proposition de partitionnement pour un u_des fourni (vide), (partiel, plein, valide ou non)
conc_int_vec (ind_u_des, n_u_des, ind_c, n_c, ind_u_des_com); // mise en commun des u_des et des c
ivu=0; // variables (v et u) a partitionner //old = setdiff in matlab
for(i=0; i<MBSdata->njoint; i++)
{
prsnt = 0;
for(j=0; j<(n_u_des + n_c) ;j++)
{
if(i == ind_u_des_com[j])
{
prsnt = 1;
}
}
if(prsnt == 0)
{
ind_vu[ivu] = i;
ivu++;
}
}
if( first_partitioning == 1)// on ne perturbe les 0 que la première fois (car seconde = système fermé)
{
for(i=0; i<n_vu; i++)// perturbation random (10-5) des valeurs nulles des (v,u_non_des): éviter les singularités
{
if (fabs(MBSdata->q[ind_vu[i]]) < 1e-6)
{
MBSdata->q[ind_vu[i]] = 1e-5*frand();
}
}
}
PART_calc_hJ(MBSdata, h_, Jac);
row_perm = get_int_vec(nl_JAC);
col_perm = get_int_vec(n_vu);
Jv = get_double_tab(nl_JAC, n_vu);
// Pivoted LU Factorisation
slctc_double_tab(Jac, nl_JAC, nc_JAC, Jv, n_vu, ind_vu);
PART_lutot(Jv, nl_JAC, n_vu, part_gen_strct->options->rowperm, &irk, &ierr, row_perm, col_perm);
// Check the user choice for independent coordinates
if (irk < Real_Rank) // choix mauvais - erreur
{
printf("<<<<<ok : %d>>>>>\n",irk );
part_gen_strct->ind_qu = NULL; part_gen_strct->ind_qv = NULL; part_gen_strct->ind_hu = NULL; part_gen_strct->ind_hv = NULL;
part_gen_strct->n_qu = 0;
part_gen_strct->n_qv = 0;
part_gen_strct->n_hu = 0;
part_gen_strct->n_hv = 0;
printf(">>PART>>\n");
printf(">>PART>> mbs_coord_part: wrong choice for the independent variable set - Jv matrix singular\n");
printf(">>PART>>\n");
*err = -1;
}
else // choix valide
{
ind_aux = get_int_vec(n_vu);
ind_irk = get_int_vec(irk);
f0123_int_vec(ind_irk, irk);
slct_int_vec(ind_vu, n_vu, col_perm, n_vu, ind_aux);
part_gen_strct->n_qv = irk;
part_gen_strct->ind_qv = get_int_vec(part_gen_strct->n_qv);
slct_int_vec(ind_aux, 0, ind_irk, irk, part_gen_strct->ind_qv); // Liste definitive des v
if (n_vu > irk) // additional independent variabes : les dernières //?? n_vu = lenth (ind_aux)
{
ind_u_nsort = get_int_vec(n_u_des+(n_vu-irk));
copy_int_vec(ind_u_des, ind_u_nsort,n_u_des);
for(i=0; i<(n_vu-irk); i++)
{
ind_u_nsort[n_u_des + i] = ind_aux[irk+i];
}
part_gen_strct->n_qu = n_u_des +(n_vu-irk);
}
else
{
part_gen_strct->n_qu = n_u_des;
ind_u_nsort = get_int_vec(part_gen_strct->n_qu);
copy_int_vec(ind_u_des, ind_u_nsort,part_gen_strct->n_qu); // no additional independent variable
}
part_gen_strct->ind_qu = get_int_vec(part_gen_strct->n_qu);
sort_int_vec(ind_u_nsort, part_gen_strct->ind_qu, part_gen_strct->n_qu);
free_int_vec(ind_u_nsort);
for(i=0; i<part_gen_strct->n_qu; i++)
{
MBSdata->q[part_gen_strct->ind_qu[i]+1] = q_safe[part_gen_strct->ind_qu[i]]; // +1 for MBSdata convention
}
if (irk == nl_JAC) // Case : no redundant constraints
{
part_gen_strct->n_hu = nl_JAC;
part_gen_strct->ind_hu = get_int_vec(part_gen_strct->n_hu);
copy_int_vec(row_perm, part_gen_strct->ind_hu, part_gen_strct->n_hu);
part_gen_strct->ind_hv = NULL;
part_gen_strct->n_hv = 0;
}
else // Case : redundant constraints
{
part_gen_strct->n_hu = irk;
part_gen_strct->ind_hu = get_int_vec(part_gen_strct->n_hu);
copy_int_vec(row_perm, part_gen_strct->ind_hu, part_gen_strct->n_hu);
part_gen_strct->n_hv = nl_JAC-irk;
ind_hv_nsort = get_int_vec(part_gen_strct->n_hv);
copy_int_vec(&(row_perm[irk]), ind_hv_nsort, part_gen_strct->n_hv);
part_gen_strct->ind_hv = get_int_vec(part_gen_strct->n_hv);
sort_int_vec(ind_hv_nsort, part_gen_strct->ind_hv, part_gen_strct->n_hv);
free_int_vec(ind_hv_nsort);
}
free_int_vec(ind_aux);
free_int_vec(ind_irk );
}
free_int_vec(row_perm);
free_int_vec(col_perm);
free_double_tab(Jv, nl_JAC);
free_double_vec(q_rand);
free_double_vec(q_safe);
free_double_vec(h_);
free_double_tab(Jac, nl_JAC);
free_int_vec(ind_u_des_com);
free_int_vec(ind_vu);
}
int rank_double_tab(double** matrix_in, int x, int y)
{
double **matrix;
/*
int n[2], rank;
readmatrix(matrix, n);
printf("\tThe matrix you have entered is shown below\n\n");
printmatrix(matrix,n);
rank = rank_double_vec(matrix, n[0], n[1]);
*/
int n[2], i, retest=1, grp, p, r, j, rank;
int* initzeros;
matrix = get_double_tab(x,y);
copy_double_tab(matrix_in, matrix,x, y);
n[0] = x;
n[1] = y;
initzeros = get_int_vec(y);
/*
printf("\tThe matrix you have entered is shown below %d , %d\n\n", x, y);
printmatrix(matrix,n);
//*/
update_initzeros(initzeros,matrix,n);
arrange_matrix(matrix,n,initzeros);
/*printf("\tThe matrix after arrange %d , %d\n\n", x, y);
printmatrix(matrix,n);
if(matrix[0][0]==0)
{
printf("\n\tError: Invalid Marix \n\n");
}
*/
update_initzeros(initzeros,matrix,n);
scale_matrix(matrix,n,initzeros);
while(retest==1)
{
grp=0;
for(i=0;i<n[0];++i)
{
p=0;
while(initzeros[i+p]==initzeros[i+p+1]&&(i+p+1)<n[0])
{
grp=grp+1;
p=p+1;
}
if(grp!=0)
{
while(grp!=0)
{
for(j=0;j<n[1];++j)
{
matrix[i+grp][j]=matrix[i+grp][j]-matrix[i][j];
}
grp=grp-1;
}
break;
}
}
update_initzeros(initzeros,matrix,n);
arrange_matrix(matrix,n,initzeros);
update_initzeros(initzeros,matrix,n);
scale_matrix(matrix,n,initzeros);
retest=0;
for(r=0;r<n[0];++r)
{
if(initzeros[r]==initzeros[r+1]&&r+1<n[0])
{
if(initzeros[r]!=n[1])
retest=1;
}
}
}
//printf("\n\n\t Reduced matrix is as shown below\n\n");
//printmatrix(matrix,n);
rank =0;
for (i=0;i<n[0];++i)
{
if (initzeros[i]!=n[1])
{
++rank;
}
}
free_double_tab(matrix, n[0]);
//printf("\n Rank Of The Matrix = %d\n\n", rank);
return rank;
}
