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 contacts = new List(); + // 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); - float rtA = Cross(contact.RA, tangent); - float rtB = Cross(contact.RB, tangent); + contact.Tangent = new Vector2(-contact.Normal.Y, contact.Normal.X); + float rtA = Cross(contact.RA, contact.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); - 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 vt = Vector2.Dot(rv, contact.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; + float oldImpulseT = contact.AccumulatedTangentImpulse; + contact.AccumulatedTangentImpulse = Math.Clamp(oldImpulseT + lambdaT, -maxFriction, maxFriction); + 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 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; - if (dist < 1e-6f) - normal = new Vector2(1, 0); - else - normal = d / dist; - + Vector2 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. - 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); - } - + 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); + if (minOverlap <= 0) return; + TestAxis(axisX2, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, false, ref axisFromB1); + if (minOverlap <= 0) return; + 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 - incNormal = -normal; // outward normal of incident face points toward reference box + refNormal = normal; + 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 clipped = new List { incFace[0], incFace[1] }; - ClipSegmentAgainstPlane(clipped, edgeDir, Vector2.Dot(edgeDir, refV1)); - ClipSegmentAgainstPlane(clipped, -edgeDir, -Vector2.Dot(edgeDir, refV2)); + clipped = ClipSegmentAgainstPlane(clipped, edgeDir, Vector2.Dot(edgeDir, refV1)); + 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 - 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. + // fallback for edge case precision drops 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 - + Vector2 contactPoint = (incFace[0] + incFace[1]) * 0.5f; 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); - outwardNormal = Vector2.Normalize(outwardNormal); - if (Vector2.Dot(outwardNormal, faceNormalWorld) > 0.999f) - return new Vector2[] { v1, v2 }; + Vector2 outwardNormal = Vector2.Normalize(new Vector2(edge.Y, -edge.X)); + float dot = Vector2.Dot(outwardNormal, faceNormalWorld); + if (dot > maxDot) + { + 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 segment, Vector2 normal, float offset) + private static List ClipSegmentAgainstPlane(List segment, Vector2 normal, float offset) { - if (segment.Count == 0) return; List result = new List(); - float d0 = Vector2.Dot(segment[0], normal) - offset; - float d1 = Vector2.Dot(segment[1], normal) - offset; + if (segment.Count < 2) return result; - if (d0 >= 0) result.Add(segment[0]); - if (d1 >= 0) result.Add(segment[1]); + Vector2 v0 = segment[0]; + 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(); - segment.AddRange(result); + if (d1 >= 0) result.Add(v1); + + return result; } private static Vector2 GetRelativeVelocity(Contact contact) diff --git a/PhysicsEngine/Physics/Forces/GravityForce.cs b/PhysicsEngine/Physics/Forces/GravityForce.cs index ad9b6da..8c24f67 100644 --- a/PhysicsEngine/Physics/Forces/GravityForce.cs +++ b/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); } } } \ No newline at end of file diff --git a/PhysicsEngine/Physics/Solver.cs b/PhysicsEngine/Physics/Solver.cs index 852ba68..8b8cf7a 100644 --- a/PhysicsEngine/Physics/Solver.cs +++ b/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 bodies, List constraints, List 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. + // accumulate global forces 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(); diff --git a/PhysicsEngine/Physics/WorldModel.cs b/PhysicsEngine/Physics/WorldModel.cs index 1bccd45..4755f22 100644 --- a/PhysicsEngine/Physics/WorldModel.cs +++ b/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) diff --git a/PhysicsEngine/UI/DrawHelper.cs b/PhysicsEngine/UI/DrawHelper.cs index ff096b2..a85ec3d 100644 --- a/PhysicsEngine/UI/DrawHelper.cs +++ b/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;