diff --git a/src/Jitter2/Collision/CollisionIsland.cs b/src/Jitter2/Collision/CollisionIsland.cs
index ba9b35bb..4987a4db 100644
--- a/src/Jitter2/Collision/CollisionIsland.cs
+++ b/src/Jitter2/Collision/CollisionIsland.cs
@@ -42,7 +42,7 @@ public sealed class Island : IListIndex
     public ReadOnlyHashSet<RigidBody> Bodies { get; private set; }
 
     int IListIndex.ListIndex { get; set; } = -1;
-    
+
     /// <summary>
     /// Initializes a new instance of the <see cref="Island"/> class.
     /// </summary>
diff --git a/src/Jitter2/Collision/DynamicTree.cs b/src/Jitter2/Collision/DynamicTree.cs
index ab9677e7..d5509545 100644
--- a/src/Jitter2/Collision/DynamicTree.cs
+++ b/src/Jitter2/Collision/DynamicTree.cs
@@ -181,7 +181,7 @@ void SetTime(Timings type)
             {
                 var sl = lists[ntask];
                 updatedProxies += sl.Count;
-                
+
                 for (int i = 0; i < sl.Count; i++)
                 {
                     T proxy = sl[i];
diff --git a/src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs b/src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs
index 15464437..7a0face8 100644
--- a/src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs
+++ b/src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs
@@ -108,7 +108,7 @@ public static bool Equals(in Edge a, in Edge b)
     /// The return value may be invalidated by subsequent calls to <see cref="AddVertex"/> or <see cref="AddPoint"/>.
     /// </summary>
     public readonly bool OriginEnclosed => originEnclosed;
-    
+
     /// <summary>
     /// Computes the barycentric coordinates of the origin projected onto a given triangle. 
     /// These coordinates are used to retrieve points in A- and B-space.
diff --git a/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs b/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs
index 3b78a506..36fa190b 100644
--- a/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs
+++ b/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs
@@ -508,7 +508,7 @@ public static bool Raycast(ISupportMap support, in JMatrix orientation,
 
         return result;
     }
-    
+
     /// <summary>
     /// Performs a raycast against a shape.
     /// </summary>
@@ -525,12 +525,12 @@ public static bool Raycast(ISupportMap support, in JVector origin, in JVector di
     {
         solver.MKD.SupportA = support;
         solver.MKD.SupportB = null!;
-        
+
         bool result = solver.Raycast(origin, direction, out fraction, out normal);
 
         return result;
     }
-    
+
     /// <summary>
     /// Determines whether two convex shapes overlap, providing detailed information for both overlapping and separated
     /// cases. Internally, the method employs the Expanding Polytope Algorithm (EPA) to gather collision information.
@@ -633,7 +633,7 @@ public static bool MPREPA(ISupportMap supportA, ISupportMap supportB,
 
         return res;
     }
-    
+
     /// <summary>
     /// Detects whether two convex shapes overlap and provides detailed collision information.
     /// It assumes that support shape A is at position zero and not rotated.
@@ -669,5 +669,4 @@ public static bool MPREPA(ISupportMap supportA, ISupportMap supportB,
 
         return res;
     }
-
 }
\ No newline at end of file
diff --git a/src/Jitter2/Collision/Shapes/Shape.cs b/src/Jitter2/Collision/Shapes/Shape.cs
index 41b77b32..1485860e 100644
--- a/src/Jitter2/Collision/Shapes/Shape.cs
+++ b/src/Jitter2/Collision/Shapes/Shape.cs
@@ -43,7 +43,7 @@ public abstract class Shape : ISupportMap, IListIndex, IDynamicTreeProxy
     /// arranging shapes in a well-defined order.
     /// </summary>
     public readonly ulong ShapeId;
-    
+
     public Shape()
     {
         ShapeId = World.IdCounter++;
@@ -51,10 +51,17 @@ public Shape()
 
     internal bool AttachRigidBody(RigidBody? body)
     {
-        RigidBody ??= body;
-        return RigidBody == body;
+        if (RigidBody == null)
+        {
+            RigidBody = body;
+            return true;
+        }
+
+        return false;
     }
 
+    public bool IsRegistered => (this as IListIndex).ListIndex != -1;
+
     internal void DetachRigidBody()
     {
         RigidBody = null!;
@@ -63,7 +70,7 @@ internal void DetachRigidBody()
     /// <summary>
     /// The instance of <see cref="RigidBody"/> to which this shape is attached.
     /// </summary>
-    public RigidBody? RigidBody { get; private set; } = null!;
+    public RigidBody? RigidBody { get; private set; }
 
     /// <summary>
     /// The bounding box of the shape in world space. It is automatically updated when the position or
diff --git a/src/Jitter2/Collision/Shapes/SphereShape.cs b/src/Jitter2/Collision/Shapes/SphereShape.cs
index 322ea95b..7348d8b9 100644
--- a/src/Jitter2/Collision/Shapes/SphereShape.cs
+++ b/src/Jitter2/Collision/Shapes/SphereShape.cs
@@ -63,7 +63,7 @@ public override void SupportMap(in JVector direction, out JVector result)
         result.Normalize();
         JVector.Multiply(result, radius, out result);
     }
-    
+
     public override void CalculateBoundingBox(in JMatrix orientation, in JVector position, out JBBox box)
     {
         box.Min.X = -radius;
diff --git a/src/Jitter2/Collision/Shapes/TriangleShape.cs b/src/Jitter2/Collision/Shapes/TriangleShape.cs
index 4b4cfb37..82f4cdb3 100644
--- a/src/Jitter2/Collision/Shapes/TriangleShape.cs
+++ b/src/Jitter2/Collision/Shapes/TriangleShape.cs
@@ -77,7 +77,7 @@ public void GetWorldVertices(out JVector a, out JVector b, out JVector c)
         c = Mesh.Vertices[triangle.IndexC];
 
         if (RigidBody == null) return;
-        
+
         ref JMatrix orientation = ref RigidBody.Data.Orientation;
         ref JVector position = ref RigidBody.Data.Position;
 
diff --git a/src/Jitter2/Collision/TriangleMesh.cs b/src/Jitter2/Collision/TriangleMesh.cs
index 5400cdb7..bdced104 100644
--- a/src/Jitter2/Collision/TriangleMesh.cs
+++ b/src/Jitter2/Collision/TriangleMesh.cs
@@ -23,6 +23,7 @@
 
 using System;
 using System.Collections.Generic;
+using System.Runtime.CompilerServices;
 using Jitter2.LinearMath;
 
 namespace Jitter2.Collision;
@@ -45,7 +46,6 @@ public DegenerateTriangleException(string message, Exception inner) : base(messa
     }
 }
 
-
 /// <summary>
 /// Encapsulates the data of a triangle mesh. An instance of this can be supplied to the <see cref="Jitter2.Collision.Shapes.TriangleShape"/>.
 /// The triangles within this class contain indices pointing to neighboring triangles.
@@ -85,7 +85,7 @@ public Triangle(int a, int b, int c)
 #if NET6_0
             System.Runtime.CompilerServices.Unsafe.SkipInit(out Normal);
 #endif
-            
+
             IndexA = a;
             IndexB = b;
             IndexC = c;
@@ -116,7 +116,7 @@ public TriangleMesh(List<JTriangle> triangles)
     {
         Dictionary<JVector, int> tmpIndices = new();
         List<JVector> tmpVertices = new();
-        
+
         // 1. step: build indices and vertices for triangles (JTriangle contains raw x, y, z coordinates).
 
         Indices = new Triangle[triangles.Count];
@@ -125,7 +125,7 @@ int PushVector(JVector v)
         {
             if (!tmpIndices.TryGetValue(v, out int result))
             {
-                result = (int)tmpVertices.Count;
+                result = tmpVertices.Count;
                 tmpIndices.Add(v, result);
                 tmpVertices.Add(v);
             }
@@ -145,9 +145,9 @@ int PushVector(JVector v)
         }
 
         Vertices = tmpVertices.ToArray();
-        
+
         // 2. step: Identify the neighbors.
-        
+
         Dictionary<Edge, int> tmpEdges = new();
 
         int GetEdge(Edge e)
@@ -179,7 +179,7 @@ int GetEdge(Edge e)
             JVector A = Vertices[Indices[i].IndexA];
             JVector B = Vertices[Indices[i].IndexB];
             JVector C = Vertices[Indices[i].IndexC];
-            
+
             JVector normal = (C - A) % (B - A);
 
             if (MathHelper.CloseToZero(normal, 1e-12f))
diff --git a/src/Jitter2/DataStructures/ActiveList.cs b/src/Jitter2/DataStructures/ActiveList.cs
index cc6cddf6..61020eb3 100644
--- a/src/Jitter2/DataStructures/ActiveList.cs
+++ b/src/Jitter2/DataStructures/ActiveList.cs
@@ -47,7 +47,10 @@ public ReadOnlyActiveList(ActiveList<T> list)
 
     public T this[int i] => list[i];
 
-    public bool IsActive(T element) => list.IsActive(element);
+    public bool IsActive(T element)
+    {
+        return list.IsActive(element);
+    }
 
     public IEnumerator<T> GetEnumerator()
     {
@@ -151,7 +154,10 @@ private void Swap(int index0, int index1)
         elements[index1].ListIndex = index1;
     }
 
-    public bool IsActive(T element) => (element.ListIndex < Active);
+    public bool IsActive(T element)
+    {
+        return (element.ListIndex < Active);
+    }
 
     public void MoveToActive(T element)
     {
diff --git a/src/Jitter2/DataStructures/ReadOnlyHashset.cs b/src/Jitter2/DataStructures/ReadOnlyHashset.cs
index 5b9cfb88..2ee58ed5 100644
--- a/src/Jitter2/DataStructures/ReadOnlyHashset.cs
+++ b/src/Jitter2/DataStructures/ReadOnlyHashset.cs
@@ -29,7 +29,6 @@ namespace Jitter2.DataStructures;
 /// <summary>
 /// Implements a wrapper for <see cref="HashSet{T}"/>, eliminating garbage collection (GC) overhead during enumeration.
 /// </summary>
-
 public class ReadOnlyHashSet<T> : IReadOnlyCollection<T>
 {
     private readonly HashSet<T> hashset;
diff --git a/src/Jitter2/Dynamics/Arbiter.cs b/src/Jitter2/Dynamics/Arbiter.cs
index 82b36c7a..8b9d4b4a 100644
--- a/src/Jitter2/Dynamics/Arbiter.cs
+++ b/src/Jitter2/Dynamics/Arbiter.cs
@@ -35,7 +35,7 @@ public class Arbiter
 
     public RigidBody Body1 = null!;
     public RigidBody Body2 = null!;
-    
+
     public JHandle<ContactData> Handle;
 }
 
diff --git a/src/Jitter2/Dynamics/Constraints/AngularMotor.cs b/src/Jitter2/Dynamics/Constraints/AngularMotor.cs
index c748df5f..4be4e8d7 100644
--- a/src/Jitter2/Dynamics/Constraints/AngularMotor.cs
+++ b/src/Jitter2/Dynamics/Constraints/AngularMotor.cs
@@ -100,7 +100,7 @@ public float TargetVelocity
     }
 
     public JVector LocalAxis1 => handle.Data.LocalAxis1;
-    
+
     public JVector LocalAxis2 => handle.Data.LocalAxis2;
 
     public float MaximumForce
diff --git a/src/Jitter2/Dynamics/Constraints/Constraint.cs b/src/Jitter2/Dynamics/Constraints/Constraint.cs
index 452004a1..186a4a99 100644
--- a/src/Jitter2/Dynamics/Constraints/Constraint.cs
+++ b/src/Jitter2/Dynamics/Constraints/Constraint.cs
@@ -22,7 +22,6 @@
  */
 
 using System;
-using System.Runtime.CompilerServices;
 using System.Runtime.InteropServices;
 using Jitter2.UnmanagedMemory;
 
@@ -69,7 +68,7 @@ public abstract class Constraint : IDebugDrawable
     /// </summary>
     public JHandle<ConstraintData> Handle { internal set; get; }
 
-    public JHandle<SmallConstraintData> SmallHandle => JHandle<ConstraintData>.AsHandle<SmallConstraintData>(this.Handle);
+    public JHandle<SmallConstraintData> SmallHandle => JHandle<ConstraintData>.AsHandle<SmallConstraintData>(Handle);
 
     /// <summary>
     /// This method must be overridden. It initializes the function pointers for
diff --git a/src/Jitter2/Dynamics/Constraints/DistanceLimit.cs b/src/Jitter2/Dynamics/Constraints/DistanceLimit.cs
index f775d8ac..9e66762d 100644
--- a/src/Jitter2/Dynamics/Constraints/DistanceLimit.cs
+++ b/src/Jitter2/Dynamics/Constraints/DistanceLimit.cs
@@ -151,10 +151,7 @@ public float TargetDistance
             ref DistanceLimitData data = ref handle.Data;
             data.Distance = value;
         }
-        get
-        {
-            return handle.Data.Distance;
-        }
+        get => handle.Data.Distance;
     }
 
     public float Distance
diff --git a/src/Jitter2/Dynamics/Constraints/LinearMotor.cs b/src/Jitter2/Dynamics/Constraints/LinearMotor.cs
index d174533d..124a7d75 100644
--- a/src/Jitter2/Dynamics/Constraints/LinearMotor.cs
+++ b/src/Jitter2/Dynamics/Constraints/LinearMotor.cs
@@ -67,9 +67,9 @@ protected override void Create()
         prepareForIteration = &PrepareForIteration;
         handle = JHandle<ConstraintData>.AsHandle<LinearMotorData>(Handle);
     }
-    
+
     public JVector LocalAxis1 => handle.Data.LocalAxis1;
-    
+
     public JVector LocalAxis2 => handle.Data.LocalAxis2;
 
     /// <summary>
diff --git a/src/Jitter2/Dynamics/RigidBody.cs b/src/Jitter2/Dynamics/RigidBody.cs
index f48856d5..0a8016d0 100644
--- a/src/Jitter2/Dynamics/RigidBody.cs
+++ b/src/Jitter2/Dynamics/RigidBody.cs
@@ -62,7 +62,7 @@ public struct RigidBodyData
 public sealed class RigidBody : IListIndex, IDebugDrawable
 {
     internal JHandle<RigidBodyData> handle;
-    
+
     public readonly ulong RigidBodyId;
 
     /// <summary>
@@ -319,7 +319,7 @@ private void AttachToShape(Shape shape)
     {
         if (!shape.AttachRigidBody(this))
         {
-            throw new InvalidOperationException("Shape has already been added to another body.");
+            throw new ArgumentException("Shape has already been added to another body.", nameof(shape));
         }
 
         if (shape.Mass == 0)
@@ -360,6 +360,11 @@ public void AddShape(IEnumerable<Shape> shapes, bool setMassInertia = true)
     /// mass properties, assuming a unit density for the shape. If false, the inertia and mass remain unchanged.</param>
     public void AddShape(Shape shape, bool setMassInertia = true)
     {
+        if (shape.IsRegistered)
+        {
+            throw new ArgumentException("Shape can not be added. Is the shape already registered?");
+        }
+
         AttachToShape(shape);
         shapes.Add(shape);
         if (setMassInertia) SetMassInertia();
@@ -413,7 +418,7 @@ public void RemoveShape(Shape shape, bool setMassInertia = true)
     {
         if (!shapes.Remove(shape))
         {
-            throw new InvalidOperationException(
+            throw new ArgumentException(
                 "Shape is not part of this body.");
         }
 
@@ -421,7 +426,7 @@ public void RemoveShape(Shape shape, bool setMassInertia = true)
 
         foreach (var contact in Contacts)
         {
-            if (contact.Handle.Data.Key.Key1 == shape.ShapeId || contact.Handle.Data.Key.Key2 == shape.ShapeId )
+            if (contact.Handle.Data.Key.Key1 == shape.ShapeId || contact.Handle.Data.Key.Key2 == shape.ShapeId)
             {
                 toRemoveArbiter.Push(contact);
             }
diff --git a/src/Jitter2/Jitter2.csproj b/src/Jitter2/Jitter2.csproj
index d022c602..138685f1 100644
--- a/src/Jitter2/Jitter2.csproj
+++ b/src/Jitter2/Jitter2.csproj
@@ -13,8 +13,8 @@
 
     <PropertyGroup>
         <Description>
-        Jitter Physics 2, the evolution of Jitter Physics, is an impulse-based dynamics engine with a semi-implicit Euler integrator.
-        It is a fast, simple, and dependency-free engine written in C# with a clear and user-friendly API.
+            Jitter Physics 2, the evolution of Jitter Physics, is an impulse-based dynamics engine with a semi-implicit Euler integrator.
+            It is a fast, simple, and dependency-free engine written in C# with a clear and user-friendly API.
         </Description>
         <GenerateDocumentationFile>true</GenerateDocumentationFile>
         <PackageReadmeFile>readme.md</PackageReadmeFile>
@@ -32,8 +32,8 @@
     </PropertyGroup>
 
     <ItemGroup>
-      <None Include="./../../publish/readme.md" Pack="true" PackagePath=""/>
-      <None Include="./../../publish/icon.png" Pack="true" PackagePath=""/>
+        <None Include="./../../publish/readme.md" Pack="true" PackagePath=""/>
+        <None Include="./../../publish/icon.png" Pack="true" PackagePath=""/>
     </ItemGroup>
 
     <PropertyGroup Condition="'$(Configuration)' == 'Release'">
diff --git a/src/Jitter2/LinearMath/MathHelper.cs b/src/Jitter2/LinearMath/MathHelper.cs
index 8eee0dee..4eec9c93 100644
--- a/src/Jitter2/LinearMath/MathHelper.cs
+++ b/src/Jitter2/LinearMath/MathHelper.cs
@@ -67,14 +67,14 @@ public static bool LineLineIntersect(in JVector p1, in JVector p2, in JVector p3
     }
 
     */
-    
+
     public static JMatrix InverseSquareRoot(JMatrix m, int sweeps = 2)
     {
         float phi, cp, sp;
         Unsafe.SkipInit(out JMatrix r);
-        
+
         JMatrix rotation = JMatrix.Identity;
-        
+
         for (int i = 0; i < sweeps; i++)
         {
             // M32
@@ -111,10 +111,10 @@ public static JMatrix InverseSquareRoot(JMatrix m, int sweeps = 2)
             }
         }
 
-        JMatrix d = new JMatrix(1.0f / MathF.Sqrt(m.M11), 0, 0, 
-            0, 1.0f / MathF.Sqrt(m.M22), 0, 
+        JMatrix d = new JMatrix(1.0f / MathF.Sqrt(m.M11), 0, 0,
+            0, 1.0f / MathF.Sqrt(m.M22), 0,
             0, 0, 1.0f / MathF.Sqrt(m.M33));
-        
+
         return rotation * d * JMatrix.Transpose(rotation);
     }
 
diff --git a/src/Jitter2/Parallelization/ThreadPool.cs b/src/Jitter2/Parallelization/ThreadPool.cs
index 790ba29f..d56fd65b 100644
--- a/src/Jitter2/Parallelization/ThreadPool.cs
+++ b/src/Jitter2/Parallelization/ThreadPool.cs
@@ -107,10 +107,7 @@ private ThreadPool()
         ChangeThreadCount(ThreadCountSuggestion);
     }
 
-    public static int ThreadCountSuggestion
-    {
-        get => Math.Max((int)(Environment.ProcessorCount * ThreadsPerProcessor), 1);
-    }
+    public static int ThreadCountSuggestion => Math.Max((int)(Environment.ProcessorCount * ThreadsPerProcessor), 1);
 
     /// <summary>
     /// Changes the number of worker threads.
diff --git a/src/Jitter2/SoftBodies/SoftBody.cs b/src/Jitter2/SoftBodies/SoftBody.cs
index 7b285555..297e577c 100644
--- a/src/Jitter2/SoftBodies/SoftBody.cs
+++ b/src/Jitter2/SoftBodies/SoftBody.cs
@@ -22,9 +22,7 @@
  */
 
 using System.Collections.Generic;
-using System.Threading;
 using Jitter2.Collision.Shapes;
-using Jitter2.DataStructures;
 using Jitter2.Dynamics;
 using Jitter2.Dynamics.Constraints;
 
@@ -32,13 +30,9 @@ namespace Jitter2.SoftBodies;
 
 public class SoftBody
 {
-    private List<RigidBody> points = new();
-    private List<Constraint> springs = new();
-    private List<Shape> shapes = new();
-
-    public ReadOnlyList<RigidBody> Points { get; }
-    public ReadOnlyList<Constraint> Springs  { get; }
-    public ReadOnlyList<Shape> Shapes  { get; }
+    public List<RigidBody> Points { get; } = new();
+    public List<Constraint> Springs { get; } = new();
+    public List<Shape> Shapes { get; } = new();
 
     protected World world;
 
@@ -48,16 +42,8 @@ public SoftBody(World world)
     {
         this.world = world;
         world.PostStep += WorldOnPostStep;
-
-        this.Points = new ReadOnlyList<RigidBody>(points);
-        this.Springs = new ReadOnlyList<Constraint>(springs);
-        this.Shapes = new ReadOnlyList<Shape>(shapes);
     }
 
-    public void Add(Shape shape) => shapes.Add(shape);
-    public void Add(Constraint constraint) => springs.Add(constraint);
-    public void Add(RigidBody bodies) => points.Add(bodies);
-
     public void Destroy()
     {
         world.PostStep -= WorldOnPostStep;
diff --git a/src/Jitter2/SoftBodies/SoftBodyTriangle.cs b/src/Jitter2/SoftBodies/SoftBodyTriangle.cs
index e923100a..08bb07cc 100644
--- a/src/Jitter2/SoftBodies/SoftBodyTriangle.cs
+++ b/src/Jitter2/SoftBodies/SoftBodyTriangle.cs
@@ -51,7 +51,7 @@ public SoftBodyTriangle(SoftBody body, RigidBody p1, RigidBody p2, RigidBody p3)
     }
 
     public override JVector Velocity => 1.0f / 3.0f * (p1.Data.Velocity + p2.Data.Velocity + p3.Data.Velocity);
-    
+
     public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out float mass)
     {
         inertia = JMatrix.Identity;
diff --git a/src/Jitter2/UnmanagedMemory/UnmanagedActiveList.cs b/src/Jitter2/UnmanagedMemory/UnmanagedActiveList.cs
index 91d73443..8e991508 100644
--- a/src/Jitter2/UnmanagedMemory/UnmanagedActiveList.cs
+++ b/src/Jitter2/UnmanagedMemory/UnmanagedActiveList.cs
@@ -202,12 +202,12 @@ public JHandle<T> Allocate(bool active = false, bool clear = false)
                 throw new MaximumSizeException($"{nameof(UnmanagedActiveList<T>)} reached " +
                                                $"its maximum size limit ({nameof(maximumSize)}={maximumSize}).");
             }
-            
+
             size = Math.Min(2 * osize, maximumSize);
 
             Trace.WriteLine($"{nameof(UnmanagedActiveList<T>)}: " +
                             $"Resizing to {size}x{typeof(T)} ({size}x{sizeof(T)} Bytes).");
-            
+
             var oldmemory = memory;
             memory = (T*)Marshal.AllocHGlobal(size * sizeof(T));
 
diff --git a/src/Jitter2/World.Detect.cs b/src/Jitter2/World.Detect.cs
index a91e89f1..9bda2670 100644
--- a/src/Jitter2/World.Detect.cs
+++ b/src/Jitter2/World.Detect.cs
@@ -247,7 +247,7 @@ public void RegisterContact(ulong id0, ulong id1, RigidBody body0, RigidBody bod
         in JVector point1, in JVector point2, in JVector normal, float penetration)
     {
         GetArbiter(id0, id1, body0, body1, out Arbiter arbiter);
-        
+
         lock (arbiter)
         {
             memContacts.ResizeLock.EnterReadLock();
@@ -259,7 +259,7 @@ public void RegisterContact(ulong id0, ulong id1, RigidBody body0, RigidBody bod
     [ThreadStatic] private static ConvexHullIntersection cvh;
 
     [MethodImpl(MethodImplOptions.AggressiveInlining)]
-    private unsafe void Detect(Shape sA, Shape sB)
+    private void Detect(Shape sA, Shape sB)
     {
         if (sB.ShapeId < sA.ShapeId)
         {
@@ -421,7 +421,7 @@ static void Support(Shape shape, in JVector direction, out JVector v)
             memContacts.ResizeLock.ExitReadLock();
         }
     }
-    
+
     [MethodImpl(MethodImplOptions.AggressiveInlining)]
     public void GetArbiter(ulong id0, ulong id1, RigidBody b0, RigidBody b1, out Arbiter arbiter)
     {
diff --git a/src/Jitter2/World.Raycast.cs b/src/Jitter2/World.Raycast.cs
index 8fa4a596..71ebbb6e 100644
--- a/src/Jitter2/World.Raycast.cs
+++ b/src/Jitter2/World.Raycast.cs
@@ -93,6 +93,7 @@ public bool Raycast(Shape shape, JVector origin, JVector direction, out JVector
         {
             return NarrowPhase.Raycast(shape, origin, direction, out fraction, out normal);
         }
+
         ref RigidBodyData body = ref shape.RigidBody.Data;
 
         if (shape is TriangleShape tms)
diff --git a/src/Jitter2/World.Step.cs b/src/Jitter2/World.Step.cs
index f38b6306..ad406cba 100644
--- a/src/Jitter2/World.Step.cs
+++ b/src/Jitter2/World.Step.cs
@@ -127,7 +127,7 @@ void SetTime(Timings type)
         {
             ThreadPool.Instance.SignalWait();
         }
-        
+
         PreStep?.Invoke(dt);
 
         SetTime(Timings.Integrate);
@@ -138,7 +138,7 @@ void SetTime(Timings type)
 
         HandleDeferredArbiters();
         SetTime(Timings.AddArbiter);
-        
+
         CheckDeactivation();
         SetTime(Timings.CheckDeactivation);
 
@@ -190,7 +190,7 @@ void SetTime(Timings type)
         // New arbiters are added to deferredArbiters
         DynamicTree.Update(multiThread);
         SetTime(Timings.CollisionDetect2);
-        
+
         PostStep?.Invoke(dt);
 
         // Signal the threadpool that threads can go into a wait state. If threadModel is set to
@@ -316,7 +316,7 @@ private void PrepareContactsCallback(Parallel.Batch batch)
             UnlockTwoBody(ref b1, ref b2);
         }
     }
-    
+
     private unsafe void PrepareSmallConstraintsCallback(Parallel.Batch batch)
     {
         float istep_dt = 1.0f / step_dt;
@@ -360,7 +360,7 @@ private unsafe void IterateSmallConstraintsCallback(Parallel.Batch batch)
             UnlockTwoBody(ref b1, ref b2);
         }
     }
-    
+
     private unsafe void PrepareConstraintsCallback(Parallel.Batch batch)
     {
         float istep_dt = 1.0f / step_dt;
@@ -466,7 +466,7 @@ private void AssertNullBody()
         Debug.Assert(rigidBody.InverseMass == 0.0f);
         Debug.Assert(rigidBody.InverseInertiaWorld.Equals(JMatrix.Zero));
     }
-    
+
     private void ForeachActiveShape(bool multiThread)
     {
         if (multiThread)
@@ -543,12 +543,12 @@ private void HandleDeferredArbiters()
         {
             Arbiter arb = deferredArbiters.Pop();
             IslandHelper.ArbiterCreated(islands, arb);
-            
+
             AddToActiveList(arb.Body1.island);
             AddToActiveList(arb.Body2.island);
         }
     }
-    
+
     /// <summary>
     /// Spin-wait loop to prevent accessing a body from multiple threads.
     /// </summary>
@@ -557,13 +557,31 @@ public static void LockTwoBody(ref RigidBodyData b1, ref RigidBodyData b2)
     {
         if (Unsafe.IsAddressGreaterThan(ref b1, ref b2))
         {
-            if (!b1.IsStatic) while (Interlocked.CompareExchange(ref b1._lockFlag, 1, 0) != 0) { Thread.SpinWait(10); }
-            if (!b2.IsStatic) while (Interlocked.CompareExchange(ref b2._lockFlag, 1, 0) != 0) { Thread.SpinWait(10); }
+            if (!b1.IsStatic)
+                while (Interlocked.CompareExchange(ref b1._lockFlag, 1, 0) != 0)
+                {
+                    Thread.SpinWait(10);
+                }
+
+            if (!b2.IsStatic)
+                while (Interlocked.CompareExchange(ref b2._lockFlag, 1, 0) != 0)
+                {
+                    Thread.SpinWait(10);
+                }
         }
         else
         {
-            if (!b2.IsStatic) while (Interlocked.CompareExchange(ref b2._lockFlag, 1, 0) != 0) { Thread.SpinWait(10); }
-            if (!b1.IsStatic) while (Interlocked.CompareExchange(ref b1._lockFlag, 1, 0) != 0) { Thread.SpinWait(10); }
+            if (!b2.IsStatic)
+                while (Interlocked.CompareExchange(ref b2._lockFlag, 1, 0) != 0)
+                {
+                    Thread.SpinWait(10);
+                }
+
+            if (!b1.IsStatic)
+                while (Interlocked.CompareExchange(ref b1._lockFlag, 1, 0) != 0)
+                {
+                    Thread.SpinWait(10);
+                }
         }
     }
 
diff --git a/src/Jitter2/World.cs b/src/Jitter2/World.cs
index 1ab34380..8d8189fe 100644
--- a/src/Jitter2/World.cs
+++ b/src/Jitter2/World.cs
@@ -32,7 +32,6 @@
 using Jitter2.Dynamics;
 using Jitter2.Dynamics.Constraints;
 using Jitter2.LinearMath;
-using Jitter2.SoftBodies;
 using Jitter2.UnmanagedMemory;
 
 namespace Jitter2;
@@ -68,19 +67,22 @@ public SpanData(World world)
 
         public readonly Span<ConstraintData> ActiveConstraints => world.memConstraints.Active;
         public readonly Span<ConstraintData> Constraints => world.memConstraints.Elements;
+
+        public readonly Span<SmallConstraintData> ActiveSmallConstraints => world.memSmallConstraints.Active;
+        public readonly Span<SmallConstraintData> SmallConstraints => world.memSmallConstraints.Elements;
     }
 
     private readonly UnmanagedActiveList<ContactData> memContacts;
     private readonly UnmanagedActiveList<RigidBodyData> memRigidBodies;
     private readonly UnmanagedActiveList<ConstraintData> memConstraints;
     private readonly UnmanagedActiveList<SmallConstraintData> memSmallConstraints;
-    
+
     public delegate void WorldStep(float dt);
-        
+
     // Post- and Pre-step
     public event WorldStep? PreStep;
     public event WorldStep? PostStep;
-    
+
     /// <summary>
     /// Grants access to objects residing in unmanaged memory. This operation can be potentially unsafe. Utilize
     /// the corresponding native properties where possible to mitigate risk.
@@ -92,9 +94,9 @@ public SpanData(World world)
     private readonly ActiveList<Island> islands = new();
     private readonly ActiveList<RigidBody> bodies = new();
     private readonly ActiveList<Shape> shapes = new();
-    
+
     public static ulong IdCounter;
-    
+
     /// <summary>
     /// Defines the two available thread models. The <see cref="ThreadModelType.Persistent"/> model keeps the worker
     /// threads active continuously, even when the <see cref="World.Step(float, bool)"/> is not in operation, which might
@@ -169,6 +171,7 @@ public int NumberSubsteps
                 throw new ArgumentOutOfRangeException(nameof(value),
                     "The number of substeps has to be larger than zero.");
             }
+
             substeps = value;
         }
     }
@@ -243,9 +246,9 @@ public void Clear()
         {
             Remove(bodyStack.Pop());
         }
-        
+
         // Left-over shapes not associated with a rigid body.
-        Stack<Shape> shapeStack = new Stack<Shape>(this.shapes);
+        Stack<Shape> shapeStack = new Stack<Shape>(shapes);
 
         while (shapeStack.Count > 0)
         {
@@ -312,7 +315,7 @@ public void Remove(Constraint constraint)
         {
             memConstraints.Free(constraint.Handle);
         }
-        
+
         constraint.Handle = JHandle<ConstraintData>.Zero;
     }
 
@@ -331,7 +334,7 @@ public void Remove(Arbiter arbiter)
         memContacts.Free(arbiter.Handle);
 
         Arbiter.Pool.Push(arbiter);
-        
+
         arbiter.Handle = JHandle<ContactData>.Zero;
     }
 
@@ -346,11 +349,11 @@ internal void UpdateShape(Shape shape)
     /// </summary>
     public void AddShape(Shape shape)
     {
-        if ((shape as IListIndex).ListIndex != -1)
+        if (shape.IsRegistered)
         {
-            throw new ArgumentException("Shape already registered.");
+            throw new ArgumentException("Shape can not be added. Is the shape already registered?");
         }
-        
+
         shapes.Add(shape, true);
         shape.UpdateWorldBoundingBox();
         DynamicTree.AddProxy(shape);
@@ -360,9 +363,9 @@ public void Remove(Shape shape)
     {
         if (shape.RigidBody != null)
         {
-            throw new InvalidOperationException("Shape can not be removed because it is attached to a rigid body.");
+            throw new ArgumentException("Shape can not be removed because it is attached to a rigid body.");
         }
-        
+
         if ((shape as IListIndex).ListIndex == -1)
         {
             throw new ArgumentException("Shape can not be removed because it is not registered.");
@@ -371,15 +374,15 @@ public void Remove(Shape shape)
         DynamicTree.RemoveProxy(shape);
         shapes.Remove(shape);
     }
-    
+
     public void DeactivateShape(Shape shape)
-    {        
+    {
         if (shape.RigidBody != null)
         {
             throw new InvalidOperationException(
                 "Can not modify activation state if a shape which is attached to a rigid body.");
         }
-        
+
         shapes.MoveToInactive(shape);
     }
 
diff --git a/src/JitterDemo/Demos/Common.cs b/src/JitterDemo/Demos/Common.cs
index 09ba4fa7..65458545 100644
--- a/src/JitterDemo/Demos/Common.cs
+++ b/src/JitterDemo/Demos/Common.cs
@@ -24,7 +24,7 @@ public bool Filter(Shape shapeA, Shape shapeB)
 
             if (b < a) (a, b) = (b, a);
 
-            bool contains = ignore.Contains(new (a, b));
+            bool contains = ignore.Contains(new ValueTuple<ulong, ulong>(a, b));
             return contains;
         }
 
@@ -32,10 +32,10 @@ public void IgnoreCollisionBetween(Shape shapeA, Shape shapeB)
         {
             ulong a = shapeA.ShapeId;
             ulong b = shapeB.ShapeId;
-            
+
             if (b < a) (a, b) = (b, a);
 
-            ignore.Add(new (a, b));
+            ignore.Add(new ValueTuple<ulong, ulong>(a, b));
         }
     }
 
diff --git a/src/JitterDemo/Demos/Demo16.cs b/src/JitterDemo/Demos/Demo16.cs
index 74c7405f..33123232 100644
--- a/src/JitterDemo/Demos/Demo16.cs
+++ b/src/JitterDemo/Demos/Demo16.cs
@@ -71,7 +71,7 @@ public void Draw()
         var dr = RenderWindow.Instance.DebugRenderer;
         foreach (var cube in cubes)
         {
-            foreach (var spring in cube.Edges)
+            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));
diff --git a/src/JitterDemo/Demos/IDemo.cs b/src/JitterDemo/Demos/IDemo.cs
index aa9d1397..29d03a4d 100644
--- a/src/JitterDemo/Demos/IDemo.cs
+++ b/src/JitterDemo/Demos/IDemo.cs
@@ -1,6 +1,5 @@
 namespace JitterDemo;
 
-
 public interface ICleanDemo
 {
     public void CleanUp();
diff --git a/src/JitterDemo/Demos/SoftBody/PressurizedSphere.cs b/src/JitterDemo/Demos/SoftBody/PressurizedSphere.cs
index 71fad1e1..5dc9d628 100644
--- a/src/JitterDemo/Demos/SoftBody/PressurizedSphere.cs
+++ b/src/JitterDemo/Demos/SoftBody/PressurizedSphere.cs
@@ -34,7 +34,7 @@ private static IEnumerable<JTriangle> GenSphereTriangles(JVector offset)
     {
         return ShapeHelper.MakeHull(new UnitSphere(offset));
     }
-    
+
     private static IEnumerable<JTriangle> GenSphereTrianglesFromMesh(JVector offset, string filename)
     {
         Mesh m = Mesh.LoadMesh(filename);
diff --git a/src/JitterDemo/Demos/SoftBody/SoftBodyCloth.cs b/src/JitterDemo/Demos/SoftBody/SoftBodyCloth.cs
index e0139f44..51747630 100644
--- a/src/JitterDemo/Demos/SoftBody/SoftBodyCloth.cs
+++ b/src/JitterDemo/Demos/SoftBody/SoftBodyCloth.cs
@@ -102,7 +102,7 @@ private void Build()
             RigidBody body = world.CreateRigidBody();
             body.SetMassInertia(JMatrix.Identity * 1000, 0.1f);
             body.Position = vertex;
-            this.Add(body);
+            Points.Add(body);
         }
 
         foreach (var edge in edges)
@@ -110,7 +110,7 @@ private void Build()
             var constraint = world.CreateConstraint<SpringConstraint>(Points[edge.IndexA], Points[edge.IndexB]);
             constraint.Initialize(Points[edge.IndexA].Position, Points[edge.IndexB].Position);
             constraint.Softness = 0.1f;
-            this.Add(constraint);
+            Springs.Add(constraint);
         }
 
         foreach (var triangle in triangles)
@@ -118,7 +118,7 @@ private void Build()
             var tri = new SoftBodyTriangle(this, Points[triangle.IndexA], Points[triangle.IndexB], Points[triangle.IndexC]);
             tri.UpdateWorldBoundingBox();
             world.AddShape(tri);
-            this.Add(tri);
+            Shapes.Add(tri);
         }
     }
 }
