LCOV - code coverage report
Current view: top level - rbc - BroadPhase.hpp (source / functions) Coverage Total Hit
Test: coverage_clean.info Lines: 100.0 % 12 12
Test Date: 2026-03-30 03:13:12 Functions: 100.0 % 1 1

            Line data    Source code
       1              : #pragma once
       2              : #include <vector>
       3              : #include <cstdint>
       4              : #include <limits>
       5              : #include "rbc/AABB.hpp"
       6              : #include "rbc/shapes/ShapeTypes.hpp"
       7              : 
       8              : namespace rbc
       9              : {
      10              :     // =========================================================================
      11              :     //  BROAD PHASE — Sort and Sweep (SAP) Algorithm Aka Sweep and Prune
      12              :     // =========================================================================
      13              :     //
      14              :     //  ALGORITHM OVERVIEW
      15              :     //  ------------------
      16              :     //  Sort and Sweep projects each AABB onto one axis (here, X) and maintains
      17              :     //  a sorted list of interval endpoints [min_i, max_i].  A sweep through the
      18              :     //  list with an "active set" finds all pairs whose X-intervals overlap in
      19              :     //  O(n + k) time after the sort, where k is the number of overlapping pairs.
      20              :     //
      21              :     //  Each candidate pair is then verified against the full 3-D AABB test to
      22              :     //  discard false positives from the 1-axis projection.
      23              :     //
      24              :     //  WHY SAP OVER OCTREES / BVH?
      25              :     //  ----------------------------
      26              :     //   - Octrees require expensive node re-insertion every frame for moving objects.
      27              :     //   - BVH (dynamic AABB tree) is excellent but adds per-node pointer overhead.
      28              :     //   - SAP is a flat sorted array — cache-coherent, simple, allocation-free
      29              :     //     at steady state, and nearly O(n) per frame when objects move little
      30              :     //     (insertion sort is O(n) on nearly-sorted data).
      31              :     //   - SAP is the algorithm used internally by PhysX and Havok.
      32              :     //
      33              :     //  TEMPORAL COHERENCE
      34              :     //  ------------------
      35              :     //  AABBs are "fattened" by a configurable margin.  A moving object only
      36              :     //  triggers a AABB update (and a re-sort) when it leaves its fattened AABB,
      37              :     //  not every frame.  This keeps the sorted list nearly-sorted between frames,
      38              :     //  which is exactly the best case for insertion sort.
      39              :     //
      40              :     //  DATA LAYOUT (data-driven, no virtual dispatch)
      41              :     //  -----------------------------------------------
      42              :     //    BroadPhaseState  — owns all allocations
      43              :     //    BPHandle         — opaque index returned by broad_phase_insert()
      44              :     //    BPObject         — per-object AABB + user ID
      45              :     //    SAPEndpoint      — one entry per AABB endpoint (2 per object)
      46              :     //    BroadPhasePair   — output pair of user IDs
      47              :     // =========================================================================
      48              : 
      49              :     // Opaque handle for a registered object. Never store raw indices; use these.
      50              :     using BPHandle = uint32_t;
      51              :     static constexpr BPHandle BP_INVALID_HANDLE = std::numeric_limits<uint32_t>::max();
      52              : 
      53              :     // Output: a pair of user IDs that may be colliding (needs narrow phase).
      54              :     struct BroadPhasePair
      55              :     {
      56              :         uint32_t id_a;
      57              :         uint32_t id_b;
      58              :     };
      59              : 
      60              :     // Internal: one sorted endpoint per AABB edge on the projection axis.
      61              :     // Packed into 8 bytes: 4-byte scalar + 4-byte bitfield.
      62              :     struct alignas(8) SAPEndpoint
      63              :     {
      64              :         float    value;        // projected value on the sweep axis
      65              :         uint32_t slot  : 31;  // index into BroadPhaseState::objects
      66              :         uint32_t is_max : 1;  // 0 = min endpoint, 1 = max endpoint
      67              :     };
      68              :     static_assert(sizeof(SAPEndpoint) == 8, "SAPEndpoint size check");
      69              : 
      70              :     // Internal: per-registered-object record.
      71              :     struct BPObject
      72              :     {
      73              :         AABB     fat_aabb;     // fattened AABB actually stored in the sorted list
      74              :         AABB     tight_aabb;   // last tight AABB provided by the caller
      75              :         uint32_t user_id;      // opaque ID provided by the caller
      76              :         bool     alive;        // false = slot is in free_list, available for reuse
      77              :     };
      78              : 
      79              :     // Configurable parameters for the broad phase.
      80              :     struct BroadPhaseConfig
      81              :     {
      82              :         // How much to fatten AABBs (world-space units).
      83              :         // Larger = fewer updates but more false positives in the narrow phase.
      84              :         m3d::scalar fat_margin = 0.1;
      85              : 
      86              :         // If an object's tight AABB grows beyond its current fat AABB,
      87              :         // we reinsert. `reinsert_threshold` is an extra slack so small
      88              :         // oscillations don't trigger constant reinsertion.
      89              :         m3d::scalar reinsert_threshold = 0.05;
      90              :     };
      91              : 
      92              :     // The full broad phase state. Treat as opaque; mutate only through the API.
      93              :     struct BroadPhaseState
      94              :     {
      95              :         BroadPhaseConfig          config;
      96              :         std::vector<BPObject>     objects;      // slot array (may have gaps)
      97              :         std::vector<uint32_t>     free_list;    // recycled slot indices
      98              :         std::vector<SAPEndpoint>  endpoints;    // sorted on `value` (X axis)
      99              :         std::vector<uint32_t>     active_set;   // scratch buffer for sweep
     100              :         std::vector<BroadPhasePair> pairs;      // OUTPUT — filled each broad_phase_update()
     101              :         bool                      dirty;        // true = full re-sort needed
     102              :     };
     103              : 
     104              :     // -------------------------------------------------------------------------
     105              :     //  API
     106              :     // -------------------------------------------------------------------------
     107              : 
     108              :     // Initialise a BroadPhaseState with optional config.
     109              :     void broad_phase_init(BroadPhaseState &bp,
     110              :                           const BroadPhaseConfig &cfg = BroadPhaseConfig{});
     111              : 
     112              :     // Register an object. `user_id` is whatever the caller wants (body index, etc.)
     113              :     // Returns a BPHandle used for future move/remove calls.
     114              :     BPHandle broad_phase_insert(BroadPhaseState &bp,
     115              :                                 uint32_t         user_id,
     116              :                                 const AABB      &tight_aabb);
     117              : 
     118              :     // Unregister an object. The handle is invalidated.
     119              :     void broad_phase_remove(BroadPhaseState &bp, BPHandle h);
     120              : 
     121              :     // Notify that an object has moved. Internally checks whether the tight AABB
     122              :     // is still contained in the fat AABB; only triggers a re-sort if it escaped.
     123              :     void broad_phase_move(BroadPhaseState &bp,
     124              :                           BPHandle         h,
     125              :                           const AABB      &new_tight_aabb);
     126              : 
     127              :     // Run the broad phase. Fills bp.pairs with all potentially-colliding pairs.
     128              :     // Call once per frame after all broad_phase_move() calls.
     129              :     void broad_phase_update(BroadPhaseState &bp);
     130              : 
     131              :     // Convenience: compute an AABB from a shape + transform and call broad_phase_move.
     132              :     inline void broad_phase_move_shape(BroadPhaseState &bp,
     133              :                                        BPHandle         h,
     134              :                                        const Shape     &shape,
     135              :                                        const m3d::tf   &tf)
     136              :     {
     137              :         broad_phase_move(bp, h, compute_aabb(shape, tf));
     138              :     }
     139              : 
     140              :     // -------------------------------------------------------------------------
     141              :     //  Velocity-expanded move (anti-tunnelling)
     142              :     //
     143              :     //  Expands the AABB *asymmetrically* per axis based on linear velocity and
     144              :     //  the physics timestep. Ensures the stored fat AABB covers the entire volume
     145              :     //  the object will sweep through before the next broad_phase_update() call.
     146              :     //
     147              :     //  Per-axis rule:
     148              :     //    vel > 0  →  max side expands by vel*dt,    min side by fat_margin
     149              :     //    vel < 0  →  min side expands by |vel|*dt,  max side by fat_margin
     150              :     //    vel == 0 →  both sides expand by fat_margin  (same as standard move)
     151              :     //
     152              :     //  Temporal coherence is preserved: broad_phase_move() is still called
     153              :     //  internally, so re-sorts are skipped if the swept AABB fits inside the
     154              :     //  previously stored fat_aabb.
     155              :     //
     156              :     //  Usage:
     157              :     //    broad_phase_move_swept(bp, handle, tight_aabb, body.linear_vel, dt);
     158              :     // -------------------------------------------------------------------------
     159         1179 :     inline void broad_phase_move_swept(BroadPhaseState &bp,
     160              :                                        BPHandle         h,
     161              :                                        const AABB      &tight_aabb,
     162              :                                        const m3d::vec3 &linear_velocity,
     163              :                                        m3d::scalar      dt)
     164              :     {
     165         1179 :         const m3d::scalar margin = bp.config.fat_margin;
     166         1179 :         const m3d::vec3   dv     = linear_velocity * dt;
     167              : 
     168              :         // Expand min side where velocity is negative, max side where positive.
     169              :         const m3d::vec3 expand_min(
     170         1179 :             margin + (dv.x < 0 ? -dv.x : m3d::scalar(0)),
     171         1179 :             margin + (dv.y < 0 ? -dv.y : m3d::scalar(0)),
     172         3537 :             margin + (dv.z < 0 ? -dv.z : m3d::scalar(0)));
     173              : 
     174              :         const m3d::vec3 expand_max(
     175         1179 :             margin + (dv.x > 0 ?  dv.x : m3d::scalar(0)),
     176         1179 :             margin + (dv.y > 0 ?  dv.y : m3d::scalar(0)),
     177         3537 :             margin + (dv.z > 0 ?  dv.z : m3d::scalar(0)));
     178              : 
     179         2358 :         broad_phase_move(bp, h, AABB{tight_aabb.min - expand_min,
     180         1179 :                                      tight_aabb.max + expand_max});
     181         1179 :     }
     182              : 
     183              :     // Convenience: shape + transform + velocity.
     184              :     inline void broad_phase_move_shape_swept(BroadPhaseState &bp,
     185              :                                               BPHandle         h,
     186              :                                               const Shape     &shape,
     187              :                                               const m3d::tf   &tf,
     188              :                                               const m3d::vec3 &linear_velocity,
     189              :                                               m3d::scalar      dt)
     190              :     {
     191              :         broad_phase_move_swept(bp, h, compute_aabb(shape, tf), linear_velocity, dt);
     192              :     }
     193              : 
     194              :     // -------------------------------------------------------------------------
     195              :     //  Internal helpers (exposed for testing; not part of the public API)
     196              :     // -------------------------------------------------------------------------
     197              : 
     198              :     // Insertion sort — O(n) when nearly sorted (the common case each frame).
     199              :     void bp_insertion_sort(std::vector<SAPEndpoint> &endpoints);
     200              : 
     201              :     // Sweep the sorted endpoint list and fill bp.pairs.
     202              :     void bp_sweep(BroadPhaseState &bp);
     203              : 
     204              : } // namespace rbc
        

Generated by: LCOV version 2.0-1