Add Matrix4f.transformTranspose() and Vector4f.mulTranspose()

JOML-CI · Apr 8, 2020 · 094f9fd · 094f9fd
1 parent b2d980c
commit 094f9fd
Show file tree

Hide file tree

Showing 5 changed files with 119 additions and 4 deletions.
diff --git a/src/org/joml/Matrix4f.java b/src/org/joml/Matrix4f.java
@@ -4611,6 +4611,16 @@ public Vector4f transform(float x, float y, float z, float w, Vector4f dest) {
        return dest.set(x, y, z, w).mul(this);
     }
 
+    public Vector4f transformTranspose(Vector4f v) {
+        return v.mulTranspose(this);
+    }
+    public Vector4f transformTranspose(Vector4fc v, Vector4f dest) {
+        return v.mulTranspose(this, dest);
+    }
+    public Vector4f transformTranspose(float x, float y, float z, float w, Vector4f dest) {
+       return dest.set(x, y, z, w).mulTranspose(this);
+    }
+
     /* (non-Javadoc)
      * @see org.joml.Matrix4fc#transformProject(org.joml.Vector4f)
      */

diff --git a/src/org/joml/Matrix4fc.java b/src/org/joml/Matrix4fc.java
@@ -1390,6 +1390,47 @@ Matrix4f mul3x3(
      */
     Vector4f transform(float x, float y, float z, float w, Vector4f dest);
 
+    /**
+     * Transform/multiply the given vector by the transpose of this matrix and store the result in that vector.
+     * 
+     * @see Vector4f#mulTranspose(Matrix4fc)
+     * 
+     * @param v
+     *          the vector to transform and to hold the final result
+     * @return v
+     */
+    Vector4f transformTranspose(Vector4f v);
+
+    /**
+     * Transform/multiply the given vector by the transpose of this matrix and store the result in <code>dest</code>.
+     * 
+     * @see Vector4f#mulTranspose(Matrix4fc, Vector4f)
+     * 
+     * @param v
+     *          the vector to transform
+     * @param dest
+     *          will contain the result
+     * @return dest
+     */
+    Vector4f transformTranspose(Vector4fc v, Vector4f dest);
+
+    /**
+     * Transform/multiply the vector <code>(x, y, z, w)</code> by the transpose of this matrix and store the result in <code>dest</code>.
+     * 
+     * @param x
+     *          the x coordinate of the vector to transform
+     * @param y
+     *          the y coordinate of the vector to transform
+     * @param z
+     *          the z coordinate of the vector to transform
+     * @param w
+     *          the w coordinate of the vector to transform
+     * @param dest
+     *          will contain the result
+     * @return dest
+     */
+    Vector4f transformTranspose(float x, float y, float z, float w, Vector4f dest);
+
     /**
      * Transform/multiply the given vector by this matrix, perform perspective divide and store the result in that vector.
      * 

diff --git a/src/org/joml/Vector4f.java b/src/org/joml/Vector4f.java
@@ -994,16 +994,31 @@ public Vector4f mul(Matrix4fc mat) {
             return mulAffine(mat, this);
         return mulGeneric(mat, this);
     }
-
-    /* (non-Javadoc)
-     * @see org.joml.Vector4fc#mul(org.joml.Matrix4fc, org.joml.Vector4f)
-     */
     public Vector4f mul(Matrix4fc mat, Vector4f dest) {
         if ((mat.properties() & Matrix4fc.PROPERTY_AFFINE) != 0)
             return mulAffine(mat, dest);
         return mulGeneric(mat, dest);
     }
 
+    /**
+     * Multiply the transpose of the given matrix <code>mat</code> with this Vector4f and store the result in
+     * <code>this</code>.
+     * 
+     * @param mat
+     *          the matrix whose transpose to multiply the vector with
+     * @return a vector holding the result
+     */
+    public Vector4f mulTranspose(Matrix4fc mat) {
+        if ((mat.properties() & Matrix4fc.PROPERTY_AFFINE) != 0)
+            return mulAffineTranspose(mat, this);
+        return mulGenericTranspose(mat, this);
+    }
+    public Vector4f mulTranspose(Matrix4fc mat, Vector4f dest) {
+        if ((mat.properties() & Matrix4fc.PROPERTY_AFFINE) != 0)
+            return mulAffineTranspose(mat, dest);
+        return mulGenericTranspose(mat, dest);
+    }
+
     /* (non-Javadoc)
      * @see org.joml.Vector4fc#mulAffine(org.joml.Matrix4fc, org.joml.Vector4f)
      */
@@ -1025,6 +1040,23 @@ private Vector4f mulGeneric(Matrix4fc mat, Vector4f dest) {
         return dest;
     }
 
+    public Vector4f mulAffineTranspose(Matrix4fc mat, Vector4f dest) {
+        float x = this.x, y = this.y, z = this.z, w = this.w;
+        dest.x = Math.fma(mat.m00(), x, Math.fma(mat.m01(), y, Math.fma(mat.m02(), z, mat.m03() * w)));
+        dest.y = Math.fma(mat.m10(), x, Math.fma(mat.m11(), y, Math.fma(mat.m12(), z, mat.m13() * w)));
+        dest.z = Math.fma(mat.m20(), x, Math.fma(mat.m21(), y, Math.fma(mat.m22(), z, mat.m23() * w)));
+        dest.w = w;
+        return dest;
+    }
+    private Vector4f mulGenericTranspose(Matrix4fc mat, Vector4f dest) {
+        float x = this.x, y = this.y, z = this.z, w = this.w;
+        dest.x = Math.fma(mat.m00(), x, Math.fma(mat.m01(), y, Math.fma(mat.m02(), z, mat.m03() * w)));
+        dest.y = Math.fma(mat.m10(), x, Math.fma(mat.m11(), y, Math.fma(mat.m12(), z, mat.m13() * w)));
+        dest.z = Math.fma(mat.m20(), x, Math.fma(mat.m21(), y, Math.fma(mat.m22(), z, mat.m23() * w)));
+        dest.w = Math.fma(mat.m30(), x, Math.fma(mat.m31(), y, Math.fma(mat.m32(), z, mat.m33() * w)));
+        return dest;
+    }
+
     /**
      * Multiply the given matrix mat with this Vector4f and store the result in
      * <code>this</code>.

diff --git a/src/org/joml/Vector4fc.java b/src/org/joml/Vector4fc.java
@@ -281,6 +281,18 @@ public interface Vector4fc {
      */
     Vector4f mul(Matrix4fc mat, Vector4f dest);
 
+    /**
+     * Multiply the transpose of the given matrix <code>mat</code> with this Vector4f and store the result in
+     * <code>dest</code>.
+     * 
+     * @param mat
+     *          the matrix whose transpose to multiply the vector with
+     * @param dest
+     *          the destination vector to hold the result
+     * @return dest
+     */
+    Vector4f mulTranspose(Matrix4fc mat, Vector4f dest);
+
     /**
      * Multiply the given affine matrix mat with this Vector4f and store the result in
      * <code>dest</code>.
@@ -293,6 +305,18 @@ public interface Vector4fc {
      */
     Vector4f mulAffine(Matrix4fc mat, Vector4f dest);
 
+    /**
+     * Multiply the transpose of the given affine matrix <code>mat</code> with this Vector4f and store the result in
+     * <code>dest</code>.
+     * 
+     * @param mat
+     *          the affine matrix whose transpose to multiply the vector with
+     * @param dest
+     *          the destination vector to hold the result
+     * @return dest
+     */
+    Vector4f mulAffineTranspose(Matrix4fc mat, Vector4f dest);
+
     /**
      * Multiply the given matrix mat with this Vector4f and store the result in
      * <code>dest</code>.

diff --git a/test/org/joml/test/Matrix4fTest.java b/test/org/joml/test/Matrix4fTest.java
@@ -531,4 +531,12 @@ public static void testTestAab() {
         assertFalse(m.testAab(10.001f*0.5f, 0, 0, 10, 1, 1));
     }
 
+    public static void testTransformTranspose() {
+        Matrix4f m = new Matrix4f(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16);
+        TestUtil.assertVector4fEquals(
+                m.transformTranspose(new Vector4f(4, 5, 6, 7)), 
+                m.transpose(new Matrix4f()).transform(new Vector4f(4, 5, 6, 7)), 
+                1E-6f);
+    }
+
 }