rigidbody, forces and collision testing

PhysicsEngine/Physics/Bodies/Body.cs

@@ -0,0 +1,123 @@
+using System;
+using System.Numerics;
+using Raylib_cs;
+
+namespace PhysicsEngine
+{
+    public class Body
+    {
+        public Vector2 Position;
+        public Vector2 Velocity;
+        public Vector2 ForceAccumulator;
+        public Color BaseColor = Color.Blue;
+        public float Rotation;
+        public float AngularVelocity;
+        public float TorqueAccumulator;
+        public float Mass;
+        public float InverseMass => Mass > 0.0f ? 1.0f / Mass : 0.0f;
+        public float MomentOfInertia;
+        public float InverseInertia => MomentOfInertia > 0.0f ? 1.0f / MomentOfInertia : 0.0f;
+        public float Restitution = 0.5f;
+        public float Friction = 0.2f;
+        public float DragCoefficient = 0.0f; // Drag per m^2 (ignored if 0.0)
+        public bool IsStatic => Mass == 0.0f;
+
+        public void ApplyForce(Vector2 force)
+        {
+            if (IsStatic) return;
+            ForceAccumulator += force;
+        }
+
+        public void ApplyTorque(float torque)
+        {
+            if (IsStatic) return;
+            TorqueAccumulator += torque;
+        }
+
+        // --- Impulse Methods ---
+
+        public void ApplyImpulse(Vector2 impulse)
+        {
+            if (IsStatic) return;
+            Velocity += impulse * InverseMass;
+        }
+
+        public void ApplyImpulseAtOffset(Vector2 impulse, Vector2 localOffset)
+        {
+            if (IsStatic) return;
+
+            // Direct linear velocity change
+            Velocity += impulse * InverseMass;
+
+            // Rotate local offset into world space based on body rotation
+            float cos = MathF.Cos(Rotation);
+            float sin = MathF.Sin(Rotation);
+            Vector2 worldOffset = new Vector2(
+                localOffset.X * cos - localOffset.Y * sin,
+                localOffset.X * sin + localOffset.Y * cos
+            );
+
+            // 2D Cross product for torque: r x J
+            float torque = worldOffset.X * impulse.Y - worldOffset.Y * impulse.X;
+            AngularVelocity += torque * InverseInertia;
+        }
+
+        public void ApplyImpulseAtWorldPosition(Vector2 impulse, Vector2 worldPosition)
+        {
+            if (IsStatic) return;
+
+            // Direct linear velocity change
+            Velocity += impulse * InverseMass;
+
+            // Offset from center of mass in world space
+            Vector2 worldOffset = worldPosition - Position;
+
+            // 2D Cross product for torque: r x J
+            float torque = worldOffset.X * impulse.Y - worldOffset.Y * impulse.X;
+            AngularVelocity += torque * InverseInertia;
+        }
+
+        // --- Other Useful Methods to Consider ---
+
+        public void ApplyForceAtWorldPosition(Vector2 force, Vector2 worldPosition)
+        {
+            if (IsStatic) return;
+
+            ForceAccumulator += force;
+            Vector2 worldOffset = worldPosition - Position;
+            float torque = worldOffset.X * force.Y - worldOffset.Y * force.X;
+            TorqueAccumulator += torque;
+        }
+
+        public Vector2 GetWorldPointFromLocal(Vector2 localPoint)
+        {
+            float cos = MathF.Cos(Rotation);
+            float sin = MathF.Sin(Rotation);
+            Vector2 rotated = new Vector2(
+                localPoint.X * cos - localPoint.Y * sin,
+                localPoint.X * sin + localPoint.Y * cos
+            );
+            return Position + rotated;
+        }
+
+        public Vector2 GetLocalPointFromWorld(Vector2 worldPoint)
+        {
+            Vector2 delta = worldPoint - Position;
+            float cos = MathF.Cos(-Rotation);
+            float sin = MathF.Sin(-Rotation);
+            return new Vector2(
+                delta.X * cos - delta.Y * sin,
+                delta.X * sin + delta.Y * cos
+            );
+        }
+
+        public void ClearForces()
+        {
+            ForceAccumulator = Vector2.Zero;
+            TorqueAccumulator = 0.0f;
+        }
+
+        public virtual void Draw(float pixelsPerMeter = 100.0f) {}
+        public virtual void ApplyAerodynamicDrag(float dt) {}
+    }
+}

