some annotations

[blank.git] / src / world / world.cpp

diff --git a/src/world/world.cpp b/src/world/world.cpp
index 738fa4ad9b06bb7256f66886bfeca86bd5e3bd2c..d2b05d66850e96923704bfd262335af5cc464832 100644 (file)
--- a/src/world/world.cpp
+++ b/src/world/world.cpp
@@ -20,6 +20,7 @@
 #include <limits>
 #include <glm/gtx/euler_angles.hpp>
 #include <glm/gtx/io.hpp>
+#include <glm/gtx/projection.hpp>
 #include <glm/gtx/quaternion.hpp>
 #include <glm/gtx/rotate_vector.hpp>
 #include <glm/gtx/transform.hpp>
@@ -219,8 +220,8 @@ void Entity::OrientBody(float dt) noexcept {
 }
 
 void Entity::OrientHead(float dt) noexcept {
-       // maximum yaw of head (90°)
-       constexpr float max_head_yaw = PI_0p5;
+       // maximum yaw of head (60°)
+       constexpr float max_head_yaw = PI / 3.0f;
        // use local Y as up
        const glm::vec3 up(model_transform[1]);
        // if yaw is bigger than max, rotate the body to accomodate
@@ -604,8 +605,8 @@ void World::Update(Entity &entity, float dt) {
 
        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);
+       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;
@@ -666,38 +667,19 @@ glm::vec3 World::CollisionForce(
 ) {
        col.clear();
        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);
-               }
-               glm::vec3 correction(0.0f);
-               // only apply correction for axes where penetration is only in one direction
-               for (std::size_t i = 0; i < 3; ++i) {
-                       if (min_pen[i] < -std::numeric_limits<float>::epsilon()) {
-                               if (max_pen[i] < std::numeric_limits<float>::epsilon()) {
-                                       correction[i] = -min_pen[i];
-                               }
-                       } else {
-                               correction[i] = -max_pen[i];
-                       }
-               }
+               glm::vec3 correction = -CombinedInterpenetration(state, col);
                // correction may be zero in which case normalize() returns NaNs
-               if (dot(correction, correction) < std::numeric_limits<float>::epsilon()) {
+               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(normalize(correction));
-               glm::vec3 normal_velocity(normal * dot(state.velocity, normal));
+               glm::vec3 normal_velocity(proj(state.velocity, correction));
                // apply force proportional to penetration
-               // use velocity projected onto normal as damper
+               // 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
@@ -708,11 +690,57 @@ glm::vec3 World::CollisionForce(
        }
 }
 
+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 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);
+       }
+       glm::vec3 pen(0.0f);
+       // only apply correction for axes where penetration is only in one direction
+       for (std::size_t i = 0; i < 3; ++i) {
+               if (min_pen[i] < -std::numeric_limits<float>::epsilon()) {
+                       if (max_pen[i] < std::numeric_limits<float>::epsilon()) {
+                               pen[i] = min_pen[i];
+                       }
+               } else {
+                       pen[i] = max_pen[i];
+               }
+       }
+       return pen;
+}
+
 glm::vec3 World::Gravity(
        const Entity &entity,
        const EntityState &state
 ) {
-       return glm::vec3(0.0f);
+       glm::vec3 force(0.0f);
+       ExactLocation::Coarse begin(state.pos.chunk - 1);
+       ExactLocation::Coarse end(state.pos.chunk + 2);
+
+       for (ExactLocation::Coarse pos(begin); pos.z < end.z; ++pos.z) {
+               for (pos.y = begin.y; pos.y < end.y; ++pos.y) {
+                       for (pos.x = begin.x; pos.x < end.x; ++pos.x) {
+                               Chunk *chunk = chunks.Get(pos);
+                               if (chunk) {
+                                       force += chunk->GravityAt(state.pos);
+                               }
+                       }
+               }
+       }
+
+       return force;
 }
 
 World::EntityHandle World::RemoveEntity(EntityHandle &eh) {