62 lines
2.2 KiB
C#
62 lines
2.2 KiB
C#
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();
|
||
}
|
||
}
|
||
}
|
||
}
|
||
} |