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 AtlasGeomChunkTracer::castLeafRay(const Point2I pos, const Point3F &start, const Point3F &end, const F32 &startT, const F32 &endT, RayInfo *info) { if(AtlasInstance::smRayCollisionDebugLevel == AtlasInstance::RayCollisionDebugToColTree) { const F32 invSize = 1.f / F32(BIT(mTreeDepth-1)); // This is a bit of a hack. But good for testing. // Do collision against the collision tree leaf bounding box and return the result... F32 t; Point3F n; Box3F box; box.minExtents.set(Point3F(F32(pos.x ) * invSize, F32(pos.y ) * invSize, getSquareMin(0, pos))); box.maxExtents.set(Point3F(F32(pos.x+1) * invSize, F32(pos.y+1) * invSize, getSquareMax(0, pos))); //Con::printf(" checking at xy = {%f, %f}->{%f, %f}, [%d, %d]", start.x, start.y, end.x, end.y, pos.x, pos.y); if(box.collideLine(start, end, &t, &n) && t >= startT && t <= endT) { info->t = t; info->normal = n; return true; } return false; } else if( AtlasInstance::smRayCollisionDebugLevel == AtlasInstance::RayCollisionDebugToMesh ) { bool haveHit = false; U32 currentIdx = 0; U32 numIdx = mChunk->mIndexCount; F32 bestT = F32_MAX; U32 bestTri = -1; Point2F bestBary; while( !haveHit && currentIdx < numIdx ) { const Point3F& a = mChunk->mVert[ mChunk->mIndex[ currentIdx ] ].point; const Point3F& b = mChunk->mVert[ mChunk->mIndex[ currentIdx + 1 ] ].point; const Point3F& c = mChunk->mVert[ mChunk->mIndex[ currentIdx + 2 ] ].point; F32 localT; Point2F localBary; // Do the cast, using our conveniently precalculated ray delta... if(castRayTriangle(mRayStart, mRayDelta, a,b,c, localT, localBary)) { if(localT < bestT) { // And it hit before anything else we've seen. bestTri = currentIdx; bestT = localT; bestBary = localBary; haveHit = true; } } currentIdx += 3; } // Fill in extra info for the hit. if(!haveHit) return false; // Calculate the normal, we skip that for the initial check. Point3F norm; // Hi norm! const Point3F &a = mChunk->mVert[mChunk->mIndex[bestTri+0]].point; const Point3F &b = mChunk->mVert[mChunk->mIndex[bestTri+1]].point; const Point3F &c = mChunk->mVert[mChunk->mIndex[bestTri+2]].point; const Point2F &aTC = mChunk->mVert[mChunk->mIndex[bestTri+0]].texCoord; const Point2F &bTC = mChunk->mVert[mChunk->mIndex[bestTri+1]].texCoord; const Point2F &cTC = mChunk->mVert[mChunk->mIndex[bestTri+2]].texCoord; // Store everything relevant into the info structure. info->t = bestT; const Point3F e0 = b-a; const Point3F e1 = c-a; info->normal = mCross(e1, e0); info->normal.normalize(); // Calculate and store the texture coords. const Point2F e0TC = bTC-aTC; const Point2F e1TC = cTC-aTC; info->texCoord = e0TC * bestBary.x + e1TC * bestBary.y + aTC; // Return true, we hit something! return true; } else { // Get the triangle list... U16 *triOffset = mChunk->mColIndicesBuffer + mChunk->mColIndicesOffsets[pos.x * BIT(mChunk->mColTreeDepth-1) + pos.y]; // Store best hit results... bool gotHit = false; F32 bestT = F32_MAX; U16 bestTri = -1, offset; Point2F bestBary; while((offset = *triOffset) != 0xFFFF) { // Advance to the next triangle.. triOffset++; // Get each triangle, and do a raycast against it. Point3F a,b,c; AssertFatal(offset < mChunk->mIndexCount, "AtlasGeomTracer2::castLeafRay - offset past end of index list."); a = mChunk->mVert[mChunk->mIndex[offset+0]].point; b = mChunk->mVert[mChunk->mIndex[offset+1]].point; c = mChunk->mVert[mChunk->mIndex[offset+2]].point; /*Con::printf(" o testing triangle %d ({%f,%f,%f},{%f,%f,%f},{%f,%f,%f})", offset, a.x, a.y, a.z, b.x, b.y, b.z, c.x, c.y, c.z); */ F32 localT; Point2F localBary; // Do the cast, using our conveniently precalculated ray delta... if(castRayTriangle(mRayStart, mRayDelta, a,b,c, localT, localBary)) { //Con::printf(" - hit triangle %d at t=%f", offset, localT); // The ray intersected, but we have to make sure we hit actually on // the line segment. (ie, a ray isn't a ray, Ray.) // BJGTODO - This should prevent some nasty edge cases, but we // seem to be calculating the wrong start and end T's. // So I've disabled this for now but it will cause // problems later! //if(localT < startT || localT > endT) // continue; // It really, really hit, wow! if(localT < bestT) { // And it hit before anything else we've seen. bestTri = offset; bestT = localT; bestBary = localBary; gotHit = true; } } else { //Con::printf(" - didn't hit triangle %d at t=%f", offset, localT); } } // Fill in extra info for the hit. if(!gotHit) return false; // Calculate the normal, we skip that for the initial check. Point3F norm; // Hi norm! const Point3F &a = mChunk->mVert[mChunk->mIndex[bestTri+0]].point; const Point3F &b = mChunk->mVert[mChunk->mIndex[bestTri+1]].point; const Point3F &c = mChunk->mVert[mChunk->mIndex[bestTri+2]].point; const Point2F &aTC = mChunk->mVert[mChunk->mIndex[bestTri+0]].texCoord; const Point2F &bTC = mChunk->mVert[mChunk->mIndex[bestTri+1]].texCoord; const Point2F &cTC = mChunk->mVert[mChunk->mIndex[bestTri+2]].texCoord; // Store everything relevant into the info structure. info->t = bestT; const Point3F e0 = b-a; const Point3F e1 = c-a; info->normal = mCross(e1, e0); info->normal.normalize(); // Calculate and store the texture coords. const Point2F e0TC = bTC-aTC; const Point2F e1TC = cTC-aTC; info->texCoord = e0TC * bestBary.x + e1TC * bestBary.y + aTC; // Return true, we hit something! return true; } }