/// Subtracts this matrix and another matrix.
/// Performance warning: a new object is created.
public static ChState operator -(ChState a, ChMatrix matbis)
{
ChState result = new ChState(matbis.rows, a.integrable);
result.matrix.MatrSub(result.matrix, matbis);
return(result);
}
// (override/implement interfaces for global state vectors, see ChPhysicsItem for comments.)
public override void IntStateGather(int off_x,
ref ChState x,
int off_v,
ref ChStateDelta v,
ref double T)
{
int displ_x = off_x - this.offset_x;
int displ_v = off_v - this.offset_w;
for (int i = 0; i < bodylist.Count; i++)
{
if (bodylist[i].IsActive())
{
bodylist[i].IntStateGather(displ_x + bodylist[i].GetOffset_x(), ref x, displ_v + bodylist[i].GetOffset_w(), ref v, ref T);
}
}
for (int i = 0; i < linklist.Count; i++)
{
if (linklist[i].IsActive())
{
linklist[i].IntStateGather(displ_x + linklist[i].GetOffset_x(), ref x, displ_v + linklist[i].GetOffset_w(), ref v, ref T);
}
}
for (int i = 0; i < otherphysicslist.Count; i++)
{
otherphysicslist[i].IntStateGather(displ_x + otherphysicslist[i].GetOffset_x(), ref x, displ_v + otherphysicslist[i].GetOffset_w(), ref v, ref T);
}
T = GetChTime();
}
/// Increments this matrix by another matrix, in place
/* public static ChState operator +(ChState state, ChMatrix<double> matbis) {
* state.MatrInc(matbis);
* return state;
* }*/
public static ChState operator +(ChState y, ChStateDelta Dy)
{
ChState tmp_y = new ChState(y);
y.GetIntegrable().StateIncrement(ref y, tmp_y, Dy);
return(y);
}
public override void IntStateIncrement(int off_x,
ref ChState x_new,
ChState x,
int off_v,
ChStateDelta Dv)
{
int displ_x = off_x - (int)this.offset_x;
int displ_v = off_v - (int)this.offset_w;
for (int i = 0; i < bodylist.Count; i++)
{
if (bodylist[i].IsActive())
{
bodylist[i].IntStateIncrement(displ_x + bodylist[i].GetOffset_x(), ref x_new, x, displ_v + bodylist[i].GetOffset_w(), Dv);
}
}
for (int i = 0; i < linklist.Count; i++)
{
if (linklist[i].IsActive())
{
linklist[i].IntStateIncrement(displ_x + linklist[i].GetOffset_x(), ref x_new, x, displ_v + linklist[i].GetOffset_w(), Dv);
}
}
for (int i = 0; i < otherphysicslist.Count; i++)
{
otherphysicslist[i].IntStateIncrement(displ_x + otherphysicslist[i].GetOffset_x(), ref x_new, x, displ_v + otherphysicslist[i].GetOffset_w(), Dv);
}
}
public override void IntStateScatter(int off_x,
ChState x,
int off_v,
ChStateDelta v,
double T)
{
int displ_x = off_x - this.offset_x;
int displ_v = off_v - this.offset_w;
for (int i = 0; i < bodylist.Count; i++)
{
if (bodylist[i].IsActive())
{
bodylist[i].IntStateScatter(displ_x + bodylist[i].GetOffset_x(), x, displ_v + bodylist[i].GetOffset_w(), v, T);
}
}
for (int i = 0; i < linklist.Count; i++)
{
if (linklist[i].IsActive())
{
linklist[i].IntStateScatter(displ_x + linklist[i].GetOffset_x(), x, displ_v + linklist[i].GetOffset_w(), v, T);
}
}
for (int i = 0; i < otherphysicslist.Count; i++)
{
otherphysicslist[i].IntStateScatter(displ_x + otherphysicslist[i].GetOffset_x(), x, displ_v + otherphysicslist[i].GetOffset_w(), v, T);
}
SetChTime(T);
// Note: all those IntStateScatter() above should call Update() automatically
// for each object in the loop, therefore:
// -do not call Update() on this.
// -do not call ChPhysicsItem::IntStateScatter() -it calls this.Update() anyway-
// because this would cause redundant updates.
}
/// From item's state to global state vectors y={x,v}
/// pasting the states at the specified offsets.
public virtual void IntStateGather(int off_x, //< offset in x state vector
ref ChState x, //< state vector, position part
int off_v, ///< offset in v state vector
ref ChStateDelta v, ///< state vector, speed part
ref double T ///< time
)
{
}
public override void IntStateScatter(int off_x,
ChState x,
int off_v,
ChStateDelta v,
double T)
{
// aux = x(off_x);
aux_dt = v.matrix[off_v];
}
public override void IntStateScatter(int off_x,
ChState x,
int off_v,
ChStateDelta v,
double T)
{
SetPos(x.matrix[off_x]);
SetPos_dt(v.matrix[off_v]);
update(T);
}
public override void IntStateGather(int off_x,
ref ChState x,
int off_v,
ref ChStateDelta v,
ref double T)
{
x.matrix[off_x] = 0; // aux;
v.matrix[off_v] = aux_dt;
T = GetChTime();
}
/// From global state vectors y={x,v} to item's state (and update)
/// fetching the states at the specified offsets.
public virtual void IntStateScatter(int off_x, //< offset in x state vector
ChState x, ///< state vector, position part
int off_v, //< offset in v state vector
ChStateDelta v, //< state vector, speed part
double T ///< time
)
{
// Default behavior: even if no state is used, at least call Update()
//update(T);
}
/// Computes x_new = x + Dt , using vectors at specified offsets.
/// By default, when DOF = DOF_w, it does just the sum, but in some cases (ex when using quaternions
/// for rotations) it could do more complex stuff, and children classes might overload it.
public virtual void IntStateIncrement(int off_x, //< offset in x state vector
ref ChState x_new, //< state vector, position part, incremented result
ChState x, //< state vector, initial position part
int off_v, //< offset in v state vector
ChStateDelta Dv //< state vector, increment
)
{
for (int i = 0; i < GetDOF(); ++i)
{
x_new.matrix[off_x + i] = x.matrix[off_x + i] + Dv.matrix[off_v + i];
}
}
/// Compute all forces in a contiguous array.
/// Used in finite-difference Jacobian approximation.
public void CalculateQ(ChState stateA_x, //< state positions for objA
ChStateDelta stateA_w, //< state velocities for objA
ChState stateB_x, //< state positions for objB
ChStateDelta stateB_w, //< state velocities for objB
ChMaterialCompositeSMC mat, //< composite material for contact pair
ref ChVectorDynamic <double> Q //< output generalized forces
)
{
ChBody oA = (this.objA as ChBody);
ChBody oB = (this.objB as ChBody);
// Express contact points in local frames.
// We assume that these points remain fixed to their respective contactable objects.
ChVector p1_loc = oA.GetCsysForCollisionModel().TransformPointParentToLocal(this.p1);
ChVector p2_loc = oB.GetCsysForCollisionModel().TransformPointParentToLocal(this.p2);
// Express the local points in global frame
ChVector p1_abs = oA.GetContactPoint(p1_loc, stateA_x);
ChVector p2_abs = oB.GetContactPoint(p2_loc, stateB_x);
/*
* Note: while this can be somewhat justified for a ChBody, it will not work
* for a mesh vertex for instance...
*
* // Project the points onto the unperturbed normal line
* p1_abs = this.p1 + Vdot(p1_abs - this.p1, this.normal) * this.normal;
* p2_abs = this.p2 + Vdot(p2_abs - this.p2, this.normal) * this.normal;
*/
// Calculate normal direction (expressed in global frame)
ChVector normal_dir = (p1_abs - p2_abs).GetNormalized();
// Calculate penetration depth
double delta = (p1_abs - p2_abs).Length();
// If the normal direction flipped sign, change sign of delta
if (ChVector.Vdot(normal_dir, this.normal) < 0)
{
delta = -delta;
}
// Calculate velocity of contact points (expressed in global frame)
ChVector vel1 = oA.GetContactPointSpeed(p1_loc, stateA_x, stateA_w);
ChVector vel2 = oB.GetContactPointSpeed(p2_loc, stateB_x, stateB_w);
// Compute the contact force.
ChVector force = CalculateForce(delta, normal_dir, vel1, vel2, mat);
// Compute and load the generalized contact forces.
oA.ContactForceLoadQ(-force, p1_abs, stateA_x, ref Q, 0);
oB.ContactForceLoadQ(force, p2_abs, stateB_x, ref Q, oA.ContactableGet_ndof_w());
}
public override void IntStateIncrement(int off_x,
ref ChState x_new,
ChState x,
int off_v,
ChStateDelta Dv)
{
// First, inherit to parent class
base.IntStateIncrement(off_x, ref x_new, x, off_v, Dv);
if (eng_mode == eCh_eng_mode.ENG_MODE_TO_POWERTRAIN_SHAFT)
{
innershaft1.IntStateIncrement(off_x + 0, ref x_new, x, off_v + 0, Dv);
innershaft2.IntStateIncrement(off_x + 1, ref x_new, x, off_v + 1, Dv);
}
}
// (override/implement interfaces for global state vectors, see ChPhysicsItem for comments.)
// (beyond the base link implementations, it also have to
// add the raint coming from the inner shaft etc.)
public override void IntStateGather(int off_x,
ref ChState x,
int off_v,
ref ChStateDelta v,
ref double T)
{
// First, inherit to parent class
base.IntStateGather(off_x, ref x, off_v, ref v, ref T);
if (eng_mode == eCh_eng_mode.ENG_MODE_TO_POWERTRAIN_SHAFT)
{
innershaft1.IntStateGather(off_x + 0, ref x, off_v + 0, ref v, ref T);
innershaft2.IntStateGather(off_x + 1, ref x, off_v + 1, ref v, ref T);
}
}
public override void IntStateScatter(int off_x,
ChState x,
int off_v,
ChStateDelta v,
double T)
{
// First, inherit to parent class
base.IntStateScatter(off_x, x, off_v, v, T);
if (eng_mode == eCh_eng_mode.ENG_MODE_TO_POWERTRAIN_SHAFT)
{
innershaft1.IntStateScatter(off_x + 0, x, off_v + 0, v, T);
innershaft2.IntStateScatter(off_x + 1, x, off_v + 1, v, T);
}
}
/// 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
}
/// Copy constructor
public ChState(ChState msource) : base(msource)
{
integrable = msource.integrable;
}
/// Calculate Jacobian of generalized contact forces.
public void CalculateJacobians(ChMaterialCompositeSMC mat)
{
// Compute a finite-difference approximations to the Jacobians of the contact forces and
// load dQ/dx into m_Jac.m_K and dQ/dw into m_Jac.m_R.
// Note that we only calculate these Jacobians whenever the contact force itself is calculated,
// that is only once per step. The Jacobian of generalized contact forces will therefore be
// constant over the time step.
ChBody oA = (this.objA as ChBody);
ChBody oB = (this.objB as ChBody);
// Get states for objA
int ndofA_x = oA.ContactableGet_ndof_x();
int ndofA_w = oA.ContactableGet_ndof_w();
ChState stateA_x = new ChState(ndofA_x, null);
ChStateDelta stateA_w = new ChStateDelta(ndofA_w, null);
oA.ContactableGetStateBlock_x(ref stateA_x);
oA.ContactableGetStateBlock_w(ref stateA_w);
// Get states for objB
int ndofB_x = oB.ContactableGet_ndof_x();
int ndofB_w = oB.ContactableGet_ndof_w();
ChState stateB_x = new ChState(ndofB_x, null);
ChStateDelta stateB_w = new ChStateDelta(ndofB_w, null);
oB.ContactableGetStateBlock_x(ref stateB_x);
oB.ContactableGetStateBlock_w(ref stateB_w);
// Compute Q at current state
ChVectorDynamic <double> Q0 = new ChVectorDynamic <double>(ndofA_w + ndofB_w);
CalculateQ(stateA_x, stateA_w, stateB_x, stateB_w, mat, ref Q0);
// Finite-difference approximation perturbation.
// Note that ChState and ChStateDelta are set to 0 on construction.
// To accommodate objects with quaternion states, use the method ContactableIncrementState while
// calculating Jacobian columns corresponding to position states.
double perturbation = 1e-5;
ChState stateA_x1 = new ChState(ndofA_x, null);
ChState stateB_x1 = new ChState(ndofB_x, null);
ChStateDelta prtrbA = new ChStateDelta(ndofA_w, null);
ChStateDelta prtrbB = new ChStateDelta(ndofB_w, null);
ChVectorDynamic <double> Q1 = new ChVectorDynamic <double>(ndofA_w + ndofB_w);
ChVectorDynamic <double> Jcolumn = new ChVectorDynamic <double>(ndofA_w + ndofB_w);
// Jacobian w.r.t. variables of objA
for (int i = 0; i < ndofA_w; i++)
{
prtrbA.matrix[i] += perturbation;
oA.ContactableIncrementState(stateA_x, prtrbA, ref stateA_x1);
CalculateQ(stateA_x1, stateA_w, stateB_x, stateB_w, mat, ref Q1);
prtrbA.matrix[i] -= perturbation;
Jcolumn = (Q1 - Q0.matrix) * (-1 / perturbation); // note sign change
m_Jac.m_K.matrix.PasteMatrix(Jcolumn.matrix, 0, i);
stateA_w.matrix[i] += perturbation;
CalculateQ(stateA_x, stateA_w, stateB_x, stateB_w, mat, ref Q1);
stateA_w.matrix[i] -= perturbation;
Jcolumn = (Q1 - Q0.matrix) * (-1 / perturbation); // note sign change
m_Jac.m_R.matrix.PasteMatrix(Jcolumn.matrix, 0, i);
}
// Jacobian w.r.t. variables of objB
for (int i = 0; i < ndofB_w; i++)
{
prtrbB.matrix[i] += perturbation;
oB.ContactableIncrementState(stateB_x, prtrbB, ref stateB_x1);
CalculateQ(stateA_x, stateA_w, stateB_x1, stateB_w, mat, ref Q1);
prtrbB.matrix[i] -= perturbation;
Jcolumn = (Q1 - Q0.matrix) * (-1 / perturbation); // note sign change
m_Jac.m_K.matrix.PasteMatrix(Jcolumn.matrix, 0, ndofA_w + i);
stateB_w.matrix[i] += perturbation;
CalculateQ(stateA_x, stateA_w, stateB_x, stateB_w, mat, ref Q1);
stateB_w.matrix[i] -= perturbation;
Jcolumn = (Q1 - Q0.matrix) * (-1 / perturbation); // note sign change
m_Jac.m_R.matrix.PasteMatrix(Jcolumn.matrix, 0, ndofA_w + i);
}
}
/// 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
}
}
}
