Fixes 07 (#97)

#84 
#82 
Added B3_RESTITUTION_ITERATIONS to override restitution iterations.
Fix regression in hull stacking stability.
This commit is contained in:
Erin Catto 2026-07-20 22:05:11 -07:00 • committed by GitHub
commit 23861418d8
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
27 changed files with 877 additions and 448 deletions

View file

@ -84,6 +84,14 @@ option(BOX3D_DISABLE_SIMD "Disable SIMD math (slower)" OFF)
option(BOX3D_COMPILE_WARNING_AS_ERROR "Compile warnings as errors" OFF)
option(BOX3D_DOUBLE_PRECISION "Enable double precision for large worlds" OFF)
# These are read by the library build, so they must precede add_subdirectory(src),
# otherwise the first configure and a re-configure produce different libraries.
# Top level only, so they stay hidden from FetchContent consumers.
if(PROJECT_IS_TOP_LEVEL)
option(BOX3D_PROFILE "Enable profiling with Tracy" OFF)
option(BOX3D_VALIDATE "Enable heavy validation" ON)
endif()
# Emscripten maps the SSE2 path onto wasm SIMD128. simd.h is a header included by
# non-library targets like the tests, so these flags must be directory-wide, not
# PRIVATE to the box3d library.
@ -127,8 +135,6 @@ if(PROJECT_IS_TOP_LEVEL)
option(BOX3D_SAMPLES "Build the Box3D samples" ON)
option(BOX3D_BENCHMARKS "Build the Box3D benchmarks" OFF)
option(BOX3D_DOCS "Build the Box3D documentation" OFF)
option(BOX3D_PROFILE "Enable profiling with Tracy" OFF)
option(BOX3D_VALIDATE "Enable heavy validation" ON)
option(BOX3D_UNIT_TESTS "Build the Box3D unit tests" ON)
# Enable this for shader development using sokol-shdc.

View file

@ -1,6 +1,7 @@
# Collision
Box3D provides geometric types and functions. These include:
- primitives: spheres, capsules, and convex hulls
- triangle meshes and height fields for static terrain
- convex hull construction from point clouds
@ -24,6 +25,7 @@ physics simulation. At a minimum, you should understand how to create primitives
that can later be attached to rigid bodies.
Box3D shape primitives support several operations:
- Test a point or proxy for overlap with the primitive
- Perform a ray cast against the primitive
- Compute the primitive's bounding box
@ -381,6 +383,7 @@ this for stable stacking.
Normally you do not compute manifolds directly — you use the contact data
returned by the simulation. The `b3Manifold` struct contains:
- `normal`: unit normal from shape A toward shape B
- `points[B3_MAX_MANIFOLD_POINTS]`: contact points with separation, impulses,
and feature ids
@ -401,15 +404,16 @@ b3CollideHulls(&manifold, B3_MAX_MANIFOLD_POINTS,
```
Available collide functions:
- `b3CollideSpheres`
- `b3CollideCapsuleAndSphere`
- `b3CollideCapsules`
- `b3CollideHullAndSphere`
- `b3CollideHullAndCapsule`
- `b3CollideHulls`
- `b3CollideCapsuleAndTriangle`
- `b3CollideHullAndTriangle`
- `b3CollideSphereAndTriangle`
- `b3CollideTriangleAndCapsule`
- `b3CollideTriangleAndHull`
- `b3CollideTriangleAndSphere`
The SAT cache in `b3CollideHulls` warm-starts the separating axis search between
frames, the same idea as the simplex cache for GJK distance.

View file

@ -44,7 +44,8 @@ B3_API uint64_t b3DynamicTree_GetCategoryBits( b3DynamicTree* tree, int proxyId
B3_API b3TreeStats b3DynamicTree_Query( const b3DynamicTree* tree, b3AABB aabb, uint64_t maskBits, bool requireAllBits,
b3TreeQueryCallbackFcn* callback, void* context );
/// Query an AABB for the closest object. The callback function is called for each proxy that might be closest to the supplied point.
/// Query an AABB for the closest object. The callback function is called for each proxy that might be closest to the supplied
/// point.
/// @param tree the dynamic tree to query
/// @param point the query point
/// @param maskBits nodes are skipped if the bit-wise AND with the node category bits is zero
@ -634,17 +635,17 @@ B3_API void b3CollideHullAndCapsule( b3LocalManifold* manifold, int capacity, co
B3_API void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB,
b3Transform transformBtoA, b3SATCache* cache );
/// Collide a capsule and a triangle.
B3_API void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA,
const b3Vec3* triangleB, b3SimplexCache* cache );
/// Collide a triangle and capsule. Normal points from triangle to capsule.
B3_API void b3CollideTriangleAndCapsule( b3LocalManifold* manifold, int capacity, const b3Vec3* triangleA,
const b3Capsule* capsuleB, b3SimplexCache* cache );
/// Collide a hull and a triangle.
B3_API void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, b3Vec3 v1, b3Vec3 v2,
b3Vec3 v3, int triangleFlags, b3SATCache* cache, bool enableSpeculative );
/// Collide a triangle and hull. Normal points from triangle to hull.
B3_API void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, int triangleFlags,
const b3HullData* hullB, b3SATCache* cache, bool enableSpeculative );
/// Collide a sphere and a triangle.
B3_API void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const b3Sphere* sphereA,
const b3Vec3* triangleB );
/// Collide a triangle and sphere. Normal points from triangle to sphere.
B3_API void b3CollideTriangleAndSphere( b3LocalManifold* manifold, int capacity, const b3Vec3* triangleA,
const b3Sphere* sphereB );
/**@}*/ // collision

View file

@ -31,3 +31,5 @@
// Enable assertions when NDEBUG is defined
// #define B3_ENABLE_ASSERT
// #define B3_RESTITUTION_ITERATIONS

View file

@ -131,3 +131,9 @@ B3_API float b3GetStallThreshold( void );
#define B3_CHILD_POWER ( 64 - 2 * B3_SHAPE_POWER )
#define B3_MAX_SHAPES ( 1 << B3_SHAPE_POWER )
#define B3_MAX_CHILD_SHAPES ( 1 << B3_CHILD_POWER )
/// Increase this if your application needs more accurate restitution. Doing so will
/// slow down the simulation. Must be 1 or more.
#ifndef B3_RESTITUTION_ITERATIONS
#define B3_RESTITUTION_ITERATIONS 1
#endif

View file

@ -98,8 +98,8 @@ void CharacterMover::SolveMove( float timeStep, b3Vec3 forward, b3Vec3 right, b3
float drop = control * m_friction * timeStep;
float newSpeed = b3MaxFloat( 0.0f, speed - drop );
float ratio = newSpeed / speed;
m_velocity.x *= newSpeed / speed;
m_velocity.z *= newSpeed / speed;
m_velocity.x *= ratio;
m_velocity.z *= ratio;
}
float maxSpeed = m_sprint ? 1.5f * m_maxSpeed : m_maxSpeed;

View file

@ -1,13 +1,12 @@
// SPDX-FileCopyrightText: 2025 Erin Catto
// SPDX-License-Identifier: MIT
#include "gfx/debug_adapter.h"
#include "gfx/draw.h"
#include "imgui.h"
#include "sample.h"
#include "gfx/draw.h"
#include "utils.h"
#include "gfx/debug_adapter.h"
#include "box3d/box3d.h"
#include <stdio.h>
@ -442,7 +441,7 @@ public:
m_runCount = 0;
m_failure = false;
m_autoGenerate = false;
m_collide = true;
m_stepWhilePaused = true;
Generate();
@ -549,13 +548,16 @@ public:
// Don't allow shapes to collide with each other. This
// makes isolating failures easier.
shapeDef.filter.categoryBits = 2;
shapeDef.filter.maskBits = 1;
if ( m_collide == false )
{
shapeDef.filter.categoryBits = 2;
shapeDef.filter.maskBits = 1;
}
g_randomSeed = (uint32_t)b3GetTicks();
bool simulateAll = true;
//g_randomSeed = 1910133196;
// g_randomSeed = 1910133196;
m_runCount += 1;
m_stepCount = 0;
@ -624,6 +626,11 @@ public:
Generate();
}
if ( ImGui::Checkbox( "Collide", &m_collide ) )
{
Generate();
}
if ( ImGui::Button( "Generate" ) )
{
Generate();
@ -645,8 +652,7 @@ public:
{
PickRay pickRay = m_camera->BuildPickRay( m_context->mouseX, m_context->mouseY );
b3RayResult result =
b3World_CastRayClosest( m_worldId, pickRay.origin, pickRay.translation, b3DefaultQueryFilter() );
b3RayResult result = b3World_CastRayClosest( m_worldId, pickRay.origin, pickRay.translation, b3DefaultQueryFilter() );
if ( result.hit )
{
@ -736,6 +742,7 @@ public:
int m_runCount;
bool m_failure;
bool m_autoGenerate;
bool m_collide;
};
static int sampleMeshDrop = RegisterSample( "Continuous", "Mesh Drop", MeshDrop::Create );
@ -862,13 +869,12 @@ public:
if ( context->restart == false )
{
m_camera->SetView( 0.0f, 15.0f, 50.0f, { 0.0f, 15.0f, 0.0f } );
}
AddGroundBox( 40.0f );
b3BoxHull box = b3MakeBoxHull( 0.5f, 10.0f, 0.5f );
b3Capsule capsule = {{0.0f, -9.5f, 0.0f}, {0.0f, 9.5f, 0.0f}, 0.5f};
b3Capsule capsule = { { 0.0f, -9.5f, 0.0f }, { 0.0f, 9.5f, 0.0f }, 0.5f };
bool useCapsule = false;
@ -881,7 +887,7 @@ public:
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
if (useCapsule)
if ( useCapsule )
{
b3CreateCapsuleShape( bodyId, &shapeDef, &capsule );
}

View file

@ -35,7 +35,7 @@ public:
{
Sample::Step( );
if ( m_done == false )
if ( m_done == false && (m_context->pause == false || m_context->singleStep > 0) )
{
m_done = UpdateFallingRagdolls( m_worldId, &m_data );
if (m_done)

View file

@ -1146,3 +1146,160 @@ public:
};
static int sampleWheelStack = RegisterSample( "Issues", "GMod Wheel Stack", WheelStack::Create );
class RestitutionOvershoot : public Sample
{
public:
static constexpr float m_boxHalf = 0.5f;
static constexpr float m_floorHalfXZ = 0.375f;
static constexpr float m_floorHalfY = 0.25f;
static constexpr float m_dropHeight = 10.0f;
static constexpr float m_tolerance = 0.05f;
explicit RestitutionOvershoot( SampleContext* context )
: Sample( context )
{
if ( context->restart == false )
{
m_camera->SetView( 20.0f, 0.0f, 28.0f, { 0.0f, m_dropHeight + m_boxHalf, 0.0f } );
}
b3BoxHull floor = b3MakeBoxHull( m_floorHalfXZ, m_floorHalfY, m_floorHalfXZ );
b3BodyDef floorDef = b3DefaultBodyDef();
floorDef.type = b3_staticBody;
floorDef.position = { 0.0f, -m_floorHalfY, 0.0f };
b3BodyId floorBody = b3CreateBody( m_worldId, &floorDef );
b3ShapeDef floorShape = b3DefaultShapeDef();
b3CreateHullShape( floorBody, &floorShape, &floor.base );
b3BoxHull box = b3MakeBoxHull( m_boxHalf, m_boxHalf, m_boxHalf );
b3BodyDef boxDef = b3DefaultBodyDef();
boxDef.type = b3_dynamicBody;
boxDef.position = { 0.0f, m_dropHeight, 0.0f };
m_boxBody = b3CreateBody( m_worldId, &boxDef );
b3ShapeDef boxShape = b3DefaultShapeDef();
boxShape.baseMaterial.restitution = 1.0f;
b3CreateHullShape( m_boxBody, &boxShape, &box.base );
m_currentY = m_dropHeight;
m_maxBounceY = 0.0f;
m_bounced = false;
m_failed = false;
}
void Step() override
{
Sample::Step();
if ( B3_IS_NON_NULL( m_boxBody ) == false )
{
return;
}
b3Pos position = b3Body_GetPosition( m_boxBody );
m_currentY = position.y;
b3Vec3 velocity = b3Body_GetLinearVelocity( m_boxBody );
if ( m_bounced == false && velocity.y > 0.0f )
{
m_bounced = true;
}
if ( m_bounced )
{
if ( position.y > m_maxBounceY )
{
m_maxBounceY = position.y;
}
if ( position.y > m_dropHeight + m_tolerance )
{
m_failed = true;
}
}
b3Pos markerPoint = { 0.0f, m_dropHeight + m_boxHalf, 0.0f };
DrawPlane( b3Vec3_axisY, markerPoint, MakeColor( b3_colorYellow ) );
DrawTextLine( "drop height = %.2f m", m_dropHeight );
DrawTextLine( "current y = %.2f m", m_currentY );
DrawTextLine( "max bounce = %.2f m", m_maxBounceY );
if ( m_bounced == false )
{
DrawTextLine( "waiting for first bounce..." );
}
else if ( m_failed )
{
DrawTextLine( "FAIL: box exceeded drop height" );
}
else
{
DrawTextLine( "PASS: bounce stays at or below drop height" );
}
}
static Sample* Create( SampleContext* context )
{
return new RestitutionOvershoot( context );
}
b3BodyId m_boxBody = {};
float m_currentY = 0.0f;
float m_maxBounceY = 0.0f;
bool m_bounced = false;
bool m_failed = false;
};
static int sampleRestitutionOvershoot = RegisterSample( "Issues", "Restitution Overshoot", RestitutionOvershoot::Create );
class SlideTwistOffCenterShape : public Sample
{
public:
explicit SlideTwistOffCenterShape( SampleContext* context )
: Sample( context )
{
if ( context->restart == false )
{
m_camera->SetView( -30.0f, 17.0f, 30.0f, { 0.0f, 5.0f, 0.0f } );
}
AddGroundBox( 50.0f );
b3Quat orientation = b3MakeQuatFromAxisAngle( b3Vec3_axisX, 20.0f * B3_DEG_TO_RAD );
b3BodyDef bodyDef = b3DefaultBodyDef();
b3ShapeDef shapeDef = b3DefaultShapeDef();
bodyDef.position = { 0.0f, 4.0f, 0.0f };
bodyDef.rotation = orientation;
b3BodyId planeBody = b3CreateBody( m_worldId, &bodyDef );
b3BoxHull plane = b3MakeBoxHull( 10.0f, 0.5f, 10.0f );
shapeDef.baseMaterial.friction = 0.6f;
b3CreateHullShape( planeBody, &shapeDef, &plane.base );
b3Vec3 boxLocalCenter = { 1.0f, 0.5f, 1.0f };
b3Vec3 boxOffset = b3RotateVector( orientation, boxLocalCenter );
bodyDef.type = b3_dynamicBody;
bodyDef.position = { -boxOffset.x, 5.0f - boxOffset.y, -boxOffset.z };
bodyDef.rotation = orientation;
//bodyDef.angularVelocity = 25.0f * b3RotateVector( orientation, b3Vec3_axisY );
b3BodyId boxBody = b3CreateBody( m_worldId, &bodyDef );
b3BoxHull mBox = b3MakeOffsetBoxHull( 1.0f, 0.5f, 1.0f, boxLocalCenter );
shapeDef.baseMaterial.friction = 0.3f;
b3CreateHullShape( boxBody, &shapeDef, &mBox.base );
b3Body_SetAngularVelocity( boxBody, 25.0f * b3RotateVector( orientation, b3Vec3_axisY ) );
}
static Sample* Create( SampleContext* context )
{
return new SlideTwistOffCenterShape( context );
}
};
static int sampleSlideTwistOffCenterShape =
RegisterSample( "Issues", "Slide Twist Off Center Shape", SlideTwistOffCenterShape::Create );

View file

@ -517,7 +517,7 @@ public:
b3TransformPoint( xf, m_triangle[2] ),
};
b3CollideSphereAndTriangle( &m_manifold, m_pointCapacity, &m_sphere, localTriangle );
b3CollideTriangleAndSphere( &m_manifold, m_pointCapacity, localTriangle, &m_sphere );
}
b3Sphere m_sphere;
@ -668,7 +668,7 @@ public:
b3TransformPoint( xf, m_triangle[2] ),
};
b3CollideCapsuleAndTriangle( &m_manifold, m_pointCapacity, &m_capsule, localTriangle, &m_simplexCache );
b3CollideTriangleAndCapsule( &m_manifold, m_pointCapacity, localTriangle, &m_capsule, &m_simplexCache );
}
b3Capsule m_capsule;
@ -913,8 +913,8 @@ public:
b3TransformPoint( xf, m_triangle[2] ),
};
b3CollideHullAndTriangle( &m_manifold, m_pointCapacity, m_hull, localTriangle[0], localTriangle[1], localTriangle[2],
m_flags, &m_satCache, true );
b3CollideTriangleAndHull( &m_manifold, m_pointCapacity, localTriangle[0], localTriangle[1], localTriangle[2],
m_flags, m_hull, &m_satCache, true );
}
int m_flags;

View file

@ -1,10 +1,11 @@
// SPDX-FileCopyrightText: 2025 Erin Catto
// SPDX-License-Identifier: MIT
#include "gfx/draw.h"
#include "human.h"
#include "imgui.h"
#include "sample.h"
#include "gfx/draw.h"
#include "utils.h"
#include "box3d/box3d.h"
@ -232,9 +233,14 @@ public:
m_groundMesh = b3CreateGridMesh( 20, 20, 1.0f, 1, true );
b3CreateMeshShape( groundId, &shapeDef, m_groundMesh, b3Vec3_one );
g_randomSeed = 42;
float a = 0.1f * e_count;
b3Vec3 lower = { -a, -a, -a };
b3Vec3 upper = { a, a, a };
for ( int i = 0; i < e_count; ++i )
{
b3Pos position = { 0.1f * i, 2.0f + 0.5f * i, -0.1f * i };
b3Vec3 offset = RandomVec3( lower, upper );
b3Pos position = { offset.x, 2.0f, offset.z };
float torque = 10.0f;
float hertz = 0.5f;
float damping = 0.7f;

View file

@ -64,7 +64,7 @@ public:
m_camera->SetView( -140.0f, 17.0f, 60.0f, { 0.0f, 7.5f, 0.0f } );
}
AddGroundBox( 50.0f );
AddGroundBox( 80.0f );
b3BodyDef bodyDef = b3DefaultBodyDef();
b3ShapeDef shapeDef = b3DefaultShapeDef();

View file

@ -210,7 +210,7 @@ public:
bodyDef.name = "cube";
bodyDef.type = b3_dynamicBody;
bodyDef.position = { 0.0f, 0.5f, 0.0f };
//bodyDef.angularVelocity = { 0.0f, 10.0f, 0.0f };
// bodyDef.angularVelocity = { 0.0f, 10.0f, 0.0f };
m_bodyId = b3CreateBody( m_worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
@ -409,7 +409,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 35.0f, 15.0f, 30.0f, { 0.0f, 10.0f, 0.0f } );
m_camera->SetView( 35.0f, 15.0f, 12.0f, { 0.0f, 2.0f, 0.0f } );
}
m_shapeType = b3_hullShape;
@ -418,42 +418,43 @@ public:
void CreateStack()
{
AddGroundBox( 60.0f );
AddGroundBox( 20.0f );
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.baseMaterial.rollingResistance = m_shapeType == b3_capsuleShape ? 0.1f : 0.05f;
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
float h = 1.0f;
float r = 0.1f;
b3BoxHull box = b3MakeBoxHull( h, r, r );
b3Capsule capsule = { { -h, 0.0f, 0.0f }, { h, 0.0f, 0.0f }, r };
int count = 30;
for ( int i = 0; i < count; ++i )
{
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.baseMaterial.rollingResistance = m_shapeType == b3_capsuleShape ? 0.1f : 0.01f;
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
float alpha = ( i & 1 ) == 1 ? 0.0f : 0.5f * B3_PI;
b3BoxHull box = b3MakeBoxHull( 2.5f, 0.25f, 0.25f );
b3Capsule capsule = { { -2.5f, 0.0f, 0.0f }, { 2.5f, 0.0f, 0.0f }, 0.25f };
float x = ( i & 1 ) == 0 ? h - 2.0f * r : 0.0f;
float z = ( i & 1 ) == 0 ? 0.0f : h - 2.0f * r;
for ( int i = 0; i < m_size; ++i )
bodyDef.position = { x, ( 2.1f * i + 0.5f ) * r, z };
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisY, alpha );
b3BodyId boxBody1 = b3CreateBody( m_worldId, &bodyDef );
bodyDef.position = { -x, ( 2.1f * i + 0.5f ) * r, -z };
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisY, alpha );
b3BodyId boxBody2 = b3CreateBody( m_worldId, &bodyDef );
if ( m_shapeType == b3_capsuleShape )
{
float alpha = ( i & 1 ) == 1 ? 0.0f : 0.5f * B3_PI;
float x = ( i & 1 ) == 0 ? 1.75f : 0.0f;
float z = ( i & 1 ) == 0 ? 0.0f : 1.75f;
bodyDef.position = { x, 0.5f * i + 0.25f, z };
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisY, alpha );
b3BodyId boxBody1 = b3CreateBody( m_worldId, &bodyDef );
bodyDef.position = { -x, 0.5f * i + 0.25f, -z };
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisY, alpha );
b3BodyId boxBody2 = b3CreateBody( m_worldId, &bodyDef );
if ( m_shapeType == b3_capsuleShape )
{
b3CreateCapsuleShape( boxBody1, &shapeDef, &capsule );
b3CreateCapsuleShape( boxBody2, &shapeDef, &capsule );
}
else
{
b3CreateHullShape( boxBody1, &shapeDef, &box.base );
b3CreateHullShape( boxBody2, &shapeDef, &box.base );
}
b3CreateCapsuleShape( boxBody1, &shapeDef, &capsule );
b3CreateCapsuleShape( boxBody2, &shapeDef, &capsule );
}
else
{
b3CreateHullShape( boxBody1, &shapeDef, &box.base );
b3CreateHullShape( boxBody2, &shapeDef, &box.base );
}
}
}
@ -484,7 +485,6 @@ public:
return new JengaStack( context );
}
static constexpr int m_size = 40;
b3ShapeType m_shapeType;
};
@ -811,7 +811,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 0.0f, 30.0f, 50.0f, { 0.0f, 5.0f, 0.0f } );
m_camera->SetView( 40.0f, 15.0f, 50.0f, { 0.0f, 12.0f, 0.0f } );
}
AddGroundBox( 40.0f );
@ -847,3 +847,93 @@ public:
};
static int samplePyramid2D = RegisterSample( "Stacking", "Pyramid2D", Pyramid2D::Create );
// This stresses the edge-edge handling.
class EdgeCrossing : public Sample
{
public:
explicit EdgeCrossing( SampleContext* context )
: Sample( context )
{
if ( context->restart == false )
{
m_camera->SetView( 0.0f, 25.0f, 10.0f, b3Pos_zero );
}
AddGroundBox( 40.0f );
b3Vec3 h = { 0.2f, 0.02f, 0.04f };
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
b3ShapeDef shapeDef = b3DefaultShapeDef();
bodyDef.position.x = -10.0f;
bodyDef.position.z = -2.0f;
b3BoxHull box1 = b3MakeBoxHull( h.x, h.y, h.z );
b3BoxHull box2 = b3MakeBoxHull( h.x, h.z, h.y );
b3Vec3 axis = b3Normalize( { 0.1f, 0.9f, 0.0f } );
for ( float angle = -B3_PI; angle < B3_PI + 0.001f; angle += 0.1f * B3_PI )
{
bodyDef.position.y = h.y;
bodyDef.rotation = b3Quat_identity;
b3BodyId bodyId1 = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( bodyId1, &shapeDef, &box1.base );
bodyDef.position.y = 20.0f * h.y;
bodyDef.rotation = b3MakeQuatFromAxisAngle( axis, angle );
b3BodyId boxBody2 = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( boxBody2, &shapeDef, &box1.base );
bodyDef.position.x += 1.0f;
}
bodyDef.position.x = -10.0f;
bodyDef.position.z = 0.0f;
for ( float angle = -B3_PI; angle < B3_PI + 0.001f; angle += 0.1f * B3_PI )
{
bodyDef.position.y = h.z;
bodyDef.rotation = b3Quat_identity;
b3BodyId bodyId1 = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( bodyId1, &shapeDef, &box2.base );
bodyDef.position.y = 20.0f * h.z;
bodyDef.rotation = b3MakeQuatFromAxisAngle( axis, angle );
b3BodyId boxBody2 = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( boxBody2, &shapeDef, &box2.base );
bodyDef.position.x += 1.0f;
}
bodyDef.position.x = -10.0f;
bodyDef.position.z = 2.0f;
for ( float angle = -B3_PI; angle < B3_PI + 0.001f; angle += 0.1f * B3_PI )
{
bodyDef.position.y = h.y;
bodyDef.rotation = b3Quat_identity;
b3BodyId bodyId1 = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( bodyId1, &shapeDef, &box1.base );
bodyDef.position.y = 20.0f * h.y;
bodyDef.rotation = b3MakeQuatFromAxisAngle( axis, angle );
b3BodyId boxBody2 = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( boxBody2, &shapeDef, &box2.base );
bodyDef.position.x += 1.0f;
}
}
static Sample* Create( SampleContext* context )
{
return new EdgeCrossing( context );
}
};
static int sampleEdgeCrossing = RegisterSample( "Stacking", "Edge Crossing", EdgeCrossing::Create );

View file

@ -4,7 +4,8 @@
#include "utils.h"
#if defined( _WIN64 )
#include <Windows.h>
// Lower-case windows.h intentionally for cross compiling on mingw.
#include <windows.h>
#elif defined( __APPLE__ )
#include <unistd.h>
#elif defined( __linux__ )

View file

@ -154,7 +154,8 @@ if (BOX3D_PROFILE)
endif()
if(BOX3D_VALIDATE)
message(STATUS "Box3D validation ON")
# Only effective when NDEBUG is undefined, see base.h
message(STATUS "Box3D validation ON (debug builds)")
target_compile_definitions(box3d PRIVATE BOX3D_VALIDATE)
endif()

View file

@ -1558,7 +1558,7 @@ void b3WarmStartContacts_Convex( b3SolverBlock block, b3StepContext* context )
int pointCount1 = b3MaxInt( c->pointCounts[0], c->pointCounts[1] );
int pointCount2 = b3MaxInt( c->pointCounts[2], c->pointCounts[3] );
int pointCount = b3MaxInt( pointCount1, pointCount2 );
B3_VALIDATE( 0 < pointCount && pointCount <= B3_SIMD_WIDTH );
B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS );
// Normal impulses
for ( int j = 0; j < pointCount; ++j )
@ -1632,7 +1632,7 @@ void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context, bool u
int pointCount1 = b3MaxInt( c->pointCounts[0], c->pointCounts[1] );
int pointCount2 = b3MaxInt( c->pointCounts[2], c->pointCounts[3] );
int pointCount = b3MaxInt( pointCount1, pointCount2 );
B3_VALIDATE( 0 < pointCount && pointCount <= B3_SIMD_WIDTH );
B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS );
b3BodyStateW bA = b3GatherBodies( states, c->indexA );
b3BodyStateW bB = b3GatherBodies( states, c->indexB );
@ -1847,7 +1847,7 @@ void b3ApplyRestitution_Convex( b3SolverBlock block, b3StepContext* context )
int pointCount1 = b3MaxInt( c->pointCounts[0], c->pointCounts[1] );
int pointCount2 = b3MaxInt( c->pointCounts[2], c->pointCounts[3] );
int pointCount = b3MaxInt( pointCount1, pointCount2 );
B3_VALIDATE( 0 < pointCount && pointCount <= B3_SIMD_WIDTH );
B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS );
// Single gather for all manifolds
b3BodyStateW bA = b3GatherBodies( states, c->indexA );

View file

@ -1,4 +1,4 @@
// SPDX-FileCopyrightText: 2025 Erin Catto
// SPDX-FileCopyrightText: 2026 Erin Catto
// SPDX-License-Identifier: MIT
#include "algorithm.h"
@ -82,8 +82,8 @@ static int b3ClipSegmentToHullFace( b3ClipVertex segment[2], const b3HullData* h
return 2;
}
static b3FaceQuery b3QueryFaceDirectionHullAndCapsule( const b3HullData* hull, const b3Capsule* capsule,
b3Transform capsuleTransform )
static b3SeparatingAxis b3QueryFaceDirectionHullAndCapsule( const b3HullData* hull, const b3Capsule* capsule,
b3Transform capsuleTransform )
{
int maxFaceIndex = -1;
int maxVertexIndex = -1;
@ -110,15 +110,16 @@ static b3FaceQuery b3QueryFaceDirectionHullAndCapsule( const b3HullData* hull, c
}
}
return (b3FaceQuery){
return (b3SeparatingAxis){
.normal = planes[maxFaceIndex].normal,
.separation = maxFaceSeparation,
.faceIndex = (uint8_t)maxFaceIndex,
.vertexIndex = (uint8_t)maxVertexIndex,
.indexA = (uint8_t)maxFaceIndex,
.indexB = (uint8_t)maxVertexIndex,
};
}
static b3EdgeQuery b3QueryEdgeDirectionHullAndCapsule( const b3HullData* hull, const b3Capsule* capsule,
b3Transform capsuleTransform )
static b3SeparatingAxis b3QueryEdgeDirectionHullAndCapsule( const b3HullData* hull, const b3Capsule* capsule,
b3Transform capsuleTransform )
{
// Find axis of minimum penetration
b3Vec3 maxNormal = b3Vec3_zero;
@ -194,7 +195,7 @@ static b3EdgeQuery b3QueryEdgeDirectionHullAndCapsule( const b3HullData* hull, c
}
// Save result
return (b3EdgeQuery){
return (b3SeparatingAxis){
.normal = maxNormal,
.separation = maxSeparation,
.indexA = maxIndexA,
@ -704,13 +705,13 @@ void b3CollideCapsules( b3LocalManifold* manifold, int capacity, const b3Capsule
}
static bool b3BuildHullFaceAndCapsuleContact( b3LocalManifold* manifold, const b3HullData* hullA, const b3Capsule* capsuleB,
b3Transform transformBtoA, b3FaceQuery query )
b3Transform transformBtoA, b3SeparatingAxis query )
{
// Work in shapeA coordinates
const b3Plane* planes = b3GetHullPlanes( hullA );
// Clip the capsule edge against the side planes of the reference face
int refFace = query.faceIndex;
int refFace = query.indexA;
b3Plane refPlane = planes[refFace];
b3ClipVertex segmentB[2];
@ -771,7 +772,7 @@ static inline float b3DeepestPointSeparation( const b3LocalManifold* manifold )
}
static bool b3BuildHullAndCapsuleEdgeContact( b3LocalManifold* manifold, int capacity, const b3HullData* hullA,
const b3Capsule* capsuleB, b3Transform transformBtoA, b3EdgeQuery query )
const b3Capsule* capsuleB, b3Transform transformBtoA, b3SeparatingAxis query )
{
if ( capacity < 1 )
{
@ -916,14 +917,14 @@ void b3CollideHullAndCapsule( b3LocalManifold* manifold, int capacity, const b3H
// Deep penetration
b3FaceQuery faceQuery = b3QueryFaceDirectionHullAndCapsule( hullA, capsuleB, transformBtoA );
b3SeparatingAxis faceQuery = b3QueryFaceDirectionHullAndCapsule( hullA, capsuleB, transformBtoA );
if ( faceQuery.separation > capsuleB->radius )
{
// We found a separating axis
return;
}
b3EdgeQuery edgeQuery = b3QueryEdgeDirectionHullAndCapsule( hullA, capsuleB, transformBtoA );
b3SeparatingAxis edgeQuery = b3QueryEdgeDirectionHullAndCapsule( hullA, capsuleB, transformBtoA );
if ( edgeQuery.separation > capsuleB->radius )
{
// We found a separating axis
@ -999,20 +1000,24 @@ static int b3BuildPolygon( b3ClipVertex* out, b3Transform transform, const b3Hul
}
static bool b3BuildFaceAContact( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB,
b3Transform transformBtoA, b3FaceQuery query, b3SATCache* cache )
b3Transform transformBtoA, b3SeparatingAxis query, b3SATCache* cache )
{
B3_VALIDATE( query.type == b3_faceAxisA );
B3_VALIDATE( 0 <= query.indexA && query.indexA < hullA->faceCount );
B3_VALIDATE( 0 <= query.indexB && query.indexB < hullB->vertexCount );
const b3HullFace* facesA = b3GetHullFaces( hullA );
const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA );
const b3Plane* planesA = b3GetHullPlanes( hullA );
const b3Vec3* pointsA = b3GetHullPoints( hullA );
// Reference face
int refFace = query.faceIndex;
int refFace = query.indexA;
b3Plane refPlane = planesA[refFace];
// Find incident face
b3Vec3 refNormalInB = b3InvRotateVector( transformBtoA.q, refPlane.normal );
int incFace = b3FindIncidentFace( hullB, refNormalInB, query.vertexIndex );
int incFace = b3FindIncidentFace( hullB, refNormalInB, query.indexB );
// Build clip polygon from incident face in frame A
b3ClipVertex buffer1[B3_MAX_CLIP_POINTS], buffer2[B3_MAX_CLIP_POINTS];
@ -1089,19 +1094,30 @@ static bool b3BuildFaceAContact( b3LocalManifold* manifold, int capacity, const
// Save cache
cache->separation = minSeparation;
cache->type = (uint8_t)b3_faceAxisA;
cache->indexA = (uint8_t)query.faceIndex;
cache->indexB = (uint8_t)query.vertexIndex;
cache->indexA = (uint8_t)query.indexA;
cache->indexB = (uint8_t)query.indexB;
return true;
}
static bool b3BuildFaceBContact( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB,
b3Transform transformBtoA, b3FaceQuery query, b3SATCache* cache )
b3Transform transformBtoA, b3SeparatingAxis query, b3SATCache* cache )
{
B3_VALIDATE( query.type == b3_faceAxisB );
b3Transform transformAtoB = b3InvertTransform( transformBtoA );
bool touching = b3BuildFaceAContact( manifold, capacity, hullB, hullA, transformAtoB, query, cache );
b3SeparatingAxis flippedQuery = {
.normal = b3Neg( query.normal ),
.separation = query.separation,
.indexA = query.indexB,
.indexB = query.indexA,
.type = b3_faceAxisA,
};
bool touching = b3BuildFaceAContact( manifold, capacity, hullB, hullA, transformAtoB, flippedQuery, cache );
if ( touching == false )
{
*cache = (b3SATCache){ 0 };
return false;
}
@ -1110,9 +1126,6 @@ static bool b3BuildFaceBContact( b3LocalManifold* manifold, int capacity, const
// Transform and flip normal so it points from A to B, even though the B has the reference face.
manifold->normal = b3Neg( b3MulMV( matrix, manifold->normal ) );
cache->type = (uint8_t)b3_faceAxisB;
cache->indexA = (uint8_t)query.vertexIndex;
cache->indexB = (uint8_t)query.faceIndex;
// Transform points from frame B to frame A.
// Also flip the pairs to ensure correct matches.
@ -1123,12 +1136,20 @@ static bool b3BuildFaceBContact( b3LocalManifold* manifold, int capacity, const
pt->pair = b3FlipPair( pt->pair );
}
cache->type = (uint8_t)b3_faceAxisB;
cache->indexA = (uint8_t)query.indexA;
cache->indexB = (uint8_t)query.indexB;
return true;
}
static bool b3BuildEdgeContact( b3LocalManifold* manifold, const b3HullData* hullA, const b3HullData* hullB,
b3Transform transformBtoA, b3EdgeQuery query, b3SATCache* cache )
b3Transform transformBtoA, b3SeparatingAxis query, b3SATCache* cache )
{
B3_VALIDATE( query.type == b3_edgePairAxis );
B3_VALIDATE( 0 <= query.indexA && query.indexA < hullA->edgeCount );
B3_VALIDATE( 0 <= query.indexB && query.indexB < hullB->edgeCount );
// Work in shapeA coordinates
const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA );
const b3Vec3* pointsA = b3GetHullPoints( hullA );
@ -1295,22 +1316,39 @@ static inline void b3GetSupportWide( b3Vec3 normal, const float* vx, const float
// SIMD separating axis test based on an implementation developed by Cairn Overturf.
// See his article: https://cairno.substack.com/p/improvements-to-the-separating-axis
b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, int axisOverride )
b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, bool earlyReturn )
{
B3_VALIDATE( axisOverride == b3_invalidAxis || axisOverride == b3_manualFaceAxisA || axisOverride == b3_manualFaceAxisB ||
axisOverride == b3_manualEdgePairAxis );
b3Matrix3 R = b3MakeMatrixFromQuat( xfB.q );
b3Matrix3 invR = b3Transpose( R );
float speculativeDistance = B3_SPECULATIVE_DISTANCE;
b3AxisQuery res = {
.normal = b3Vec3_zero,
.separation = -INFINITY,
.indexA = B3_NULL_INDEX,
.indexB = B3_NULL_INDEX,
.type = b3_invalidAxis,
.faceA =
{
.normal = b3Vec3_zero,
.separation = -INFINITY,
.indexA = B3_NULL_INDEX,
.indexB = B3_NULL_INDEX,
.type = b3_faceAxisA,
},
.faceB =
{
.normal = b3Vec3_zero,
.separation = -INFINITY,
.indexA = B3_NULL_INDEX,
.indexB = B3_NULL_INDEX,
.type = b3_faceAxisB,
},
.edge =
{
.normal = b3Vec3_zero,
.separation = -INFINITY,
.indexA = B3_NULL_INDEX,
.indexB = B3_NULL_INDEX,
.type = b3_edgePairAxis,
},
.separatedFeature = b3_invalidAxis,
};
int faceCountA = hullA->faceCount;
@ -1325,38 +1363,30 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData*
b3Vec3 hB = b3AABB_Extents( hullB->aabb );
// Test A's face planes against B's vertices.
if ( axisOverride != b3_manualFaceAxisB && axisOverride != b3_manualEdgePairAxis )
for ( int i = 0; i < faceCountA; ++i )
{
for ( int i = 0; i < faceCountA; ++i )
b3Plane plane = planesA[i];
b3Vec3 direction = b3Neg( b3MulMV( invR, plane.normal ) );
float planeSeparation = b3Dot( plane.normal, xfB.p ) - plane.offset;
float biasB = b3Dot( direction, cB ) + 1.0625f * b3Dot( b3Abs( direction ), hB );
float support;
int vertexIndex;
b3GetSupportWide( direction, vxB, vyB, vzB, soaVertexCountB, biasB, &support, &vertexIndex );
float separation = planeSeparation - support;
if ( separation > res.faceA.separation )
{
b3Plane plane = planesA[i];
b3Vec3 direction = b3Neg( b3MulMV( invR, plane.normal ) );
float planeSeparation = b3Dot( plane.normal, xfB.p ) - plane.offset;
float biasB = b3Dot( direction, cB ) + 1.0625f * b3Dot( b3Abs( direction ), hB );
float support;
int vertexIndex;
b3GetSupportWide( direction, vxB, vyB, vzB, soaVertexCountB, biasB, &support, &vertexIndex );
float separation = planeSeparation - support;
if ( separation > res.separation )
res.faceA.normal = plane.normal;
res.faceA.separation = separation;
res.faceA.indexA = i;
res.faceA.indexB = vertexIndex;
if ( separation > speculativeDistance && earlyReturn )
{
res.type = b3_faceAxisA;
res.separation = separation;
res.indexA = i;
res.indexB = vertexIndex;
res.normal = plane.normal;
if ( separation > speculativeDistance )
{
return res;
}
res.separatedFeature = b3_faceAxisA;
return res;
}
}
}
if ( axisOverride == b3_manualFaceAxisA )
{
return res;
}
int faceCountB = hullB->faceCount;
const b3Plane* planesB = b3GetHullPlanes( hullB );
@ -1369,39 +1399,31 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData*
b3Vec3 hA = b3AABB_Extents( hullA->aabb );
// Test B's face planes against A's vertices.
if ( axisOverride != b3_manualEdgePairAxis )
for ( int i = 0; i < faceCountB; ++i )
{
for ( int i = 0; i < faceCountB; ++i )
b3Plane plane = planesB[i];
b3Vec3 direction = b3Neg( b3MulMV( R, plane.normal ) );
float planeSeparation = b3Dot( direction, xfB.p ) - plane.offset;
float biasA = b3Dot( direction, cA ) + 1.0625f * b3Dot( b3Abs( direction ), hA );
float support;
int vertexIndex;
b3GetSupportWide( direction, vxA, vyA, vzA, soaVertexCountA, biasA, &support, &vertexIndex );
float separation = planeSeparation - support;
if ( separation > res.faceB.separation )
{
b3Plane plane = planesB[i];
b3Vec3 direction = b3Neg( b3MulMV( R, plane.normal ) );
float planeSeparation = b3Dot( direction, xfB.p ) - plane.offset;
float biasA = b3Dot( direction, cA ) + 1.0625f * b3Dot( b3Abs( direction ), hA );
float support;
int vertexIndex;
b3GetSupportWide( direction, vxA, vyA, vzA, soaVertexCountA, biasA, &support, &vertexIndex );
float separation = planeSeparation - support;
if ( separation > res.separation )
res.faceB.normal = direction;
res.faceB.separation = separation;
res.faceB.indexA = vertexIndex;
res.faceB.indexB = i;
// This points from A to B and is in frame A
if ( separation > speculativeDistance && earlyReturn )
{
res.type = b3_faceAxisB;
res.separation = separation;
res.indexA = vertexIndex;
res.indexB = i;
// This points from A to B and is in frame A
res.normal = direction;
if ( separation > speculativeDistance )
{
return res;
}
res.separatedFeature = b3_faceAxisB;
return res;
}
}
}
if ( axisOverride == b3_manualFaceAxisB )
{
return res;
}
// Transform B into A's space once, into SoA arrays. Extra space so
// tail can be set to zero in all cases.
_Static_assert( ( B3_MAX_HULL_EDGES & 3 ) == 0, "must be multiple of 4" );
@ -1536,9 +1558,6 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData*
b3StoreW( aV0z + na, zero );
b3StoreW( aTol + na, zero );
// Prefer face contact for more contact points.
float absFaceBias = 0.1f * B3_LINEAR_SLOP;
int edgeCountB = halfEdgeCountB / 2;
#if defined( B3_SIMD_NONE )
@ -1608,20 +1627,18 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData*
float sz = aV0z[i] + bv0z;
float separation = -( sx * nx + ( sy * ny + sz * nz ) );
if ( separation > res.separation + absFaceBias )
if ( separation > res.edge.separation )
{
res.normal = (b3Vec3){ nx, ny, nz };
res.separation = separation;
res.type = b3_edgePairAxis;
res.edge.normal = (b3Vec3){ nx, ny, nz };
res.edge.separation = separation;
// Half edge index
res.indexA = 2 * i;
res.indexB = 2 * j;
// Edge beats face, remove bias
absFaceBias = 0.0f;
if ( separation > speculativeDistance )
res.edge.indexA = 2 * i;
res.edge.indexB = 2 * j;
if ( separation > speculativeDistance && earlyReturn )
{
res.separatedFeature = b3_edgePairAxis;
return res;
}
}
@ -1629,7 +1646,7 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData*
}
#else
// Edge phase, one B edge against four A edges at a time, no transforms in the loop.
const b3FloatW EPS = b3SplatW( -0.0001f );
const b3FloatW INF = b3SplatW( INFINITY );
@ -1718,7 +1735,7 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData*
// Test all 4 supports against the running best at once. If none beats it, skip the
// store and scalar reduction. res->support only turns negative just before returning,
// so this never skips a lane that would trigger the early out.
b3FloatW improves = b3GreaterThanW( separation, b3SplatW( res.separation ) );
b3FloatW improves = b3GreaterThanW( separation, b3SplatW( res.edge.separation ) );
if ( b3AnyTrueW( improves ) == false )
{
continue;
@ -1740,20 +1757,18 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData*
{
int ei = i + lane;
float s = sA[lane];
if ( s > res.separation + absFaceBias )
if ( s > res.edge.separation )
{
res.normal = (b3Vec3){ nxA[lane], nyA[lane], nzA[lane] };
res.separation = s;
res.type = b3_edgePairAxis;
res.edge.normal = (b3Vec3){ nxA[lane], nyA[lane], nzA[lane] };
res.edge.separation = s;
// Half edge index
res.indexA = 2 * ei;
res.indexB = 2 * j;
res.edge.indexA = 2 * ei;
res.edge.indexB = 2 * j;
// Edge beats face, remove bias
absFaceBias = 0.0f;
if ( s > speculativeDistance )
if ( s > speculativeDistance && earlyReturn )
{
res.separatedFeature = b3_edgePairAxis;
return res;
}
}
@ -1824,10 +1839,12 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData*
}
// Attempt face contact using cached feature
b3FaceQuery faceQuery;
b3SeparatingAxis faceQuery;
faceQuery.normal = plane.normal;
faceQuery.separation = 0.0f;
faceQuery.faceIndex = cache->indexA;
faceQuery.vertexIndex = vertexIndex;
faceQuery.indexA = cache->indexA;
faceQuery.indexB = vertexIndex;
faceQuery.type = b3_faceAxisA;
b3SATCache localCache = { 0 };
bool touching = b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, &localCache );
@ -1861,10 +1878,12 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData*
}
// Attempt face contact using cached feature
b3FaceQuery faceQuery;
b3SeparatingAxis faceQuery;
faceQuery.normal = b3Neg( plane.normal );
faceQuery.separation = 0.0f;
faceQuery.faceIndex = cache->indexB;
faceQuery.vertexIndex = vertexIndex;
faceQuery.indexA = vertexIndex;
faceQuery.indexB = cache->indexB;
faceQuery.type = b3_faceAxisB;
b3SATCache localCache = { 0 };
bool touching = b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, &localCache );
@ -1933,11 +1952,12 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData*
}
// Try to rebuild contact from last features
b3EdgeQuery edgeQuery = { 0 };
b3SeparatingAxis edgeQuery = { 0 };
edgeQuery.normal = b3Neg( axis );
edgeQuery.separation = 0.0f;
edgeQuery.indexA = cache->indexA;
edgeQuery.indexB = cache->indexB;
edgeQuery.type = b3_edgePairAxis;
b3SATCache localCache = { 0 };
bool touching = b3BuildEdgeContact( manifold, hullA, hullB, transformBtoA, edgeQuery, &localCache );
@ -1957,13 +1977,8 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData*
// This case is for testing
case b3_manualFaceAxisA:
{
b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, b3_manualFaceAxisA );
b3FaceQuery faceQuery = {
.separation = axisQuery.separation,
.faceIndex = axisQuery.indexA,
.vertexIndex = axisQuery.indexB,
};
b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, false );
b3SeparatingAxis faceQuery = axisQuery.faceA;
b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, cache );
return;
}
@ -1971,12 +1986,8 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData*
// This case is for testing
case b3_manualFaceAxisB:
{
b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, b3_manualFaceAxisB );
b3FaceQuery faceQuery = {
.separation = axisQuery.separation,
.faceIndex = axisQuery.indexB,
.vertexIndex = axisQuery.indexA,
};
b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, false );
b3SeparatingAxis faceQuery = axisQuery.faceB;
b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, cache );
return;
}
@ -1984,14 +1995,8 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData*
// This case is for testing
case b3_manualEdgePairAxis:
{
b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, b3_manualEdgePairAxis );
b3EdgeQuery edgeQuery = {
.normal = axisQuery.normal,
.indexA = axisQuery.indexA,
.indexB = axisQuery.indexB,
.separation = axisQuery.separation,
};
b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, false );
b3SeparatingAxis edgeQuery = axisQuery.edge;
if ( edgeQuery.indexA != B3_NULL_INDEX )
{
b3BuildEdgeContact( manifold, hullA, hullB, transformBtoA, edgeQuery, cache );
@ -2007,74 +2012,94 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData*
manifold->pointCount = 0;
*cache = (b3SATCache){ 0 };
b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, b3_invalidAxis );
b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, true );
B3_VALIDATE( 0 <= axisQuery.indexA && axisQuery.indexA <= UINT8_MAX );
B3_VALIDATE( 0 <= axisQuery.indexB && axisQuery.indexB <= UINT8_MAX );
B3_ASSERT( axisQuery.type != b3_invalidAxis );
cache->separation = axisQuery.separation;
cache->type = (uint8_t)axisQuery.type;
cache->indexA = (uint8_t)axisQuery.indexA;
cache->indexB = (uint8_t)axisQuery.indexB;
if ( axisQuery.separation > speculativeDistance )
if ( axisQuery.separatedFeature != b3_invalidAxis )
{
// We found a separating axis
cache->type = axisQuery.separatedFeature;
if ( axisQuery.separatedFeature == b3_faceAxisA )
{
B3_VALIDATE( axisQuery.faceA.separation > speculativeDistance );
cache->separation = axisQuery.faceA.separation;
cache->indexA = (uint8_t)axisQuery.faceA.indexA;
cache->indexB = (uint8_t)axisQuery.faceA.indexB;
}
else if ( axisQuery.separatedFeature == b3_faceAxisB )
{
B3_VALIDATE( axisQuery.faceB.separation > speculativeDistance );
cache->separation = axisQuery.faceB.separation;
cache->indexA = (uint8_t)axisQuery.faceB.indexA;
cache->indexB = (uint8_t)axisQuery.faceB.indexB;
}
else
{
B3_ASSERT( axisQuery.separatedFeature == b3_edgePairAxis );
B3_VALIDATE( axisQuery.edge.separation > speculativeDistance );
cache->separation = axisQuery.edge.separation;
cache->indexA = (uint8_t)axisQuery.edge.indexA;
cache->indexB = (uint8_t)axisQuery.edge.indexB;
}
return;
}
if ( axisQuery.type == b3_faceAxisA )
{
B3_ASSERT( axisQuery.indexA < hullA->faceCount );
B3_ASSERT( axisQuery.indexB < hullB->vertexCount );
B3_VALIDATE( axisQuery.faceA.separation <= speculativeDistance || axisQuery.faceB.separation <= speculativeDistance ||
axisQuery.edge.separation <= speculativeDistance );
b3FaceQuery faceQuery = {
.separation = axisQuery.separation,
.faceIndex = axisQuery.indexA,
.vertexIndex = axisQuery.indexB,
};
if ( axisQuery.faceA.separation > axisQuery.faceB.separation )
{
b3SeparatingAxis faceQuery = axisQuery.faceA;
B3_VALIDATE( 0 <= faceQuery.indexA && faceQuery.indexA < hullA->faceCount );
B3_VALIDATE( 0 <= faceQuery.indexB && faceQuery.indexB < hullB->vertexCount );
// Face contact A
b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, cache );
return;
B3_VALIDATE( cache->indexA < hullA->faceCount );
B3_VALIDATE( cache->indexB < hullB->vertexCount );
}
if ( axisQuery.type == b3_faceAxisB )
else
{
B3_ASSERT( axisQuery.indexA < hullA->vertexCount );
B3_ASSERT( axisQuery.indexB < hullB->faceCount );
b3FaceQuery faceQuery = {
.separation = axisQuery.separation,
.faceIndex = axisQuery.indexB,
.vertexIndex = axisQuery.indexA,
};
b3SeparatingAxis faceQuery = axisQuery.faceB;
B3_VALIDATE( 0 <= faceQuery.indexA && faceQuery.indexA < hullA->vertexCount );
B3_VALIDATE( 0 <= faceQuery.indexB && faceQuery.indexB < hullB->faceCount );
// Face contact B
b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, cache );
B3_VALIDATE( cache->indexA < hullA->vertexCount );
B3_VALIDATE( cache->indexB < hullB->faceCount );
}
b3SeparatingAxis edgeQuery = axisQuery.edge;
if ( edgeQuery.indexA == B3_NULL_INDEX )
{
// There are no valid edge pairs (all edges parallel)
return;
}
B3_ASSERT( axisQuery.type == b3_edgePairAxis );
float clipSeparation = cache->separation;
float edgeTol = linearSlop;
// Face contact can be empty if it does not realize the axis of minimum penetration.
// Create edge contact if face contact fails or edge contact is significantly better!
if ( manifold->pointCount == 0 || edgeQuery.separation > clipSeparation + edgeTol )
{
B3_ASSERT( 0 <= edgeQuery.indexA && edgeQuery.indexA < hullA->edgeCount );
B3_ASSERT( 0 <= edgeQuery.indexB && edgeQuery.indexB < hullB->edgeCount );
// Edge contact
b3LocalManifold edgeManifold = { 0 };
b3LocalManifoldPoint edgePoint = { 0 };
edgeManifold.points = &edgePoint;
b3EdgeQuery edgeQuery = {
.normal = axisQuery.normal,
.indexA = axisQuery.indexA,
.indexB = axisQuery.indexB,
.separation = axisQuery.separation,
};
b3BuildEdgeContact( &edgeManifold, hullA, hullB, transformBtoA, edgeQuery, cache );
b3SATCache edgeCache = { 0 };
b3BuildEdgeContact( &edgeManifold, hullA, hullB, transformBtoA, edgeQuery, &edgeCache );
// It is possible with speculation to have vertex-vertex collision that is missed by SAT,
// so edge contact yields no points. In that case perhaps the face contact has some points.
if ( edgeManifold.pointCount == 1 )
{
// Copy edge manifold out, being careful to preserve manifold point buffer.
@ -2082,12 +2107,30 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData*
*manifold = edgeManifold;
manifold->points = points;
manifold->points[0] = edgePoint;
*cache = edgeCache;
}
}
}
#else
// todo this code has gone stale, will be deleted soon
typedef struct b3FaceQuery
{
float separation;
int faceIndex;
int vertexIndex;
} b3FaceQuery;
typedef struct b3EdgeQuery
{
b3Vec3 normal;
float separation;
int indexA;
int indexB;
} b3EdgeQuery;
// Old non-SIMD version. Keeping this for testing and comparisons
static b3FaceQuery b3QueryFaceDirections( const b3HullData* hullA, const b3HullData* hullB, b3Transform relativeTransform )

View file

@ -57,7 +57,12 @@
#define B3_SIMD_WIDTH 4
//#pragma message("B3_SIMD_SSE2")
#elif defined( B3_CPU_ARM )
// ARMv7 Neon doesn't have divide or sqrt so cannot be used.
#if defined( __aarch64__ ) || defined( _M_ARM64 )
#define B3_SIMD_NEON
#else
#define B3_SIMD_NONE
#endif
#define B3_SIMD_WIDTH 4
//#pragma message("B3_SIMD_NEON")
#elif defined( B3_CPU_WASM )

View file

@ -17,6 +17,8 @@
// Then from that edge, we select the adjacent face that is most anti-parallel to the reference normal.
int b3FindIncidentFace( const b3HullData* hull, b3Vec3 refNormal, int vertexIndex )
{
B3_ASSERT( 0 <= vertexIndex && vertexIndex < hull->vertexCount );
const b3HullVertex* vertices = b3GetHullVertices( hull );
const b3HullHalfEdge* edges = b3GetHullEdges( hull );
const b3Plane* planes = b3GetHullPlanes( hull );

View file

@ -8,28 +8,21 @@
#define B3_MAX_CLIP_POINTS 64
typedef struct b3FaceQuery
{
float separation;
int faceIndex;
int vertexIndex;
} b3FaceQuery;
typedef struct b3EdgeQuery
{
b3Vec3 normal;
float separation;
int indexA;
int indexB;
} b3EdgeQuery;
typedef struct b3AxisQuery
typedef struct b3SeparatingAxis
{
b3Vec3 normal;
float separation;
int indexA;
int indexB;
b3SeparatingFeature type;
} b3SeparatingAxis;
typedef struct b3AxisQuery
{
b3SeparatingAxis faceA;
b3SeparatingAxis faceB;
b3SeparatingAxis edge;
b3SeparatingFeature separatedFeature;
} b3AxisQuery;
typedef struct b3ClipVertex
@ -52,7 +45,7 @@ b3FeaturePair b3FlipPair( b3FeaturePair pair );
int b3ClipPolygon( b3ClipVertex* out, b3ClipVertex* polygon, int count, b3Plane clipPlane, int edge, b3Plane refPlane );
b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, int axisOverride );
b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, bool earlyReturn );
#if B3_ENABLE_VALIDATION
bool b3ValidatePolygon( b3ClipVertex* polygon, int count );
@ -66,3 +59,27 @@ static inline uint32_t b3MakeFeatureId( b3FeaturePair pair )
return ( (uint32_t)pair.owner1 << 24 ) | ( (uint32_t)pair.index1 << 16 ) | ( (uint32_t)pair.owner2 << 8 ) |
(uint32_t)pair.index2;
}
static inline b3SeparatingAxis b3GetBestAxis( const b3AxisQuery* query )
{
B3_VALIDATE( query->faceA.type == b3_faceAxisA );
B3_VALIDATE( query->edge.type == b3_edgePairAxis );
B3_VALIDATE( query->faceB.type == b3_faceAxisB );
if ( query->faceA.separation > query->faceB.separation )
{
if ( query->edge.separation > query->faceA.separation )
{
return query->edge;
}
return query->faceA;
}
if ( query->edge.separation > query->faceB.separation )
{
return query->edge;
}
return query->faceB;
}

View file

@ -608,7 +608,7 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact
switch ( shapeB->type )
{
case b3_capsuleShape:
b3CollideCapsuleAndTriangle( manifold, pointCapacity, &shapeB->capsule, vertices, &cache->simplexCache );
b3CollideTriangleAndCapsule( manifold, pointCapacity, vertices, &shapeB->capsule, &cache->simplexCache );
break;
case b3_hullShape:
@ -619,14 +619,14 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact
cache->satCache = (b3SATCache){ 0 };
}
b3CollideHullAndTriangle( manifold, pointCapacity, hullB, vertices[0], vertices[1], vertices[2], triangle.flags,
b3CollideTriangleAndHull( manifold, pointCapacity, vertices[0], vertices[1], vertices[2], triangle.flags, hullB,
&cache->satCache, enableSpeculative );
context->satCallCount += 1;
context->satCacheHitCount += cache->satCache.hit;
break;
case b3_sphereShape:
b3CollideSphereAndTriangle( manifold, pointCapacity, &shapeB->sphere, vertices );
b3CollideTriangleAndSphere( manifold, pointCapacity, vertices, &shapeB->sphere );
break;
default:

View file

@ -24,6 +24,8 @@
#include <stddef.h>
#include <stdio.h>
_Static_assert( B3_RESTITUTION_ITERATIONS >= 1, "must be 1 or more" );
// these are useful for solver testing
#define ITERATIONS 1
#define RELAX_ITERATIONS 1
@ -1313,6 +1315,7 @@ static void b3SolverTask( void* taskContext )
stageIndex += 1 + activeColorCount + ITERATIONS * activeColorCount + 1 + RELAX_ITERATIONS * activeColorCount;
// Restitution
for ( int iteration = 0; iteration < B3_RESTITUTION_ITERATIONS; ++iteration )
{
b3ApplyRestitution_Overflow( context );
@ -1324,7 +1327,7 @@ static void b3SolverTask( void* taskContext )
b3ExecuteMainStage( stages + iterStageIndex, context, syncBits );
iterStageIndex += 1;
}
// graphSyncIndex += 1;
graphSyncIndex += 1;
stageIndex += activeColorCount;
}
@ -1690,7 +1693,7 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
// b3_stageRelax
stageCount += RELAX_ITERATIONS * activeColorCount;
// b3_stageRestitution
stageCount += activeColorCount;
stageCount += B3_RESTITUTION_ITERATIONS * activeColorCount;
// b3_stageStoreWideImpulses
stageCount += 1;
// b3_stageStoreImpulses
@ -1775,8 +1778,8 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
stage = b3InitColorStages( stage, b3_stageRelax, RELAX_ITERATIONS, activeColorCount, graphColorBlocks, graphBlockCounts,
activeColorIndices );
// Note: joint blocks mixed in, could have joint limit restitution
stage = b3InitColorStages( stage, b3_stageRestitution, 1, activeColorCount, graphColorBlocks, graphBlockCounts,
activeColorIndices );
stage = b3InitColorStages( stage, b3_stageRestitution, B3_RESTITUTION_ITERATIONS, activeColorCount, graphColorBlocks,
graphBlockCounts, activeColorIndices );
stage = b3InitStage( stage, b3_stageStoreWideImpulses, convexBlocks, convexPrepareDim.count, UINT8_MAX );
stage = b3InitStage( stage, b3_stageStoreImpulses, meshBlocks, meshPrepareDim.count, UINT8_MAX );

View file

@ -23,7 +23,8 @@
#define WIN32_LEAN_AND_MEAN 1
#endif
#include <Windows.h>
// Lower-case windows.h intentionally for cross compiling on mingw.
#include <windows.h>
#include <limits.h>
static double s_invFrequency = 0.0;

View file

@ -50,7 +50,7 @@ static b3TriangleFeature b3GetTriangleFeature( const b3SimplexCache* cache )
return s_triangleFeatures[mask];
}
void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const b3Sphere* sphereA, const b3Vec3* triangleB )
void b3CollideTriangleAndSphere( b3LocalManifold* manifold, int capacity, const b3Vec3* triangleA, const b3Sphere* sphereB )
{
manifold->pointCount = 0;
@ -59,8 +59,8 @@ void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const
return;
}
b3Vec3 center = sphereA->center;
b3Vec3 v1 = triangleB[0], v2 = triangleB[1], v3 = triangleB[2];
b3Vec3 center = sphereB->center;
b3Vec3 v1 = triangleA[0], v2 = triangleA[1], v3 = triangleA[2];
b3Plane plane = b3MakePlaneFromPoints( v1, v2, v3 );
float offset = b3PlaneSeparation( plane, center );
@ -70,19 +70,23 @@ void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const
return;
}
float radius = sphereB->radius;
// Closest point on triangle to sphere center
b3TrianglePoint closest = b3ClosestPointOnTriangle( v1, v2, v3, center );
// Test separating axis
float squaredDistance = b3DistanceSquared( closest.point, center );
float speculativeDistance = B3_SPECULATIVE_DISTANCE;
float maxDistance = sphereA->radius + speculativeDistance;
float maxDistance = radius + speculativeDistance;
if ( squaredDistance > maxDistance * maxDistance )
{
return;
}
float distance = sqrtf( squaredDistance );
// Normal points from triangle to sphere
b3Vec3 normal;
if ( distance * distance > 1000.0f * FLT_MIN )
{
@ -94,7 +98,8 @@ void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const
}
// contact point mid-way
b3Vec3 contactPoint = b3MulSV( 0.5f, b3Add( b3Sub( center, b3MulSV( sphereA->radius, normal ) ), closest.point ) );
// p = 0.5 * (c + q) - 0.5 * r * n
b3Vec3 contactPoint = b3MulSV( 0.5f, b3Add( b3Sub( center, b3MulSV( radius, normal ) ), closest.point ) );
manifold->normal = normal;
manifold->pointCount = 1;
@ -103,7 +108,7 @@ void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const
b3LocalManifoldPoint* mp = manifold->points + 0;
mp->point = contactPoint;
mp->separation = distance - sphereA->radius;
mp->separation = distance - radius;
mp->pair = b3FeaturePair_single;
}
@ -156,28 +161,30 @@ static bool b3ClipSegmentToTriangleFace( b3ClipVertex segment[2], const b3Vec3*
return true;
}
static b3FaceQuery b3QueryTriangleFaceAndCapsule( b3Plane plane, const b3Capsule* capsule )
static b3SeparatingAxis b3QueryTriangleFaceAndCapsule( b3Plane plane, const b3Capsule* capsule )
{
float separation1 = b3PlaneSeparation( plane, capsule->center1 );
float separation2 = b3PlaneSeparation( plane, capsule->center2 );
if ( separation1 < separation2 )
{
return (b3FaceQuery){
return (b3SeparatingAxis){
.normal = plane.normal,
.separation = separation1,
.faceIndex = 0,
.vertexIndex = 0,
.indexA = 0,
.indexB = 0,
};
}
return (b3FaceQuery){
return (b3SeparatingAxis){
.normal = plane.normal,
.separation = separation2,
.faceIndex = 0,
.vertexIndex = 1,
.indexA = 0,
.indexB = 1,
};
}
static b3EdgeQuery b3QueryTriangleAndCapsuleEdges( const b3Vec3* vertices, b3Plane plane, const b3Capsule* capsule )
static b3SeparatingAxis b3QueryTriangleAndCapsuleEdges( const b3Vec3* vertices, b3Plane plane, const b3Capsule* capsule )
{
// Work in the local space of the capsule
b3Vec3 p1 = capsule->center1;
@ -243,7 +250,7 @@ static b3EdgeQuery b3QueryTriangleAndCapsuleEdges( const b3Vec3* vertices, b3Pla
}
// Save result
return (b3EdgeQuery){
return (b3SeparatingAxis){
.normal = maxNormal,
.separation = maxSeparation,
.indexA = maxIndex1,
@ -300,7 +307,7 @@ static void b3BuildTriangleAndCapsuleFaceContact( b3LocalManifold* manifold, con
}
static void b3BuildTriangleAndCapsuleEdgeContact( b3LocalManifold* manifold, const b3Vec3* triangle, b3Plane plane,
const b3Capsule* capsule, b3EdgeQuery query )
const b3Capsule* capsule, b3SeparatingAxis query )
{
B3_ASSERT( 0 <= query.indexA && query.indexA < 3 );
@ -355,7 +362,7 @@ static void b3BuildTriangleAndCapsuleEdgeContact( b3LocalManifold* manifold, con
pt->pair = b3MakeFeaturePair( b3_featureShapeA, query.indexA, b3_featureShapeB, query.indexB );
}
void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA, const b3Vec3* triangleB,
void b3CollideTriangleAndCapsule( b3LocalManifold* manifold, int capacity, const b3Vec3* triangleA, const b3Capsule* capsuleB,
b3SimplexCache* cache )
{
manifold->pointCount = 0;
@ -365,9 +372,9 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const
return;
}
b3Vec3 v1 = triangleB[0], v2 = triangleB[1], v3 = triangleB[2];
b3Vec3 v1 = triangleA[0], v2 = triangleA[1], v3 = triangleA[2];
b3Plane plane = b3MakePlaneFromPoints( v1, v2, v3 );
b3Vec3 capsuleCenter = b3Lerp( capsuleA->center1, capsuleA->center2, 0.5f );
b3Vec3 capsuleCenter = b3Lerp( capsuleB->center1, capsuleB->center2, 0.5f );
float offset = b3PlaneSeparation( plane, capsuleCenter );
if ( offset < 0.0f )
@ -377,14 +384,14 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const
}
b3DistanceInput distanceInput;
distanceInput.proxyA = (b3ShapeProxy){ triangleB, 3, 0.0f };
distanceInput.proxyB = (b3ShapeProxy){ &capsuleA->center1, 2, 0.0f };
distanceInput.proxyA = (b3ShapeProxy){ triangleA, 3, 0.0f };
distanceInput.proxyB = (b3ShapeProxy){ &capsuleB->center1, 2, 0.0f };
distanceInput.transform = b3Transform_identity;
distanceInput.useRadii = false;
b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, cache, NULL, 0 );
float radius = capsuleA->radius;
float radius = capsuleB->radius;
if ( distanceOutput.distance > radius + B3_SPECULATIVE_DISTANCE )
{
// Shapes are separated, persist the cache
@ -403,14 +410,14 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const
{
// Clip capsule segment against side planes of reference face
b3ClipVertex segment[2];
segment[0].position = capsuleA->center1;
segment[0].position = capsuleB->center1;
segment[0].separation = 0.0f;
segment[0].pair = b3MakeFeaturePair( b3_featureShapeA, 0, b3_featureShapeA, 0 );
segment[1].position = capsuleA->center2;
segment[1].position = capsuleB->center2;
segment[1].separation = 0.0f;
segment[1].pair = b3MakeFeaturePair( b3_featureShapeA, 1, b3_featureShapeA, 1 );
bool havePoints = b3ClipSegmentToTriangleFace( segment, triangleB, plane );
bool havePoints = b3ClipSegmentToTriangleFace( segment, triangleA, plane );
if ( havePoints == true )
{
@ -421,6 +428,7 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const
b3Vec3 point1 = b3MulSub( segment[0].position, 0.5f * ( radius + distance1 ), normal );
b3Vec3 point2 = b3MulSub( segment[1].position, 0.5f * ( radius + distance2 ), normal );
// Normal points from triangle to capsule
manifold->normal = normal;
manifold->feature = b3_featureTriangleFace;
manifold->pointCount = 2;
@ -439,9 +447,10 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const
}
}
// Create contact from closest points
// Create contact from closest points.
b3Vec3 point = b3MulSV( 0.5f, b3Add( b3Sub( distanceOutput.pointA, b3MulSV( radius, delta ) ), distanceOutput.pointB ) );
// Normal points from triangle to capsule.
manifold->normal = delta;
manifold->pointCount = 1;
manifold->feature = b3GetTriangleFeature( cache );
@ -456,14 +465,14 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const
// Deep penetration
b3FaceQuery faceQuery = b3QueryTriangleFaceAndCapsule( plane, capsuleA );
b3SeparatingAxis faceQuery = b3QueryTriangleFaceAndCapsule( plane, capsuleB );
if ( faceQuery.separation > radius )
{
// Shapes are separated
return;
}
b3EdgeQuery edgeQuery = b3QueryTriangleAndCapsuleEdges( triangleB, plane, capsuleA );
b3SeparatingAxis edgeQuery = b3QueryTriangleAndCapsuleEdges( triangleA, plane, capsuleB );
if ( edgeQuery.separation > radius )
{
// Shapes are separated
@ -472,7 +481,7 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const
// Create face contact
float faceSeparation = faceQuery.separation - radius;
b3BuildTriangleAndCapsuleFaceContact( manifold, triangleB, plane, capsuleA );
b3BuildTriangleAndCapsuleFaceContact( manifold, triangleA, plane, capsuleB );
if ( manifold->pointCount == 2 )
{
faceSeparation = b3MinFloat( manifold->points[0].separation, manifold->points[1].separation );
@ -487,7 +496,7 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const
if ( manifold->pointCount == 0 || edgeSeparation > kRelEdgeTolerance * faceSeparation + kAbsTolerance )
{
// Edge contact
b3BuildTriangleAndCapsuleEdgeContact( manifold, triangleB, plane, capsuleA, edgeQuery );
b3BuildTriangleAndCapsuleEdgeContact( manifold, triangleA, plane, capsuleB, edgeQuery );
}
}
@ -512,31 +521,35 @@ static inline int b3GetTriangleSupport( b3Vec3* points, b3Vec3 direction )
return index;
}
static b3FaceQuery b3QueryTriangleFace( const b3TriangleData* triangle, const b3HullData* hull )
static b3SeparatingAxis b3QueryTriangleFace( const b3TriangleData* triangle, const b3HullData* hull )
{
const b3Vec3* hullPoints = b3GetHullPoints( hull );
b3Plane plane = triangle->plane;
int vertexIndex = b3FindHullSupportVertex( hull, b3Neg( plane.normal ) );
b3Vec3 normal = b3Neg( plane.normal );
int vertexIndex = b3FindHullSupportVertex( hull, normal );
b3Vec3 support = hullPoints[vertexIndex];
float separation = b3PlaneSeparation( plane, support );
return (b3FaceQuery){
return (b3SeparatingAxis){
.normal = plane.normal,
.separation = separation,
.faceIndex = 0,
.vertexIndex = (uint8_t)vertexIndex,
.indexA = 0,
.indexB = vertexIndex,
.type = b3_faceAxisA,
};
}
static b3FaceQuery b3QueryHullFace( const b3TriangleData* triangle, const b3HullData* hull )
static b3SeparatingAxis b3QueryHullFace( const b3TriangleData* triangle, const b3HullData* hull )
{
const b3Plane* hullPlanes = b3GetHullPlanes( hull );
int faceCount = hull->faceCount;
b3Vec3 trianglePoints[] = { triangle->v1, triangle->v2, triangle->v3 };
int maxFaceIndex = -1;
int maxVertexIndex = -1;
float maxFaceSeparation = -FLT_MAX;
b3Vec3 maxNormal = b3Vec3_zero;
float maxFaceSeparation = -INFINITY;
int maxFaceIndex = B3_NULL_INDEX;
int maxVertexIndex = B3_NULL_INDEX;
for ( int faceIndex = 0; faceIndex < faceCount; ++faceIndex )
{
@ -547,26 +560,32 @@ static b3FaceQuery b3QueryHullFace( const b3TriangleData* triangle, const b3Hull
float separation = b3PlaneSeparation( plane, support );
if ( separation > maxFaceSeparation )
{
maxNormal = plane.normal;
maxFaceSeparation = separation;
maxFaceIndex = faceIndex;
maxVertexIndex = vertexIndex;
maxFaceSeparation = separation;
}
}
return (b3FaceQuery){
// Normal points from triangle to hull
return (b3SeparatingAxis){
.normal = b3Neg( maxNormal ),
.separation = maxFaceSeparation,
.faceIndex = maxFaceIndex,
.vertexIndex = maxVertexIndex,
.indexA = maxVertexIndex,
.indexB = maxFaceIndex,
.type = b3_faceAxisB,
};
}
static b3EdgeQuery b3QueryTriangleAndHullEdges( const b3TriangleData* triangle, const b3HullData* hull )
// A: hull, B: triangle
static b3SeparatingAxis b3QueryTriangleAndHullEdges( const b3TriangleData* triangle, const b3HullData* hull )
{
b3EdgeQuery result = {
b3SeparatingAxis result = {
.normal = b3Vec3_zero,
.separation = -FLT_MAX,
.separation = -INFINITY,
.indexA = B3_NULL_INDEX,
.indexB = B3_NULL_INDEX,
.type = b3_edgePairAxis,
};
b3Vec3 trianglePoints[] = { triangle->v1, triangle->v2, triangle->v3 };
@ -645,8 +664,12 @@ static b3EdgeQuery b3QueryTriangleAndHullEdges( const b3TriangleData* triangle,
}
static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, const b3TriangleData* triangle,
const b3HullData* hull, b3FaceQuery query, b3SATCache* cache, bool enableSpeculative )
const b3HullData* hull, b3SeparatingAxis query, b3SATCache* cache, bool enableSpeculative )
{
B3_VALIDATE( query.type == b3_faceAxisB );
B3_VALIDATE( 0 <= query.indexA && query.indexA < 3 );
B3_VALIDATE( 0 <= query.indexB && query.indexB < hull->faceCount );
manifold->pointCount = 0;
const b3HullFace* hullFaces = b3GetHullFaces( hull );
@ -655,8 +678,7 @@ static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, co
const b3Vec3* hullPoints = b3GetHullPoints( hull );
// Reference hull face
int refFace = query.faceIndex;
b3Plane refPlane = hullPlanes[refFace];
b3Plane refPlane = hullPlanes[query.indexB];
// Build clip polygon from triangle face (the incident face)
b3ClipVertex buffer1[B3_MAX_CLIP_POINTS], buffer2[B3_MAX_CLIP_POINTS];
@ -679,7 +701,7 @@ static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, co
b3ClipVertex* input = buffer1;
b3ClipVertex* output = buffer2;
const b3HullFace* face = hullFaces + refFace;
const b3HullFace* face = hullFaces + query.indexB;
int edgeIndex = face->edge;
do
@ -751,14 +773,17 @@ static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, co
// Save cache
cache->separation = minSeparation;
cache->type = b3_faceAxisB;
cache->indexA = (uint8_t)query.vertexIndex;
cache->indexB = (uint8_t)query.faceIndex;
cache->indexA = (uint8_t)query.indexA;
cache->indexB = (uint8_t)query.indexB;
return minSeparation;
}
static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity, const b3TriangleData* triangle,
const b3HullData* hull, b3FaceQuery query, b3SATCache* cache, bool enableSpeculative )
const b3HullData* hull, b3SeparatingAxis query, b3SATCache* cache, bool enableSpeculative )
{
B3_VALIDATE( query.type == b3_faceAxisA );
B3_VALIDATE( query.indexA == 0 );
B3_VALIDATE( 0 <= query.indexB && query.indexB < hull->vertexCount );
B3_VALIDATE( manifold->pointCount == 0 );
const b3HullFace* hullFaces = b3GetHullFaces( hull );
@ -766,10 +791,9 @@ static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity
const b3Vec3* hullPoints = b3GetHullPoints( hull );
// Find incident face
B3_ASSERT( query.faceIndex == 0 );
b3Plane refPlane = triangle->plane;
int incFace = b3FindIncidentFace( hull, refPlane.normal, query.vertexIndex );
int incFace = b3FindIncidentFace( hull, refPlane.normal, query.indexB );
// Build clip polygon from incident face
b3ClipVertex buffer1[2 * B3_MAX_CLIP_POINTS], buffer2[2 * B3_MAX_CLIP_POINTS];
@ -865,16 +889,18 @@ static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity
// Save cache
cache->separation = minSeparation;
cache->type = b3_faceAxisA;
cache->indexA = (uint8_t)query.faceIndex;
cache->indexB = (uint8_t)query.vertexIndex;
cache->indexA = (uint8_t)query.indexA;
cache->indexB = (uint8_t)query.indexB;
return minSeparation;
}
static void b3CollideHullAndTriangleEdges( b3LocalManifold* manifold, int capacity, b3Vec3 trianglePoint, b3Vec3 triangleEdge,
const b3HullData* hull, b3EdgeQuery query, b3SATCache* cache )
static void b3CollideTriangleAndHullEdges( b3LocalManifold* manifold, int capacity, b3Vec3 trianglePoint, b3Vec3 triangleEdge,
const b3HullData* hull, b3SeparatingAxis query, b3SATCache* cache )
{
B3_VALIDATE( query.type == b3_edgePairAxis );
B3_VALIDATE( 0 <= query.indexA && query.indexA < 3 );
B3_VALIDATE( 0 <= query.indexB && query.indexB < hull->edgeCount );
B3_VALIDATE( query.separation <= 2.0f * B3_SPECULATIVE_DISTANCE );
B3_ASSERT( query.indexA < 3 );
b3Vec3 pA = trianglePoint;
b3Vec3 eA = triangleEdge;
@ -927,9 +953,9 @@ static void b3CollideHullAndTriangleEdges( b3LocalManifold* manifold, int capaci
b3AtomicInt b3_triangleConvexCalls;
b3AtomicInt b3_triangleCacheHits;
// Computes the manifold in the local space of the hull
void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3,
int triangleFlags, b3SATCache* cache, bool enableSpeculative )
// Triangle is in the local space of the hull for efficiency.
void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, int triangleFlags,
const b3HullData* hullB, b3SATCache* cache, bool enableSpeculative )
{
manifold->pointCount = 0;
manifold->feature = b3_featureNone;
@ -942,7 +968,7 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
b3Plane trianglePlane = b3MakePlaneFromPoints( v1, v2, v3 );
float linearSlop = B3_LINEAR_SLOP;
float offset = b3PlaneSeparation( trianglePlane, hullA->center );
float offset = b3PlaneSeparation( trianglePlane, hullB->center );
if ( cache->type == b3_backsideAxis )
{
// Use hysteresis to avoid jitter on wavy meshes
@ -976,9 +1002,9 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
.flags = triangleFlags,
};
const b3HullHalfEdge* edges = b3GetHullEdges( hullA );
const b3Plane* hullPlanes = b3GetHullPlanes( hullA );
const b3Vec3* hullPoints = b3GetHullPoints( hullA );
const b3HullHalfEdge* edges = b3GetHullEdges( hullB );
const b3Plane* hullPlanes = b3GetHullPlanes( hullB );
const b3Vec3* hullPoints = b3GetHullPoints( hullB );
float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f;
cache->hit = 1;
@ -990,7 +1016,7 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
{
B3_ASSERT( cache->indexA == 0 );
int vertexIndex = b3FindHullSupportVertex( hullA, b3Neg( trianglePlane.normal ) );
int vertexIndex = b3FindHullSupportVertex( hullB, b3Neg( trianglePlane.normal ) );
b3Vec3 support = hullPoints[vertexIndex];
float separation = b3PlaneSeparation( trianglePlane, support );
if ( separation > speculativeDistance )
@ -999,15 +1025,17 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
return;
}
b3FaceQuery faceQuery;
b3SeparatingAxis faceQuery;
faceQuery.normal = trianglePlane.normal;
faceQuery.separation = separation;
faceQuery.faceIndex = cache->indexA;
faceQuery.vertexIndex = vertexIndex;
faceQuery.indexA = cache->indexA;
faceQuery.indexB = vertexIndex;
faceQuery.type = b3_faceAxisA;
// Read cache but don't modify it
b3SATCache localCache = *cache;
float clippedSeparation =
b3CollideTriangleFace( manifold, capacity, &triangle, hullA, faceQuery, &localCache, enableSpeculative );
b3CollideTriangleFace( manifold, capacity, &triangle, hullB, faceQuery, &localCache, enableSpeculative );
if ( manifold->pointCount > 0 && b3AbsFloat( cache->separation - clippedSeparation ) < linearSlop )
{
@ -1023,7 +1051,7 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
case b3_faceAxisB:
{
B3_ASSERT( cache->indexB < hullA->faceCount );
B3_ASSERT( cache->indexB < hullB->faceCount );
b3Plane plane = hullPlanes[cache->indexB];
@ -1058,15 +1086,17 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
if ( isDeep == false )
{
// Try to rebuild contact from last features
b3FaceQuery faceQuery;
b3SeparatingAxis faceQuery;
faceQuery.normal = b3Neg( plane.normal );
faceQuery.separation = separation;
faceQuery.faceIndex = cache->indexB;
faceQuery.vertexIndex = vertexIndex;
faceQuery.indexA = vertexIndex;
faceQuery.indexB = cache->indexB;
faceQuery.type = b3_faceAxisB;
// Read cache but don't modify it
b3SATCache localCache = *cache;
float clippedSeparation =
b3CollideHullFace( manifold, capacity, &triangle, hullA, faceQuery, &localCache, enableSpeculative );
b3CollideHullFace( manifold, capacity, &triangle, hullB, faceQuery, &localCache, enableSpeculative );
// Cache reuse is only successful if it creates contact points and the clipped separation didn't change much.
if ( manifold->pointCount > 0 && b3AbsFloat( cache->separation - clippedSeparation ) < linearSlop )
@ -1090,7 +1120,7 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
b3Vec3 triPoint = trianglePoints[indexA];
b3Vec3 triEdge = triangleEdges[indexA];
B3_ASSERT( cache->indexB < hullA->edgeCount - 1 );
B3_ASSERT( cache->indexB < hullB->edgeCount - 1 );
int indexB = cache->indexB;
const b3HullHalfEdge* edge2 = edges + indexB;
@ -1134,15 +1164,16 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
{
// Try to rebuild contact from last features
// Flip normal to point from triangle to hull
b3EdgeQuery edgeQuery;
b3SeparatingAxis edgeQuery;
edgeQuery.normal = b3Neg( axis );
edgeQuery.indexA = indexA;
edgeQuery.indexB = indexB;
edgeQuery.separation = separation;
edgeQuery.type = b3_edgePairAxis;
// Read cache but don't modify it
b3SATCache localCache = *cache;
b3CollideHullAndTriangleEdges( manifold, capacity, triPoint, triEdge, hullA, edgeQuery, &localCache );
b3CollideTriangleAndHullEdges( manifold, capacity, triPoint, triEdge, hullB, edgeQuery, &localCache );
if ( manifold->pointCount > 0 )
{
@ -1161,28 +1192,28 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
// This case is for testing
case b3_manualFaceAxisA:
{
b3FaceQuery faceQueryA = b3QueryTriangleFace( &triangle, hullA );
b3CollideTriangleFace( manifold, capacity, &triangle, hullA, faceQueryA, cache, enableSpeculative );
b3SeparatingAxis query = b3QueryTriangleFace( &triangle, hullB );
b3CollideTriangleFace( manifold, capacity, &triangle, hullB, query, cache, enableSpeculative );
return;
}
// This case is for testing
case b3_manualFaceAxisB:
{
b3FaceQuery faceQueryB = b3QueryHullFace( &triangle, hullA );
b3CollideHullFace( manifold, capacity, &triangle, hullA, faceQueryB, cache, enableSpeculative );
b3SeparatingAxis query = b3QueryHullFace( &triangle, hullB );
b3CollideHullFace( manifold, capacity, &triangle, hullB, query, cache, enableSpeculative );
return;
}
// This case is for testing
case b3_manualEdgePairAxis:
{
b3EdgeQuery edgeQuery = b3QueryTriangleAndHullEdges( &triangle, hullA );
if ( edgeQuery.indexA != B3_NULL_INDEX )
b3SeparatingAxis query = b3QueryTriangleAndHullEdges( &triangle, hullB );
if ( query.indexA != B3_NULL_INDEX )
{
b3Vec3 trianglePoint = trianglePoints[edgeQuery.indexA];
b3Vec3 triangleEdge = triangleEdges[edgeQuery.indexA];
b3CollideHullAndTriangleEdges( manifold, capacity, trianglePoint, triangleEdge, hullA, edgeQuery, cache );
b3Vec3 trianglePoint = trianglePoints[query.indexA];
b3Vec3 triangleEdge = triangleEdges[query.indexA];
b3CollideTriangleAndHullEdges( manifold, capacity, trianglePoint, triangleEdge, hullB, query, cache );
}
return;
}
@ -1196,29 +1227,29 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
cache->hit = 0;
// Find axis of minimum penetration
b3FaceQuery faceQueryA = b3QueryTriangleFace( &triangle, hullA );
b3SeparatingAxis faceQueryA = b3QueryTriangleFace( &triangle, hullB );
if ( faceQueryA.separation > speculativeDistance )
{
// Separating axis found
cache->separation = faceQueryA.separation;
cache->type = b3_faceAxisA;
cache->indexA = 0;
cache->indexB = UINT8_MAX;
cache->indexA = (uint8_t)faceQueryA.indexA;
cache->indexB = (uint8_t)faceQueryA.indexB;
return;
}
b3FaceQuery faceQueryB = b3QueryHullFace( &triangle, hullA );
b3SeparatingAxis faceQueryB = b3QueryHullFace( &triangle, hullB );
if ( faceQueryB.separation > speculativeDistance )
{
// Separating axis found
cache->separation = faceQueryB.separation;
cache->type = b3_faceAxisB;
cache->indexA = UINT8_MAX;
cache->indexB = (uint8_t)faceQueryB.faceIndex;
cache->indexA = (uint8_t)faceQueryB.indexA;
cache->indexB = (uint8_t)faceQueryB.indexB;
return;
}
b3EdgeQuery edgeQuery = b3QueryTriangleAndHullEdges( &triangle, hullA );
b3SeparatingAxis edgeQuery = b3QueryTriangleAndHullEdges( &triangle, hullB );
if ( edgeQuery.separation > speculativeDistance )
{
// Separating axis found
@ -1229,20 +1260,18 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
return;
}
float clippedFaceSeparation;
float clipSeparation;
// Don't admit a hull face significantly opposed to the triangle face.
// Need a tolerance to avoid ghost collisions.
b3Vec3 hullNormal = hullPlanes[faceQueryB.faceIndex].normal;
bool pushingDown = b3Dot( hullNormal, trianglePlane.normal ) > 0.25f;
bool pushingDown = b3Dot( faceQueryB.normal, trianglePlane.normal ) < -0.25f;
if ( faceQueryB.separation >= faceQueryA.separation && pushingDown == false )
{
clippedFaceSeparation = b3CollideHullFace( manifold, capacity, &triangle, hullA, faceQueryB, cache, enableSpeculative );
clipSeparation = b3CollideHullFace( manifold, capacity, &triangle, hullB, faceQueryB, cache, enableSpeculative );
}
else
{
clippedFaceSeparation =
b3CollideTriangleFace( manifold, capacity, &triangle, hullA, faceQueryA, cache, enableSpeculative );
clipSeparation = b3CollideTriangleFace( manifold, capacity, &triangle, hullB, faceQueryA, cache, enableSpeculative );
}
// Does an edge axis exist?
@ -1253,13 +1282,13 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
float maxFaceSeparation = b3MaxFloat( faceQueryA.separation, faceQueryB.separation );
if ( ( manifold->pointCount == 0 && edgeQuery.separation > maxFaceSeparation ) ||
( manifold->pointCount == 1 && edgeQuery.separation > clippedFaceSeparation + linearSlop ) )
( manifold->pointCount == 1 && edgeQuery.separation > clipSeparation + linearSlop ) )
{
B3_ASSERT( 0 <= edgeQuery.indexA && edgeQuery.indexA < 3 );
b3Vec3 trianglePoint = trianglePoints[edgeQuery.indexA];
b3Vec3 triangleEdge = triangleEdges[edgeQuery.indexA];
manifold->pointCount = 0;
b3CollideHullAndTriangleEdges( manifold, capacity, trianglePoint, triangleEdge, hullA, edgeQuery, cache );
b3CollideTriangleAndHullEdges( manifold, capacity, trianglePoint, triangleEdge, hullB, edgeQuery, cache );
}
}
@ -1274,7 +1303,7 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
.count = 3,
.radius = 0.0f,
};
input.proxyB = (b3ShapeProxy){ .points = hullPoints, .count = hullA->vertexCount, .radius = 0.0f };
input.proxyB = (b3ShapeProxy){ .points = hullPoints, .count = hullB->vertexCount, .radius = 0.0f };
input.transform = b3Transform_identity;
input.useRadii = false;
@ -1294,5 +1323,8 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
// This feature pair not accurate but maybe it doesn't matter
manifold->points[0].pair = b3FeaturePair_single;
}
// No way to cache this scenario
*cache = (b3SATCache){ 0 };
}
}

View file

@ -20,8 +20,8 @@
#if defined( BOX3D_DOUBLE_PRECISION )
#define RAGDOLL_SLEEP_STEP 297
#define RAGDOLL_HASH 0x27FF38C1
#define WAVE_PILE_SLEEP_STEP 258
#define WAVE_PILE_HASH 0xA3CDC61B
#define WAVE_PILE_SLEEP_STEP 297
#define WAVE_PILE_HASH 0x420CA784
#define QUERY_SPAWN_SLEEP_STEP 242
#define QUERY_SPAWN_HASH 0x1737F5BC
#define QUERY_SPAWN_HIT_COUNT 59
@ -31,8 +31,8 @@
#else
#define RAGDOLL_SLEEP_STEP 308
#define RAGDOLL_HASH 0x1E5EDD79
#define WAVE_PILE_SLEEP_STEP 239
#define WAVE_PILE_HASH 0xA2F4D472
#define WAVE_PILE_SLEEP_STEP 273
#define WAVE_PILE_HASH 0x47233541
#define QUERY_SPAWN_SLEEP_STEP 242
#define QUERY_SPAWN_HASH 0xB9F993A5
#define QUERY_SPAWN_HIT_COUNT 59

View file

@ -420,7 +420,7 @@ static int TriangleEdgeTest( void )
b3LocalManifold manifold = { 0 };
manifold.points = points;
b3SATCache cache = { .type = b3_manualEdgePairAxis };
b3CollideHullAndTriangle( &manifold, 8, &hull.base, v1, v2, v3, 0, &cache, true );
b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, 0, &hull.base, &cache, true );
b3Vec3 expectedNormal = b3Neg( axis );
b3Vec3 expectedPoint = b3MulAdd( hullPoint, 0.5f * gap, axis );
@ -453,7 +453,7 @@ static int TriangleEdgeTest( void )
b3LocalManifold manifold = { 0 };
manifold.points = points;
b3SATCache cache = { 0 };
b3CollideHullAndTriangle( &manifold, 8, &hull.base, v1, v2, v3, 0, &cache, true );
b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, 0, &hull.base, &cache, true );
ENSURE( manifold.pointCount == 0 );
ENSURE( cache.type == b3_edgePairAxis );
@ -486,7 +486,7 @@ static int TriangleParallelEdgeTest( void )
b3LocalManifold manifold = { 0 };
manifold.points = points;
b3SATCache cache = { 0 };
b3CollideHullAndTriangle( &manifold, 8, &hull.base, v1, v2, v3, 0, &cache, true );
b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, 0, &hull.base, &cache, true );
ENSURE( manifold.pointCount == 4 );
ENSURE( cache.type == b3_faceAxisA );
@ -501,72 +501,111 @@ static int TriangleParallelEdgeTest( void )
return 0;
}
// Two long roof ridges laid across each other. Unlike the stacked cubes the axis of minimum
// penetration really is the edge pair, so the query has to hand the winning pair to the contact
// builder. Closing the crossing angle drives that pair toward parallel, where the cross product
// the builder needs to orient the contact loses its precision.
// A crossed ridge pair must land on a four point roof face contact. The clipped face
// separation can be no deeper than root2 times the vertical overlap.
static int CheckRoofFaceContact( const b3LocalManifold* manifold, const b3SATCache* cache, float overlap )
{
ENSURE( manifold->pointCount == 4 );
ENSURE( cache->type == b3_faceAxisA || cache->type == b3_faceAxisB );
// A roof face of one hull, so 45 degrees off the vertical
ENSURE_SMALL( manifold->normal.y - kHalfRoot2, 1e-4f );
float minSeparation = MinSeparation( manifold );
ENSURE( minSeparation < -kHalfRoot2 * overlap + 1e-4f );
ENSURE( minSeparation > -kRoot2 * overlap - 1e-4f );
return 0;
}
// Two long roof ridges laid across each other. The axis of minimum penetration is the edge
// pair, but a one point edge contact is weak for stacking. The collider builds the roof face
// contact first and only switches to the edge contact when the edge axis beats the clipped
// face separation by more than the slop. This pins all three regimes of that policy.
static int RidgeCrossingTest( void )
{
b3BoxHull hullA = b3MakeTransformedBoxHull( 1.5f, 0.1f, 0.1f, ExactRotation( kAxisX, 0.25f * B3_PI ) );
b3BoxHull hullB = b3MakeTransformedBoxHull( 1.5f, 0.1f, 0.1f, ExactRotation( kAxisX, 0.25f * B3_PI ) );
float ridgeY = 0.1f * kRoot2;
float overlap = 0.01f;
float lift = 2.0f * ridgeY - overlap;
// Well clear of the rejection threshold. The ridges cross over the origin and the axis is y.
float crossingAngles[] = { 0.02f, 0.1f, 0.5f };
for ( int i = 0; i < ARRAY_COUNT( crossingAngles ); ++i )
// Shallow overlap. The edge axis is better by only ( root2 - 1 ) * overlap, inside the
// slop, so the four point face contact carries the crossing at every angle.
{
b3Transform transform = { { 0.0f, lift, 0.0f }, ExactQuat( kAxisY, crossingAngles[i] ) };
float overlap = 0.01f;
float lift = 2.0f * ridgeY - overlap;
float crossingAngles[] = { 0.0f, 1e-3f, 0.02f, 0.1f, 0.5f };
b3LocalManifoldPoint points[8];
b3LocalManifold manifold = { 0 };
manifold.points = points;
b3SATCache cache = { 0 };
b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache );
for ( int i = 0; i < ARRAY_COUNT( crossingAngles ); ++i )
{
b3Transform transform = { { 0.0f, lift, 0.0f }, ExactQuat( kAxisY, crossingAngles[i] ) };
ENSURE( manifold.pointCount == 1 );
ENSURE( cache.type == b3_edgePairAxis );
ENSURE_SMALL( manifold.normal.x, 1e-5f );
ENSURE_SMALL( manifold.normal.y - 1.0f, 1e-5f );
ENSURE_SMALL( manifold.normal.z, 1e-5f );
ENSURE_SMALL( manifold.points[0].separation + overlap, 1e-4f );
ENSURE_SMALL( manifold.points[0].point.y - ( ridgeY - 0.5f * overlap ), 1e-4f );
b3LocalManifoldPoint points[8];
b3LocalManifold manifold = { 0 };
manifold.points = points;
b3SATCache cache = { 0 };
b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache );
// Where the point lands along the ridges is ill conditioned at a shallow crossing since
// the closest point solve divides by the square of the sine of the angle. It only has to
// land near the crossing, not at the end of a three meter beam.
ENSURE_SMALL( manifold.points[0].point.x, 0.01f );
ENSURE_SMALL( manifold.points[0].point.z, 0.01f );
if ( CheckRoofFaceContact( &manifold, &cache, overlap ) != 0 )
{
return 1;
}
}
}
// Inside the rejection threshold. A one point edge contact off a parallel pair would have a
// normal built from noise, so the query drops the pair and the roof faces carry the contact.
float shallowAngles[] = { 0.0f, 1e-4f, 1e-3f, 0.003f };
for ( int i = 0; i < ARRAY_COUNT( shallowAngles ); ++i )
// Deep overlap at a clear crossing. The edge axis now beats the clipped face separation
// by more than the slop, so the edge contact replaces the face contact.
{
b3Transform transform = { { 0.0f, lift, 0.0f }, ExactQuat( kAxisY, shallowAngles[i] ) };
float overlap = 0.05f;
float lift = 2.0f * ridgeY - overlap;
float crossingAngles[] = { 0.05f, 0.1f, 0.2f, 0.5f };
b3LocalManifoldPoint points[8];
b3LocalManifold manifold = { 0 };
manifold.points = points;
b3SATCache cache = { 0 };
b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache );
for ( int i = 0; i < ARRAY_COUNT( crossingAngles ); ++i )
{
b3Transform transform = { { 0.0f, lift, 0.0f }, ExactQuat( kAxisY, crossingAngles[i] ) };
ENSURE( manifold.pointCount == 4 );
ENSURE( cache.type != b3_edgePairAxis );
b3LocalManifoldPoint points[8];
b3LocalManifold manifold = { 0 };
manifold.points = points;
b3SATCache cache = { 0 };
b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache );
// A roof face of one hull, so 45 degrees off the vertical
ENSURE_SMALL( manifold.normal.x, 1e-4f );
ENSURE_SMALL( manifold.normal.y - kHalfRoot2, 1e-4f );
ENSURE_SMALL( b3AbsFloat( manifold.normal.z ) - kHalfRoot2, 1e-4f );
ENSURE( manifold.pointCount == 1 );
ENSURE( cache.type == b3_edgePairAxis );
ENSURE_SMALL( manifold.normal.x, 1e-4f );
ENSURE_SMALL( manifold.normal.y - 1.0f, 1e-4f );
ENSURE_SMALL( manifold.normal.z, 1e-4f );
ENSURE_SMALL( manifold.points[0].separation + overlap, 1e-4f );
ENSURE_SMALL( manifold.points[0].point.y - ( ridgeY - 0.5f * overlap ), 1e-4f );
float minSeparation = MinSeparation( &manifold );
ENSURE( minSeparation < 0.0f );
ENSURE( minSeparation > -2.0f * overlap );
// Only has to land near the crossing, not at the end of a three meter beam
ENSURE_SMALL( manifold.points[0].point.x, 0.01f );
ENSURE_SMALL( manifold.points[0].point.z, 0.01f );
}
}
// Deep overlap near parallel. A one point edge contact off a parallel pair would have a
// normal built from noise, so the roof faces keep the contact.
{
float overlap = 0.05f;
float lift = 2.0f * ridgeY - overlap;
float shallowAngles[] = { 0.0f, 1e-4f, 1e-3f, 0.003f };
for ( int i = 0; i < ARRAY_COUNT( shallowAngles ); ++i )
{
b3Transform transform = { { 0.0f, lift, 0.0f }, ExactQuat( kAxisY, shallowAngles[i] ) };
b3LocalManifoldPoint points[8];
b3LocalManifold manifold = { 0 };
manifold.points = points;
b3SATCache cache = { 0 };
b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache );
if ( CheckRoofFaceContact( &manifold, &cache, overlap ) != 0 )
{
return 1;
}
}
}
return 0;
@ -841,7 +880,7 @@ static int TriangleHullEdgeSweepTest( void )
b3LocalManifold manifold = { 0 };
manifold.points = points;
b3SATCache cache = { .type = b3_manualEdgePairAxis };
b3CollideHullAndTriangle( &manifold, 8, &hull.base, v1, v2, v3, 0, &cache, true );
b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, 0, &hull.base, &cache, true );
if ( cache.type != b3_edgePairAxis || manifold.pointCount != 1 )
{
@ -912,7 +951,7 @@ static int CapsuleTriangleEdgeDeepTest( void )
b3LocalManifold manifold = { 0 };
manifold.points = points;
b3SimplexCache cache = { 0 };
b3CollideCapsuleAndTriangle( &manifold, 8, &capsule, triangle, &cache );
b3CollideTriangleAndCapsule( &manifold, 8, triangle, &capsule, &cache );
// Only the edge contacts exercise the new axis. Face contacts are handled elsewhere.
if ( manifold.pointCount != 1 || manifold.feature < b3_featureEdge1 || manifold.feature > b3_featureEdge3 )

View file

@ -124,7 +124,8 @@ static int FaceAxisASeparatedTest( void )
b3BoxHull hullB = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
b3Transform xfB = { { 1.2f, 0.0f, 0.0f }, b3Quat_identity };
b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis );
b3AxisQuery aq = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, true );
b3SeparatingAxis q = b3GetBestAxis( &aq );
ENSURE( q.type == b3_faceAxisA );
ENSURE_SMALL( q.separation - 0.2f, 1e-5f );
@ -155,7 +156,8 @@ static int FaceAxisBSeparatedTest( void )
float d = aExtent + 0.5f + gap;
b3Transform xfB = { { d, 0.0f, 0.0f }, b3Quat_identity };
b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis );
b3AxisQuery aq = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, true );
b3SeparatingAxis q = b3GetBestAxis( &aq );
ENSURE( q.type == b3_faceAxisB );
ENSURE_SMALL( q.separation - gap, 1e-5f );
@ -181,7 +183,8 @@ static int FaceFarSeparatedTest( void )
b3BoxHull hullB = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
b3Transform xfB = { { 3.0f, 0.0f, 0.0f }, b3Quat_identity };
b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis );
b3AxisQuery aq = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, true );
b3SeparatingAxis q = b3GetBestAxis( &aq );
ENSURE( q.type == b3_faceAxisA );
ENSURE_SMALL( q.separation - 2.0f, 1e-5f );
@ -209,7 +212,8 @@ static int OffsetFaceAxisBTest( void )
float d = aExtent + 0.5f + gap;
b3Transform xfB = { { d, 0.0f, 0.0f }, b3Quat_identity };
b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis );
b3AxisQuery aq = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, true );
b3SeparatingAxis q = b3GetBestAxis( &aq );
ENSURE( q.type == b3_faceAxisB );
ENSURE_SMALL( q.separation - gap, 1e-4f );
@ -248,7 +252,8 @@ static int EdgePairSweepTest( void )
float expected = d - kRoot2;
b3Transform xfB = { { d, 0.0f, 0.0f }, b3Quat_identity };
b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis );
b3AxisQuery aq = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, true );
b3SeparatingAxis q = b3GetBestAxis( &aq );
ENSURE( q.type == b3_edgePairAxis );
ENSURE_SMALL( q.separation - expected, 1e-4f );
@ -327,7 +332,8 @@ static int SeparatingAxisOracleTest( void )
b3Transform xfB = { b3MulSV( reach, NextDirection() ), ExactQuat( NextDirection(), NextFloat( 0.0f, B3_PI ) ) };
b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis );
b3AxisQuery aq = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, true );
b3SeparatingAxis q = b3GetBestAxis( &aq );
b3Vec3 oracleNormal;
float oracleSep = OracleSeparation( &hullA.base, &hullB.base, xfB, &oracleNormal );
@ -423,7 +429,8 @@ static int OffsetHullOracleTest( void )
xfB = (b3Transform){ b3Sub( b3MulSV( reach, dir ), b3RotateVector( qB, offset ) ), qB };
}
b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis );
b3AxisQuery aq = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, true );
b3SeparatingAxis q = b3GetBestAxis( &aq );
b3Vec3 oracleNormal;
float oracleSep = OracleSeparation( &hullA.base, &hullB.base, xfB, &oracleNormal );