Better quaternion vector transformation

notgiven688 · Jun 6, 2024 · 62f0176 · 62f0176
1 parent 62e3c32
commit 62f0176
Showing 1 changed file with 20 additions and 30 deletions.
diff --git a/src/Jitter2/LinearMath/JVector.cs b/src/Jitter2/LinearMath/JVector.cs
@@ -254,48 +254,38 @@ public static void TransposedTransform(in JVector vector, in JMatrix matrix, out
     /// Transforms the vector by a quaternion.
     /// </summary>
     [MethodImpl(MethodImplOptions.AggressiveInlining)]
-    public static void Transform(in JVector vector, in JQuaternion quaternion, out JVector result)
+    public unsafe static void Transform(in JVector vector, in JQuaternion rotation, out JVector result)
     {
-        float x = vector.X;
-        float y = vector.Y;
-        float z = vector.Z;
-        float qx = quaternion.X;
-        float qy = quaternion.Y;
-        float qz = quaternion.Z;
-        float qw = quaternion.W;
+        float numx = 2.0f * (rotation.Y * vector.Z - rotation.Z * vector.Y);
+        float numy = 2.0f * (rotation.Z * vector.X - rotation.X * vector.Z);
+        float numz = 2.0f * (rotation.X * vector.Y - rotation.Y * vector.X);
 
-        float num0 = qw * x + qy * z - qz * y;
-        float num1 = qw * y + qz * x - qx * z;
-        float num2 = qw * z + qx * y - qy * x;
-        float num3 = -qx * x - qy * y - qz * z;
+        float num00 = rotation.Y * numz - rotation.Z * numy;
+        float num11 = rotation.Z * numx - rotation.X * numz;
+        float num22 = rotation.X * numy - rotation.Y * numx;
 
-        result.X = num0 * qw - num3 * qx - num1 * qz + num2 * qy;
-        result.Y = num1 * qw - num3 * qy - num2 * qx + num0 * qz;
-        result.Z = num2 * qw - num3 * qz - num0 * qy + num1 * qx;
+        result.X = vector.X + rotation.W * numx + num00;
+        result.Y = vector.Y + rotation.W * numy + num11;
+        result.Z = vector.Z + rotation.W * numz + num22;
     }
 
     /// <summary>
     /// Transforms the vector by a conjugated quaternion.
     /// </summary>
     [MethodImpl(MethodImplOptions.AggressiveInlining)]
-    public static void ConjugatedTransform(in JVector vector, in JQuaternion quaternion, out JVector result)
+    public static void ConjugatedTransform(in JVector vector, in JQuaternion rotation, out JVector result)
     {
-        float x = vector.X;
-        float y = vector.Y;
-        float z = vector.Z;
-        float qx = quaternion.X;
-        float qy = quaternion.Y;
-        float qz = quaternion.Z;
-        float qw = quaternion.W;
+        float numx = 2.0f * (rotation.Z * vector.Y - rotation.Y * vector.Z);
+        float numy = 2.0f * (rotation.X * vector.Z - rotation.Z * vector.X);
+        float numz = 2.0f * (rotation.Y * vector.X - rotation.X * vector.Y);
 
-        float num0 = qw * x - qy * z + qz * y;
-        float num1 = qw * y - qz * x + qx * z;
-        float num2 = qw * z - qx * y + qy * x;
-        float num3 = qx * x + qy * y + qz * z;
+        float num00 = rotation.Z * numy - rotation.Y * numz;
+        float num11 = rotation.X * numz - rotation.Z * numx;
+        float num22 = rotation.Y * numx - rotation.X * numy;
 
-        result.X = num0 * qw + num3 * qx + num1 * qz - num2 * qy;
-        result.Y = num1 * qw + num3 * qy + num2 * qx - num0 * qz;
-        result.Z = num2 * qw + num3 * qz + num0 * qy - num1 * qx;
+        result.X = vector.X + rotation.W * numx + num00;
+        result.Y = vector.Y + rotation.W * numy + num11;
+        result.Z = vector.Z + rotation.W * numz + num22;
     }
 
     /// <summary>