fix atan2 and add more unit tests

added point overlap test

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 );
 

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

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 ) );

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 )
 {

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 );
 

test/test_math.c

@@ -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;