59 lines
1.8 KiB
C#
59 lines
1.8 KiB
C#
using System.Collections.Generic;
|
|
using System.Numerics;
|
|
|
|
namespace PhysicsEngine
|
|
{
|
|
public class Solver
|
|
{
|
|
private const int DefaultSubSteps = 8;
|
|
|
|
public void Step(float dt, List<Body> bodies, List<Constraint> constraints, List<GlobalForce> 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();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
} |