diff --git a/PhysicsEngine/Assets/Fonts/Roboto-Regular.ttf b/PhysicsEngine/Assets/Fonts/Roboto-Regular.ttf new file mode 100644 index 0000000..3db0d1f Binary files /dev/null and b/PhysicsEngine/Assets/Fonts/Roboto-Regular.ttf differ diff --git a/PhysicsEngine/Core/Program.cs b/PhysicsEngine/Core/Program.cs new file mode 100644 index 0000000..e634975 --- /dev/null +++ b/PhysicsEngine/Core/Program.cs @@ -0,0 +1,392 @@ +using System.Numerics; +using Raylib_cs; + +namespace PhysicsEngine +{ + class Program + { + private static Font robotoFont; + private static WorldModel world; // Instance of our WorldModel + + static void Main(string[] args) + { + Raylib.SetConfigFlags(ConfigFlags.FullscreenMode | ConfigFlags.VSyncHint); + Raylib.InitWindow(0, 0, "Physics Engine"); + + robotoFont = Raylib.LoadFont("Roboto-Regular.ttf"); + Raylib.SetTextureFilter(robotoFont.Texture, TextureFilter.Bilinear); + + Camera camera = new Camera(); + world = new WorldModel(); // Initialize simulation world + + ScenarioLarge(); + + const float physicsTimeStep = 1.0f / 60.0f; + float accumulator = 0.0f; + + // Main loop + while (!Raylib.WindowShouldClose()) + { + float frameTime = Raylib.GetFrameTime(); + if (frameTime > 0.25f) + { + frameTime = 0.25f; + } + + // Update Camera + camera.Update(frameTime); + + // Update Physics accumulator + accumulator += frameTime; + while (accumulator >= physicsTimeStep) + { + UpdatePhysics(physicsTimeStep); + accumulator -= physicsTimeStep; + } + + // Render loop + Raylib.BeginDrawing(); + Raylib.ClearBackground(MuiDarkColor.BackgroundDefault.ToColor()); + + // Draw background screen-space elements (Grid) + camera.DrawGrid(); + + // Draw World Bodies inside the Camera's 2D World Transform + Raylib.BeginMode2D(camera.RaylibCamera); + RenderScene(); + Raylib.EndMode2D(); + + // Draw UI / HUD elements + camera.DrawHUD(); + DrawText("Physics Engine", 20, 20, 30, MuiDarkColor.TextPrimary.ToColor()); + + Raylib.EndDrawing(); + } + + Raylib.UnloadFont(robotoFont); + Raylib.CloseWindow(); + } + + public static void DrawText(string text, int posX, int posY, float fontSize, Color color) + { + Raylib.DrawTextEx(robotoFont, text, new Vector2(posX, posY), fontSize, 1.0f, color); + } + + private static void UpdatePhysics(float dt) + { + world.Step(dt); + } + + private static void RenderScene() + { + // Iterate and draw all bodies in the world + foreach (var body in world.Bodies) + { + body.Draw(); + } + } + + private static void Scenario() + { + float worldWidth = 12.8f; + float worldHeight = 7.2f; + float wallThickness = 1.0f; + + // 1. Static world borders + Box floor = new Box(new Vector2(worldWidth * 0.5f, worldHeight + (wallThickness * 0.5f)), new Vector2(worldWidth + wallThickness * 2, wallThickness), 0f); + floor.BaseColor = Color.DarkGray; + world.AddBody(floor); + + Box ceiling = new Box(new Vector2(worldWidth * 0.5f, -wallThickness * 0.5f), new Vector2(worldWidth + wallThickness * 2, wallThickness), 0f); + ceiling.BaseColor = Color.DarkGray; + world.AddBody(ceiling); + + Box leftWall = new Box(new Vector2(-wallThickness * 0.5f, worldHeight * 0.5f), new Vector2(wallThickness, worldHeight + wallThickness * 2), 0f); + leftWall.BaseColor = Color.DarkGray; + world.AddBody(leftWall); + + Box rightWall = new Box(new Vector2(worldWidth + wallThickness * 0.5f, worldHeight * 0.5f), new Vector2(wallThickness, worldHeight + wallThickness * 2), 0f); + rightWall.BaseColor = Color.DarkGray; + world.AddBody(rightWall); + + // 2. Static ramps and platforms (rotated boxes) + // Left ramp (slopes up to the right) + Box leftRamp = new Box(new Vector2(2.5f, 5.5f), new Vector2(4.0f, 0.4f), 0f); + leftRamp.Rotation = -0.4f; // tilt upward to the right + leftRamp.BaseColor = Color.Gray; + world.AddBody(leftRamp); + + // Right ramp (slopes down to the right) + Box rightRamp = new Box(new Vector2(10.5f, 5.5f), new Vector2(4.0f, 0.4f), 0f); + rightRamp.Rotation = 0.4f; // tilt downward to the right + rightRamp.BaseColor = Color.Gray; + world.AddBody(rightRamp); + + // Floating platform in the middle + Box platform = new Box(new Vector2(6.4f, 3.5f), new Vector2(3.0f, 0.4f), 0f); + platform.BaseColor = Color.LightGray; + world.AddBody(platform); + + // Small static circle bumper near the bottom center + Circle staticBumper = new Circle(new Vector2(6.4f, 6.0f), 0.3f, 0f); + staticBumper.BaseColor = Color.Orange; + world.AddBody(staticBumper); + + // 3. Dynamic bodies – a mix of boxes and circles with varied properties + + // Tall, thin box (will tip over when hitting the ground) + Box tallBox = new Box(new Vector2(2.0f, 1.0f), new Vector2(0.3f, 1.8f), 30f); + tallBox.BaseColor = Color.SkyBlue; + tallBox.Rotation = 0.3f; + tallBox.AngularVelocity = 0.5f; + tallBox.Friction = 0.8f; + tallBox.Restitution = 0.1f; + world.AddBody(tallBox); + + // Heavy square box + Box heavyBox = new Box(new Vector2(4.0f, 1.5f), new Vector2(0.8f, 0.8f), 80f); + heavyBox.BaseColor = Color.DarkGreen; + heavyBox.Friction = 0.9f; + heavyBox.Restitution = 0.0f; + heavyBox.Velocity = new Vector2(1.5f, -1.0f); + world.AddBody(heavyBox); + + // Long plank (will slide and possibly balance) + Box plank = new Box(new Vector2(8.0f, 0.5f), new Vector2(2.5f, 0.25f), 25f); + plank.BaseColor = Color.Brown; + plank.Rotation = -0.2f; + plank.AngularVelocity = -0.3f; + plank.Friction = 0.6f; + world.AddBody(plank); + + // Bouncy ball (high restitution, low friction) + Circle bouncyBall = new Circle(new Vector2(9.0f, 1.0f), 0.3f, 5f); + bouncyBall.BaseColor = Color.Red; + bouncyBall.Restitution = 0.9f; + bouncyBall.Friction = 0.05f; + bouncyBall.Velocity = new Vector2(-30.0f, -20.0f); + bouncyBall.AngularVelocity = 10f; + world.AddBody(bouncyBall); + + // Heavy circle (rolls slowly) + Circle heavyBall = new Circle(new Vector2(5.0f, 0.8f), 0.5f, 40f); + heavyBall.BaseColor = Color.Purple; + heavyBall.Friction = 0.7f; + heavyBall.Restitution = 0.1f; + heavyBall.Velocity = new Vector2(2.0f, -0.5f); + world.AddBody(heavyBall); + + // Small fast circle + Circle fastCircle = new Circle(new Vector2(1.0f, 2.0f), 0.2f, 2f); + fastCircle.BaseColor = Color.Yellow; + fastCircle.Restitution = 0.7f; + fastCircle.Friction = 0.2f; + fastCircle.Velocity = new Vector2(6.0f, 2.0f); + world.AddBody(fastCircle); + + // Another box with high drag (will slow down quickly) + Box dragBox = new Box(new Vector2(10.0f, 2.0f), new Vector2(0.6f, 0.6f), 15f); + dragBox.BaseColor = Color.Violet; + dragBox.DragCoefficient = 2.0f; + dragBox.Velocity = new Vector2(-2.0f, -1.0f); + dragBox.AngularVelocity = 2.0f; + world.AddBody(dragBox); + + // Circle with high drag (like a light ball in air) + Circle dragCircle = new Circle(new Vector2(3.0f, 3.0f), 0.4f, 8f); + dragCircle.BaseColor = Color.Pink; + dragCircle.DragCoefficient = 1.5f; + dragCircle.Velocity = new Vector2(1.0f, 3.0f); + world.AddBody(dragCircle); + + // A box launched with an impulse at an offset (will spin) + Box spinningBox = new Box(new Vector2(7.0f, 2.5f), new Vector2(0.5f, 0.5f), 10f); + spinningBox.BaseColor = Color.Beige; + spinningBox.ApplyImpulseAtOffset(new Vector2(3.0f, -5.0f), new Vector2(0.25f, 0.0f)); + world.AddBody(spinningBox); + + // A circle launched with angular impulse + Circle spinningCircle = new Circle(new Vector2(11.0f, 1.0f), 0.35f, 12f); + spinningCircle.BaseColor = Color.Lime; + spinningCircle.ApplyImpulseAtOffset(new Vector2(0.0f, -8.0f), new Vector2(0.1f, 0.0f)); + world.AddBody(spinningCircle); + } + + private static void ScenarioLarge() + { + // Larger world dimensions + float worldWidth = 20.0f; + float worldHeight = 12.0f; + float wallThickness = 1.0f; + + // 1. Static world borders + Box floor = new Box(new Vector2(worldWidth * 0.5f, worldHeight + (wallThickness * 0.5f)), new Vector2(worldWidth + wallThickness * 2, wallThickness), 0f); + floor.BaseColor = Color.DarkGray; + world.AddBody(floor); + + Box ceiling = new Box(new Vector2(worldWidth * 0.5f, -wallThickness * 0.5f), new Vector2(worldWidth + wallThickness * 2, wallThickness), 0f); + ceiling.BaseColor = Color.DarkGray; + world.AddBody(ceiling); + + Box leftWall = new Box(new Vector2(-wallThickness * 0.5f, worldHeight * 0.5f), new Vector2(wallThickness, worldHeight + wallThickness * 2), 0f); + leftWall.BaseColor = Color.DarkGray; + world.AddBody(leftWall); + + Box rightWall = new Box(new Vector2(worldWidth + wallThickness * 0.5f, worldHeight * 0.5f), new Vector2(wallThickness, worldHeight + wallThickness * 2), 0f); + rightWall.BaseColor = Color.DarkGray; + world.AddBody(rightWall); + + // 2. Static ramps (angled platforms) + Box ramp1 = new Box(new Vector2(4.0f, 9.0f), new Vector2(5.0f, 0.4f), 0f); + ramp1.Rotation = -0.35f; + ramp1.BaseColor = Color.Gray; + world.AddBody(ramp1); + + Box ramp2 = new Box(new Vector2(16.0f, 9.0f), new Vector2(5.0f, 0.4f), 0f); + ramp2.Rotation = 0.35f; + ramp2.BaseColor = Color.Gray; + world.AddBody(ramp2); + + // Static horizontal platforms + Box platform1 = new Box(new Vector2(7.0f, 6.0f), new Vector2(4.0f, 0.4f), 0f); + platform1.BaseColor = Color.LightGray; + world.AddBody(platform1); + + Box platform2 = new Box(new Vector2(13.0f, 6.0f), new Vector2(4.0f, 0.4f), 0f); + platform2.BaseColor = Color.LightGray; + world.AddBody(platform2); + + // Static vertical pillars to create obstacles + Box pillar1 = new Box(new Vector2(5.0f, 4.0f), new Vector2(0.3f, 3.0f), 0f); + pillar1.BaseColor = Color.DarkBrown; + world.AddBody(pillar1); + + Box pillar2 = new Box(new Vector2(15.0f, 4.0f), new Vector2(0.3f, 3.0f), 0f); + pillar2.BaseColor = Color.DarkBrown; + world.AddBody(pillar2); + + // Static circular bumpers + Circle bumper1 = new Circle(new Vector2(10.0f, 8.0f), 0.4f, 0f); + bumper1.BaseColor = Color.Orange; + world.AddBody(bumper1); + + Circle bumper2 = new Circle(new Vector2(10.0f, 3.0f), 0.4f, 0f); + bumper2.BaseColor = Color.Orange; + world.AddBody(bumper2); + + Circle bumper3 = new Circle(new Vector2(2.0f, 7.0f), 0.3f, 0f); + bumper3.BaseColor = Color.Orange; + world.AddBody(bumper3); + + Circle bumper4 = new Circle(new Vector2(18.0f, 7.0f), 0.3f, 0f); + bumper4.BaseColor = Color.Orange; + world.AddBody(bumper4); + + // 3. Dynamic bodies – many with high energy, low damping + + // High restitution balls (bouncy) + for (int i = 0; i < 6; i++) + { + Circle bouncyBall = new Circle(new Vector2(2.0f + i * 3.0f, 1.5f), 0.25f, 3.0f); + bouncyBall.BaseColor = Color.Red; + bouncyBall.Restitution = 0.95f; + bouncyBall.Friction = 0.05f; + bouncyBall.Velocity = new Vector2((i % 2 == 0 ? 3.0f : -3.0f), -2.0f); + bouncyBall.AngularVelocity = 5.0f; + world.AddBody(bouncyBall); + } + + // Heavy boxes (low restitution, high friction) + Box heavy1 = new Box(new Vector2(6.0f, 2.0f), new Vector2(1.0f, 0.8f), 60f); + heavy1.BaseColor = Color.DarkGreen; + heavy1.Friction = 0.9f; + heavy1.Restitution = 0.0f; + heavy1.Velocity = new Vector2(2.0f, -1.0f); + world.AddBody(heavy1); + + Box heavy2 = new Box(new Vector2(14.0f, 2.0f), new Vector2(1.0f, 0.8f), 60f); + heavy2.BaseColor = Color.DarkGreen; + heavy2.Friction = 0.9f; + heavy2.Restitution = 0.0f; + heavy2.Velocity = new Vector2(-2.0f, -1.0f); + world.AddBody(heavy2); + + // Long planks (will slide and rotate) + Box plank1 = new Box(new Vector2(8.0f, 10.5f), new Vector2(3.0f, 0.3f), 20f); + plank1.BaseColor = Color.Brown; + plank1.Rotation = 0.1f; + plank1.AngularVelocity = -0.4f; + plank1.Friction = 0.6f; + plank1.Velocity = new Vector2(1.0f, -0.5f); + world.AddBody(plank1); + + Box plank2 = new Box(new Vector2(12.0f, 10.5f), new Vector2(3.0f, 0.3f), 20f); + plank2.BaseColor = Color.Brown; + plank2.Rotation = -0.1f; + plank2.AngularVelocity = 0.4f; + plank2.Friction = 0.6f; + plank2.Velocity = new Vector2(-1.0f, -0.5f); + world.AddBody(plank2); + + // Spinning boxes (launched with off-center impulses) + Box spinBox1 = new Box(new Vector2(5.0f, 7.0f), new Vector2(0.6f, 0.6f), 10f); + spinBox1.BaseColor = Color.Beige; + spinBox1.ApplyImpulseAtOffset(new Vector2(4.0f, -6.0f), new Vector2(0.3f, 0.0f)); + world.AddBody(spinBox1); + + Box spinBox2 = new Box(new Vector2(15.0f, 7.0f), new Vector2(0.6f, 0.6f), 10f); + spinBox2.BaseColor = Color.Beige; + spinBox2.ApplyImpulseAtOffset(new Vector2(-4.0f, -6.0f), new Vector2(-0.3f, 0.0f)); + world.AddBody(spinBox2); + + // Small fast circles + Circle fast1 = new Circle(new Vector2(3.0f, 11.0f), 0.2f, 2f); + fast1.BaseColor = Color.Yellow; + fast1.Restitution = 0.8f; + fast1.Friction = 0.2f; + fast1.Velocity = new Vector2(8.0f, 1.0f); + world.AddBody(fast1); + + Circle fast2 = new Circle(new Vector2(17.0f, 11.0f), 0.2f, 2f); + fast2.BaseColor = Color.Yellow; + fast2.Restitution = 0.8f; + fast2.Friction = 0.2f; + fast2.Velocity = new Vector2(-8.0f, 1.0f); + world.AddBody(fast2); + + // High-drag objects that still move initially + Box dragBox = new Box(new Vector2(10.0f, 9.0f), new Vector2(0.8f, 0.8f), 15f); + dragBox.BaseColor = Color.Violet; + dragBox.DragCoefficient = 2.0f; + dragBox.Velocity = new Vector2(3.0f, -2.0f); + dragBox.AngularVelocity = 2.0f; + world.AddBody(dragBox); + + Circle dragCircle = new Circle(new Vector2(10.0f, 5.0f), 0.4f, 8f); + dragCircle.BaseColor = Color.Pink; + dragCircle.DragCoefficient = 1.5f; + dragCircle.Velocity = new Vector2(-2.0f, 3.0f); + world.AddBody(dragCircle); + + // A few more random objects to fill the space + Circle extra1 = new Circle(new Vector2(1.5f, 5.0f), 0.35f, 12f); + extra1.BaseColor = Color.Lime; + extra1.Restitution = 0.7f; + extra1.Velocity = new Vector2(5.0f, -3.0f); + extra1.AngularVelocity = 8.0f; + world.AddBody(extra1); + + Circle extra2 = new Circle(new Vector2(18.5f, 5.0f), 0.35f, 12f); + extra2.BaseColor = Color.Lime; + extra2.Restitution = 0.7f; + extra2.Velocity = new Vector2(-5.0f, -3.0f); + extra2.AngularVelocity = -8.0f; + world.AddBody(extra2); + + Box extraBox = new Box(new Vector2(10.0f, 1.0f), new Vector2(1.2f, 0.6f), 25f); + extraBox.BaseColor = Color.SkyBlue; + extraBox.Velocity = new Vector2(0.5f, -2.0f); + world.AddBody(extraBox); + } + } +} \ No newline at end of file diff --git a/PhysicsEngine/Camera.cs b/PhysicsEngine/Graphics/Camera.cs similarity index 95% rename from PhysicsEngine/Camera.cs rename to PhysicsEngine/Graphics/Camera.cs index b4adfba..63616e4 100644 --- a/PhysicsEngine/Camera.cs +++ b/PhysicsEngine/Graphics/Camera.cs @@ -12,7 +12,7 @@ namespace PhysicsEngine private const float MoveLerpSpeed = 20.0f; private const float ZoomFactor = 0.1f; private const float MinZoom = 0.2f; - private const float MaxZoom = 500000000000000000000000000000000000.0f; + private const float MaxZoom = 50.0f; public Camera2D RaylibCamera { get; private set; } public Vector2 Position => RaylibCamera.Target / PixelsPerMeter; @@ -82,6 +82,8 @@ namespace PhysicsEngine targetPosition = Vector2.Zero; targetZoom = 1.0f; isDragging = false; + cam.Target = Vector2.Zero; + cam.Zoom = 1.0f; } // zooming @@ -114,7 +116,6 @@ namespace PhysicsEngine { cam.Target.X += (targetPosition.X - cam.Target.X) * MoveLerpSpeed * frameTime; cam.Target.Y += (targetPosition.Y - cam.Target.Y) * MoveLerpSpeed * frameTime; - targetPosition = cam.Target; } RaylibCamera = cam; @@ -180,8 +181,8 @@ namespace PhysicsEngine public void DrawHUD() { float screenH = Raylib.GetScreenHeight(); - string hudText = $"X: {Position.X:F1}m Y: {Position.Y:F1}m Zoom: {Zoom:F2}x"; - Raylib.DrawText(hudText, 20, (int)screenH - 40, 20, MuiDarkColor.TextPrimary.ToColor()); + string hudText = $"X: {Position.X:F2}m Y: {Position.Y:F2}m Zoom: {Zoom:F2}x"; + Program.DrawText(hudText, 20, (int)screenH - 40, 20, MuiDarkColor.TextPrimary.ToColor()); } } } \ No newline at end of file diff --git a/PhysicsEngine/Physics/Bodies/Body.cs b/PhysicsEngine/Physics/Bodies/Body.cs new file mode 100644 index 0000000..804de86 --- /dev/null +++ b/PhysicsEngine/Physics/Bodies/Body.cs @@ -0,0 +1,123 @@ +using System; +using System.Numerics; +using Raylib_cs; + +namespace PhysicsEngine +{ + public class Body + { + public Vector2 Position; + public Vector2 Velocity; + public Vector2 ForceAccumulator; + public Color BaseColor = Color.Blue; + public float Rotation; + public float AngularVelocity; + public float TorqueAccumulator; + public float Mass; + public float InverseMass => Mass > 0.0f ? 1.0f / Mass : 0.0f; + public float MomentOfInertia; + public float InverseInertia => MomentOfInertia > 0.0f ? 1.0f / MomentOfInertia : 0.0f; + public float Restitution = 0.5f; + public float Friction = 0.2f; + public float DragCoefficient = 0.0f; // Drag per m^2 (ignored if 0.0) + public bool IsStatic => Mass == 0.0f; + + public void ApplyForce(Vector2 force) + { + if (IsStatic) return; + ForceAccumulator += force; + } + + public void ApplyTorque(float torque) + { + if (IsStatic) return; + TorqueAccumulator += torque; + } + + // --- Impulse Methods --- + + public void ApplyImpulse(Vector2 impulse) + { + if (IsStatic) return; + Velocity += impulse * InverseMass; + } + + public void ApplyImpulseAtOffset(Vector2 impulse, Vector2 localOffset) + { + if (IsStatic) return; + + // Direct linear velocity change + Velocity += impulse * InverseMass; + + // Rotate local offset into world space based on body rotation + float cos = MathF.Cos(Rotation); + float sin = MathF.Sin(Rotation); + Vector2 worldOffset = new Vector2( + localOffset.X * cos - localOffset.Y * sin, + localOffset.X * sin + localOffset.Y * cos + ); + + // 2D Cross product for torque: r x J + float torque = worldOffset.X * impulse.Y - worldOffset.Y * impulse.X; + AngularVelocity += torque * InverseInertia; + } + + public void ApplyImpulseAtWorldPosition(Vector2 impulse, Vector2 worldPosition) + { + if (IsStatic) return; + + // Direct linear velocity change + Velocity += impulse * InverseMass; + + // Offset from center of mass in world space + Vector2 worldOffset = worldPosition - Position; + + // 2D Cross product for torque: r x J + float torque = worldOffset.X * impulse.Y - worldOffset.Y * impulse.X; + AngularVelocity += torque * InverseInertia; + } + + // --- Other Useful Methods to Consider --- + + public void ApplyForceAtWorldPosition(Vector2 force, Vector2 worldPosition) + { + if (IsStatic) return; + + ForceAccumulator += force; + Vector2 worldOffset = worldPosition - Position; + float torque = worldOffset.X * force.Y - worldOffset.Y * force.X; + TorqueAccumulator += torque; + } + + public Vector2 GetWorldPointFromLocal(Vector2 localPoint) + { + float cos = MathF.Cos(Rotation); + float sin = MathF.Sin(Rotation); + Vector2 rotated = new Vector2( + localPoint.X * cos - localPoint.Y * sin, + localPoint.X * sin + localPoint.Y * cos + ); + return Position + rotated; + } + + public Vector2 GetLocalPointFromWorld(Vector2 worldPoint) + { + Vector2 delta = worldPoint - Position; + float cos = MathF.Cos(-Rotation); + float sin = MathF.Sin(-Rotation); + return new Vector2( + delta.X * cos - delta.Y * sin, + delta.X * sin + delta.Y * cos + ); + } + + public void ClearForces() + { + ForceAccumulator = Vector2.Zero; + TorqueAccumulator = 0.0f; + } + + public virtual void Draw(float pixelsPerMeter = 100.0f) {} + public virtual void ApplyAerodynamicDrag(float dt) {} + } +} \ No newline at end of file diff --git a/PhysicsEngine/Physics/Bodies/Box.cs b/PhysicsEngine/Physics/Bodies/Box.cs new file mode 100644 index 0000000..f04f053 --- /dev/null +++ b/PhysicsEngine/Physics/Bodies/Box.cs @@ -0,0 +1,78 @@ +using System; +using System.Numerics; +using Raylib_cs; + +namespace PhysicsEngine +{ + public class Box : Body + { + public Vector2 Size; + + public Box(Vector2 position, Vector2 size, float mass) + { + Position = position; + Size = size; + Mass = mass; + MomentOfInertia = (1.0f / 12.0f) * mass * (size.X * size.X + size.Y * size.Y); + } + + public override void Draw(float pixelsPerMeter = 100.0f) + { + DrawHelper.DrawRotatedBox(Position, Size, Rotation, BaseColor, pixelsPerMeter); + } + + private Vector2 RotatePoint(Vector2 point, float radians) + { + float cos = MathF.Cos(radians); + float sin = MathF.Sin(radians); + return new Vector2( + point.X * cos - point.Y * sin, + point.X * sin + point.Y * cos + ); + } + + public override void ApplyAerodynamicDrag(float dt) + { + if (DragCoefficient <= 0.0f) return; + + const float airDensity = 1.225f; + float speed = Velocity.Length(); + if (speed < 0.01f) return; + + // 1. Oncoming relative wind direction (opposite to velocity) + Vector2 windDir = -Velocity / speed; + + float cos = MathF.Cos(Rotation); + float sin = MathF.Sin(Rotation); + + // Box local axes transformed into world space (Assuming Size.Y is length/major axis) + Vector2 worldAxisY = new Vector2(-sin, cos); // Major axis vector + Vector2 worldAxisX = new Vector2(cos, sin); // Minor axis vector + + // 2. Calculate effective frontal width (projected area in 2D) based on current orientation + Vector2 windPerp = new Vector2(-windDir.Y, windDir.X); + float projX = MathF.Abs(Vector2.Dot(worldAxisX, windPerp)); + float projY = MathF.Abs(Vector2.Dot(worldAxisY, windPerp)); + float effectiveWidth = Size.X * projX + Size.Y * projY; + + // 3. Translational Drag Force (scales with dynamic pressure and current effective width) + float dragMagnitude = 0.5f * airDensity * speed * speed * DragCoefficient * effectiveWidth; + Vector2 dragForce = windDir * dragMagnitude; + ApplyForce(dragForce); + + // 4. Weathercock / Fin-Effect Restoring Torque + // Measures angular misalignment between the box's major axis and the wind direction + float cross = worldAxisY.X * windDir.Y - worldAxisY.Y * windDir.X; + float restoringTorque = cross * 0.5f * airDensity * speed * speed * DragCoefficient * Size.X * Size.Y; + ApplyTorque(restoringTorque); + + // 5. Angular Damping (Quadratic Rotational Drag) + float angularSpeed = MathF.Abs(AngularVelocity); + if (angularSpeed > 0.0001f) + { + float angularDragTorque = -MathF.Sign(AngularVelocity) * 0.5f * airDensity * angularSpeed * angularSpeed * DragCoefficient * (Size.X + Size.Y); + ApplyTorque(angularDragTorque); + } + } + } +} \ No newline at end of file diff --git a/PhysicsEngine/Physics/Bodies/Circle.cs b/PhysicsEngine/Physics/Bodies/Circle.cs new file mode 100644 index 0000000..5a9bb44 --- /dev/null +++ b/PhysicsEngine/Physics/Bodies/Circle.cs @@ -0,0 +1,44 @@ +using System; +using System.Numerics; +using Raylib_cs; + +namespace PhysicsEngine +{ + public class Circle : Body + { + public float Radius; + + public Circle(Vector2 position, float radius, float mass) + { + Position = position; + Radius = radius; + Mass = mass; + + if (mass > 0.0f) + { + MomentOfInertia = 0.5f * mass * radius * radius; + } + } + + public override void Draw(float pixelsPerMeter = 100.0f) + { + DrawHelper.DrawCircleBody(Position, Radius, Rotation, BaseColor, pixelsPerMeter); + } + + public override void ApplyAerodynamicDrag(float dt) + { + if (DragCoefficient <= 0.0f) return; + + const float airDensity = 1.225f; + float area = Radius * 2.0f; + float speed = Velocity.Length(); + + if (speed > 0.0001f) + { + Vector2 dragDir = -Vector2.Normalize(Velocity); + float dragMagnitude = 0.5f * airDensity * speed * speed * DragCoefficient * area; + ApplyForce(dragDir * dragMagnitude); + } + } + } +} \ No newline at end of file diff --git a/PhysicsEngine/Physics/CollisionEngine.cs b/PhysicsEngine/Physics/CollisionEngine.cs new file mode 100644 index 0000000..f9a28b3 --- /dev/null +++ b/PhysicsEngine/Physics/CollisionEngine.cs @@ -0,0 +1,505 @@ +using System; +using System.Collections.Generic; +using System.Numerics; + +namespace PhysicsEngine +{ + public static class CollisionEngine + { + private const float Slop = 0.01f; + private const float PositionCorrectionPercent = 0.1f; + private const float MaxCorrection = 0.2f; + private const int VelocityIterations = 5; + + private class Contact + { + public Vector2 Point; + public Vector2 Normal; // from BodyA to BodyB + public float Penetration; + public float AccumulatedNormalImpulse; + public float AccumulatedTangentImpulse; + public Body BodyA; + public Body BodyB; + public Vector2 RA; + public Vector2 RB; + public float NormalMass; + public float TangentMass; + public float Friction; + public float Restitution; + } + + public static void ResolveCollisions(List bodies, float dt) + { + List contacts = new List(); + + for (int i = 0; i < bodies.Count; i++) + { + for (int j = i + 1; j < bodies.Count; j++) + { + Body b1 = bodies[i]; + Body b2 = bodies[j]; + if (b1.IsStatic && b2.IsStatic) continue; + + if (b1 is Circle && b2 is Circle) + ResolveCircleCircle((Circle)b1, (Circle)b2, contacts); + else if (b1 is Box && b2 is Circle) + ResolveBoxCircle((Box)b1, (Circle)b2, contacts, false); + else if (b1 is Circle && b2 is Box) + ResolveBoxCircle((Box)b2, (Circle)b1, contacts, true); + else if (b1 is Box && b2 is Box) + ResolveBoxBox((Box)b1, (Box)b2, contacts); + } + } + + foreach (var contact in contacts) + { + contact.AccumulatedNormalImpulse = 0; + contact.AccumulatedTangentImpulse = 0; + contact.RA = contact.Point - contact.BodyA.Position; + contact.RB = contact.Point - contact.BodyB.Position; + + float invMassA = contact.BodyA.InverseMass; + float invMassB = contact.BodyB.InverseMass; + float invIA = contact.BodyA.InverseInertia; + float invIB = contact.BodyB.InverseInertia; + + float rnA = Cross(contact.RA, contact.Normal); + float rnB = Cross(contact.RB, contact.Normal); + float denom = invMassA + invMassB + rnA * rnA * invIA + rnB * rnB * invIB; + contact.NormalMass = denom > 0 ? 1.0f / denom : 0; + + Vector2 tangent = new Vector2(-contact.Normal.Y, contact.Normal.X); + float rtA = Cross(contact.RA, tangent); + float rtB = Cross(contact.RB, tangent); + denom = invMassA + invMassB + rtA * rtA * invIA + rtB * rtB * invIB; + contact.TangentMass = denom > 0 ? 1.0f / denom : 0; + + contact.Friction = (float)Math.Sqrt(contact.BodyA.Friction * contact.BodyB.Friction); + contact.Restitution = Math.Max(contact.BodyA.Restitution, contact.BodyB.Restitution); + } + + for (int iter = 0; iter < VelocityIterations; iter++) + { + foreach (var contact in contacts) + { + Vector2 rv = GetRelativeVelocity(contact); + float vn = Vector2.Dot(rv, contact.Normal); + float lambda = -contact.NormalMass * vn; + + float newImpulse = Math.Max(contact.AccumulatedNormalImpulse + lambda, 0); + lambda = newImpulse - contact.AccumulatedNormalImpulse; + contact.AccumulatedNormalImpulse = newImpulse; + + ApplyImpulse(contact, contact.Normal * lambda); + + rv = GetRelativeVelocity(contact); + Vector2 tangent = rv - contact.Normal * Vector2.Dot(rv, contact.Normal); + if (tangent.LengthSquared() > 1e-6f) + tangent = Vector2.Normalize(tangent); + else + tangent = new Vector2(-contact.Normal.Y, contact.Normal.X); + + float vt = Vector2.Dot(rv, tangent); + float lambdaT = -contact.TangentMass * vt; + float maxFriction = contact.Friction * contact.AccumulatedNormalImpulse; + float newImpulseT = Math.Clamp(contact.AccumulatedTangentImpulse + lambdaT, -maxFriction, maxFriction); + lambdaT = newImpulseT - contact.AccumulatedTangentImpulse; + contact.AccumulatedTangentImpulse = newImpulseT; + + ApplyImpulse(contact, tangent * lambdaT); + } + } + + var pairMap = new Dictionary<(Body, Body), Contact>(); + foreach (var contact in contacts) + { + var key = (contact.BodyA, contact.BodyB); + if (!pairMap.ContainsKey(key) || pairMap[key].Penetration < contact.Penetration) + pairMap[key] = contact; + } + + foreach (var kvp in pairMap) + { + var contact = kvp.Value; + float correctionMagnitude = Math.Max(contact.Penetration - Slop, 0) * PositionCorrectionPercent; + correctionMagnitude = Math.Min(correctionMagnitude, MaxCorrection); + if (correctionMagnitude <= 0) continue; + + Vector2 correction = contact.Normal * correctionMagnitude; + float totalInvMass = contact.BodyA.InverseMass + contact.BodyB.InverseMass; + if (totalInvMass <= 0) continue; + + float ratioA = contact.BodyA.InverseMass / totalInvMass; + float ratioB = contact.BodyB.InverseMass / totalInvMass; + + contact.BodyA.Position -= correction * ratioA; + contact.BodyB.Position += correction * ratioB; + } + } + + private static void ResolveCircleCircle(Circle c1, Circle c2, List contacts) + { + Vector2 d = c2.Position - c1.Position; + float distSq = d.LengthSquared(); + float radiusSum = c1.Radius + c2.Radius; + if (distSq >= radiusSum * radiusSum) return; + + float dist = (float)Math.Sqrt(distSq); + Vector2 normal; + if (dist < 1e-6f) + normal = new Vector2(1, 0); + else + normal = d / dist; + + float penetration = radiusSum - dist; + Vector2 contactPoint = c1.Position + normal * (c1.Radius - penetration * 0.5f); + + contacts.Add(new Contact + { + Point = contactPoint, + Normal = normal, + Penetration = penetration, + BodyA = c1, + BodyB = c2 + }); + } + + private static void ResolveBoxCircle(Box box, Circle circle, List contacts, bool swapped) + { + Vector2 localCircleCenter = circle.Position - box.Position; + float cos = (float)Math.Cos(box.Rotation); + float sin = (float)Math.Sin(box.Rotation); + Vector2 local = new Vector2( + localCircleCenter.X * cos + localCircleCenter.Y * sin, + -localCircleCenter.X * sin + localCircleCenter.Y * cos); + + float hx = box.Size.X * 0.5f; + float hy = box.Size.Y * 0.5f; + Vector2 closestLocal = new Vector2(Math.Clamp(local.X, -hx, hx), Math.Clamp(local.Y, -hy, hy)); + + Vector2 normalWorld; + Vector2 contactPointWorld; + float penetration; + + bool inside = (closestLocal == local); + if (inside) + { + float distLeft = local.X + hx; + float distRight = hx - local.X; + float distBottom = local.Y + hy; + float distTop = hy - local.Y; + float minDist = Math.Min(Math.Min(distLeft, distRight), Math.Min(distBottom, distTop)); + + Vector2 normalLocal; + if (minDist == distLeft) normalLocal = new Vector2(-1, 0); + else if (minDist == distRight) normalLocal = new Vector2(1, 0); + else if (minDist == distBottom) normalLocal = new Vector2(0, -1); + else normalLocal = new Vector2(0, 1); + + normalWorld = new Vector2(normalLocal.X * cos - normalLocal.Y * sin, + normalLocal.X * sin + normalLocal.Y * cos); + penetration = circle.Radius + minDist; + + Vector2 faceLocal = local; + if (normalLocal.X != 0) faceLocal.X = normalLocal.X > 0 ? hx : -hx; + else faceLocal.Y = normalLocal.Y > 0 ? hy : -hy; + contactPointWorld = box.Position + new Vector2(faceLocal.X * cos - faceLocal.Y * sin, + faceLocal.X * sin + faceLocal.Y * cos); + } + else + { + Vector2 closestWorld = box.Position + new Vector2(closestLocal.X * cos - closestLocal.Y * sin, + closestLocal.X * sin + closestLocal.Y * cos); + Vector2 delta = circle.Position - closestWorld; + float distSq = delta.LengthSquared(); + float radiusSq = circle.Radius * circle.Radius; + if (distSq >= radiusSq) return; + + float dist = (float)Math.Sqrt(distSq); + normalWorld = dist > 1e-6f ? delta / dist : new Vector2(1, 0); + penetration = circle.Radius - dist; + contactPointWorld = closestWorld; + } + + Contact contact = new Contact + { + Point = contactPointWorld, + Normal = normalWorld, + Penetration = penetration, + BodyA = box, + BodyB = circle + }; + + if (swapped) + { + contact.BodyA = circle; + contact.BodyB = box; + contact.Normal = -contact.Normal; + } + + contacts.Add(contact); + } + + private static void ResolveBoxBox(Box b1, Box b2, List contacts) + { + float h1x = b1.Size.X * 0.5f; + float h1y = b1.Size.Y * 0.5f; + float h2x = b2.Size.X * 0.5f; + float h2y = b2.Size.Y * 0.5f; + + float cos1 = (float)Math.Cos(b1.Rotation); + float sin1 = (float)Math.Sin(b1.Rotation); + float cos2 = (float)Math.Cos(b2.Rotation); + float sin2 = (float)Math.Sin(b2.Rotation); + + Vector2 axisX1 = new Vector2(cos1, sin1); + Vector2 axisY1 = new Vector2(-sin1, cos1); + Vector2 axisX2 = new Vector2(cos2, sin2); + Vector2 axisY2 = new Vector2(-sin2, cos2); + + Vector2 d = b2.Position - b1.Position; + float minOverlap = float.MaxValue; + Vector2 bestAxis = Vector2.Zero; + bool axisFromB1 = false; + + // Test axes. If one body is static, only test that body's axes for stability. + if (b1.IsStatic && !b2.IsStatic) + { + TestAxis(axisX1, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, true, ref axisFromB1); + TestAxis(axisY1, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, true, ref axisFromB1); + } + else if (b2.IsStatic && !b1.IsStatic) + { + TestAxis(axisX2, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, false, ref axisFromB1); + TestAxis(axisY2, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, false, ref axisFromB1); + } + else + { + TestAxis(axisX1, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, true, ref axisFromB1); + TestAxis(axisY1, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, true, ref axisFromB1); + TestAxis(axisX2, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, false, ref axisFromB1); + TestAxis(axisY2, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, false, ref axisFromB1); + } + + if (minOverlap <= 0 || minOverlap == float.MaxValue) return; + + Vector2 normal = Vector2.Dot(d, bestAxis) > 0 ? bestAxis : -bestAxis; + + Box refBox, incBox; + Vector2 refNormal, incNormal; + if (axisFromB1) + { + refBox = b1; + incBox = b2; + refNormal = normal; // outward normal of reference face points toward incident box + incNormal = -normal; // outward normal of incident face points toward reference box + } + else + { + refBox = b2; + incBox = b1; + refNormal = -normal; + incNormal = normal; + } + + Vector2[] refFace = GetFaceVertices(refBox, refNormal); + Vector2[] incFace = GetFaceVertices(incBox, incNormal); + if (refFace.Length < 2 || incFace.Length < 2) return; + + Vector2 refV1 = refFace[0]; + Vector2 refV2 = refFace[1]; + Vector2 edgeDir = refV2 - refV1; + if (edgeDir.LengthSquared() < 1e-8f) return; + edgeDir = Vector2.Normalize(edgeDir); + + int contactsBefore = contacts.Count; // to check if we added any new contacts + + // Clip incident face against reference face side planes + List clipped = new List { incFace[0], incFace[1] }; + ClipSegmentAgainstPlane(clipped, edgeDir, Vector2.Dot(edgeDir, refV1)); + ClipSegmentAgainstPlane(clipped, -edgeDir, -Vector2.Dot(edgeDir, refV2)); + + // Add contacts from clipped incident face points + foreach (var p in clipped) + { + float pen = -Vector2.Dot(p - refV1, refNormal); + if (pen > 0) + { + contacts.Add(new Contact + { + Point = p, + Normal = normal, + Penetration = pen, + BodyA = b1, + BodyB = b2 + }); + } + } + + // Check reference face vertices that penetrate the incident face + foreach (var refVertex in refFace) + { + // Penetration depth along incident face normal (positive if inside) + float pen = -Vector2.Dot(refVertex - incFace[0], incNormal); + if (pen > 0) + { + // Avoid duplicate contacts at nearly the same location + bool duplicate = false; + foreach (var c in contacts) + { + if (Vector2.DistanceSquared(c.Point, refVertex) < 1e-6f) + { + duplicate = true; + break; + } + } + if (!duplicate) + { + contacts.Add(new Contact + { + Point = refVertex, + Normal = normal, + Penetration = pen, + BodyA = b1, + BodyB = b2 + }); + } + } + } + + // Fallback: if no contacts were generated despite SAT overlap, create a single contact at the midpoint + // of the overlapping region along the normal. This prevents any missed collisions. + if (contacts.Count == contactsBefore) + { + // Compute the projection of both boxes onto the normal + float projB1 = h1x * Math.Abs(Vector2.Dot(axisX1, normal)) + h1y * Math.Abs(Vector2.Dot(axisY1, normal)); + float projB2 = h2x * Math.Abs(Vector2.Dot(axisX2, normal)) + h2y * Math.Abs(Vector2.Dot(axisY2, normal)); + float dist = Vector2.Dot(d, normal); // distance from b1 to b2 along normal + float min1 = -projB1; + float max1 = projB1; + float min2 = dist - projB2; + float max2 = dist + projB2; + + float overlapMin = Math.Max(min1, min2); + float overlapMax = Math.Min(max1, max2); + float overlapMid = (overlapMin + overlapMax) * 0.5f; + Vector2 contactPoint = b1.Position + normal * overlapMid; // rough position along normal + + contacts.Add(new Contact + { + Point = contactPoint, + Normal = normal, + Penetration = minOverlap, // use SAT overlap + BodyA = b1, + BodyB = b2 + }); + } + } + + private static void TestAxis(Vector2 axis, Box b1, Box b2, + float h1x, float h1y, float h2x, float h2y, + Vector2 axisX1, Vector2 axisY1, Vector2 axisX2, Vector2 axisY2, + Vector2 d, ref float minOverlap, ref Vector2 bestAxis, bool fromB1, ref bool axisFromB1) + { + float r1 = h1x * Math.Abs(Vector2.Dot(axisX1, axis)) + h1y * Math.Abs(Vector2.Dot(axisY1, axis)); + float r2 = h2x * Math.Abs(Vector2.Dot(axisX2, axis)) + h2y * Math.Abs(Vector2.Dot(axisY2, axis)); + float distance = Math.Abs(Vector2.Dot(d, axis)); + float overlap = r1 + r2 - distance; + + if (overlap <= 0) + { + minOverlap = overlap; + return; + } + + if (overlap < minOverlap) + { + minOverlap = overlap; + bestAxis = axis; + axisFromB1 = fromB1; + } + } + + private static Vector2[] GetBoxVertices(Box box) + { + float hx = box.Size.X * 0.5f; + float hy = box.Size.Y * 0.5f; + float cos = (float)Math.Cos(box.Rotation); + float sin = (float)Math.Sin(box.Rotation); + Vector2 axisX = new Vector2(cos, sin); + Vector2 axisY = new Vector2(-sin, cos); + + return new Vector2[] + { + box.Position - axisX * hx - axisY * hy, + box.Position + axisX * hx - axisY * hy, + box.Position + axisX * hx + axisY * hy, + box.Position - axisX * hx + axisY * hy + }; + } + + private static Vector2[] GetFaceVertices(Box box, Vector2 faceNormalWorld) + { + Vector2[] vertices = GetBoxVertices(box); + for (int i = 0; i < 4; i++) + { + Vector2 v1 = vertices[i]; + Vector2 v2 = vertices[(i + 1) % 4]; + Vector2 edge = v2 - v1; + Vector2 outwardNormal = new Vector2(edge.Y, -edge.X); + outwardNormal = Vector2.Normalize(outwardNormal); + if (Vector2.Dot(outwardNormal, faceNormalWorld) > 0.999f) + return new Vector2[] { v1, v2 }; + } + return new Vector2[] { vertices[0], vertices[1] }; + } + + private static void ClipSegmentAgainstPlane(List 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 (d0 >= 0) result.Add(segment[0]); + if (d1 >= 0) result.Add(segment[1]); + + if (d0 * d1 < 0) + { + float t = d0 / (d0 - d1); + Vector2 intersection = segment[0] + t * (segment[1] - segment[0]); + result.Add(intersection); + } + + segment.Clear(); + segment.AddRange(result); + } + + private static Vector2 GetRelativeVelocity(Contact contact) + { + Body a = contact.BodyA; + Body b = contact.BodyB; + Vector2 va = a.Velocity + Cross(a.AngularVelocity, contact.RA); + Vector2 vb = b.Velocity + Cross(b.AngularVelocity, contact.RB); + return vb - va; + } + + private static void ApplyImpulse(Contact contact, Vector2 impulse) + { + if (!contact.BodyA.IsStatic) + contact.BodyA.ApplyImpulseAtWorldPosition(-impulse, contact.Point); + if (!contact.BodyB.IsStatic) + contact.BodyB.ApplyImpulseAtWorldPosition(impulse, contact.Point); + } + + private static float Cross(Vector2 a, Vector2 b) + { + return a.X * b.Y - a.Y * b.X; + } + + private static Vector2 Cross(float s, Vector2 a) + { + return new Vector2(-s * a.Y, s * a.X); + } + } +} \ No newline at end of file diff --git a/PhysicsEngine/Physics/Constraints/Constraint.cs b/PhysicsEngine/Physics/Constraints/Constraint.cs new file mode 100644 index 0000000..a8bfd15 --- /dev/null +++ b/PhysicsEngine/Physics/Constraints/Constraint.cs @@ -0,0 +1,24 @@ +using System.Numerics; + +namespace PhysicsEngine +{ + public abstract class Constraint + { + public Body BodyA; + public Body BodyB; + public Vector2 LocalAnchorA; + public Vector2 LocalAnchorB; + + public Vector2 GetWorldAnchorA() + { + return BodyA.Position + LocalAnchorA; + } + + public Vector2 GetWorldAnchorB() + { + return BodyB.Position + LocalAnchorB; + } + + public abstract void Solve(); + } +} \ No newline at end of file diff --git a/PhysicsEngine/Physics/Forces/DragForce.cs b/PhysicsEngine/Physics/Forces/DragForce.cs new file mode 100644 index 0000000..b199fb8 --- /dev/null +++ b/PhysicsEngine/Physics/Forces/DragForce.cs @@ -0,0 +1,10 @@ +namespace PhysicsEngine +{ + public static class DragForce + { + public static void Apply(Body body, float dt) + { + body.ApplyAerodynamicDrag(dt); + } + } +} \ No newline at end of file diff --git a/PhysicsEngine/Physics/Forces/GlobalForce.cs b/PhysicsEngine/Physics/Forces/GlobalForce.cs new file mode 100644 index 0000000..daa747e --- /dev/null +++ b/PhysicsEngine/Physics/Forces/GlobalForce.cs @@ -0,0 +1,4 @@ +namespace PhysicsEngine +{ + public delegate void GlobalForce(Body body, float dt); +} \ No newline at end of file diff --git a/PhysicsEngine/Physics/Forces/GravityForce.cs b/PhysicsEngine/Physics/Forces/GravityForce.cs new file mode 100644 index 0000000..ad9b6da --- /dev/null +++ b/PhysicsEngine/Physics/Forces/GravityForce.cs @@ -0,0 +1,12 @@ +using System.Numerics; + +namespace PhysicsEngine +{ + public static class GravityForce + { + public static void Apply(Body body, float dt) + { + body.ApplyForce(Vector2.UnitY * 9.81f * body.Mass); + } + } +} \ No newline at end of file diff --git a/PhysicsEngine/Physics/Solver.cs b/PhysicsEngine/Physics/Solver.cs new file mode 100644 index 0000000..852ba68 --- /dev/null +++ b/PhysicsEngine/Physics/Solver.cs @@ -0,0 +1,62 @@ +using System.Collections.Generic; +using System.Numerics; + +namespace PhysicsEngine +{ + public class Solver + { + // Optional: set a default number of sub‑steps (e.g. 8 for better stability) + private const int DefaultSubSteps = 8; + + public void Step(float dt, List 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. + foreach (var body in bodies) + { + if (body.IsStatic) continue; + + foreach (var forceGenerator in globalForces) + { + forceGenerator(body, subDt); + } + } + + // 2. Numerical Integration (Velocity & Position updates) + foreach (var body in bodies) + { + if (body.IsStatic) continue; + + // Linear motion + Vector2 acceleration = body.ForceAccumulator * body.InverseMass; + body.Velocity += acceleration * subDt; + body.Position += body.Velocity * subDt; + + // Angular motion + float angularAcceleration = body.TorqueAccumulator * body.InverseInertia; + body.AngularVelocity += angularAcceleration * subDt; + body.Rotation += body.AngularVelocity * subDt; + + // Clear accumulators for the next sub‑step + body.ClearForces(); + } + + // 3. Resolve Collisions + CollisionEngine.ResolveCollisions(bodies, subDt); + + // 4. Resolve constraints + foreach (var constraint in constraints) + { + constraint.Solve(); + } + } + } + } +} \ No newline at end of file diff --git a/PhysicsEngine/Physics/WorldModel.cs b/PhysicsEngine/Physics/WorldModel.cs new file mode 100644 index 0000000..1bccd45 --- /dev/null +++ b/PhysicsEngine/Physics/WorldModel.cs @@ -0,0 +1,50 @@ +using System.Collections.Generic; + +namespace PhysicsEngine +{ + public class WorldModel + { + public List Bodies { get; private set; } = new(); + public List Constraints { get; private set; } = new(); + public List GlobalForces = new(); + + private readonly Solver _solver = new(); + + public WorldModel() + { + GlobalForces.Add(GravityForce.Apply); + //GlobalForces.Add(DragForce.Apply); + } + + public void AddBody(Body body) + { + if (!Bodies.Contains(body)) + { + Bodies.Add(body); + } + } + + public void RemoveBody(Body body) + { + Bodies.Remove(body); + } + + public void AddConstraint(Constraint constraint) + { + if (!Constraints.Contains(constraint)) + { + Constraints.Add(constraint); + } + } + + public void RemoveConstraint(Constraint constraint) + { + Constraints.Remove(constraint); + } + + public void Step(float dt) + { + _solver.Step(dt, Bodies, Constraints, GlobalForces, 8); + } + } +} \ No newline at end of file diff --git a/PhysicsEngine/PhysicsEngine.csproj b/PhysicsEngine/PhysicsEngine.csproj index c33b934..30e05c6 100644 --- a/PhysicsEngine/PhysicsEngine.csproj +++ b/PhysicsEngine/PhysicsEngine.csproj @@ -11,4 +11,10 @@ + + + PreserveNewest + + + diff --git a/PhysicsEngine/Program.cs b/PhysicsEngine/Program.cs deleted file mode 100644 index 62d3743..0000000 --- a/PhysicsEngine/Program.cs +++ /dev/null @@ -1,68 +0,0 @@ -using Raylib_cs; - -namespace PhysicsEngine -{ - class Program - { - static void Main(string[] args) - { - Raylib.SetConfigFlags(ConfigFlags.FullscreenMode | ConfigFlags.VSyncHint); - Raylib.InitWindow(0, 0, "Physics Engine"); - - Camera camera = new Camera(); - - const float physicsTimeStep = 1.0f / 60.0f; - float accumulator = 0.0f; - - // Main loop - while (!Raylib.WindowShouldClose()) - { - float frameTime = Raylib.GetFrameTime(); - if (frameTime > 0.25f) - { - frameTime = 0.25f; - } - - // Update Camera - camera.Update(frameTime); - - // Update Physics accumulator - accumulator += frameTime; - while (accumulator >= physicsTimeStep) - { - UpdatePhysics(physicsTimeStep); - accumulator -= physicsTimeStep; - } - - // Render loop - Raylib.BeginDrawing(); - Raylib.ClearBackground(MuiDarkColor.BackgroundDefault.ToColor()); - - // Draw background screen-space elements (Grid) - camera.DrawGrid(); - - // Draw world-space elements inside 2D mode - Raylib.BeginMode2D(camera.RaylibCamera); - Raylib.EndMode2D(); - - // Draw UI / HUD elements - RenderScene(); - camera.DrawHUD(); - - Raylib.EndDrawing(); - } - - Raylib.CloseWindow(); - } - - private static void UpdatePhysics(float dt) - { - // Physics simulation step - } - - private static void RenderScene() - { - Raylib.DrawText("Physics Engine", 20, 20, 20, MuiDarkColor.TextPrimary.ToColor()); - } - } -} \ No newline at end of file diff --git a/PhysicsEngine/UI/DrawHelper.cs b/PhysicsEngine/UI/DrawHelper.cs new file mode 100644 index 0000000..ff096b2 --- /dev/null +++ b/PhysicsEngine/UI/DrawHelper.cs @@ -0,0 +1,63 @@ +using System; +using System.Numerics; +using Raylib_cs; + +namespace PhysicsEngine +{ + public static class DrawHelper + { + public static Color GetDarkerColor(Color color, float factor = 0.5f) + { + return new Color( + (byte)(color.R * factor), + (byte)(color.G * factor), + (byte)(color.B * factor), + color.A + ); + } + + public static void DrawRotatedBox(Vector2 position, Vector2 size, float rotationRadians, Color fillColor, float pixelsPerMeter = 100.0f, float outlineThickness = 3.0f) + { + Vector2 sizePixels = size * pixelsPerMeter; + Vector2 positionPixels = position * pixelsPerMeter; + float rotationDegrees = rotationRadians * (180.0f / MathF.PI); + + Color outlineColor = GetDarkerColor(fillColor); + + // 1. Draw the outer outline rectangle (exact original size, outline colour) + Vector2 outerSizePixels = sizePixels; + Vector2 outerOrigin = outerSizePixels * 0.5f; + Rectangle outerRec = new Rectangle(positionPixels.X, positionPixels.Y, outerSizePixels.X, outerSizePixels.Y); + Raylib.DrawRectanglePro(outerRec, outerOrigin, rotationDegrees, outlineColor); + + // 2. Draw the inner fill rectangle, reduced by outlineThickness on all sides + Vector2 innerSizePixels = new Vector2( + MathF.Max(0, sizePixels.X - outlineThickness), + MathF.Max(0, sizePixels.Y - outlineThickness) + ); + Vector2 innerOrigin = innerSizePixels * 0.5f; + Rectangle innerRec = new Rectangle(positionPixels.X, positionPixels.Y, innerSizePixels.X, innerSizePixels.Y); + Raylib.DrawRectanglePro(innerRec, innerOrigin, rotationDegrees, fillColor); + } + + public static void DrawCircleBody(Vector2 position, float radius, float rotationRadians, Color fillColor, float pixelsPerMeter = 100.0f, float outlineThickness = 3.0f) + { + Vector2 positionPixels = position * pixelsPerMeter; + float radiusPixels = radius * pixelsPerMeter; + Color outlineColor = GetDarkerColor(fillColor); + + // 1. Draw outer circle (outline colour, exact radius) + Raylib.DrawCircleV(positionPixels, radiusPixels, outlineColor); + + // 2. Draw inner circle (fill colour, radius reduced by outlineThickness) + float innerRadius = MathF.Max(0, radiusPixels - outlineThickness); + Raylib.DrawCircleV(positionPixels, innerRadius, fillColor); + + // Draw rotation indicator line inside the fill (same as before, but from centre to inner edge) + float cos = MathF.Cos(rotationRadians); + float sin = MathF.Sin(rotationRadians); + Vector2 edge = positionPixels + new Vector2(cos, sin) * innerRadius; + Raylib.DrawLineEx(positionPixels, edge, 2.0f, outlineColor); + } + } +} \ No newline at end of file diff --git a/PhysicsEngine/MuiDarkColor.cs b/PhysicsEngine/UI/MuiDarkColor.cs similarity index 100% rename from PhysicsEngine/MuiDarkColor.cs rename to PhysicsEngine/UI/MuiDarkColor.cs diff --git a/PhysicsEngine/bin/Debug/net10.0/PhysicsEngine.dll b/PhysicsEngine/bin/Debug/net10.0/PhysicsEngine.dll index b338564..dba7cc1 100644 Binary files a/PhysicsEngine/bin/Debug/net10.0/PhysicsEngine.dll and b/PhysicsEngine/bin/Debug/net10.0/PhysicsEngine.dll differ diff --git a/PhysicsEngine/bin/Debug/net10.0/PhysicsEngine.exe b/PhysicsEngine/bin/Debug/net10.0/PhysicsEngine.exe index 3cb9bbd..e21da49 100644 Binary files a/PhysicsEngine/bin/Debug/net10.0/PhysicsEngine.exe and b/PhysicsEngine/bin/Debug/net10.0/PhysicsEngine.exe differ diff --git a/PhysicsEngine/bin/Debug/net10.0/PhysicsEngine.pdb b/PhysicsEngine/bin/Debug/net10.0/PhysicsEngine.pdb index 7ff6390..e7dda7c 100644 Binary files a/PhysicsEngine/bin/Debug/net10.0/PhysicsEngine.pdb and b/PhysicsEngine/bin/Debug/net10.0/PhysicsEngine.pdb differ diff --git a/PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.AssemblyInfo.cs b/PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.AssemblyInfo.cs index 3f5cdad..b9339c5 100644 --- a/PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.AssemblyInfo.cs +++ b/PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.AssemblyInfo.cs @@ -13,7 +13,7 @@ using System.Reflection; [assembly: System.Reflection.AssemblyCompanyAttribute("PhysicsEngine")] [assembly: System.Reflection.AssemblyConfigurationAttribute("Debug")] [assembly: System.Reflection.AssemblyFileVersionAttribute("1.0.0.0")] -[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0+5680beef02a5c7a4cdc1dc725326855b4af1faab")] +[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0+cf1f083cc54031dbb6bf827cde69ef08608683e9")] [assembly: System.Reflection.AssemblyProductAttribute("PhysicsEngine")] [assembly: System.Reflection.AssemblyTitleAttribute("PhysicsEngine")] [assembly: System.Reflection.AssemblyVersionAttribute("1.0.0.0")] diff --git a/PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.AssemblyInfoInputs.cache b/PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.AssemblyInfoInputs.cache index 6b1a841..d4cfcc1 100644 --- a/PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.AssemblyInfoInputs.cache +++ b/PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.AssemblyInfoInputs.cache @@ -1 +1 @@ -65813feab20f3482f44a4d97f31d1282ff5b6ec731137cac82a3e7db08a5bb63 +44a2f7ef34c114c65155ce102612520c953e6a96d159fd0c9c3838e4efb4baeb diff --git a/PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.csproj.CoreCompileInputs.cache b/PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.csproj.CoreCompileInputs.cache index a98e6da..6e391c0 100644 --- a/PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.csproj.CoreCompileInputs.cache +++ b/PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.csproj.CoreCompileInputs.cache @@ -1 +1 @@ -6b2f1f27b472d9ef51ad7745525496297fe0832151173bc3df4acdae2fd1d6fe +baeac40138d8aced527e9be06cd30c4fff093062425aa1213346799e177123c4 diff --git a/PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.dll b/PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.dll index b338564..dba7cc1 100644 Binary files a/PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.dll and b/PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.dll differ diff --git a/PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.pdb b/PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.pdb index 7ff6390..e7dda7c 100644 Binary files a/PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.pdb and b/PhysicsEngine/obj/Debug/net10.0/PhysicsEngine.pdb differ diff --git a/PhysicsEngine/obj/Debug/net10.0/apphost.exe b/PhysicsEngine/obj/Debug/net10.0/apphost.exe index 3cb9bbd..e21da49 100644 Binary files a/PhysicsEngine/obj/Debug/net10.0/apphost.exe and b/PhysicsEngine/obj/Debug/net10.0/apphost.exe differ diff --git a/PhysicsEngine/obj/Debug/net10.0/ref/PhysicsEngine.dll b/PhysicsEngine/obj/Debug/net10.0/ref/PhysicsEngine.dll index e7b76a4..291dbc5 100644 Binary files a/PhysicsEngine/obj/Debug/net10.0/ref/PhysicsEngine.dll and b/PhysicsEngine/obj/Debug/net10.0/ref/PhysicsEngine.dll differ diff --git a/PhysicsEngine/obj/Debug/net10.0/refint/PhysicsEngine.dll b/PhysicsEngine/obj/Debug/net10.0/refint/PhysicsEngine.dll index e7b76a4..291dbc5 100644 Binary files a/PhysicsEngine/obj/Debug/net10.0/refint/PhysicsEngine.dll and b/PhysicsEngine/obj/Debug/net10.0/refint/PhysicsEngine.dll differ