stable collisions

PhysicsEngine/Core/Program.cs

@@ -6,7 +6,7 @@ namespace PhysicsEngine
     class Program
     {
         private static Font robotoFont;
-        private static WorldModel world; // Instance of our WorldModel
+        private static WorldModel world;
 
         static void Main(string[] args)
         {
@@ -17,14 +17,14 @@ namespace PhysicsEngine
             Raylib.SetTextureFilter(robotoFont.Texture, TextureFilter.Bilinear);
 
             Camera camera = new Camera();
-            world = new WorldModel(); // Initialize simulation world
+            world = new WorldModel();
 
-            ScenarioLarge();
+            Scenario();
 
             const float physicsTimeStep = 1.0f / 60.0f;
             float accumulator = 0.0f;
 
-            // Main loop
+            // main loop
             while (!Raylib.WindowShouldClose())
             {
                 float frameTime = Raylib.GetFrameTime();
@@ -33,10 +33,10 @@ namespace PhysicsEngine
                     frameTime = 0.25f;
                 }
 
-                // Update Camera
+                // update Camera
                 camera.Update(frameTime);
 
-                // Update Physics accumulator
+                // update physics accumulator
                 accumulator += frameTime;
                 while (accumulator >= physicsTimeStep)
                 {
@@ -44,19 +44,17 @@ namespace PhysicsEngine
                     accumulator -= physicsTimeStep;
                 }
 
-                // Render loop
+                // 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
+                // draw bodies
                 Raylib.BeginMode2D(camera.RaylibCamera);
                 RenderScene();
                 Raylib.EndMode2D();
 
-                // Draw UI / HUD elements
                 camera.DrawHUD();
                 DrawText("Physics Engine", 20, 20, 30, MuiDarkColor.TextPrimary.ToColor());
 
@@ -79,7 +77,6 @@ namespace PhysicsEngine
 
         private static void RenderScene()
         {
-            // Iterate and draw all bodies in the world
             foreach (var body in world.Bodies)
             {
                 body.Draw();
@@ -95,6 +92,7 @@ namespace PhysicsEngine
             // 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;
+            floor.Friction = 0.9f;
             world.AddBody(floor);
 
             Box ceiling = new Box(new Vector2(worldWidth * 0.5f, -wallThickness * 0.5f), new Vector2(worldWidth + wallThickness * 2, wallThickness), 0f);
@@ -109,284 +107,77 @@ namespace PhysicsEngine
             rightWall.BaseColor = Color.DarkGray;
             world.AddBody(rightWall);
 
-            // 2. Static ramps and platforms (rotated boxes)
+            // 2. Static Ramps and Platforms
-            // Left ramp (slopes up to the right)
+            Box platform = new Box(new Vector2(9.0f, 4.5f), new Vector2(3.5f, 0.3f), 0f);
-            Box leftRamp = new Box(new Vector2(2.5f, 5.5f), new Vector2(4.0f, 0.4f), 0f);
+            platform.BaseColor = Color.Gray;
-            leftRamp.Rotation = -0.4f; // tilt upward to the right
+            platform.Friction = 0.8f;
-            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
+            Box ramp = new Box(new Vector2(2.0f, 3.5f), new Vector2(3.5f, 0.3f), 0f);
-            Circle staticBumper = new Circle(new Vector2(6.4f, 6.0f), 0.3f, 0f);
+            ramp.Rotation = 0.35f; // Slopes down toward center stack
-            staticBumper.BaseColor = Color.Orange;
+            ramp.BaseColor = Color.Gray;
-            world.AddBody(staticBumper);
+            ramp.Friction = 0.6f;
+            world.AddBody(ramp);
 
-            // 3. Dynamic bodies – a mix of boxes and circles with varied properties
+            // 3. Main Ground Box Tower (5 boxes high)
+            float stackX = 5.0f;
+            float boxHeight = 0.6f;
+            float boxWidth = 0.6f;
+            float startY = worldHeight - (boxHeight * 0.5f); // Sits cleanly on the floor surface (Y = 7.2)
 
-            // Tall, thin box (will tip over when hitting the ground)
+            Color[] stackColors = new Color[] 
-            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);
+                Color.DarkGreen, Color.Green, Color.Lime, Color.DarkPurple, Color.Yellow 
-                bouncyBall.BaseColor = Color.Red;
+            };
-                bouncyBall.Restitution = 0.95f;
+
-                bouncyBall.Friction = 0.05f;
+            for (int i = 0; i < 5; i++)
-                bouncyBall.Velocity = new Vector2((i % 2 == 0 ? 3.0f : -3.0f), -2.0f);
+            {
-                bouncyBall.AngularVelocity = 5.0f;
+                float y = startY - (i * boxHeight);
-                world.AddBody(bouncyBall);
+                Box stackBox = new Box(new Vector2(stackX, y), new Vector2(boxWidth, boxHeight), 20f);
+                stackBox.BaseColor = stackColors[i % stackColors.Length];
+                stackBox.Friction = 0.8f;
+                stackBox.Restitution = 0.0f;
+                world.AddBody(stackBox);
             }
 
-            // Heavy boxes (low restitution, high friction)
+            // 4. Secondary Platform Stack (3 boxes high)
-            Box heavy1 = new Box(new Vector2(6.0f, 2.0f), new Vector2(1.0f, 0.8f), 60f);
+            float platformStackX = 9.0f;
-            heavy1.BaseColor = Color.DarkGreen;
+            float platformTopY = 4.5f - 0.15f; // Top surface of platform
-            heavy1.Friction = 0.9f;
+            float platformStartY = platformTopY - (boxHeight * 0.5f);
-            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);
+            for (int i = 0; i < 3; i++)
-            heavy2.BaseColor = Color.DarkGreen;
+            {
-            heavy2.Friction = 0.9f;
+                float y = platformStartY - (i * boxHeight);
-            heavy2.Restitution = 0.0f;
+                Box pBox = new Box(new Vector2(platformStackX, y), new Vector2(0.5f, boxHeight), 12f);
-            heavy2.Velocity = new Vector2(-2.0f, -1.0f);
+                pBox.BaseColor = Color.Blue;
-            world.AddBody(heavy2);
+                pBox.Friction = 0.8f;
+                pBox.Restitution = 0.0f;
+                world.AddBody(pBox);
+            }
 
-            // Long planks (will slide and rotate)
+            // 5. Dynamic Circles for Stack Interaction
-            Box plank1 = new Box(new Vector2(8.0f, 10.5f), new Vector2(3.0f, 0.3f), 20f);
+            // Circle balanced on top of the main ground stack
-            plank1.BaseColor = Color.Brown;
+            float mainStackTopY = startY - (4 * boxHeight) - (boxHeight * 0.5f);
-            plank1.Rotation = 0.1f;
+            Circle stackCap = new Circle(new Vector2(stackX, mainStackTopY - 0.3f), 0.3f, 8f);
-            plank1.AngularVelocity = -0.4f;
+            stackCap.BaseColor = Color.Orange;
-            plank1.Friction = 0.6f;
+            stackCap.Friction = 0.8f;
-            plank1.Velocity = new Vector2(1.0f, -0.5f);
+            stackCap.Restitution = 0.0f;
-            world.AddBody(plank1);
+            world.AddBody(stackCap);
 
-            Box plank2 = new Box(new Vector2(12.0f, 10.5f), new Vector2(3.0f, 0.3f), 20f);
+            // Heavy ball spawned on the ramp to roll down and strike the main stack
-            plank2.BaseColor = Color.Brown;
+            Circle rollingBall = new Circle(new Vector2(1.0f, 1.5f), 0.45f, 350f);
-            plank2.Rotation = -0.1f;
+            rollingBall.BaseColor = Color.Red;
-            plank2.AngularVelocity = 0.4f;
+            rollingBall.Friction = 0.5f;
-            plank2.Friction = 0.6f;
+            rollingBall.Restitution = 0.1f;
-            plank2.Velocity = new Vector2(-1.0f, -0.5f);
+            world.AddBody(rollingBall);
-            world.AddBody(plank2);
 
-            // Spinning boxes (launched with off-center impulses)
+            // Bouncy ball dropped over the platform stack to test stability under impact
-            Box spinBox1 = new Box(new Vector2(5.0f, 7.0f), new Vector2(0.6f, 0.6f), 10f);
+            Circle droppingBall = new Circle(new Vector2(9.0f, 0.8f), 0.35f, 10f);
-            spinBox1.BaseColor = Color.Beige;
+            droppingBall.BaseColor = Color.Gold;
-            spinBox1.ApplyImpulseAtOffset(new Vector2(4.0f, -6.0f), new Vector2(0.3f, 0.0f));
+            droppingBall.Friction = 0.4f;
-            world.AddBody(spinBox1);
+            droppingBall.Restitution = 0.4f;
-
+            droppingBall.Velocity = new Vector2(0.0f, 2.0f);
-            Box spinBox2 = new Box(new Vector2(15.0f, 7.0f), new Vector2(0.6f, 0.6f), 10f);
+            world.AddBody(droppingBall);
-            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/Physics/Bodies/Body.cs

@@ -34,8 +34,6 @@ namespace PhysicsEngine
             TorqueAccumulator += torque;
         }
 
