static Node *
SubtreeInsert(Node *subtree, Node *leaf, cpBBTree *tree)
{
if(subtree == NULL){
return leaf;
} else if(NodeIsLeaf(subtree)){
return NodeNew(tree, leaf, subtree);
} else {
cpFloat cost_a = cpBBArea(subtree->B->bb) + cpBBMergedArea(subtree->A->bb, leaf->bb);
cpFloat cost_b = cpBBArea(subtree->A->bb) + cpBBMergedArea(subtree->B->bb, leaf->bb);
if(cost_a == cost_b){
cost_a = cpBBProximity(subtree->A->bb, leaf->bb);
cost_b = cpBBProximity(subtree->B->bb, leaf->bb);
}
if(cost_b < cost_a){
NodeSetB(subtree, SubtreeInsert(subtree->B, leaf, tree));
} else {
NodeSetA(subtree, SubtreeInsert(subtree->A, leaf, tree));
}
subtree->bb = cpBBMerge(subtree->bb, leaf->bb);
return subtree;
}
}
static void
NodeRender(Node *node, int depth)
{
if(!NodeIsLeaf(node) && depth <= 10){
NodeRender(node->a, depth + 1);
NodeRender(node->b, depth + 1);
}
cpBB bb = node->bb;
// GLfloat v = depth/2.0f;
// glColor3f(1.0f - v, v, 0.0f);
glLineWidth(cpfmax(5.0f - depth, 1.0f));
glBegin(GL_LINES); {
glVertex2f(bb.l, bb.b);
glVertex2f(bb.l, bb.t);
glVertex2f(bb.l, bb.t);
glVertex2f(bb.r, bb.t);
glVertex2f(bb.r, bb.t);
glVertex2f(bb.r, bb.b);
glVertex2f(bb.r, bb.b);
glVertex2f(bb.l, bb.b);
}; glEnd();
}
static void
SubtreeRecycle(cpBBTree *tree, Node *node)
{
if(!NodeIsLeaf(node)){
SubtreeRecycle(tree, node->A);
SubtreeRecycle(tree, node->B);
NodeRecycle(tree, node);
}
}
static void
MarkSubtree(Node *subtree, MarkContext *context)
{
if(NodeIsLeaf(subtree)){
MarkLeaf(subtree, context);
} else {
MarkSubtree(subtree->A, context);
MarkSubtree(subtree->B, context); // TODO Force TCO here?
}
}
static void
MarkSubtree(Node *subtree, MarkContext *context)
{
if(NodeIsLeaf(subtree)){
MarkLeaf(subtree, context);
} else {
MarkSubtree(subtree->A, context);
MarkSubtree(subtree->B, context);
}
}
static void
SubtreeQuery(Node *subtree, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data)
{
if(cpBBIntersects(subtree->bb, bb)){
if(NodeIsLeaf(subtree)){
func(obj, subtree->obj, 0, data);
} else {
SubtreeQuery(subtree->A, obj, bb, func, data);
SubtreeQuery(subtree->B, obj, bb, func, data);
}
}
}
static void
MarkLeafQuery(Node *subtree, Node *leaf, cpBool left, MarkContext *context)
{
if(cpBBIntersects(leaf->bb, subtree->bb)){
if(NodeIsLeaf(subtree)){
if(left){
PairInsert(leaf, subtree, context->tree);
} else {
if(subtree->STAMP < leaf->STAMP) PairInsert(subtree, leaf, context->tree);
context->func(leaf->obj, subtree->obj, 0, context->data);
}
} else {
MarkLeafQuery(subtree->A, leaf, left, context);
MarkLeafQuery(subtree->B, leaf, left, context);
}
}
}
static inline void
NodeReplaceChild(Node *parent, Node *child, Node *value, cpBBTree *tree)
{
cpAssertSoft(!NodeIsLeaf(parent), "Internal Error: Cannot replace child of a leaf.");
cpAssertSoft(child == parent->A || child == parent->B, "Internal Error: Node is not a child of parent.");
if(parent->A == child){
NodeRecycle(tree, parent->A);
NodeSetA(parent, value);
} else {
NodeRecycle(tree, parent->B);
NodeSetB(parent, value);
}
for(Node *node=parent; node; node = node->parent){
node->bb = cpBBMerge(node->A->bb, node->B->bb);
}
}
static cpFloat
SubtreeSegmentQuery(Node *subtree, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data)
{
if(NodeIsLeaf(subtree)){
return func(obj, subtree->obj, data);
} else {
cpFloat t_a = cpBBSegmentQuery(subtree->A->bb, a, b);
cpFloat t_b = cpBBSegmentQuery(subtree->B->bb, a, b);
if(t_a < t_b){
if(t_a < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->A, obj, a, b, t_exit, func, data));
if(t_b < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->B, obj, a, b, t_exit, func, data));
} else {
if(t_b < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->B, obj, a, b, t_exit, func, data));
if(t_a < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->A, obj, a, b, t_exit, func, data));
}
return t_exit;
}
}