PhysicsEngine/Physics/Bodies/Box.cs

@@ -0,0 +1,78 @@
+using System;
+using System.Numerics;
+using Raylib_cs;
+
+namespace PhysicsEngine
+{
+    public class Box : Body
+    {
+        public Vector2 Size;
+
+        public Box(Vector2 position, Vector2 size, float mass)
+        {
+            Position = position;
+            Size = size;
+            Mass = mass;
+            MomentOfInertia = (1.0f / 12.0f) * mass * (size.X * size.X + size.Y * size.Y);
+        }
+
+        public override void Draw(float pixelsPerMeter = 100.0f)
+        {
+            DrawHelper.DrawRotatedBox(Position, Size, Rotation, BaseColor, pixelsPerMeter);
+        }
+
+        private Vector2 RotatePoint(Vector2 point, float radians)
+        {
+            float cos = MathF.Cos(radians);
+            float sin = MathF.Sin(radians);
+            return new Vector2(
+                point.X * cos - point.Y * sin,
+                point.X * sin + point.Y * cos
+            );
+        }
+
+        public override void ApplyAerodynamicDrag(float dt)
+        {
+            if (DragCoefficient <= 0.0f) return;
+
+            const float airDensity = 1.225f;
+            float speed = Velocity.Length();
+            if (speed < 0.01f) return;
+
+            // 1. Oncoming relative wind direction (opposite to velocity)
+            Vector2 windDir = -Velocity / speed;
+
+            float cos = MathF.Cos(Rotation);
+            float sin = MathF.Sin(Rotation);
+
+            // Box local axes transformed into world space (Assuming Size.Y is length/major axis)
+            Vector2 worldAxisY = new Vector2(-sin, cos); // Major axis vector
+            Vector2 worldAxisX = new Vector2(cos, sin);  // Minor axis vector
+
+            // 2. Calculate effective frontal width (projected area in 2D) based on current orientation
+            Vector2 windPerp = new Vector2(-windDir.Y, windDir.X);
+            float projX = MathF.Abs(Vector2.Dot(worldAxisX, windPerp));
+            float projY = MathF.Abs(Vector2.Dot(worldAxisY, windPerp));
+            float effectiveWidth = Size.X * projX + Size.Y * projY;
+
+            // 3. Translational Drag Force (scales with dynamic pressure and current effective width)
+            float dragMagnitude = 0.5f * airDensity * speed * speed * DragCoefficient * effectiveWidth;
+            Vector2 dragForce = windDir * dragMagnitude;
+            ApplyForce(dragForce);
+
+            // 4. Weathercock / Fin-Effect Restoring Torque
+            // Measures angular misalignment between the box's major axis and the wind direction
+            float cross = worldAxisY.X * windDir.Y - worldAxisY.Y * windDir.X;
+            float restoringTorque = cross * 0.5f * airDensity * speed * speed * DragCoefficient * Size.X * Size.Y;
+            ApplyTorque(restoringTorque);
+
+            // 5. Angular Damping (Quadratic Rotational Drag)
+            float angularSpeed = MathF.Abs(AngularVelocity);
+            if (angularSpeed > 0.0001f)
+            {
+                float angularDragTorque = -MathF.Sign(AngularVelocity) * 0.5f * airDensity * angularSpeed * angularSpeed * DragCoefficient * (Size.X + Size.Y);
+                ApplyTorque(angularDragTorque);
+            }
+        }
+    }
+}

PhysicsEngine/Physics/Bodies/Circle.cs

@@ -0,0 +1,44 @@
+using System;
+using System.Numerics;
+using Raylib_cs;
+
+namespace PhysicsEngine
+{
+    public class Circle : Body
+    {
+        public float Radius;
+
+        public Circle(Vector2 position, float radius, float mass)
+        {
+            Position = position;
+            Radius = radius;
+            Mass = mass;
+            
+            if (mass > 0.0f)
+            {
+                MomentOfInertia = 0.5f * mass * radius * radius;
+            }
+        }
+
+        public override void Draw(float pixelsPerMeter = 100.0f)
+        {
+            DrawHelper.DrawCircleBody(Position, Radius, Rotation, BaseColor, pixelsPerMeter);
+        }
+
+        public override void ApplyAerodynamicDrag(float dt)
+        {
+            if (DragCoefficient <= 0.0f) return;
+
+            const float airDensity = 1.225f;
+            float area = Radius * 2.0f;
+            float speed = Velocity.Length();
+
+            if (speed > 0.0001f)
+            {
+                Vector2 dragDir = -Vector2.Normalize(Velocity);
+                float dragMagnitude = 0.5f * airDensity * speed * speed * DragCoefficient * area;
+                ApplyForce(dragDir * dragMagnitude);
+            }
+        }
+    }
+}

