//
// 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);
}
}
public override void update(double mytime, bool update_assets)
{
// Inherit parent class:
base.update(mytime, update_assets);
// Override the rotational jacobian [Cq] and the rotational residual C,
// by assuming an additional hidden frame that rotates about frame2:
if (this.Body1 != null && this.Body2 != null)
{
ChFrame <double> aframe1 = ChFrame <double> .BitShiftRight(this.frame1, (this.Body1));
ChFrame <double> aframe2 = ChFrame <double> .BitShiftRight(this.frame2, (this.Body2));
ChFrame <double> aframe12 = new ChFrame <double>();
aframe2.TransformParentToLocal(aframe1, aframe12);
ChFrame <double> aframe2rotating = new ChFrame <double>();
double aux_rotation;
if (this.avoid_angle_drift)
{
aux_rotation = this.aux_dt + this.rot_offset;
}
else
{
// to have it aligned to current rot, to allow C=0.
aux_rotation = aframe12.GetRot().Q_to_Rotv().z;
}
aframe2rotating.SetRot(aframe2.GetRot() * ChQuaternion.Q_from_AngAxis2(aux_rotation, ChVector.VECT_Z));
// TODO this needs to be addressed, with it it causes rotation problems, seems to work fine without the TransformParentToLocal?
ChFrame <double> aframe12rotating = new ChFrame <double>();
// aframe2rotating.TransformParentToLocal(aframe1, aframe12rotating);
ChMatrix33 <double> Jw1 = new ChMatrix33 <double>();
ChMatrix33 <double> Jw2 = new ChMatrix33 <double>();
ChMatrix33 <double> mtempM = new ChMatrix33 <double>();
ChMatrix33 <double> mtempQ = new ChMatrix33 <double>();
ChMatrix33 <double> abs_plane_rotating = aframe2rotating.GetA();
Jw1.nm.matrix.MatrTMultiply(abs_plane_rotating.nm.matrix, Body1.GetA().nm.matrix);
Jw2.nm.matrix.MatrTMultiply(abs_plane_rotating.nm.matrix, Body2.GetA().nm.matrix);
Jw2.nm.matrix.MatrNeg();
// TODO this also needs to be addressed, with it it causes rotation problems/
// Premultiply by Jw1 and Jw2 by 0.5*[Fp(q_resid)]' to get residual as imaginary part of a quaternion.
/* mtempM.Set_X_matrix((aframe12rotating.GetRot().GetVector()) * 0.5);
* mtempM[0, 0] = 0.5 * aframe12rotating.GetRot().e0;
* mtempM[1, 1] = 0.5 * aframe12rotating.GetRot().e0;
* mtempM[2, 2] = 0.5 * aframe12rotating.GetRot().e0;
* mtempQ.MatrTMultiply(mtempM, Jw1);
* Jw1 = mtempQ;
* mtempQ.MatrTMultiply(mtempM, Jw2);
* Jw2 = mtempQ;*/
int nc = 0;
if (c_x)
{
nc++;
}
if (c_y)
{
nc++;
}
if (c_z)
{
nc++;
}
if (c_rx)
{
this.C.matrix.ElementN(nc) = aframe12rotating.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) = aframe12rotating.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) = aframe12rotating.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++;
}
}
}
public override void update(double mytime, bool update_assets)
{
// Inherit parent class:
base.update(mytime, update_assets);
// Override the rotational jacobian [Cq] and the rotational residual C,
// by assuming an additional hidden frame that rotates about frame2:
if (this.Body1 != null && this.Body2 != null)
{
ChFrame <double> aframe1 = this.frame1.BitShiftRight(this.Body1);
ChFrame <double> aframe2 = this.frame2.BitShiftRight(this.Body2);
ChFrame <double> aframe12 = new ChFrame <double>();// ChFrame<double>.FNULL;
aframe2.TransformParentToLocal(aframe1, aframe12);
ChFrame <double> aframe2rotating = new ChFrame <double>();// ChFrame<double>.FNULL;
double aux_rotation;
aux_rotation = m_func.Get_y(mytime) + rot_offset;
aframe2rotating.SetRot(aframe2.GetRot() * ChQuaternion.Q_from_AngAxis2(aux_rotation, ChVector.VECT_Z));
ChFrame <double> aframe12rotating = new ChFrame <double>();
// aframe2rotating.TransformParentToLocal(aframe1, aframe12rotating);
ChMatrix33 <double> Jw1 = new ChMatrix33 <double>(0), Jw2 = new ChMatrix33 <double>(0);
ChMatrix33 <double> mtempM = new ChMatrix33 <double>(0), mtempQ = new ChMatrix33 <double>(0);
ChMatrix33 <double> abs_plane_rotating = aframe2rotating.GetA();
Jw1.nm.matrix.MatrTMultiply(abs_plane_rotating.nm.matrix, Body1.GetA().nm.matrix);
Jw2.nm.matrix.MatrTMultiply(abs_plane_rotating.nm.matrix, Body2.GetA().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.
/* mtempM.Set_X_matrix((aframe12rotating.GetRot().GetVector()) * 0.5);
* mtempM.nm.matrix[0, 0] = 0.5 * aframe12rotating.GetRot().e0;
* mtempM.nm.matrix[1, 1] = 0.5 * aframe12rotating.GetRot().e0;
* mtempM.nm.matrix[2, 2] = 0.5 * aframe12rotating.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)
{
nc++;
}
if (c_y)
{
nc++;
}
if (c_z)
{
nc++;
}
if (c_rx)
{
this.C.matrix.ElementN(nc) = aframe12rotating.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) = aframe12rotating.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) = aframe12rotating.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++;
}
}
}
/// 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);
}
//
// 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;
}
}
//
// 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++;
}
}
}
