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