void ftToroidalGrid::moveShape(ftBroadphaseHandle handle, const ftShape *shape, const ftTransform &transform)
{
int32 oldXStart = ftFloor(m_elements[handle].aabb.min.x / m_config.cellSize);
int32 oldYStart = ftFloor(m_elements[handle].aabb.min.y / m_config.cellSize);
int32 oldXEnd = ftCeil(m_elements[handle].aabb.max.x / m_config.cellSize) - 1;
int32 oldYEnd = ftCeil(m_elements[handle].aabb.max.y / m_config.cellSize) - 1;
m_elements[handle].aabb = shape->constructAABB(transform);
real minX = m_elements[handle].aabb.min.x;
real minY = m_elements[handle].aabb.min.y;
real maxX = m_elements[handle].aabb.max.x;
real maxY = m_elements[handle].aabb.max.y;
int32 newXStart = ftFloor(minX / m_config.cellSize);
int32 newYStart = ftFloor(minY / m_config.cellSize);
int32 newXEnd = ftCeil(maxX / m_config.cellSize) - 1;
int32 newYEnd = ftCeil(maxY / m_config.cellSize) - 1;
if (oldXStart != newXStart || oldYStart != newYStart ||
oldXEnd != newXEnd || oldYEnd != newYEnd)
{
removeFromBucket(handle);
m_elements[handle].xStart = newXStart;
m_elements[handle].yStart = newYStart;
m_elements[handle].xEnd = newXEnd;
m_elements[handle].yEnd = newYEnd;
insertToBucket(handle);
}
}
void HGrid::updateBody(const ProxyID* pProxyID)
{
AABB aabb = pProxyID->pBody->getAABB();
Object object = m_objectBucket[pProxyID->bucket][pProxyID->idx];
object.sphere.c = aabb.c;
object.sphere.r = 0;
object.sphere.r = length(aabb.e);
for (int i = 0; i < 3; ++i)
{
if ((object.sphere.c[i] + object.sphere.r) > m_maxExtend[i])
m_maxExtend[i] = object.sphere.c[i] + object.sphere.r;
else if ((object.sphere.c[i] - object.sphere.r) < m_minExtend[i])
m_minExtend[i] = object.sphere.c[i] + object.sphere.r;
}
int level = 0;
float size = MIN_CELL_SIZE, diameter = 2.0f * object.sphere.r;
for (level = 0; size* SPHERE_TO_CELL_RATIO < diameter; ++level)
size *= CELL_TO_CELL_RATIO;
assert(level < MAX_LEVELS);
ProxyID* id = object.id;
assert(id == pProxyID);
if (level != id->level)
{
removeFromLevel(pProxyID);
id->level = level;
m_objectsAtLevel[id->level]++;
m_occupiedLevelsMask |= (1 << id->level);
}
int x = (int)(object.sphere.c.x / size);
int y = (int)(object.sphere.c.y / size);
int z = (int)(object.sphere.c.z / size);
object.x = x;
object.y = y;
object.z = z;
int bucket = calculateBucketID(x,y,z, level);
if (bucket != pProxyID->bucket)
{
removeFromBucket(pProxyID);
id->bucket = bucket;
id->idx = m_objectBucket[id->bucket].size();
m_objectBucket[id->bucket].push_back(object);
}
}
void ftToroidalGrid::removeShape(ftBroadphaseHandle handle)
{
removeFromBucket(handle);
m_elements[handle].next = m_free;
m_free = handle;
}
void HGrid::removeBody(const ProxyID* pProxyID)
{
ProxyID* id = m_objectBucket[pProxyID->bucket][pProxyID->idx].id;
removeFromLevel(pProxyID);
removeFromBucket(pProxyID);
m_proxyIDAllocator.sDelete(id);
}
