bool TerrainBlock::castRayBlock(const Point3F &pStart, const Point3F &pEnd, const Point2I &aBlockPos, U32 aLevel, F32 invDeltaX, F32 invDeltaY, F32 aStartT, F32 aEndT, RayInfo *info, bool collideEmpty)
{
F32 invBlockSize = 1 / F32(BlockSquareWidth);
static TerrLOSStackNode stack[BlockShift * 3 + 1];
U32 stackSize = 1;
stack[0].startT = aStartT;
stack[0].endT = aEndT;
stack[0].blockPos = aBlockPos;
stack[0].level = aLevel;
if(!mTile && !aBlockPos.isZero())
return false;
while(stackSize--)
{
TerrLOSStackNode *sn = stack + stackSize;
U32 level = sn->level;
F32 startT = sn->startT;
F32 endT = sn->endT;
Point2I blockPos = sn->blockPos;
GridSquare *sq = findSquare(level, Point2I(blockPos.x & BlockMask, blockPos.y & BlockMask));
F32 startZ = startT * (pEnd.z - pStart.z) + pStart.z;
F32 endZ = endT * (pEnd.z - pStart.z) + pStart.z;
F32 minHeight = fixedToFloat(sq->minHeight);
if(startZ <= minHeight && endZ <= minHeight)
{
//drawLineTest(startT, sn->endT, false);
continue;
}
F32 maxHeight = fixedToFloat(sq->maxHeight);
if(startZ >= maxHeight && endZ >= maxHeight)
{
//drawLineTest(startT, endT, false);
continue;
}
if (!collideEmpty && (sq->flags & GridSquare::Empty) &&
blockPos.x == (blockPos.x & BlockMask) && blockPos.y == (blockPos.y & BlockMask))
{
//drawLineTest(startT, endT, false);
continue;
}
if(level == 0)
{
F32 xs = blockPos.x * invBlockSize;
F32 ys = blockPos.y * invBlockSize;
F32 zBottomLeft = fixedToFloat(getHeight(blockPos.x, blockPos.y));
F32 zBottomRight= fixedToFloat(getHeight(blockPos.x + 1, blockPos.y));
F32 zTopLeft = fixedToFloat(getHeight(blockPos.x, blockPos.y + 1));
F32 zTopRight = fixedToFloat(getHeight(blockPos.x + 1, blockPos.y + 1));
PlaneF p1, p2;
PlaneF divider;
Point3F planePoint;
if(sq->flags & GridSquare::Split45)
{
p1.set(zBottomLeft - zBottomRight, zBottomRight - zTopRight, invBlockSize);
p2.set(zTopLeft - zTopRight, zBottomLeft - zTopLeft, invBlockSize);
planePoint.set(xs, ys, zBottomLeft);
divider.x = 1;
divider.y = -1;
divider.z = 0;
}
else
{
p1.set(zTopLeft - zTopRight, zBottomRight - zTopRight, invBlockSize);
p2.set(zBottomLeft - zBottomRight, zBottomLeft - zTopLeft, invBlockSize);
planePoint.set(xs + invBlockSize, ys, zBottomRight);
divider.x = 1;
divider.y = 1;
divider.z = 0;
}
p1.setPoint(planePoint);
p2.setPoint(planePoint);
divider.setPoint(planePoint);
F32 t1 = p1.intersect(pStart, pEnd);
F32 t2 = p2.intersect(pStart, pEnd);
F32 td = divider.intersect(pStart, pEnd);
F32 dStart = divider.distToPlane(pStart);
F32 dEnd = divider.distToPlane(pEnd);
// see if the line crosses the divider
if((dStart >= 0 && dEnd < 0) || (dStart < 0 && dEnd >= 0))
{
if(dStart < 0)
{
F32 temp = t1;
t1 = t2;
t2 = temp;
}
if(t1 >= startT && t1 && t1 <= td && t1 <= endT)
{
info->t = t1;
info->normal = p1;
return true;
}
if(t2 >= td && t2 >= startT && t2 <= endT)
{
info->t = t2;
info->normal = p2;
return true;
}
}
else
{
F32 t;
if(dStart >= 0) {
t = t1;
info->normal = p1;
}
else {
t = t2;
info->normal = p2;
}
if(t >= startT && t <= endT)
{
info->t = t;
return true;
}
}
continue;
}
int subSqWidth = 1 << (level - 1);
F32 xIntercept = (blockPos.x + subSqWidth) * invBlockSize;
F32 xInt = calcInterceptX(pStart.x, invDeltaX, xIntercept);
F32 yIntercept = (blockPos.y + subSqWidth) * invBlockSize;
F32 yInt = calcInterceptY(pStart.y, invDeltaY, yIntercept);
F32 startX = startT * (pEnd.x - pStart.x) + pStart.x;
F32 startY = startT * (pEnd.y - pStart.y) + pStart.y;
if(xInt < startT)
xInt = MAX_FLOAT;
if(yInt < startT)
yInt = MAX_FLOAT;
U32 x0 = (startX > xIntercept) * subSqWidth;
U32 y0 = (startY > yIntercept) * subSqWidth;
U32 x1 = subSqWidth - x0;
U32 y1 = subSqWidth - y0;
U32 nextLevel = level - 1;
// push the items on the stack in reverse order of processing
if(xInt > endT && yInt > endT)
{
// only test the square the point started in:
stack[stackSize].blockPos.set(blockPos.x + x0, blockPos.y + y0);
stack[stackSize].level = nextLevel;
stackSize++;
}
else if(xInt < yInt)
{
F32 nextIntersect = endT;
if(yInt <= endT)
{
stack[stackSize].blockPos.set(blockPos.x + x1, blockPos.y + y1);
stack[stackSize].startT = yInt;
stack[stackSize].endT = endT;
stack[stackSize].level = nextLevel;
nextIntersect = yInt;
stackSize++;
}
stack[stackSize].blockPos.set(blockPos.x + x1, blockPos.y + y0);
stack[stackSize].startT = xInt;
stack[stackSize].endT = nextIntersect;
stack[stackSize].level = nextLevel;
stack[stackSize+1].blockPos.set(blockPos.x + x0, blockPos.y + y0);
stack[stackSize+1].startT = startT;
stack[stackSize+1].endT = xInt;
stack[stackSize+1].level = nextLevel;
stackSize += 2;
}
else if(yInt < xInt)
{
F32 nextIntersect = endT;
if(xInt <= endT)
{
stack[stackSize].blockPos.set(blockPos.x + x1, blockPos.y + y1);
stack[stackSize].startT = xInt;
stack[stackSize].endT = endT;
stack[stackSize].level = nextLevel;
nextIntersect = xInt;
stackSize++;
}
stack[stackSize].blockPos.set(blockPos.x + x0, blockPos.y + y1);
stack[stackSize].startT = yInt;
stack[stackSize].endT = nextIntersect;
stack[stackSize].level = nextLevel;
stack[stackSize+1].blockPos.set(blockPos.x + x0, blockPos.y + y0);
stack[stackSize+1].startT = startT;
stack[stackSize+1].endT = yInt;
stack[stackSize+1].level = nextLevel;
stackSize += 2;
}
else
{
stack[stackSize].blockPos.set(blockPos.x + x1, blockPos.y + y1);
stack[stackSize].startT = xInt;
stack[stackSize].endT = endT;
stack[stackSize].level = nextLevel;
stack[stackSize+1].blockPos.set(blockPos.x + x0, blockPos.y + y0);
stack[stackSize+1].startT = startT;
stack[stackSize+1].endT = xInt;
stack[stackSize+1].level = nextLevel;
stackSize += 2;
}
}
return false;
}
bool QuadTreeTracer::castRay(const Point3F &start, const Point3F &end, RayInfo *info)
{
PROFILE_START(QuadTreeTracer_castRay);
// Do some precalculations we'll use for the rest of this routine.
// Set up our intercept calculation methods.
F32 invDeltaX;
if(end.x == start.x)
{
calcInterceptX = calcInterceptNone;
invDeltaX = 0;
}
else
{
invDeltaX = 1.f / (end.x - start.x);
calcInterceptX = calcInterceptV;
}
F32 invDeltaY;
if(end.y == start.y)
{
calcInterceptY = calcInterceptNone;
invDeltaY = 0;
}
else
{
invDeltaY = 1.f / (end.y - start.y);
calcInterceptY = calcInterceptV;
}
// Subdivide our space based on the size of the lowest level of the tree...
const F32 invSize = 1.f / F32(BIT(mTreeDepth-1));
// Grab this off the frame allocator, we don't want to do a proper alloc
// on every ray!
FrameAllocatorMarker stackAlloc;
RayStackNode *stack = (RayStackNode*)stackAlloc.alloc(sizeof(RayStackNode) * (mTreeDepth * 3 + 1));
U32 stackSize = 1;
// Kick off the stack with the root node.
stack[0].startT = 0;
stack[0].endT = 1;
stack[0].squarePos.set(0,0);
stack[0].level = mTreeDepth - 1;
//Con::printf("QuadTreeTracer::castRay(%x)", this);
// Aright, now let's do some raycasting!
while(stackSize--)
{
// Get the current node for easy access...
RayStackNode *sn = stack + stackSize;
const U32 level = sn->level;
const F32 startT = sn->startT;
const F32 endT = sn->endT;
const Point2I squarePos = sn->squarePos;
AssertFatal((startT >= 0.f) && (startT <= 1.f), "QuadTreeTracer::castRay - out of range startT on stack!");
AssertFatal((endT >= 0.f) && (endT <= 1.f), "QuadTreeTracer::castRay - out of range endT on stack!");
//Con::printf(" -- node(%d, %d @ %d), sT=%f, eT=%f", squarePos.x, squarePos.y, level, startT, endT);
// Figure our start and end Z.
const F32 startZ = startT * (end.z - start.z) + start.z;
const F32 endZ = endT * (end.z - start.z) + start.z;
// Ok, now let's see if we hit the lower bound
const F32 squareMin = getSquareMin(level, squarePos);
if(startZ < squareMin && endZ < squareMin)
continue; //Nope, skip out.
// Hmm, let's check the upper bound.
const F32 squareMax = getSquareMax(level, squarePos);
if(startZ > squareMax && endZ > squareMax)
continue; //Nope, skip out.
// We might be intersecting something... If we've hit
// the tree depth let's deal with the leaf intersection.
if(level == 0)
{
//Con::printf(" ++ check node(%d, %d @ %d), sT=%f, eT=%f", squarePos.x, squarePos.y, level, startT, endT);
if(castLeafRay(squarePos, start, end, startT, endT, info))
{
PROFILE_END();
return true; // We hit, tell 'em so!
}
continue; // Otherwise, keep looking.
}
else
{
// Ok, we have to push our children as we're an inner node.
// First, figure out some widths...
U32 subSqSize = BIT(level - 1);
// Now, calculate intercepts so we know how to deal with this
// situation... (intercept = position, int = t value for that pos)
const F32 xIntercept = (squarePos.x + subSqSize) * invSize;
F32 xInt = calcInterceptX(start.x, invDeltaX, xIntercept);
const F32 yIntercept = (squarePos.y + subSqSize) * invSize;
F32 yInt = calcInterceptY(start.y, invDeltaY, yIntercept);
// Our starting position for this subray...
const F32 startX = startT * (end.x - start.x) + start.x;
const F32 startY = startT * (end.y - start.y) + start.y;
// Deal with squares that might be "behind" the ray.
if(xInt < startT) xInt = F32_MAX;
if(yInt < startT) yInt = F32_MAX;
// Do a little magic to calculate our next checks...
const U32 x0 = (startX > xIntercept) * subSqSize;
const U32 y0 = (startY > yIntercept) * subSqSize;
const U32 x1 = subSqSize - x0;
const U32 y1 = subSqSize - y0;
const U32 nextLevel = level - 1;
// Ok, now let's figure out what nodes, in what order, need to go
// on the stack. We push things on in reverse order of processing.
if(xInt > endT && yInt > endT)
{
stack[stackSize].squarePos.set(squarePos.x + x0, squarePos.y + y0);
stack[stackSize].level = nextLevel;
stackSize++;
}
else if(xInt < yInt)
{
F32 nextIntersect = endT;
if(yInt <= endT)
{
stack[stackSize].squarePos.set(squarePos.x + x1, squarePos.y + y1);
stack[stackSize].startT = yInt;
stack[stackSize].endT = endT;
stack[stackSize].level = nextLevel;
nextIntersect = yInt;
stackSize++;
}
// Do middle two, order doesn't matter.
stack[stackSize].squarePos.set(squarePos.x + x1, squarePos.y + y0);
stack[stackSize].startT = xInt;
stack[stackSize].endT = nextIntersect;
stack[stackSize].level = nextLevel;
stack[stackSize+1].squarePos.set(squarePos.x + x0, squarePos.y + y0);
stack[stackSize+1].startT = startT;
stack[stackSize+1].endT = xInt;
stack[stackSize+1].level = nextLevel;
stackSize += 2;
}
else if(yInt < xInt)
{
F32 nextIntersect = endT;
if(xInt <= endT)
{
stack[stackSize].squarePos.set(squarePos.x + x1, squarePos.y + y1);
stack[stackSize].startT = xInt;
stack[stackSize].endT = endT;
stack[stackSize].level = nextLevel;
nextIntersect = xInt;
stackSize++;
}
stack[stackSize].squarePos.set(squarePos.x + x0, squarePos.y + y1);
stack[stackSize].startT = yInt;
stack[stackSize].endT = nextIntersect;
stack[stackSize].level = nextLevel;
stack[stackSize+1].squarePos.set(squarePos.x + x0, squarePos.y + y0);
stack[stackSize+1].startT = startT;
stack[stackSize+1].endT = yInt;
stack[stackSize+1].level = nextLevel;
stackSize += 2;
}
else
{
stack[stackSize].squarePos.set(squarePos.x + x1, squarePos.y + y1);
stack[stackSize].startT = xInt;
stack[stackSize].endT = endT;
stack[stackSize].level = nextLevel;
stack[stackSize+1].squarePos.set(squarePos.x + x0, squarePos.y + y0);
stack[stackSize+1].startT = startT;
stack[stackSize+1].endT = xInt;
stack[stackSize+1].level = nextLevel;
stackSize += 2;
}
}
}
// Nothing found, so give up.
PROFILE_END();
return false;
}
bool TerrainBlock::castRayI(const Point3F &start, const Point3F &end, RayInfo *info, bool collideEmpty)
{
lineCount = 0;
lineStart = start;
lineEnd = end;
info->object = this;
if(start.x == end.x && start.y == end.y)
{
if (end.z == start.z)
return false;
F32 height;
if(!getNormalAndHeight(Point2F(start.x, start.y), &info->normal, &height, true))
return false;
F32 t = (height - start.z) / (end.z - start.z);
if(t < 0 || t > 1)
return false;
info->t = t;
return true;
}
F32 invBlockWorldSize = 1 / F32(mSquareSize * BlockSquareWidth);
Point3F pStart(start.x * invBlockWorldSize, start.y * invBlockWorldSize, start.z);
Point3F pEnd(end.x * invBlockWorldSize, end.y * invBlockWorldSize, end.z);
int blockX = (S32)mFloor(pStart.x);
int blockY = (S32)mFloor(pStart.y);
int dx, dy;
F32 invDeltaX;
if(pEnd.x == pStart.x)
{
calcInterceptX = calcInterceptNone;
invDeltaX = 0;
dx = 0;
}
else
{
invDeltaX = 1 / (pEnd.x - pStart.x);
calcInterceptX = calcInterceptV;
if(pEnd.x < pStart.x)
dx = -1;
else
dx = 1;
}
F32 invDeltaY;
if(pEnd.y == pStart.y)
{
calcInterceptY = calcInterceptNone;
invDeltaY = 0;
dy = 0;
}
else
{
invDeltaY = 1 / (pEnd.y - pStart.y);
calcInterceptY = calcInterceptV;
if(pEnd.y < pStart.y)
dy = -1;
else
dy = 1;
}
F32 startT = 0;
for(;;)
{
F32 nextXInt = calcInterceptX(pStart.x, invDeltaX, (F32)(blockX + (dx == 1)));
F32 nextYInt = calcInterceptY(pStart.y, invDeltaY, (F32)(blockY + (dy == 1)));
F32 intersectT = 1;
if(nextXInt < intersectT)
intersectT = nextXInt;
if(nextYInt < intersectT)
intersectT = nextYInt;
if(castRayBlock(pStart, pEnd, Point2I(blockX * BlockSquareWidth, blockY * BlockSquareWidth), BlockShift, invDeltaX, invDeltaY, startT, intersectT, info, collideEmpty)) {
info->normal.z *= BlockSquareWidth * mSquareSize;
info->normal.normalize();
return true;
}
startT = intersectT;
if(intersectT >= 1)
break;
if(nextXInt < nextYInt)
blockX += dx;
else if(nextYInt < nextXInt)
blockY += dy;
else
{
blockX += dx;
blockY += dy;
}
}
return false;
}
