/// Someone (ex. an ChExternalObject() ) may send this message to
/// the marker to tell that time has changed (even if simulation is
/// not running! - so it is different from the usual UpdateTime() -)
public void UpdatedExternalTime(double prevtime, double mtime)
{
double mstep = mtime - prevtime;
ChCoordsys m_rel_pos_dt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));
ChCoordsys m_rel_pos_dtdt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));
// do not try to switch on the M_MOTION_KEYFRAMED mode if
// we are already in the M_MOTION_EXTERNAL mode, maybe because
// a link point-surface is already moving the marker and
// it will handle the accelerations by itself
if (this.motion_type == eChMarkerMotion.M_MOTION_EXTERNAL)
{
return;
}
// otherwise see if a BDF is needed, cause an external 3rd party is moving the marker
this.motion_type = eChMarkerMotion.M_MOTION_FUNCTIONS;
// if POSITION or ROTATION ("rel_pos") has been changed in acceptable time step...
if ((!(ChVector.Vequal(FrameMoving.coord.pos, last_rel_coord.pos)) || !(ChQuaternion.Qequal(FrameMoving.coord.rot, last_rel_coord.rot))) && (Math.Abs(mstep) < 0.1) &&
(mstep != 0))
{
// ... and if motion wasn't caused by motion laws, then it was a keyframed movement!
if ((motion_X.Get_y(mtime) == 0) && (motion_Y.Get_y(mtime) == 0) && (motion_Z.Get_y(mtime) == 0) &&
(motion_ang.Get_y(mtime) == 0) && (motion_X.Get_Type() == ChFunction.FunctionType.FUNCT_CONST) &&
(motion_Y.Get_Type() == ChFunction.FunctionType.FUNCT_CONST) && (motion_Z.Get_Type() == ChFunction.FunctionType.FUNCT_CONST) &&
(motion_ang.Get_Type() == ChFunction.FunctionType.FUNCT_CONST))
{
// compute the relative speed by BDF !
m_rel_pos_dt.pos = ChVector.Vmul(ChVector.Vsub(FrameMoving.coord.pos, last_rel_coord.pos), 1 / mstep);
m_rel_pos_dt.rot = ChQuaternion.Qscale(ChQuaternion.Qsub(FrameMoving.coord.rot, last_rel_coord.rot), 1 / mstep);
// compute the relative acceleration by BDF !
m_rel_pos_dtdt.pos = ChVector.Vmul(ChVector.Vsub(m_rel_pos_dt.pos, last_rel_coord_dt.pos), 1 / mstep);
m_rel_pos_dtdt.rot = ChQuaternion.Qscale(ChQuaternion.Qsub(m_rel_pos_dt.rot, last_rel_coord_dt.rot), 1 / mstep);
// Set the position, speed and acceleration in relative space,
// automatically getting also the absolute values,
FrameMoving.SetCoord_dt(m_rel_pos_dt);
FrameMoving.SetCoord_dtdt(m_rel_pos_dtdt);
// update the remaining state variables
this.UpdateState();
// remember that the movement of this guy won't need further update
// of speed and acc. via motion laws!
this.motion_type = eChMarkerMotion.M_MOTION_KEYFRAMED;
}
}
// restore state buffers and that's all.
last_time = ChTime;
last_rel_coord = (ChCoordsys)FrameMoving.coord;
last_rel_coord_dt = (ChCoordsys)FrameMoving.coord_dt;
}
/// Transform generic cartesian force into absolute force+torque applied to body COG.
/// If local=1, force & application point are intended as expressed in local
/// coordinates, if =0, in absolute.
public void To_abs_forcetorque(ChVector force,
ChVector appl_point,
bool local,
ref ChVector resultforce,
ref ChVector resulttorque)
{
if (local)
{
// local space
ChVector mforce_abs = TransformDirectionLocalToParent(force);
resultforce = mforce_abs;
resulttorque = ChVector.Vcross(TransformDirectionLocalToParent(appl_point), mforce_abs);
}
else
{
// absolute space
resultforce = force;
resulttorque = ChVector.Vcross(ChVector.Vsub(appl_point, coord.pos), force);
}
}
/// Updates motion laws, marker positions, etc.
public override void UpdateTime(double mytime)
{
// First, inherit to parent class
base.UpdateTime(mytime);
ChFrame <double> abs_shaft1 = ChFrame <double> .FNULL; //new ChFrame<double>();
ChFrame <double> abs_shaft2 = ChFrame <double> .FNULL; //new ChFrame<double>();
((ChFrame <double>)Body1).TransformLocalToParent(local_shaft1, abs_shaft1);
((ChFrame <double>)Body2).TransformLocalToParent(local_shaft2, abs_shaft2);
ChVector dcc_w = ChVector.Vsub(Get_shaft_pos2(), Get_shaft_pos1());
// compute actual rotation of the two wheels (relative to truss).
ChVector md1 = abs_shaft1.GetA().MatrT_x_Vect(dcc_w);
md1.z = 0;
md1 = ChVector.Vnorm(md1);
ChVector md2 = abs_shaft2.GetA().MatrT_x_Vect(dcc_w);
md2.z = 0;
md2 = ChVector.Vnorm(md2);
double periodic_a1 = ChMaths.ChAtan2(md1.x, md1.y);
double periodic_a2 = ChMaths.ChAtan2(md2.x, md2.y);
double old_a1 = a1;
double old_a2 = a2;
double turns_a1 = Math.Floor(old_a1 / ChMaths.CH_C_2PI);
double turns_a2 = Math.Floor(old_a2 / ChMaths.CH_C_2PI);
double a1U = turns_a1 * ChMaths.CH_C_2PI + periodic_a1 + ChMaths.CH_C_2PI;
double a1M = turns_a1 * ChMaths.CH_C_2PI + periodic_a1;
double a1L = turns_a1 * ChMaths.CH_C_2PI + periodic_a1 - ChMaths.CH_C_2PI;
a1 = a1M;
if (Math.Abs(a1U - old_a1) < Math.Abs(a1M - old_a1))
{
a1 = a1U;
}
if (Math.Abs(a1L - a1) < Math.Abs(a1M - a1))
{
a1 = a1L;
}
double a2U = turns_a2 * ChMaths.CH_C_2PI + periodic_a2 + ChMaths.CH_C_2PI;
double a2M = turns_a2 * ChMaths.CH_C_2PI + periodic_a2;
double a2L = turns_a2 * ChMaths.CH_C_2PI + periodic_a2 - ChMaths.CH_C_2PI;
a2 = a2M;
if (Math.Abs(a2U - old_a2) < Math.Abs(a2M - old_a2))
{
a2 = a2U;
}
if (Math.Abs(a2L - a2) < Math.Abs(a2M - a2))
{
a2 = a2L;
}
// correct marker positions if phasing is not correct
double m_delta = 0;
if (checkphase)
{
double realtau = tau;
m_delta = a1 - phase - (a2 / realtau);
if (m_delta > ChMaths.CH_C_PI)
{
m_delta -= (ChMaths.CH_C_2PI); // range -180..+180 is better than 0...360
}
if (m_delta > (ChMaths.CH_C_PI / 4.0))
{
m_delta = (ChMaths.CH_C_PI / 4.0); // phase correction only in +/- 45°
}
if (m_delta < -(ChMaths.CH_C_PI / 4.0))
{
m_delta = -(ChMaths.CH_C_PI / 4.0);
}
//***TODO***
}
// Move markers 1 and 2 to align them as pulley ends
ChVector d21_w = dcc_w - Get_shaft_dir1() * ChVector.Vdot(Get_shaft_dir1(), dcc_w);
ChVector D21_w = ChVector.Vnorm(d21_w);
shaft_dist = d21_w.Length();
ChVector U1_w = ChVector.Vcross(Get_shaft_dir1(), D21_w);
double gamma1 = Math.Acos((r1 - r2) / shaft_dist);
ChVector Ru_w = D21_w * Math.Cos(gamma1) + U1_w * Math.Sin(gamma1);
ChVector Rl_w = D21_w * Math.Cos(gamma1) - U1_w * Math.Sin(gamma1);
belt_up1 = Get_shaft_pos1() + Ru_w * r1;
belt_low1 = Get_shaft_pos1() + Rl_w * r1;
belt_up2 = Get_shaft_pos1() + d21_w + Ru_w * r2;
belt_low2 = Get_shaft_pos1() + d21_w + Rl_w * r2;
// marker alignment
ChMatrix33 <double> maU = new ChMatrix33 <double>(0);
ChMatrix33 <double> maL = new ChMatrix33 <double>(0);
ChVector Dxu = ChVector.Vnorm(belt_up2 - belt_up1);
ChVector Dyu = Ru_w;
ChVector Dzu = ChVector.Vnorm(ChVector.Vcross(Dxu, Dyu));
Dyu = ChVector.Vnorm(ChVector.Vcross(Dzu, Dxu));
maU.Set_A_axis(Dxu, Dyu, Dzu);
// ! Require that the BDF routine of marker won't handle speed and acc.calculus of the moved marker 2!
marker2.SetMotionType(ChMarker.eChMarkerMotion.M_MOTION_EXTERNAL);
marker1.SetMotionType(ChMarker.eChMarkerMotion.M_MOTION_EXTERNAL);
ChCoordsys newmarkpos = new ChCoordsys();
// move marker1 in proper positions
newmarkpos.pos = this.belt_up1;
newmarkpos.rot = maU.Get_A_quaternion();
marker1.Impose_Abs_Coord(newmarkpos); // move marker1 into teeth position
// move marker2 in proper positions
newmarkpos.pos = this.belt_up2;
newmarkpos.rot = maU.Get_A_quaternion();
marker2.Impose_Abs_Coord(newmarkpos); // move marker2 into teeth position
double phase_correction_up = m_delta * r1;
double phase_correction_low = -phase_correction_up;
double hU = ChVector.Vlength(belt_up2 - belt_up1) + phase_correction_up;
double hL = ChVector.Vlength(belt_low2 - belt_low1) + phase_correction_low;
// imposed relative positions/speeds
deltaC.pos = new ChVector(-hU, 0, 0);
deltaC_dt.pos = ChVector.VNULL;
deltaC_dtdt.pos = ChVector.VNULL;
deltaC.rot = ChQuaternion.QUNIT; // no relative rotations imposed!
deltaC_dt.rot = ChQuaternion.QNULL;
deltaC_dtdt.rot = ChQuaternion.QNULL;
}
/// 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
//
/// 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++;
}
}
}
// Updates motion laws, marker positions, etc.
public override void UpdateTime(double mytime)
{
// First, inherit to parent class
base.UpdateTime(mytime);
// If LEARN MODE, just record motion
if (learn)
{
/* do not change deltas, in free mode maybe that 'limit on X' changed them
* deltaC.pos = VNULL;
* deltaC_dt.pos = VNULL;
* deltaC_dtdt.pos = VNULL;
* deltaC.rot = QUNIT;
* deltaC_dt.rot = QNULL;
* deltaC_dtdt.rot = QNULL;
*/
// if (dist_funct.Get_Type() != ChFunction.FunctionType.FUNCT_RECORDER)
// dist_funct = new ChFunction_Recorder();
// record point
double rec_dist = ChVector.Vlength(ChVector.Vsub(marker1.GetAbsCoord().pos, marker2.GetAbsCoord().pos));
rec_dist -= offset;
// (ChFunction_Recorder)(dist_funct).AddPoint(mytime, rec_dist, 1); // (x,y,w) x=t
}
// Move (well, rotate...) marker 2 to align it in actuator direction
// ! Require that the BDF routine of marker won't handle speed and acc.calculus of the moved marker 2!
marker2.SetMotionType(ChMarker.eChMarkerMotion.M_MOTION_EXTERNAL);
// ChMatrix33<double> ma = new ChMatrix33<double>(0);
ma.Set_A_quaternion(marker2.GetAbsCoord().rot);
ChVector absdist = ChVector.Vsub(marker1.GetAbsCoord().pos, marker2.GetAbsCoord().pos);
ChVector mx = ChVector.Vnorm(absdist);
ChVector my = ma.Get_A_Yaxis();
if (ChVector.Vequal(mx, my))
{
if (mx.x == 1.0)
{
my = ChVector.VECT_Y;
}
else
{
my = ChVector.VECT_X;
}
}
ChVector mz = ChVector.Vnorm(ChVector.Vcross(mx, my));
my = ChVector.Vnorm(ChVector.Vcross(mz, mx));
ma.Set_A_axis(mx, my, mz);
ChCoordsys newmarkpos;
ChVector oldpos = marker2.FrameMoving.GetPos(); // backup to avoid numerical err.accumulation
newmarkpos.pos = marker2.GetAbsCoord().pos;
newmarkpos.rot = ma.Get_A_quaternion();
marker2.Impose_Abs_Coord(newmarkpos); // rotate "main" marker2 into tangent position (may add err.accumulation)
marker2.FrameMoving.SetPos(oldpos); // backup to avoid numerical err.accumulation
if (learn)
{
return; // no need to go on further...--.>>>
}
// imposed relative positions/speeds
deltaC.pos = ChVector.VNULL;
deltaC.pos.x = dist_funct.Get_y(ChTime) + offset; // distance is always on M2 'X' axis
deltaC_dt.pos = ChVector.VNULL;
deltaC_dt.pos.x = dist_funct.Get_y_dx(ChTime); // distance speed
deltaC_dtdt.pos = ChVector.VNULL;
deltaC_dtdt.pos.x = dist_funct.Get_y_dxdx(ChTime); // distance acceleration
// add also the centripetal acceleration if distance vector's rotating,
// as centripetal acc. of point sliding on a sphere surface.
ChVector tang_speed = GetRelM_dt().pos;
tang_speed.x = 0; // only z-y coords in relative tang speed vector
double len_absdist = ChVector.Vlength(absdist); // don't divide by zero
if (len_absdist > 1E-6)
{
deltaC_dtdt.pos.x -= Math.Pow(ChVector.Vlength(tang_speed), 2) / ChVector.Vlength(absdist); // An = Adelta -(Vt^2 / r)
}
deltaC.rot = ChQuaternion.QUNIT; // no relative rotations imposed!
deltaC_dt.rot = ChQuaternion.QNULL;
deltaC_dtdt.rot = ChQuaternion.QNULL;
// Compute motor variables
// double m_rotation;
// double m_torque;
mot_rerot = (deltaC.pos.x - offset) / mot_tau;
mot_rerot_dt = deltaC_dt.pos.x / mot_tau;
mot_rerot_dtdt = deltaC_dtdt.pos.x / mot_tau;
mot_retorque = mot_rerot_dtdt * mot_inertia + (react_force.x * mot_tau) / mot_eta;
// m_rotation = (deltaC.pos.x() - offset) / mot_tau;
// m_torque = (deltaC_dtdt.pos.x() / mot_tau) * mot_inertia + (react_force.x() * mot_tau) / mot_eta;
if (learn_torque_rotation)
{
// if (mot_torque.Get_Type() != ChFunction.FunctionType.FUNCT_RECORDER)
// mot_torque = new ChFunction_Recorder();
// if (mot_rot.Get_Type() != ChFunction.FunctionType.FUNCT_RECORDER)
// mot_rot = new ChFunction_Recorder();
// std::static_pointer_cast<ChFunction_Recorder>(mot_torque).AddPoint(mytime, mot_retorque, 1); // (x,y,w) x=t
// std::static_pointer_cast<ChFunction_Recorder>(mot_rot).AddPoint(mytime, mot_rerot, 1); // (x,y,w) x=t
}
}
// Updates motion laws, marker positions, etc.
public override void UpdateTime(double mytime)
{
// First, inherit to parent class
base.UpdateTime(mytime);
// Move markers 1 and 2 to align them as gear teeth
ChMatrix33 <double> ma1 = new ChMatrix33 <double>(0);
ChMatrix33 <double> ma2 = new ChMatrix33 <double>(0);
ChMatrix33 <double> mrotma = new ChMatrix33 <double>(0);
ChMatrix33 <double> marot_beta = new ChMatrix33 <double>(0);
ChVector mx;
ChVector my;
ChVector mz;
ChVector mr;
ChVector mmark1;
ChVector mmark2;
ChVector lastX;
ChVector vrota;
ChCoordsys newmarkpos = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));
ChFrame <double> abs_shaft1 = ChFrame <double> .FNULL; // new ChFrame<double>();
ChFrame <double> abs_shaft2 = ChFrame <double> .FNULL; //new ChFrame<double>();
((ChFrame <double>)Body1).TransformLocalToParent(local_shaft1, abs_shaft1);
((ChFrame <double>)Body2).TransformLocalToParent(local_shaft2, abs_shaft2);
ChVector vbdist = ChVector.Vsub(Get_shaft_pos1(), Get_shaft_pos2());
// ChVector Trad1 = ChVector.Vnorm(ChVector.Vcross(Get_shaft_dir1(), ChVector.Vnorm(ChVector.Vcross(Get_shaft_dir1(), vbdist))));
// ChVector Trad2 = ChVector.Vnorm(ChVector.Vcross(ChVector.Vnorm(ChVector.Vcross(Get_shaft_dir2(), vbdist)), Get_shaft_dir2()));
double dist = ChVector.Vlength(vbdist);
// compute actual rotation of the two wheels (relative to truss).
ChVector md1 = abs_shaft1.GetA().MatrT_x_Vect(-vbdist);
md1.z = 0;
md1 = ChVector.Vnorm(md1);
ChVector md2 = abs_shaft2.GetA().MatrT_x_Vect(-vbdist);
md2.z = 0;
md2 = ChVector.Vnorm(md2);
double periodic_a1 = ChMaths.ChAtan2(md1.x, md1.y);
double periodic_a2 = ChMaths.ChAtan2(md2.x, md2.y);
double old_a1 = a1;
double old_a2 = a2;
double turns_a1 = Math.Floor(old_a1 / ChMaths.CH_C_2PI);
double turns_a2 = Math.Floor(old_a2 / ChMaths.CH_C_2PI);
double a1U = turns_a1 * ChMaths.CH_C_2PI + periodic_a1 + ChMaths.CH_C_2PI;
double a1M = turns_a1 * ChMaths.CH_C_2PI + periodic_a1;
double a1L = turns_a1 * ChMaths.CH_C_2PI + periodic_a1 - ChMaths.CH_C_2PI;
a1 = a1M;
if (Math.Abs(a1U - old_a1) < Math.Abs(a1M - old_a1))
{
a1 = a1U;
}
if (Math.Abs(a1L - a1) < Math.Abs(a1M - a1))
{
a1 = a1L;
}
double a2U = turns_a2 * ChMaths.CH_C_2PI + periodic_a2 + ChMaths.CH_C_2PI;
double a2M = turns_a2 * ChMaths.CH_C_2PI + periodic_a2;
double a2L = turns_a2 * ChMaths.CH_C_2PI + periodic_a2 - ChMaths.CH_C_2PI;
a2 = a2M;
if (Math.Abs(a2U - old_a2) < Math.Abs(a2M - old_a2))
{
a2 = a2U;
}
if (Math.Abs(a2L - a2) < Math.Abs(a2M - a2))
{
a2 = a2L;
}
// compute new markers coordsystem alignment
my = ChVector.Vnorm(vbdist);
mz = Get_shaft_dir1();
mx = ChVector.Vnorm(ChVector.Vcross(my, mz));
mr = ChVector.Vnorm(ChVector.Vcross(mz, mx));
mz = ChVector.Vnorm(ChVector.Vcross(mx, my));
ChVector mz2, mx2, mr2, my2;
my2 = my;
mz2 = Get_shaft_dir2();
mx2 = ChVector.Vnorm(ChVector.Vcross(my2, mz2));
mr2 = ChVector.Vnorm(ChVector.Vcross(mz2, mx2));
ma1.Set_A_axis(mx, my, mz);
// rotate csys because of beta
vrota.x = 0.0;
vrota.y = beta;
vrota.z = 0.0;
mrotma.Set_A_Rxyz(vrota);
marot_beta.nm.matrix.MatrMultiply(ma1.nm.matrix, mrotma.nm.matrix);
// rotate csys because of alpha
vrota.x = 0.0;
vrota.y = 0.0;
vrota.z = alpha;
if (react_force.x < 0)
{
vrota.z = alpha;
}
else
{
vrota.z = -alpha;
}
mrotma.Set_A_Rxyz(vrota);
ma1.nm.matrix.MatrMultiply(marot_beta.nm.matrix, mrotma.nm.matrix);
ma2.nm.matrix.CopyFromMatrix(ma1.nm.matrix);
// is a bevel gear?
double be = Math.Acos(ChVector.Vdot(Get_shaft_dir1(), Get_shaft_dir2()));
bool is_bevel = true;
if (Math.Abs(ChVector.Vdot(Get_shaft_dir1(), Get_shaft_dir2())) > 0.96)
{
is_bevel = false;
}
// compute wheel radii so that:
// w2 = - tau * w1
if (!is_bevel)
{
double pardist = ChVector.Vdot(mr, vbdist);
double inv_tau = 1.0 / tau;
if (!epicyclic)
{
r2 = pardist - pardist / (inv_tau + 1.0);
}
else
{
r2 = pardist - (tau * pardist) / (tau - 1.0);
}
r1 = r2 * tau;
}
else
{
double gamma2;
if (!epicyclic)
{
gamma2 = be / (tau + 1.0);
}
else
{
gamma2 = be / (-tau + 1.0);
}
double al = ChMaths.CH_C_PI - Math.Acos(ChVector.Vdot(Get_shaft_dir2(), my));
double te = ChMaths.CH_C_PI - al - be;
double fd = Math.Sin(te) * (dist / Math.Sin(be));
r2 = fd * Math.Tan(gamma2);
r1 = r2 * tau;
}
// compute markers positions, supposing they
// stay on the ideal wheel contact point
mmark1 = ChVector.Vadd(Get_shaft_pos2(), ChVector.Vmul(mr2, r2));
mmark2 = mmark1;
contact_pt = mmark1;
// correct marker 1 position if phasing is not correct
if (checkphase)
{
double realtau = tau;
if (epicyclic)
{
realtau = -tau;
}
double m_delta;
m_delta = -(a2 / realtau) - a1 - phase;
if (m_delta > ChMaths.CH_C_PI)
{
m_delta -= (ChMaths.CH_C_2PI); // range -180..+180 is better than 0...360
}
if (m_delta > (ChMaths.CH_C_PI / 4.0))
{
m_delta = (ChMaths.CH_C_PI / 4.0); // phase correction only in +/- 45°
}
if (m_delta < -(ChMaths.CH_C_PI / 4.0))
{
m_delta = -(ChMaths.CH_C_PI / 4.0);
}
vrota.x = vrota.y = 0.0;
vrota.z = -m_delta;
mrotma.Set_A_Rxyz(vrota); // rotate about Z of shaft to correct
mmark1 = abs_shaft1.GetA().MatrT_x_Vect(ChVector.Vsub(mmark1, Get_shaft_pos1()));
mmark1 = mrotma.Matr_x_Vect(mmark1);
mmark1 = ChVector.Vadd(abs_shaft1.GetA().Matr_x_Vect(mmark1), Get_shaft_pos1());
}
// Move Shaft 1 along its direction if not aligned to wheel
double offset = ChVector.Vdot(Get_shaft_dir1(), (contact_pt - Get_shaft_pos1()));
ChVector moff = Get_shaft_dir1() * offset;
if (Math.Abs(offset) > 0.0001)
{
local_shaft1.SetPos(local_shaft1.GetPos() + Body1.TransformDirectionParentToLocal(moff));
}
// ! Require that the BDF routine of marker won't handle speed and acc.calculus of the moved marker 2!
marker2.SetMotionType(ChMarker.eChMarkerMotion.M_MOTION_EXTERNAL);
marker1.SetMotionType(ChMarker.eChMarkerMotion.M_MOTION_EXTERNAL);
// move marker1 in proper positions
newmarkpos.pos = mmark1;
newmarkpos.rot = ma1.Get_A_quaternion();
marker1.Impose_Abs_Coord(newmarkpos); // move marker1 into teeth position
// move marker2 in proper positions
newmarkpos.pos = mmark2;
newmarkpos.rot = ma2.Get_A_quaternion();
marker2.Impose_Abs_Coord(newmarkpos); // move marker2 into teeth position
// imposed relative positions/speeds
deltaC.pos = ChVector.VNULL;
deltaC_dt.pos = ChVector.VNULL;
deltaC_dtdt.pos = ChVector.VNULL;
deltaC.rot = ChQuaternion.QUNIT; // no relative rotations imposed!
deltaC_dt.rot = ChQuaternion.QNULL;
deltaC_dtdt.rot = ChQuaternion.QNULL;
}
