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