public void AddObject(HGridObject obj)
{
// Find lowest level where object fully fits inside cell, taking RATIO into account
int level;
float size = MIN_CELL_SIZE;
float diameter = 2.0f * obj.radius;
for (level = 0; size *SPHERE_TO_CELL_RATIO < diameter; level++)
{
size *= CELL_TO_CELL_RATIO;
}
// If object is larger than largest grid cell throw exception
if (level > HGRID_MAX_LEVELS)
{
throw new Exception("Object to large");
}
// Add object to grid square, and remember cell and level numbers
Cell cellPos;
cellPos.x = (int)(obj.pos.x / size); cellPos.y = (int)(obj.pos.y / size); cellPos.z = level;
int bucket = ComputeHashBucketIndex(cellPos);
obj.bucket = bucket;
obj.level = level;
obj.pNextObject = objectBucket[bucket];
objectBucket[bucket] = obj;
// Mark this level as having one more object. Also indicate level is in use
objectsAtLevel[level]++;
occupiedLevelsMask |= (uint)(1 << level);
}
public void RemoveObject(HGridObject obj)
{
// One less object on this grid level. Mark level as unused if no objects left
if (--objectsAtLevel[obj.level] == 0)
{
occupiedLevelsMask &= (uint)~(1 << obj.level);
}
// Now scan through list and unlink object ’obj’
int bucket = obj.bucket;
HGridObject p = objectBucket[bucket];
// Special-case updating list header when object is first in list
if (p == obj)
{
objectBucket[bucket] = p.pNextObject;
return;
}
// Traverse rest of list, unlinking ’obj’ when found
while (p != null)
{
// Keep q as trailing pointer to previous element
HGridObject q = p;
p = p.pNextObject;
if (p == obj)
{
q.pNextObject = p.pNextObject; // unlink by bypassing
return;
}
}
throw new Exception("Object does not exist in HGrid"); // No such object in hgrid
}
List <HGridObject> AddSpheresToHGrid()
{
List <HGridObject> objects = new List <HGridObject> ();
foreach (var sphere in Spheres)
{
HGridObject obj = new HGridObject(sphere, sphere.x, (float)sphere.r);
hgrid.AddObject(obj);
objects.Add(obj);
}
return(objects);
}
// Test collisions between object and all objects in hgrid
public List <Sphere> CheckObjAgainstGrid(HGridObject obj)
{
List <Sphere> collisions = new List <Sphere>();
const float EPSILON = 0.001f;
float size = MIN_CELL_SIZE;
int startLevel = 0;
uint occupiedLevelsMask = this.occupiedLevelsMask;
Vec3 pos = obj.pos;
// For each new query, increase time stamp counter
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, z1..z2] 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; /* Not sure about this */
int x1 = (int)Math.Floor((pos.x - delta) * ooSize);
int y1 = (int)Math.Floor((pos.y - delta) * ooSize);
//int z1 = (int)Math.Floor((pos.z - delta) * ooSize);
int x2 = (int)Math.Ceiling((pos.x + delta) * ooSize);
int y2 = (int)Math.Ceiling((pos.y + delta) * ooSize);
//int z2 = (int)Math.Ceiling((pos.z + 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++)
{
Cell cellPos;
cellPos.x = x; cellPos.y = y; cellPos.z = level;
int bucket = ComputeHashBucketIndex(cellPos);
// Has this hash bucket already been checked for this object?
if (timeStamp[bucket] == tick)
{
continue;
}
timeStamp[bucket] = tick;
// Loop through all objects in the bucket to find nearby objects
HGridObject p = objectBucket[bucket];
while (p != null)
{
if (p != obj)
{
float dist2 = (float)(pos - p.pos).SqLength;
if (dist2 <= (float)Math.Pow(obj.radius + p.radius + EPSILON, 2))
{
collisions.Add(p.rb);
}
}
p = p.pNextObject;
}
}
}
} // end for level
return(collisions);
}