diff --git a/PhysicsEngine/Core/Program.cs b/PhysicsEngine/Core/Program.cs index e634975..65a3fd4 100644 --- a/PhysicsEngine/Core/Program.cs +++ b/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) - // 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; + // 2. Static Ramps and Platforms + Box platform = new Box(new Vector2(9.0f, 4.5f), new Vector2(3.5f, 0.3f), 0f); + platform.BaseColor = Color.Gray; + platform.Friction = 0.8f; 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); + Box ramp = new Box(new Vector2(2.0f, 3.5f), new Vector2(3.5f, 0.3f), 0f); + ramp.Rotation = 0.35f; // Slopes down toward center stack + ramp.BaseColor = Color.Gray; + 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) - 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); + Color[] stackColors = new Color[] + { + Color.DarkGreen, Color.Green, Color.Lime, Color.DarkPurple, Color.Yellow + }; - // 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++) + for (int i = 0; i < 5; 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); + float y = startY - (i * boxHeight); + 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) - 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); + // 4. Secondary Platform Stack (3 boxes high) + float platformStackX = 9.0f; + float platformTopY = 4.5f - 0.15f; // Top surface of platform + float platformStartY = platformTopY - (boxHeight * 0.5f); - 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); + for (int i = 0; i < 3; i++) + { + float y = platformStartY - (i * boxHeight); + Box pBox = new Box(new Vector2(platformStackX, y), new Vector2(0.5f, boxHeight), 12f); + pBox.BaseColor = Color.Blue; + pBox.Friction = 0.8f; + pBox.Restitution = 0.0f; + world.AddBody(pBox); + } - // 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); + // 5. Dynamic Circles for Stack Interaction + // Circle balanced on top of the main ground stack + float mainStackTopY = startY - (4 * boxHeight) - (boxHeight * 0.5f); + Circle stackCap = new Circle(new Vector2(stackX, mainStackTopY - 0.3f), 0.3f, 8f); + stackCap.BaseColor = Color.Orange; + stackCap.Friction = 0.8f; + stackCap.Restitution = 0.0f; + world.AddBody(stackCap); - 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); + // Heavy ball spawned on the ramp to roll down and strike the main stack + Circle rollingBall = new Circle(new Vector2(1.0f, 1.5f), 0.45f, 350f); + rollingBall.BaseColor = Color.Red; + rollingBall.Friction = 0.5f; + rollingBall.Restitution = 0.1f; + world.AddBody(rollingBall); - // 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); + // Bouncy ball dropped over the platform stack to test stability under impact + Circle droppingBall = new Circle(new Vector2(9.0f, 0.8f), 0.35f, 10f); + droppingBall.BaseColor = Color.Gold; + droppingBall.Friction = 0.4f; + droppingBall.Restitution = 0.4f; + droppingBall.Velocity = new Vector2(0.0f, 2.0f); + world.AddBody(droppingBall); } } } \ No newline at end of file diff --git a/PhysicsEngine/Physics/Bodies/Body.cs b/PhysicsEngine/Physics/Bodies/Body.cs index 804de86..982750d 100644 --- a/PhysicsEngine/Physics/Bodies/Body.cs +++ b/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; diff --git a/PhysicsEngine/Physics/Bodies/Box.cs b/PhysicsEngine/Physics/Bodies/Box.cs index f04f053..03ab62b 100644 --- a/PhysicsEngine/Physics/Bodies/Box.cs +++ b/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) { diff --git a/PhysicsEngine/Physics/CollisionEngine.cs b/PhysicsEngine/Physics/CollisionEngine.cs index f9a28b3..7e79ca3 100644 --- a/PhysicsEngine/Physics/CollisionEngine.cs +++ b/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
bodies, float dt) { + if (dt <= 0) return; + List