KDTree.h

Go to the documentation of this file.

#ifndef G3D_KDTree_h
#define G3D_KDTree_h

#include "G3D/platform.h"
#include "G3D/Array.h"
#include "G3D/Table.h"
#include "G3D/Vector2.h"
#include "G3D/Vector3.h"
#include "G3D/Vector4.h"
#include "G3D/AABox.h"
#include "G3D/Sphere.h"
#include "G3D/Box.h"
#include "G3D/Triangle.h"
#include "G3D/Ray.h"
#include "G3D/Frustum.h"
#include "G3D/BinaryInput.h"
#include "G3D/BinaryOutput.h"
#include "G3D/CollisionDetection.h"
#include "G3D/BoundsTrait.h"
#include <algorithm>

// If defined, in debug mode the tree is checked for consistency
// as a way of detecting corruption due to implementation bugs
// #define VERIFY_TREE

template<> struct BoundsTrait<class G3D::Vector2> {
 static void getBounds(const G3D::Vector2& v, G3D::AABox& out) { out = G3D::AABox(G3D::Vector3(v, 0)); }
};

template<> struct BoundsTrait<class G3D::Vector3> {
 static void getBounds(const G3D::Vector3& v, G3D::AABox& out) { out = G3D::AABox(v); }
};

template<> struct BoundsTrait<class G3D::Vector4> {
 static void getBounds(const G3D::Vector4& v, G3D::AABox& out) { out = G3D::AABox(v.xyz()); }
};

template<> struct BoundsTrait<class G3D::AABox> {
 static void getBounds(const G3D::AABox& v, G3D::AABox& out) { out = v; }
};

template<> struct BoundsTrait<class G3D::Sphere> {
 static void getBounds(const G3D::Sphere& s, G3D::AABox& out) { s.getBounds(out); }
};

template<> struct BoundsTrait<class G3D::Box> {
 static void getBounds(const G3D::Box& b, G3D::AABox& out) { b.getBounds(out); }
};

template<> struct BoundsTrait<class G3D::Vector2*> {
 static void getBounds(const G3D::Vector2*& v, G3D::AABox& out) { out = G3D::AABox(G3D::Vector3(*v, 0)); }
};

template<> struct BoundsTrait<class G3D::Vector3*> {
 static void getBounds(const G3D::Vector3*& v, G3D::AABox& out) { out = G3D::AABox(*v); }
};

template<> struct BoundsTrait<class G3D::Vector4*> {
 static void getBounds(const G3D::Vector4*& v, G3D::AABox& out) { out = G3D::AABox(v->xyz()); }
};

template<> struct BoundsTrait<class G3D::AABox*> {
 static void getBounds(const G3D::AABox*& v, G3D::AABox& out) { out = *v; }
};

template<> struct BoundsTrait<class G3D::Sphere*> {
 static void getBounds(const G3D::Sphere*& s, G3D::AABox& out) { s->getBounds(out); }
};

template<> struct BoundsTrait<class G3D::Box*> {
 static void getBounds(const G3D::Box*& b, G3D::AABox& out) { b->getBounds(out); }
};


template<> struct BoundsTrait<class G3D::Triangle*> {
 static void getBounds(const G3D::Triangle*& t, G3D::AABox& out) { t->getBounds(out); }
};

namespace G3D {
 namespace _internal {

 template<class Type>
 class Indirector {
 public:
 Type* handle;

 inline Indirector(Type* h) : handle(h) {}

 inline Indirector() : handle(NULL) {}

 inline bool operator==(const Indirector& m) const {
 return *handle == *(m.handle);
 }

 inline bool operator==(const Type& m) const {
 return *handle == m;
 }

 inline size_t hashCode() const {
 return handle->hashCode();
 }
 };
 } // namespace internal
} // namespace G3D

template <class Handle> struct HashTrait<typename G3D::_internal::Indirector<Handle> > {
 static size_t hashCode(const G3D::_internal::Indirector<Handle>& key) { return key.hashCode(); }
};

namespace G3D {

template< class T, 
 class BoundsFunc = BoundsTrait<T>, 
 class HashFunc = HashTrait<T>, 
 class EqualsFunc = EqualsTrait<T> > 
class KDTree {
protected:
#define TreeType KDTree<T, BoundsFunc, HashFunc, EqualsFunc>

 class Handle {
 public:
 AABox bounds;

 Vector3 center;

 T value;

 Handle() {}

 inline Handle(const T& v) : value(v) {
 BoundsFunc::getBounds(v, bounds);
 bounds = bounds.intersect(AABox::large());
 center = bounds.center();
 }

 inline bool operator==(const Handle& other) const {
 return EqualsFunc::equals(value, other.value);
 }

