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;
}
}
Rectf RigidBody2D::GetAABB() const
{
if (m_shapes.empty())
return Rectf::Zero();
auto it = m_shapes.begin();
cpBB bb = cpShapeGetBB(*it++);
for (; it != m_shapes.end(); ++it)
bb = cpBBMerge(bb, cpShapeGetBB(*it));
return Rectf(Rect<cpFloat>(bb.l, bb.b, bb.r - bb.l, bb.t - bb.b));
}
static Node *
NodeNew(cpBBTree *tree, Node *a, Node *b)
{
Node *node = NodeFromPool(tree);
node->obj = NULL;
node->bb = cpBBMerge(a->bb, b->bb);
node->parent = NULL;
NodeSetA(node, a);
NodeSetB(node, b);
return node;
}
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 Node *
partitionNodes(cpBBTree *tree, Node **nodes, int count)
{
if(count == 1){
return nodes[0];
} else if(count == 2) {
return NodeNew(tree, nodes[0], nodes[1]);
}
// Find the AABB for these nodes
cpBB bb = nodes[0]->bb;
for(int i=1; i<count; i++) bb = cpBBMerge(bb, nodes[i]->bb);
// Split it on it's longest axis
cpBool splitWidth = (bb.r - bb.l > bb.t - bb.b);
// Sort the bounds and use the median as the splitting point
cpFloat *bounds = (cpFloat *)cpcalloc(count*2, sizeof(cpFloat));
if(splitWidth){
for(int i=0; i<count; i++){
bounds[2*i + 0] = nodes[i]->bb.l;
bounds[2*i + 1] = nodes[i]->bb.r;
}
} else {
for(int i=0; i<count; i++){
bounds[2*i + 0] = nodes[i]->bb.b;
bounds[2*i + 1] = nodes[i]->bb.t;
}
}
qsort(bounds, count*2, sizeof(cpFloat), (int (*)(const void *, const void *))cpfcompare);
cpFloat split = (bounds[count - 1] + bounds[count])*0.5f; // use the medain as the split
cpfree(bounds);
// Generate the child BBs
cpBB a = bb, b = bb;
if(splitWidth) a.r = b.l = split; else a.t = b.b = split;
// Partition the nodes
int right = count;
for(int left=0; left < right;){
Node *node = nodes[left];
if(cpBBMergedArea(node->bb, b) < cpBBMergedArea(node->bb, a)){
// if(cpBBProximity(node->bb, b) < cpBBProximity(node->bb, a)){
right--;
nodes[left] = nodes[right];
nodes[right] = node;
} else {
left++;
}
}
if(right == count){
Node *node = NULL;
for(int i=0; i<count; i++) node = SubtreeInsert(node, nodes[i], tree);
return node;
}
// Recurse and build the node!
return NodeNew(tree,
partitionNodes(tree, nodes, right),
partitionNodes(tree, nodes + right, count - right)
);
}
static VALUE
rb_cpBBmerge(VALUE self, VALUE other) {
return BBNEW(cpBBMerge(*BBGET(self), *BBGET(other)));
}
