Collection of smaller changes

docs/docs/02_documentation/04constraints.md → docs/docs/02_documentation/04-constraints.md

@@ -68,7 +68,7 @@ For most constraints a softness and bias value can be set.
 These values define how strict the constraint limits are enforced.
 Softer constraints might improve simulation stability but do not fully enforce the constraint limits.
 The softness and bias parameters can be tweaked for optimal results.
-Better constraint behaviour can also be archived by sub-stepping, see [world.Step](/docs/documentation/01world).
+Better constraint behaviour can also be archived by sub-stepping, see [world.Step](/docs/documentation/world).
 
 ### Enable/Disable constraints
 

src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs

@@ -327,6 +327,7 @@ public void InitHeap()
     /// the return value of this method.
     /// </summary>
     /// <returns>Indicates whether the polyhedron successfully incorporated the new vertex.</returns>
+    [System.Runtime.CompilerServices.SkipLocalsInit]
     public bool AddVertex(in Vertex vertex)
     {
         Debug.Assert(vPointer < MaxVertices, "Maximum number of vertices exceeded.");

src/Jitter2/Collision/NarrowPhase/SimplexSolver.cs

@@ -159,13 +159,13 @@ float Determinant(in JVector a, in JVector b, in JVector c, in JVector d)
         }
 
         float detT = Determinant(v0, v1, v2, v3);
-        float idetT = 1.0f / detT;
+        float inverseDetT = 1.0f / detT;
 
         bool degenerate = detT * detT < Epsilon;
 
-        float lambda0 = Determinant(JVector.Zero, v1, v2, v3) * idetT;
-        float lambda1 = Determinant(v0, JVector.Zero, v2, v3) * idetT;
-        float lambda2 = Determinant(v0, v1, JVector.Zero, v3) * idetT;
+        float lambda0 = Determinant(JVector.Zero, v1, v2, v3) * inverseDetT;
+        float lambda1 = Determinant(v0, JVector.Zero, v2, v3) * inverseDetT;
+        float lambda2 = Determinant(v0, v1, JVector.Zero, v3) * inverseDetT;
         float lambda3 = 1.0f - lambda0 - lambda1 - lambda2;
 
         float bestDistance = float.MaxValue;

src/Jitter2/Collision/NarrowPhase/SimplexSolverAB.cs

@@ -187,13 +187,13 @@ float Determinant(in JVector a, in JVector b, in JVector c, in JVector d)
         }
 
         float detT = Determinant(v0.V, v1.V, v2.V, v3.V);
-        float idetT = 1.0f / detT;
+        float inverseDetT = 1.0f / detT;
 
         bool degenerate = detT * detT < Epsilon;
 
-        float lambda0 = Determinant(JVector.Zero, v1.V, v2.V, v3.V) * idetT;
-        float lambda1 = Determinant(v0.V, JVector.Zero, v2.V, v3.V) * idetT;
-        float lambda2 = Determinant(v0.V, v1.V, JVector.Zero, v3.V) * idetT;
+        float lambda0 = Determinant(JVector.Zero, v1.V, v2.V, v3.V) * inverseDetT;
+        float lambda1 = Determinant(v0.V, JVector.Zero, v2.V, v3.V) * inverseDetT;
+        float lambda2 = Determinant(v0.V, v1.V, JVector.Zero, v3.V) * inverseDetT;
         float lambda3 = 1.0f - lambda0 - lambda1 - lambda2;
 
         float bestDistance = float.MaxValue;

src/Jitter2/Collision/Shapes/TransformedShape.cs

@@ -43,7 +43,7 @@ private enum TransformationType
 
     /// <summary>
     /// Constructs a transformed shape through an affine transformation define by
-    /// a linear map and a translation. 
+    /// a linear map and a translation.
     /// </summary>
     /// <param name="shape">The original shape which should be transformed.</param>
     /// <param name="translation">Shape is translated by this vector.</param>
@@ -79,7 +79,8 @@ private void AnalyzeTransformation()
     {
         if (MathHelper.IsRotationMatrix(transformation))
         {
-            type = MathHelper.UnsafeIsZero(transformation - JMatrix.Identity) ? TransformationType.Identity : TransformationType.Rotation;
+            JMatrix delta = transformation - JMatrix.Identity;
+            type = MathHelper.UnsafeIsZero(ref delta) ? TransformationType.Identity : TransformationType.Rotation;
         }
         else
         {

src/Jitter2/Dynamics/Constraints/Limit.cs

@@ -48,10 +48,10 @@ public AngularLimit(JAngle from, JAngle to)
         To = to;
     }
 
-    internal readonly void Deconstruct(out JAngle LimitMin, out JAngle LimitMax)
+    public readonly void Deconstruct(out JAngle limitMin, out JAngle limitMax)
     {
-        LimitMin = From;
-        LimitMax = To;
+        limitMin = From;
+        limitMax = To;
     }
 }
 
