SweptCollision.hpp

Go to the documentation of this file.

 
#pragma once
 
#include "PhysicsPerfStats.hpp"
#include "SurfaceType.hpp"
 
#include <cmath>
#include <cstdint>
#include <functional>
#include <glm/geometric.hpp>
#include <glm/vec3.hpp>
#include <span>
#include <vector>

namespace physics
{

struct Plane
{
    glm::vec3 normal; 
    float distance;   
    SurfaceType surfaceType = SurfaceType::Concrete; 
};

struct WorldAABB
{
    glm::vec3 min;                                   
    glm::vec3 max;                                   
    SurfaceType surfaceType = SurfaceType::Concrete; 
};

struct WorldBrush
{
    static constexpr int k_maxPlanes = 64;
    Plane planes[k_maxPlanes];
    int planeCount{0};
    SurfaceType surfaceType = SurfaceType::Concrete; 
};

struct WorldCylinder
{
    glm::vec3 base;                                  
    float radius;                                    
    float height;                                    
    SurfaceType surfaceType = SurfaceType::Concrete; 
};

struct WorldSphere
{
    glm::vec3 center;                                
    float radius;                                    
    SurfaceType surfaceType = SurfaceType::Concrete; 
};

struct BVHNode
{
    glm::vec3 boundsMin; 
    glm::vec3 boundsMax; 
    int leftFirst;       
    int count;           
};

struct WorldTriMesh
{
    std::vector<glm::vec3> vertices;  
    std::vector<uint32_t> indices;    
    std::vector<BVHNode> bvhNodes;    
    std::vector<uint32_t> triIndices; 
    glm::vec3 boundsMin{0.0f};        
    glm::vec3 boundsMax{0.0f};        
 
    // Welding data (one entry per canonical triangle index in `indices`).
    std::vector<glm::vec3> faceNormals; 
    std::vector<uint8_t> edgeActive; 
    std::vector<uint8_t> vertActive; 

    std::vector<uint32_t> edgeNeighbor;
 
    // Phase 3 material data.  If `triangleMaterials` is empty, every triangle
    // falls back to `defaultSurface`.  Authored by `MapLoader` from Blender
    // per-face materials.
    std::vector<uint8_t> triangleMaterials;             
    SurfaceType defaultSurface = SurfaceType::Concrete; 
};

struct StaticWorldBroadphase
{
    std::vector<BVHNode> nodes;
    std::vector<uint32_t> meshIndices;
    std::vector<WorldAABB> meshBounds;
};

void buildStaticWorldBroadphase(StaticWorldBroadphase& broadphase, std::span<const WorldTriMesh> triMeshes);

void queryStaticWorldBroadphase(const StaticWorldBroadphase& broadphase,
                                const WorldAABB& query,
                                const std::function<bool(uint32_t meshIndex)>& visit);
 
[[nodiscard]] inline bool broadphaseAabbOverlap(const WorldAABB& a, const WorldAABB& b) noexcept
{
    return a.max.x >= b.min.x && a.min.x <= b.max.x && a.max.y >= b.min.y && a.min.y <= b.max.y && a.max.z >= b.min.z &&
           a.min.z <= b.max.z;
}
 
template <class Visit>
inline void
queryStaticWorldBroadphaseFast(const StaticWorldBroadphase& broadphase, const WorldAABB& query, Visit&& visit)
{
    perf::add(&perf::FrameStats::staticBroadphaseQueries);
    if (broadphase.nodes.empty())
        return;
 
    constexpr int kStackCapacity = 128;
    int stack[kStackCapacity];
    std::vector<int> overflowStack;
    int stackPtr = 0;
    stack[0] = 0;
 
    auto pushNode = [&](int nodeIndex) {
        if (stackPtr + 1 < kStackCapacity) {
            stack[++stackPtr] = nodeIndex;
        } else {
            overflowStack.push_back(nodeIndex);
        }
    };
 
    while (stackPtr >= 0 || !overflowStack.empty()) {
        int nodeIdx = 0;
        if (stackPtr >= 0) {
            nodeIdx = stack[stackPtr--];
        } else {
            nodeIdx = overflowStack.back();
            overflowStack.pop_back();
        }
        const BVHNode& node = broadphase.nodes[static_cast<std::size_t>(nodeIdx)];
        const WorldAABB nodeBounds{.min = node.boundsMin, .max = node.boundsMax};
        if (!broadphaseAabbOverlap(nodeBounds, query))
            continue;
 
        if (node.count > 0) {
            for (int i = node.leftFirst; i < node.leftFirst + node.count; ++i) {
                const uint32_t meshIndex = broadphase.meshIndices[static_cast<std::size_t>(i)];
                if (meshIndex >= broadphase.meshBounds.size() ||
                    !broadphaseAabbOverlap(broadphase.meshBounds[meshIndex], query))
                    continue;
                perf::add(&perf::FrameStats::staticBroadphaseMeshes);
                if (!visit(meshIndex))
                    return;
            }
        } else {
            pushNode(node.leftFirst);
            pushNode(node.leftFirst + 1);
        }
    }
}

struct WorldGeometry
{
    std::span<const Plane> planes;
    std::span<const WorldAABB> boxes;
    std::span<const WorldBrush> brushes;
    std::span<const WorldCylinder> cylinders;
    std::span<const WorldSphere> spheres;
    std::span<const WorldTriMesh> triMeshes;
    const StaticWorldBroadphase* staticBroadphase{nullptr};
};

struct CapsuleShape
{
    float radius{16.0f};            
    float halfHeight{20.0f};        
    glm::vec3 up{0.0f, 1.0f, 0.0f}; 

