...
 
Commits (2)
......@@ -34,6 +34,7 @@ MbsAux * initMbsAux(MbsData *s)
nqv = s->nqv;
nqc = s->nqc;
#ifdef INVDYNARED
nqa = s->nqa;
#endif
......@@ -46,6 +47,14 @@ MbsAux * initMbsAux(MbsData *s)
mbs_aux->NRerr = s->NRerr;
mbs_aux->MAX_NR_ITER = 100;
// Added for the online coordinate partitioning
mbs_aux->use_det = 0;
mbs_aux->permu = 0;
mbs_aux->iter_part = 5;
mbs_aux->Jv_verbose = 0;
mbs_aux->online_part = 0;
mbs_aux->min_det = 1e-3;
if (Ncons>0)
{
mbs_aux->h = get_dvec_1(Ncons);
......@@ -97,14 +106,13 @@ MbsAux * initMbsAux(MbsData *s)
if (nquc)
{
mbs_aux->iquc = get_ivec_1(nquc);
mbs_aux->iquc[0]=nquc;
for(i=1;i<=nqu;i++)
mbs_aux->iquc[i] = s->qu[i];
for(i=1;i<=nqc;i++)
mbs_aux->iquc[i+nqu] = s->qc[i];
}
else
mbs_aux->iquc = NULL;
mbs_aux->iquc = get_ivec_1(nquc);
allocate_sensor(mbs_aux->psens, njoint);
init_sensor(mbs_aux->psens, njoint);
......
......@@ -25,6 +25,17 @@
int i;
double xh, hh, h6, *dym, *dyt, *yt;
//----------------- Modif Alex Start -----------------
double *q_save, *qd_save;
q_save = get_dvec_1(s->njoint);
qd_save = get_dvec_1(s->njoint);
copy_dvec_1(s->q, q_save);
copy_dvec_1(s->qd, qd_save);
//----------------- Modif Alex End -----------------
dym = &((MbsDirdynRK4 *)dd->integrator_struct)->dym[0];
dyt = &((MbsDirdynRK4 *)dd->integrator_struct)->dyt[0];
yt = &((MbsDirdynRK4 *)dd->integrator_struct)->yt[0];
......@@ -40,11 +51,39 @@
// Second step
(*derivs)(xh, yt, dyt, s, dd);
//------------ Modif Alex Start --------------
if (dd->mbs_aux->permu) {
copy_dvec_1(q_save, s->q);
copy_dvec_1(qd_save, s->qd);
free_dvec_1(q_save);
free_dvec_1(qd_save);
return;
}
//------------ Modif Alex End --------------
for (i = 0; i < n; i++)
yt[i] = y[i] + hh*dyt[i];
// Third step
(*derivs)(xh, yt, dym, s, dd);
//------------ Modif Alex Start --------------
if (dd->mbs_aux->permu) {
copy_dvec_1(q_save, s->q);
copy_dvec_1(qd_save, s->qd);
free_dvec_1(q_save);
free_dvec_1(qd_save);
return;
}
//------------ Modif Alex End --------------
for (i = 0; i < n; i++)
{
yt[i] = y[i] + h*dym[i];
......@@ -54,7 +93,22 @@
// Fourth step
(*derivs)(x + h, yt, dyt, s, dd);
//------------ Modif Alex Start --------------
if (dd->mbs_aux->permu) {
copy_dvec_1(q_save, s->q);
copy_dvec_1(qd_save, s->qd);
free_dvec_1(q_save);
free_dvec_1(qd_save);
return;
}
//------------ Modif Alex End --------------
// Accumulate increments with proper weights
for (i = 0; i < n; i++)
yout[i] = y[i] + h6*(dydx[i] + dyt[i] + 2.0*dym[i]);
free_dvec_1(q_save);
free_dvec_1(qd_save);
}