parry3d/query/sat/
sat_cuboid_point.rs

1use crate::math::{Isometry, Point, Real, Vector};
2use crate::shape::{Cuboid, SupportMap};
3
4use na::Unit;
5
6// NOTE: this only works with cuboid on the rhs because it has its symmetry origin at zero
7// (therefore we can check only one normal direction).
8/// Computes the separation between a point and a cuboid, along the given direction `normal1`.
9pub fn point_cuboid_find_local_separating_normal_oneway(
10    point1: Point<Real>,
11    normal1: Option<Unit<Vector<Real>>>,
12    shape2: &Cuboid,
13    pos12: &Isometry<Real>,
14) -> (Real, Vector<Real>) {
15    let mut best_separation = -Real::MAX;
16    let mut best_dir = Vector::zeros();
17
18    if let Some(normal1) = normal1 {
19        let axis1 = if (pos12.translation.vector - point1.coords).dot(&normal1) >= 0.0 {
20            normal1
21        } else {
22            -normal1
23        };
24
25        let pt2 = shape2.support_point_toward(pos12, &-axis1);
26        let separation = (pt2 - point1).dot(&axis1);
27
28        if separation > best_separation {
29            best_separation = separation;
30            best_dir = *axis1;
31        }
32    }
33
34    (best_separation, best_dir)
35}