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:
parent
275afdc1d5
commit
8afb8190af
|
@ -35,6 +35,39 @@ int sgn( T aVal )
|
|||
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
|
||||
{
|
||||
|
@ -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( aActual )
|
||||
*aActual = sqrt( dist_sq );
|
||||
*aActual = isqrt( dist_sq );
|
||||
|
||||
return true;
|
||||
}
|
||||
|
@ -284,13 +317,13 @@ VECTOR2I SEG::LineProject( const VECTOR2I& aP ) const
|
|||
|
||||
int SEG::Distance( const SEG& aSeg ) const
|
||||
{
|
||||
return KiROUND( sqrt( SquaredDistance( aSeg ) ) );
|
||||
return isqrt( SquaredDistance( aSeg ) );
|
||||
}
|
||||
|
||||
|
||||
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 q = ecoord{ B.x } - A.x;
|
||||
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 );
|
||||
}
|
||||
|
|
|
@ -340,7 +340,7 @@ static const std::vector<SEG_VECTOR_DISTANCE_CASE> seg_vec_dist_cases = {
|
|||
"At end (not collinear)",
|
||||
{ { 0, 0 }, { 1000, 0 } },
|
||||
{ 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
|
||||
|
|
Loading…
Reference in New Issue