/// 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; }
/// 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); }