axiolid_collide/
sat.rs

1// SPDX-License-Identifier: MPL-2.0
2
3//! The separating axis test.
4//!
5//! # The degenerate-axis trap
6//!
7//! Candidate axes include cross products of edge pairs. When two edges are
8//! nearly parallel their cross product is near zero, and normalising it
9//! amplifies rounding into a direction that points essentially anywhere.
10//! Projecting onto such an axis can report a separation of ~1e-16 for two
11//! shapes that genuinely touch, which turns a clearance check into a
12//! coin flip.
13//!
14//! Those axes are skipped by squared length before normalisation. Skipping is
15//! safe: a degenerate cross product carries no separating information that
16//! the two edges' own face normals do not already carry.
17
18use axiolid_core::{Point3, Scalar, Vec3};
19
20use crate::{project, ConvexShape};
21
22/// The axis along which two shapes are furthest apart.
23#[derive(Debug, Clone, Copy, PartialEq)]
24pub struct Contact {
25    /// Unit direction of least overlap, pointing from `a` towards `b`.
26    pub axis: Vec3,
27    /// Gap along `axis`.
28    pub distance: Scalar,
29}
30
31/// Outcome of a separating-axis query.
32#[derive(Debug, Clone, Copy, PartialEq)]
33#[non_exhaustive]
34pub enum SeparationResult {
35    /// A separating axis exists: the shapes are disjoint.
36    Apart {
37        /// Unit direction that separates them.
38        axis: Vec3,
39        /// Gap along that axis. The largest gap found, which is the
40        /// shapes' true separation distance for convex operands.
41        distance: Scalar,
42    },
43    /// No separating axis exists: the shapes share at least one point.
44    ///
45    /// No depth is reported. See the crate docs: penetration depth and
46    /// contact extent are different measurements and this crate refuses to
47    /// conflate them.
48    Overlapping,
49}
50
51/// Find a separating axis between two convex shapes, or prove none exists.
52///
53/// `tolerance` is the gap below which two projections count as touching
54/// rather than apart, and the length below which a candidate axis is treated
55/// as degenerate and skipped.
56#[must_use]
57pub fn separation(a: &ConvexShape, b: &ConvexShape, tolerance: Scalar) -> SeparationResult {
58    if a.is_empty() || b.is_empty() {
59        // An empty shape occupies no space, so it cannot overlap anything.
60        // Reporting a direction would be inventing one.
61        return SeparationResult::Apart {
62            axis: Vec3::new(1.0, 0.0, 0.0),
63            distance: Scalar::INFINITY,
64        };
65    }
66
67    let mut best_axis = Vec3::ZERO;
68    let mut best_gap = Scalar::NEG_INFINITY;
69
70    for axis in candidate_axes(a, b, tolerance) {
71        let (a_min, a_max) = project(&a.points, axis);
72        let (b_min, b_max) = project(&b.points, axis);
73        // Gap is positive when the projections are disjoint. Take the larger
74        // of the two orderings so the sign convention is "b beyond a".
75        let gap = (b_min - a_max).max(a_min - b_max);
76        if gap > best_gap {
77            best_gap = gap;
78            best_axis = if b_min - a_max >= a_min - b_max {
79                axis
80            } else {
81                -axis
82            };
83        }
84        // A single axis with a real gap proves disjointness; no need to test
85        // the rest.
86        if gap > tolerance {
87            return SeparationResult::Apart {
88                axis: best_axis,
89                distance: gap,
90            };
91        }
92    }
93
94    if best_gap > tolerance {
95        SeparationResult::Apart {
96            axis: best_axis,
97            distance: best_gap,
98        }
99    } else {
100        SeparationResult::Overlapping
101    }
102}
103
104/// Every axis worth testing: both shapes' face normals, plus edge-pair cross
105/// products, each normalised and filtered for degeneracy.
106fn candidate_axes(a: &ConvexShape, b: &ConvexShape, tolerance: Scalar) -> Vec<Vec3> {
107    // Below this squared length a cross product is numerical noise rather
108    // than a direction.
109    let floor = (tolerance * tolerance).max(1e-24);
110    let mut axes = Vec::new();
111
112    let push = |v: Vec3, axes: &mut Vec<Vec3>| {
113        let length_squared = v.length_squared();
114        if length_squared > floor {
115            axes.push(v / length_squared.sqrt());
116        }
117    };
118
119    for &normal in a.normals.iter().chain(b.normals.iter()) {
120        push(normal, &mut axes);
121    }
122    for &edge_a in &a.edges {
123        for &edge_b in &b.edges {
124            push(edge_a.cross(edge_b), &mut axes);
125        }
126    }
127    // With no face normals and no usable edge pairs (two coincident points,
128    // say) fall back to the line between the shapes so the query still has an
129    // answer rather than silently reporting overlap.
130    if axes.is_empty() {
131        let direction = centroid(&b.points) - centroid(&a.points);
132        push(direction, &mut axes);
133    }
134    axes
135}
136
137fn centroid(points: &[Point3]) -> Point3 {
138    let mut sum = Vec3::ZERO;
139    for point in points {
140        sum += *point;
141    }
142    sum / points.len() as Scalar
143}
axiolid_collide/sat.rs

axiolid_collide/
sat.rs
