void dgCollisionScene::CollidePair(dgCollidingPairCollector::dgPair* const pair,
dgCollisionParamProxy& proxy) const
{
const dgNode *stackPool[DG_SCENE_MAX_STACK_DEPTH];
_ASSERTE(pair->m_body1->GetCollision() == this);
_ASSERTE(
pair->m_body1->GetCollision()->IsType(dgCollision::dgCollisionScene_RTTI));
dgVector p0;
dgVector p1;
_ASSERTE(m_world == pair->m_body1->GetWorld());
dgMatrix matrix(pair->m_body0->m_matrix * pair->m_body1->m_matrix.Inverse());
pair->m_body0->GetCollision()->CalcAABB(matrix, p0, p1);
dgInt32 stack = 1;
stackPool[0] = m_rootNode;
while (stack)
{
stack--;
const dgNode* const me = stackPool[stack];
if (dgOverlapTest(me->m_minBox, me->m_maxBox, p0, p1))
{
if (!me->m_left)
{
_ASSERTE(!me->m_right);
const dgProxy* const sceneProxy = (dgProxy*) me;
m_world->SceneContacts(*sceneProxy, pair, proxy);
}
else
{
_ASSERTE(me->m_left);
_ASSERTE(stack < dgInt32 (sizeof (stackPool) / sizeof (dgNode*)));
stackPool[stack] = me->m_left;
stack++;
_ASSERTE(me->m_right);
_ASSERTE(stack < dgInt32 (sizeof (stackPool) / sizeof (dgNode*)));
stackPool[stack] = me->m_right;
stack++;
}
}
}
}
void dgBroadPhaseAggregate::SummitPairs(dgBody* const body, dgFloat32 timestep, dgInt32 threadID) const
{
if (m_root) {
if (m_root->IsLeafNode()) {
dgAssert (m_root->GetBody());
m_broadPhase->AddPair(body, m_root->GetBody(), timestep, threadID);
} else if (!(m_isInEquilibrium & body->m_equilibrium)) {
dgBroadPhaseNode* pool[DG_BROADPHASE_MAX_STACK_DEPTH/2];
pool[0] = m_root;
dgInt32 stack = 1;
const dgVector& boxP0 = body->m_minAABB;
const dgVector& boxP1 = body->m_maxAABB;
while (stack) {
stack--;
dgBroadPhaseNode* const rootNode = pool[stack];
if (dgOverlapTest(rootNode->m_minBox, rootNode->m_maxBox, boxP0, boxP1)) {
if (rootNode->IsLeafNode()) {
dgBody* const body1 = rootNode->GetBody();
dgAssert (body1);
m_broadPhase->AddPair(body, body1, timestep, threadID);
} else {
dgBroadPhaseTreeNode* const tmpNode = (dgBroadPhaseTreeNode*)rootNode;
dgAssert(tmpNode->m_left);
dgAssert(tmpNode->m_right);
pool[stack] = tmpNode->m_left;
stack++;
dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0])));
pool[stack] = tmpNode->m_right;
stack++;
dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0])));
}
}
}
}
}
}
void dgBroadPhaseAggregate::SummitSeltPairs(dgBroadPhaseNode* const node0, dgBroadPhaseNode* const node1, dgFloat32 timestep, dgInt32 threadID) const
{
dgInt32 stack = 1;
dgBroadPhaseNode* pool[DG_BROADPHASE_MAX_STACK_DEPTH][2];
pool[0][0] = node0;
pool[0][1] = node1;
while (stack) {
stack--;
dgBroadPhaseNode* const root0 = pool[stack][0];
dgBroadPhaseNode* const root1 = pool[stack][1];
if (dgOverlapTest(root0->m_minBox, root0->m_maxBox, root1->m_minBox, root1->m_maxBox)) {
if (root0->IsLeafNode()) {
if (root1->IsLeafNode()) {
dgBody* const body0 = root0->GetBody();
dgBody* const body1 = root1->GetBody();
dgAssert(body0);
dgAssert(body1);
m_broadPhase->AddPair(body0, body1, timestep, threadID);
} else {
dgBroadPhaseTreeNode* const tmpNode1 = (dgBroadPhaseTreeNode*)root1;
dgAssert(tmpNode1->m_left);
dgAssert(tmpNode1->m_right);
pool[stack][0] = root0;
pool[stack][1] = tmpNode1->m_left;
stack++;
dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0])));
pool[stack][0] = root0;
pool[stack][1] = tmpNode1->m_right;
stack++;
dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0])));
}
} else if (root1->IsLeafNode()) {
dgBroadPhaseTreeNode* const tmpNode0 = (dgBroadPhaseTreeNode*)root0;
dgAssert(tmpNode0->m_left);
dgAssert(tmpNode0->m_right);
pool[stack][0] = root1;
pool[stack][1] = tmpNode0->m_left;
stack++;
dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0])));
pool[stack][0] = root1;
pool[stack][1] = tmpNode0->m_right;
stack++;
dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0])));
} else {
dgBroadPhaseTreeNode* const tmpNode0 = (dgBroadPhaseTreeNode*)root0;
dgBroadPhaseTreeNode* const tmpNode1 = (dgBroadPhaseTreeNode*)root1;
dgAssert(tmpNode0->m_left);
dgAssert(tmpNode0->m_right);
dgAssert(tmpNode1->m_left);
dgAssert(tmpNode1->m_right);
pool[stack][0] = tmpNode0->m_left;
pool[stack][1] = tmpNode1->m_left;
stack++;
dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0])));
pool[stack][0] = tmpNode0->m_left;
pool[stack][1] = tmpNode1->m_right;
stack++;
dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0])));
pool[stack][0] = tmpNode0->m_right;
pool[stack][1] = tmpNode1->m_left;
stack++;
dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0])));
pool[stack][0] = tmpNode0->m_right;
pool[stack][1] = tmpNode1->m_right;
stack++;
dgAssert(stack < dgInt32(sizeof (pool) / sizeof (pool[0])));
}
}
}
}
