setAxisAngleFromRotationMatrix method
Implementation
Vector4 setAxisAngleFromRotationMatrix(Matrix3 m) {
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/index.htm
// assumes the upper 3x3 of m is a pure rotation matrix (i.e, unscaled)
double angle, x, y, z; // variables for result
double epsilon = 0.01, // margin to allow for rounding errors
epsilon2 = 0.1; // margin to distinguish between 0 and 180 degrees
final te = m.elements;
double m11 = te[0];
double m12 = te[4];
double m13 = te[8];
double m21 = te[1];
double m22 = te[5];
double m23 = te[9];
double m31 = te[2];
double m32 = te[6];
double m33 = te[10];
if ((Math.abs(m12 - m21) < epsilon) &&
(Math.abs(m13 - m31) < epsilon) &&
(Math.abs(m23 - m32) < epsilon)) {
// singularity found
// first check for identity matrix which must have +1 for all terms
// in leading diagonal and zero in other terms
if ((Math.abs(m12 + m21) < epsilon2) &&
(Math.abs(m13 + m31) < epsilon2) &&
(Math.abs(m23 + m32) < epsilon2) &&
(Math.abs(m11 + m22 + m33 - 3) < epsilon2)) {
// this singularity is identity matrix so angle = 0
set(1, 0, 0, 0);
return this; // zero angle, arbitrary axis
}
// otherwise this singularity is angle = 180
angle = Math.PI;
var xx = (m11 + 1) / 2;
var yy = (m22 + 1) / 2;
var zz = (m33 + 1) / 2;
var xy = (m12 + m21) / 4;
var xz = (m13 + m31) / 4;
var yz = (m23 + m32) / 4;
if ((xx > yy) && (xx > zz)) {
// m11 is the largest diagonal term
if (xx < epsilon) {
x = 0;
y = 0.707106781;
z = 0.707106781;
} else {
x = Math.sqrt(xx);
y = xy / x;
z = xz / x;
}
} else if (yy > zz) {
// m22 is the largest diagonal term
if (yy < epsilon) {
x = 0.707106781;
y = 0;
z = 0.707106781;
} else {
y = Math.sqrt(yy);
x = xy / y;
z = yz / y;
}
} else {
// m33 is the largest diagonal term so base result on this
if (zz < epsilon) {
x = 0.707106781;
y = 0.707106781;
z = 0;
} else {
z = Math.sqrt(zz);
x = xz / z;
y = yz / z;
}
}
set(x, y, z, angle);
return this; // return 180 deg rotation
}
// as we have reached here there are no singularities so we can handle normally
var s = Math.sqrt((m32 - m23) * (m32 - m23) +
(m13 - m31) * (m13 - m31) +
(m21 - m12) * (m21 - m12)); // used to normalize
if (Math.abs(s) < 0.001) s = 1;
// prevent divide by zero, should not happen if matrix is orthogonal and should be
// caught by singularity test above, but I've left it in just in case
this.x = (m32 - m23) / s;
this.y = (m13 - m31) / s;
this.z = (m21 - m12) / s;
w = Math.acos((m11 + m22 + m33 - 1) / 2);
return this;
}