示例#1
0
 public void Set(ChQuaternion q)
 {
     this.e0 = q.e0;
     this.e1 = q.e1;
     this.e2 = q.e2;
     this.e3 = q.e3;
 }
示例#2
0
 /// Set the quaternion ddq/dtdt. Inputs: the axis of ang. acceleration 'axis' (assuming it is already
 /// normalized and expressed in absolute coords), the angular acceleration 'angle_dtdt' (scalar value),
 /// the rotation expressed as a quaternion 'quat' and th rotation speed 'q_dt'.
 public void Qdtdt_from_AngAxis(ChQuaternion q,
                                ChQuaternion q_dt,
                                double angle_dtdt,
                                ChVector axis)
 {
     this.Qdtdt_from_Aabs2(angle_dtdt * axis, q, q_dt);
 }
示例#3
0
        // TRANSFORMATIONS, USING POSITION AND ROTATION QUATERNION

        /// This function transforms a point from the parent coordinate
        /// system to a local coordinate system, whose relative position
        /// is given by the 'origin' translation and 'alignment' quaternion q.
        ///  Since the function is static, you do not need a ChTransform object, for example
        /// use it as: mresult=ChTransform<>::TransformParentToLocal(mpar, morig, malign)
        /// \return The point in local coordinate, as local=q*[(parent-origin)]*q

        public static ChVector TransformParentToLocal(
            ChVector parent,        //< point to transform, given in parent coordinates
            ChVector origin,        //< origin of frame respect to parent, in parent coords,
            ChQuaternion alignment  //< rotation of frame respect to parent, in parent coords.
            )
        {
            // It could be simply "return alignment.RotateBack(parent-origin);"
            // but for faster execution do this:
            double e0e0 = alignment.e0 * alignment.e0;
            double e1e1 = alignment.e1 * alignment.e1;
            double e2e2 = alignment.e2 * alignment.e2;
            double e3e3 = alignment.e3 * alignment.e3;
            double e0e1 = -alignment.e0 * alignment.e1;
            double e0e2 = -alignment.e0 * alignment.e2;
            double e0e3 = -alignment.e0 * alignment.e3;
            double e1e2 = alignment.e1 * alignment.e2;
            double e1e3 = alignment.e1 * alignment.e3;
            double e2e3 = alignment.e2 * alignment.e3;

            double dx = parent.x - origin.x;
            double dy = parent.y - origin.y;
            double dz = parent.z - origin.z;

            return(new ChVector(((e0e0 + e1e1) * 2.0 - 1.0) * dx + ((e1e2 - e0e3) * 2.0) * dy + ((e1e3 + e0e2) * 2.0) * dz,
                                ((e1e2 + e0e3) * 2.0) * dx + ((e0e0 + e2e2) * 2.0 - 1.0) * dy + ((e2e3 - e0e1) * 2.0) * dz,
                                ((e1e3 - e0e2) * 2.0) * dx + ((e2e3 + e0e1) * 2.0) * dy + ((e0e0 + e3e3) * 2.0 - 1.0) * dz));
        }
示例#4
0
        public static ChQuaternion Qnorm(ChQuaternion q)
        {
            double invlength;

            invlength = 1 / (Qlength(q));
            return(Qscale(q, invlength));
        }
示例#5
0
 /// Set this quaternion to the sum of A and B: this = A + B.
 public void Add(ChQuaternion A, ChQuaternion B)
 {
     this.e0 = A.e0 + B.e0;
     this.e1 = A.e1 + B.e1;
     this.e2 = A.e2 + B.e2;
     this.e3 = A.e3 + B.e3;
 }
示例#6
0
 /// Paste a quaternion into the matrix.
 public void PasteQuaternion(ChQuaternion qa, int insrow, int inscol)
 {
     SetElement(insrow + 0, inscol, qa.e0);
     SetElement(insrow + 1, inscol, qa.e1);
     SetElement(insrow + 2, inscol, qa.e2);
     SetElement(insrow + 3, inscol, qa.e3);
 }
