- if (entity.WorldCollidable() && Intersection(entity, state, col)) {
- // determine displacement for each cardinal axis and move entity accordingly
- glm::vec3 min_pen(0.0f);
- glm::vec3 max_pen(0.0f);
- for (const WorldCollision &c : col) {
- if (!c.Blocks()) continue;
- glm::vec3 local_pen(c.normal * c.depth);
- // swap if neccessary (normal may point away from the entity)
- if (dot(c.normal, state.RelativePosition(c.ChunkPos()) - c.BlockCoords()) > 0) {
- local_pen *= -1;
- }
- min_pen = min(min_pen, local_pen);
- max_pen = max(max_pen, local_pen);
+ 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 {
+ // determine displacement for each cardinal axis and move entity accordingly
+ glm::vec3 min_pen(0.0f);
+ glm::vec3 max_pen(0.0f);
+ for (const WorldCollision &c : col) {
+ if (!c.Blocks()) continue;
+ glm::vec3 normal(c.normal);
+ // swap if neccessary (normal may point away from the entity)
+ if (dot(normal, state.RelativePosition(c.ChunkPos()) - c.BlockCoords()) < 0) {
+ normal = -normal;