/*! \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); }