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; }