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]))); } } } }