-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);
-}
-