rigidbody, forces and collision testing

2026-09-03 16:50:36 +02:00
parent cf1f083cc5
commit 815bd2d260
28 changed files with 1381 additions and 75 deletions
--- a/PhysicsEngine/Physics/Bodies/Body.cs
+++ 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) {}
+    }
+}