Line data Source code
1 : #pragma once
2 : #include "rbc/shapes/ShapeTypes.hpp"
3 : #include "rbc/Contact.hpp"
4 : #include <math3d/math3d.hpp>
5 :
6 : namespace rbc
7 : {
8 :
9 : // ── Sphere vs Box ────────────────────────────────────────────────────────────
10 : // Strategy: find the closest point on the OBB surface to the sphere centre.
11 : // If that distance < radius, we have a collision.
12 : //
13 : // Normal convention: A→B (sphere → box)
14 : // The XPBD solver computes delta_lambda = -penetration / w (negative)
15 : // and applies impulse = delta_lambda * normal to body A (sphere).
16 : // For the sphere to be pushed AWAY from the box, impulse must point
17 : // away from the box, so delta_lambda * normal.toward_box = positive,
18 : // which requires normal to point from A (sphere) toward B (box).
19 : //
20 : // All work is done in the Box's local space to keep the clamping simple,
21 : // then results are transformed back to world space.
22 :
23 : template <>
24 : struct CollisionAlgorithm<Sphere, Box>
25 : {
26 8963 : static bool test(const Sphere &sphere, const m3d::tf &tf_sphere,
27 : const Box &box, const m3d::tf &tf_box,
28 : ContactManifold &out)
29 : {
30 : // Sphere centre expressed in the box's local frame.
31 : const m3d::vec3 local_centre =
32 8963 : tf_box.inverse_rotate_vector(tf_sphere.pos - tf_box.pos);
33 :
34 : // Closest point on the box AABB (local space) to the sphere centre.
35 : const m3d::vec3 closest(
36 8963 : m3d::clamp(local_centre.x, -box.half_extents.x, box.half_extents.x),
37 8963 : m3d::clamp(local_centre.y, -box.half_extents.y, box.half_extents.y),
38 8963 : m3d::clamp(local_centre.z, -box.half_extents.z, box.half_extents.z));
39 :
40 8963 : const m3d::vec3 local_diff = local_centre - closest; // B→A (box surface → sphere)
41 8963 : const m3d::scalar dist = m3d::length(local_diff);
42 :
43 : // ── Case 1: sphere centre is OUTSIDE the box ──────────────────────
44 8963 : if (dist > m3d::EPSILON)
45 : {
46 8961 : if (dist >= sphere.radius)
47 5304 : return false; // separated
48 :
49 : // local_diff / dist points B→A.
50 : // Negate for the required A→B convention (sphere → box).
51 3657 : out.num_points = 1;
52 3657 : out.normal = -tf_box.rotate_vector(local_diff / dist);
53 3657 : out.points[0].penetration_depth = sphere.radius - dist;
54 3657 : out.points[0].position = tf_box.transform_point(closest); // box surface contact
55 3657 : return true;
56 : }
57 :
58 : // ── Case 2: sphere centre is INSIDE the box ───────────────────────
59 : // Find the face whose penetration is smallest — that is the exit direction.
60 : // Penetration along each face axis = half_extent − |local_coord|
61 2 : const m3d::scalar px = box.half_extents.x - m3d::abs(local_centre.x);
62 2 : const m3d::scalar py = box.half_extents.y - m3d::abs(local_centre.y);
63 2 : const m3d::scalar pz = box.half_extents.z - m3d::abs(local_centre.z);
64 :
65 2 : m3d::vec3 local_normal; // outward face normal in box local space (B→A)
66 2 : if (px <= py && px <= pz)
67 1 : local_normal = m3d::vec3(local_centre.x > 0 ? 1.0 : -1.0, 0.0, 0.0);
68 1 : else if (py <= px && py <= pz)
69 1 : local_normal = m3d::vec3(0.0, local_centre.y > 0 ? 1.0 : -1.0, 0.0);
70 : else
71 0 : local_normal = m3d::vec3(0.0, 0.0, local_centre.z > 0 ? 1.0 : -1.0);
72 :
73 : // Negate outward face normal (B→A) → A→B convention for the solver.
74 2 : out.num_points = 1;
75 2 : out.normal = -tf_box.rotate_vector(local_normal);
76 2 : out.points[0].penetration_depth = sphere.radius + m3d::min(px, m3d::min(py, pz));
77 :
78 : // Contact point: closest point on the exit face to the sphere centre.
79 : // The exit-face axis is fixed by local_normal (one component is ±1, rest 0).
80 : // Along that axis use the face position; along the other axes clamp to box.
81 : const m3d::vec3 face_point(
82 2 : local_normal.x != 0.0
83 1 : ? local_normal.x * box.half_extents.x
84 1 : : m3d::clamp(local_centre.x, -box.half_extents.x, box.half_extents.x),
85 2 : local_normal.y != 0.0
86 1 : ? local_normal.y * box.half_extents.y
87 1 : : m3d::clamp(local_centre.y, -box.half_extents.y, box.half_extents.y),
88 2 : local_normal.z != 0.0
89 0 : ? local_normal.z * box.half_extents.z
90 4 : : m3d::clamp(local_centre.z, -box.half_extents.z, box.half_extents.z));
91 :
92 2 : out.points[0].position = tf_box.transform_point(face_point);
93 2 : return true;
94 : }
95 : };
96 :
97 : // ── Box vs Sphere — symmetric shim ───────────────────────────────────────────
98 : template <>
99 : struct CollisionAlgorithm<Box, Sphere>
100 : : CollisionAlgorithmSym<Box, Sphere>
101 : {
102 : };
103 :
104 : } // namespace rbc
|
