From 4abf3fd7391688231883ff6a4264b7585b7d4c6c Mon Sep 17 00:00:00 2001 From: notgiven688 Date: Tue, 29 Oct 2024 10:38:14 +0100 Subject: [PATCH 1/7] Add SkipLocalsInit attribute --- src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs | 1 + src/Jitter2/Dynamics/Constraints/PointOnLine.cs | 2 ++ src/Jitter2/World.Detect.cs | 1 + 3 files changed, 4 insertions(+) diff --git a/src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs b/src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs index 627e1adc..0789c37d 100644 --- a/src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs +++ b/src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs @@ -327,6 +327,7 @@ public void InitHeap() /// the return value of this method. /// /// Indicates whether the polyhedron successfully incorporated the new vertex. + [System.Runtime.CompilerServices.SkipLocalsInit] public bool AddVertex(in Vertex vertex) { Debug.Assert(vPointer < MaxVertices, "Maximum number of vertices exceeded."); diff --git a/src/Jitter2/Dynamics/Constraints/PointOnLine.cs b/src/Jitter2/Dynamics/Constraints/PointOnLine.cs index c315f5c1..2e2a04ac 100644 --- a/src/Jitter2/Dynamics/Constraints/PointOnLine.cs +++ b/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(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(Unsafe.AsPointer(ref constraint)); diff --git a/src/Jitter2/World.Detect.cs b/src/Jitter2/World.Detect.cs index f44499cb..eb72d6e6 100644 --- a/src/Jitter2/World.Detect.cs +++ b/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) { From c40084396a0dd5eaf43d218ba5ae08c30c39a4f1 Mon Sep 17 00:00:00 2001 From: notgiven688 Date: Tue, 29 Oct 2024 10:39:00 +0100 Subject: [PATCH 2/7] Cosmetic fix in JMatrix comments --- src/Jitter2/LinearMath/JMatrix.cs | 21 +++++++++------------ 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/src/Jitter2/LinearMath/JMatrix.cs b/src/Jitter2/LinearMath/JMatrix.cs index 039d2a7b..f89ce6fa 100644 --- a/src/Jitter2/LinearMath/JMatrix.cs +++ b/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; From 0ab8192870305d95d0c243b73004be1ec9bf0589 Mon Sep 17 00:00:00 2001 From: notgiven688 Date: Tue, 29 Oct 2024 10:39:52 +0100 Subject: [PATCH 3/7] Rename local variable in SimplexSolver(AB) --- src/Jitter2/Collision/NarrowPhase/SimplexSolver.cs | 8 ++++---- src/Jitter2/Collision/NarrowPhase/SimplexSolverAB.cs | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/Jitter2/Collision/NarrowPhase/SimplexSolver.cs b/src/Jitter2/Collision/NarrowPhase/SimplexSolver.cs index 7ed7e062..47048383 100644 --- a/src/Jitter2/Collision/NarrowPhase/SimplexSolver.cs +++ b/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; diff --git a/src/Jitter2/Collision/NarrowPhase/SimplexSolverAB.cs b/src/Jitter2/Collision/NarrowPhase/SimplexSolverAB.cs index 2f2cbe1f..aa2aaaf2 100644 --- a/src/Jitter2/Collision/NarrowPhase/SimplexSolverAB.cs +++ b/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; From 819f753fc693853656154a3ba337601bf979e1de Mon Sep 17 00:00:00 2001 From: notgiven688 Date: Tue, 29 Oct 2024 10:46:07 +0100 Subject: [PATCH 4/7] Slightly improved MathHelper.cs --- .../Collision/Shapes/TransformedShape.cs | 5 +- src/Jitter2/LinearMath/MathHelper.cs | 46 +++++++++---------- src/Jitter2/World.Step.cs | 2 +- 3 files changed, 26 insertions(+), 27 deletions(-) diff --git a/src/Jitter2/Collision/Shapes/TransformedShape.cs b/src/Jitter2/Collision/Shapes/TransformedShape.cs index 4256821c..1a06f9b7 100644 --- a/src/Jitter2/Collision/Shapes/TransformedShape.cs +++ b/src/Jitter2/Collision/Shapes/TransformedShape.cs @@ -43,7 +43,7 @@ private enum TransformationType /// /// Constructs a transformed shape through an affine transformation define by - /// a linear map and a translation. + /// a linear map and a translation. /// /// The original shape which should be transformed. /// Shape is translated by this vector. @@ -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 { diff --git a/src/Jitter2/LinearMath/MathHelper.cs b/src/Jitter2/LinearMath/MathHelper.cs index 1f05e25e..37bf2779 100644 --- a/src/Jitter2/LinearMath/MathHelper.cs +++ b/src/Jitter2/LinearMath/MathHelper.cs @@ -68,9 +68,14 @@ public static bool LineLineIntersect(in JVector p1, in JVector p2, in JVector p3 */ + /// + /// Checks if matrix is a pure rotation matrix. + /// 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; } @@ -78,19 +83,9 @@ public static bool IsRotationMatrix(in JMatrix matrix, float epsilon = 1e-06f) 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; - } - + /// + /// Checks if all entries of a vector are close to zero. + /// 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) + /// + /// Checks if all entries of a matrix are close to zero. + /// + 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; } + /// + /// Calculates (M^T \times M)^(-1/2) using Jacobi iterations. + /// + /// The number of Jacobi iterations. 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; } - /// /// 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. /// /// The input matrix to check for an orthonormal basis. /// True if the columns of the matrix form an orthonormal basis; otherwise, false. - 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); } /// diff --git a/src/Jitter2/World.Step.cs b/src/Jitter2/World.Step.cs index 8f5e34f7..90646bda 100644 --- a/src/Jitter2/World.Step.cs +++ b/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) From 0151d64e36dd45e47695b7284ff4aa3870d07103 Mon Sep 17 00:00:00 2001 From: notgiven688 Date: Tue, 29 Oct 2024 11:06:21 +0100 Subject: [PATCH 5/7] Fix debug check in World.Step.cs --- src/Jitter2/World.Step.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Jitter2/World.Step.cs b/src/Jitter2/World.Step.cs index 90646bda..40aff697 100644 --- a/src/Jitter2/World.Step.cs +++ b/src/Jitter2/World.Step.cs @@ -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); } } From f0970fe13f9e1f7e319165086a07167f6c9b3cfe Mon Sep 17 00:00:00 2001 From: notgiven688 Date: Tue, 29 Oct 2024 11:21:29 +0100 Subject: [PATCH 6/7] Rename markdown doc files --- docs/docs/02_documentation/{01world.md => 01-world.md} | 0 docs/docs/02_documentation/{02bodies.md => 02-bodies.md} | 0 docs/docs/02_documentation/{03shapes.md => 03-shapes.md} | 0 .../02_documentation/{04constraints.md => 04-constraints.md} | 2 +- .../02_documentation/{05dynamictree.md => 05-dynamictree.md} | 0 docs/docs/02_documentation/{06filters.md => 06-filters.md} | 0 6 files changed, 1 insertion(+), 1 deletion(-) rename docs/docs/02_documentation/{01world.md => 01-world.md} (100%) rename docs/docs/02_documentation/{02bodies.md => 02-bodies.md} (100%) rename docs/docs/02_documentation/{03shapes.md => 03-shapes.md} (100%) rename docs/docs/02_documentation/{04constraints.md => 04-constraints.md} (98%) rename docs/docs/02_documentation/{05dynamictree.md => 05-dynamictree.md} (100%) rename docs/docs/02_documentation/{06filters.md => 06-filters.md} (100%) diff --git a/docs/docs/02_documentation/01world.md b/docs/docs/02_documentation/01-world.md similarity index 100% rename from docs/docs/02_documentation/01world.md rename to docs/docs/02_documentation/01-world.md diff --git a/docs/docs/02_documentation/02bodies.md b/docs/docs/02_documentation/02-bodies.md similarity index 100% rename from docs/docs/02_documentation/02bodies.md rename to docs/docs/02_documentation/02-bodies.md diff --git a/docs/docs/02_documentation/03shapes.md b/docs/docs/02_documentation/03-shapes.md similarity index 100% rename from docs/docs/02_documentation/03shapes.md rename to docs/docs/02_documentation/03-shapes.md diff --git a/docs/docs/02_documentation/04constraints.md b/docs/docs/02_documentation/04-constraints.md similarity index 98% rename from docs/docs/02_documentation/04constraints.md rename to docs/docs/02_documentation/04-constraints.md index 5765f946..44b54c5d 100644 --- a/docs/docs/02_documentation/04constraints.md +++ b/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 diff --git a/docs/docs/02_documentation/05dynamictree.md b/docs/docs/02_documentation/05-dynamictree.md similarity index 100% rename from docs/docs/02_documentation/05dynamictree.md rename to docs/docs/02_documentation/05-dynamictree.md diff --git a/docs/docs/02_documentation/06filters.md b/docs/docs/02_documentation/06-filters.md similarity index 100% rename from docs/docs/02_documentation/06filters.md rename to docs/docs/02_documentation/06-filters.md From 4f2afd39a019a736b01e647bd0819648d766b39f Mon Sep 17 00:00:00 2001 From: notgiven688 Date: Tue, 29 Oct 2024 11:58:36 +0100 Subject: [PATCH 7/7] Make Deconstruct public in Limit.cs --- src/Jitter2/Dynamics/Constraints/Limit.cs | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/Jitter2/Dynamics/Constraints/Limit.cs b/src/Jitter2/Dynamics/Constraints/Limit.cs index df16c3c3..c2672d9d 100644 --- a/src/Jitter2/Dynamics/Constraints/Limit.cs +++ b/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; } } \ No newline at end of file