/// 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*/
}
/// 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
Vold = V;
// 1
// Do a Anitescu/Trinkle timestepper (it could be without the C/dt correction):
//
// [ M - dt*dF/dv - dt^2*dF/dx Cq' ] [ v_new ] = [ M*(v_old) + dt*f]
// [ Cq 0 ] [ -dt*l ] = [ -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, 0); // may be avoided
mintegrable.LoadConstraint_Ct(ref Qc, 1.0);
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
mintegrable.StateScatterReactions(L); // . system auxiliary data
// 2
// Do the position stabilization (single NR step on constraints, with mass matrix as metric)
Dl.Reset(mintegrable.GetNconstr());
R.Reset(mintegrable.GetNcoords_v());
Qc.Reset(mintegrable.GetNconstr());
L.Reset(mintegrable.GetNconstr());
Vold.Reset(mintegrable.GetNcoords_v(), V.GetIntegrable());
//
// [ M Cq' ] [ dpos ] = [ 0 ]
// [ Cq 0 ] [ l ] = [ -C ]
mintegrable.LoadConstraint_C(ref Qc, 1.0, false, 0);
mintegrable.StateSolveCorrection(
ref Vold, ref L, R, Qc,
1.0, // factor for M
0, // factor for dF/dv
0, // factor for dF/dx
X, V, T, // 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
);
X += Vold; // here we used 'Vold' as 'dpos' to recycle Vold and avoid allocating a new vector dpos
mintegrable.StateScatter(X, V, T); // state . system
}
/// Perform the assembly analysis.
/// Assembly is performed by satisfying constraints at a position, velocity, and acceleration levels.
/// Assembly at position level involves solving a non-linear problem. Assembly at velocity level is
/// performed by taking a small integration step. Consistent accelerations are obtained through
/// finite differencing.
public void AssemblyAnalysis(int action, double dt = 1e-7)
{
ChVectorDynamic <double> R = new ChVectorDynamic <double>();
ChVectorDynamic <double> Qc = new ChVectorDynamic <double>();
double T = 0;
// Set up main vectors
integrable.StateSetup(ref X, ref V, ref A);
if (action != 0 && AssemblyLevel.Enum.POSITION != 0)
{
ChStateDelta Dx = new ChStateDelta();
for (int m_iter = 0; m_iter < max_assembly_iters; m_iter++)
{
// Set up auxiliary vectors
Dx.Reset(integrable.GetNcoords_v(), GetIntegrable());
R.Reset(integrable.GetNcoords_v());
Qc.Reset(integrable.GetNconstr());
L.Reset(integrable.GetNconstr());
integrable.StateGather(ref X, ref V, ref T); // state <- system
// Solve:
//
// [M Cq' ] [ dx ] = [ 0]
// [ Cq 0 ] [ l ] = [ C]
integrable.LoadConstraint_C(ref Qc, 1.0);
integrable.StateSolveCorrection(
ref Dx, ref L, R, Qc,
1.0, // factor for M
0, // factor for dF/dv
0, // factor for dF/dx (the stiffness matrix)
X, V, T, // 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
);
X += Dx;
integrable.StateScatter(X, V, T); // state -> system
}
}
if ((action != 0 & AssemblyLevel.Enum.VELOCITY != 0) || (action != 0 & AssemblyLevel.Enum.ACCELERATION != 0))
{
ChStateDelta Vold = new ChStateDelta();
// setup auxiliary vectors
Vold.Reset(integrable.GetNcoords_v(), GetIntegrable());
R.Reset(integrable.GetNcoords_v());
Qc.Reset(integrable.GetNconstr());
L.Reset(integrable.GetNconstr());
integrable.StateGather(ref X, ref V, ref T); // state <- system
Vold = V;
// Perform a linearized semi-implicit Euler integration step
//
// [ M - dt*dF/dv - dt^2*dF/dx Cq' ] [ v_new ] = [ M*(v_old) + dt*f]
// [ Cq 0 ] [ -dt*l ] = [ C/dt + Ct ]
integrable.LoadResidual_F(ref R, dt);
integrable.LoadResidual_Mv(ref R, V, 1.0);
integrable.LoadConstraint_C(ref Qc, 1.0 / dt, false);
integrable.LoadConstraint_Ct(ref Qc, 1.0);
integrable.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
);
integrable.StateScatter(X, V, T); // state -> system
L *= (1.0 / dt); // Note it is not -(1.0/dt) because we assume StateSolveCorrection already flips sign of L
if (action != 0 & AssemblyLevel.Enum.ACCELERATION != 0)
{
integrable.StateScatterAcceleration(
(V - Vold) * (1 / dt)); // -> system auxiliary data (i.e acceleration as measure, fits DVI/MDI)
integrable.StateScatterReactions(L); // -> system auxiliary data
}
}
}