Spatial Partitioning - Real-Time Collision Detection - page 305

// For each new query, increase time stamp counter
grid- > tick++;

for (int level = startLevel; level < HGRID_MAX_LEVELS;
size *= CELL_TO_CELL_RATIO, occupiedLevelsMask >>= 1, level++) {
// If no objects in rest of grid, stop now
if (occupiedLevelsMask == 0) break;
// If no objects at this level, go on to the next level
if ((occupiedLevelsMask & 1) == 0) continue;

// Compute ranges [x1..x2, y1..y2] of cells overlapped on this level. To
// make sure objects in neighboring cells are tested, by increasing range by
// the maximum object overlap: size * SPHERE_TO_CELL_RATIO
float delta = obj->radius + size * SPHERE_TO_CELL_RATIO + EPSILON;
float ooSize = 1.0f / size;
int x1 = (int)floorf((pos.x - delta) * ooSize);
int y1 = (int)floorf((pos.y - delta) * ooSize);
int x2 = (int) ceilf((pos.x + delta) * ooSize);
int y2 = (int) ceilf((pos.y + delta) * ooSize);

// Check all the grid cells overlapped on current level
for (int x = x1; x <= x2; x++) {
for (int y = y1; y <= y2; y++) {
// Treat level as a third dimension coordinate
Cell cellPos(x, y, level);
int bucket = ComputeHashBucketIndex(cellPos);

// Has this hash bucket already been checked for this object?
if (grid- > timeStamp[bucket] == grid- > tick) continue;
grid- > timeStamp[bucket] = grid- > tick;

// Loop through all objects in the bucket to find nearby objects
Object *p = grid- > objectBucket[bucket];
while (p) {
if (p != obj) {
float dist2 = Sqr(pos.x - p->pos.x) + Sqr(pos.y - p->pos.y);
if (dist2 <= Sqr(obj->radius + p->radius + EPSILON))
pCallbackFunc(obj, p); // Close, call callback function

}
p=p- > pNextObject;

}

}

}
} // end for level

}