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