+void World::Update(Entity &entity, float dt) {
+ EntityState state(entity.GetState());
+
+ EntityDerivative a(CalculateStep(entity, state, 0.0f, EntityDerivative()));
+ EntityDerivative b(CalculateStep(entity, state, dt * 0.5f, a));
+ EntityDerivative c(CalculateStep(entity, state, dt * 0.5f, b));
+ EntityDerivative d(CalculateStep(entity, state, 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);
+
+ state.pos.block += f.position * dt;
+ state.velocity += f.velocity * dt;
+ CollisionFix(entity, state);
+ state.AdjustPosition();
+
+ entity.SetState(state);
+}
+
+EntityDerivative World::CalculateStep(
+ const Entity &entity,
+ const EntityState &cur,
+ float dt,
+ const EntityDerivative &delta
+) {
+ EntityState next(cur);
+ next.pos.block += delta.position * dt;
+ next.velocity += delta.velocity * dt;
+ CollisionFix(entity, next);
+ next.AdjustPosition();
+
+ if (dot(next.velocity, next.velocity) > entity.MaxVelocity() * entity.MaxVelocity()) {
+ next.velocity = normalize(next.velocity) * entity.MaxVelocity();
+ }
+
+ EntityDerivative out;
+ out.position = next.velocity;
+ out.velocity = CalculateForce(entity, next); // by mass = 1kg
+ return out;
+}
+
+glm::vec3 World::CalculateForce(
+ const Entity &entity,
+ const EntityState &state
+) {
+ glm::vec3 force(ControlForce(entity, state));
+ if (dot(force, force) > entity.MaxControlForce() * entity.MaxControlForce()) {
+ force = normalize(force) * entity.MaxControlForce();
+ }
+ return force + Gravity(entity, state);
+}
+
+glm::vec3 World::ControlForce(
+ const Entity &entity,
+ const EntityState &state
+) {
+ return entity.ControlForce(state);
+}
+
+namespace {
+
+std::vector<WorldCollision> col;
+
+}
+
+void World::CollisionFix(
+ const Entity &entity,
+ EntityState &state
+) {
+ col.clear();
+ if (!entity.WorldCollidable() || !Intersection(entity, state, col)) {
+ // no collision, no fix
+ return;
+ }
+ glm::vec3 correction = CombinedInterpenetration(state, col);
+ // correction may be zero in which case normalize() returns NaNs
+ if (iszero(correction)) {
+ return;
+ }
+ // if entity is already going in the direction of correction,
+ // let the problem resolve itself
+ if (dot(state.velocity, correction) >= 0.0f) {
+ return;
+ }
+ // apply correction, maybe could use some damping, gotta test
+ state.pos.block += correction;
+ // kill velocity?
+ glm::vec3 normal_velocity(proj(state.velocity, correction));
+ state.velocity -= normal_velocity;
+}
+
+glm::vec3 World::CombinedInterpenetration(
+ const EntityState &state,
+ const std::vector<WorldCollision> &col
+) noexcept {