Ejemplo n.º 1
0
  /**
   * Simply returns {@code log(a. inverse() * b)}.
   *
   * <p>Useful for {@link #squadTangent(Quaternion, Quaternion, Quaternion)}.
   *
   * @param a the first Quaternion
   * @param b the second Quaternion
   */
  public static final Quaternion lnDif(Quaternion a, Quaternion b) {
    Quaternion dif = a.inverse();
    dif.multiply(b);

    dif.normalize();
    return dif.log();
  }
Ejemplo n.º 2
0
  /**
   * Set the Quaternion from a (supposedly correct) 3x3 rotation matrix.
   *
   * <p>The matrix is expressed in European format: its three columns are the images by the rotation
   * of the three vectors of an orthogonal basis.
   *
   * <p>{@link #fromRotatedBasis(PVector, PVector, PVector)} sets a Quaternion from the three axis
   * of a rotated frame. It actually fills the three columns of a matrix with these rotated basis
   * vectors and calls this method.
   *
   * @param m the 3*3 matrix of float values
   */
  public final void fromRotationMatrix(float m[][]) {
    // Compute one plus the trace of the matrix
    float onePlusTrace = 1.0f + m[0][0] + m[1][1] + m[2][2];

    if (onePlusTrace > 1E-5f) {
      // Direct computation
      float s = PApplet.sqrt(onePlusTrace) * 2.0f;
      this.x = (m[2][1] - m[1][2]) / s;
      this.y = (m[0][2] - m[2][0]) / s;
      this.z = (m[1][0] - m[0][1]) / s;
      this.w = 0.25f * s;
    } else {
      // Computation depends on major diagonal term
      if ((m[0][0] > m[1][1]) & (m[0][0] > m[2][2])) {
        float s = PApplet.sqrt(1.0f + m[0][0] - m[1][1] - m[2][2]) * 2.0f;
        this.x = 0.25f * s;
        this.y = (m[0][1] + m[1][0]) / s;
        this.z = (m[0][2] + m[2][0]) / s;
        this.w = (m[1][2] - m[2][1]) / s;
      } else if (m[1][1] > m[2][2]) {
        float s = PApplet.sqrt(1.0f + m[1][1] - m[0][0] - m[2][2]) * 2.0f;
        this.x = (m[0][1] + m[1][0]) / s;
        this.y = 0.25f * s;
        this.z = (m[1][2] + m[2][1]) / s;
        this.w = (m[0][2] - m[2][0]) / s;
      } else {
        float s = PApplet.sqrt(1.0f + m[2][2] - m[0][0] - m[1][1]) * 2.0f;
        this.x = (m[0][2] + m[2][0]) / s;
        this.y = (m[1][2] + m[2][1]) / s;
        this.z = 0.25f * s;
        this.w = (m[0][1] - m[1][0]) / s;
      }
    }
    normalize();
  }