/*! \relates XsSdiData \brief Initialize an %XsSdiData object with the optional arguments. \param orientationIncrement The orientation increment to initialize the object with, may be 0 \param velocityIncrement The velocity increment to initialize the object with, may be 0 */ void XsSdiData_construct(XsSdiData* thisPtr, const XsReal* orientationIncrement, const XsReal* velocityIncrement) { if (orientationIncrement) { thisPtr->m_orientationIncrement.m_data[0] = orientationIncrement[0]; thisPtr->m_orientationIncrement.m_data[1] = orientationIncrement[1]; thisPtr->m_orientationIncrement.m_data[2] = orientationIncrement[2]; thisPtr->m_orientationIncrement.m_data[3] = orientationIncrement[3]; } else XsQuaternion_destruct(&thisPtr->m_orientationIncrement); if (velocityIncrement) XsVector3_assign(&thisPtr->m_velocityIncrement, velocityIncrement); else XsVector3_destruct(&thisPtr->m_velocityIncrement); }
/*! \relates XsQuaternion \brief Create a quaternion representation of orientation matrix \a ori \details The matrix \a ori is interpreted as an orthonormal orientation matrix, which is translated into a quaternion representation. If \a ori is not a 3x3 matrix, a null-quaternion is returned. \param ori The source orientation matrix */ void XsQuaternion_fromRotationMatrix(XsQuaternion* thisPtr, const XsMatrix* ori) { XsReal trace; // Trace of matrix XsReal s; if (!XsMatrix_dimensionsMatch(ori, 3,3)) { XsQuaternion_destruct(thisPtr); return; } // XsQuaternion result; // Calculate trace of matrix // T = 4 - 4x^2 - 4y^2 - 4z^2 // = 4 ( 1 - x^2 - y^2 - z^2) // = 1 + fRgs[0][0] + fRgs[1][1] + fRgs[2][2] (= 4q0^2) trace = XsMatrix_value(ori, 0, 0) + XsMatrix_value(ori, 1, 1) + XsMatrix_value(ori, 2, 2) + XsMath_one; // If the trace of the matrix is greater than zero, // then perform an "instant" calculation. // Important note wrt. rounding errors: // Test if (T > 0.0000001) to avoid large distortions! // If the trace of the matrix is equal to zero, then identify // which major diagonal element has the greatest value if (trace*trace >= XsMath_tinyValue) { s = XsMath_two * sqrt(trace); thisPtr->m_w = XsMath_pt25 * s; #ifndef XSENS_MATH_MATLAB_COMPLIANT s = XsMath_one/s; thisPtr->m_x = (XsMatrix_value(ori, 1, 2) - XsMatrix_value(ori, 2, 1)) * s; thisPtr->m_y = (XsMatrix_value(ori, 2, 0) - XsMatrix_value(ori, 0, 2)) * s; thisPtr->m_z = (XsMatrix_value(ori, 0, 1) - XsMatrix_value(ori, 1, 0)) * s; #else thisPtr->m_x = (XsMatrix_value(ori, 1, 2) - XsMatrix_value(ori, 2, 1)) / s; thisPtr->m_y = (XsMatrix_value(ori, 2, 0) - XsMatrix_value(ori, 0, 2)) / s; thisPtr->m_z = (XsMatrix_value(ori, 0, 1) - XsMatrix_value(ori, 1, 0)) / s; #endif } else if ((XsMatrix_value(ori, 0, 0) > XsMatrix_value(ori, 1, 1)) && (XsMatrix_value(ori, 0, 0) > XsMatrix_value(ori, 2, 2))) { trace = XsMath_one + XsMatrix_value(ori, 0, 0) - XsMatrix_value(ori, 1, 1) - XsMatrix_value(ori, 2, 2); s = XsMath_two * sqrt(trace); thisPtr->m_x = XsMath_pt25 * s; #ifndef XSENS_MATH_MATLAB_COMPLIANT s = XsMath_one/s; thisPtr->m_w = (XsMatrix_value(ori, 1, 2) - XsMatrix_value(ori, 2, 1)) * s; thisPtr->m_y = (XsMatrix_value(ori, 0, 1) + XsMatrix_value(ori, 1, 0)) * s; thisPtr->m_z = (XsMatrix_value(ori, 2, 0) + XsMatrix_value(ori, 0, 2)) * s; #else thisPtr->m_w = (XsMatrix_value(ori, 1, 2) - XsMatrix_value(ori, 2, 1)) / s; thisPtr->m_y = (XsMatrix_value(ori, 0, 1) + XsMatrix_value(ori, 1, 0)) / s; thisPtr->m_z = (XsMatrix_value(ori, 2, 0) + XsMatrix_value(ori, 0, 2)) / s; #endif } else if (XsMatrix_value(ori, 1, 1) > XsMatrix_value(ori, 2, 2)) { trace = XsMath_one + XsMatrix_value(ori, 1, 1) - XsMatrix_value(ori, 0, 0) - XsMatrix_value(ori, 2, 2); s = XsMath_two * sqrt(trace); thisPtr->m_y = XsMath_pt25 * s; #ifndef XSENS_MATH_MATLAB_COMPLIANT s = XsMath_one/s; thisPtr->m_w = (XsMatrix_value(ori, 2, 0) - XsMatrix_value(ori, 0, 2)) * s; thisPtr->m_x = (XsMatrix_value(ori, 0, 1) + XsMatrix_value(ori, 1, 0)) * s; thisPtr->m_z = (XsMatrix_value(ori, 1, 2) + XsMatrix_value(ori, 2, 1)) * s; #else thisPtr->m_w = (XsMatrix_value(ori, 2, 0) - XsMatrix_value(ori, 0, 2)) / s; thisPtr->m_x = (XsMatrix_value(ori, 0, 1) + XsMatrix_value(ori, 1, 0)) / s; thisPtr->m_z = (XsMatrix_value(ori, 1, 2) + XsMatrix_value(ori, 2, 1)) / s; #endif } else { trace = XsMath_one + XsMatrix_value(ori, 2, 2) - XsMatrix_value(ori, 0, 0) - XsMatrix_value(ori, 1, 1); s = XsMath_two * sqrt(trace); thisPtr->m_z = XsMath_pt25 * s; #ifndef XSENS_MATH_MATLAB_COMPLIANT s = XsMath_one/s; thisPtr->m_w = (XsMatrix_value(ori, 0, 1) - XsMatrix_value(ori, 1, 0)) * s; thisPtr->m_x = (XsMatrix_value(ori, 2, 0) + XsMatrix_value(ori, 0, 2)) * s; thisPtr->m_y = (XsMatrix_value(ori, 1, 2) + XsMatrix_value(ori, 2, 1)) * s; #else thisPtr->m_w = (XsMatrix_value(ori, 0, 1) - XsMatrix_value(ori, 1, 0)) / s; thisPtr->m_x = (XsMatrix_value(ori, 2, 0) + XsMatrix_value(ori, 0, 2)) / s; thisPtr->m_y = (XsMatrix_value(ori, 1, 2) + XsMatrix_value(ori, 2, 1)) / s; #endif } XsQuaternion_inverse(thisPtr, thisPtr); }
/*! \relates XsSdiData \brief Destruct the object, makes the fields invalid */ void XsSdiData_destruct(XsSdiData* thisPtr) { XsQuaternion_destruct(&thisPtr->m_orientationIncrement); XsVector3_destruct(&thisPtr->m_velocityIncrement); }