/**
* Converts {@link georegression.struct.so.Rodrigues_F32} into a unit {@link
* georegression.struct.so.Quaternion_F32}.
*
* @param rodrigues The angle of rotation around the rotation axis.
* @param quat Storage for quaternion coordinate. If null a new quaternion is created. Modified.
* @return unit quaternion coordinate.
*/
public static Quaternion_F32 rodriguesToQuaternion(Rodrigues_F32 rodrigues, Quaternion_F32 quat) {
if (quat == null) quat = new Quaternion_F32();
quat.w = (float) Math.cos(rodrigues.theta / 2.0f);
float s = (float) Math.sin(rodrigues.theta / 2.0f);
quat.x = rodrigues.unitAxisRotation.x * s;
quat.y = rodrigues.unitAxisRotation.y * s;
quat.z = rodrigues.unitAxisRotation.z * s;
return quat;
}
public static Quaternion_F32 eulerToQuaternion(
EulerType type, float rotA, float rotB, float rotC, Quaternion_F32 q) {
if (q == null) q = new Quaternion_F32();
float ca = (float) Math.cos(rotA * 0.5f);
float sa = (float) Math.sin(rotA * 0.5f);
float cb = (float) Math.cos(rotB * 0.5f);
float sb = (float) Math.sin(rotB * 0.5f);
float cc = (float) Math.cos(rotC * 0.5f);
float sc = (float) Math.sin(rotC * 0.5f);
float w = 0, x = 0, y = 0, z = 0;
switch (type) {
case ZYX:
w = ca * cb * cc - sa * sb * sc;
x = cc * sa * sb + ca * cb * sc;
y = ca * cc * sb - cb * sa * sc;
z = cb * cc * sa + ca * sb * sc;
break;
case ZYZ:
w = ca * cb * cc - cb * sa * sc;
x = cc * sa * sb - ca * sb * sc;
y = ca * cc * sb + sa * sb * sc;
z = cb * cc * sa + ca * cb * sc;
break;
case ZXY:
w = ca * cb * cc + sa * sb * sc;
x = ca * cc * sb + cb * sa * sc;
y = -cc * sa * sb + ca * cb * sc;
z = cb * cc * sa - ca * sb * sc;
break;
case ZXZ:
w = ca * cb * cc - cb * sa * sc;
x = ca * cc * sb + sa * sb * sc;
y = -cc * sa * sb + ca * sb * sc;
z = cb * cc * sa + ca * cb * sc;
break;
case YXZ:
w = ca * cb * cc - sa * sb * sc;
x = ca * cc * sb - cb * sa * sc;
y = cb * cc * sa + ca * sb * sc;
z = cc * sa * sb + ca * cb * sc;
break;
case YXY:
w = ca * cb * cc - cb * sa * sc;
x = ca * cc * sb + sa * sb * sc;
y = cb * cc * sa + ca * cb * sc;
z = cc * sa * sb - ca * sb * sc;
break;
case YZX:
w = ca * cb * cc + sa * sb * sc;
x = -cc * sa * sb + ca * cb * sc;
y = cb * cc * sa - ca * sb * sc;
z = ca * cc * sb + cb * sa * sc;
break;
case YZY:
w = ca * cb * cc - cb * sa * sc;
x = -cc * sa * sb + ca * sb * sc;
y = cb * cc * sa + ca * cb * sc;
z = ca * cc * sb + sa * sb * sc;
break;
case XYZ:
w = ca * cb * cc + sa * sb * sc;
x = cb * cc * sa - ca * sb * sc;
y = ca * cc * sb + cb * sa * sc;
z = -cc * sa * sb + ca * cb * sc;
break;
case XYX:
w = ca * cb * cc - cb * sa * sc;
x = cb * cc * sa + ca * cb * sc;
y = ca * cc * sb + sa * sb * sc;
z = -cc * sa * sb + ca * sb * sc;
break;
case XZY:
w = ca * cb * cc - sa * sb * sc;
x = cb * cc * sa + ca * sb * sc;
y = cc * sa * sb + ca * cb * sc;
z = ca * cc * sb - cb * sa * sc;
break;
case XZX:
w = ca * cb * cc - cb * sa * sc;
x = cb * cc * sa + ca * cb * sc;
y = cc * sa * sb - ca * sb * sc;
z = ca * cc * sb + sa * sb * sc;
break;
}
q.set(w, x, y, z);
return q;
}
/**
* Extracts quaternions from the provided rotation matrix.
*
* @param R (Input) rotation matrix
* @param quat (Output) Optional storage for quaternion. If null a new class will be used.
* @return unit quaternion representation of the rotation matrix.
*/
public static Quaternion_F32 matrixToQuaternion(DenseMatrix64F R, Quaternion_F32 quat) {
if (quat == null) quat = new Quaternion_F32();
// algorithm from:
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
//
// Designed to minimize numerical error by not dividing by very small numbers
float m00 = (float) R.unsafe_get(0, 0);
float m01 = (float) R.unsafe_get(0, 1);
float m02 = (float) R.unsafe_get(0, 2);
float m10 = (float) R.unsafe_get(1, 0);
float m11 = (float) R.unsafe_get(1, 1);
float m12 = (float) R.unsafe_get(1, 2);
float m20 = (float) R.unsafe_get(2, 0);
float m21 = (float) R.unsafe_get(2, 1);
float m22 = (float) R.unsafe_get(2, 2);
float trace = m00 + m11 + m22;
if (trace > 0) {
float S = (float) Math.sqrt(trace + 1.0f) * 2; // S=4*qw
quat.w = 0.25f * S;
quat.x = (m21 - m12) / S;
quat.y = (m02 - m20) / S;
quat.z = (m10 - m01) / S;
} else if ((m00 > m11) & (m00 > m22)) {
float S = (float) Math.sqrt(1.0f + m00 - m11 - m22) * 2; // S=4*qx
quat.w = (m21 - m12) / S;
quat.x = 0.25f * S;
quat.y = (m01 + m10) / S;
quat.z = (m02 + m20) / S;
} else if (m11 > m22) {
float S = (float) Math.sqrt(1.0f + m11 - m00 - m22) * 2; // S=4*qy
quat.w = (m02 - m20) / S;
quat.x = (m01 + m10) / S;
quat.y = 0.25f * S;
quat.z = (m12 + m21) / S;
} else {
float S = (float) Math.sqrt(1.0f + m22 - m00 - m11) * 2; // S=4*qz
quat.w = (m10 - m01) / S;
quat.x = (m02 + m20) / S;
quat.y = (m12 + m21) / S;
quat.z = 0.25f * S;
}
return quat;
}
