dali-physics/third-party/chipmunk2d/src/cpBBTree.c

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