/// <summary>Advance the simulation</summary>
private void Integrate(float dt)
{
Equalibrium = true;
// Apply forces (2nd order integrator)
const float mass = 1f;
foreach (var body in m_body)
{
// If the simulation blows up, ignore the forces
var force = Math_.IsFinite(body.Force) || body.Force.Length < 1000.0 ? body.Force : v4.Zero;
var a = force / mass;
var v = body.Vel + 0.5f * dt * a;
Equalibrium &= a.LengthSq < Math_.Sqr(m_opts.Scatter.Equilibrium);
body.Pos += v * dt + 0.5f * a * Math_.Sqr(dt);
body.Vel += a;
body.Force = v4.Zero;
}
// Ensure the centroid is always at (0,0,0)
var centre = v4.Zero;
foreach (var body in m_body)
{
centre += body.Pos.w0;
}
centre /= m_body.Count;
foreach (var body in m_body)
{
body.Pos -= centre;
}
}
public static string Mat4x4(m4x4 mat)
{
Debug.Assert(Math_.IsFinite(mat));
return($"{Vec4(mat.x)} {Vec4(mat.y)} {Vec4(mat.z)} {Vec4(mat.w)}");
}
public static string Vec4(v4 vec)
{
Debug.Assert(Math_.IsFinite(vec));
return($"{vec.x} {vec.y} {vec.z} {vec.w}");
}
public static string Vec3(v2 vec)
{
Debug.Assert(Math_.IsFinite(vec));
return($"{vec.x} {vec.y} 0");
}