Ejemplo n.º 1
0
        /// 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);
        }