/**
   * Multiply the right matrix by the left and place the result in a third matrix.
   *
   * @param left The left source matrix
   * @param right The right source matrix
   * @param dest The destination matrix, or null if a new one is to be created
   * @return the destination matrix
   */
  public static Matrix4f mul(Matrix4f left, Matrix4f right, Matrix4f dest) {
    if (dest == null) dest = new Matrix4f();

    float m00 =
        left.m00 * right.m00 + left.m10 * right.m01 + left.m20 * right.m02 + left.m30 * right.m03;
    float m01 =
        left.m01 * right.m00 + left.m11 * right.m01 + left.m21 * right.m02 + left.m31 * right.m03;
    float m02 =
        left.m02 * right.m00 + left.m12 * right.m01 + left.m22 * right.m02 + left.m32 * right.m03;
    float m03 =
        left.m03 * right.m00 + left.m13 * right.m01 + left.m23 * right.m02 + left.m33 * right.m03;
    float m10 =
        left.m00 * right.m10 + left.m10 * right.m11 + left.m20 * right.m12 + left.m30 * right.m13;
    float m11 =
        left.m01 * right.m10 + left.m11 * right.m11 + left.m21 * right.m12 + left.m31 * right.m13;
    float m12 =
        left.m02 * right.m10 + left.m12 * right.m11 + left.m22 * right.m12 + left.m32 * right.m13;
    float m13 =
        left.m03 * right.m10 + left.m13 * right.m11 + left.m23 * right.m12 + left.m33 * right.m13;
    float m20 =
        left.m00 * right.m20 + left.m10 * right.m21 + left.m20 * right.m22 + left.m30 * right.m23;
    float m21 =
        left.m01 * right.m20 + left.m11 * right.m21 + left.m21 * right.m22 + left.m31 * right.m23;
    float m22 =
        left.m02 * right.m20 + left.m12 * right.m21 + left.m22 * right.m22 + left.m32 * right.m23;
    float m23 =
        left.m03 * right.m20 + left.m13 * right.m21 + left.m23 * right.m22 + left.m33 * right.m23;
    float m30 =
        left.m00 * right.m30 + left.m10 * right.m31 + left.m20 * right.m32 + left.m30 * right.m33;
    float m31 =
        left.m01 * right.m30 + left.m11 * right.m31 + left.m21 * right.m32 + left.m31 * right.m33;
    float m32 =
        left.m02 * right.m30 + left.m12 * right.m31 + left.m22 * right.m32 + left.m32 * right.m33;
    float m33 =
        left.m03 * right.m30 + left.m13 * right.m31 + left.m23 * right.m32 + left.m33 * right.m33;

    dest.m00 = m00;
    dest.m01 = m01;
    dest.m02 = m02;
    dest.m03 = m03;
    dest.m10 = m10;
    dest.m11 = m11;
    dest.m12 = m12;
    dest.m13 = m13;
    dest.m20 = m20;
    dest.m21 = m21;
    dest.m22 = m22;
    dest.m23 = m23;
    dest.m30 = m30;
    dest.m31 = m31;
    dest.m32 = m32;
    dest.m33 = m33;

    return dest;
  }
 /**
  * Scales the source matrix and put the result in the destination matrix
  *
  * @param vec The vector to scale by
  * @param src The source matrix
  * @param dest The destination matrix, or null if a new matrix is to be created
  * @return The scaled matrix
  */
 public static Matrix4f scale(Vector3f vec, Matrix4f src, Matrix4f dest) {
   if (dest == null) dest = new Matrix4f();
   dest.m00 = src.m00 * vec.x;
   dest.m01 = src.m01 * vec.x;
   dest.m02 = src.m02 * vec.x;
   dest.m03 = src.m03 * vec.x;
   dest.m10 = src.m10 * vec.y;
   dest.m11 = src.m11 * vec.y;
   dest.m12 = src.m12 * vec.y;
   dest.m13 = src.m13 * vec.y;
   dest.m20 = src.m20 * vec.z;
   dest.m21 = src.m21 * vec.z;
   dest.m22 = src.m22 * vec.z;
   dest.m23 = src.m23 * vec.z;
   return dest;
 }
  /**
   * Rotates the source matrix around the given axis the specified angle and put the result in the
   * destination matrix.
   *
   * @param angle the angle, in radians.
   * @param axis The vector representing the rotation axis. Must be normalized.
   * @param src The matrix to rotate
   * @param dest The matrix to put the result, or null if a new matrix is to be created
   * @return The rotated matrix
   */
  public static Matrix4f rotate(float angle, Vector3f axis, Matrix4f src, Matrix4f dest) {
    if (dest == null) dest = new Matrix4f();
    float c = (float) Math.cos(angle);
    float s = (float) Math.sin(angle);
    float oneminusc = 1.0f - c;
    float xy = axis.x * axis.y;
    float yz = axis.y * axis.z;
    float xz = axis.x * axis.z;
    float xs = axis.x * s;
    float ys = axis.y * s;
    float zs = axis.z * s;

    float f00 = axis.x * axis.x * oneminusc + c;
    float f01 = xy * oneminusc + zs;
    float f02 = xz * oneminusc - ys;
    // n[3] not used
    float f10 = xy * oneminusc - zs;
    float f11 = axis.y * axis.y * oneminusc + c;
    float f12 = yz * oneminusc + xs;
    // n[7] not used
    float f20 = xz * oneminusc + ys;
    float f21 = yz * oneminusc - xs;
    float f22 = axis.z * axis.z * oneminusc + c;

    float t00 = src.m00 * f00 + src.m10 * f01 + src.m20 * f02;
    float t01 = src.m01 * f00 + src.m11 * f01 + src.m21 * f02;
    float t02 = src.m02 * f00 + src.m12 * f01 + src.m22 * f02;
    float t03 = src.m03 * f00 + src.m13 * f01 + src.m23 * f02;
    float t10 = src.m00 * f10 + src.m10 * f11 + src.m20 * f12;
    float t11 = src.m01 * f10 + src.m11 * f11 + src.m21 * f12;
    float t12 = src.m02 * f10 + src.m12 * f11 + src.m22 * f12;
    float t13 = src.m03 * f10 + src.m13 * f11 + src.m23 * f12;
    dest.m20 = src.m00 * f20 + src.m10 * f21 + src.m20 * f22;
    dest.m21 = src.m01 * f20 + src.m11 * f21 + src.m21 * f22;
    dest.m22 = src.m02 * f20 + src.m12 * f21 + src.m22 * f22;
    dest.m23 = src.m03 * f20 + src.m13 * f21 + src.m23 * f22;
    dest.m00 = t00;
    dest.m01 = t01;
    dest.m02 = t02;
    dest.m03 = t03;
    dest.m10 = t10;
    dest.m11 = t11;
    dest.m12 = t12;
    dest.m13 = t13;
    return dest;
  }
  /**
   * Set the given matrix to 0.
   *
   * @param m The matrix to set to 0
   * @return m
   */
  public static Matrix4f setZero(Matrix4f m) {
    m.m00 = 0.0f;
    m.m01 = 0.0f;
    m.m02 = 0.0f;
    m.m03 = 0.0f;
    m.m10 = 0.0f;
    m.m11 = 0.0f;
    m.m12 = 0.0f;
    m.m13 = 0.0f;
    m.m20 = 0.0f;
    m.m21 = 0.0f;
    m.m22 = 0.0f;
    m.m23 = 0.0f;
    m.m30 = 0.0f;
    m.m31 = 0.0f;
    m.m32 = 0.0f;
    m.m33 = 0.0f;

    return m;
  }
  /**
   * Set the given matrix to be the identity matrix.
   *
   * @param m The matrix to set to the identity
   * @return m
   */
  public static Matrix4f setIdentity(Matrix4f m) {
    m.m00 = 1.0f;
    m.m01 = 0.0f;
    m.m02 = 0.0f;
    m.m03 = 0.0f;
    m.m10 = 0.0f;
    m.m11 = 1.0f;
    m.m12 = 0.0f;
    m.m13 = 0.0f;
    m.m20 = 0.0f;
    m.m21 = 0.0f;
    m.m22 = 1.0f;
    m.m23 = 0.0f;
    m.m30 = 0.0f;
    m.m31 = 0.0f;
    m.m32 = 0.0f;
    m.m33 = 1.0f;

    return m;
  }
  /**
   * Copy the source matrix to the destination matrix
   *
   * @param src The source matrix
   * @param dest The destination matrix, or null of a new one is to be created
   * @return The copied matrix
   */
  public static Matrix4f load(Matrix4f src, Matrix4f dest) {
    if (dest == null) dest = new Matrix4f();
    dest.m00 = src.m00;
    dest.m01 = src.m01;
    dest.m02 = src.m02;
    dest.m03 = src.m03;
    dest.m10 = src.m10;
    dest.m11 = src.m11;
    dest.m12 = src.m12;
    dest.m13 = src.m13;
    dest.m20 = src.m20;
    dest.m21 = src.m21;
    dest.m22 = src.m22;
    dest.m23 = src.m23;
    dest.m30 = src.m30;
    dest.m31 = src.m31;
    dest.m32 = src.m32;
    dest.m33 = src.m33;

    return dest;
  }
  /**
   * Subtract the right matrix from the left and place the result in a third matrix.
   *
   * @param left The left source matrix
   * @param right The right source matrix
   * @param dest The destination matrix, or null if a new one is to be created
   * @return the destination matrix
   */
  public static Matrix4f sub(Matrix4f left, Matrix4f right, Matrix4f dest) {
    if (dest == null) dest = new Matrix4f();

    dest.m00 = left.m00 - right.m00;
    dest.m01 = left.m01 - right.m01;
    dest.m02 = left.m02 - right.m02;
    dest.m03 = left.m03 - right.m03;
    dest.m10 = left.m10 - right.m10;
    dest.m11 = left.m11 - right.m11;
    dest.m12 = left.m12 - right.m12;
    dest.m13 = left.m13 - right.m13;
    dest.m20 = left.m20 - right.m20;
    dest.m21 = left.m21 - right.m21;
    dest.m22 = left.m22 - right.m22;
    dest.m23 = left.m23 - right.m23;
    dest.m30 = left.m30 - right.m30;
    dest.m31 = left.m31 - right.m31;
    dest.m32 = left.m32 - right.m32;
    dest.m33 = left.m33 - right.m33;

    return dest;
  }
  /**
   * Add two matrices together and place the result in a third matrix.
   *
   * @param left The left source matrix
   * @param right The right source matrix
   * @param dest The destination matrix, or null if a new one is to be created
   * @return the destination matrix
   */
  public static Matrix4f add(Matrix4f left, Matrix4f right, Matrix4f dest) {
    if (dest == null) dest = new Matrix4f();

    dest.m00 = left.m00 + right.m00;
    dest.m01 = left.m01 + right.m01;
    dest.m02 = left.m02 + right.m02;
    dest.m03 = left.m03 + right.m03;
    dest.m10 = left.m10 + right.m10;
    dest.m11 = left.m11 + right.m11;
    dest.m12 = left.m12 + right.m12;
    dest.m13 = left.m13 + right.m13;
    dest.m20 = left.m20 + right.m20;
    dest.m21 = left.m21 + right.m21;
    dest.m22 = left.m22 + right.m22;
    dest.m23 = left.m23 + right.m23;
    dest.m30 = left.m30 + right.m30;
    dest.m31 = left.m31 + right.m31;
    dest.m32 = left.m32 + right.m32;
    dest.m33 = left.m33 + right.m33;

    return dest;
  }
  /**
   * Transpose the source matrix and place the result in the destination matrix
   *
   * @param src The source matrix
   * @param dest The destination matrix or null if a new matrix is to be created
   * @return the transposed matrix
   */
  public static Matrix4f transpose(Matrix4f src, Matrix4f dest) {
    if (dest == null) dest = new Matrix4f();
    float m00 = src.m00;
    float m01 = src.m10;
    float m02 = src.m20;
    float m03 = src.m30;
    float m10 = src.m01;
    float m11 = src.m11;
    float m12 = src.m21;
    float m13 = src.m31;
    float m20 = src.m02;
    float m21 = src.m12;
    float m22 = src.m22;
    float m23 = src.m32;
    float m30 = src.m03;
    float m31 = src.m13;
    float m32 = src.m23;
    float m33 = src.m33;

    dest.m00 = m00;
    dest.m01 = m01;
    dest.m02 = m02;
    dest.m03 = m03;
    dest.m10 = m10;
    dest.m11 = m11;
    dest.m12 = m12;
    dest.m13 = m13;
    dest.m20 = m20;
    dest.m21 = m21;
    dest.m22 = m22;
    dest.m23 = m23;
    dest.m30 = m30;
    dest.m31 = m31;
    dest.m32 = m32;
    dest.m33 = m33;

    return dest;
  }
