diff --git a/Standalone/Core/src/ResolvedLayoutInternal.h b/Standalone/Core/src/ResolvedLayoutInternal.h index 7ea6c8c..b653d01 100644 --- a/Standalone/Core/src/ResolvedLayoutInternal.h +++ b/Standalone/Core/src/ResolvedLayoutInternal.h @@ -10,6 +10,7 @@ #include #include #include +#include namespace VoxelForgeCore { @@ -776,6 +777,119 @@ namespace /static_cast(denominator); return negative?-static_cast(rounded):static_cast(rounded); } + struct LandingFloorGradientConstraintV1 + { + std::int64_t dxSubvoxels=0,dySubvoxels=0,minimumRiseSubvoxels=0; + std::int64_t centerOffsetSign=0; + }; + bool FitLandingFloorGradientAboveRouteV1( + const std::array& preferred, + const std::vector& constraints, + std::int64_t maximumMagnitude,std::int64_t centerOffsetSubvoxels, + std::array& result) + { + struct FConstraint + { + std::int64_t x=0,y=0,rhs=0; + }; + std::vector q16Constraints; + q16Constraints.reserve(constraints.size()); + for(const auto& constraint:constraints) + { + const std::int64_t rise=constraint.minimumRiseSubvoxels + +constraint.centerOffsetSign*centerOffsetSubvoxels; + const std::int64_t rhs=rise*65536; + if(constraint.dxSubvoxels==0&&constraint.dySubvoxels==0) + { + if(rhs>0)return false; + continue; + } + q16Constraints.push_back({constraint.dxSubvoxels, + constraint.dySubvoxels,rhs}); + } + if(q16Constraints.empty()) + { + const std::int64_t x=preferred[0],y=preferred[1]; + if(x*x+y*y>maximumMagnitude*maximumMagnitude)return false; + result=preferred;return true; + } + + bool found=false; + std::int64_t bestX=0,bestY=0,bestDistance=0; + const auto consider=[&](double x,double y) + { + if(!std::isfinite(x)||!std::isfinite(y))return; + const std::int64_t roundedX=static_cast(std::llround(x)); + const std::int64_t roundedY=static_cast(std::llround(y)); + for(std::int64_t ox=-2;ox<=2;++ox) + for(std::int64_t oy=-2;oy<=2;++oy) + { + const std::int64_t candidateX=roundedX+ox; + const std::int64_t candidateY=roundedY+oy; + if(candidateX*candidateX+candidateY*candidateY + >maximumMagnitude*maximumMagnitude)continue; + bool feasible=true; + for(const auto& constraint:q16Constraints) + if(constraint.x*candidateX+constraint.y*candidateY(maximumMagnitude); + const double preferredLength=std::hypot(preferredX,preferredY); + if(preferredLength>gradeRadius&&preferredLength>0.0) + consider(preferredX*gradeRadius/preferredLength, + preferredY*gradeRadius/preferredLength); + for(const auto& constraint:q16Constraints) + { + const double x=static_cast(constraint.x); + const double y=static_cast(constraint.y); + const double normSquared=x*x+y*y; + const double targetDot=x*preferredX+y*preferredY; + const double projection=(static_cast(constraint.rhs)-targetDot) + /normSquared; + consider(preferredX+projection*x,preferredY+projection*y); + + const double minimumNormSquared=static_cast(constraint.rhs) + *constraint.rhs/normSquared; + const double remaining=gradeRadius*gradeRadius-minimumNormSquared; + if(remaining>=0.0) + { + const double baseX=static_cast(constraint.rhs)*x/normSquared; + const double baseY=static_cast(constraint.rhs)*y/normSquared; + const double offset=std::sqrt(remaining/normSquared); + consider(baseX-offset*y,baseY+offset*x); + consider(baseX+offset*y,baseY-offset*x); + } + } + for(std::size_t i=0;i(a.rhs)*b.y + -static_cast(a.y)*b.rhs)/determinant; + const double y=(static_cast(a.x)*b.rhs + -static_cast(a.rhs)*b.x)/determinant; + consider(x,y); + } + if(!found)return false; + result={bestX,bestY}; + return true; + } std::array PassageRandomXY(std::uint64_t seed, std::int64_t inner,std::int64_t outer) { @@ -4983,9 +5097,9 @@ bool PlanResolvedLayoutPassagesV1(ResolvedLayoutSnapshotV3& layout,std::int32_t std::llround(4.5*qPerMetre)); const auto& endpoint=passage.controlPoints[endpointIndex]; const auto& neighbor=passage.controlPoints[neighborIndex]; - const std::int64_t endpointFloor=static_cast( + std::int64_t endpointFloor=static_cast( std::llround(chamberFloorProfile[endpointIndex])); - const std::int64_t neighborFloor=static_cast( + std::int64_t neighborFloor=static_cast( std::llround(chamberFloorProfile[neighborIndex])); chamber.floorCenterSubvoxels[2]=endpointFloor; const std::int64_t dx=endpoint.positionSubvoxels[0] @@ -5003,50 +5117,139 @@ bool PlanResolvedLayoutPassagesV1(ResolvedLayoutSnapshotV3& layout,std::int32_t horizontalSquared)}; if(host.generatorKind=="SurfaceWorld"&&side[0]=='u') { - // The surface route enters the outer half of this chamber. Fit - // its landing plane to that receiving route, while keeping its - // center height equal to the passage's planned h(s) endpoint. + // Keep the chamber plane at h(s) on the bore center, then fit + // its slope above every segment of the receiving walk surface + // that intersects the chamber footprint. const auto surfaceRoute=routePoints(host); - const double radiusSquared=static_cast( - chamber.radiusSubvoxels)*chamber.radiusSubvoxels; - double farthestDistanceSquared=0.0; - std::array farthestPoint{}; - for(const auto& routePoint:surfaceRoute) + std::vector floorConstraints; + const std::int64_t centerX=chamber.floorCenterSubvoxels[0]; + const std::int64_t centerY=chamber.floorCenterSubvoxels[1]; + const std::int64_t centerFloor=chamber.floorCenterSubvoxels[2]; + const double radius=static_cast(chamber.radiusSubvoxels); + const double radiusSquared=radius*radius; + const auto addRouteFloorConstraint=[&](double t, + const std::array& a,const std::array& b) + { + const double px=std::lerp(a[0],b[0],t); + const double py=std::lerp(a[1],b[1],t); + const double pz=std::lerp(a[2],b[2],t); + const std::int64_t x=static_cast(std::llround(px)); + const std::int64_t y=static_cast(std::llround(py)); + const std::int64_t floor=static_cast(std::llround(pz)) + -surfacePlayerCenterAboveFloorQ; + const std::int64_t dxQ=x-centerX,dyQ=y-centerY; + const double distanceSquared=static_cast(dxQ)*dxQ + +static_cast(dyQ)*dyQ; + if(distanceSquared>radiusSquared+4.0*radius+4.0)return; + // Four subvoxels cover the rounded XY/Z projection while + // leaving a deterministic clearance over ground and below + // the route capsule's foot envelope. + const std::int64_t marginQ=(dxQ==0&&dyQ==0)?0:4; + floorConstraints.push_back({dxQ,dyQ, + floor-centerFloor+marginQ,-1}); + const std::int64_t maximumFloor=static_cast( + std::llround(pz))-static_cast( + std::llround(0.88*qPerMetre))-marginQ; + floorConstraints.push_back({-dxQ,-dyQ, + centerFloor-maximumFloor,1}); + }; + for(const auto& point:surfaceRoute) + addRouteFloorConstraint(0.0,point,point); + for(std::size_t i=1;ifarthestDistanceSquared) + const auto& a=surfaceRoute[i-1];const auto& b=surfaceRoute[i]; + const double dx=b[0]-a[0],dy=b[1]-a[1]; + const double ox=a[0]-centerX,oy=a[1]-centerY; + const double aa=dx*dx+dy*dy; + if(aa<=0.0)continue; + const double bb=2.0*(ox*dx+oy*dy); + const double cc=ox*ox+oy*oy-radiusSquared; + const double discriminant=bb*bb-4.0*aa*cc; + if(discriminant<0.0)continue; + const double root=std::sqrt(discriminant); + const double first=(-bb-root)/(2.0*aa); + const double second=(-bb+root)/(2.0*aa); + if(first>=0.0&&first<=1.0) + addRouteFloorConstraint(first,a,b); + if(second>=0.0&&second<=1.0) + addRouteFloorConstraint(second,a,b); + } + std::sort(floorConstraints.begin(),floorConstraints.end(), + [](const auto& a,const auto& b) + {return std::tie(a.dxSubvoxels,a.dySubvoxels, + a.minimumRiseSubvoxels,a.centerOffsetSign) + (0, + surfacePlayerCenterAboveFloorQ + -static_cast(std::llround(0.88*qPerMetre))-4); + const auto profileOffsetAt=[&](std::size_t index, + std::int64_t centerRaiseQ) + { + const std::int64_t distanceQ=endpointIndex==0 + ?cumulative[index]:mouthHorizontal-cumulative[index]; + const double t=std::clamp(static_cast(distanceQ) + /(10.0*qPerMetre),0.0,1.0); + const double smooth=t*t*(3.0-2.0*t); + return static_cast(std::llround( + centerRaiseQ*(1.0-smooth))); + }; + bool fitted=false; + std::int64_t centerRaiseQ=0; + std::array fittedGradient{}; + for(std::int64_t candidateRaiseQ=0; + candidateRaiseQ<=maximumCenterRaiseQ;++candidateRaiseQ) + { + const std::int64_t candidateEndpointFloor=endpointFloor + +profileOffsetAt(endpointIndex,candidateRaiseQ); + const std::int64_t candidateNeighborFloor=neighborFloor + +profileOffsetAt(neighborIndex,candidateRaiseQ); + const std::int64_t candidateDz=candidateEndpointFloor + -candidateNeighborFloor; + const std::array preferredGradient={ + PassageRoundRatio(dx<0?-candidateDz:candidateDz, + std::abs(dx)*65536,horizontalSquared), + PassageRoundRatio(dy<0?-candidateDz:candidateDz, + std::abs(dy)*65536,horizontalSquared)}; + if(FitLandingFloorGradientAboveRouteV1(preferredGradient, + floorConstraints,17560,candidateRaiseQ, + fittedGradient)) { - farthestDistanceSquared=distanceSquared; - farthestPoint=routePoint; + fitted=true;centerRaiseQ=candidateRaiseQ;break; } } - double mouthRouteHeight=0.0,mouthRouteDistanceSquared=0.0; - if(farthestDistanceSquared>0.0 - &&VoxelForgeCore::PassageSpline::ClosestRouteHeightAtXY( - surfaceRoute,static_cast(mouth[0]), - static_cast(mouth[1]),mouthRouteHeight, - mouthRouteDistanceSquared)) + if(floorConstraints.empty()||!fitted) { - const std::int64_t surfaceDx=static_cast( - std::llround(farthestPoint[0]-mouth[0])); - const std::int64_t surfaceDy=static_cast( - std::llround(farthestPoint[1]-mouth[1])); - const std::int64_t surfaceDz=static_cast( - std::llround(farthestPoint[2]-mouthRouteHeight)); - const std::int64_t surfaceHorizontalSquared=surfaceDx*surfaceDx - +surfaceDy*surfaceDy; - if(surfaceHorizontalSquared>0) - chamber.floorGradientQ16={ - PassageRoundRatio(surfaceDx<0?-surfaceDz:surfaceDz, - std::abs(surfaceDx)*65536, - surfaceHorizontalSquared), - PassageRoundRatio(surfaceDy<0?-surfaceDz:surfaceDz, - std::abs(surfaceDy)*65536, - surfaceHorizontalSquared)}; + error=passage.stableId+" upper landing floor cannot stay between its receiving route and capsule foot within the 15-degree grade"; + return false; + } + if(centerRaiseQ>0) + { + for(std::size_t i=0;i( + static_cast(std::llround( + chamberFloorProfile[i])) + +profileOffsetAt(i,centerRaiseQ)); + passage.controlPoints[i].positionSubvoxels[2]= + static_cast(std::llround( + chamberFloorProfile[i])) + +passage.controlPoints[i].radiusSubvoxels; + } + endpointFloor=static_cast(std::llround( + chamberFloorProfile[endpointIndex])); + neighborFloor=static_cast(std::llround( + chamberFloorProfile[neighborIndex])); + chamber.floorCenterSubvoxels[2]=endpointFloor; } + chamber.floorGradientQ16=fittedGradient; } const std::uint64_t gradientSquared=static_cast( chamber.floorGradientQ16[0]*chamber.floorGradientQ16[0]