[4057] added: Line of sight (vmaps) [part 2] (last part)
[mangos-git.git] / contrib / vmap_debugger / G3D / AABSPTree.h
blobe3c5f530c0a3a51c011c691eaac8a8679bfd5552
1 /**
2 @file AABSPTree.h
4 @maintainer Morgan McGuire, matrix@graphics3d.com
6 @created 2004-01-11
7 @edited 2005-02-24
9 Copyright 2000-2005, Morgan McGuire.
10 All rights reserved.
14 #ifndef G3D_AABSPTREE_H
15 #define G3D_AABSPTREE_H
17 #include "G3D/platform.h"
18 #include "G3D/Array.h"
19 #include "G3D/Table.h"
20 #include "G3D/Vector3.h"
21 #include "G3D/AABox.h"
22 #include "G3D/Sphere.h"
23 #include "G3D/Box.h"
24 #include "G3D/Triangle.h"
25 #include "G3D/GCamera.h"
26 //#include "G3D/BinaryInput.h"
27 //#include "G3D/BinaryOutput.h"
28 #include "G3D/CollisionDetection.h"
29 #include <algorithm>
31 #include "RayIntersectionIterator.h"
33 #ifdef _ASSEMBLER_DEBUG
34 #ifndef g_df
35 extern FILE *g_df;
36 #endif
37 #endif
40 inline void getBounds(const G3D::Vector3& v, G3D::AABox& out) {
41 out = G3D::AABox(v);
45 inline void getBounds(const G3D::AABox& a, G3D::AABox& out) {
46 out = a;
50 inline void getBounds(const G3D::Sphere& s, G3D::AABox& out) {
51 s.getBounds(out);
55 inline void getBounds(const G3D::Box& b, G3D::AABox& out) {
56 b.getBounds(out);
60 inline void getBounds(const G3D::Triangle& t, G3D::AABox& out) {
61 t.getBounds(out);
64 namespace G3D {
66 /**
67 A set that supports spatial queries using an axis-aligned
68 BSP tree for speed.
70 AABSPTree is as powerful as but more general than a Quad Tree, Oct Tree, or KD Tree,
71 but less general than an unconstrained BSP tree (which is much slower
72 to create).
74 Internally, objects
75 are arranged into an axis-aligned BSP-tree according to their
76 axis-aligned bounds. This increases the cost of insertion to
77 O(log n) but allows fast overlap queries.
79 <B>Template Parameters</B>
80 <DT>The template parameter <I>T</I> must be one for which
81 the following functions are overloaded:
83 <P><CODE>void ::getBounds(const T&, G3D::AABox&);</CODE>
84 <DT><CODE>bool operator==(const T&, const T&);</CODE>
85 <DT><CODE>unsigned int ::hashCode(const T&);</CODE>
86 <DT><CODE>T::T();</CODE> <I>(public constructor of no arguments)</I>
88 <B>Moving %Set Members</B>
89 <DT>It is important that objects do not move without updating the
90 AABSPTree. If the axis-aligned bounds of an object are about
91 to change, AABSPTree::remove it before they change and
92 AABSPTree::insert it again afterward. For objects
93 where the hashCode and == operator are invariant with respect
94 to the 3D position,
95 you can use the AABSPTree::update method as a shortcut to
96 insert/remove an object in one step after it has moved.
99 Note: Do not mutate any value once it has been inserted into AABSPTree. Values
100 are copied interally. All AABSPTree iterators convert to pointers to constant
101 values to reinforce this.
103 If you want to mutate the objects you intend to store in a AABSPTree simply
104 insert <I>pointers</I> to your objects instead of the objects themselves, and ensure
105 that the above operations are defined. (And actually, because values are
106 copied, if your values are large you may want to insert pointers anyway, to
107 save space and make the balance operation faster.)
109 <B>Dimensions</B>
110 Although designed as a 3D-data structure, you can use the AABSPTree
111 for data distributed along 2 or 1 axes by simply returning bounds
112 that are always zero along one or more dimensions.
115 template<class T> class AABSPTree {
116 private:
117 public:
118 /** Wrapper for a value that includes a cache of its bounds. */
119 class Handle {
120 public:
121 T value;
123 /** The bounds of each object are constrained to AABox::maxFinite */
124 AABox bounds;
126 /** Center of bounds. We cache this value to avoid recomputing it
127 during the median sort, and because MSVC 6 std::sort goes into
128 an infinite loop if we compute the midpoint on the fly (possibly
129 a floating point roundoff issue, where B<A and A<B both are true).*/
130 Vector3 center;
132 Handle() {}
134 inline Handle(const T& v) : value(v) {
135 getBounds(v, bounds);
136 bounds = bounds.intersect(AABox::maxFinite());
137 center = bounds.center();
141 /** Returns the bounds of the sub array. Used by makeNode. */
142 static AABox computeBounds(
143 const Array<Handle>& point,
144 int beginIndex,
145 int endIndex) {
147 Vector3 lo = Vector3::inf();
148 Vector3 hi = -lo;
150 for (int p = beginIndex; p <= endIndex; ++p) {
151 lo = lo.min(point[p].bounds.low());
152 hi = hi.max(point[p].bounds.high());
155 return AABox(lo, hi);
160 A sort predicate that returns true if the midpoint of the
161 first argument is less than the midpoint of the second
162 along the specified axis.
164 Used by makeNode.
166 class CenterLT {
167 public:
168 Vector3::Axis sortAxis;
170 CenterLT(Vector3::Axis a) : sortAxis(a) {}
172 inline bool operator()(const Handle& a, const Handle& b) const {
173 return a.center[sortAxis] < b.center[sortAxis];
179 A sort predicate based on a box's location on the specified axis. Each
180 box is given a value -1, 0, or 1 based on whether it is strictly less
181 than, overlapping, or strictly greater than the value on the specified
182 axis. This predicate compares the values for two boxes. The result is
183 that the array is separated into three contiguous (though possibly empty)
184 groups: less than, overlapping, or greater than.
186 class PivotLT {
187 public:
188 Vector3::Axis sortAxis;
189 float sortLocation;
191 PivotLT(Vector3::Axis a, float l) : sortAxis(a), sortLocation(l) {}
193 inline int location(const AABox& box) const {
194 if(box.low()[sortAxis] < sortLocation) {
195 if(box.high()[sortAxis] < sortLocation) {
196 return -1;
197 } else {
198 return 0;
200 } else if(box.low()[sortAxis] > sortLocation) {
201 return 1;
202 } else {
203 return 0;
207 inline bool operator()(const Handle&a, const Handle& b) const {
208 const AABox& A = a.bounds;
209 const AABox& B = b.bounds;
211 return location(A) < location(B);
215 class Node {
216 public:
218 /** Spatial bounds on all values at this node and its children, based purely on
219 the parent's splitting planes. May be infinite */
220 AABox splitBounds;
222 Vector3::Axis splitAxis;
224 /** Location along the specified axis */
225 float splitLocation;
227 /** child[0] contains all values strictly
228 smaller than splitLocation along splitAxis.
230 child[1] contains all values strictly
231 larger.
233 Both may be NULL if there are not enough
234 values to bother recursing.
236 Node* child[2];
238 /** Array of values at this node (i.e. values
239 straddling the split plane + all values if
240 this is a leaf node). */
241 Array<Handle> valueArray;
243 /** Creates node with NULL children */
244 Node() {
245 splitAxis = Vector3::X_AXIS;
246 splitLocation = 0;
247 splitBounds = AABox(-Vector3::inf(), Vector3::inf());
248 for (int i = 0; i < 2; ++i) {
249 child[i] = NULL;
254 Doesn't clone children.
256 Node(const Node& other) : valueArray(other.valueArray) {
257 splitAxis = other.splitAxis;
258 splitLocation = other.splitLocation;
259 splitBounds = other.splitBounds;
260 for (int i = 0; i < 2; ++i) {
261 child[i] = NULL;
265 /** Copies the specified subarray of pt into point, NULLs the children.
266 Assumes a second pass will set splitBounds. */
267 Node(const Array<Handle>& pt, int beginIndex, int endIndex) {
268 splitAxis = Vector3::X_AXIS;
269 splitLocation = 0;
270 for (int i = 0; i < 2; ++i) {
271 child[i] = NULL;
274 int n = endIndex - beginIndex + 1;
276 valueArray.resize(n);
277 for (int i = n - 1; i >= 0; --i) {
278 valueArray[i] = pt[i + beginIndex];
283 /** Deletes the children (but not the values) */
284 ~Node() {
285 for (int i = 0; i < 2; ++i) {
286 delete child[i];
291 /** Returns true if this node is a leaf (no children) */
292 inline bool isLeaf() const {
293 return (child[0] == NULL) && (child[1] == NULL);
298 Recursively appends all handles and children's handles
299 to the array.
301 void getHandles(Array<Handle>& handleArray) const {
302 handleArray.append(valueArray);
303 for (int i = 0; i < 2; ++i) {
304 if (child[i] != NULL) {
305 child[i]->getHandles(handleArray);
311 void verifyNode(const Vector3& lo, const Vector3& hi) {
312 // debugPrintf("Verifying: split %d @ %f [%f, %f, %f], [%f, %f, %f]\n",
313 // splitAxis, splitLocation, lo.x, lo.y, lo.z, hi.x, hi.y, hi.z);
315 debugAssert(lo == splitBounds.low());
316 debugAssert(hi == splitBounds.high());
318 for (int i = 0; i < valueArray.length(); ++i) {
319 const AABox& b = valueArray[i].bounds;
321 for(int axis = 0; axis < 3; ++axis) {
322 debugAssert(b.low()[axis] <= b.high()[axis]);
323 debugAssert(b.low()[axis] >= lo[axis]);
324 debugAssert(b.high()[axis] <= hi[axis]);
328 if (child[0] || child[1]) {
329 debugAssert(lo[splitAxis] < splitLocation);
330 debugAssert(hi[splitAxis] > splitLocation);
333 Vector3 newLo = lo;
334 newLo[splitAxis] = splitLocation;
335 Vector3 newHi = hi;
336 newHi[splitAxis] = splitLocation;
338 if (child[0] != NULL) {
339 child[0]->verifyNode(lo, newHi);
342 if (child[1] != NULL) {
343 child[1]->verifyNode(newLo, hi);
347 #if 0
349 Stores the locations of the splitting planes (the structure but not the content)
350 so that the tree can be quickly rebuilt from a previous configuration without
351 calling balance.
353 static void serializeStructure(const Node* n, BinaryOutput& bo) {
354 if (n == NULL) {
355 bo.writeUInt8(0);
356 } else {
357 bo.writeUInt8(1);
358 n->splitBounds.serialize(bo);
359 serialize(n->splitAxis, bo);
360 bo.writeFloat32(n->splitLocation);
361 for (int c = 0; c < 2; ++c) {
362 serializeStructure(n->child[c], bo);
367 /** Clears the member table */
368 static Node* deserializeStructure(BinaryInput& bi) {
369 if (bi.readUInt8() == 0) {
370 return NULL;
371 } else {
372 Node* n = new Node();
373 n->splitBounds.deserialize(bi);
374 deserialize(n->splitAxis, bi);
375 n->splitLocation = bi.readFloat32();
376 for (int c = 0; c < 2; ++c) {
377 n->child[c] = deserializeStructure(bi);
381 #endif
383 /** Returns the deepest node that completely contains bounds. */
384 Node* findDeepestContainingNode(const AABox& bounds) {
386 // See which side of the splitting plane the bounds are on
387 if (bounds.high()[splitAxis] < splitLocation) {
388 // Bounds are on the low side. Recurse into the child
389 // if it exists.
390 if (child[0] != NULL) {
391 return child[0]->findDeepestContainingNode(bounds);
393 } else if (bounds.low()[splitAxis] > splitLocation) {
394 // Bounds are on the high side, recurse into the child
395 // if it exists.
396 if (child[1] != NULL) {
397 return child[1]->findDeepestContainingNode(bounds);
401 // There was no containing child, so this node is the
402 // deepest containing node.
403 return this;
407 /** Appends all members that intersect the box.
408 If useSphere is true, members that pass the box test
409 face a second test against the sphere. */
410 void getIntersectingMembers(
411 const AABox& box,
412 const Sphere& sphere,
413 Array<T>& members,
414 bool useSphere) const {
416 // Test all values at this node
417 for (int v = 0; v < valueArray.size(); ++v) {
418 const AABox& bounds = valueArray[v].bounds;
419 if (bounds.intersects(box) &&
420 (! useSphere || bounds.intersects(sphere))) {
421 members.append(valueArray[v].value);
425 // If the left child overlaps the box, recurse into it
426 if ((child[0] != NULL) && (box.low()[splitAxis] < splitLocation)) {
427 child[0]->getIntersectingMembers(box, sphere, members, useSphere);
430 // If the right child overlaps the box, recurse into it
431 if ((child[1] != NULL) && (box.high()[splitAxis] > splitLocation)) {
432 child[1]->getIntersectingMembers(box, sphere, members, useSphere);
437 Recurse through the tree, assigning splitBounds fields.
439 void assignSplitBounds(const AABox& myBounds) {
440 splitBounds = myBounds;
442 AABox childBounds[2];
443 myBounds.split(splitAxis, splitLocation, childBounds[0], childBounds[1]);
445 for (int c = 0; c < 2; ++c) {
446 if (child[c]) {
447 child[c]->assignSplitBounds(childBounds[c]);
455 Recursively subdivides the subarray.
456 Begin and end indices are inclusive.
458 Call assignSplitBounds() on the root node after making a tree.
460 Node* makeNode(Array<Handle>& point, int beginIndex,
461 int endIndex, int valuesPerNode,
462 int numMeanSplits) {
464 Node* node = NULL;
466 if (endIndex - beginIndex + 1 <= valuesPerNode) {
467 // Make a new leaf node
468 node = new Node(point, beginIndex, endIndex);
470 // Set the pointers in the memberTable
471 for (int i = beginIndex; i <= endIndex; ++i) {
472 memberTable.set(point[i].value, node);
475 } else {
476 // Make a new internal node
477 node = new Node();
479 const AABox bounds = computeBounds(point, beginIndex, endIndex);
480 const Vector3 extent = bounds.high() - bounds.low();
482 Vector3::Axis splitAxis = extent.primaryAxis();
484 double splitLocation;
486 if (numMeanSplits > 0) {
487 // Compute the mean along the axis
489 splitLocation = (bounds.high()[splitAxis] +
490 bounds.low()[splitAxis]) / 2.0;
492 } else {
494 // Compute the median along the axis
496 // Sort only the subarray
497 std::sort(
498 point.getCArray() + beginIndex,
499 point.getCArray() + endIndex + 1,
500 CenterLT(splitAxis));
502 // Intentional integer divide
503 int midIndex = (beginIndex + endIndex) / 2;
505 // Choose the split location between the two middle elements
506 const Vector3 median =
507 (point[midIndex].bounds.high() +
508 point[iMin(midIndex + 1,
509 point.size() - 1)].bounds.low()) * 0.5;
511 splitLocation = median[splitAxis];
515 // Re-sort around the split. This will allow us to easily
516 // test for overlap
517 std::sort(
518 point.getCArray() + beginIndex,
519 point.getCArray() + endIndex + 1,
520 PivotLT(splitAxis, splitLocation));
522 // Some number of nodes may actually overlap the midddle, so
523 // they must be found and added to *this* node, not one of
524 // its children
525 int overlapBeginIndex, overlapEndIndex;
527 for (overlapBeginIndex = beginIndex;
528 (overlapBeginIndex <= endIndex) &&
529 (point[overlapBeginIndex].bounds.high()[splitAxis] <
530 splitLocation);
531 ++overlapBeginIndex) {}
533 for (overlapEndIndex = endIndex;
534 (overlapEndIndex >= beginIndex) &&
535 (point[overlapEndIndex].bounds.low()[splitAxis] >
536 splitLocation);
537 --overlapEndIndex) {}
538 #ifdef _ASSEMBLER_DEBUG
539 fprintf(::g_df,"splitLocation: %f, beginIndex: %d, endIndex:%d, overlapBeginIndex: %d, overlapEndIndex: %d\n",splitLocation, beginIndex, endIndex, overlapBeginIndex, overlapEndIndex);
540 if(beginIndex == 220) {
541 float oldf = -100000.0f;
543 for (int xxx = beginIndex;
544 (xxx <= endIndex);
545 ++xxx) {
546 fprintf(::g_df," xxx:%d, point[xxx].bounds.high()[splitAxis]: %f\n",xxx, point[xxx].bounds.high()[splitAxis]);
547 if(oldf > point[xxx].bounds.high()[splitAxis]) {
548 fprintf(::g_df," huch ...\n");
550 oldf = point[xxx].bounds.high()[splitAxis];
553 #endif
554 // put overlapping boxes in this node
555 for (int i = overlapBeginIndex; i <= overlapEndIndex; ++i) {
556 node->valueArray.push(point[i]);
557 memberTable.set(point[i].value, node);
560 node->splitAxis = splitAxis;
561 node->splitLocation = splitLocation;
563 if (overlapBeginIndex > beginIndex) {
564 node->child[0] =
565 makeNode(point, beginIndex, overlapBeginIndex - 1,
566 valuesPerNode, numMeanSplits - 1);
569 if (overlapEndIndex < endIndex) {
570 node->child[1] =
571 makeNode(point, overlapEndIndex + 1, endIndex, valuesPerNode, numMeanSplits - 1);
576 return node;
580 Recursively clone the passed in node tree, setting
581 pointers for members in the memberTable as appropriate.
582 called by the assignment operator.
584 Node* cloneTree(Node* src) {
585 Node* dst = new Node(*src);
587 // Make back pointers
588 for (int i = 0; i < dst->valueArray.size(); ++i) {
589 memberTable.set(dst->valueArray[i].value, dst);
592 // Clone children
593 for (int i = 0; i < 2; ++i) {
594 if (src->child[i] != NULL) {
595 dst->child[i] = cloneTree(src->child[i]);
599 return dst;
602 /** Maps members to the node containing them */
603 Table<T, Node*> memberTable;
605 Node* root;
609 /** To construct a balanced tree, insert the elements and then call
610 AABSPTree::balance(). */
611 AABSPTree() : root(NULL) {}
614 AABSPTree(const AABSPTree& src) : root(NULL) {
615 *this = src;
619 AABSPTree& operator=(const AABSPTree& src) {
620 delete root;
621 // Clone tree takes care of filling out the memberTable.
622 root = cloneTree(src.root);
626 ~AABSPTree() {
627 clear();
631 Throws out all elements of the set.
633 void clear() {
634 memberTable.clear();
635 delete root;
636 root = NULL;
639 int size() const {
640 return memberTable.size();
644 Inserts an object into the set if it is not
645 already present. O(log n) time. Does not
646 cause the tree to be balanced.
648 void insert(const T& value) {
649 if (contains(value)) {
650 // Already in the set
651 return;
654 Handle h(value);
656 if (root == NULL) {
657 // This is the first node; create a root node
658 root = new Node();
661 Node* node = root->findDeepestContainingNode(h.bounds);
663 // Insert into the node
664 node->valueArray.append(h);
666 // Insert into the node table
667 memberTable.set(value, node);
670 /** Inserts each elements in the array in turn. If the tree
671 begins empty (no structure and no elements), this is faster
672 than inserting each element in turn. You still need to balance
673 the tree at the end.*/
674 void insert(const Array<T>& valueArray) {
675 if (root == NULL) {
676 // Optimized case for an empty tree; don't bother
677 // searching or reallocating the root node's valueArray
678 // as we incrementally insert.
679 root = new Node();
680 root->valueArray.resize(valueArray.size());
681 for (int i = 0; i < valueArray.size(); ++i) {
682 // Insert in opposite order so that we have the exact same
683 // data structure as if we inserted each (i.e., order is reversed
684 // from array).
685 root->valueArray[valueArray.size() - i - 1] = Handle(valueArray[i]);
686 memberTable.set(valueArray[i], root);
688 } else {
689 // Insert at appropriate tree depth.
690 for (int i = 0; i < valueArray.size(); ++i) {
691 insert(valueArray[i]);
698 Returns true if this object is in the set, otherwise
699 returns false. O(1) time.
701 bool contains(const T& value) {
702 return memberTable.containsKey(value);
707 Removes an object from the set in O(1) time.
708 It is an error to remove members that are not already
709 present. May unbalance the tree.
711 Removing an element never causes a node (split plane) to be removed...
712 nodes are only changed when the tree is rebalanced. This behavior
713 is desirable because it allows the split planes to be serialized,
714 and then deserialized into an empty tree which can be repopulated.
716 void remove(const T& value) {
717 debugAssertM(contains(value),
718 "Tried to remove an element from a "
719 "AABSPTree that was not present");
721 Array<Handle>& list = memberTable[value]->valueArray;
723 // Find the element and remove it
724 for (int i = list.length() - 1; i >= 0; --i) {
725 if (list[i].value == value) {
726 list.fastRemove(i);
727 break;
730 memberTable.remove(value);
735 If the element is in the set, it is removed.
736 The element is then inserted.
738 This is useful when the == and hashCode methods
739 on <I>T</I> are independent of the bounds. In
740 that case, you may call update(v) to insert an
741 element for the first time and call update(v)
742 again every time it moves to keep the tree
743 up to date.
745 void update(const T& value) {
746 if (contains(value)) {
747 remove(value);
749 insert(value);
754 Rebalances the tree (slow). Call when objects
755 have moved substantially from their original positions
756 (which unbalances the tree and causes the spatial
757 queries to be slow).
759 @param valuesPerNode Maximum number of elements to put at
760 a node.
762 @param numMeanSplits numMeanSplits = 0 gives a
763 fully axis aligned BSP-tree, where the balance operation attempts to balance
764 the tree so that every splitting plane has an equal number of left
765 and right children (i.e. it is a <B>median</B> split along that axis).
766 This tends to maximize average performance.
768 You can override this behavior by
769 setting a number of <B>mean</B> (average) splits. numMeanSplits = MAX_INT
770 creates a full oct-tree, which tends to optimize peak performance at the expense of
771 average performance. It tends to have better clustering behavior when
772 members are not uniformly distributed.
774 void balance(int valuesPerNode = 5, int numMeanSplits = 3) {
775 if (root == NULL) {
776 // Tree is empty
777 return;
780 Array<Handle> handleArray;
781 root->getHandles(handleArray);
783 // Delete the old tree
784 clear();
786 root = makeNode(handleArray, 0, handleArray.size() - 1,
787 valuesPerNode, numMeanSplits);
789 // Walk the tree, assigning splitBounds. We start with unbounded
790 // space.
791 root->assignSplitBounds(AABox::maxFinite());
793 #ifdef _DEBUG
794 root->verifyNode(Vector3::minFinite(), Vector3::maxFinite());
795 #endif
798 private:
801 @param parentMask The mask that this node returned from culledBy.
803 static void getIntersectingMembers(
804 const Array<Plane>& plane,
805 Array<T>& members,
806 Node* node,
807 uint32 parentMask) {
809 int dummy;
811 if (parentMask == 0) {
812 // None of these planes can cull anything
813 for (int v = node->valueArray.size() - 1; v >= 0; --v) {
814 members.append(node->valueArray[v].value);
817 // Iterate through child nodes
818 for (int c = 0; c < 2; ++c) {
819 if (node->child[c]) {
820 getIntersectingMembers(plane, members, node->child[c], 0);
823 } else {
825 // Test values at this node against remaining planes
826 for (int v = node->valueArray.size() - 1; v >= 0; --v) {
827 if (! node->valueArray[v].bounds.culledBy(plane, dummy, parentMask)) {
828 members.append(node->valueArray[v].value);
832 uint32 childMask = 0xFFFFFF;
834 // Iterate through child nodes
835 for (int c = 0; c < 2; ++c) {
836 if (node->child[c] &&
837 ! node->child[c]->splitBounds.culledBy(plane, dummy, parentMask, childMask)) {
838 // This node was not culled
839 getIntersectingMembers(plane, members, node->child[c], childMask);
845 public:
848 Returns all members inside the set of planes.
849 @param members The results are appended to this array.
851 void getIntersectingMembers(const Array<Plane>& plane, Array<T>& members) const {
852 if (root == NULL) {
853 return;
856 getIntersectingMembers(plane, members, root, 0xFFFFFF);
860 Typically used to find all visible
861 objects inside the view frustum (see also GCamera::getClipPlanes)... i.e. all objects
862 <B>not<B> culled by frustum.
864 Example:
865 <PRE>
866 Array<Object*> visible;
867 tree.getIntersectingMembers(camera.frustum(), visible);
868 // ... Draw all objects in the visible array.
869 </PRE>
870 @param members The results are appended to this array.
872 void getIntersectingMembers(const GCamera::Frustum& frustum, Array<T>& members) const {
873 Array<Plane> plane;
875 for (int i = 0; i < frustum.faceArray.size(); ++i) {
876 plane.append(frustum.faceArray[i].plane);
879 getIntersectingMembers(plane, members);
883 C++ STL style iterator variable. See beginBoxIntersection().
884 The iterator overloads the -> (dereference) operator, so this acts like a pointer
885 to the current member.
887 // This iterator turns Node::getIntersectingMembers into a
888 // coroutine. It first translates that method from recursive to
889 // stack based, then captures the system state (analogous to a Scheme
890 // continuation) after each element is appended to the member array,
891 // and allowing the computation to be restarted.
892 class BoxIntersectionIterator {
893 private:
894 friend class AABSPTree<T>;
896 /** True if this is the "end" iterator instance */
897 bool isEnd;
899 /** The box that we're testing against. */
900 AABox box;
902 /** Node that we're currently looking at. Undefined if isEnd is true. */
903 Node* node;
905 /** Nodes waiting to be processed */
906 // We could use backpointers within the tree and careful
907 // state management to avoid ever storing the stack-- but
908 // it is much easier this way and only inefficient if the
909 // caller uses post increment (which they shouldn't!).
910 Array<Node*> stack;
912 /** The next index of current->valueArray to return.
913 Undefined when isEnd is true.*/
914 int nextValueArrayIndex;
916 BoxIntersectionIterator() : isEnd(true) {}
918 BoxIntersectionIterator(const AABox& b, const Node* root) :
919 box(b), isEnd(root == NULL), nextValueArrayIndex(-1), node(const_cast<Node*>(root)) {
921 // We intentionally start at the "-1" index of the current node
922 // so we can use the preincrement operator to move ourselves to
923 // element 0 instead of repeating all of the code from the preincrement
924 // method. Note that this might cause us to become the "end"
925 // instance.
926 ++(*this);
929 public:
931 inline bool operator!=(const BoxIntersectionIterator& other) const {
932 return ! (*this == other);
935 bool operator==(const BoxIntersectionIterator& other) const {
936 if (isEnd) {
937 return other.isEnd;
938 } else if (other.isEnd) {
939 return false;
940 } else {
941 // Two non-end iterators; see if they match. This is kind of
942 // silly; users shouldn't call == on iterators in general unless
943 // one of them is the end iterator.
944 if ((box != other.box) || (node != other.node) ||
945 (nextValueArrayIndex != other.nextValueArrayIndex) ||
946 (stack.length() != other.stack.length())) {
947 return false;
950 // See if the stacks are the same
951 for (int i = 0; i < stack.length(); ++i) {
952 if (stack[i] != other.stack[i]) {
953 return false;
957 // We failed to find a difference; they must be the same
958 return true;
963 Pre increment.
965 BoxIntersectionIterator& operator++() {
966 ++nextValueArrayIndex;
968 bool foundIntersection = false;
969 while (! isEnd && ! foundIntersection) {
971 // Search for the next node if we've exhausted this one
972 while ((! isEnd) && (nextValueArrayIndex >= node->valueArray.length())) {
973 // If we entered this loop, then the iterator has exhausted the elements at
974 // node (possibly because it just switched to a child node with no members).
975 // This loop continues until it finds a node with members or reaches
976 // the end of the whole intersection search.
978 // If the right child overlaps the box, push it onto the stack for
979 // processing.
980 if ((node->child[1] != NULL) &&
981 (box.high()[node->splitAxis] > node->splitLocation)) {
982 stack.push(node->child[1]);
985 // If the left child overlaps the box, push it onto the stack for
986 // processing.
987 if ((node->child[0] != NULL) &&
988 (box.low()[node->splitAxis] < node->splitLocation)) {
989 stack.push(node->child[0]);
992 if (stack.length() > 0) {
993 // Go on to the next node (which may be either one of the ones we
994 // just pushed, or one from farther back the tree).
995 node = stack.pop();
996 nextValueArrayIndex = 0;
997 } else {
998 // That was the last node; we're done iterating
999 isEnd = true;
1003 // Search for the next intersection at this node until we run out of children
1004 while (! isEnd && ! foundIntersection && (nextValueArrayIndex < node->valueArray.length())) {
1005 if (box.intersects(node->valueArray[nextValueArrayIndex].bounds)) {
1006 foundIntersection = true;
1007 } else {
1008 ++nextValueArrayIndex;
1009 // If we exhaust this node, we'll loop around the master loop
1010 // to find a new node.
1015 return *this;
1019 Post increment (much slower than preincrement!).
1021 BoxIntersectionIterator operator++(int) {
1022 BoxIntersectionIterator old = *this;
1023 ++this;
1024 return old;
1027 /** Overloaded dereference operator so the iterator can masquerade as a pointer
1028 to a member */
1029 const T& operator*() const {
1030 alwaysAssertM(! isEnd, "Can't dereference the end element of an iterator");
1031 return node->valueArray[nextValueArrayIndex].value;
1034 /** Overloaded dereference operator so the iterator can masquerade as a pointer
1035 to a member */
1036 T const * operator->() const {
1037 alwaysAssertM(! isEnd, "Can't dereference the end element of an iterator");
1038 return &(stack.last()->valueArray[nextValueArrayIndex].value);
1041 /** Overloaded cast operator so the iterator can masquerade as a pointer
1042 to a member */
1043 operator T*() const {
1044 alwaysAssertM(! isEnd, "Can't dereference the end element of an iterator");
1045 return &(stack.last()->valueArray[nextValueArrayIndex].value);
1051 Iterates through the members that intersect the box
1053 BoxIntersectionIterator beginBoxIntersection(const AABox& box) const {
1054 return BoxIntersectionIterator(box, root);
1057 BoxIntersectionIterator endBoxIntersection() const {
1058 // The "end" iterator instance
1059 return BoxIntersectionIterator();
1063 Appends all members whose bounds intersect the box.
1064 See also AABSPTree::beginBoxIntersection.
1066 void getIntersectingMembers(const AABox& box, Array<T>& members) const {
1067 if (root == NULL) {
1068 return;
1070 root->getIntersectingMembers(box, Sphere(Vector3::ZERO, 0), members, false);
1075 @param members The results are appended to this array.
1077 void getIntersectingMembers(const Sphere& sphere, Array<T>& members) const {
1078 if (root == NULL) {
1079 return;
1082 AABox box;
1083 sphere.getBounds(box);
1084 root->getIntersectingMembers(box, sphere, members, true);
1088 /** See AABSPTree::beginRayIntersection */
1089 class RayIntersectionIterator {
1090 private:
1091 friend class AABSPTree<T>;
1093 /** The stack frame contains all the info needed to resume
1094 computation for the RayIntersectionIterator */
1096 struct StackFrame {
1097 const Node* node;
1099 /** the total checking bounds for this frame */
1100 float minTime;
1102 /** minTime^2 */
1103 float minTime2;
1104 float maxTime;
1106 /** what we're checking right now, either from minTime to the
1107 split, or from the split to maxTime (more or less...
1108 there are edge cases) */
1109 float startTime;
1110 float endTime;
1112 /** endTime^2 */
1113 float endTime2;
1115 int nextChild;
1117 /** current index into node's valueArray */
1118 int valIndex;
1121 /** cache intersection values when they're checked on the preSide,
1122 split so they don't need to be checked again after the split. */
1123 Array<float> intersectionCache;
1125 void init(const Node* inNode, const Ray& ray,
1126 float inMinTime, float inMaxTime) {
1127 node = inNode;
1128 minTime = inMinTime;
1129 maxTime = inMaxTime;
1130 minTime2 = square(minTime);
1131 valIndex = -1;
1133 intersectionCache.resize(node->valueArray.length());
1135 if (node->child[0] == NULL && node->child[1] == NULL) {
1136 startTime = minTime;
1137 endTime = maxTime;
1138 endTime2 = square(maxTime);
1139 nextChild = -1;
1140 return;
1143 Vector3::Axis splitAxis = node->splitAxis;
1144 double splitLocation = node->splitLocation;
1146 // this is the time along the ray until the split.
1147 // could be negative if the split is behind.
1148 double splitTime =
1149 (splitLocation - ray.origin[splitAxis]) /
1150 ray.direction[splitAxis];
1152 // If splitTime <= minTime we'll never reach the
1153 // split, so set it to inf so as not to confuse endTime.
1154 // It will be noted below that when splitTime is inf
1155 // only one of this node's children will be searched
1156 // (the pre child). Therefore it is critical that
1157 // the correct child is gone too.
1158 if (splitTime <= minTime) {
1159 splitTime = inf();
1162 startTime = minTime;
1163 endTime = min((double)maxTime, splitTime);
1164 endTime2 = square(endTime);
1166 double rayLocation = ray.origin[splitAxis] +
1167 ray.direction[splitAxis] * minTime;
1169 if (rayLocation == splitLocation) {
1170 // We're right on the split. Look ahead.
1171 rayLocation = ray.origin[splitAxis] +
1172 ray.direction[splitAxis] * maxTime;
1175 if (rayLocation == splitLocation) {
1176 // right on the split, looking exactly along
1177 // it, so consider no children.
1178 nextChild = -1;
1179 } else if(rayLocation <= splitLocation) {
1180 nextChild = 0;
1181 } else {
1182 nextChild = 1;
1187 public:
1188 /** A minimum bound on the distance to the intersection. */
1189 double minDistance;
1191 /** A maximum bound on the distance to the intersection. */
1192 double maxDistance;
1194 /** Counts how many bounding box intersection tests have been made so
1195 far. */
1196 int debugCounter;
1198 private:
1199 Ray ray;
1200 bool isEnd;
1202 Array<StackFrame> stack;
1203 int stackLength;
1204 int stackIndex;
1205 int breakFrameIndex;
1206 bool skipAABoxTests;
1207 double boxMaxDist2;
1209 RayIntersectionIterator(const Ray& r, const Node* root, double pMaxTime, bool skip)
1210 : minDistance(0), maxDistance(G3D::inf()), debugCounter(0),
1211 ray(r), isEnd(root == NULL),
1212 stackLength(20), stackIndex(0), breakFrameIndex(-1),
1213 skipAABoxTests(skip)
1215 stack.resize(stackLength);
1216 stack[stackIndex].init(root, ray, 0, G3D::inf());
1217 boxMaxDist2 = pMaxTime*pMaxTime;
1219 ++(*this);
1223 public:
1224 /* public so we can have empty ones */
1225 RayIntersectionIterator() : isEnd(true) {}
1227 inline bool operator!=(const RayIntersectionIterator& other) const {
1228 return ! (*this == other);
1231 /** Compares two iterators, but will only return true if both are at
1232 the end. */
1233 bool operator==(const RayIntersectionIterator& other) const {
1234 if (isEnd) {
1235 return other.isEnd;
1238 return false;
1242 Marks the node where the most recent intersection occurred. If
1243 the iterator exhausts this node it will stop and set itself to
1244 the end iterator.
1246 Use this after you find a true intersection to stop the iterator
1247 from searching more than necessary.
1248 <B>Beta API-- subject to change</B>
1250 // In theory this method could be smarter: the caller could pass in
1251 // the distance of the actual collision and the iterator would keep
1252 // itself from checking nodes or boxes beyond that distance.
1253 void markBreakNode() {
1254 breakFrameIndex = stackIndex;
1258 Clears the break node. Can be used before or after the iterator
1259 stops from a break.
1260 <B>Beta API-- subject to change</B>
1262 void clearBreakNode() {
1263 if (breakFrameIndex < 0) {
1264 return;
1267 if (isEnd && stackIndex >= 0) {
1268 isEnd = false;
1271 breakFrameIndex = -1;
1275 RayIntersectionIterator& operator++() {
1276 alwaysAssertM(!isEnd, "Can't increment the end element of an iterator");
1278 StackFrame* s = &stack[stackIndex];
1280 // leave the loop if:
1281 // end is reached (ie: stack is empty)
1282 // found an intersection
1284 while (true) {
1285 ++s->valIndex;
1287 if (s->valIndex >= s->node->valueArray.length()) {
1288 // This node is exhausted, look at its
1289 // children.
1291 Node* child = (s->nextChild >= 0) ?
1292 s->node->child[s->nextChild] : NULL;
1293 double childStartTime = s->startTime;
1294 double childEndTime = s->endTime;
1296 if (s->endTime < s->maxTime) {
1297 // we can come back to this frame,
1298 // so reset it
1299 s->valIndex = -1;
1300 s->startTime = s->endTime;
1301 s->endTime = s->maxTime;
1302 s->endTime2 = square(s->maxTime);
1303 s->nextChild = (s->nextChild >= 0) ?
1304 (1 - s->nextChild) : -1;
1306 // this could be changed somehow,
1307 // since Array already does the
1308 // power-of-two growth stuff
1309 if (stackIndex == stackLength) {
1310 stackLength *= 2;
1311 stack.resize(stackLength);
1313 } else {
1314 // tail-recursion: we won't come
1315 // back to this frame, so we can
1316 // remove it.
1318 if (stackIndex == breakFrameIndex) {
1319 // This will be the case if the
1320 // break frame is set on a node, but
1321 // the node is exhausted so it won't
1322 // be put on the stack. Here we
1323 // decrement the break frame so that
1324 // the break occurs when the current
1325 // frame's parent is resumed.
1326 --breakFrameIndex;
1329 --stackIndex;
1332 // There could have been a resize on the array, so
1333 // do not use s (pointer into the array)!
1335 if (child != NULL) {
1336 ++stackIndex;
1337 stack[stackIndex].init(
1338 child, ray,
1339 childStartTime, childEndTime);
1342 if ((stackIndex < 0) || (stackIndex == breakFrameIndex)) {
1343 isEnd = true;
1344 break;
1347 s = &stack[stackIndex];
1348 continue;
1351 if (skipAABoxTests) {
1352 // No AABox test-- return everything
1353 minDistance = s->startTime;
1354 maxDistance = s->endTime;
1355 break;
1356 } else {
1357 double t2;
1358 // this can be an exact equals because the two
1359 // variables are initialized to the same thing
1360 if (s->startTime == s->minTime) {
1361 Vector3 location;
1362 bool insiteOk;
1363 Vector3 mynormal;
1364 if (
1365 VMAP::MyCollisionDetection::collisionLocationForMovingPointFixedAABox(
1366 ray.origin, ray.direction,
1367 s->node->valueArray[s->valIndex].bounds,
1368 location,insiteOk, mynormal )) {
1369 // Optimization: store t-squared
1370 t2 = (location - ray.origin).squaredLength();
1371 } else {
1372 t2 = inf();
1374 if(t2 > boxMaxDist2)
1375 t2=inf(); // too far off
1376 //t = ray.intersectionTime(s->node->valueArray[s->valIndex].bounds);
1377 s->intersectionCache[s->valIndex] = t2;
1378 ++debugCounter;
1379 } else {
1380 t2 = s->intersectionCache[s->valIndex];
1383 // use minTime here because intersection may be
1384 // detected pre-split, but be valid post-split, too.
1385 if ((t2 >= s->minTime2) && (t2 < s->endTime2)) {
1386 // Gives slightly tighter bounds but runs slower:
1387 // minDistance = max(t, s->startTime);
1388 minDistance = s->startTime;
1389 maxDistance = s->endTime;
1390 break;
1396 return *this;
1400 /** Overloaded dereference operator so the iterator can masquerade as a pointer
1401 to a member */
1402 const T& operator*() const {
1403 alwaysAssertM(! isEnd, "Can't dereference the end element of an iterator");
1404 return stack[stackIndex].node->valueArray[stack[stackIndex].valIndex].value;
1407 /** Overloaded dereference operator so the iterator can masquerade as a pointer
1408 to a member */
1409 T const * operator->() const {
1410 alwaysAssertM(! isEnd, "Can't dereference the end element of an iterator");
1411 return &(stack[stackIndex].node->valueArray[stack[stackIndex].valIndex].value);
1414 /** Overloaded cast operator so the iterator can masquerade as a pointer
1415 to a member */
1416 operator T*() const {
1417 alwaysAssertM(! isEnd, "Can't dereference the end element of an iterator");
1418 return &(stack[stackIndex].node->valueArray[stack[stackIndex].valIndex].value);
1425 Generates a RayIntersectionIterator that produces successive
1426 elements from the set whose bounding boxes are intersected by the ray.
1427 Typically used for ray tracing, hit-scan, and collision detection.
1429 The elements are generated mostly in the order that they are hit by the
1430 ray, so that iteration may end abruptly when the closest intersection to
1431 the ray origin has been reached. Because the elements within a given
1432 kd-tree node are unordered, iteration may need to proceed a little past
1433 the first member returned in order to find the closest intersection. The
1434 iterator doesn't automatically find the first intersection because it is
1435 looking at bounding boxes, not the true intersections.
1437 When the caller finds a true intersection it should call markBreakNode()
1438 on the iterator. This will stop the iterator (setting it to the end
1439 iterator) when the current node and relevant children are exhausted.
1441 Complicating the matter further, some members straddle the plane. The
1442 iterator produces these members <I>twice</I>. The first time it is
1443 produced the caller should only consider intersections on the near side of
1444 the split plane. The second time, the caller should only consider
1445 intersections on the far side. The minDistance and maxDistance fields
1446 specify the range on which intersections should be considered. Be aware
1447 that they may be inf or zero.
1449 An example of how to use the iterator follows. Almost all ray intersection
1450 tests will have identical structure.
1452 <PRE>
1454 void findFirstIntersection(
1455 const Ray& ray,
1456 Object*& firstObject,
1457 double& firstTime) {
1459 firstObject = NULL;
1460 firstDistance = inf();
1462 typedef AABSPTree<Object*>::RayIntersectionIterator IT;
1463 const IT end = tree.endRayIntersection();
1465 for (IT obj = tree.beginRayIntersection(ray);
1466 obj != end;
1467 ++obj) { // (preincrement is *much* faster than postincrement!)
1469 // Call your accurate intersection test here. It is guaranteed
1470 // that the ray hits the bounding box of obj. (*obj) has type T,
1471 // so you can call methods directly using the "->" operator.
1472 double t = obj->distanceUntilIntersection(ray);
1474 // Often methods like "distanceUntilIntersection" can be made more
1475 // efficient by providing them with the time at which to start and
1476 // to give up looking for an intersection; that is,
1477 // obj.minDistance and iMin(firstDistance, obj.maxDistance).
1479 static const double epsilon = 0.00001;
1480 if ((t < firstDistance) &&
1481 (t <= obj.maxDistance + epsilon) &&
1482 (t >= obj.minDistance - epsilon)) {
1484 // This is the new best collision time
1485 firstObject = obj;
1486 firstDistance = t;
1488 // Tell the iterator that we've found at least one
1489 // intersection. It will finish looking at all
1490 // objects in this node and then terminate.
1491 obj.markBreakNode();
1495 </PRE>
1498 @param skipAABoxTests Set to true when the intersection test for a
1499 member is faster than an AABox-ray intersection test. In that case,
1500 the iterator will not use a bounding box test on values that are
1501 returned. Leave false (the default) for objects with slow intersection
1502 tests. In that case, the iterator guarantees that the ray hits the
1503 bounds of any object returned.
1505 @cite Implementation by Pete Hopkins
1507 RayIntersectionIterator beginRayIntersection(const Ray& ray, double pMaxTime, bool skipAABoxTests = false) const {
1508 return RayIntersectionIterator(ray, root, pMaxTime, skipAABoxTests);
1511 RayIntersectionIterator endRayIntersection() const {
1512 return RayIntersectionIterator();
1517 Returns an array of all members of the set. See also AABSPTree::begin.
1519 void getMembers(Array<T>& members) const {
1520 memberTable.getKeys(members);
1525 C++ STL style iterator variable. See begin().
1526 Overloads the -> (dereference) operator, so this acts like a pointer
1527 to the current member.
1529 class Iterator {
1530 private:
1531 friend class AABSPTree<T>;
1533 // Note: this is a Table iterator, we are currently defining
1534 // Set iterator
1535 typename Table<T, Node*>::Iterator it;
1537 Iterator(const typename Table<T, Node*>::Iterator& it) : it(it) {}
1539 public:
1540 inline bool operator!=(const Iterator& other) const {
1541 return !(*this == other);
1544 bool operator==(const Iterator& other) const {
1545 return it == other.it;
1549 Pre increment.
1551 Iterator& operator++() {
1552 ++it;
1553 return *this;
1557 Post increment (slower than preincrement).
1559 Iterator operator++(int) {
1560 Iterator old = *this;
1561 ++(*this);
1562 return old;
1565 const T& operator*() const {
1566 return it->key;
1569 T* operator->() const {
1570 return &(it->key);
1573 operator T*() const {
1574 return &(it->key);
1580 C++ STL style iterator method. Returns the first member.
1581 Use preincrement (++entry) to get to the next element (iteration
1582 order is arbitrary).
1583 Do not modify the set while iterating.
1585 Iterator begin() const {
1586 return Iterator(memberTable.begin());
1591 C++ STL style iterator method. Returns one after the last iterator
1592 element.
1594 Iterator end() const {
1595 return Iterator(memberTable.end());
1599 #define KDTreeSet AABSPTree
1603 #endif