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:
parent
9e02b7f3b3
commit
d8fbeb444e
53 changed files with 8973 additions and 1812 deletions
220
src/aabb.c
220
src/aabb.c
|
|
@ -3,122 +3,174 @@
|
|||
|
||||
#include "aabb.h"
|
||||
|
||||
#include "math_internal.h"
|
||||
|
||||
#include "box3d/math_functions.h"
|
||||
|
||||
#include <float.h>
|
||||
|
||||
// From Real-time Collision Detection, p179.
|
||||
b3CastOutput b3RayCastAABB( b3AABB a, b3Vec3 p1, b3Vec3 p2 )
|
||||
// Similar to Real-time Collision Detection, p179.
|
||||
// todo try
|
||||
// https://www.scratchapixel.com/lessons/3d-basic-rendering/minimal-ray-tracer-rendering-simple-shapes/ray-box-intersection.html
|
||||
bool b3RayCastAABB( b3AABB a, b3Vec3 p1, b3Vec3 p2, float* minFraction, float* maxFraction )
|
||||
{
|
||||
// Radius not handled
|
||||
b3CastOutput output = { 0 };
|
||||
|
||||
float tmin = -FLT_MAX;
|
||||
float tmax = FLT_MAX;
|
||||
|
||||
b3Vec3 p = p1;
|
||||
// Ray direction and length
|
||||
b3Vec3 d = b3Sub( p2, p1 );
|
||||
b3Vec3 absD = b3Abs( d );
|
||||
float rayLength = b3Length( d );
|
||||
|
||||
b3Vec3 normal = b3Vec3_zero;
|
||||
|
||||
// x-coordinate
|
||||
if ( absD.x < FLT_EPSILON )
|
||||
// Handle degenerate ray
|
||||
if ( rayLength < FLT_EPSILON )
|
||||
{
|
||||
// parallel
|
||||
if ( p.x < a.lowerBound.x || a.upperBound.x < p.x )
|
||||
// Check if point is inside AABB
|
||||
if ( p1.x >= a.lowerBound.x && p1.x <= a.upperBound.x && p1.y >= a.lowerBound.y && p1.y <= a.upperBound.y &&
|
||||
p1.z >= a.lowerBound.z && p1.z <= a.upperBound.z )
|
||||
{
|
||||
return output;
|
||||
*minFraction = 0.0f;
|
||||
*maxFraction = 0.0f;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
b3Vec3 rayDir = b3MulSV( 1.0f / rayLength, d );
|
||||
|
||||
// Slab method for ray-AABB intersection
|
||||
float tMin = 0.0f;
|
||||
float tMax = rayLength;
|
||||
|
||||
// x-axis
|
||||
{
|
||||
float rayComponent = rayDir.x;
|
||||
float rayStart = p1.x;
|
||||
float boxMin = a.lowerBound.x;
|
||||
float boxMax = a.upperBound.x;
|
||||
|
||||
if ( b3AbsFloat( rayComponent ) < FLT_EPSILON )
|
||||
{
|
||||
// Ray is parallel to slab, check if ray origin is within slab
|
||||
if ( rayStart < boxMin || rayStart > boxMax )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// Compute intersection distances
|
||||
float t1 = ( boxMin - rayStart ) / rayComponent;
|
||||
float t2 = ( boxMax - rayStart ) / rayComponent;
|
||||
|
||||
// Ensure t1 <= t2
|
||||
if ( t1 > t2 )
|
||||
{
|
||||
float temp = t1;
|
||||
t1 = t2;
|
||||
t2 = temp;
|
||||
}
|
||||
|
||||
// Update intersection interval
|
||||
tMin = b3MaxFloat( tMin, t1 );
|
||||
tMax = b3MinFloat( tMax, t2 );
|
||||
|
||||
// Check for no intersection
|
||||
if ( tMin > tMax )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
|
||||
// y-axis
|
||||
{
|
||||
float inv_d = 1.0f / d.x;
|
||||
float t1 = ( a.lowerBound.x - p.x ) * inv_d;
|
||||
float t2 = ( a.upperBound.x - p.x ) * inv_d;
|
||||
float rayComponent = rayDir.y;
|
||||
float rayStart = p1.y;
|
||||
float boxMin = a.lowerBound.y;
|
||||
float boxMax = a.upperBound.y;
|
||||
|
||||
// Sign of the normal vector.
|
||||
float s = -1.0f;
|
||||
|
||||
if ( t1 > t2 )
|
||||
if ( b3AbsFloat( rayComponent ) < FLT_EPSILON )
|
||||
{
|
||||
float tmp = t1;
|
||||
t1 = t2;
|
||||
t2 = tmp;
|
||||
s = 1.0f;
|
||||
// Ray is parallel to slab, check if ray origin is within slab
|
||||
if ( rayStart < boxMin || rayStart > boxMax )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// Push the min up
|
||||
if ( t1 > tmin )
|
||||
else
|
||||
{
|
||||
normal.y = 0.0f;
|
||||
normal.x = s;
|
||||
tmin = t1;
|
||||
}
|
||||
// Compute intersection distances
|
||||
float t1 = ( boxMin - rayStart ) / rayComponent;
|
||||
float t2 = ( boxMax - rayStart ) / rayComponent;
|
||||
|
||||
// Pull the max down
|
||||
tmax = b3MinFloat( tmax, t2 );
|
||||
// Ensure t1 <= t2
|
||||
if ( t1 > t2 )
|
||||
{
|
||||
float temp = t1;
|
||||
t1 = t2;
|
||||
t2 = temp;
|
||||
}
|
||||
|
||||
if ( tmin > tmax )
|
||||
{
|
||||
return output;
|
||||
// Update intersection interval
|
||||
tMin = b3MaxFloat( tMin, t1 );
|
||||
tMax = b3MinFloat( tMax, t2 );
|
||||
|
||||
// Check for no intersection
|
||||
if ( tMin > tMax )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// y-coordinate
|
||||
if ( absD.y < FLT_EPSILON )
|
||||
// z-axis
|
||||
{
|
||||
// parallel
|
||||
if ( p.y < a.lowerBound.y || a.upperBound.y < p.y )
|
||||
float rayComponent = rayDir.z;
|
||||
float rayStart = p1.z;
|
||||
float boxMin = a.lowerBound.z;
|
||||
float boxMax = a.upperBound.z;
|
||||
|
||||
if ( b3AbsFloat( rayComponent ) < FLT_EPSILON )
|
||||
{
|
||||
return output;
|
||||
// Ray is parallel to slab, check if ray origin is within slab
|
||||
if ( rayStart < boxMin || rayStart > boxMax )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
float inv_d = 1.0f / d.y;
|
||||
float t1 = ( a.lowerBound.y - p.y ) * inv_d;
|
||||
float t2 = ( a.upperBound.y - p.y ) * inv_d;
|
||||
|
||||
// Sign of the normal vector.
|
||||
float s = -1.0f;
|
||||
|
||||
if ( t1 > t2 )
|
||||
else
|
||||
{
|
||||
float tmp = t1;
|
||||
t1 = t2;
|
||||
t2 = tmp;
|
||||
s = 1.0f;
|
||||
}
|
||||
// Compute intersection distances
|
||||
float t1 = ( boxMin - rayStart ) / rayComponent;
|
||||
float t2 = ( boxMax - rayStart ) / rayComponent;
|
||||
|
||||
// Push the min up
|
||||
if ( t1 > tmin )
|
||||
{
|
||||
normal.x = 0.0f;
|
||||
normal.y = s;
|
||||
tmin = t1;
|
||||
}
|
||||
// Ensure t1 <= t2
|
||||
if ( t1 > t2 )
|
||||
{
|
||||
float temp = t1;
|
||||
t1 = t2;
|
||||
t2 = temp;
|
||||
}
|
||||
|
||||
// Pull the max down
|
||||
tmax = b3MinFloat( tmax, t2 );
|
||||
// Update intersection interval
|
||||
tMin = b3MaxFloat( tMin, t1 );
|
||||
tMax = b3MinFloat( tMax, t2 );
|
||||
|
||||
if ( tmin > tmax )
|
||||
{
|
||||
return output;
|
||||
// Check for no intersection
|
||||
if ( tMin > tMax )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Does the ray start inside the box?
|
||||
// Does the ray intersect beyond the max fraction?
|
||||
if ( tmin < 0.0f || 1.0f < tmin )
|
||||
// Check if intersection is behind ray start
|
||||
if ( tMax < 0.0f )
|
||||
{
|
||||
return output;
|
||||
return false;
|
||||
}
|
||||
|
||||
// Intersection.
|
||||
output.fraction = tmin;
|
||||
output.normal = normal;
|
||||
output.point = b3Lerp( p1, p2, tmin );
|
||||
output.hit = true;
|
||||
return output;
|
||||
// Convert distances to fractions
|
||||
*minFraction = b3ClampFloat( tMin / rayLength, 0.0f, 1.0f );
|
||||
*maxFraction = b3ClampFloat( tMax / rayLength, 0.0f, 1.0f );
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -5,8 +5,8 @@
|
|||
|
||||
#include "box3d/types.h"
|
||||
|
||||
// Ray cast an AABB
|
||||
b3CastOutput b3RayCastAABB( b3AABB a, b3Vec3 p1, b3Vec3 p2 );
|
||||
// Ray cast an AABB. This is a custom function used by height fields.
|
||||
bool b3RayCastAABB( b3AABB a, b3Vec3 p1, b3Vec3 p2, float* minFraction, float* maxFraction );
|
||||
|
||||
// Get the surface area (perimeter)
|
||||
static inline float b3Perimeter( b3AABB a )
|
||||
|
|
|
|||
|
|
@ -66,93 +66,6 @@ int b3GetPointSupport( const b3Vec3* points, int count, b3Vec3 axis )
|
|||
return maxIndex;
|
||||
}
|
||||
|
||||
// Similar to Real-time Collision Detection, p179.
|
||||
// todo try https://www.scratchapixel.com/lessons/3d-basic-rendering/minimal-ray-tracer-rendering-simple-shapes/ray-box-intersection.html
|
||||
bool b3IntersectRayAndAABB( b3Vec3 lowerBound, b3Vec3 upperBound, b3Vec3 p1, b3Vec3 p2, float* minFraction,
|
||||
float* maxFraction )
|
||||
{
|
||||
// Ray direction and length
|
||||
b3Vec3 d = b3Sub( p2, p1 );
|
||||
float rayLength = b3Length( d );
|
||||
|
||||
// Handle degenerate ray
|
||||
if ( rayLength < FLT_EPSILON )
|
||||
{
|
||||
// Check if point is inside AABB
|
||||
if ( p1.x >= lowerBound.x && p1.x <= upperBound.x && p1.y >= lowerBound.y && p1.y <= upperBound.y &&
|
||||
p1.z >= lowerBound.z && p1.z <= upperBound.z )
|
||||
{
|
||||
*minFraction = 0.0f;
|
||||
*maxFraction = 0.0f;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
b3Vec3 rayDir = b3MulSV( 1.0f / rayLength, d );
|
||||
|
||||
// Slab method for ray-AABB intersection
|
||||
float tMin = 0.0f;
|
||||
float tMax = rayLength;
|
||||
|
||||
// Test each axis
|
||||
for ( int i = 0; i < 3; ++i )
|
||||
{
|
||||
float rayComponent = b3GetByIndex( rayDir, i );
|
||||
float rayStart = b3GetByIndex( p1, i );
|
||||
float boxMin = b3GetByIndex( lowerBound, i );
|
||||
float boxMax = b3GetByIndex( upperBound, i );
|
||||
|
||||
if ( b3AbsFloat( rayComponent ) < FLT_EPSILON )
|
||||
{
|
||||
// Ray is parallel to slab, check if ray origin is within slab
|
||||
if ( rayStart < boxMin || rayStart > boxMax )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// Compute intersection distances
|
||||
float t1 = ( boxMin - rayStart ) / rayComponent;
|
||||
float t2 = ( boxMax - rayStart ) / rayComponent;
|
||||
|
||||
// Ensure t1 <= t2
|
||||
if ( t1 > t2 )
|
||||
{
|
||||
float temp = t1;
|
||||
t1 = t2;
|
||||
t2 = temp;
|
||||
}
|
||||
|
||||
// Update intersection interval
|
||||
tMin = b3MaxFloat( tMin, t1 );
|
||||
tMax = b3MinFloat( tMax, t2 );
|
||||
|
||||
// Check for no intersection
|
||||
if ( tMin > tMax )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Check if intersection is behind ray start
|
||||
if ( tMax < 0.0f )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// Convert distances to fractions
|
||||
*minFraction = b3ClampFloat( tMin / rayLength, 0.0f, 1.0f );
|
||||
*maxFraction = b3ClampFloat( tMax / rayLength, 0.0f, 1.0f );
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static void b3BarycentricCoordsEdge( float out[3], b3Vec3 a, b3Vec3 b )
|
||||
{
|
||||
b3Vec3 ab = b3Sub( b, a );
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
// SPDX-FileCopyrightText: 2025 Erin Catto
|
||||
// SPDX-License-Identifier: MIT
|
||||
|
||||
#include "aabb.h"
|
||||
#include "algorithm.h"
|
||||
#include "body.h"
|
||||
#include "core.h"
|
||||
|
|
@ -615,8 +616,7 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightField* heightField, const b3S
|
|||
b3Add( b3Add( heightField->aabb.upperBound, shapeExtents ), margin ) };
|
||||
|
||||
float minFraction, maxFraction;
|
||||
bool intersects = b3IntersectRayAndAABB( combinedBounds.lowerBound, combinedBounds.upperBound, shapeStart, shapeEnd,
|
||||
&minFraction, &maxFraction );
|
||||
bool intersects = b3RayCastAABB( combinedBounds, shapeStart, shapeEnd, &minFraction, &maxFraction );
|
||||
if ( intersects == false )
|
||||
{
|
||||
return result;
|
||||
|
|
|
|||
|
|
@ -236,7 +236,7 @@ b3Quat b3MakeQuatFromMatrix( const b3Matrix3* m )
|
|||
|
||||
b3Quat q;
|
||||
|
||||
float trace = b3Trace( *m );
|
||||
float trace = m->cx.x + m->cy.y + m->cz.z;
|
||||
if ( trace >= 0.0f )
|
||||
{
|
||||
q.v.x = c2.z - c3.y;
|
||||
|
|
@ -337,9 +337,9 @@ b3ClosestApproachResult b3ClosestApproachLines( b3Vec3 p1, b3Vec3 d1, b3Vec3 p2,
|
|||
float s2 = 0.0f;
|
||||
|
||||
result.point1 = b3MulAdd( p1, s1, d1 );
|
||||
result.lambda1 = s1;
|
||||
result.fraction1 = s1;
|
||||
result.point2 = b3MulAdd( p2, s2, d2 );
|
||||
result.lambda2 = s2;
|
||||
result.fraction2 = s2;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
|
@ -348,9 +348,9 @@ b3ClosestApproachResult b3ClosestApproachLines( b3Vec3 p1, b3Vec3 d1, b3Vec3 p2,
|
|||
float s2 = ( a11 * b2 - a21 * b1 ) / det;
|
||||
|
||||
result.point1 = b3MulAdd( p1, s1, d1 );
|
||||
result.lambda1 = s1;
|
||||
result.fraction1 = s1;
|
||||
result.point2 = b3MulAdd( p2, s2, d2 );
|
||||
result.lambda2 = s2;
|
||||
result.fraction2 = s2;
|
||||
return result;
|
||||
}
|
||||
|
||||
|
|
@ -374,9 +374,9 @@ b3ClosestApproachResult b3ClosestApproachSegments( b3Vec3 p1, b3Vec3 q1, b3Vec3
|
|||
{
|
||||
// Both segments degenerate into points
|
||||
result.point1 = p1;
|
||||
result.lambda1 = 0.0f;
|
||||
result.fraction1 = 0.0f;
|
||||
result.point2 = p2;
|
||||
result.lambda2 = 0.0f;
|
||||
result.fraction2 = 0.0f;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
|
@ -387,9 +387,9 @@ b3ClosestApproachResult b3ClosestApproachSegments( b3Vec3 p1, b3Vec3 q1, b3Vec3
|
|||
float s2 = b3ClampFloat( f / e, 0.0f, 1.0f );
|
||||
|
||||
result.point1 = p1;
|
||||
result.lambda1 = 0.0f;
|
||||
result.fraction1 = 0.0f;
|
||||
result.point2 = b3MulAdd( p2, s2, d2 );
|
||||
result.lambda2 = s2;
|
||||
result.fraction2 = s2;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
|
@ -400,9 +400,9 @@ b3ClosestApproachResult b3ClosestApproachSegments( b3Vec3 p1, b3Vec3 q1, b3Vec3
|
|||
float s1 = b3ClampFloat( -c / a, 0.0f, 1.0f );
|
||||
|
||||
result.point1 = b3MulAdd( p1, s1, d1 );
|
||||
result.lambda1 = s1;
|
||||
result.fraction1 = s1;
|
||||
result.point2 = p2;
|
||||
result.lambda2 = 0.0f;
|
||||
result.fraction2 = 0.0f;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
|
@ -425,9 +425,9 @@ b3ClosestApproachResult b3ClosestApproachSegments( b3Vec3 p1, b3Vec3 q1, b3Vec3
|
|||
}
|
||||
|
||||
result.point1 = b3MulAdd( p1, s1, d1 );
|
||||
result.lambda1 = s1;
|
||||
result.fraction1 = s1;
|
||||
result.point2 = b3MulAdd( p2, s2, d2 );
|
||||
result.lambda2 = s2;
|
||||
result.fraction2 = s2;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -45,6 +45,24 @@ typedef struct b3ShapeExtent
|
|||
|
||||
b3TrianglePoint b3ClosestPointOnTriangle( b3Vec3 a, b3Vec3 b, b3Vec3 c, b3Vec3 q );
|
||||
|
||||
// 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 );
|
||||
|
||||
float b3IntersectSegmentTriangle( b3Vec3 p, b3Vec3 q, b3Vec3 a, b3Vec3 b, b3Vec3 c );
|
||||
float b3IntersectSegmentSphere( b3Vec3 p, b3Vec3 q, b3Vec3 c, float r );
|
||||
|
||||
|
|
@ -62,8 +80,6 @@ b3Matrix3 b3Steiner( float mass, b3Vec3 origin );
|
|||
int b3GetProxySupport( const b3ShapeProxy* proxy, b3Vec3 axis );
|
||||
int b3GetPointSupport( const b3Vec3* points, int count, b3Vec3 axis );
|
||||
|
||||
bool b3IntersectRayAndAABB( b3Vec3 lowerBound, b3Vec3 upperBound, b3Vec3 p1, b3Vec3 p2, float* minFraction, float* maxFraction );
|
||||
|
||||
static inline size_t b3AlignUp8( size_t x )
|
||||
{
|
||||
return ( x + 7u ) & ~(size_t)7u;
|
||||
|
|
@ -218,10 +234,10 @@ static inline b3Quat b3IntegrateRotation( b3Quat q1, b3Vec3 deltaRotation )
|
|||
|
||||
// Pseudo angular velocity from a quaternion target
|
||||
// w = 2 * (target - q) * conj(q)
|
||||
static inline b3Vec3 b3DeltaQuatToRotation(b3Quat q, b3Quat target)
|
||||
static inline b3Vec3 b3DeltaQuatToRotation( b3Quat q, b3Quat target )
|
||||
{
|
||||
b3Quat s = q;
|
||||
if (b3DotQuat(q, target) < 0.0f)
|
||||
if ( b3DotQuat( q, target ) < 0.0f )
|
||||
{
|
||||
// Correct polarity
|
||||
s = b3NegateQuat( q );
|
||||
|
|
@ -244,7 +260,7 @@ static inline float b3ScalarTripleProduct( b3Vec3 a, b3Vec3 b, b3Vec3 c )
|
|||
// Get a value by index. Avoid undefined behavior of code like (&v.x)[2].
|
||||
static inline float b3GetByIndex( b3Vec3 v, int index )
|
||||
{
|
||||
B3_ASSERT( 0 <= index && index < 3 );
|
||||
B3_VALIDATE( 0 <= index && index < 3 );
|
||||
float temp[3] = { v.x, v.y, v.z };
|
||||
return temp[index];
|
||||
}
|
||||
|
|
@ -369,7 +385,18 @@ static inline b3Vec2 b3Solve2( b3Matrix2 m, b3Vec2 b )
|
|||
return B3_LITERAL( b3Vec2 ){ 0.0f, 0.0f };
|
||||
}
|
||||
|
||||
static inline b3Vec3 b3ModifiedCross(b3Vec3 a, b3Vec3 b)
|
||||
// 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;
|
||||
|
|
@ -378,15 +405,30 @@ static inline b3Vec3 b3ModifiedCross(b3Vec3 a, b3Vec3 b)
|
|||
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 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;
|
||||
}
|
||||
|
||||
static inline b3Plane b3NormalizePlane( b3Plane plane )
|
||||
{
|
||||
float invLength = 1.0f / b3Length( plane.normal );
|
||||
return B3_LITERAL( b3Plane ){ b3MulSV( invLength, plane.normal ), invLength * plane.offset };
|
||||
return (b3Plane){ b3MulSV( invLength, plane.normal ), invLength * plane.offset };
|
||||
}
|
||||
|
||||
static inline b3Plane b3MakePlaneFromNormalAndPoint( b3Vec3 normal, b3Vec3 point )
|
||||
{
|
||||
return B3_LITERAL( b3Plane ){ normal, b3Dot( normal, point ) };
|
||||
return (b3Plane){ normal, b3Dot( normal, point ) };
|
||||
}
|
||||
|
||||
static inline b3Plane b3MakePlaneFromPoints( b3Vec3 point1, b3Vec3 point2, b3Vec3 point3 )
|
||||
|
|
@ -430,7 +472,8 @@ static inline float b3SignedVolume( b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, b3Vec3 p )
|
|||
// todo eliminate this
|
||||
static inline bool b3IsWithinSegments( const b3ClosestApproachResult* result )
|
||||
{
|
||||
return ( 0.0f <= result->lambda1 && result->lambda1 <= 1.0f ) && ( 0.0f <= result->lambda2 && result->lambda2 <= 1.0f );
|
||||
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 )
|
||||
|
|
@ -446,3 +489,31 @@ static inline b3Matrix3 b3TransformInertia( b3Transform transform, b3Matrix3 cen
|
|||
inertia = b3AddMM( inertia, b3Steiner( mass, transform.p ) );
|
||||
return inertia;
|
||||
}
|
||||
|
||||
// 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 ) };
|
||||
}
|
||||
|
|
|
|||
|
|
@ -288,7 +288,14 @@ static inline bool b3IsBetterCullCandidate( float score, float separation, float
|
|||
return separation < bestSeparation - separationTol;
|
||||
}
|
||||
|
||||
int b3CullPoints( b3Point2D* points, int count )
|
||||
typedef struct b3Point2D
|
||||
{
|
||||
b3Vec2 p;
|
||||
float separation;
|
||||
int originalIndex;
|
||||
} b3Point2D;
|
||||
|
||||
static int b3CullPoints( b3Point2D* points, int count )
|
||||
{
|
||||
if ( count <= 1 )
|
||||
{
|
||||
|
|
|
|||
|
|
@ -764,8 +764,11 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex,
|
|||
}
|
||||
else if ( island->constraintRemoveCount > 0 )
|
||||
{
|
||||
// body wants to sleep but its island needs splitting first
|
||||
if ( body->sleepTime > taskContext->splitSleepTime )
|
||||
// Body wants to sleep but its island needs splitting first. Track the sleepiest candidate.
|
||||
// Break sleep time ties using the island id to ensure determinism. The cross worker reduction
|
||||
// breaks ties the same way.
|
||||
if ( body->sleepTime > taskContext->splitSleepTime ||
|
||||
( body->sleepTime == taskContext->splitSleepTime && body->islandId > taskContext->splitIslandId ) )
|
||||
{
|
||||
// pick the sleepiest candidate
|
||||
taskContext->splitIslandId = body->islandId;
|
||||
|
|
|
|||
|
|
@ -299,7 +299,7 @@ static void b3BuildTriangleAndCapsuleEdgeContact( b3LocalManifold* manifold, con
|
|||
|
||||
b3ClosestApproachResult result = b3ClosestApproachLines( v1, triangleEdge, p1, capsuleEdge );
|
||||
|
||||
if ( result.lambda1 < 0.0f || 1.0f < result.lambda1 || result.lambda2 < 0.0f || 1.0f < result.lambda2 )
|
||||
if ( result.fraction1 < 0.0f || 1.0f < result.fraction1 || result.fraction2 < 0.0f || 1.0f < result.fraction2 )
|
||||
{
|
||||
// closest point beyond end points
|
||||
return;
|
||||
|
|
@ -854,7 +854,7 @@ static void b3CollideHullAndTriangleEdges( b3LocalManifold* manifold, int capaci
|
|||
b3ClosestApproachResult result = b3ClosestApproachLines( pA, eA, pB, eB );
|
||||
|
||||
// Is one of the closest points outside of the associated edge segment?
|
||||
if ( capacity == 0 || result.lambda1 < 0.0f || 1.0f < result.lambda1 || result.lambda2 < 0.0f || 1.0f < result.lambda2 )
|
||||
if ( capacity == 0 || result.fraction1 < 0.0f || 1.0f < result.fraction1 || result.fraction2 < 0.0f || 1.0f < result.fraction2 )
|
||||
{
|
||||
// Invalid edge pair, no points generated
|
||||
B3_ASSERT( manifold->pointCount == 0 );
|
||||
|
|
|
|||
Loading…
Reference in a new issue