// Get the X axis of a coordsystem, given the quaternion which
// represents the alignment of the coordsystem.
ChVector<double> VaxisXfromQuat(const ChQuaternion<double>& quat) {
ChVector<double> res;
res.x() = (pow(quat.e0(), 2) + 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;
}
// Return the conjugate of the quaternion [s,v1,v2,v3] is [s,-v1,-v2,-v3]
ChQuaternion<double> Qconjugate(const ChQuaternion<double>& q) {
ChQuaternion<double> res;
res.e0() = q.e0();
res.e1() = -q.e1();
res.e2() = -q.e2();
res.e3() = -q.e3();
return (res);
}
ChQuaternion<double> Qsub(const ChQuaternion<double>& qa, const ChQuaternion<double>& qb) {
ChQuaternion<double> result;
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;
}
ChQuaternion<double> Qscale(const ChQuaternion<double>& q, double fact) {
ChQuaternion<double> result;
result.e0() = q.e0() * fact;
result.e1() = q.e1() * fact;
result.e2() = q.e2() * fact;
result.e3() = q.e3() * fact;
return result;
}
// Given the imaginary (vectorial) {e1 e2 e3} part of a quaternion,
// find the entire quaternion q = {e0, e1, e2, e3}.
// Note: singularities are possible.
ChQuaternion<double> ImmQ_complete(const ChVector<double>& qimm) {
ChQuaternion<double> mq;
mq.e1() = qimm.x();
mq.e2() = qimm.y();
mq.e3() = qimm.z();
mq.e0() = sqrt(1 - mq.e1() * mq.e1() - mq.e2() * mq.e2() - mq.e3() * mq.e3());
return mq;
}
// Given the imaginary (vectorial) {e1 e2 e3} part of a quaternion time derivative,
// find the entire quaternion q = {e0, e1, e2, e3}.
// Note: singularities are possible.
ChQuaternion<double> ImmQ_dt_complete(const ChQuaternion<double>& mq, const ChVector<double>& qimm_dt) {
ChQuaternion<double> mqdt;
mqdt.e1() = qimm_dt.x();
mqdt.e2() = qimm_dt.y();
mqdt.e3() = qimm_dt.z();
mqdt.e0() = (-mq.e1() * mqdt.e1() - mq.e2() * mqdt.e2() - mq.e3() * mqdt.e3()) / mq.e0();
return mqdt;
}
ChVector<double> Q_to_NasaAngles(const ChQuaternion<double>& q1) {
ChVector<double> mnasa;
double sqw = q1.e0() * q1.e0();
double sqx = q1.e1() * q1.e1();
double sqy = q1.e2() * q1.e2();
double sqz = q1.e3() * q1.e3();
// heading
mnasa.z() = atan2(2.0 * (q1.e1() * q1.e2() + q1.e3() * q1.e0()), (sqx - sqy - sqz + sqw));
// bank
mnasa.y() = atan2(2.0 * (q1.e2() * q1.e3() + q1.e1() * q1.e0()), (-sqx - sqy + sqz + sqw));
// attitude
mnasa.x() = asin(-2.0 * (q1.e1() * q1.e3() - q1.e2() * q1.e0()));
return mnasa;
}
// Return the product of two quaternions. It is non-commutative (like cross product in vectors).
ChQuaternion<double> Qcross(const ChQuaternion<double>& qa, const ChQuaternion<double>& qb) {
ChQuaternion<double> res;
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);
}
// Get the quaternion time derivative from the vector of angular speed, with w specified in _local_ coords.
ChQuaternion<double> Qdt_from_Wrel(const ChVector<double>& w, const ChQuaternion<double>& q) {
ChQuaternion<double> qw;
double half = 0.5;
qw.e0() = 0;
qw.e1() = w.x();
qw.e2() = w.y();
qw.e3() = w.z();
return Qscale(Qcross(q, qw), half); // {q_dt} = 1/2 {q}*{0,w_rel}
}
// Get the quaternion from a source vector and a destination vector which specifies
// the rotation from one to the other. The vectors do not need to be normalized.
ChQuaternion<double> Q_from_Vect_to_Vect(const ChVector<double>& fr_vect, const ChVector<double>& to_vect) {
const double ANGLE_TOLERANCE = 1e-6;
ChQuaternion<double> quat;
double halfang;
double sinhalf;
ChVector<double> axis;
double lenXlen = fr_vect.Length() * to_vect.Length();
axis = fr_vect % to_vect;
double sinangle = ChClamp(axis.Length() / lenXlen, -1.0, +1.0);
double cosangle = ChClamp(fr_vect ^ to_vect / lenXlen, -1.0, +1.0);
// Consider three cases: Parallel, Opposite, non-collinear
if (std::abs(sinangle) == 0.0 && cosangle > 0) {
// fr_vect & to_vect are parallel
quat.e0() = 1.0;
quat.e1() = 0.0;
quat.e2() = 0.0;
quat.e3() = 0.0;
} else if (std::abs(sinangle) < ANGLE_TOLERANCE && cosangle < 0) {
// fr_vect & to_vect are opposite, i.e. ~180 deg apart
axis = fr_vect.GetOrthogonalVector() + (-to_vect).GetOrthogonalVector();
axis.Normalize();
quat.e0() = 0.0;
quat.e1() = ChClamp(axis.x(), -1.0, +1.0);
quat.e2() = ChClamp(axis.y(), -1.0, +1.0);
quat.e3() = ChClamp(axis.z(), -1.0, +1.0);
} else {
// fr_vect & to_vect are not co-linear case
axis.Normalize();
halfang = 0.5 * ChAtan2(sinangle, cosangle);
sinhalf = sin(halfang);
quat.e0() = cos(halfang);
quat.e1() = ChClamp(axis.x(), -1.0, +1.0);
quat.e2() = ChClamp(axis.y(), -1.0, +1.0);
quat.e3() = ChClamp(axis.z(), -1.0, +1.0);
}
return (quat);
}
// Get the quaternion from an angle of rotation and an axis, defined in _abs_ coords.
// The axis is supposed to be fixed, i.e. it is constant during rotation.
// The 'axis' vector must be normalized.
ChQuaternion<double> Q_from_AngAxis(double angle, const ChVector<double>& axis) {
ChQuaternion<double> quat;
double halfang;
double sinhalf;
halfang = (angle * 0.5);
sinhalf = sin(halfang);
quat.e0() = cos(halfang);
quat.e1() = axis.x() * sinhalf;
quat.e2() = axis.y() * sinhalf;
quat.e3() = axis.z() * sinhalf;
return (quat);
}
ChQuaternion<double> Q_from_NasaAngles(const ChVector<double>& mang) {
ChQuaternion<double> mq;
double c1 = cos(mang.z() / 2);
double s1 = sin(mang.z() / 2);
double c2 = cos(mang.x() / 2);
double s2 = sin(mang.x() / 2);
double c3 = cos(mang.y() / 2);
double s3 = sin(mang.y() / 2);
double c1c2 = c1 * c2;
double s1s2 = s1 * s2;
mq.e0() = c1c2 * c3 + s1s2 * s3;
mq.e1() = c1c2 * s3 - s1s2 * c3;
mq.e2() = c1 * s2 * c3 + s1 * c2 * s3;
mq.e3() = s1 * c2 * c3 - c1 * s2 * s3;
return mq;
}
void Q_to_AngAxis(const ChQuaternion<double>& quat, double& angle, ChVector<double>& axis) {
if (fabs(quat.e0()) < 0.99999999) {
double arg = acos(quat.e0());
double invsine = 1 / sin(arg);
ChVector<double> vtemp;
vtemp.x() = invsine * quat.e1();
vtemp.y() = invsine * quat.e2();
vtemp.z() = invsine * quat.e3();
angle = 2 * arg;
axis = Vnorm(vtemp);
} else {
axis.x() = 1;
axis.y() = 0;
axis.z() = 0;
angle = 0;
}
}
// Check if quaternion is not null
bool Qnotnull(const ChQuaternion<double>& qa) {
return (qa.e0() != 0) || (qa.e1() != 0) || (qa.e2() != 0) || (qa.e3() != 0);
}
double Qlength(const ChQuaternion<double>& q) {
return (sqrt(pow(q.e0(), 2) + pow(q.e1(), 2) + pow(q.e2(), 2) + pow(q.e3(), 2)));
}