    [[nodiscard]] glm::vec3 segA(glm::vec3 center) const noexcept { return center + up * halfHeight; }

    [[nodiscard]] glm::vec3 segB(glm::vec3 center) const noexcept { return center - up * halfHeight; }

    [[nodiscard]] glm::vec3 enclosingHalfExtents() const noexcept
    {
        return {std::abs(up.x) * halfHeight + radius,
                std::abs(up.y) * halfHeight + radius,
                std::abs(up.z) * halfHeight + radius};
    }

    [[nodiscard]] float minkowskiExtent(glm::vec3 n) const noexcept
    {
        return radius + halfHeight * std::abs(glm::dot(up, n));
    }

    [[nodiscard]] float effectiveStepHeight(float requestedStepHeight) const noexcept
    {
        const float maxAllowed = std::max(halfHeight - 0.5f, 0.0f);
        const float clamped = std::min(std::max(requestedStepHeight, 0.0f), maxAllowed);
        return clamped;
    }

    [[nodiscard]] CapsuleShape walkShape(float requestedStepHeight) const noexcept
    {
        const float effStep = effectiveStepHeight(requestedStepHeight);
        return CapsuleShape{.radius = radius, .halfHeight = halfHeight - effStep * 0.5f, .up = up};
    }

    [[nodiscard]] glm::vec3 walkCenterOffset(float requestedStepHeight) const noexcept
    {
        return up * (effectiveStepHeight(requestedStepHeight) * 0.5f);
    }
};

struct HitResult
{
    bool hit{false};                    
    float tFirst{1.0f};                 
    glm::vec3 normal{0.0f, 1.0f, 0.0f}; 
    SurfaceType surfaceType{SurfaceType::Concrete}; 
};

HitResult sweepAABB(glm::vec3 halfExtents, glm::vec3 start, glm::vec3 end, std::span<const Plane> planes);

HitResult sweepAABBvsBox(glm::vec3 halfExtents, glm::vec3 start, glm::vec3 end, const WorldAABB& box);

HitResult sweepAABBvsBrush(glm::vec3 halfExtents, glm::vec3 start, glm::vec3 end, const WorldBrush& brush);

HitResult sweepAABBvsCylinder(glm::vec3 halfExtents, glm::vec3 start, glm::vec3 end, const WorldCylinder& cyl);

HitResult sweepAABBvsSphere(glm::vec3 halfExtents, glm::vec3 start, glm::vec3 end, const WorldSphere& sph);

HitResult sweepAll(glm::vec3 halfExtents, glm::vec3 start, glm::vec3 end, const WorldGeometry& world);
 
// Capsule swept-collision against convex primitives. Planes and brushes use
// per-plane capsule extents; box, cylinder, and sphere keep conservative
// dev-arena approximations. Production map geometry is expected to use the
// trimesh capsule path in TriMeshCollision.hpp.

HitResult sweepCapsuleVsPlanes(CapsuleShape capsule, glm::vec3 start, glm::vec3 end, std::span<const Plane> planes);

HitResult sweepCapsuleVsBox(CapsuleShape capsule, glm::vec3 start, glm::vec3 end, const WorldAABB& box);

HitResult sweepCapsuleVsBrush(CapsuleShape capsule, glm::vec3 start, glm::vec3 end, const WorldBrush& brush);

HitResult sweepCapsuleVsCylinder(CapsuleShape capsule, glm::vec3 start, glm::vec3 end, const WorldCylinder& cyl);

HitResult sweepCapsuleVsSphere(CapsuleShape capsule, glm::vec3 start, glm::vec3 end, const WorldSphere& sph);

HitResult sweepAll(CapsuleShape capsule, glm::vec3 start, glm::vec3 end, const WorldGeometry& world);

struct ClearanceResult
{
    bool contact{false};                            
    float distance{1e30f};                          
    glm::vec3 pointOnGeometry{0.0f};                
    glm::vec3 normal{0.0f, 1.0f, 0.0f};             
    SurfaceType surfaceType{SurfaceType::Concrete}; 
};
 
// Capsule-vs-primitive closest-point queries (Phase C clearance CA).
//
// Each query returns the directed distance from the capsule's surface to
// the nearest point on the primitive (positive = outside, zero = touching,
// negative = penetrating) and the contact normal pointing into free space.
//
// EXACT for planes, brushes, and spheres.  For boxes, cylinders, and
// trimeshes the query uses the underlying segment-vs-primitive geometry
// where exact (capsule axis as the moving feature), falling back to
// capsule's enclosing AABB where exact would require disproportionate
// per-feature math (boxes — exact would need full SAT-vs-segment).
//
// All results are conservative: the reported `distance` is always
// ≤ the true distance, so an advance of `distance / velIntoSurface`
// along the velocity direction never penetrates.
 
ClearanceResult clearanceCapsuleVsPlanes(CapsuleShape capsule, glm::vec3 pos, std::span<const Plane> planes);
ClearanceResult clearanceCapsuleVsBox(CapsuleShape capsule, glm::vec3 pos, const WorldAABB& box);
ClearanceResult clearanceCapsuleVsBrush(CapsuleShape capsule, glm::vec3 pos, const WorldBrush& brush);
ClearanceResult clearanceCapsuleVsCylinder(CapsuleShape capsule, glm::vec3 pos, const WorldCylinder& cyl);
ClearanceResult clearanceCapsuleVsSphere(CapsuleShape capsule, glm::vec3 pos, const WorldSphere& sph);

ClearanceResult clearanceCapsuleVsWorld(CapsuleShape capsule,
                                        glm::vec3 pos,
                                        const WorldGeometry& world,
                                        float maxMeshSearchRadius = 1024.0f);
 
// Character-controller utilities (modern KCC pattern)

struct GroundProbeResult
{
    bool hit{false};                    
    bool walkable{false};               
    float distance{1e30f};              
    glm::vec3 point{0.0f};              
    glm::vec3 normal{0.0f, 1.0f, 0.0f}; 
    SurfaceType surfaceType{SurfaceType::Concrete}; 
};

GroundProbeResult probeGround(CapsuleShape capsule, glm::vec3 pos, float maxDistance, const WorldGeometry& world);

struct DepenContact
{
    bool valid{false};
    float depth{0.0f};
    glm::vec3 normal{0.0f, 1.0f, 0.0f};
    SurfaceType surfaceType{SurfaceType::Concrete};
};
 
struct DepenetrationOptions
{
    float maxPushDistance{1e30f};
    bool allowEmergencyUnstick{true};
};
 
struct DepenetrationResult
{
    float pushDistance{0.0f};
    int passes{0};
    bool emergencyUnstuck{false};
    bool exceededMaxPush{false};
    bool unresolvedOverlap{false};
};

DepenContact deepestCapsuleContact(CapsuleShape capsule, glm::vec3 pos, glm::vec3 vel, const WorldGeometry& world);

void depenetrateCapsuleVsWorld(glm::vec3& pos, glm::vec3& vel, CapsuleShape capsule, const WorldGeometry& world);
 
DepenetrationResult depenetrateCapsuleVsWorldDetailed(glm::vec3& pos,
                                                      glm::vec3& vel,
                                                      CapsuleShape capsule,
                                                      const WorldGeometry& world,
                                                      DepenetrationOptions options = {});

bool emergencyUnstick(glm::vec3& pos, glm::vec3& vel, CapsuleShape capsule, const WorldGeometry& world);
 
// Sphere cast

struct SphereHitResult
{
    bool hit{false};
    float t{1.0f};                                  
    glm::vec3 normal{0.0f, 1.0f, 0.0f};             
    glm::vec3 point{0.0f};                          
    SurfaceType surfaceType{SurfaceType::Concrete}; 
};

SphereHitResult sphereCast(float radius, glm::vec3 start, glm::vec3 end, const WorldGeometry& world);
 
} // namespace physics