/// Set body-relative coord. and update auxiliary variables
/// Also, current position becomes the 'resting position' coordinates
/// for the current time.
public void Impose_Rel_Coord(ChCoordsys m_coord)
{
ChQuaternion qtemp;// = new ChQuaternion(1, 0, 0, 0);
// set the actual coordinates
FrameMoving.SetCoord(m_coord);
// set the resting position coordinates
rest_coord.pos.x = m_coord.pos.x - motion_X.Get_y(ChTime);
rest_coord.pos.y = m_coord.pos.y - motion_Y.Get_y(ChTime);
rest_coord.pos.z = m_coord.pos.z - motion_Z.Get_y(ChTime);
qtemp = ChQuaternion.Q_from_AngAxis2(-(motion_ang.Get_y(ChTime)), motion_axis);
rest_coord.rot = ChQuaternion.Qcross(m_coord.rot, qtemp); // ***%%% check
// set also the absolute positions, and other.
UpdateState();
}
/// Set absolute coordinates and update auxiliary variables
/// Also, current position becomes the 'resting position' coordinates
/// for the current time.
public void Impose_Abs_Coord(ChCoordsys m_coord)
{
ChBody my_body;
my_body = GetBody();
ChCoordsys csys;// = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));//ChCoordsys.CSYSNULL;
// coordsys: transform the representation from the parent reference frame
// to the local reference frame.
csys.pos = ChTransform <double> .TransformParentToLocal(m_coord.pos, my_body.BodyFrame.GetCoord().pos, my_body.BodyFrame.GetA());
csys.rot = ChQuaternion.Qcross(ChQuaternion.Qconjugate(my_body.BodyFrame.GetCoord().rot), m_coord.rot);
// apply the imposition on local coordinate and resting coordinate:
Impose_Rel_Coord(csys);
}
//
// UPDATING
//
/// Updates the time.dependant variables (ex: ChFunction objects
/// which impose the body-relative motion, etc.)
public void UpdateTime(double mytime)
{
ChCoordsys csys = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));
ChCoordsys csys_dt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));
ChCoordsys csys_dtdt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));
ChQuaternion qtemp;// = new ChQuaternion(1, 0, 0, 0);
double ang, ang_dt, ang_dtdt;
ChTime = mytime;
// if a imposed motion (keyframed movement) affects the marker position (example,from R3D animation system),
// compute the speed and acceleration values by BDF (example,see the UpdatedExternalTime() function, later)
// so the updating via motion laws can be skipped!
if (motion_type == eChMarkerMotion.M_MOTION_KEYFRAMED)
{
return;
}
// skip relative-position-functions evaluation also if
// someone is already handling this from outside..
if (motion_type == eChMarkerMotion.M_MOTION_EXTERNAL)
{
return;
}
// positions:
// update positions: rel_pos
csys.pos.x = motion_X.Get_y(mytime);
csys.pos.y = motion_Y.Get_y(mytime);
csys.pos.z = motion_Z.Get_y(mytime);
if (motion_X.Get_Type() != ChFunction.FunctionType.FUNCT_MOCAP)
{
csys.pos += rest_coord.pos;
}
// update speeds: rel_pos_dt
csys_dt.pos.x = motion_X.Get_y_dx(mytime);
csys_dt.pos.y = motion_Y.Get_y_dx(mytime);
csys_dt.pos.z = motion_Z.Get_y_dx(mytime);
// update accelerations
csys_dtdt.pos.x = motion_X.Get_y_dxdx(mytime);
csys_dtdt.pos.y = motion_Y.Get_y_dxdx(mytime);
csys_dtdt.pos.z = motion_Z.Get_y_dxdx(mytime);
// rotations:
ang = motion_ang.Get_y(mytime);
ang_dt = motion_ang.Get_y_dx(mytime);
ang_dtdt = motion_ang.Get_y_dxdx(mytime);
if ((ang != 0) || (ang_dt != 0) || (ang_dtdt != 0))
{
// update q
ChVector motion_axis_versor = ChVector.Vnorm(motion_axis);
qtemp = ChQuaternion.Q_from_AngAxis2(ang, motion_axis_versor);
csys.rot = ChQuaternion.Qcross(qtemp, rest_coord.rot);
// update q_dt
csys_dt.rot = ChQuaternion.Qdt_from_AngAxis(csys.rot, ang_dt, motion_axis_versor);
// update q_dtdt
csys_dtdt.rot = ChQuaternion.Qdtdt_from_AngAxis(ang_dtdt, motion_axis_versor, csys.rot, csys_dt.rot);
}
else
{
csys.rot = FrameMoving.coord.rot; // rel_pos.rot;
csys_dt.rot = ChQuaternion.QNULL;
csys_dtdt.rot = ChQuaternion.QNULL;
}
// Set the position, speed and acceleration in relative space,
// automatically getting also the absolute values,
if (!(csys == this.FrameMoving.coord))
{
FrameMoving.SetCoord(csys);
}
if (!(csys_dt == this.FrameMoving.coord_dt) || !(csys_dt.rot == new ChQuaternion(0, 0, 0, 0)))
{
FrameMoving.SetCoord_dt(csys_dt);
}
if (!(csys_dtdt == this.FrameMoving.coord_dtdt) || !(csys_dtdt.rot == new ChQuaternion(0, 0, 0, 0)))
{
FrameMoving.SetCoord_dtdt(csys_dtdt);
}
}
/// 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);
}
