/// Performs the static analysis,
/// doing a linear solve.
public override void StaticAnalysis()
{
ChIntegrableIIorder mintegrable = (ChIntegrableIIorder)this.integrable;
// setup main vectors
mintegrable.StateSetup(ref X, ref V, ref A);
ChState Xnew = new ChState();
ChStateDelta Dx = new ChStateDelta();
ChVectorDynamic <double> R = new ChVectorDynamic <double>();
ChVectorDynamic <double> Qc = new ChVectorDynamic <double>();
double T = 0;
// setup auxiliary vectors
Dx.Reset(mintegrable.GetNcoords_v(), GetIntegrable());
Xnew.Reset(mintegrable.GetNcoords_x(), mintegrable);
R.Reset(mintegrable.GetNcoords_v());
Qc.Reset(mintegrable.GetNconstr());
L.Reset(mintegrable.GetNconstr());
mintegrable.StateGather(ref X, ref V, ref T); // state <- system
// Set speed to zero
V.matrix.FillElem(0);
// Extrapolate a prediction as warm start
Xnew = X;
// use Newton Raphson iteration to solve implicit Euler for v_new
//
// [ - dF/dx Cq' ] [ Dx ] = [ f ]
// [ Cq 0 ] [ L ] = [ C ]
for (int i = 0; i < this.GetMaxiters(); ++i)
{
mintegrable.StateScatter(Xnew, V, T); // state -> system
R.Reset();
Qc.Reset();
mintegrable.LoadResidual_F(ref R, 1.0);
mintegrable.LoadConstraint_C(ref Qc, 1.0);
double cfactor = ChMaths.ChMin(1.0, ((double)(i + 2) / (double)(incremental_steps + 1)));
R *= cfactor;
Qc *= cfactor;
// GetLog()<< "Non-linear statics iteration=" << i << " |R|=" << R.NormInf() << " |Qc|=" << Qc.NormInf()
//<< "\n";
if ((R.matrix.NormInf() < this.GetTolerance()) && (Qc.matrix.NormInf() < this.GetTolerance()))
{
break;
}
mintegrable.StateSolveCorrection(
ref Dx, ref L, R, Qc,
0, // factor for M
0, // factor for dF/dv
-1.0, // factor for dF/dx (the stiffness matrix)
Xnew, V, T, // not needed here
false, // do not StateScatter update to Xnew Vnew T+dt before computing correction
true // force a call to the solver's Setup() function
);
Xnew += Dx;
}
X = Xnew;
mintegrable.StateScatter(X, V, T); // state -> system
mintegrable.StateScatterReactions(L); // -> system auxiliary data
}