示例#7
0
        /// Operator for quaternion product: A%B means the typical quaternion product AxB.
        public static ChQuaternion operator %(ChQuaternion q, ChQuaternion other) // works fine
        {
            ChQuaternion a = new ChQuaternion(1, 0, 0, 0);                        // QUNIT;

            a.Cross(q, other);
            return(a);
        }
示例#8
0
        public static ChQuaternion operator *(ChQuaternion a, ChQuaternion other)
        {
            ChQuaternion q = new ChQuaternion(1, 0, 0, 0);//QUNIT;

            q.Cross(a, other);
            return(q);
        }
示例#9
0
 /// Paste a quaternion into the matrix, summing it with preexisting values.
 public void PasteSumQuaternion(ChQuaternion qa, int insrow, int inscol)
 {
     Element(insrow + 0, inscol) += qa.e0;
     Element(insrow + 1, inscol) += qa.e1;
     Element(insrow + 2, inscol) += qa.e2;
     Element(insrow + 3, inscol) += qa.e3;
 }
示例#10
0
 public ChQuaternion(ChQuaternion other) : this()
 {
     // data = new double[4];
     this.e0 = other.e0;
     this.e1 = other.e1;
     this.e2 = other.e2;
     this.e3 = other.e3;
 }
示例#11
0
        /// Updates the r3d time, so perform differentiation for computing speed in case of keyframed motion

        public override void UpdatedExternalTime(double prevtime, double time)
        {
            last_r3time        = ChTime;
            last_r3mot_rot     = Get_mot_rot();
            last_r3mot_rot_dt  = Get_mot_rot_dt();
            last_r3relm_rot    = GetRelM().rot;
            last_r3relm_rot_dt = GetRelM_dt().rot;
        }
示例#12
0
        // Get the time derivative from a quaternion, a speed of rotation and an axis, defined in _abs_ coords.
        public static ChQuaternion Qdt_from_AngAxis(ChQuaternion quat, double angle_dt, ChVector axis)
        {
            ChVector W;

            W = ChVector.Vmul(axis, angle_dt);

            return(Qdt_from_Wabs(W, quat));
        }
示例#13
0
        public ChQuaternion GetInverse()
        {
            ChQuaternion invq = this.GetConjugate();

            // dynamic l = this.Length();
            invq.Scale(1 / this.Length());
            return(invq);
        }
示例#14
0
 public void Conjugate(ChQuaternion A)
 {
     // dynamic a = A;
     this.e0 = +A.e0;
     this.e1 = -A.e1;
     this.e2 = -A.e2;
     this.e3 = -A.e3;
 }
示例#15
0
        /// Sets the rotation matrix from an gale of rotation and an axis,
        /// defined in _absolute_ coords. NOTE, axis must be normalized!
        public void Set_A_AngAxis(double angle,             //< angle of rotation, in radians
                                  ChVector axis)            //< axis of rotation, normalized
        {
            ChQuaternion mr = new ChQuaternion(0, 0, 0, 0); // ChQuaternion.QNULL;

            mr.Q_from_AngAxis(angle, axis);
            this.Set_A_quaternion(mr);
        }
示例#16
0
        // Get the quaternion first derivative from the vector of angular acceleration with a specified in _absolute_ coords.
        public static ChQuaternion Qdtdt_from_Aabs(ChVector a,
                                                   ChQuaternion q,
                                                   ChQuaternion q_dt)
        {
            ChQuaternion ret = new ChQuaternion(1, 0, 0, 0); //QNULL;

            ret.Qdtdt_from_Aabs2(a, q, q_dt);
            return(ret);
        }