 inline size_t hashCode() const {
 return HashFunc::hashCode(value);
 }
 };

 static AABox computeBounds(
 const Array<Handle*>& point, 
 int beginIndex,
 int endIndex) {
 
 Vector3 lo = Vector3::inf();
 Vector3 hi = -lo;

 debugAssertM(beginIndex <= endIndex, "No points");
 for (int p = beginIndex; p <= endIndex; ++p) {
 // This code is written with the vector min and max expanded
 // because otherwise it compiles incorrectly with -O3 on
 // gcc 3.4 

 const Vector3& pLo = point[p]->bounds.low();
 const Vector3& pHi = point[p]->bounds.high();
 for (int a = 0; a < 3; ++a) {
 lo[a] = G3D::min(lo[a], pLo[a]);
 hi[a] = G3D::max(hi[a], pHi[a]);
 }
 }

 return AABox(lo, hi);
 }

 class CenterComparator {
 public:
 Vector3::Axis sortAxis;

 CenterComparator(Vector3::Axis a) : sortAxis(a) {}

 inline int operator()(Handle* A, const Handle* B) const {
 float a = A->center[sortAxis];
 float b = B->center[sortAxis];

 if (a < b) {
 return 1;
 } else if (a > b) {
 return -1;
 } else {
 return 0;
 }
 }
 };


 class BoundsComparator {
 public:
 Vector3::Axis sortAxis;

 BoundsComparator(Vector3::Axis a) : sortAxis(a) {}

 inline int operator()(Handle* A, const Handle* B) const {
 const AABox& a = A->bounds;
 const AABox& b = B->bounds;

 if (a.high()[sortAxis] < b.low()[sortAxis]) {
 return 1;
 } else if (a.low()[sortAxis] > b.high()[sortAxis]) {
 return -1;
 } else {
 return 0;
 }
 }
 };


 class Comparator {
 public:
 Vector3::Axis sortAxis;
 float sortLocation;

 Comparator(Vector3::Axis a, float l) : sortAxis(a), sortLocation(l) {}

 inline int operator()(Handle* ignore, const Handle* handle) const {
 (void)ignore;
 const AABox& box = handle->bounds;
 debugAssert(ignore == NULL);

 if (box.high()[sortAxis] < sortLocation) {
 // Box is strictly below the sort location
 return -1;
 } else if (box.low()[sortAxis] > sortLocation) {
 // Box is strictly above the sort location
 return 1;
 } else {
 // Box overlaps the sort location
 return 0;
 }
 }
 };

 // Using System::malloc with this class provided no speed improvement.
 class Node {
 public:

 AABox splitBounds;

 Vector3::Axis splitAxis;

 float splitLocation;
 
 Node* child[2];

 Array<Handle*> valueArray;

 Array<AABox> boundsArray;

 Node() {
 splitAxis = Vector3::X_AXIS;
 splitLocation = 0;
 splitBounds = AABox(-Vector3::inf(), Vector3::inf());
 for (int i = 0; i < 2; ++i) {
 child[i] = NULL;
 }
 }

 Node(const Node& other) : valueArray(other.valueArray), boundsArray(other.boundsArray) {
 splitAxis = other.splitAxis;
 splitLocation = other.splitLocation;
 splitBounds = other.splitBounds; 
 for (int i = 0; i < 2; ++i) {
 child[i] = NULL;
 }
 }

 Node(const Array<Handle*>& pt) : valueArray(pt) {
 splitAxis = Vector3::X_AXIS;
 splitLocation = 0;
 for (int i = 0; i < 2; ++i) {
 child[i] = NULL;
 }

 boundsArray.resize(valueArray.size());
 for (int i = 0; i < valueArray.size(); ++i) {
 boundsArray[i] = valueArray[i]->bounds;
 }
 }

 ~Node() {
 for (int i = 0; i < 2; ++i) {
 delete child[i];
 }
 }

 inline bool isLeaf() const {
 return (child[0] == NULL) && (child[1] == NULL);
 }


 void getHandles(Array<Handle*>& handleArray) const {
 handleArray.append(valueArray);
 for (int i = 0; i < 2; ++i) {
 if (child[i] != NULL) {
 child[i]->getHandles(handleArray);
 }
 }
 }

