void AABBTree::updateBodies()
{
for(auto it = bodiesNode.begin(); it!=bodiesNode.end();)
{
bool removed = false;
AbstractWorkBody *body = it->first;
if(body->isActive())
{
AABBNode *leaf = it->second;
const AABBox<float> &leafFatAABBox = leaf->getAABBox();
const AABBox<float> &bodyAABBox = leaf->getBodyNodeData()->retrieveBodyAABBox();
if(!leafFatAABBox.include(bodyAABBox))
{
PairContainer *alternativePairContainer = leaf->getBodyNodeData()->getAlternativePairContainer();
removeNode(leaf);
it = bodiesNode.erase(it);
removed = true;
addBody(body, alternativePairContainer);
}
}
if(!removed)
{
++it;
}
}
}
void SweepAndPrune::updateBodies()
{
for(std::map<AbstractWorkBody *, BodyBox *>::iterator it = bodiesBox.begin(); it!=bodiesBox.end(); ++it)
{
AbstractWorkBody *body = it->first;
if(body->isActive())
{
BodyBox *bodyBox = it->second;
const AABBox<float> &aabbox = body->getShape()->toAABBox(body->getPhysicsTransform());
for(unsigned int axis=0; axis<3; ++axis)
{
unsigned int minEndPointIndex = bodyBox->min[axis];
unsigned int maxEndPointIndex = bodyBox->max[axis];
const float aabboxMin = aabbox.getMin()[axis] - contactBreakingThreshold;
const float aabboxMax = aabbox.getMax()[axis] + contactBreakingThreshold;
const float minDelta = aabboxMin - sortedEndPoints[axis][minEndPointIndex].getValue();
const float maxDelta = aabboxMax - sortedEndPoints[axis][maxEndPointIndex].getValue();
sortedEndPoints[axis][bodyBox->min[axis]].setValue(aabboxMin);
sortedEndPoints[axis][bodyBox->max[axis]].setValue(aabboxMax);
//1. add overlap if necessary
if(minDelta < 0.0f)
{ //new minimum point is smaller
sortMinDown(axis, minEndPointIndex, true);
}
if(maxDelta > 0.0f)
{ //new maximum point is bigger
sortMaxUp(axis, maxEndPointIndex, true);
}
//2. remove overlap if necessary
if(minDelta > 0.0f)
{ //new minimum point is bigger
sortMinUp(axis, minEndPointIndex, true);
}
if(maxDelta < 0.0f)
{ //new maximum point is smaller
sortMaxDown(axis, maxEndPointIndex, true);
}
}
}
}
#ifdef _DEBUG
//printSortedEndPoints();
#endif
}