box2d_rust/distance/
mod.rs

1// Port of box2d-cpp-reference/src/distance.c and the distance group of
2// include/box2d/collision.h.
3//
4// Split to satisfy the 800-line file limit:
5// - types.rs — proxies, simplex cache, distance/cast/TOI inputs and outputs
6// - gjk.rs   — the GJK distance algorithm (b2ShapeDistance) and simplex solvers
7// - cast.rs  — linear shape cast via conservative advancement (b2ShapeCast)
8// - toi.rs   — time of impact via local separating axes (b2TimeOfImpact)
9//
10// This file holds the standalone helpers: sweep evaluation, segment-segment
11// distance, and proxy construction.
12//
13// The experimental `b2ShapeCastMerged` in distance.c sits inside `#if 0` and is
14// not ported. The B2_SNOOP_TOI_COUNTERS globals are profiling-only and disabled
15// in normal builds; they are not ported.
16//
17// SPDX-FileCopyrightText: 2023 Erin Catto
18// SPDX-License-Identifier: MIT
19
20mod cast;
21mod gjk;
22mod toi;
23mod types;
24
25pub use cast::shape_cast;
26pub use gjk::shape_distance;
27pub use toi::time_of_impact;
28pub use types::{
29    DistanceInput, DistanceOutput, SegmentDistanceResult, ShapeCastPairInput, ShapeProxy, Simplex,
30    SimplexCache, SimplexVertex, Sweep, ToiInput, ToiOutput, ToiState,
31};
32
33use crate::hull::MAX_POLYGON_VERTICES;
34use crate::math_functions::{
35    add, clamp_float, distance_squared, dot, min_int, mul_add, mul_sv, normalize_rot,
36    rotate_vector, sub, transform_point, Rot, Transform, Vec2,
37};
38
39/// Evaluate the transform sweep at a specific time. (b2GetSweepTransform)
40pub fn get_sweep_transform(sweep: &Sweep, time: f32) -> Transform {
41    // https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/
42    let mut xf = Transform {
43        p: add(mul_sv(1.0 - time, sweep.c1), mul_sv(time, sweep.c2)),
44        q: normalize_rot(Rot {
45            c: (1.0 - time) * sweep.q1.c + time * sweep.q2.c,
46            s: (1.0 - time) * sweep.q1.s + time * sweep.q2.s,
47        }),
48    };
49
50    // Shift to origin
51    xf.p = sub(xf.p, rotate_vector(xf.q, sweep.local_center));
52    xf
53}
54
55/// Compute the distance between two line segments, clamping at the end points
56/// if needed. (b2SegmentDistance)
57///
58/// Follows Ericson 5.1.9 Closest Points of Two Line Segments.
59pub fn segment_distance(p1: Vec2, q1: Vec2, p2: Vec2, q2: Vec2) -> SegmentDistanceResult {
60    let mut result = SegmentDistanceResult::default();
61
62    let d1 = sub(q1, p1);
63    let d2 = sub(q2, p2);
64    let r = sub(p1, p2);
65    let dd1 = dot(d1, d1);
66    let dd2 = dot(d2, d2);
67    let rd1 = dot(r, d1);
68    let rd2 = dot(r, d2);
69
70    let eps_sqr = f32::EPSILON * f32::EPSILON;
71
72    if dd1 < eps_sqr || dd2 < eps_sqr {
73        // Handle all degeneracies
74        if dd1 >= eps_sqr {
75            // Segment 2 is degenerate
76            result.fraction1 = clamp_float(-rd1 / dd1, 0.0, 1.0);
77            result.fraction2 = 0.0;
78        } else if dd2 >= eps_sqr {
79            // Segment 1 is degenerate
80            result.fraction1 = 0.0;
81            result.fraction2 = clamp_float(rd2 / dd2, 0.0, 1.0);
82        } else {
83            result.fraction1 = 0.0;
84            result.fraction2 = 0.0;
85        }
86    } else {
87        // Non-degenerate segments
88        let d12 = dot(d1, d2);
89
90        let denominator = dd1 * dd2 - d12 * d12;
91
92        // Fraction on segment 1
93        let mut f1 = 0.0;
94        if denominator != 0.0 {
95            // not parallel
96            f1 = clamp_float((d12 * rd2 - rd1 * dd2) / denominator, 0.0, 1.0);
97        }
98
99        // Compute point on segment 2 closest to p1 + f1 * d1
100        let mut f2 = (d12 * f1 + rd2) / dd2;
101
102        // Clamping of segment 2 requires a do over on segment 1
103        if f2 < 0.0 {
104            f2 = 0.0;
105            f1 = clamp_float(-rd1 / dd1, 0.0, 1.0);
106        } else if f2 > 1.0 {
107            f2 = 1.0;
108            f1 = clamp_float((d12 - rd1) / dd1, 0.0, 1.0);
109        }
110
111        result.fraction1 = f1;
112        result.fraction2 = f2;
113    }
114
115    result.closest1 = mul_add(p1, result.fraction1, d1);
116    result.closest2 = mul_add(p2, result.fraction2, d2);
117    result.distance_squared = distance_squared(result.closest1, result.closest2);
118    result
119}
120
121/// Make a proxy for use in overlap, shape cast, and related functions. This is
122/// a deep copy of the points. (b2MakeProxy)
123pub fn make_proxy(points: &[Vec2], radius: f32) -> ShapeProxy {
124    let count = min_int(points.len() as i32, MAX_POLYGON_VERTICES as i32);
125    let mut proxy = ShapeProxy {
126        points: [Vec2::default(); MAX_POLYGON_VERTICES],
127        count,
128        radius,
129    };
130    proxy.points[..count as usize].copy_from_slice(&points[..count as usize]);
131    proxy
132}
133
134/// Make a proxy with a transform. This is a deep copy of the points.
135/// (b2MakeOffsetProxy)
136pub fn make_offset_proxy(
137    points: &[Vec2],
138    radius: f32,
139    position: Vec2,
140    rotation: Rot,
141) -> ShapeProxy {
142    let count = min_int(points.len() as i32, MAX_POLYGON_VERTICES as i32);
143    let transform = Transform {
144        p: position,
145        q: rotation,
146    };
147    let mut proxy = ShapeProxy {
148        points: [Vec2::default(); MAX_POLYGON_VERTICES],
149        count,
150        radius,
151    };
152    for (dst, src) in proxy.points[..count as usize]
153        .iter_mut()
154        .zip(&points[..count as usize])
155    {
156        *dst = transform_point(transform, *src);
157    }
158    proxy
159}
box2d_rust/distance/mod.rs

box2d_rust/distance/
mod.rs
