1#[cfg(test)]
2mod test;
3
4use crate::{
5 geometry::{
6 Coordinate, Coordinates,
7 bbox::BoundingBox,
8 bvh::{
9 BoundingVolumeHierarchy, Hit,
10 node::{Node, NodeKind},
11 primitive::Primitive,
12 ray::Ray,
13 },
14 },
15 math::Scalar,
16};
17
18impl<const D: usize> BoundingVolumeHierarchy<D> {
19 pub fn build_node(&mut self, primitives: &mut [Primitive<D>], leaf_size: usize) -> usize {
20 assert!(leaf_size > 0);
21 assert!(!primitives.is_empty());
22 let bounding_box = BoundingBox::from(&primitives[..]);
23 let node_index = self.nodes.len();
24 self.nodes.push(Node::from((
25 &bounding_box,
26 NodeKind::Leaf { start: 0, end: 0 },
27 )));
28 if primitives.len() <= leaf_size {
29 let start = self.items.len();
30 self.items
31 .extend(primitives.iter().map(|primitive| primitive.index()));
32 let end = self.items.len();
33 self.nodes[node_index] = Node::from((bounding_box, NodeKind::Leaf { start, end }));
34 return node_index;
35 }
36 let axis = bounding_box.longest_axis();
37 let mid = primitives.len() / 2;
38 primitives.select_nth_unstable_by(mid, |a, b| {
39 a.centroid()[axis].partial_cmp(&b.centroid()[axis]).unwrap()
40 });
41 let (left_primitives, right_primitives) = primitives.split_at_mut(mid);
42 let left = self.build_node(left_primitives, leaf_size);
43 let right = self.build_node(right_primitives, leaf_size);
44 self.nodes[node_index] = Node::from((bounding_box, NodeKind::Tree { left, right }));
45 node_index
46 }
47}
48
49impl BoundingVolumeHierarchy<3> {
50 pub fn intersect(
51 &self,
52 ray: &Ray<3>,
53 coordinates: &Coordinates<3>,
54 elements: &[&[usize]],
55 ) -> Option<Hit> {
56 let mut hit = None;
57 if !self.nodes.is_empty()
58 && let Some(entry) = ray.intersects(self.nodes[0].bounding_box())
59 {
60 self.intersect_node(0, entry, ray, coordinates, elements, &mut hit);
61 }
62 hit
63 }
64 pub fn intersections(
65 &self,
66 ray: &Ray<3>,
67 coordinates: &Coordinates<3>,
68 elements: &[&[usize]],
69 ) -> usize {
70 let mut count = 0;
71 if !self.nodes.is_empty() {
72 self.count_node(0, ray, coordinates, elements, &mut count);
73 }
74 count
75 }
76 fn count_node(
77 &self,
78 node_index: usize,
79 ray: &Ray<3>,
80 coordinates: &Coordinates<3>,
81 elements: &[&[usize]],
82 count: &mut usize,
83 ) {
84 let node = &self.nodes[node_index];
85 if ray.intersects(node.bounding_box()).is_none() {
86 return;
87 }
88 match node.kind() {
89 NodeKind::Leaf { start, end } => {
90 self.items[*start..*end].iter().for_each(|&item| {
91 let element = elements[item];
92 if ray
93 .intersects_triangle(
94 &coordinates[element[0]],
95 &coordinates[element[1]],
96 &coordinates[element[2]],
97 )
98 .is_some()
99 {
100 *count += 1;
101 }
102 });
103 }
104 NodeKind::Tree { left, right } => {
105 self.count_node(*left, ray, coordinates, elements, count);
106 self.count_node(*right, ray, coordinates, elements, count);
107 }
108 }
109 }
110 fn intersect_node(
111 &self,
112 node_index: usize,
113 entry: Scalar,
114 ray: &Ray<3>,
115 coordinates: &Coordinates<3>,
116 elements: &[&[usize]],
117 hit: &mut Option<Hit>,
118 ) {
119 if hit
120 .as_ref()
121 .is_some_and(|closest| entry >= closest.distance())
122 {
123 return;
124 }
125 let node = &self.nodes[node_index];
126 match node.kind() {
127 NodeKind::Leaf { start, end } => {
128 self.items[*start..*end].iter().for_each(|&item| {
129 let element = elements[item];
130 if let Some(distance) = ray.intersects_triangle(
131 &coordinates[element[0]],
132 &coordinates[element[1]],
133 &coordinates[element[2]],
134 ) && hit
135 .as_ref()
136 .is_none_or(|closest| distance < closest.distance())
137 {
138 *hit = Some(Hit {
139 distance,
140 index: item,
141 });
142 }
143 });
144 }
145 NodeKind::Tree { left, right } => {
146 let left_entry = ray.intersects(self.nodes[*left].bounding_box());
147 let right_entry = ray.intersects(self.nodes[*right].bounding_box());
148 match (left_entry, right_entry) {
149 (Some(left_entry), Some(right_entry)) => {
150 let (near, near_entry, far, far_entry) = if left_entry <= right_entry {
151 (*left, left_entry, *right, right_entry)
152 } else {
153 (*right, right_entry, *left, left_entry)
154 };
155 self.intersect_node(near, near_entry, ray, coordinates, elements, hit);
156 self.intersect_node(far, far_entry, ray, coordinates, elements, hit);
157 }
158 (Some(left_entry), None) => {
159 self.intersect_node(*left, left_entry, ray, coordinates, elements, hit);
160 }
161 (None, Some(right_entry)) => {
162 self.intersect_node(*right, right_entry, ray, coordinates, elements, hit);
163 }
164 (None, None) => {}
165 }
166 }
167 }
168 }
169 pub fn overlapping(&self, query: &BoundingBox<3>) -> Vec<usize> {
170 let mut found = Vec::new();
171 if !self.nodes.is_empty() {
172 self.overlapping_node(0, query, &mut found);
173 }
174 found
175 }
176 fn overlapping_node(&self, node_index: usize, query: &BoundingBox<3>, found: &mut Vec<usize>) {
177 let node = &self.nodes[node_index];
178 if !query.overlaps(node.bounding_box()) {
179 return;
180 }
181 match node.kind() {
182 NodeKind::Leaf { start, end } => found.extend_from_slice(&self.items[*start..*end]),
183 NodeKind::Tree { left, right } => {
184 self.overlapping_node(*left, query, found);
185 self.overlapping_node(*right, query, found);
186 }
187 }
188 }
189 pub fn closest_point(
190 &self,
191 point: &Coordinate<3>,
192 coordinates: &Coordinates<3>,
193 elements: &[&[usize]],
194 ) -> Option<(Coordinate<3>, usize)> {
195 let mut closest = None;
196 if !self.nodes.is_empty() {
197 self.closest_point_node(0, point, coordinates, elements, &mut closest);
198 }
199 closest.map(|(_, candidate, index)| (candidate, index))
200 }
201 fn closest_point_node(
202 &self,
203 node_index: usize,
204 point: &Coordinate<3>,
205 coordinates: &Coordinates<3>,
206 elements: &[&[usize]],
207 closest: &mut Option<(Scalar, Coordinate<3>, usize)>,
208 ) {
209 let node = &self.nodes[node_index];
210 if closest.as_ref().is_some_and(|(distance, ..)| {
211 point_box_distance_squared(point, node.bounding_box()) >= *distance
212 }) {
213 return;
214 }
215 match node.kind() {
216 NodeKind::Leaf { start, end } => {
217 self.items[*start..*end].iter().for_each(|&item| {
218 let element = elements[item];
219 let candidate = closest_point_on_triangle(
220 point,
221 &coordinates[element[0]],
222 &coordinates[element[1]],
223 &coordinates[element[2]],
224 );
225 let offset = &candidate - point;
226 let distance = &offset * &offset;
227 if closest
228 .as_ref()
229 .is_none_or(|(nearest, ..)| distance < *nearest)
230 {
231 *closest = Some((distance, candidate, item));
232 }
233 });
234 }
235 NodeKind::Tree { left, right } => {
236 let (near, far) =
237 if point_box_distance_squared(point, self.nodes[*left].bounding_box())
238 <= point_box_distance_squared(point, self.nodes[*right].bounding_box())
239 {
240 (*left, *right)
241 } else {
242 (*right, *left)
243 };
244 self.closest_point_node(near, point, coordinates, elements, closest);
245 self.closest_point_node(far, point, coordinates, elements, closest);
246 }
247 }
248 }
249}
250
251fn point_box_distance_squared<const D: usize>(
252 point: &Coordinate<D>,
253 bounding_box: &BoundingBox<D>,
254) -> Scalar {
255 (0..D)
256 .map(|axis| {
257 let value = point[axis];
258 let (low, high) = (bounding_box.minimum()[axis], bounding_box.maximum()[axis]);
259 let delta = if value < low {
260 low - value
261 } else if value > high {
262 value - high
263 } else {
264 0.0
265 };
266 delta * delta
267 })
268 .sum()
269}
270
271fn closest_point_on_triangle(
272 point: &Coordinate<3>,
273 a: &Coordinate<3>,
274 b: &Coordinate<3>,
275 c: &Coordinate<3>,
276) -> Coordinate<3> {
277 let ab = b - a;
278 let ac = c - a;
279 let ap = point - a;
280 let d1 = &ab * ≈
281 let d2 = &ac * ≈
282 if d1 <= 0.0 && d2 <= 0.0 {
283 return a.clone();
284 }
285 let bp = point - b;
286 let d3 = &ab * &bp;
287 let d4 = &ac * &bp;
288 if d3 >= 0.0 && d4 <= d3 {
289 return b.clone();
290 }
291 let vc = d1 * d4 - d3 * d2;
292 if vc <= 0.0 && d1 >= 0.0 && d3 <= 0.0 {
293 return a + &(&ab * (d1 / (d1 - d3)));
294 }
295 let cp = point - c;
296 let d5 = &ab * &cp;
297 let d6 = &ac * &cp;
298 if d6 >= 0.0 && d5 <= d6 {
299 return c.clone();
300 }
301 let vb = d5 * d2 - d1 * d6;
302 if vb <= 0.0 && d2 >= 0.0 && d6 <= 0.0 {
303 return a + &(&ac * (d2 / (d2 - d6)));
304 }
305 let va = d3 * d6 - d5 * d4;
306 if va <= 0.0 && (d4 - d3) >= 0.0 && (d5 - d6) >= 0.0 {
307 return b + &(&(c - b) * ((d4 - d3) / ((d4 - d3) + (d5 - d6))));
308 }
309 let denominator = 1.0 / (va + vb + vc);
310 &(a + &(&ab * (vb * denominator))) + &(&ac * (vc * denominator))
311}