Fix Vector3/4.rotateAbout()

Thanks to @theagentd
JOML-CI · May 27, 2017 · e341b03 · e341b03
1 parent 659ce11
commit e341b03
Show file tree

Hide file tree

Showing 8 changed files with 56 additions and 68 deletions.
diff --git a/src/org/joml/Vector3d.java b/src/org/joml/Vector3d.java
@@ -1748,19 +1748,16 @@ public Vector3d rotateAbout(double angle, double x, double y, double z) {
     /* (non-Javadoc)
      * @see org.joml.Vector3dc#rotateAbout(double, double, double, double, org.joml.Vector3d)
      */
-    public Vector3d rotateAbout(double angle, double x, double y, double z, Vector3d dest) {
-        double q0x = this.x, q0y = this.y, q0z = this.z;
-        double sin = Math.sin(angle * 0.5);
-        double cos = Math.cosFromSin(sin, angle * 0.5);
-        double q1x = x * sin, q1y = y * sin, q1z = z * sin, q1w = cos;
-        double scale = 1.0f / (q1x * q1x + q1y * q1y + q1z * q1z);
-        double q2x =  q1w * q0x + q1y * q0z - q1z * q0y;
-        double q2y =  q1w * q0y - q1x * q0z + q1z * q0x;
-        double q2z =  q1w * q0z + q1x * q0y - q1y * q0x;
-        double q2w = -q1x * q0x - q1y * q0y - q1z * q0z;
-        dest.x = (-q2w * q1x + q2x * q1w - q2y * q1z + q2z * q1y) * scale;
-        dest.y = (-q2w * q1y + q2x * q1z + q2y * q1w - q2z * q1x) * scale;
-        dest.z = (-q2w * q1z - q2x * q1y + q2y * q1x + q2z * q1w) * scale;
+    public Vector3d rotateAbout(double angle, double aX, double aY, double aZ, Vector3d dest) {
+        double hangle = angle * 0.5;
+        double sinAngle = Math.sin(hangle);
+        double qx = aX * sinAngle, qy = aY * sinAngle, qz = aZ * sinAngle;
+        double qw = Math.cosFromSin(sinAngle, hangle);
+        double w2 = qw * qw, x2 = qx * qx, y2 = qy * qy, z2 = qz * qz, zw = qz * qw;
+        double xy = qx * qy, xz = qx * qz, yw = qy * qw, yz = qy * qz, xw = qx * qw;
+        dest.x = (w2 + x2 - z2 - y2) * x + (-zw + xy - zw + xy) * y + (yw + xz + xz + yw) * z;
+        dest.y = (xy + zw + zw + xy) * x + ( y2 - z2 + w2 - x2) * y + (yz + yz - xw - xw) * z;
+        dest.z = (xz - yw + xz - yw) * x + ( yz + yz + xw + xw) * y + (z2 - y2 - x2 + w2) * z;
         return this;
     }
 

diff --git a/src/org/joml/Vector3dc.java b/src/org/joml/Vector3dc.java
@@ -664,17 +664,17 @@ public interface Vector3dc {
      * 
      * @param angle
      *          the angle in radians
-     * @param x
+     * @param aX
      *          the x component of the rotation axis
-     * @param y
+     * @param aY
      *          the y component of the rotation axis
-     * @param z
+     * @param aZ
      *          the z component of the rotation axis
      * @param dest
      *          will hold the result
      * @return dest
      */
-    Vector3d rotateAbout(double angle, double x, double y, double z, Vector3d dest);
+    Vector3d rotateAbout(double angle, double aX, double aY, double aZ, Vector3d dest);
 
     /**
      * Divide this Vector3d by the given scalar value and store the result in <code>dest</code>.

diff --git a/src/org/joml/Vector3f.java b/src/org/joml/Vector3f.java
@@ -1307,19 +1307,16 @@ public Vector3f rotateAbout(float angle, float x, float y, float z) {
     /* (non-Javadoc)
      * @see org.joml.Vector3fc#rotateAbout(float, float, float, float, org.joml.Vector3f)
      */
-    public Vector3f rotateAbout(float angle, float x, float y, float z, Vector3f dest) {
-        float q0x = this.x, q0y = this.y, q0z = this.z;
-        float sin = (float) Math.sin(angle * 0.5);
-        float cos = (float) Math.cosFromSin(sin, angle * 0.5);
-        float q1x = x * sin, q1y = y * sin, q1z = z * sin, q1w = cos;
-        float scale = 1.0f / (q1x * q1x + q1y * q1y + q1z * q1z);
-        float q2x =  q1w * q0x + q1y * q0z - q1z * q0y;
-        float q2y =  q1w * q0y - q1x * q0z + q1z * q0x;
-        float q2z =  q1w * q0z + q1x * q0y - q1y * q0x;
-        float q2w = -q1x * q0x - q1y * q0y - q1z * q0z;
-        dest.x = (-q2w * q1x + q2x * q1w - q2y * q1z + q2z * q1y) * scale;
-        dest.y = (-q2w * q1y + q2x * q1z + q2y * q1w - q2z * q1x) * scale;
-        dest.z = (-q2w * q1z - q2x * q1y + q2y * q1x + q2z * q1w) * scale;
+    public Vector3f rotateAbout(float angle, float aX, float aY, float aZ, Vector3f dest) {
+        float hangle = angle * 0.5f;
+        float sinAngle = (float) Math.sin(hangle);
+        float qx = aX * sinAngle, qy = aY * sinAngle, qz = aZ * sinAngle;
+        float qw = (float) Math.cosFromSin(sinAngle, hangle);
+        float w2 = qw * qw, x2 = qx * qx, y2 = qy * qy, z2 = qz * qz, zw = qz * qw;
+        float xy = qx * qy, xz = qx * qz, yw = qy * qw, yz = qy * qz, xw = qx * qw;
+        dest.x = (w2 + x2 - z2 - y2) * x + (-zw + xy - zw + xy) * y + (yw + xz + xz + yw) * z;
+        dest.y = (xy + zw + zw + xy) * x + ( y2 - z2 + w2 - x2) * y + (yz + yz - xw - xw) * z;
+        dest.z = (xz - yw + xz - yw) * x + ( yz + yz + xw + xw) * y + (z2 - y2 - x2 + w2) * z;
         return this;
     }
 

diff --git a/src/org/joml/Vector3fc.java b/src/org/joml/Vector3fc.java
@@ -487,17 +487,17 @@ public interface Vector3fc {
      * 
      * @param angle
      *          the angle in radians
-     * @param x
+     * @param aX
      *          the x component of the rotation axis
-     * @param y
+     * @param aY
      *          the y component of the rotation axis
-     * @param z
+     * @param aZ
      *          the z component of the rotation axis
      * @param dest
      *          will hold the result
      * @return dest
      */
-    Vector3f rotateAbout(float angle, float x, float y, float z, Vector3f dest);
+    Vector3f rotateAbout(float angle, float aX, float aY, float aZ, Vector3f dest);
 
     /**
      * Return the length squared of this vector.

diff --git a/src/org/joml/Vector4d.java b/src/org/joml/Vector4d.java
@@ -1172,19 +1172,16 @@ public Vector4d rotateAbout(double angle, double x, double y, double z) {
     /* (non-Javadoc)
      * @see org.joml.Vector4dc#rotateAbout(double, double, double, double, org.joml.Vector4d)
      */
-    public Vector4d rotateAbout(double angle, double x, double y, double z, Vector4d dest) {
-        double q0x = this.x, q0y = this.y, q0z = this.z;
-        double sin = Math.sin(angle * 0.5);
-        double cos = Math.cosFromSin(sin, angle * 0.5);
-        double q1x = x * sin, q1y = y * sin, q1z = z * sin, q1w = cos;
-        double scale = 1.0f / (q1x * q1x + q1y * q1y + q1z * q1z);
-        double q2x =  q1w * q0x + q1y * q0z - q1z * q0y;
-        double q2y =  q1w * q0y - q1x * q0z + q1z * q0x;
-        double q2z =  q1w * q0z + q1x * q0y - q1y * q0x;
-        double q2w = -q1x * q0x - q1y * q0y - q1z * q0z;
-        dest.x = (-q2w * q1x + q2x * q1w - q2y * q1z + q2z * q1y) * scale;
-        dest.y = (-q2w * q1y + q2x * q1z + q2y * q1w - q2z * q1x) * scale;
-        dest.z = (-q2w * q1z - q2x * q1y + q2y * q1x + q2z * q1w) * scale;
+    public Vector4d rotateAbout(double angle, double aX, double aY, double aZ, Vector4d dest) {
+        double hangle = angle * 0.5;
+        double sinAngle = Math.sin(hangle);
+        double qx = aX * sinAngle, qy = aY * sinAngle, qz = aZ * sinAngle;
+        double qw = Math.cosFromSin(sinAngle, hangle);
+        double w2 = qw * qw, x2 = qx * qx, y2 = qy * qy, z2 = qz * qz, zw = qz * qw;
+        double xy = qx * qy, xz = qx * qz, yw = qy * qw, yz = qy * qz, xw = qx * qw;
+        dest.x = (w2 + x2 - z2 - y2) * x + (-zw + xy - zw + xy) * y + (yw + xz + xz + yw) * z;
+        dest.y = (xy + zw + zw + xy) * x + ( y2 - z2 + w2 - x2) * y + (yz + yz - xw - xw) * z;
+        dest.z = (xz - yw + xz - yw) * x + ( yz + yz + xw + xw) * y + (z2 - y2 - x2 + w2) * z;
         return this;
     }
 

diff --git a/src/org/joml/Vector4dc.java b/src/org/joml/Vector4dc.java
@@ -303,17 +303,17 @@ public interface Vector4dc {
      * 
      * @param angle
      *          the angle in radians
-     * @param x
+     * @param aX
      *          the x component of the rotation axis
-     * @param y
+     * @param aY
      *          the y component of the rotation axis
-     * @param z
+     * @param aZ
      *          the z component of the rotation axis
      * @param dest
      *          will hold the result
      * @return dest
      */
-    Vector4d rotateAbout(double angle, double x, double y, double z, Vector4d dest);
+    Vector4d rotateAbout(double angle, double aX, double aY, double aZ, Vector4d dest);
 
     /**
      * Return the length squared of this vector.

diff --git a/src/org/joml/Vector4f.java b/src/org/joml/Vector4f.java
@@ -1107,19 +1107,16 @@ public Vector4f rotateAbout(float angle, float x, float y, float z) {
     /* (non-Javadoc)
      * @see org.joml.Vector4fc#rotateAbout(float, float, float, float, org.joml.Vector4f)
      */
-    public Vector4f rotateAbout(float angle, float x, float y, float z, Vector4f dest) {
-        float q0x = this.x, q0y = this.y, q0z = this.z;
-        float sin = (float) Math.sin(angle * 0.5);
-        float cos = (float) Math.cosFromSin(sin, angle * 0.5);
-        float q1x = x * sin, q1y = y * sin, q1z = z * sin, q1w = cos;
-        float scale = 1.0f / (q1x * q1x + q1y * q1y + q1z * q1z);
-        float q2x =  q1w * q0x + q1y * q0z - q1z * q0y;
-        float q2y =  q1w * q0y - q1x * q0z + q1z * q0x;
-        float q2z =  q1w * q0z + q1x * q0y - q1y * q0x;
-        float q2w = -q1x * q0x - q1y * q0y - q1z * q0z;
-        dest.x = (-q2w * q1x + q2x * q1w - q2y * q1z + q2z * q1y) * scale;
-        dest.y = (-q2w * q1y + q2x * q1z + q2y * q1w - q2z * q1x) * scale;
-        dest.z = (-q2w * q1z - q2x * q1y + q2y * q1x + q2z * q1w) * scale;
+    public Vector4f rotateAbout(float angle, float aX, float aY, float aZ, Vector4f dest) {
+        float hangle = angle * 0.5f;
+        float sinAngle = (float) Math.sin(hangle);
+        float qx = aX * sinAngle, qy = aY * sinAngle, qz = aZ * sinAngle;
+        float qw = (float) Math.cosFromSin(sinAngle, hangle);
+        float w2 = qw * qw, x2 = qx * qx, y2 = qy * qy, z2 = qz * qz, zw = qz * qw;
+        float xy = qx * qy, xz = qx * qz, yw = qy * qw, yz = qy * qz, xw = qx * qw;
+        dest.x = (w2 + x2 - z2 - y2) * x + (-zw + xy - zw + xy) * y + (yw + xz + xz + yw) * z;
+        dest.y = (xy + zw + zw + xy) * x + ( y2 - z2 + w2 - x2) * y + (yz + yz - xw - xw) * z;
+        dest.z = (xz - yw + xz - yw) * x + ( yz + yz + xw + xw) * y + (z2 - y2 - x2 + w2) * z;
         return this;
     }
 

diff --git a/src/org/joml/Vector4fc.java b/src/org/joml/Vector4fc.java
@@ -339,17 +339,17 @@ public interface Vector4fc {
      * 
      * @param angle
      *          the angle in radians
-     * @param x
+     * @param aX
      *          the x component of the rotation axis
-     * @param y
+     * @param aY
      *          the y component of the rotation axis
-     * @param z
+     * @param aZ
      *          the z component of the rotation axis
      * @param dest
      *          will hold the result
      * @return dest
      */
-    Vector4f rotateAbout(float angle, float x, float y, float z, Vector4f dest);
+    Vector4f rotateAbout(float angle, float aX, float aY, float aZ, Vector4f dest);
 
     /**
      * Return the length squared of this vector.