Switch to JQuaternion for orientations (#133)

src/Jitter2/Collision/NarrowPhase/MinkowskiDifference.cs

@@ -34,13 +34,13 @@ namespace Jitter2.Collision;
 public struct MinkowskiDifference
 {
     public ISupportMap SupportA, SupportB;
-    public JMatrix OrientationB;
+    public JQuaternion OrientationB;
     public JVector PositionB;
 
     [MethodImpl(MethodImplOptions.AggressiveInlining)]
     private readonly void SupportMapTransformedRelative(in JVector direction, out JVector result)
     {
-        JVector.TransposedTransform(direction, OrientationB, out JVector tmp);
+        JVector.ConjugatedTransform(direction, OrientationB, out JVector tmp);
         SupportB.SupportMap(tmp, out result);
         JVector.Transform(result, OrientationB, out result);
         JVector.Add(result, PositionB, out result);

src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs

@@ -650,7 +650,7 @@ public static bool PointTest(ISupportMap support, in JMatrix orientation,
     /// hit, this parameter will be zero.
     /// </param>
     /// <returns>Returns true if the ray intersects with the shape; otherwise, false.</returns>
-    public static bool RayCast(ISupportMap support, in JMatrix orientation,
+    public static bool RayCast(ISupportMap support, in JQuaternion orientation,
         in JVector position, in JVector origin, in JVector direction, out float fraction, out JVector normal)
     {
         // rotate the ray into the reference frame of bodyA..
@@ -712,7 +712,7 @@ public static bool RayCast(ISupportMap support, in JVector origin, in JVector di
     /// failure, collision information reverts to the type's default values.
     /// </returns>
     public static bool GJKEPA(ISupportMap supportA, ISupportMap supportB,
-        in JMatrix orientationA, in JMatrix orientationB,
+        in JQuaternion orientationA, in JQuaternion orientationB,
         in JVector positionA, in JVector positionB,
         out JVector pointA, out JVector pointB, out JVector normal, out float penetration)
     {
@@ -721,9 +721,9 @@ public static bool GJKEPA(ISupportMap supportA, ISupportMap supportB,
         mkd.SupportB = supportB;
 
         // rotate into the reference frame of bodyA..
-        JMatrix.TransposedMultiply(orientationA, orientationB, out mkd.OrientationB);
+        JQuaternion.ConjugateMultiply(orientationA, orientationB, out mkd.OrientationB);
         JVector.Subtract(positionB, positionA, out mkd.PositionB);
-        JVector.TransposedTransform(mkd.PositionB, orientationA, out mkd.PositionB);
+        JVector.ConjugatedTransform(mkd.PositionB, orientationA, out mkd.PositionB);
 
         // ..perform collision detection..
         bool success = solver.SolveGJKEPA(mkd, out pointA, out pointB, out normal, out penetration);
@@ -761,7 +761,7 @@ public static bool GJKEPA(ISupportMap supportA, ISupportMap supportB,
     /// <param name="penetration">The penetration depth.</param>
     /// <returns>Returns true if the shapes overlap (collide), and false otherwise.</returns>
     public static bool MPREPA(ISupportMap supportA, ISupportMap supportB,
-        in JMatrix orientationA, in JMatrix orientationB,
+        in JQuaternion orientationA, in JQuaternion orientationB,
         in JVector positionA, in JVector positionB,
         out JVector pointA, out JVector pointB, out JVector normal, out float penetration)
     {
@@ -770,9 +770,9 @@ public static bool MPREPA(ISupportMap supportA, ISupportMap supportB,
         mkd.SupportB = supportB;
 
         // rotate into the reference frame of bodyA..
-        JMatrix.TransposedMultiply(orientationA, orientationB, out mkd.OrientationB);
+        JQuaternion.ConjugateMultiply(orientationA, orientationB, out mkd.OrientationB);
         JVector.Subtract(positionB, positionA, out mkd.PositionB);
-        JVector.TransposedTransform(mkd.PositionB, orientationA, out mkd.PositionB);
+        JVector.ConjugatedTransform(mkd.PositionB, orientationA, out mkd.PositionB);
 
         // ..perform collision detection..
         bool res = solver.SolveMPR(mkd, out pointA, out pointB, out normal, out penetration);
@@ -808,7 +808,7 @@ public static bool MPREPA(ISupportMap supportA, ISupportMap supportB,
     /// <param name="penetration">The penetration depth.</param>
     /// <returns>Returns true if the shapes overlap (collide), and false otherwise.</returns>
     public static bool MPREPA(ISupportMap supportA, ISupportMap supportB,
-        in JMatrix orientationB, in JVector positionB,
+        in JQuaternion orientationB, in JVector positionB,
         out JVector pointA, out JVector pointB, out JVector normal, out float penetration)
     {
         Unsafe.SkipInit(out MinkowskiDifference mkd);
@@ -832,7 +832,7 @@ public static bool MPREPA(ISupportMap supportA, ISupportMap supportB,
     /// <param name="fraction">Time of impact. Infinity if no hit is detected.</param>
     /// <returns>True if the shapes hit, false otherwise.</returns>
     public static bool SweepTest(ISupportMap supportA, ISupportMap supportB,
-        in JMatrix orientationA, in JMatrix orientationB,
+        in JQuaternion orientationA, in JQuaternion orientationB,
         in JVector positionA, in JVector positionB,
         in JVector sweepA, in JVector sweepB,
         out JVector pointA, out JVector pointB, out JVector normal, out float fraction)
@@ -843,13 +843,13 @@ public static bool SweepTest(ISupportMap supportA, ISupportMap supportB,
         mkd.SupportB = supportB;
 
         // rotate into the reference frame of bodyA..
-        JMatrix.TransposedMultiply(orientationA, orientationB, out mkd.OrientationB);
+        JQuaternion.ConjugateMultiply(orientationA, orientationB, out mkd.OrientationB);
         JVector.Subtract(positionB, positionA, out mkd.PositionB);
-        JVector.TransposedTransform(mkd.PositionB, orientationA, out mkd.PositionB);
+        JVector.ConjugatedTransform(mkd.PositionB, orientationA, out mkd.PositionB);
 
         // we also transform the relative velocities
         JVector sweep = sweepB - sweepA;
-        JVector.TransposedTransform(sweep, orientationA, out sweep);
+        JVector.ConjugatedTransform(sweep, orientationA, out sweep);
 
         // ..perform toi calculation
         bool res = solver.SweepTest(ref mkd, sweep, out pointA, out pointB, out normal, out fraction);
@@ -880,7 +880,7 @@ public static bool SweepTest(ISupportMap supportA, ISupportMap supportB,
     /// <param name="fraction">Time of impact. Infinity if no hit is detected.</param>
     /// <returns>True if the shapes hit, false otherwise.</returns>
     public static bool SweepTest(ISupportMap supportA, ISupportMap supportB,
-        in JMatrix orientationB, in JVector positionB, in JVector sweepB,
+        in JQuaternion orientationB, in JVector positionB, in JVector sweepB,
         out JVector pointA, out JVector pointB, out JVector normal, out float fraction)
     {
         Unsafe.SkipInit(out MinkowskiDifference mkd);

src/Jitter2/Collision/Shapes/BoxShape.cs

@@ -85,16 +85,13 @@ public override void SupportMap(in JVector direction, out JVector result)
         result.Z = Math.Sign(direction.Z) * halfSize.Z;
     }
 
-    public override void CalculateBoundingBox(in JMatrix orientation, in JVector position, out JBBox box)
+    public override void CalculateBoundingBox(in JQuaternion orientation, in JVector position, out JBBox box)
     {
-        JMatrix.Absolute(in orientation, out JMatrix abs);
-        JVector.Transform(halfSize, abs, out JVector temp);
+        JMatrix.Absolute(JMatrix.CreateFromQuaternion(orientation), out JMatrix absm);
+        var ths = JVector.Transform(halfSize, absm);
 
-        box.Max = temp;
-        JVector.Negate(temp, out box.Min);
-
-        JVector.Add(box.Min, position, out box.Min);
-        JVector.Add(box.Max, position, out box.Max);
+        box.Min = position - ths;
+        box.Max = position + ths;
     }
 
     public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out float mass)

src/Jitter2/Collision/Shapes/CapsuleShape.cs

@@ -89,9 +89,9 @@ public override void SupportMap(in JVector direction, out JVector result)
         result.Y += MathF.Sign(direction.Y) * halfLength;
     }
 
-    public override void CalculateBoundingBox(in JMatrix orientation, in JVector position, out JBBox box)
+    public override void CalculateBoundingBox(in JQuaternion orientation, in JVector position, out JBBox box)
     {
-        JVector delta = halfLength * orientation.GetColumn(1);
+        JVector delta = halfLength * orientation.GetBasisY();
 
         box.Min.X = -radius - MathF.Abs(delta.X);
         box.Min.Y = -radius - MathF.Abs(delta.Y);

src/Jitter2/Collision/Shapes/ConeShape.cs

@@ -100,11 +100,11 @@ public override void SupportMap(in JVector direction, out JVector result)
         }
     }
 
-    public override void CalculateBoundingBox(in JMatrix orientation, in JVector position, out JBBox box)
+    public override void CalculateBoundingBox(in JQuaternion orientation, in JVector position, out JBBox box)
     {
         const float ZeroEpsilon = 1e-12f;
 
-        JVector upa = orientation.GetColumn(1);
+        JVector upa = orientation.GetBasisY();
 
         float xx = upa.X * upa.X;
         float yy = upa.Y * upa.Y;

src/Jitter2/Collision/Shapes/ConvexHullShape.cs

@@ -254,12 +254,13 @@ public override void CalculateMassInertia(out JMatrix inertia, out JVector com,
         com *= 1.0f / mass;
     }
 
-    public override void CalculateBoundingBox(in JMatrix orientation, in JVector position, out JBBox box)
+    public override void CalculateBoundingBox(in JQuaternion orientation, in JVector position, out JBBox box)
     {
         JVector halfSize = 0.5f * (initBox.Max - initBox.Min);
         JVector center = 0.5f * (initBox.Max + initBox.Min);
 
-        JMatrix.Absolute(in orientation, out JMatrix abs);
+        JMatrix ori = JMatrix.CreateFromQuaternion(orientation);
+        JMatrix.Absolute(in ori, out JMatrix abs);
         JVector.Transform(halfSize, abs, out JVector temp);
         JVector.Transform(center, orientation, out JVector temp2);
 

src/Jitter2/Collision/Shapes/CylinderShape.cs

@@ -90,11 +90,11 @@ public override void SupportMap(in JVector direction, out JVector result)
         }
     }
 
-    public override void CalculateBoundingBox(in JMatrix orientation, in JVector position, out JBBox box)
+    public override void CalculateBoundingBox(in JQuaternion orientation, in JVector position, out JBBox box)
     {
         const float ZeroEpsilon = 1e-12f;
 
-        JVector upa = orientation.GetColumn(1);
+        JVector upa = orientation.GetBasisY();
 
         float xx = upa.X * upa.X;
         float yy = upa.Y * upa.Y;

src/Jitter2/Collision/Shapes/Shape.cs

@@ -139,7 +139,7 @@ public virtual void UpdateWorldBoundingBox()
     {
         if (RigidBody == null)
         {
-            CalculateBoundingBox(JMatrix.Identity, JVector.Zero, out JBBox box);
+            CalculateBoundingBox(JQuaternion.Identity, JVector.Zero, out JBBox box);
             WorldBoundingBox = box;
         }
         else
@@ -203,9 +203,10 @@ public void SweptExpandBoundingBox(in JVector sweptDirection)
     /// <see cref="SupportMap"/> function. Child classes should override this implementation to improve
     /// performance.
     /// </summary>
-    public virtual void CalculateBoundingBox(in JMatrix orientation, in JVector position, out JBBox box)
+    public virtual void CalculateBoundingBox(in JQuaternion orientation, in JVector position, out JBBox box)
     {
-        JMatrix oriT = JMatrix.Transpose(orientation);
+        // TODO: Can this be done smarter?
+        JMatrix oriT = JMatrix.Transpose(JMatrix.CreateFromQuaternion(orientation));
 
         SupportMap(oriT.GetColumn(0), out JVector res);
         box.Max.X = JVector.Dot(oriT.GetColumn(0), res);

src/Jitter2/Collision/Shapes/SphereShape.cs

@@ -64,7 +64,7 @@ public override void SupportMap(in JVector direction, out JVector result)
         JVector.Multiply(result, radius, out result);
     }
 
-    public override void CalculateBoundingBox(in JMatrix orientation, in JVector position, out JBBox box)
+    public override void CalculateBoundingBox(in JQuaternion orientation, in JVector position, out JBBox box)
     {
         box.Min.X = -radius;
         box.Min.Y = -radius;

src/Jitter2/Collision/Shapes/TransformedShape.cs

@@ -116,7 +116,7 @@ public override void SupportMap(in JVector direction, out JVector result)
         }
     }
 
-    public override void CalculateBoundingBox(in JMatrix orientation, in JVector position, out JBBox box)
+    public override void CalculateBoundingBox(in JQuaternion orientation, in JVector position, out JBBox box)
     {
         if (type == TransformationType.General)
         {
@@ -125,7 +125,8 @@ public override void CalculateBoundingBox(in JMatrix orientation, in JVector pos
         }
         else
         {
-            OriginalShape.CalculateBoundingBox(orientation * this.transformation,
+            JQuaternion quat = JQuaternion.CreateFromMatrix(this.transformation);
+            OriginalShape.CalculateBoundingBox(orientation * quat,
                 JVector.Transform(translation, orientation) + position, out box);
         }
     }

src/Jitter2/Collision/Shapes/TriangleShape.cs

@@ -78,7 +78,7 @@ public void GetWorldVertices(out JVector a, out JVector b, out JVector c)
 
         if (RigidBody == null) return;
 
-        ref JMatrix orientation = ref RigidBody.Data.Orientation;
+        ref JQuaternion orientation = ref RigidBody.Data.Orientation;
         ref JVector position = ref RigidBody.Data.Position;
 
         JVector.Transform(a, orientation, out a);
@@ -90,7 +90,7 @@ public void GetWorldVertices(out JVector a, out JVector b, out JVector c)
         c += position;
     }
 
-    public override void CalculateBoundingBox(in JMatrix orientation, in JVector position, out JBBox box)
+    public override void CalculateBoundingBox(in JQuaternion orientation, in JVector position, out JBBox box)
     {
         const float extraMargin = 0.01f;
 

src/Jitter2/Dynamics/Constraints/AngularMotor.cs

@@ -81,8 +81,8 @@ public void Initialize(JVector axis1, JVector axis2)
         axis1.Normalize();
         axis2.Normalize();
 
-        JVector.TransposedTransform(axis1, body1.Orientation, out data.LocalAxis1);
-        JVector.TransposedTransform(axis2, body2.Orientation, out data.LocalAxis2);
+        JVector.ConjugatedTransform(axis1, body1.Orientation, out data.LocalAxis1);
+        JVector.ConjugatedTransform(axis2, body2.Orientation, out data.LocalAxis2);
 
         data.MaxForce = 0;
         data.Velocity = 0;

src/Jitter2/Dynamics/Constraints/BallSocket.cs

@@ -86,8 +86,8 @@ public void Initialize(JVector anchor)
         JVector.Subtract(anchor, body1.Position, out data.LocalAnchor1);
         JVector.Subtract(anchor, body2.Position, out data.LocalAnchor2);
 
-        JVector.TransposedTransform(data.LocalAnchor1, body1.Orientation, out data.LocalAnchor1);
-        JVector.TransposedTransform(data.LocalAnchor2, body2.Orientation, out data.LocalAnchor2);
+        JVector.ConjugatedTransform(data.LocalAnchor1, body1.Orientation, out data.LocalAnchor1);
+        JVector.ConjugatedTransform(data.LocalAnchor2, body2.Orientation, out data.LocalAnchor2);
 
         data.BiasFactor = 0.2f;
         data.Softness = 0.00f;

src/Jitter2/Dynamics/Constraints/ConeLimit.cs

@@ -85,8 +85,8 @@ public void Initialize(JVector axis, AngularLimit limit)
 
         axis.Normalize();
 
-        JVector.TransposedTransform(axis, body1.Orientation, out data.LocalAxis1);
-        JVector.TransposedTransform(axis, body2.Orientation, out data.LocalAxis2);
+        JVector.ConjugatedTransform(axis, body1.Orientation, out data.LocalAxis1);
+        JVector.ConjugatedTransform(axis, body2.Orientation, out data.LocalAxis2);
 
         data.Softness = 0.001f;
         data.BiasFactor = 0.2f;

src/Jitter2/Dynamics/Constraints/DistanceLimit.cs

@@ -96,8 +96,8 @@ public void Initialize(JVector anchor1, JVector anchor2, LinearLimit limit)
         JVector.Subtract(anchor1, body1.Position, out data.LocalAnchor1);
         JVector.Subtract(anchor2, body2.Position, out data.LocalAnchor2);
 
-        JVector.TransposedTransform(data.LocalAnchor1, body1.Orientation, out data.LocalAnchor1);
-        JVector.TransposedTransform(data.LocalAnchor2, body2.Orientation, out data.LocalAnchor2);
+        JVector.ConjugatedTransform(data.LocalAnchor1, body1.Orientation, out data.LocalAnchor1);
+        JVector.ConjugatedTransform(data.LocalAnchor2, body2.Orientation, out data.LocalAnchor2);
 
         data.Softness = 0.001f;
         data.BiasFactor = 0.2f;
@@ -113,7 +113,7 @@ public JVector Anchor1
             ref DistanceLimitData data = ref handle.Data;
             ref RigidBodyData body1 = ref data.Body1.Data;
             JVector.Subtract(value, body1.Position, out data.LocalAnchor1);
-            JVector.TransposedTransform(data.LocalAnchor1, body1.Orientation, out data.LocalAnchor1);
+            JVector.ConjugatedTransform(data.LocalAnchor1, body1.Orientation, out data.LocalAnchor1);
         }
         get
         {
@@ -132,7 +132,7 @@ public JVector Anchor2
             ref DistanceLimitData data = ref handle.Data;
             ref RigidBodyData body2 = ref data.Body2.Data;
             JVector.Subtract(value, body2.Position, out data.LocalAnchor2);
-            JVector.TransposedTransform(data.LocalAnchor2, body2.Orientation, out data.LocalAnchor2);
+            JVector.ConjugatedTransform(data.LocalAnchor2, body2.Orientation, out data.LocalAnchor2);
         }
         get
         {

src/Jitter2/Dynamics/Constraints/FixedAngle.cs

@@ -81,8 +81,8 @@ public void Initialize()
         data.Softness = 0.001f;
         data.BiasFactor = 0.2f;
 
-        JQuaternion q1 = JQuaternion.CreateFromMatrix(body1.Orientation);
-        JQuaternion q2 = JQuaternion.CreateFromMatrix(body2.Orientation);
+        JQuaternion q1 = body1.Orientation;
+        JQuaternion q2 = body2.Orientation;
 
         data.Q0 = q2.Conj() * q1;
     }
@@ -94,8 +94,8 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt)
         ref RigidBodyData body1 = ref data.Body1.Data;
         ref RigidBodyData body2 = ref data.Body2.Data;
 
-        JQuaternion q1 = JQuaternion.CreateFromMatrix(body1.Orientation);
-        JQuaternion q2 = JQuaternion.CreateFromMatrix(body2.Orientation);
+        JQuaternion q1 = body1.Orientation;
+        JQuaternion q2 = body2.Orientation;
 
         JQuaternion quat0 = data.Q0 * q1.Conj() * q2;
 

src/Jitter2/Dynamics/Constraints/HingeAngle.cs

@@ -96,8 +96,8 @@ public void Initialize(JVector axis, AngularLimit angle)
 
         data.Axis = JVector.TransposedTransform(axis, body2.Orientation);
 
-        JQuaternion q1 = JQuaternion.CreateFromMatrix(body1.Orientation);
-        JQuaternion q2 = JQuaternion.CreateFromMatrix(body2.Orientation);
+        JQuaternion q1 = body1.Orientation;
+        JQuaternion q2 = body2.Orientation;
 
         data.Q0 = q2.Conj() * q1;
     }
@@ -119,8 +119,8 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt)
         ref RigidBodyData body1 = ref data.Body1.Data;
         ref RigidBodyData body2 = ref data.Body2.Data;
 
-        JQuaternion q1 = JQuaternion.CreateFromMatrix(body1.Orientation);
-        JQuaternion q2 = JQuaternion.CreateFromMatrix(body2.Orientation);
+        JQuaternion q1 = body1.Orientation;
+        JQuaternion q2 = body2.Orientation;
 
         JVector p0 = MathHelper.CreateOrthonormal(data.Axis);
         JVector p1 = data.Axis % p0;
@@ -193,8 +193,8 @@ public JAngle Angle
         get
         {
             ref HingeAngleData data = ref handle.Data;
-            JQuaternion q1 = JQuaternion.CreateFromMatrix(data.Body1.Data.Orientation);
-            JQuaternion q2 = JQuaternion.CreateFromMatrix(data.Body2.Data.Orientation);
+            JQuaternion q1 = data.Body1.Data.Orientation;
+            JQuaternion q2 = data.Body2.Data.Orientation;
 
             JQuaternion quat0 = data.Q0 * q1.Conj() * q2;
 

src/Jitter2/Dynamics/Constraints/LinearMotor.cs

@@ -94,8 +94,8 @@ public void Initialize(JVector axis1, JVector axis2)
         axis1.Normalize();
         axis2.Normalize();
 
-        JVector.TransposedTransform(axis1, body1.Orientation, out data.LocalAxis1);
-        JVector.TransposedTransform(axis2, body2.Orientation, out data.LocalAxis2);
+        JVector.ConjugatedTransform(axis1, body1.Orientation, out data.LocalAxis1);
+        JVector.ConjugatedTransform(axis2, body2.Orientation, out data.LocalAxis2);
 
         data.MaxForce = 0;
         data.Velocity = 0;