public AABBTreeRayTestResult rayTest(AABBTree[] trees, AABBTreeRay ray, Func <AABBTree, AABBExternalNode, AABBTreeRay, double, double, AABBTreeRayTestResult> callback) { // // // we traverse both trees at once // keeping a priority list of nodes to check next. // TODO: possibly implement priority list more effeciently? // binary heap probably too much overhead in typical case. List <PriorityNode> priorityList = new List <PriorityNode>(); //current upperBound on distance to first intersection //and current closest object properties AABBTreeRayTestResult minimumResult = null; var upperBound = ray.maxFactor; for (int i = 0; i < trees.Length; i += 1) { AABBTree tree = trees[i]; if (tree.getNodeCount() != 0) { upperBound = processNode(tree, ray, 0, upperBound, callback, ref priorityList, ref minimumResult); } } while (priorityList.Count != 0) { var nodeObj = priorityList.Last(); priorityList.RemoveAt(priorityList.Count - 1); // A node inserted into priority list after this one may have // moved the upper bound. if (nodeObj.distance >= upperBound) { continue; } var nodeIndex = nodeObj.nodeIndex; AABBTree tree = nodeObj.tree; var nodes = tree.getNodes(); var node = nodes[nodeIndex]; var maxIndex = nodeIndex + node.escapeNodeOffset; var childIndex = nodeIndex + 1; do { upperBound = processNode(tree, ray, childIndex, upperBound, callback, ref priorityList, ref minimumResult); childIndex += nodes[childIndex].escapeNodeOffset; }while (childIndex < maxIndex); } return(minimumResult); }
//if node is a leaf, intersect ray with shape // otherwise insert node into priority list. double processNode(AABBTree tree, AABBTreeRay ray, int nodeIndex, double upperBound, Func <AABBTree, AABBExternalNode, AABBTreeRay, double, double, AABBTreeRayTestResult> callback, ref List <PriorityNode> priorityList, ref AABBTreeRayTestResult minimumResult) { var nodes = tree.getNodes(); var node = nodes[nodeIndex]; var distance = distanceExtents(ray, node.extents, upperBound); if (distance == null) { return(upperBound); } if (node.externalNode != null) { var result = callback(tree, node.externalNode, ray, distance.Value, upperBound); if (result != null) { minimumResult = result; upperBound = result.factor; } } else { // TODO: change to binary search? var length = priorityList.Count; int i; for (i = 0; i < length; i += 1) { var curObj = priorityList[i]; if (distance > curObj.distance) { break; } } if (i >= length - 1) { //insert node at index i priorityList.Add(new PriorityNode() { tree = tree, nodeIndex = nodeIndex, distance = distance.Value }); } else { //insert node at index i priorityList.Insert(i + 1, new PriorityNode() { tree = tree, nodeIndex = nodeIndex, distance = distance.Value }); } } return(upperBound); }