\ No newline at end of file
diff --git a/src/JitterDemo/Demos/SoftBody/SoftBodyCube.cs b/src/JitterDemo/Demos/SoftBody/SoftBodyCube.cs
index 9abeb6ef..9894a37b 100644
--- a/src/JitterDemo/Demos/SoftBody/SoftBodyCube.cs
+++ b/src/JitterDemo/Demos/SoftBody/SoftBodyCube.cs
@@ -9,9 +9,13 @@ namespace JitterDemo;
 
 public class SoftBodyCube : SoftBody
 {
-    public readonly ValueTuple<int, int>[] Edges = new ValueTuple<int, int>[12];
+    public static readonly ValueTuple<int, int>[] Edges = {
+        (0, 1), (1, 2), (2, 3), (3, 0),
+        (4, 5), (5, 6), (6, 7), (7, 4),
+        (0, 4), (1, 5), (2, 6), (3, 7)
+    };
 
-    public RigidBody Center { get; private set; }
+    public RigidBody Center { get; }
 
     public SoftBodyCube(World world, JVector offset) : base(world)
     {
@@ -31,7 +35,7 @@ public SoftBodyCube(World world, JVector offset) : base(world)
             var rb = world.CreateRigidBody();
             rb.SetMassInertia(JMatrix.Identity * 100000, 0.2f);
             rb.Position = vertices[i] + offset;
-            this.Add(rb);
+            Points.Add(rb);
         }
 
         SoftBodyTetrahedron[] tetrahedra = new SoftBodyTetrahedron[5];
@@ -45,7 +49,7 @@ public SoftBodyCube(World world, JVector offset) : base(world)
         {
             tetrahedra[i].UpdateWorldBoundingBox();
             world.AddShape(tetrahedra[i]);
-            this.Add(tetrahedra[i]);
+            Shapes.Add(tetrahedra[i]);
         }
 
         Center = world.CreateRigidBody();
@@ -58,18 +62,5 @@ public SoftBodyCube(World world, JVector offset) : base(world)
             constraint.Initialize(Points[i].Position);
             constraint.Softness = 1;
         }
