348 lines
12 KiB
C++
348 lines
12 KiB
C++
#include "vf/Collide.h"
|
|
|
|
#include <cmath>
|
|
#include <limits>
|
|
|
|
namespace vf {
|
|
|
|
namespace {
|
|
|
|
constexpr float gatherPadding = 1.0e-3f;
|
|
constexpr float parallelMotion = 1.0e-12f;
|
|
|
|
float component(const Vec3& v, int a) { return (&v.x)[a]; }
|
|
void setComponent(Vec3& v, int a, float value) { (&v.x)[a] = value; }
|
|
|
|
int axisIndex(ContactAxis axis) {
|
|
switch (axis) {
|
|
case ContactAxis::X: return 0;
|
|
case ContactAxis::Y: return 1;
|
|
case ContactAxis::Z: return 2;
|
|
default: return -1;
|
|
}
|
|
}
|
|
|
|
ContactAxis axisFromIndex(int a) {
|
|
switch (a) {
|
|
case 0: return ContactAxis::X;
|
|
case 1: return ContactAxis::Y;
|
|
default: return ContactAxis::Z;
|
|
}
|
|
}
|
|
|
|
int floorToCell(float value) { return static_cast<int>(std::floor(value)); }
|
|
|
|
bool motionIsIdle(const Vec3& motion) {
|
|
return std::fabs(motion.x) < parallelMotion && std::fabs(motion.y) < parallelMotion &&
|
|
std::fabs(motion.z) < parallelMotion;
|
|
}
|
|
|
|
SweptBox unitCellBox(const Vec3i& cell, float loY, float hiY, float inset) {
|
|
float x = static_cast<float>(cell.x);
|
|
float y = static_cast<float>(cell.y);
|
|
float z = static_cast<float>(cell.z);
|
|
return SweptBox{{x + inset, y + loY, z + inset}, {x + 1.0f - inset, y + hiY, z + 1.0f - inset}};
|
|
}
|
|
|
|
} // namespace
|
|
|
|
SweptBox makeBoxAtFeet(const Vec3& feetCentre, float width, float height) {
|
|
float half = width * 0.5f;
|
|
return SweptBox{{feetCentre.x - half, feetCentre.y, feetCentre.z - half},
|
|
{feetCentre.x + half, feetCentre.y + height, feetCentre.z + half}};
|
|
}
|
|
|
|
SweptBox translatedBox(const SweptBox& box, const Vec3& delta) {
|
|
return SweptBox{box.min + delta, box.max + delta};
|
|
}
|
|
|
|
SweptBox inflatedBox(const SweptBox& box, float amount) {
|
|
Vec3 pad{amount, amount, amount};
|
|
return SweptBox{box.min - pad, box.max + pad};
|
|
}
|
|
|
|
SweptBox motionEnvelope(const SweptBox& box, const Vec3& motion) {
|
|
SweptBox moved = translatedBox(box, motion);
|
|
SweptBox out;
|
|
for (int a = 0; a < 3; ++a) {
|
|
setComponent(out.min, a, std::fmin(component(box.min, a), component(moved.min, a)));
|
|
setComponent(out.max, a, std::fmax(component(box.max, a), component(moved.max, a)));
|
|
}
|
|
return out;
|
|
}
|
|
|
|
bool boxesOverlap(const SweptBox& a, const SweptBox& b) {
|
|
return a.min.x < b.max.x && a.max.x > b.min.x && a.min.y < b.max.y && a.max.y > b.min.y &&
|
|
a.min.z < b.max.z && a.max.z > b.min.z;
|
|
}
|
|
|
|
bool boxesTouchOrOverlap(const SweptBox& a, const SweptBox& b, float tolerance) {
|
|
return boxesOverlap(inflatedBox(a, tolerance), b);
|
|
}
|
|
|
|
SweptBox aabbToSweptBox(const Aabb& box) { return SweptBox{box.min, box.max}; }
|
|
|
|
Aabb sweptBoxToAabb(const SweptBox& box) { return Aabb{box.min, box.max}; }
|
|
|
|
CollisionShape collisionShapeOf(Block block) {
|
|
switch (block) {
|
|
case Block::Slab: return CollisionShape::SlabBottom;
|
|
case Block::Stairs: return CollisionShape::Stairs;
|
|
case Block::Fence: return CollisionShape::Fence;
|
|
case Block::Ladder: return CollisionShape::Ladder;
|
|
case Block::Vine: return CollisionShape::Vine;
|
|
case Block::SnowLayer: return CollisionShape::Carpet;
|
|
default: return isSolid(block) ? CollisionShape::Full : CollisionShape::None;
|
|
}
|
|
}
|
|
|
|
bool isClimbableShape(CollisionShape shape) {
|
|
return shape == CollisionShape::Ladder || shape == CollisionShape::Vine;
|
|
}
|
|
|
|
bool shapeBlocksMovement(CollisionShape shape) {
|
|
switch (shape) {
|
|
case CollisionShape::None:
|
|
case CollisionShape::Ladder:
|
|
case CollisionShape::Vine:
|
|
return false;
|
|
default:
|
|
return true;
|
|
}
|
|
}
|
|
|
|
ShapeBoxes shapeBoxesAt(CollisionShape shape, const Vec3i& cell) {
|
|
ShapeBoxes out;
|
|
switch (shape) {
|
|
case CollisionShape::Full:
|
|
out.boxes[out.count++] = unitCellBox(cell, 0.0f, 1.0f, 0.0f);
|
|
break;
|
|
case CollisionShape::SlabBottom:
|
|
out.boxes[out.count++] = unitCellBox(cell, 0.0f, 0.5f, 0.0f);
|
|
break;
|
|
case CollisionShape::SlabTop:
|
|
out.boxes[out.count++] = unitCellBox(cell, 0.5f, 1.0f, 0.0f);
|
|
break;
|
|
case CollisionShape::Stairs:
|
|
// Lower slab plus the upper step over the +X half of the cell.
|
|
out.boxes[out.count++] = unitCellBox(cell, 0.0f, 0.5f, 0.0f);
|
|
{
|
|
SweptBox upper = unitCellBox(cell, 0.5f, 1.0f, 0.0f);
|
|
upper.min.x = static_cast<float>(cell.x) + 0.5f;
|
|
out.boxes[out.count++] = upper;
|
|
}
|
|
break;
|
|
case CollisionShape::Fence:
|
|
// The post reaches above its own cell, which is why a body cannot
|
|
// hop a fence the way it hops a full block.
|
|
out.boxes[out.count++] = unitCellBox(cell, 0.0f, fencePostHeight, 0.375f);
|
|
break;
|
|
case CollisionShape::Carpet:
|
|
out.boxes[out.count++] = unitCellBox(cell, 0.0f, carpetHeight, 0.0f);
|
|
break;
|
|
case CollisionShape::None:
|
|
case CollisionShape::Ladder:
|
|
case CollisionShape::Vine:
|
|
break;
|
|
}
|
|
return out;
|
|
}
|
|
|
|
SweepHit sweepBoxAgainstBox(const SweptBox& mover, const Vec3& motion, const SweptBox& obstacle) {
|
|
SweepHit hit;
|
|
Vec3 moverSize = mover.size();
|
|
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
|