repose_collision/
shapes.rs

1//! Sphere and capsule collision primitives and exact signed-distance queries.
2
3use bincode::Decode;
4use bincode::Encode;
5use cu_spatial_payloads::Transform3D;
6use cu29::prelude::*;
7use cu29::units::si::f64::Length as Length64;
8use cu29::units::si::length::meter;
9use serde::Deserialize;
10use serde::Serialize;
11
12/// 3D sphere primitive defined by a center point and radius.
13#[derive(
14    Default, Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect,
15)]
16pub struct CuSphere {
17    pub center: [Length64; 3],
18    pub radius: Length64,
19}
20
21impl CuSphere {
22    /// Creates a sphere, rejecting non-finite coordinates and negative or non-finite radii.
23    pub fn new(center: [Length64; 3], radius: Length64) -> CuResult<Self> {
24        let sphere = Self { center, radius };
25        CuCollisionShape::Sphere(sphere).validate()?;
26        Ok(sphere)
27    }
28}
29
30/// 3D capsule (swept sphere along line segment `a` -> `b`) with radius.
31#[derive(
32    Default, Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect,
33)]
34pub struct CuCapsule {
35    pub a: [Length64; 3],
36    pub b: [Length64; 3],
37    pub radius: Length64,
38}
39
40impl CuCapsule {
41    /// Creates a capsule, rejecting non-finite endpoints and negative or non-finite radii.
42    pub fn new(a: [Length64; 3], b: [Length64; 3], radius: Length64) -> CuResult<Self> {
43        let capsule = Self { a, b, radius };
44        CuCollisionShape::Capsule(capsule).validate()?;
45        Ok(capsule)
46    }
47}
48
49/// Convex collision shape primitive (`Sphere` or `Capsule`).
50///
51/// RON: `{"sphere": {"center": [x, y, z], "radius": r}}` or
52/// `{"capsule": {"a": [x, y, z], "b": [x, y, z], "radius": r}}`, in meters.
53#[derive(Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect)]
54#[serde(rename_all = "snake_case")]
55pub enum CuCollisionShape {
56    Sphere(CuSphere),
57    Capsule(CuCapsule),
58}
59
60impl Default for CuCollisionShape {
61    fn default() -> Self {
62        Self::Sphere(CuSphere::default())
63    }
64}
65
66impl CuCollisionShape {
67    /// Checks that all coordinates are finite and the radius is finite and non-negative.
68    pub fn validate(&self) -> CuResult<()> {
69        let finite3 = |p: &[Length64; 3]| p.iter().all(|v| v.get::<meter>().is_finite());
70        let (finite, radius) = match self {
71            Self::Sphere(s) => (finite3(&s.center), s.radius),
72            Self::Capsule(c) => (finite3(&c.a) && finite3(&c.b), c.radius),
73        };
74        let r = radius.get::<meter>();
75        if finite && r.is_finite() && r >= 0.0 {
76            Ok(())
77        } else {
78            Err(CuError::from(
79                "repose_collision: invalid or non-finite collision shape geometry",
80            ))
81        }
82    }
83}
84
85/// A robot collision shape and the frame it is expressed in.
86///
87/// `frame` is a joint-frame index (`0..N`, as produced by
88/// `CuKinematicChain::forward_frames`) or `None` for the robot base frame.
89///
90/// RON: `{"shape": {"sphere": {...}}, "frame": 2}`; omit `frame` for the base frame.
91#[derive(
92    Debug, Clone, Copy, Default, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect,
93)]
94#[serde(deny_unknown_fields)]
95pub struct CuLinkShape {
96    /// Geometry in the local coordinates of `frame`.
97    pub shape: CuCollisionShape,
98    /// Joint-frame index, or `None` for the base frame.
99    #[serde(default)]
100    pub frame: Option<usize>,
101}
102
103impl CuLinkShape {
104    /// Creates a link shape on `frame` (`None` = base frame).
105    pub const fn new(shape: CuCollisionShape, frame: Option<usize>) -> Self {
106        Self { shape, frame }
107    }
108}
109
110/// Transforms a local `CuCollisionShape` by `frame` and writes the world-space shape into `out_world`.
111pub fn transform_shape(
112    frame: &Transform3D<f64>,
113    local: &CuCollisionShape,
114    out_world: &mut CuCollisionShape,
115) {
116    let mat = frame.to_matrix();
117    match local {
118        CuCollisionShape::Sphere(sph) => {
119            let c = transform_point_raw(&mat, sph.center);
120            *out_world = CuCollisionShape::Sphere(CuSphere {
121                center: c,
122                radius: sph.radius,
123            });
124        }
125        CuCollisionShape::Capsule(cap) => {
126            let a = transform_point_raw(&mat, cap.a);
127            let b = transform_point_raw(&mat, cap.b);
128            *out_world = CuCollisionShape::Capsule(CuCapsule {
129                a,
130                b,
131                radius: cap.radius,
132            });
133        }
134    }
135}
136
137/// Computes the exact signed surface-to-surface distance between two spheres.
138///
139/// Negative values indicate penetration depth.
140pub fn sphere_sphere_signed_distance(a: &CuSphere, b: &CuSphere) -> Length64 {
141    let pa = to_meters(a.center);
142    let pb = to_meters(b.center);
143    let dist = norm3(sub3(pa, pb));
144    let r_sum = a.radius.get::<meter>() + b.radius.get::<meter>();
145    Length64::new::<meter>(dist - r_sum)
146}
147
148/// Computes the exact signed surface-to-surface distance between a capsule and a sphere.
149///
150/// Negative values indicate penetration depth.
151pub fn capsule_sphere_signed_distance(cap: &CuCapsule, sph: &CuSphere) -> Length64 {
152    let a = to_meters(cap.a);
153    let b = to_meters(cap.b);
154    let p = to_meters(sph.center);
155    let dist = point_segment_distance(p, a, b);
156    let r_sum = cap.radius.get::<meter>() + sph.radius.get::<meter>();
157    Length64::new::<meter>(dist - r_sum)
158}
159
160/// Computes the exact signed surface-to-surface distance between two capsules.
161///
162/// Negative values indicate penetration depth.
163pub fn capsule_capsule_signed_distance(a: &CuCapsule, b: &CuCapsule) -> Length64 {
164    let p1 = to_meters(a.a);
165    let q1 = to_meters(a.b);
166    let p2 = to_meters(b.a);
167    let q2 = to_meters(b.b);
168    let dist = segment_segment_distance(p1, q1, p2, q2);
169    let r_sum = a.radius.get::<meter>() + b.radius.get::<meter>();
170    Length64::new::<meter>(dist - r_sum)
171}
172
173/// Computes the exact signed surface-to-surface distance between any two `CuCollisionShape` primitives.
174pub fn shape_signed_distance(a: &CuCollisionShape, b: &CuCollisionShape) -> Length64 {
175    match (a, b) {
176        (CuCollisionShape::Sphere(sa), CuCollisionShape::Sphere(sb)) => {
177            sphere_sphere_signed_distance(sa, sb)
178        }
179        (CuCollisionShape::Capsule(ca), CuCollisionShape::Sphere(sb)) => {
180            capsule_sphere_signed_distance(ca, sb)
181        }
182        (CuCollisionShape::Sphere(sa), CuCollisionShape::Capsule(cb)) => {
183            capsule_sphere_signed_distance(cb, sa)
184        }
185        (CuCollisionShape::Capsule(ca), CuCollisionShape::Capsule(cb)) => {
186            capsule_capsule_signed_distance(ca, cb)
187        }
188    }
189}
190
191/// Returns `true` if the signed distance between `a` and `b` is strictly less than `safety_margin`.
192pub fn shapes_violate_margin(
193    a: &CuCollisionShape,
194    b: &CuCollisionShape,
195    safety_margin: Length64,
196) -> bool {
197    shape_signed_distance(a, b).get::<meter>() < safety_margin.get::<meter>()
198}
199
200fn transform_point_raw(mat: &[[f64; 4]; 4], pt: [Length64; 3]) -> [Length64; 3] {
201    let x = pt[0].get::<meter>();
202    let y = pt[1].get::<meter>();
203    let z = pt[2].get::<meter>();
204    [
205        Length64::new::<meter>(mat[0][0] * x + mat[0][1] * y + mat[0][2] * z + mat[0][3]),
206        Length64::new::<meter>(mat[1][0] * x + mat[1][1] * y + mat[1][2] * z + mat[1][3]),
207        Length64::new::<meter>(mat[2][0] * x + mat[2][1] * y + mat[2][2] * z + mat[2][3]),
208    ]
209}
210
211fn to_meters(pt: [Length64; 3]) -> [f64; 3] {
212    [
213        pt[0].get::<meter>(),
214        pt[1].get::<meter>(),
215        pt[2].get::<meter>(),
216    ]
217}
218
219const fn sub3(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
220    [a[0] - b[0], a[1] - b[1], a[2] - b[2]]
221}
222
223const fn dot3(a: [f64; 3], b: [f64; 3]) -> f64 {
224    a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
225}
226
227fn norm3(v: [f64; 3]) -> f64 {
228    libm::sqrt(dot3(v, v))
229}
230
231fn point_segment_distance(p: [f64; 3], a: [f64; 3], b: [f64; 3]) -> f64 {
232    let ab = sub3(b, a);
233    let ap = sub3(p, a);
234    let len_sq = dot3(ab, ab);
235    if len_sq <= 1e-18 {
236        return norm3(ap);
237    }
238    let t = (dot3(ap, ab) / len_sq).clamp(0.0, 1.0);
239    let closest = [a[0] + t * ab[0], a[1] + t * ab[1], a[2] + t * ab[2]];
240    norm3(sub3(p, closest))
241}
242
243/// Exact distance between two 3D line segments `p1..q1` and `p2..q2` (Ericson, Real-Time Collision Detection).
244fn segment_segment_distance(p1: [f64; 3], q1: [f64; 3], p2: [f64; 3], q2: [f64; 3]) -> f64 {
245    let d1 = sub3(q1, p1);
246    let d2 = sub3(q2, p2);
247    let r = sub3(p1, p2);
248    let a = dot3(d1, d1);
249    let e = dot3(d2, d2);
250    let f = dot3(d2, r);
251
252    let eps = 1e-18;
253    if a <= eps && e <= eps {
254        return norm3(r);
255    }
256    if a <= eps {
257        return point_segment_distance(p1, p2, q2);
258    }
259    if e <= eps {
260        return point_segment_distance(p2, p1, q1);
261    }
262
263    let c = dot3(d1, r);
264    let b = dot3(d1, d2);
265    let denom = a * e - b * b;
266
267    let mut s = if denom != 0.0 {
268        ((b * f - c * e) / denom).clamp(0.0, 1.0)
269    } else {
270        0.0
271    };
272
273    let mut t = (b * s + f) / e;
274    if t < 0.0 {
275        t = 0.0;
276        s = (-c / a).clamp(0.0, 1.0);
277    } else if t > 1.0 {
278        t = 1.0;
279        s = ((b - c) / a).clamp(0.0, 1.0);
280    }
281
282    let c1 = [p1[0] + s * d1[0], p1[1] + s * d1[1], p1[2] + s * d1[2]];
283    let c2 = [p2[0] + t * d2[0], p2[1] + t * d2[1], p2[2] + t * d2[2]];
284    norm3(sub3(c1, c2))
285}
286
287/// Builds a sphere from meter values without validation (crate-internal presets and tests).
288pub(crate) fn sphere_m(center: [f64; 3], radius: f64) -> CuSphere {
289    CuSphere {
290        center: center.map(Length64::new::<meter>),
291        radius: Length64::new::<meter>(radius),
292    }
293}
294
295/// Builds a capsule from meter values without validation (crate-internal presets and tests).
296pub(crate) fn capsule_m(a: [f64; 3], b: [f64; 3], radius: f64) -> CuCapsule {
297    CuCapsule {
298        a: a.map(Length64::new::<meter>),
299        b: b.map(Length64::new::<meter>),
300        radius: Length64::new::<meter>(radius),
301    }
302}
repose_collision/shapes.rs

repose_collision/
shapes.rs
