public ChAssemblyAnalysis(ChIntegrableIIorder mintegrable)
{
integrable = mintegrable;
L.Reset(0);
X.Reset(1, mintegrable);
V.Reset(1, mintegrable);
A.Reset(1, mintegrable);
max_assembly_iters = 4;
}
/// Constructor
public ChStaticAnalysis(ChIntegrableIIorder mintegrable)
{
integrable = mintegrable;
L.Reset(0);
X.Reset(1, mintegrable);
V.Reset(1, mintegrable);
A.Reset(1, mintegrable);
}
/// 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);
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());
R.Reset(mintegrable.GetNcoords_v());
Qc.Reset(mintegrable.GetNconstr());
L.Reset(mintegrable.GetNconstr());
mintegrable.StateGather(ref X, ref V, ref T); // state <- system
// Set V speed to zero
V.matrix.FillElem(0);
mintegrable.StateScatter(X, V, T); // state -> system
// Solve:
//
// [-dF/dx Cq' ] [ dx ] = [ f]
// [ Cq 0 ] [ l ] = [ C]
mintegrable.LoadResidual_F(ref R, 1.0);
mintegrable.LoadConstraint_C(ref Qc, 1.0);
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)
X, 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
);
X += Dx;
mintegrable.StateScatter(X, V, T); // state -> system
mintegrable.StateScatterReactions(L); // -> system auxiliary data
}
/// 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
}
/// 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
}
/// 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*/
}
/// 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
}
}
}