/*! \relates XsVector
\brief Get an effective angular velocity from the quaternion, which must represent a delta angle.
\param deltaT The length of the time interval over which \a quat was integrated in seconds
\param quat The orientation increment to convert to an angular velocity
\returns A vector containing the effective angular velocity in radians around each axis.
*/
void XsVector_angularVelocityFromQuaternion(XsVector* thisPtr, XsReal deltaT, const XsQuaternion* quat)
{
XsReal a;
if (XsQuaternion_empty(quat))
{
XsVector_destruct(thisPtr);
return;
}
XsVector_assign(thisPtr, 3, &quat->m_data[1]);
a = XsVector_cartesianLength(thisPtr);
XsVector_multiplyScalar(thisPtr, (a > XsMath_tinyValue) ? (XsMath_two*asin(a)/(a*deltaT)) : (XsMath_two/deltaT), thisPtr);
}
/*! \relates XsEuler
\brief Get an euler angle representation of the quaternion.
*/
void XsEuler_fromQuaternion(XsEuler* thisPtr, const XsQuaternion* quat)
{
XsReal sqw, dphi, dpsi;
if (XsQuaternion_empty(quat))
{
XsEuler_destruct(thisPtr);
return;
}
sqw = quat->m_w * quat->m_w;
dphi = XsMath_two * (sqw + quat->m_z * quat->m_z) - XsMath_one;
dpsi = XsMath_two * (sqw + quat->m_x * quat->m_x) - XsMath_one;
thisPtr->m_x = XsMath_rad2deg(atan2(XsMath_two*(quat->m_y*quat->m_z + quat->m_w*quat->m_x), dphi));
thisPtr->m_y = -XsMath_rad2deg(XsMath_asinClamped(XsMath_two*(quat->m_x*quat->m_z - quat->m_w*quat->m_y)));
thisPtr->m_z = XsMath_rad2deg(atan2(XsMath_two*(quat->m_x*quat->m_y + quat->m_w*quat->m_z), dpsi));
}
/*! \relates XsMatrix \brief Get an orientation matrix representation of the quaternion. */
void XsMatrix_fromQuaternion(XsMatrix* thisPtr, const XsQuaternion* quat)
{
XsReal q00, q11, q22, q33, q01, q02, q03, q12, q13, q23;
if (XsQuaternion_empty(quat))
{
XsMatrix_destruct(thisPtr);
return;
}
q00 = quat->m_w*quat->m_w;
q11 = quat->m_x*quat->m_x;
q22 = quat->m_y*quat->m_y;
q33 = quat->m_z*quat->m_z;
q01 = quat->m_w*quat->m_x;
q02 = quat->m_w*quat->m_y;
q03 = quat->m_w*quat->m_z;
q12 = quat->m_x*quat->m_y;
q13 = quat->m_x*quat->m_z;
q23 = quat->m_y*quat->m_z;
XsMatrix_assign(thisPtr, 3, 3, 3, 0, 0);
XsMatrix_setValue(thisPtr, 0, 0, q00 + q11 - q22 - q33);
XsMatrix_setValue(thisPtr, 0, 1, (q12 - q03) * XsMath_two);
XsMatrix_setValue(thisPtr, 0, 2, (q13 + q02) * XsMath_two);
XsMatrix_setValue(thisPtr, 1, 0, (q12 + q03) * XsMath_two);
XsMatrix_setValue(thisPtr, 1, 1, q00 - q11 + q22 - q33);
XsMatrix_setValue(thisPtr, 1, 2, (q23 - q01) * XsMath_two);
XsMatrix_setValue(thisPtr, 2, 0, (q13 - q02) * XsMath_two);
XsMatrix_setValue(thisPtr, 2, 1, (q23 + q01) * XsMath_two);
XsMatrix_setValue(thisPtr, 2, 2, q00 - q11 - q22 + q33);
}
