1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79

use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext;
use crate::geometry::{sat, Proximity, Shape};
use crate::math::Isometry;
use ncollide::shape::Cuboid;

pub fn detect_proximity_cuboid_cuboid(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity {
    if let (Shape::Cuboid(cube1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) {
        detect_proximity(
            ctxt.prediction_distance,
            cube1,
            ctxt.position1,
            cube2,
            ctxt.position2,
        )
    } else {
        unreachable!()
    }
}

pub fn detect_proximity<'a>(
    prediction_distance: f32,
    cube1: &'a Cuboid<f32>,
    pos1: &'a Isometry<f32>,
    cube2: &'a Cuboid<f32>,
    pos2: &'a Isometry<f32>,
) -> Proximity {
    let pos12 = pos1.inverse() * pos2;
    let pos21 = pos12.inverse();

    /*
     *
     * Point-Face
     *
     */
    let sep1 =
        sat::cuboid_cuboid_find_local_separating_normal_oneway(cube1, cube2, &pos12, &pos21).0;
    if sep1 > prediction_distance {
        return Proximity::Disjoint;
    }

    let sep2 =
        sat::cuboid_cuboid_find_local_separating_normal_oneway(cube2, cube1, &pos21, &pos12).0;
    if sep2 > prediction_distance {
        return Proximity::Disjoint;
    }

    /*
     *
     * Edge-Edge cases
     *
     */
    #[cfg(feature = "dim2")]
    let sep3 = -f32::MAX; // This case does not exist in 2D.
    #[cfg(feature = "dim3")]
    let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cube1, cube2, &pos12, &pos21).0;
    if sep3 > prediction_distance {
        return Proximity::Disjoint;
    }

    if sep2 > sep1 && sep2 > sep3 {
        if sep2 > 0.0 {
            Proximity::WithinMargin
        } else {
            Proximity::Intersecting
        }
    } else if sep3 > sep1 {
        if sep3 > 0.0 {
            Proximity::WithinMargin
        } else {
            Proximity::Intersecting
        }
    } else {
        if sep1 > 0.0 {
            Proximity::WithinMargin
        } else {
            Proximity::Intersecting
        }
    }
}