parry3d/query/sat/
sat_cuboid_support_map.rs

1use crate::math::{Isometry, Real, Vector, DIM};
2use crate::shape::{Cuboid, SupportMap};
3
4use na::Unit;
5
6/// Computes the separation between a cuboid an a convex shape implementing the `SupportMap` trait,
7/// along the given axis.
8// TODO: this is a very slow approach. We should only do special-cases instead.
9#[cfg(feature = "dim3")]
10pub fn cuboid_support_map_compute_separation_wrt_local_line(
11    cube1: &Cuboid,
12    shape2: &impl SupportMap,
13    pos12: &Isometry<Real>,
14    axis1: &Unit<Vector<Real>>,
15) -> (Real, Unit<Vector<Real>>) {
16    let axis1_2 = pos12.inverse_transform_unit_vector(axis1);
17    let separation1 = {
18        let axis2 = -axis1_2;
19        let local_pt1 = cube1.local_support_point_toward(axis1);
20        let local_pt2 = shape2.local_support_point_toward(&axis2);
21        let pt2 = pos12 * local_pt2;
22        (pt2 - local_pt1).dot(axis1)
23    };
24
25    let separation2 = {
26        let axis2 = axis1_2;
27        let local_pt1 = cube1.local_support_point_toward(&-*axis1);
28        let local_pt2 = shape2.local_support_point_toward(&axis2);
29        let pt2 = pos12 * local_pt2;
30        (pt2 - local_pt1).dot(&-*axis1)
31    };
32
33    if separation1 > separation2 {
34        (separation1, *axis1)
35    } else {
36        (separation2, -*axis1)
37    }
38}
39
40/// Finds the best separating edge between a cuboid and a convex shape implementing the `Supportmap` trait.
41///
42/// Only the axes given by `axes` are tested.
43#[cfg(feature = "dim3")]
44pub fn cuboid_support_map_find_local_separating_edge_twoway(
45    cube1: &Cuboid,
46    shape2: &impl SupportMap,
47    axes: &[Vector<Real>],
48    pos12: &Isometry<Real>,
49) -> (Real, Vector<Real>) {
50    use approx::AbsDiffEq;
51    let mut best_separation = -Real::MAX;
52    let mut best_dir = Vector::zeros();
53
54    for axis1 in axes {
55        if let Some(axis1) = Unit::try_new(*axis1, Real::default_epsilon()) {
56            let (separation, axis1) =
57                cuboid_support_map_compute_separation_wrt_local_line(cube1, shape2, pos12, &axis1);
58
59            if separation > best_separation {
60                best_separation = separation;
61                best_dir = *axis1;
62            }
63        }
64    }
65
66    (best_separation, best_dir)
67}
68
69/// Finds the best separating normal between a cuboid and a convex shape implementing the `SupportMap` trait.
70///
71/// Only the normals of `cube1` are tested.
72pub fn cuboid_support_map_find_local_separating_normal_oneway<S: SupportMap>(
73    cube1: &Cuboid,
74    shape2: &S,
75    pos12: &Isometry<Real>,
76) -> (Real, Vector<Real>) {
77    let mut best_separation = -Real::MAX;
78    let mut best_dir = Vector::zeros();
79
80    for i in 0..DIM {
81        for sign in &[-1.0, 1.0] {
82            let axis1 = Vector::ith(i, *sign);
83            let pt2 = shape2.support_point_toward(pos12, &Unit::new_unchecked(-axis1));
84            let separation = pt2[i] * *sign - cube1.half_extents[i];
85
86            if separation > best_separation {
87                best_separation = separation;
88                best_dir = axis1;
89            }
90        }
91    }
92
93    (best_separation, best_dir)
94}
parry3d/query/sat/sat_cuboid_support_map.rs

parry3d/query/sat/
sat_cuboid_support_map.rs
