示例#1
0
 /**
  * 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));
   }
 }
示例#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);
 }