geometry: use dedicated 64-bit integer square root for distance computations

Guarantees 1 LSB error, while the C++ double type has 55 mantissa bits (meaning for sqrt(X) >~ 2^22.5) the error is not guaranteed.


(cherry picked from commit e6ebc2b9b9)
This commit is contained in:
Tomasz Wlostowski 2022-06-03 22:18:25 +02:00 committed by Jon Evans
parent 275afdc1d5
commit 8afb8190af
2 changed files with 46 additions and 5 deletions

View File

@ -35,6 +35,39 @@ int sgn( T aVal )
return ( T( 0 ) < aVal ) - ( aVal < T( 0 ) ); return ( T( 0 ) < aVal ) - ( aVal < T( 0 ) );
} }
template <typename T>
constexpr T sqrt_helper(T x, T lo, T hi)
{
if (lo == hi)
return lo;
const T mid = (lo + hi + 1) / 2;
if (x / mid < mid)
return sqrt_helper<T>(x, lo, mid - 1);
else
return sqrt_helper(x, mid, hi);
}
template <typename T>
constexpr T ct_sqrt(T x)
{
return sqrt_helper<T>(x, 0, x / 2 + 1);
}
template <typename T>
T isqrt(T x)
{
T r = (T) std::sqrt((double) x);
T sqrt_max = ct_sqrt(std::numeric_limits<T>::max());
while (r < sqrt_max && r * r < x)
r++;
while (r > sqrt_max || r * r > x)
r--;
return r;
}
SEG::ecoord SEG::SquaredDistance( const SEG& aSeg ) const SEG::ecoord SEG::SquaredDistance( const SEG& aSeg ) const
{ {
@ -209,7 +242,7 @@ bool SEG::Collide( const SEG& aSeg, int aClearance, int* aActual ) const
if( dist_sq == 0 || dist_sq < (ecoord) aClearance * aClearance ) if( dist_sq == 0 || dist_sq < (ecoord) aClearance * aClearance )
{ {
if( aActual ) if( aActual )
*aActual = sqrt( dist_sq ); *aActual = isqrt( dist_sq );
return true; return true;
} }
@ -284,13 +317,13 @@ VECTOR2I SEG::LineProject( const VECTOR2I& aP ) const
int SEG::Distance( const SEG& aSeg ) const int SEG::Distance( const SEG& aSeg ) const
{ {
return KiROUND( sqrt( SquaredDistance( aSeg ) ) ); return isqrt( SquaredDistance( aSeg ) );
} }
int SEG::Distance( const VECTOR2I& aP ) const int SEG::Distance( const VECTOR2I& aP ) const
{ {
return KiROUND( sqrt( SquaredDistance( aP ) ) ); return isqrt( SquaredDistance( aP ) );
} }
@ -299,8 +332,16 @@ int SEG::LineDistance( const VECTOR2I& aP, bool aDetermineSide ) const
ecoord p = ecoord{ A.y } - B.y; ecoord p = ecoord{ A.y } - B.y;
ecoord q = ecoord{ B.x } - A.x; ecoord q = ecoord{ B.x } - A.x;
ecoord r = -p * A.x - q * A.y; ecoord r = -p * A.x - q * A.y;
ecoord l = p * p + q * q;
ecoord det = p * aP.x + q * aP.y + r;
ecoord dist_sq = 0;
ecoord dist = KiROUND( ( p * aP.x + q * aP.y + r ) / sqrt( p * p + q * q ) ); if( l > 0 )
{
dist_sq = rescale( det, det, l );
}
ecoord dist = isqrt( dist_sq );
return aDetermineSide ? dist : std::abs( dist ); return aDetermineSide ? dist : std::abs( dist );
} }

View File

@ -340,7 +340,7 @@ static const std::vector<SEG_VECTOR_DISTANCE_CASE> seg_vec_dist_cases = {
"At end (not collinear)", "At end (not collinear)",
{ { 0, 0 }, { 1000, 0 } }, { { 0, 0 }, { 1000, 0 } },
{ 1000 + 200, 200 }, { 1000 + 200, 200 },
283, // sqrt(200^2 + 200^2) = 282.8, rounded to nearest 282, // sqrt(200^2 + 200^2) = 282.8, rounded to nearest
}, },
}; };
// clang-format on // clang-format on