public void Set(ChQuaternion q) { this.e0 = q.e0; this.e1 = q.e1; this.e2 = q.e2; this.e3 = q.e3; }
/// 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); }
// 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)); }
public static ChQuaternion Qnorm(ChQuaternion q) { double invlength; invlength = 1 / (Qlength(q)); return(Qscale(q, invlength)); }
/// 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; }
/// 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); }
/// 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); }
public static ChQuaternion operator *(ChQuaternion a, ChQuaternion other) { ChQuaternion q = new ChQuaternion(1, 0, 0, 0);//QUNIT; q.Cross(a, other); return(q); }
/// 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; }
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; }
/// 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; }
// 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)); }
public ChQuaternion GetInverse() { ChQuaternion invq = this.GetConjugate(); // dynamic l = this.Length(); invq.Scale(1 / this.Length()); return(invq); }
public void Conjugate(ChQuaternion A) { // dynamic a = A; this.e0 = +A.e0; this.e1 = -A.e1; this.e2 = -A.e2; this.e3 = -A.e3; }
/// 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); }
// 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); }
/// 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); }
// 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); }
/// 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; }
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); }
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); }
/// 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)); }
// ----------------------------------------------------------------------------- // 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); }
/// 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)); }
/// 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); }
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); }
// 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)); }
// 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} }
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; }
/// 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(); }