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