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 ) );
|
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 );
|
||||||
}
|
}
|
||||||
|
|
|
@ -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
|
||||||
|
|
Loading…
Reference in New Issue