From bbe606da5398a2dec4891bca15f9d2bbdc43a677 Mon Sep 17 00:00:00 2001
From: the_fiddler <the_fiddler@ebc5dd9b-fb1d-0410-b6f8-d24c324e9604>
Date: Mon, 16 Nov 2009 13:23:04 +0000
Subject: [PATCH] Vector3(d).Cross: do not modify 'result' more than once to
 ensure that Cross(ref a, ref b, out a) works correctly. Optimized
 vector-quaternion transform. Fixes issue [#1373]: "[Math] optimize Vector
 transform by Quaternion".

---
 Source/OpenTK/Math/Vector3.cs  | 27 +++++++++++++++------------
 Source/OpenTK/Math/Vector3d.cs | 27 +++++++++++++++------------
 2 files changed, 30 insertions(+), 24 deletions(-)

diff --git a/Source/OpenTK/Math/Vector3.cs b/Source/OpenTK/Math/Vector3.cs
index f297c110..9baa652e 100644
--- a/Source/OpenTK/Math/Vector3.cs
+++ b/Source/OpenTK/Math/Vector3.cs
@@ -837,9 +837,9 @@ namespace OpenTK
         /// <returns>The cross product of the two inputs</returns>
         public static Vector3 Cross(Vector3 left, Vector3 right)
         {
-            return new Vector3(left.Y * right.Z - left.Z * right.Y,
-                               left.Z * right.X - left.X * right.Z,
-                               left.X * right.Y - left.Y * right.X);
+            Vector3 result;
+            Cross(ref left, ref right, out result);
+            return result;
         }
 
         /// <summary>
@@ -851,9 +851,9 @@ namespace OpenTK
         /// <param name="result">The cross product of the two inputs</param>
         public static void Cross(ref Vector3 left, ref Vector3 right, out Vector3 result)
         {
-            result.X = left.Y * right.Z - left.Z * right.Y;
-            result.Y = left.Z * right.X - left.X * right.Z;
-            result.Z = left.X * right.Y - left.Y * right.X;
+            result = new Vector3(left.Y * right.Z - left.Z * right.Y,
+                left.Z * right.X - left.X * right.Z,
+                left.X * right.Y - left.Y * right.X);
         }
 
         #endregion
@@ -1115,12 +1115,15 @@ namespace OpenTK
         /// <param name="result">The result of the operation.</param>
         public static void Transform(ref Vector3 vec, ref Quaternion quat, out Vector3 result)
         {
-            Quaternion v = new Quaternion(vec.X, vec.Y, vec.Z, 0), i, t;
-            Quaternion.Invert(ref quat, out i);
-            Quaternion.Multiply(ref quat, ref v, out t);
-            Quaternion.Multiply(ref t, ref i, out v);
-
-            result = new Vector3(v.X, v.Y, v.Z);
+            // Since vec.W == 0, we can optimize quat * vec * quat^-1 as follows:
+            // vec + 2.0 * cross(quat.xyz, cross(quat.xyz, vec) + quat.w * vec)
+            Vector3 xyz = quat.Xyz, temp, temp2;
+            Vector3.Cross(ref xyz, ref vec, out temp);
+            Vector3.Multiply(ref vec, quat.W, out temp2);
+            Vector3.Add(ref temp, ref temp2, out temp);
+            Vector3.Cross(ref xyz, ref temp, out temp);
+            Vector3.Multiply(ref temp, 2, out temp);
+            Vector3.Add(ref vec, ref temp, out result);
         }
 
         /// <summary>Transform a Vector3 by the given Matrix, and project the resulting Vector4 back to a Vector3</summary>
diff --git a/Source/OpenTK/Math/Vector3d.cs b/Source/OpenTK/Math/Vector3d.cs
index 2c1fdc99..d7e4d685 100644
--- a/Source/OpenTK/Math/Vector3d.cs
+++ b/Source/OpenTK/Math/Vector3d.cs
@@ -836,9 +836,9 @@ namespace OpenTK
         /// <returns>The cross product of the two inputs</returns>
         public static Vector3d Cross(Vector3d left, Vector3d right)
         {
-            return new Vector3d(left.Y * right.Z - left.Z * right.Y,
-                                left.Z * right.X - left.X * right.Z,
-                                left.X * right.Y - left.Y * right.X);
+            Vector3d result;
+            Cross(ref left, ref right, out result);
+            return result;
         }
 
         /// <summary>
@@ -850,9 +850,9 @@ namespace OpenTK
         /// <param name="result">The cross product of the two inputs</param>
         public static void Cross(ref Vector3d left, ref Vector3d right, out Vector3d result)
         {
-            result.X = left.Y * right.Z - left.Z * right.Y;
-            result.Y = left.Z * right.X - left.X * right.Z;
-            result.Z = left.X * right.Y - left.Y * right.X;
+            result = new Vector3d(left.Y * right.Z - left.Z * right.Y,
+                left.Z * right.X - left.X * right.Z,
+                left.X * right.Y - left.Y * right.X);
         }
 
         #endregion
@@ -1111,12 +1111,15 @@ namespace OpenTK
         /// <param name="result">The result of the operation.</param>
         public static void Transform(ref Vector3d vec, ref Quaterniond quat, out Vector3d result)
         {
-            Quaterniond v = new Quaterniond(vec.X, vec.Y, vec.Z, 0), i, t;
-            Quaterniond.Invert(ref quat, out i);
-            Quaterniond.Multiply(ref quat, ref v, out t);
-            Quaterniond.Multiply(ref t, ref i, out v);
-
-            result = new Vector3d(v.X, v.Y, v.Z);
+            // Since vec.W == 0, we can optimize quat * vec * quat^-1 as follows:
+            // vec + 2.0 * cross(quat.xyz, cross(quat.xyz, vec) + quat.w * vec)
+            Vector3d xyz = quat.Xyz, temp, temp2;
+            Vector3d.Cross(ref xyz, ref vec, out temp);
+            Vector3d.Multiply(ref vec, quat.W, out temp2);
+            Vector3d.Add(ref temp, ref temp2, out temp);
+            Vector3d.Cross(ref xyz, ref temp, out temp);
+            Vector3d.Multiply(ref temp, 2, out temp);
+            Vector3d.Add(ref vec, ref temp, out result);
         }
 
         /// <summary>