/**
* Sorts the maximum end point for specified axis and adds overlapping pair if necessary.
*/
void SweepAndPrune::sortMaxUp(unsigned int axis, unsigned int endPointIndex, bool testOverlap)
{
BodyEndPoint *endPoint = &sortedEndPoints[axis][endPointIndex];
BodyEndPoint *nextEndPoint = &sortedEndPoints[axis][endPointIndex+1];
while(endPoint->getValue() > nextEndPoint->getValue())
{
//update indexes in bodies
if(nextEndPoint->isMin())
{
const int axis1 = (1 << axis) & 3;
const int axis2 = (1 << axis1) & 3;
if(testOverlap && isOverlap(endPoint->getBodyBox(), nextEndPoint->getBodyBox(), axis1, axis2))
{
createOverlappingPair(endPoint->getBodyBox(), nextEndPoint->getBodyBox());
}
nextEndPoint->getBodyBox()->min[axis]--;
}else
{
nextEndPoint->getBodyBox()->max[axis]--;
}
endPoint->getBodyBox()->max[axis]++;
//swap end points
BodyEndPoint tmp = *endPoint;
*endPoint = *nextEndPoint;
*nextEndPoint = tmp;
//increment
endPoint++;
nextEndPoint++;
}
}
/**
* Sorts the minimum end point for specified axis and adds overlapping pair if necessary.
*/
void SweepAndPrune::sortMinDown(unsigned int axis, unsigned int endPointIndex, bool testOverlap)
{
BodyEndPoint *endPoint = &sortedEndPoints[axis][endPointIndex];
BodyEndPoint *prevEndPoint = &sortedEndPoints[axis][endPointIndex-1];
while(endPoint->getValue() < prevEndPoint->getValue())
{
//update indexes in bodies
if(prevEndPoint->isMin())
{
prevEndPoint->getBodyBox()->min[axis]++;
}else
{
const int axis1 = (1 << axis) & 3;
const int axis2 = (1 << axis1) & 3;
if(testOverlap && isOverlap(endPoint->getBodyBox(), prevEndPoint->getBodyBox(), axis1, axis2))
{
createOverlappingPair(endPoint->getBodyBox(), prevEndPoint->getBodyBox());
}
prevEndPoint->getBodyBox()->max[axis]++;
}
endPoint->getBodyBox()->min[axis]--;
//swap end points
BodyEndPoint tmp = *endPoint;
*endPoint = *prevEndPoint;
*prevEndPoint = tmp;
//decrement
endPoint--;
prevEndPoint--;
}
}
void AABBTree::computeOverlappingPairsFor(AABBNode *leafNode, AABBNode *rootNode)
{
std::stack<AABBNode *> stackNodes;
stackNodes.push(rootNode);
while(!stackNodes.empty())
{ //tree traversal: pre-order (iterative)
AABBNode *currentNode = stackNodes.top();
stackNodes.pop();
if(leafNode!=currentNode && leafNode->getAABBox().collideWithAABBox(currentNode->getAABBox()))
{
if (currentNode->isLeaf())
{
createOverlappingPair(leafNode->getBodyNodeData(), currentNode->getBodyNodeData());
}else
{
stackNodes.push(currentNode->getRightChild());
stackNodes.push(currentNode->getLeftChild());
}
}
}
}
