     }
     
     deinit {
-        self.boundingBoxes?.deallocate()
+        self.boundingBoxes.deallocate()
     }
     
     func visit(callback: (BVHNode, Int) -> Bool) {
-        guard self.boundingBoxes != nil else {
-            return
+        func visit(index: Int, depth: Int, callback: (BVHNode, Int) -> Bool) {
+            guard callback(BVHNode(boundingBox: self.boundingBoxes[index],
+                                   type: index < self.inodeCount ? .internal: .leaf(elementIndex: BVH.leafNodeIndexToElementIndex(index, leafCount: self.elementCount, lastRowIndex: self.lastRowIndex))),
+                           depth) == true else {
+                return
+            }
+            if index < self.inodeCount {
+                let nextDepth = depth + 1
+                visit(index: 2*index+1, depth: nextDepth, callback: callback)
+                visit(index: 2*index+2, depth: nextDepth, callback: callback)
+            }
         }
-        root.visit(callback: callback)
+        visit(index: 0, depth: 0, callback: callback)
     }
     
     func intersects(callback: (Int, Int) -> Void) {
         let inodecount = self.inodeCount
-        guard let boxes = self.boundingBoxes else {
-            return
-        }
         func intersects(index: Int, callback: (Int, Int) -> Void) {
             if index >= inodecount { // if it's a leaf node
                 let elementIndex1 = BVH.leafNodeIndexToElementIndex(index, leafCount: self.elementCount, lastRowIndex: lastRowIndex)


internal class BVH {
    
    fileprivate let boundingBoxes: UnsafePointer<BoundingBox>?
    fileprivate var inodeCount: Int
    fileprivate var lastRowIndex: Int
    fileprivate var elementCount: Int
    
    fileprivate var root: BVHNode {
        return BVHNode(index: 0, bvh: Unmanaged.passUnretained(self) )
    }
    
    var boundingBox: BoundingBox {
        if let boxes = self.boundingBoxes {
            return boxes[0]
        }
        else {
            return BoundingBox.empty
        }
    }
    
    fileprivate static func leafNodeIndexToElementIndex(_ nodeIndex: Int, leafCount: Int, lastRowIndex: Int) -> Int {

    
    init(boxes leafBoxes: [BoundingBox]) {
        // create a complete binary tree of bounding boxes where boxes[0] is the root and left child is 2*index+1 and right child is 2*index+2
        assert(leafBoxes.count > 0)
        let boxes = UnsafeMutablePointer<BoundingBox>.allocate(capacity: 2*leafBoxes.count-1)
        
        self.elementCount = leafBoxes.count
        self.inodeCount = leafBoxes.count-1
        
        var lastRowIndex = 0
