void eulerToMatrix(FCL_REAL a, FCL_REAL b, FCL_REAL c, Matrix3f& R)
{
FCL_REAL c1 = cos(a);
FCL_REAL c2 = cos(b);
FCL_REAL c3 = cos(c);
FCL_REAL s1 = sin(a);
FCL_REAL s2 = sin(b);
FCL_REAL s3 = sin(c);
R.setValue(c1 * c2, - c2 * s1, s2,
c3 * s1 + c1 * s2 * s3, c1 * c3 - s1 * s2 * s3, - c2 * s3,
s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3, c2 * c3);
}
void Quaternion3f::toRotation(Matrix3f& R) const
{
assertNormalized();
FCL_REAL twoX = 2.0*data[1];
FCL_REAL twoY = 2.0*data[2];
FCL_REAL twoZ = 2.0*data[3];
FCL_REAL twoWX = twoX*data[0];
FCL_REAL twoWY = twoY*data[0];
FCL_REAL twoWZ = twoZ*data[0];
FCL_REAL twoXX = twoX*data[1];
FCL_REAL twoXY = twoY*data[1];
FCL_REAL twoXZ = twoZ*data[1];
FCL_REAL twoYY = twoY*data[2];
FCL_REAL twoYZ = twoZ*data[2];
FCL_REAL twoZZ = twoZ*data[3];
R.setValue(1.0 - (twoYY + twoZZ), twoXY - twoWZ, twoXZ + twoWY,
twoXY + twoWZ, 1.0 - (twoXX + twoZZ), twoYZ - twoWX,
twoXZ - twoWY, twoYZ + twoWX, 1.0 - (twoXX + twoYY));
}