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/Solver.cs
+++ 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<Body> bodies, List<Constraint> constraints, List<GlobalForce> 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();
+                }
+            }
+        }
+    }
+}