示例#17
0
        /// Get the contact coordinate system, expressed in absolute frame.
        /// This represents the 'main' reference of the link: reaction forces
        /// are expressed in this coordinate system. Its origin is point P2.
        /// (It is the coordinate system of the contact plane and normal)
        public ChCoordsys GetContactCoords()
        {
            ChCoordsys   mcsys = ChCoordsys.CSYSNULL;
            ChQuaternion mrot  = this.contact_plane.Get_A_quaternion();

            mcsys.rot.Set(mrot.e0, mrot.e1, mrot.e2, mrot.e3);
            mcsys.pos = this.p2;
            return(mcsys);
        }
示例#18
0
        // Get the X axis of a coordsystem, given the quaternion which
        // represents the alignment of the coordsystem.
        public static ChVector VaxisXfromQuat(ChQuaternion quat)
        {
            ChVector res;// = new ChVector(0, 0, 0);// ChVector.VNULL;

            res.x = (Math.Pow(quat.e0, 2) + Math.Pow(quat.e1, 2)) * 2 - 1;
            res.y = ((quat.e1 * quat.e2) + (quat.e0 * quat.e3)) * 2;
            res.z = ((quat.e1 * quat.e3) - (quat.e0 * quat.e2)) * 2;
            return(res);
        }
示例#19
0
        /// 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;
        }
示例#20
0
        public static ChQuaternion Qadd(ChQuaternion qa, ChQuaternion qb)
        {
            ChQuaternion result;//= new ChQuaternion(1, 0, 0, 0);

            result.e0 = qa.e0 + qb.e0;
            result.e1 = qa.e1 + qb.e1;
            result.e2 = qa.e2 + qb.e2;
            result.e3 = qa.e3 + qb.e3;
            return(result);
        }
示例#21
0
        public static ChQuaternion Qsub(ChQuaternion qa, ChQuaternion qb)
        {
            ChQuaternion result;// = QUNIT;// new ChQuaternion(1, 0, 0, 0);

            result.e0 = qa.e0 - qb.e0;
            result.e1 = qa.e1 - qb.e1;
            result.e2 = qa.e2 - qb.e2;
            result.e3 = qa.e3 - qb.e3;
            return(result);
        }
示例#22
0
        /// Computes the product q=[Gl(mq)]*v  without the need of having
        /// the [Gl] matrix (just pass the mq quaternion, since Gl is function of mq)
        public ChQuaternion GlT_x_Vect(ChQuaternion mq, ChVector v)
        {
            double de0 = 2 * mq.e0;
            double de1 = 2 * mq.e1;
            double de2 = 2 * mq.e2;
            double de3 = 2 * mq.e3;

            return(new  ChQuaternion(-de1 * v.x - de2 * v.y - de3 * v.z, +de0 * v.x - de3 * v.y + de2 * v.z,
                                     +de3 * v.x + de0 * v.y - de1 * v.z, -de2 * v.x + de1 * v.y + de0 * v.z));
        }
示例#23
0
        // -----------------------------------------------------------------------------
        // STATIC QUATERNION MATH OPERATIONS
        //
        // These functions are here for people which prefer to use static functions
        // instead of ChQuaternion class' member functions.
        // NOTE: sometimes a wise adoption of the following functions may give faster
        // results than using overloaded operators +/-/* in the quaternion class.

        // Return the conjugate of the quaternion [s,v1,v2,v3] is [s,-v1,-v2,-v3]
        public static ChQuaternion Qconjugate(ChQuaternion q)
        {
            ChQuaternion res;// = QUNIT;// new ChQuaternion(1, 0, 0, 0);// QNULL;

            res.e0 = q.e0;
            res.e1 = -q.e1;
            res.e2 = -q.e2;
            res.e3 = -q.e3;
            return(res);
        }
