diff --git a/world/Segment.cpp b/world/Segment.cpp index 53f1f904..79efac76 100644 --- a/world/Segment.cpp +++ b/world/Segment.cpp @@ -129,6 +129,13 @@ bool TSegment::Init(glm::dvec3 &NewPoint1, glm::dvec3 NewCPointOut, glm::dvec3 N fStep = fLength / iSegCount; // update step to equalize size of individual pieces + InitTFromSInterpolateData(); + return true; +} + +void TSegment::InitTFromSInterpolateData() +{ + // pre-compute the curve parameter t at equal distance intervals fTsBuffer.resize( iSegCount + 1 ); fTsBuffer[ 0 ] = 0.0; for( int i = 1; i < iSegCount; ++i ) { @@ -136,7 +143,22 @@ bool TSegment::Init(glm::dvec3 &NewPoint1, glm::dvec3 NewCPointOut, glm::dvec3 N } fTsBuffer[ iSegCount ] = 1.0; - return true; + // pre-compute Hermite slopes dt/du = fStep / |B'(t)| for FastGetTFromS + // (dt/ds = 1/|B'(t)|, scaled by fStep to the interval coordinate u) + fTsSlope.resize( iSegCount + 1 ); + for( int i = 0; i <= iSegCount; ++i ) { + // compute ds/dt derivative = |B'(t)| + double const speed = glm::length( GetFirstDerivative( fTsBuffer[ i ] ) ); + // compute dt/du derivative = fStep / (ds/dt) + fTsSlope[ i ] = speed > 0.0 ? fStep / speed : 0.0; + } + // clamp slopes to keep t(s) monotone (PCHIP / Fritsch-Carlson condition) + for( int i = 0; i <= iSegCount; ++i ) { + double limit = std::numeric_limits::max(); + if( i > 0 ) { limit = std::min( limit, 3.0 * ( fTsBuffer[ i ] - fTsBuffer[ i - 1 ] ) ); } + if( i < iSegCount ) { limit = std::min( limit, 3.0 * ( fTsBuffer[ i + 1 ] - fTsBuffer[ i ] ) ); } + fTsSlope[ i ] = std::min( fTsSlope[ i ], limit ); + } } glm::dvec3 TSegment::GetFirstDerivative(double const fTime) const @@ -194,7 +216,7 @@ double TSegment::GetTFromS(double const s) const { // initial guess for Newton's method double fTolerance = 0.001; - double fRatio = s / RombergIntegral(0, 1); + double fRatio = s / fLength; // fLength is approx RombergIntegral(0, 1), good enough to start Newton double fTime = std::lerp( 0.0, 1.0, fRatio ); int iteration = 0; @@ -217,6 +239,27 @@ double TSegment::GetTFromS(double const s) const return fTime; }; +double TSegment::FastGetTFromS(double const s) const +{ // Faster version of GetTFromS: cubic Hermite interpolation using + // the precomputed values in fTsBuffer and slopes in fTsSlope. It + // avoids the expensive RombergIntegral+Newton per call. Hermite + // interpolation is C1-continuous, so no sudden speed changes when + // moving from one interval to the next. + // NOTE: for s outside <0, fLength> the result is extrapolated + // outside <0, 1>, which is the expected behavior (see e.g. usage + // in GetDirection()) + int const i = std::clamp(static_cast(s / fStep), 0, iSegCount - 1); // interval index + double const u = (s - i * fStep) / fStep; // position within interval (possibly outside) + double const u2 = u * u; + double const u3 = u2 * u; + double const h01 = -2.0 * u3 + 3.0 * u2; + double const t = ( ( 1.0 - h01 ) * fTsBuffer[ i ] + + ( u3 - 2.0 * u2 + u ) * fTsSlope [ i ] + + ( h01 ) * fTsBuffer[ i + 1 ] + + ( u3 - u2 ) * fTsSlope [ i + 1 ] ); + return t; +} + glm::dvec3 TSegment::RaInterpolate(double const t) const { // wyliczenie XYZ na krzywej Beziera z użyciem współczynników return t * (t * (t * vA + vB) + vC) + Point1; // 9 mnożeń, 9 dodawań @@ -295,10 +338,10 @@ const double fDirectionOffset = 0.1; // długość wektora do wyliczenia kierunk glm::dvec3 TSegment::GetDirection(double const fDistance) const { // takie toporne liczenie pochodnej dla podanego dystansu od Point1 - double t1 = GetTFromS(fDistance - fDirectionOffset); + double t1 = FastGetTFromS(fDistance - fDirectionOffset); if (t1 <= 0.0) return CPointOut - Point1; // na zewnątrz jako prosta - double t2 = GetTFromS(fDistance + fDirectionOffset); + double t2 = FastGetTFromS(fDistance + fDirectionOffset); if (t2 >= 1.0) return Point1 - CPointIn; // na zewnątrz jako prosta return FastGetPoint(t2) - FastGetPoint(t1); @@ -319,7 +362,7 @@ Math3D::vector3 TSegment::GetPoint(double const fDistance) const { // wyliczenie współrzędnych XYZ na torze w odległości (fDistance) od Point1 if (bCurve) { // można by wprowadzić uproszczony wzór dla okręgów płaskich - double t = GetTFromS(fDistance); // aproksymacja dystansu na krzywej Beziera + double t = FastGetTFromS(fDistance); // aproksymacja dystansu na krzywej Beziera // return std::lerp(t,Point1,CPointOut,CPointIn,Point2); return RaInterpolate(t); } @@ -338,7 +381,7 @@ Math3D::vector3 TSegment::GetPoint(double const fDistance) const void TSegment::RaPositionGet(double const fDistance, glm::dvec3 &position, glm::vec3 &rotation) const { if (bCurve) { // można by wprowadzić uproszczony wzór dla okręgów płaskich - auto const t = GetTFromS(fDistance); // aproksymacja dystansu na krzywej Beziera na parametr (t) + auto const t = FastGetTFromS(fDistance); // aproksymacja dystansu na krzywej Beziera na parametr (t) position = FastGetPoint( t ); // przechyłka w danym miejscu (zmienia się liniowo) rotation.x = std::lerp( fRoll1, fRoll2, t ); diff --git a/world/Segment.h b/world/Segment.h index fccaf3af..645e5663 100644 --- a/world/Segment.h +++ b/world/Segment.h @@ -46,7 +46,8 @@ class TSegment fRoll1 { 0.f }, fRoll2 { 0.f }; // przechyłka na końcach double fLength { -1.0 }; // długość policzona - std::vector fTsBuffer; // wartości parametru krzywej dla równych odcinków + std::vector fTsBuffer; // wartości parametru krzywej t dla równych odcinków s + std::vector fTsSlope; // nachylenie dt/du (= dt/ds * fStep) w tych samych punktach double fStep = 0.0; int iSegCount = 0; // ilość odcinków do rysowania krzywej double fDirection = 0.0; // Ra: kąt prostego w planie; dla łuku kąt od Point1 @@ -56,7 +57,9 @@ class TSegment glm::dvec3 GetFirstDerivative(double const fTime) const; double RombergIntegral(double const fA, double const fB) const; + void InitTFromSInterpolateData(); double GetTFromS(double const s) const; + double FastGetTFromS(double const s) const; glm::dvec3 RaInterpolate(double const t) const; glm::dvec3 RaInterpolate0(double const t) const;