ncollide3d/query/point/
point_aabb.rs

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

ncollide3d/query/point/
point_aabb.rs