 void verifyNode(const Vector3& lo, const Vector3& hi) {
 // debugPrintf("Verifying: split %d @ %f [%f, %f, %f], [%f, %f, %f]\n",
 // splitAxis, splitLocation, lo.x, lo.y, lo.z, hi.x, hi.y, hi.z);
 
 debugAssertM(lo == splitBounds.low(),
 format("lo = %s, splitBounds.lo = %s",
 lo.toString().c_str(), splitBounds.low().toString().c_str()));
 debugAssert(hi == splitBounds.high());
 
 for (int i = 0; i < valueArray.length(); ++i) {
 const AABox& b = valueArray[i]->bounds;
 debugAssert(b == boundsArray[i]);
 (void)b;
 
 for(int axis = 0; axis < 3; ++axis) {
 debugAssert(b.low()[axis] <= b.high()[axis]);
 debugAssert(b.low()[axis] >= lo[axis]);
 debugAssert(b.high()[axis] <= hi[axis]);
 }
 }
 
 if (child[0] || child[1]) {
 debugAssert(lo[splitAxis] < splitLocation);
 debugAssert(hi[splitAxis] > splitLocation);
 }
 
 Vector3 newLo = lo;
 newLo[splitAxis] = splitLocation;
 Vector3 newHi = hi;
 newHi[splitAxis] = splitLocation;
 
 if (child[0] != NULL) {
 child[0]->verifyNode(lo, newHi);
 }
 
 if (child[1] != NULL) {
 child[1]->verifyNode(newLo, hi);
 }
 }
 

 static void serializeStructure(const Node* n, BinaryOutput& bo) {
 if (n == NULL) {
 bo.writeUInt8(0);
 } else {
 bo.writeUInt8(1);
 n->splitBounds.serialize(bo);
 serialize(n->splitAxis, bo);
 bo.writeFloat32(n->splitLocation);
 for (int c = 0; c < 2; ++c) {
 serializeStructure(n->child[c], bo);
 }
 }
 }

 static Node* deserializeStructure(BinaryInput& bi) {
 if (bi.readUInt8() == 0) {
 return NULL;
 } else {
 Node* n = new Node();
 n->splitBounds.deserialize(bi);
 deserialize(n->splitAxis, bi);
 n->splitLocation = bi.readFloat32();
 for (int c = 0; c < 2; ++c) {
 n->child[c] = deserializeStructure(bi);
 }
 return n;
 }
 }

 Node* findDeepestContainingNode(const AABox& bounds) {

 // See which side of the splitting plane the bounds are on
 if (bounds.high()[splitAxis] < splitLocation) {
 // Bounds are on the low side. Recurse into the child
 // if it exists.
 if (child[0] != NULL) {
 return child[0]->findDeepestContainingNode(bounds);
 }
 } else if (bounds.low()[splitAxis] > splitLocation) {
 // Bounds are on the high side, recurse into the child
 // if it exists.
 if (child[1] != NULL) {
 return child[1]->findDeepestContainingNode(bounds);
 }
 }

 // There was no containing child, so this node is the
 // deepest containing node.
 return this;
 }


 void getIntersectingMembers(
 const AABox& box,
 const Sphere& sphere,
 Array<T*>& members,
 bool useSphere) const {

 // Test all values at this node
 for (int v = 0; v < boundsArray.size(); ++v) {
 const AABox& bounds = boundsArray[v];
 if (bounds.intersects(box) &&
 (! useSphere || bounds.intersects(sphere))) {
 members.append(& (valueArray[v]->value));
 }
 }

 // If the left child overlaps the box, recurse into it
 if ((child[0] != NULL) && (box.low()[splitAxis] < splitLocation)) {
 child[0]->getIntersectingMembers(box, sphere, members, useSphere);
 }

 // If the right child overlaps the box, recurse into it
 if ((child[1] != NULL) && (box.high()[splitAxis] > splitLocation)) {
 child[1]->getIntersectingMembers(box, sphere, members, useSphere);
 }
 }

 void assignSplitBounds(const AABox& myBounds) {
 splitBounds = myBounds;

 AABox childBounds[2];
 myBounds.split(splitAxis, splitLocation, childBounds[0], childBounds[1]);

# if defined(G3D_DEBUG) && defined(VERIFY_TREE)
 // Verify the split
 for (int v = 0; v < boundsArray.size(); ++v) {
 const AABox& bounds = boundsArray[v];
 debugAssert(myBounds.contains(bounds));
 }
# endif

 for (int c = 0; c < 2; ++c) {
 if (child[c]) {
 child[c]->assignSplitBounds(childBounds[c]);
 }
 }
 }

 bool intersects(const Ray& ray, float distance) const {
 // See if the ray will ever hit this node or its children
 Vector3 location;
 bool alreadyInsideBounds = false;
 bool rayWillHitBounds = 
 CollisionDetection::collisionLocationForMovingPointFixedAABox(
 ray.origin(), ray.direction(), splitBounds, location, alreadyInsideBounds);
 
 bool canHitThisNode = (alreadyInsideBounds || 
 (rayWillHitBounds && ((location - ray.origin()).squaredLength() < square(distance))));

 return canHitThisNode;
 }

