// 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 b3TrianglePoint { b3Vec3 point; b3TriangleFeature feature; } b3TrianglePoint; typedef struct b3ShapeExtent { float minExtent; b3Vec3 maxExtent; } b3ShapeExtent; b3TrianglePoint b3ClosestPointOnTriangle( b3Vec3 a, b3Vec3 b, b3Vec3 c, b3Vec3 q ); 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) int b3GetProxySupport( const b3ShapeProxy* proxy, b3Vec3 axis ); int b3GetPointSupport( const b3Vec3* points, int count, b3Vec3 axis ); // Align up to 8 byte alignment. static inline size_t b3AlignUp8( size_t x ) { return ( x + 7u ) & ~(size_t)7u; } // https://en.wikipedia.org/wiki/Floor_and_ceiling_functions static inline int b3CeilingInt( int numerator, int denominator ) { B3_VALIDATE( denominator > 0 ); return ( numerator + denominator - 1 ) / denominator; } // Assumes denominator == 2^exponent static inline int b3CeilingPow2( int numerator, int denominator, int exponent ) { B3_VALIDATE( exponent > 0 && ( denominator == 1 << exponent ) ); return ( numerator + denominator - 1 ) >> exponent; } bool b3IsSweepNormalized( b3Sweep* sweep ); static inline float b3Dot2( b3Vec2 v1, b3Vec2 v2 ) { return v1.x * v2.x + v1.y * v2.y; } static inline float b3Length2( b3Vec2 v ) { return sqrtf( b3Dot2( v, v ) ); } static inline float b3LengthSquared2( b3Vec2 v ) { return b3Dot2( v, v ); } static inline b3Vec2 b3MinVec2( b3Vec2 v1, b3Vec2 v2 ) { b3Vec2 v; v.x = b3MinFloat( v1.x, v2.x ); v.y = b3MinFloat( v1.y, v2.y ); return v; } static inline b3Vec2 b3MaxVec2( b3Vec2 v1, b3Vec2 v2 ) { b3Vec2 v; v.x = b3MaxFloat( v1.x, v2.x ); v.y = b3MaxFloat( v1.y, v2.y ); return v; } static inline void b3Store( float* dst, b3Vec3 src ) { dst[0] = src.x; dst[1] = src.y; dst[2] = src.z; } static inline b3Vec3 b3ClampLength( b3Vec3 v, float maxLength ) { 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 static inline b3Vec3 b3ArbitraryPerp( b3Vec3 v ) { // 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 ); } static inline b3Quat b3QuatFromExponentialMap( b3Vec3 v ) { // 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 static inline b3Quat b3IntegrateRotation( b3Quat q1, b3Vec3 deltaRotation ) { #if 1 // 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; #else return b3NormalizeQuat( b3MulQuat(b3QuatFromExponentialMap( deltaRotation ), q1) ); #endif } // Pseudo angular velocity from a quaternion target // w = 2 * (target - q) * conj(q) static inline b3Vec3 b3DeltaQuatToRotation( b3Quat q, b3Quat target ) { b3Quat s = q; if ( b3DotQuat( q, target ) < 0.0f ) { // 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 ); } static inline float b3ScalarTripleProduct( b3Vec3 a, b3Vec3 b, b3Vec3 c ) { 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]. static inline float b3GetByIndex( b3Vec3 v, int index ) { B3_VALIDATE( 0 <= index && index < 3 ); float temp[3] = { v.x, v.y, v.z }; return temp[index]; } static inline int b3MajorAxis( b3Vec3 v ) { return v.x < v.y ? ( v.y < v.z ? 2 : 1 ) : ( v.x < v.z ? 2 : 0 ); } static inline float b3MinElement( b3Vec3 v ) { return b3MinFloat( v.x, b3MinFloat( v.y, v.z ) ); } static inline float b3MaxElement( b3Vec3 v ) { return b3MaxFloat( v.x, b3MaxFloat( v.y, v.z ) ); } static inline int b3MaxElementIndex( b3Vec3 v ) { return v.x < v.y ? ( v.y < v.z ? 2 : 1 ) : ( v.x < v.z ? 2 : 0 ); } static inline b3Vec2 b3Add2( b3Vec2 a, b3Vec2 b ) { b3Vec2 c = { a.x + b.x, a.y + b.y }; return c; } static inline b3Vec2 b3Sub2( b3Vec2 a, b3Vec2 b ) { b3Vec2 c = { a.x - b.x, a.y - b.y }; return c; } static inline b3Vec2 b3Neg2( b3Vec2 v ) { b3Vec2 c = { -v.x, -v.y }; return c; } static inline b3Vec2 b3MulSV2( float s, b3Vec2 v ) { b3Vec2 c = { s * v.x, s * v.y }; return c; } // a + s * b static inline b3Vec2 b3MulAdd2( b3Vec2 a, float s, b3Vec2 b ) { b3Vec2 c = { a.x + s * b.x, a.y + s * b.y }; return c; } // a - s * b static inline b3Vec2 b3MulSub2( b3Vec2 a, float s, b3Vec2 b ) { b3Vec2 c = { a.x - s * b.x, a.y - s * b.y }; return c; } static inline float b3Cross2( b3Vec2 a, b3Vec2 b ) { return a.x * b.y - a.y * b.x; } static inline float b3DistanceSquared2( b3Vec2 a, b3Vec2 b ) { float dx = b.x - a.x; float dy = b.y - a.y; return dx * dx + dy * dy; } static inline b3Vec2 b3MulMV2( b3Matrix2 m, b3Vec2 a ) { b3Vec2 b = { m.cx.x * a.x + m.cy.x * a.y, m.cx.y * a.x + m.cy.y * a.y }; return b; } static inline b3Matrix2 b3MulMM2( b3Matrix2 m1, b3Matrix2 m2 ) { b3Matrix2 out; out.cx = b3MulMV2( m1, m2.cx ); out.cy = b3MulMV2( m1, m2.cy ); return out; } static inline float b3Det2( b3Matrix2 m ) { return m.cx.x * m.cy.y - m.cx.y * m.cy.x; } static inline b3Matrix2 b3Invert2( b3Matrix2 m ) { 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 static inline b3Vec2 b3Solve2( b3Matrix2 m, b3Vec2 b ) { 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 }; } // 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 ) { 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; } 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 b3Plane b3NormalizePlane( b3Plane plane ) { float invLength = 1.0f / b3Length( plane.normal ); return (b3Plane){ b3MulSV( invLength, plane.normal ), invLength * plane.offset }; } static inline b3Plane b3MakePlaneFromNormalAndPoint( b3Vec3 normal, b3Vec3 point ) { return (b3Plane){ normal, b3Dot( normal, point ) }; } static inline b3Plane b3MakePlaneFromPoints( b3Vec3 point1, b3Vec3 point2, b3Vec3 point3 ) { b3Plane plane; plane.normal = b3Cross( b3Sub( point2, point1 ), b3Sub( point3, point1 ) ); plane.normal = b3Normalize( plane.normal ); plane.offset = b3Dot( plane.normal, point1 ); return plane; } static inline b3Vec3 b3MakeNormalFromPoints( b3Vec3 point1, b3Vec3 point2, b3Vec3 point3 ) { b3Vec3 normal = b3Cross( b3Sub( point2, point1 ), b3Sub( point3, point1 ) ); return b3Normalize( normal ); } // normal2 = q * normal1 // offset2 = dot(normal2, p) + offset1 static inline b3Plane b3TransformPlane( b3Transform transform, b3Plane plane ) { b3Vec3 normal = b3RotateVector( transform.q, plane.normal ); return B3_LITERAL( b3Plane ){ normal, plane.offset + b3Dot( normal, transform.p ) }; } /// Signed separation of a point from a plane static inline float b3PlaneSeparation( b3Plane plane, b3Vec3 point ) { return b3Dot( plane.normal, point ) - plane.offset; } // Negative if p is below the triangle v1-v2-v3 static inline float b3SignedVolume( b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, b3Vec3 p ) { b3Vec3 e1 = b3Sub( v2, v1 ); b3Vec3 e2 = b3Sub( v3, v1 ); b3Vec3 n = b3Cross( e1, e2 ); return b3Dot( n, b3Sub( p, v1 ) ); } // todo eliminate this static inline bool b3IsWithinSegments( const b3SegmentDistanceResult* result ) { return ( 0.0f <= result->fraction1 && result->fraction1 <= 1.0f ) && ( 0.0f <= result->fraction2 && result->fraction2 <= 1.0f ); } static inline b3Matrix3 b3RotateInertia( b3Quat q, b3Matrix3 centralInertia ) { b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( q ); b3Matrix3 inertia = b3MulMM( rotationMatrix, b3MulMM( centralInertia, b3Transpose( rotationMatrix ) ) ); return inertia; } static inline b3Matrix3 b3TransformInertia( b3Transform transform, b3Matrix3 centralInertia, float mass ) { b3Matrix3 inertia = b3RotateInertia( transform.q, centralInertia ); inertia = b3AddMM( inertia, b3Steiner( mass, transform.p ) ); return inertia; } // 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 ) }; }