parry3d/query/point/
point_aabb.rs

1use crate::bounding_volume::Aabb;
2use crate::math::{Point, Real, Vector, DIM};
3use crate::num::{Bounded, Zero};
4use crate::query::{PointProjection, PointQuery};
5use crate::shape::FeatureId;
6use na;
7
8impl Aabb {
9    fn do_project_local_point(
10        &self,
11        pt: &Point<Real>,
12        solid: bool,
13    ) -> (bool, Point<Real>, Vector<Real>) {
14        let mins_pt = self.mins - pt;
15        let pt_maxs = pt - self.maxs;
16        let shift = mins_pt.sup(&na::zero()) - pt_maxs.sup(&na::zero());
17
18        let inside = shift.is_zero();
19
20        if !inside {
21            (false, pt + shift, shift)
22        } else if solid {
23            (true, *pt, shift)
24        } else {
25            let _max: Real = Bounded::max_value();
26            let mut best = -_max;
27            let mut is_mins = false;
28            let mut best_id = 0;
29
30            for i in 0..DIM {
31                let mins_pt_i = mins_pt[i];
32                let pt_maxs_i = pt_maxs[i];
33
34                if mins_pt_i < pt_maxs_i {
35                    if pt_maxs[i] > best {
36                        best_id = i;
37                        is_mins = false;
38                        best = pt_maxs_i
39                    }
40                } else if mins_pt_i > best {
41                    best_id = i;
42                    is_mins = true;
43                    best = mins_pt_i
44                }
45            }
46
47            let mut shift: Vector<Real> = na::zero();
48
49            if is_mins {
50                shift[best_id] = best;
51            } else {
52                shift[best_id] = -best;
53            }
54
55            (inside, pt + shift, shift)
56        }
57    }
58}
59
60impl PointQuery for Aabb {
61    #[inline]
62    fn project_local_point(&self, pt: &Point<Real>, solid: bool) -> PointProjection {
63        let (inside, ls_pt, _) = self.do_project_local_point(pt, solid);
64        PointProjection::new(inside, ls_pt)
65    }
66
67    #[allow(unused_assignments)] // For last_zero_shift which is used only in 3D.
68    #[allow(unused_variables)] // For last_zero_shift which is used only in 3D.
69    #[inline]
70    fn project_local_point_and_get_feature(
71        &self,
72        pt: &Point<Real>,
73    ) -> (PointProjection, FeatureId) {
74        let (inside, ls_pt, shift) = self.do_project_local_point(pt, false);
75        let proj = PointProjection::new(inside, ls_pt);
76        let mut nzero_shifts = 0;
77        let mut last_zero_shift = 0;
78        let mut last_not_zero_shift = 0;
79
80        for i in 0..DIM {
81            if shift[i].is_zero() {
82                nzero_shifts += 1;
83                last_zero_shift = i;
84            } else {
85                last_not_zero_shift = i;
86            }
87        }
88
89        if nzero_shifts == DIM {
90            for i in 0..DIM {
91                if ls_pt[i] > self.maxs[i] - crate::math::DEFAULT_EPSILON {
92                    return (proj, FeatureId::Face(i as u32));
93                }
94                if ls_pt[i] <= self.mins[i] + crate::math::DEFAULT_EPSILON {
95                    return (proj, FeatureId::Face((i + DIM) as u32));
96                }
97            }
98
99            (proj, FeatureId::Unknown)
100        } else if nzero_shifts == DIM - 1 {
101            // On a 3D face.
102            if ls_pt[last_not_zero_shift] < self.center()[last_not_zero_shift] {
103                (proj, FeatureId::Face((last_not_zero_shift + DIM) as u32))
104            } else {
105                (proj, FeatureId::Face(last_not_zero_shift as u32))
106            }
107        } else {
108            // On a vertex or edge.
109            let mut id = 0;
110            let center = self.center();
111
112            for i in 0..DIM {
113                if ls_pt[i] < center[i] {
114                    id |= 1 << i;
115                }
116            }
117
118            #[cfg(feature = "dim3")]
119            {
120                if nzero_shifts == 0 {
121                    (proj, FeatureId::Vertex(id))
122                } else {
123                    (proj, FeatureId::Edge((id << 2) | (last_zero_shift as u32)))
124                }
125            }
126
127            #[cfg(feature = "dim2")]
128            {
129                (proj, FeatureId::Vertex(id))
130            }
131        }
132    }
133
134    #[inline]
135    fn distance_to_local_point(&self, pt: &Point<Real>, solid: bool) -> Real {
136        let mins_pt = self.mins - pt;
137        let pt_maxs = pt - self.maxs;
138        let shift = mins_pt.sup(&pt_maxs).sup(&na::zero());
139
140        if solid || !shift.is_zero() {
141            shift.norm()
142        } else {
143            // TODO: optimize that.
144            -na::distance(pt, &self.project_local_point(pt, solid).point)
145        }
146    }
147}
parry3d/query/point/point_aabb.rs

parry3d/query/point/
point_aabb.rs
