Dev2 (#33)

* Rename properties in soft bodies

* Make IdCounter internal in World.cs

* Correct thickness definition in SoftBodyTriangle.cs

* Fix bug in TransformedShape.cs

* Use NativeMemory.Alloc instead of Marshal.AllocHGlobal in Jitter

* Update comment in World.cs

src/Jitter2/Collision/Shapes/TransformedShape.cs

@@ -126,7 +126,7 @@ public override void CalculateBoundingBox(in JMatrix orientation, in JVector pos
         else
         {
             OriginalShape.CalculateBoundingBox(orientation * this.transformation,
-                JVector.Transform(translation, transformation) + position, out box);
+                JVector.Transform(translation, orientation) + position, out box);
         }
     }
 

src/Jitter2/SoftBodies/SoftBody.cs

@@ -30,13 +30,13 @@ namespace Jitter2.SoftBodies;
 
 public class SoftBody
 {
-    public List<RigidBody> Points { get; } = new();
+    public List<RigidBody> Vertices { get; } = new();
     public List<Constraint> Springs { get; } = new();
     public List<Shape> Shapes { get; } = new();
 
     protected World world;
 
-    public bool IsActive => Points[0].IsActive;
+    public bool IsActive => Vertices[0].IsActive;
 
     public SoftBody(World world)
     {
@@ -58,7 +58,7 @@ public void Destroy()
             world.Remove(spring);
         }
 
-        foreach (var point in Points)
+        foreach (var point in Vertices)
         {
             world.Remove(point);
         }

src/Jitter2/SoftBodies/SoftBodyTetrahedron.cs

@@ -29,19 +29,19 @@ namespace Jitter2.SoftBodies;
 
 public class SoftBodyTetrahedron : Shape, ISoftBodyShape
 {
-    public SoftBodyTetrahedron(SoftBody body, RigidBody p1, RigidBody p2, RigidBody p3, RigidBody p4)
+    public SoftBodyTetrahedron(SoftBody body, RigidBody v1, RigidBody v2, RigidBody v3, RigidBody v4)
     {
-        Bodies[0] = p1;
-        Bodies[1] = p2;
-        Bodies[2] = p3;
-        Bodies[3] = p4;
+        Vertices[0] = v1;
+        Vertices[1] = v2;
+        Vertices[2] = v3;
+        Vertices[3] = v4;
 
         SoftBody = body;
 
         UpdateShape();
     }
 
-    public RigidBody[] Bodies { get; } = new RigidBody[4];
+    public RigidBody[] Vertices { get; } = new RigidBody[4];
 
     public override JVector Velocity
     {
@@ -51,7 +51,7 @@ public override JVector Velocity
 
             for (int i = 0; i < 4; i++)
             {
-                vel += Bodies[i].Velocity;
+                vel += Vertices[i].Velocity;
             }
 
             vel *= 0.25f;
@@ -68,7 +68,7 @@ public override void CalculateMassInertia(out JMatrix inertia, out JVector com,
 
         for (int i = 0; i < 4; i++)
         {
-            com += Bodies[i].Position;
+            com += Vertices[i].Position;
         }
 
         com *= 0.25f;
@@ -81,15 +81,15 @@ public RigidBody GetClosest(in JVector pos)
 
         for (int i = 0; i < 4; i++)
         {
-            float len = (pos - Bodies[i].Position).LengthSquared();
+            float len = (pos - Vertices[i].Position).LengthSquared();
             if (len < dist)
             {
                 dist = len;
                 closest = i;
             }
         }
 
-        return Bodies[closest];
+        return Vertices[closest];
     }
 
     public SoftBody SoftBody { get; }
@@ -103,8 +103,8 @@ public override void UpdateWorldBoundingBox()
 
         for (int i = 0; i < 4; i++)
         {
-            box.AddPoint(Bodies[i].Position);
-            GeometricCenter += Bodies[i].Position;
+            box.AddPoint(Vertices[i].Position);
+            GeometricCenter += Vertices[i].Position;
         }
 
         GeometricCenter *= 0.25f;
@@ -122,14 +122,14 @@ public override void SupportMap(in JVector direction, out JVector result)
 
         for (int i = 0; i < 4; i++)
         {
-            float dot = JVector.Dot(direction, Bodies[i].Position);
+            float dot = JVector.Dot(direction, Vertices[i].Position);
             if (dot > maxDot)
             {
                 maxDot = dot;
                 furthest = i;
             }
         }
 
-        result = Bodies[furthest].Position;
+        result = Vertices[furthest].Position;
     }
 }

src/Jitter2/SoftBodies/SoftBodyTriangle.cs

@@ -30,78 +30,84 @@ namespace Jitter2.SoftBodies;
 
 public class SoftBodyTriangle : Shape, ISoftBodyShape
 {
-    private readonly RigidBody p2;
-    private readonly RigidBody p3;
-    private readonly RigidBody p1;
+    private readonly RigidBody v2;
+    private readonly RigidBody v3;
+    private readonly RigidBody v1;
 
-    public RigidBody Body1 => p1;
-    public RigidBody Body2 => p2;
-    public RigidBody Body3 => p3;
+    public RigidBody Vertex1 => v1;
+    public RigidBody Vertex2 => v2;
+    public RigidBody Vertex3 => v3;
 
-    public float Thickness { get; set; } = 0.05f;
+    private float halfThickness = 0.05f;
 
-    public SoftBodyTriangle(SoftBody body, RigidBody p1, RigidBody p2, RigidBody p3)
+    public float Thickness
     {
-        this.p1 = p1;
-        this.p2 = p2;
-        this.p3 = p3;
+        get => halfThickness * 2.0f;
+        set => halfThickness = value * 0.5f;
+    }
+
+    public SoftBodyTriangle(SoftBody body, RigidBody v1, RigidBody v2, RigidBody v3)
+    {
+        this.v1 = v1;
+        this.v2 = v2;
+        this.v3 = v3;
 
         SoftBody = body;
         UpdateShape();
     }
 
-    public override JVector Velocity => 1.0f / 3.0f * (p1.Data.Velocity + p2.Data.Velocity + p3.Data.Velocity);
+    public override JVector Velocity => 1.0f / 3.0f * (v1.Data.Velocity + v2.Data.Velocity + v3.Data.Velocity);
 
     public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out float mass)
     {
         inertia = JMatrix.Identity;
         mass = 1;
-        com = 1.0f / 3.0f * (p1.Position + p2.Position + p3.Position);
+        com = 1.0f / 3.0f * (v1.Position + v2.Position + v3.Position);
     }
 
     public RigidBody GetClosest(in JVector pos)
     {
-        float len1 = (pos - p1.Position).LengthSquared();
-        float len2 = (pos - p2.Position).LengthSquared();
-        float len3 = (pos - p3.Position).LengthSquared();
+        float len1 = (pos - v1.Position).LengthSquared();
+        float len2 = (pos - v2.Position).LengthSquared();
+        float len3 = (pos - v3.Position).LengthSquared();
 
         if (len1 < len2 && len1 < len3)
         {
-            return p1;
+            return v1;
         }
 
         if (len2 < len3 && len2 <= len1)
         {
-            return p2;
+            return v2;
         }
 
-        return p3;
+        return v3;
     }
 
     public SoftBody SoftBody { get; }
 
     public override void UpdateWorldBoundingBox()
     {
-        float extraMargin = MathF.Max(Thickness, 0.01f);
+        float extraMargin = MathF.Max(halfThickness, 0.01f);
 
         var box = JBBox.SmallBox;
-        box.AddPoint(p1.Position);
-        box.AddPoint(p2.Position);
-        box.AddPoint(p3.Position);
+        box.AddPoint(v1.Position);
+        box.AddPoint(v2.Position);
+        box.AddPoint(v3.Position);
 
         box.Min -= JVector.One * extraMargin;
         box.Max += JVector.One * extraMargin;
 
         WorldBoundingBox = box;
 
-        GeometricCenter = 1.0f / 3.0f * (p1.Position + p2.Position + p3.Position);
+        GeometricCenter = 1.0f / 3.0f * (v1.Position + v2.Position + v3.Position);
     }
 
     public override void SupportMap(in JVector direction, out JVector result)
     {
-        JVector a = p1.Position;
-        JVector b = p2.Position;
-        JVector c = p3.Position;
+        JVector a = v1.Position;
+        JVector b = v2.Position;
+        JVector c = v3.Position;
 
         float min = JVector.Dot(a, direction);
         float dot = JVector.Dot(b, direction);
@@ -121,6 +127,6 @@ public override void SupportMap(in JVector direction, out JVector result)
             result = c;
         }
 
-        result += JVector.Normalize(direction) * Thickness;
+        result += JVector.Normalize(direction) * halfThickness;
     }
 }

