using System.Collections.Generic; using System.Numerics; namespace PhysicsEngine { public class Solver { private const int DefaultSubSteps = 8; public void Step(float dt, List bodies, List constraints, List globalForces, int subSteps = DefaultSubSteps) { if (subSteps <= 0) subSteps = 1; float subDt = dt / subSteps; for (int step = 0; step < subSteps; step++) { // accumulate global forces foreach (var body in bodies) { if (body.IsStatic) continue; foreach (var forceGenerator in globalForces) { forceGenerator(body, subDt); } } // 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 body.ClearForces(); } // collisions CollisionEngine.ResolveCollisions(bodies, subDt); // constraints foreach (var constraint in constraints) { constraint.Solve(); } } } } }