示例#1
0
  public Matrix make_dF_dR1(double r1, double r2, double r3) {
    double s1 = Math.sin(r1);
    double c1 = Math.cos(r1);

    double s2 = Math.sin(r2);
    double c2 = Math.cos(r2);

    double s3 = Math.sin(r3);
    double c3 = Math.cos(r3);

    /*
     * 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
     *
     * dF/d(r1)
     * -s1*c2*c3-c1*s3	c1*c2*c3-s1*s3	0
     * s1*c2*s3-c1*c3	-c1*c2*c3-s1*c3	0
     * -s1*s2			c1*s2			0
     */
    Matrix r = new Matrix(3, 3);
    r.setItem(0, 0, -s1 * c2 * c3 - c1 * s3);
    r.setItem(1, 0, c1 * c2 * c3 - s1 * s3);
    r.setItem(2, 0, 0);
    r.setItem(0, 1, s1 * c2 * s3 - c1 * c3);
    r.setItem(1, 1, -c1 * c2 * c3 - s1 * c3);
    r.setItem(2, 1, 0);
    r.setItem(0, 2, s1 * s2);
    r.setItem(1, 2, c1 * s2);
    r.setItem(2, 2, 0);
    return r;
  }
示例#2
0
  /**
   * Return a rotation matrix R=R1*R2*R3
   *
   * <p>r1 = rz1 r2 = ry r3 = rz2
   */
  public Matrix makeAngles(double r1, double r2, double r3) {
    double s1 = Math.sin(r1);
    double c1 = Math.cos(r1);

    double s2 = Math.sin(r2);
    double c2 = Math.cos(r2);

    double s3 = Math.sin(r3);
    double c3 = Math.cos(r3);

    /*
     * 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
     */
    Matrix r = new Matrix(3, 3);
    r.setItem(0, 0, c1 * c2 * c3 - s1 * s3);
    r.setItem(1, 0, s1 * c2 * c3 + c1 * s3);
    r.setItem(2, 0, -s2 * c3);

    r.setItem(0, 1, -c1 * c2 * s3 - s1 * c3);
    r.setItem(1, 1, -s1 * c2 * s3 + c1 * c3);
    r.setItem(2, 1, s2 * s3);

    r.setItem(0, 2, c1 * s2);
    r.setItem(1, 2, s1 * s2);
    r.setItem(2, 2, c2);
    return r;
  }
示例#3
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));
   }
 }
示例#4
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);
 }