Initial implementation of bvh construction

src/bvh.rs

@@ -1,5 +1,3 @@
-use vecmath::vec3_add;
-
 use crate::{primitives::Box3, stl::parser::StlSolid};
 
 /// A node representing a node single in the bounding volume hierarchy
@@ -15,6 +13,7 @@ struct BvhNode {
 /// Struct representing the total bounding volume hierarchy
 pub struct Bvh {
     nodes: Vec<BvhNode>,
+    tri_idx: Vec<usize>,
 }
 
 /// Return type for get_subdivision_position_and_axis
@@ -24,18 +23,8 @@ struct SplitPlane {
 }
 
 impl Bvh {
-    /// Create a new Bounding volume hierarchy from an STL solid
+    /// Create a new Bounding volume hierarchy from an STL solid.
     pub fn new(solid: &StlSolid) -> Self {
-        // First we need to get all of the triangle centroids
-        let centroids: Vec<[f32; 3]> = solid
-            .triangles
-            .iter()
-            .map(|tri| {
-                tri.iter()
-                    .fold([0.0; 3], |acc, idx| vec3_add(solid.vertices[*idx], acc))
-            })
-            .collect();
-
         // Initialize the root node
         let mut nodes = vec![BvhNode {
             prim_idx: 0,
@@ -46,15 +35,90 @@ impl Bvh {
             .aabb
             .shrink_wrap_primitives(&solid.vertices, &solid.triangles);
 
-        let mut bvh = Bvh { nodes };
+        let mut bvh = Bvh {
+            nodes,
+            tri_idx: Vec::from_iter(0..solid.triangles.len()),
+        };
 
-        bvh.subdivide(0);
+        bvh.subdivide(0, solid);
         bvh
     }
 
     /// Subdivide a node, subdivision doesn't occur if the node has only 2 primitives
-    pub fn subdivide(&mut self, node_idx: usize) {
-        let split_plane = self.get_split_plane(node_idx);
+    pub fn subdivide(&mut self, node_idx: usize, solid: &StlSolid) {
+        let SplitPlane {
+            axis: split_axis,
+            pos: split_position,
+        } = self.get_split_plane(node_idx);
+
+        let prim_count = self.nodes[node_idx].prim_count;
+        let prim_idx = self.nodes[node_idx].prim_idx;
+        // Reorganize the triangles so the triangles are ordered, this allows triangles belonging
+        // to a specific node to be adjacent
+        let mut start_idx = prim_idx;
+        let mut end_idx = prim_idx + prim_count - 1;
+        while start_idx <= end_idx {
+            if solid.triangle_centroids[self.tri_idx[start_idx]][split_axis] < split_position as f32
+            {
+                start_idx += 1;
+            } else {
+                self.tri_idx.swap(start_idx, end_idx);
+                end_idx -= 1;
+            }
+        }
+
+        // Now split the primitives into two nodes
+        let left_count = start_idx - prim_idx;
+        let right_count = prim_count - left_count;
+
+        // If all primitives belong to either the left or right half exit because the parent node
+        // is a leaf
+        if left_count == 0 || left_count == prim_count {
+            return;
+        }
+
+        // Otherwise split the nodes into left and right child
+        let left_child = self.nodes.len();
+        let right_child = self.nodes.len() + 1;
+
+        // FIX: Having to copy the [usize; 3] that defines each triangle just to create the
+        // axis-aligned bounding box seems wasteful although it makes the code interface a lot
+        // cleaner. Maybe spend some time to redesign this part to make it cleaner once we really
+        // start focusing on performance tuning
+        let left_prim_triangles: Vec<_> = (prim_idx..left_count)
+            .map(|idx| solid.triangles[idx])
+            .collect();
+        let mut aabb = Box3::default();
+        aabb.shrink_wrap_primitives(&solid.vertices, &left_prim_triangles);
+
+        self.nodes.push(BvhNode {
+            aabb,
+            left_child: 0,
+            right_child: 0,
+            prim_idx,
+            prim_count: left_count,
+        });
+
+        let right_prim_triangles: Vec<_> = ((prim_idx + left_count)..prim_count)
+            .map(|idx| solid.triangles[idx])
+            .collect();
+        let mut aabb = Box3::default();
+        aabb.shrink_wrap_primitives(&solid.vertices, &right_prim_triangles);
+
+        self.nodes.push(BvhNode {
+            aabb,
+            left_child: 0,
+            right_child: 0,
+            prim_idx: prim_idx + left_count,
+            prim_count: right_count,
+        });
+        self.nodes[node_idx].prim_count = 0;
+        self.nodes[node_idx].left_child = left_child;
+        self.nodes[node_idx].right_child = right_child;
+
+        // Recurse to divide children
+        self.subdivide(left_child, solid);
+        self.subdivide(right_child, solid);
     }
 
     /// Calculate the optimal split plane axis and position.

src/stl/parser.rs

@@ -4,6 +4,7 @@ use std::collections::HashMap;
 use std::fs::File;
 use std::io::{BufRead, BufReader, Read};
 use std::path::Path;
+use vecmath::vec3_add;
 
 /// Representation of an STL mesh
 /// Vertices are stored in vec with repeat vertices removed. Each vertex is represented as an
@@ -12,6 +13,7 @@ use std::path::Path;
 pub struct StlSolid {
     pub vertices: Vec<[f32; 3]>,
     pub triangles: Vec<[usize; 3]>,
+    pub triangle_centroids: Vec<[f32; 3]>,
     pub normals: Vec<[f32; 3]>,
 }
 
@@ -110,9 +112,11 @@ pub fn parse_ascii_stl(file_path: &impl AsRef<Path>) -> Result<StlSolid> {
         ];
     }
 
+    let triangle_centroids = calc_triangle_centroids(&vertices, &triangles);
     Ok(StlSolid {
         vertices,
         triangles,
+        triangle_centroids,
         normals,
     })
 }
@@ -183,13 +187,26 @@ pub fn parse_binary_stl(file_path: &impl AsRef<Path>) -> Result<StlSolid> {
         triangles.push(indices);
     }
 
+    let triangle_centroids = calc_triangle_centroids(&vertices, &triangles);
     Ok(StlSolid {
         vertices,
         triangles,
+        triangle_centroids,
         normals,
     })
 }
 
+/// Calculate the centroids for each triangle in a list
+fn calc_triangle_centroids(vertices: &[[f32; 3]], triangles: &[[usize; 3]]) -> Vec<[f32; 3]> {
+    triangles
+        .iter()
+        .map(|tri| {
+            tri.iter()
+                .fold([0.0; 3], |acc, idx| vec3_add(vertices[*idx], acc))
+        })
+        .collect()
+}
+
 #[cfg(test)]
 pub mod test {
     use super::*;

src/utilities/intersection_checks.rs

@@ -34,7 +34,7 @@ pub fn tri_aabb_intersect(bx: &Box3, tri: &Triangle) -> bool {
             return false;
         }
     }
-    return true;
+    true
 }
 
 /// Project points unto an Axis and return the max and min