+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;
+ 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;
+ 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) + CollisionForce(entity, state) + Gravity(entity, state));
+ if (dot(force, force) > entity.MaxControlForce() * entity.MaxControlForce()) {
+ return normalize(force) * entity.MaxControlForce();
+ } else {
+ return force;
+ }
+}
+
+glm::vec3 World::ControlForce(
+ const Entity &entity,
+ const EntityState &state
+) {
+ return entity.ControlForce(state);
+}
+
+namespace {
+
+std::vector<WorldCollision> col;
+
+}
+
+glm::vec3 World::CollisionForce(
+ const Entity &entity,
+ const EntityState &state
+) {
+ col.clear();
+ if (entity.WorldCollidable() && Intersection(entity, state, col)) {
+ glm::vec3 correction = -CombinedInterpenetration(state, col);
+ // correction may be zero in which case normalize() returns NaNs
+ if (iszero(correction)) {
+ return glm::vec3(0.0f);
+ }
+ // if entity is already going in the direction of correction,
+ // let the problem resolve itself
+ if (dot(state.velocity, correction) >= 0.0f) {
+ return glm::vec3(0.0f);
+ }
+ glm::vec3 normal_velocity(proj(state.velocity, correction));
+ // apply force proportional to penetration
+ // use velocity projected onto correction as damper
+ constexpr float k = 1000.0f; // spring constant
+ constexpr float b = 10.0f; // damper constant
+ const glm::vec3 x(-correction); // endpoint displacement from equilibrium in m
+ const glm::vec3 v(normal_velocity); // relative velocity between endpoints in m/s
+ return (((-k) * x) - (b * v)); // times 1kg/s, in kg*m/s²
+ } else {
+ return glm::vec3(0.0f);
+ }
+}
+
+glm::vec3 World::CombinedInterpenetration(
+ const EntityState &state,
+ const std::vector<WorldCollision> &col
+) noexcept {