/**
   * Extracts quaternions from the provided rotation matrix.
   *
   * @param R (Input) rotation matrix
   * @param quat (Output) Optional storage for quaternion. If null a new class will be used.
   * @return unit quaternion representation of the rotation matrix.
   */
  public static Quaternion_F32 matrixToQuaternion(DenseMatrix64F R, Quaternion_F32 quat) {

    if (quat == null) quat = new Quaternion_F32();

    // algorithm from:
    // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
    //
    // Designed to minimize numerical error by not dividing by very small numbers

    float m00 = (float) R.unsafe_get(0, 0);
    float m01 = (float) R.unsafe_get(0, 1);
    float m02 = (float) R.unsafe_get(0, 2);
    float m10 = (float) R.unsafe_get(1, 0);
    float m11 = (float) R.unsafe_get(1, 1);
    float m12 = (float) R.unsafe_get(1, 2);
    float m20 = (float) R.unsafe_get(2, 0);
    float m21 = (float) R.unsafe_get(2, 1);
    float m22 = (float) R.unsafe_get(2, 2);

    float trace = m00 + m11 + m22;

    if (trace > 0) {
      float S = (float) Math.sqrt(trace + 1.0f) * 2; // S=4*qw
      quat.w = 0.25f * S;
      quat.x = (m21 - m12) / S;
      quat.y = (m02 - m20) / S;
      quat.z = (m10 - m01) / S;
    } else if ((m00 > m11) & (m00 > m22)) {
      float S = (float) Math.sqrt(1.0f + m00 - m11 - m22) * 2; // S=4*qx
      quat.w = (m21 - m12) / S;
      quat.x = 0.25f * S;
      quat.y = (m01 + m10) / S;
      quat.z = (m02 + m20) / S;
    } else if (m11 > m22) {
      float S = (float) Math.sqrt(1.0f + m11 - m00 - m22) * 2; // S=4*qy
      quat.w = (m02 - m20) / S;
      quat.x = (m01 + m10) / S;
      quat.y = 0.25f * S;
      quat.z = (m12 + m21) / S;
    } else {
      float S = (float) Math.sqrt(1.0f + m22 - m00 - m11) * 2; // S=4*qz
      quat.w = (m10 - m01) / S;
      quat.x = (m02 + m20) / S;
      quat.y = (m12 + m21) / S;
      quat.z = 0.25f * S;
    }

    return quat;
  }
  /**
   * Converts a rotation matrix into {@link georegression.struct.so.Rodrigues_F32}.
   *
   * @param R Rotation matrix.
   * @param rodrigues Storage used for solution. If null a new instance is declared.
   * @return The found axis and rotation angle.
   */
  public static Rodrigues_F32 matrixToRodrigues(DenseMatrix64F R, Rodrigues_F32 rodrigues) {
    if (rodrigues == null) {
      rodrigues = new Rodrigues_F32();
    }
    // parts of this are from wikipedia
    // http://en.wikipedia.org/wiki/Rotation_representation_%28mathematics%29#Rotation_matrix_.E2.86f.94_Euler_axis.2Fangle

    float diagSum =
        ((float) (R.unsafe_get(0, 0) + R.unsafe_get(1, 1) + R.unsafe_get(2, 2)) - 1.0f) / 2.0f;

    float absDiagSum = (float) Math.abs(diagSum);

    if (absDiagSum <= 1.0f && 1.0f - absDiagSum > 10.0f * GrlConstants.F_EPS) {
      // if numerically stable use a faster technique
      rodrigues.theta = (float) Math.acos(diagSum);
      float bottom = 2.0f * (float) Math.sin(rodrigues.theta);

      // in cases where bottom is close to zero that means theta is also close to zero and the
      // vector
      // doesn't matter that much
      rodrigues.unitAxisRotation.x = (float) (R.unsafe_get(2, 1) - R.unsafe_get(1, 2)) / bottom;
      rodrigues.unitAxisRotation.y = (float) (R.unsafe_get(0, 2) - R.unsafe_get(2, 0)) / bottom;
      rodrigues.unitAxisRotation.z = (float) (R.unsafe_get(1, 0) - R.unsafe_get(0, 1)) / bottom;

      // in extreme underflow situations the result can be unnormalized
      rodrigues.unitAxisRotation.normalize();

      // In theory this might be more stable
      // rotationAxis( R, rodrigues.unitAxisRotation);
    } else {

      // this handles the special case where the bottom is very very small or equal to zero
      if (diagSum >= 1.0f) rodrigues.theta = 0;
      else if (diagSum <= -1.0f) rodrigues.theta = (float) Math.PI;
      else rodrigues.theta = (float) Math.acos(diagSum);

      // compute the value of x,y,z up to a sign ambiguity
      rodrigues.unitAxisRotation.x = (float) Math.sqrt((R.get(0, 0) + 1) / 2);
      rodrigues.unitAxisRotation.y = (float) Math.sqrt((R.get(1, 1) + 1) / 2);
      rodrigues.unitAxisRotation.z = (float) Math.sqrt((R.get(2, 2) + 1) / 2);

      float x = rodrigues.unitAxisRotation.x;
      float y = rodrigues.unitAxisRotation.y;
      float z = rodrigues.unitAxisRotation.z;

      if (Math.abs(R.get(1, 0) - 2 * x * y) > GrlConstants.F_EPS) {
        x *= -1;
      }
      if (Math.abs(R.get(2, 0) - 2 * x * z) > GrlConstants.F_EPS) {
        z *= -1;
      }
      if (Math.abs(R.get(2, 1) - 2 * z * y) > GrlConstants.F_EPS) {
        y *= -1;
        x *= -1;
      }

      rodrigues.unitAxisRotation.x = x;
      rodrigues.unitAxisRotation.y = y;
      rodrigues.unitAxisRotation.z = z;
    }

    return rodrigues;
  }
Пример #3
0
 public Double get(int index) {
   return values.unsafe_get(index, 0);
 }