PhysicsEngine/Physics/CollisionEngine.cs

@@ -0,0 +1,505 @@
+using System;
+using System.Collections.Generic;
+using System.Numerics;
+
+namespace PhysicsEngine
+{
+    public static class CollisionEngine
+    {
+        private const float Slop = 0.01f;
+        private const float PositionCorrectionPercent = 0.1f;
+        private const float MaxCorrection = 0.2f;
+        private const int VelocityIterations = 5;
+
+        private class Contact
+        {
+            public Vector2 Point;
+            public Vector2 Normal;          // from BodyA to BodyB
+            public float Penetration;
+            public float AccumulatedNormalImpulse;
+            public float AccumulatedTangentImpulse;
+            public Body BodyA;
+            public Body BodyB;
+            public Vector2 RA;
+            public Vector2 RB;
+            public float NormalMass;
+            public float TangentMass;
+            public float Friction;
+            public float Restitution;
+        }
+
+        public static void ResolveCollisions(List<Body> bodies, float dt)
+        {
+            List<Contact> contacts = new List<Contact>();
+
+            for (int i = 0; i < bodies.Count; i++)
+            {
+                for (int j = i + 1; j < bodies.Count; j++)
+                {
+                    Body b1 = bodies[i];
+                    Body b2 = bodies[j];
+                    if (b1.IsStatic && b2.IsStatic) continue;
+
+                    if (b1 is Circle && b2 is Circle)
+                        ResolveCircleCircle((Circle)b1, (Circle)b2, contacts);
+                    else if (b1 is Box && b2 is Circle)
+                        ResolveBoxCircle((Box)b1, (Circle)b2, contacts, false);
+                    else if (b1 is Circle && b2 is Box)
+                        ResolveBoxCircle((Box)b2, (Circle)b1, contacts, true);
+                    else if (b1 is Box && b2 is Box)
+                        ResolveBoxBox((Box)b1, (Box)b2, contacts);
+                }
+            }
+
+            foreach (var contact in contacts)
+            {
+                contact.AccumulatedNormalImpulse = 0;
+                contact.AccumulatedTangentImpulse = 0;
+                contact.RA = contact.Point - contact.BodyA.Position;
+                contact.RB = contact.Point - contact.BodyB.Position;
+
+                float invMassA = contact.BodyA.InverseMass;
+                float invMassB = contact.BodyB.InverseMass;
+                float invIA = contact.BodyA.InverseInertia;
+                float invIB = contact.BodyB.InverseInertia;
+
+                float rnA = Cross(contact.RA, contact.Normal);
+                float rnB = Cross(contact.RB, contact.Normal);
+                float denom = invMassA + invMassB + rnA * rnA * invIA + rnB * rnB * invIB;
+                contact.NormalMass = denom > 0 ? 1.0f / denom : 0;
+
+                Vector2 tangent = new Vector2(-contact.Normal.Y, contact.Normal.X);
+                float rtA = Cross(contact.RA, tangent);
+                float rtB = Cross(contact.RB, tangent);
+                denom = invMassA + invMassB + rtA * rtA * invIA + rtB * rtB * invIB;
+                contact.TangentMass = denom > 0 ? 1.0f / denom : 0;
+
+                contact.Friction = (float)Math.Sqrt(contact.BodyA.Friction * contact.BodyB.Friction);
+                contact.Restitution = Math.Max(contact.BodyA.Restitution, contact.BodyB.Restitution);
+            }
+
+            for (int iter = 0; iter < VelocityIterations; iter++)
+            {
+                foreach (var contact in contacts)
+                {
+                    Vector2 rv = GetRelativeVelocity(contact);
+                    float vn = Vector2.Dot(rv, contact.Normal);
+                    float lambda = -contact.NormalMass * vn;
+
+                    float newImpulse = Math.Max(contact.AccumulatedNormalImpulse + lambda, 0);
+                    lambda = newImpulse - contact.AccumulatedNormalImpulse;
+                    contact.AccumulatedNormalImpulse = newImpulse;
+
+                    ApplyImpulse(contact, contact.Normal * lambda);
+  
+                    rv = GetRelativeVelocity(contact);
+                    Vector2 tangent = rv - contact.Normal * Vector2.Dot(rv, contact.Normal);
+                    if (tangent.LengthSquared() > 1e-6f)
+                        tangent = Vector2.Normalize(tangent);
+                    else
+                        tangent = new Vector2(-contact.Normal.Y, contact.Normal.X);
+
+                    float vt = Vector2.Dot(rv, tangent);
+                    float lambdaT = -contact.TangentMass * vt;
+                    float maxFriction = contact.Friction * contact.AccumulatedNormalImpulse;
+                    float newImpulseT = Math.Clamp(contact.AccumulatedTangentImpulse + lambdaT, -maxFriction, maxFriction);
+                    lambdaT = newImpulseT - contact.AccumulatedTangentImpulse;
+                    contact.AccumulatedTangentImpulse = newImpulseT;
+
+                    ApplyImpulse(contact, tangent * lambdaT);
+                }
+            }
+
+            var pairMap = new Dictionary<(Body, Body), Contact>();
+            foreach (var contact in contacts)
+            {
+                var key = (contact.BodyA, contact.BodyB);
+                if (!pairMap.ContainsKey(key) || pairMap[key].Penetration < contact.Penetration)
+                    pairMap[key] = contact;
+            }
+
+            foreach (var kvp in pairMap)
+            {
+                var contact = kvp.Value;
+                float correctionMagnitude = Math.Max(contact.Penetration - Slop, 0) * PositionCorrectionPercent;
+                correctionMagnitude = Math.Min(correctionMagnitude, MaxCorrection);
+                if (correctionMagnitude <= 0) continue;
+
+                Vector2 correction = contact.Normal * correctionMagnitude;
+                float totalInvMass = contact.BodyA.InverseMass + contact.BodyB.InverseMass;
+                if (totalInvMass <= 0) continue;
+
+                float ratioA = contact.BodyA.InverseMass / totalInvMass;
+                float ratioB = contact.BodyB.InverseMass / totalInvMass;
+
+                contact.BodyA.Position -= correction * ratioA;
+                contact.BodyB.Position += correction * ratioB;
+            }
+        }
+
+        private static void ResolveCircleCircle(Circle c1, Circle c2, List<Contact> contacts)
+        {
+            Vector2 d = c2.Position - c1.Position;
+            float distSq = d.LengthSquared();
+            float radiusSum = c1.Radius + c2.Radius;
+            if (distSq >= radiusSum * radiusSum) return;
+
+            float dist = (float)Math.Sqrt(distSq);
+            Vector2 normal;
+            if (dist < 1e-6f)
+                normal = new Vector2(1, 0);
+            else
+                normal = d / dist;
+
+            float penetration = radiusSum - dist;
+            Vector2 contactPoint = c1.Position + normal * (c1.Radius - penetration * 0.5f);
+
+            contacts.Add(new Contact
+            {
+                Point = contactPoint,
+                Normal = normal,
+                Penetration = penetration,
+                BodyA = c1,
+                BodyB = c2
+            });
+        }
+
+        private static void ResolveBoxCircle(Box box, Circle circle, List<Contact> contacts, bool swapped)
+        {
+            Vector2 localCircleCenter = circle.Position - box.Position;
+            float cos = (float)Math.Cos(box.Rotation);
+            float sin = (float)Math.Sin(box.Rotation);
+            Vector2 local = new Vector2(
+                localCircleCenter.X * cos + localCircleCenter.Y * sin,
+                -localCircleCenter.X * sin + localCircleCenter.Y * cos);
+
+            float hx = box.Size.X * 0.5f;
+            float hy = box.Size.Y * 0.5f;
+            Vector2 closestLocal = new Vector2(Math.Clamp(local.X, -hx, hx), Math.Clamp(local.Y, -hy, hy));
+
+            Vector2 normalWorld;
+            Vector2 contactPointWorld;
+            float penetration;
+
+            bool inside = (closestLocal == local);
+            if (inside)
+            {
+                float distLeft = local.X + hx;
+                float distRight = hx - local.X;
+                float distBottom = local.Y + hy;
+                float distTop = hy - local.Y;
+                float minDist = Math.Min(Math.Min(distLeft, distRight), Math.Min(distBottom, distTop));
+
+                Vector2 normalLocal;
+                if (minDist == distLeft) normalLocal = new Vector2(-1, 0);
+                else if (minDist == distRight) normalLocal = new Vector2(1, 0);
+                else if (minDist == distBottom) normalLocal = new Vector2(0, -1);
+                else normalLocal = new Vector2(0, 1);
+
+                normalWorld = new Vector2(normalLocal.X * cos - normalLocal.Y * sin,
+                                          normalLocal.X * sin + normalLocal.Y * cos);
+                penetration = circle.Radius + minDist;
+
+                Vector2 faceLocal = local;
+                if (normalLocal.X != 0) faceLocal.X = normalLocal.X > 0 ? hx : -hx;
+                else faceLocal.Y = normalLocal.Y > 0 ? hy : -hy;
+                contactPointWorld = box.Position + new Vector2(faceLocal.X * cos - faceLocal.Y * sin,
+                                                               faceLocal.X * sin + faceLocal.Y * cos);
+            }
+            else
+            {
+                Vector2 closestWorld = box.Position + new Vector2(closestLocal.X * cos - closestLocal.Y * sin,
+                                                                  closestLocal.X * sin + closestLocal.Y * cos);
+                Vector2 delta = circle.Position - closestWorld;
+                float distSq = delta.LengthSquared();
+                float radiusSq = circle.Radius * circle.Radius;
+                if (distSq >= radiusSq) return;
+
+                float dist = (float)Math.Sqrt(distSq);
+                normalWorld = dist > 1e-6f ? delta / dist : new Vector2(1, 0);
+                penetration = circle.Radius - dist;
+                contactPointWorld = closestWorld;
+            }
+
+            Contact contact = new Contact
+            {
+                Point = contactPointWorld,
+                Normal = normalWorld,
+                Penetration = penetration,
+                BodyA = box,
+                BodyB = circle
+            };
+
+            if (swapped)
+            {
+                contact.BodyA = circle;
+                contact.BodyB = box;
+                contact.Normal = -contact.Normal;
+            }
+
+            contacts.Add(contact);
+        }
+
+        private static void ResolveBoxBox(Box b1, Box b2, List<Contact> contacts)
+        {
+            float h1x = b1.Size.X * 0.5f;
+            float h1y = b1.Size.Y * 0.5f;
+            float h2x = b2.Size.X * 0.5f;
+            float h2y = b2.Size.Y * 0.5f;
+
+            float cos1 = (float)Math.Cos(b1.Rotation);
+            float sin1 = (float)Math.Sin(b1.Rotation);
+            float cos2 = (float)Math.Cos(b2.Rotation);
+            float sin2 = (float)Math.Sin(b2.Rotation);
+
+            Vector2 axisX1 = new Vector2(cos1, sin1);
+            Vector2 axisY1 = new Vector2(-sin1, cos1);
+            Vector2 axisX2 = new Vector2(cos2, sin2);
+            Vector2 axisY2 = new Vector2(-sin2, cos2);
+
+            Vector2 d = b2.Position - b1.Position;
+            float minOverlap = float.MaxValue;
+            Vector2 bestAxis = Vector2.Zero;
+            bool axisFromB1 = false;
+
+            // Test axes. If one body is static, only test that body's axes for stability.
+            if (b1.IsStatic && !b2.IsStatic)
+            {
+                TestAxis(axisX1, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, true, ref axisFromB1);
+                TestAxis(axisY1, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, true, ref axisFromB1);
+            }
+            else if (b2.IsStatic && !b1.IsStatic)
+            {
+                TestAxis(axisX2, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, false, ref axisFromB1);
+                TestAxis(axisY2, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, false, ref axisFromB1);
+            }
+            else
+            {
+                TestAxis(axisX1, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, true, ref axisFromB1);
+                TestAxis(axisY1, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, true, ref axisFromB1);
+                TestAxis(axisX2, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, false, ref axisFromB1);
+                TestAxis(axisY2, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, false, ref axisFromB1);
+            }
+
+            if (minOverlap <= 0 || minOverlap == float.MaxValue) return;
+
+            Vector2 normal = Vector2.Dot(d, bestAxis) > 0 ? bestAxis : -bestAxis;
+
+            Box refBox, incBox;
+            Vector2 refNormal, incNormal;
+            if (axisFromB1)
+            {
+                refBox = b1;
+                incBox = b2;
+                refNormal = normal;      // outward normal of reference face points toward incident box
+                incNormal = -normal;     // outward normal of incident face points toward reference box
+            }
+            else
+            {
+                refBox = b2;
+                incBox = b1;
+                refNormal = -normal;
+                incNormal = normal;
+            }
+
+            Vector2[] refFace = GetFaceVertices(refBox, refNormal);
+            Vector2[] incFace = GetFaceVertices(incBox, incNormal);
+            if (refFace.Length < 2 || incFace.Length < 2) return;
+
+            Vector2 refV1 = refFace[0];
+            Vector2 refV2 = refFace[1];
+            Vector2 edgeDir = refV2 - refV1;
+            if (edgeDir.LengthSquared() < 1e-8f) return;
+            edgeDir = Vector2.Normalize(edgeDir);
+
+            int contactsBefore = contacts.Count;  // to check if we added any new contacts
+
+            // Clip incident face against reference face side planes
+            List<Vector2> clipped = new List<Vector2> { incFace[0], incFace[1] };
+            ClipSegmentAgainstPlane(clipped, edgeDir, Vector2.Dot(edgeDir, refV1));
+            ClipSegmentAgainstPlane(clipped, -edgeDir, -Vector2.Dot(edgeDir, refV2));
+
+            // Add contacts from clipped incident face points
+            foreach (var p in clipped)
+            {
+                float pen = -Vector2.Dot(p - refV1, refNormal);
+                if (pen > 0)
+                {
+                    contacts.Add(new Contact
+                    {
+                        Point = p,
+                        Normal = normal,
+                        Penetration = pen,
+                        BodyA = b1,
+                        BodyB = b2
+                    });
+                }
+            }
+
+            // Check reference face vertices that penetrate the incident face
+            foreach (var refVertex in refFace)
+            {
+                // Penetration depth along incident face normal (positive if inside)
+                float pen = -Vector2.Dot(refVertex - incFace[0], incNormal);
+                if (pen > 0)
+                {
+                    // Avoid duplicate contacts at nearly the same location
+                    bool duplicate = false;
+                    foreach (var c in contacts)
+                    {
+                        if (Vector2.DistanceSquared(c.Point, refVertex) < 1e-6f)
+                        {
+                            duplicate = true;
+                            break;
+                        }
+                    }
+                    if (!duplicate)
+                    {
+                        contacts.Add(new Contact
+                        {
+                            Point = refVertex,
+                            Normal = normal,
+                            Penetration = pen,
+                            BodyA = b1,
+                            BodyB = b2
+                        });
+                    }
+                }
+            }
+
+            // Fallback: if no contacts were generated despite SAT overlap, create a single contact at the midpoint
+            // of the overlapping region along the normal. This prevents any missed collisions.
+            if (contacts.Count == contactsBefore)
+            {
+                // Compute the projection of both boxes onto the normal
+                float projB1 = h1x * Math.Abs(Vector2.Dot(axisX1, normal)) + h1y * Math.Abs(Vector2.Dot(axisY1, normal));
+                float projB2 = h2x * Math.Abs(Vector2.Dot(axisX2, normal)) + h2y * Math.Abs(Vector2.Dot(axisY2, normal));
+                float dist = Vector2.Dot(d, normal); // distance from b1 to b2 along normal
+                float min1 = -projB1;
+                float max1 = projB1;
+                float min2 = dist - projB2;
+                float max2 = dist + projB2;
+
+                float overlapMin = Math.Max(min1, min2);
+                float overlapMax = Math.Min(max1, max2);
+                float overlapMid = (overlapMin + overlapMax) * 0.5f;
+                Vector2 contactPoint = b1.Position + normal * overlapMid; // rough position along normal
+
+                contacts.Add(new Contact
+                {
+                    Point = contactPoint,
+                    Normal = normal,
+                    Penetration = minOverlap, // use SAT overlap
+                    BodyA = b1,
+                    BodyB = b2
+                });
+            }
+        }
+
+        private static void TestAxis(Vector2 axis, Box b1, Box b2,
+            float h1x, float h1y, float h2x, float h2y,
+            Vector2 axisX1, Vector2 axisY1, Vector2 axisX2, Vector2 axisY2,
+            Vector2 d, ref float minOverlap, ref Vector2 bestAxis, bool fromB1, ref bool axisFromB1)
+        {
+            float r1 = h1x * Math.Abs(Vector2.Dot(axisX1, axis)) + h1y * Math.Abs(Vector2.Dot(axisY1, axis));
+            float r2 = h2x * Math.Abs(Vector2.Dot(axisX2, axis)) + h2y * Math.Abs(Vector2.Dot(axisY2, axis));
+            float distance = Math.Abs(Vector2.Dot(d, axis));
+            float overlap = r1 + r2 - distance;
+
+            if (overlap <= 0)
+            {
+                minOverlap = overlap;
+                return;
+            }
+
+            if (overlap < minOverlap)
+            {
+                minOverlap = overlap;
+                bestAxis = axis;
+                axisFromB1 = fromB1;
+            }
+        }
+
+        private static Vector2[] GetBoxVertices(Box box)
+        {
+            float hx = box.Size.X * 0.5f;
+            float hy = box.Size.Y * 0.5f;
+            float cos = (float)Math.Cos(box.Rotation);
+            float sin = (float)Math.Sin(box.Rotation);
+            Vector2 axisX = new Vector2(cos, sin);
+            Vector2 axisY = new Vector2(-sin, cos);
+
+            return new Vector2[]
+            {
+                box.Position - axisX * hx - axisY * hy,
+                box.Position + axisX * hx - axisY * hy,
+                box.Position + axisX * hx + axisY * hy,
+                box.Position - axisX * hx + axisY * hy
+            };
+        }
+
+        private static Vector2[] GetFaceVertices(Box box, Vector2 faceNormalWorld)
+        {
+            Vector2[] vertices = GetBoxVertices(box);
+            for (int i = 0; i < 4; i++)
+            {
+                Vector2 v1 = vertices[i];
+                Vector2 v2 = vertices[(i + 1) % 4];
+                Vector2 edge = v2 - v1;
+                Vector2 outwardNormal = new Vector2(edge.Y, -edge.X);
+                outwardNormal = Vector2.Normalize(outwardNormal);
+                if (Vector2.Dot(outwardNormal, faceNormalWorld) > 0.999f)
+                    return new Vector2[] { v1, v2 };
+            }
+            return new Vector2[] { vertices[0], vertices[1] };
+        }
+
+        private static void ClipSegmentAgainstPlane(List<Vector2> segment, Vector2 normal, float offset)
+        {
+            if (segment.Count == 0) return;
+            List<Vector2> result = new List<Vector2>();
+            float d0 = Vector2.Dot(segment[0], normal) - offset;
+            float d1 = Vector2.Dot(segment[1], normal) - offset;
+
+            if (d0 >= 0) result.Add(segment[0]);
+            if (d1 >= 0) result.Add(segment[1]);
+
+            if (d0 * d1 < 0)
+            {
+                float t = d0 / (d0 - d1);
+                Vector2 intersection = segment[0] + t * (segment[1] - segment[0]);
+                result.Add(intersection);
+            }
+
+            segment.Clear();
+            segment.AddRange(result);
+        }
+
+        private static Vector2 GetRelativeVelocity(Contact contact)
+        {
+            Body a = contact.BodyA;
+            Body b = contact.BodyB;
+            Vector2 va = a.Velocity + Cross(a.AngularVelocity, contact.RA);
+            Vector2 vb = b.Velocity + Cross(b.AngularVelocity, contact.RB);
+            return vb - va;
+        }
+
+        private static void ApplyImpulse(Contact contact, Vector2 impulse)
+        {
+            if (!contact.BodyA.IsStatic)
+                contact.BodyA.ApplyImpulseAtWorldPosition(-impulse, contact.Point);
+            if (!contact.BodyB.IsStatic)
+                contact.BodyB.ApplyImpulseAtWorldPosition(impulse, contact.Point);
+        }
+
+        private static float Cross(Vector2 a, Vector2 b)
+        {
+            return a.X * b.Y - a.Y * b.X;
+        }
+
+        private static Vector2 Cross(float s, Vector2 a)
+        {
+            return new Vector2(-s * a.Y, s * a.X);
+        }
+    }
+}