 template<typename RayCallback>
 void intersectRay(
 const Ray& ray, 
 RayCallback& intersectCallback, 
 float& distance,
 bool intersectCallbackIsFast) const {
 
 if (! intersects(ray, distance)) {
 // The ray doesn't hit this node, so it can't hit the children of the node.
 return;
 }

 // Test for intersection against every object at this node.
 for (int v = 0; v < valueArray.size(); ++v) { 
 bool canHitThisObject = true;

 if (! intersectCallbackIsFast) {
 // See if
 Vector3 location;
 const AABox& bounds = boundsArray[v];
 bool alreadyInsideBounds = false;
 bool rayWillHitBounds = 
 CollisionDetection::collisionLocationForMovingPointFixedAABox(
 ray.origin(), ray.direction(), bounds, location, alreadyInsideBounds);

 canHitThisObject = (alreadyInsideBounds || 
 (rayWillHitBounds && ((location - ray.origin()).squaredLength() < square(distance))));
 }

 if (canHitThisObject) {
 // It is possible that this ray hits this object. Look for the intersection using the
 // callback.
 const T& value = valueArray[v]->value;
 intersectCallback(ray, value, distance);
 }
 }

 // There are three cases to consider next:
 // 
 // 1. the ray can start on one side of the splitting plane and never enter the other,
 // 2. the ray can start on one side and enter the other, and
 // 3. the ray can travel exactly down the splitting plane

 enum {NONE = -1};
 int firstChild = NONE;
 int secondChild = NONE;

 if (ray.origin()[splitAxis] < splitLocation) {
 
 // The ray starts on the small side
 firstChild = 0;

 if (ray.direction()[splitAxis] > 0) {
 // The ray will eventually reach the other side
 secondChild = 1;
 }

 } else if (ray.origin()[splitAxis] > splitLocation) {

 // The ray starts on the large side
 firstChild = 1;

 if (ray.direction()[splitAxis] < 0) {
 secondChild = 0;
 }
 } else {
 // The ray starts on the splitting plane
 if (ray.direction()[splitAxis] < 0) {
 // ...and goes to the small side
 firstChild = 0;
 } else if (ray.direction()[splitAxis] > 0) {
 // ...and goes to the large side
 firstChild = 1;
 }
 }

 // Test on the side closer to the ray origin.
 if ((firstChild != NONE) && child[firstChild]) {
 child[firstChild]->intersectRay(ray, intersectCallback, distance, intersectCallbackIsFast);
 }

 if (ray.direction()[splitAxis] != 0) {
 // See if there was an intersection before hitting the splitting plane. 
 // If so, there is no need to look on the far side and recursion terminates.
 float distanceToSplittingPlane = (splitLocation - ray.origin()[splitAxis]) / ray.direction()[splitAxis];
 if (distanceToSplittingPlane > distance) {
 // We aren't going to hit anything else before hitting the splitting plane,
 // so don't bother looking on the far side of the splitting plane at the other
 // child.
 return;
 }
 }

 // Test on the side farther from the ray origin.
 if ((secondChild != NONE) && child[secondChild]) {
 child[secondChild]->intersectRay(ray, intersectCallback, distance, intersectCallbackIsFast);
 }

 }
 };


 Node* makeNode(
 Array<Handle*>& source, 
 int valuesPerNode, 
 int numMeanSplits,
 Array<Handle*>& temp) {

 Node* node = NULL;
 
 if (source.size() <= valuesPerNode) {
 // Make a new leaf node
 node = new Node(source);
 
 // Set the pointers in the memberTable
 for (int i = 0; i < source.size(); ++i) {
 memberTable.set(Member(source[i]), node);
 }
 source.clear();
 
 } else {
 // Make a new internal node
 node = new Node();
 
 const AABox& bounds = computeBounds(source, 0, source.size() - 1);
 const Vector3& extent = bounds.high() - bounds.low();
 
 Vector3::Axis splitAxis = extent.primaryAxis();
 
 float splitLocation;

 // Arrays for holding the children
 Array<Handle*> lt, gt;
 
 if (numMeanSplits <= 0) {

 source.medianPartition(lt, node->valueArray, gt, temp, CenterComparator(splitAxis));

 // Choose the split location to be the center of whatever fell in the center
 splitLocation = node->valueArray[0]->center[splitAxis];

 // Some of the elements in the lt or gt array might really overlap the split location.
 // Move them as needed.
 for (int i = 0; i < lt.size(); ++i) {
 const AABox& bounds = lt[i]->bounds;
 if ((bounds.low()[splitAxis] <= splitLocation) && (bounds.high()[splitAxis] >= splitLocation)) {
 node->valueArray.append(lt[i]);
 // Remove this element and process the new one that
 // is swapped in in its place.
 lt.fastRemove(i); --i;
 }
 }

 for (int i = 0; i < gt.size(); ++i) {
 const AABox& bounds = gt[i]->bounds;
 if ((bounds.low()[splitAxis] <= splitLocation) && (bounds.high()[splitAxis] >= splitLocation)) {
 node->valueArray.append(gt[i]);
 // Remove this element and process the new one that
 // is swapped in in its place.
 gt.fastRemove(i); --i;
 }
 } 

 if ((node->valueArray.size() > (source.size() / 2)) &&
 (source.size() > 6)) {
 // This was a bad partition; we ended up putting the splitting plane right in the middle of most of the 
 // objects. We could try to split on a different axis, or use a different partition (e.g., the extents mean, 
 // or geometric mean). This implementation falls back on the extents mean, since that case is already handled 
 // below.
 numMeanSplits = 1;
 }
 }

 // Note: numMeanSplits may have been increased by the code in the previous case above in order to
 // force a re-partition.

 if (numMeanSplits > 0) {
 // Split along the mean
 splitLocation = 
 bounds.high()[splitAxis] * 0.5f + 
 bounds.low()[splitAxis] * 0.5f;
 
 debugAssertM(isFinite(splitLocation),
 "Internal error: split location must be finite.");

 source.partition(NULL, lt, node->valueArray, gt, Comparator(splitAxis, splitLocation));

 // The Comparator ensures that elements are strictly on the correct side of the split
 }


# if defined(G3D_DEBUG) && defined(VERIFY_TREE)
 debugAssert(lt.size() + node->valueArray.size() + gt.size() == source.size());
 // Verify that all objects ended up on the correct side of the split.
 // (i.e., make sure that the Array partition was correct) 
 for (int i = 0; i < lt.size(); ++i) {
 const AABox& bounds = lt[i]->bounds;
 debugAssert(bounds.high()[splitAxis] < splitLocation);
 }

 for (int i = 0; i < gt.size(); ++i) {
 const AABox& bounds = gt[i]->bounds;
 debugAssert(bounds.low()[splitAxis] > splitLocation);
 }

 for (int i = 0; i < node->valueArray.size(); ++i) {
 const AABox& bounds = node->valueArray[i]->bounds;
 debugAssert(bounds.high()[splitAxis] >= splitLocation);
 debugAssert(bounds.low()[splitAxis] <= splitLocation);
 }
# endif

 // The source array is no longer needed
 source.clear();

 node->splitAxis = splitAxis;
 node->splitLocation = splitLocation;

 // Update the bounds array and member table
 node->boundsArray.resize(node->valueArray.size());
 for (int i = 0; i < node->valueArray.size(); ++i) {
 Handle* v = node->valueArray[i];
 node->boundsArray[i] = v->bounds;
 memberTable.set(Member(v), node);
 }

 if (lt.size() > 0) { 
 node->child[0] = makeNode(lt, valuesPerNode, numMeanSplits - 1, temp);
 }
 
 if (gt.size() > 0) {
 node->child[1] = makeNode(gt, valuesPerNode, numMeanSplits - 1, temp);
 }
 
 }
 
 return node;
 }

 Node* cloneTree(Node* src) {
 Node* dst = new Node(*src);

 // Make back pointers
 for (int i = 0; i < dst->valueArray.size(); ++i) {
 memberTable.set(Member(dst->valueArray[i]), dst);
 }

 // Clone children
 for (int i = 0; i < 2; ++i) {
 if (src->child[i] != NULL) {
 dst->child[i] = cloneTree(src->child[i]);
 }
 }

 return dst;
 }

 typedef _internal::Indirector<Handle> Member;

 typedef Table<Member, Node*> MemberTable;

 MemberTable memberTable;

 Node* root;

public:

 KDTree() : root(NULL) {}


 KDTree(const KDTree& src) : root(NULL) {
 *this = src;
 }


 KDTree& operator=(const KDTree& src) {
 delete root;
 // Clone tree takes care of filling out the memberTable.
 root = cloneTree(src.root);
 return *this;
 }


 ~KDTree() {
 clear();
 }

 void clear() {
 typedef typename Table<_internal::Indirector<Handle>, Node*>::Iterator It;
 
 // Delete all handles stored in the member table
 It cur = memberTable.begin();
 It end = memberTable.end();
 while (cur != end) {
 delete cur->key.handle;
 cur->key.handle = NULL;
 ++cur;
 }
 memberTable.clear();

 // Delete the tree structure itself
 delete root;
 root = NULL;
 }

 int size() const {
 return memberTable.size();
 }

 void insert(const T& value) {
 if (contains(value)) {
 // Already in the set
 return;
 }

 Handle* h = new Handle(value);

 if (root == NULL) {
 // This is the first node; create a root node
 root = new Node();
 }

 Node* node = root->findDeepestContainingNode(h->bounds);

 // Insert into the node
 node->valueArray.append(h);
 node->boundsArray.append(h->bounds);
 
 // Insert into the node table
 Member m(h);
 memberTable.set(m, node);
 }

 void insert(const Array<T>& valueArray) {
 if (root == NULL) {
 // Optimized case for an empty tree; don't bother
 // searching or reallocating the root node's valueArray
 // as we incrementally insert.
 root = new Node();
 root->valueArray.resize(valueArray.size());
 root->boundsArray.resize(root->valueArray.size());
 for (int i = 0; i < valueArray.size(); ++i) {
 // Insert in opposite order so that we have the exact same
 // data structure as if we inserted each (i.e., order is reversed
 // from array).
 Handle* h = new Handle(valueArray[i]);
 int j = valueArray.size() - i - 1;
 root->valueArray[j] = h;
 root->boundsArray[j] = h->bounds;
 memberTable.set(Member(h), root);
 }

 } else {
 // Insert at appropriate tree depth.
 for (int i = 0; i < valueArray.size(); ++i) {
 insert(valueArray[i]);
 }
 }
 }


 bool contains(const T& value) {
 // Temporarily create a handle and member
 Handle h(value);
 return memberTable.containsKey(Member(&h));
 }


 void remove(const T& value) {
 debugAssertM(contains(value),
 "Tried to remove an element from a "
 "KDTree that was not present");

 // Get the list of elements at the node
 Handle h(value);
 Member m(&h);

 Array<Handle*>& list = memberTable[m]->valueArray;

 Handle* ptr = NULL;

 // Find the element and remove it
 for (int i = list.length() - 1; i >= 0; --i) {
 if (list[i]->value == value) {
 // This was the element. Grab the pointer so that
 // we can delete it below
 ptr = list[i];

 // Remove the handle from the node
 list.fastRemove(i);

 // Remove the corresponding bounds
 memberTable[m]->boundsArray.fastRemove(i);
 break;
 }
 }

 // Remove the member
 memberTable.remove(m);

 // Delete the handle data structure
 delete ptr;
 ptr = NULL;
 }


 void update(const T& value) {
 if (contains(value)) {
 remove(value);
 }
 insert(value);
 }


 void balance(int valuesPerNode = 5, int numMeanSplits = 3) {
 if (root == NULL) {
 // Tree is empty
 return;
 }

 // Get all handles and delete the old tree structure
 Node* oldRoot = root;
 for (int c = 0; c < 2; ++c) {
 if (root->child[c] != NULL) {
 root->child[c]->getHandles(root->valueArray);

 // Delete the child; this will delete all structure below it
 delete root->child[c];
 root->child[c] = NULL;
 }
 }

 Array<Handle*> temp;
 // Make a new root. Work with a copy of the value array because 
 // makeNode clears the source array as it progresses
 Array<Handle*> copy(oldRoot->valueArray);
 root = makeNode(copy, valuesPerNode, numMeanSplits, temp);

 // Throw away the old root node
 delete oldRoot;
 oldRoot = NULL;

 // Walk the tree, assigning splitBounds. We start with unbounded
 // space. This will override the current member table.
 const AABox& LARGE = AABox::large();
 root->assignSplitBounds(LARGE);

# ifdef _DEBUG
 {
 // Ensure that the balanced tree is still correct
 root->verifyNode(LARGE.low(), LARGE.high());
 }
# endif
 }


 void setContents(const Array<T>& array, int valuesPerNode = 5, int numMeanSplits = 3) {
 clear();
 insert(array);
 balance(valuesPerNode, numMeanSplits);
 }


protected:

 static void getIntersectingMembers(
 const Array<Plane>& plane,
 Array<T*>& members,
 Node* node,
 uint32 parentMask) {

 int dummy;

 if (parentMask == 0) {
 // None of these planes can cull anything
 for (int v = node->valueArray.size() - 1; v >= 0; --v) {
 members.append(& (node->valueArray[v]->value));
 }

 // Iterate through child nodes
 for (int c = 0; c < 2; ++c) {
 if (node->child[c]) {
 getIntersectingMembers(plane, members, node->child[c], 0);
 }
 }
 } else {

 // Test values at this node against remaining planes
 for (int v = node->boundsArray.size() - 1; v >= 0; --v) {
 if (! node->boundsArray[v].culledBy(plane, dummy, parentMask)) {
 members.append(&(node->valueArray[v]->value));
 }
 }

 uint32 childMask = 0xFFFFFF;

 // Iterate through child nodes
 for (int c = 0; c < 2; ++c) {
 if (node->child[c] &&
 ! node->child[c]->splitBounds.culledBy(plane, dummy, parentMask, childMask)) {
 // This node was not culled
 getIntersectingMembers(plane, members, node->child[c], childMask);
 }
 }
 }
 }

public:

 void getIntersectingMembers(const Array<Plane>& plane, Array<T*>& members) const {
 if (root == NULL) {
 return;
 }

 getIntersectingMembers(plane, members, root, 0xFFFFFF);
 }

 void getIntersectingMembers(const Array<Plane>& plane, Array<T>& members) const {
 Array<T*> temp;
 getIntersectingMembers(plane, temp, root, 0xFFFFFF);
 for (int i = 0; i < temp.size(); ++i) {
 members.append(*temp[i]);
 }
 }

 void getIntersectingMembers(const Frustum& frustum, Array<T*>& members) const {
 Array<Plane> plane;
 
 for (int i = 0; i < frustum.faceArray.size(); ++i) {
 plane.append(frustum.faceArray[i].plane);
 }

 getIntersectingMembers(plane, members);
 }

 void getIntersectingMembers(const Frustum& frustum, Array<T>& members) const {
 Array<T*> temp;
 getIntersectingMembers(frustum, temp);
 for (int i = 0; i < temp.size(); ++i) {
 members.append(*temp[i]);
 }
 }

 // This iterator turns Node::getIntersectingMembers into a
 // coroutine. It first translates that method from recursive to
 // stack based, then captures the system state (analogous to a Scheme
 // continuation) after each element is appended to the member array,
 // and allowing the computation to be restarted.
 class BoxIntersectionIterator {
 private:
 friend class TreeType;

 bool isEnd;

 AABox box;

 Node* node;

 // We could use backpointers within the tree and careful
 // state management to avoid ever storing the stack-- but
 // it is much easier this way and only inefficient if the
 // caller uses post increment (which they shouldn't!).
 Array<Node*> stack;

 int nextValueArrayIndex;

 BoxIntersectionIterator() : isEnd(true) {}
 
 BoxIntersectionIterator(const AABox& b, const Node* root) : 
 isEnd(root == NULL), box(b), 
 node(const_cast<Node*>(root)), nextValueArrayIndex(-1) {

 // We intentionally start at the "-1" index of the current
 // node so we can use the preincrement operator to move
 // ourselves to element 0 instead of repeating all of the
 // code from the preincrement method. Note that this might
 // cause us to become the "end" instance.
 ++(*this);
 }

 public:

 inline bool operator!=(const BoxIntersectionIterator& other) const {
 return ! (*this == other);
 }

 bool operator==(const BoxIntersectionIterator& other) const {
 if (isEnd) {
 return other.isEnd;
 } else if (other.isEnd) {
 return false;
 } else {
 // Two non-end iterators; see if they match. This is kind of 
 // silly; users shouldn't call == on iterators in general unless
 // one of them is the end iterator.
 if ((box != other.box) || (node != other.node) || 
 (nextValueArrayIndex != other.nextValueArrayIndex) ||
 (stack.length() != other.stack.length())) {
 return false;
 }

 // See if the stacks are the same
 for (int i = 0; i < stack.length(); ++i) {
 if (stack[i] != other.stack[i]) {
 return false;
 }
 }

 // We failed to find a difference; they must be the same
 return true;
 }
 }

 BoxIntersectionIterator& operator++() {
 ++nextValueArrayIndex;

 bool foundIntersection = false;
 while (! isEnd && ! foundIntersection) {

 // Search for the next node if we've exhausted this one
 while ((! isEnd) && (nextValueArrayIndex >= node->valueArray.length())) {
 // If we entered this loop, then the iterator has exhausted the elements at 
 // node (possibly because it just switched to a child node with no members).
 // This loop continues until it finds a node with members or reaches
 // the end of the whole intersection search.
 
 // If the right child overlaps the box, push it onto the stack for
 // processing.
 if ((node->child[1] != NULL) &&
 (box.high()[node->splitAxis] > node->splitLocation)) {
 stack.push(node->child[1]);
 }
 
 // If the left child overlaps the box, push it onto the stack for
 // processing.
 if ((node->child[0] != NULL) &&
 (box.low()[node->splitAxis] < node->splitLocation)) {
 stack.push(node->child[0]);
 }
 
 if (stack.length() > 0) {
 // Go on to the next node (which may be either one of the ones we 
 // just pushed, or one from farther back the tree).
 node = stack.pop();
 nextValueArrayIndex = 0;
 } else {
 // That was the last node; we're done iterating
 isEnd = true;
 }
 }
 
 // Search for the next intersection at this node until we run out of children
 while (! isEnd && ! foundIntersection && (nextValueArrayIndex < node->valueArray.length())) {
 if (box.intersects(node->boundsArray[nextValueArrayIndex])) {
 foundIntersection = true;
 } else {
 ++nextValueArrayIndex;
 // If we exhaust this node, we'll loop around the master loop 
 // to find a new node.
 }
 }
 }

 return *this;
 }

 private:
 BoxIntersectionIterator operator++(int);
 /*{
 BoxIntersectionIterator old = *this;
 ++this;
 return old;
 }*/

 public:

 const T& operator*() const {
 alwaysAssertM(! isEnd, "Can't dereference the end element of an iterator");
 return node->valueArray[nextValueArrayIndex]->value;
 }

 T const * operator->() const {
 alwaysAssertM(! isEnd, "Can't dereference the end element of an iterator");
 return &(stack.last()->valueArray[nextValueArrayIndex]->value);
 }

 operator T*() const {
 alwaysAssertM(! isEnd, "Can't dereference the end element of an iterator");
 return &(stack.last()->valueArray[nextValueArrayIndex]->value);
 }
 };


 BoxIntersectionIterator beginBoxIntersection(const AABox& box) const {
 return BoxIntersectionIterator(box, root);
 }

 BoxIntersectionIterator endBoxIntersection() const {
 // The "end" iterator instance
 return BoxIntersectionIterator();
 }

 void getIntersectingMembers(const AABox& box, Array<T*>& members) const {
 if (root == NULL) {
 return;
 }
 root->getIntersectingMembers(box, Sphere(Vector3::zero(), 0), members, false);
 }

 void getIntersectingMembers(const AABox& box, Array<T>& members) const {
 Array<T*> temp;
 getIntersectingMembers(box, temp);
 for (int i = 0; i < temp.size(); ++i) {
 members.append(*temp[i]);
 }
 }


 template<typename RayCallback>
 void intersectRay(
 const Ray& ray, 
 RayCallback& intersectCallback, 
 float& distance,
 bool intersectCallbackIsFast = false) const {
 
 root->intersectRay(ray, intersectCallback, distance, intersectCallbackIsFast);
 }


 void getIntersectingMembers(const Sphere& sphere, Array<T*>& members) const {
 if (root == NULL) {
 return;
 }

 AABox box;
 sphere.getBounds(box);
 root->getIntersectingMembers(box, sphere, members, true);
 }

 void getIntersectingMembers(const Sphere& sphere, Array<T>& members) const {
 Array<T*> temp;
 getIntersectingMembers(sphere, temp);
 for (int i = 0; i < temp.size(); ++i) {
 members.append(*temp[i]);
 }
 }

 void serializeStructure(BinaryOutput& bo) const {
 Node::serializeStructure(root, bo);
 }

 void deserializeStructure(BinaryInput& bi) {
 clear();
 root = Node::deserializeStructure(bi);
 }

 void getMembers(Array<T>& members) const {
 Array<Member> temp;
 memberTable.getKeys(temp);
 for (int i = 0; i < temp.size(); ++i) {
 members.append(*(temp.handle));
 }
 }


 const T* getPointer(const T& value) const {
 // Temporarily create a handle and member
 Handle h(value);
 const Member* member = memberTable.getKeyPointer(Member(&h));
 if (member == NULL) {
 // Not found
 return NULL;
 } else {
 return &(member->handle->value);
 }
 }


 class Iterator {
 private:
 friend class TreeType;

 // Note: this is a Table iterator, we are currently defining
 // Set iterator
 typename Table<Member, Node*>::Iterator it;

 Iterator(const typename Table<Member, Node*>::Iterator& it) : it(it) {}

 public:

 inline bool operator!=(const Iterator& other) const {
 return !(*this == other);
 }

 bool operator==(const Iterator& other) const {
 return it == other.it;
 }

 Iterator& operator++() {
 ++it;
 return *this;
 }

 private:
 Iterator operator++(int);/* {
 Iterator old = *this;
 ++(*this);
 return old;
 }*/
 public:

 const T& operator*() const {
 return it->key.handle->value;
 }

 T* operator->() const {
 return &(it->key.handle->value);
 }

 operator T*() const {
 return &(it->key.handle->value);
 }
 };


 Iterator begin() const {
 return Iterator(memberTable.begin());
 }


 Iterator end() const {
 return Iterator(memberTable.end());
 }
#undef TreeType
};


}

#endif