From 302f299193a1115b065bcd0fc64800e1b398c570 Mon Sep 17 00:00:00 2001 From: notgiven688 Date: Sat, 12 Sep 2026 15:36:10 +0200 Subject: [PATCH 1/9] Harden world lifecycle validation --- src/Jitter2/World.Step.cs | 19 ++++++------ src/Jitter2/World.cs | 25 +++++++++++++++- src/JitterTests/Api/WorldTests.cs | 29 +++++++++++++++++++ .../Robustness/DisposedWorldTests.cs | 24 +++++++++++++++ 4 files changed, 86 insertions(+), 11 deletions(-) diff --git a/src/Jitter2/World.Step.cs b/src/Jitter2/World.Step.cs index 26218683..710839de 100644 --- a/src/Jitter2/World.Step.cs +++ b/src/Jitter2/World.Step.cs @@ -134,7 +134,7 @@ private void InitParallelCallbacks() /// When is true, and /// may be called concurrently from worker threads. /// - /// Thrown if is negative. + /// Thrown if is negative or not finite. public void Step(Real dt, bool multiThread = true) { if (!multiThread) @@ -151,7 +151,7 @@ private void StepInternal(Real dt, bool multiThread) { ThrowIfDisposed(); AssertNullBody(); - DebugCheck.IsFinite(dt, nameof(dt)); + ArgumentCheck.Finite(dt, nameof(dt)); switch (dt) { @@ -310,8 +310,8 @@ void SetTime(Timings type) /// before resuming normal simulation with . /// /// - /// Thrown if is negative, is less than 1, - /// or is negative. + /// Thrown if is negative or not finite, + /// is less than 1, or is negative. /// public void Stabilize(Real dt, int solverIterations, int relaxationIterations = 0, bool multiThread = true) { @@ -329,14 +329,11 @@ private void StabilizeInternal(Real dt, int solverIterations, int relaxationIter { ThrowIfDisposed(); AssertNullBody(); - DebugCheck.IsFinite(dt, nameof(dt)); + ArgumentCheck.Finite(dt, nameof(dt)); - switch (dt) + if (dt < (Real)0.0) { - case < (Real)0.0: - throw new ArgumentException("Time step cannot be negative.", nameof(dt)); - case < Real.Epsilon: - return; // nothing to do + throw new ArgumentException("Time step cannot be negative.", nameof(dt)); } if (solverIterations < 1) @@ -349,6 +346,8 @@ private void StabilizeInternal(Real dt, int solverIterations, int relaxationIter throw new ArgumentException("Relaxation iterations can not be smaller than zero.", nameof(relaxationIterations)); } + if (dt < Real.Epsilon) return; // nothing to do + try { stepDt = dt; diff --git a/src/Jitter2/World.cs b/src/Jitter2/World.cs index bb617f29..89055b63 100644 --- a/src/Jitter2/World.cs +++ b/src/Jitter2/World.cs @@ -256,7 +256,15 @@ public readonly struct SpanData(World world) /// Grants access to objects residing in unmanaged memory. This operation can be potentially unsafe. Use /// the corresponding managed properties where possible to mitigate risk. /// - public SpanData RawData => new(this); + /// Thrown if this world has been disposed. + public SpanData RawData + { + get + { + ThrowIfDisposed(); + return new SpanData(this); + } + } private readonly ShardedDictionary arbiters = new(Parallelization.ThreadPool.ThreadCountSuggestion); @@ -466,9 +474,14 @@ public void Clear() /// and constraints. /// /// The rigid body to remove. + /// Thrown if is . /// Thrown if does not belong to this world. + /// Thrown if this world has been disposed. public void Remove(RigidBody body) { + ThrowIfDisposed(); + ArgumentNullException.ThrowIfNull(body); + if (body.World != this) throw new ArgumentException("The body does not belong to this world.", nameof(body)); @@ -513,11 +526,16 @@ public void Remove(RigidBody body) /// property. /// /// The constraint to be removed. + /// Thrown if is . /// /// Thrown if does not belong to this world. /// + /// Thrown if this world has been disposed. public void Remove(Constraint constraint) { + ThrowIfDisposed(); + ArgumentNullException.ThrowIfNull(constraint); + if (constraint.Body1.World != this) throw new ArgumentException("The constraint does not belong to this world.", nameof(constraint)); @@ -543,11 +561,16 @@ public void Remove(Constraint constraint) /// Removes a particular arbiter from the world. /// /// The arbiter to remove. + /// Thrown if is . /// /// Thrown if does not belong to this world. /// + /// Thrown if this world has been disposed. public void Remove(Arbiter arbiter) { + ThrowIfDisposed(); + ArgumentNullException.ThrowIfNull(arbiter); + if (arbiter.Body1.World != this) throw new ArgumentException("The arbiter does not belong to this world.", nameof(arbiter)); diff --git a/src/JitterTests/Api/WorldTests.cs b/src/JitterTests/Api/WorldTests.cs index d5735fcb..14c84906 100644 --- a/src/JitterTests/Api/WorldTests.cs +++ b/src/JitterTests/Api/WorldTests.cs @@ -147,6 +147,16 @@ public void Step_ZeroDt_DoesNotThrow() world.Dispose(); } + [TestCase] + public void Step_NonFiniteDt_Throws() + { + using var world = new World(); + + Assert.Throws(() => world.Step(Real.NaN, false)); + Assert.Throws(() => world.Step(Real.PositiveInfinity, false)); + Assert.Throws(() => world.Step(Real.NegativeInfinity, false)); + } + [TestCase] public void Step_WithNoBody_DoesNotThrow() { @@ -187,6 +197,25 @@ public void Stabilize_ZeroDt_DoesNotThrow() world.Dispose(); } + [TestCase] + public void Stabilize_NonFiniteDt_Throws() + { + using var world = new World(); + + Assert.Throws(() => world.Stabilize(Real.NaN, 1, 0, false)); + Assert.Throws(() => world.Stabilize(Real.PositiveInfinity, 1, 0, false)); + Assert.Throws(() => world.Stabilize(Real.NegativeInfinity, 1, 0, false)); + } + + [TestCase] + public void Stabilize_ZeroDt_StillValidatesIterationCounts() + { + using var world = new World(); + + Assert.Throws(() => world.Stabilize(0, 0, 0, false)); + Assert.Throws(() => world.Stabilize(0, 1, -1, false)); + } + [TestCase] public void Stabilize_SolverIterationsBelowOne_Throws() { diff --git a/src/JitterTests/Robustness/DisposedWorldTests.cs b/src/JitterTests/Robustness/DisposedWorldTests.cs index 171be6f2..00a92afb 100644 --- a/src/JitterTests/Robustness/DisposedWorldTests.cs +++ b/src/JitterTests/Robustness/DisposedWorldTests.cs @@ -68,6 +68,30 @@ public void CreateConstraint_AfterDispose_ThrowsObjectDisposedException() Assert.Throws(() => world.CreateConstraint(bodyA, bodyB)); } + [TestCase] + public void Remove_AfterDispose_ThrowsObjectDisposedException() + { + var world = new World(); + var bodyA = world.CreateRigidBody(); + var bodyB = world.CreateRigidBody(); + var constraint = world.CreateConstraint(bodyA, bodyB); + world.GetOrCreateArbiter(1, 2, bodyA, bodyB, out var arbiter); + world.Dispose(); + + Assert.Throws(() => world.Remove(bodyA)); + Assert.Throws(() => world.Remove(constraint)); + Assert.Throws(() => world.Remove(arbiter)); + } + + [TestCase] + public void RawData_AfterDispose_ThrowsObjectDisposedException() + { + var world = new World(); + world.Dispose(); + + Assert.Throws(() => _ = world.RawData); + } + [TestCase] public void Dispose_CanBeCalledTwice() { From 4c557a1230df08ce41e00274d26818cf4a04928e Mon Sep 17 00:00:00 2001 From: notgiven688 Date: Sat, 12 Sep 2026 15:39:05 +0200 Subject: [PATCH 2/9] Harden unmanaged and worker pool inputs --- src/Jitter2/Parallelization/ThreadPool.cs | 8 +++ src/Jitter2/Unmanaged/MemoryHelper.cs | 49 +++++++++++++++++-- src/Jitter2/Unmanaged/PartitionedBuffer.cs | 22 +++++++-- src/JitterTests/Robustness/ParallelTests.cs | 20 ++++++++ .../Robustness/PartitionedBufferTests.cs | 21 ++++++++ 5 files changed, 110 insertions(+), 10 deletions(-) diff --git a/src/Jitter2/Parallelization/ThreadPool.cs b/src/Jitter2/Parallelization/ThreadPool.cs index abefe9e8..8efaa6fd 100644 --- a/src/Jitter2/Parallelization/ThreadPool.cs +++ b/src/Jitter2/Parallelization/ThreadPool.cs @@ -202,8 +202,13 @@ private ThreadPool() /// Existing worker threads are stopped and new ones are created. /// This operation blocks until all previous threads have terminated. /// + /// + /// Thrown when is less than one. + /// public void ChangeThreadCount(int numThreads) { + ArgumentOutOfRangeException.ThrowIfLessThan(numThreads, 1); + if (numThreads == threadCount) return; running = false; @@ -253,8 +258,11 @@ public void ChangeThreadCount(int numThreads) /// Tasks are not executed until is called. /// This method is not thread-safe and must be called from a single thread. /// + /// Thrown when is . public void AddTask(Action action, T parameter) { + ArgumentNullException.ThrowIfNull(action); + var instance = Task.GetFree(); instance.Action = action; instance.Parameter = parameter; diff --git a/src/Jitter2/Unmanaged/MemoryHelper.cs b/src/Jitter2/Unmanaged/MemoryHelper.cs index e642a5e8..07593055 100644 --- a/src/Jitter2/Unmanaged/MemoryHelper.cs +++ b/src/Jitter2/Unmanaged/MemoryHelper.cs @@ -4,6 +4,7 @@ * SPDX-License-Identifier: MIT */ +using System; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; @@ -106,9 +107,13 @@ public struct MemBlock16Real { } /// Safety: The caller must free the returned pointer using . /// The memory is not zero-initialized. /// + /// Thrown when is negative. + /// Thrown when the requested byte count exceeds the native address space. public static T* AllocateHeap(int num) where T : unmanaged { - return (T*)AllocateHeap(num * sizeof(T)); + ArgumentOutOfRangeException.ThrowIfNegative(num); + nuint byteCount = checked((nuint)num * (nuint)sizeof(T)); + return (T*)NativeMemory.Alloc(byteCount); } /// @@ -122,9 +127,16 @@ public struct MemBlock16Real { } /// Safety: The caller must free the returned pointer using . /// The memory is not zero-initialized. /// + /// + /// Thrown when is negative or is not a positive power of two. + /// + /// Thrown when the requested byte count exceeds the native address space. public static T* AlignedAllocateHeap(int num, int alignment) where T : unmanaged { - return (T*)AlignedAllocateHeap(num * sizeof(T), alignment); + ArgumentOutOfRangeException.ThrowIfNegative(num); + ValidateAlignment(alignment); + nuint byteCount = checked((nuint)num * (nuint)sizeof(T)); + return (T*)NativeMemory.AlignedAlloc(byteCount, (nuint)alignment); } /// @@ -146,7 +158,12 @@ public static void Free(T* ptr) where T : unmanaged /// Safety: The caller must free the returned pointer using . /// The memory is not zero-initialized. /// - public static void* AllocateHeap(int len) => NativeMemory.Alloc((nuint)len); + /// Thrown when is negative. + public static void* AllocateHeap(int len) + { + ArgumentOutOfRangeException.ThrowIfNegative(len); + return NativeMemory.Alloc((nuint)len); + } /// /// Allocates a block of aligned unmanaged memory of the specified length in bytes. @@ -158,7 +175,15 @@ public static void Free(T* ptr) where T : unmanaged /// Safety: The caller must free the returned pointer using . /// The memory is not zero-initialized. /// - public static void* AlignedAllocateHeap(int len, int alignment) => NativeMemory.AlignedAlloc((nuint)len, (nuint)alignment); + /// + /// Thrown when is negative or is not a positive power of two. + /// + public static void* AlignedAllocateHeap(int len, int alignment) + { + ArgumentOutOfRangeException.ThrowIfNegative(len); + ValidateAlignment(alignment); + return NativeMemory.AlignedAlloc((nuint)len, (nuint)alignment); + } /// /// Frees a block of unmanaged memory previously allocated. @@ -177,5 +202,19 @@ public static void Free(T* ptr) where T : unmanaged /// /// A pointer to the memory block to zero out. /// The length of the memory block to zero out, in bytes. - public static void MemSet(void* buffer, int len) => Unsafe.InitBlock(buffer, 0, (uint)len); + /// Thrown when is negative. + public static void MemSet(void* buffer, int len) + { + ArgumentOutOfRangeException.ThrowIfNegative(len); + Unsafe.InitBlockUnaligned(buffer, 0, (uint)len); + } + + private static void ValidateAlignment(int alignment) + { + if (alignment <= 0 || (alignment & (alignment - 1)) != 0) + { + throw new ArgumentOutOfRangeException(nameof(alignment), alignment, + "Alignment must be a positive power of two."); + } + } } diff --git a/src/Jitter2/Unmanaged/PartitionedBuffer.cs b/src/Jitter2/Unmanaged/PartitionedBuffer.cs index c2ada01a..683f5c57 100644 --- a/src/Jitter2/Unmanaged/PartitionedBuffer.cs +++ b/src/Jitter2/Unmanaged/PartitionedBuffer.cs @@ -141,6 +141,7 @@ public MaximumSizeException(string message, Exception inner) : base(message, inn private const int PageMask = PageSize - 1; private const int PageShift = 12; // 2^12 = 4096 private const int MaxPages = 1024 * 16; // Capacity for ~67 million handles + private const int MaximumCapacity = MaxPages * PageSize; private T* memory; private T*** pages; // Array of pointers to pages (indirection table) @@ -175,8 +176,14 @@ public MaximumSizeException(string message, Exception inner) : base(message, inn /// /// Thrown when is too small to store the internal element ID. /// + /// + /// Thrown when is less than one or exceeds the maximum capacity. + /// public PartitionedBuffer(int initialSize = 1024, bool aligned64 = false) { + ArgumentOutOfRangeException.ThrowIfLessThan(initialSize, 1); + ArgumentOutOfRangeException.ThrowIfGreaterThan(initialSize, MaximumCapacity); + if (sizeof(T) < sizeof(int)) { throw new ArgumentException($"Type {typeof(T).Name} is too small. It must be at least {sizeof(int)} bytes to store the internal ID."); @@ -189,7 +196,7 @@ public PartitionedBuffer(int initialSize = 1024, bool aligned64 = false) if (aligned64) { - try { memory = (T*)MemoryHelper.AlignedAllocateHeap(size * sizeof(T), 64); } + try { memory = MemoryHelper.AlignedAllocateHeap(size, 64); } catch { Logger.Warning("Could not allocate aligned memory. Falling back to unaligned memory."); @@ -199,7 +206,7 @@ public PartitionedBuffer(int initialSize = 1024, bool aligned64 = false) if (!aligned64) { - memory = (T*)MemoryHelper.AllocateHeap(size * sizeof(T)); + memory = MemoryHelper.AllocateHeap(size); } this.Aligned64 = aligned64; @@ -414,11 +421,16 @@ public JHandle Allocate(bool active = false, bool clear = false) try { int oldSize = size; - int newSize = checked(size * 2); + if (size == MaximumCapacity) + { + throw new MaximumSizeException("Internal indirection table limit reached."); + } + + int newSize = Math.Min(checked(size * 2), MaximumCapacity); T* oldMemory = memory; - if (Aligned64) newMemory = (T*)MemoryHelper.AlignedAllocateHeap(newSize * sizeof(T), 64); - else newMemory = (T*)MemoryHelper.AllocateHeap(newSize * sizeof(T)); + if (Aligned64) newMemory = MemoryHelper.AlignedAllocateHeap(newSize, 64); + else newMemory = MemoryHelper.AllocateHeap(newSize); // Ensure handles are ready before publishing any new data pointers. EnsureHandleCapacity(newSize); diff --git a/src/JitterTests/Robustness/ParallelTests.cs b/src/JitterTests/Robustness/ParallelTests.cs index 8ce9cb8e..74288628 100644 --- a/src/JitterTests/Robustness/ParallelTests.cs +++ b/src/JitterTests/Robustness/ParallelTests.cs @@ -93,6 +93,26 @@ public static void ThreadPool_RethrowsTaskExceptionAndCanExecuteAgain() } } + [TestCase(-1)] + [TestCase(0)] + public static void ThreadPool_RejectsInvalidThreadCountWithoutChangingState(int threadCount) + { + var threadPool = ThreadPool.Instance; + int originalThreadCount = threadPool.ThreadCount; + + var exception = Assert.Throws(() => + threadPool.ChangeThreadCount(threadCount)); + + Assert.That(exception!.ParamName, Is.EqualTo("numThreads")); + Assert.That(threadPool.ThreadCount, Is.EqualTo(originalThreadCount)); + } + + [TestCase] + public static void ThreadPool_RejectsNullTask() + { + Assert.Throws(() => ThreadPool.Instance.AddTask(null!, 0)); + } + [TestCase] public static void PartitionedBuffer_ResizePreservesHandlesAndReleasesLock() { diff --git a/src/JitterTests/Robustness/PartitionedBufferTests.cs b/src/JitterTests/Robustness/PartitionedBufferTests.cs index d30cb720..31c5ee5f 100644 --- a/src/JitterTests/Robustness/PartitionedBufferTests.cs +++ b/src/JitterTests/Robustness/PartitionedBufferTests.cs @@ -19,6 +19,27 @@ private sealed class Entry(JHandle handle, int value, bool active) public bool Active = active; } + [TestCase(-1)] + [TestCase(0)] + [TestCase(int.MaxValue)] + public void Constructor_WithInvalidInitialSize_Throws(int initialSize) + { + var exception = Assert.Throws(() => + _ = new PartitionedBuffer(initialSize)); + + Assert.That(exception!.ParamName, Is.EqualTo("initialSize")); + } + + [TestCase] + public unsafe void MemoryHelper_WithInvalidSizesOrAlignment_Throws() + { + Assert.Throws(() => MemoryHelper.AllocateHeap(-1)); + Assert.Throws(() => MemoryHelper.AllocateHeap(-1)); + Assert.Throws(() => MemoryHelper.AlignedAllocateHeap(16, 3)); + Assert.Throws(() => MemoryHelper.AlignedAllocateHeap(1, 0)); + Assert.Throws(() => MemoryHelper.MemSet(null, -1)); + } + [TestCase] public void FreeActive_PreservesRemainingElementsAndPartitions() { From e62e4e823cd6f6abc29e779aab5bb613cd3abf90 Mon Sep 17 00:00:00 2001 From: notgiven688 Date: Sat, 12 Sep 2026 15:40:51 +0200 Subject: [PATCH 3/9] Validate dynamic tree optimization inputs --- src/Jitter2/Collision/DynamicTree/DynamicTree.cs | 2 +- src/JitterTests/Behavior/DynamicTreeBalanceTests.cs | 10 ++++++++++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/Jitter2/Collision/DynamicTree/DynamicTree.cs b/src/Jitter2/Collision/DynamicTree/DynamicTree.cs index 60bc1816..dffa46d1 100644 --- a/src/Jitter2/Collision/DynamicTree/DynamicTree.cs +++ b/src/Jitter2/Collision/DynamicTree/DynamicTree.cs @@ -717,7 +717,7 @@ public void Optimize(int sweeps = 100, Real chance = (Real)0.01, bool incrementa public void Optimize(Func getNextRandom, int sweeps, Real chance, bool incremental) { if (sweeps <= 0) throw new ArgumentOutOfRangeException(nameof(sweeps), "Sweeps must be greater than zero."); - if (chance is < 0 or > 1) throw new ArgumentOutOfRangeException(nameof(chance), "Chance must be between 0 and 1."); + ArgumentCheck.InRange(chance, (Real)0.0, (Real)1.0, nameof(chance)); for (int e = 0; e < sweeps; e++) { diff --git a/src/JitterTests/Behavior/DynamicTreeBalanceTests.cs b/src/JitterTests/Behavior/DynamicTreeBalanceTests.cs index 68504847..a355f363 100644 --- a/src/JitterTests/Behavior/DynamicTreeBalanceTests.cs +++ b/src/JitterTests/Behavior/DynamicTreeBalanceTests.cs @@ -194,6 +194,16 @@ public void OptimizePreservesExpandedBoxesHandlesPairsAndActivity() AssertValidTree(tree, shapes.Count); } + [Test] + public void OptimizeRejectsNonFiniteChance() + { + using World world = new(); + DynamicTree tree = world.DynamicTree; + + Assert.Throws(() => + tree.Optimize(sweeps: 1, chance: Real.NaN, incremental: true)); + } + private static int AssertValidTree(DynamicTree tree, int expectedLeafCount) { if (expectedLeafCount == 0) From 2fda68a754345e62fa60a05d826495cd44697353 Mon Sep 17 00:00:00 2001 From: notgiven688 Date: Sat, 12 Sep 2026 17:33:45 +0200 Subject: [PATCH 4/9] Harden PairHashSet resizing --- src/Jitter2/Collision/PairHashSet.cs | 22 +++++++----- .../Robustness/PairHashSetTests.cs | 34 +++++++++++++++++++ 2 files changed, 48 insertions(+), 8 deletions(-) diff --git a/src/Jitter2/Collision/PairHashSet.cs b/src/Jitter2/Collision/PairHashSet.cs index d4c1c335..d6d052f4 100644 --- a/src/Jitter2/Collision/PairHashSet.cs +++ b/src/Jitter2/Collision/PairHashSet.cs @@ -181,7 +181,7 @@ private void Resize(int size) } } - Slots = newSlots; + Volatile.Write(ref Slots, newSlots); } private static int FindSlot(Pair[] slots, int hash, long id) @@ -279,7 +279,7 @@ public bool ConcurrentAdd(Pair pair) // Fast path: This is a *huge* optimization in case of frequent additions // of already existing entries. Entirely bypassing any locks or synchronization. - Pair[] fpSlots = Slots; + Pair[] fpSlots = Volatile.Read(ref Slots); _ = FindSlotConcurrent(fpSlots, hash, pair.ID, out long fpSlotId); if (fpSlotId == pair.ID) return false; @@ -307,17 +307,23 @@ public bool ConcurrentAdd(Pair pair) Interlocked.Increment(ref count); rwLock.ExitReadLock(); - if (Slots.Length < 2 * count) + Pair[] currentSlots = Volatile.Read(ref Slots); + if (currentSlots.Length < 2 * count) { rwLock.EnterWriteLock(); - // check if another thread already performed a resize. - if (Slots.Length < 2 * count) + try { - Resize(PickSize(Slots.Length * 2)); + // Check if another thread already performed a resize. + if (Slots.Length < 2 * count) + { + Resize(PickSize(Slots.Length * 2)); + } + } + finally + { + rwLock.ExitWriteLock(); } - - rwLock.ExitWriteLock(); } return true; diff --git a/src/JitterTests/Robustness/PairHashSetTests.cs b/src/JitterTests/Robustness/PairHashSetTests.cs index bdf5599d..e4c01c16 100644 --- a/src/JitterTests/Robustness/PairHashSetTests.cs +++ b/src/JitterTests/Robustness/PairHashSetTests.cs @@ -5,6 +5,40 @@ namespace JitterTests.Robustness; public class PairHashSetTests { + [Test] + public void ConcurrentAdd_ResizesAndPreservesEntries() + { + const int pairCount = PairHashSet.MinimumSize / 2 + 512; + + PairHashSet hashSet = new(); + PairHashSet.Pair[] pairs = new PairHashSet.Pair[pairCount]; + int rejected = 0; + + for (int i = 0; i < pairs.Length; i++) + { + pairs[i] = new PairHashSet.Pair(i + 1, i + pairCount + 1); + } + + System.Threading.Tasks.Parallel.For(0, pairs.Length, i => + { + if (!hashSet.ConcurrentAdd(pairs[i])) Interlocked.Increment(ref rejected); + }); + + int missing = 0; + foreach (PairHashSet.Pair pair in pairs) + { + if (!hashSet.Contains(pair)) missing++; + } + + Assert.Multiple(() => + { + Assert.That(rejected, Is.Zero); + Assert.That(missing, Is.Zero); + Assert.That(hashSet.Count, Is.EqualTo(pairCount)); + Assert.That(hashSet.Slots, Has.Length.EqualTo(PairHashSet.MinimumSize * 2)); + }); + } + [Test] public void ConcurrentAdd_WithDuplicateAndCollidingPairs_PreservesSetSemantics() { From a77af9a36e05bc1ee2db4f06d260e6ebcaadef03 Mon Sep 17 00:00:00 2001 From: notgiven688 Date: Sat, 12 Sep 2026 17:43:40 +0200 Subject: [PATCH 5/9] Avoid PairHashSet resize thrashing --- src/Jitter2/Collision/PairHashSet.cs | 7 +++- .../Robustness/PairHashSetTests.cs | 32 +++++++++++++++++++ 2 files changed, 38 insertions(+), 1 deletion(-) diff --git a/src/Jitter2/Collision/PairHashSet.cs b/src/Jitter2/Collision/PairHashSet.cs index d6d052f4..ca086360 100644 --- a/src/Jitter2/Collision/PairHashSet.cs +++ b/src/Jitter2/Collision/PairHashSet.cs @@ -127,6 +127,11 @@ public void Reset() /// public const int TrimFactor = 8; + /// + /// Factor used to leave growth headroom after shrinking the hash set. + /// + public const int TrimTargetFactor = 4; + /// /// Gets the number of pairs in the hash set. /// @@ -372,7 +377,7 @@ public bool Remove(int slot) if (Slots.Length > MinimumSize && count * TrimFactor < Slots.Length) { - Resize(PickSize(count * 2)); + Resize(PickSize(count * TrimTargetFactor)); } return true; diff --git a/src/JitterTests/Robustness/PairHashSetTests.cs b/src/JitterTests/Robustness/PairHashSetTests.cs index e4c01c16..b0ac688a 100644 --- a/src/JitterTests/Robustness/PairHashSetTests.cs +++ b/src/JitterTests/Robustness/PairHashSetTests.cs @@ -5,6 +5,38 @@ namespace JitterTests.Robustness; public class PairHashSetTests { + [Test] + public void Remove_ShrinksWithGrowthHeadroom() + { + const int initialCount = PairHashSet.MinimumSize + 1; + const int targetCount = PairHashSet.MinimumSize / 2 - 1; + + PairHashSet hashSet = new(); + PairHashSet.Pair[] pairs = new PairHashSet.Pair[initialCount]; + int rejected = 0; + + for (int i = 0; i < pairs.Length; i++) + { + pairs[i] = new PairHashSet.Pair(i + 1, i + initialCount + 1); + if (!hashSet.Add(pairs[i])) rejected++; + } + + int missing = 0; + for (int i = targetCount; i < pairs.Length; i++) + { + if (!hashSet.Remove(pairs[i])) missing++; + } + + Assert.Multiple(() => + { + Assert.That(rejected, Is.Zero); + Assert.That(missing, Is.Zero); + Assert.That(hashSet.Count, Is.EqualTo(targetCount)); + Assert.That(hashSet.Slots, + Has.Length.EqualTo(PairHashSet.MinimumSize * 2)); + }); + } + [Test] public void ConcurrentAdd_ResizesAndPreservesEntries() { From 4ff0a4d8aa711b457d46dde6f5318fc2331f21ff Mon Sep 17 00:00:00 2001 From: notgiven688 Date: Sun, 13 Sep 2026 15:03:37 +0200 Subject: [PATCH 6/9] Scope island pooling to world instances --- src/Jitter2/Collision/IslandHelper.cs | 56 +++++++++---------- src/Jitter2/World.Step.cs | 4 +- src/Jitter2/World.cs | 17 +++--- .../Robustness/IslandPoolingTests.cs | 48 ++++++++++++++++ 4 files changed, 86 insertions(+), 39 deletions(-) create mode 100644 src/JitterTests/Robustness/IslandPoolingTests.cs diff --git a/src/Jitter2/Collision/IslandHelper.cs b/src/Jitter2/Collision/IslandHelper.cs index 4fa97f62..7999b0e7 100644 --- a/src/Jitter2/Collision/IslandHelper.cs +++ b/src/Jitter2/Collision/IslandHelper.cs @@ -17,27 +17,25 @@ namespace Jitter2.Collision; /// Helper class to update islands. Methods must not be called concurrently for the same world. /// /// -/// Scratch data and pooled islands are stored per thread. Separate worlds may use this helper -/// concurrently on different external threads as long as each individual world is not used -/// concurrently. +/// Search scratch data is stored per thread. Pooled islands are stored by the owning world. +/// Separate worlds may use this helper concurrently on different external threads as long as +/// each individual world is not used concurrently. /// internal static class IslandHelper { - [ThreadStatic] private static Stack? pool; [ThreadStatic] private static Queue? leftSearchQueue; [ThreadStatic] private static Queue? rightSearchQueue; [ThreadStatic] private static List? visitedBodiesLeft; [ThreadStatic] private static List? visitedBodiesRight; - private static Stack Pool => pool ??= new Stack(); private static Queue LeftSearchQueue => leftSearchQueue ??= new Queue(); private static Queue RightSearchQueue => rightSearchQueue ??= new Queue(); private static List VisitedBodiesLeft => visitedBodiesLeft ??= []; private static List VisitedBodiesRight => visitedBodiesRight ??= []; - private static Island GetFromPool() + private static Island GetFromPool(Stack pool) { - if (!Pool.TryPop(out var island)) + if (!pool.TryPop(out var island)) { island = new Island(); } @@ -48,12 +46,12 @@ private static Island GetFromPool() return island; } - private static void ReturnToPool(Island island) + private static void ReturnToPool(Stack pool, Island island) { - Pool.Push(island); + pool.Push(island); } - public static void ArbiterCreated(IslandSet islands, Arbiter arbiter) + public static void ArbiterCreated(IslandSet islands, Stack islandPool, Arbiter arbiter) { RigidBody b1 = arbiter.Body1; RigidBody b2 = arbiter.Body2; @@ -61,55 +59,55 @@ public static void ArbiterCreated(IslandSet islands, Arbiter arbiter) b1.InternalContacts.Add(arbiter); b2.InternalContacts.Add(arbiter); - AddConnection(islands, b1, b2); + AddConnection(islands, islandPool, b1, b2); } - public static void ArbiterRemoved(IslandSet islands, Arbiter arbiter) + public static void ArbiterRemoved(IslandSet islands, Stack islandPool, Arbiter arbiter) { arbiter.Body1.InternalContacts.Remove(arbiter); arbiter.Body2.InternalContacts.Remove(arbiter); - RemoveConnection(islands, arbiter.Body1, arbiter.Body2); + RemoveConnection(islands, islandPool, arbiter.Body1, arbiter.Body2); } - public static void ConstraintCreated(IslandSet islands, Constraint constraint) + public static void ConstraintCreated(IslandSet islands, Stack islandPool, Constraint constraint) { constraint.Body1.InternalConstraints.Add(constraint); constraint.Body2.InternalConstraints.Add(constraint); - AddConnection(islands, constraint.Body1, constraint.Body2); + AddConnection(islands, islandPool, constraint.Body1, constraint.Body2); } - public static void ConstraintRemoved(IslandSet islands, Constraint constraint) + public static void ConstraintRemoved(IslandSet islands, Stack islandPool, Constraint constraint) { constraint.Body1.InternalConstraints.Remove(constraint); constraint.Body2.InternalConstraints.Remove(constraint); - RemoveConnection(islands, constraint.Body1, constraint.Body2); + RemoveConnection(islands, islandPool, constraint.Body1, constraint.Body2); } - public static void BodyAdded(IslandSet islands, RigidBody body) + public static void BodyAdded(IslandSet islands, Stack islandPool, RigidBody body) { - body.InternalIsland = GetFromPool(); + body.InternalIsland = GetFromPool(islandPool); islands.Add(body.InternalIsland, true); body.InternalIsland.InternalBodies.Add(body); } - public static void BodyRemoved(IslandSet islands, RigidBody body) + public static void BodyRemoved(IslandSet islands, Stack islandPool, RigidBody body) { body.InternalIsland.ClearLists(); - ReturnToPool(body.InternalIsland); + ReturnToPool(islandPool, body.InternalIsland); islands.Remove(body.InternalIsland); } - public static void AddConnection(IslandSet islands, RigidBody body1, RigidBody body2) + public static void AddConnection(IslandSet islands, Stack islandPool, RigidBody body1, RigidBody body2) { bool needsUpdate = (!islands.IsActive(body1.Island) || !islands.IsActive(body2.Island)); bool bothNotStatic = body1.Data.MotionType != MotionType.Static && body2.Data.MotionType != MotionType.Static; if (bothNotStatic) { - MergeIslands(islands, body1, body2); + MergeIslands(islands, islandPool, body1, body2); body1.InternalConnections.Add(body2); body2.InternalConnections.Add(body1); } @@ -121,7 +119,7 @@ public static void AddConnection(IslandSet islands, RigidBody body1, RigidBody b } } - public static void RemoveConnection(IslandSet islands, RigidBody body1, RigidBody body2) + public static void RemoveConnection(IslandSet islands, Stack islandPool, RigidBody body1, RigidBody body2) { static void RemoveRef(List list, RigidBody body) { @@ -138,11 +136,11 @@ static void RemoveRef(List list, RigidBody body) if (body1.InternalIsland == body2.InternalIsland) { - SplitIslands(islands, body1, body2); + SplitIslands(islands, islandPool, body1, body2); } } - private static void SplitIslands(IslandSet islands, RigidBody body1, RigidBody body2) + private static void SplitIslands(IslandSet islands, Stack islandPool, RigidBody body1, RigidBody body2) { Debug.Assert(body1.InternalIsland == body2.InternalIsland, "Islands not the same or null."); @@ -209,7 +207,7 @@ private static void SplitIslands(IslandSet islands, RigidBody body1, RigidBody b } } - Island island = GetFromPool(); + Island island = GetFromPool(islandPool); island.NeedsUpdate = sourceNeedsUpdate; island.MarkedAsActive = sourceMarkedAsActive; islands.Add(island, sourceIslandActive); @@ -255,7 +253,7 @@ private static void SplitIslands(IslandSet islands, RigidBody body1, RigidBody b } // Both bodies must be !static - private static void MergeIslands(IslandSet islands, RigidBody body1, RigidBody body2) + private static void MergeIslands(IslandSet islands, Stack islandPool, RigidBody body1, RigidBody body2) { if (body1.InternalIsland == body2.InternalIsland) return; @@ -278,7 +276,7 @@ private static void MergeIslands(IslandSet islands, RigidBody body1, RigidBody b Island giveBackIsland = smallIslandOwner.InternalIsland; - ReturnToPool(giveBackIsland); + ReturnToPool(islandPool, giveBackIsland); islands.Remove(giveBackIsland); foreach (RigidBody b in giveBackIsland.InternalBodies) diff --git a/src/Jitter2/World.Step.cs b/src/Jitter2/World.Step.cs index 710839de..50375053 100644 --- a/src/Jitter2/World.Step.cs +++ b/src/Jitter2/World.Step.cs @@ -820,7 +820,7 @@ private void RemoveBrokenArbiters() AddToActiveList(arb.Body1.InternalIsland); AddToActiveList(arb.Body2.InternalIsland); - IslandHelper.ArbiterRemoved(islands, arb); + IslandHelper.ArbiterRemoved(islands, islandPool, arb); arbiters.Remove(handle.Data.Key); arb.Body1.RaiseEndCollide(arb); @@ -855,7 +855,7 @@ private void HandleDeferredArbiters() { foreach (var arb in deferredArbiters) { - IslandHelper.ArbiterCreated(islands, arb); + IslandHelper.ArbiterCreated(islands, islandPool, arb); AddToActiveList(arb.Body1.InternalIsland); AddToActiveList(arb.Body2.InternalIsland); diff --git a/src/Jitter2/World.cs b/src/Jitter2/World.cs index 89055b63..3ed3b425 100644 --- a/src/Jitter2/World.cs +++ b/src/Jitter2/World.cs @@ -275,6 +275,7 @@ public SpanData RawData private readonly PartitionedSet islands = []; private readonly PartitionedSet bodies = []; + private readonly Stack islandPool = []; private static ulong _idCounter; @@ -514,7 +515,7 @@ public void Remove(RigidBody body) body.Handle = JHandle.Zero; - IslandHelper.BodyRemoved(islands, body); + IslandHelper.BodyRemoved(islands, islandPool, body); body.InternalIsland = null!; @@ -542,7 +543,7 @@ public void Remove(Constraint constraint) ActivateBodyNextStep(constraint.Body1); ActivateBodyNextStep(constraint.Body2); - IslandHelper.ConstraintRemoved(islands, constraint); + IslandHelper.ConstraintRemoved(islands, islandPool, constraint); if (constraint.IsSmallConstraint) { @@ -577,7 +578,7 @@ public void Remove(Arbiter arbiter) ActivateBodyNextStep(arbiter.Body1); ActivateBodyNextStep(arbiter.Body2); - IslandHelper.ArbiterRemoved(islands, arbiter); + IslandHelper.ArbiterRemoved(islands, islandPool, arbiter); arbiters.Remove(arbiter.Handle.Data.Key); brokenArbiters.Remove(arbiter.Handle); @@ -651,12 +652,12 @@ internal void BuildConnectionsFromExistingContacts(RigidBody body) { foreach (var constraint in body.InternalConstraints) { - IslandHelper.AddConnection(islands, constraint.Body1, constraint.Body2); + IslandHelper.AddConnection(islands, islandPool, constraint.Body1, constraint.Body2); } foreach (var contact in body.InternalContacts) { - IslandHelper.AddConnection(islands, contact.Body1, contact.Body2); + IslandHelper.AddConnection(islands, islandPool, contact.Body1, contact.Body2); } } @@ -677,7 +678,7 @@ internal void RemoveConnections(RigidBody body) for (int i = 0; i < count; i++) { - IslandHelper.RemoveConnection(islands, body, connections[i]); + IslandHelper.RemoveConnection(islands, islandPool, body, connections[i]); } } finally @@ -785,7 +786,7 @@ public void ForceSleepIsland(Island island) constraint.Handle.Data.ConstraintId = constraintId; } - IslandHelper.ConstraintCreated(islands, constraint); + IslandHelper.ConstraintCreated(islands, islandPool, constraint); AddToActiveList(body1.InternalIsland); AddToActiveList(body2.InternalIsland); @@ -844,7 +845,7 @@ public RigidBody CreateRigidBody() bodies.Add(body, true); - IslandHelper.BodyAdded(islands, body); + IslandHelper.BodyAdded(islands, islandPool, body); AddToActiveList(body.InternalIsland); diff --git a/src/JitterTests/Robustness/IslandPoolingTests.cs b/src/JitterTests/Robustness/IslandPoolingTests.cs new file mode 100644 index 00000000..0ad62bcb --- /dev/null +++ b/src/JitterTests/Robustness/IslandPoolingTests.cs @@ -0,0 +1,48 @@ +using System.Reflection; + +namespace JitterTests.Robustness; + +public class IslandPoolingTests +{ + private static readonly TimeSpan Timeout = TimeSpan.FromSeconds(10); + + [Test] + public void RemovedBodyIsland_IsReusedAcrossThreadsInOwningWorld() + { + using var world = new World(); + RigidBody body = world.CreateRigidBody(); + Island island = body.Island; + + Thread worker = new(() => world.Remove(body)) { IsBackground = true }; + worker.Start(); + Assert.That(worker.Join(Timeout), Is.True, "Worker did not finish."); + + var islandPool = GetField>(world, "islandPool"); + Assert.That(islandPool, Does.Contain(island)); + + RigidBody reused = world.CreateRigidBody(); + Assert.That(reused.Island, Is.SameAs(island)); + } + + [Test] + public void RemovedBodyIsland_IsScopedToOwningWorld() + { + using var world1 = new World(); + using var world2 = new World(); + + RigidBody body = world1.CreateRigidBody(); + Island island = body.Island; + world1.Remove(body); + + RigidBody otherWorldBody = world2.CreateRigidBody(); + Assert.That(otherWorldBody.Island, Is.Not.SameAs(island)); + + RigidBody reused = world1.CreateRigidBody(); + Assert.That(reused.Island, Is.SameAs(island)); + } + + private static T GetField(object instance, string name) + { + return (T)instance.GetType().GetField(name, BindingFlags.Instance | BindingFlags.NonPublic)!.GetValue(instance)!; + } +} From fa59687adb8765254c99bfb402146f7e6fd422cf Mon Sep 17 00:00:00 2001 From: notgiven688 Date: Sun, 13 Sep 2026 15:32:11 +0200 Subject: [PATCH 7/9] Add world trim --- src/Jitter2/Collision/CollisionIsland.cs | 5 ++ .../Collision/DynamicTree/DynamicTree.cs | 8 ++ src/Jitter2/DataStructures/PartitionedSet.cs | 26 +++++++ .../DataStructures/ShardedDictionary.cs | 11 +++ src/Jitter2/DataStructures/SlimBag.cs | 24 ++++++ src/Jitter2/Dynamics/RigidBody.cs | 8 ++ src/Jitter2/World.cs | 56 ++++++++++++++ src/JitterTests/Api/SequentialTests.cs | 43 +++++++++++ src/JitterTests/Robustness/WorldTrimTests.cs | 74 +++++++++++++++++++ 9 files changed, 255 insertions(+) create mode 100644 src/JitterTests/Robustness/WorldTrimTests.cs diff --git a/src/Jitter2/Collision/CollisionIsland.cs b/src/Jitter2/Collision/CollisionIsland.cs index 226b8db0..4afe9479 100644 --- a/src/Jitter2/Collision/CollisionIsland.cs +++ b/src/Jitter2/Collision/CollisionIsland.cs @@ -45,4 +45,9 @@ internal void ClearLists() { InternalBodies.Clear(); } + + internal void TrimLists() + { + InternalBodies.TrimExcess(); + } } diff --git a/src/Jitter2/Collision/DynamicTree/DynamicTree.cs b/src/Jitter2/Collision/DynamicTree/DynamicTree.cs index dffa46d1..a4706863 100644 --- a/src/Jitter2/Collision/DynamicTree/DynamicTree.cs +++ b/src/Jitter2/Collision/DynamicTree/DynamicTree.cs @@ -38,6 +38,14 @@ private struct OverlapEnumerationParam /// public ReadOnlyPartitionedSet Proxies => new(proxies); + internal void Trim() + { + proxies.Trim(); + movedProxies.Trim(); + tempList.TrimExcess(); + freeNodes.TrimExcess(); + } + private readonly PairHashSet potentialPairs = []; /// diff --git a/src/Jitter2/DataStructures/PartitionedSet.cs b/src/Jitter2/DataStructures/PartitionedSet.cs index 3447f7e8..8203024f 100644 --- a/src/Jitter2/DataStructures/PartitionedSet.cs +++ b/src/Jitter2/DataStructures/PartitionedSet.cs @@ -116,6 +116,7 @@ public void Reset() } } + private readonly int initialSize; private T[] elements; /// Gets the number of active elements in the set. @@ -127,9 +128,15 @@ public void Reset() /// The initial capacity of the internal array. public PartitionedSet(int initialSize = 1024) { + ArgumentOutOfRangeException.ThrowIfLessThan(initialSize, 1); + + this.initialSize = initialSize; elements = new T[initialSize]; } + /// Gets the number of elements the set can hold before the next resize. + public int Capacity => elements.Length; + /// Gets the element at the specified index. public T this[int i] => elements[i]; @@ -158,6 +165,25 @@ public void Clear() /// Gets the total number of elements in the set. public int Count { get; private set; } + /// + /// Shrinks the internal storage to the smallest retained capacity that can hold the current elements. + /// + /// if the set was resized; otherwise, . + public bool Trim() + { + int newSize = initialSize; + + while (newSize < Count) + { + newSize = checked(newSize * 2); + } + + if (newSize >= elements.Length) return false; + + Array.Resize(ref elements, newSize); + return true; + } + /// Returns a span of all elements in the set. public Span AsSpan() => this.elements.AsSpan(0, Count); diff --git a/src/Jitter2/DataStructures/ShardedDictionary.cs b/src/Jitter2/DataStructures/ShardedDictionary.cs index fdb00b98..fa440781 100644 --- a/src/Jitter2/DataStructures/ShardedDictionary.cs +++ b/src/Jitter2/DataStructures/ShardedDictionary.cs @@ -93,6 +93,17 @@ public ShardedDictionary(int threads) } } + /// + /// Sets the capacity of each shard to the actual number of entries it contains. + /// + public void TrimExcess() + { + for (int i = 0; i < dictionaries.Length; i++) + { + dictionaries[i].TrimExcess(); + } + } + /// /// Attempts to get the value associated with the specified key. /// diff --git a/src/Jitter2/DataStructures/SlimBag.cs b/src/Jitter2/DataStructures/SlimBag.cs index 64c67621..dd265a6a 100644 --- a/src/Jitter2/DataStructures/SlimBag.cs +++ b/src/Jitter2/DataStructures/SlimBag.cs @@ -60,6 +60,7 @@ public void Dispose() { } public void Reset() => index = -1; } + private readonly int initialSize; private T[] array; private int counter; private int nullOut; @@ -71,6 +72,9 @@ public void Dispose() { } /// The initial size of the internal array. Defaults to 4 if not specified. public SlimBag(int initialSize = 4) { + ArgumentOutOfRangeException.ThrowIfLessThan(initialSize, 1); + + this.initialSize = initialSize; array = new T[initialSize]; nullOut = 0; } @@ -80,6 +84,26 @@ public SlimBag(int initialSize = 4) /// public int InternalSize => array.Length; + /// + /// Shrinks the internal storage to the smallest retained capacity that can hold the current elements. + /// + /// if the bag was resized; otherwise, . + public bool Trim() + { + int newSize = initialSize; + + while (newSize < counter) + { + newSize = checked(newSize * 2); + } + + if (newSize >= array.Length) return false; + + Array.Resize(ref array, newSize); + nullOut = int.Min(nullOut, counter); + return true; + } + /// /// Returns a span over the valid elements of the internal array. /// diff --git a/src/Jitter2/Dynamics/RigidBody.cs b/src/Jitter2/Dynamics/RigidBody.cs index c4611b13..8cc626b4 100644 --- a/src/Jitter2/Dynamics/RigidBody.cs +++ b/src/Jitter2/Dynamics/RigidBody.cs @@ -288,6 +288,14 @@ internal void RaiseEndCollide(Arbiter arbiter) EndCollide?.Invoke(arbiter); } + internal void TrimLists() + { + InternalShapes.TrimExcess(); + InternalConnections.TrimExcess(); + InternalContacts.TrimExcess(); + InternalConstraints.TrimExcess(); + } + /// /// Contains all bodies this body is in contact with or shares a constraint with. /// diff --git a/src/Jitter2/World.cs b/src/Jitter2/World.cs index 3ed3b425..40be2aff 100644 --- a/src/Jitter2/World.cs +++ b/src/Jitter2/World.cs @@ -470,6 +470,62 @@ public void Clear() } + /// + /// Reduces excess capacity in internal world storage. + /// + /// + /// This method trims world-level sets, broadphase scratch storage, island pools, deterministic + /// solver scratch containers, and per-body/per-island collection backing storage. + /// + /// + /// Existing bodies, constraints, contacts, and arbiters remain valid. Unmanaged simulation + /// buffers are not compacted by this method. + /// + /// + /// + /// Treat trimming as an exclusive maintenance operation. Do not call it concurrently with + /// , contact registration, body/constraint/shape changes, or broadphase queries. + /// + /// + /// Thrown if this world has been disposed. + public void Trim() + { + ThrowIfDisposed(); + + bodies.Trim(); + islands.Trim(); + + deferredArbiters.Trim(); + brokenArbiters.Trim(); + + arbiters.TrimExcess(); + + foreach (Island island in islandPool) + { + island.TrimLists(); + } + + islandPool.TrimExcess(); + + DynamicTree.Trim(); + + handleToIsland.TrimExcess(); + islandRanges.TrimExcess(); + sortedContacts.TrimExcess(); + sortedSmallConstraints.TrimExcess(); + sortedConstraints.TrimExcess(); + + foreach (RigidBody body in bodies) + { + body.TrimLists(); + } + + foreach (Island island in islands) + { + island.TrimLists(); + } + } + /// /// Removes the specified body from the world. This operation also automatically discards any associated contacts /// and constraints. diff --git a/src/JitterTests/Api/SequentialTests.cs b/src/JitterTests/Api/SequentialTests.cs index 403c1721..1abfcd4e 100644 --- a/src/JitterTests/Api/SequentialTests.cs +++ b/src/JitterTests/Api/SequentialTests.cs @@ -87,3 +87,46 @@ public static void AddRemoveTest() Assert.That(elements, Does.Contain(num2)); } } + +public class TrimTests +{ + [TestCase] + public static void PartitionedSet_TrimPreservesElementsAndInitialCapacityFloor() + { + var set = new PartitionedSet(4); + var entries = Enumerable.Range(0, 10).Select(i => new SequentialTests.Number(i)).ToArray(); + + foreach (var entry in entries) + { + set.Add(entry, entry.Value % 2 == 0); + } + + Assert.That(set.Capacity, Is.EqualTo(16)); + + for (int i = entries.Length - 1; i >= 5; i--) + { + set.Remove(entries[i]); + } + + Assert.That(set.Trim(), Is.True); + Assert.That(set.Capacity, Is.EqualTo(8)); + Assert.That(set.Count, Is.EqualTo(5)); + Assert.That(set.ActiveCount, Is.EqualTo(3)); + Assert.That(set.Elements.ToArray(), Is.EquivalentTo(entries[..5])); + + foreach (var entry in entries[..5]) + { + Assert.That(set.Contains(entry), Is.True); + } + + for (int i = 4; i >= 0; i--) + { + set.Remove(entries[i]); + } + + Assert.That(set.Trim(), Is.True); + Assert.That(set.Capacity, Is.EqualTo(4)); + Assert.That(set.Trim(), Is.False); + Assert.That(set.Capacity, Is.EqualTo(4)); + } +} diff --git a/src/JitterTests/Robustness/WorldTrimTests.cs b/src/JitterTests/Robustness/WorldTrimTests.cs new file mode 100644 index 00000000..64ddeb41 --- /dev/null +++ b/src/JitterTests/Robustness/WorldTrimTests.cs @@ -0,0 +1,74 @@ +using System.Reflection; +using Jitter2.Collision; +using Jitter2.DataStructures; +using Jitter2.Unmanaged; + +namespace JitterTests.Robustness; + +public class WorldTrimTests +{ + [Test] + public void Trim_ReducesWorldStorageAndKeepsRemainingObjectsValid() + { + using var world = new World(); + var bodies = new List(); + + for (int i = 0; i < 1100; i++) + { + RigidBody body = world.CreateRigidBody(); + bodies.Add(body); + } + + var rigidBodyBuffer = GetField>(world, "memRigidBodies"); + var bodySet = GetField>(world, "bodies"); + var islandSet = GetField>(world, "islands"); + + Assert.That(bodySet.Capacity, Is.GreaterThan(1024)); + Assert.That(islandSet.Capacity, Is.GreaterThan(1024)); + Assert.That(GetPartitionedBufferCapacity(rigidBodyBuffer), Is.GreaterThan(1024)); + + RigidBody body1 = bodies[0]; + RigidBody body2 = bodies[1]; + + for (int i = bodies.Count - 1; i >= 2; i--) + { + world.Remove(bodies[i]); + } + + int beforeBodySet = bodySet.Capacity; + int beforeIslandSet = islandSet.Capacity; + int beforeRigidBodies = GetPartitionedBufferCapacity(rigidBodyBuffer); + + world.Trim(); + + Assert.That(bodySet.Capacity, Is.LessThan(beforeBodySet)); + Assert.That(islandSet.Capacity, Is.LessThan(beforeIslandSet)); + Assert.That(GetPartitionedBufferCapacity(rigidBodyBuffer), Is.EqualTo(beforeRigidBodies)); + + Assert.That(bodySet.Capacity, Is.EqualTo(1024)); + Assert.That(islandSet.Capacity, Is.EqualTo(1024)); + Assert.That(world.RigidBodies.Count, Is.EqualTo(3)); + Assert.That(body1.IsValid, Is.True); + Assert.That(body2.IsValid, Is.True); + Assert.That(world.NullBody.IsValid, Is.True); + } + + [Test] + public void Trim_OnDisposedWorld_Throws() + { + var world = new World(); + world.Dispose(); + + Assert.Throws(() => world.Trim()); + } + + private static T GetField(object instance, string name) + { + return (T)instance.GetType().GetField(name, BindingFlags.Instance | BindingFlags.NonPublic)!.GetValue(instance)!; + } + + private static int GetPartitionedBufferCapacity(PartitionedBuffer buffer) where T : unmanaged + { + return GetField(buffer, "size"); + } +} From a66ebd33f168eed9b6a8ffa5498df17f158c6e03 Mon Sep 17 00:00:00 2001 From: notgiven688 Date: Mon, 14 Sep 2026 16:02:14 +0200 Subject: [PATCH 8/9] Tighten collision detection hot path --- src/Jitter2/World.Detect.cs | 45 ++++++++++++++++++++----------------- 1 file changed, 24 insertions(+), 21 deletions(-) diff --git a/src/Jitter2/World.Detect.cs b/src/Jitter2/World.Detect.cs index 19388d36..7a7a79c7 100644 --- a/src/Jitter2/World.Detect.cs +++ b/src/Jitter2/World.Detect.cs @@ -119,9 +119,10 @@ public InvalidCollisionTypeException(Type proxyA, Type proxyB) [MethodImpl(MethodImplOptions.AggressiveInlining)] private void Detect(IDynamicTreeProxy proxyA, IDynamicTreeProxy proxyB) { - if (BroadPhaseFilter != null) + IBroadPhaseFilter? broadPhaseFilter = BroadPhaseFilter; + if (broadPhaseFilter != null) { - if (!BroadPhaseFilter.Filter(proxyA, proxyB)) + if (!broadPhaseFilter.Filter(proxyA, proxyB)) { return; } @@ -137,22 +138,24 @@ private void Detect(IDynamicTreeProxy proxyA, IDynamicTreeProxy proxyB) (sA, sB) = (sB, sA); } - Debug.Assert(sA.RigidBody != sB.RigidBody); - Debug.Assert(sA.RigidBody.World == this); - Debug.Assert(sB.RigidBody.World == this); + RigidBody bodyA = sA.RigidBody; + RigidBody bodyB = sB.RigidBody; - Debug.Assert(sA.RigidBody != null); - Debug.Assert(sB.RigidBody != null); + Debug.Assert(bodyA != null); + Debug.Assert(bodyB != null); + Debug.Assert(bodyA != bodyB); + Debug.Assert(bodyA.World == this); + Debug.Assert(bodyB.World == this); - if (!sA.RigidBody.Data.IsActive && !sB.RigidBody.Data.IsActive) return; + ref RigidBodyData b1 = ref bodyA.Data; + ref RigidBodyData b2 = ref bodyB.Data; - if ((sA.RigidBody.Data.MotionType != MotionType.Dynamic) && - (sB.RigidBody.Data.MotionType != MotionType.Dynamic)) return; + if (!b1.IsActive && !b2.IsActive) return; - ref RigidBodyData b1 = ref sA.RigidBody.Data; - ref RigidBodyData b2 = ref sB.RigidBody.Data; + if (b1.MotionType != MotionType.Dynamic && b2.MotionType != MotionType.Dynamic) return; - bool speculative = sA.RigidBody.EnableSpeculativeContacts || sB.RigidBody.EnableSpeculativeContacts; + bool speculative = bodyA.EnableSpeculativeContacts || bodyB.EnableSpeculativeContacts; + INarrowPhaseFilter? narrowPhaseFilter = NarrowPhaseFilter; var colliding = NarrowPhase.MprEpa(sA, sB, b1.Orientation, b2.Orientation, b1.Position, b2.Position, out JVector pA, out JVector pB, out JVector normal, out var penetration); @@ -161,7 +164,7 @@ private void Detect(IDynamicTreeProxy proxyA, IDynamicTreeProxy proxyB) { if (!speculative) return; - JVector dv = sB.RigidBody.Velocity - sA.RigidBody.Velocity; + JVector dv = b2.Velocity - b1.Velocity; if (dv.LengthSquared() < SpeculativeVelocityThreshold * SpeculativeVelocityThreshold) return; @@ -174,23 +177,23 @@ private void Detect(IDynamicTreeProxy proxyA, IDynamicTreeProxy proxyB) penetration = normal * (pA - pB) * SpeculativeRelaxationFactor; - if (NarrowPhaseFilter != null) + if (narrowPhaseFilter != null) { - if (!NarrowPhaseFilter.Filter(sA, sB, ref pA, ref pB, ref normal, ref penetration)) + if (!narrowPhaseFilter.Filter(sA, sB, ref pA, ref pB, ref normal, ref penetration)) { return; } } - RegisterContact(sA.ShapeId, sB.ShapeId, sA.RigidBody, sB.RigidBody, + RegisterContact(sA.ShapeId, sB.ShapeId, bodyA, bodyB, pA, pB, normal, ContactData.SolveMode.Angular); return; } - if (NarrowPhaseFilter != null) + if (narrowPhaseFilter != null) { - if (!NarrowPhaseFilter.Filter(sA, sB, ref pA, ref pB, ref normal, ref penetration)) + if (!narrowPhaseFilter.Filter(sA, sB, ref pA, ref pB, ref normal, ref penetration)) { return; } @@ -201,11 +204,11 @@ private void Detect(IDynamicTreeProxy proxyA, IDynamicTreeProxy proxyB) Unsafe.SkipInit(out CollisionManifold manifold); manifold.BuildManifold(sA, sB, pA, pB, normal); - RegisterContact(sA.ShapeId, sB.ShapeId, sA.RigidBody, sB.RigidBody, normal, ref manifold); + RegisterContact(sA.ShapeId, sB.ShapeId, bodyA, bodyB, normal, ref manifold); } else { - RegisterContact(sA.ShapeId, sB.ShapeId, sA.RigidBody, sB.RigidBody, pA, pB, normal); + RegisterContact(sA.ShapeId, sB.ShapeId, bodyA, bodyB, pA, pB, normal); } } From e6d221b4bd96b9ee5284c8caab2050f1358e4430 Mon Sep 17 00:00:00 2001 From: notgiven688 Date: Mon, 14 Sep 2026 23:11:03 +0200 Subject: [PATCH 9/9] Generic clean --- .../TriangleEdgeCollisionFilter.cs | 64 +++++----- .../DynamicTree/DynamicTree.FindNearest.cs | 27 ++--- .../DynamicTree/DynamicTree.RayCast.cs | 19 +-- .../Collision/DynamicTree/DynamicTree.cs | 24 ++-- src/Jitter2/Collision/IslandHelper.cs | 4 +- .../Collision/NarrowPhase/NarrowPhase.cs | 15 ++- src/Jitter2/Collision/PairHashSet.cs | 7 +- src/Jitter2/Collision/Shapes/BoxShape.cs | 1 - .../Collision/Shapes/ConvexHullShape.cs | 7 +- src/Jitter2/Collision/Shapes/ShapeHelper.cs | 111 +++++++++++------- src/Jitter2/Collision/Shapes/TriangleMesh.cs | 7 +- src/Jitter2/DataStructures/PartitionedSet.cs | 28 +++-- .../DataStructures/ShardedDictionary.cs | 4 +- src/Jitter2/DataStructures/SlimBag.cs | 31 ++++- src/Jitter2/DataStructures/SpanHelper.cs | 2 + .../Dynamics/Constraints/BallSocket.cs | 2 +- src/Jitter2/Dynamics/Constraints/ConeLimit.cs | 2 +- .../Dynamics/Constraints/DistanceLimit.cs | 8 +- .../Dynamics/Constraints/HingeAngle.cs | 4 +- .../Dynamics/Constraints/PointOnLine.cs | 3 +- .../Dynamics/Constraints/PointOnPlane.cs | 2 +- .../Dynamics/Constraints/TwistAngle.cs | 2 +- src/Jitter2/Dynamics/Contact.cs | 4 +- src/Jitter2/Dynamics/Joints/Joint.cs | 6 +- src/Jitter2/Dynamics/RigidBody.cs | 14 +-- src/Jitter2/LinearMath/JBoundingBox.cs | 6 +- src/Jitter2/LinearMath/JMatrix.cs | 29 +++-- src/Jitter2/LinearMath/JQuaternion.cs | 24 ++-- src/Jitter2/LinearMath/JTriangle.cs | 2 +- src/Jitter2/LinearMath/JVector.cs | 20 ++-- src/Jitter2/LinearMath/MathHelper.cs | 36 +++--- src/Jitter2/Logger.cs | 4 +- src/Jitter2/Parallelization/Parallel.cs | 10 +- src/Jitter2/Parallelization/ThreadPool.cs | 14 ++- .../SoftBodies/BroadPhaseCollisionFilter.cs | 44 +++---- .../SoftBodies/DynamicTreeCollisionFilter.cs | 6 +- src/Jitter2/SoftBodies/SoftBodyTetrahedron.cs | 8 +- src/Jitter2/SoftBodies/SoftBodyTriangle.cs | 18 +-- src/Jitter2/SoftBodies/SpringConstraint.cs | 14 +-- src/Jitter2/Tracer.cs | 2 +- src/Jitter2/Unmanaged/MemoryHelper.cs | 2 +- src/Jitter2/Unmanaged/PartitionedBuffer.cs | 2 +- src/Jitter2/World.Deterministic.cs | 8 +- src/Jitter2/World.Step.cs | 23 ++-- src/Jitter2/World.cs | 16 +-- 45 files changed, 374 insertions(+), 312 deletions(-) diff --git a/src/Jitter2/Collision/CollisionFilter/TriangleEdgeCollisionFilter.cs b/src/Jitter2/Collision/CollisionFilter/TriangleEdgeCollisionFilter.cs index eae57703..7fee0476 100644 --- a/src/Jitter2/Collision/CollisionFilter/TriangleEdgeCollisionFilter.cs +++ b/src/Jitter2/Collision/CollisionFilter/TriangleEdgeCollisionFilter.cs @@ -160,7 +160,7 @@ public bool Filter(RigidBodyShape shapeA, RigidBodyShape shapeB, } EdgeContactEvaluation evaluation = EvaluateTriangleContact(triangleShape, c2, - c1 ? pointA : pointB, normal, out JVector tnormal, out JVector nnormal); + c1 ? pointA : pointB, normal, out JVector triangleNormal, out JVector neighborNormal); if (evaluation == EdgeContactEvaluation.Discard) return false; if (evaluation == EdgeContactEvaluation.Keep) return true; @@ -184,12 +184,12 @@ public bool Filter(RigidBodyShape shapeA, RigidBodyShape shapeB, if (!result) { - // this should not happen + // MPR refinement failed; reject the contact. return false; } evaluation = EvaluateTriangleContact(triangleShape, c2, - c1 ? pointA : pointB, normal, out tnormal, out nnormal); + c1 ? pointA : pointB, normal, out triangleNormal, out neighborNormal); if (evaluation == EdgeContactEvaluation.Discard) return false; if (evaluation == EdgeContactEvaluation.Keep) return true; @@ -197,24 +197,24 @@ public bool Filter(RigidBodyShape shapeA, RigidBodyShape shapeB, JVector midPoint = (Real)0.5 * (pointA + pointB); - // now the fun part + // Now the fun part. // // we have a collision close to an edge, with // - // tnormal -> the triangle normal where collision occurred - // nnormal -> the normal of neighboring triangle - // normal -> the collision normal - if (JVector.Dot(tnormal, nnormal) > cosAngle) + // triangleNormal -> the triangle normal where collision occurred + // neighborNormal -> the normal of the neighboring triangle + // normal -> the collision normal + if (JVector.Dot(triangleNormal, neighborNormal) > cosAngle) { - // tnormal and nnormal are the same - // -------------------------------- - Real f5 = JVector.Dot(normal, nnormal); - Real f6 = JVector.Dot(normal, tnormal); + // triangleNormal and neighborNormal are the same + // ---------------------------------------------- + Real f5 = JVector.Dot(normal, neighborNormal); + Real f6 = JVector.Dot(normal, triangleNormal); if (f5 > f6) { #if DEBUG_EDGEFILTER - Console.WriteLine($"case #1: adjusting; normal {normal} -> {nnormal}"); + Console.WriteLine($"case #1: adjusting; normal {normal} -> {neighborNormal}"); #endif if (!isSpeculative) @@ -223,12 +223,12 @@ public bool Filter(RigidBodyShape shapeA, RigidBodyShape shapeB, pointA = pointB = midPoint; } - normal = nnormal; + normal = neighborNormal; } else { #if DEBUG_EDGEFILTER - Console.WriteLine($"case #1: adjusting; normal {normal} -> {tnormal}"); + Console.WriteLine($"case #1: adjusting; normal {normal} -> {triangleNormal}"); #endif if (!isSpeculative) @@ -237,17 +237,17 @@ public bool Filter(RigidBodyShape shapeA, RigidBodyShape shapeB, pointA = pointB = midPoint; } - normal = tnormal; + normal = triangleNormal; } return true; } - // nnormal and tnormal are different - // ---------------------------------- + // neighborNormal and triangleNormal are different + // ----------------------------------------------- - // 1st step, project the normal onto the plane given by tnormal and nnormal + // 1st step, project the normal onto the plane given by triangleNormal and neighborNormal // by removing the component along the cross product axis - JVector cross = nnormal % tnormal; + JVector cross = neighborNormal % triangleNormal; Real crossLenSq = cross.LengthSquared(); JVector proj = normal - (cross * normal / crossLenSq) * cross; @@ -257,34 +257,34 @@ public bool Filter(RigidBodyShape shapeA, RigidBodyShape shapeB, Console.WriteLine($"case #3: discarding"); #endif - // can not project onto the plane, discard + // Cannot project onto the plane, discard. return false; } - // 2nd step, determine if "proj" is between nnormal and tnormal + // 2nd step, determine if "proj" is between neighborNormal and triangleNormal // - // / nnormal + // / neighborNormal // / // / // ----- proj // \ // \ - // \ tnormal - Real f1 = proj % nnormal * cross; - Real f2 = proj % tnormal * cross; + // \ triangleNormal + Real f1 = proj % neighborNormal * cross; + Real f2 = proj % triangleNormal * cross; bool between = f1 <= (Real)0.0 && f2 >= (Real)0.0; if (!between) { // not in-between, snap normal - Real f3 = JVector.Dot(normal, nnormal); - Real f4 = JVector.Dot(normal, tnormal); + Real f3 = JVector.Dot(normal, neighborNormal); + Real f4 = JVector.Dot(normal, triangleNormal); if (f3 > f4) { #if DEBUG_EDGEFILTER - Console.WriteLine($"case #2: adjusting; normal {normal} -> {nnormal}"); + Console.WriteLine($"case #2: adjusting; normal {normal} -> {neighborNormal}"); #endif if (!isSpeculative) @@ -293,12 +293,12 @@ public bool Filter(RigidBodyShape shapeA, RigidBodyShape shapeB, pointA = pointB = midPoint; } - normal = nnormal; + normal = neighborNormal; } else { #if DEBUG_EDGEFILTER - Console.WriteLine($"case #2: adjusting; normal {normal} -> {tnormal}"); + Console.WriteLine($"case #2: adjusting; normal {normal} -> {triangleNormal}"); #endif if (!isSpeculative) { @@ -306,7 +306,7 @@ public bool Filter(RigidBodyShape shapeA, RigidBodyShape shapeB, pointA = pointB = midPoint; } - normal = tnormal; + normal = triangleNormal; } } diff --git a/src/Jitter2/Collision/DynamicTree/DynamicTree.FindNearest.cs b/src/Jitter2/Collision/DynamicTree/DynamicTree.FindNearest.cs index 161847f7..a21ecc74 100644 --- a/src/Jitter2/Collision/DynamicTree/DynamicTree.FindNearest.cs +++ b/src/Jitter2/Collision/DynamicTree/DynamicTree.FindNearest.cs @@ -95,7 +95,8 @@ public bool FindNearestSphere(Real radius, in JVector position, Real maxDistance private struct DistanceQuery(in JBoundingBox box, in JQuaternion orientation, in JVector position) { - public readonly JBoundingBox Box = box; + public readonly JVector BoxExtents = (box.Max - box.Min) * (Real)0.5; + public readonly JVector BoxCenter = (box.Max + box.Min) * (Real)0.5; public readonly JQuaternion Orientation = orientation; public readonly JVector Position = position; @@ -181,14 +182,11 @@ public bool FindNearest(in T support, in JQuaternion orientation, in JVector // Returns the minimum distance between the query AABB and a tree node's expanded AABB. // Uses the Minkowski sum: expand the target by the query half-extents, then measure // point-to-AABB distance from the query center. Returns 0 if the AABBs overlap. - private static Real MinDistBox(in JBoundingBox queryBox, in TreeBox targetBox) + private static Real MinDistBox(in JVector queryExtents, in JVector queryCenter, in TreeBox targetBox) { - JVector extents = (queryBox.Max - queryBox.Min) * (Real)0.5; - JVector center = (queryBox.Max + queryBox.Min) * (Real)0.5; - - Real dx = MathR.Max(MathR.Max(targetBox.Min.X - extents.X - center.X, center.X - targetBox.Max.X - extents.X), (Real)0.0); - Real dy = MathR.Max(MathR.Max(targetBox.Min.Y - extents.Y - center.Y, center.Y - targetBox.Max.Y - extents.Y), (Real)0.0); - Real dz = MathR.Max(MathR.Max(targetBox.Min.Z - extents.Z - center.Z, center.Z - targetBox.Max.Z - extents.Z), (Real)0.0); + Real dx = MathR.Max(MathR.Max(targetBox.Min.X - queryExtents.X - queryCenter.X, queryCenter.X - targetBox.Max.X - queryExtents.X), (Real)0.0); + Real dy = MathR.Max(MathR.Max(targetBox.Min.Y - queryExtents.Y - queryCenter.Y, queryCenter.Y - targetBox.Max.Y - queryExtents.Y), (Real)0.0); + Real dz = MathR.Max(MathR.Max(targetBox.Min.Z - queryExtents.Z - queryCenter.Z, queryCenter.Z - targetBox.Max.Z - queryExtents.Z), (Real)0.0); return MathR.Sqrt(dx * dx + dy * dy + dz * dz); } @@ -217,14 +215,15 @@ private bool QueryDistance(in T support, in DistanceQuery query, out FindNear if (node.IsLeaf) { - if (node.Proxy is not IDistanceTestable distCastable) continue; - if (query.FilterPre != null && !query.FilterPre(node.Proxy!)) continue; + IDynamicTreeProxy proxy = node.Proxy!; + if (proxy is not IDistanceTestable distanceTestable) continue; + if (query.FilterPre != null && !query.FilterPre(proxy)) continue; Unsafe.SkipInit(out FindNearestResult res); - bool separated = distCastable.Distance(support, + bool separated = distanceTestable.Distance(support, query.Orientation, query.Position, out res.PointA, out res.PointB, out res.Normal, out res.Distance); - res.Entity = node.Proxy; + res.Entity = proxy; if (!separated) { @@ -246,8 +245,8 @@ private bool QueryDistance(in T support, in DistanceQuery query, out FindNear ref Node leftNode = ref nodes[node.Left]; ref Node rightNode = ref nodes[node.Right]; - Real leftDist = MinDistBox(query.Box, leftNode.ExpandedBox); - Real rightDist = MinDistBox(query.Box, rightNode.ExpandedBox); + Real leftDist = MinDistBox(query.BoxExtents, query.BoxCenter, leftNode.ExpandedBox); + Real rightDist = MinDistBox(query.BoxExtents, query.BoxCenter, rightNode.ExpandedBox); bool leftHit = leftDist <= result.Distance; bool rightHit = rightDist <= result.Distance; diff --git a/src/Jitter2/Collision/DynamicTree/DynamicTree.RayCast.cs b/src/Jitter2/Collision/DynamicTree/DynamicTree.RayCast.cs index f1750d48..d12594e0 100644 --- a/src/Jitter2/Collision/DynamicTree/DynamicTree.RayCast.cs +++ b/src/Jitter2/Collision/DynamicTree/DynamicTree.RayCast.cs @@ -60,8 +60,8 @@ private struct Ray(in JVector origin, in JVector direction) /// /// Origin of the ray. /// Direction of the ray. Does not have to be normalized. - /// Optional pre-filter which allows to skip shapes in the detection. - /// Optional post-filter which allows to skip detections. + /// Optional pre-filter that can skip candidate shapes. + /// Optional post-filter that can skip candidate results. /// The shape which was hit. /// /// The surface normal at the hit point. if the ray does not hit, @@ -107,7 +107,7 @@ private bool QueryRay(in Ray ray, out RayCastResult result) { result = new RayCastResult(); - if (root == -1) + if (root == NullNode) { return false; } @@ -122,19 +122,20 @@ private bool QueryRay(in Ray ray, out RayCastResult result) while (stack.Count > baseCount) { - int pop = stack.Pop(); + int index = stack.Pop(); - ref Node node = ref nodes[pop]; + ref Node node = ref nodes[index]; if (node.IsLeaf) { - if (node.Proxy is not IRayCastable irc) continue; + IDynamicTreeProxy proxy = node.Proxy!; + if (proxy is not IRayCastable rayCastable) continue; - if (ray.FilterPre != null && !ray.FilterPre(node.Proxy)) continue; + if (ray.FilterPre != null && !ray.FilterPre(proxy)) continue; Unsafe.SkipInit(out RayCastResult res); - bool hit = irc.RayCast(ray.Origin, ray.Direction, out res.Normal, out res.Lambda); - res.Entity = node.Proxy; + bool hit = rayCastable.RayCast(ray.Origin, ray.Direction, out res.Normal, out res.Lambda); + res.Entity = proxy; if (hit && res.Lambda < result.Lambda) { diff --git a/src/Jitter2/Collision/DynamicTree/DynamicTree.cs b/src/Jitter2/Collision/DynamicTree/DynamicTree.cs index a4706863..12019935 100644 --- a/src/Jitter2/Collision/DynamicTree/DynamicTree.cs +++ b/src/Jitter2/Collision/DynamicTree/DynamicTree.cs @@ -244,8 +244,8 @@ private void EnumerateOverlapsCallback(OverlapEnumerationParam parameter) var proxyA = nodes[node.ID1].Proxy; var proxyB = nodes[node.ID2].Proxy; - if(proxyA == null || proxyB == null) continue; - if(!Filter(proxyA, proxyB)) continue; + if (proxyA == null || proxyB == null) continue; + if (!Filter(proxyA, proxyB)) continue; if (!JBoundingBox.Disjoint(proxyA.WorldBoundingBox, proxyB.WorldBoundingBox)) { @@ -280,7 +280,7 @@ public void EnumerateOverlaps(Action actio { Parallel.GetBounds(slotsLength, taskCount, i, out int start, out int end); overlapEnumerationParam.Batch = new Parallel.Batch(start, end); - ThreadPool.Instance.AddTask(enumerateOverlaps, overlapEnumerationParam); + tpi.AddTask(enumerateOverlaps, overlapEnumerationParam); } tpi.Execute(); @@ -536,8 +536,8 @@ private void EnumerateTreeBoxes(ref Node node, Action action, int private uint stepper; /// - /// Removes entries from the internal bookkeeping which are both marked as inactive or - /// whose expanded bounding box do not overlap any longer. + /// Removes entries from the internal bookkeeping when both proxies are inactive or + /// their expanded bounding boxes no longer overlap. /// Only searches a small subset of all elements per call to reduce overhead. /// private void PruneInvalidPairs() @@ -916,7 +916,7 @@ private void InternalAddRemoveProxy(IDynamicTreeProxy proxy) // InsertLeaf takes 'where' as a hint, i.e. it still walks up the tree until // the new node is fully contained. Note: The insertion node could also be found when searching - // from the root, since the search always descents into child nodes fully containing the new node. + // from the root, since the search always descends into child nodes fully containing the new node. InsertLeaf(index, parent); } @@ -1196,7 +1196,7 @@ private int FindBestHeuristic(int node, int where) while (!nodes[where].IsLeaf) { int left = nodes[where].Left; - int rght = nodes[where].Right; + int right = nodes[where].Right; double cost = 2.0d * nodes[where].ExpandedBox.GetSurfaceArea(); @@ -1215,20 +1215,20 @@ private int FindBestHeuristic(int node, int where) leftCost = newArea - oldArea; } - if (nodes[rght].IsLeaf) + if (nodes[right].IsLeaf) { - rightCost = TreeBox.MergedSurface(nodes[rght].ExpandedBox, nodeTreeBox); + rightCost = TreeBox.MergedSurface(nodes[right].ExpandedBox, nodeTreeBox); } else { - double oldArea = nodes[rght].ExpandedBox.GetSurfaceArea(); - double newArea = TreeBox.MergedSurface(nodes[rght].ExpandedBox, nodeTreeBox); + double oldArea = nodes[right].ExpandedBox.GetSurfaceArea(); + double newArea = TreeBox.MergedSurface(nodes[right].ExpandedBox, nodeTreeBox); rightCost = newArea - oldArea; } if (cost < leftCost && cost < rightCost) break; - where = leftCost < rightCost ? left : rght; + where = leftCost < rightCost ? left : right; } return where; diff --git a/src/Jitter2/Collision/IslandHelper.cs b/src/Jitter2/Collision/IslandHelper.cs index 7999b0e7..ff0c5848 100644 --- a/src/Jitter2/Collision/IslandHelper.cs +++ b/src/Jitter2/Collision/IslandHelper.cs @@ -114,8 +114,8 @@ public static void AddConnection(IslandSet islands, Stack islandPool, Ri if (needsUpdate) { - if(body1.Data.MotionType != MotionType.Static) body1.Island.NeedsUpdate = true; - if(body2.Data.MotionType != MotionType.Static) body2.Island.NeedsUpdate = true; + if (body1.Data.MotionType != MotionType.Static) body1.Island.NeedsUpdate = true; + if (body2.Data.MotionType != MotionType.Static) body2.Island.NeedsUpdate = true; } } diff --git a/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs b/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs index 46cc8c9a..7a2c8d78 100644 --- a/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs +++ b/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs @@ -403,7 +403,7 @@ public bool Collision(in Ta supportA, in Tb supportB, in JQuaternion orie point1 = point2 = normal = JVector.Zero; penetration = (Real)0.0; - Logger.Warning("{0}: EPA, Could not converge within {1} iterations.\"", nameof(NarrowPhase), maxIter); + Logger.Warning("{0}: EPA, Could not converge within {1} iterations.", nameof(NarrowPhase), maxIter); return false; @@ -948,8 +948,7 @@ public static bool MprEpa(in Ta supportA, in Tb supportB, Real epaThreshold = EpaPenetrationThreshold) where Ta : ISupportMappable where Tb : ISupportMappable { - // ..perform collision detection.. - return _solver.SolveMpr(supportA, supportB, orientationB, positionB , epaThreshold, out pointA, out pointB, out normal, out penetration); + return _solver.SolveMpr(supportA, supportB, orientationB, positionB, epaThreshold, out pointA, out pointB, out normal, out penetration); } /// @@ -1025,16 +1024,16 @@ public static bool Sweep(in Ta supportA, in Tb supportB, Real sweepLinearProj = JVector.Dot(normal, sweepA - sweepB); Real sweepLen = sweepLinearProj + maxAngularSpeed; - if(sweepLen < NumericEpsilon || (sweepLinearProj < 0 && distance > combinedRadius)) + if (sweepLen < NumericEpsilon || (sweepLinearProj < 0 && distance > combinedRadius)) { normal = JVector.Zero; lambda = Real.PositiveInfinity; return false; } - Real tmpLambda = distance / sweepLen; + Real lambdaStep = distance / sweepLen; - lambda += tmpLambda; + lambda += lambdaStep; Debug.Assert(!Real.IsNaN(lambda)); @@ -1049,13 +1048,13 @@ public static bool Sweep(in Ta supportA, in Tb supportB, if (iter++ > maxIter) break; - bool res = Distance(supportA, supportB, oriA, oriB, posA, posB, out pointA, out pointB, out JVector nn, out distance); + bool hasDistance = Distance(supportA, supportB, oriA, oriB, posA, posB, out pointA, out pointB, out JVector newNormal, out distance); // We are a bit in a pickle here. // If the advanced shapes are slightly overlapping (Distance returns false; this can either happen if the // simplex solver encompasses the origin or the closest point on the simplex is close enough to the origin), // we have valid posA and posB information, but the normal is not well-defined. So we keep the old normal. - if(res) normal = nn; + if (hasDistance) normal = newNormal; if (distance < collideEpsilon) break; diff --git a/src/Jitter2/Collision/PairHashSet.cs b/src/Jitter2/Collision/PairHashSet.cs index ca086360..7c250801 100644 --- a/src/Jitter2/Collision/PairHashSet.cs +++ b/src/Jitter2/Collision/PairHashSet.cs @@ -396,11 +396,16 @@ public bool Remove(Pair pair) } /// - public IEnumerator GetEnumerator() + public Enumerator GetEnumerator() { return new Enumerator(this); } + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + IEnumerator IEnumerable.GetEnumerator() { return GetEnumerator(); diff --git a/src/Jitter2/Collision/Shapes/BoxShape.cs b/src/Jitter2/Collision/Shapes/BoxShape.cs index 9ff19f23..0621a34b 100644 --- a/src/Jitter2/Collision/Shapes/BoxShape.cs +++ b/src/Jitter2/Collision/Shapes/BoxShape.cs @@ -157,7 +157,6 @@ public override bool LocalRayCast(in JVector origin, in JVector direction, out J lambda = t0; normal = direction.Z < (Real)0.0 ? JVector.UnitZ : -JVector.UnitZ; } - //if (t1 < exit) exit = t1; } else if (origin.Z < min.Z || origin.Z > max.Z) { diff --git a/src/Jitter2/Collision/Shapes/ConvexHullShape.cs b/src/Jitter2/Collision/Shapes/ConvexHullShape.cs index 9ae6d7a5..6c5e7d0a 100644 --- a/src/Jitter2/Collision/Shapes/ConvexHullShape.cs +++ b/src/Jitter2/Collision/Shapes/ConvexHullShape.cs @@ -76,8 +76,9 @@ public ConvexHullShape(ReadOnlySpan triangles) throw new ArgumentException("Triangle set must contain at least one triangle.", nameof(triangles)); } - Dictionary tmpIndices = new(); - List tmpVertices = []; + int maxVertexCount = triangles.Length * 3; + Dictionary tmpIndices = new(maxVertexCount); + List tmpVertices = new(maxVertexCount); ushort PushVector(CHullVector v) { @@ -165,7 +166,7 @@ private ConvexHullShape() /// Helper to sort and add unique elements from source to destination. /// Replaces LINQ Distinct() for better performance and zero allocations. /// - private static void AddDistinct(List source, List destination) + private static void AddDistinct(List? source, List destination) { if (source == null || source.Count == 0) return; diff --git a/src/Jitter2/Collision/Shapes/ShapeHelper.cs b/src/Jitter2/Collision/Shapes/ShapeHelper.cs index 88919be3..88145a73 100644 --- a/src/Jitter2/Collision/Shapes/ShapeHelper.cs +++ b/src/Jitter2/Collision/Shapes/ShapeHelper.cs @@ -17,6 +17,11 @@ namespace Jitter2.Collision.Shapes; public static class ShapeHelper { private const Real GoldenRatio = (Real)1.6180339887498948482045; + private static readonly JMatrix canonicalTetrahedronInertia = new( + (Real)(1.0 / 60.0), (Real)(1.0 / 120.0), (Real)(1.0 / 120.0), + (Real)(1.0 / 120.0), (Real)(1.0 / 60.0), (Real)(1.0 / 120.0), + (Real)(1.0 / 120.0), (Real)(1.0 / 120.0), (Real)(1.0 / 60.0)); + private static readonly JVector[] icosahedronVertices = [ new(0, +1, +GoldenRatio), new(0, -1, +GoldenRatio), new(0, +1, -GoldenRatio), new(0, -1, -GoldenRatio), @@ -31,6 +36,18 @@ public static class ShapeHelper { 4, 8, 9 }, { 8, 5, 9 }, { 10, 6, 11 }, { 7, 10, 11 } }; + private static int GetTessellationTriangleCapacity(int subdivisions) + { + int capacity = 20; + + for (int i = 1; i < subdivisions; i++) + { + capacity = checked(capacity * 4); + } + + return capacity; + } + /// /// A collection to which the triangles are added. public static void Tessellate(in TSupport support, TCollection hullCollection, int subdivisions = 3) @@ -87,9 +104,9 @@ private static void Subdivide(in TSupport support, ref TSink hu return; } - // There is a re-project onto the sphere missing here and here. - // The quality of the points does not suffer that badly from it, and - // we get rid of many, many normalize-calls. So we keep it like this. + // Deliberately skip re-projecting the midpoint directions onto the sphere. + // The resulting samples are still good enough for this approximation, and + // avoiding those normalization calls keeps tessellation inexpensive. JVector h1 = (v1 + v2) * (Real)0.5; JVector h2 = (v2 + v3) * (Real)0.5; JVector h3 = (v3 + v1) * (Real)0.5; @@ -118,7 +135,7 @@ private static void Subdivide(in TSupport support, ref TSink hu public static List Tessellate(in TSupport support, int subdivisions = 3) where TSupport : ISupportMappable { - List triangles = new(); + List triangles = new(GetTessellationTriangleCapacity(subdivisions)); Tessellate(in support, triangles, subdivisions); return triangles; } @@ -183,23 +200,27 @@ public static void CalculateBoundingBox(in TSupport support, { JMatrix oriT = JMatrix.Transpose(JMatrix.CreateFromQuaternion(orientation)); - support.SupportMap(oriT.GetColumn(0), out JVector res); - box.Max.X = JVector.Dot(oriT.GetColumn(0), res); + JVector axisX = oriT.GetColumn(0); + JVector axisY = oriT.GetColumn(1); + JVector axisZ = oriT.GetColumn(2); + + support.SupportMap(axisX, out JVector res); + box.Max.X = JVector.Dot(axisX, res); - support.SupportMap(oriT.GetColumn(1), out res); - box.Max.Y = JVector.Dot(oriT.GetColumn(1), res); + support.SupportMap(axisY, out res); + box.Max.Y = JVector.Dot(axisY, res); - support.SupportMap(oriT.GetColumn(2), out res); - box.Max.Z = JVector.Dot(oriT.GetColumn(2), res); + support.SupportMap(axisZ, out res); + box.Max.Z = JVector.Dot(axisZ, res); - support.SupportMap(-oriT.GetColumn(0), out res); - box.Min.X = JVector.Dot(oriT.GetColumn(0), res); + support.SupportMap(-axisX, out res); + box.Min.X = JVector.Dot(axisX, res); - support.SupportMap(-oriT.GetColumn(1), out res); - box.Min.Y = JVector.Dot(oriT.GetColumn(1), res); + support.SupportMap(-axisY, out res); + box.Min.Y = JVector.Dot(axisY, res); - support.SupportMap(-oriT.GetColumn(2), out res); - box.Min.Z = JVector.Dot(oriT.GetColumn(2), res); + support.SupportMap(-axisZ, out res); + box.Min.Z = JVector.Dot(axisZ, res); JVector.Add(box.Min, position, out box.Min); JVector.Add(box.Max, position, out box.Max); @@ -262,7 +283,8 @@ public static List SampleHull(IEnumerable vertices, int subdiv public static List SampleHull(in TSupport support, int subdivisions = 3) where TSupport : ISupportMappable { - Stack<(JTriangle triangle, int depth)> stack = new(); + int triangleCapacity = GetTessellationTriangleCapacity(subdivisions); + Stack<(JTriangle triangle, int depth)> stack = new(triangleCapacity); for (int i = 0; i < 20; i++) { @@ -272,7 +294,7 @@ public static List SampleHull(in TSupport support, int subdiv stack.Push((new JTriangle(v1, v2, v3), subdivisions)); } - HashSet hull = new(); + HashSet hull = new(triangleCapacity); while (stack.Count > 0) { @@ -300,6 +322,29 @@ public static List SampleHull(in TSupport support, int subdiv return new List(hull); } + private struct MassInertiaSink : ISink + { + public JMatrix Inertia; + public JVector CenterOfMass; + public Real Mass; + + public void Add(in JTriangle triangle) + { + JMatrix transformation = JMatrix.FromColumns(triangle.V0, triangle.V1, triangle.V2); + Real detA = transformation.Determinant(); + + JMatrix tetrahedronInertia = + JMatrix.Multiply(transformation * canonicalTetrahedronInertia * JMatrix.Transpose(transformation), detA); + + JVector tetrahedronCom = (Real)(1.0 / 4.0) * (triangle.V0 + triangle.V1 + triangle.V2); + Real tetrahedronMass = (Real)(1.0 / 6.0) * detA; + + Inertia += tetrahedronInertia; + CenterOfMass += tetrahedronMass * tetrahedronCom; + Mass += tetrahedronMass; + } + } + /// /// Calculates the mass properties of an implicitly defined shape, assuming unit mass density. /// @@ -324,31 +369,11 @@ public static void CalculateMassInertia(in TSupport support, out JMatr out Real mass, int subdivisions = 4) where TSupport : ISupportMappable { - centerOfMass = JVector.Zero; - inertia = JMatrix.Zero; - mass = 0; - - const Real a = (Real)(1.0 / 60.0), b = (Real)(1.0 / 120.0); - JMatrix canonicalInertia = new(a, b, b, b, a, b, b, b, a); - - foreach (JTriangle triangle in Tessellate(in support, subdivisions)) - { - JMatrix transformation = JMatrix.FromColumns(triangle.V0, triangle.V1, triangle.V2); - Real detA = transformation.Determinant(); - - // now transform this canonical tetrahedron to the target tetrahedron - // inertia by a linear transformation A - JMatrix tetrahedronInertia = JMatrix.Multiply(transformation * canonicalInertia * JMatrix.Transpose(transformation), detA); - - JVector tetrahedronCom = (Real)(1.0 / 4.0) * (triangle.V0 + triangle.V1 + triangle.V2); - Real tetrahedronMass = (Real)(1.0 / 6.0) * detA; - - inertia += tetrahedronInertia; - centerOfMass += tetrahedronMass * tetrahedronCom; - mass += tetrahedronMass; - } + MassInertiaSink sink = default; + Tessellate(in support, ref sink, subdivisions); - inertia = JMatrix.Multiply(JMatrix.Identity, inertia.Trace()) - inertia; - centerOfMass *= (Real)1.0 / mass; + inertia = JMatrix.Multiply(JMatrix.Identity, sink.Inertia.Trace()) - sink.Inertia; + centerOfMass = sink.CenterOfMass * ((Real)1.0 / sink.Mass); + mass = sink.Mass; } } diff --git a/src/Jitter2/Collision/Shapes/TriangleMesh.cs b/src/Jitter2/Collision/Shapes/TriangleMesh.cs index b26d5bea..9727c49a 100644 --- a/src/Jitter2/Collision/Shapes/TriangleMesh.cs +++ b/src/Jitter2/Collision/Shapes/TriangleMesh.cs @@ -189,8 +189,9 @@ private static ReadOnlySpan CastVertices(ReadOnlySpan private void BuildFromSoup(ReadOnlySpan triangles, bool ignoreDegenerated) { - var vertexMap = new Dictionary(); - var vertexList = new List(); + int maxVertexCount = triangles.Length * 3; + var vertexMap = new Dictionary(maxVertexCount); + var vertexList = new List(maxVertexCount); var triangleList = new List(triangles.Length); // Helper to deduplicate vertices @@ -294,7 +295,7 @@ private void BuildFromIndexed(ReadOnlySpan vertices, ReadOnlySpan private void AssignNeighbors() { - var edgeToTriangle = new Dictionary(); + var edgeToTriangle = new Dictionary(indices.Length * 3); for (int i = 0; i < indices.Length; i++) { diff --git a/src/Jitter2/DataStructures/PartitionedSet.cs b/src/Jitter2/DataStructures/PartitionedSet.cs index 8203024f..35c5e48d 100644 --- a/src/Jitter2/DataStructures/PartitionedSet.cs +++ b/src/Jitter2/DataStructures/PartitionedSet.cs @@ -240,8 +240,9 @@ public bool MoveToActive(T element) Debug.Assert(element.SetIndex != -1); Debug.Assert(elements[element.SetIndex] == element); - if (element.SetIndex < ActiveCount) return false; - Swap(ActiveCount, element.SetIndex); + int index = element.SetIndex; + if (index < ActiveCount) return false; + if (index != ActiveCount) Swap(ActiveCount, index); ActiveCount += 1; return true; } @@ -256,9 +257,10 @@ public bool MoveToInactive(T element) Debug.Assert(element.SetIndex != -1); Debug.Assert(elements[element.SetIndex] == element); - if (element.SetIndex >= ActiveCount) return false; + int index = element.SetIndex; + if (index >= ActiveCount) return false; ActiveCount -= 1; - Swap(ActiveCount, element.SetIndex); + if (index != ActiveCount) Swap(ActiveCount, index); return true; } @@ -269,7 +271,7 @@ public bool MoveToInactive(T element) /// if the element is found; otherwise, . public bool Contains(T element) { - if(element.SetIndex >= Count || element.SetIndex < 0) return false; + if (element.SetIndex >= Count || element.SetIndex < 0) return false; return (elements[element.SetIndex] == element); } @@ -284,13 +286,19 @@ public void Remove(T element) MoveToInactive(element); - int li = element.SetIndex; + int index = element.SetIndex; + int lastIndex = Count - 1; - Count -= 1; + Count = lastIndex; - elements[li] = elements[Count]; - elements[li].SetIndex = li; - elements[Count] = null!; + if (index != lastIndex) + { + T moved = elements[lastIndex]; + elements[index] = moved; + moved.SetIndex = index; + } + + elements[lastIndex] = null!; element.SetIndex = -1; } diff --git a/src/Jitter2/DataStructures/ShardedDictionary.cs b/src/Jitter2/DataStructures/ShardedDictionary.cs index fa440781..9f0e782d 100644 --- a/src/Jitter2/DataStructures/ShardedDictionary.cs +++ b/src/Jitter2/DataStructures/ShardedDictionary.cs @@ -17,8 +17,8 @@ namespace Jitter2.DataStructures; /// -/// A thread-safe dictionary that partitions entries across multiple shards to reduce lock contention. -/// Each shard has its own lock, allowing concurrent access to different shards. +/// A sharded dictionary whose entries are partitioned to reduce lock contention. +/// Each shard has its own lock, allowing callers to synchronize different shards independently. /// /// The type of keys in the dictionary. /// The type of values in the dictionary. diff --git a/src/Jitter2/DataStructures/SlimBag.cs b/src/Jitter2/DataStructures/SlimBag.cs index dd265a6a..37ddc550 100644 --- a/src/Jitter2/DataStructures/SlimBag.cs +++ b/src/Jitter2/DataStructures/SlimBag.cs @@ -118,6 +118,17 @@ public Span AsSpan() /// The collection of elements to add. public void AddRange(IEnumerable list) { + ArgumentNullException.ThrowIfNull(list); + + if (list is ICollection collection) + { + EnsureCapacity(checked(counter + collection.Count)); + } + else if (list is IReadOnlyCollection readOnlyCollection) + { + EnsureCapacity(checked(counter + readOnlyCollection.Count)); + } + foreach (T elem in list) Add(elem); } @@ -127,12 +138,23 @@ public void AddRange(IEnumerable list) /// The element to add. public void Add(T item) { - if (counter == array.Length) + EnsureCapacity(counter + 1); + + array[counter++] = item; + } + + private void EnsureCapacity(int capacity) + { + if (capacity <= array.Length) return; + + int newLength = array.Length; + do { - Array.Resize(ref array, array.Length * 2); + newLength = checked(newLength * 2); } + while (newLength < capacity); - array[counter++] = item; + Array.Resize(ref array, newLength); } private readonly Jitter2.Parallelization.ReaderWriterLock rwLock = new(); @@ -194,7 +216,8 @@ public void Remove(T item) /// The zero-based index of the element to remove. public void RemoveAt(int index) { - array[index] = array[--counter]; + counter -= 1; + if (index != counter) array[index] = array[counter]; } /// diff --git a/src/Jitter2/DataStructures/SpanHelper.cs b/src/Jitter2/DataStructures/SpanHelper.cs index 6e745d17..a1ab77b9 100644 --- a/src/Jitter2/DataStructures/SpanHelper.cs +++ b/src/Jitter2/DataStructures/SpanHelper.cs @@ -30,6 +30,8 @@ internal static class SpanHelper /// public static ReadOnlySpan AsReadOnlySpan(IEnumerable elements, out T[]? backingArray) where T : struct { + ArgumentNullException.ThrowIfNull(elements); + backingArray = null; switch (elements) diff --git a/src/Jitter2/Dynamics/Constraints/BallSocket.cs b/src/Jitter2/Dynamics/Constraints/BallSocket.cs index ede5616d..2c70db2f 100644 --- a/src/Jitter2/Dynamics/Constraints/BallSocket.cs +++ b/src/Jitter2/Dynamics/Constraints/BallSocket.cs @@ -180,7 +180,7 @@ public static void PrepareForIterationBallSocket(ref ConstraintData constraint, /// Gets or sets the softness (compliance) of the constraint. /// /// - /// Default is 0. Higher values allow more positional error but improve stability. + /// Default is . Higher values allow more positional error but improve stability. /// Scaled by inverse timestep during solving. /// public Real Softness diff --git a/src/Jitter2/Dynamics/Constraints/ConeLimit.cs b/src/Jitter2/Dynamics/Constraints/ConeLimit.cs index 3b25690f..ad148283 100644 --- a/src/Jitter2/Dynamics/Constraints/ConeLimit.cs +++ b/src/Jitter2/Dynamics/Constraints/ConeLimit.cs @@ -125,7 +125,7 @@ public void Initialize(JVector axis, AngularLimit limit) if (limit.From > (JAngle)0.0) { Logger.Warning( - "{0}.{1}(): The lower limit is larger 0 but this overload initializes both body axes " + + "{0}.{1}(): The lower limit is greater than 0, but this overload initializes both body axes " + "from the same world-space axis (rest angle = 0). Use the two-axis overload " + "if you need a non-zero minimum angle.", nameof(ConeLimit), diff --git a/src/Jitter2/Dynamics/Constraints/DistanceLimit.cs b/src/Jitter2/Dynamics/Constraints/DistanceLimit.cs index 4c587ffe..9acc9c01 100644 --- a/src/Jitter2/Dynamics/Constraints/DistanceLimit.cs +++ b/src/Jitter2/Dynamics/Constraints/DistanceLimit.cs @@ -49,7 +49,7 @@ public struct DistanceLimitData } private static readonly uint RegisteredDispatchId = - RegisterFullConstraint(&PrepareForIterationFixedAngle, &IterateFixedAngle); + RegisterFullConstraint(&PrepareForIterationDistanceLimit, &IterateDistanceLimit); protected override void Create() @@ -204,7 +204,7 @@ public Real Distance } } - public static void PrepareForIterationFixedAngle(ref ConstraintData constraint, Real idt) + public static void PrepareForIterationDistanceLimit(ref ConstraintData constraint, Real idt) { ref var data = ref Unsafe.As(ref constraint); ref RigidBodyData body1 = ref data.Body1.Data; @@ -270,7 +270,7 @@ public static void PrepareForIterationFixedAngle(ref ConstraintData constraint, /// Gets or sets the softness (compliance) of the constraint. /// /// - /// Default is 0.001. Higher values allow more distance error but improve stability. + /// Default is . Higher values allow more distance error but improve stability. /// public Real Softness { @@ -303,7 +303,7 @@ public Real Bias /// public Real Impulse => Data.AccumulatedImpulse; - public static void IterateFixedAngle(ref ConstraintData constraint, Real idt) + public static void IterateDistanceLimit(ref ConstraintData constraint, Real idt) { ref var data = ref Unsafe.As(ref constraint); ref RigidBodyData body1 = ref constraint.Body1.Data; diff --git a/src/Jitter2/Dynamics/Constraints/HingeAngle.cs b/src/Jitter2/Dynamics/Constraints/HingeAngle.cs index a9e808ff..be96f02a 100644 --- a/src/Jitter2/Dynamics/Constraints/HingeAngle.cs +++ b/src/Jitter2/Dynamics/Constraints/HingeAngle.cs @@ -182,8 +182,8 @@ public static void PrepareForIterationHingeAngle(ref ConstraintData constraint, data.EffectiveMass.M31 = data.EffectiveMass.M13 = 0; data.EffectiveMass.M32 = data.EffectiveMass.M23 = 0; - // TODO: do he have to set them to zero here, explicitly? - // does this also has to be done in PointOnLine? + // TODO: Check whether these rows must be cleared explicitly here + // and whether PointOnLine needs the same treatment. data.Jacobian.M13 = data.Jacobian.M23 = data.Jacobian.M33 = 0; } diff --git a/src/Jitter2/Dynamics/Constraints/PointOnLine.cs b/src/Jitter2/Dynamics/Constraints/PointOnLine.cs index c7bce866..b78eea4a 100644 --- a/src/Jitter2/Dynamics/Constraints/PointOnLine.cs +++ b/src/Jitter2/Dynamics/Constraints/PointOnLine.cs @@ -48,7 +48,6 @@ public struct PointOnLineData public ushort Clamp; - // public MemBlock96 J0; } private static readonly uint RegisteredDispatchId = @@ -275,7 +274,7 @@ public Real Softness /// Gets or sets the bias factor controlling how aggressively positional error is corrected. /// /// - /// Default is 0.01. Higher values correct errors faster but may cause instability. + /// Default is . Higher values correct errors faster but may cause instability. /// public Real Bias { diff --git a/src/Jitter2/Dynamics/Constraints/PointOnPlane.cs b/src/Jitter2/Dynamics/Constraints/PointOnPlane.cs index 1e507a4b..3c421242 100644 --- a/src/Jitter2/Dynamics/Constraints/PointOnPlane.cs +++ b/src/Jitter2/Dynamics/Constraints/PointOnPlane.cs @@ -195,7 +195,7 @@ public Real Softness /// Gets or sets the bias factor controlling how aggressively positional error is corrected. /// /// - /// Default is 0.01. Higher values correct errors faster but may cause instability. + /// Default is . Higher values correct errors faster but may cause instability. /// public Real Bias { diff --git a/src/Jitter2/Dynamics/Constraints/TwistAngle.cs b/src/Jitter2/Dynamics/Constraints/TwistAngle.cs index 6da2f940..7056df39 100644 --- a/src/Jitter2/Dynamics/Constraints/TwistAngle.cs +++ b/src/Jitter2/Dynamics/Constraints/TwistAngle.cs @@ -226,7 +226,7 @@ public JAngle Angle /// Gets or sets the softness (compliance) of the constraint. /// /// - /// Default is 0.0001. Higher values allow more angular error but improve stability. + /// Default is . Higher values allow more angular error but improve stability. /// public Real Softness { diff --git a/src/Jitter2/Dynamics/Contact.cs b/src/Jitter2/Dynamics/Contact.cs index 070c81e1..5495be07 100644 --- a/src/Jitter2/Dynamics/Contact.cs +++ b/src/Jitter2/Dynamics/Contact.cs @@ -77,8 +77,8 @@ public enum SolveMode #pragma warning restore CS0649 /// - /// The least four significant bits indicate which contacts are considered intact (bit set), broken (bit unset). - /// Bits 5-8 indicate which contacts were intact/broken during the solving-phase. + /// The low nibble indicates which contacts are currently intact (bit set) or broken (bit unset). + /// The next nibble indicates which contacts were intact or broken during the solving phase. /// /// /// A sphere may slide down a ramp. Within one timestep Jitter may detect the collision, create the contact, diff --git a/src/Jitter2/Dynamics/Joints/Joint.cs b/src/Jitter2/Dynamics/Joints/Joint.cs index 009a5acc..46b8edcf 100644 --- a/src/Jitter2/Dynamics/Joints/Joint.cs +++ b/src/Jitter2/Dynamics/Joints/Joint.cs @@ -15,15 +15,15 @@ namespace Jitter2.Dynamics.Constraints; public class Joint : IDebugDrawable { private readonly List constraints = new(2); - public ReadOnlyList Constraints => new (constraints); + public ReadOnlyList Constraints => new(constraints); /// - /// Add a constraint to the internal bookkeeping + /// Adds a constraint to the internal bookkeeping. /// protected void Register(Constraint constraint) => constraints.Add(constraint); /// - /// Remove a constraint from the internal bookkeeping + /// Removes a constraint from the internal bookkeeping. /// protected void Deregister(Constraint constraint) => constraints.Remove(constraint); diff --git a/src/Jitter2/Dynamics/RigidBody.cs b/src/Jitter2/Dynamics/RigidBody.cs index 8cc626b4..723d03b5 100644 --- a/src/Jitter2/Dynamics/RigidBody.cs +++ b/src/Jitter2/Dynamics/RigidBody.cs @@ -601,7 +601,7 @@ public JVector Velocity if (handle.Data.MotionType == MotionType.Static) { throw new InvalidOperationException( - $"Can not set velocity for static objects, objects must be kinematic or dynamic. See {nameof(MotionType)}."); + $"Cannot set velocity for static objects, objects must be kinematic or dynamic. See {nameof(MotionType)}."); } handle.Data.Velocity = value; @@ -634,7 +634,7 @@ public JVector AngularVelocity if (handle.Data.MotionType == MotionType.Static) { throw new InvalidOperationException( - $"Can not set angular velocity for static objects, objects must be kinematic or dynamic. See {nameof(MotionType)}."); + $"Cannot set angular velocity for static objects, objects must be kinematic or dynamic. See {nameof(MotionType)}."); } handle.Data.AngularVelocity = value; @@ -656,7 +656,7 @@ public JVector AngularVelocity public bool AffectedByGravity { get; set; } = true; /// - /// A managed pointer to custom user data. This is not utilized by the engine. + /// A managed pointer to custom user data. This is not used by the engine. /// public object? Tag { get; set; } @@ -861,7 +861,7 @@ public void AddShapes(IEnumerable shapes, MassInertiaUpdateMode if (shape.IsRegistered) { - throw new ArgumentException("Shape can not be added. Shape already registered elsewhere.", nameof(shapes)); + throw new ArgumentException("Shape cannot be added. Shape already registered elsewhere.", nameof(shapes)); } AttachToShape(shape); @@ -928,7 +928,7 @@ public void AddShape(RigidBodyShape shape, MassInertiaUpdateMode massInertiaMode if (shape.IsRegistered) { - throw new ArgumentException("Shape can not be added. Shape already registered elsewhere.", nameof(shape)); + throw new ArgumentException("Shape cannot be added. Shape already registered elsewhere.", nameof(shape)); } AttachToShape(shape); @@ -1331,7 +1331,7 @@ public void SetMassInertia(Real mass) if (mass <= (Real)0.0) { - throw new ArgumentException("Mass can not be zero or negative.", nameof(mass)); + throw new ArgumentException("Mass cannot be zero or negative.", nameof(mass)); } SetMassInertia(); @@ -1382,7 +1382,7 @@ public void SetMassInertia(in JMatrix inertia, Real mass, bool setAsInverse = fa { if (mass <= (Real)0.0) { - throw new ArgumentException("Mass can not be zero or negative.", nameof(mass)); + throw new ArgumentException("Mass cannot be zero or negative.", nameof(mass)); } if (!JMatrix.Inverse(inertia, out inverseInertia)) diff --git a/src/Jitter2/LinearMath/JBoundingBox.cs b/src/Jitter2/LinearMath/JBoundingBox.cs index 592a8fcb..2ab21397 100644 --- a/src/Jitter2/LinearMath/JBoundingBox.cs +++ b/src/Jitter2/LinearMath/JBoundingBox.cs @@ -269,16 +269,14 @@ public static JBoundingBox CreateFromPoints(IEnumerable points) return box; } - - /// /// Determines the relationship between this box and another box. /// /// The other bounding box to test. /// /// if they do not touch.
- /// if is strictly inside this box.
- /// if they overlap but one does not strictly contain the other. + /// if is fully contained by this box.
+ /// if they overlap but this box does not fully contain the other. /// public readonly ContainmentType Contains(in JBoundingBox box) { diff --git a/src/Jitter2/LinearMath/JMatrix.cs b/src/Jitter2/LinearMath/JMatrix.cs index d922b0fd..f556d322 100644 --- a/src/Jitter2/LinearMath/JMatrix.cs +++ b/src/Jitter2/LinearMath/JMatrix.cs @@ -60,11 +60,11 @@ static JMatrix() [MethodImpl(MethodImplOptions.AggressiveInlining)] public static JMatrix FromColumns(in JVector col1, in JVector col2, in JVector col3) { - Unsafe.SkipInit(out JMatrix res); - res.UnsafeGet(0) = col1; - res.UnsafeGet(1) = col2; - res.UnsafeGet(2) = col3; - return res; + Unsafe.SkipInit(out JMatrix result); + result.UnsafeGet(0) = col1; + result.UnsafeGet(1) = col2; + result.UnsafeGet(2) = col3; + return result; } /// @@ -458,16 +458,15 @@ public static JMatrix Multiply(JMatrix matrix1, Real scaleFactor) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Multiply(in JMatrix matrix1, Real scaleFactor, out JMatrix result) { - Real num = scaleFactor; - result.M11 = matrix1.M11 * num; - result.M12 = matrix1.M12 * num; - result.M13 = matrix1.M13 * num; - result.M21 = matrix1.M21 * num; - result.M22 = matrix1.M22 * num; - result.M23 = matrix1.M23 * num; - result.M31 = matrix1.M31 * num; - result.M32 = matrix1.M32 * num; - result.M33 = matrix1.M33 * num; + result.M11 = matrix1.M11 * scaleFactor; + result.M12 = matrix1.M12 * scaleFactor; + result.M13 = matrix1.M13 * scaleFactor; + result.M21 = matrix1.M21 * scaleFactor; + result.M22 = matrix1.M22 * scaleFactor; + result.M23 = matrix1.M23 * scaleFactor; + result.M31 = matrix1.M31 * scaleFactor; + result.M32 = matrix1.M32 * scaleFactor; + result.M33 = matrix1.M33 * scaleFactor; } /// diff --git a/src/Jitter2/LinearMath/JQuaternion.cs b/src/Jitter2/LinearMath/JQuaternion.cs index d317fbfc..c57accfd 100644 --- a/src/Jitter2/LinearMath/JQuaternion.cs +++ b/src/Jitter2/LinearMath/JQuaternion.cs @@ -490,12 +490,12 @@ public readonly Real LengthSquared() [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Normalize() { - Real num2 = X * X + Y * Y + Z * Z + W * W; - Real num = (Real)1.0 / MathR.Sqrt(num2); - X *= num; - Y *= num; - Z *= num; - W *= num; + Real lengthSquared = X * X + Y * Y + Z * Z + W * W; + Real invLength = (Real)1.0 / MathR.Sqrt(lengthSquared); + X *= invLength; + Y *= invLength; + Z *= invLength; + W *= invLength; } /// @@ -504,12 +504,12 @@ public void Normalize() /// The quaternion to normalize. public static void NormalizeInPlace(ref JQuaternion quaternion) { - Real num2 = quaternion.LengthSquared(); - Real num = (Real)1.0 / MathR.Sqrt(num2); - quaternion.X *= num; - quaternion.Y *= num; - quaternion.Z *= num; - quaternion.W *= num; + Real lengthSquared = quaternion.LengthSquared(); + Real invLength = (Real)1.0 / MathR.Sqrt(lengthSquared); + quaternion.X *= invLength; + quaternion.Y *= invLength; + quaternion.Z *= invLength; + quaternion.W *= invLength; } /// diff --git a/src/Jitter2/LinearMath/JTriangle.cs b/src/Jitter2/LinearMath/JTriangle.cs index f0d1d287..fb934a21 100644 --- a/src/Jitter2/LinearMath/JTriangle.cs +++ b/src/Jitter2/LinearMath/JTriangle.cs @@ -72,7 +72,7 @@ public readonly bool RayIntersect(in JVector origin, in JVector direction, CullM if (Math.Abs(denominator) < (Real)1e-06) { - // triangle and ray are parallel + // Triangle and ray are parallel. goto return_false; } diff --git a/src/Jitter2/LinearMath/JVector.cs b/src/Jitter2/LinearMath/JVector.cs index 5bdd7e66..4263c088 100644 --- a/src/Jitter2/LinearMath/JVector.cs +++ b/src/Jitter2/LinearMath/JVector.cs @@ -546,11 +546,11 @@ public static JVector NormalizeSafe(in JVector value, Real epsilonSquared = (Rea [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Normalize() { - Real num2 = X * X + Y * Y + Z * Z; - Real num = (Real)1.0 / MathR.Sqrt(num2); - X *= num; - Y *= num; - Z *= num; + Real lengthSquared = X * X + Y * Y + Z * Z; + Real invLength = (Real)1.0 / MathR.Sqrt(lengthSquared); + X *= invLength; + Y *= invLength; + Z *= invLength; } /// @@ -559,11 +559,11 @@ public void Normalize() /// The vector to normalize. public static void NormalizeInPlace(ref JVector toNormalize) { - Real num2 = toNormalize.LengthSquared(); - Real num = (Real)1.0 / MathR.Sqrt(num2); - toNormalize.X *= num; - toNormalize.Y *= num; - toNormalize.Z *= num; + Real lengthSquared = toNormalize.LengthSquared(); + Real invLength = (Real)1.0 / MathR.Sqrt(lengthSquared); + toNormalize.X *= invLength; + toNormalize.Y *= invLength; + toNormalize.Z *= invLength; } /// diff --git a/src/Jitter2/LinearMath/MathHelper.cs b/src/Jitter2/LinearMath/MathHelper.cs index 5516adda..7f462f2f 100644 --- a/src/Jitter2/LinearMath/MathHelper.cs +++ b/src/Jitter2/LinearMath/MathHelper.cs @@ -54,27 +54,25 @@ public static JQuaternion RotationQuaternion(in JVector omega, Real dt) Real dt3 = dt * dt * dt; Real angle2 = angle * angle; - Real scale = (Real)0.5 * dt - ((Real)1.0 / (Real)48.0) * dt3 * angle2; - JVector.Multiply(omega, scale, out var axis); + Real smallAngleScale = (Real)0.5 * dt - ((Real)1.0 / (Real)48.0) * dt3 * angle2; + JVector.Multiply(omega, smallAngleScale, out var smallAngleAxis); Real cos = (Real)1.0 - ((Real)1.0 / (Real)8.0) * theta * theta; - JQuaternion res = new JQuaternion(axis.X, axis.Y, axis.Z, cos); - Debug.Assert(MathHelper.IsZero(res.Length() - 1, (Real)1e-2)); - return res; + JQuaternion smallAngleResult = new JQuaternion(smallAngleAxis.X, smallAngleAxis.Y, smallAngleAxis.Z, cos); + Debug.Assert(MathHelper.IsZero(smallAngleResult.Length() - 1, (Real)1e-2)); + return smallAngleResult; } - else - { - Real halfAngleDt = (Real)0.5 * angle * dt; - (Real sinD, Real cosD) = StableMath.SinCos(halfAngleDt); - Real scale = sinD / angle; - JVector.Multiply(omega, scale, out var axis); + Real halfAngleDt = (Real)0.5 * angle * dt; + (Real sinD, Real cosD) = StableMath.SinCos(halfAngleDt); - JQuaternion res = new JQuaternion(axis.X, axis.Y, axis.Z, cosD); - Debug.Assert(MathHelper.IsZero(res.Length() - 1, (Real)1e-2)); - return res; - } + Real scale = sinD / angle; + JVector.Multiply(omega, scale, out var axis); + + JQuaternion result = new JQuaternion(axis.X, axis.Y, axis.Z, cosD); + Debug.Assert(MathHelper.IsZero(result.Length() - 1, (Real)1e-2)); + return result; } /// @@ -149,7 +147,7 @@ public static JMatrix InverseSquareRoot(JMatrix m, int sweeps = 2) { Real phi, cp, sp; - // M32 + // M23 if (MathR.Abs(m.M23) > (Real)1e-6) { phi = StableMath.Atan2((Real)1.0, (m.M33 - m.M22) / ((Real)2.0 * m.M23)) / (Real)2.0; @@ -203,9 +201,9 @@ public static JVector CreateOrthonormal(in JVector vec) { Debug.Assert(!CloseToZero(vec), "Cannot create orthonormal of a zero vector"); - Real ax = Math.Abs(vec.X); - Real ay = Math.Abs(vec.Y); - Real az = Math.Abs(vec.Z); + Real ax = MathR.Abs(vec.X); + Real ay = MathR.Abs(vec.Y); + Real az = MathR.Abs(vec.Z); JVector r; diff --git a/src/Jitter2/Logger.cs b/src/Jitter2/Logger.cs index 647d53dd..b9118275 100644 --- a/src/Jitter2/Logger.cs +++ b/src/Jitter2/Logger.cs @@ -74,7 +74,7 @@ public enum LogLevel public static void Error(scoped ReadOnlySpan format, T1 arg1, T2 arg2, T3 arg3) => LogFormat(LogLevel.Error, format, arg1, arg2, arg3); /// - /// Internal logging method that invokes all registered listeners with the given message. + /// Internal logging method that invokes the registered listener with the given message. /// /// The log level of the message. /// The message to log. @@ -84,7 +84,7 @@ private static void Log(LogLevel level, scoped ReadOnlySpan format) } /// - /// Formats a log message with one argument and invokes the listeners. + /// Formats a log message with one argument and invokes the registered listener. /// private static void LogFormat(LogLevel level, scoped ReadOnlySpan format, T1 arg1) { diff --git a/src/Jitter2/Parallelization/Parallel.cs b/src/Jitter2/Parallelization/Parallel.cs index 8ae2ead6..f2a23fa3 100644 --- a/src/Jitter2/Parallelization/Parallel.cs +++ b/src/Jitter2/Parallelization/Parallel.cs @@ -18,8 +18,8 @@ namespace Jitter2.Parallelization; public static class Parallel { /// - /// Represents a batch defined by a start index, an end index, and a batch index. - /// This struct is utilized in to facilitate multithreaded batch processing within a for-loop. + /// Represents a half-open index range processed by a parallel batch. + /// This struct is used by to split a for-loop into worker tasks. /// public readonly struct Batch(int start, int end) { @@ -65,9 +65,9 @@ public static void GetBounds(int numElements, int numDivisions, int part, out in /// /// The inclusive lower bound of the range to be processed. /// The exclusive upper bound of the range to be processed. - /// The number of batches to divide the work into. - /// The callback function to execute for each batch. - /// Indicates whether to execute the tasks immediately after adding them to the thread pool. + /// The number of batches to create. + /// The callback to execute for each batch. + /// Whether to execute the queued batches immediately. /// /// This method splits the range [lower, upper) into batches and processes each batch in parallel. /// The callback is invoked for each batch, which is represented by a struct. diff --git a/src/Jitter2/Parallelization/ThreadPool.cs b/src/Jitter2/Parallelization/ThreadPool.cs index 8efaa6fd..81706c62 100644 --- a/src/Jitter2/Parallelization/ThreadPool.cs +++ b/src/Jitter2/Parallelization/ThreadPool.cs @@ -228,7 +228,7 @@ public void ChangeThreadCount(int numThreads) threads = new Thread[threadCount - 1]; - var initWaitHandle = new AutoResetEvent(false); + using AutoResetEvent initWaitHandle = new(false); for (int i = 0; i < threadCount - 1; i++) { @@ -329,9 +329,10 @@ private void ThreadProc(int index) if (performedTasks > 0) { // steal from other queues - for (int i = 1; i < queues.Length; i++) + int queueCount = queues.Length; + for (int i = 1; i < queueCount; i++) { - int queueIndex = (i + index) % queues.Length; + int queueIndex = (i + index) % queueCount; while (queues[queueIndex].TryDequeue(out var task)) { @@ -383,16 +384,17 @@ public void Execute() Interlocked.Exchange(ref capturedException, null); int totalTasks = taskList.Count; + int queueCount = ThreadCount; Volatile.Write(ref tasksLeft.Value, totalTasks); for (int i = 0; i < totalTasks; i++) { - queues[i % this.ThreadCount].Enqueue(taskList[i]); + queues[i % queueCount].Enqueue(taskList[i]); } taskList.Clear(); - // the main thread's queue. + // Process the main thread's queue first. var myQueue = queues[0]; while (myQueue.TryDequeue(out var task)) @@ -400,7 +402,7 @@ public void Execute() PerformTask(task); } - // steal from other queues + // Steal from other queues. for (int i = 1; i < queues.Length; i++) { while (queues[i].TryDequeue(out var task)) diff --git a/src/Jitter2/SoftBodies/BroadPhaseCollisionFilter.cs b/src/Jitter2/SoftBodies/BroadPhaseCollisionFilter.cs index a9a688ec..a932dd32 100644 --- a/src/Jitter2/SoftBodies/BroadPhaseCollisionFilter.cs +++ b/src/Jitter2/SoftBodies/BroadPhaseCollisionFilter.cs @@ -32,26 +32,26 @@ public BroadPhaseCollisionFilter(World world) /// public bool Filter(IDynamicTreeProxy proxyA, IDynamicTreeProxy proxyB) { - SoftBodyShape? i1 = proxyA as SoftBodyShape; - SoftBodyShape? i2 = proxyB as SoftBodyShape; + SoftBodyShape? softShapeA = proxyA as SoftBodyShape; + SoftBodyShape? softShapeB = proxyB as SoftBodyShape; - if (i1 != null && i2 != null) + if (softShapeA != null && softShapeB != null) { - if (i2.ShapeId < i1.ShapeId) + if (softShapeB.ShapeId < softShapeA.ShapeId) { - (i1, i2) = (i2, i1); + (softShapeA, softShapeB) = (softShapeB, softShapeA); } - if (!i1.SoftBody.IsActive && !i2.SoftBody.IsActive) return false; + if (!softShapeA.SoftBody.IsActive && !softShapeB.SoftBody.IsActive) return false; - bool colliding = NarrowPhase.MprEpa(i1, i2, + bool colliding = NarrowPhase.MprEpa(softShapeA, softShapeB, JQuaternion.Identity, JVector.Zero, out JVector pA, out JVector pB, out JVector normal, out _); if (!colliding) return false; - var closestA = i1.GetClosest(pA); - var closestB = i2.GetClosest(pB); + var closestA = softShapeA.GetClosest(pA); + var closestB = softShapeB.GetClosest(pB); world.RegisterContact(closestA.RigidBodyId, closestB.RigidBodyId, closestA, closestB, pA, pB, normal); @@ -59,39 +59,41 @@ public bool Filter(IDynamicTreeProxy proxyA, IDynamicTreeProxy proxyB) return false; } - if (i1 != null) + if (softShapeA != null) { - var rb = (proxyB as RigidBodyShape)!.RigidBody; + var rigidShapeB = (proxyB as RigidBodyShape)!; + var rigidBodyB = rigidShapeB.RigidBody; - if (!i1.SoftBody.IsActive && !rb.Data.IsActive) return false; + if (!softShapeA.SoftBody.IsActive && !rigidBodyB.Data.IsActive) return false; - bool colliding = NarrowPhase.MprEpa(i1, (proxyB as RigidBodyShape)!, rb.Orientation, rb.Position, + bool colliding = NarrowPhase.MprEpa(softShapeA, rigidShapeB, rigidBodyB.Orientation, rigidBodyB.Position, out JVector pA, out JVector pB, out JVector normal, out _); if (!colliding) return false; - var closest = i1.GetClosest(pA); + var closest = softShapeA.GetClosest(pA); - world.RegisterContact(closest.RigidBodyId, rb.RigidBodyId, closest, rb, + world.RegisterContact(closest.RigidBodyId, rigidBodyB.RigidBodyId, closest, rigidBodyB, pA, pB, normal, ContactData.SolveMode.AngularBody1); return false; } - if (i2 != null) + if (softShapeB != null) { - var ra = (proxyA as RigidBodyShape)!.RigidBody; + var rigidShapeA = (proxyA as RigidBodyShape)!; + var rigidBodyA = rigidShapeA.RigidBody; - if (!i2.SoftBody.IsActive && !ra.Data.IsActive) return false; + if (!softShapeB.SoftBody.IsActive && !rigidBodyA.Data.IsActive) return false; - bool colliding = NarrowPhase.MprEpa(i2, (proxyA as RigidBodyShape)!, ra.Orientation, ra.Position, + bool colliding = NarrowPhase.MprEpa(softShapeB, rigidShapeA, rigidBodyA.Orientation, rigidBodyA.Position, out JVector pA, out JVector pB, out JVector normal, out _); if (!colliding) return false; - var closest = i2.GetClosest(pA); + var closest = softShapeB.GetClosest(pA); - world.RegisterContact(closest.RigidBodyId, ra.RigidBodyId, closest, ra, + world.RegisterContact(closest.RigidBodyId, rigidBodyA.RigidBodyId, closest, rigidBodyA, pA, pB, normal, ContactData.SolveMode.AngularBody1); return false; diff --git a/src/Jitter2/SoftBodies/DynamicTreeCollisionFilter.cs b/src/Jitter2/SoftBodies/DynamicTreeCollisionFilter.cs index a16647c5..d2d8a3e1 100644 --- a/src/Jitter2/SoftBodies/DynamicTreeCollisionFilter.cs +++ b/src/Jitter2/SoftBodies/DynamicTreeCollisionFilter.cs @@ -32,9 +32,9 @@ public static bool Filter(IDynamicTreeProxy proxyA, IDynamicTreeProxy proxyB) else if (proxyA is SoftBodyShape softBodyShapeA && proxyB is SoftBodyShape softBodyShapeB) { - SoftBody ta = softBodyShapeA.SoftBody; - SoftBody tb = softBodyShapeB.SoftBody; - return ta != tb; + SoftBody softBodyA = softBodyShapeA.SoftBody; + SoftBody softBodyB = softBodyShapeB.SoftBody; + return softBodyA != softBodyB; } return true; diff --git a/src/Jitter2/SoftBodies/SoftBodyTetrahedron.cs b/src/Jitter2/SoftBodies/SoftBodyTetrahedron.cs index c66a46fa..a8070a5e 100644 --- a/src/Jitter2/SoftBodies/SoftBodyTetrahedron.cs +++ b/src/Jitter2/SoftBodies/SoftBodyTetrahedron.cs @@ -60,15 +60,15 @@ public override JVector Velocity /// public override RigidBody GetClosest(in JVector pos) { - Real dist = Real.MaxValue; + Real minDistanceSquared = Real.MaxValue; int closest = 0; for (int i = 0; i < 4; i++) { - Real len = (pos - Vertices[i].Position).LengthSquared(); - if (len < dist) + Real distanceSquared = (pos - Vertices[i].Position).LengthSquared(); + if (distanceSquared < minDistanceSquared) { - dist = len; + minDistanceSquared = distanceSquared; closest = i; } } diff --git a/src/Jitter2/SoftBodies/SoftBodyTriangle.cs b/src/Jitter2/SoftBodies/SoftBodyTriangle.cs index 6799a2b2..43b7959a 100644 --- a/src/Jitter2/SoftBodies/SoftBodyTriangle.cs +++ b/src/Jitter2/SoftBodies/SoftBodyTriangle.cs @@ -67,12 +67,12 @@ public SoftBodyTriangle(SoftBody body, RigidBody v1, RigidBody v2, RigidBody v3) /// public override RigidBody GetClosest(in JVector pos) { - Real len1 = (pos - v1.Position).LengthSquared(); - Real len2 = (pos - v2.Position).LengthSquared(); - Real len3 = (pos - v3.Position).LengthSquared(); + Real distance1Squared = (pos - v1.Position).LengthSquared(); + Real distance2Squared = (pos - v2.Position).LengthSquared(); + Real distance3Squared = (pos - v3.Position).LengthSquared(); - return (len1 < len2 && len1 < len3) ? v1 : - (len2 < len3) ? v2 : v3; + return (distance1Squared < distance2Squared && distance1Squared < distance3Squared) ? v1 : + (distance2Squared < distance3Squared) ? v2 : v3; } /// @@ -101,20 +101,20 @@ public override void SupportMap(in JVector direction, out JVector result) JVector b = v2.Position; JVector c = v3.Position; - Real min = JVector.Dot(a, direction); + Real maxDot = JVector.Dot(a, direction); Real dot = JVector.Dot(b, direction); result = a; - if (dot > min) + if (dot > maxDot) { - min = dot; + maxDot = dot; result = b; } dot = JVector.Dot(c, direction); - if (dot > min) + if (dot > maxDot) { result = c; } diff --git a/src/Jitter2/SoftBodies/SpringConstraint.cs b/src/Jitter2/SoftBodies/SpringConstraint.cs index 7785cf29..74a21193 100644 --- a/src/Jitter2/SoftBodies/SpringConstraint.cs +++ b/src/Jitter2/SoftBodies/SpringConstraint.cs @@ -102,7 +102,7 @@ public void Initialize(JVector anchor1, JVector anchor2) /// /// Sets the spring parameters using physical properties. This method calculates and sets /// the and properties. It assumes that the mass - /// of the involved bodies and the timestep size does not change. + /// of the involved bodies and the timestep size do not change. /// /// The frequency in Hz. /// The damping ratio (0 = no damping, 1 = critical damping). @@ -124,12 +124,12 @@ public void SetSpringParameters(Real frequency, Real damping, Real dt) Real effectiveMass = (Real)1.0 / (body1.InverseMass + body2.InverseMass); Real omega = (Real)2.0 * MathR.PI * frequency; - Real d = (Real)2.0 * effectiveMass * damping * omega; - Real k = effectiveMass * omega * omega; + Real dampingCoefficient = (Real)2.0 * effectiveMass * damping * omega; + Real springStiffness = effectiveMass * omega * omega; Real h = dt; - data.Softness = (Real)1.0 / (d + h * k); - data.BiasFactor = h * k * data.Softness; + data.Softness = (Real)1.0 / (dampingCoefficient + h * springStiffness); + data.BiasFactor = h * springStiffness * data.Softness; } /// @@ -245,7 +245,7 @@ public static void PrepareForIterationSpringConstraint(ref ConstraintData constr Real error = dp.Length() - data.Distance; - JVector n = p2 - p1; + JVector n = dp; if (n.LengthSquared() > (Real)1e-12) JVector.NormalizeInPlace(ref n); data.Jacobian = n; @@ -263,7 +263,7 @@ public static void PrepareForIterationSpringConstraint(ref ConstraintData constr /// Gets or sets the softness (compliance) of the spring constraint. /// /// - /// Default is 0.001. Higher values allow more positional error but produce a softer spring. + /// Default is . Higher values allow more positional error but produce a softer spring. /// Scaled by inverse timestep during solving. /// public Real Softness diff --git a/src/Jitter2/Tracer.cs b/src/Jitter2/Tracer.cs index 05522e8d..ad7299a1 100644 --- a/src/Jitter2/Tracer.cs +++ b/src/Jitter2/Tracer.cs @@ -89,7 +89,7 @@ static Tracer() string message = $">>> PROFILING ENABLED! <<< Use {nameof(Tracer)}.{nameof(WriteToFile)} to dump trace to disk."; - if(Logger.Listener == null) Console.WriteLine(message); + if (Logger.Listener == null) Console.WriteLine(message); else Logger.Warning(message); StartGcTracing(); diff --git a/src/Jitter2/Unmanaged/MemoryHelper.cs b/src/Jitter2/Unmanaged/MemoryHelper.cs index 07593055..b9f5b27e 100644 --- a/src/Jitter2/Unmanaged/MemoryHelper.cs +++ b/src/Jitter2/Unmanaged/MemoryHelper.cs @@ -48,7 +48,7 @@ public struct IsolatedInt } /// - /// Represents an integer value padded to one cache-line-sized array element. + /// Represents a padded array element intended to keep adjacent integer values off the same cache line. /// /// /// This type is intended for arrays of counters where adjacent fields diff --git a/src/Jitter2/Unmanaged/PartitionedBuffer.cs b/src/Jitter2/Unmanaged/PartitionedBuffer.cs index 683f5c57..e72d609e 100644 --- a/src/Jitter2/Unmanaged/PartitionedBuffer.cs +++ b/src/Jitter2/Unmanaged/PartitionedBuffer.cs @@ -244,7 +244,7 @@ private void EnsureHandleCapacity(int requiredCount) } /// - /// Removes the associated native structure from the buffer and invalidates the handle. + /// Removes the associated unmanaged element from the buffer and invalidates the handle. /// /// The handle to free. /// diff --git a/src/Jitter2/World.Deterministic.cs b/src/Jitter2/World.Deterministic.cs index 65027540..ec87ddd0 100644 --- a/src/Jitter2/World.Deterministic.cs +++ b/src/Jitter2/World.Deterministic.cs @@ -42,9 +42,11 @@ public sealed partial class World /// /// The solver strategy used during . Defaults to . - /// can be significantly slower than - /// . /// + /// + /// can be significantly slower than + /// . + /// /// Thrown when an undefined /// value is assigned. public SolveMode SolveMode @@ -94,7 +96,7 @@ private struct ConstraintEntry public ulong ConstraintId; } - // Maps RigidBodyData._index → island SetIndex. Cleared each frame; capacity grows on demand but is not trimmed. + // Maps RigidBodyData._index to island SetIndex. Cleared each frame; capacity grows on demand and can be reduced by Trim(). private readonly Dictionary handleToIsland = new(); // Per-frame island ranges built from the sorted buffers. diff --git a/src/Jitter2/World.Step.cs b/src/Jitter2/World.Step.cs index 50375053..f0c28a0d 100644 --- a/src/Jitter2/World.Step.cs +++ b/src/Jitter2/World.Step.cs @@ -338,12 +338,12 @@ private void StabilizeInternal(Real dt, int solverIterations, int relaxationIter if (solverIterations < 1) { - throw new ArgumentException("Solver iterations can not be smaller than one.", nameof(solverIterations)); + throw new ArgumentException("Solver iterations cannot be smaller than one.", nameof(solverIterations)); } if (relaxationIterations < 0) { - throw new ArgumentException("Relaxation iterations can not be smaller than zero.", nameof(relaxationIterations)); + throw new ArgumentException("Relaxation iterations cannot be smaller than zero.", nameof(relaxationIterations)); } if (dt < Real.Epsilon) return; // nothing to do @@ -1040,12 +1040,11 @@ private static JVector SolveGyroscopic(in JMatrix inertiaWorld, in JVector omega // we solve the same implicit equation directly in **world space** using // I_w = R_n I_b R_nᵀ (assembled from the orientation at t_n). // - // The two approaches are algebraically equivalent: - - // • Catto: keep I_b fixed, rotate ω′ with R_n | - // • Here: keep I_w fixed (= R_n I_b R_nᵀ) while solving | - // Both introduce the same first-order O(h) approximation - either “freeze” - // the inertia tensor (our method) or rotate ω′ with an orientation that is one - // step out of date (Catto). + // The two approaches are algebraically equivalent: + // - Catto keeps I_b fixed and rotates omega' with R_n. + // - This method keeps I_w fixed (= R_n I_b R_n^T) while solving. + // Both introduce the same first-order O(h) approximation: either freeze + // the inertia tensor or rotate omega' with an orientation that is one step out of date. JVector f = dt * (omega % JVector.Transform(omega, inertiaWorld)); @@ -1065,8 +1064,8 @@ private void IntegrateVelocities(Parallel.Batch batch) { ref RigidBodyData rigidBody = ref span[i]; - // only dynamic and kinematic objects have a velocity - if(rigidBody.MotionType == MotionType.Static) continue; + // Only dynamic and kinematic objects have a velocity. + if (rigidBody.MotionType == MotionType.Static) continue; JVector linearVelocity = rigidBody.Velocity; JVector angularVelocity = rigidBody.AngularVelocity; @@ -1212,8 +1211,8 @@ private bool DeactivateBodyForSleep(RigidBody body, bool clearMotion) bool deactivatedBody = wasActive && body.MotionType != MotionType.Static; // Static bodies have contacts and constraints, but they do not form - // collision islands. Do not deactivate contacts or constraints of - // static bodies, as the island of the static body goes to sleep. + // collision islands. Do not deactivate their contacts or constraints + // when a connected dynamic island goes to sleep. if (body.MotionType != MotionType.Static) { foreach (var c in body.InternalContacts) diff --git a/src/Jitter2/World.cs b/src/Jitter2/World.cs index 40be2aff..2958f790 100644 --- a/src/Jitter2/World.cs +++ b/src/Jitter2/World.cs @@ -154,7 +154,7 @@ public readonly struct SpanData(World world) /// public delegate void WorldStep(Real dt); - // Post- and Pre-step + // Step callbacks. /// /// Raised at the beginning of a simulation step, before any collision detection, @@ -297,7 +297,7 @@ public static ulong RequestId() /// Thrown when count is less than 1. public static (ulong min, ulong max) RequestId(int count) { - if (count < 1) throw new ArgumentOutOfRangeException(nameof(count), "Count must be greater zero."); + if (count < 1) throw new ArgumentOutOfRangeException(nameof(count), "Count must be greater than zero."); ulong count64 = (ulong)count; ulong max = Interlocked.Add(ref _idCounter, count64) + 1; return (max - count64, max); @@ -355,14 +355,14 @@ public static (ulong min, ulong max) RequestId(int count) { throw new ArgumentOutOfRangeException( nameof(value.solver), value.solver, - "Solver iterations can not be smaller than one."); + "Solver iterations cannot be smaller than one."); } if (value.relaxation < 0) { throw new ArgumentOutOfRangeException( nameof(value.relaxation), value.relaxation, - "Relaxation iterations can not be smaller than zero."); + "Relaxation iterations cannot be smaller than zero."); } solverIterations = value.solver; @@ -386,7 +386,7 @@ public int SubstepCount if (value < 1) { throw new ArgumentOutOfRangeException(nameof(value), - "The number of substeps has to be larger than zero."); + "The number of substeps must be greater than zero."); } substeps = value; @@ -668,7 +668,7 @@ internal void ActivateBodyNextStep(RigidBody body, bool wakeUpStatic = false) foreach (var c in body.Contacts) { - ActivateBodyNextStep(c.Body1 == body ? c.Body2 : c.Body1); + ActivateBodyNextStep(c.Body1 == body ? c.Body2 : c.Body1); } } @@ -816,7 +816,7 @@ public void ForceSleepIsland(Island island) /// /// Thrown if and are the same. /// - /// Raised when the maximum size limit is exceeded. + /// Raised when the constraint buffer maximum size is exceeded. public T CreateConstraint(RigidBody body1, RigidBody body2) where T : Constraint, new() { ThrowIfDisposed(); @@ -892,7 +892,7 @@ private void AssertIslandActivationInvariants() /// Creates and adds a new rigid body to the simulation world. /// /// A newly created instance of . - /// Raised when the maximum size limit is exceeded. + /// Raised when the rigid-body buffer maximum size is exceeded. public RigidBody CreateRigidBody() { ThrowIfDisposed();