PhysicsEngine/Physics/Constraints/Constraint.cs

@@ -0,0 +1,24 @@
+using System.Numerics;
+
+namespace PhysicsEngine
+{
+    public abstract class Constraint
+    {
+        public Body BodyA;
+        public Body BodyB;
+        public Vector2 LocalAnchorA;
+        public Vector2 LocalAnchorB;
+
+        public Vector2 GetWorldAnchorA()
+        {
+            return BodyA.Position + LocalAnchorA;
+        }
+
+        public Vector2 GetWorldAnchorB()
+        {
+            return BodyB.Position + LocalAnchorB;
+        }
+
+        public abstract void Solve();
+    }
+}

PhysicsEngine/Physics/Forces/DragForce.cs

@@ -0,0 +1,10 @@
+namespace PhysicsEngine
+{
+    public static class DragForce
+    {
+        public static void Apply(Body body, float dt)
+        {
+            body.ApplyAerodynamicDrag(dt);
+        }
+    }
+}

PhysicsEngine/Physics/Forces/GlobalForce.cs

@@ -0,0 +1,4 @@
+namespace PhysicsEngine
+{
+    public delegate void GlobalForce(Body body, float dt);
+}

PhysicsEngine/Physics/Forces/GravityForce.cs

@@ -0,0 +1,12 @@
+using System.Numerics;
+
+namespace PhysicsEngine
+{
+    public static class GravityForce
+    {
+        public static void Apply(Body body, float dt)
+        {
+            body.ApplyForce(Vector2.UnitY * 9.81f * body.Mass);
+        }
+    }
+}

PhysicsEngine/Physics/Solver.cs

@@ -0,0 +1,62 @@
+using System.Collections.Generic;
+using System.Numerics;
+
+namespace PhysicsEngine
+{
+    public class Solver
+    {
+        // Optional: set a default number of sub‑steps (e.g. 8 for better stability)
+        private const int DefaultSubSteps = 8;
+
+        public void Step(float dt, List<Body> bodies, List<Constraint> constraints, List<GlobalForce> globalForces, int subSteps = DefaultSubSteps)
+        {
+            // Avoid division by zero or negative subSteps
+            if (subSteps <= 0) subSteps = 1;
+
+            float subDt = dt / subSteps;
+
+            for (int step = 0; step < subSteps; step++)
+            {
+                // 1. Accumulate global forces (gravity, drag, etc.)
+                //    They are applied each sub‑step using the sub‑step dt.
+                foreach (var body in bodies)
+                {
+                    if (body.IsStatic) continue;
+
+                    foreach (var forceGenerator in globalForces)
+                    {
+                        forceGenerator(body, subDt);
+                    }
+                }
+
+                // 2. Numerical Integration (Velocity & Position updates)
+                foreach (var body in bodies)
+                {
+                    if (body.IsStatic) continue;
+
+                    // Linear motion
+                    Vector2 acceleration = body.ForceAccumulator * body.InverseMass;
+                    body.Velocity += acceleration * subDt;
+                    body.Position += body.Velocity * subDt;
+
+                    // Angular motion
+                    float angularAcceleration = body.TorqueAccumulator * body.InverseInertia;
+                    body.AngularVelocity += angularAcceleration * subDt;
+                    body.Rotation += body.AngularVelocity * subDt;
+
+                    // Clear accumulators for the next sub‑step
+                    body.ClearForces();
+                }
+
+                // 3. Resolve Collisions
+                CollisionEngine.ResolveCollisions(bodies, subDt);
+
+                // 4. Resolve constraints
+                foreach (var constraint in constraints)
+                {
+                    constraint.Solve();
+                }
+            }
+        }
+    }
+}

