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