Commit 37bc005b authored by Nicolas Docquier's avatar Nicolas Docquier
Browse files

Move function pointer to a sub-structure of mbs_data

parent 65256214
......@@ -61,7 +61,7 @@ g = s->g;
// Driven Variables
s->user_DrivenJoints(s,tsim);
s->fct.user.user_DrivenJoints(s,tsim);
NRh2 = 1.0;
while((NRh2 > NR_ERR) && (iter++ < MAX_NR_ITER))
......@@ -178,7 +178,7 @@ if(iter > MAX_NR_ITER) return(-1);
// Joint Forces
Qq = s->user_JointForces(s,tsim);
Qq = s->fct.user.user_JointForces(s,tsim);
// Link Kinematics: Distance Z , Relative Velocity ZD
......@@ -195,7 +195,7 @@ if(iter > MAX_NR_ITER) return(-1);
// Link Forces
Flnk1 = s->user_LinkForces(Z1,Zd1,s,tsim,1);
Flnk1 = s->fct.user.user_LinkForces(Z1,Zd1,s,tsim,1);
// Link Dynamics : Forces projection on body-fixed frames
......
......@@ -83,7 +83,7 @@ MbsData *s, double tsim)
// Link Force Computation
Flink1 = s->user_LinkForces(Z1,Zd1,s,tsim,1);
Flink1 = s->fct.user.user_LinkForces(Z1,Zd1,s,tsim,1);
// = = Block_0_1_0_2_2_1 = =
......
......@@ -28,9 +28,9 @@ extern "C" {
int main(int argc, char const *argv[])
{
MbsData *mbs_data;
MbsPart *mbs_part;
MbsDirdyn *mbs_dirdyn;
......@@ -40,11 +40,11 @@ int main(int argc, char const *argv[])
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
/* LOADING *
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
printf("Loading the PendulumSpring data file !\n");
mbs_data = mbs_load(PROJECT_SOURCE_DIR"/../dataR/PendulumSpringC.mbs");
printf("*.mbs file loaded!\n");
mbs_get_project_functions(mbs_data);
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
......@@ -55,7 +55,7 @@ int main(int argc, char const *argv[])
mbs_part->options->rowperm=1;
mbs_part->options->verbose = 1;
mbs_run_part(mbs_part, mbs_data);
mbs_delete_part(mbs_part);
......@@ -71,17 +71,17 @@ int main(int argc, char const *argv[])
mbs_dirdyn->options->save2file = 1;
mbs_dirdyn->options->respath = PROJECT_SOURCE_DIR"/../resultsR";
mbs_dirdyn->options->realtime = 1;
mbs_run_dirdyn(mbs_dirdyn, mbs_data);
mbs_delete_dirdyn(mbs_dirdyn, mbs_data);
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
/* CLOSING OPERATIONS *
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
mbs_delete_data(mbs_data);
return 0;
}
......@@ -150,13 +150,13 @@ void mbs_get_project_functions(MbsData *mbs_data)
//mbs_data->mbs_cons_jdqd = mbs_cons_jdqd;
#ifdef UNIX
mbs_data->mbs_link = dlsym(lib_handle, "mbs_link");
mbs_data->mbs_link3D = dlsym(lib_handle, "mbs_link3D");
mbs_data->mbs_extforces = dlsym(lib_handle, "mbs_extforces");
mbs_data->mbs_accelred = dlsym(lib_handle, "mbs_accelred");
mbs_data->mbs_dirdyna = dlsym(lib_handle, "mbs_dirdyna");
mbs_data->mbs_cons_hJ = dlsym(lib_handle, "mbs_cons_hJ");
mbs_data->mbs_cons_jdqd = dlsym(lib_handle, "mbs_cons_jdqd");
mbs_data->fct.symb.mbs_link = dlsym(lib_handle, "mbs_link");
mbs_data->fct.symb.mbs_link3D = dlsym(lib_handle, "mbs_link3D");
mbs_data->fct.symb.mbs_extforces = dlsym(lib_handle, "mbs_extforces");
mbs_data->fct.symb.mbs_accelred = dlsym(lib_handle, "mbs_accelred");
mbs_data->fct.symb.mbs_dirdyna = dlsym(lib_handle, "mbs_dirdyna");
mbs_data->fct.symb.mbs_cons_hJ = dlsym(lib_handle, "mbs_cons_hJ");
mbs_data->fct.symb.mbs_cons_jdqd = dlsym(lib_handle, "mbs_cons_jdqd");
if ((error = dlerror()) != NULL)
{
......
......@@ -5,8 +5,8 @@
MbsData* mbs_load(const char* mbs_filename){
MDS_gen_strct *mds;
MbsData *mbs_data;
MbsData *mbs_data;
mds = MDS_mbs_reader(mbs_filename);
mbs_data = MDS_create_MBSdataStruct(mds);
free_MDS_gen_strct(mds);
......@@ -15,7 +15,7 @@ MbsData* mbs_load(const char* mbs_filename){
strcpy(mbs_data->mbs_filename, mbs_filename);
return mbs_data;
return mbs_data;
}
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
......@@ -25,8 +25,8 @@ MbsData* mbs_load(const char* mbs_filename){
int* q_MBSdataStruct;
if(nq)
{
int i;
int i;
q_MBSdataStruct = (int*) calloc(nq+1,sizeof(int));
q_MBSdataStruct[0] = nq;
for(i=0; i<nq; i++)
......@@ -68,8 +68,8 @@ MbsData* MDS_create_MBSdataStruct(MDS_gen_strct* mds_gen_strct)
}else{
s->Nloopc = mds_gen_strct->cuts->n_rod + 3 * mds_gen_strct->cuts->n_ball + 6 * mds_gen_strct->cuts->n_solid;
}
s->Nuserc = 0;
s->Nuserc = 0;
s->Ncons = s->Nloopc + s->Nuserc;
//s->nhu = 8; // caution WEMOOV dependent !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
......@@ -83,7 +83,7 @@ MbsData* MDS_create_MBSdataStruct(MDS_gen_strct* mds_gen_strct)
s->Nlink = 0;
s->Nlink3D = 0;
}
s->Nsensor = mds_gen_strct->n_sensor;
s->Nxfrc = mds_gen_strct->n_extforce;
......@@ -111,12 +111,12 @@ MbsData* MDS_create_MBSdataStruct(MDS_gen_strct* mds_gen_strct)
{
for(j=0; j<3; j++)
{
s->dpt[j+1][i+1] = mds_gen_strct->point_list[i]->pt[j];
s->dpt[j+1][i+1] = mds_gen_strct->point_list[i]->pt[j];
mds_gen_strct->point_list[i]->d_pt[j]->MBSdata_d_ptr = &(s->dpt[j+1][i+1]);
}
}
}
else
else
for(i=1;i<=3;i++)
s->dpt[i] = NULL;
......@@ -130,7 +130,7 @@ MbsData* MDS_create_MBSdataStruct(MDS_gen_strct* mds_gen_strct)
s->l[i] = (double*) calloc(s->nbody+1,sizeof(double));
s->l[i][0] = (double) s->nbody;
}
s->m = (double*) calloc(s->nbody+1,sizeof(double));
s->m[0] = (double) s->nbody;
......@@ -146,23 +146,23 @@ MbsData* MDS_create_MBSdataStruct(MDS_gen_strct* mds_gen_strct)
ind_joint = ind_joint + mds_gen_strct->bodytree->body_list[i]->n_joint;
for(j=0; j<3; j++)
{
s->l[j+1][ind_joint] = mds_gen_strct->bodytree->body_list[i]->com[j];
s->l[j+1][ind_joint] = mds_gen_strct->bodytree->body_list[i]->com[j];
}
s->m[ind_joint] = mds_gen_strct->bodytree->body_list[i]->mass;
s->In[1][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[0];
s->In[2][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[1];
s->In[3][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[2];
s->In[4][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[1];
s->In[3][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[2];
s->In[4][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[1];
s->In[5][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[3];
s->In[6][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[4];
s->In[7][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[2];
s->In[6][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[4];
s->In[7][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[2];
s->In[8][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[4];
s->In[9][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[5];
s->In[9][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[5];
}
// g
// g
s->g[0] = 0.0;
copy_double_vec(mds_gen_strct->base->gravity, &(s->g[1]), 3);
......@@ -175,9 +175,9 @@ MbsData* MDS_create_MBSdataStruct(MDS_gen_strct* mds_gen_strct)
s->qdriven = MDS_translate_q(mds_gen_strct->bodytree->qdriven, mds_gen_strct->bodytree->n_qdriven);
s->qa = MDS_translate_q(mds_gen_strct->bodytree->qa, mds_gen_strct->bodytree->n_qa);
if (s->nhu) // to change
if (s->nhu) // to change
{
s->hu = (int*) calloc(s->nhu+1,sizeof(int));
s->hu[0] = s->nhu;
......@@ -220,7 +220,7 @@ MbsData* MDS_create_MBSdataStruct(MDS_gen_strct* mds_gen_strct)
s->qmax = NULL;
// Frc, Trq, Qq, tsim
// frc
s->frc[0] = NULL;
for(i=1;i<=3;i++)
......@@ -258,7 +258,7 @@ MbsData* MDS_create_MBSdataStruct(MDS_gen_strct* mds_gen_strct)
// 1 to stop the simulation, 0 otherwise
s->flag_stop = 0;
// Constraint data
// Constraint data
// lrod
if (mds_gen_strct->cuts!=NULL && mds_gen_strct->cuts->n_rod)
......@@ -279,7 +279,7 @@ MbsData* MDS_create_MBSdataStruct(MDS_gen_strct* mds_gen_strct)
// Link Data
// Z, Zd, Fl
// Z, Zd, Fl
if (s->Nlink)
{
s->Z = (double*) calloc(s->Nlink+1,sizeof(double));
......@@ -302,7 +302,7 @@ MbsData* MDS_create_MBSdataStruct(MDS_gen_strct* mds_gen_strct)
s->Fl = NULL;
}
// l3DWr
// l3DWr
if (s->Nlink3D)
{
s->l3DWr = (double**) calloc(s->Nlink3D+1,sizeof(double*));
......@@ -317,9 +317,9 @@ MbsData* MDS_create_MBSdataStruct(MDS_gen_strct* mds_gen_strct)
else
s->l3DWr = NULL;
// Ext. Forces Data
// Ext. Forces Data
// xfidpt
// xfidpt
if (s->Nxfrc)
{
s->xfidpt = (int*) calloc(s->Nxfrc+1,sizeof(int));
......@@ -347,7 +347,7 @@ MbsData* MDS_create_MBSdataStruct(MDS_gen_strct* mds_gen_strct)
else
s->SWr = NULL;
// Wheel Data
// Wheel Data
// rnom
if (s->Nwheel)
......@@ -365,8 +365,8 @@ MbsData* MDS_create_MBSdataStruct(MDS_gen_strct* mds_gen_strct)
// user_model
if (s->Nuser_model)
{
s->user_model = s->mbs_new_user_model();
s->mbs_load_user_model_xml(mds_gen_strct, s->user_model);
s->user_model = s->fct.user.mbs_new_user_model();
s->fct.user.mbs_load_user_model_xml(mds_gen_strct, s->user_model);
}
else
{
......@@ -382,7 +382,7 @@ MbsData* MDS_create_MBSdataStruct(MDS_gen_strct* mds_gen_strct)
s->ux[0] = (double) s->Nux;
s->uxd[0] = (double) s->Nux;
s->ux0[0] = (double) s->Nux;
for(i=0; i<mds_gen_strct->n_state; i++)
{
for(j=0; j<mds_gen_strct->state_list[i]->n_value; j++)
......@@ -400,7 +400,7 @@ MbsData* MDS_create_MBSdataStruct(MDS_gen_strct* mds_gen_strct)
s->ux0 = NULL;
}
s->user_IO = s->mbs_new_user_IO(s);
s->user_IO = s->fct.user.mbs_new_user_IO(s);
#endif
......@@ -427,7 +427,7 @@ MbsData* MDS_create_MBSdataStruct(MDS_gen_strct* mds_gen_strct)
}
//////////////////////////////////////////////////////////////////////////// CAUTION CAUTION
//////////////////////////////////////////////////////////////////////////// CAUTION CAUTION
void mbs_delete_data(MbsData *s)
......@@ -518,16 +518,16 @@ void mbs_delete_data(MbsData *s)
{
free(s->ux);
free(s->uxd);
free(s->ux0);
free(s->ux0);
}
#ifndef SENSORKIN
// User models
if (s->Nuser_model)
{
s->mbs_delete_user_model(s->user_model);
s->fct.user.mbs_delete_user_model(s->user_model);
}
s->mbs_delete_user_IO(s->user_IO);
s->fct.user.mbs_delete_user_IO(s->user_IO);
#endif
// Other
......@@ -536,7 +536,7 @@ void mbs_delete_data(MbsData *s)
free(s->udd);
}
if (s->nhu) // to change
if (s->nhu) // to change
{
free(s->hu);
}
......
......@@ -22,12 +22,12 @@ void mbs_calc_force(MbsData* s){
}
}
if(s->Nlink > 0) s->mbs_link(s->frc,s->trq,s->Fl,s->Z,s->Zd,s,s->tsim);
if(s->Nlink > 0) s->fct.symb.mbs_link(s->frc,s->trq,s->Fl,s->Z,s->Zd,s,s->tsim);
if(s->Nlink3D > 0) s->mbs_link3D(s->frc,s->trq,s,s->tsim);
if(s->Nlink3D > 0) s->fct.symb.mbs_link3D(s->frc,s->trq,s,s->tsim);
if(s->Nxfrc > 0) s->mbs_extforces(s->frc,s->trq,s,s->tsim);
if(s->Nxfrc > 0) s->fct.symb.mbs_extforces(s->frc,s->trq,s,s->tsim);
s->Qq = s->user_JointForces(s,s->tsim);
s->Qq = s->fct.user.user_JointForces(s,s->tsim);
}
......@@ -53,7 +53,7 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
if(mbs_aux->norm_h > mbs_aux->NRerr)
{
// err
// err
for(i=1;i<=s->nhu;i++)
{
mbs_aux->mJv_h[i] = mbs_aux->h[s->hu[i]];
......@@ -74,7 +74,7 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
}
void mbs_close_kin(MbsData *s, MbsAux *mbs_aux)
{
{
int i,j,k;
int nL,nC,nk;
......@@ -127,11 +127,11 @@ void mbs_calc_hJ(MbsData *s, MbsAux *mbs_aux)
// contraintes de fermeture de boucles
s->mbs_cons_hJ(mbs_aux->h,mbs_aux->Jac,s,s->tsim);
s->fct.symb.mbs_cons_hJ(mbs_aux->h,mbs_aux->Jac,s,s->tsim);
// contraintes user
if (s->Nuserc>0) {
s->user_cons_hJ(mbs_aux->huserc, mbs_aux->Juserc, s, s->tsim);
s->fct.user.user_cons_hJ(mbs_aux->huserc, mbs_aux->Juserc, s, s->tsim);
// ajout des contraintes user aux contraintes de fermeture
for (i=1;i<=s->Nuserc;i++) {
......@@ -145,25 +145,25 @@ void mbs_calc_hJ(MbsData *s, MbsAux *mbs_aux)
void mbs_calc_jdqd(MbsData *s, MbsAux *mbs_aux)
{
int i;
// compute contribution of symbolic constraints to jdqd
s->mbs_cons_jdqd(mbs_aux->jdqd,s,s->tsim);
s->fct.symb.mbs_cons_jdqd(mbs_aux->jdqd,s,s->tsim);
// compute contribution of user constraints to jdqd
if (s->Nuserc>0) {
s->user_cons_jdqd(mbs_aux->jdqduserc, s, s->tsim);
s->fct.user.user_cons_jdqd(mbs_aux->jdqduserc, s, s->tsim);
// ajout des contraintes user aux contraintes de fermeture
for (i=1;i<=s->Nuserc;i++) {
mbs_aux->jdqd[s->Nloopc+i] = mbs_aux->jdqduserc[i];
}
}
// bp = (-Jv)\jdqd
for(i=1;i<=s->nhu;i++)
{
mbs_aux->bp[i] = mbs_aux->jdqd[s->hu[i]];
}
lubksb(mbs_aux->mJv,s->nqv,mbs_aux->ind_mJv,mbs_aux->bp);
}
......@@ -29,10 +29,10 @@
MbsDirdyn* mbs_new_dirdyn(MbsData* mbs_data)
{
MbsAux* mbs_aux;
// Initialize the local data struct
mbs_aux = initMbsAux(mbs_data);
return mbs_new_dirdyn_aux(mbs_data, mbs_aux);
}
......@@ -59,10 +59,10 @@ MbsDirdyn* mbs_new_dirdyn_aux(MbsData* mbs_data, MbsAux* mbs_aux)
opts->max_save_user = 12;
opts->buffersize = -1;
opts->resfilename = NULL;
opts->respath = NULL;
opts->respath = NULL;
opts->realtime = 0;
opts->accelred = 0;
// adaptive time step integrator
dopr_opt = (MbsDirdynOptDopri5*) malloc(sizeof(MbsDirdynOptDopri5));
dopr_opt->flag_use = 0;
......@@ -113,22 +113,22 @@ void mbs_run_dirdyn(MbsDirdyn* dd, MbsData* mbs_data)
FILE* animFile;
// 1. Initialize the simulation
// - - - - - - - - - - - - - -
// - - - - - - - - - - - - - -
mbs_dirdyn_init(dd, mbs_data);
// 2. Run the simulation
// - - - - - - - - - - -
// - - - - - - - - - - -
mbs_dirdyn_loop(dd, mbs_data);
// 3. Finish the simulation
// - - - - - - - - - - - -
// - - - - - - - - - - - -
mbs_dirdyn_finish(dd, mbs_data);
}
void mbs_dirdyn_save(MbsDirdyn* dd, double t){
int i;
for (i=0 ; i < dd->bufferNb ; i++){
mbs_buffer_save(dd->buffers[i], t, dd->savedArrays[i]);
......@@ -139,8 +139,8 @@ void mbs_dirdyn_save(MbsDirdyn* dd, double t){
void mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data)
{
int i;
int i;
// INITIALIZATION
// - - - - - - - -
......@@ -196,7 +196,7 @@ void mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data)
dd->dt = dd->options->dt0;
// user intialization
mbs_data->user_init(mbs_data, dd);
mbs_data->fct.user.user_init(mbs_data, dd);
// Simulation state initialization
for(i=1; i<=mbs_data->nqu; i++)
......@@ -215,7 +215,7 @@ void mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data)
char* f;
const char * fnameSuffix[] = {"q", "qd", "qdd", "Qq", "ux", "uxd"};
int bufElemNb[] = {mbs_data->njoint, mbs_data->njoint, mbs_data->njoint, mbs_data->njoint, mbs_data->Nux, mbs_data->Nux};
njoint = mbs_data->njoint;
Nux = mbs_data->Nux;
......@@ -226,36 +226,36 @@ void mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data)
else{
resfilename = dd->options->resfilename;
}
// set the path to result files if not specified
if(dd->options->respath == NULL){
respath = "../../resultsR";
}
else{
respath = dd->options->respath;
}
}
// set the buffer size if not specified
if (dd->options->buffersize<1){
// compute size assuming a constant time step size
dd->options->buffersize = (int)(dd->options->tf - dd->options->t0)/
(dd->dt * dd->options->saveperiod)
+ 1;
+ 1;
// add some extra space
dd->options->buffersize *= 1.1;
dd->options->buffersize += 10;
}
// initialize the buffers for saving results
dd->bufferNb=4;
// add 2 buffers for user state
dd->bufferNb += Nux?2:0;
//printf("Buffer nb = %d \n", dd->bufferNb);
dd->buffers = (MbsBuffer**)malloc(dd->bufferNb*sizeof(MbsBuffer*));
f = (char*)malloc(sizeof(char)*(strlen(respath)+strlen(resfilename)+10));
for(bufId=0 ; bufId < dd->bufferNb; bufId++){
sprintf(f, "%s/%s_%s.res", respath, resfilename, fnameSuffix[bufId]);
......@@ -340,7 +340,7 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data)
{
// user loop
mbs_data->user_loop(mbs_data, dd);
mbs_data->fct.user.user_loop(mbs_data, dd);
mbs_fct_dirdyn(dd->tsim, dd->y, dd->yd, mbs_data, dd);
rk4(dd->y, dd->yd, dd->nState, dd->tsim, dd->dt, dd->yout, mbs_fct_dirdyn, mbs_data, dd);
......@@ -360,7 +360,7 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data)
{
break;
}
// printf("DIRDYN: current time: %f \n", dd->tsim);
}
}
......@@ -368,7 +368,7 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data)
void mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data){
int i;
// ENDING SIMULATION
// - - - - - - - -
......@@ -381,7 +381,7 @@ void mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data){
#endif
// user finalization
mbs_data->user_finish(mbs_data, dd);
mbs_data->fct.user.user_finish(mbs_data, dd);
// release memory allocated for dopri5
if (dd->options->dopri5->flag_use)
......@@ -404,12 +404,12 @@ void mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data){
free(dd->y);
free(dd->yd);
free(dd->yout);
if(dd->options->save2file){
// write the buffer (in case they were not saved automatically)
for (i=0 ; i < dd->bufferNb ; i++){
mbs_buffer_write(dd->buffers[i]);
}
}
// free memory used by buffers
for(i=0 ; i < dd->bufferNb; i++){
......@@ -432,7 +432,7 @@ void mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data){
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */