rigidbody, forces and collision testing

PhysicsEngine/Core/Program.cs

@@ -0,0 +1,392 @@
+using System.Numerics;
+using Raylib_cs;
+
+namespace PhysicsEngine
+{
+    class Program
+    {
+        private static Font robotoFont;
+        private static WorldModel world; // Instance of our WorldModel
+
+        static void Main(string[] args)
+        {
+            Raylib.SetConfigFlags(ConfigFlags.FullscreenMode | ConfigFlags.VSyncHint);
+            Raylib.InitWindow(0, 0, "Physics Engine");
+
+            robotoFont = Raylib.LoadFont("Roboto-Regular.ttf");
+            Raylib.SetTextureFilter(robotoFont.Texture, TextureFilter.Bilinear);
+
+            Camera camera = new Camera();
+            world = new WorldModel(); // Initialize simulation world
+
+            ScenarioLarge();
+
+            const float physicsTimeStep = 1.0f / 60.0f;
+            float accumulator = 0.0f;
+
+            // Main loop
+            while (!Raylib.WindowShouldClose())
+            {
+                float frameTime = Raylib.GetFrameTime();
+                if (frameTime > 0.25f)
+                {
+                    frameTime = 0.25f;
+                }
+
+                // Update Camera
+                camera.Update(frameTime);
+
+                // Update Physics accumulator
+                accumulator += frameTime;
+                while (accumulator >= physicsTimeStep)
+                {
+                    UpdatePhysics(physicsTimeStep);
+                    accumulator -= physicsTimeStep;
+                }
+
+                // Render loop
+                Raylib.BeginDrawing();
+                Raylib.ClearBackground(MuiDarkColor.BackgroundDefault.ToColor());
+
+                // Draw background screen-space elements (Grid)
+                camera.DrawGrid();
+
+                // Draw World Bodies inside the Camera's 2D World Transform
+                Raylib.BeginMode2D(camera.RaylibCamera);
+                RenderScene();
+                Raylib.EndMode2D();
+
+                // Draw UI / HUD elements
+                camera.DrawHUD();
+                DrawText("Physics Engine", 20, 20, 30, MuiDarkColor.TextPrimary.ToColor());
+
+                Raylib.EndDrawing();
+            }
+
+            Raylib.UnloadFont(robotoFont);
+            Raylib.CloseWindow();
+        }
+
+        public static void DrawText(string text, int posX, int posY, float fontSize, Color color)
+        {
+            Raylib.DrawTextEx(robotoFont, text, new Vector2(posX, posY), fontSize, 1.0f, color);
+        }
+
+        private static void UpdatePhysics(float dt)
+        {
+            world.Step(dt);
+        }
+
+        private static void RenderScene()
+        {
+            // Iterate and draw all bodies in the world
+            foreach (var body in world.Bodies)
+            {
+                body.Draw();
+            }
+        }
+
+        private static void Scenario()
+        {
+            float worldWidth = 12.8f;
+            float worldHeight = 7.2f;
+            float wallThickness = 1.0f;
+
+            // 1. Static world borders
+            Box floor = new Box(new Vector2(worldWidth * 0.5f, worldHeight + (wallThickness * 0.5f)), new Vector2(worldWidth + wallThickness * 2, wallThickness), 0f);
+            floor.BaseColor = Color.DarkGray;
+            world.AddBody(floor);
+
+            Box ceiling = new Box(new Vector2(worldWidth * 0.5f, -wallThickness * 0.5f), new Vector2(worldWidth + wallThickness * 2, wallThickness), 0f);
+            ceiling.BaseColor = Color.DarkGray;
+            world.AddBody(ceiling);
+
+            Box leftWall = new Box(new Vector2(-wallThickness * 0.5f, worldHeight * 0.5f), new Vector2(wallThickness, worldHeight + wallThickness * 2), 0f);
+            leftWall.BaseColor = Color.DarkGray;
+            world.AddBody(leftWall);
+
+            Box rightWall = new Box(new Vector2(worldWidth + wallThickness * 0.5f, worldHeight * 0.5f), new Vector2(wallThickness, worldHeight + wallThickness * 2), 0f);
+            rightWall.BaseColor = Color.DarkGray;
+            world.AddBody(rightWall);
+
+            // 2. Static ramps and platforms (rotated boxes)
+            // Left ramp (slopes up to the right)
+            Box leftRamp = new Box(new Vector2(2.5f, 5.5f), new Vector2(4.0f, 0.4f), 0f);
+            leftRamp.Rotation = -0.4f; // tilt upward to the right
+            leftRamp.BaseColor = Color.Gray;
+            world.AddBody(leftRamp);
+
+            // Right ramp (slopes down to the right)
+            Box rightRamp = new Box(new Vector2(10.5f, 5.5f), new Vector2(4.0f, 0.4f), 0f);
+            rightRamp.Rotation = 0.4f; // tilt downward to the right
+            rightRamp.BaseColor = Color.Gray;
+            world.AddBody(rightRamp);
+
+            // Floating platform in the middle
+            Box platform = new Box(new Vector2(6.4f, 3.5f), new Vector2(3.0f, 0.4f), 0f);
+            platform.BaseColor = Color.LightGray;
+            world.AddBody(platform);
+
+            // Small static circle bumper near the bottom center
+            Circle staticBumper = new Circle(new Vector2(6.4f, 6.0f), 0.3f, 0f);
+            staticBumper.BaseColor = Color.Orange;
+            world.AddBody(staticBumper);
+
+            // 3. Dynamic bodies – a mix of boxes and circles with varied properties
+
+            // Tall, thin box (will tip over when hitting the ground)
+            Box tallBox = new Box(new Vector2(2.0f, 1.0f), new Vector2(0.3f, 1.8f), 30f);
+            tallBox.BaseColor = Color.SkyBlue;
+            tallBox.Rotation = 0.3f;
+            tallBox.AngularVelocity = 0.5f;
+            tallBox.Friction = 0.8f;
+            tallBox.Restitution = 0.1f;
+            world.AddBody(tallBox);
+
+            // Heavy square box
+            Box heavyBox = new Box(new Vector2(4.0f, 1.5f), new Vector2(0.8f, 0.8f), 80f);
+            heavyBox.BaseColor = Color.DarkGreen;
+            heavyBox.Friction = 0.9f;
+            heavyBox.Restitution = 0.0f;
+            heavyBox.Velocity = new Vector2(1.5f, -1.0f);
+            world.AddBody(heavyBox);
+
+            // Long plank (will slide and possibly balance)
+            Box plank = new Box(new Vector2(8.0f, 0.5f), new Vector2(2.5f, 0.25f), 25f);
+            plank.BaseColor = Color.Brown;
+            plank.Rotation = -0.2f;
+            plank.AngularVelocity = -0.3f;
+            plank.Friction = 0.6f;
+            world.AddBody(plank);
+
+            // Bouncy ball (high restitution, low friction)
+            Circle bouncyBall = new Circle(new Vector2(9.0f, 1.0f), 0.3f, 5f);
+            bouncyBall.BaseColor = Color.Red;
+            bouncyBall.Restitution = 0.9f;
+            bouncyBall.Friction = 0.05f;
+            bouncyBall.Velocity = new Vector2(-30.0f, -20.0f);
+            bouncyBall.AngularVelocity = 10f;
+            world.AddBody(bouncyBall);
+
+            // Heavy circle (rolls slowly)
+            Circle heavyBall = new Circle(new Vector2(5.0f, 0.8f), 0.5f, 40f);
+            heavyBall.BaseColor = Color.Purple;
+            heavyBall.Friction = 0.7f;
+            heavyBall.Restitution = 0.1f;
+            heavyBall.Velocity = new Vector2(2.0f, -0.5f);
+            world.AddBody(heavyBall);
+
+            // Small fast circle
+            Circle fastCircle = new Circle(new Vector2(1.0f, 2.0f), 0.2f, 2f);
+            fastCircle.BaseColor = Color.Yellow;
+            fastCircle.Restitution = 0.7f;
+            fastCircle.Friction = 0.2f;
+            fastCircle.Velocity = new Vector2(6.0f, 2.0f);
+            world.AddBody(fastCircle);
+
+            // Another box with high drag (will slow down quickly)
+            Box dragBox = new Box(new Vector2(10.0f, 2.0f), new Vector2(0.6f, 0.6f), 15f);
+            dragBox.BaseColor = Color.Violet;
+            dragBox.DragCoefficient = 2.0f;
+            dragBox.Velocity = new Vector2(-2.0f, -1.0f);
+            dragBox.AngularVelocity = 2.0f;
+            world.AddBody(dragBox);
+
+            // Circle with high drag (like a light ball in air)
+            Circle dragCircle = new Circle(new Vector2(3.0f, 3.0f), 0.4f, 8f);
+            dragCircle.BaseColor = Color.Pink;
+            dragCircle.DragCoefficient = 1.5f;
+            dragCircle.Velocity = new Vector2(1.0f, 3.0f);
+            world.AddBody(dragCircle);
+
+            // A box launched with an impulse at an offset (will spin)
+            Box spinningBox = new Box(new Vector2(7.0f, 2.5f), new Vector2(0.5f, 0.5f), 10f);
+            spinningBox.BaseColor = Color.Beige;
+            spinningBox.ApplyImpulseAtOffset(new Vector2(3.0f, -5.0f), new Vector2(0.25f, 0.0f));
+            world.AddBody(spinningBox);
+
+            // A circle launched with angular impulse
+            Circle spinningCircle = new Circle(new Vector2(11.0f, 1.0f), 0.35f, 12f);
+            spinningCircle.BaseColor = Color.Lime;
+            spinningCircle.ApplyImpulseAtOffset(new Vector2(0.0f, -8.0f), new Vector2(0.1f, 0.0f));
+            world.AddBody(spinningCircle);
+        }
+
+        private static void ScenarioLarge()
+        {
+            // Larger world dimensions
+            float worldWidth = 20.0f;
+            float worldHeight = 12.0f;
+            float wallThickness = 1.0f;
+
+            // 1. Static world borders
+            Box floor = new Box(new Vector2(worldWidth * 0.5f, worldHeight + (wallThickness * 0.5f)), new Vector2(worldWidth + wallThickness * 2, wallThickness), 0f);
+            floor.BaseColor = Color.DarkGray;
+            world.AddBody(floor);
+
+            Box ceiling = new Box(new Vector2(worldWidth * 0.5f, -wallThickness * 0.5f), new Vector2(worldWidth + wallThickness * 2, wallThickness), 0f);
+            ceiling.BaseColor = Color.DarkGray;
+            world.AddBody(ceiling);
+
+            Box leftWall = new Box(new Vector2(-wallThickness * 0.5f, worldHeight * 0.5f), new Vector2(wallThickness, worldHeight + wallThickness * 2), 0f);
+            leftWall.BaseColor = Color.DarkGray;
+            world.AddBody(leftWall);
+
+            Box rightWall = new Box(new Vector2(worldWidth + wallThickness * 0.5f, worldHeight * 0.5f), new Vector2(wallThickness, worldHeight + wallThickness * 2), 0f);
+            rightWall.BaseColor = Color.DarkGray;
+            world.AddBody(rightWall);
+
+            // 2. Static ramps (angled platforms)
+            Box ramp1 = new Box(new Vector2(4.0f, 9.0f), new Vector2(5.0f, 0.4f), 0f);
+            ramp1.Rotation = -0.35f;
+            ramp1.BaseColor = Color.Gray;
+            world.AddBody(ramp1);
+
+            Box ramp2 = new Box(new Vector2(16.0f, 9.0f), new Vector2(5.0f, 0.4f), 0f);
+            ramp2.Rotation = 0.35f;
+            ramp2.BaseColor = Color.Gray;
+            world.AddBody(ramp2);
+
+            // Static horizontal platforms
+            Box platform1 = new Box(new Vector2(7.0f, 6.0f), new Vector2(4.0f, 0.4f), 0f);
+            platform1.BaseColor = Color.LightGray;
+            world.AddBody(platform1);
+
+            Box platform2 = new Box(new Vector2(13.0f, 6.0f), new Vector2(4.0f, 0.4f), 0f);
+            platform2.BaseColor = Color.LightGray;
+            world.AddBody(platform2);
+
+            // Static vertical pillars to create obstacles
+            Box pillar1 = new Box(new Vector2(5.0f, 4.0f), new Vector2(0.3f, 3.0f), 0f);
+            pillar1.BaseColor = Color.DarkBrown;
+            world.AddBody(pillar1);
+
+            Box pillar2 = new Box(new Vector2(15.0f, 4.0f), new Vector2(0.3f, 3.0f), 0f);
+            pillar2.BaseColor = Color.DarkBrown;
+            world.AddBody(pillar2);
+
+            // Static circular bumpers
+            Circle bumper1 = new Circle(new Vector2(10.0f, 8.0f), 0.4f, 0f);
+            bumper1.BaseColor = Color.Orange;
+            world.AddBody(bumper1);
+
+            Circle bumper2 = new Circle(new Vector2(10.0f, 3.0f), 0.4f, 0f);
+            bumper2.BaseColor = Color.Orange;
+            world.AddBody(bumper2);
+
+            Circle bumper3 = new Circle(new Vector2(2.0f, 7.0f), 0.3f, 0f);
+            bumper3.BaseColor = Color.Orange;
+            world.AddBody(bumper3);
+
+            Circle bumper4 = new Circle(new Vector2(18.0f, 7.0f), 0.3f, 0f);
+            bumper4.BaseColor = Color.Orange;
+            world.AddBody(bumper4);
+
+            // 3. Dynamic bodies – many with high energy, low damping
+
+            // High restitution balls (bouncy)
+            for (int i = 0; i < 6; i++)
+            {
+                Circle bouncyBall = new Circle(new Vector2(2.0f + i * 3.0f, 1.5f), 0.25f, 3.0f);
+                bouncyBall.BaseColor = Color.Red;
+                bouncyBall.Restitution = 0.95f;
+                bouncyBall.Friction = 0.05f;
+                bouncyBall.Velocity = new Vector2((i % 2 == 0 ? 3.0f : -3.0f), -2.0f);
+                bouncyBall.AngularVelocity = 5.0f;
+                world.AddBody(bouncyBall);
+            }
+
+            // Heavy boxes (low restitution, high friction)
+            Box heavy1 = new Box(new Vector2(6.0f, 2.0f), new Vector2(1.0f, 0.8f), 60f);
+            heavy1.BaseColor = Color.DarkGreen;
+            heavy1.Friction = 0.9f;
+            heavy1.Restitution = 0.0f;
+            heavy1.Velocity = new Vector2(2.0f, -1.0f);
+            world.AddBody(heavy1);
+
+            Box heavy2 = new Box(new Vector2(14.0f, 2.0f), new Vector2(1.0f, 0.8f), 60f);
+            heavy2.BaseColor = Color.DarkGreen;
+            heavy2.Friction = 0.9f;
+            heavy2.Restitution = 0.0f;
+            heavy2.Velocity = new Vector2(-2.0f, -1.0f);
+            world.AddBody(heavy2);
+
+            // Long planks (will slide and rotate)
+            Box plank1 = new Box(new Vector2(8.0f, 10.5f), new Vector2(3.0f, 0.3f), 20f);
+            plank1.BaseColor = Color.Brown;
+            plank1.Rotation = 0.1f;
+            plank1.AngularVelocity = -0.4f;
+            plank1.Friction = 0.6f;
+            plank1.Velocity = new Vector2(1.0f, -0.5f);
+            world.AddBody(plank1);
+
+            Box plank2 = new Box(new Vector2(12.0f, 10.5f), new Vector2(3.0f, 0.3f), 20f);
+            plank2.BaseColor = Color.Brown;
+            plank2.Rotation = -0.1f;
+            plank2.AngularVelocity = 0.4f;
+            plank2.Friction = 0.6f;
+            plank2.Velocity = new Vector2(-1.0f, -0.5f);
+            world.AddBody(plank2);
+
+            // Spinning boxes (launched with off-center impulses)
+            Box spinBox1 = new Box(new Vector2(5.0f, 7.0f), new Vector2(0.6f, 0.6f), 10f);
+            spinBox1.BaseColor = Color.Beige;
+            spinBox1.ApplyImpulseAtOffset(new Vector2(4.0f, -6.0f), new Vector2(0.3f, 0.0f));
+            world.AddBody(spinBox1);
+
+            Box spinBox2 = new Box(new Vector2(15.0f, 7.0f), new Vector2(0.6f, 0.6f), 10f);
+            spinBox2.BaseColor = Color.Beige;
+            spinBox2.ApplyImpulseAtOffset(new Vector2(-4.0f, -6.0f), new Vector2(-0.3f, 0.0f));
+            world.AddBody(spinBox2);
+
+            // Small fast circles
+            Circle fast1 = new Circle(new Vector2(3.0f, 11.0f), 0.2f, 2f);
+            fast1.BaseColor = Color.Yellow;
+            fast1.Restitution = 0.8f;
+            fast1.Friction = 0.2f;
+            fast1.Velocity = new Vector2(8.0f, 1.0f);
+            world.AddBody(fast1);
+
+            Circle fast2 = new Circle(new Vector2(17.0f, 11.0f), 0.2f, 2f);
+            fast2.BaseColor = Color.Yellow;
+            fast2.Restitution = 0.8f;
+            fast2.Friction = 0.2f;
+            fast2.Velocity = new Vector2(-8.0f, 1.0f);
+            world.AddBody(fast2);
+
+            // High-drag objects that still move initially
+            Box dragBox = new Box(new Vector2(10.0f, 9.0f), new Vector2(0.8f, 0.8f), 15f);
+            dragBox.BaseColor = Color.Violet;
+            dragBox.DragCoefficient = 2.0f;
+            dragBox.Velocity = new Vector2(3.0f, -2.0f);
+            dragBox.AngularVelocity = 2.0f;
+            world.AddBody(dragBox);
+
+            Circle dragCircle = new Circle(new Vector2(10.0f, 5.0f), 0.4f, 8f);
+            dragCircle.BaseColor = Color.Pink;
+            dragCircle.DragCoefficient = 1.5f;
+            dragCircle.Velocity = new Vector2(-2.0f, 3.0f);
+            world.AddBody(dragCircle);
+
+            // A few more random objects to fill the space
+            Circle extra1 = new Circle(new Vector2(1.5f, 5.0f), 0.35f, 12f);
+            extra1.BaseColor = Color.Lime;
+            extra1.Restitution = 0.7f;
+            extra1.Velocity = new Vector2(5.0f, -3.0f);
+            extra1.AngularVelocity = 8.0f;
+            world.AddBody(extra1);
+
+            Circle extra2 = new Circle(new Vector2(18.5f, 5.0f), 0.35f, 12f);
+            extra2.BaseColor = Color.Lime;
+            extra2.Restitution = 0.7f;
+            extra2.Velocity = new Vector2(-5.0f, -3.0f);
+            extra2.AngularVelocity = -8.0f;
+            world.AddBody(extra2);
+
+            Box extraBox = new Box(new Vector2(10.0f, 1.0f), new Vector2(1.2f, 0.6f), 25f);
+            extraBox.BaseColor = Color.SkyBlue;
+            extraBox.Velocity = new Vector2(0.5f, -2.0f);
+            world.AddBody(extraBox);
+        }
+    }
+}

PhysicsEngine/Graphics/Camera.cs

@@ -12,7 +12,7 @@ namespace PhysicsEngine
         private const float MoveLerpSpeed = 20.0f;
         private const float ZoomFactor = 0.1f;
         private const float MinZoom = 0.2f;
-        private const float MaxZoom = 500000000000000000000000000000000000.0f;
+        private const float MaxZoom = 50.0f;
 
         public Camera2D RaylibCamera { get; private set; }
         public Vector2 Position => RaylibCamera.Target / PixelsPerMeter;
@@ -82,6 +82,8 @@ namespace PhysicsEngine
                 targetPosition = Vector2.Zero;
                 targetZoom = 1.0f;
                 isDragging = false;
+                cam.Target = Vector2.Zero;
+                cam.Zoom = 1.0f;
             }
 
             // zooming
@@ -114,7 +116,6 @@ namespace PhysicsEngine
             {
                 cam.Target.X += (targetPosition.X - cam.Target.X) * MoveLerpSpeed * frameTime;
                 cam.Target.Y += (targetPosition.Y - cam.Target.Y) * MoveLerpSpeed * frameTime;
-                targetPosition = cam.Target;
             }
 
             RaylibCamera = cam;
@@ -180,8 +181,8 @@ namespace PhysicsEngine
         public void DrawHUD()
         {
             float screenH = Raylib.GetScreenHeight();
-            string hudText = $"X: {Position.X:F1}m  Y: {Position.Y:F1}m  Zoom: {Zoom:F2}x";
-            Raylib.DrawText(hudText, 20, (int)screenH - 40, 20, MuiDarkColor.TextPrimary.ToColor());
+            string hudText = $"X: {Position.X:F2}m  Y: {Position.Y:F2}m  Zoom: {Zoom:F2}x";
+            Program.DrawText(hudText, 20, (int)screenH - 40, 20, MuiDarkColor.TextPrimary.ToColor());
         }
     }
 }

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);
+        }
+    }
+}

PhysicsEngine/PhysicsEngine.csproj

@@ -11,4 +11,10 @@
     <PackageReference Include="Raylib-cs" Version="8.0.0" />
   </ItemGroup>
 
+  <ItemGroup>
+    <None Update="Roboto-Regular.ttf">
+      <CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
+    </None>
+  </ItemGroup>
+
 </Project>

PhysicsEngine/Program.cs

@@ -1,68 +0,0 @@
-using Raylib_cs;
-
-namespace PhysicsEngine
-{
-    class Program
-    {
-        static void Main(string[] args)
-        {
-            Raylib.SetConfigFlags(ConfigFlags.FullscreenMode | ConfigFlags.VSyncHint);
-            Raylib.InitWindow(0, 0, "Physics Engine");
-
-            Camera camera = new Camera();
-
-            const float physicsTimeStep = 1.0f / 60.0f;
-            float accumulator = 0.0f;
-
-            // Main loop
-            while (!Raylib.WindowShouldClose())
-            {
-                float frameTime = Raylib.GetFrameTime();
-                if (frameTime > 0.25f)
-                {
-                    frameTime = 0.25f;
-                }
-
-                // Update Camera
-                camera.Update(frameTime);
-
-                // Update Physics accumulator
-                accumulator += frameTime;
-                while (accumulator >= physicsTimeStep)
-                {
-                    UpdatePhysics(physicsTimeStep);
-                    accumulator -= physicsTimeStep;
-                }
-
-                // Render loop
-                Raylib.BeginDrawing();
-                Raylib.ClearBackground(MuiDarkColor.BackgroundDefault.ToColor());
-
-                // Draw background screen-space elements (Grid)
-                camera.DrawGrid();
-
-                // Draw world-space elements inside 2D mode
-                Raylib.BeginMode2D(camera.RaylibCamera);
-                Raylib.EndMode2D();
-
-                // Draw UI / HUD elements
-                RenderScene();
-                camera.DrawHUD();
-
-                Raylib.EndDrawing();
-            }
-
-            Raylib.CloseWindow();
-        }
-
-        private static void UpdatePhysics(float dt)
-        {
-            // Physics simulation step
-        }
-
-        private static void RenderScene()
-        {
-            Raylib.DrawText("Physics Engine", 20, 20, 20, MuiDarkColor.TextPrimary.ToColor());
-        }
-    }
-}

PhysicsEngine/UI/DrawHelper.cs

@@ -0,0 +1,63 @@
+using System;
+using System.Numerics;
+using Raylib_cs;
+
+namespace PhysicsEngine
+{
+    public static class DrawHelper
+    {
+        public static Color GetDarkerColor(Color color, float factor = 0.5f)
+        {
+            return new Color(
+                (byte)(color.R * factor),
+                (byte)(color.G * factor),
+                (byte)(color.B * factor),
+                color.A
+            );
+        }
+
+        public static void DrawRotatedBox(Vector2 position, Vector2 size, float rotationRadians, Color fillColor, float pixelsPerMeter = 100.0f, float outlineThickness = 3.0f)
+        {
+            Vector2 sizePixels = size * pixelsPerMeter;
+            Vector2 positionPixels = position * pixelsPerMeter;
+            float rotationDegrees = rotationRadians * (180.0f / MathF.PI);
+
+            Color outlineColor = GetDarkerColor(fillColor);
+
+            // 1. Draw the outer outline rectangle (exact original size, outline colour)
+            Vector2 outerSizePixels = sizePixels;
+            Vector2 outerOrigin = outerSizePixels * 0.5f;
+            Rectangle outerRec = new Rectangle(positionPixels.X, positionPixels.Y, outerSizePixels.X, outerSizePixels.Y);
+            Raylib.DrawRectanglePro(outerRec, outerOrigin, rotationDegrees, outlineColor);
+
+            // 2. Draw the inner fill rectangle, reduced by outlineThickness on all sides
+            Vector2 innerSizePixels = new Vector2(
+                MathF.Max(0, sizePixels.X - outlineThickness),
+                MathF.Max(0, sizePixels.Y - outlineThickness)
+            );
+            Vector2 innerOrigin = innerSizePixels * 0.5f;
+            Rectangle innerRec = new Rectangle(positionPixels.X, positionPixels.Y, innerSizePixels.X, innerSizePixels.Y);
+            Raylib.DrawRectanglePro(innerRec, innerOrigin, rotationDegrees, fillColor);
+        }
+
+        public static void DrawCircleBody(Vector2 position, float radius, float rotationRadians, Color fillColor, float pixelsPerMeter = 100.0f, float outlineThickness = 3.0f)
+        {
+            Vector2 positionPixels = position * pixelsPerMeter;
+            float radiusPixels = radius * pixelsPerMeter;
+            Color outlineColor = GetDarkerColor(fillColor);
+
+            // 1. Draw outer circle (outline colour, exact radius)
+            Raylib.DrawCircleV(positionPixels, radiusPixels, outlineColor);
+
+            // 2. Draw inner circle (fill colour, radius reduced by outlineThickness)
+            float innerRadius = MathF.Max(0, radiusPixels - outlineThickness);
+            Raylib.DrawCircleV(positionPixels, innerRadius, fillColor);
+
+            // Draw rotation indicator line inside the fill (same as before, but from centre to inner edge)
+            float cos = MathF.Cos(rotationRadians);
+            float sin = MathF.Sin(rotationRadians);
+            Vector2 edge = positionPixels + new Vector2(cos, sin) * innerRadius;
+            Raylib.DrawLineEx(positionPixels, edge, 2.0f, outlineColor);
+        }
+    }
+}

PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.AssemblyInfo.cs

@@ -13,7 +13,7 @@ using System.Reflection;
 [assembly: System.Reflection.AssemblyCompanyAttribute("PhysicsEngine")]
 [assembly: System.Reflection.AssemblyConfigurationAttribute("Debug")]
 [assembly: System.Reflection.AssemblyFileVersionAttribute("1.0.0.0")]
-[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0+5680beef02a5c7a4cdc1dc725326855b4af1faab")]
+[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0+cf1f083cc54031dbb6bf827cde69ef08608683e9")]
 [assembly: System.Reflection.AssemblyProductAttribute("PhysicsEngine")]
 [assembly: System.Reflection.AssemblyTitleAttribute("PhysicsEngine")]
 [assembly: System.Reflection.AssemblyVersionAttribute("1.0.0.0")]

PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.AssemblyInfoInputs.cache

@@ -1 +1 @@
-65813feab20f3482f44a4d97f31d1282ff5b6ec731137cac82a3e7db08a5bb63
+44a2f7ef34c114c65155ce102612520c953e6a96d159fd0c9c3838e4efb4baeb

PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.csproj.CoreCompileInputs.cache

@@ -1 +1 @@
-6b2f1f27b472d9ef51ad7745525496297fe0832151173bc3df4acdae2fd1d6fe
+baeac40138d8aced527e9be06cd30c4fff093062425aa1213346799e177123c4