Line data Source code
1 : #pragma once
2 : #include "rbc/Dispatcher.hpp"
3 : #include <algorithm>
4 : #include <array>
5 : #include <limits>
6 :
7 : // ============================================================================
8 : // Box vs Box — SAT + Sutherland-Hodgman contact manifold
9 : //
10 : // Pipeline:
11 : // 1. SAT over 15 axes → find minimum-overlap axis (the contact normal).
12 : // 2. Identify the REFERENCE face (on A, most aligned with normal) and the
13 : // INCIDENT face (on B, most anti-aligned with normal).
14 : // 3. Clip the incident face polygon against the 4 side planes of the
15 : // reference face (Sutherland-Hodgman).
16 : // 4. Keep clipped points that are below the reference face plane (penetrating).
17 : // 5. Reduce to at most 4 contacts by keeping the extremal points.
18 : //
19 : // Result: ContactManifold with 1-4 points, all physically meaningful.
20 : // ============================================================================
21 :
22 : namespace rbc
23 : {
24 :
25 : namespace detail
26 : {
27 :
28 : // ---------------------------------------------------------------------------
29 : // Low-level geometry helpers
30 : // ---------------------------------------------------------------------------
31 :
32 : // Project OBB onto a world-space axis → half-extent (support radius).
33 172 : inline m3d::scalar obb_radius(const Box &box,
34 : const m3d::tf &tf,
35 : const m3d::vec3 &axis)
36 : {
37 172 : const m3d::vec3 ax = tf.rotate_vector(m3d::vec3(1, 0, 0));
38 172 : const m3d::vec3 ay = tf.rotate_vector(m3d::vec3(0, 1, 0));
39 172 : const m3d::vec3 az = tf.rotate_vector(m3d::vec3(0, 0, 1));
40 172 : return m3d::abs(m3d::dot(ax, axis)) * box.half_extents.x
41 172 : + m3d::abs(m3d::dot(ay, axis)) * box.half_extents.y
42 172 : + m3d::abs(m3d::dot(az, axis)) * box.half_extents.z;
43 : }
44 :
45 : // Test one SAT axis. Updates best_depth / best_normal if this is the
46 : // smallest penetration seen so far. Returns false → separating axis found.
47 107 : inline bool test_axis(const m3d::vec3 &axis,
48 : const Box &a, const m3d::tf &tf_a,
49 : const Box &b, const m3d::tf &tf_b,
50 : const m3d::vec3 ¢re_diff,
51 : m3d::scalar &best_depth,
52 : m3d::vec3 &best_normal,
53 : int feature_id, // which axis index
54 : int &best_feature)
55 : {
56 107 : const m3d::scalar len_sq = m3d::length_sq(axis);
57 107 : if (len_sq < m3d::EPSILON * m3d::EPSILON)
58 21 : return true; // degenerate (parallel edges) → skip but don't separate
59 :
60 86 : const m3d::vec3 norm_axis = axis / m3d::sqrt(len_sq);
61 86 : const m3d::scalar rA = obb_radius(a, tf_a, norm_axis);
62 86 : const m3d::scalar rB = obb_radius(b, tf_b, norm_axis);
63 86 : const m3d::scalar dist = m3d::abs(m3d::dot(centre_diff, norm_axis));
64 86 : const m3d::scalar overlap = rA + rB - dist;
65 :
66 86 : if (overlap < 0.0)
67 2 : return false;
68 :
69 84 : if (overlap < best_depth)
70 : {
71 9 : best_depth = overlap;
72 9 : best_normal = (m3d::dot(centre_diff, norm_axis) >= 0.0)
73 9 : ? norm_axis : -norm_axis;
74 9 : best_feature = feature_id;
75 : }
76 84 : return true;
77 : }
78 :
79 : // ---------------------------------------------------------------------------
80 : // Face helpers
81 : // ---------------------------------------------------------------------------
82 :
83 : // Return the 4 corners of the face on `box` (in world space) that is most
84 : // aligned with `dir` (i.e. the face whose outward normal is closest to dir).
85 14 : inline void get_best_face(const Box &box,
86 : const m3d::tf &tf,
87 : const m3d::vec3 &dir,
88 : m3d::vec3 out_corners[4],
89 : m3d::vec3 &out_normal)
90 : {
91 : // Local axes in world space
92 : const m3d::vec3 axes[3] = {
93 14 : tf.rotate_vector(m3d::vec3(1, 0, 0)),
94 14 : tf.rotate_vector(m3d::vec3(0, 1, 0)),
95 14 : tf.rotate_vector(m3d::vec3(0, 0, 1)),
96 42 : };
97 : const m3d::scalar half[3] = {
98 14 : box.half_extents.x,
99 14 : box.half_extents.y,
100 14 : box.half_extents.z,
101 14 : };
102 :
103 : // Find which face axis is most aligned with dir
104 14 : int best_axis = 0;
105 14 : m3d::scalar best_dot = m3d::abs(m3d::dot(axes[0], dir));
106 42 : for (int i = 1; i < 3; ++i)
107 : {
108 28 : m3d::scalar d = m3d::abs(m3d::dot(axes[i], dir));
109 28 : if (d > best_dot) { best_dot = d; best_axis = i; }
110 : }
111 :
112 : // Face normal (sign chosen so it points along dir)
113 14 : const m3d::scalar sign = (m3d::dot(axes[best_axis], dir) >= 0.0) ? 1.0 : -1.0;
114 14 : out_normal = axes[best_axis] * sign;
115 :
116 : // Face centre
117 14 : const m3d::vec3 face_centre = tf.pos + out_normal * half[best_axis];
118 :
119 : // The other two axes span the face
120 14 : const int u = (best_axis + 1) % 3;
121 14 : const int v = (best_axis + 2) % 3;
122 :
123 14 : out_corners[0] = face_centre + axes[u] * half[u] + axes[v] * half[v];
124 14 : out_corners[1] = face_centre - axes[u] * half[u] + axes[v] * half[v];
125 14 : out_corners[2] = face_centre - axes[u] * half[u] - axes[v] * half[v];
126 14 : out_corners[3] = face_centre + axes[u] * half[u] - axes[v] * half[v];
127 14 : }
128 :
129 : // ---------------------------------------------------------------------------
130 : // Sutherland-Hodgman clipping
131 : // ---------------------------------------------------------------------------
132 :
133 : // Clip polygon `poly` (size `n`) against a half-space defined by plane
134 : // normal `plane_n` and a point on the plane `plane_p`.
135 : // Points on the positive side of the plane are KEPT.
136 : // Returns the number of output vertices (written into `out`).
137 28 : inline int clip_polygon_by_plane(const m3d::vec3 *poly, int n,
138 : const m3d::vec3 &plane_n,
139 : const m3d::vec3 &plane_p,
140 : m3d::vec3 *out)
141 : {
142 28 : int out_n = 0;
143 200 : for (int i = 0; i < n; ++i)
144 : {
145 172 : const m3d::vec3 &A = poly[i];
146 172 : const m3d::vec3 &B = poly[(i + 1) % n];
147 :
148 172 : const m3d::scalar dA = m3d::dot(A - plane_p, plane_n);
149 172 : const m3d::scalar dB = m3d::dot(B - plane_p, plane_n);
150 :
151 172 : if (dA >= 0.0) // A inside
152 168 : out[out_n++] = A;
153 :
154 172 : if ((dA > 0.0) != (dB > 0.0)) // edge crosses plane
155 : {
156 44 : const m3d::scalar t = dA / (dA - dB);
157 44 : out[out_n++] = A + (B - A) * t;
158 : }
159 : }
160 28 : return out_n;
161 : }
162 :
163 : // ---------------------------------------------------------------------------
164 : // Manifold point reduction
165 : // ---------------------------------------------------------------------------
166 : // Keep at most 4 contact points from `pts[n]`:
167 : // 1. Deepest penetration point.
168 : // 2. Farthest from point 1.
169 : // 3. Farthest from line 1-2.
170 : // 4. Farthest from triangle 1-2-3 (area maximisation).
171 : // This gives a stable support polygon for the solver.
172 7 : inline int reduce_to_4(const m3d::vec3 *pts,
173 : const m3d::scalar *depths,
174 : int n,
175 : m3d::vec3 *out_pts,
176 : m3d::scalar *out_depths)
177 : {
178 7 : if (n <= 4)
179 : {
180 10 : for (int i = 0; i < n; ++i) { out_pts[i] = pts[i]; out_depths[i] = depths[i]; }
181 2 : return n;
182 : }
183 :
184 : // 1. Deepest point
185 5 : int i0 = 0;
186 60 : for (int i = 1; i < n; ++i)
187 55 : if (depths[i] > depths[i0]) i0 = i;
188 :
189 : // 2. Farthest from i0
190 5 : int i1 = (i0 == 0) ? 1 : 0;
191 5 : m3d::scalar best = m3d::length_sq(pts[i1] - pts[i0]);
192 65 : for (int i = 0; i < n; ++i)
193 : {
194 60 : if (i == i0) continue;
195 55 : m3d::scalar d = m3d::length_sq(pts[i] - pts[i0]);
196 55 : if (d > best) { best = d; i1 = i; }
197 : }
198 :
199 : // 3. Farthest from segment i0-i1
200 5 : int i2 = -1;
201 5 : best = 0.0;
202 5 : const m3d::vec3 seg = pts[i1] - pts[i0];
203 65 : for (int i = 0; i < n; ++i)
204 : {
205 60 : if (i == i0 || i == i1) continue;
206 50 : m3d::scalar d = m3d::length_sq(m3d::cross(pts[i] - pts[i0], seg));
207 50 : if (d > best) { best = d; i2 = i; }
208 : }
209 5 : if (i2 < 0) { i2 = 0; if (i2 == i0 || i2 == i1) i2 = 1; if (i2 == i0 || i2 == i1) i2 = 2; }
210 :
211 : // 4. Farthest from triangle i0-i1-i2 (signed area)
212 5 : int i3 = -1;
213 5 : best = 0.0;
214 5 : const m3d::vec3 tri_n = m3d::cross(pts[i1] - pts[i0], pts[i2] - pts[i0]);
215 65 : for (int i = 0; i < n; ++i)
216 : {
217 60 : if (i == i0 || i == i1 || i == i2) continue;
218 45 : m3d::scalar d = m3d::abs(m3d::dot(pts[i] - pts[i0], tri_n));
219 45 : if (d > best) { best = d; i3 = i; }
220 : }
221 10 : if (i3 < 0) { for (int i = 0; i < n; ++i) if (i!=i0&&i!=i1&&i!=i2){i3=i;break;} }
222 :
223 5 : out_pts[0] = pts[i0]; out_depths[0] = depths[i0];
224 5 : out_pts[1] = pts[i1]; out_depths[1] = depths[i1];
225 5 : out_pts[2] = pts[i2]; out_depths[2] = depths[i2];
226 5 : if (i3 >= 0) { out_pts[3] = pts[i3]; out_depths[3] = depths[i3]; return 4; }
227 0 : return 3;
228 : }
229 :
230 : } // namespace detail
231 :
232 : // ===========================================================================
233 : // CollisionAlgorithm<Box, Box>
234 : // ===========================================================================
235 : template <>
236 : struct CollisionAlgorithm<Box, Box>
237 : {
238 9 : static bool test(const Box &a, const m3d::tf &tf_a,
239 : const Box &b, const m3d::tf &tf_b,
240 : ContactManifold &manifold)
241 : {
242 9 : const m3d::vec3 centre_diff = tf_b.pos - tf_a.pos;
243 :
244 : // World-space local axes of A and B
245 : const m3d::vec3 ax[3] = {
246 9 : tf_a.rotate_vector(m3d::vec3(1,0,0)),
247 9 : tf_a.rotate_vector(m3d::vec3(0,1,0)),
248 9 : tf_a.rotate_vector(m3d::vec3(0,0,1)),
249 27 : };
250 : const m3d::vec3 bx[3] = {
251 9 : tf_b.rotate_vector(m3d::vec3(1,0,0)),
252 9 : tf_b.rotate_vector(m3d::vec3(0,1,0)),
253 9 : tf_b.rotate_vector(m3d::vec3(0,0,1)),
254 27 : };
255 :
256 9 : m3d::scalar best_depth = std::numeric_limits<m3d::scalar>::max();
257 9 : m3d::vec3 best_normal = m3d::vec3(1, 0, 0);
258 9 : int best_feature = 0; // 0-5 = face axes, 6-14 = edge-edge axes
259 :
260 9 : int feat = 0;
261 :
262 : // 3 face axes of A
263 30 : for (int i = 0; i < 3; ++i, ++feat)
264 23 : if (!detail::test_axis(ax[i], a, tf_a, b, tf_b,
265 : centre_diff, best_depth, best_normal,
266 : feat, best_feature))
267 2 : return false;
268 :
269 : // 3 face axes of B
270 28 : for (int i = 0; i < 3; ++i, ++feat)
271 21 : if (!detail::test_axis(bx[i], a, tf_a, b, tf_b,
272 : centre_diff, best_depth, best_normal,
273 : feat, best_feature))
274 0 : return false;
275 :
276 : // 9 edge cross-product axes
277 28 : for (int i = 0; i < 3; ++i)
278 84 : for (int j = 0; j < 3; ++j, ++feat)
279 63 : if (!detail::test_axis(m3d::cross(ax[i], bx[j]),
280 : a, tf_a, b, tf_b,
281 : centre_diff, best_depth, best_normal,
282 : feat, best_feature))
283 0 : return false;
284 :
285 : // ── All 15 axes passed — we have a collision ─────────────────────────
286 :
287 7 : manifold.normal = best_normal;
288 7 : manifold.num_points = 0;
289 :
290 : // ── Face-face contact (features 0-5): use Sutherland-Hodgman clipping
291 7 : if (best_feature < 6)
292 : {
293 : // Reference face: on A, most aligned with the contact normal
294 : // Incident face: on B, most anti-aligned with the contact normal
295 35 : m3d::vec3 ref_corners[4], ref_normal;
296 35 : m3d::vec3 inc_corners[4], inc_normal;
297 :
298 7 : detail::get_best_face(a, tf_a, best_normal, ref_corners, ref_normal);
299 7 : detail::get_best_face(b, tf_b, -best_normal, inc_corners, inc_normal);
300 :
301 : // Sutherland-Hodgman: clip incident face against 4 side planes of reference face
302 : // The 4 side planes are perpendicular to the reference face and pass through its edges.
303 : // side normal for edge (ref_corners[i] → ref_corners[(i+1)%4]) is
304 : // cross(ref_normal, edge_dir) (pointing inward)
305 :
306 : // Working buffers (max 8 vertices after each clip)
307 231 : m3d::vec3 buf0[16], buf1[16];
308 7 : int cnt = 4;
309 35 : for (int i = 0; i < 4; ++i) buf0[i] = inc_corners[i];
310 :
311 35 : for (int i = 0; i < 4; ++i)
312 : {
313 28 : const m3d::vec3 edge = ref_corners[(i+1)%4] - ref_corners[i];
314 28 : const m3d::vec3 side_n = m3d::normalize(m3d::cross(ref_normal, edge)); // inward
315 28 : cnt = detail::clip_polygon_by_plane(buf0, cnt, side_n, ref_corners[i], buf1);
316 28 : if (cnt == 0) break;
317 240 : for (int k = 0; k < cnt; ++k) buf0[k] = buf1[k];
318 : }
319 :
320 7 : if (cnt == 0) return false; // clipped away entirely
321 :
322 : // Keep only points that penetrate the reference face plane
323 : // (i.e. on the negative side of ref_normal through ref_corners[0])
324 7 : const m3d::scalar ref_d = m3d::dot(ref_corners[0], ref_normal);
325 :
326 119 : m3d::vec3 keep_pts[16];
327 : m3d::scalar keep_dep[16];
328 7 : int keep_n = 0;
329 :
330 75 : for (int i = 0; i < cnt; ++i)
331 : {
332 68 : const m3d::scalar signed_dist = m3d::dot(buf0[i], ref_normal) - ref_d;
333 68 : if (signed_dist <= m3d::EPSILON) // on or below the reference face
334 : {
335 68 : keep_pts[keep_n] = buf0[i];
336 68 : keep_dep[keep_n] = -signed_dist + best_depth * 0.0; // penetration ≈ depth of this point
337 : // More precise per-point depth: project along normal onto box surfaces
338 68 : keep_dep[keep_n] = m3d::abs(signed_dist) > 0.0
339 68 : ? m3d::abs(signed_dist)
340 : : best_depth;
341 68 : ++keep_n;
342 : }
343 : }
344 :
345 7 : if (keep_n == 0)
346 : {
347 : // Fall back: single point at the box-contact surface midpoint
348 0 : manifold.num_points = 1;
349 0 : manifold.points[0].penetration_depth = best_depth;
350 : manifold.points[0].position = tf_a.pos
351 0 : + tf_a.rotate_vector(m3d::vec3(0,0,0)) // placeholder
352 0 : - best_normal * (best_depth * 0.5);
353 0 : return true;
354 : }
355 :
356 : // Reduce to ≤4 contact points
357 35 : m3d::vec3 red_pts[4];
358 : m3d::scalar red_dep[4];
359 7 : int red_n = detail::reduce_to_4(keep_pts, keep_dep, keep_n, red_pts, red_dep);
360 :
361 7 : manifold.num_points = red_n;
362 35 : for (int i = 0; i < red_n; ++i)
363 : {
364 28 : manifold.points[i].position = red_pts[i];
365 28 : manifold.points[i].penetration_depth = best_depth; // uniform for face-face
366 : }
367 : }
368 : else
369 : {
370 : // ── Edge-edge contact (features 6-14): single contact point ──────
371 : // For edge-edge the closest points on both edges form the contact.
372 : // We compute them via the standard segment-closest-point formula.
373 :
374 0 : const int ei = (best_feature - 6) / 3; // edge axis index on A (0,1,2)
375 0 : const int ej = (best_feature - 6) % 3; // edge axis index on B (0,1,2)
376 :
377 : // Support points on each box along the edge direction
378 0 : const m3d::vec3 pa = tf_a.pos + tf_a.rotate_vector(
379 0 : m3d::vec3(
380 0 : (m3d::dot(ax[0], best_normal) >= 0 ? 1.0 : -1.0) * (ei==0 ? 0.0 : a.half_extents.x),
381 0 : (m3d::dot(ax[1], best_normal) >= 0 ? 1.0 : -1.0) * (ei==1 ? 0.0 : a.half_extents.y),
382 0 : (m3d::dot(ax[2], best_normal) >= 0 ? 1.0 : -1.0) * (ei==2 ? 0.0 : a.half_extents.z)
383 0 : ));
384 0 : const m3d::vec3 pb = tf_b.pos + tf_b.rotate_vector(
385 0 : m3d::vec3(
386 0 : (m3d::dot(bx[0],-best_normal) >= 0 ? 1.0 : -1.0) * (ej==0 ? 0.0 : b.half_extents.x),
387 0 : (m3d::dot(bx[1],-best_normal) >= 0 ? 1.0 : -1.0) * (ej==1 ? 0.0 : b.half_extents.y),
388 0 : (m3d::dot(bx[2],-best_normal) >= 0 ? 1.0 : -1.0) * (ej==2 ? 0.0 : b.half_extents.z)
389 0 : ));
390 :
391 : // Closest point between the two edges (midpoint approximation)
392 0 : const m3d::vec3 contact_pt = (pa + pb) * 0.5;
393 :
394 0 : manifold.num_points = 1;
395 0 : manifold.points[0].position = contact_pt;
396 0 : manifold.points[0].penetration_depth = best_depth;
397 : }
398 :
399 7 : return true;
400 : }
401 : };
402 :
403 : } // namespace rbc
|