-        // --- Impulse Methods ---
-
         public void ApplyImpulse(Vector2 impulse)
         {
             if (IsStatic) return;
@@ -46,10 +44,8 @@ namespace PhysicsEngine
         {
             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(
@@ -57,7 +53,6 @@ namespace PhysicsEngine
                 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;
         }
@@ -66,19 +61,14 @@ namespace PhysicsEngine
         {
             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;

PhysicsEngine/Physics/Bodies/Box.cs

@@ -39,34 +39,27 @@ namespace PhysicsEngine
             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)
             {

PhysicsEngine/Physics/CollisionEngine.cs

@@ -7,14 +7,15 @@ namespace PhysicsEngine
     public static class CollisionEngine
     {
         private const float Slop = 0.01f;
-        private const float PositionCorrectionPercent = 0.1f;
+        private const float PositionCorrectionPercent = 0.2f;
         private const float MaxCorrection = 0.2f;
-        private const int VelocityIterations = 5;
+        private const int VelocityIterations = 8;
 
         private class Contact
         {
             public Vector2 Point;
             public Vector2 Normal;          // from BodyA to BodyB
+            public Vector2 Tangent;         // perpendicular to Normal
             public float Penetration;
             public float AccumulatedNormalImpulse;
             public float AccumulatedTangentImpulse;
@@ -26,12 +27,17 @@ namespace PhysicsEngine
             public float TangentMass;
             public float Friction;
             public float Restitution;
+            public float RestitutionBias;
+            public float Bias;             // baumgarte position correction bias
         }
 
         public static void ResolveCollisions(List<Body> bodies, float dt)
         {
+            if (dt <= 0) return;
+
             List<Contact> contacts = new List<Contact>();
 
+            // broadphase & barrowphase collision detection
             for (int i = 0; i < bodies.Count; i++)
             {
                 for (int j = i + 1; j < bodies.Count; j++)
@@ -51,6 +57,7 @@ namespace PhysicsEngine
                 }
             }
 
+            // pre-step / initialization
             foreach (var contact in contacts)
             {
                 contact.AccumulatedNormalImpulse = 0;
@@ -68,73 +75,54 @@ namespace PhysicsEngine
                 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);
+                contact.Tangent = new Vector2(-contact.Normal.Y, contact.Normal.X);
-                float rtA = Cross(contact.RA, tangent);
+                float rtA = Cross(contact.RA, contact.Tangent);
-                float rtB = Cross(contact.RB, tangent);
+                float rtB = Cross(contact.RB, contact.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);
+
+                float rvNormal = Vector2.Dot(GetRelativeVelocity(contact), contact.Normal);
+                contact.RestitutionBias = rvNormal < -0.5f ? -contact.Restitution * rvNormal : 0;
+
+                float penetrationError = Math.Max(contact.Penetration - Slop, 0.0f);
+                contact.Bias = (PositionCorrectionPercent / dt) * penetrationError;
+                contact.Bias = Math.Min(contact.Bias, MaxCorrection / dt);
             }
 
+            // velocity Impulse Solver
             for (int iter = 0; iter < VelocityIterations; iter++)
             {
                 foreach (var contact in contacts)
                 {
+                    // normal impulse
                     Vector2 rv = GetRelativeVelocity(contact);
                     float vn = Vector2.Dot(rv, contact.Normal);
-                    float lambda = -contact.NormalMass * vn;
 
-                    float newImpulse = Math.Max(contact.AccumulatedNormalImpulse + lambda, 0);
+                    float targetVn = contact.RestitutionBias + contact.Bias;
+                    float lambda = -contact.NormalMass * (vn - targetVn);
+
+                    float newImpulse = Math.Max(contact.AccumulatedNormalImpulse + lambda, 0.0f);
                     lambda = newImpulse - contact.AccumulatedNormalImpulse;
                     contact.AccumulatedNormalImpulse = newImpulse;
 
                     ApplyImpulse(contact, contact.Normal * lambda);
 
+                    // tangent impulse
                     rv = GetRelativeVelocity(contact);
-                    Vector2 tangent = rv - contact.Normal * Vector2.Dot(rv, contact.Normal);
+                    float vt = Vector2.Dot(rv, contact.Tangent);
-                    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);
+                    float oldImpulseT = contact.AccumulatedTangentImpulse;
-                    lambdaT = newImpulseT - contact.AccumulatedTangentImpulse;
+                    contact.AccumulatedTangentImpulse = Math.Clamp(oldImpulseT + lambdaT, -maxFriction, maxFriction);
-                    contact.AccumulatedTangentImpulse = newImpulseT;
+                    lambdaT = contact.AccumulatedTangentImpulse - oldImpulseT;
 
-                    ApplyImpulse(contact, tangent * lambdaT);
+                    ApplyImpulse(contact, 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)
@@ -142,15 +130,10 @@ namespace PhysicsEngine
             Vector2 d = c2.Position - c1.Position;
             float distSq = d.LengthSquared();
             float radiusSum = c1.Radius + c2.Radius;
-            if (distSq >= radiusSum * radiusSum) return;
+            if (distSq >= radiusSum * radiusSum || distSq < 1e-12f) return;
 
             float dist = (float)Math.Sqrt(distSq);
-            Vector2 normal;
+            Vector2 normal = d / dist;
-            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);
 
@@ -262,25 +245,13 @@ namespace PhysicsEngine
             Vector2 bestAxis = Vector2.Zero;
             bool axisFromB1 = false;
 
-            // Test axes. If one body is static, only test that body's axes for stability.
+            TestAxis(axisX1, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, true, ref axisFromB1);
-            if (b1.IsStatic && !b2.IsStatic)
+            if (minOverlap <= 0) return;
-            {
+            TestAxis(axisY1, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, true, ref axisFromB1);
-                TestAxis(axisX1, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, true, ref axisFromB1);
+            if (minOverlap <= 0) return;
-                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);
-            }
+            if (minOverlap <= 0) return;
-            else if (b2.IsStatic && !b1.IsStatic)
+            TestAxis(axisY2, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, false, 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);
-            }
-            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;
@@ -291,8 +262,8 @@ namespace PhysicsEngine
             {
                 refBox = b1;
                 incBox = b2;
-                refNormal = normal;      // outward normal of reference face points toward incident box
+                refNormal = normal;
-                incNormal = -normal;     // outward normal of incident face points toward reference box
+                incNormal = -normal;
             }
             else
             {
@@ -304,7 +275,6 @@ namespace PhysicsEngine
 
             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];
@@ -312,14 +282,12 @@ namespace PhysicsEngine
             if (edgeDir.LengthSquared() < 1e-8f) return;
             edgeDir = Vector2.Normalize(edgeDir);
 
-            int contactsBefore = contacts.Count;  // to check if we added any new contacts
+            int contactsBefore = contacts.Count;
 
-            // 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));
+            clipped = ClipSegmentAgainstPlane(clipped, edgeDir, Vector2.Dot(edgeDir, refV1));
-            ClipSegmentAgainstPlane(clipped, -edgeDir, -Vector2.Dot(edgeDir, refV2));
+            clipped = 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);
@@ -336,60 +304,15 @@ namespace PhysicsEngine
                 }
             }
 
