bool intersectLineSegmentAABB(const float bmin[3],const float bmax[3],const float p1[3],const float dir[3],float &dist,float intersect[3])
{
bool ret = false;
if ( dist > RAYAABB_EPSILON )
{
ret = intersectRayAABB(bmin,bmax,p1,dir,intersect);
if ( ret )
{
float dx = p1[0]-intersect[0];
float dy = p1[1]-intersect[1];
float dz = p1[2]-intersect[2];
float d = dx*dx+dy*dy+dz*dz;
if ( d < dist*dist )
{
dist = sqrtf(d);
}
else
{
ret = false;
}
}
}
return ret;
}
bool HGrid::queryRay(const vec3& origin, const vec3& dir, RayQueryResult* hit, float tmax)
{
m_tick++;
struct Cell
{
int i, j, k;
vec3 t;
vec3 d;
};
Cell cells[MAX_LEVELS];
int iEnd;
int jEnd;
int kEnd;
int di = ((dir.x > 0) ? 1 : (dir.x < 0) ? -1 : 0);
int dj = ((dir.y > 0) ? 1 : (dir.y < 0) ? -1 : 0);
int dk = ((dir.z > 0) ? 1 : (dir.z < 0) ? -1 : 0);
float maxSize = MIN_CELL_SIZE;
int occupiedLevelsMask = m_occupiedLevelsMask;
int maxLevel = 0;
for (maxLevel = 0; maxLevel < MAX_LEVELS; ++maxLevel, occupiedLevelsMask >>= 1)
{
if (occupiedLevelsMask == 0)
break;
maxSize *= CELL_TO_CELL_RATIO;
}
vec3 end = origin + maxSize * dir;
float size = MIN_CELL_SIZE;
for (int level = 0; level < maxLevel; ++level)
{
cells[level].i = (int)floorf(origin.x / size);
cells[level].j = (int)floorf(origin.y / size);
cells[level].k = (int)floorf(origin.z / size);
float minx = size * floorf(origin.x / size);
float miny = size * floorf(origin.y / size);
float minz = size * floorf(origin.z / size);
float maxx = minx + size;
float maxy = miny + size;
float maxz = minz + size;
cells[level].t.x = ((dir.x < 0) ? (origin.x - minx) : (maxx - origin.x)) / abs(dir.x);
cells[level].t.y = ((dir.y < 0) ? (origin.y - miny) : (maxy - origin.y)) / abs(dir.y);
cells[level].t.z = ((dir.z < 0) ? (origin.z - miny) : (maxy - origin.z)) / abs(dir.z);
cells[level].d.x = size / abs(dir.x);
cells[level].d.y = size / abs(dir.y);
cells[level].d.z = size / abs(dir.z);
size *= CELL_TO_CELL_RATIO;
}
cells[maxLevel].i = (int)floorf(origin.x / maxSize);
cells[maxLevel].j = (int)floorf(origin.y / maxSize);
cells[maxLevel].k = (int)floorf(origin.z / maxSize);
iEnd = (int)floorf((di >= 0 ? m_maxExtend.x : m_minExtend.x) / maxSize);
jEnd = (int)floorf((dj >= 0 ? m_maxExtend.y : m_minExtend.y) / maxSize);
kEnd = (int)floorf((dk >= 0 ? m_maxExtend.z : m_minExtend.z) / maxSize);
RayQueryResult minResult;
minResult.collider = 0;
minResult.t = FLT_MAX;
//check origin for neighbouring cells
size = MIN_CELL_SIZE;
for (int level = 0; level < maxLevel; ++level)
{
if (m_objectsAtLevel[level] == 0)
continue;
int x0 = cells[level].i, x1 = cells[level].i;
int y0 = cells[level].j, y1 = cells[level].j;
int z0 = cells[level].k, z1 = cells[level].k;
if ((int)floorf(origin.x / size - SPHERE_TO_CELL_RATIO * size) != cells[level].i)
--x0;
if ((int)floorf(origin.y / size + SPHERE_TO_CELL_RATIO * size) != cells[level].i)
++x1;
if ((int)floorf(origin.y / size - SPHERE_TO_CELL_RATIO * size) != cells[level].j)
--y0;
if ((int)floorf(origin.y / size + SPHERE_TO_CELL_RATIO * size) != cells[level].j)
++y1;
if ((int)floorf(origin.z / size - SPHERE_TO_CELL_RATIO * size) != cells[level].k)
--z0;
if ((int)floorf(origin.y / size + SPHERE_TO_CELL_RATIO * size) != cells[level].k)
++z1;
for (int x = x0; x <= x1; ++x)
{
for (int y = y0; y <= y1; ++y)
{
for (int z = z0; z <= z1; ++z)
{
int bucket = calculateBucketID(x, y, z, level);
if (m_timeStamp[bucket] == m_tick)
continue;
m_timeStamp[bucket] = m_tick;
for (Object& obj : m_objectBucket[bucket])
{
float tmin;
vec3 p;
if (intersectRayAABB(origin, dir, obj.id->pBody->getAABB(), tmin, p) && tmin < tmax)
{
RayQueryResult result = { 0 };
if (obj.id->pBody->queryRay(origin, dir, &result, tmax))
{
if (result.t < minResult.t)
minResult = result;
}
}
}
}
}
}
size *= CELL_TO_CELL_RATIO;
}
int bucket;
//todo FIX !!!
static int MAX_ITERS = 100;
for (int iter = 0 ; iter < MAX_ITERS;++ iter)
{
float ooSize = 1.0f / maxSize;
for (int l = maxLevel - 1; l >= 0; --l)
{
ooSize *= CELL_TO_CELL_RATIO;
if (m_objectsAtLevel[l] == 0)
continue;
for (;;)
{
int x0 = cells[l].i, x1 = cells[l].i;
int y0 = cells[l].k, y1 = cells[l].j;
int z0 = cells[l].j, z1 = cells[l].k;
bool quit = false;
if (cells[l].t.x <= cells[l].t.y && cells[l].t.x <= cells[l].t.z)
{
vec3 p = origin + cells[l].t.x * dir;
if ((int)floorf(p.y - SPHERE_TO_CELL_RATIO * 1.0f / ooSize) != cells[l].j)
--y0;
else if ((int)floorf(p.y + SPHERE_TO_CELL_RATIO * 1.0f / ooSize) != cells[l].j)
++y1;
if ((int)floorf(p.z - SPHERE_TO_CELL_RATIO * 1.0f / ooSize) != cells[l].k)
--z0;
else if ((int)floorf(p.z + SPHERE_TO_CELL_RATIO * 1.0f / ooSize) != cells[l].k)
++z1;
cells[l].t.x += cells[l].d.x;
cells[l].i += di;
if ((int)floorf(cells[l].i/(maxSize*ooSize)) != cells[maxLevel].i)
quit = true;
}
else if (cells[l].t.y <= cells[l].t.z)
{
vec3 p = origin + cells[l].t.y * dir;
if ((int)floorf(p.x - SPHERE_TO_CELL_RATIO * 1.0f / ooSize) != cells[l].i)
--x0;
else if ((int)floorf(p.x + SPHERE_TO_CELL_RATIO * 1.0f / ooSize) != cells[l].i)
++x1;
if ((int)floorf(p.z - SPHERE_TO_CELL_RATIO * 1.0f / ooSize) != cells[l].k)
--z0;
else if ((int)floorf(p.z + SPHERE_TO_CELL_RATIO * 1.0f / ooSize) != cells[l].k)
++z1;
cells[l].t.y += cells[l].d.y;
cells[l].j += dj;
if ((int)floorf(cells[l].j / (maxSize*ooSize)) != cells[maxLevel].j)
quit = true;
}
else
{
vec3 p = origin + cells[l].t.z * dir;
if ((int)floorf(p.x - SPHERE_TO_CELL_RATIO * 1.0f / ooSize) != cells[l].i)
--x0;
else if ((int)floorf(p.x + SPHERE_TO_CELL_RATIO * 1.0f / ooSize) != cells[l].i)
++x1;
if ((int)floorf(p.y - SPHERE_TO_CELL_RATIO * 1.0f / ooSize) != cells[l].j)
--y0;
else if ((int)floorf(p.y + SPHERE_TO_CELL_RATIO * 1.0f / ooSize) != cells[l].j)
++y1;
cells[l].t.z += cells[l].d.z;
cells[l].k += dk;
if ((int)floorf(cells[l].k / (maxSize*ooSize)) != cells[maxLevel].k)
quit = true;
}
for (int x = x0; x <= x1; ++x)
{
for (int y = y0; y <= y1; ++y)
{
for (int z = z0; z <= z1; ++z)
{
bucket = calculateBucketID(x, y, z, l);
if (m_timeStamp[bucket] == m_tick)
continue;
m_timeStamp[bucket] = m_tick;
for (Object& obj : m_objectBucket[bucket])
{
float tmin;
vec3 p;
if (intersectRayAABB(origin, dir, obj.id->pBody->getAABB(), tmin, p) && tmin < tmax)
{
RayQueryResult result = { 0 };
if (obj.id->pBody->queryRay(origin, dir, &result, tmax))
{
if (result.t < minResult.t)
minResult = result;
}
}
}
}
}
}
if (quit)
break;
}
}
if (minResult.collider != 0)
{
*hit = minResult;
return true;
}
if (cells[maxLevel].t.x <= cells[maxLevel].t.y && cells[maxLevel].t.x <= cells[maxLevel].t.z)
{
if (cells[maxLevel].i == iEnd)
return false;
cells[maxLevel].t.x += cells[maxLevel].d.x;
cells[maxLevel].i += di;
}
else if (cells[maxLevel].t.y <= cells[maxLevel].t.z)
{
if (cells[maxLevel].j == jEnd)
return false;
cells[maxLevel].t.y += cells[maxLevel].d.y;
cells[maxLevel].j += dj;
}
else
{
if (cells[maxLevel].k == kEnd)
return false;
cells[maxLevel].t.z += cells[maxLevel].d.z;
cells[maxLevel].k += dk;
}
}
}