src/Jitter2/UnmanagedMemory/MemoryHelper.cs

@@ -27,6 +27,22 @@ namespace Jitter2.UnmanagedMemory;
 
 public static unsafe class MemoryHelper
 {
+    /// <summary>
+    /// A block of 32 bytes of memory.
+    /// </summary>
+    [StructLayout(LayoutKind.Sequential, Size = 16)]
+    public struct MemBlock16
+    {
+    }
+
+    /// <summary>
+    /// A block of 32 bytes of memory.
+    /// </summary>
+    [StructLayout(LayoutKind.Sequential, Size = 32)]
+    public struct MemBlock32
+    {
+    }
+
     /// <summary>
     /// A block of 48 bytes of memory.
     /// </summary>
@@ -45,13 +61,16 @@ public struct MemBlock64
 
     public static T* AllocateHeap<T>(int num) where T : unmanaged
     {
-        return (T*)Marshal.AllocHGlobal(num * sizeof(T));
+        return (T*)AllocateHeap(num * sizeof(T));
     }
 
     public static void Free<T>(T* ptr) where T : unmanaged
     {
-        Marshal.FreeHGlobal((nint)ptr);
+        Free((void*)ptr);
     }
+
+    public static void* AllocateHeap(int len) => NativeMemory.Alloc((nuint)len);
+    public static void Free(void* ptr) => NativeMemory.Free(ptr);
 
     /// <summary>
     /// Zeros out unmanaged memory.

src/Jitter2/UnmanagedMemory/UnmanagedActiveList.cs

@@ -85,8 +85,8 @@ public UnmanagedActiveList(int maximumSize, int initialSize = 1024)
         size = initialSize;
         this.maximumSize = maximumSize;
 
-        memory = (T*)Marshal.AllocHGlobal(size * sizeof(T));
-        handles = (T**)Marshal.AllocHGlobal(maximumSize * sizeof(IntPtr));
+        memory = (T*)MemoryHelper.AllocateHeap(size * sizeof(T));
+        handles = (T**)MemoryHelper.AllocateHeap(maximumSize * sizeof(IntPtr));
 
         for (int i = 0; i < size; i++)
         {
@@ -209,7 +209,7 @@ public JHandle<T> Allocate(bool active = false, bool clear = false)
                             $"Resizing to {size}x{typeof(T)} ({size}x{sizeof(T)} Bytes).");
 
             var oldmemory = memory;
-            memory = (T*)Marshal.AllocHGlobal(size * sizeof(T));
+            memory = (T*)MemoryHelper.AllocateHeap(size * sizeof(T));
 
             for (int i = 0; i < osize; i++)
             {
@@ -222,7 +222,7 @@ public JHandle<T> Allocate(bool active = false, bool clear = false)
                 Unsafe.AsRef<int>(&memory[i]) = i;
             }
 
-            Marshal.FreeHGlobal((IntPtr)oldmemory);
+            MemoryHelper.Free(oldmemory);
             ResizeLock.ExitWriteLock();
         }
 
