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