Skip to content

Commit

Permalink
back to sqrtf
Browse files Browse the repository at this point in the history
  • Loading branch information
@erincatto
erincatto committed Aug 28, 2024
1 parent a524645 commit 8e19813
Show file tree
Hide file tree
Showing 7 changed files with 36 additions and 483 deletions.
83 changes: 14 additions & 69 deletions include/box2d/math_functions.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,12 +9,6 @@
#include <math.h>
#include <stdbool.h>

#if defined( B2_SIMD_AVX2 ) || defined( B2_SIMD_SSE2 )
#include <emmintrin.h>
#elif defined( B2_SIMD_NEON )
#include <arm_neon.h>
#endif

/**
* @defgroup math Math
* @brief Vector math types and functions
Expand Down Expand Up @@ -247,49 +241,24 @@ B2_INLINE b2Vec2 b2Clamp( b2Vec2 v, b2Vec2 a, b2Vec2 b )
return c;
}

#if 1
B2_INLINE float b2Sqrt( float x )
{
return sqrtf( x );

//#if defined( B2_SIMD_AVX2 ) || defined( B2_SIMD_SSE2 )
// return _mm_cvtss_f32( _mm_sqrt_ss( _mm_set1_ps( x ) ) );
//#elif defined( B2_SIMD_NEON )
// float32x4_t v = vdupq_n_f32( x );
// return vgetq_lane_f32( vsqrtq_f32( v ), 0 );
//#else
// return sqrtf( x );
//#endif
}

/// Get the length of this vector (the norm)
B2_INLINE float b2Length( b2Vec2 v )
{
return b2Sqrt( v.x * v.x + v.y * v.y );
return sqrtf( v.x * v.x + v.y * v.y );
}

/// Get the distance between two points
B2_INLINE float b2Distance( b2Vec2 a, b2Vec2 b )
{
float dx = b.x - a.x;
float dy = b.y - a.y;
return b2Sqrt( dx * dx + dy * dy );
return sqrtf( dx * dx + dy * dy );
}

/// Convert a vector into a unit vector if possible, otherwise returns the zero vector.
B2_INLINE b2Vec2 b2Normalize( b2Vec2 v )
{
float length = b2Sqrt( v.x * v.x + v.y * v.y );
if ( length < FLT_EPSILON )
{
return b2Vec2_zero;
}

float invLength = 1.0f / length;
b2Vec2 n = { invLength * v.x, invLength * v.y };
return n;
}

B2_INLINE b2Vec2 b2NormalizeChecked( b2Vec2 v )
{
float length = b2Length( v );
float length = sqrtf( v.x * v.x + v.y * v.y );
if ( length < FLT_EPSILON )
{
return b2Vec2_zero;
Expand All @@ -300,6 +269,8 @@ B2_INLINE b2Vec2 b2NormalizeChecked( b2Vec2 v )
return n;
}

/// Convert a vector into a unit vector if possible, otherwise returns the zero vector. Also
/// outputs the length.
B2_INLINE b2Vec2 b2GetLengthAndNormalize( float* length, b2Vec2 v )
{
*length = b2Length( v );
Expand All @@ -313,57 +284,31 @@ B2_INLINE b2Vec2 b2GetLengthAndNormalize( float* length, b2Vec2 v )
return n;
}

/// Normalize rotation
B2_INLINE b2Rot b2NormalizeRot( b2Rot q )
{
float mag = b2Sqrt( q.s * q.s + q.c * q.c );
float mag = sqrtf( q.s * q.s + q.c * q.c );
float invMag = mag > 0.0 ? 1.0f / mag : 0.0f;
b2Rot qn = { q.c * invMag, q.s * invMag };
return qn;
}

/// Integration rotation from angular velocity
/// @param q1 initial rotation
/// @param deltaAngle the angular displacement in radians
B2_INLINE b2Rot b2IntegrateRotation( b2Rot q1, float deltaAngle )
{
// dc/dt = -omega * sin(t)
// ds/dt = omega * cos(t)
// c2 = c1 - omega * h * s1
// s2 = s1 + omega * h * c1
b2Rot q2 = { q1.c - deltaAngle * q1.s, q1.s + deltaAngle * q1.c };
float mag = b2Sqrt( q2.s * q2.s + q2.c * q2.c );
float mag = sqrtf( q2.s * q2.s + q2.c * q2.c );
float invMag = mag > 0.0 ? 1.0f / mag : 0.0f;
b2Rot qn = { q2.c * invMag, q2.s * invMag };
return qn;
}

#else

B2_API float b2Sqrt( float x );

/// Convert a vector into a unit vector if possible, otherwise returns the zero vector.
B2_API b2Vec2 b2Normalize( b2Vec2 v );

/// Convert a vector into a unit vector if possible, otherwise asserts.
B2_API b2Vec2 b2NormalizeChecked( b2Vec2 v );

/// Convert a vector into a unit vector if possible, otherwise returns the zero vector. Also
/// outputs the length.
B2_API b2Vec2 b2GetLengthAndNormalize( float* length, b2Vec2 v );

/// Get the length of this vector (the norm)
B2_API float b2Length( b2Vec2 v );

/// Get the distance between two points
B2_API float b2Distance( b2Vec2 a, b2Vec2 b );

/// Integration rotation from angular velocity
/// @param q1 initial rotation
/// @param deltaAngle the angular displacement in radians
B2_API b2Rot b2IntegrateRotation( b2Rot q1, float deltaAngle );

/// Normalize rotation
B2_API b2Rot b2NormalizeRot( b2Rot q );

#endif

/// Get the length squared of this vector
B2_INLINE float b2LengthSquared( b2Vec2 v )
{
Expand Down
2 changes: 1 addition & 1 deletion src/contact.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@
// For example, anything slides on ice.
static inline float b2MixFriction( float friction1, float friction2 )
{
return b2Sqrt( friction1 * friction2 );
return sqrtf( friction1 * friction2 );
}

// Restitution mixing law. The idea is allow for anything to bounce off an inelastic surface.
Expand Down
2 changes: 1 addition & 1 deletion src/geometry.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -515,7 +515,7 @@ b2CastOutput b2RayCastCircle( const b2RayCastInput* input, const b2Circle* shape
}

// Pythagorus
float h = b2Sqrt( rr - cc );
float h = sqrtf( rr - cc );

float fraction = t - h;

Expand Down
12 changes: 7 additions & 5 deletions src/manifold.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,9 @@ static b2Polygon b2MakeCapsule( b2Vec2 p1, b2Vec2 p2, float radius )
shape.vertices[1] = p2;
shape.centroid = b2Lerp( p1, p2, 0.5f );

b2Vec2 axis = b2NormalizeChecked( b2Sub( p2, p1 ) );
b2Vec2 d = b2Sub( p2, p1 );
B2_ASSERT( b2LengthSquared( d ) > FLT_EPSILON );
b2Vec2 axis = b2Normalize(d);
b2Vec2 normal = b2RightPerp( axis );

shape.normals[0] = normal;
Expand Down Expand Up @@ -582,7 +584,7 @@ b2Manifold b2CollidePolygons( const b2Polygon* polygonA, b2Transform xfA, const
// v11 - v21
b2Vec2 normal = b2Sub( v21, v11 );
B2_ASSERT( result.distanceSquared > 0.0f );
float distance = b2Sqrt( result.distanceSquared );
float distance = sqrtf( result.distanceSquared );
if ( distance > b2_speculativeDistance + radius )
{
return manifold;
Expand All @@ -605,7 +607,7 @@ b2Manifold b2CollidePolygons( const b2Polygon* polygonA, b2Transform xfA, const
// v11 - v22
b2Vec2 normal = b2Sub( v22, v11 );
B2_ASSERT( result.distanceSquared > 0.0f );
float distance = b2Sqrt( result.distanceSquared );
float distance = sqrtf( result.distanceSquared );
if ( distance > b2_speculativeDistance + radius )
{
return manifold;
Expand All @@ -628,7 +630,7 @@ b2Manifold b2CollidePolygons( const b2Polygon* polygonA, b2Transform xfA, const
// v12 - v21
b2Vec2 normal = b2Sub( v21, v12 );
B2_ASSERT( result.distanceSquared > 0.0f );
float distance = b2Sqrt( result.distanceSquared );
float distance = sqrtf( result.distanceSquared );
if ( distance > b2_speculativeDistance + radius )
{
return manifold;
Expand All @@ -651,7 +653,7 @@ b2Manifold b2CollidePolygons( const b2Polygon* polygonA, b2Transform xfA, const
// v12 - v22
b2Vec2 normal = b2Sub( v22, v12 );
B2_ASSERT( result.distanceSquared > 0.0f );
float distance = b2Sqrt( result.distanceSquared );
float distance = sqrtf( result.distanceSquared );
if ( distance > b2_speculativeDistance + radius )
{
return manifold;
Expand Down
Loading

0 comments on commit 8e19813

Please sign in to comment.