/// Initialization based on passing two vectors (point + dir) on the
/// two bodies, they will represent the X axes of the two frames (Y and Z will
/// be built from the X vector via Gram Schmidt orthonormalization).
/// Use the other ChLinkMateGeneric::Initialize() if you want to set the two frames directly.
public virtual void Initialize(ChBodyFrame mbody1, //< first body to link
ChBodyFrame mbody2, //< second body to link
bool pos_are_relative, //< true: following pos. are relative to bodies.
ChVector mpt1, //< origin of slave frame 1, for 1st body (rel. or abs., see flag above)
ChVector mpt2, //< origin of master frame 2, for 2nd body (rel. or abs., see flag above)
ChVector mnorm1, //< X axis of slave plane, for 1st body (rel. or abs., see flag above)
ChVector mnorm2 //< X axis of master plane, for 2nd body (rel. or abs., see flag above)
)
{
Debug.Assert(mbody1 != mbody2);
this.Body1 = mbody1;
this.Body2 = mbody2;
// this.SetSystem(mbody1.GetSystem());
this.mask.SetTwoBodiesVariables(Body1.Variables(), Body2.Variables());
ChVector mx = new ChVector(0, 0, 0);
ChVector my = new ChVector(0, 0, 0);
ChVector mz = new ChVector(0, 0, 0);
ChVector mN = new ChVector(0, 0, 0);
ChMatrix33 <double> mrot = new ChMatrix33 <double>();
ChFrame <double> mfr1 = new ChFrame <double>();
ChFrame <double> mfr2 = new ChFrame <double>();
if (pos_are_relative)
{
mN = mnorm1;
mN.DirToDxDyDz(ref mx, ref my, ref mz, new ChVector(0, 1, 0));
mrot.Set_A_axis(mx, my, mz);
mfr1.SetRot(mrot);
mfr1.SetPos(mpt1);
mN = mnorm2;
mN.DirToDxDyDz(ref mx, ref my, ref mz, new ChVector(0, 1, 0));
mrot.Set_A_axis(mx, my, mz);
mfr2.SetRot(mrot);
mfr2.SetPos(mpt2);
}
else
{
ChVector temp = ChVector.VECT_Z;
// from abs to body-rel
mN = this.Body1.TransformDirectionParentToLocal(mnorm1);
mN.DirToDxDyDz(ref mx, ref my, ref mz, temp);
mrot.Set_A_axis(mx, my, mz);
mfr1.SetRot(mrot);
mfr1.SetPos(this.Body1.TransformPointParentToLocal(mpt1));
mN = this.Body2.TransformDirectionParentToLocal(mnorm2);
mN.DirToDxDyDz(ref mx, ref my, ref mz, temp);
mrot.Set_A_axis(mx, my, mz);
mfr2.SetRot(mrot);
mfr2.SetPos(this.Body2.TransformPointParentToLocal(mpt2));
}
this.frame1 = mfr1;
this.frame2 = mfr2;
}
/// Specialized initialization for generic mate, given the two bodies to be connected, the
/// positions of the two frames to connect on the bodies (each expressed
/// in body or abs. coordinates).
public virtual void Initialize(ChBodyFrame mbody1, //< first body to link
ChBodyFrame mbody2, /// second body to link
bool pos_are_relative, //< true: following pos. are relative to bodies.
ChFrame <double> mpos1, //< mate frame (slave), for 1st body (rel. or abs., see flag above)
ChFrame <double> mpos2 //< mate frame (master), for 2nd body (rel. or abs., see flag above)
)
{
Debug.Assert(mbody1 != mbody2);
this.Body1 = mbody1;
this.Body2 = mbody2;
// this.SetSystem(mbody1.GetSystem());
this.mask.SetTwoBodiesVariables(Body1.Variables(), Body2.Variables());
if (pos_are_relative)
{
this.frame1 = mpos1;
this.frame2 = mpos2;
}
else
{
// from abs to body-rel
(this.Body1).TransformParentToLocal(mpos1, this.frame1);
this.Body2.TransformParentToLocal(mpos2, this.frame2);
}
}
//
// 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);
}
}
}
/// overwrites inherited implementation of this method
public override void SetUpMarkers(ChMarker mark1, ChMarker mark2)
{
base.SetUpMarkers(mark1, mark2);
// could the line below be: assert(this.Body1 && this.Body2); ?
if (this.Body1 != null && this.Body2 != null)
{
((ChLinkMaskLF)this.mask).SetTwoBodiesVariables(Body1.Variables(), Body2.Variables());
// This is needed because only if all constraints in mask are now active, and C,Ct,etc.
// matrices must be allocated accordingly, otherwise are null.
DestroyLink();
BuildLink();
}
}
//
// SOLVER INTERFACE (OLD)
//
public override void ConstraintsFbLoadForces(double factor = 1)
{
// compute instant torque
double mT = m_func.Get_y(this.GetChTime());
ChFrame <double> aframe1 = ChFrame <double> .BitShiftRight(this.frame1, (this.Body1));
ChFrame <double> aframe2 = ChFrame <double> .BitShiftRight(this.frame2, (this.Body2));
ChVector m_abs_torque = aframe2.GetA().Matr_x_Vect(new ChVector(0, 0, mT));
Body2.Variables().Get_fb().matrix.PasteSumVector(Body2.TransformDirectionParentToLocal(m_abs_torque) * -factor, 3, 0);
Body1.Variables().Get_fb().matrix.PasteSumVector(Body1.TransformDirectionParentToLocal(m_abs_torque) * factor, 3, 0);
}
//
// STATE FUNCTIONS
//
public override void IntLoadResidual_F(int off, ref ChVectorDynamic <double> R, double c)
{
// compute instant torque
double mT = m_func.Get_y(this.GetChTime());
ChFrame <double> aframe1 = ChFrame <double> .BitShiftRight(this.frame1, (this.Body1));
ChFrame <double> aframe2 = ChFrame <double> .BitShiftRight(this.frame2, (this.Body2));
ChVector m_abs_torque = aframe2.GetA().Matr_x_Vect(new ChVector(0, 0, mT));
if (Body2.Variables().IsActive())
{
R.matrix.PasteSumVector(Body2.TransformDirectionParentToLocal(m_abs_torque) * -c, Body2.Variables().GetOffset() + 3,
0);
}
if (Body1.Variables().IsActive())
{
R.matrix.PasteSumVector(Body1.TransformDirectionParentToLocal(m_abs_torque) * c, Body1.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);
}
}
//
// UPDATING FUNCTIONS
//
/// Override _all_ time, jacobian etc. updating.
public override void update(double mytime, bool update_assets = true)
{
// Inherit time changes of parent class (ChLink), basically doing nothing :)
base.update(mytime, update_assets);
if (this.Body1 != null && this.Body2 != null)
{
this.mask.SetTwoBodiesVariables(Body1.Variables(), Body2.Variables());
ChFrame <double> aframe = ChFrame <double> .BitShiftRight(this.frame1, (this.Body1));
ChVector p1_abs = aframe.GetPos();
ChFrame <double> aframe2 = ChFrame <double> .BitShiftRight(this.frame2, (this.Body2));
ChVector p2_abs = aframe2.GetPos();
ChFrame <double> bframe = new ChFrame <double>();
(this.Body2).TransformParentToLocal(aframe, bframe);
this.frame2.TransformParentToLocal(bframe, aframe);
// Now 'aframe' contains the position/rotation of frame 1 respect to frame 2, in frame 2 coords.
//***TODO*** check if it is faster to do aframe2.TransformParentToLocal(aframe, bframe); instead of two transforms above
ChMatrix33 <double> Jx1 = new ChMatrix33 <double>(0);
ChMatrix33 <double> Jx2 = new ChMatrix33 <double>(0);
ChMatrix33 <double> Jr1 = new ChMatrix33 <double>(0);
ChMatrix33 <double> Jr2 = new ChMatrix33 <double>(0);
ChMatrix33 <double> Jw1 = new ChMatrix33 <double>(0);
ChMatrix33 <double> Jw2 = new ChMatrix33 <double>(0);
ChMatrix33 <double> mtempM = new ChMatrix33 <double>(0);
ChMatrix33 <double> mtempQ = new ChMatrix33 <double>(0);
ChMatrix33 <double> abs_plane = new ChMatrix33 <double>(0);
abs_plane.nm.matrix.MatrMultiply(Body2.GetA().nm.matrix, frame2.GetA().nm.matrix);
Jx1.nm.matrix.CopyFromMatrixT(abs_plane.nm.matrix);
Jx2.nm.matrix.CopyFromMatrixT(abs_plane.nm.matrix);
Jx2.nm.matrix.MatrNeg();
Jw1.nm.matrix.MatrTMultiply(abs_plane.nm.matrix, Body1.GetA().nm.matrix);
Jw2.nm.matrix.MatrTMultiply(abs_plane.nm.matrix, Body2.GetA().nm.matrix);
mtempM.Set_X_matrix(frame1.GetPos());
Jr1.nm.matrix.MatrMultiply(Jw1.nm.matrix, mtempM.nm.matrix);
Jr1.nm.matrix.MatrNeg();
mtempM.Set_X_matrix(frame2.GetPos());
Jr2.nm.matrix.MatrMultiply(Jw2.nm.matrix, mtempM.nm.matrix);
ChVector p2p1_base2 = (Body2.GetA()).MatrT_x_Vect(ChVector.Vsub(p1_abs, p2_abs));
mtempM.Set_X_matrix(p2p1_base2);
mtempQ.nm.matrix.MatrTMultiply(frame2.GetA().nm.matrix, mtempM.nm.matrix);
Jr2.nm.matrix.MatrInc(mtempQ.nm.matrix);
Jw2.nm.matrix.MatrNeg();
// Premultiply by Jw1 and Jw2 by 0.5*[Fp(q_resid)]' to get residual as imaginary part of a quaternion.
// For small misalignment this effect is almost insignificant cause [Fp(q_resid)]=[I],
// but otherwise it is needed (if you want to use the stabilization term - if not, you can live without).
mtempM.Set_X_matrix((aframe.GetRot().GetVector()) * 0.5);
mtempM.nm.matrix[0, 0] = 0.5 * aframe.GetRot().e0;
mtempM.nm.matrix[1, 1] = 0.5 * aframe.GetRot().e0;
mtempM.nm.matrix[2, 2] = 0.5 * aframe.GetRot().e0;
mtempQ.nm.matrix.MatrTMultiply(mtempM.nm.matrix, Jw1.nm.matrix);
Jw1 = mtempQ;
mtempQ.nm.matrix.MatrTMultiply(mtempM.nm.matrix, Jw2.nm.matrix);
Jw2 = mtempQ;
int nc = 0;
if (c_x)
{
this.C.matrix.ElementN(nc) = aframe.GetPos().x;
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jx1.nm.matrix, 0, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jr1.nm.matrix, 0, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jx2.nm.matrix, 0, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jr2.nm.matrix, 0, 0, 1, 3, 0, 3);
nc++;
}
if (c_y)
{
this.C.matrix.ElementN(nc) = aframe.GetPos().y;
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jx1.nm.matrix, 1, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jr1.nm.matrix, 1, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jx2.nm.matrix, 1, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jr2.nm.matrix, 1, 0, 1, 3, 0, 3);
nc++;
}
if (c_z)
{
this.C.matrix.ElementN(nc) = aframe.GetPos().z;
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jx1.nm.matrix, 2, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jr1.nm.matrix, 2, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jx2.nm.matrix, 2, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jr2.nm.matrix, 2, 0, 1, 3, 0, 3);
nc++;
}
if (c_rx)
{
this.C.matrix.ElementN(nc) = aframe.GetRot().e1;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 0, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 0, 0, 1, 3, 0, 3);
nc++;
}
if (c_ry)
{
this.C.matrix.ElementN(nc) = aframe.GetRot().e2;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 1, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 1, 0, 1, 3, 0, 3);
nc++;
}
if (c_rz)
{
this.C.matrix.ElementN(nc) = aframe.GetRot().e3;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 2, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 2, 0, 1, 3, 0, 3);
nc++;
}
}
}
