/// Performs an integration timestep
public override void Advance(double dt //< timestep to advance
)
{
// downcast
ChIntegrableIIorder mintegrable = (ChIntegrableIIorder)this.integrable;
// setup main vectors
mintegrable.StateSetup(ref X, ref V, ref A);
// setup auxiliary vectors
Dl.Reset(mintegrable.GetNconstr());
R.Reset(mintegrable.GetNcoords_v());
Qc.Reset(mintegrable.GetNconstr());
L.Reset(mintegrable.GetNconstr());
mintegrable.StateGather(ref X, ref V, ref T); // state <- system
mintegrable.StateGatherReactions(ref L); // state <- system (may be needed for warm starting StateSolveCorrection)
L *= dt; // because reactions = forces, here L = impulses
Vold = V;
// solve only 1st NR step, using v_new = 0, so Dv = v_new , therefore
//
// [ M - dt*dF/dv - dt^2*dF/dx Cq' ] [ Dv ] = [ M*(v_old - v_new) + dt*f]
// [ Cq 0 ] [ -dt*Dl ] = [ -C/dt - Ct ]
//
// becomes the Anitescu/Trinkle timestepper:
//
// [ M - dt*dF/dv - dt^2*dF/dx Cq' ] [ v_new ] = [ M*(v_old) + dt*f]
// [ Cq 0 ] [ -dt*l ] = [ -C/dt - Ct ]
mintegrable.LoadResidual_F(ref R, dt);
mintegrable.LoadResidual_Mv(ref R, V, 1.0);
mintegrable.LoadConstraint_C(ref Qc, 1.0 / dt, Qc_do_clamp, Qc_clamping);
mintegrable.LoadConstraint_Ct(ref Qc, 1.0);
// Can't shave anymore off this, Solver() is the only cpu drain.
mintegrable.StateSolveCorrection(
ref V, ref L, R, Qc,
1.0, // factor for M
-dt, // factor for dF/dv
-dt * dt, // factor for dF/dx
X, V, T + dt, // not needed
false, // do not StateScatter update to Xnew Vnew T+dt before computing correction
true // force a call to the solver's Setup() function
);
L *= (1.0 / dt); // Note it is not -(1.0/dt) because we assume StateSolveCorrection already flips sign of Dl
mintegrable.StateScatterAcceleration(
(V - Vold) * (1 / dt)); // . system auxiliary data (i.e acceleration as measure, fits DVI/MDI)
X += V * dt;
T += dt;
mintegrable.StateScatter(X, V, T); // state . system // Big cpu drain
mintegrable.StateScatterReactions(L); // . system auxiliary data*/
}