feat: add sweepBoxAgainstWorld and boxOverlapsWorld to Collide

core/src/Collide.cpp

@@ -154,3 +154,194 @@ SweepHit sweepBoxAgainstBox(const SweptBox& mover, const Vec3& motion, const Swe
     SweptBox expanded{obstacle.min - moverSize, obstacle.max};
     Vec3 point = mover.min;
 
+    float enter = -std::numeric_limits<float>::infinity();
+    float exit = std::numeric_limits<float>::infinity();
+    int enterAxis = -1;
+    float enterSign = 0.0f;
+
+    for (int a = 0; a < 3; ++a) {
+        float origin = component(point, a);
+        float travel = component(motion, a);
+        float lo = component(expanded.min, a);
+        float hi = component(expanded.max, a);
+        if (std::fabs(travel) < parallelMotion) {
+            if (origin <= lo || origin >= hi) return hit;
+            continue;
+        }
+        float toLow = (lo - origin) / travel;
+        float toHigh = (hi - origin) / travel;
+        float axisEnter = toLow;
+        float axisExit = toHigh;
+        float sign = -1.0f;
+        if (toLow > toHigh) {
+            axisEnter = toHigh;
+            axisExit = toLow;
+            sign = 1.0f;
+        }
+        if (axisEnter > enter) {
+            enter = axisEnter;
+            enterAxis = a;
+            enterSign = sign;
+        }
+        if (axisExit < exit) exit = axisExit;
+    }
+
+    if (enterAxis < 0) return hit;
+    if (enter > exit || enter >= 1.0f || exit <= 0.0f || enter < 0.0f) return hit;
+
+    hit.hit = true;
+    hit.time = enter;
+    hit.axis = axisFromIndex(enterAxis);
+    setComponent(hit.normal, enterAxis, enterSign);
+    return hit;
+}
+
+void BroadPhase::gather(const World& world, const SweptBox& box, const Vec3& motion) {
+    cells_.clear();
+    envelope_ = inflatedBox(motionEnvelope(box, motion), gatherPadding);
+
+    int x0 = floorToCell(envelope_.min.x);
+    int x1 = floorToCell(envelope_.max.x);
+    int y0 = floorToCell(envelope_.min.y) - shapeOverhangCells;
+    int y1 = floorToCell(envelope_.max.y);
+    int z0 = floorToCell(envelope_.min.z);
+    int z1 = floorToCell(envelope_.max.z);
+
+    for (int x = x0; x <= x1; ++x) {
+        for (int y = y0; y <= y1; ++y) {
+            for (int z = z0; z <= z1; ++z) {
+                Vec3i cell{x, y, z};
+                Block block = world.blockAt(x, y, z);
+                CollisionShape shape = collisionShapeOf(block);
+                if (!shapeBlocksMovement(shape)) continue;
+                ShapeBoxes solids = shapeBoxesAt(shape, cell);
+                bool relevant = false;
+                for (int i = 0; i < solids.count; ++i) {
+                    if (boxesOverlap(envelope_, solids.boxes[i])) relevant = true;
+                }
+                if (!relevant) continue;
+                cells_.push_back(BroadPhaseCell{cell, block, shape, solids});
+            }
+        }
+    }
+}
+
+void BroadPhase::gatherStatic(const World& world, const SweptBox& box) {
+    gather(world, box, Vec3{0.0f, 0.0f, 0.0f});
+}
+
+SweepHit sweepBoxAgainstWorld(const World& world, const SweptBox& box, const Vec3& motion,
+                              BroadPhase& broadPhase) {
+    broadPhase.gather(world, box, motion);
+    SweepHit best;
+    if (motionIsIdle(motion)) return best;
+
+    for (const BroadPhaseCell& entry : broadPhase.cells()) {
+        for (int i = 0; i < entry.solids.count; ++i) {
+            SweepHit candidate = sweepBoxAgainstBox(box, motion, entry.solids.boxes[i]);
+            if (!candidate.hit) continue;
+            if (best.hit && candidate.time >= best.time) continue;
+            candidate.cell = entry.cell;
+            candidate.block = entry.block;
+            best = candidate;
+        }
+    }
+    return best;
+}
+
+bool boxOverlapsWorld(const World& world, const SweptBox& box) {
+    BroadPhase broadPhase;
+    broadPhase.gatherStatic(world, box);
+    for (const BroadPhaseCell& entry : broadPhase.cells()) {
+        for (int i = 0; i < entry.solids.count; ++i) {
+            if (boxesOverlap(box, entry.solids.boxes[i])) return true;
+        }
+    }
+    return false;
+}
+
+bool restsOnGround(const World& world, const SweptBox& box, float probeDepth) {
+    SweptBox probe = box;
+    probe.max.y = box.min.y;
+    probe.min.y = box.min.y - probeDepth;
+    // Pull the footprint in so a body flush against a wall does not read the
+    // wall itself as a floor.
+    probe.min.x += contactSkin;
+    probe.min.z += contactSkin;
+    probe.max.x -= contactSkin;
+    probe.max.z -= contactSkin;
+    return boxOverlapsWorld(world, probe);
+}
+
+float supportHeightBelow(const World& world, const SweptBox& box, float maxDrop) {
+    BroadPhase broadPhase;
+    Vec3 drop{0.0f, -maxDrop, 0.0f};
+    SweepHit hit = sweepBoxAgainstWorld(world, box, drop, broadPhase);
+    if (!hit.hit) return box.min.y - maxDrop;
+    return box.min.y - maxDrop * hit.time;
+}
+
+CollisionResult resolveMove(const World& world, const SweptBox& box, const Vec3& motion,
+                            BroadPhase& broadPhase) {
+    CollisionResult out;
+    out.box = box;
+    Vec3 remaining = motion;
+
+    for (int pass = 0; pass < maxSlidePasses; ++pass) {
+        if (motionIsIdle(remaining)) break;
+        out.passes = pass + 1;
+
+        SweepHit hit = sweepBoxAgainstWorld(world, out.box, remaining, broadPhase);
+        if (!hit.hit) {
+            out.box = translatedBox(out.box, remaining);
+            out.appliedMotion += remaining;
+            remaining = Vec3{0.0f, 0.0f, 0.0f};
+            break;
+        }
+        if (!out.firstHit.hit) out.firstHit = hit;
+
+        Vec3 step = remaining * hit.time;
+        Vec3 leftover = remaining - step;
+
+        int a = axisIndex(hit.axis);
+        float backedOff = component(step, a) + component(hit.normal, a) * contactSkin;
+        if (backedOff * component(remaining, a) < 0.0f) backedOff = 0.0f;
+        setComponent(step, a, backedOff);
+
+        out.box = translatedBox(out.box, step);
+        out.appliedMotion += step;
+
+        switch (hit.axis) {
+            case ContactAxis::X: out.touchedX = true; break;
+            case ContactAxis::Y:
+                out.touchedY = true;
+                if (hit.normal.y > 0.0f) out.onGround = true;
+                if (hit.normal.y < 0.0f) out.hitCeiling = true;
+                break;
+            case ContactAxis::Z: out.touchedZ = true; break;
+            case ContactAxis::None: break;
+        }
+
+        setComponent(leftover, a, 0.0f);
+        remaining = leftover;
+    }
+
+    out.remainingMotion = remaining;
+    if (!out.onGround && restsOnGround(world, out.box)) out.onGround = true;
+    return out;
+}
+
+CollisionResult resolveMove(const World& world, const SweptBox& box, const Vec3& motion) {
+    BroadPhase broadPhase;
+    return resolveMove(world, box, motion, broadPhase);
+}
+
+Vec3 clipVelocity(const Vec3& velocity, const CollisionResult& collision) {
+    Vec3 out = velocity;
+    if (collision.touchedX) out.x = 0.0f;
+    if (collision.touchedY) out.y = 0.0f;
+    if (collision.touchedZ) out.z = 0.0f;
+    return out;
+}
+
+} // namespace vf