void Block::Collide(dxGeom* g1, dxGeom* g2, void* UserData, dNearCallback* Callback){
#ifdef DRAWBLOCKS
DrawBlock(this);
#endif
// Collide against local list
while (g2){
if (GEOM_ENABLED(g2)){
collideAABBs (g1, g2, UserData, Callback);
}
g2 = g2->next;
}
// Collide against children
if (Children){
for (int i = 0; i < SPLITS; i++){
// Early out for empty blocks
if (Children[i].GeomCount == 0){
continue;
}
// Does the geom's AABB collide with the block?
// Dont do AABB tests for single geom blocks.
if (Children[i].GeomCount == 1 && Children[i].First){
//
}
else if (true){
if (g1->aabb[AXIS0 * 2 + 0] > Children[i].MaxX ||
g1->aabb[AXIS0 * 2 + 1] < Children[i].MinX ||
g1->aabb[AXIS1 * 2 + 0] > Children[i].MaxZ ||
g1->aabb[AXIS1 * 2 + 1] < Children[i].MinZ) continue;
}
Children[i].Collide(g1, Children[i].First, UserData, Callback);
}
}
}
void dxSAPSpace::collide (void *data, dNearCallback *callback)
{
dAASSERT (callback);
lock_count++;
cleanGeoms();
// by now all geoms are in GeomList, and DirtyList must be empty
int geomSize = GeomList.size();
dUASSERT( geomSize == count, "geom counts messed up" );
// separate all geoms into infinite AABBs and normal AABBs
TmpGeomList.setSize(0);
TmpInfGeomList.setSize(0);
int axis0max = SortAxes.mAxis0*2+1;
for( int i = 0; i < geomSize; ++i ) {
dxGeom* g =GeomList[i];
if( !GEOM_ENABLED(g) ) // skip disabled ones
continue;
const dReal& amax = g->aabb[axis0max];
if( amax == dInfinity ) // HACK? probably not...
TmpInfGeomList.push( g );
else
TmpGeomList.push( g );
}
// do SAP on normal AABBs
Pairs overlapBoxes;
bool isok = complete_box_pruning( TmpGeomList.size(), (const dxGeom**)TmpGeomList.data(), overlapBoxes, SortAxes );
// collide overlapping
udword overlapCount = overlapBoxes.GetNbPairs();
for( udword j = 0; j < overlapCount; ++j ) {
const Pair* pair = overlapBoxes.GetPair(j);
dxGeom* g1 = TmpGeomList[pair->id0];
dxGeom* g2 = TmpGeomList[pair->id1];
collideGeomsNoAABBs( g1, g2, data, callback );
}
int infSize = TmpInfGeomList.size();
int normSize = TmpGeomList.size();
int m, n;
for( m = 0; m < infSize; ++m ) {
dxGeom* g1 = TmpInfGeomList[m];
// collide infinite ones
for( n = m+1; n < infSize; ++n ) {
dxGeom* g2 = TmpInfGeomList[n];
collideGeomsNoAABBs( g1, g2, data, callback );
}
// collide infinite ones with normal ones
for( n = 0; n < normSize; ++n ) {
dxGeom* g2 = TmpGeomList[n];
collideGeomsNoAABBs( g1, g2, data, callback );
}
}
lock_count--;
}
void Block::CollideLocal(dxGeom* g1, void* UserData, dNearCallback* Callback){
// Collide against local list
dxGeom* g2 = First;
while (g2){
if (GEOM_ENABLED(g2)){
collideAABBs (g1, g2, UserData, Callback);
}
g2 = g2->next;
}
}
void Block::Collide(void* UserData, dNearCallback* Callback){
#ifdef DRAWBLOCKS
DrawBlock(this);
#endif
// Collide the local list
dxGeom* g = First;
while (g){
if (GEOM_ENABLED(g)){
Collide(g, g->next, UserData, Callback);
}
g = g->next;
}
// Recurse for children
if (Children){
for (int i = 0; i < SPLITS; i++){
if (Children[i].GeomCount <= 1){ // Early out
continue;
}
Children[i].Collide(UserData, Callback);
}
}
}