@@ -248,10 +248,10 @@ private void FreeResources()
     {
         if (!disposed)
         {
-            Marshal.FreeHGlobal((IntPtr)handles);
+            MemoryHelper.Free(handles);
             handles = (T**)0;
 
-            Marshal.FreeHGlobal((IntPtr)memory);
+            MemoryHelper.Free(memory);
             memory = (T*)0;
 
             disposed = true;

src/Jitter2/World.cs

@@ -95,7 +95,7 @@ public SpanData(World world)
     private readonly ActiveList<RigidBody> bodies = new();
     private readonly ActiveList<Shape> shapes = new();
 
-    public static ulong IdCounter;
+    internal static ulong IdCounter;
 
     /// <summary>
     /// Defines the two available thread models. The <see cref="ThreadModelType.Persistent"/> model keeps the worker
@@ -209,7 +209,7 @@ public JVector Gravity
     public World(int numBodies = 32768, int numContacts = 65536, int numConstraints = 32768)
     {
         // int numBodies = 32768, int numContacts = 65536, int numConstraints = 32768
-        // with this choice 1024 KB are directly allocated on the heap.
+        // with this choice (32768 + 65536 + 2 x 32768) x 8 Bytes = 1280 KB are allocated on the heap.
         memRigidBodies = new UnmanagedActiveList<RigidBodyData>(numBodies);
         memContacts = new UnmanagedActiveList<ContactData>(numContacts);
         memConstraints = new UnmanagedActiveList<ConstraintData>(numConstraints);

src/JitterDemo/Demos/Demo16.cs

@@ -73,8 +73,8 @@ public void Draw()
         {
             foreach (var spring in SoftBodyCube.Edges)
             {
-                dr.PushLine(DebugRenderer.Color.Green, Conversion.FromJitter(cube.Points[spring.Item1].Position),
-                    Conversion.FromJitter(cube.Points[spring.Item2].Position));
+                dr.PushLine(DebugRenderer.Color.Green, Conversion.FromJitter(cube.Vertices[spring.Item1].Position),
+                    Conversion.FromJitter(cube.Vertices[spring.Item2].Position));
 
                 dr.PushPoint(DebugRenderer.Color.White, Conversion.FromJitter(cube.Center.Position), 0.2f);
             }

src/JitterDemo/Demos/Demo17.cs

@@ -65,19 +65,19 @@ public void Build()
         world.DynamicTree.Filter = DynamicTreeCollisionFilter.Filter;
         world.BroadPhaseFilter = new BroadPhaseCollisionFilter(world);
 
-        RigidBody fb0 = cloth.Points.OrderByDescending(item => +item.Position.X + item.Position.Z).First();
+        RigidBody fb0 = cloth.Vertices.OrderByDescending(item => +item.Position.X + item.Position.Z).First();
         var c0 = world.CreateConstraint<BallSocket>(fb0, world.NullBody);
         c0.Initialize(fb0.Position);
 
-        RigidBody fb1 = cloth.Points.OrderByDescending(item => +item.Position.X - item.Position.Z).First();
+        RigidBody fb1 = cloth.Vertices.OrderByDescending(item => +item.Position.X - item.Position.Z).First();
         var c1 = world.CreateConstraint<BallSocket>(fb1, world.NullBody);
         c1.Initialize(fb1.Position);
 
-        RigidBody fb2 = cloth.Points.OrderByDescending(item => -item.Position.X + item.Position.Z).First();
+        RigidBody fb2 = cloth.Vertices.OrderByDescending(item => -item.Position.X + item.Position.Z).First();
         var c2 = world.CreateConstraint<BallSocket>(fb2, world.NullBody);
         c2.Initialize(fb2.Position);
 
-        RigidBody fb3 = cloth.Points.OrderByDescending(item => -item.Position.X - item.Position.Z).First();
+        RigidBody fb3 = cloth.Vertices.OrderByDescending(item => -item.Position.X - item.Position.Z).First();
         var c3 = world.CreateConstraint<BallSocket>(fb3, world.NullBody);
         c3.Initialize(fb3.Position);