-        
-        Edges[0] = new ValueTuple<int, int>(0, 1);
-        Edges[1] = new ValueTuple<int, int>(1, 2);
-        Edges[2] = new ValueTuple<int, int>(2, 3);
-        Edges[3] = new ValueTuple<int, int>(3, 0);
-        Edges[4] = new ValueTuple<int, int>(4, 5);
-        Edges[5] = new ValueTuple<int, int>(5, 6);
-        Edges[6] = new ValueTuple<int, int>(6, 7);
-        Edges[7] = new ValueTuple<int, int>(7, 4);
-        Edges[8] = new ValueTuple<int, int>(0, 4);
-        Edges[9] = new ValueTuple<int, int>(1, 5);
-        Edges[10] = new ValueTuple<int, int>(2, 6);
-        Edges[11] = new ValueTuple<int, int>(3, 7);
     }
 }
\ No newline at end of file
diff --git a/src/JitterDemo/Playground.Debug.cs b/src/JitterDemo/Playground.Debug.cs
index 3d5bbe16..66a4d2cb 100644
--- a/src/JitterDemo/Playground.Debug.cs
+++ b/src/JitterDemo/Playground.Debug.cs
@@ -65,7 +65,6 @@ public void DebugDraw()
                         foreach (Shape s in body.Shapes)
                             JBBox.CreateMerged(box, s.WorldBoundingBox, out box);
                     }