-            // Check reference face vertices that penetrate the incident face
+            // fallback for edge case precision drops
-            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
+                Vector2 contactPoint = (incFace[0] + incFace[1]) * 0.5f;
-                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
+                    Penetration = Math.Max(minOverlap, 1e-4f),
                     BodyA = b1,
                     BodyB = b2
                 });
@@ -441,38 +364,48 @@ namespace PhysicsEngine
         private static Vector2[] GetFaceVertices(Box box, Vector2 faceNormalWorld)
         {
             Vector2[] vertices = GetBoxVertices(box);
+            float maxDot = -float.MaxValue;
+            int bestIndex = 0;
+
             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);
+                Vector2 outwardNormal = Vector2.Normalize(new Vector2(edge.Y, -edge.X));
-                outwardNormal = Vector2.Normalize(outwardNormal);
+                float dot = Vector2.Dot(outwardNormal, faceNormalWorld);
-                if (Vector2.Dot(outwardNormal, faceNormalWorld) > 0.999f)
+                if (dot > maxDot)
-                    return new Vector2[] { v1, v2 };
+                {
+                    maxDot = dot;
+                    bestIndex = i;
+                }
             }
-            return new Vector2[] { vertices[0], vertices[1] };
+            return new Vector2[] { vertices[bestIndex], vertices[(bestIndex + 1) % 4] };
         }
 
