/// 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
)
{
}
/// After a solver solution, fetch values from variables and constraints into vectors:
public virtual void IntFromDescriptor(
int off_v, //< offset for \e v
ref ChStateDelta v, //< vector to where the \e q 'unknowns' term of the variables will be copied
int off_L, //< offset for \e L
ref ChVectorDynamic <double> L //< vector to where \e L 'lagrangian ' term of the constraints will be copied
)
{
}
public override void IntToDescriptor(int off_v, ChStateDelta v, ChVectorDynamic <double> R, int off_L, ChVectorDynamic <double> L, ChVectorDynamic <double> Qc)
{
constraint.Set_l_i(L.matrix[off_L]);
constraint.Set_b_i(Qc.matrix[off_L]);
this.variable.Get_qb().matrix[0, 0] = v.matrix[off_v];
this.variable.Get_fb().matrix[0, 0] = R.matrix[off_v];
}
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];
}
/// Prepare variables and constraints to accommodate a solution:
public virtual void IntToDescriptor(
int off_v, //< offset for \e v and \e R
ChStateDelta v, //< vector that will be copied into the \e q 'unknowns' term of the variables (for warm starting)
ChVectorDynamic <double> R, //< vector that will be copied into the \e F 'force' term of the variables
int off_L, //< offset for \e L and \e Qc
ChVectorDynamic <double> L, //< vector that will be copied into the \e L 'lagrangian ' term of the constraints (for warm starting)
ChVectorDynamic <double> Qc //< vector that will be copied into the \e Qb 'constraint' term of the constraints
)
{
}
/// 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);
}
public override void IntFromDescriptor(int off_v,
ref ChStateDelta v,
int off_L,
ref ChVectorDynamic <double> L)
{
// inherit parent
base.IntFromDescriptor(off_v, ref v, off_L, ref L);
v.matrix[off_v] = this.variable.Get_qb().matrix[0, 0];
}
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 IntToDescriptor(int off_v,
ChStateDelta v,
ChVectorDynamic <double> R,
int off_L,
ChVectorDynamic <double> L,
ChVectorDynamic <double> Qc)
{
variables.Get_qb().matrix[0, 0] = v.matrix[off_v];
variables.Get_fb().matrix[0, 0] = R.matrix[off_v];
}
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();
}
public override void IntStateScatterAcceleration(int off_a, ChStateDelta a)
{
// First, inherit to parent class
base.IntStateScatterAcceleration(off_a, a);
if (eng_mode == eCh_eng_mode.ENG_MODE_TO_POWERTRAIN_SHAFT)
{
innershaft1.IntStateScatterAcceleration(off_a + 0, a);
innershaft2.IntStateScatterAcceleration(off_a + 1, a);
}
}
public override void IntToDescriptor(int off_v,
ChStateDelta v,
ChVectorDynamic <double> R,
int off_L,
ChVectorDynamic <double> L,
ChVectorDynamic <double> Qc)
{
constraint.Set_l_i(L.matrix[off_L]);
constraint.Set_b_i(Qc.matrix[off_L]);
}
public override void IntFromDescriptor(int off_v,
ref ChStateDelta v,
int off_L,
ref ChVectorDynamic <double> L)
{
if (!IsActive())
{
return;
}
L.matrix[off_L] = Cx.Get_l_i();
}
/// 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 IntToDescriptor(int off_v,
ChStateDelta v,
ChVectorDynamic <double> R,
int off_L,
ChVectorDynamic <double> L,
ChVectorDynamic <double> Qc)
{
// inherit parent
base.IntToDescriptor(off_v, v, R, off_L, L, Qc);
this.variable.Get_qb().matrix[0, 0] = v.matrix[off_v];
this.variable.Get_fb().matrix[0, 0] = R.matrix[off_v];
}
public override void IntFromDescriptor(int off_v,
ref ChStateDelta v,
int off_L,
ref ChVectorDynamic <double> L)
{
/* if (!IsActive())
* return;
*
* L.matrix[off_L + 0] = m_cnstr_x.Get_l_i();
* L.matrix[off_L + 1] = m_cnstr_y.Get_l_i();
* L.matrix[off_L + 2] = m_cnstr_z.Get_l_i();
* L.matrix[off_L + 3] = m_cnstr_dot.Get_l_i();*/
}
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);
}
}
// (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 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);
}
}
public override void IntFromDescriptor(int off_v,
ref ChStateDelta v,
int off_L,
ref ChVectorDynamic <double> L)
{
int cnt = 0;
for (int i = 0; i < mask.nconstr; i++)
{
if (mask.Constr_N(i).IsActive())
{
L.matrix[off_L + cnt] = mask.Constr_N(i).Get_l_i(); // PROBLEM not return correct l_i values
cnt++;
}
}
}
public override void IntToDescriptor(int off_v,
ChStateDelta v,
ChVectorDynamic <double> R,
int off_L,
ChVectorDynamic <double> L,
ChVectorDynamic <double> Qc)
{
if (!IsActive())
{
return;
}
Cx.Set_l_i(L.matrix[off_L]);
Cx.Set_b_i(Qc.matrix[off_L]);
}
public override void IntFromDescriptor(int off_v,
ref ChStateDelta v,
int off_L,
ref ChVectorDynamic <double> L)
{
// First, inherit to parent class
base.IntFromDescriptor(off_v, ref v, off_L, ref L);
if (eng_mode == eCh_eng_mode.ENG_MODE_TO_POWERTRAIN_SHAFT)
{
innershaft1.IntFromDescriptor(off_v, ref v, off_L, ref L);
innershaft2.IntFromDescriptor(off_v + 1, ref v, off_L, ref L);
innerconstraint1.IntFromDescriptor(off_v, ref v, off_L, ref L);
innerconstraint2.IntFromDescriptor(off_v, ref v, off_L + 1, ref L);
}
}
/// 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
}
public override void IntToDescriptor(int off_v,
ChStateDelta v,
ChVectorDynamic <double> R,
int off_L,
ChVectorDynamic <double> L,
ChVectorDynamic <double> Qc)
{
// First, inherit to parent class
base.IntToDescriptor(off_v, v, R, off_L, L, Qc);
if (eng_mode == eCh_eng_mode.ENG_MODE_TO_POWERTRAIN_SHAFT)
{
innershaft1.IntToDescriptor(off_v, v, R, off_L, L, Qc);
innershaft2.IntToDescriptor(off_v + 1, v, R, off_L, L, Qc);
innerconstraint1.IntToDescriptor(off_v, v, R, off_L, L, Qc);
innerconstraint2.IntToDescriptor(off_v, v, R, off_L + 1, L, Qc);
}
}
public override void IntToDescriptor(int off_v,
ChStateDelta v,
ChVectorDynamic <double> R,
int off_L,
ChVectorDynamic <double> L,
ChVectorDynamic <double> Qc)
{
int cnt = 0;
for (int i = 0; i < mask.nconstr; i++)
{
if (mask.Constr_N(i).IsActive())
{
mask.Constr_N(i).Set_l_i(L.matrix[off_L + cnt]); // This isn't the l_i problem!
mask.Constr_N(i).Set_b_i(Qc.matrix[off_L + cnt]);
cnt++;
}
}
}
public override void IntToDescriptor(int off_v,
ChStateDelta v,
ChVectorDynamic <double> R,
int off_L,
ChVectorDynamic <double> L,
ChVectorDynamic <double> Qc)
{
/* if (!IsActive())
* return;
*
* m_cnstr_x.Set_l_i(L.matrix[off_L + 0]);
* m_cnstr_y.Set_l_i(L.matrix[off_L + 1]);
* m_cnstr_z.Set_l_i(L.matrix[off_L + 2]);
* m_cnstr_dot.Set_l_i(L.matrix[off_L + 3]);
*
* m_cnstr_x.Set_b_i(Qc.matrix[off_L + 0]);
* m_cnstr_y.Set_b_i(Qc.matrix[off_L + 1]);
* m_cnstr_z.Set_b_i(Qc.matrix[off_L + 2]);
* m_cnstr_dot.Set_b_i(Qc.matrix[off_L + 3]);*/
}
public override void IntStateScatterAcceleration(int off_a, ChStateDelta a)
{
int displ_a = off_a - this.offset_w;
for (int i = 0; i < bodylist.Count; i++)
{
if (bodylist[i].IsActive())
{
bodylist[i].IntStateScatterAcceleration(displ_a + bodylist[i].GetOffset_w(), a);
}
}
for (int i = 0; i < linklist.Count; i++)
{
if (linklist[i].IsActive())
{
linklist[i].IntStateScatterAcceleration(displ_a + linklist[i].GetOffset_w(), a);
}
}
for (int i = 0; i < otherphysicslist.Count; i++)
{
otherphysicslist[i].IntStateScatterAcceleration(displ_a + otherphysicslist[i].GetOffset_w(), a);
}
}
public override void IntStateScatterAcceleration(int off_a, ChStateDelta a)
{
aux_dtdt = a.matrix[off_a];
}
public override void IntStateGatherAcceleration(int off_a, ref ChStateDelta a)
{
a.matrix[off_a] = aux_dtdt;
}
