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