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);
}
/*
* 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 arrange_matrix(double** matrix,int* n,int* initzeros)
{
int l,reqrow,i,k,lastrow,tempvar,large;
double* rowtemp;
rowtemp = get_double_vec(n[1]);
lastrow=n[0]-1;
for(l=0;l<n[0];++l)
{
large=initzeros[0];
for(i=0;i<n[0];++i)
{
if(large<=initzeros[i])
{
large=initzeros[i];
reqrow=i;
}
}
initzeros[reqrow]=-1;
tempvar=initzeros[reqrow];
initzeros[reqrow]=initzeros[lastrow];
initzeros[lastrow]=tempvar;
for(k=0;k<n[1];++k)
{
rowtemp[k]=matrix[lastrow][k];
}
for(k=0;k<n[1];++k)
{
matrix[lastrow][k]=matrix[reqrow][k];
}
for(k=0;k<n[1];++k)
{
matrix[reqrow][k]=rowtemp[k];
}
lastrow=lastrow-1;
}
}
PART_gen_strct* init_PART_gen_strct(MDS_gen_strct* mds_gen_strct)
{
PART_gen_strct *part_gen_strct;
part_gen_strct = (PART_gen_strct*) malloc(sizeof(PART_gen_strct));
part_gen_strct->options = init_PART_option_strct();
part_gen_strct->n_qu = 0;
part_gen_strct->ind_qu = NULL;
part_gen_strct->n_qv = 0;
part_gen_strct->ind_qv = NULL;
part_gen_strct->n_hu = 0;
part_gen_strct->ind_hu = NULL;
part_gen_strct->n_hv = 0;
part_gen_strct->ind_hv = NULL;
part_gen_strct->q_closed = get_double_vec(mds_gen_strct->bodytree->n_joint);
return part_gen_strct;
}
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);
}
