public override void IntFromDescriptor(int off_v,
ref ChStateDelta v,
int off_L,
ref ChVectorDynamic <double> L)
{
int displ_L = off_L - this.offset_L;
int displ_v = off_v - this.offset_w;
for (int i = 0; i < bodylist.Count; i++)
{
if (bodylist[i].IsActive())
{
bodylist[i].IntFromDescriptor(displ_v + bodylist[i].GetOffset_w(), ref v, displ_L + bodylist[i].GetOffset_L(), ref L);
}
}
for (int i = 0; i < linklist.Count; i++)
{
if (linklist[i].IsActive())
{
linklist[i].IntFromDescriptor(displ_v + linklist[i].GetOffset_w(), ref v, displ_L + linklist[i].GetOffset_L(), ref L);
}
}
for (int i = 0; i < otherphysicslist.Count; i++)
{
otherphysicslist[i].IntFromDescriptor(displ_v + otherphysicslist[i].GetOffset_w(), ref v, displ_L + otherphysicslist[i].GetOffset_L(), ref L);
}
}
public override void IntFromDescriptor(int off_v,
ref ChStateDelta v,
int off_L,
ref ChVectorDynamic <double> L)
{
v.matrix[off_v] = variables.Get_qb().matrix[0, 0];
}
public override void IntLoadConstraint_C(int off_L,
ref ChVectorDynamic <double> Qc,
double c,
bool do_clamp,
double recovery_clamp)
{
int cnt = 0;
for (int i = 0; i < mask.nconstr; i++)
{
if (mask.Constr_N(i).IsActive())
{
if (do_clamp)
{
if (mask.Constr_N(i).IsUnilateral())
{
Qc.matrix[off_L + cnt] += ChMaths.ChMax(c * C.matrix.ElementN(cnt), -recovery_clamp);
}
else
{
Qc.matrix[off_L + cnt] += ChMaths.ChMin(ChMaths.ChMax(c * C.matrix.ElementN(cnt), -recovery_clamp), recovery_clamp);
}
}
else
{
Qc.matrix[off_L + cnt] += c * C.matrix.ElementN(cnt);
}
cnt++; // The Gremlin in the works was here! I had accidently locked this variable in the above else, causing joint jittering
}
}
}
public override void IntFromDescriptor(int off_v,
ref ChStateDelta v,
int off_L,
ref ChVectorDynamic <double> L)
{
L.matrix[off_L] = constraint.Get_l_i();
}
public override void IntLoadResidual_Mv(int off,
ref ChVectorDynamic <double> R,
ChVectorDynamic <double> w,
double c)
{
R.matrix[off] += c * 1.0 * w.matrix[off];
}
public override void IntLoadResidual_Mv(int off,
ref ChVectorDynamic <double> R,
ChVectorDynamic <double> w,
double c)
{
int displ_v = off - this.offset_w;
for (int i = 0; i < bodylist.Count; i++)
{
if (bodylist[i].IsActive())
{
bodylist[i].IntLoadResidual_Mv(displ_v + bodylist[i].GetOffset_w(), ref R, w, c);
}
}
for (int i = 0; i < linklist.Count; i++)
{
if (linklist[i].IsActive())
{
linklist[i].IntLoadResidual_Mv(displ_v + linklist[i].GetOffset_w(), ref R, w, c);
}
}
for (int i = 0; i < otherphysicslist.Count; i++)
{
otherphysicslist[i].IntLoadResidual_Mv(displ_v + otherphysicslist[i].GetOffset_w(), ref R, w, c);
}
}
/// Takes the M*v term, multiplying mass by a vector, scale and adds to R at given offset:
/// R += c*M*w
public virtual void IntLoadResidual_Mv(int off, //< offset in R residual
ref ChVectorDynamic <double> R, //< result: the R residual, R += c*M*v
ChVectorDynamic <double> w, //< the w vector
double c //< a scaling factor
)
{
}
public override void IntLoadResidual_CqL(int off_L,
ref ChVectorDynamic <double> R,
ChVectorDynamic <double> L,
double c)
{
int displ_L = off_L - this.offset_L;
for (int i = 0; i < bodylist.Count; i++)
{
if (bodylist[i].IsActive())
{
bodylist[i].IntLoadResidual_CqL(displ_L + bodylist[i].GetOffset_L(), ref R, L, c);
}
}
for (int i = 0; i < linklist.Count; i++)
{
if (linklist[i].IsActive())
{
linklist[i].IntLoadResidual_CqL(displ_L + linklist[i].GetOffset_L(), ref R, L, c);
}
}
for (int i = 0; i < otherphysicslist.Count; i++)
{
otherphysicslist[i].IntLoadResidual_CqL(displ_L + otherphysicslist[i].GetOffset_L(), ref R, L, c);
}
}
public override void IntLoadConstraint_C(int off_L,
ref ChVectorDynamic <double> Qc,
double c,
bool do_clamp,
double recovery_clamp)
{
int displ_L = off_L - this.offset_L;
for (int i = 0; i < bodylist.Count; i++)
{
if (bodylist[i].IsActive())
{
bodylist[i].IntLoadConstraint_C(displ_L + bodylist[i].GetOffset_L(), ref Qc, c, do_clamp, recovery_clamp);
}
}
for (int i = 0; i < linklist.Count; i++)
{
if (linklist[i].IsActive())
{
linklist[i].IntLoadConstraint_C(displ_L + linklist[i].GetOffset_L(), ref Qc, c, do_clamp, recovery_clamp);
}
}
for (int i = 0; i < otherphysicslist.Count; i++)
{
otherphysicslist[i].IntLoadConstraint_C(displ_L + otherphysicslist[i].GetOffset_L(), ref Qc, c, do_clamp, recovery_clamp);
}
}
public override void IntLoadConstraint_C(int off_L,
ref ChVectorDynamic <double> Qc,
double c,
bool do_clamp,
double recovery_clamp)
{
/* if (!IsActive())
* return;
*
* double cnstr_x_violation = c * m_C.matrix.GetElement(0, 0);
* double cnstr_y_violation = c * m_C.matrix.GetElement(1, 0);
* double cnstr_z_violation = c * m_C.matrix.GetElement(2, 0);
* double cnstr_dot_violation = c * m_C.matrix.GetElement(3, 0);
*
* if (do_clamp)
* {
* cnstr_x_violation = ChMaths.ChMin(ChMaths.ChMax(cnstr_x_violation, -recovery_clamp), recovery_clamp);
* cnstr_y_violation = ChMaths.ChMin(ChMaths.ChMax(cnstr_y_violation, -recovery_clamp), recovery_clamp);
* cnstr_z_violation = ChMaths.ChMin(ChMaths.ChMax(cnstr_z_violation, -recovery_clamp), recovery_clamp);
* cnstr_dot_violation = ChMaths.ChMin(ChMaths.ChMax(cnstr_dot_violation, -recovery_clamp), recovery_clamp);
* }
* Qc.matrix[off_L + 0] += cnstr_x_violation;
* Qc.matrix[off_L + 1] += cnstr_y_violation;
* Qc.matrix[off_L + 2] += cnstr_z_violation;
* Qc.matrix[off_L + 3] += cnstr_dot_violation;*/
}
public override void IntStateScatterReactions(int off_L, ChVectorDynamic <double> L)
{
if (!this.IsActive())
{
return;
}
m_multipliers[0] = L.matrix[off_L + 0];
m_multipliers[1] = L.matrix[off_L + 1];
// Also compute 'intuitive' reactions:
double lam_dist = m_multipliers[0]; // ||pos2_abs - pos1_abs|| - dist = 0
double lam_dot = m_multipliers[1]; // dot(dir1_abs, pos2_abs - pos1_abs) = 0
// Calculate the reaction torques and forces on Body 2 in the joint frame
// (Note: origin of the joint frame is at the center of the revolute joint
// which is defined on body 1, the x-axis is along the vector from the
// point on body 1 to the point on body 2. The z axis is along the revolute
// axis defined for the joint)
react_force.x = lam_dist;
react_force.y = 0;
react_force.z = lam_dot;
react_torque.x = 0;
react_torque.y = -m_cur_dist * lam_dot;
react_torque.z = 0;
}
public override void IntLoadResidual_CqL(int off_L,
ref ChVectorDynamic <double> R,
ChVectorDynamic <double> L,
double c)
{
constraint.MultiplyTandAdd(R.matrix, L.matrix[off_L] * c);
}
public override void IntLoadConstraint_C(int off_L,
ref ChVectorDynamic <double> Qc,
double c,
bool do_clamp,
double recovery_clamp)
{
int cnt = 0;
for (int i = 0; i < mask.nconstr; i++)
{
if (mask.Constr_N(i).IsActive())
{
if (do_clamp)
{
if (mask.Constr_N(i).IsUnilateral())
{
Qc.matrix[off_L + cnt] += ChMaths.ChMax(c * C.matrix.ElementN(cnt), -recovery_clamp);
}
else
{
Qc.matrix[off_L + cnt] += ChMaths.ChMin(ChMaths.ChMax(c * C.matrix.ElementN(cnt), -recovery_clamp), recovery_clamp);
}
}
else
{
Qc.matrix[off_L + cnt] += c * C.matrix.ElementN(cnt);
}
cnt++;
}
}
}
public virtual void ContIntLoadResidual_CqL(int off_L, //< offset in L multipliers
ref ChVectorDynamic <double> R, //< result: the R residual, R += c*Cq'*L
ChVectorDynamic <double> L, //< the L vector
double c //< a scaling factor
)
{
}
public override void ContIntFromDescriptor(int off_L,
ref ChVectorDynamic <double> L
)
{
L.matrix[off_L] = Nx.Get_l_i();
L.matrix[off_L + 1] = Tu.Get_l_i();
L.matrix[off_L + 2] = Tv.Get_l_i();
}
public override void IntStateScatterReactions(int off_L, ChVectorDynamic <double> L)
{
react_force = new ChVector(0, 0, 0); // Do not update 'intuitive' react force and torque here: just set as 0.
react_torque = new ChVector(0, 0, 0); // Child classes implementations should compute them.
// if (react != null)
react.matrix.PasteClippedMatrix(L.matrix, off_L, 0, react.matrix.GetRows(), 1, 0, 0);
}
public override void IntLoadResidual_CqL(int off_L,
ref ChVectorDynamic <double> R,
ChVectorDynamic <double> L,
double c)
{
m_cnstr_dist.MultiplyTandAdd(R.matrix, L.matrix[off_L + 0] * c);
m_cnstr_dot.MultiplyTandAdd(R.matrix, L.matrix[off_L + 1] * c);
}
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];
}
/// 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 IntStateScatterReactions(int off_L, ChVectorDynamic <double> L)
{
react_force.x = -L.matrix[off_L];
react_force.y = 0;
react_force.z = 0;
react_torque = ChVector.VNULL;
}
public virtual void ContIntLoadConstraint_C(int off_L, //< offset in Qc residual
ref ChVectorDynamic <double> Qc, //< result: the Qc residual, Qc += c*C
double c, //< a scaling factor
bool do_clamp, //< apply clamping to c*C?
double recovery_clamp ///< value for min/max clamping of c*C
)
{
}
//
// STATE FUNCTIONS
//
// (override/implement interfaces for global state vectors, see ChPhysicsItem for comments.)
public override void IntStateGatherReactions(int off_L, ref ChVectorDynamic <double> L)
{
if (!this.IsActive())
{
return;
}
int nc = 0;
if (c_x)
{
if (mask.Constr_N(nc).IsActive())
{
L.matrix[off_L + nc] = -react_force.x;
}
nc++;
}
if (c_y)
{
if (mask.Constr_N(nc).IsActive())
{
L.matrix[off_L + nc] = -react_force.y;
}
nc++;
}
if (c_z)
{
if (mask.Constr_N(nc).IsActive())
{
L.matrix[off_L + nc] = -react_force.z;
}
nc++;
}
if (c_rx)
{
if (mask.Constr_N(nc).IsActive())
{
L.matrix[off_L + nc] = -2.0 * react_torque.x;
}
nc++;
}
if (c_ry)
{
if (mask.Constr_N(nc).IsActive())
{
L.matrix[off_L + nc] = -2.0 * react_torque.y;
}
nc++;
}
if (c_rz)
{
if (mask.Constr_N(nc).IsActive())
{
L.matrix[off_L + nc] = -2.0 * react_torque.z;
}
nc++;
}
}
// UPDATING FUNCTIONS
public override void ContIntStateGatherReactions(int off_L, ref ChVectorDynamic <double> L)
{
// base behaviour too
base.ContIntStateGatherReactions(off_L, ref L);
L.matrix[off_L + 3] = react_torque.x;
L.matrix[off_L + 4] = react_torque.y;
L.matrix[off_L + 5] = react_torque.z;
}
public override void IntLoadConstraint_Ct(int off_L, ref ChVectorDynamic <double> Qc, double c)
{
double mCt = -m_func.Get_y(this.GetChTime());
if (mask.Constr_N(0).IsActive())
{
Qc.matrix[off_L + 0] += c * mCt;
}
}
public override void ContIntStateScatterReactions(int off_L, ChVectorDynamic <double> L)
{
// base behaviour too
base.ContIntStateScatterReactions(off_L, L);
react_torque.x = L.matrix[off_L + 3];
react_torque.y = L.matrix[off_L + 4];
react_torque.z = L.matrix[off_L + 5];
}
//
// STATE FUNCTIONS
//
public override void IntStateGatherReactions(int off_L, ref ChVectorDynamic <double> L)
{
if (!this.IsActive())
{
return;
}
L.matrix[off_L + 0] = m_multipliers[0];
L.matrix[off_L + 1] = m_multipliers[1];
}
//
// STATE FUNCTIONS
//
public override void IntLoadConstraint_Ct(int off_L, ref ChVectorDynamic <double> Qc, double c)
{
double mCt = -0.5 * m_func.Get_y_dx(this.GetChTime());
int ncrz = mask.nconstr - 1;
if (mask.Constr_N(ncrz).IsActive())
{
Qc.matrix[off_L + ncrz] += c * mCt;
}
}
//
// STATE FUNCTIONS
//
// (override/implement interfaces for global state vectors, see ChPhysicsItem for comments.)
public override void IntStateGatherReactions(int off_L, ref ChVectorDynamic <double> L)
{
/* if (!this.IsActive())
* return;
*
* L.matrix[off_L + 0] = m_multipliers[0];
* L.matrix[off_L + 1] = m_multipliers[1];
* L.matrix[off_L + 2] = m_multipliers[2];
* L.matrix[off_L + 3] = m_multipliers[3];*/
}
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 ContIntLoadResidual_CqL(int off_L,
ref ChVectorDynamic <double> R,
ChVectorDynamic <double> L,
double c
)
{
this.Nx.MultiplyTandAdd(R.matrix, L.matrix[off_L] * c);
this.Tu.MultiplyTandAdd(R.matrix, L.matrix[off_L + 1] * c);
this.Tv.MultiplyTandAdd(R.matrix, L.matrix[off_L + 2] * c);
}
