diff --git a/core/src/Collide.cpp b/core/src/Collide.cpp index 075a026..86d34ad 100644 --- a/core/src/Collide.cpp +++ b/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::infinity(); + float exit = std::numeric_limits::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