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