From 207a2796e9db91ed70ffc35d3a0767f4015438e0 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Thu, 10 Oct 2024 22:53:41 -0700 Subject: [PATCH] fix atan2 and add more unit tests added point overlap test --- include/box2d/box2d.h | 6 ++- include/box2d/math_functions.h | 1 + src/math_functions.c | 78 ++++++++++++++++------------------ src/world.c | 7 +++ test/test_determinism.c | 6 +-- test/test_math.c | 78 +++++++++++++++++++++++++++++----- 6 files changed, 120 insertions(+), 56 deletions(-) diff --git a/include/box2d/box2d.h b/include/box2d/box2d.h index 240df1833..a14d1e5ee 100644 --- a/include/box2d/box2d.h +++ b/include/box2d/box2d.h @@ -54,7 +54,11 @@ B2_API b2ContactEvents b2World_GetContactEvents( b2WorldId worldId ); B2_API b2TreeStats b2World_OverlapAABB( b2WorldId worldId, b2AABB aabb, b2QueryFilter filter, b2OverlapResultFcn* fcn, void* context ); -/// Overlap test for for all shapes that overlap the provided circle +/// Overlap test for for all shapes that overlap the provided point. +B2_API b2TreeStats b2World_OverlapPoint( b2WorldId worldId, b2Vec2 point, b2Transform transform, + b2QueryFilter filter, b2OverlapResultFcn* fcn, void* context ); + +/// Overlap test for for all shapes that overlap the provided circle. A zero radius may be used for a point query. B2_API b2TreeStats b2World_OverlapCircle( b2WorldId worldId, const b2Circle* circle, b2Transform transform, b2QueryFilter filter, b2OverlapResultFcn* fcn, void* context ); diff --git a/include/box2d/math_functions.h b/include/box2d/math_functions.h index a5218c172..0b0c01d6d 100644 --- a/include/box2d/math_functions.h +++ b/include/box2d/math_functions.h @@ -79,6 +79,7 @@ static const b2Mat22 b2Mat22_zero = { { 0.0f, 0.0f }, { 0.0f, 0.0f } }; /// Compute an approximate arctangent in the range [-pi, pi] /// This is hand coded for cross platform determinism. The atan2f /// function in the standard library is not cross platform deterministic. +/// Accurate to around 0.0023 degrees B2_API float b2Atan2( float y, float x ); /// @return the minimum of two floats diff --git a/src/math_functions.c b/src/math_functions.c index 039ace249..60f62a16b 100644 --- a/src/math_functions.c +++ b/src/math_functions.c @@ -52,51 +52,47 @@ bool b2Rot_IsValid( b2Rot q ) return b2IsNormalized( q ); } -// https://mazzo.li/posts/vectorized-atan2.html -static inline float b2Atan( float x ) -{ - float a1 = 0.99997726f; - float a3 = -0.33262347f; - float a5 = 0.19354346f; - float a7 = -0.11643287f; - float a9 = 0.05265332f; - float a11 = -0.01172120f; - - float x2 = x * x; - return x * ( a1 + x2 * ( a3 + x2 * ( a5 + x2 * ( a7 + x2 * ( a9 + x2 * a11 ) ) ) ) ); -} - -// I tested atan2f and got different results on Apple Clang (Arm) than MSVC (x64). +// https://stackoverflow.com/questions/46210708/atan2-approximation-with-11bits-in-mantissa-on-x86with-sse2-and-armwith-vfpv4 float b2Atan2( float y, float x ) { - float pi = b2_pi; - float halfPi = 0.5f * b2_pi; - - bool swap = b2AbsFloat( x ) < b2AbsFloat( y ); - float atanInput = ( swap ? x : y ) / ( swap ? y : x ); - - // Approximate atan - float res = b2Atan( atanInput ); - - // If swapped, adjust atan output - res = swap ? ( atanInput >= 0.0f ? halfPi : -halfPi ) - res : res; - // Adjust quadrants - if ( x >= 0.0f && y >= 0.0f ) + // Added check for (0,0) to match atan2f and avoid NaN + if (x == 0.0f && y == 0.0f) { - } // 1st quadrant - else if ( x < 0.0f && y >= 0.0f ) + return 0.0f; + } + + float ax = b2AbsFloat( x ); + float ay = b2AbsFloat( y ); + float mx = b2MaxFloat( ay, ax ); + float mn = b2MinFloat( ay, ax ); + float a = mn / mx; + + // Minimax polynomial approximation to atan(a) on [0,1] + float s = a * a; + float c = s * a; + float q = s * s; + float r = 0.024840285f * q + 0.18681418f; + float t = -0.094097948f * q - 0.33213072f; + r = r * s + t; + r = r * c + a; + + // Map to full circle + if ( ay > ax ) { - res = pi + res; - } // 2nd quadrant - else if ( x < 0.0f && y < 0.0f ) + r = 1.57079637f - r; + } + + if ( x < 0 ) { - res = -pi + res; - } // 3rd quadrant - else if ( x >= 0.0f && y < 0.0f ) + r = 3.14159274f - r; + } + + if ( y < 0 ) { - } // 4th quadrant + r = -r; + } - return res; + return r; } // Approximate cosine and sine for determinism. In my testing cosf and sinf produced @@ -113,13 +109,13 @@ b2CosSin b2ComputeCosSin( float angle ) b2Rot q; // cosine needs angle in [-pi/2, pi/2] - if (x < -0.5f * b2_pi) + if ( x < -0.5f * b2_pi ) { float y = x + b2_pi; float y2 = y * y; q.c = -( pi2 - 4.0f * y2 ) / ( pi2 + y2 ); } - else if (x > 0.5f * b2_pi) + else if ( x > 0.5f * b2_pi ) { float y = x - b2_pi; float y2 = y * y; @@ -132,7 +128,7 @@ b2CosSin b2ComputeCosSin( float angle ) } // sine needs angle in [0, pi] - if (x < 0.0f) + if ( x < 0.0f ) { float y = x + b2_pi; q.s = -16.0f * y * ( b2_pi - y ) / ( 5.0f * pi2 - 4.0f * y * ( b2_pi - y ) ); diff --git a/src/world.c b/src/world.c index f9226810c..4a8d448cf 100644 --- a/src/world.c +++ b/src/world.c @@ -1962,6 +1962,13 @@ static bool TreeOverlapCallback( int proxyId, int shapeId, void* context ) return result; } +b2TreeStats b2World_OverlapPoint( b2WorldId worldId, b2Vec2 point, b2Transform transform, b2QueryFilter filter, + b2OverlapResultFcn* fcn, void* context ) +{ + b2Circle circle = { point, 0.0f }; + return b2World_OverlapCircle( worldId, &circle, transform, filter, fcn, context ); +} + b2TreeStats b2World_OverlapCircle( b2WorldId worldId, const b2Circle* circle, b2Transform transform, b2QueryFilter filter, b2OverlapResultFcn* fcn, void* context ) { diff --git a/test/test_determinism.c b/test/test_determinism.c index b5cfe4f83..ffcfe0729 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -330,10 +330,8 @@ static int CrossPlatformTest(void) } ENSURE( stepCount < maxSteps ); - - // sleep step = 310, hash = 0x5e70e5fe - ENSURE( sleepStep == 310 ); - ENSURE( hash == 0x5e70e5fe ); + ENSURE( sleepStep == 313 ); + ENSURE( hash == 0x1fc667fa ); free( bodies ); diff --git a/test/test_math.c b/test/test_math.c index fb1460a6a..1878e7323 100644 --- a/test/test_math.c +++ b/test/test_math.c @@ -8,31 +8,89 @@ #include #include +// 0.0023 degrees +#define ATAN_TOL 0.00004f + int MathTest( void ) { - for (float x = -10.0f * b2_pi; x < 10.0f * b2_pi; x += 0.01f ) + for ( float t = -10.0f; t < 10.0f; t += 0.01f ) { - b2Rot r = b2MakeRot( x ); - float c = cosf( x ); - float s = sinf( x ); + float angle = b2_pi * t; + b2Rot r = b2MakeRot( angle ); + float c = cosf( angle ); + float s = sinf( angle ); - // The cosine and sine approximations are accurate to about 0.1 degrees (0.002 radians) - //printf( "%g %g\n", r.c - c, r.s - s ); + // The cosine and sine approximations are accurate to about 0.1 degrees (0.002 radians) + // printf( "%g %g\n", r.c - c, r.s - s ); ENSURE_SMALL( r.c - c, 0.002f ); ENSURE_SMALL( r.s - s, 0.002f ); - float xn = b2UnwindLargeAngle( x ); + float xn = b2UnwindLargeAngle( angle ); float a = b2Atan2( s, c ); + ENSURE( b2IsValid( a ) ); + float diff = b2AbsFloat( a - xn ); - + // The two results can be off by 360 degrees (-pi and pi) - if (diff > b2_pi) + if ( diff > b2_pi ) { diff -= 2.0f * b2_pi; } // The approximate atan2 is quite accurate - ENSURE_SMALL( diff, 1e-5f ); + ENSURE_SMALL( diff, ATAN_TOL ); + } + + for ( float y = -1.0f; y <= 1.0f; y += 0.01f ) + { + for ( float x = -1.0f; x <= 1.0f; x += 0.01f ) + { + float a1 = b2Atan2( y, x ); + float a2 = atan2f( y, x ); + float diff = b2AbsFloat( a1 - a2 ); + ENSURE( b2IsValid( a1 ) ); + ENSURE_SMALL( diff, ATAN_TOL ); + } + } + + { + float a1 = b2Atan2( 1.0f, 0.0f ); + float a2 = atan2f( 1.0f, 0.0f ); + float diff = b2AbsFloat( a1 - a2 ); + ENSURE( b2IsValid( a1 ) ); + ENSURE_SMALL( diff, ATAN_TOL ); + } + + { + float a1 = b2Atan2( -1.0f, 0.0f ); + float a2 = atan2f( -1.0f, 0.0f ); + float diff = b2AbsFloat( a1 - a2 ); + ENSURE( b2IsValid( a1 ) ); + ENSURE_SMALL( diff, ATAN_TOL ); + } + + { + float a1 = b2Atan2( 0.0f, 1.0f ); + float a2 = atan2f( 0.0f, 1.0f ); + float diff = b2AbsFloat( a1 - a2 ); + ENSURE( b2IsValid( a1 ) ); + ENSURE_SMALL( diff, ATAN_TOL ); + } + + { + float a1 = b2Atan2( 0.0f, -1.0f ); + float a2 = atan2f( 0.0f, -1.0f ); + float diff = b2AbsFloat( a1 - a2 ); + ENSURE( b2IsValid( a1 ) ); + ENSURE_SMALL( diff, ATAN_TOL ); + } + + { + float a1 = b2Atan2( 0.0f, 0.0f ); + float a2 = atan2f( 0.0f, 0.0f ); + float diff = b2AbsFloat( a1 - a2 ); + ENSURE( b2IsValid( a1 ) ); + ENSURE_SMALL( diff, ATAN_TOL ); } b2Vec2 zero = b2Vec2_zero;