parry3d/query/sat/
sat_cuboid_cuboid.rs

1use crate::math::{Isometry, Real, Vector, DIM};
2use crate::shape::{Cuboid, SupportMap};
3
4/// Computes the separation distance between two cuboids along a given axis.
5///
6/// This function is part of the Separating Axis Theorem (SAT) implementation for cuboid-cuboid
7/// collision detection. It projects both cuboids onto the specified axis and computes how far
8/// apart they are (positive = separated, negative = overlapping).
9///
10/// # How It Works
11///
12/// 1. Orients the axis to point from cuboid1 toward cuboid2 (using the translation vector)
13/// 2. Finds the support points (furthest points) on each cuboid in that direction
14/// 3. Computes the signed distance between these support points along the axis
15///
16/// # Parameters
17///
18/// - `cuboid1`: The first cuboid (in its local coordinate frame)
19/// - `cuboid2`: The second cuboid
20/// - `pos12`: The position of cuboid2 relative to cuboid1 (transforms from cuboid2's space to cuboid1's space)
21/// - `axis1`: The axis direction in cuboid1's local space to test for separation
22///
23/// # Returns
24///
25/// A tuple containing:
26/// - `Real`: The separation distance along the axis
27///   - **Positive**: Shapes are separated by this distance
28///   - **Negative**: Shapes are overlapping (penetration depth is the absolute value)
29///   - **Zero**: Shapes are exactly touching
30/// - `Vector<Real>`: The oriented axis direction (pointing from cuboid1 toward cuboid2)
31///
32/// # Example
33///
34/// ```rust
35/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
36/// use parry3d::shape::Cuboid;
37/// use parry3d::query::sat::cuboid_cuboid_compute_separation_wrt_local_line;
38/// use nalgebra::{Isometry3, Vector3};
39///
40/// let cube1 = Cuboid::new(Vector3::new(1.0, 1.0, 1.0));
41/// let cube2 = Cuboid::new(Vector3::new(1.0, 1.0, 1.0));
42///
43/// // Position cube2 at (3, 0, 0) relative to cube1
44/// let pos12 = Isometry3::translation(3.0, 0.0, 0.0);
45///
46/// // Test separation along the X axis
47/// let (separation, _axis) = cuboid_cuboid_compute_separation_wrt_local_line(
48///     &cube1,
49///     &cube2,
50///     &pos12,
51///     &Vector3::x()
52/// );
53///
54/// // Should be separated by 1.0 (distance 3.0 - half_extents 1.0 - 1.0)
55/// assert!(separation > 0.0);
56/// # }
57/// ```
58#[cfg(feature = "dim3")]
59pub fn cuboid_cuboid_compute_separation_wrt_local_line(
60    cuboid1: &Cuboid,
61    cuboid2: &Cuboid,
62    pos12: &Isometry<Real>,
63    axis1: &Vector<Real>,
64) -> (Real, Vector<Real>) {
65    #[expect(clippy::unnecessary_cast)]
66    let signum = (1.0 as Real).copysign(pos12.translation.vector.dot(axis1));
67    let axis1 = axis1 * signum;
68    let axis2 = pos12.inverse_transform_vector(&-axis1);
69    let local_pt1 = cuboid1.local_support_point(&axis1);
70    let local_pt2 = cuboid2.local_support_point(&axis2);
71    let pt2 = pos12 * local_pt2;
72    let separation = (pt2 - local_pt1).dot(&axis1);
73    (separation, axis1)
74}
75
76/// Finds the best separating axis by testing all edge-edge combinations between two cuboids.
77///
78/// In 3D, edge-edge contact is common when two boxes collide. This function tests all possible
79/// axes formed by the cross product of edges from each cuboid to find the axis with maximum
80/// separation (or minimum penetration).
81///
82/// # Why Test Edge Cross Products?
83///
84/// When two 3D convex polyhedra collide, the separating axis (if one exists) must be either:
85/// 1. A face normal from one shape
86/// 2. A face normal from the other shape
87/// 3. **Perpendicular to an edge from each shape** (the cross product of the edges)
88///
89/// This function handles case 3. For two cuboids, there are 3 edges per cuboid (aligned with X, Y, Z),
90/// giving 3 × 3 = 9 possible edge pair combinations to test.
91///
92/// # Parameters
93///
94/// - `cuboid1`: The first cuboid (in its local coordinate frame)
95/// - `cuboid2`: The second cuboid
96/// - `pos12`: The position of cuboid2 relative to cuboid1
97///
98/// # Returns
99///
100/// A tuple containing:
101/// - `Real`: The best (maximum) separation found across all edge-edge axes
102///   - **Positive**: Shapes are separated
103///   - **Negative**: Shapes are overlapping
104/// - `Vector<Real>`: The axis direction that gives this separation
105///
106/// # Example
107///
108/// ```rust
109/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
110/// use parry3d::shape::Cuboid;
111/// use parry3d::query::sat::cuboid_cuboid_find_local_separating_edge_twoway;
112/// use nalgebra::{Isometry3, Vector3};
113///
114/// let cube1 = Cuboid::new(Vector3::new(1.0, 1.0, 1.0));
115/// let cube2 = Cuboid::new(Vector3::new(1.0, 1.0, 1.0));
116///
117/// // Rotate and position cube2 so edge-edge contact is likely
118/// let pos12 = Isometry3::translation(2.0, 2.0, 0.0);
119///
120/// let (separation, _axis) = cuboid_cuboid_find_local_separating_edge_twoway(
121///     &cube1,
122///     &cube2,
123///     &pos12
124/// );
125///
126/// if separation > 0.0 {
127///     println!("Separated by {} along an edge-edge axis", separation);
128/// }
129/// # }
130/// ```
131///
132/// # Note
133///
134/// This function only tests edge-edge axes. For a complete SAT test, you must also test
135/// face normals using [`cuboid_cuboid_find_local_separating_normal_oneway`].
136#[cfg(feature = "dim3")]
137pub fn cuboid_cuboid_find_local_separating_edge_twoway(
138    cuboid1: &Cuboid,
139    cuboid2: &Cuboid,
140    pos12: &Isometry<Real>,
141) -> (Real, Vector<Real>) {
142    use approx::AbsDiffEq;
143    let mut best_separation = -Real::MAX;
144    let mut best_dir = Vector::zeros();
145
146    let x2 = pos12 * Vector::x();
147    let y2 = pos12 * Vector::y();
148    let z2 = pos12 * Vector::z();
149
150    // We have 3 * 3 = 9 axes to test.
151    let axes = [
152        // Vector::{x, y ,z}().cross(y2)
153        Vector::new(0.0, -x2.z, x2.y),
154        Vector::new(x2.z, 0.0, -x2.x),
155        Vector::new(-x2.y, x2.x, 0.0),
156        // Vector::{x, y ,z}().cross(y2)
157        Vector::new(0.0, -y2.z, y2.y),
158        Vector::new(y2.z, 0.0, -y2.x),
159        Vector::new(-y2.y, y2.x, 0.0),
160        // Vector::{x, y ,z}().cross(y2)
161        Vector::new(0.0, -z2.z, z2.y),
162        Vector::new(z2.z, 0.0, -z2.x),
163        Vector::new(-z2.y, z2.x, 0.0),
164    ];
165
166    for axis1 in &axes {
167        let norm1 = axis1.norm();
168        if norm1 > Real::default_epsilon() {
169            let (separation, axis1) = cuboid_cuboid_compute_separation_wrt_local_line(
170                cuboid1,
171                cuboid2,
172                pos12,
173                &(axis1 / norm1),
174            );
175
176            if separation > best_separation {
177                best_separation = separation;
178                best_dir = axis1;
179            }
180        }
181    }
182
183    (best_separation, best_dir)
184}
185
186/// Finds the best separating axis by testing the face normals of the first cuboid.
187///
188/// This function tests the face normals (X, Y, Z axes in 2D/3D) of `cuboid1` to find which
189/// direction gives the maximum separation between the two cuboids. This is the "one-way" test
190/// that only considers faces from one cuboid.
191///
192/// # Why "One-Way"?
193///
194/// For a complete SAT test between two cuboids, you need to test:
195/// 1. Face normals from cuboid1 (this function)
196/// 2. Face normals from cuboid2 (call this function again with swapped arguments)
197/// 3. Edge-edge cross products in 3D (`cuboid_cuboid_find_local_separating_edge_twoway`)
198///
199/// By testing only one shape's normals at a time, the implementation can be more efficient
200/// and reusable.
201///
202/// # Parameters
203///
204/// - `cuboid1`: The cuboid whose face normals will be tested
205/// - `cuboid2`: The other cuboid
206/// - `pos12`: The position of cuboid2 relative to cuboid1
207///
208/// # Returns
209///
210/// A tuple containing:
211/// - `Real`: The maximum separation found among cuboid1's face normals
212///   - **Positive**: Shapes are separated by at least this distance
213///   - **Negative**: Shapes are overlapping (penetration)
214/// - `Vector<Real>`: The face normal direction that gives this separation
215///
216/// # Example
217///
218/// ```rust
219/// # #[cfg(all(feature = "dim2", feature = "f32"))] {
220/// use parry2d::shape::Cuboid;
221/// use parry2d::query::sat::cuboid_cuboid_find_local_separating_normal_oneway;
222/// use nalgebra::{Isometry2, Vector2};
223///
224/// let rect1 = Cuboid::new(Vector2::new(1.0, 1.0));
225/// let rect2 = Cuboid::new(Vector2::new(0.5, 0.5));
226///
227/// // Position rect2 to the right of rect1
228/// let pos12 = Isometry2::translation(2.5, 0.0);
229///
230/// // Test rect1's face normals (X and Y axes)
231/// let (sep1, normal1) = cuboid_cuboid_find_local_separating_normal_oneway(
232///     &rect1, &rect2, &pos12
233/// );
234///
235/// // Test rect2's face normals by swapping arguments and inverting the transform
236/// let (sep2, normal2) = cuboid_cuboid_find_local_separating_normal_oneway(
237///     &rect2, &rect1, &pos12.inverse()
238/// );
239///
240/// // The maximum separation indicates if shapes collide
241/// let max_separation = sep1.max(sep2);
242/// if max_separation > 0.0 {
243///     println!("Shapes are separated!");
244/// }
245/// # }
246/// ```
247///
248/// # Algorithm Details
249///
250/// For each axis (X, Y, and Z in 3D):
251/// 1. Computes the support point on cuboid2 in the negative axis direction
252/// 2. Transforms it to cuboid1's space
253/// 3. Measures the signed distance from cuboid1's boundary
254/// 4. Tracks the axis with maximum separation
255pub fn cuboid_cuboid_find_local_separating_normal_oneway(
256    cuboid1: &Cuboid,
257    cuboid2: &Cuboid,
258    pos12: &Isometry<Real>,
259) -> (Real, Vector<Real>) {
260    let mut best_separation = -Real::MAX;
261    let mut best_dir = Vector::zeros();
262
263    for i in 0..DIM {
264        #[expect(clippy::unnecessary_cast)]
265        let sign = (1.0 as Real).copysign(pos12.translation.vector[i]);
266        let axis1 = Vector::ith(i, sign);
267        let axis2 = pos12.inverse_transform_vector(&-axis1);
268        let local_pt2 = cuboid2.local_support_point(&axis2);
269        let pt2 = pos12 * local_pt2;
270        let separation = pt2[i] * sign - cuboid1.half_extents[i];
271
272        if separation > best_separation {
273            best_separation = separation;
274            best_dir = axis1;
275        }
276    }
277
278    (best_separation, best_dir)
279}
parry3d/query/sat/sat_cuboid_cuboid.rs

parry3d/query/sat/
sat_cuboid_cuboid.rs
