int PART_run_part(MDS_gen_strct* mds_gen_strct, MBSdataStruct* MBSdata, PART_gen_strct* part_gen_strct)
{
int i;
int err, nbiter;
int* ind_c = NULL;
int* ind_u_des = NULL;
LocalDataStruct *lds; //change it !
if(MBSdata->Ncons == 0) // change it !
{
if(part_gen_strct->options->verbose)
{
printf(">>PART>> no coordinate partitioning required for this system !\n");
}
MBSdata->DonePart = 1;
return MBSdata->DonePart;
}
if(part_gen_strct->options->verbose)
{
printf(">>PART>> PART_run start\n");
}
// sort qu and qc
ind_c = get_int_vec(mds_gen_strct->bodytree->n_qc);
ind_u_des = get_int_vec(mds_gen_strct->bodytree->n_qu);
sort_int_vec(mds_gen_strct->bodytree->qc, ind_c, mds_gen_strct->bodytree->n_qc);
sort_int_vec(mds_gen_strct->bodytree->qu, ind_u_des, mds_gen_strct->bodytree->n_qu);
// call user_drivenJoints
user_DrivenJoints(MBSdata, MBSdata->tsim);
/*
for ( i=0; i<100; i++)
{
printf("%f\n",frand());
}*/
/*
print_int_vec(mds_gen_strct->bodytree->qu, s->nqu);
print_int_vec(ind_u_des, s->nqu);
*/
// First partitioning
if (part_gen_strct->options->verbose)
{
printf(">>PART>> ***** Coordinate partitioning *****\n");
}
PART_coord_part(mds_gen_strct, MBSdata, part_gen_strct, ind_u_des, mds_gen_strct->bodytree->n_qu, ind_c, mds_gen_strct->bodytree->n_qc, 1, &err); // d'abord on cherche un choix valable
if( err == -1)
{
printf(">>PART>>\n");
printf(">>PART>> ***** mbs_run_part : Coordinate partitioning interrupted : *****\n");
printf(">>PART>>\n");
//myproject_part = MBS_part;
MBSdata->DonePart = 0;
return MBSdata->DonePart;
}
// Fermeture
if (part_gen_strct->options->verbose)
{
printf(">>PART>> ***** Geometrical Constraint solution *****\n");
}
lds = initLocalDataStruct(MBSdata);
nbiter = mbs_close_geo(MBSdata, lds); // on essaie de fermer
freeLocalDataStruct(lds, MBSdata);
if (nbiter == -1)
{
printf(">>PART>>\n");
printf(">>PART>> ***** mbs_run_part : impossible to close the MBS : *****\n");
printf(">>PART>> -> try with other desired independent variables u\n");
printf(">>PART>> and/or\n");
printf(">>PART>> -> try with other initial values for q (u, v)\n");
printf(">>PART>> -> ... but have a look at the proposed {u,v} partition\n");
printf(">>PART>>\n");
}
else
{
if (part_gen_strct->options->verbose)
{
printf("***** Constaints solved after %d iterations *****\n", nbiter);
}
copy_double_vec(MBSdata->q, part_gen_strct->q_closed ,mds_gen_strct->bodytree->n_joint);
// Second partitioning sur la structure fermée (peaufinement éventuel toujours sur base des ind_u_des)
PART_coord_part(mds_gen_strct, MBSdata, part_gen_strct, ind_u_des, mds_gen_strct->bodytree->n_qu, ind_c, mds_gen_strct->bodytree->n_qc, 0, &err);
if (err == -1) // peu probable mais à envisager ...
{
printf(">>PART>>\n");
printf(">>PART>> ***** mbs_run_part : Second coordinate partitioning interrupted : *****\n");
printf(">>PART>>\n");
//myproject_part = MBS_part;
MBSdata->DonePart = 0;
return MBSdata->DonePart;
}
else
{
MBSdata->DonePart = 1;
}
}
// Stockage des résultats dans la structure part_gen_strct et s//tockage
/*
part_gen_strct.nu = length(ind_u);
part_gen_strct.ind_u = ind_u;
part_gen_strct.nv = length(ind_v);
part_gen_strct.ind_v = ind_v;
part_gen_strct.nhu = length(ind_hu); // JFC : 15/01/2008 : ajout
part_gen_strct.hu = ind_hu;
part_gen_strct.nhv = length(ind_hv); // PF : 09/04/2009 : ajout
part_gen_strct.hv = ind_hv;
*/
// Affectation global=>Workspace
//myproject_part = MBS_part;
if (1)
{
MBSdata->nqu = part_gen_strct->n_qu;
for(i=0; i<part_gen_strct->n_qu; i++)
{
MBSdata->qu[i+1] = part_gen_strct->ind_qu[i] +1;
}
MBSdata->nqv = part_gen_strct->n_qv;
for(i=0; i<part_gen_strct->n_qv; i++)
{
MBSdata->qv[i+1] = part_gen_strct->ind_qv[i] +1;
}
MBSdata->nhu = part_gen_strct->n_hu;
for(i=0; i<part_gen_strct->n_hu; i++)
{
MBSdata->hu[i+1] = part_gen_strct->ind_hu[i] +1;
}
/*MBSdata->nhv = part_gen_strct->n_hv;
for(i=0; i<part_gen_strct->n_hv; i++)
{
MBSdata->hv[i+1] = part_gen_strct->ind_hv[i] +1;
}*/
}
if (part_gen_strct->options->verbose)
{
printf(">>PART>> ***** mbs_run_part end *****\n");
}
//system("pause");
return MBSdata->DonePart;
}
int dirdynared(LocalDataStruct *lds,MBSdataStruct *s)
{
int i,j,k;
int iter=0;
int nL,nC,nk;
double term;
int has_a_line_of_zeros;
// Expression des variables commandées
if (s->nqc>0) user_DrivenJoints(s,s->tsim);
// Résolution des Contraintes
if (s->Ncons > 0)
{
// résolution géométrique
iter = mbs_close_geo(s, lds);
if (iter>=lds->MAX_NR_ITER)
{
return -1;
}
// résolution cinématique
mbs_close_kin(s, lds);
}//if(s->Ncons > 0)
// calcul des forces appliquées sur les corps
for(i=1;i<=s->nbody;i++)
{
for(j=1;j<=3;j++)
{
s->frc[j][i]=0.0;
s->trq[j][i]=0.0;
}
}
#ifdef CXX
if(s->Nlink > 0) link1D(s->frc,s->trq,s->Fl,s->Z,s->Zd,s,s->tsim);
#else
if(s->Nlink > 0) link(s->frc,s->trq,s->Fl,s->Z,s->Zd,s,s->tsim);
#endif
if(s->Nxfrc > 0) extforces(s->frc,s->trq,s,s->tsim);
s->Qq = user_JointForces(s,s->tsim);
// calcul de la matrice de masse et du vecteur des forces non linéaires
dirdyna(lds->M,lds->c,s,s->tsim);
for(i=1;i<=s->njoint;i++)
{
lds->F[i] = lds->c[i] - s->Qq[i];
}
// calcul de la matrice de masse et du vecteur des forces reduites (DAE => ODE)
if (s->nqv>0)
{
// Mr_uc = Muc_uc + Muc_v*Bvuc + Bvuc'*Mv_uc + Bvuc'*Mvv*Bvuc;
// Mr_uc = Muc_uc + Muc_v*Bvuc + (Muc_v*Bvuc)' + (Bvuc'*Mvv)*Bvuc;
// Fr_uc = Fuc + Muc_v*bprim + Bvuc'*Fv + Bvuc'*Mvv*bprim;
// Fr_uc = Fuc + (Muc_v+ Bvuc'*Mvv)*bprim + Bvuc'*Fv ;
// BtMvu = Bvuc'*Mv_uc
nL = nC = lds->iquc[0];
nk = s->nqv;
for(i=1;i<=nL;i++)
{
for(j=1;j<=nC;j++)
{
term = 0.0;
for(k=1;k<=nk;k++)
{
term += lds->Bvuc[k][i] * lds->M[s->qv[k]][lds->iquc[j]];
}
lds->BtMvu[i][j] = term;
}
}
// BtMvv = Bvuc'*Mvv
nL = lds->iquc[0];
nC = nk = s->nqv;
for(i=1;i<=nL;i++)
{
for(j=1;j<=nC;j++)
{
term = 0.0;
for(k=1;k<=nk;k++)
{
term += lds->Bvuc[k][i] * lds->M[s->qv[k]][s->qv[j]];
}
lds->BtMvv[i][j] = term;
}
}
// BtFv = Bvuc'*Fv
nL = lds->iquc[0];
nk = s->nqv;
for(i=1;i<=nL;i++)
{
term = 0.0;
for(k=1;k<=nk;k++)
{
term += lds->Bvuc[k][i] * lds->F[s->qv[k]];
}
lds->BtFv[i] = term;
}
// BtMB = BtMvv*Bvuc
nL = nC = lds->iquc[0];
nk = s->nqv;
for(i=1;i<=nL;i++)
{
for(j=1;j<=nC;j++)
{
term = 0.0;
for(k=1;k<=nk;k++)
{
term += lds->BtMvv[i][k] * lds->Bvuc[k][j];
}
lds->BtMB[i][j] = term;
}
}
// MBMb = (Muv+Bvuc'*Mvv)*bprim
nL = lds->iquc[0];
nk = s->nqv;
for(i=1;i<=nL;i++)
{
term = 0.0;
for(k=1;k<=nk;k++)
{
term += (lds->M[lds->iquc[i]][s->qv[k]] + lds->BtMvv[i][k]) * lds->bp[k];
}
lds->MBMb[i] = term;
}
/*
nL = lds->iquc[0];
nk = s->nqv;
for(i=1;i<=nL;i++)
{
// BtMvu = Bvuc'*Mvu
nC = lds->iquc[0];
for(j=1;j<=nC;j++)
{
term = 0.0;
for(k=1;k<=nk;k++)
{
term += lds->Bvuc[k][i] * lds->M[s->qv[k]][lds->iquc[j]];
}
lds->BtMvu[i][j] = term;
}
// BtMvv = Bvuc'*Mvv
nC = s->nqv;
for(j=1;j<=nC;j++)
{
term = 0.0;
for(k=1;k<=nk;k++)
{
term += lds->Bvuc[k][i] * lds->M[s->qv[k]][s->qv[j]];
}
lds->BtMvv[i][j] = term;
}
// BtFv = Bvuc'*Fv
term = 0.0;
for(k=1;k<=nk;k++)
{
term += lds->Bvuc[k][i] * lds->F[s->qv[k]];
}
lds->BtFv[i] = term;
// BtMB = BtMvv*Bvuc
nC = lds->iquc[0];
for(j=1;j<=nC;j++)
{
term = 0.0;
for(k=1;k<=nk;k++)
{
term += lds->BtMvv[i][k] * lds->Bvuc[k][j];
}
lds->BtMB[i][j] = term;
}
// MBMb = (Muv+Bvuc'*Mvv)*bprim
nC = s->nqv;
term = 0.0;
for(k=1;k<=nk;k++)
{
term += (lds->M[lds->iquc[i]][s->qv[k]] + lds->BtMvv[i][k]) * lds->bp[k];
}
lds->MBMb[i] = term;
}
*/
// Mruc Fruc
nL = nC = lds->iquc[0];
for(i=1;i<=nL;i++)
{
for(j=1;j<=nC;j++)
{
lds->Mruc[i][j] = lds->M[lds->iquc[i]][lds->iquc[j]] +
lds->BtMvu[i][j] + lds->BtMvu[j][i] +
lds->BtMB[i][j];
}
lds->Fruc[i] = lds->F[lds->iquc[i]] + lds->MBMb[i] + lds->BtFv[i];
}
}
else
{
// Mruc Fruc
nL = nC = lds->iquc[0];
for(i=1;i<=nL;i++)
{
for(j=1;j<=nC;j++)
{
lds->Mruc[i][j] = lds->M[lds->iquc[i]][lds->iquc[j]];
}
lds->Fruc[i] = lds->F[lds->iquc[i]];
}
}
// Mr Fr
nL = nC = s->nqu;
for(i=1;i<=nL;i++)
{
has_a_line_of_zeros = 1;
for(j=1;j<=nC;j++)
{
lds->Mr[i][j] = lds->Mruc[i][j]; // Muu
if (lds->Mr[i][j]>1e-16)
has_a_line_of_zeros = 0;
}
if (has_a_line_of_zeros)
{
printf("The line %d of the reduced mass matrix, associated to q(%d), is full of zeros\n",i,lds->iquc[i]);
for(k=1;k<=nC;k++)
printf("lds->Mr[%d][%d] = %e;\n",i,k,lds->Mr[i][k]);
fprintf(stderr,"The reduced mass matrix has a line of zeros\n");
}
term = 0.0;
for(k=1;k<=s->nqc;k++)
{
term += lds->Mruc[i][s->nqu+k] * s->qdd[lds->iquc[s->nqu+k]];
}
lds->Fr[i] = -(lds->Fruc[i] + term);
}
// calcul des accelerations reduites : 'resolution' du systeme ODE = Mr*qddu = Fr;
choldc(lds->Mr,s->nqu,lds->p_Mr);
cholsl(lds->Mr,s->nqu,lds->p_Mr,lds->Fr,s->qddu);
nL = s->nqu;
for(i=1;i<=nL;i++)
{
s->qdd[s->qu[i]] = s->qddu[i];
}
if (s->nqv>0)
{
// qdd_v = Bvuc*qdd_u + bp
nL = s->nqv;
nk = s->nqu;
for(i=1;i<=nL;i++)
{
term = 0.0;
for(k=1;k<=nk;k++)
{
term += lds->Bvuc[i][k] * s->qddu[k];
}
s->qdd[s->qv[i]] = term + lds->bp[i];
}
}
// Qc = Mruc_cu * qddu + Mruc_cc * qddc + Fruc_c
if (s->nqc>0)
{
nL = s->nqc;
for(i=1;i<=nL;i++)
{
term = 0.0;
nk = s->nqu;
for(k=1;k<=nk;k++)
{
term += lds->Mruc[s->nqu+i][k] * s->qddu[k];
}
nk = s->nqc;
for(k=1;k<=nk;k++)
{
term += lds->Mruc[s->nqu+i][s->nqu+k] * s->qdd[lds->iquc[s->nqu+k]];
}
lds->Qc[i] = term + lds->Fruc[s->nqu+i];
}
}
return iter;
}
