setAxisAngleFromRotationMatrix method

Vector4 setAxisAngleFromRotationMatrix(Matrix3 m)

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.storage;
  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 (((m12 - m21).abs() < epsilon) &&
      ((m13 - m31).abs() < epsilon) &&
      ((m23 - m32).abs() < epsilon)) {
    // singularity found
    // first check for identity matrix which must have +1 for all terms
    // in leading diagonal and zero in other terms

    if (((m12 + m21).abs() < epsilon2) &&
        ((m13 + m31).abs() < epsilon2) &&
        ((m23 + m32).abs() < epsilon2) &&
        ((m11 + m22 + m33 - 3).abs() < epsilon2)) {
      // this singularity is identity matrix so angle = 0

      setValues(1, 0, 0, 0);

      return this; // zero angle, arbitrary axis

    }

    // otherwise this singularity is angle = 180

    angle = math.pi;

    final xx = (m11 + 1) / 2;
    final yy = (m22 + 1) / 2;
    final zz = (m33 + 1) / 2;
    final xy = (m12 + m21) / 4;
    final xz = (m13 + m31) / 4;
    final 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;
      }
    }

    setValues(x, y, z, angle);

    return this; // return 180 deg rotation

  }

  // as we have reached here there are no singularities so we can handle normally

  double s = math.sqrt((m32 - m23) * (m32 - m23) +
      (m13 - m31) * (m13 - m31) +
      (m21 - m12) * (m21 - m12)); // used to normalize

  if (s.abs() < 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;
}