Line data Source code
1 : #pragma once
2 : // ============================================================================
3 : // ContactManifoldGenerator.hpp
4 : //
5 : // Given an EPA result (one point, one normal, one depth) this generates a
6 : // proper 1-4 point manifold by:
7 : //
8 : // 1. Finding the REFERENCE face (most aligned with normal) on shape A.
9 : // 2. Finding the INCIDENT face (most anti-aligned) on shape B.
10 : // 3. Clipping the incident face polygon against the reference face's side
11 : // planes (Sutherland-Hodgman), then depth-testing against the reference
12 : // face plane.
13 : // 4. Reducing to ≤4 contact points.
14 : //
15 : // For primitive pairs that have an analytic fast-path (SphereSphere,
16 : // SphereBox, etc.) the analytic algorithm should fill the manifold directly
17 : // and not call this generator.
18 : //
19 : // This is used by the GJK/EPA *fallback* path in CollisionAlgorithm<A,B>.
20 : // ============================================================================
21 :
22 : #include "rbc/Contact.hpp"
23 : #include "rbc/gjk/GJK.hpp"
24 : #include "rbc/gjk/EPA.hpp"
25 : #include "rbc/gjk/MinkowskiDiff.hpp"
26 : #include "rbc/shapes/ShapeTypes.hpp"
27 : #include <cstring>
28 : #include <algorithm>
29 :
30 : namespace rbc
31 : {
32 :
33 : // ---------------------------------------------------------------------------
34 : // Sutherland-Hodgman clip (shared with BoxBox.hpp — defined here as inline
35 : // so there is no ODR violation; BoxBox.hpp has its own copy in detail::)
36 : // ---------------------------------------------------------------------------
37 :
38 : namespace manifold_detail
39 : {
40 :
41 0 : inline int clip_polygon_by_plane(const m3d::vec3 *poly, int n,
42 : const m3d::vec3 &plane_n,
43 : const m3d::vec3 &plane_p,
44 : m3d::vec3 *out)
45 : {
46 0 : int out_n = 0;
47 0 : for (int i = 0; i < n; ++i)
48 : {
49 0 : const m3d::vec3 &A = poly[i];
50 0 : const m3d::vec3 &B = poly[(i + 1) % n];
51 :
52 0 : const m3d::scalar dA = m3d::dot(A - plane_p, plane_n);
53 0 : const m3d::scalar dB = m3d::dot(B - plane_p, plane_n);
54 :
55 0 : if (dA >= 0.0)
56 0 : out[out_n++] = A;
57 :
58 0 : if ((dA > 0.0) != (dB > 0.0))
59 : {
60 0 : const m3d::scalar t = dA / (dA - dB);
61 0 : out[out_n++] = A + (B - A) * t;
62 : }
63 : }
64 0 : return out_n;
65 : }
66 :
67 : // Reduce n contact candidates to ≤4 by maximising the support area.
68 0 : inline int reduce_to_4(const m3d::vec3 *pts,
69 : const m3d::scalar *depths,
70 : int n,
71 : m3d::vec3 *out_pts,
72 : m3d::scalar *out_depths)
73 : {
74 0 : if (n <= 4)
75 : {
76 0 : for (int i = 0; i < n; ++i)
77 : {
78 0 : out_pts[i] = pts[i];
79 0 : out_depths[i] = depths[i];
80 : }
81 0 : return n;
82 : }
83 :
84 : // 1. Deepest
85 0 : int i0 = 0;
86 0 : for (int i = 1; i < n; ++i)
87 0 : if (depths[i] > depths[i0])
88 0 : i0 = i;
89 :
90 : // 2. Farthest from i0
91 0 : int i1 = (i0 == 0) ? 1 : 0;
92 0 : m3d::scalar best = m3d::length_sq(pts[i1] - pts[i0]);
93 0 : for (int i = 0; i < n; ++i)
94 : {
95 0 : if (i == i0)
96 0 : continue;
97 0 : m3d::scalar d = m3d::length_sq(pts[i] - pts[i0]);
98 0 : if (d > best)
99 : {
100 0 : best = d;
101 0 : i1 = i;
102 : }
103 : }
104 :
105 : // 3. Farthest from segment
106 0 : int i2 = -1;
107 0 : best = 0.0;
108 0 : const m3d::vec3 seg = pts[i1] - pts[i0];
109 0 : for (int i = 0; i < n; ++i)
110 : {
111 0 : if (i == i0 || i == i1)
112 0 : continue;
113 0 : m3d::scalar d = m3d::length_sq(m3d::cross(pts[i] - pts[i0], seg));
114 0 : if (d > best)
115 : {
116 0 : best = d;
117 0 : i2 = i;
118 : }
119 : }
120 0 : if (i2 < 0)
121 : {
122 0 : for (int i = 0; i < n; ++i)
123 0 : if (i != i0 && i != i1)
124 : {
125 0 : i2 = i;
126 0 : break;
127 : }
128 : }
129 :
130 : // 4. Farthest from triangle
131 0 : int i3 = -1;
132 0 : best = 0.0;
133 0 : if (i2 >= 0)
134 : {
135 0 : const m3d::vec3 tri_n = m3d::cross(pts[i1] - pts[i0], pts[i2] - pts[i0]);
136 0 : for (int i = 0; i < n; ++i)
137 : {
138 0 : if (i == i0 || i == i1 || i == i2)
139 0 : continue;
140 0 : m3d::scalar d = m3d::abs(m3d::dot(pts[i] - pts[i0], tri_n));
141 0 : if (d > best)
142 : {
143 0 : best = d;
144 0 : i3 = i;
145 : }
146 : }
147 0 : if (i3 < 0)
148 0 : for (int i = 0; i < n; ++i)
149 0 : if (i != i0 && i != i1 && i != i2)
150 : {
151 0 : i3 = i;
152 0 : break;
153 : }
154 : }
155 :
156 0 : out_pts[0] = pts[i0];
157 0 : out_depths[0] = depths[i0];
158 0 : out_pts[1] = pts[i1];
159 0 : out_depths[1] = depths[i1];
160 0 : if (i2 >= 0)
161 : {
162 0 : out_pts[2] = pts[i2];
163 0 : out_depths[2] = depths[i2];
164 : }
165 0 : if (i3 >= 0)
166 : {
167 0 : out_pts[3] = pts[i3];
168 0 : out_depths[3] = depths[i3];
169 0 : return 4;
170 : }
171 0 : return (i2 >= 0) ? 3 : 2;
172 : }
173 :
174 : // ---------------------------------------------------------------------------
175 : // Support-based polygon extraction for a generic shape.
176 : //
177 : // For convex shapes (Box, Capsule endpoints, Ellipsoid…) we approximate the
178 : // reference / incident "face" by sampling support points in a small disc
179 : // perpendicular to the contact normal and centered on the EPA support point.
180 : //
181 : // For BOX shapes we use exact face corners (identical to the BoxBox path).
182 : // Other shapes get a 4-point disc approximation — good enough for sphere
183 : // caps, cylinder ends, and ellipsoid poles.
184 : // ---------------------------------------------------------------------------
185 :
186 : // Return the 4 corners of the best face of a BOX.
187 0 : inline int get_box_face_corners(const Box &box,
188 : const m3d::tf &tf,
189 : const m3d::vec3 &dir,
190 : m3d::vec3 corners[4])
191 : {
192 : const m3d::vec3 axes[3] = {
193 0 : tf.rotate_vector(m3d::vec3(1, 0, 0)),
194 0 : tf.rotate_vector(m3d::vec3(0, 1, 0)),
195 0 : tf.rotate_vector(m3d::vec3(0, 0, 1)),
196 0 : };
197 0 : const m3d::scalar half[3] = {box.half_extents.x, box.half_extents.y, box.half_extents.z};
198 :
199 0 : int best = 0;
200 0 : m3d::scalar best_d = m3d::abs(m3d::dot(axes[0], dir));
201 0 : for (int i = 1; i < 3; ++i)
202 : {
203 0 : m3d::scalar d = m3d::abs(m3d::dot(axes[i], dir));
204 0 : if (d > best_d)
205 : {
206 0 : best_d = d;
207 0 : best = i;
208 : }
209 : }
210 :
211 0 : const m3d::scalar sign = (m3d::dot(axes[best], dir) >= 0.0) ? 1.0 : -1.0;
212 0 : const m3d::vec3 centre = tf.pos + axes[best] * (sign * half[best]);
213 0 : const int u = (best + 1) % 3, v = (best + 2) % 3;
214 :
215 0 : corners[0] = centre + axes[u] * half[u] + axes[v] * half[v];
216 0 : corners[1] = centre - axes[u] * half[u] + axes[v] * half[v];
217 0 : corners[2] = centre - axes[u] * half[u] - axes[v] * half[v];
218 0 : corners[3] = centre + axes[u] * half[u] - axes[v] * half[v];
219 0 : return 4;
220 : }
221 :
222 : // Generic fallback: a small disc of 4 points around the support point.
223 : // `support_pt` is the EPA support point on the shape surface.
224 : // `radius` is a representative "face radius" (half-extent, capsule radius…).
225 0 : inline int get_generic_face_corners(const m3d::vec3 &support_pt,
226 : const m3d::vec3 &normal,
227 : m3d::scalar radius,
228 : m3d::vec3 corners[4])
229 : {
230 : // Build a tangent frame
231 0 : m3d::vec3 t1, t2;
232 0 : if (m3d::abs(normal.x) < 0.9)
233 0 : t1 = m3d::normalize(m3d::cross(normal, m3d::vec3(1, 0, 0)));
234 : else
235 0 : t1 = m3d::normalize(m3d::cross(normal, m3d::vec3(0, 1, 0)));
236 0 : t2 = m3d::cross(normal, t1);
237 :
238 0 : const m3d::scalar r = radius * 0.5; // conservative
239 0 : corners[0] = support_pt + (t1 + t2) * r;
240 0 : corners[1] = support_pt + (-t1 + t2) * r;
241 0 : corners[2] = support_pt + (-t1 - t2) * r;
242 0 : corners[3] = support_pt + (t1 - t2) * r;
243 0 : return 4;
244 : }
245 :
246 : } // namespace manifold_detail
247 :
248 : // ---------------------------------------------------------------------------
249 : // Main entry point
250 : // ---------------------------------------------------------------------------
251 :
252 : // Expand a single-point EPA result into a full manifold.
253 : // `epa_normal` — penetration normal (A→B convention from EPA)
254 : // `epa_depth` — penetration depth
255 : // `epa_contact` — EPA contact point (barycentric on the Minkowski diff face)
256 : // `shape_a/b` — the two shapes
257 : // `tf_a/tf_b` — their world transforms
258 : //
259 : // The function fills `manifold` with 1-4 contact points.
260 0 : inline void generate_manifold(const m3d::vec3 &epa_normal,
261 : m3d::scalar epa_depth,
262 : const m3d::vec3 &epa_contact,
263 : const Shape &shape_a,
264 : const m3d::tf &tf_a,
265 : const Shape &shape_b,
266 : const m3d::tf &tf_b,
267 : ContactManifold &manifold)
268 : {
269 0 : manifold.normal = epa_normal;
270 0 : manifold.num_points = 0;
271 :
272 : // ── Determine reference and incident "face polygons" ──────────────────
273 : // We branch on shape type to get the best polygon representation.
274 :
275 0 : m3d::vec3 ref_corners[4], inc_corners[4];
276 0 : int ref_n = 0, inc_n = 0;
277 :
278 : // --- Reference face (shape A, in direction of normal) ---
279 0 : if (shape_a.type == ShapeType::Box)
280 : {
281 0 : ref_n = manifold_detail::get_box_face_corners(
282 0 : shape_a.box, tf_a, epa_normal, ref_corners);
283 : }
284 : else
285 : {
286 : // Generic: find the support point of A in the normal direction,
287 : // then build a disc around it.
288 0 : const m3d::vec3 local_dir = tf_a.inverse_rotate_vector(epa_normal);
289 0 : const m3d::vec3 sup_local = shape_support(shape_a, local_dir);
290 0 : const m3d::vec3 sup_world = tf_a.transform_point(sup_local);
291 :
292 : // Representative radius: largest half-extent or capsule radius
293 0 : m3d::scalar r = 0.1; // fallback
294 0 : if (shape_a.type == ShapeType::Sphere)
295 0 : r = shape_a.sphere.radius;
296 0 : if (shape_a.type == ShapeType::Capsule)
297 0 : r = shape_a.capsule.radius;
298 0 : if (shape_a.type == ShapeType::Ellipsoid)
299 0 : r = m3d::max(shape_a.ellipsoid.half_extents.x,
300 0 : m3d::max(shape_a.ellipsoid.half_extents.y,
301 0 : shape_a.ellipsoid.half_extents.z));
302 :
303 0 : ref_n = manifold_detail::get_generic_face_corners(
304 : sup_world, epa_normal, r, ref_corners);
305 : }
306 :
307 : // --- Incident face (shape B, in direction of -normal) ---
308 0 : if (shape_b.type == ShapeType::Box)
309 : {
310 0 : inc_n = manifold_detail::get_box_face_corners(
311 0 : shape_b.box, tf_b, -epa_normal, inc_corners);
312 : }
313 : else
314 : {
315 0 : const m3d::vec3 local_dir = tf_b.inverse_rotate_vector(-epa_normal);
316 0 : const m3d::vec3 sup_local = shape_support(shape_b, local_dir);
317 0 : const m3d::vec3 sup_world = tf_b.transform_point(sup_local);
318 :
319 0 : m3d::scalar r = 0.1;
320 0 : if (shape_b.type == ShapeType::Sphere)
321 0 : r = shape_b.sphere.radius;
322 0 : if (shape_b.type == ShapeType::Capsule)
323 0 : r = shape_b.capsule.radius;
324 0 : if (shape_b.type == ShapeType::Ellipsoid)
325 0 : r = m3d::max(shape_b.ellipsoid.half_extents.x,
326 0 : m3d::max(shape_b.ellipsoid.half_extents.y,
327 0 : shape_b.ellipsoid.half_extents.z));
328 :
329 0 : inc_n = manifold_detail::get_generic_face_corners(
330 0 : sup_world, -epa_normal, r, inc_corners);
331 : }
332 :
333 : // ── Sutherland-Hodgman: clip incident polygon against reference face ──
334 : // Build the reference face normal (outward = epa_normal direction)
335 : // The reference face plane passes through ref_corners[0].
336 0 : const m3d::vec3 ref_face_n = epa_normal; // outward normal of reference face
337 0 : const m3d::scalar ref_d = m3d::dot(ref_corners[0], ref_face_n);
338 :
339 : // For the side planes we need to know the face "tangent frame".
340 : // We approximate with the two axes perpendicular to ref_face_n.
341 : // Use the winding of ref_corners to build edge normals.
342 0 : m3d::vec3 buf0[16], buf1[16];
343 0 : int cnt = inc_n;
344 0 : for (int i = 0; i < inc_n; ++i)
345 0 : buf0[i] = inc_corners[i];
346 :
347 0 : for (int i = 0; i < ref_n && cnt > 0; ++i)
348 : {
349 0 : const m3d::vec3 edge = ref_corners[(i + 1) % ref_n] - ref_corners[i];
350 0 : const m3d::vec3 side_n = m3d::normalize(m3d::cross(ref_face_n, edge));
351 0 : cnt = manifold_detail::clip_polygon_by_plane(
352 0 : buf0, cnt, side_n, ref_corners[i], buf1);
353 0 : for (int k = 0; k < cnt; ++k)
354 0 : buf0[k] = buf1[k];
355 : }
356 :
357 : // Keep only points that are on or behind the reference face plane
358 0 : m3d::vec3 keep_pts[16];
359 : m3d::scalar keep_dep[16];
360 0 : int keep_n = 0;
361 :
362 0 : for (int i = 0; i < cnt; ++i)
363 : {
364 0 : const m3d::scalar sd = m3d::dot(buf0[i], ref_face_n) - ref_d;
365 0 : if (sd <= m3d::EPSILON) // on or below reference face
366 : {
367 0 : keep_pts[keep_n] = buf0[i];
368 : // Penetration depth for this point along the normal.
369 : // A rough estimate: use the global EPA depth (conservative).
370 : // More precise: distance from the point to shape A's reference face.
371 0 : keep_dep[keep_n] = (sd < 0.0) ? (-sd) : epa_depth;
372 0 : ++keep_n;
373 : }
374 : }
375 :
376 : // Fall back to the single EPA contact point if clipping gave nothing
377 0 : if (keep_n == 0)
378 : {
379 0 : manifold.num_points = 1;
380 0 : manifold.points[0].position = epa_contact;
381 0 : manifold.points[0].penetration_depth = epa_depth;
382 0 : return;
383 : }
384 :
385 : // Reduce to ≤4
386 0 : m3d::vec3 red_pts[4];
387 : m3d::scalar red_dep[4];
388 0 : int red_n = manifold_detail::reduce_to_4(
389 : keep_pts, keep_dep, keep_n, red_pts, red_dep);
390 :
391 0 : manifold.num_points = red_n;
392 0 : for (int i = 0; i < red_n; ++i)
393 : {
394 0 : manifold.points[i].position = red_pts[i];
395 0 : manifold.points[i].penetration_depth = red_dep[i];
396 : }
397 : }
398 :
399 : // ---------------------------------------------------------------------------
400 : // Convenience: run GJK → EPA → generate_manifold in one call.
401 : // This is what the primary CollisionAlgorithm<A,B> template should call
402 : // instead of filling manifold manually.
403 : // ---------------------------------------------------------------------------
404 0 : inline bool gjk_epa_manifold(const Shape &sa,
405 : const m3d::tf &tf_a,
406 : const Shape &sb,
407 : const m3d::tf &tf_b,
408 : ContactManifold &manifold)
409 : {
410 0 : MinkowskiDiff md(&sa, &sb, tf_a, tf_b);
411 :
412 0 : m3d::vec3 guess = tf_b.pos - tf_a.pos;
413 0 : if (m3d::length_sq(guess) < m3d::EPSILON)
414 0 : guess = m3d::vec3(1.0, 0.0, 0.0);
415 :
416 0 : GJK gjk;
417 0 : if (gjk.evaluate(md, guess) != GJK::Inside)
418 0 : return false;
419 :
420 0 : EPA epa;
421 0 : if (epa.evaluate(gjk, md) != EPA::Valid)
422 0 : return false;
423 :
424 0 : generate_manifold(epa.normal, epa.depth, epa.contact_point,
425 : sa, tf_a, sb, tf_b, manifold);
426 0 : return true;
427 0 : }
428 :
429 : } // namespace rbc
|