Ejemplo n.º 10
0
  /**
   * Invert the source matrix and put the result in the destination
   *
   * @param src The source matrix
   * @param dest The destination matrix, or null if a new matrix is to be created
   * @return The inverted matrix if successful, null otherwise
   */
  public static Matrix4f invert(Matrix4f src, Matrix4f dest) {
    float determinant = src.determinant();

    if (determinant != 0) {
      /*
       * m00 m01 m02 m03
       * m10 m11 m12 m13
       * m20 m21 m22 m23
       * m30 m31 m32 m33
       */
      if (dest == null) dest = new Matrix4f();
      float determinant_inv = 1f / determinant;

      // first row
      float t00 =
          determinant3x3(
              src.m11, src.m12, src.m13, src.m21, src.m22, src.m23, src.m31, src.m32, src.m33);
      float t01 =
          -determinant3x3(
              src.m10, src.m12, src.m13, src.m20, src.m22, src.m23, src.m30, src.m32, src.m33);
      float t02 =
          determinant3x3(
              src.m10, src.m11, src.m13, src.m20, src.m21, src.m23, src.m30, src.m31, src.m33);
      float t03 =
          -determinant3x3(
              src.m10, src.m11, src.m12, src.m20, src.m21, src.m22, src.m30, src.m31, src.m32);
      // second row
      float t10 =
          -determinant3x3(
              src.m01, src.m02, src.m03, src.m21, src.m22, src.m23, src.m31, src.m32, src.m33);
      float t11 =
          determinant3x3(
              src.m00, src.m02, src.m03, src.m20, src.m22, src.m23, src.m30, src.m32, src.m33);
      float t12 =
          -determinant3x3(
              src.m00, src.m01, src.m03, src.m20, src.m21, src.m23, src.m30, src.m31, src.m33);
      float t13 =
          determinant3x3(
              src.m00, src.m01, src.m02, src.m20, src.m21, src.m22, src.m30, src.m31, src.m32);
      // third row
      float t20 =
          determinant3x3(
              src.m01, src.m02, src.m03, src.m11, src.m12, src.m13, src.m31, src.m32, src.m33);
      float t21 =
          -determinant3x3(
              src.m00, src.m02, src.m03, src.m10, src.m12, src.m13, src.m30, src.m32, src.m33);
      float t22 =
          determinant3x3(
              src.m00, src.m01, src.m03, src.m10, src.m11, src.m13, src.m30, src.m31, src.m33);
      float t23 =
          -determinant3x3(
              src.m00, src.m01, src.m02, src.m10, src.m11, src.m12, src.m30, src.m31, src.m32);
      // fourth row
      float t30 =
          -determinant3x3(
              src.m01, src.m02, src.m03, src.m11, src.m12, src.m13, src.m21, src.m22, src.m23);
      float t31 =
          determinant3x3(
              src.m00, src.m02, src.m03, src.m10, src.m12, src.m13, src.m20, src.m22, src.m23);
      float t32 =
          -determinant3x3(
              src.m00, src.m01, src.m03, src.m10, src.m11, src.m13, src.m20, src.m21, src.m23);
      float t33 =
          determinant3x3(
              src.m00, src.m01, src.m02, src.m10, src.m11, src.m12, src.m20, src.m21, src.m22);

      // transpose and divide by the determinant
      dest.m00 = t00 * determinant_inv;
      dest.m11 = t11 * determinant_inv;
      dest.m22 = t22 * determinant_inv;
      dest.m33 = t33 * determinant_inv;
      dest.m01 = t10 * determinant_inv;
      dest.m10 = t01 * determinant_inv;
      dest.m20 = t02 * determinant_inv;
      dest.m02 = t20 * determinant_inv;
      dest.m12 = t21 * determinant_inv;
      dest.m21 = t12 * determinant_inv;
      dest.m03 = t30 * determinant_inv;
      dest.m30 = t03 * determinant_inv;
      dest.m13 = t31 * determinant_inv;
      dest.m31 = t13 * determinant_inv;
      dest.m32 = t23 * determinant_inv;
      dest.m23 = t32 * determinant_inv;
      return dest;
    } else return null;
  }