Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix atan2 and add more unit tests #821

Merged
merged 1 commit into from
Oct 11, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 5 additions & 1 deletion include/box2d/box2d.h
Original file line number Diff line number Diff line change
Expand Up @@ -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 );

Expand Down
1 change: 1 addition & 0 deletions include/box2d/math_functions.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
78 changes: 37 additions & 41 deletions src/math_functions.c
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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;
Expand All @@ -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 ) );
Expand Down
7 changes: 7 additions & 0 deletions src/world.c
Original file line number Diff line number Diff line change
Expand Up @@ -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 )
{
Expand Down
6 changes: 2 additions & 4 deletions test/test_determinism.c
Original file line number Diff line number Diff line change
Expand Up @@ -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 );

Expand Down
78 changes: 68 additions & 10 deletions test/test_math.c
Original file line number Diff line number Diff line change
Expand Up @@ -8,31 +8,89 @@
#include <float.h>
#include <stdio.h>

// 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;
Expand Down