box3d_rust/distance/
cast.rs

1// Linear shape cast and sweep evaluation from distance.c.
2// SPDX-FileCopyrightText: 2026 Erin Catto
3// SPDX-License-Identifier: MIT
4
5use super::gjk::shape_distance;
6use super::types::{CastOutput, DistanceInput, ShapeCastPairInput, SimplexCache, Sweep};
7use crate::constants::linear_slop;
8use crate::math_functions::{
9    dot, is_normalized, lerp, max_float, mul_add, nlerp, rotate_vector, sub, Transform, VEC3_ZERO,
10};
11
12/// Evaluate the transform sweep at a specific time. (b3GetSweepTransform)
13pub fn get_sweep_transform(sweep: &Sweep, time: f32) -> Transform {
14    let mut transform = Transform {
15        q: nlerp(sweep.q1, sweep.q2, time),
16        p: VEC3_ZERO,
17    };
18    transform.p = sub(
19        lerp(sweep.c1, sweep.c2, time),
20        rotate_vector(transform.q, sweep.local_center),
21    );
22    transform
23}
24
25/// Final sweep transform at t = 1 without nlerp. (b3GetFinalSweepTransform)
26pub(crate) fn get_final_sweep_transform(sweep: &Sweep) -> Transform {
27    let mut transform = Transform {
28        q: sweep.q2,
29        p: VEC3_ZERO,
30    };
31    transform.p = sub(sweep.c2, rotate_vector(transform.q, sweep.local_center));
32    transform
33}
34
35/// Perform a linear shape cast of shape B moving and shape A fixed. Determines
36/// the hit point, normal, and translation fraction. The query runs in frame A,
37/// so the hit point and normal are returned in frame A. Initially touching
38/// shapes are a miss unless `can_encroach` allows it. (b3ShapeCast)
39///
40/// Shape cast using conservative advancement.
41pub fn shape_cast(input: &ShapeCastPairInput) -> CastOutput {
42    // Compute tolerance
43    let linear_slop = linear_slop();
44    let total_radius = input.proxy_a.radius + input.proxy_b.radius;
45    let mut target = max_float(linear_slop, total_radius - linear_slop);
46    let tolerance = 0.25 * linear_slop;
47
48    debug_assert!(target > tolerance);
49
50    // Prepare input for distance query
51    let mut cache = SimplexCache::default();
52
53    let mut alpha = 0.0;
54
55    let mut distance_input = DistanceInput {
56        proxy_a: input.proxy_a,
57        proxy_b: input.proxy_b,
58        // The whole cast runs in frame A. Advance the relative pose of B in
59        // float each iteration, which keeps the math near the local origin and
60        // avoids re-relativizing world poses.
61        transform: input.transform,
62        use_radii: false,
63    };
64
65    let delta2 = input.translation_b;
66    let mut output = CastOutput::default();
67
68    let max_iterations = 20;
69
70    for iteration in 0..max_iterations {
71        output.iterations += 1;
72
73        let distance_output = shape_distance(&distance_input, &mut cache, None);
74
75        if distance_output.distance < target + tolerance {
76            if iteration == 0 {
77                if input.can_encroach && distance_output.distance > 2.0 * linear_slop {
78                    target = distance_output.distance - linear_slop;
79                } else {
80                    // Initial overlap
81                    output.hit = true;
82
83                    // Compute a common point
84                    let c1 = mul_add(
85                        distance_output.point_a,
86                        input.proxy_a.radius,
87                        distance_output.normal,
88                    );
89                    let c2 = mul_add(
90                        distance_output.point_b,
91                        -input.proxy_b.radius,
92                        distance_output.normal,
93                    );
94                    output.point = lerp(c1, c2, 0.5);
95                    return output;
96                }
97            } else {
98                // Logging for bad input data (C calls b3Log); skip logs, keep early return.
99                if distance_output.distance > 0.0 && !is_normalized(distance_output.normal) {
100                    // Numerical problem. Likely extreme input.
101                    return output;
102                }
103
104                // Hitting this assert implies that the algorithm brought the shapes too close.
105                // debug_assert!(distance_output.distance > 0.0 && is_normalized(distance_output.normal));
106
107                output.fraction = alpha;
108                output.point = mul_add(
109                    distance_output.point_a,
110                    input.proxy_a.radius,
111                    distance_output.normal,
112                );
113                output.normal = distance_output.normal;
114                output.hit = true;
115                return output;
116            }
117        }
118
119        debug_assert!(distance_output.distance > 0.0);
120        debug_assert!(is_normalized(distance_output.normal));
121
122        // Check if shapes are approaching each other
123        let denominator = dot(delta2, distance_output.normal);
124        if denominator >= 0.0 {
125            // Miss
126            return output;
127        }
128
129        // Advance sweep
130        alpha += (target - distance_output.distance) / denominator;
131        if alpha >= input.max_fraction {
132            // Success!
133            return output;
134        }
135
136        distance_input.transform.p = mul_add(input.transform.p, alpha, delta2);
137    }
138
139    // Failure!
140    output
141}
box3d_rust/distance/cast.rs

box3d_rust/distance/
cast.rs
