2026-05-10 13:36:52 -07:00
|
|
|
// SPDX-FileCopyrightText: 2025 Erin Catto
|
|
|
|
|
// SPDX-License-Identifier: MIT
|
|
|
|
|
|
|
|
|
|
#pragma once
|
|
|
|
|
|
|
|
|
|
#include "core.h"
|
|
|
|
|
|
|
|
|
|
#include "box3d/collision.h"
|
|
|
|
|
#include "box3d/math_functions.h"
|
|
|
|
|
|
|
|
|
|
struct b3Sweep;
|
|
|
|
|
struct b3Plane;
|
|
|
|
|
|
|
|
|
|
#define B3_TWO_PI 6.283185307f
|
|
|
|
|
#define B3_PI_OVER_TWO 1.570796327f
|
|
|
|
|
#define B3_PI_OVER_FOUR 0.785398163f
|
|
|
|
|
#define B3_SQRT3 1.732050808f
|
|
|
|
|
|
|
|
|
|
// todo eliminate this
|
|
|
|
|
static const b3AABB B3_BOUNDS3_EMPTY = { { FLT_MAX, FLT_MAX, FLT_MAX }, { -FLT_MAX, -FLT_MAX, -FLT_MAX } };
|
|
|
|
|
|
|
|
|
|
typedef struct b3Matrix2
|
|
|
|
|
{
|
|
|
|
|
b3Vec2 cx, cy;
|
|
|
|
|
} b3Matrix2;
|
|
|
|
|
|
|
|
|
|
typedef struct b3Triangle
|
|
|
|
|
{
|
|
|
|
|
b3Vec3 vertices[3];
|
|
|
|
|
int i1, i2, i3;
|
|
|
|
|
int flags;
|
|
|
|
|
} b3Triangle;
|
|
|
|
|
|
|
|
|
|
typedef struct
|
|
|
|
|
{
|
|
|
|
|
b3Vec3 point;
|
|
|
|
|
b3TriangleFeature feature;
|
|
|
|
|
} b3TrianglePoint;
|
|
|
|
|
|
|
|
|
|
typedef struct b3ShapeExtent
|
|
|
|
|
{
|
|
|
|
|
float minExtent;
|
|
|
|
|
b3Vec3 maxExtent;
|
|
|
|
|
} b3ShapeExtent;
|
|
|
|
|
|
|
|
|
|
b3TrianglePoint b3ClosestPointOnTriangle( b3Vec3 a, b3Vec3 b, b3Vec3 c, b3Vec3 q );
|
|
|
|
|
|
2026-06-10 13:21:14 -07:00
|
|
|
// The closest points between to segments or infinite lines.
|
|
|
|
|
typedef struct b3ClosestApproachResult
|
|
|
|
|
{
|
|
|
|
|
b3Vec3 point1;
|
|
|
|
|
float fraction1;
|
|
|
|
|
b3Vec3 point2;
|
|
|
|
|
float fraction2;
|
|
|
|
|
} b3ClosestApproachResult;
|
|
|
|
|
|
|
|
|
|
// Compute the closest point on the segment a-b to the target q
|
|
|
|
|
b3Vec3 b3ClosestPointOnSegment( b3Vec3 a, b3Vec3 b, b3Vec3 q );
|
|
|
|
|
|
|
|
|
|
// Compute the closest points on two infinite lines
|
|
|
|
|
b3ClosestApproachResult b3ClosestApproachLines( b3Vec3 p1, b3Vec3 d1, b3Vec3 p2, b3Vec3 d2 );
|
|
|
|
|
|
|
|
|
|
// Compute the closest points on two line segments
|
|
|
|
|
b3ClosestApproachResult b3ClosestApproachSegments( b3Vec3 p1, b3Vec3 q1, b3Vec3 p2, b3Vec3 q2 );
|
|
|
|
|
|
2026-05-10 13:36:52 -07:00
|
|
|
float b3IntersectSegmentTriangle( b3Vec3 p, b3Vec3 q, b3Vec3 a, b3Vec3 b, b3Vec3 c );
|
|
|
|
|
float b3IntersectSegmentSphere( b3Vec3 p, b3Vec3 q, b3Vec3 c, float r );
|
|
|
|
|
|
|
|
|
|
b3MassData b3ComputeMassProperties( int triangleCount, const int* triangles, int vertexCount, const b3Vec3* vertices,
|
|
|
|
|
float density );
|
|
|
|
|
|
|
|
|
|
bool b3IsValidMassData( const b3MassData* massData );
|
|
|
|
|
|
|
|
|
|
b3Matrix3 b3SphereInertia( float mass, float radius );
|
|
|
|
|
b3Matrix3 b3CylinderInertia( float mass, float radius, float height );
|
|
|
|
|
b3Matrix3 b3BoxInertia( float mass, b3Vec3 min, b3Vec3 max );
|
|
|
|
|
|
|
|
|
|
// Inertia helper (Io = Ic + Is and Ic = Io - Is)
|
|
|
|
|
b3Matrix3 b3Steiner( float mass, b3Vec3 origin );
|
|
|
|
|
int b3GetProxySupport( const b3ShapeProxy* proxy, b3Vec3 axis );
|
|
|
|
|
int b3GetPointSupport( const b3Vec3* points, int count, b3Vec3 axis );
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline size_t b3AlignUp8( size_t x )
|
|
|
|
|
{
|
|
|
|
|
return ( x + 7u ) & ~(size_t)7u;
|
2026-05-10 13:36:52 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// https://en.wikipedia.org/wiki/Floor_and_ceiling_functions
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline int b3CeilingInt( int numerator, int denominator )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
B3_VALIDATE( denominator > 0 );
|
|
|
|
|
return ( numerator + denominator - 1 ) / denominator;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Assumes denominator == 2^exponent
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline int b3CeilingPow2( int numerator, int denominator, int exponent )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
B3_VALIDATE( exponent > 0 && ( denominator == 1 << exponent ) );
|
|
|
|
|
return ( numerator + denominator - 1 ) >> exponent;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
bool b3IsSweepNormalized( b3Sweep* sweep );
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline float b3Dot2( b3Vec2 v1, b3Vec2 v2 )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
return v1.x * v2.x + v1.y * v2.y;
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline float b3Length2( b3Vec2 v )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
return sqrtf( b3Dot2( v, v ) );
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline float b3LengthSquared2( b3Vec2 v )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
return b3Dot2( v, v );
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline b3Vec2 b3MinVec2( b3Vec2 v1, b3Vec2 v2 )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
b3Vec2 v;
|
|
|
|
|
v.x = b3MinFloat( v1.x, v2.x );
|
|
|
|
|
v.y = b3MinFloat( v1.y, v2.y );
|
|
|
|
|
return v;
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline b3Vec2 b3MaxVec2( b3Vec2 v1, b3Vec2 v2 )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
b3Vec2 v;
|
|
|
|
|
v.x = b3MaxFloat( v1.x, v2.x );
|
|
|
|
|
v.y = b3MaxFloat( v1.y, v2.y );
|
|
|
|
|
return v;
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline void b3Store( float* dst, b3Vec3 src )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
dst[0] = src.x;
|
|
|
|
|
dst[1] = src.y;
|
|
|
|
|
dst[2] = src.z;
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline b3Vec3 b3ClampLength( b3Vec3 v, float maxLength )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
float lengthSq = b3LengthSquared( v );
|
|
|
|
|
if ( lengthSq <= maxLength * maxLength )
|
|
|
|
|
{
|
|
|
|
|
return v;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float length = sqrtf( lengthSq );
|
|
|
|
|
return b3MulSV( maxLength / length, v );
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Assume v is a unit vector
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline b3Vec3 b3ArbitraryPerp( b3Vec3 v )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
// 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 )
|
|
|
|
|
{
|
|
|
|
|
// x is non-zero and it should not go into the x component
|
|
|
|
|
// dot([ay + bz, cx, dx], [x, y, z]) = ayx + bzx + cxy + dzx
|
|
|
|
|
// for the dot product to be zero need: c = -a, d = -b
|
|
|
|
|
float a = 0.67f;
|
|
|
|
|
float b = -0.42f;
|
|
|
|
|
p = B3_LITERAL( b3Vec3 ){ a * v.y + b * v.z, -a * v.x, -b * v.x };
|
|
|
|
|
}
|
|
|
|
|
else if ( v.y < -0.5f || 0.5f < v.y )
|
|
|
|
|
{
|
|
|
|
|
// y is non-zero and it should not go into the y component
|
|
|
|
|
// p = [ay, bx + cz, dy]
|
|
|
|
|
// axy + bxy + cyz + dyz = 0
|
|
|
|
|
// b = -a, d = -c
|
|
|
|
|
float a = 0.67f;
|
|
|
|
|
float c = -0.42f;
|
|
|
|
|
p = B3_LITERAL( b3Vec3 ){ a * v.y, -a * v.x + c * v.z, -c * v.y };
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
|
|
|
|
// This would trip if the input is not a unit vector
|
|
|
|
|
B3_VALIDATE( v.z < -0.5f || 0.5f < v.z );
|
|
|
|
|
|
|
|
|
|
// z is non-zero and it should not go into the z component
|
|
|
|
|
// p = [az, bz, cx + dy]
|
|
|
|
|
// axz + byz + cxz + dyz = 0
|
|
|
|
|
// c = -a, d = -b
|
|
|
|
|
float a = 0.67f;
|
|
|
|
|
float b = -0.42f;
|
|
|
|
|
p = B3_LITERAL( b3Vec3 ){ a * v.z, b * v.z, -a * v.x - b * v.y };
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
B3_VALIDATE( b3LengthSquared( p ) > 0.1f );
|
|
|
|
|
B3_VALIDATE( b3AbsFloat( b3Dot( p, v ) ) < 100.0f * FLT_EPSILON );
|
|
|
|
|
|
|
|
|
|
return b3Normalize( p );
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline b3Quat b3QuatFromExponentialMap( b3Vec3 v )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
// Exponential map (Grassia)
|
|
|
|
|
float threshold = 0.018581361f;
|
|
|
|
|
|
|
|
|
|
float angle = b3Length( v );
|
|
|
|
|
if ( angle < threshold )
|
|
|
|
|
{
|
|
|
|
|
// Taylor expansion
|
|
|
|
|
b3Quat out;
|
|
|
|
|
out.v = b3MulSV( 0.5f + angle * angle / 48.0f, v );
|
|
|
|
|
out.s = b3Cos( 0.5f * angle );
|
|
|
|
|
|
|
|
|
|
return out;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
return b3MakeQuatFromAxisAngle( b3MulSV( 1.0f / angle, v ), angle );
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Integrate rotation from angular velocity
|
|
|
|
|
/// @param q1 initial rotation
|
|
|
|
|
/// @param deltaRotation the angular displacement vector in radians (angular velocity multiplied by the time step)
|
|
|
|
|
/// q2 = q1 + 0.5 * omega * q1
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline b3Quat b3IntegrateRotation( b3Quat q1, b3Vec3 deltaRotation )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
// https://fgiesen.wordpress.com/2012/08/24/quaternion-differentiation/
|
|
|
|
|
b3Quat qd = { b3MulSV( 0.5f, deltaRotation ), 0.0f };
|
|
|
|
|
qd = b3MulQuat( qd, q1 );
|
|
|
|
|
b3Quat q2 = { b3Add( q1.v, qd.v ), qd.s + q1.s };
|
|
|
|
|
q2 = b3NormalizeQuat( q2 );
|
|
|
|
|
return q2;
|
|
|
|
|
|
|
|
|
|
// return b3NormalizeQuat( b3MulQuat(b3QuatFromExponentialMap( deltaRotation ), q1) );
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Pseudo angular velocity from a quaternion target
|
|
|
|
|
// w = 2 * (target - q) * conj(q)
|
2026-06-10 13:21:14 -07:00
|
|
|
static inline b3Vec3 b3DeltaQuatToRotation( b3Quat q, b3Quat target )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
b3Quat s = q;
|
2026-06-10 13:21:14 -07:00
|
|
|
if ( b3DotQuat( q, target ) < 0.0f )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
// Correct polarity
|
|
|
|
|
s = b3NegateQuat( q );
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
b3Quat diff = { b3Sub( target.v, s.v ), target.s - s.s };
|
|
|
|
|
b3Quat product = b3MulQuat( diff, b3Conjugate( s ) );
|
|
|
|
|
return b3MulSV( 2.0f, product.v );
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline float b3ScalarTripleProduct( b3Vec3 a, b3Vec3 b, b3Vec3 c )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
b3Vec3 d;
|
|
|
|
|
d.x = b.y * c.z - b.z * c.y;
|
|
|
|
|
d.y = b.z * c.x - b.x * c.z;
|
|
|
|
|
d.z = b.x * c.y - b.y * c.x;
|
|
|
|
|
return a.x * d.x + a.y * d.y + a.z * d.z;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Get a value by index. Avoid undefined behavior of code like (&v.x)[2].
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline float b3GetByIndex( b3Vec3 v, int index )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
2026-06-10 13:21:14 -07:00
|
|
|
B3_VALIDATE( 0 <= index && index < 3 );
|
2026-05-10 13:36:52 -07:00
|
|
|
float temp[3] = { v.x, v.y, v.z };
|
|
|
|
|
return temp[index];
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline int b3MajorAxis( b3Vec3 v )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
return v.x < v.y ? ( v.y < v.z ? 2 : 1 ) : ( v.x < v.z ? 2 : 0 );
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline float b3MinElement( b3Vec3 v )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
return b3MinFloat( v.x, b3MinFloat( v.y, v.z ) );
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline float b3MaxElement( b3Vec3 v )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
return b3MaxFloat( v.x, b3MaxFloat( v.y, v.z ) );
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline int b3MaxElementIndex( b3Vec3 v )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
return v.x < v.y ? ( v.y < v.z ? 2 : 1 ) : ( v.x < v.z ? 2 : 0 );
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline b3Vec2 b3Add2( b3Vec2 a, b3Vec2 b )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
b3Vec2 c = { a.x + b.x, a.y + b.y };
|
|
|
|
|
return c;
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline b3Vec2 b3Sub2( b3Vec2 a, b3Vec2 b )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
b3Vec2 c = { a.x - b.x, a.y - b.y };
|
|
|
|
|
return c;
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline b3Vec2 b3Neg2( b3Vec2 v )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
b3Vec2 c = { -v.x, -v.y };
|
|
|
|
|
return c;
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline b3Vec2 b3MulSV2( float s, b3Vec2 v )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
b3Vec2 c = { s * v.x, s * v.y };
|
|
|
|
|
return c;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// a + s * b
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline b3Vec2 b3MulAdd2( b3Vec2 a, float s, b3Vec2 b )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
b3Vec2 c = { a.x + s * b.x, a.y + s * b.y };
|
|
|
|
|
return c;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// a - s * b
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline b3Vec2 b3MulSub2( b3Vec2 a, float s, b3Vec2 b )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
b3Vec2 c = { a.x - s * b.x, a.y - s * b.y };
|
|
|
|
|
return c;
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline float b3Cross2( b3Vec2 a, b3Vec2 b )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
return a.x * b.y - a.y * b.x;
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline float b3DistanceSquared2( b3Vec2 a, b3Vec2 b )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
float dx = b.x - a.x;
|
|
|
|
|
float dy = b.y - a.y;
|
|
|
|
|
return dx * dx + dy * dy;
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline b3Vec2 b3MulMV2( b3Matrix2 m, b3Vec2 a )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
b3Vec2 b = { m.cx.x * a.x + m.cy.x * a.y, m.cx.y * a.x + m.cy.y * a.y };
|
|
|
|
|
return b;
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline b3Matrix2 b3MulMM2( b3Matrix2 m1, b3Matrix2 m2 )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
b3Matrix2 out;
|
|
|
|
|
out.cx = b3MulMV2( m1, m2.cx );
|
|
|
|
|
out.cy = b3MulMV2( m1, m2.cy );
|
|
|
|
|
return out;
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline float b3Det2( b3Matrix2 m )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
return m.cx.x * m.cy.y - m.cx.y * m.cy.x;
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline b3Matrix2 b3Invert2( b3Matrix2 m )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
float det = b3Det2( m );
|
|
|
|
|
if ( b3AbsFloat( det ) > 1000.0f * FLT_MIN )
|
|
|
|
|
{
|
|
|
|
|
float invDet = 1.0f / det;
|
|
|
|
|
return B3_LITERAL( b3Matrix2 ){
|
|
|
|
|
{ invDet * m.cy.y, -invDet * m.cx.y },
|
|
|
|
|
{ -invDet * m.cy.x, invDet * m.cx.x },
|
|
|
|
|
};
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
return B3_LITERAL( b3Matrix2 ){ { 0.0f, 0.0f }, { 0.0f, 0.0f } };
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Assumes positive semi-definite
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline b3Vec2 b3Solve2( b3Matrix2 m, b3Vec2 b )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
float det = b3Det2( m );
|
|
|
|
|
if ( det > 1000.0f * FLT_MIN )
|
|
|
|
|
{
|
|
|
|
|
float invDet = 1.0f / det;
|
|
|
|
|
return B3_LITERAL( b3Vec2 ){
|
|
|
|
|
invDet * m.cy.y * b.x - invDet * m.cy.x * b.y,
|
|
|
|
|
-invDet * m.cx.y * b.x + invDet * m.cx.x * b.y,
|
|
|
|
|
};
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
return B3_LITERAL( b3Vec2 ){ 0.0f, 0.0f };
|
|
|
|
|
}
|
|
|
|
|
|
2026-06-10 13:21:14 -07:00
|
|
|
// Convenience function: s * a + t * b + u * c
|
|
|
|
|
static 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;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static inline b3Vec3 b3ModifiedCross( b3Vec3 a, b3Vec3 b )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
b3Vec3 c;
|
|
|
|
|
c.x = a.y * b.z + a.z * b.y;
|
|
|
|
|
c.y = a.z * b.x + a.x * b.z;
|
|
|
|
|
c.z = a.x * b.y + a.y * b.x;
|
|
|
|
|
return c;
|
|
|
|
|
}
|
|
|
|
|
|
2026-06-10 13:21:14 -07:00
|
|
|
static inline b3Matrix3 b3MakeDiagonalMatrix( float a, float b, float c )
|
|
|
|
|
{
|
|
|
|
|
return (b3Matrix3){ { a, 0.0f, 0.0f }, { 0.0f, b, 0.0f }, { 0.0f, 0.0f, c } };
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static inline b3Matrix3 b3Skew( b3Vec3 v )
|
|
|
|
|
{
|
|
|
|
|
b3Matrix3 out;
|
|
|
|
|
out.cx = (b3Vec3){ 0, v.z, -v.y };
|
|
|
|
|
out.cy = (b3Vec3){ -v.z, 0, v.x };
|
|
|
|
|
out.cz = (b3Vec3){ v.y, -v.x, 0 };
|
|
|
|
|
|
|
|
|
|
return out;
|
|
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline b3Plane b3NormalizePlane( b3Plane plane )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
float invLength = 1.0f / b3Length( plane.normal );
|
2026-06-10 13:21:14 -07:00
|
|
|
return (b3Plane){ b3MulSV( invLength, plane.normal ), invLength * plane.offset };
|
2026-05-10 13:36:52 -07:00
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline b3Plane b3MakePlaneFromNormalAndPoint( b3Vec3 normal, b3Vec3 point )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
2026-06-10 13:21:14 -07:00
|
|
|
return (b3Plane){ normal, b3Dot( normal, point ) };
|
2026-05-10 13:36:52 -07:00
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline b3Plane b3MakePlaneFromPoints( b3Vec3 point1, b3Vec3 point2, b3Vec3 point3 )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
2026-05-10 23:50:16 -07:00
|
|
|
b3Plane plane;
|
|
|
|
|
plane.normal = b3Cross( b3Sub( point2, point1 ), b3Sub( point3, point1 ) );
|
|
|
|
|
plane.normal = b3Normalize( plane.normal );
|
|
|
|
|
plane.offset = b3Dot( plane.normal, point1 );
|
|
|
|
|
return plane;
|
2026-05-10 13:36:52 -07:00
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline b3Vec3 b3MakeNormalFromPoints( b3Vec3 point1, b3Vec3 point2, b3Vec3 point3 )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
2026-05-10 23:50:16 -07:00
|
|
|
b3Vec3 normal = b3Cross( b3Sub( point2, point1 ), b3Sub( point3, point1 ) );
|
|
|
|
|
return b3Normalize( normal );
|
2026-05-10 13:36:52 -07:00
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
// normal2 = q * normal1
|
|
|
|
|
// offset2 = dot(normal2, p) + offset1
|
|
|
|
|
static inline b3Plane b3TransformPlane( b3Transform transform, b3Plane plane )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
2026-05-10 23:50:16 -07:00
|
|
|
b3Vec3 normal = b3RotateVector( transform.q, plane.normal );
|
|
|
|
|
return B3_LITERAL( b3Plane ){ normal, plane.offset + b3Dot( normal, transform.p ) };
|
2026-05-10 13:36:52 -07:00
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
/// Signed separation of a point from a plane
|
|
|
|
|
static inline float b3PlaneSeparation( b3Plane plane, b3Vec3 point )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
2026-05-10 23:50:16 -07:00
|
|
|
return b3Dot( plane.normal, point ) - plane.offset;
|
2026-05-10 13:36:52 -07:00
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
// Negative if p is below the triangle v1-v2-v3
|
|
|
|
|
static inline float b3SignedVolume( b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, b3Vec3 p )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
2026-05-10 23:50:16 -07:00
|
|
|
b3Vec3 e1 = b3Sub( v2, v1 );
|
|
|
|
|
b3Vec3 e2 = b3Sub( v3, v1 );
|
|
|
|
|
b3Vec3 n = b3Cross( e1, e2 );
|
|
|
|
|
return b3Dot( n, b3Sub( p, v1 ) );
|
2026-05-10 13:36:52 -07:00
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
// todo eliminate this
|
|
|
|
|
static inline bool b3IsWithinSegments( const b3ClosestApproachResult* result )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
2026-06-10 13:21:14 -07:00
|
|
|
return ( 0.0f <= result->fraction1 && result->fraction1 <= 1.0f ) &&
|
|
|
|
|
( 0.0f <= result->fraction2 && result->fraction2 <= 1.0f );
|
2026-05-10 13:36:52 -07:00
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline b3Matrix3 b3RotateInertia( b3Quat q, b3Matrix3 centralInertia )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
2026-05-10 23:50:16 -07:00
|
|
|
b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( q );
|
|
|
|
|
b3Matrix3 inertia = b3MulMM( rotationMatrix, b3MulMM( centralInertia, b3Transpose( rotationMatrix ) ) );
|
|
|
|
|
return inertia;
|
2026-05-10 13:36:52 -07:00
|
|
|
}
|
|
|
|
|
|
2026-05-10 23:50:16 -07:00
|
|
|
static inline b3Matrix3 b3TransformInertia( b3Transform transform, b3Matrix3 centralInertia, float mass )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
2026-05-10 23:50:16 -07:00
|
|
|
b3Matrix3 inertia = b3RotateInertia( transform.q, centralInertia );
|
|
|
|
|
inertia = b3AddMM( inertia, b3Steiner( mass, transform.p ) );
|
|
|
|
|
return inertia;
|
2026-05-10 13:36:52 -07:00
|
|
|
}
|
2026-06-10 13:21:14 -07:00
|
|
|
|
|
|
|
|
// Twist angle around the z-axis, used for twist limit and revolute angle limit
|
|
|
|
|
static 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
|
|
|
|
|
static 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;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Add a point to an AABB.
|
|
|
|
|
static inline b3AABB b3AABB_AddPoint( b3AABB a, b3Vec3 point )
|
|
|
|
|
{
|
|
|
|
|
return (b3AABB){ b3Min( a.lowerBound, point ), b3Max( a.upperBound, point ) };
|
|
|
|
|
}
|