@@ -77,9 +77,9 @@ public static LinearLimit FromMinMax(float min, float max)
         return new LinearLimit(min, max);
     }
 
-    internal readonly void Deconstruct(out float LimitMin, out float LimitMax)
+    public readonly void Deconstruct(out float limitMin, out float limitMax)
     {
-        LimitMin = From;
-        LimitMax = To;
+        limitMin = From;
+        limitMax = To;
     }
 }

src/Jitter2/Dynamics/Constraints/PointOnLine.cs

@@ -139,6 +139,7 @@ public float Distance
         }
     }
 
+    [System.Runtime.CompilerServices.SkipLocalsInit]
     public static void PrepareForIteration(ref ConstraintData constraint, float idt)
     {
         ref PointOnLineData data = ref Unsafe.AsRef<PointOnLineData>(Unsafe.AsPointer(ref constraint));
@@ -277,6 +278,7 @@ public float LimitBias
         set => handle.Data.LimitBias = value;
     }
 
+    [System.Runtime.CompilerServices.SkipLocalsInit]
     public static void Iterate(ref ConstraintData constraint, float idt)
     {
         ref PointOnLineData data = ref Unsafe.AsRef<PointOnLineData>(Unsafe.AsPointer(ref constraint));

src/Jitter2/LinearMath/JMatrix.cs

@@ -194,10 +194,9 @@ public static JMatrix CreateRotationX(float radians)
         float c = (float)Math.Cos(radians);
         float s = (float)Math.Sin(radians);
 
-        // [  1  0  0  0 ]
-        // [  0  c -s  0 ]
-        // [  0  s  c  0 ]
-        // [  0  0  0  1 ]
+        // [  1  0  0  ]
+        // [  0  c -s  ]
+        // [  0  s  c  ]
         result.M22 = c;
         result.M23 = -s;
         result.M32 = s;
@@ -213,10 +212,9 @@ public static JMatrix CreateRotationY(float radians)
         float c = (float)Math.Cos(radians);
         float s = (float)Math.Sin(radians);
 
-        // [  c  0  s  0 ]
-        // [  0  1  0  0 ]
-        // [ -s  0  c  0 ]
-        // [  0  0  0  1 ]
+        // [  c  0  s  ]
+        // [  0  1  0  ]
+        // [ -s  0  c  ]
         result.M11 = c;
         result.M13 = s;
         result.M31 = -s;
@@ -232,10 +230,9 @@ public static JMatrix CreateRotationZ(float radians)
         float c = (float)Math.Cos(radians);
         float s = (float)Math.Sin(radians);
 
-        // [  c -s  0  0 ]
-        // [  s  c  0  0 ]
-        // [  0  0  1  0 ]
-        // [  0  0  0  1 ]
+        // [  c -s  0  ]
+        // [  s  c  0  ]
+        // [  0  0  1  ]
         result.M11 = c;
         result.M12 = -s;
         result.M21 = s;

src/Jitter2/LinearMath/MathHelper.cs

@@ -68,29 +68,24 @@ public static bool LineLineIntersect(in JVector p1, in JVector p2, in JVector p3
 
     */
 
+    /// <summary>
+    /// Checks if matrix is a pure rotation matrix.
+    /// </summary>
     public static bool IsRotationMatrix(in JMatrix matrix, float epsilon = 1e-06f)
     {
-        if (!UnsafeIsZero(JMatrix.MultiplyTransposed(matrix, matrix) - JMatrix.Identity, epsilon))
+        JMatrix delta = JMatrix.MultiplyTransposed(matrix, matrix) - JMatrix.Identity;
+
+        if (!UnsafeIsZero(ref delta, epsilon))
         {
             return false;
         }
 
         return MathF.Abs(matrix.Determinant() - 1.0f) < epsilon;
     }
 
-    public static void UnsafeDecomposeMatrix(in JMatrix matrix, out JMatrix orientation, out JVector scale)
-    {
-        orientation = matrix;
-
-        scale.X = orientation.UnsafeGet(0).Length();
-        scale.Y = orientation.UnsafeGet(1).Length();
-        scale.Z = orientation.UnsafeGet(2).Length();
-
-        orientation.UnsafeGet(0) *= 1.0f / scale.X;
-        orientation.UnsafeGet(1) *= 1.0f / scale.Y;
-        orientation.UnsafeGet(2) *= 1.0f / scale.Z;
-    }
-
+    /// <summary>
+    /// Checks if all entries of a vector are close to zero.
+    /// </summary>
     public static bool IsZero(in JVector vector, float epsilon = 1e-6f)
     {
         float x = MathF.Abs(vector.X);
@@ -100,14 +95,21 @@ public static bool IsZero(in JVector vector, float epsilon = 1e-6f)
         return MathF.Max(x, MathF.Max(y, z)) < epsilon;
     }
 
-    public static bool UnsafeIsZero(in JMatrix matrix, float epsilon = 1e-6f)
+    /// <summary>
+    /// Checks if all entries of a matrix are close to zero.
+    /// </summary>
+    public static bool UnsafeIsZero(ref JMatrix matrix, float epsilon = 1e-6f)
     {
-        if (!IsZero(matrix.UnsafeGet(0))) return false;
-        if (!IsZero(matrix.UnsafeGet(1))) return false;
-        if (!IsZero(matrix.UnsafeGet(2))) return false;
+        if (!IsZero(matrix.UnsafeGet(0), epsilon)) return false;
+        if (!IsZero(matrix.UnsafeGet(1), epsilon)) return false;
+        if (!IsZero(matrix.UnsafeGet(2), epsilon)) return false;
         return true;
     }
 
+    /// <summary>
+    /// Calculates (M^T \times M)^(-1/2) using Jacobi iterations.
+    /// </summary>
+    /// <param name="sweeps">The number of Jacobi iterations.</param>
     public static JMatrix InverseSquareRoot(JMatrix m, int sweeps = 2)
     {
         float phi, cp, sp;
@@ -194,20 +196,16 @@ public static JVector CreateOrthonormal(in JVector vec)
         return result;
     }
 
-
     /// <summary>
     /// Verifies whether the columns of the given matrix constitute an orthonormal basis.
     /// An orthonormal basis means that the columns are mutually perpendicular and have unit length.
     /// </summary>
     /// <param name="matrix">The input matrix to check for an orthonormal basis.</param>
     /// <returns>True if the columns of the matrix form an orthonormal basis; otherwise, false.</returns>
-    public static bool CheckOrthonormalBasis(in JMatrix matrix)
+    public static bool CheckOrthonormalBasis(in JMatrix matrix, float epsilon = 1e-6f)
     {
         JMatrix delta = JMatrix.MultiplyTransposed(matrix, matrix) - JMatrix.Identity;
-        if (JVector.MaxAbs(delta.UnsafeGet(0)) > 1e-6f) return false;
-        if (JVector.MaxAbs(delta.UnsafeGet(1)) > 1e-6f) return false;
-        if (JVector.MaxAbs(delta.UnsafeGet(2)) > 1e-6f) return false;
-        return true;
+        return UnsafeIsZero(ref delta, epsilon);
     }
 
     /// <summary>

src/Jitter2/World.Detect.cs

@@ -88,6 +88,7 @@ public void Reset()
             manifoldCount = 0;
         }
 
+        [System.Runtime.CompilerServices.SkipLocalsInit]
         public void BuildManifold(RigidBodyShape shapeA, RigidBodyShape shapeB,
             in JVector pA, in JVector pB, in JVector normal, float penetration)
         {

src/Jitter2/World.Step.cs

@@ -435,7 +435,7 @@ private void AssertNullBody()
         ref RigidBodyData rigidBody = ref NullBody.Data;
         Debug.Assert(rigidBody.IsStatic);
         Debug.Assert(rigidBody.InverseMass == 0.0f);
-        Debug.Assert(MathHelper.UnsafeIsZero(rigidBody.InverseInertiaWorld));
+        Debug.Assert(MathHelper.UnsafeIsZero(ref rigidBody.InverseInertiaWorld));
     }
 
     private void ForeachActiveShape(bool multiThread)
@@ -458,7 +458,7 @@ private void ForeachActiveBody(bool multiThread)
         {
             if (body.IsStatic)
             {
-                System.Diagnostics.Debug.Assert(MathHelper.UnsafeIsZero(body.Data.InverseInertiaWorld));
+                System.Diagnostics.Debug.Assert(MathHelper.UnsafeIsZero(ref body.Data.InverseInertiaWorld));
                 System.Diagnostics.Debug.Assert(body.Data.InverseMass == 0.0f);
             }
         }