/// Get the master coordinate system for the assets (this will return the
/// absolute coordinate system of the 'master' marker2)
// public override ChFrame<double> GetAssetsFrame(int nclone = 0) { return marker2.GetAbsFrame(); }
//
// UPDATING FUNCTIONS
//
/// Updates auxiliary vars relM, relM_dt, relM_dtdt,
/// dist, dist_dt et simila.
public virtual void UpdateRelMarkerCoords()
{
// FOR ALL THE 6(or3) COORDINATES OF RELATIVE MOTION OF THE TWO MARKERS:
// COMPUTE THE relM, relM_dt relM_dtdt COORDINATES, AND AUXILIARY DATA (distance,etc.)
ChVector PQw = ChVector.Vsub(marker1.GetAbsCoord().pos, marker2.GetAbsCoord().pos);
ChVector PQw_dt = ChVector.Vsub(marker1.GetAbsCoord_dt().pos, marker2.GetAbsCoord_dt().pos);
ChVector PQw_dtdt = ChVector.Vsub(marker1.GetAbsCoord_dtdt().pos, marker2.GetAbsCoord_dtdt().pos);
dist = ChVector.Vlength(PQw); // distance between origins, modulus
dist_dt = ChVector.Vdot(ChVector.Vnorm(PQw), PQw_dt); // speed between origins, modulus.
ChVector vtemp1; // for intermediate calculus
ChVector vtemp2;
ChQuaternion qtemp1;
// ChMatrixNM<IntInterface.Three, IntInterface.Four> relGw = new ChMatrixNM<IntInterface.Three, IntInterface.Four>(0);
/* ChQuaternion q_AD;
* ChQuaternion q_BC;
* ChQuaternion q_8;
* ChVector q_4;*/
ChQuaternion temp1 = marker1.FrameMoving.GetCoord_dt().rot;
ChQuaternion temp2 = marker2.FrameMoving.GetCoord_dt().rot;
if (ChQuaternion.Qnotnull(temp1) || ChQuaternion.Qnotnull(temp2))
{
q_AD = // q'qqq + qqqq'
ChQuaternion.Qadd(ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord_dt().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.GetBody().BodyFrame.GetCoord().rot),
ChQuaternion.Qcross((marker1.GetBody().BodyFrame.GetCoord().rot), (marker1.FrameMoving.GetCoord().rot)))),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.GetBody().BodyFrame.GetCoord().rot),
ChQuaternion.Qcross((marker1.GetBody().BodyFrame.GetCoord().rot), (marker1.FrameMoving.GetCoord_dt().rot)))));
}
else
{
//q_AD = ChQuaternion.QNULL;
q_BC = // qq'qq + qqq'q
ChQuaternion.Qadd(ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.GetBody().BodyFrame.GetCoord_dt().rot),
ChQuaternion.Qcross((marker1.GetBody().BodyFrame.GetCoord().rot), (marker1.FrameMoving.GetCoord().rot)))),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.GetBody().BodyFrame.GetCoord().rot),
ChQuaternion.Qcross((marker1.GetBody().BodyFrame.GetCoord_dt().rot), (marker1.FrameMoving.GetCoord().rot)))));
}
// q_8 = q''qqq + 2q'q'qq + 2q'qq'q + 2q'qqq'
// + 2qq'q'q + 2qq'qq' + 2qqq'q' + qqqq''
temp2 = marker2.FrameMoving.GetCoord_dtdt().rot;
if (ChQuaternion.Qnotnull(temp2))
{
q_8 = ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord_dtdt().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord().rot),
ChQuaternion.Qcross(Body1.GetCoord().rot,
marker1.FrameMoving.GetCoord().rot))); // q_dtdt'm2 * q'o2 * q,o1 * q,m1
}
else
{
//q_8 = ChQuaternion.QNULL;
temp1 = marker1.FrameMoving.GetCoord_dtdt().rot;
}
if (ChQuaternion.Qnotnull(temp1))
{
qtemp1 = ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord().rot),
ChQuaternion.Qcross(Body1.GetCoord().rot,
marker1.FrameMoving.GetCoord_dtdt().rot))); // q'm2 * q'o2 * q,o1 * q_dtdt,m1
q_8 = ChQuaternion.Qadd(q_8, qtemp1);
}
temp2 = marker2.FrameMoving.GetCoord_dt().rot;
if (ChQuaternion.Qnotnull(temp2))
{
qtemp1 = ChQuaternion.Qcross(
ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord_dt().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord_dt().rot), ChQuaternion.Qcross(Body1.GetCoord().rot, marker1.FrameMoving.GetCoord().rot)));
qtemp1 = ChQuaternion.Qscale(qtemp1, 2); // 2( q_dt'm2 * q_dt'o2 * q,o1 * q,m1)
q_8 = ChQuaternion.Qadd(q_8, qtemp1);
}
temp2 = marker2.FrameMoving.GetCoord_dt().rot;
if (ChQuaternion.Qnotnull(temp2))
{
qtemp1 = ChQuaternion.Qcross(
ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord_dt().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord().rot), ChQuaternion.Qcross(Body1.GetCoord_dt().rot, marker1.FrameMoving.GetCoord().rot)));
qtemp1 = ChQuaternion.Qscale(qtemp1, 2); // 2( q_dt'm2 * q'o2 * q_dt,o1 * q,m1)
q_8 = ChQuaternion.Qadd(q_8, qtemp1);
}
temp1 = marker1.FrameMoving.GetCoord_dt().rot;
temp2 = marker2.FrameMoving.GetCoord_dt().rot;
if (ChQuaternion.Qnotnull(temp2) && ChQuaternion.Qnotnull(temp1))
{
qtemp1 = ChQuaternion.Qcross(
ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord_dt().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord().rot), ChQuaternion.Qcross(Body1.GetCoord().rot, marker1.FrameMoving.GetCoord_dt().rot)));
qtemp1 = ChQuaternion.Qscale(qtemp1, 2); // 2( q_dt'm2 * q'o2 * q,o1 * q_dt,m1)
q_8 = ChQuaternion.Qadd(q_8, qtemp1);
}
qtemp1 =
ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord_dt().rot), ChQuaternion.Qcross(Body1.GetCoord_dt().rot, marker1.FrameMoving.GetCoord().rot)));
qtemp1 = ChQuaternion.Qscale(qtemp1, 2); // 2( q'm2 * q_dt'o2 * q_dt,o1 * q,m1)
q_8 = ChQuaternion.Qadd(q_8, qtemp1);
temp1 = marker1.FrameMoving.GetCoord_dt().rot;
if (ChQuaternion.Qnotnull(temp1))
{
qtemp1 = ChQuaternion.Qcross(
ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord_dt().rot), ChQuaternion.Qcross(Body1.GetCoord().rot, marker1.FrameMoving.GetCoord_dt().rot)));
qtemp1 = ChQuaternion.Qscale(qtemp1, 2); // 2( q'm2 * q_dt'o2 * q,o1 * q_dt,m1)
q_8 = ChQuaternion.Qadd(q_8, qtemp1);
}
temp1 = marker1.FrameMoving.GetCoord_dt().rot;
if (ChQuaternion.Qnotnull(temp1))
{
qtemp1 = ChQuaternion.Qcross(
ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord().rot), ChQuaternion.Qcross(Body1.GetCoord_dt().rot, marker1.FrameMoving.GetCoord_dt().rot)));
qtemp1 = ChQuaternion.Qscale(qtemp1, 2); // 2( q'm2 * q'o2 * q_dt,o1 * q_dt,m1)
q_8 = ChQuaternion.Qadd(q_8, qtemp1);
}
// q_4 = [Adtdt]'[A]'q + 2[Adt]'[Adt]'q
// + 2[Adt]'[A]'qdt + 2[A]'[Adt]'qdt
// ChMatrix33<double> m2_Rel_A_dt = new ChMatrix33<double>(0);
marker2.FrameMoving.Compute_Adt(ref m2_Rel_A_dt);
// ChMatrix33<double> m2_Rel_A_dtdt = new ChMatrix33<double>(0);
marker2.FrameMoving.Compute_Adtdt(ref m2_Rel_A_dtdt);
vtemp1 = Body2.GetA_dt().MatrT_x_Vect(PQw);
vtemp2 = m2_Rel_A_dt.MatrT_x_Vect(vtemp1);
q_4 = ChVector.Vmul(vtemp2, 2); // 2[Aq_dt]'[Ao2_dt]'*Qpq,w
vtemp1 = Body2.GetA().MatrT_x_Vect(PQw_dt);
vtemp2 = m2_Rel_A_dt.MatrT_x_Vect(vtemp1);
vtemp2 = ChVector.Vmul(vtemp2, 2); // 2[Aq_dt]'[Ao2]'*Qpq,w_dt
q_4 = ChVector.Vadd(q_4, vtemp2);
vtemp1 = Body2.GetA_dt().MatrT_x_Vect(PQw_dt);
vtemp2 = marker2.FrameMoving.GetA().MatrT_x_Vect(vtemp1);
vtemp2 = ChVector.Vmul(vtemp2, 2); // 2[Aq]'[Ao2_dt]'*Qpq,w_dt
q_4 = ChVector.Vadd(q_4, vtemp2);
vtemp1 = Body2.GetA().MatrT_x_Vect(PQw);
vtemp2 = m2_Rel_A_dtdt.MatrT_x_Vect(vtemp1);
q_4 = ChVector.Vadd(q_4, vtemp2); // [Aq_dtdt]'[Ao2]'*Qpq,w
// ----------- RELATIVE MARKER COORDINATES
// relM.pos
relM.pos = marker2.FrameMoving.GetA().MatrT_x_Vect(Body2.GetA().MatrT_x_Vect(PQw));
// relM.rot
relM.rot = ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.GetBody().BodyFrame.GetCoord().rot),
ChQuaternion.Qcross((marker1.GetBody().BodyFrame.GetCoord().rot), (marker1.FrameMoving.GetCoord().rot))));
// relM_dt.pos
relM_dt.pos = ChVector.Vadd(ChVector.Vadd(m2_Rel_A_dt.MatrT_x_Vect(Body2.GetA().MatrT_x_Vect(PQw)),
marker2.FrameMoving.GetA().MatrT_x_Vect(Body2.GetA_dt().MatrT_x_Vect(PQw))),
marker2.FrameMoving.GetA().MatrT_x_Vect(Body2.GetA().MatrT_x_Vect(PQw_dt)));
// relM_dt.rot
relM_dt.rot = ChQuaternion.Qadd(q_AD, q_BC);
// relM_dtdt.pos
relM_dtdt.pos = ChVector.Vadd(ChVector.Vadd(marker2.FrameMoving.GetA().MatrT_x_Vect(Body2.GetA_dtdt().MatrT_x_Vect(PQw)),
marker2.FrameMoving.GetA().MatrT_x_Vect(Body2.GetA().MatrT_x_Vect(PQw_dtdt))),
q_4);
// relM_dtdt.rot
qtemp1 = ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord_dtdt().rot),
ChQuaternion.Qcross(Body1.GetCoord().rot,
marker1.FrameMoving.GetCoord().rot))); // ( q'm2 * q_dtdt'o2 * q,o1 * q,m1)
relM_dtdt.rot = ChQuaternion.Qadd(q_8, qtemp1);
qtemp1 = ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord().rot),
ChQuaternion.Qcross(Body1.GetCoord_dtdt().rot,
marker1.FrameMoving.GetCoord().rot))); // ( q'm2 * q'o2 * q_dtdt,o1 * q,m1)
relM_dtdt.rot = ChQuaternion.Qadd(relM_dtdt.rot, qtemp1); // = q_8 + qq''qq + qqq''q
// ... and also "user-friendly" relative coordinates:
// relAngle and relAxis
ChQuaternion.Q_to_AngAxis(relM.rot, ref relAngle, ref relAxis);
// flip rel rotation axis if jerky sign
if (relAxis.z < 0)
{
relAxis = ChVector.Vmul(relAxis, -1);
relAngle = -relAngle;
}
// rotation axis
relRotaxis = ChVector.Vmul(relAxis, relAngle);
// relWvel
ChFrame <double> .SetMatrix_Gw(ref relGw, relM.rot); // relGw.Set_Gw_matrix(relM.rot);
relWvel = relGw.matrix.Matr34_x_Quat(relM_dt.rot);
// relWacc
relWacc = relGw.matrix.Matr34_x_Quat(relM_dtdt.rot);
}
/// This function transforms a direction from 'this' local coordinate
/// system to parent frame coordinate system.
/// \return The direction in local frame coordinate
public virtual ChVector TransformDirectionParentToLocal(
ChVector mdirection //< direction to transform, given in parent coordinates
)
{
return(Amatrix.MatrT_x_Vect(mdirection));
}
//
// UPDATING FUNCTIONS
//
/// Perform the update of this joint at the specified time: compute jacobians
/// and constraint violations, cache in internal structures
public override void update(double time, bool update_assets = true)
{
// Inherit time changes of parent class
base.UpdateTime(time);
// Express the joint frames in absolute frame
ChFrame <double> frame1_abs = m_frame1.BitShiftRight(Body1);
ChFrame <double> frame2_abs = m_frame2.BitShiftRight(Body2);
// Calculate violations of the spherical constraints
m_C.matrix.SetElement(0, 0, frame2_abs.coord.pos.x - frame1_abs.coord.pos.x);
m_C.matrix.SetElement(1, 0, frame2_abs.coord.pos.y - frame1_abs.coord.pos.y);
m_C.matrix.SetElement(2, 0, frame2_abs.coord.pos.z - frame1_abs.coord.pos.z);
// Compute Jacobian of the spherical constraints
// pos2_abs - pos1_abs = 0
{
ChMatrix33 <double> tilde1 = new ChMatrix33 <double>(0);
ChMatrix33 <double> tilde2 = new ChMatrix33 <double>(0);
tilde1.Set_X_matrix(m_frame1.coord.pos);
tilde2.Set_X_matrix(m_frame2.coord.pos);
ChMatrix33 <double> Phi_pi1 = Body1.GetA() * tilde1;
ChMatrix33 <double> Phi_pi2 = Body2.GetA() * tilde2;
m_cnstr_x.Get_Cq_a().ElementN(0) = -1;
m_cnstr_x.Get_Cq_b().ElementN(0) = +1;
m_cnstr_x.Get_Cq_a().ElementN(1) = 0;
m_cnstr_x.Get_Cq_b().ElementN(1) = 0;
m_cnstr_x.Get_Cq_a().ElementN(2) = 0;
m_cnstr_x.Get_Cq_b().ElementN(2) = 0;
m_cnstr_x.Get_Cq_a().ElementN(3) = Phi_pi1.nm.matrix[0, 0];
m_cnstr_x.Get_Cq_b().ElementN(3) = -Phi_pi2.nm.matrix[0, 0];
m_cnstr_x.Get_Cq_a().ElementN(4) = Phi_pi1.nm.matrix[0, 1];
m_cnstr_x.Get_Cq_b().ElementN(4) = -Phi_pi2.nm.matrix[0, 1];
m_cnstr_x.Get_Cq_a().ElementN(5) = Phi_pi1.nm.matrix[0, 2];
m_cnstr_x.Get_Cq_b().ElementN(5) = -Phi_pi2.nm.matrix[0, 2];
m_cnstr_y.Get_Cq_a().ElementN(0) = 0;
m_cnstr_y.Get_Cq_b().ElementN(0) = 0;
m_cnstr_y.Get_Cq_a().ElementN(1) = -1;
m_cnstr_y.Get_Cq_b().ElementN(1) = +1;
m_cnstr_y.Get_Cq_a().ElementN(2) = 0;
m_cnstr_y.Get_Cq_b().ElementN(2) = 0;
m_cnstr_y.Get_Cq_a().ElementN(3) = Phi_pi1.nm.matrix[1, 0];
m_cnstr_y.Get_Cq_b().ElementN(3) = -Phi_pi2.nm.matrix[1, 0];
m_cnstr_y.Get_Cq_a().ElementN(4) = Phi_pi1.nm.matrix[1, 1];
m_cnstr_y.Get_Cq_b().ElementN(4) = -Phi_pi2.nm.matrix[1, 1];
m_cnstr_y.Get_Cq_a().ElementN(5) = Phi_pi1.nm.matrix[1, 2];
m_cnstr_y.Get_Cq_b().ElementN(5) = -Phi_pi2.nm.matrix[1, 2];
m_cnstr_z.Get_Cq_a().ElementN(0) = 0;
m_cnstr_z.Get_Cq_b().ElementN(0) = 0;
m_cnstr_z.Get_Cq_a().ElementN(1) = 0;
m_cnstr_z.Get_Cq_b().ElementN(1) = 0;
m_cnstr_z.Get_Cq_a().ElementN(2) = -1;
m_cnstr_z.Get_Cq_b().ElementN(2) = +1;
m_cnstr_z.Get_Cq_a().ElementN(3) = Phi_pi1.nm.matrix[2, 0];
m_cnstr_z.Get_Cq_b().ElementN(3) = -Phi_pi2.nm.matrix[2, 0];
m_cnstr_z.Get_Cq_a().ElementN(4) = Phi_pi1.nm.matrix[2, 1];
m_cnstr_z.Get_Cq_b().ElementN(4) = -Phi_pi2.nm.matrix[2, 1];
m_cnstr_z.Get_Cq_a().ElementN(5) = Phi_pi1.nm.matrix[2, 2];
m_cnstr_z.Get_Cq_b().ElementN(5) = -Phi_pi2.nm.matrix[2, 2];
}
// Calculate violation of the dot constraint
ChVector u1 = frame1_abs.GetA().Get_A_Xaxis();
ChVector v2 = frame2_abs.GetA().Get_A_Yaxis();
m_C.matrix.SetElement(3, 0, ChVector.Vdot(u1, v2));
// Compute Jacobian of the dot constraint
// dot(u1_abs, v2_abs) = 0
{
ChMatrix33 <double> mat1 = Body1.GetA() * m_u1_tilde;
ChMatrix33 <double> mat2 = Body2.GetA() * m_v2_tilde;
ChVector Phi_pi1 = mat1.MatrT_x_Vect(v2);
ChVector Phi_pi2 = mat2.MatrT_x_Vect(u1);
m_cnstr_dot.Get_Cq_a().ElementN(0) = 0;
m_cnstr_dot.Get_Cq_a().ElementN(1) = 0;
m_cnstr_dot.Get_Cq_a().ElementN(2) = 0;
m_cnstr_dot.Get_Cq_a().ElementN(3) = -Phi_pi1.x;
m_cnstr_dot.Get_Cq_a().ElementN(4) = -Phi_pi1.y;
m_cnstr_dot.Get_Cq_a().ElementN(5) = -Phi_pi1.z;
m_cnstr_dot.Get_Cq_b().ElementN(0) = 0;
m_cnstr_dot.Get_Cq_b().ElementN(1) = 0;
m_cnstr_dot.Get_Cq_b().ElementN(2) = 0;
m_cnstr_dot.Get_Cq_b().ElementN(3) = -Phi_pi2.x;
m_cnstr_dot.Get_Cq_b().ElementN(4) = -Phi_pi2.y;
m_cnstr_dot.Get_Cq_b().ElementN(5) = -Phi_pi2.z;
}
}
