Doxygen comments (#12)

- organized headers.
- added comments to headers.
- fixed a solver determinism bug.
- removed some un-used math functions
This commit is contained in:
Erin Catto 2026-06-10 13:21:14 -07:00 • committed by GitHub
commit d8fbeb444e
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
53 changed files with 8973 additions and 1812 deletions

View file

@ -17,12 +17,26 @@
* @{
*/
/// https://en.wikipedia.org/wiki/Pi
#define B3_PI 3.14159265359f
/// Convenience macro to convert from degrees to radians.
#define B3_DEG_TO_RAD 0.01745329251f
/// Convenience macro to convert from radians to degrees.
#define B3_RAD_TO_DEG 57.2957795131f
/// Minimum scale used for scaling collision meshes, etc.
#define B3_MIN_SCALE 0.01f
/// A 2D vector.
typedef struct b3Vec2
{
float x;
float y;
} b3Vec2;
/// A 3D vector.
typedef struct b3Vec3
{
float x;
@ -30,8 +44,8 @@ typedef struct b3Vec3
float z;
} b3Vec3;
/// Cosine and sine pair
/// This uses a custom implementation designed for cross-platform determinism
/// Cosine and sine pair.
/// This uses a custom implementation designed for cross-platform determinism.
typedef struct b3CosSin
{
/// cosine and sine
@ -39,29 +53,34 @@ typedef struct b3CosSin
float sine;
} b3CosSin;
/// A quaternion.
typedef struct b3Quat
{
b3Vec3 v;
float s;
} b3Quat;
/// A rigid transform.
typedef struct b3Transform
{
b3Vec3 p;
b3Quat q;
} b3Transform;
/// A 3x3 matrix.
typedef struct b3Matrix3
{
b3Vec3 cx, cy, cz;
} b3Matrix3;
/// Axis aligned bounding box.
typedef struct b3AABB
{
b3Vec3 lowerBound;
b3Vec3 upperBound;
} b3AABB;
/// A plane.
/// separation = dot(normal, point) - offset
typedef struct b3Plane
{
@ -69,87 +88,70 @@ typedef struct b3Plane
float offset;
} b3Plane;
/**@}*/
/**
* @addtogroup math
* @{
*/
#define B3_PI 3.141592654f
#define B3_DEG_TO_RAD 0.01745329251f
#define B3_RAD_TO_DEG 57.2957795131f
/// Minimum scale used for scaling collision meshes, etc.
#define B3_MIN_SCALE 0.01f
static const b3Vec3 b3Vec3_zero = { 0.0f, 0.0f, 0.0f };
static const b3Vec3 b3Vec3_one = { 1.0f, 1.0f, 1.0f };
static const b3Vec3 b3Vec3_axisX = { 1.0f, 0.0f, 0.0f };
static const b3Vec3 b3Vec3_axisY = { 0.0f, 1.0f, 0.0f };
static const b3Vec3 b3Vec3_axisZ = { 0.0f, 0.0f, 1.0f };
static const b3Quat b3Quat_identity = { { 0.0f, 0.0f, 0.0f }, 1.0f };
static const b3Transform b3Transform_identity = { { 0.0f, 0.0f, 0.0f }, { { 0.0f, 0.0f, 0.0f }, 1.0f } };
static const b3Matrix3 b3Mat3_zero = {
{ 0.0f, 0.0f, 0.0f },
{ 0.0f, 0.0f, 0.0f },
{ 0.0f, 0.0f, 0.0f },
};
static const b3Matrix3 b3Mat3_identity = {
{ 1.0f, 0.0f, 0.0f },
{ 0.0f, 1.0f, 0.0f },
{ 0.0f, 0.0f, 1.0f },
};
/// @return the minimum of two integers
/// @return the minimum of two integers.
B3_INLINE int b3MinInt( int a, int b )
{
return a < b ? a : b;
}
/// @return the maximum of two integers
/// @return the maximum of two integers.
B3_INLINE int b3MaxInt( int a, int b )
{
return a > b ? a : b;
}
/// @return an integer clamped between a lower and upper bound
/// @return an integer clamped between a lower and upper bound.
B3_INLINE int b3ClampInt( int a, int lower, int upper )
{
return a < lower ? lower : ( upper < a ? upper : a );
}
/// Is this float valid (finite and not NaN)
/// @return is this float valid (finite and not NaN).
B3_API bool b3IsValidFloat( float a );
/// @return the absolute value of a float
/// @return the absolute value of a float.
B3_INLINE float b3AbsFloat( float a )
{
return a < 0 ? -a : a;
}
/// @return the minimum of two floats
/// @return the minimum of two floats.
B3_INLINE float b3MinFloat( float a, float b )
{
return a < b ? a : b;
}
/// @return the maximum of two floats
/// @return the maximum of two floats.
B3_INLINE float b3MaxFloat( float a, float b )
{
return a > b ? a : b;
}
/// @return a float clamped between a lower and upper bound
/// @return a float clamped between a lower and upper bound.
B3_INLINE float b3ClampFloat( float a, float lower, float upper )
{
return a < lower ? lower : ( upper < a ? upper : a );
}
/// Interpolate a scalar
/// Interpolate a scalar.
B3_INLINE float b3LerpFloat( float a, float b, float alpha )
{
return ( 1.0f - alpha ) * a + alpha * b;
@ -158,86 +160,91 @@ B3_INLINE float b3LerpFloat( float a, float b, float alpha )
/// 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
/// Accurate to around 0.0023 degrees.
B3_API float b3Atan2( float y, float x );
/// Compute the cosine and sine of an angle in radians. Implemented
/// for cross-platform determinism.
B3_API b3CosSin b3ComputeCosSin( float radians );
/// @deprecated
B3_INLINE float b3Sin( float radians )
{
b3CosSin cs = b3ComputeCosSin( radians );
return cs.sine;
}
/// @deprecated
B3_INLINE float b3Cos( float radians )
{
b3CosSin cs = b3ComputeCosSin( radians );
return cs.cosine;
}
/// Convert any angle into the range [-pi, pi]
/// Convert any angle into the range [-pi, pi].
B3_INLINE float b3UnwindAngle( float radians )
{
// Assuming this is deterministic
return remainderf( radians, 2.0f * B3_PI );
}
/// Vector addition
/// Vector addition.
B3_INLINE b3Vec3 b3Add( b3Vec3 a, b3Vec3 b )
{
return B3_LITERAL( b3Vec3 ){ a.x + b.x, a.y + b.y, a.z + b.z };
}
/// Vector subtraction
/// Vector subtraction.
B3_INLINE b3Vec3 b3Sub( b3Vec3 a, b3Vec3 b )
{
return B3_LITERAL( b3Vec3 ){ a.x - b.x, a.y - b.y, a.z - b.z };
}
/// Vector component-wise multiplication
/// Vector component-wise multiplication.
B3_INLINE b3Vec3 b3Mul( b3Vec3 a, b3Vec3 b )
{
return B3_LITERAL( b3Vec3 ){ a.x * b.x, a.y * b.y, a.z * b.z };
}
/// Vector negation
/// Vector negation.
B3_INLINE b3Vec3 b3Neg( b3Vec3 a )
{
return B3_LITERAL( b3Vec3 ){ -a.x, -a.y, -a.z };
}
/// Vector dot product
/// Vector dot product.
B3_INLINE float b3Dot( b3Vec3 a, b3Vec3 b )
{
return a.x * b.x + a.y * b.y + a.z * b.z;
}
/// Vector length
/// Vector length.
B3_INLINE float b3Length( b3Vec3 v )
{
return sqrtf( b3Dot( v, v ) );
}
/// Vector length squared
/// Vector length squared.
B3_INLINE float b3LengthSquared( b3Vec3 a )
{
return a.x * a.x + a.y * a.y + a.z * a.z;
}
B3_INLINE float b3Distance( b3Vec3 v1, b3Vec3 v2 )
/// Distance between two points.
B3_INLINE float b3Distance( b3Vec3 a, b3Vec3 b )
{
b3Vec3 dv = { v2.x - v1.x, v2.y - v1.y, v2.z - v1.z };
b3Vec3 dv = { b.x - a.x, b.y - a.y, b.z - a.z };
return b3Length( dv );
}
B3_INLINE float b3DistanceSquared( b3Vec3 v1, b3Vec3 v2 )
/// Squared distance between two points.
B3_INLINE float b3DistanceSquared( b3Vec3 a, b3Vec3 b )
{
b3Vec3 dv = { v2.x - v1.x, v2.y - v1.y, v2.z - v1.z };
b3Vec3 dv = { b.x - a.x, b.y - a.y, b.z - a.z };
return dv.x * dv.x + dv.y * dv.y + dv.z * dv.z;
}
/// Normalize a vector. Returns a zero vector if the input vector is very small.
B3_INLINE b3Vec3 b3Normalize( b3Vec3 a )
{
float lengthSquared = a.x * a.x + a.y * a.y + a.z * a.z;
@ -252,37 +259,41 @@ B3_INLINE b3Vec3 b3Normalize( b3Vec3 a )
return B3_LITERAL( b3Vec3 ){ 0.0f, 0.0f, 0.0f };
}
B3_INLINE b3Vec3 b3GetLengthAndNormalize( float* length, b3Vec3 v )
/// Normalize a vector and return the length. Returns a zero vector
/// if the input is very small.
B3_INLINE b3Vec3 b3GetLengthAndNormalize( float* length, b3Vec3 a )
{
*length = b3Length( v );
*length = b3Length( a );
if ( *length < FLT_EPSILON )
{
return b3Vec3_zero;
}
float invLength = 1.0f / *length;
b3Vec3 n = { invLength * v.x, invLength * v.y, invLength * v.z };
b3Vec3 n = { invLength * a.x, invLength * a.y, invLength * a.z };
return n;
}
B3_INLINE b3Vec3 b3Perp( b3Vec3 v )
/// Get a unit vector that is perpendicular to the supplied vector.
B3_INLINE b3Vec3 b3Perp( b3Vec3 a )
{
// Suppose vector a has all equal components and is a unit vector: a = (s, s, s)
// Then 3*s*s = 1, s = sqrt(1/3) = 0.57735. This means that at least one component
// of a unit vector must be greater or equal to 0.57735.
b3Vec3 p;
if ( v.x < -0.5f || 0.5f < v.x )
if ( a.x < -0.5f || 0.5f < a.x )
{
p = B3_LITERAL( b3Vec3 ){ v.y, -v.x, 0.0f };
p = B3_LITERAL( b3Vec3 ){ a.y, -a.x, 0.0f };
}
else
{
p = B3_LITERAL( b3Vec3 ){ 0.0f, v.z, -v.y };
p = B3_LITERAL( b3Vec3 ){ 0.0f, a.z, -a.y };
}
return b3Normalize( p );
}
/// Is a vector normalized? In other words, does it have unit length?
B3_INLINE bool b3IsNormalized( b3Vec3 a )
{
float aa = b3Dot( a, a );
@ -301,12 +312,13 @@ B3_INLINE b3Vec3 b3MulSub( b3Vec3 a, float s, b3Vec3 b )
return B3_LITERAL( b3Vec3 ){ a.x - s * b.x, a.y - s * b.y, a.z - s * b.z };
}
/// a + s * b
/// s * a
B3_INLINE b3Vec3 b3MulSV( float s, b3Vec3 a )
{
return B3_LITERAL( b3Vec3 ){ s * a.x, s * a.y, s * a.z };
}
/// https://en.wikipedia.org/wiki/Cross_product
B3_INLINE b3Vec3 b3Cross( b3Vec3 a, b3Vec3 b )
{
b3Vec3 c;
@ -316,6 +328,7 @@ B3_INLINE b3Vec3 b3Cross( b3Vec3 a, b3Vec3 b )
return c;
}
/// Linearly interpolate between two vectors.
B3_INLINE b3Vec3 b3Lerp( b3Vec3 a, b3Vec3 b, float alpha )
{
B3_ASSERT( 0.0f <= alpha && alpha <= 1.0f );
@ -328,6 +341,7 @@ B3_INLINE b3Vec3 b3Lerp( b3Vec3 a, b3Vec3 b, float alpha )
return c;
}
/// Blend two vectors: s * a + t * b
B3_INLINE b3Vec3 b3Blend2( float s, b3Vec3 a, float t, b3Vec3 b )
{
b3Vec3 d = {
@ -338,16 +352,7 @@ B3_INLINE b3Vec3 b3Blend2( float s, b3Vec3 a, float t, b3Vec3 b )
return d;
}
B3_INLINE b3Vec3 b3Blend3( float s, b3Vec3 a, float t, b3Vec3 b, float u, b3Vec3 c )
{
b3Vec3 d = {
s * a.x + t * b.x + u * c.x,
s * a.y + t * b.y + u * c.y,
s * a.z + t * b.z + u * c.z,
};
return d;
}
/// Component-wise absolute value.
B3_INLINE b3Vec3 b3Abs( b3Vec3 a )
{
return B3_LITERAL( b3Vec3 ){
@ -357,7 +362,7 @@ B3_INLINE b3Vec3 b3Abs( b3Vec3 a )
};
}
// -1 or 1 (1 if zero)
/// Component-wise -1 or 1 (1 if zero).
B3_INLINE b3Vec3 b3Sign( b3Vec3 a )
{
return B3_LITERAL( b3Vec3 ){
@ -367,6 +372,7 @@ B3_INLINE b3Vec3 b3Sign( b3Vec3 a )
};
}
/// Component-wise minimum value.
B3_INLINE b3Vec3 b3Min( b3Vec3 a, b3Vec3 b )
{
return B3_LITERAL( b3Vec3 ){
@ -376,6 +382,7 @@ B3_INLINE b3Vec3 b3Min( b3Vec3 a, b3Vec3 b )
};
}
/// Component-wise maximum value.
B3_INLINE b3Vec3 b3Max( b3Vec3 a, b3Vec3 b )
{
return B3_LITERAL( b3Vec3 ){
@ -385,6 +392,7 @@ B3_INLINE b3Vec3 b3Max( b3Vec3 a, b3Vec3 b )
};
}
/// Component-wise clamped value.
B3_INLINE b3Vec3 b3Clamp( b3Vec3 a, b3Vec3 lower, b3Vec3 upper )
{
b3Vec3 b;
@ -394,8 +402,8 @@ B3_INLINE b3Vec3 b3Clamp( b3Vec3 a, b3Vec3 lower, b3Vec3 upper )
return b;
}
// Create a safe scaling value for scaling collision. This allows
// negative scale, but keeps scale sufficiently far from zero.
/// Create a safe scaling value for scaling collision. This allows
/// negative scale, but keeps scale sufficiently far from zero.
B3_INLINE b3Vec3 b3SafeScale( b3Vec3 a )
{
b3Vec3 absScale = b3Abs( a );
@ -404,13 +412,14 @@ B3_INLINE b3Vec3 b3SafeScale( b3Vec3 a )
return safeScale;
}
/// Does the supplied quaternion have unit length?
B3_INLINE bool b3IsNormalizedQuat( b3Quat q )
{
float qq = q.v.x * q.v.x + q.v.y * q.v.y + q.v.z * q.v.z + q.s * q.s;
return 1.0f - 20.0f * FLT_EPSILON < qq && qq < 1.0f + 20.0f * FLT_EPSILON;
}
/// Rotate a vector
/// Rotate a vector.
B3_INLINE b3Vec3 b3RotateVector( b3Quat q, b3Vec3 v )
{
// v + 2 * cross(q.v, cross(q.v, v) + q.s * v)
@ -421,7 +430,7 @@ B3_INLINE b3Vec3 b3RotateVector( b3Quat q, b3Vec3 v )
return b3MulAdd( v, 2.0f, t3 );
}
/// Inverse rotate a vector
/// Inverse rotate a vector.
B3_INLINE b3Vec3 b3InvRotateVector( b3Quat q, b3Vec3 v )
{
// v + 2 * cross(q.v, cross(q.v, v) - q.s * v)
@ -432,11 +441,13 @@ B3_INLINE b3Vec3 b3InvRotateVector( b3Quat q, b3Vec3 v )
return b3MulAdd( v, 2.0f, t3 );
}
/// Compute dot product of two quaternions. Useful for polarity tests.
B3_INLINE float b3DotQuat( b3Quat a, b3Quat b )
{
return a.v.x * b.v.x + a.v.y * b.v.y + a.v.z * b.v.z + a.s * b.s;
}
/// Multiply two quaternions.
B3_INLINE b3Quat b3MulQuat( b3Quat q1, b3Quat q2 )
{
b3Vec3 t1 = b3Cross( q1.v, q2.v );
@ -446,7 +457,8 @@ B3_INLINE b3Quat b3MulQuat( b3Quat q1, b3Quat q2 )
return q;
}
// inv(q1) * q2
/// Compute a relative quaternion.
/// inv(q1) * q2
B3_INLINE b3Quat b3InvMulQuat( b3Quat q1, b3Quat q2 )
{
b3Vec3 t1 = b3Cross( q2.v, q1.v );
@ -456,16 +468,19 @@ B3_INLINE b3Quat b3InvMulQuat( b3Quat q1, b3Quat q2 )
return q;
}
/// Quaternion conjugate (cheap inverse).
B3_INLINE b3Quat b3Conjugate( b3Quat q )
{
return B3_LITERAL( b3Quat ){ { -q.v.x, -q.v.y, -q.v.z }, q.s };
}
/// Component-wise quaternion negation.
B3_INLINE b3Quat b3NegateQuat( b3Quat q )
{
return B3_LITERAL( b3Quat ){ { -q.v.x, -q.v.y, -q.v.z }, -q.s };
}
/// Normalize a quaternion.
B3_INLINE b3Quat b3NormalizeQuat( b3Quat q )
{
float lengthSq = b3DotQuat( q, q );
@ -479,6 +494,7 @@ B3_INLINE b3Quat b3NormalizeQuat( b3Quat q )
return b3Quat_identity;
}
/// Make a quaternion that is equivalent to rotating around an axis by a specified angle.
B3_INLINE b3Quat b3MakeQuatFromAxisAngle( b3Vec3 axis, float radians )
{
B3_ASSERT( b3IsNormalized( axis ) );
@ -487,6 +503,7 @@ B3_INLINE b3Quat b3MakeQuatFromAxisAngle( b3Vec3 axis, float radians )
return q;
}
/// Get the axis and angle from a quaternion. Assumes the quaternion is normalized.
B3_INLINE b3Vec3 b3GetAxisAngle( float* radians, b3Quat q )
{
float length = sqrtf( q.v.x * q.v.x + q.v.y * q.v.y + q.v.z * q.v.z );
@ -508,38 +525,16 @@ B3_INLINE float b3GetQuatAngle( b3Quat q )
return 2.0f * b3Atan2( length, q.s );
}
// Twist angle around the z-axis, used for twist limit and revolute angle limit
B3_INLINE float b3GetTwistAngle( b3Quat q )
{
// Account for polarity to keep the twist angle in range.
// This is simpler than asking the user to check polarity or unwinding.
float twist = q.s < 0.0f ? b3Atan2( -q.v.z, -q.s ) : b3Atan2( q.v.z, q.s );
twist *= 2.0f;
B3_ASSERT( -B3_PI <= twist && twist <= B3_PI );
return twist;
}
// Swing angle used for cone limit
B3_INLINE float b3GetSwingAngle( b3Quat q )
{
// Polarity should not matter because all terms are squared.
float x = sqrtf( q.v.z * q.v.z + q.s * q.s );
float y = sqrtf( q.v.x * q.v.x + q.v.y * q.v.y );
float swing = 2.0f * b3Atan2( y, x );
B3_ASSERT( 0.0f <= swing && swing <= B3_PI );
return swing;
}
/// Extract a quaternion from a rotation matrix
/// Extract a quaternion from a rotation matrix.
B3_API b3Quat b3MakeQuatFromMatrix( const b3Matrix3* m );
/// Find a quaternion that rotations one vector to another
/// Find a quaternion that rotates one vector to another.
B3_API b3Quat b3ComputeQuatBetweenUnitVectors( b3Vec3 v1, b3Vec3 v2 );
/// Linearly interpolate and normalize between two quaternions
B3_INLINE b3Quat b3NLerp( b3Quat q1, b3Quat q2, float alpha )
{
B3_ASSERT( 0.0f <= alpha && alpha <= 1.0f );
B3_VALIDATE( 0.0f <= alpha && alpha <= 1.0f );
if ( b3DotQuat( q1, q2 ) < 0.0f )
{
q1 = B3_LITERAL( b3Quat ){ { -q1.v.x, -q1.v.y, -q1.v.z }, -q1.s };
@ -574,6 +569,7 @@ B3_FORCE_INLINE b3Transform b3InvMulTransforms( b3Transform a, b3Transform b )
return out;
}
/// Get the inverse of a transform.
B3_INLINE b3Transform b3InvertTransform( b3Transform t )
{
b3Transform out;
@ -582,35 +578,26 @@ B3_INLINE b3Transform b3InvertTransform( b3Transform t )
return out;
}
B3_INLINE b3Transform b3NormalizeTransform( b3Transform t )
{
b3Transform out;
out.p = t.p;
out.q = b3NormalizeQuat( t.q );
return out;
}
/// Transform a point.
B3_INLINE b3Vec3 b3TransformPoint( b3Transform t, b3Vec3 v )
{
b3Vec3 rv = b3RotateVector( t.q, v );
return b3Add( rv, t.p );
}
/// Inverse transform a point.
B3_INLINE b3Vec3 b3InvTransformPoint( b3Transform t, b3Vec3 v )
{
return b3InvRotateVector( t.q, b3Sub( v, t.p ) );
}
B3_INLINE b3Matrix3 b3MakeDiagonalMatrix( float a, float b, float c )
{
return B3_LITERAL( b3Matrix3 ){ { a, 0.0f, 0.0f }, { 0.0f, b, 0.0f }, { 0.0f, 0.0f, c } };
}
/// Compute the determinant of a 3-by-3 matrix.
B3_INLINE float b3Det( b3Matrix3 m )
{
return b3Dot( m.cx, b3Cross( m.cy, m.cz ) );
}
/// Multiply a matrix times a column vector.
B3_INLINE b3Vec3 b3MulMV( b3Matrix3 m, b3Vec3 a )
{
b3Vec3 b = {
@ -621,6 +608,7 @@ B3_INLINE b3Vec3 b3MulMV( b3Matrix3 m, b3Vec3 a )
return b;
}
/// Negate a matrix.
B3_INLINE b3Matrix3 b3NegateMat3( b3Matrix3 a )
{
return B3_LITERAL( b3Matrix3 ){
@ -630,6 +618,8 @@ B3_INLINE b3Matrix3 b3NegateMat3( b3Matrix3 a )
};
}
/// Matrix addition.
/// @return a + b
B3_INLINE b3Matrix3 b3AddMM( b3Matrix3 a, b3Matrix3 b )
{
return B3_LITERAL( b3Matrix3 ){
@ -639,6 +629,8 @@ B3_INLINE b3Matrix3 b3AddMM( b3Matrix3 a, b3Matrix3 b )
};
}
/// Matrix subtraction.
/// @return a - b
B3_INLINE b3Matrix3 b3SubMM( b3Matrix3 a, b3Matrix3 b )
{
return B3_LITERAL( b3Matrix3 ){
@ -648,6 +640,7 @@ B3_INLINE b3Matrix3 b3SubMM( b3Matrix3 a, b3Matrix3 b )
};
}
/// Multiply a matrix by a scalar, component-wise.
B3_INLINE b3Matrix3 b3MulSM( float s, b3Matrix3 a )
{
return B3_LITERAL( b3Matrix3 ){
@ -657,6 +650,8 @@ B3_INLINE b3Matrix3 b3MulSM( float s, b3Matrix3 a )
};
}
/// Matrix multiplication.
/// @return a * b
B3_INLINE b3Matrix3 b3MulMM( b3Matrix3 a, b3Matrix3 b )
{
b3Matrix3 out;
@ -666,6 +661,7 @@ B3_INLINE b3Matrix3 b3MulMM( b3Matrix3 a, b3Matrix3 b )
return out;
}
/// Matrix transpose.
B3_INLINE b3Matrix3 b3Transpose( b3Matrix3 m )
{
b3Matrix3 out;
@ -676,6 +672,7 @@ B3_INLINE b3Matrix3 b3Transpose( b3Matrix3 m )
return out;
}
/// General matrix inverse.
B3_INLINE b3Matrix3 b3InvertMatrix( b3Matrix3 m )
{
float det = b3Det( m );
@ -693,21 +690,23 @@ B3_INLINE b3Matrix3 b3InvertMatrix( b3Matrix3 m )
return b3Mat3_zero;
}
B3_INLINE b3Vec3 b3Solve3( b3Matrix3 m, b3Vec3 x )
/// Solve a matrix equation.
/// @return inv(m) * a
B3_INLINE b3Vec3 b3Solve3( b3Matrix3 m, b3Vec3 a )
{
float det = b3Det( m );
if ( b3AbsFloat( det ) > 1000.0f * FLT_MIN )
{
float invDet = 1.0f / det;
b3Matrix3 a;
a.cx = b3Cross( m.cy, m.cz );
a.cy = b3Cross( m.cz, m.cx );
a.cz = b3Cross( m.cx, m.cy );
b3Matrix3 s;
s.cx = b3Cross( m.cy, m.cz );
s.cy = b3Cross( m.cz, m.cx );
s.cz = b3Cross( m.cx, m.cy );
b3Vec3 b = {
invDet * b3Dot( a.cx, x ),
invDet * b3Dot( a.cy, x ),
invDet * b3Dot( a.cz, x ),
invDet * b3Dot( s.cx, a ),
invDet * b3Dot( s.cy, a ),
invDet * b3Dot( s.cz, a ),
};
return b;
@ -716,6 +715,7 @@ B3_INLINE b3Vec3 b3Solve3( b3Matrix3 m, b3Vec3 x )
return b3Vec3_zero;
}
/// Invert a matrix.
B3_INLINE b3Matrix3 b3InvertT( b3Matrix3 m )
{
float det = b3Det( m );
@ -732,32 +732,7 @@ B3_INLINE b3Matrix3 b3InvertT( b3Matrix3 m )
return b3Mat3_zero;
}
B3_INLINE b3Matrix3 b3InvertSymmetric( b3Matrix3 m )
{
float det = b3Det( m );
if ( b3AbsFloat( det ) > 1000.0f * FLT_MIN )
{
float invDet = 1.0f / det;
b3Matrix3 out;
out.cx = b3MulSV( invDet, b3Cross( m.cy, m.cz ) );
out.cy = b3MulSV( invDet, b3Cross( m.cz, m.cx ) );
out.cz = b3MulSV( invDet, b3Cross( m.cx, m.cy ) );
return out;
}
return b3Mat3_zero;
}
B3_INLINE b3Matrix3 b3Skew( b3Vec3 v )
{
b3Matrix3 out;
out.cx = B3_LITERAL( b3Vec3 ){ 0, v.z, -v.y };
out.cy = B3_LITERAL( b3Vec3 ){ -v.z, 0, v.x };
out.cz = B3_LITERAL( b3Vec3 ){ v.y, -v.x, 0 };
return out;
}
/// Get the component-wise absolute value of a matrix.
B3_INLINE b3Matrix3 b3AbsMatrix3( b3Matrix3 m )
{
b3Matrix3 out;
@ -768,21 +743,9 @@ B3_INLINE b3Matrix3 b3AbsMatrix3( b3Matrix3 m )
return out;
}
B3_INLINE float b3Trace( b3Matrix3 m )
{
return m.cx.x + m.cy.y + m.cz.z;
}
B3_INLINE b3Matrix3 b3ScaleMatrix( float s, b3Matrix3 m )
{
return B3_LITERAL( b3Matrix3 ){
b3MulSV( s, m.cx ),
b3MulSV( s, m.cy ),
b3MulSV( s, m.cz ),
};
}
// This force inline improves the performance of b3ShapeDistance
/// Make a matrix from a quaternion. This is useful if you need to
/// rotate many vectors.
/// The force inline improves the performance of b3ShapeDistance.
B3_FORCE_INLINE b3Matrix3 b3MakeMatrixFromQuat( b3Quat q )
{
float xx = q.v.x * q.v.x;
@ -802,9 +765,11 @@ B3_FORCE_INLINE b3Matrix3 b3MakeMatrixFromQuat( b3Quat q )
};
}
// https://en.wikipedia.org/wiki/Parallel_axis_theorem
/// Get the inertia tensor of an offset point.
/// https://en.wikipedia.org/wiki/Parallel_axis_theorem
B3_API b3Matrix3 b3Steiner( float mass, b3Vec3 origin );
/// Get the AABB of a point cloud.
B3_INLINE b3AABB b3MakeAABB( const b3Vec3* points, int count, float radius )
{
B3_ASSERT( count > 0 );
@ -822,23 +787,7 @@ B3_INLINE b3AABB b3MakeAABB( const b3Vec3* points, int count, float radius )
return a;
}
B3_INLINE b3AABB b3AABB_AddPoint( b3AABB a, b3Vec3 point )
{
return B3_LITERAL( b3AABB ){ b3Min( a.lowerBound, point ), b3Max( a.upperBound, point ) };
}
B3_INLINE bool b3AABB_ContainsPoint( b3AABB a, b3Vec3 point )
{
if ( a.lowerBound.x > point.x || point.x > a.upperBound.x )
return false;
if ( a.lowerBound.y > point.y || point.y > a.upperBound.y )
return false;
if ( a.lowerBound.z > point.z || point.z > a.upperBound.z )
return false;
return true;
}
/// Does a fully contain b?
B3_INLINE bool b3AABB_Contains( b3AABB a, b3AABB b )
{
if ( a.lowerBound.x > b.lowerBound.x || b.upperBound.x > a.upperBound.x )
@ -851,40 +800,46 @@ B3_INLINE bool b3AABB_Contains( b3AABB a, b3AABB b )
return true;
}
B3_INLINE float b3AABB_Area( b3AABB bounds )
/// Get the surface area of an axis-aligned bounding box.
B3_INLINE float b3AABB_Area( b3AABB a )
{
b3Vec3 delta = b3Sub( bounds.upperBound, bounds.lowerBound );
b3Vec3 delta = b3Sub( a.upperBound, a.lowerBound );
return 2.0f * ( delta.x * delta.y + delta.y * delta.z + delta.z * delta.x );
}
B3_INLINE b3Vec3 b3AABB_Center( b3AABB bounds )
/// Get the center of an axis-aligned bounding box.
B3_INLINE b3Vec3 b3AABB_Center( b3AABB a )
{
return b3MulSV( 0.5f, b3Add( bounds.upperBound, bounds.lowerBound ) );
return b3MulSV( 0.5f, b3Add( a.upperBound, a.lowerBound ) );
}
B3_INLINE b3Vec3 b3AABB_Extents( b3AABB bounds )
/// Get the extents (half-widths) of an axis-aligned bounding box.
B3_INLINE b3Vec3 b3AABB_Extents( b3AABB a )
{
return b3MulSV( 0.5f, b3Sub( bounds.upperBound, bounds.lowerBound ) );
return b3MulSV( 0.5f, b3Sub( a.upperBound, a.lowerBound ) );
}
B3_INLINE b3AABB b3AABB_Union( b3AABB bounds1, b3AABB bounds2 )
/// Get the union of two axis-aligned bounding boxes.
B3_INLINE b3AABB b3AABB_Union( b3AABB a, b3AABB b )
{
b3AABB out;
out.lowerBound = b3Min( bounds1.lowerBound, bounds2.lowerBound );
out.upperBound = b3Max( bounds1.upperBound, bounds2.upperBound );
out.lowerBound = b3Min( a.lowerBound, b.lowerBound );
out.upperBound = b3Max( a.upperBound, b.upperBound );
return out;
}
B3_INLINE b3AABB b3AABB_Inflate( b3AABB bounds, float extension )
/// Add uniform padding to an axis-aligned bounding box.
B3_INLINE b3AABB b3AABB_Inflate( b3AABB a, float extension )
{
b3Vec3 radius = { extension, extension, extension };
b3AABB out;
out.lowerBound = b3Sub( bounds.lowerBound, radius );
out.upperBound = b3Add( bounds.upperBound, radius );
out.lowerBound = b3Sub( a.lowerBound, radius );
out.upperBound = b3Add( a.upperBound, radius );
return out;
}
/// Do two axis-aligned boxes overlap?
B3_INLINE bool b3AABB_Overlaps( b3AABB a, b3AABB b )
{
// No intersection if separated along one axis
@ -899,6 +854,9 @@ B3_INLINE bool b3AABB_Overlaps( b3AABB a, b3AABB b )
return true;
}
/// Transform an axis-aligned bounding box. This can create a larger box
/// than if you recomputed the AABB of the original shape with the transform
/// applied.
B3_INLINE b3AABB b3AABB_Transform( b3Transform transform, b3AABB a )
{
b3Vec3 center = b3TransformPoint( transform, b3AABB_Center( a ) );
@ -908,6 +866,7 @@ B3_INLINE b3AABB b3AABB_Transform( b3Transform transform, b3AABB a )
return out;
}
/// Get the closest point on an axis-aligned bounding box.
B3_INLINE b3Vec3 b3ClosestPointToAABB( b3Vec3 point, b3AABB a )
{
return b3Clamp( point, a.lowerBound, a.upperBound );
@ -937,7 +896,7 @@ B3_API bool b3IsSaneAABB( b3AABB a );
/// Is this a valid plane? Normal is a unit vector. Not Nan or infinity.
B3_API bool b3IsValidPlane( b3Plane a );
/**@}*/
/**@}*/ // math
/**
* @defgroup math_cpp C++ Math
@ -949,6 +908,7 @@ B3_API bool b3IsValidPlane( b3Plane a );
#ifdef __cplusplus
/// Vector addition.
B3_FORCE_INLINE b3Vec3& operator+=( b3Vec3& a, b3Vec3 b )
{
a.x += b.x;
@ -957,6 +917,7 @@ B3_FORCE_INLINE b3Vec3& operator+=( b3Vec3& a, b3Vec3 b )
return a;
}
/// Vector subtraction.
B3_FORCE_INLINE b3Vec3& operator-=( b3Vec3& a, b3Vec3 b )
{
a.x -= b.x;
@ -965,6 +926,7 @@ B3_FORCE_INLINE b3Vec3& operator-=( b3Vec3& a, b3Vec3 b )
return a;
}
/// Vector scaling.
B3_FORCE_INLINE b3Vec3& operator*=( b3Vec3& a, float s )
{
a.x *= s;
@ -973,87 +935,42 @@ B3_FORCE_INLINE b3Vec3& operator*=( b3Vec3& a, float s )
return a;
}
/// Vector negation.
B3_FORCE_INLINE b3Vec3 operator-( b3Vec3 a )
{
return { -a.x, -a.y, -a.z };
}
/// Vector scaling.
B3_FORCE_INLINE b3Vec3 operator*( float s, b3Vec3 a )
{
return { s * a.x, s * a.y, s * a.z };
}
/// Vector scaling.
B3_FORCE_INLINE b3Vec3 operator*( b3Vec3 a, float s )
{
return { s * a.x, s * a.y, s * a.z };
}
/// Component-wise vector multiplication.
B3_FORCE_INLINE b3Vec3 operator*( b3Vec3 a, b3Vec3 b )
{
return { a.x * b.x, a.y * b.y, a.z * b.z };
}
/// Vector addition.
B3_FORCE_INLINE b3Vec3 operator+( b3Vec3 a, b3Vec3 b )
{
return { a.x + b.x, a.y + b.y, a.z + b.z };
}
/// Vector subtraction.
B3_FORCE_INLINE b3Vec3 operator-( b3Vec3 a, b3Vec3 b )
{
return { a.x - b.x, a.y - b.y, a.z - b.z };
}
B3_INLINE b3Vec3 operator*( b3Matrix3 m, b3Vec3 v )
{
return b3MulMV( m, v );
}
B3_INLINE b3Matrix3& operator+=( b3Matrix3& a, b3Matrix3 b )
{
a = b3AddMM( a, b );
return a;
}
B3_INLINE b3Matrix3 operator*( b3Matrix3 a, b3Matrix3 b )
{
return b3MulMM( a, b );
}
B3_INLINE b3Matrix3 operator+( b3Matrix3 a, b3Matrix3 b )
{
return b3AddMM( a, b );
}
B3_INLINE b3Matrix3 operator-( b3Matrix3 a, b3Matrix3 b )
{
return b3SubMM( a, b );
}
B3_INLINE b3Matrix3 operator*( float s, b3Matrix3 a )
{
return b3MulSM( s, a );
}
B3_INLINE b3Quat operator+( b3Quat q1, b3Quat q2 )
{
return { q1.v + q2.v, q1.s + q2.s };
}
B3_INLINE b3Quat operator-( b3Quat q1, b3Quat q2 )
{
return { q1.v - q2.v, q1.s - q2.s };
}
B3_INLINE b3Quat operator*( float f, b3Quat q )
{
return { f * q.v, f * q.s };
}
B3_INLINE b3Quat operator*( b3Quat q, float f )
{
return { q.v * f, q.s * f };
}
#endif
/**@}*/
/**@}*/ // math_cpp