inline void
__push_heap_aux(_RandomAccessIterator __first,
_RandomAccessIterator __last, _Distance*, _Tp*)
{
__push_heap(__first, _Distance((__last - __first) - 1), _Distance(0),
_Tp(*(__last - 1)));
}
inline void
__pop_heap(_RandomAccessIterator __first, _RandomAccessIterator __last,
_RandomAccessIterator __result, _Tp __value, _Distance*)
{
*__result = *__first;//把原始堆的根节点元素放在容器的末尾
//调整剩下的节点元素，使其成为新的heap
__adjust_heap(__first, _Distance(0), _Distance(__last - __first), __value);
}
inline void
__push_heap_aux(_RandomAccessIterator __first,
_RandomAccessIterator __last, _Distance*, _Tp*)
{
//__last - __first) - 1表示插入后元素的个数，也是容器的最后一个下标数字
//新插入的元素必须位于容器的末尾
__push_heap(__first, _Distance((__last - __first) - 1), _Distance(0),
_Tp(*(__last - 1)));
}
inline void
__pop_heap(_RandomAccessIterator __first, _RandomAccessIterator __last,
_RandomAccessIterator __result, _Tp __value, _Compare __comp,
_Distance*)
{
*__result = *__first;
__adjust_heap(__first, _Distance(0), _Distance(__last - __first),
__value, __comp);
}
void SSVTreeCollider::_Distance(const AABBCollisionNode* b0, const AABBCollisionNode* b1,
float& minD, Point& point0, Point& point1)
{
if (debug) std::cout << "_Distance()" << std::endl;
mNowNode0 = b0;
mNowNode1 = b1;
float d;
// Perform BV-BV distance test
d = SsvSsvDist(b0, b1);
if(d > minD) return;
if(b0->IsLeaf() && b1->IsLeaf()) {
Point p0, p1;
d = PrimDist(b0->GetPrimitive(), b1->GetPrimitive(), p0, p1);
if (d < minD){
minD = d;
point0 = p0;
point1 = p1;
mId0 = b0->GetPrimitive();
mId1 = b1->GetPrimitive();
}
return;
}
if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize())))
{
_Distance(b0->GetNeg(), b1, minD, point0, point1);
_Distance(b0->GetPos(), b1, minD, point0, point1);
}
else
{
_Distance(b0, b1->GetNeg(), minD, point0, point1);
_Distance(b0, b1->GetPos(), minD, point0, point1);
}
}
void SSVTreeCollider::Distance(const AABBCollisionTree* tree0,
const AABBCollisionTree* tree1,
const Matrix4x4* world0, const Matrix4x4* world1,
Pair* cache, float& minD, Point &point0, Point&point1)
{
if (debug) std::cout << "Distance()" << std::endl;
// Init collision query
InitQuery(world0, world1);
// Compute initial value using temporal coherency
// todo : cache should be used
const AABBCollisionNode *n;
for (unsigned int i=0; i<tree0->GetNbNodes(); i++){
n = tree0->GetNodes()+i;
if (n->IsLeaf()){
mId0 = n->GetPrimitive();
break;
}
}
for (unsigned int i=0; i<tree1->GetNbNodes(); i++){
n = tree1->GetNodes()+i;
if (n->IsLeaf()){
mId1 = n->GetPrimitive();
break;
}
}
Point p0, p1;
minD = PrimDist(mId0, mId1, p0, p1);
// Perform distance computation
_Distance(tree0->GetNodes(), tree1->GetNodes(), minD, p0, p1);
// transform points
TransformPoint4x3(point0, p0, *world1);
TransformPoint4x3(point1, p1, *world1);
// update cache
cache->id0 = mId0;
cache->id1 = mId1;
}