-
                 }
 
                 DebugRenderer.PushBox(DebugRenderer.Color.Red, Conversion.FromJitter(box.Min),
diff --git a/src/JitterDemo/Playground.Gui.cs b/src/JitterDemo/Playground.Gui.cs
index 78928e05..4ef0581a 100644
--- a/src/JitterDemo/Playground.Gui.cs
+++ b/src/JitterDemo/Playground.Gui.cs
@@ -125,6 +125,7 @@ void AddRow(string a, string b, string c)
             AddRow("Bodies", $"{data.RigidBodies.Length}", $"{data.ActiveRigidBodies.Length}");
             AddRow("Arbiter", $"{data.Contacts.Length}", $"{data.ActiveContacts.Length}");
             AddRow("Constraints", $"{data.Constraints.Length}", $"{data.ActiveConstraints.Length}");
+            AddRow("SmallConstraints", $"{data.SmallConstraints.Length}", $"{data.ActiveSmallConstraints.Length}");
             AddRow("Shapes", $"{World.Shapes.Count}", $"{World.Shapes.Active}");
 
             ImGui.EndTable();
diff --git a/src/JitterDemo/Playground.Picking.cs b/src/JitterDemo/Playground.Picking.cs
index ca144b86..fad284aa 100644
--- a/src/JitterDemo/Playground.Picking.cs
+++ b/src/JitterDemo/Playground.Picking.cs
@@ -1,4 +1,3 @@
-using System;
 using Jitter2.Collision.Shapes;
 using Jitter2.Dynamics;
 using Jitter2.Dynamics.Constraints;
@@ -65,7 +64,7 @@ private void Pick()
             grepBody = null;
             bool result = World.Raycast(origin, jdir, null, null,
                 out Shape? grepShape, out JVector rayn, out hitDistance);
-            
+
 
             if (grepShape != null)
             {
@@ -78,8 +77,6 @@ private void Pick()
             }
 
 
-            
-
             if (result && grepBody != null && !grepBody.IsStatic)
             {
                 grepping = true;
diff --git a/src/JitterDemo/Renderer/OpenGL/Objects/Shader.cs b/src/JitterDemo/Renderer/OpenGL/Objects/Shader.cs
index e1f74fe3..927fa5f7 100644
--- a/src/JitterDemo/Renderer/OpenGL/Objects/Shader.cs
+++ b/src/JitterDemo/Renderer/OpenGL/Objects/Shader.cs
@@ -23,6 +23,7 @@
 using System;
 using System.Collections.Generic;
 using System.Collections.ObjectModel;
+using System.Diagnostics;
 using JitterDemo.Renderer.OpenGL.Native;
 
 namespace JitterDemo.Renderer.OpenGL;
@@ -151,7 +152,7 @@ public void Link()
         {
             GL.GetProgramInfoLog(Handle, 1024, out int _, out string infoLog);
 #if DEBUG
-            System.Diagnostics.Debug.Fail(infoLog);
+            Debug.Fail(infoLog);
 #endif
             throw new ShaderLinkException(infoLog);
         }
diff --git a/src/JitterTests/AddRemoveTests.cs b/src/JitterTests/AddRemoveTests.cs
new file mode 100644
index 00000000..5737872b
--- /dev/null
+++ b/src/JitterTests/AddRemoveTests.cs
@@ -0,0 +1,69 @@
+namespace JitterTests;
+
+public class AddRemoveTests
+{
+    private World world = null!;
+
+    [SetUp]
+    public void Setup()
+    {
+        world = new World(100, 100, 100);
+    }
+
+    [TestCase]
+    public void AddRemoveShapes()
+    {
+        RigidBody rb = world.CreateRigidBody();
+        Shape sh1 = new SphereShape();
+        Shape sh2 = new SphereShape();
+
+        RigidBody rb2 = world.CreateRigidBody();
+        Shape sh3 = new SphereShape();
+        rb2.AddShape(sh3);
+
+        // add shape to world
+        world.AddShape(sh2);
+
+        // adding the same shape again should fail
+        Assert.That(() => world.AddShape(sh2), Throws.TypeOf<ArgumentException>());
+
+        // adding the same shape to a body should fail
+        Assert.That(() => rb.AddShape(sh2), Throws.TypeOf<ArgumentException>());
+
+        // removing from body should fail
+        Assert.That(() => rb.RemoveShape(sh2), Throws.TypeOf<ArgumentException>());
+
+        // removing from world should work
+        world.Remove(sh2);
+
+        // add shape to rigid body
+        rb.AddShape(sh2);
+
+        // add shape to rigid body
+        rb.AddShape(sh1);
+
+        // contact is added to deferredArbiters
+        world.Step(1.0f / 100, false);
+
+        // adding again to rigid body should fail
+        Assert.That(() => rb.AddShape(sh1), Throws.TypeOf<ArgumentException>());
+
+        // adding to world should fail
+        Assert.That(() => world.AddShape(sh1), Throws.TypeOf<ArgumentException>());
+
+        // removing shape from world should fail, since it is owned by the body
+        Assert.That(() => world.Remove(sh1), Throws.TypeOf<ArgumentException>());
+
+        // removing the shape here should work.
+        rb.RemoveShape(sh1);
+
+        // there should be only one shape registered.
+        Assert.That(rb.Shapes.Count, Is.EqualTo(1));
+
+        Assert.That(world.Shapes.Count, Is.EqualTo(2));
+
+        world.Clear();
+
+        Assert.That(world.Shapes.Count, Is.EqualTo(0));
+    }
+}
\ No newline at end of file