-        private static void ClipSegmentAgainstPlane(List<Vector2> segment, Vector2 normal, float offset)
+        private static List<Vector2> 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;
+            if (segment.Count < 2) return result;
-            float d1 = Vector2.Dot(segment[1], normal) - offset;
 
-            if (d0 >= 0) result.Add(segment[0]);
+            Vector2 v0 = segment[0];
-            if (d1 >= 0) result.Add(segment[1]);
+            Vector2 v1 = segment[1];
+
+            float d0 = Vector2.Dot(v0, normal) - offset;
+            float d1 = Vector2.Dot(v1, normal) - offset;
+
+            if (d0 >= 0) result.Add(v0);
 
             if (d0 * d1 < 0)
             {
                 float t = d0 / (d0 - d1);
-                Vector2 intersection = segment[0] + t * (segment[1] - segment[0]);
+                Vector2 intersection = v0 + t * (v1 - v0);
                 result.Add(intersection);
             }
 
-            segment.Clear();
+            if (d1 >= 0) result.Add(v1);
-            segment.AddRange(result);
+
+            return result;
         }
 
         private static Vector2 GetRelativeVelocity(Contact contact)

PhysicsEngine/Physics/Forces/GravityForce.cs

@@ -6,7 +6,7 @@ namespace PhysicsEngine
     {
         public static void Apply(Body body, float dt)
         {
-            body.ApplyForce(Vector2.UnitY * 9.81f * body.Mass);
+            body.ApplyForce(Vector2.UnitY * 9.81f/10 * body.Mass);
         }
     }
 }

