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
|