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