示例#24
0
 /// Multiplies this 3x4 matrix by a quaternion, as v=[G]*q
 /// The matrix must be 3x4.
 ///  \return The result of the multiplication, i.e. a vector.
 public ChVector Matr34_x_Quat(ChQuaternion qua)
 {
     // Debug.Assert((rows == 3) && (columns == 4));
     return(new ChVector(Get34Element(0, 0) * qua.e0 + Get34Element(0, 1) * qua.e1 +
                         Get34Element(0, 2) * qua.e2 + Get34Element(0, 3) * qua.e3,
                         Get34Element(1, 0) * qua.e0 + Get34Element(1, 1) * qua.e1 +
                         Get34Element(1, 2) * qua.e2 + Get34Element(1, 3) * qua.e3,
                         Get34Element(2, 0) * qua.e0 + Get34Element(2, 1) * qua.e1 +
                         Get34Element(2, 2) * qua.e2 + Get34Element(2, 3) * qua.e3));
 }
示例#25
0
        /// Return the product of two quaternions. It is non-commutative (like cross product in vectors).
        public static ChQuaternion Qcross(ChQuaternion qa, ChQuaternion qb)
        {
            ChQuaternion res = ChQuaternion.QUNIT;// new ChQuaternion(1, 0, 0, 0); //QNULL;

            res.e0 = qa.e0 * qb.e0 - qa.e1 * qb.e1 - qa.e2 * qb.e2 - qa.e3 * qb.e3;
            res.e1 = qa.e0 * qb.e1 + qa.e1 * qb.e0 - qa.e3 * qb.e2 + qa.e2 * qb.e3;
            res.e2 = qa.e0 * qb.e2 + qa.e2 * qb.e0 + qa.e3 * qb.e1 - qa.e1 * qb.e3;
            res.e3 = qa.e0 * qb.e3 + qa.e3 * qb.e0 - qa.e2 * qb.e1 + qa.e1 * qb.e2;
            return(res);
        }
示例#26
0
        public static ChQuaternion Qscale(ChQuaternion q, double fact)
        {
            ChQuaternion result;// = QUNIT;// new ChQuaternion(1, 0, 0, 0);

            result.e0 = q.e0 * fact;
            result.e1 = q.e1 * fact;
            result.e2 = q.e2 * fact;
            result.e3 = q.e3 * fact;
            return(result);
        }
示例#27
0
        // Get the second time derivative from a quaternion, an angular acceleration and an axis, defined in _abs_ coords.
        public static ChQuaternion Qdtdt_from_AngAxis(double angle_dtdt,
                                                      ChVector axis,
                                                      ChQuaternion q,
                                                      ChQuaternion q_dt)
        {
            ChVector Acc;

            Acc = ChVector.Vmul(axis, angle_dtdt);

            return(Qdtdt_from_Aabs(Acc, q, q_dt));
        }
示例#28
0
        // Get the quaternion time derivative from the vector of angular speed, with w specified in _absolute_ coords.
        public static ChQuaternion Qdt_from_Wabs(ChVector w, ChQuaternion q)
        {
            ChQuaternion qw;// = new ChQuaternion(1, 0, 0, 0);
            double       half = 0.5;

            qw.e0 = 0;
            qw.e1 = w.x;
            qw.e2 = w.y;
            qw.e3 = w.z;

            return(Qscale(Qcross(qw, q), half));  // {q_dt} = 1/2 {0,w}*{q}
        }
示例#29
0
        public void Cross(ChQuaternion qa, ChQuaternion qb)
        {
            // dynamic q1 = qa;
            double w = qa.e0 * qb.e0 - qa.e1 * qb.e1 - qa.e2 * qb.e2 - qa.e3 * qb.e3;
            double x = qa.e0 * qb.e1 + qa.e1 * qb.e0 - qa.e3 * qb.e2 + qa.e2 * qb.e3;
            double y = qa.e0 * qb.e2 + qa.e2 * qb.e0 + qa.e3 * qb.e1 - qa.e1 * qb.e3;
            double z = qa.e0 * qb.e3 + qa.e3 * qb.e0 - qa.e2 * qb.e1 + qa.e1 * qb.e2;

            this.e0 = w;
            this.e1 = x;
            this.e2 = y;
            this.e3 = z;
        }
示例#30
0
        /// 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();
        }