box2d_rust/distance/
cast.rs

1// Linear shape cast via conservative advancement (b2ShapeCast) from distance.c.
2// SPDX-FileCopyrightText: 2023 Erin Catto
3// SPDX-License-Identifier: MIT
4
5use super::gjk::shape_distance;
6use super::types::{DistanceInput, ShapeCastPairInput, SimplexCache};
7use crate::collision::CastOutput;
8use crate::constants::linear_slop;
9use crate::math_functions::{dot, is_normalized, lerp, max_float, mul_add};
10
11/// Perform a linear shape cast of shape B moving and shape A fixed. Determines
12/// the hit point, normal, and translation fraction. The query runs in frame A,
13/// so the hit point and normal are returned in frame A. Initially touching
14/// shapes are a miss unless `can_encroach` allows it. (b2ShapeCast)
15///
16/// Shape cast using conservative advancement.
17pub fn shape_cast(input: &ShapeCastPairInput) -> CastOutput {
18    // Compute tolerance
19    let slop = linear_slop();
20    let total_radius = input.proxy_a.radius + input.proxy_b.radius;
21    let mut target = max_float(slop, total_radius - slop);
22    let tolerance = 0.25 * slop;
23
24    debug_assert!(target > tolerance);
25
26    // Prepare input for distance query
27    let mut cache = SimplexCache::default();
28
29    let mut fraction = 0.0;
30
31    let mut distance_input = DistanceInput {
32        proxy_a: input.proxy_a,
33        proxy_b: input.proxy_b,
34        // The whole cast runs in frame A. Advance the relative pose of B in
35        // float each iteration, which keeps the math near the local origin and
36        // avoids re-relativizing world poses.
37        transform: input.transform,
38        use_radii: false,
39    };
40
41    let delta2 = input.translation_b;
42    let mut output = CastOutput::default();
43
44    let max_iterations = 20;
45
46    for iteration in 0..max_iterations {
47        output.iterations += 1;
48
49        let distance_output = shape_distance(&distance_input, &mut cache, None);
50
51        if distance_output.distance < target + tolerance {
52            if iteration == 0 {
53                if input.can_encroach && distance_output.distance > 2.0 * slop {
54                    target = distance_output.distance - slop;
55                } else {
56                    // Initial overlap
57                    output.hit = true;
58
59                    // Compute a common point
60                    let c1 = mul_add(
61                        distance_output.point_a,
62                        input.proxy_a.radius,
63                        distance_output.normal,
64                    );
65                    let c2 = mul_add(
66                        distance_output.point_b,
67                        -input.proxy_b.radius,
68                        distance_output.normal,
69                    );
70                    output.point = lerp(c1, c2, 0.5);
71                    return output;
72                }
73            } else {
74                // Regular hit
75                debug_assert!(
76                    distance_output.distance > 0.0 && is_normalized(distance_output.normal)
77                );
78                output.fraction = fraction;
79                output.point = mul_add(
80                    distance_output.point_a,
81                    input.proxy_a.radius,
82                    distance_output.normal,
83                );
84                output.normal = distance_output.normal;
85                output.hit = true;
86                return output;
87            }
88        }
89
90        debug_assert!(distance_output.distance > 0.0);
91        debug_assert!(is_normalized(distance_output.normal));
92
93        // Check if shapes are approaching each other
94        let denominator = dot(delta2, distance_output.normal);
95        if denominator >= 0.0 {
96            // Miss
97            return output;
98        }
99
100        // Advance sweep
101        fraction += (target - distance_output.distance) / denominator;
102        if fraction >= input.max_fraction {
103            // Miss
104            return output;
105        }
106
107        distance_input.transform.p = mul_add(input.transform.p, fraction, delta2);
108    }
109
110    // Failure!
111    output
112}
box2d_rust/distance/cast.rs

box2d_rust/distance/
cast.rs
