//
// STATE FUNCTIONS
//
/// Adds force to residual R, as R*= F*c
/// NOTE: here the off offset in R is NOT used because add F at the TWO offsets of the two connected bodies,
/// so it is assumed that offsets for Body1 and Body2 variables have been already set properly!
public override void IntLoadResidual_F(int off, ref ChVectorDynamic <double> R, double c)
{
if (Body1 == null || Body2 == null)
{
return;
}
ChVector mbody_force = new ChVector(0, 0, 0);
ChVector mbody_torque = new ChVector(0, 0, 0);
if (ChVector.Vnotnull(C_force))
{
ChVector m_abs_force = Body2.GetA().Matr_x_Vect(marker2.FrameMoving.GetA().Matr_x_Vect(C_force));
if (Body2.Variables().IsActive())
{
Body2.To_abs_forcetorque(m_abs_force,
marker1.GetAbsCoord().pos, // absolute application point is always marker1
false, // from abs. space
ref mbody_force, ref mbody_torque); // resulting force-torque, both in abs coords
R.matrix.PasteSumVector(mbody_force * -c, Body2.Variables().GetOffset(), 0);
R.matrix.PasteSumVector(Body2.TransformDirectionParentToLocal(mbody_torque) * -c,
Body2.Variables().GetOffset() + 3, 0);
}
if (Body1.Variables().IsActive())
{
Body1.To_abs_forcetorque(m_abs_force,
marker1.GetAbsCoord().pos, // absolute application point is always marker1
false, // from abs. space
ref mbody_force, ref mbody_torque); // resulting force-torque, both in abs coords
R.matrix.PasteSumVector(mbody_force * c, Body1.Variables().GetOffset(), 0);
R.matrix.PasteSumVector(Body1.TransformDirectionParentToLocal(mbody_torque) * c,
Body1.Variables().GetOffset() + 3, 0);
}
}
if (ChVector.Vnotnull(C_torque))
{
ChVector m_abs_torque = Body2.GetA().Matr_x_Vect(marker2.FrameMoving.GetA().Matr_x_Vect(C_torque));
// load torques in 'fb' vector accumulator of body variables (torques in local coords)
if (Body1.Variables().IsActive())
{
R.matrix.PasteSumVector(Body1.TransformDirectionParentToLocal(m_abs_torque) * c,
Body1.Variables().GetOffset() + 3, 0);
}
if (Body2.Variables().IsActive())
{
R.matrix.PasteSumVector(Body2.TransformDirectionParentToLocal(m_abs_torque) * -c,
Body2.Variables().GetOffset() + 3, 0);
}
}
}
//
// SOLVER INTERFACE
//
/// Overrides the empty behaviour of the parent ChLink implementation, which
/// does not consider any user-imposed force between the two bodies.
/// It adds the current link-forces, if any, (caused by springs, etc.) to the 'fb' vectors
/// of the ChVariables referenced by encapsulated ChConstraints.
/// In details, it adds the effect caused by C_force and C_torque.
/// Both C_force and C_torque these forces are considered expressed in the
/// reference coordsystem of marker2 (the MAIN marker),
/// and their application point is the origin of marker1 (the SLAVE marker).
public override void ConstraintsFbLoadForces(double factor = 1)
{
if (Body1 == null || Body2 == null)
{
return;
}
ChVector mbody_force = new ChVector(0, 0, 0);
ChVector mbody_torque = new ChVector(0, 0, 0);
if (ChVector.Vnotnull(C_force))
{
ChVector m_abs_force = Body2.GetA().Matr_x_Vect(marker2.FrameMoving.GetA().Matr_x_Vect(C_force));
Body2.To_abs_forcetorque(m_abs_force,
marker1.GetAbsCoord().pos, // absolute application point is always marker1
false, // from abs. space
ref mbody_force, ref mbody_torque); // resulting force-torque, both in abs coords
Body2.Variables().Get_fb().matrix.PasteSumVector(mbody_force * -factor, 0, 0);
Body2.Variables().Get_fb().matrix.PasteSumVector(Body2.TransformDirectionParentToLocal(mbody_torque) * -factor, 3,
0);
Body1.To_abs_forcetorque(m_abs_force,
marker1.GetAbsCoord().pos, // absolute application point is always marker1
false, // from abs. space
ref mbody_force, ref mbody_torque); // resulting force-torque, both in abs coords
Body1.Variables().Get_fb().matrix.PasteSumVector(mbody_force * factor, 0, 0);
Body1.Variables().Get_fb().matrix.PasteSumVector(Body1.TransformDirectionParentToLocal(mbody_torque) * factor, 3, 0);
}
if (ChVector.Vnotnull(C_torque))
{
ChVector m_abs_torque = Body2.GetA().Matr_x_Vect(marker2.FrameMoving.GetA().Matr_x_Vect(C_torque));
// load torques in 'fb' vector accumulator of body variables (torques in local coords)
Body1.Variables().Get_fb().matrix.PasteSumVector(Body1.TransformDirectionParentToLocal(m_abs_torque) * factor, 3, 0);
Body2.Variables().Get_fb().matrix.PasteSumVector(Body2.TransformDirectionParentToLocal(m_abs_torque) * -factor, 3,
0);
}
}