Add Quaternion.conjugateBy(Quaternion) (#199)

src/org/joml/Quaterniond.java

@@ -2627,4 +2627,39 @@ public Vector3d normalizedPositiveZ(Vector3d dir) {
         return dir;
     }
 
+    /**
+     * Conjugate <code>this</code> by the given quaternion <code>q</code> by computing <code>q * this * q^-1</code>.
+     * 
+     * @param q
+     *          the {@link Quaterniondc} to conjugate <code>this</code> by
+     * @return this
+     */
+    public Quaterniond conjugate(Quaterniondc q) {
+        return conjugateBy(q, this);
+    }
+
+    /**
+     * Conjugate <code>this</code> by the given quaternion <code>q</code> by computing <code>q * this * q^-1</code>
+     * and store the result into <code>dest</code>.
+     * 
+     * @param q
+     *          the {@link Quaterniondc} to conjugate <code>this</code> by
+     * @param dest
+     *          will hold the result
+     * @return dest
+     */
+    public Quaterniond conjugateBy(Quaterniondc q, Quaterniond dest) {
+        double invNorm = 1.0f / (q.x() * q.x() + q.y() * q.y() + q.z() * q.z() + q.w() * q.w());
+        double qix = -q.x() * invNorm, qiy = -q.y() * invNorm, qiz = -q.z() * invNorm, qiw = q.w() * invNorm;
+        double qpx = q.w() * x + q.x() * w + q.y() * z - q.z() * y;
+        double qpy = q.w() * y - q.x() * z + q.y() * w + q.z() * x;
+        double qpz = q.w() * z + q.x() * y - q.y() * x + q.z() * w;
+        double qpw = q.w() * w - q.x() * x - q.y() * y - q.z() * z;
+        dest.x = qpw * qix + qpx * qiw + qpy * qiz - qpz * qiy;
+        dest.y = qpw * qiy - qpx * qiz + qpy * qiw + qpz * qix;
+        dest.z = qpw * qiz + qpx * qiy - qpy * qix + qpz * qiw;
+        dest.w = qpw * qiw - qpx * qix - qpy * qiy - qpz * qiz;
+        return dest;
+    }
+
 }

src/org/joml/Quaterniondc.java

@@ -1055,4 +1055,16 @@ public interface Quaterniondc {
      */
     Vector3d normalizedPositiveZ(Vector3d dir);
 
-}
+    /**
+     * Conjugate <code>this</code> by the given quaternion <code>q</code> by computing <code>q * this * q^-1</code>
+     * and store the result into <code>dest</code>.
+     * 
+     * @param q
+     *          the {@link Quaterniondc} to conjugate <code>this</code> by
+     * @param dest
+     *          will hold the result
+     * @return dest
+     */
+    Quaterniond conjugateBy(Quaterniondc q, Quaterniond dest);
+
+}

src/org/joml/Quaternionf.java

@@ -2795,4 +2795,39 @@ public Vector3f normalizedPositiveZ(Vector3f dir) {
         return dir;
     }
 
+    /**
+     * Conjugate <code>this</code> by the given quaternion <code>q</code> by computing <code>q * this * q^-1</code>.
+     * 
+     * @param q
+     *          the {@link Quaternionfc} to conjugate <code>this</code> by
+     * @return this
+     */
+    public Quaternionf conjugateBy(Quaternionfc q) {
+        return conjugateBy(q, this);
+    }
+
+    /**
+     * Conjugate <code>this</code> by the given quaternion <code>q</code> by computing <code>q * this * q^-1</code>
+     * and store the result into <code>dest</code>.
+     * 
+     * @param q
+     *          the {@link Quaternionfc} to conjugate <code>this</code> by
+     * @param dest
+     *          will hold the result
+     * @return dest
+     */
+    public Quaternionf conjugateBy(Quaternionfc q, Quaternionf dest) {
+        float invNorm = 1.0f / (q.x() * q.x() + q.y() * q.y() + q.z() * q.z() + q.w() * q.w());
+        float qix = -q.x() * invNorm, qiy = -q.y() * invNorm, qiz = -q.z() * invNorm, qiw = q.w() * invNorm;
+        float qpx = q.w() * x + q.x() * w + q.y() * z - q.z() * y;
+        float qpy = q.w() * y - q.x() * z + q.y() * w + q.z() * x;
+        float qpz = q.w() * z + q.x() * y - q.y() * x + q.z() * w;
+        float qpw = q.w() * w - q.x() * x - q.y() * y - q.z() * z;
+        dest.x = qpw * qix + qpx * qiw + qpy * qiz - qpz * qiy;
+        dest.y = qpw * qiy - qpx * qiz + qpy * qiw + qpz * qix;
+        dest.z = qpw * qiz + qpx * qiy - qpy * qix + qpz * qiw;
+        dest.w = qpw * qiw - qpx * qix - qpy * qiy - qpz * qiz;
+        return dest;
+    }
+
 }

src/org/joml/Quaternionfc.java

@@ -1162,4 +1162,16 @@ public interface Quaternionfc {
      */
     Vector3f normalizedPositiveZ(Vector3f dir);
 
+    /**
+     * Conjugate <code>this</code> by the given quaternion <code>q</code> by computing <code>q * this * q^-1</code>
+     * and store the result into <code>dest</code>.
+     * 
+     * @param q
+     *          the {@link Quaternionfc} to conjugate <code>this</code> by
+     * @param dest
+     *          will hold the result
+     * @return dest
+     */
+    Quaternionf conjugateBy(Quaternionfc q, Quaternionf dest);
+
 }

test/org/joml/test/QuaternionTest.java

@@ -141,4 +141,12 @@ public static void testRotateToReturnsDestination() {
         assertSame(destination, result);
     }
 
+    public static void testConjugateBy() {
+        Quaternionf p = new Quaternionf().rotateXYZ(0.234f, -0.62f, 0.11f);
+        Quaternionf q = new Quaternionf().rotateXYZ(0.834f, 0.42f, -1.471f);
+        Quaternionf r = p.mul(q.mul(p.invert(new Quaternionf()), new Quaternionf()), new Quaternionf());
+        Quaternionf r2 = q.conjugateBy(p, new Quaternionf());
+        TestUtil.assertQuaternionfEquals(r, r2, 1E-6f);
+    }
+
 }

test/org/joml/test/TestUtil.java

@@ -237,6 +237,20 @@ public static void assertVector3fEquals(Vector3f expected, Vector3f actual, floa
         Assert.assertEquals(expected.z, actual.z, delta);
     }
 
+    /**
+     * Assert that both quaternions are equal with respect to the given delta.
+     * 
+     * @param expected
+     * @param actual
+     * @param delta
+     */
+    public static void assertQuaternionfEquals(Quaternionf expected, Quaternionf actual, float delta) {
+        Assert.assertEquals(expected.x, actual.x, delta);
+        Assert.assertEquals(expected.y, actual.y, delta);
+        Assert.assertEquals(expected.z, actual.z, delta);
+        Assert.assertEquals(expected.w, actual.w, delta);
+    }
+
     /**
      * Assert that both vectors are equal with respect to the given delta.
      *