Commit 707dc4fd authored by Olivier Lantsoght's avatar Olivier Lantsoght 🏁

Merge branch 'fix_nstate_integrators' into 'dev'

taking nstate = 0 into account in integrators (no independant joints) see issue #159

Closes #159

See merge request !257
parents b719d579 c1e29643
......@@ -27,31 +27,33 @@ void initialize_bader(MbsData *mbs_data, MbsDirdyn *mbs_dd) {
mbs_dd->integrator_struct = malloc(sizeof(MbsDirdynBader));
int nv = mbs_dd->nState;
((MbsDirdynBader *)mbs_dd->integrator_struct)->d = get_dmat_1(nv, KMAXX);
((MbsDirdynBader *)mbs_dd->integrator_struct)->dfdx = get_dvec_0(nv);
((MbsDirdynBader *)mbs_dd->integrator_struct)->dfdy = get_dmat_0(nv, nv);
((MbsDirdynBader *)mbs_dd->integrator_struct)->err = get_dvec_0(KMAXX);
((MbsDirdynBader *)mbs_dd->integrator_struct)->x = get_dvec_1(KMAXX);
((MbsDirdynBader *)mbs_dd->integrator_struct)->yerr = get_dvec_0(nv);
((MbsDirdynBader *)mbs_dd->integrator_struct)->ysav = get_dvec_0(nv);
((MbsDirdynBader *)mbs_dd->integrator_struct)->yseq = get_dvec_0(nv);
/* To freeze jacobian */
((MbsDirdynBader *)mbs_dd->integrator_struct)->Freeze_dydx = get_dvec_0(mbs_dd->nState);
((MbsDirdynBader *)mbs_dd->integrator_struct)->Freeze_dfdx = get_dvec_0(mbs_dd->nState);
((MbsDirdynBader *)mbs_dd->integrator_struct)->Freeze_dfdy = get_dmat_0(mbs_dd->nState, mbs_dd->nState);
((MbsDirdynBader *)mbs_dd->integrator_struct)->Freeze_jacobian_index = 0;
/* Pointors specific to simpr */
((MbsDirdynBader *)mbs_dd->integrator_struct)->indx = get_ivec_0(mbs_dd->nState);
((MbsDirdynBader *)mbs_dd->integrator_struct)->a = get_dmat_0(mbs_dd->nState, mbs_dd->nState);
((MbsDirdynBader *)mbs_dd->integrator_struct)->del = get_dvec_0(mbs_dd->nState);
((MbsDirdynBader *)mbs_dd->integrator_struct)->ytemp = get_dvec_0(mbs_dd->nState);
/* Pointors specific to pzextr and rzextr */
((MbsDirdynBader *)mbs_dd->integrator_struct)->c = get_dvec_0(mbs_dd->nState);
((MbsDirdynBader *)mbs_dd->integrator_struct)->fx = get_dvec_1(KMAXX);
if (nv != 0)
{
((MbsDirdynBader *)mbs_dd->integrator_struct)->d = get_dmat_1(nv, KMAXX);
((MbsDirdynBader *)mbs_dd->integrator_struct)->dfdx = get_dvec_0(nv);
((MbsDirdynBader *)mbs_dd->integrator_struct)->dfdy = get_dmat_0(nv, nv);
((MbsDirdynBader *)mbs_dd->integrator_struct)->err = get_dvec_0(KMAXX);
((MbsDirdynBader *)mbs_dd->integrator_struct)->x = get_dvec_1(KMAXX);
((MbsDirdynBader *)mbs_dd->integrator_struct)->yerr = get_dvec_0(nv);
((MbsDirdynBader *)mbs_dd->integrator_struct)->ysav = get_dvec_0(nv);
((MbsDirdynBader *)mbs_dd->integrator_struct)->yseq = get_dvec_0(nv);
/* To freeze jacobian */
((MbsDirdynBader *)mbs_dd->integrator_struct)->Freeze_dydx = get_dvec_0(mbs_dd->nState);
((MbsDirdynBader *)mbs_dd->integrator_struct)->Freeze_dfdx = get_dvec_0(mbs_dd->nState);
((MbsDirdynBader *)mbs_dd->integrator_struct)->Freeze_dfdy = get_dmat_0(mbs_dd->nState, mbs_dd->nState);
((MbsDirdynBader *)mbs_dd->integrator_struct)->Freeze_jacobian_index = 0;
/* Pointors specific to simpr */
((MbsDirdynBader *)mbs_dd->integrator_struct)->indx = get_ivec_0(mbs_dd->nState);
((MbsDirdynBader *)mbs_dd->integrator_struct)->a = get_dmat_0(mbs_dd->nState, mbs_dd->nState);
((MbsDirdynBader *)mbs_dd->integrator_struct)->del = get_dvec_0(mbs_dd->nState);
((MbsDirdynBader *)mbs_dd->integrator_struct)->ytemp = get_dvec_0(mbs_dd->nState);
/* Pointors specific to pzextr and rzextr */
((MbsDirdynBader *)mbs_dd->integrator_struct)->c = get_dvec_0(mbs_dd->nState);
((MbsDirdynBader *)mbs_dd->integrator_struct)->fx = get_dvec_1(KMAXX);
}
mbs_warning_msg("The Bader semi-implicit integrator has not been test extensively ! \n ");
}
......@@ -62,7 +64,7 @@ void loop_bader(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
yscal = get_dvec_0(mbs_dd->nState);
double hdid, hnext;
double hdid=0.0, hnext=0.0;
double dt0 = mbs_dd->options->dt0;
double h_cur = dt0;
double t = t0;
......@@ -87,8 +89,15 @@ void loop_bader(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
yscal[i] = (1.0 > mbs_dd->y[i]) ? 1.0 : mbs_dd->y[i];
}
error_code = bader(mbs_dd->y, mbs_dd->yd, mbs_dd->nState, &t, h_cur, mbs_dd->options->toler, yscal, &hdid, &hnext, h_max, mbs_fct_dirdyn, mbs_data, mbs_dd);
if (mbs_dd->nState != 0)
{
error_code = bader(mbs_dd->y, mbs_dd->yd, mbs_dd->nState, &t, h_cur, mbs_dd->options->toler, yscal, &hdid, &hnext, h_max, mbs_fct_dirdyn, mbs_data, mbs_dd);
}
else
{
t += dt0;
}
if (error_code < 0) // Error management
{
mbs_msg(">>DIRDYN>>\n");
......@@ -115,26 +124,28 @@ void loop_bader(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
}
void finish_bader(MbsData *mbs_data, MbsDirdyn *dd) {
free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->yseq);
free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->ysav);
free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->yerr);
free_dvec_1(((MbsDirdynBader *)dd->integrator_struct)->x);
free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->err);
free_dmat_0(((MbsDirdynBader *)dd->integrator_struct)->dfdy);
free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->dfdx);
free_dmat_1(((MbsDirdynBader *)dd->integrator_struct)->d);
free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->Freeze_dydx);
free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->Freeze_dfdx);
free_dmat_0(((MbsDirdynBader *)dd->integrator_struct)->Freeze_dfdy);
free_ivec_0(((MbsDirdynBader *)dd->integrator_struct)->indx);
free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->ytemp);
free_dmat_0(((MbsDirdynBader *)dd->integrator_struct)->a);
free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->del);
free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->c);
free_dvec_1(((MbsDirdynBader *)dd->integrator_struct)->fx);
if (dd->nState != 0)
{
free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->yseq);
free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->ysav);
free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->yerr);
free_dvec_1(((MbsDirdynBader *)dd->integrator_struct)->x);
free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->err);
free_dmat_0(((MbsDirdynBader *)dd->integrator_struct)->dfdy);
free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->dfdx);
free_dmat_1(((MbsDirdynBader *)dd->integrator_struct)->d);
free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->Freeze_dydx);
free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->Freeze_dfdx);
free_dmat_0(((MbsDirdynBader *)dd->integrator_struct)->Freeze_dfdy);
free_ivec_0(((MbsDirdynBader *)dd->integrator_struct)->indx);
free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->ytemp);
free_dmat_0(((MbsDirdynBader *)dd->integrator_struct)->a);
free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->del);
free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->c);
free_dvec_1(((MbsDirdynBader *)dd->integrator_struct)->fx);
}
}
......@@ -25,9 +25,10 @@ void initialize_custom(MbsData *mbs_data, MbsDirdyn *mbs_dd) {
// Allocate Custom structure
mbs_dd->integrator_struct = malloc(sizeof(MbsDirdynCustom));
((MbsDirdynCustom *)mbs_dd->integrator_struct)->yt2 = get_dvec_0(mbs_dd->nState);
if (mbs_dd->nState != 0)
{
((MbsDirdynCustom *)mbs_dd->integrator_struct)->yt2 = get_dvec_0(mbs_dd->nState);
}
mbs_warning_msg("Using our own custom integrator to implement ! \n ");
}
......@@ -62,7 +63,7 @@ void loop_custom(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
// ###### end of custom unknown integrator #########
mbs_dd->tsim += h;
t = mbs_dd->tsim;
for (i = 0; i < mbs_dd->nState; i++)
{
mbs_dd->y[i] = mbs_dd->yout[i];
......@@ -81,5 +82,8 @@ void loop_custom(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
// Free the elements in the structure of custom integrator
// (See the allocations in initialize_custom() )
void finish_custom(MbsData *mbs_data, MbsDirdyn *mbs_dd) {
free(((MbsDirdynCustom *)mbs_dd->integrator_struct)->yt2);
if (mbs_dd->nState != 0)
{
free(((MbsDirdynCustom *)mbs_dd->integrator_struct)->yt2);
}
}
......@@ -25,26 +25,31 @@ void initialize_dopri5(MbsData *mbs_data, MbsDirdyn *mbs_dd) {
// Allocate Dopri5 structure
mbs_dd->integrator_struct = malloc(sizeof(MbsDirdynDopri5));
((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab = (double**)malloc(DOPRI5_NB_ALLOC_VEC*sizeof(double*));
for (i = 0; i < DOPRI5_NB_ALLOC_VEC; i++)
if (mbs_dd->nState != 0)
{
((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab[i] = get_dvec_0(mbs_dd->nState);
}
((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab = (double**)malloc(DOPRI5_NB_ALLOC_VEC * sizeof(double*));
((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->dydt_save = get_dvec_0(mbs_dd->nState);
((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->solout_last_t = mbs_data->t0 - mbs_data->dt0;
((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->flag_save = 0;
for (i = 0; i < DOPRI5_NB_ALLOC_VEC; i++)
{
((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab[i] = get_dvec_0(mbs_dd->nState);
}
((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->dydt_save = get_dvec_0(mbs_dd->nState);
((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->solout_last_t = mbs_data->t0 - mbs_data->dt0;
((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->flag_save = 0;
}
}
void loop_dopri5(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
double hout;
int err = 0;
err = dopri5(mbs_dd->nState, fcn_dopri5, t0, mbs_dd->y, tf, &(mbs_dd->options->rtoler), &(mbs_dd->options->atoler),
0, solout_dopri5, 1, 0.0, 0.0, 0.0, 0.0, 0.0, mbs_dd->options->dt_max, mbs_dd->options->dt0, mbs_dd->options->nmax,
1, 0, 0, NULL, 0, ((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab, mbs_data, mbs_dd, &hout);
if (mbs_dd->nState != 0)
{
err = dopri5(mbs_dd->nState, fcn_dopri5, t0, mbs_dd->y, tf, &(mbs_dd->options->rtoler), &(mbs_dd->options->atoler),
0, solout_dopri5, 1, 0.0, 0.0, 0.0, 0.0, 0.0, mbs_dd->options->dt_max, mbs_dd->options->dt0, mbs_dd->options->nmax,
1, 0, 0, NULL, 0, ((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab, mbs_data, mbs_dd, &hout);
}
if (err < 0) // Error management
{
......@@ -59,14 +64,16 @@ void loop_dopri5(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
void finish_dopri5(MbsData *mbs_data, MbsDirdyn *mbs_dd) {
int i = 0;
free(((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->dydt_save);
for (i = 0; i < DOPRI5_NB_ALLOC_VEC; i++)
if (mbs_dd->nState != 0)
{
free(((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab[i]);
free(((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->dydt_save);
for (i = 0; i < DOPRI5_NB_ALLOC_VEC; i++)
{
free(((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab[i]);
}
free(((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab);
}
free(((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab);
}
......
......@@ -26,19 +26,21 @@ void initialize_euler_implicit(MbsData *mbs_data, MbsDirdyn *mbs_dd) {
mbs_dd->integrator_struct = malloc(sizeof(MbsDirdynEulerIm));
int n = mbs_dd->nState;
((MbsDirdynEulerIm *)mbs_dd->integrator_struct)->indx = get_ivec_0(n);
((MbsDirdynEulerIm *)mbs_dd->integrator_struct)->dfdx = get_dvec_0(n);
((MbsDirdynEulerIm *)mbs_dd->integrator_struct)->dfdy = get_dmat_0(n, n);
((MbsDirdynEulerIm *)mbs_dd->integrator_struct)->ysav = get_dvec_0(n);
((MbsDirdynEulerIm *)mbs_dd->integrator_struct)->dysav = get_dvec_0(n);
((MbsDirdynEulerIm *)mbs_dd->integrator_struct)->Fsav = get_dvec_1(mbs_data->nqu);
((MbsDirdynEulerIm *)mbs_dd->integrator_struct)->Msav = get_dmat_1(mbs_data->nqu, mbs_data->nqu);
/* to freeze jacobian */
((MbsDirdynEulerIm *)mbs_dd->integrator_struct)->Freeze_dydx = get_dvec_0(mbs_dd->nState);
((MbsDirdynEulerIm *)mbs_dd->integrator_struct)->Freeze_dfdx = get_dvec_0(mbs_dd->nState);
((MbsDirdynEulerIm *)mbs_dd->integrator_struct)->Freeze_dfdy = get_dmat_0(mbs_dd->nState, mbs_dd->nState);
if (n != 0)
{
((MbsDirdynEulerIm *)mbs_dd->integrator_struct)->indx = get_ivec_0(n);
((MbsDirdynEulerIm *)mbs_dd->integrator_struct)->dfdx = get_dvec_0(n);
((MbsDirdynEulerIm *)mbs_dd->integrator_struct)->dfdy = get_dmat_0(n, n);
((MbsDirdynEulerIm *)mbs_dd->integrator_struct)->ysav = get_dvec_0(n);
((MbsDirdynEulerIm *)mbs_dd->integrator_struct)->dysav = get_dvec_0(n);
((MbsDirdynEulerIm *)mbs_dd->integrator_struct)->Fsav = get_dvec_1(mbs_data->nqu);
((MbsDirdynEulerIm *)mbs_dd->integrator_struct)->Msav = get_dmat_1(mbs_data->nqu, mbs_data->nqu);
/* to freeze jacobian */
((MbsDirdynEulerIm *)mbs_dd->integrator_struct)->Freeze_dydx = get_dvec_0(mbs_dd->nState);
((MbsDirdynEulerIm *)mbs_dd->integrator_struct)->Freeze_dfdx = get_dvec_0(mbs_dd->nState);
((MbsDirdynEulerIm *)mbs_dd->integrator_struct)->Freeze_dfdy = get_dmat_0(mbs_dd->nState, mbs_dd->nState);
}
((MbsDirdynEulerIm *)mbs_dd->integrator_struct)->Freeze_jacobian_index = 0;
mbs_warning_msg("The Linear Implicit Euler integrator is still in development and may not work for specific problems ! \n ");
......@@ -65,9 +67,12 @@ void loop_euler_implicit(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs
{
mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
}
// Call the Euler Implicit method
euler_implicit(mbs_dd->y, mbs_dd->yd, mbs_dd->nState, &t, h, mbs_dd->yout, mbs_fct_dirdyn, mbs_data, mbs_dd);
if (mbs_dd->nState != 0)
{
// Call the Euler Implicit method
euler_implicit(mbs_dd->y, mbs_dd->yd, mbs_dd->nState, &t, h, mbs_dd->yout, mbs_fct_dirdyn, mbs_data, mbs_dd);
}
mbs_dd->tsim += h;
mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
......@@ -80,16 +85,18 @@ void loop_euler_implicit(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs
}
void finish_euler_implicit(MbsData *mbs_data, MbsDirdyn *dd) {
free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->dfdx);
free_dmat_0(((MbsDirdynEulerIm *)dd->integrator_struct)->dfdy);
free_ivec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->indx);
free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->ysav);
free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->dysav);
free_dvec_1(((MbsDirdynEulerIm *)dd->integrator_struct)->Fsav);
free_dmat_1(((MbsDirdynEulerIm *)dd->integrator_struct)->Msav);
free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->Freeze_dydx);
free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->Freeze_dfdx);
free_dmat_0(((MbsDirdynEulerIm *)dd->integrator_struct)->Freeze_dfdy);
if (dd->nState != 0)
{
free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->dfdx);
free_dmat_0(((MbsDirdynEulerIm *)dd->integrator_struct)->dfdy);
free_ivec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->indx);
free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->ysav);
free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->dysav);
free_dvec_1(((MbsDirdynEulerIm *)dd->integrator_struct)->Fsav);
free_dmat_1(((MbsDirdynEulerIm *)dd->integrator_struct)->Msav);
free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->Freeze_dydx);
free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->Freeze_dfdx);
free_dmat_0(((MbsDirdynEulerIm *)dd->integrator_struct)->Freeze_dfdy);
}
}
......@@ -22,10 +22,12 @@ void initialize_rk4(MbsData *mbs_data, MbsDirdyn *mbs_dd) {
// Allocate RK4 structure
mbs_dd->integrator_struct = malloc(sizeof(MbsDirdynRK4));
((MbsDirdynRK4 *)mbs_dd->integrator_struct)->yt = get_dvec_0(mbs_dd->nState);
((MbsDirdynRK4 *)mbs_dd->integrator_struct)->dyt = get_dvec_0(mbs_dd->nState);
((MbsDirdynRK4 *)mbs_dd->integrator_struct)->dym = get_dvec_0(mbs_dd->nState);
if (mbs_dd->nState != 0)
{
((MbsDirdynRK4 *)mbs_dd->integrator_struct)->yt = get_dvec_0(mbs_dd->nState);
((MbsDirdynRK4 *)mbs_dd->integrator_struct)->dyt = get_dvec_0(mbs_dd->nState);
((MbsDirdynRK4 *)mbs_dd->integrator_struct)->dym = get_dvec_0(mbs_dd->nState);
}
}
void loop_rk4(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
......@@ -49,9 +51,10 @@ void loop_rk4(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
{
mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
}
rk4(mbs_dd->y, mbs_dd->yd, mbs_dd->nState, mbs_dd->tsim, h, mbs_dd->yout, mbs_fct_dirdyn, mbs_data, mbs_dd);
if (mbs_dd->nState != 0)
{
rk4(mbs_dd->y, mbs_dd->yd, mbs_dd->nState, mbs_dd->tsim, h, mbs_dd->yout, mbs_fct_dirdyn, mbs_data, mbs_dd);
}
mbs_dd->tsim += h;
for (i = 0; i < mbs_dd->nState; i++)
......@@ -71,7 +74,10 @@ void loop_rk4(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
}
void finish_rk4(MbsData *mbs_data, MbsDirdyn *dd) {
free(((MbsDirdynRK4 *)dd->integrator_struct)->yt);
free(((MbsDirdynRK4 *)dd->integrator_struct)->dyt);
free(((MbsDirdynRK4 *)dd->integrator_struct)->dym);
if (dd->nState != 0)
{
free(((MbsDirdynRK4 *)dd->integrator_struct)->yt);
free(((MbsDirdynRK4 *)dd->integrator_struct)->dyt);
free(((MbsDirdynRK4 *)dd->integrator_struct)->dym);
}
}
\ No newline at end of file
......@@ -29,31 +29,34 @@ void initialize_rosenbrock(MbsData *mbs_data, MbsDirdyn *mbs_dd) {
// Allocate Rosenbrock structure
mbs_dd->integrator_struct = malloc(sizeof(MbsDirdynRosenbrock));
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->dydt_save = get_dvec_0(mbs_dd->nState);
int n = mbs_dd->nState;
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->flag_save = 0;
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->solout_last_t = 0.0;
int n = mbs_dd->nState;
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->indx = get_ivec_0(n);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->a = get_dmat_0(n, n);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->dfdx = get_dvec_0(n);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->dfdy = get_dmat_0(n, n);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->dysav = get_dvec_0(n);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->err = get_dvec_0(n);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->g1 = get_dvec_0(n);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->g2 = get_dvec_0(n);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->g3 = get_dvec_0(n);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->g4 = get_dvec_0(n);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->ysav = get_dvec_0(n);
/* to freeze jacobian */
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->Freeze_dydx= get_dvec_0( mbs_dd->nState);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->Freeze_dfdx = get_dvec_0(mbs_dd->nState);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->Freeze_dfdy = get_dmat_0(mbs_dd->nState, mbs_dd->nState);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->Freeze_jacobian_index = 0;
if (n != 0)
{
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->dydt_save = get_dvec_0(mbs_dd->nState);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->indx = get_ivec_0(n);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->a = get_dmat_0(n, n);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->dfdx = get_dvec_0(n);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->dfdy = get_dmat_0(n, n);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->dysav = get_dvec_0(n);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->err = get_dvec_0(n);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->g1 = get_dvec_0(n);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->g2 = get_dvec_0(n);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->g3 = get_dvec_0(n);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->g4 = get_dvec_0(n);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->ysav = get_dvec_0(n);
/* to freeze jacobian */
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->Freeze_dydx = get_dvec_0(mbs_dd->nState);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->Freeze_dfdx = get_dvec_0(mbs_dd->nState);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->Freeze_dfdy = get_dmat_0(mbs_dd->nState, mbs_dd->nState);
((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->Freeze_jacobian_index = 0;
}
mbs_warning_msg("The Rosenbrock integrator has not been tested extensively! \n ");
}
......@@ -83,9 +86,10 @@ void loop_rosenbrock(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd)
for (i = 0; i < mbs_dd->nState; i++) {
scaling[i] = (1.0 > mbs_dd->y[i]) ? 1.0 : mbs_dd->y[i];
}
err = rosenbrock(mbs_dd->nState, mbs_fct_dirdyn, &t, mbs_dd->y, mbs_dd->options->toler,
h_max, h_cur, mbs_dd->options->nmax, mbs_dd->yd,
scaling, &hnext, mbs_data, mbs_dd, &hdid);
h_max, h_cur, mbs_dd->options->nmax, mbs_dd->yd,
scaling, &hnext, mbs_data, mbs_dd, &hdid);
if (err < 0) // Error management
{
......@@ -113,22 +117,24 @@ void loop_rosenbrock(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd)
void finish_rosenbrock(MbsData *mbs_data, MbsDirdyn *dd) {
free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->dydt_save);
free_ivec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->indx);
free_dmat_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->a);
free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->dfdx);
free_dmat_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->dfdy);
free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->dysav);
free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->err);
free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->g1);
free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->g2);
free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->g3);
free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->g4);
free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->ysav);
free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->Freeze_dydx);
free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->Freeze_dfdx);
free_dmat_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->Freeze_dfdy);
if (dd->nState != 0)
{
free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->dydt_save);
free_ivec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->indx);
free_dmat_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->a);
free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->dfdx);
free_dmat_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->dfdy);
free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->dysav);
free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->err);
free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->g1);
free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->g2);
free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->g3);
free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->g4);
free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->ysav);
free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->Freeze_dydx);
free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->Freeze_dfdx);
free_dmat_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->Freeze_dfdy);
}
}
......@@ -27,20 +27,23 @@ void initialize_w_methods(MbsData *mbs_data, MbsDirdyn *mbs_dd) {
int n = mbs_dd->nState;
((MbsDirdynWMethods *)mbs_dd->integrator_struct)->indx = get_ivec_0(n);
((MbsDirdynWMethods *)mbs_dd->integrator_struct)->dfdx = get_dvec_0(n);
((MbsDirdynWMethods *)mbs_dd->integrator_struct)->dfdy = get_dmat_0(n, n);
((MbsDirdynWMethods *)mbs_dd->integrator_struct)->f = get_dvec_0(n);
((MbsDirdynWMethods *)mbs_dd->integrator_struct)->dysav = get_dvec_0(n);
((MbsDirdynWMethods *)mbs_dd->integrator_struct)->ysav = get_dvec_0(n);
((MbsDirdynWMethods *)mbs_dd->integrator_struct)->Msav = get_dmat_1(mbs_data->nqu, mbs_data->nqu);
/* to freeze jacobian */
((MbsDirdynWMethods *)mbs_dd->integrator_struct)->Freeze_dydx = get_dvec_0(mbs_dd->nState);
((MbsDirdynWMethods *)mbs_dd->integrator_struct)->Freeze_dfdx = get_dvec_0(mbs_dd->nState);
((MbsDirdynWMethods *)mbs_dd->integrator_struct)->Freeze_dfdy = get_dmat_0(mbs_dd->nState, mbs_dd->nState);
((MbsDirdynWMethods *)mbs_dd->integrator_struct)->Freeze_jacobian_index = 0;
if (mbs_dd->nState != 0)
{
((MbsDirdynWMethods *)mbs_dd->integrator_struct)->indx = get_ivec_0(n);
((MbsDirdynWMethods *)mbs_dd->integrator_struct)->dfdx = get_dvec_0(n);
((MbsDirdynWMethods *)mbs_dd->integrator_struct)->dfdy = get_dmat_0(n, n);
((MbsDirdynWMethods *)mbs_dd->integrator_struct)->f = get_dvec_0(n);
((MbsDirdynWMethods *)mbs_dd->integrator_struct)->dysav = get_dvec_0(n);
((MbsDirdynWMethods *)mbs_dd->integrator_struct)->ysav = get_dvec_0(n);
((MbsDirdynWMethods *)mbs_dd->integrator_struct)->Msav = get_dmat_1(mbs_data->nqu, mbs_data->nqu);
/* to freeze jacobian */
((MbsDirdynWMethods *)mbs_dd->integrator_struct)->Freeze_dydx = get_dvec_0(mbs_dd->nState);
((MbsDirdynWMethods *)mbs_dd->integrator_struct)->Freeze_dfdx = get_dvec_0(mbs_dd->nState);
((MbsDirdynWMethods *)mbs_dd->integrator_struct)->Freeze_dfdy = get_dmat_0(mbs_dd->nState, mbs_dd->nState);
}
((MbsDirdynWMethods *)mbs_dd->integrator_struct)->Freeze_jacobian_index = 0;
mbs_warning_msg("The W Methods integrator is still in development and may not work for specific problems ! \n ");
}
......@@ -65,8 +68,11 @@ void loop_w_methods(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd)
mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
}
// Call the W Method function
w_methods(mbs_dd->y, mbs_dd->yd, mbs_dd->nState, mbs_dd->tsim, h, mbs_dd->yout, mbs_fct_dirdyn, mbs_data, mbs_dd);
if (mbs_dd->nState != 0)
{
// Call the W Method function
w_methods(mbs_dd->y, mbs_dd->yd, mbs_dd->nState, mbs_dd->tsim, h, mbs_dd->yout, mbs_fct_dirdyn, mbs_data, mbs_dd);
}
mbs_dd->tsim += h;
......@@ -81,15 +87,18 @@ void loop_w_methods(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd)
void finish_w_methods(MbsData *mbs_data, MbsDirdyn *dd) {
free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->dfdx);
free_dmat_0(((MbsDirdynWMethods *)dd->integrator_struct)->dfdy);
free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->f);
free_ivec_0(((MbsDirdynWMethods *)dd->integrator_struct)->indx);
free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->dysav);
free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->ysav);
free_dmat_1(((MbsDirdynWMethods *)dd->integrator_struct)->Msav);
free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->Freeze_dydx);
free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->Freeze_dfdx);
free_dmat_0(((MbsDirdynWMethods *)dd->integrator_struct)->Freeze_dfdy);
if (dd->nState != 0)
{
free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->dfdx);
free_dmat_0(((MbsDirdynWMethods *)dd->integrator_struct)->dfdy);
free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->f);
free_ivec_0(((MbsDirdynWMethods *)dd->integrator_struct)->indx);
free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->dysav);
free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->ysav);
free_dmat_1(((MbsDirdynWMethods *)dd->integrator_struct)->Msav);
free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->Freeze_dydx);
free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->Freeze_dfdx);
free_dmat_0(((MbsDirdynWMethods *)dd->integrator_struct)->Freeze_dfdy);
}
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment