physics library version update
[spacedolphin:spacedolphin.git] / lib / chipmunk / src / cpBBTree.c
1 /* Copyright (c) 2009 Scott Lembcke
2  * 
3  * Permission is hereby granted, free of charge, to any person obtaining a copy
4  * of this software and associated documentation files (the "Software"), to deal
5  * in the Software without restriction, including without limitation the rights
6  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7  * copies of the Software, and to permit persons to whom the Software is
8  * furnished to do so, subject to the following conditions:
9  * 
10  * The above copyright notice and this permission notice shall be included in
11  * all copies or substantial portions of the Software.
12  * 
13  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
19  * SOFTWARE.
20  */
21
22 #include "stdlib.h"
23 #include "stdio.h"
24
25 #include "chipmunk_private.h"
26
27 static inline cpSpatialIndexClass *Klass();
28
29 typedef struct Node Node;
30 typedef struct Pair Pair;
31
32 struct cpBBTree {
33         cpSpatialIndex spatialIndex;
34         cpBBTreeVelocityFunc velocityFunc;
35         
36         cpHashSet *leaves;
37         Node *root;
38         
39         Node *pooledNodes;
40         Pair *pooledPairs;
41         cpArray *allocatedBuffers;
42         
43         cpTimestamp stamp;
44 };
45
46 struct Node {
47         void *obj;
48         cpBB bb;
49         Node *parent;
50         
51         union {
52                 // Internal nodes
53                 struct { Node *a, *b; } children;
54                 
55                 // Leaves
56                 struct {
57                         cpTimestamp stamp;
58                         Pair *pairs;
59                 } leaf;
60         } node;
61 };
62
63 // Can't use anonymous unions and still get good x-compiler compatability
64 #define A node.children.a
65 #define B node.children.b
66 #define STAMP node.leaf.stamp
67 #define PAIRS node.leaf.pairs
68
69 typedef struct Thread {
70         Pair *prev;
71         Node *leaf;
72         Pair *next;
73 } Thread;
74
75 struct Pair { Thread a, b; };
76
77 //MARK: Misc Functions
78
79 static inline cpBB
80 GetBB(cpBBTree *tree, void *obj)
81 {
82         cpBB bb = tree->spatialIndex.bbfunc(obj);
83         
84         cpBBTreeVelocityFunc velocityFunc = tree->velocityFunc;
85         if(velocityFunc){
86                 cpFloat coef = 0.1f;
87                 cpFloat x = (bb.r - bb.l)*coef;
88                 cpFloat y = (bb.t - bb.b)*coef;
89                 
90                 cpVect v = cpvmult(velocityFunc(obj), 0.1f);
91                 return cpBBNew(bb.l + cpfmin(-x, v.x), bb.b + cpfmin(-y, v.y), bb.r + cpfmax(x, v.x), bb.t + cpfmax(y, v.y));
92         } else {
93                 return bb;
94         }
95 }
96
97 static inline cpBBTree *
98 GetTree(cpSpatialIndex *index)
99 {
100         return (index && index->klass == Klass() ? (cpBBTree *)index : NULL);
101 }
102
103 static inline Node *
104 GetRootIfTree(cpSpatialIndex *index){
105         return (index && index->klass == Klass() ? ((cpBBTree *)index)->root : NULL);
106 }
107
108 static inline cpBBTree *
109 GetMasterTree(cpBBTree *tree)
110 {
111         cpBBTree *dynamicTree = GetTree(tree->spatialIndex.dynamicIndex);
112         return (dynamicTree ? dynamicTree : tree);
113 }
114
115 static inline void
116 IncrementStamp(cpBBTree *tree)
117 {
118         cpBBTree *dynamicTree = GetTree(tree->spatialIndex.dynamicIndex);
119         if(dynamicTree){
120                 dynamicTree->stamp++;
121         } else {
122                 tree->stamp++;
123         }
124 }
125
126 //MARK: Pair/Thread Functions
127
128 static void
129 PairRecycle(cpBBTree *tree, Pair *pair)
130 {
131         // Share the pool of the master tree.
132         // TODO would be lovely to move the pairs stuff into an external data structure.
133         tree = GetMasterTree(tree);
134         
135         pair->a.next = tree->pooledPairs;
136         tree->pooledPairs = pair;
137 }
138
139 static Pair *
140 PairFromPool(cpBBTree *tree)
141 {
142         // Share the pool of the master tree.
143         // TODO would be lovely to move the pairs stuff into an external data structure.
144         tree = GetMasterTree(tree);
145         
146         Pair *pair = tree->pooledPairs;
147         
148         if(pair){
149                 tree->pooledPairs = pair->a.next;
150                 return pair;
151         } else {
152                 // Pool is exhausted, make more
153                 int count = CP_BUFFER_BYTES/sizeof(Pair);
154                 cpAssertHard(count, "Internal Error: Buffer size is too small.");
155                 
156                 Pair *buffer = (Pair *)cpcalloc(1, CP_BUFFER_BYTES);
157                 cpArrayPush(tree->allocatedBuffers, buffer);
158                 
159                 // push all but the first one, return the first instead
160                 for(int i=1; i<count; i++) PairRecycle(tree, buffer + i);
161                 return buffer;
162         }
163 }
164
165 static inline void
166 ThreadUnlink(Thread thread)
167 {
168         Pair *next = thread.next;
169         Pair *prev = thread.prev;
170         
171         if(next){
172                 if(next->a.leaf == thread.leaf) next->a.prev = prev; else next->b.prev = prev;
173         }
174         
175         if(prev){
176                 if(prev->a.leaf == thread.leaf) prev->a.next = next; else prev->b.next = next;
177         } else {
178                 thread.leaf->PAIRS = next;
179         }
180 }
181
182 static void
183 PairsClear(Node *leaf, cpBBTree *tree)
184 {
185         Pair *pair = leaf->PAIRS;
186         leaf->PAIRS = NULL;
187         
188         while(pair){
189                 if(pair->a.leaf == leaf){
190                         Pair *next = pair->a.next;
191                         ThreadUnlink(pair->b);
192                         PairRecycle(tree, pair);
193                         pair = next;
194                 } else {
195                         Pair *next = pair->b.next;
196                         ThreadUnlink(pair->a);
197                         PairRecycle(tree, pair);
198                         pair = next;
199                 }
200         }
201 }
202
203 static void
204 PairInsert(Node *a, Node *b, cpBBTree *tree)
205 {
206         Pair *nextA = a->PAIRS, *nextB = b->PAIRS;
207         Pair *pair = PairFromPool(tree);
208         Pair temp = {{NULL, a, nextA},{NULL, b, nextB}};
209         
210         a->PAIRS = b->PAIRS = pair;
211         *pair = temp;
212         
213         if(nextA){
214                 if(nextA->a.leaf == a) nextA->a.prev = pair; else nextA->b.prev = pair;
215         }
216         
217         if(nextB){
218                 if(nextB->a.leaf == b) nextB->a.prev = pair; else nextB->b.prev = pair;
219         }
220 }
221
222
223 //MARK: Node Functions
224
225 static void
226 NodeRecycle(cpBBTree *tree, Node *node)
227 {
228         node->parent = tree->pooledNodes;
229         tree->pooledNodes = node;
230 }
231
232 static Node *
233 NodeFromPool(cpBBTree *tree)
234 {
235         Node *node = tree->pooledNodes;
236         
237         if(node){
238                 tree->pooledNodes = node->parent;
239                 return node;
240         } else {
241                 // Pool is exhausted, make more
242                 int count = CP_BUFFER_BYTES/sizeof(Node);
243                 cpAssertHard(count, "Internal Error: Buffer size is too small.");
244                 
245                 Node *buffer = (Node *)cpcalloc(1, CP_BUFFER_BYTES);
246                 cpArrayPush(tree->allocatedBuffers, buffer);
247                 
248                 // push all but the first one, return the first instead
249                 for(int i=1; i<count; i++) NodeRecycle(tree, buffer + i);
250                 return buffer;
251         }
252 }
253
254 static inline void
255 NodeSetA(Node *node, Node *value)
256 {
257         node->A = value;
258         value->parent = node;
259 }
260
261 static inline void
262 NodeSetB(Node *node, Node *value)
263 {
264         node->B = value;
265         value->parent = node;
266 }
267
268 static Node *
269 NodeNew(cpBBTree *tree, Node *a, Node *b)
270 {
271         Node *node = NodeFromPool(tree);
272         
273         node->obj = NULL;
274         node->bb = cpBBMerge(a->bb, b->bb);
275         node->parent = NULL;
276         
277         NodeSetA(node, a);
278         NodeSetB(node, b);
279         
280         return node;
281 }
282
283 static inline cpBool
284 NodeIsLeaf(Node *node)
285 {
286         return (node->obj != NULL);
287 }
288
289 static inline Node *
290 NodeOther(Node *node, Node *child)
291 {
292         return (node->A == child ? node->B : node->A);
293 }
294
295 static inline void
296 NodeReplaceChild(Node *parent, Node *child, Node *value, cpBBTree *tree)
297 {
298         cpAssertSoft(!NodeIsLeaf(parent), "Internal Error: Cannot replace child of a leaf.");
299         cpAssertSoft(child == parent->A || child == parent->B, "Internal Error: Node is not a child of parent.");
300         
301         if(parent->A == child){
302                 NodeRecycle(tree, parent->A);
303                 NodeSetA(parent, value);
304         } else {
305                 NodeRecycle(tree, parent->B);
306                 NodeSetB(parent, value);
307         }
308         
309         for(Node *node=parent; node; node = node->parent){
310                 node->bb = cpBBMerge(node->A->bb, node->B->bb);
311         }
312 }
313
314 //MARK: Subtree Functions
315
316 static inline cpFloat
317 cpBBProximity(cpBB a, cpBB b)
318 {
319         return cpfabs(a.l + a.r - b.l - b.r) + cpfabs(a.b + a.t - b.b - b.t);
320 }
321
322 static Node *
323 SubtreeInsert(Node *subtree, Node *leaf, cpBBTree *tree)
324 {
325         if(subtree == NULL){
326                 return leaf;
327         } else if(NodeIsLeaf(subtree)){
328                 return NodeNew(tree, leaf, subtree);
329         } else {
330                 cpFloat cost_a = cpBBArea(subtree->B->bb) + cpBBMergedArea(subtree->A->bb, leaf->bb);
331                 cpFloat cost_b = cpBBArea(subtree->A->bb) + cpBBMergedArea(subtree->B->bb, leaf->bb);
332                 
333                 if(cost_a == cost_b){
334                         cost_a = cpBBProximity(subtree->A->bb, leaf->bb);
335                         cost_b = cpBBProximity(subtree->B->bb, leaf->bb);
336                 }
337                 
338                 if(cost_b < cost_a){
339                         NodeSetB(subtree, SubtreeInsert(subtree->B, leaf, tree));
340                 } else {
341                         NodeSetA(subtree, SubtreeInsert(subtree->A, leaf, tree));
342                 }
343                 
344                 subtree->bb = cpBBMerge(subtree->bb, leaf->bb);
345                 return subtree;
346         }
347 }
348
349 static void
350 SubtreeQuery(Node *subtree, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data)
351 {
352         if(cpBBIntersects(subtree->bb, bb)){
353                 if(NodeIsLeaf(subtree)){
354                         func(obj, subtree->obj, data);
355                 } else {
356                         SubtreeQuery(subtree->A, obj, bb, func, data);
357                         SubtreeQuery(subtree->B, obj, bb, func, data);
358                 }
359         }
360 }
361
362
363 static cpFloat
364 SubtreeSegmentQuery(Node *subtree, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data)
365 {
366         if(NodeIsLeaf(subtree)){
367                 return func(obj, subtree->obj, data);
368         } else {
369                 cpFloat t_a = cpBBSegmentQuery(subtree->A->bb, a, b);
370                 cpFloat t_b = cpBBSegmentQuery(subtree->B->bb, a, b);
371                 
372                 if(t_a < t_b){
373                         if(t_a < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->A, obj, a, b, t_exit, func, data));
374                         if(t_b < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->B, obj, a, b, t_exit, func, data));
375                 } else {
376                         if(t_b < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->B, obj, a, b, t_exit, func, data));
377                         if(t_a < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->A, obj, a, b, t_exit, func, data));
378                 }
379                 
380                 return t_exit;
381         }
382 }
383
384 static void
385 SubtreeRecycle(cpBBTree *tree, Node *node)
386 {
387         if(!NodeIsLeaf(node)){
388                 SubtreeRecycle(tree, node->A);
389                 SubtreeRecycle(tree, node->B);
390                 NodeRecycle(tree, node);
391         }
392 }
393
394 static inline Node *
395 SubtreeRemove(Node *subtree, Node *leaf, cpBBTree *tree)
396 {
397         if(leaf == subtree){
398                 return NULL;
399         } else {
400                 Node *parent = leaf->parent;
401                 if(parent == subtree){
402                         Node *other = NodeOther(subtree, leaf);
403                         other->parent = subtree->parent;
404                         NodeRecycle(tree, subtree);
405                         return other;
406                 } else {
407                         NodeReplaceChild(parent->parent, parent, NodeOther(parent, leaf), tree);
408                         return subtree;
409                 }
410         }
411 }
412
413 //MARK: Marking Functions
414
415 typedef struct MarkContext {
416         cpBBTree *tree;
417         Node *staticRoot;
418         cpSpatialIndexQueryFunc func;
419         void *data;
420 } MarkContext;
421
422 static void
423 MarkLeafQuery(Node *subtree, Node *leaf, cpBool left, MarkContext *context)
424 {
425         if(cpBBIntersects(leaf->bb, subtree->bb)){
426                 if(NodeIsLeaf(subtree)){
427                         if(left){
428                                 PairInsert(leaf, subtree, context->tree);
429                         } else {
430                                 if(subtree->STAMP < leaf->STAMP) PairInsert(subtree, leaf, context->tree);
431                                 context->func(leaf->obj, subtree->obj, context->data);
432                         }
433                 } else {
434                         MarkLeafQuery(subtree->A, leaf, left, context);
435                         MarkLeafQuery(subtree->B, leaf, left, context);
436                 }
437         }
438 }
439
440 static void
441 MarkLeaf(Node *leaf, MarkContext *context)
442 {
443         cpBBTree *tree = context->tree;
444         if(leaf->STAMP == GetMasterTree(tree)->stamp){
445                 Node *staticRoot = context->staticRoot;
446                 if(staticRoot) MarkLeafQuery(staticRoot, leaf, cpFalse, context);
447                 
448                 for(Node *node = leaf; node->parent; node = node->parent){
449                         if(node == node->parent->A){
450                                 MarkLeafQuery(node->parent->B, leaf, cpTrue, context);
451                         } else {
452                                 MarkLeafQuery(node->parent->A, leaf, cpFalse, context);
453                         }
454                 }
455         } else {
456                 Pair *pair = leaf->PAIRS;
457                 while(pair){
458                         if(leaf == pair->b.leaf){
459                                 context->func(pair->a.leaf->obj, leaf->obj, context->data);
460                                 pair = pair->b.next;
461                         } else {
462                                 pair = pair->a.next;
463                         }
464                 }
465         }
466 }
467
468 static void
469 MarkSubtree(Node *subtree, MarkContext *context)
470 {
471         if(NodeIsLeaf(subtree)){
472                 MarkLeaf(subtree, context);
473         } else {
474                 MarkSubtree(subtree->A, context);
475                 MarkSubtree(subtree->B, context);
476         }
477 }
478
479 //MARK: Leaf Functions
480
481 static Node *
482 LeafNew(cpBBTree *tree, void *obj, cpBB bb)
483 {
484         Node *node = NodeFromPool(tree);
485         node->obj = obj;
486         node->bb = GetBB(tree, obj);
487         
488         node->parent = NULL;
489         node->STAMP = 0;
490         node->PAIRS = NULL;
491         
492         return node;
493 }
494
495 static cpBool
496 LeafUpdate(Node *leaf, cpBBTree *tree)
497 {
498         Node *root = tree->root;
499         cpBB bb = tree->spatialIndex.bbfunc(leaf->obj);
500         
501         if(!cpBBContainsBB(leaf->bb, bb)){
502                 leaf->bb = GetBB(tree, leaf->obj);
503                 
504                 root = SubtreeRemove(root, leaf, tree);
505                 tree->root = SubtreeInsert(root, leaf, tree);
506                 
507                 PairsClear(leaf, tree);
508                 leaf->STAMP = GetMasterTree(tree)->stamp;
509                 
510                 return cpTrue;
511         }
512         
513         return cpFalse;
514 }
515
516 static void VoidQueryFunc(void *obj1, void *obj2, void *data){}
517
518 static void
519 LeafAddPairs(Node *leaf, cpBBTree *tree)
520 {
521         cpSpatialIndex *dynamicIndex = tree->spatialIndex.dynamicIndex;
522         if(dynamicIndex){
523                 Node *dynamicRoot = GetRootIfTree(dynamicIndex);
524                 if(dynamicRoot){
525                         cpBBTree *dynamicTree = GetTree(dynamicIndex);
526                         MarkContext context = {dynamicTree, NULL, NULL, NULL};
527                         MarkLeafQuery(dynamicRoot, leaf, cpTrue, &context);
528                 }
529         } else {
530                 Node *staticRoot = GetRootIfTree(tree->spatialIndex.staticIndex);
531                 MarkContext context = {tree, staticRoot, VoidQueryFunc, NULL};
532                 MarkLeaf(leaf, &context);
533         }
534 }
535
536 //MARK: Memory Management Functions
537
538 cpBBTree *
539 cpBBTreeAlloc(void)
540 {
541         return (cpBBTree *)cpcalloc(1, sizeof(cpBBTree));
542 }
543
544 static int
545 leafSetEql(void *obj, Node *node)
546 {
547         return (obj == node->obj);
548 }
549
550 static void *
551 leafSetTrans(void *obj, cpBBTree *tree)
552 {
553         return LeafNew(tree, obj, tree->spatialIndex.bbfunc(obj));
554 }
555
556 cpSpatialIndex *
557 cpBBTreeInit(cpBBTree *tree, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex)
558 {
559         cpSpatialIndexInit((cpSpatialIndex *)tree, Klass(), bbfunc, staticIndex);
560         
561         tree->velocityFunc = NULL;
562         
563         tree->leaves = cpHashSetNew(0, (cpHashSetEqlFunc)leafSetEql);
564         tree->root = NULL;
565         
566         tree->pooledNodes = NULL;
567         tree->allocatedBuffers = cpArrayNew(0);
568         
569         tree->stamp = 0;
570         
571         return (cpSpatialIndex *)tree;
572 }
573
574 void
575 cpBBTreeSetVelocityFunc(cpSpatialIndex *index, cpBBTreeVelocityFunc func)
576 {
577         if(index->klass != Klass()){
578                 cpAssertWarn(cpFalse, "Ignoring cpBBTreeSetVelocityFunc() call to non-tree spatial index.");
579                 return;
580         }
581         
582         ((cpBBTree *)index)->velocityFunc = func;
583 }
584
585 cpSpatialIndex *
586 cpBBTreeNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex)
587 {
588         return cpBBTreeInit(cpBBTreeAlloc(), bbfunc, staticIndex);
589 }
590
591 static void
592 cpBBTreeDestroy(cpBBTree *tree)
593 {
594         cpHashSetFree(tree->leaves);
595         
596         if(tree->allocatedBuffers) cpArrayFreeEach(tree->allocatedBuffers, cpfree);
597         cpArrayFree(tree->allocatedBuffers);
598 }
599
600 //MARK: Insert/Remove
601
602 static void
603 cpBBTreeInsert(cpBBTree *tree, void *obj, cpHashValue hashid)
604 {
605         Node *leaf = (Node *)cpHashSetInsert(tree->leaves, hashid, obj, tree, (cpHashSetTransFunc)leafSetTrans);
606         
607         Node *root = tree->root;
608         tree->root = SubtreeInsert(root, leaf, tree);
609         
610         leaf->STAMP = GetMasterTree(tree)->stamp;
611         LeafAddPairs(leaf, tree);
612         IncrementStamp(tree);
613 }
614
615 static void
616 cpBBTreeRemove(cpBBTree *tree, void *obj, cpHashValue hashid)
617 {
618         Node *leaf = (Node *)cpHashSetRemove(tree->leaves, hashid, obj);
619         
620         tree->root = SubtreeRemove(tree->root, leaf, tree);
621         PairsClear(leaf, tree);
622         NodeRecycle(tree, leaf);
623 }
624
625 static cpBool
626 cpBBTreeContains(cpBBTree *tree, void *obj, cpHashValue hashid)
627 {
628         return (cpHashSetFind(tree->leaves, hashid, obj) != NULL);
629 }
630
631 //MARK: Reindex
632
633 static void
634 cpBBTreeReindexQuery(cpBBTree *tree, cpSpatialIndexQueryFunc func, void *data)
635 {
636         if(!tree->root) return;
637         
638         // LeafUpdate() may modify tree->root. Don't cache it.
639         cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)LeafUpdate, tree);
640         
641         cpSpatialIndex *staticIndex = tree->spatialIndex.staticIndex;
642         Node *staticRoot = (staticIndex && staticIndex->klass == Klass() ? ((cpBBTree *)staticIndex)->root : NULL);
643         
644         MarkContext context = {tree, staticRoot, func, data};
645         MarkSubtree(tree->root, &context);
646         if(staticIndex && !staticRoot) cpSpatialIndexCollideStatic((cpSpatialIndex *)tree, staticIndex, func, data);
647         
648         IncrementStamp(tree);
649 }
650
651 static void
652 cpBBTreeReindex(cpBBTree *tree)
653 {
654         cpBBTreeReindexQuery(tree, VoidQueryFunc, NULL);
655 }
656
657 static void
658 cpBBTreeReindexObject(cpBBTree *tree, void *obj, cpHashValue hashid)
659 {
660         Node *leaf = (Node *)cpHashSetFind(tree->leaves, hashid, obj);
661         if(leaf){
662                 if(LeafUpdate(leaf, tree)) LeafAddPairs(leaf, tree);
663                 IncrementStamp(tree);
664         }
665 }
666
667 //MARK: Query
668
669 static void
670 cpBBTreeSegmentQuery(cpBBTree *tree, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data)
671 {
672         Node *root = tree->root;
673         if(root) SubtreeSegmentQuery(root, obj, a, b, t_exit, func, data);
674 }
675
676 static void
677 cpBBTreeQuery(cpBBTree *tree, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data)
678 {
679         if(tree->root) SubtreeQuery(tree->root, obj, bb, func, data);
680 }
681
682 //MARK: Misc
683
684 static int
685 cpBBTreeCount(cpBBTree *tree)
686 {
687         return cpHashSetCount(tree->leaves);
688 }
689
690 typedef struct eachContext {
691         cpSpatialIndexIteratorFunc func;
692         void *data;
693 } eachContext;
694
695 static void each_helper(Node *node, eachContext *context){context->func(node->obj, context->data);}
696
697 static void
698 cpBBTreeEach(cpBBTree *tree, cpSpatialIndexIteratorFunc func, void *data)
699 {
700         eachContext context = {func, data};
701         cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)each_helper, &context);
702 }
703
704 static cpSpatialIndexClass klass = {
705         (cpSpatialIndexDestroyImpl)cpBBTreeDestroy,
706         
707         (cpSpatialIndexCountImpl)cpBBTreeCount,
708         (cpSpatialIndexEachImpl)cpBBTreeEach,
709         
710         (cpSpatialIndexContainsImpl)cpBBTreeContains,
711         (cpSpatialIndexInsertImpl)cpBBTreeInsert,
712         (cpSpatialIndexRemoveImpl)cpBBTreeRemove,
713         
714         (cpSpatialIndexReindexImpl)cpBBTreeReindex,
715         (cpSpatialIndexReindexObjectImpl)cpBBTreeReindexObject,
716         (cpSpatialIndexReindexQueryImpl)cpBBTreeReindexQuery,
717         
718         (cpSpatialIndexQueryImpl)cpBBTreeQuery,
719         (cpSpatialIndexSegmentQueryImpl)cpBBTreeSegmentQuery,
720 };
721
722 static inline cpSpatialIndexClass *Klass(){return &klass;}
723
724
725 //MARK: Tree Optimization
726
727 static int
728 cpfcompare(const cpFloat *a, const cpFloat *b){
729         return (*a < *b ? -1 : (*b < *a ? 1 : 0));
730 }
731
732 static void
733 fillNodeArray(Node *node, Node ***cursor){
734         (**cursor) = node;
735         (*cursor)++;
736 }
737
738 static Node *
739 partitionNodes(cpBBTree *tree, Node **nodes, int count)
740 {
741         if(count == 1){
742                 return nodes[0];
743         } else if(count == 2) {
744                 return NodeNew(tree, nodes[0], nodes[1]);
745         }
746         
747         // Find the AABB for these nodes
748         cpBB bb = nodes[0]->bb;
749         for(int i=1; i<count; i++) bb = cpBBMerge(bb, nodes[i]->bb);
750         
751         // Split it on it's longest axis
752         cpBool splitWidth = (bb.r - bb.l > bb.t - bb.b);
753         
754         // Sort the bounds and use the median as the splitting point
755         cpFloat *bounds = (cpFloat *)cpcalloc(count*2, sizeof(cpFloat));
756         if(splitWidth){
757                 for(int i=0; i<count; i++){
758                         bounds[2*i + 0] = nodes[i]->bb.l;
759                         bounds[2*i + 1] = nodes[i]->bb.r;
760                 }
761         } else {
762                 for(int i=0; i<count; i++){
763                         bounds[2*i + 0] = nodes[i]->bb.b;
764                         bounds[2*i + 1] = nodes[i]->bb.t;
765                 }
766         }
767         
768         qsort(bounds, count*2, sizeof(cpFloat), (int (*)(const void *, const void *))cpfcompare);
769         cpFloat split = (bounds[count - 1] + bounds[count])*0.5f; // use the medain as the split
770         cpfree(bounds);
771
772         // Generate the child BBs
773         cpBB a = bb, b = bb;
774         if(splitWidth) a.r = b.l = split; else a.t = b.b = split;
775         
776         // Partition the nodes
777         int right = count;
778         for(int left=0; left < right;){
779                 Node *node = nodes[left];
780                 if(cpBBMergedArea(node->bb, b) < cpBBMergedArea(node->bb, a)){
781 //              if(cpBBProximity(node->bb, b) < cpBBProximity(node->bb, a)){
782                         right--;
783                         nodes[left] = nodes[right];
784                         nodes[right] = node;
785                 } else {
786                         left++;
787                 }
788         }
789         
790         if(right == count){
791                 Node *node = NULL;
792                 for(int i=0; i<count; i++) node = SubtreeInsert(node, nodes[i], tree);
793                 return node;
794         }
795         
796         // Recurse and build the node!
797         return NodeNew(tree,
798                 partitionNodes(tree, nodes, right),
799                 partitionNodes(tree, nodes + right, count - right)
800         );
801 }
802
803 //static void
804 //cpBBTreeOptimizeIncremental(cpBBTree *tree, int passes)
805 //{
806 //      for(int i=0; i<passes; i++){
807 //              Node *root = tree->root;
808 //              Node *node = root;
809 //              int bit = 0;
810 //              unsigned int path = tree->opath;
811 //              
812 //              while(!NodeIsLeaf(node)){
813 //                      node = (path&(1<<bit) ? node->a : node->b);
814 //                      bit = (bit + 1)&(sizeof(unsigned int)*8 - 1);
815 //              }
816 //              
817 //              root = subtreeRemove(root, node, tree);
818 //              tree->root = subtreeInsert(root, node, tree);
819 //      }
820 //}
821
822 void
823 cpBBTreeOptimize(cpSpatialIndex *index)
824 {
825         if(index->klass != &klass){
826                 cpAssertWarn(cpFalse, "Ignoring cpBBTreeOptimize() call to non-tree spatial index.");
827                 return;
828         }
829         
830         cpBBTree *tree = (cpBBTree *)index;
831         Node *root = tree->root;
832         if(!root) return;
833         
834         int count = cpBBTreeCount(tree);
835         Node **nodes = (Node **)cpcalloc(count, sizeof(Node *));
836         Node **cursor = nodes;
837         
838         cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)fillNodeArray, &cursor);
839         
840         SubtreeRecycle(tree, root);
841         tree->root = partitionNodes(tree, nodes, count);
842         cpfree(nodes);
843 }
844
845 //MARK: Debug Draw
846
847 //#define CP_BBTREE_DEBUG_DRAW
848 #ifdef CP_BBTREE_DEBUG_DRAW
849 #include "OpenGL/gl.h"
850 #include "OpenGL/glu.h"
851 #include <GLUT/glut.h>
852
853 static void
854 NodeRender(Node *node, int depth)
855 {
856         if(!NodeIsLeaf(node) && depth <= 10){
857                 NodeRender(node->a, depth + 1);
858                 NodeRender(node->b, depth + 1);
859         }
860         
861         cpBB bb = node->bb;
862         
863 //      GLfloat v = depth/2.0f; 
864 //      glColor3f(1.0f - v, v, 0.0f);
865         glLineWidth(cpfmax(5.0f - depth, 1.0f));
866         glBegin(GL_LINES); {
867                 glVertex2f(bb.l, bb.b);
868                 glVertex2f(bb.l, bb.t);
869                 
870                 glVertex2f(bb.l, bb.t);
871                 glVertex2f(bb.r, bb.t);
872                 
873                 glVertex2f(bb.r, bb.t);
874                 glVertex2f(bb.r, bb.b);
875                 
876                 glVertex2f(bb.r, bb.b);
877                 glVertex2f(bb.l, bb.b);
878         }; glEnd();
879 }
880
881 void
882 cpBBTreeRenderDebug(cpSpatialIndex *index){
883         if(index->klass != &klass){
884                 cpAssertWarn(cpFalse, "Ignoring cpBBTreeRenderDebug() call to non-tree spatial index.");
885                 return;
886         }
887         
888         cpBBTree *tree = (cpBBTree *)index;
889         if(tree->root) NodeRender(tree->root, 0);
890 }
891 #endif