PhysicsEngine/Physics/Solver.cs

@@ -5,20 +5,17 @@ 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.)
+                // accumulate global forces
-                //    They are applied each sub‑step using the sub‑step dt.
                 foreach (var body in bodies)
                 {
                     if (body.IsStatic) continue;
@@ -29,29 +26,29 @@ namespace PhysicsEngine
                     }
                 }
 
-                // 2. Numerical Integration (Velocity & Position updates)
+                // velocity & position updates
                 foreach (var body in bodies)
                 {
                     if (body.IsStatic) continue;
 
-                    // Linear motion
+                    // linear motion
                     Vector2 acceleration = body.ForceAccumulator * body.InverseMass;
                     body.Velocity += acceleration * subDt;
                     body.Position += body.Velocity * subDt;
 
-                    // Angular motion
+                    // angular motion
                     float angularAcceleration = body.TorqueAccumulator * body.InverseInertia;
                     body.AngularVelocity += angularAcceleration * subDt;
                     body.Rotation += body.AngularVelocity * subDt;
 
-                    // Clear accumulators for the next sub‑step
+                    // clear
                     body.ClearForces();
                 }
 
-                // 3. Resolve Collisions
+                // collisions
                 CollisionEngine.ResolveCollisions(bodies, subDt);
 
-                // 4. Resolve constraints
+                // constraints
                 foreach (var constraint in constraints)
                 {
                     constraint.Solve();

PhysicsEngine/Physics/WorldModel.cs

@@ -13,7 +13,7 @@ namespace PhysicsEngine
         public WorldModel()
         {
             GlobalForces.Add(GravityForce.Apply);
-            //GlobalForces.Add(DragForce.Apply);
+            GlobalForces.Add(DragForce.Apply);
         }
 
         public void AddBody(Body body)

PhysicsEngine/UI/DrawHelper.cs

@@ -24,13 +24,11 @@ namespace PhysicsEngine
 
             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)
@@ -46,14 +44,11 @@ namespace PhysicsEngine
             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;