+void Entity::UpdatePhysics(World &world, float dt) {
+ EntityState s(state);
+
+ EntityDerivative a(CalculateStep(world, s, 0.0f, EntityDerivative()));
+ EntityDerivative b(CalculateStep(world, s, dt * 0.5f, a));
+ EntityDerivative c(CalculateStep(world, s, dt * 0.5f, b));
+ EntityDerivative d(CalculateStep(world, s, dt, c));
+
+ EntityDerivative f;
+ constexpr float sixth = 1.0f / 6.0f;
+ f.position = sixth * (a.position + 2.0f * (b.position + c.position) + d.position);
+ f.velocity = sixth * (a.velocity + 2.0f * (b.velocity + c.velocity) + d.velocity);
+
+ s.pos.block += f.position * dt;
+ s.velocity += f.velocity * dt;
+ limit(s.velocity, max_vel);
+ world.ResolveWorldCollision(*this, s);
+ s.AdjustPosition();
+
+ SetState(s);
+}
+
+EntityDerivative Entity::CalculateStep(
+ World &world,
+ const EntityState &cur,
+ float dt,
+ const EntityDerivative &delta
+) const {
+ EntityState next(cur);
+ next.pos.block += delta.position * dt;
+ next.velocity += delta.velocity * dt;
+ limit(next.velocity, max_vel);
+ world.ResolveWorldCollision(*this, next);
+ next.AdjustPosition();
+
+ EntityDerivative out;
+ out.position = next.velocity;
+ out.velocity = ControlForce(next) + world.GravityAt(next.pos); // by mass = 1kg
+ return out;
+}
+
+