diff --git a/Standalone/Core/src/ShapeWorldInternal.h b/Standalone/Core/src/ShapeWorldInternal.h index c266bd8..216b0a2 100644 --- a/Standalone/Core/src/ShapeWorldInternal.h +++ b/Standalone/Core/src/ShapeWorldInternal.h @@ -11481,6 +11481,12 @@ inline void FStandaloneShapeWorld::FScopedPassageTileQueries::prepareSweptTubeBa last=std::min(sweep->dimension-1, static_cast(std::floor(relativeMaximum))); }; + struct FColumnProjectionCache + { + double projectionXY=0.0; + double floorT=0.0,floorTClamped=0.0,floorDepth=0.0,floorCenterZ=0.0; + bool hasProjection=false,hasFloorProfile=false; + }; for(std::size_t passageIndex=0;passageIndex=segmentEndIndex)continue; + constexpr std::size_t MaxCandidateSegments=8; + const std::size_t candidateSegmentCount=segmentEndIndex-candidate.segmentIndex; + std::array chordProjectionMinimum{}; + std::array chordProjectionMaximum{}; + std::array maximumRadius{}; + std::array maximumChordDeviation{}; + std::array,MaxCandidateSegments> segmentMinimum{}; + std::array,MaxCandidateSegments> segmentMaximum{}; + std::array chordOrigin{},chordDirection{}; + bool canCertifyChordWindow=candidateSegmentCount<=MaxCandidateSegments; + if(canCertifyChordWindow) + { + chordOrigin=segments[candidate.segmentIndex].a; + const auto& lastSegment=segments[segmentEndIndex-1]; + const double chordX=lastSegment.b[0]-chordOrigin[0]; + const double chordY=lastSegment.b[1]-chordOrigin[1]; + const double chordZ=lastSegment.b[2]-chordOrigin[2]; + const double chordLengthSq=chordX*chordX+chordY*chordY+chordZ*chordZ; + const double chordLength=chordLengthSq>1.0e-12 + ?FStandaloneShapeWorld::DeterministicSqrt(chordLengthSq):0.0; + canCertifyChordWindow=chordLength>1.0e-6&&std::isfinite(chordLength); + if(canCertifyChordWindow) + { + const double inverseChordLength=1.0/chordLength; + chordDirection={chordX*inverseChordLength,chordY*inverseChordLength, + chordZ*inverseChordLength}; + for(std::size_t localIndex=0;localIndex first{},last{}; for(std::size_t axis=0;axis<3;++axis) gridRange(axis,candidate.sweepMinimumSubvoxels[axis], candidate.sweepMaximumSubvoxels[axis],first[axis],last[axis]); if(first[0]>last[0]||first[1]>last[1]||first[2]>last[2])continue; + const std::size_t columnWidth=static_cast(last[0]-first[0]+1); + const std::size_t columnHeight=static_cast(last[1]-first[1]+1); + const std::size_t columnsPerSegment=columnWidth*columnHeight; + std::vector columnProjectionCache( + candidateSegmentCount*columnsPerSegment); for(std::int32_t gz=first[2];gz<=last[2];++gz) { @@ -11516,9 +11594,65 @@ inline void FStandaloneShapeWorld::FScopedPassageTileQueries::prepareSweptTubeBa +static_cast(gy)*sweep->stepSubvoxels); const double qz=static_cast(sweep->gridMinimumSubvoxels[2] +static_cast(gz)*sweep->stepSubvoxels); - for(std::size_t segmentIndex=candidate.segmentIndex; - segmentIndex query={qx,qy,qz}; + const std::size_t sampleIndex=(static_cast(gz) + *static_cast(sweep->dimension)+static_cast(gy)) + *static_cast(sweep->dimension)+static_cast(gx); + auto& sample=passageSamples.samples[sampleIndex]; + std::array segmentOrder{}; + std::size_t anchorIndex=0; + double queryChordProjection=0.0; + double queryChordPerpendicularDistanceSq=0.0; + if(canCertifyChordWindow) + { + queryChordProjection=(qx-chordOrigin[0])*chordDirection[0] + +(qy-chordOrigin[1])*chordDirection[1] + +(qz-chordOrigin[2])*chordDirection[2]; + for(std::size_t axis=0;axis<3;++axis) + { + const double perpendicular=(query[axis]-chordOrigin[axis]) + -queryChordProjection*chordDirection[axis]; + queryChordPerpendicularDistanceSq+=perpendicular*perpendicular; + } + double nearestChordGap=std::numeric_limits::infinity(); + for(std::size_t localIndex=0;localIndex=offset) + segmentOrder[orderCount++]=anchorIndex-offset; + if(orderCount(gy-first[1])*columnWidth + +static_cast(gx-first[0]); + auto& column=columnProjectionCache[columnIndex]; const auto& segment=segments[segmentIndex]; if(segment.lengthSq<=0.0 ||qxsegment.sweepMaximumSubvoxels[1] ||qzsegment.sweepMaximumSubvoxels[2])continue; - const double projectionXY=(qx-segment.a[0])*segment.delta[0] - +(qy-segment.a[1])*segment.delta[1]; + if(canCertifyChordWindow) + { + // Every point on a fine segment stays within + // maximumChordDeviation of the coarse chord. Combine the + // chord-axis gap with the query's perpendicular distance, then + // subtract that deviation. The segment AABB is a second lower + // bound. Skip only when both bounds exclude every exact minimum. + const double chordGap=std::max({chordProjectionMinimum[localIndex] + -queryChordProjection,0.0, + queryChordProjection-chordProjectionMaximum[localIndex]}); + const double chordDistanceSq=queryChordPerpendicularDistanceSq + +chordGap*chordGap; + double boxDistanceSq=0.0; + for(std::size_t axis=0;axis<3;++axis) + { + const double gap=std::max({segmentMinimum[localIndex][axis] + -query[axis],0.0, + query[axis]-segmentMaximum[localIndex][axis]}); + boxDistanceSq+=gap*gap; + } + const double boundMargin=1.0e-6*qPerMetre; + const double tubeBaseLimit=sample.roundSignedDistance + +maximumRadius[localIndex]+boundMargin; + const double tubeChordLimit=tubeBaseLimit + +maximumChordDeviation[localIndex]; + const bool canImproveTube=tubeBaseLimit>=0.0&&tubeChordLimit>=0.0 + &&boxDistanceSq<=tubeBaseLimit*tubeBaseLimit + &&chordDistanceSq<=tubeChordLimit*tubeChordLimit; + const double bestFloorDistance=std::isfinite( + sample.floorCenterDistanceSq) + ?FStandaloneShapeWorld::DeterministicSqrt( + sample.floorCenterDistanceSq) + :std::numeric_limits::infinity(); + const double floorBaseLimit=bestFloorDistance+boundMargin; + const double floorChordLimit=floorBaseLimit + +maximumChordDeviation[localIndex]; + const bool canImproveFloor=floorBaseLimit>=0.0 + &&floorChordLimit>=0.0 + &&boxDistanceSq<=floorBaseLimit*floorBaseLimit + &&chordDistanceSq<=floorChordLimit*floorChordLimit; + const double shellBaseLimit=sample.shellSignedDistance + +shellThickness+maximumRadius[localIndex]+boundMargin; + const double shellChordLimit=shellBaseLimit + +maximumChordDeviation[localIndex]; + const bool canImproveShell=shellBaseLimit>=0.0 + &&shellChordLimit>=0.0 + &&boxDistanceSq<=shellBaseLimit*shellBaseLimit + &&chordDistanceSq<=shellChordLimit*shellChordLimit; + if(!canImproveTube&&!canImproveFloor&&!canImproveShell)continue; + } + if(!column.hasProjection) + { + column.projectionXY=(qx-segment.a[0])*segment.delta[0] + +(qy-segment.a[1])*segment.delta[1]; + column.hasProjection=true; + } + const double projectionXY=column.projectionXY; const double t=std::clamp((projectionXY +(qz-segment.a[2])*segment.delta[2])/segment.lengthSq,0.0,1.0); double cx=segment.a[0]+t*segment.delta[0]; @@ -11538,18 +11727,44 @@ inline void FStandaloneShapeWorld::FScopedPassageTileQueries::prepareSweptTubeBa const double ex=qx-cx,ey=qy-cy,ez=qz-cz; const double signedDistance=FStandaloneShapeWorld::DeterministicSqrt( ex*ex+ey*ey+ez*ez)-radius; - const double floorT=segment.horizontalLengthSq>1.0e-12 - ?projectionXY/segment.horizontalLengthSq:t; - const double floorTClamped=std::clamp(floorT,0.0,1.0); const double floorCenterDistanceSq=ex*ex+ey*ey+ez*ez; - const double floorRadius=segment.radiusA - +floorTClamped*segment.radiusDelta; - const double floorDepth=FStandaloneShapeWorld::PassageFlatFloorDepthSubvoxels( - constants,segmentIndex,floorTClamped,floorRadius,qPerMetre); - const double floorCenterZ= - FStandaloneShapeWorld::PassageFlatFloorCenterZSubvoxels( - constants,segments,segmentIndex,floorT,segment, - qx,qy,floorDepth,qPerMetre); + double floorT=0.0,floorTClamped=0.0,floorDepth=0.0,floorCenterZ=0.0; + if(segment.horizontalLengthSq>1.0e-12) + { + if(!column.hasFloorProfile) + { + column.floorT=projectionXY/segment.horizontalLengthSq; + column.floorTClamped=std::clamp(column.floorT,0.0,1.0); + const double floorRadius=segment.radiusA + +column.floorTClamped*segment.radiusDelta; + column.floorDepth= + FStandaloneShapeWorld::PassageFlatFloorDepthSubvoxels( + constants,segmentIndex,column.floorTClamped,floorRadius, + qPerMetre); + column.floorCenterZ= + FStandaloneShapeWorld::PassageFlatFloorCenterZSubvoxels( + constants,segments,segmentIndex,column.floorT,segment, + qx,qy,column.floorDepth,qPerMetre); + column.hasFloorProfile=true; + } + floorT=column.floorT; + floorTClamped=column.floorTClamped; + floorDepth=column.floorDepth; + floorCenterZ=column.floorCenterZ; + } + else + { + floorT=t; + floorTClamped=std::clamp(floorT,0.0,1.0); + const double floorRadius=segment.radiusA + +floorTClamped*segment.radiusDelta; + floorDepth=FStandaloneShapeWorld::PassageFlatFloorDepthSubvoxels( + constants,segmentIndex,floorTClamped,floorRadius,qPerMetre); + floorCenterZ= + FStandaloneShapeWorld::PassageFlatFloorCenterZSubvoxels( + constants,segments,segmentIndex,floorT,segment, + qx,qy,floorDepth,qPerMetre); + } const double flatDistance=FStandaloneShapeWorld::PassageFlatBottomDistance( static_cast(signedDistance),floorCenterZ,qz,floorDepth,qPerMetre); if(profile)++profileStats_->passageSweepSegmentSamplePairs; @@ -11557,10 +11772,6 @@ inline void FStandaloneShapeWorld::FScopedPassageTileQueries::prepareSweptTubeBa { if(profile)++profileStats_->passageSweepOutsideInfluencePairs; } - const std::size_t sampleIndex=(static_cast(gz) - *static_cast(sweep->dimension)+static_cast(gy)) - *static_cast(sweep->dimension)+static_cast(gx); - auto& sample=passageSamples.samples[sampleIndex]; if(floorCenterDistanceSq