fix step on & move some code somewhere

src/physics/Hitbox.hpp

@@ -66,7 +66,7 @@ struct Hitbox {
     Hitbox(BodyType type, glm::vec3 position, glm::vec3 halfsize);
 
     AABB getAABB() const {
-        return AABB(position-halfsize, position+halfsize);
+        return AABB(position - halfsize, position + halfsize);
     }
 
     glm::vec3 getHalfSize() const {

src/physics/PhysicsSolver.cpp

@@ -18,133 +18,6 @@ static debug::Logger logger("physics-solver");
 
 PhysicsSolver::PhysicsSolver(glm::vec3 gravity) : gravity(std::move(gravity)) {}
 
-void PhysicsSolver::calcSubstep(
-    const GlobalChunks& chunks,
-    Hitbox& hitbox,
-    glm::vec3& vel,
-    glm::vec3& pos,
-    bool prevGrounded,
-    float dt
-) {
-    auto initpos = pos;
-    auto half = hitbox.getHalfSize();
-    float gravityScale = hitbox.gravityScale;
-        
-    if (hitbox.type == BodyType::DYNAMIC) {
-        calcCollisions(
-            chunks,
-            hitbox,
-            vel,
-            pos,
-            half,
-            (prevGrounded && gravityScale > 0.0f) ? hitbox.stepHeight : 0.0f
-        );
-    }
-
-    vel += gravity * dt * gravityScale;
-    pos += vel * dt + gravity * gravityScale * dt * dt * 0.5f;
-
-    if (hitbox.grounded && pos.y < initpos.y) {
-        pos.y = initpos.y;
-    }
-
-    if (prevGrounded && !hitbox.grounded) {
-        vel += hitbox.groundVelocity;
-        hitbox.groundVelocity = {};
-    }
-
-    if (!hitbox.crouching || !hitbox.grounded) {
-        return;
-    }
-
-    float y = (pos.y-half.y-E);
-    for (int axis = 0; axis <= 2; axis += 2) {
-        hitbox.grounded = false;
-
-        AABB aabb(pos - half, pos + half);
-        aabb.scale(glm::vec3(1.0f - E * 4, 1.0f, 1.0f - E * 4));
-        for (int ix = 0; ix <= glm::ceil((half.x - E) * 2); ix++) {
-            float x = (initpos.x - half.x + E) + ix;
-            for (int iz = 0; iz <= glm::ceil((half.z - E) * 2); iz++){
-                float z = (pos.z - half.z + E) + iz;
-                if (chunks.isObstacleAt(x, y, z, aabb)){
-                    hitbox.grounded = true;
-                    break;
-                }
-            }
-        }
-        for (const auto& box : solidHitboxes) {
-            if (glm::distance2(box->position, pos) < E) {
-                continue;
-            }
-            auto boxhalf = box->getHalfSize();
-            if (AABB(box->position - boxhalf, box->position + boxhalf)
-                .intersects(AABB(pos - half, pos + half))) {
-                hitbox.grounded = true;
-                break;
-            }
-        }
-        if (!hitbox.grounded) {
-            pos[axis] = initpos[axis];
-            vel[axis] = 0.0f;
-        }
-    }
-    hitbox.grounded = true;
-}
-
-void PhysicsSolver::step(
-    const GlobalChunks& chunks,
-    Hitbox& hitbox,
-    float delta,
-    uint substeps,
-    entityid_t entity
-) {
-    float dt = delta / static_cast<float>(substeps);
-    float linearDamping = hitbox.linearDamping * hitbox.friction;
-
-    glm::vec3& pos = hitbox.position;
-    glm::vec3& vel = hitbox.velocity;
-   
-    bool prevGrounded = hitbox.grounded;
-    hitbox.grounded = false;
-    for (uint i = 0; i < substeps; i++) {
-        calcSubstep(chunks, hitbox, vel, pos, prevGrounded, dt);
-    }
-    vel.x /= 1.0f + delta * linearDamping;
-    vel.z /= 1.0f + delta * linearDamping;
-    if (hitbox.verticalDamping > 0.0f) {
-        vel.y /= 1.0f + delta * linearDamping * hitbox.verticalDamping;
-    }
-
-    AABB aabb;
-    aabb.a = hitbox.position - hitbox.getHalfSize();
-    aabb.b = hitbox.position + hitbox.getHalfSize();
-    for (size_t i = 0; i < sensors.size(); i++) {
-        auto& sensor = *sensors[i];
-        if (sensor.entity == entity) {
-            continue;
-        }
-
-        bool triggered = false;
-        switch (sensor.type) {
-            case SensorType::AABB:
-                triggered = aabb.intersects(sensor.calculated.aabb);
-                break;
-            case SensorType::RADIUS:
-                triggered = glm::distance2(
-                    hitbox.position, glm::vec3(sensor.calculated.radial))
-                     < sensor.calculated.radial.w;
-                break;
-        }
-        if (triggered) {
-            if (sensor.prevEntered.find(entity) == sensor.prevEntered.end()) {
-                sensor.enterCallback(sensor.entity, sensor.index, entity);
-            }
-            sensor.nextEntered.insert(entity);
-        }
-    }
-}
-
 static float calc_step_height(
     const GlobalChunks& chunks, 
     const glm::vec3& pos, 
@@ -168,6 +41,46 @@ static float calc_step_height(
     return stepHeight;
 }
 
+template <int nx, int ny, int nz>
+static void calc_collision_pos(
+    const GlobalChunks& chunks,
+    glm::vec3& pos,
+    glm::vec3& vel,
+    const glm::vec3& half,
+    float stepHeight,
+    bool collided[3]
+) {
+    if (vel[nx] <= 0.0f) {
+        return;
+    }
+    glm::vec3 offset(0.0f, stepHeight + E * 2, 0.0f);
+    for (int iy = 0; iy <= glm::ceil(((half - offset * 0.5f)[ny] - E) * 2); iy++) {
+        glm::vec3 coord;
+        coord[ny] = ((pos + offset)[ny] - half[ny] + E) + iy;
+        for (int iz = 0; iz <= glm::ceil((half[nz] - E) * 2); iz++) {
+            coord[nz] = (pos[nz] - half[nz] + E) + iz;
+            coord[nx] = (pos[nx] + half[nx] + E) + E;
+
+            auto boxAABB = AABB(pos - half, pos + half);
+            glm::vec3 scale(1.0f);
+            scale[nz] = 1.0f - E * 2.0f;
+            boxAABB.scale(scale);
+            boxAABB = boxAABB + offset;
+            boxAABB.b.y -= stepHeight + E * 2;
+
+            if (const auto aabb = chunks.isObstacleAt(coord.x, coord.y, coord.z, boxAABB)) {
+                float newx = std::floor(coord[nx]) - half[nx] + aabb->min()[nx] - E;
+                if (pos[nx] > newx) {
+                    vel[nx] = 0.0f;
+                    pos[nx] = newx;
+                    collided[nx] = true;
+                }
+                return;
+            }
+        }
+    }
+}
+
 template <int nx, int ny, int nz>
 static bool calc_collision_neg(
     const GlobalChunks& chunks,
@@ -181,13 +94,13 @@ static bool calc_collision_neg(
         return false;
     }
     glm::vec3 offset(0.0f, stepHeight + E, 0.0f);
-    for (int iy = 0; iy <= glm::ceil(((half-offset*0.5f)[ny]-E)*2); iy++) {
+    for (int iy = 0; iy <= glm::ceil(((half - offset * 0.5f)[ny] - E) * 2); iy++) {
         glm::vec3 coord;
-        coord[ny] = ((pos+offset)[ny]-half[ny]+E) + iy;
+        coord[ny] = ((pos + offset)[ny] - half[ny] + E) + iy;
-        for (int iz = 0; iz <= glm::ceil((half[nz]-E)*2); iz++){
+        for (int iz = 0; iz <= glm::ceil((half[nz] - E) * 2); iz++) {
-            coord[nz] = (pos[nz]-half[nz]+E) + iz;
+            coord[nz] = (pos[nz] - half[nz] + E) + iz;
-            coord[nx] = (pos[nx]-half[nx]-E) - E;
+            coord[nx] = (pos[nx] - half[nx] - E) - E;
-            
+
             auto boxAABB = AABB(pos - half, pos + half);
             glm::vec3 scale(1.0f);
             scale[nz] = 1.0f - E * 2.0f;
@@ -217,6 +130,23 @@ static bool calc_collision_neg_y(
     const glm::vec3& half,
     glm::vec3& groundVelocty
 ) {
+    for (const auto& box : solidHitboxes) {
+        if (glm::distance2(box->position, pos) < E) {
+            continue;
+        }
+        auto boxhalf = box->getHalfSize();
+        if (box->getAABB().intersects(AABB(pos - half, pos + half))) {
+            if (vel.y < 0.0f) {
+                vel.y = 0.0f;
+            }
+            float newx = box->position.y + boxhalf.y + half.y;
+            if (newx - pos.y <= 0.5f) {
+                pos.y = newx;
+            }
+            groundVelocty = box->velocity;
+            return true;
+        }
+    }
     if (vel.y >= 0.0f) {
         return false;
     }
@@ -244,64 +174,9 @@ static bool calc_collision_neg_y(
             }
         }
     }
-    for (const auto& box : solidHitboxes) {
-        if (glm::distance2(box->position, pos) < E) {
-            continue;
-        }
-        auto boxhalf = box->getHalfSize();
-        if (AABB(box->position - boxhalf, box->position + boxhalf).intersects(AABB(pos - half, pos + half))) {
-            vel.y = 0.0f;
-            float newx = box->position.y + boxhalf.y + half.y;
-            if (newx - pos.y <= 0.5f) {
-                pos.y = newx;
-            }
-            groundVelocty = box->velocity;
-            return true;
-        }
-    }
     return false;
 }
 
-template <int nx, int ny, int nz>
-static void calc_collision_pos(
-    const GlobalChunks& chunks,
-    glm::vec3& pos,
-    glm::vec3& vel,
-    const glm::vec3& half,
-    float stepHeight,
-    bool collided[3]
-) {
-    if (vel[nx] <= 0.0f) {
-        return;
-    }
-    glm::vec3 offset(0.0f, stepHeight + E * 2, 0.0f);
-    for (int iy = 0; iy <= glm::ceil(((half-offset*0.5f)[ny]-E)*2); iy++) {
-        glm::vec3 coord;
-        coord[ny] = ((pos+offset)[ny]-half[ny]+E) + iy;
-        for (int iz = 0; iz <= glm::ceil((half[nz]-E)*2); iz++) {
-            coord[nz] = (pos[nz]-half[nz]+E) + iz;
-            coord[nx] = (pos[nx]+half[nx]+E) + E;
-
-            auto boxAABB = AABB(pos - half, pos + half);
-            glm::vec3 scale(1.0f);
-            scale[nz] = 1.0f - E * 2.0f;
-            boxAABB.scale(scale);
-            boxAABB = boxAABB + offset;
-            boxAABB.b.y -= stepHeight + E * 2;
-
-            if (const auto aabb = chunks.isObstacleAt(coord.x, coord.y, coord.z, boxAABB)) {
-                float newx = std::floor(coord[nx]) - half[nx] + aabb->min()[nx] - E;
-                if (pos[nx] > newx) {
-                    vel[nx] = 0.0f;
-                    pos[nx] = newx;
-                    collided[nx] = true;
-                }
-                return;
-            }
-        }
-    }
-}
-
 void PhysicsSolver::calcCollisions(
     const GlobalChunks& chunks,
     Hitbox& hitbox, 
@@ -356,43 +231,169 @@ void PhysicsSolver::calcCollisions(
         }
     }
 
-    if (stepHeight > 0.0 && vel.y <= 0.0f){
+    // step on
+    if (stepHeight > 0.0 && vel.y <= 0.0f) {
         AABB boxAABB = AABB(-half, +half);
-        boxAABB.scale(glm::vec3(1.0f - E * 2, 1.0f, 1.0f - E * 2));
+        boxAABB.scale(glm::vec3(1.0f - E * 2, 1.0f - E * 2, 1.0f - E * 2));
         boxAABB = boxAABB.translated(pos);
 
-        for (int ix = 0; ix <= glm::ceil((half.x-E)*2); ix++) {
+        for (int ix = 0; ix <= glm::ceil((half.x - E) * 2); ix++) {
             float x = (pos.x-half.x) + ix;
-            for (int iz = 0; iz <= glm::ceil((half.z-E)*2); iz++) {
+            for (int iz = 0; iz <= glm::ceil((half.z - E) * 2); iz++) {
-                float z = (pos.z-half.z) + iz;
+                float z = (pos.z - half.z) + iz;
-                float y = (pos.y-half.y+E);
+                float y = (pos.y - half.y + E) + E;
-                if (auto aabb = chunks.isObstacleAt(x,y,z, boxAABB)){
+                if (auto aabb = chunks.isObstacleAt(x, y, z, boxAABB)) {
                     vel.y = 0.0f;
                     float newy = std::floor(y) + aabb->max().y + half.y;
-                    if (std::abs(newy-pos.y) <= stepHeight) {
+                    if (std::abs(newy - pos.y) <= stepHeight) {
-                        pos.y = newy;    
+                        pos.y = newy;
                     }
                     break;
                 }
             }
         }
-        // succesfully increasing code duplication
         for (const auto& box : solidHitboxes) {
             if (glm::distance2(box->position, pos) < E) {
                 continue;
             }
             auto boxhalf = box->getHalfSize();
-            if (AABB(box->position - boxhalf, box->position + boxhalf).intersects(AABB(pos - half, pos + half))) {
+            if (box->getAABB().intersects(boxAABB)) {
                 vel.y = 0.0f;
-                float newx = box->position.y + boxhalf.y + half.y;
+                float newy = box->position.y + boxhalf.y + half.y;
-                if (newx - pos.y <= 0.5f) {
+                if (std::abs(newy - pos.y) <= stepHeight) {
-                    pos.y = newx;
+                    pos.y = newy;
                 }
             }
         }
     }
 }
 
+void PhysicsSolver::calcSubstep(
+    const GlobalChunks& chunks,
+    Hitbox& hitbox,
+    glm::vec3& vel,
+    glm::vec3& pos,
+    bool prevGrounded,
+    float dt
+) {
+    auto initpos = pos;
+    auto half = hitbox.getHalfSize();
+    float gravityScale = hitbox.gravityScale;
+        
+    if (hitbox.type == BodyType::DYNAMIC) {
+        calcCollisions(
+            chunks,
+            hitbox,
+            vel,
+            pos,
+            half,
+            (prevGrounded && gravityScale > 0.0f) ? hitbox.stepHeight : 0.0f
+        );
+    }
+
+    vel += gravity * dt * gravityScale;
+    pos += vel * dt + gravity * gravityScale * dt * dt * 0.5f;
+
+    if (hitbox.grounded && pos.y < initpos.y) {
+        pos.y = initpos.y;
+    }
+
+    // crouching
+    if (!hitbox.crouching || !hitbox.grounded) {
+        return;
+    }
+    float y = (pos.y - half.y - E);
+    for (int axis = 0; axis <= 2; axis += 2) {
+        hitbox.grounded = false;
+
+        AABB aabb(pos - half, pos + half);
+        aabb.scale(glm::vec3(1.0f - E * 4, 1.0f, 1.0f - E * 4));
+        for (int ix = 0; ix <= glm::ceil((half.x - E) * 2); ix++) {
+            float x = (initpos.x - half.x + E) + ix;
+            for (int iz = 0; iz <= glm::ceil((half.z - E) * 2); iz++){
+                float z = (pos.z - half.z + E) + iz;
+                if (chunks.isObstacleAt(x, y, z, aabb)){
+                    hitbox.grounded = true;
+                    break;
+                }
+            }
+        }
+        for (const auto& box : solidHitboxes) {
+            if (glm::distance2(box->position, pos) < E) {
+                continue;
+            }
+            if (box->getAABB().intersects(AABB(pos - half, pos + half))) {
+                hitbox.grounded = true;
+                break;
+            }
+        }
+        if (!hitbox.grounded) {
+            pos[axis] = initpos[axis];
+            vel[axis] = 0.0f;
+        }
+    }
+    hitbox.grounded = true;
+}
+
+void PhysicsSolver::step(
+    const GlobalChunks& chunks,
+    Hitbox& hitbox,
+    float delta,
+    uint substeps,
+    entityid_t entity
+) {
+    float dt = delta / static_cast<float>(substeps);
+    float linearDamping = hitbox.linearDamping * hitbox.friction;
+
+    glm::vec3& pos = hitbox.position;
+    glm::vec3& vel = hitbox.velocity;
+   
+    bool prevGrounded = hitbox.grounded;
+    hitbox.grounded = false;
+    for (uint i = 0; i < substeps; i++) {
+        calcSubstep(chunks, hitbox, vel, pos, prevGrounded, dt);
+    }
+
+    if (prevGrounded && !hitbox.grounded && vel.y <= 0.0f) {
+        vel += hitbox.groundVelocity;
+        hitbox.groundVelocity = {};
+    }
+
+    vel.x /= 1.0f + delta * linearDamping;
+    vel.z /= 1.0f + delta * linearDamping;
+    if (hitbox.verticalDamping > 0.0f) {
+        vel.y /= 1.0f + delta * linearDamping * hitbox.verticalDamping;
+    }
+
+    AABB aabb;
+    aabb.a = pos - hitbox.getHalfSize();
+    aabb.b = pos + hitbox.getHalfSize();
+    for (size_t i = 0; i < sensors.size(); i++) {
+        auto& sensor = *sensors[i];
+        if (sensor.entity == entity) {
+            continue;
+        }
+
+        bool triggered = false;
+        switch (sensor.type) {
+            case SensorType::AABB:
+                triggered = aabb.intersects(sensor.calculated.aabb);
+                break;
+            case SensorType::RADIUS:
+                triggered = glm::distance2(
+                    pos, glm::vec3(sensor.calculated.radial))
+                     < sensor.calculated.radial.w;
+                break;
+        }
+        if (triggered) {
+            if (sensor.prevEntered.find(entity) == sensor.prevEntered.end()) {
+                sensor.enterCallback(sensor.entity, sensor.index, entity);
+            }
+            sensor.nextEntered.insert(entity);
+        }
+    }
+}
+
 void PhysicsSolver::removeSensor(Sensor* sensor) {
     sensors.erase(std::remove(sensors.begin(), sensors.end(), sensor), sensors.end());
 }