parry3d/query/sat/
sat_cuboid_cuboid.rs

1use crate::math::{Isometry, Real, Vector, DIM};
2use crate::shape::{Cuboid, SupportMap};
3
4/// Computes the separation of two cuboids along `axis1`.
5#[cfg(feature = "dim3")]
6pub fn cuboid_cuboid_compute_separation_wrt_local_line(
7    cuboid1: &Cuboid,
8    cuboid2: &Cuboid,
9    pos12: &Isometry<Real>,
10    axis1: &Vector<Real>,
11) -> (Real, Vector<Real>) {
12    #[expect(clippy::unnecessary_cast)]
13    let signum = (1.0 as Real).copysign(pos12.translation.vector.dot(axis1));
14    let axis1 = axis1 * signum;
15    let axis2 = pos12.inverse_transform_vector(&-axis1);
16    let local_pt1 = cuboid1.local_support_point(&axis1);
17    let local_pt2 = cuboid2.local_support_point(&axis2);
18    let pt2 = pos12 * local_pt2;
19    let separation = (pt2 - local_pt1).dot(&axis1);
20    (separation, axis1)
21}
22
23/// Finds the best separating edge between two cuboids.
24///
25/// All combinations of edges from both cuboids are taken into
26/// account.
27#[cfg(feature = "dim3")]
28pub fn cuboid_cuboid_find_local_separating_edge_twoway(
29    cuboid1: &Cuboid,
30    cuboid2: &Cuboid,
31    pos12: &Isometry<Real>,
32) -> (Real, Vector<Real>) {
33    use approx::AbsDiffEq;
34    let mut best_separation = -Real::MAX;
35    let mut best_dir = Vector::zeros();
36
37    let x2 = pos12 * Vector::x();
38    let y2 = pos12 * Vector::y();
39    let z2 = pos12 * Vector::z();
40
41    // We have 3 * 3 = 9 axes to test.
42    let axes = [
43        // Vector::{x, y ,z}().cross(y2)
44        Vector::new(0.0, -x2.z, x2.y),
45        Vector::new(x2.z, 0.0, -x2.x),
46        Vector::new(-x2.y, x2.x, 0.0),
47        // Vector::{x, y ,z}().cross(y2)
48        Vector::new(0.0, -y2.z, y2.y),
49        Vector::new(y2.z, 0.0, -y2.x),
50        Vector::new(-y2.y, y2.x, 0.0),
51        // Vector::{x, y ,z}().cross(y2)
52        Vector::new(0.0, -z2.z, z2.y),
53        Vector::new(z2.z, 0.0, -z2.x),
54        Vector::new(-z2.y, z2.x, 0.0),
55    ];
56
57    for axis1 in &axes {
58        let norm1 = axis1.norm();
59        if norm1 > Real::default_epsilon() {
60            let (separation, axis1) = cuboid_cuboid_compute_separation_wrt_local_line(
61                cuboid1,
62                cuboid2,
63                pos12,
64                &(axis1 / norm1),
65            );
66
67            if separation > best_separation {
68                best_separation = separation;
69                best_dir = axis1;
70            }
71        }
72    }
73
74    (best_separation, best_dir)
75}
76
77/// Finds the best separating normal between two cuboids.
78///
79/// Only the normals from `cuboid1` are tested.
80pub fn cuboid_cuboid_find_local_separating_normal_oneway(
81    cuboid1: &Cuboid,
82    cuboid2: &Cuboid,
83    pos12: &Isometry<Real>,
84) -> (Real, Vector<Real>) {
85    let mut best_separation = -Real::MAX;
86    let mut best_dir = Vector::zeros();
87
88    for i in 0..DIM {
89        #[expect(clippy::unnecessary_cast)]
90        let sign = (1.0 as Real).copysign(pos12.translation.vector[i]);
91        let axis1 = Vector::ith(i, sign);
92        let axis2 = pos12.inverse_transform_vector(&-axis1);
93        let local_pt2 = cuboid2.local_support_point(&axis2);
94        let pt2 = pos12 * local_pt2;
95        let separation = pt2[i] * sign - cuboid1.half_extents[i];
96
97        if separation > best_separation {
98            best_separation = separation;
99            best_dir = axis1;
100        }
101    }
102
103    (best_separation, best_dir)
104}
parry3d/query/sat/sat_cuboid_cuboid.rs

parry3d/query/sat/
sat_cuboid_cuboid.rs
