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