/**
* Extracts the rotation angles that constructed the rotation matrix M. The angles are returned in
* the angles array as angles[0] = rz1, angles[1] = ry, angles[2] = rz2
*
* <p>Ideas borrowed from
* http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToEuler/
*/
public void getRotationAngles(Matrix m, double[] angles) {
if (m.getItem(2, 2) >= threshold) {
/*
* singularity at north pole, i.e. angles[1]=0; c2=1; s2=0
* assume angles[2]=0; c3=1; s3=0
* R3*R2*R1
* c1 s1 0
* -s1 c1 0
* 0 0 1
*/
angles[0] = Math.atan2(m.getItem(1, 0), m.getItem(0, 0));
angles[1] = 0.0;
angles[2] = 0.0;
} else if (m.getItem(2, 2) <= -threshold) {
/*
* singularity at south pole, i.e. angles[1]=Math.PI; c2=-1; s2=0
* assume angles[2]=0; c3=1; s3=0
* R3*R2*R1
* -c1 -s1 0
* -s1 c1 0
* 0 0 -1
*/
angles[0] = Math.atan2(m.getItem(1, 0), m.getItem(0, 0));
angles[1] = Math.PI;
angles[2] = 0.0;
} else {
/*
* R3*R2*R1
* c1*c2*c3-s1*s3 s1*c2*c3+c1*s3 -s2*c3
* -c1*c2*s3-s1*c3 -s1*c2*s3+c1*c3 s2*s3
* c1*s2 s1*s2 c2
*/
angles[0] = Math.atan2(m.getItem(1, 2), m.getItem(0, 2));
angles[1] = Math.acos(m.getItem(2, 2));
angles[2] = Math.atan2(m.getItem(2, 1), -m.getItem(2, 0));
}
}
/**
* Transforms coordinates in target coordinate system into source coord system. To obtain a
* rotation matrix use {@link #makeAngles(double, double, double)}. The transformation is done as:
* <code>
* Since:
* Inverse(ROT) = Transpose(ROT)
* P1 = [x; y; z]
* DEST = Transpose(ROT) * P1
* </code>
*/
public void transformBackward(Matrix rot, double x, double y, double z, double dest[]) {
dest[0] = x * rot.getItem(0, 0) + y * rot.getItem(0, 1) + z * rot.getItem(0, 2);
dest[1] = x * rot.getItem(1, 0) + y * rot.getItem(1, 1) + z * rot.getItem(1, 2);
dest[2] = x * rot.getItem(2, 0) + y * rot.getItem(2, 1) + z * rot.getItem(2, 2);
}