PhysicsEngine/Physics/WorldModel.cs

@@ -0,0 +1,50 @@
+using System.Collections.Generic;
+
+namespace PhysicsEngine
+{
+    public class WorldModel
+    {
+        public List<Body> Bodies { get; private set; } = new();
+        public List<Constraint> Constraints { get; private set; } = new();
+        public List<GlobalForce> GlobalForces = new();
+
+        private readonly Solver _solver = new();
+
+        public WorldModel()
+        {
+            GlobalForces.Add(GravityForce.Apply);
+            //GlobalForces.Add(DragForce.Apply);
+        }
+
+        public void AddBody(Body body)
+        {
+            if (!Bodies.Contains(body))
+            {
+                Bodies.Add(body);
+            }
+        }
+
+        public void RemoveBody(Body body)
+        {
+            Bodies.Remove(body);
+        }
+
+        public void AddConstraint(Constraint constraint)
+        {
+            if (!Constraints.Contains(constraint))
+            {
+                Constraints.Add(constraint);
+            }
+        }
+
+        public void RemoveConstraint(Constraint constraint)
+        {
+            Constraints.Remove(constraint);
+        }
+
+        public void Step(float dt)
+        {
+            _solver.Step(dt, Bodies, Constraints, GlobalForces, 8);
+        }
+    }
+}