alice_physics/
sdf_ccd.rs

1//! SDF Sphere Tracing CCD (Continuous Collision Detection)
2//!
3//! Uses sphere tracing (ray marching) along the SDF to detect
4//! time of impact for arbitrary SDF shapes. Works with concave,
5//! CSG, and fractal geometry — anything expressible as an SDF.
6//!
7//! # Algorithm
8//!
9//! 1. Start at body position at t=0
10//! 2. Evaluate SDF distance at current position
11//! 3. Advance along trajectory by `distance - radius` (safe step)
12//! 4. Repeat until contact or t > 1
13//!
14//! This is a generalization of conservative advancement that directly
15//! uses the SDF's Lipschitz property for optimal step sizes.
16//!
17//! Author: Moroya Sakamoto
18
19#[cfg(feature = "std")]
20use crate::ccd::TOI;
21use crate::math::Fix128;
22#[cfg(feature = "std")]
23use crate::math::Vec3Fix;
24#[cfg(feature = "std")]
25use crate::sdf_collider::{SdfCollider, SdfFrame, SdfQuery};
26
27// ============================================================================
28// SDF CCD Configuration
29// ============================================================================
30
31/// Configuration for SDF sphere tracing CCD
32#[derive(Clone, Copy, Debug, PartialEq)]
33pub struct SdfCcdConfig {
34    /// Maximum sphere tracing iterations
35    pub max_iterations: usize,
36    /// Contact tolerance: `sphere_trace_sdf` stops when `gap <= tolerance`,
37    /// `ray_march_sdf` when `distance < tolerance`
38    pub tolerance: f32,
39    /// Minimum velocity magnitude to trigger CCD
40    pub velocity_threshold: Fix128,
41    /// Safety margin multiplier for step size (< 1.0 for conservative)
42    pub step_safety: f32,
43}
44
45impl Default for SdfCcdConfig {
46    fn default() -> Self {
47        Self {
48            max_iterations: 64,
49            tolerance: 0.001,
50            velocity_threshold: Fix128::from_int(5),
51            step_safety: 0.9,
52        }
53    }
54}
55
56// ============================================================================
57// SDF Sphere Tracing CCD
58// ============================================================================
59
60/// Sphere trace a moving sphere against an SDF collider.
61///
62/// Returns time of impact in [0, 1] where:
63/// - 0 = start of timestep (already penetrating)
64/// - 1 = end of timestep
65///
66/// The sphere moves from `start` to `start + displacement` over [0, 1].
67/// This is [`sphere_trace_sdf_field`] with the collider's field and
68/// [`SdfCollider::frame`]; the two return the same result bit for bit.
69#[cfg(feature = "std")]
70#[must_use]
71pub fn sphere_trace_sdf(
72    start: Vec3Fix,
73    displacement: Vec3Fix,
74    radius: Fix128,
75    sdf: &SdfCollider,
76    config: &SdfCcdConfig,
77) -> Option<TOI> {
78    sphere_trace_sdf_field(
79        start,
80        displacement,
81        radius,
82        &*sdf.field,
83        &sdf.frame(),
84        config,
85    )
86}
87
88/// Sphere trace a moving sphere against a borrowed distance field.
89///
90/// The same computation as [`sphere_trace_sdf`], for a field that is not
91/// owned by an [`SdfCollider`]: `field` is any [`SdfQuery`] (every
92/// [`SdfField`](crate::sdf_collider::SdfField), or borrowed closures through
93/// [`ClosureSdfQuery`](crate::sdf_collider::ClosureSdfQuery)) and need not be
94/// `'static`, `Send` or `Sync`. `frame` places the field in the world
95/// ([`SdfFrame::IDENTITY`] when the field is already in world coordinates).
96///
97/// The sphere of `radius` moves from `start` to `start + displacement` over
98/// `t` in [0, 1]. Each iteration evaluates the gap `distance * scale - radius`
99/// at the current position: when `gap <= config.tolerance` the hit is
100/// returned (with the world normal of the field there and the point `pos -
101/// normal * distance`), otherwise `t` advances by `gap * step_safety /
102/// |displacement|`. A hit's `t` is therefore at most the true time of impact
103/// and short of it by about `tolerance / |displacement|`.
104///
105/// Degenerate input:
106/// - zero `displacement`: `None` (also when the sphere starts inside)
107/// - zero `radius`: the trace of the centre point
108/// - `config.max_iterations == 0`: `None` (no evaluation is made)
109/// - `t` passing 1, or the iteration budget running out: `None`
110#[cfg(feature = "std")]
111#[must_use]
112pub fn sphere_trace_sdf_field<F: SdfQuery + ?Sized>(
113    start: Vec3Fix,
114    displacement: Vec3Fix,
115    radius: Fix128,
116    field: &F,
117    frame: &SdfFrame,
118    config: &SdfCcdConfig,
119) -> Option<TOI> {
120    let radius_f32 = radius.to_f32();
121    let scale = frame.scale_f32();
122    let disp_len = displacement.length();
123
124    if disp_len.is_zero() {
125        return None;
126    }
127
128    let inv_disp_len = Fix128::ONE / disp_len;
129    let mut t = Fix128::ZERO;
130
131    for _ in 0..config.max_iterations {
132        // Current position along trajectory
133        let pos = start + displacement * t;
134
135        // SDF distance query in local space
136        let (lx, ly, lz) = frame.world_to_local(pos);
137        let dist = field.query_distance(lx, ly, lz) * scale;
138
139        // Gap = distance to surface minus sphere radius
140        let gap = dist - radius_f32;
141
142        if gap <= config.tolerance {
143            // Contact found
144            let (nx, ny, nz) = field.query_normal(lx, ly, lz);
145            let normal = frame.local_normal_to_world(nx, ny, nz);
146            let point = pos - normal * Fix128::from_f32(dist);
147
148            return Some(TOI { t, point, normal });
149        }
150
151        // Safe advance: the SDF guarantees no surface within `gap` distance
152        let safe_step = Fix128::from_f32(gap * config.step_safety);
153        let dt = safe_step * inv_disp_len;
154        t = t + dt;
155
156        if t > Fix128::ONE {
157            return None; // No collision this timestep
158        }
159    }
160
161    None // Max iterations reached without convergence
162}
163
164/// Sphere trace a moving point (zero radius) against an SDF.
165///
166/// Simpler variant for raycasting against SDF geometry. `direction` need not
167/// be normalized; the returned `t` and `max_distance` are distances along it.
168#[cfg(feature = "std")]
169#[must_use]
170pub fn ray_march_sdf(
171    origin: Vec3Fix,
172    direction: Vec3Fix,
173    max_distance: Fix128,
174    sdf: &SdfCollider,
175    config: &SdfCcdConfig,
176) -> Option<TOI> {
177    let len_sq = direction.length_squared();
178    if len_sq.is_zero() {
179        return None;
180    }
181    // `t` is a distance (it is advanced by the SDF value and compared with
182    // `max_distance`), so the direction has to be a unit vector: stepping
183    // `dist` along `origin + direction * t` with `|direction| > 1` jumps
184    // `|direction| * dist` and tunnels through the surface. A unit direction
185    // (the common case) is used as given, bit for bit.
186    let direction = if len_sq == Fix128::ONE {
187        direction
188    } else {
189        direction.normalize()
190    };
191    let scale = sdf.scale_f32;
192    let mut t = Fix128::ZERO;
193
194    for _ in 0..config.max_iterations {
195        let pos = origin + direction * t;
196
197        let (lx, ly, lz) = sdf.world_to_local(pos);
198        let dist = sdf.field.distance(lx, ly, lz) * scale;
199
200        if dist < config.tolerance {
201            let (nx, ny, nz) = sdf.field.normal(lx, ly, lz);
202            let normal = sdf.local_normal_to_world(nx, ny, nz);
203            let point = pos;
204
205            return Some(TOI { t, point, normal });
206        }
207
208        t = t + Fix128::from_f32(dist * config.step_safety);
209
210        if t > max_distance {
211            return None;
212        }
213    }
214
215    None
216}
217
218/// Batch sphere trace: test multiple bodies against multiple SDF colliders.
219///
220/// Returns (`body_index`, `sdf_index`, TOI) tuples for all detected impacts.
221#[cfg(feature = "std")]
222#[must_use]
223pub fn batch_sphere_trace_sdf(
224    bodies: &[crate::solver::RigidBody],
225    displacements: &[Vec3Fix],
226    radius: Fix128,
227    sdf_colliders: &[SdfCollider],
228    config: &SdfCcdConfig,
229) -> Vec<(usize, usize, TOI)> {
230    let mut results = Vec::new();
231
232    for (body_idx, (body, disp)) in bodies.iter().zip(displacements.iter()).enumerate() {
233        if body.is_static() {
234            continue;
235        }
236
237        // Check if body is moving fast enough for CCD
238        let speed = body.velocity.length();
239        if speed < config.velocity_threshold {
240            continue;
241        }
242
243        for (sdf_idx, sdf) in sdf_colliders.iter().enumerate() {
244            if sdf.body_index == body_idx {
245                continue;
246            }
247
248            if let Some(toi) = sphere_trace_sdf(body.position, *disp, radius, sdf, config) {
249                results.push((body_idx, sdf_idx, toi));
250            }
251        }
252    }
253
254    // Sort by earliest TOI for deterministic processing
255    results.sort_by_key(|a| a.2.t);
256    results
257}
258
259#[cfg(feature = "std")]
260impl crate::solver::PhysicsWorld {
261    /// Sweep the world's moving bodies against its SDF colliders.
262    ///
263    /// A body is swept as a sphere of [`PhysicsWorld::sdf_collision_radius`](crate::solver::PhysicsWorld::sdf_collision_radius)
264    /// (the radius [`PhysicsWorld::sdf_contacts`](crate::solver::PhysicsWorld::sdf_contacts) gives a body without a shape)
265    /// along `velocity * dt`; this is the continuous counterpart of the
266    /// discrete push-out in [`PhysicsWorld::step`](crate::solver::PhysicsWorld::step), to be asked before a step to
267    /// learn which fast bodies would tunnel through a thin SDF. Static bodies,
268    /// sensors and bodies slower than `config.velocity_threshold` are skipped,
269    /// and an SDF attached to the body itself is never tested against it. Hits
270    /// come back as in [`batch_sphere_trace_sdf`], earliest time of impact first.
271    /// An SDF attached to a body is swept at the pose it was given when it was
272    /// added or at the end of the last step (see
273    /// [`PhysicsWorld::sdf_contacts`](crate::solver::PhysicsWorld::sdf_contacts)).
274    ///
275    /// `dt` is the frame step.
276    #[must_use]
277    pub fn sdf_ccd_hits(&self, dt: Fix128, config: &SdfCcdConfig) -> Vec<(usize, usize, TOI)> {
278        let displacements: Vec<Vec3Fix> = self
279            .bodies
280            .iter()
281            .map(|b| {
282                if b.is_sensor {
283                    Vec3Fix::ZERO
284                } else {
285                    b.velocity * dt
286                }
287            })
288            .collect();
289        batch_sphere_trace_sdf(
290            &self.bodies,
291            &displacements,
292            self.sdf_collision_radius,
293            &self.sdf_colliders,
294            config,
295        )
296    }
297}
298
299// ============================================================================
300// Tests
301// ============================================================================
302
303#[cfg(all(test, feature = "std"))]
304mod tests {
305    use super::*;
306    use crate::math::QuatFix;
307    use crate::sdf_collider::{ClosureSdf, SdfCollider};
308
309    fn unit_sphere() -> ClosureSdf {
310        ClosureSdf::new(
311            |x, y, z| z.mul_add(z, x.mul_add(x, y * y)).sqrt() - 1.0,
312            |x, y, z| {
313                let len = z.mul_add(z, x.mul_add(x, y * y)).sqrt();
314                if len < 1e-10 {
315                    (0.0, 1.0, 0.0)
316                } else {
317                    (x / len, y / len, z / len)
318                }
319            },
320        )
321    }
322
323    fn ground_plane() -> ClosureSdf {
324        ClosureSdf::new(|_x, y, _z| y, |_x, _y, _z| (0.0, 1.0, 0.0))
325    }
326
327    #[test]
328    fn test_sphere_trace_hit() {
329        let sdf =
330            SdfCollider::new_static(Box::new(unit_sphere()), Vec3Fix::ZERO, QuatFix::IDENTITY);
331
332        let start = Vec3Fix::from_f32(-5.0, 0.0, 0.0);
333        let displacement = Vec3Fix::from_f32(10.0, 0.0, 0.0);
334        let radius = Fix128::from_f32(0.5);
335        let config = SdfCcdConfig::default();
336
337        let result = sphere_trace_sdf(start, displacement, radius, &sdf, &config);
338        assert!(result.is_some(), "Should detect collision with sphere");
339
340        let toi = result.unwrap();
341        let t = toi.t.to_f32();
342        // Start at -5, sphere at 0, radius 1 + body radius 0.5 = contact at x = -1.5
343        // t = (5 - 1.5) / 10 = 0.35
344        assert!(t > 0.2 && t < 0.5, "TOI should be ~0.35, got {t}");
345    }
346
347    #[test]
348    fn test_sphere_trace_miss() {
349        let sdf =
350            SdfCollider::new_static(Box::new(unit_sphere()), Vec3Fix::ZERO, QuatFix::IDENTITY);
351
352        // Moving parallel to sphere, missing it
353        let start = Vec3Fix::from_f32(-5.0, 5.0, 0.0);
354        let displacement = Vec3Fix::from_f32(10.0, 0.0, 0.0);
355        let radius = Fix128::from_f32(0.5);
356        let config = SdfCcdConfig::default();
357
358        let result = sphere_trace_sdf(start, displacement, radius, &sdf, &config);
359        assert!(result.is_none(), "Should miss the sphere");
360    }
361
362    #[test]
363    fn test_sphere_trace_ground() {
364        let sdf =
365            SdfCollider::new_static(Box::new(ground_plane()), Vec3Fix::ZERO, QuatFix::IDENTITY);
366
367        // Falling sphere toward ground
368        let start = Vec3Fix::from_f32(0.0, 5.0, 0.0);
369        let displacement = Vec3Fix::from_f32(0.0, -10.0, 0.0);
370        let radius = Fix128::from_f32(0.5);
371        let config = SdfCcdConfig::default();
372
373        let result = sphere_trace_sdf(start, displacement, radius, &sdf, &config);
374        assert!(result.is_some(), "Should detect ground collision");
375
376        let toi = result.unwrap();
377        let t = toi.t.to_f32();
378        // Start at y=5, ground at y=0, radius=0.5, contact at y=0.5
379        // t = (5 - 0.5) / 10 = 0.45
380        assert!(t > 0.3 && t < 0.6, "TOI should be ~0.45, got {t}");
381    }
382
383    #[test]
384    fn test_ray_march() {
385        let sdf =
386            SdfCollider::new_static(Box::new(unit_sphere()), Vec3Fix::ZERO, QuatFix::IDENTITY);
387
388        let origin = Vec3Fix::from_f32(-5.0, 0.0, 0.0);
389        let direction = Vec3Fix::UNIT_X;
390        let max_dist = Fix128::from_int(20);
391        let config = SdfCcdConfig::default();
392
393        let result = ray_march_sdf(origin, direction, max_dist, &sdf, &config);
394        assert!(result.is_some(), "Ray should hit sphere");
395    }
396
397    #[test]
398    fn batch_sphere_trace_sdf_filters_bodies_and_sorts_by_closed_form_toi() {
399        use crate::solver::RigidBody;
400        let colliders = [
401            // 0: 地面 y = 0
402            SdfCollider::new_static(Box::new(ground_plane()), Vec3Fix::ZERO, QuatFix::IDENTITY),
403            // 1: 単位球 (原点)
404            SdfCollider::new_static(Box::new(unit_sphere()), Vec3Fix::ZERO, QuatFix::IDENTITY),
405            // 2: 単位球を body 3 に attach (body 3 に対しては self-collision skip)
406            SdfCollider::new_dynamic(Box::new(unit_sphere()), 3),
407        ];
408        let radius = Fix128::from_f32(0.5);
409        let config = SdfCcdConfig::default(); // velocity_threshold 5、tolerance 0.001
410
411        let fast_down = Vec3Fix::from_int(0, -10, 0);
412        let fast_right = Vec3Fix::from_int(10, 0, 0);
413        let mut bodies = vec![
414            // 0: (0, 5, 0) → 下へ 10: 球 top 接触 y = 1.5 → t 0.35 (sdf 1 と 2)、地面 y = 0.5 → t 0.45
415            RigidBody::new_dynamic(Vec3Fix::from_int(0, 5, 0), Fix128::ONE)
416                .with_velocity(fast_down),
417            // 1: (-5, 5, 0) → 右へ 10: 球も地面も miss
418            RigidBody::new_dynamic(Vec3Fix::from_int(-5, 5, 0), Fix128::ONE)
419                .with_velocity(fast_right),
420            // 2: static は速くても skip
421            RigidBody::new_static(Vec3Fix::from_int(0, 5, 0)).with_velocity(fast_down),
422            // 3: (-7, 0, 0) → 右へ 10: 地面は既に貫入 (gap -0.5) → t 0、球 x = -1.5 → t 0.55、sdf 2 は self skip
423            RigidBody::new_dynamic(Vec3Fix::from_int(-7, 0, 0), Fix128::ONE)
424                .with_velocity(fast_right),
425            // 4: body 0 と同じ軌道だが速度 1 < threshold 5 → skip
426            RigidBody::new_dynamic(Vec3Fix::from_int(0, 5, 0), Fix128::ONE)
427                .with_velocity(Vec3Fix::from_int(0, -1, 0)),
428        ];
429        let displacements = [fast_down, fast_right, fast_down, fast_right, fast_down];
430
431        let hits = batch_sphere_trace_sdf(&bodies, &displacements, radius, &colliders, &config);
432        let keys: Vec<(usize, usize)> = hits.iter().map(|(b, s, _)| (*b, *s)).collect();
433        // t 昇順、同値 (body 0 の sdf 1 / 2 は同じ field) は stable sort で挿入順
434        assert_eq!(keys, vec![(3, 0), (0, 1), (0, 2), (0, 0), (3, 1)]);
435        let expected_t = [0.0_f32, 0.35, 0.35, 0.45, 0.55];
436        for (k, want) in expected_t.iter().enumerate() {
437            let t = hits[k].2.t.to_f32();
438            assert!((t - want).abs() < 1e-3, "hit {k} t {t}, want {want}");
439        }
440        // 法線: 地面 +Y、球 top +Y、球の -x 側 -X
441        let n = |k: usize| hits[k].2.normal.to_f32();
442        for k in [0usize, 1, 2, 3] {
443            let (nx, ny, nz) = n(k);
444            assert!(
445                nx.abs() < 1e-4 && (ny - 1.0).abs() < 1e-4 && nz.abs() < 1e-4,
446                "hit {k}"
447            );
448        }
449        let (nx, ny, nz) = n(4);
450        assert!((nx + 1.0).abs() < 1e-4 && ny.abs() < 1e-4 && nz.abs() < 1e-4);
451        // 接触点は SDF 表面上: 地面 hit の y ≈ 0、球 hit は |p| ≈ 1
452        let (_, py, _) = hits[3].2.point.to_f32();
453        assert!(py.abs() < 2e-3, "ground contact point y {py}");
454        for k in [1usize, 2, 4] {
455            let (px, py, pz) = hits[k].2.point.to_f32();
456            let r = (px * px + py * py + pz * pz).sqrt();
457            assert!((r - 1.0).abs() < 2e-3, "hit {k} contact point radius {r}");
458        }
459
460        // velocity_threshold を 0 にすると body 4 も (body 0 と同じ 3 hit で) 拾われる
461        let lenient = SdfCcdConfig {
462            velocity_threshold: Fix128::ZERO,
463            ..SdfCcdConfig::default()
464        };
465        let all = batch_sphere_trace_sdf(&bodies, &displacements, radius, &colliders, &lenient);
466        assert_eq!(all.len(), 8);
467        assert_eq!(all.iter().filter(|(b, _, _)| *b == 4).count(), 3);
468        // zip: displacement が足りない body は評価されない
469        let short =
470            batch_sphere_trace_sdf(&bodies, &displacements[..1], radius, &colliders, &config);
471        assert_eq!(short.len(), 3);
472        assert!(short.iter().all(|(b, _, _)| *b == 0));
473        // body 3 を static にすると (3, *) が消える
474        bodies[3].inv_mass = Fix128::ZERO;
475        let no3 = batch_sphere_trace_sdf(&bodies, &displacements, radius, &colliders, &config);
476        assert_eq!(no3.len(), 3);
477        assert!(no3.iter().all(|(b, _, _)| *b == 0));
478    }
479}
alice_physics/sdf_ccd.rs

alice_physics/
sdf_ccd.rs
