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 /// The cross product of the two inputs 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; } /// @@ -851,9 +851,9 @@ namespace OpenTK /// The cross product of the two inputs 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 /// The result of the operation. 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); } /// Transform a Vector3 by the given Matrix, and project the resulting Vector4 back to a Vector3 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 /// The cross product of the two inputs 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; } /// @@ -850,9 +850,9 @@ namespace OpenTK /// The cross product of the two inputs 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 /// The result of the operation. 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); } ///