/**
* 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 a rotation matrix into {@link georegression.struct.so.Rodrigues_F32}.
*
* @param R Rotation matrix.
* @param rodrigues Storage used for solution. If null a new instance is declared.
* @return The found axis and rotation angle.
*/
public static Rodrigues_F32 matrixToRodrigues(DenseMatrix64F R, Rodrigues_F32 rodrigues) {
if (rodrigues == null) {
rodrigues = new Rodrigues_F32();
}
// parts of this are from wikipedia
// http://en.wikipedia.org/wiki/Rotation_representation_%28mathematics%29#Rotation_matrix_.E2.86f.94_Euler_axis.2Fangle
float diagSum =
((float) (R.unsafe_get(0, 0) + R.unsafe_get(1, 1) + R.unsafe_get(2, 2)) - 1.0f) / 2.0f;
float absDiagSum = (float) Math.abs(diagSum);
if (absDiagSum <= 1.0f && 1.0f - absDiagSum > 10.0f * GrlConstants.F_EPS) {
// if numerically stable use a faster technique
rodrigues.theta = (float) Math.acos(diagSum);
float bottom = 2.0f * (float) Math.sin(rodrigues.theta);
// in cases where bottom is close to zero that means theta is also close to zero and the
// vector
// doesn't matter that much
rodrigues.unitAxisRotation.x = (float) (R.unsafe_get(2, 1) - R.unsafe_get(1, 2)) / bottom;
rodrigues.unitAxisRotation.y = (float) (R.unsafe_get(0, 2) - R.unsafe_get(2, 0)) / bottom;
rodrigues.unitAxisRotation.z = (float) (R.unsafe_get(1, 0) - R.unsafe_get(0, 1)) / bottom;
// in extreme underflow situations the result can be unnormalized
rodrigues.unitAxisRotation.normalize();
// In theory this might be more stable
// rotationAxis( R, rodrigues.unitAxisRotation);
} else {
// this handles the special case where the bottom is very very small or equal to zero
if (diagSum >= 1.0f) rodrigues.theta = 0;
else if (diagSum <= -1.0f) rodrigues.theta = (float) Math.PI;
else rodrigues.theta = (float) Math.acos(diagSum);
// compute the value of x,y,z up to a sign ambiguity
rodrigues.unitAxisRotation.x = (float) Math.sqrt((R.get(0, 0) + 1) / 2);
rodrigues.unitAxisRotation.y = (float) Math.sqrt((R.get(1, 1) + 1) / 2);
rodrigues.unitAxisRotation.z = (float) Math.sqrt((R.get(2, 2) + 1) / 2);
float x = rodrigues.unitAxisRotation.x;
float y = rodrigues.unitAxisRotation.y;
float z = rodrigues.unitAxisRotation.z;
if (Math.abs(R.get(1, 0) - 2 * x * y) > GrlConstants.F_EPS) {
x *= -1;
}
if (Math.abs(R.get(2, 0) - 2 * x * z) > GrlConstants.F_EPS) {
z *= -1;
}
if (Math.abs(R.get(2, 1) - 2 * z * y) > GrlConstants.F_EPS) {
y *= -1;
x *= -1;
}
rodrigues.unitAxisRotation.x = x;
rodrigues.unitAxisRotation.y = y;
rodrigues.unitAxisRotation.z = z;
}
return rodrigues;
}
public Double get(int index) {
return values.unsafe_get(index, 0);
}
