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