SIMD hull collision (#93)
SIMD based hull collision inspired by example code from @cairnc in #54 <img width="1676" height="1332" alt="image" src="https://github.com/user-attachments/assets/58b137bd-bf7b-4c99-863c-64341b973a3d" /> - Fixed #78 - Fixed #85 and fixed CI to error on UB. - Updated benchmarks - Added AMD scalar benchmarks - Added Neon benchmarks - More determinism unit tests - Sample leak detection - Added b3Body_AllowFastRotation - Added b3Body_IsFastRotationAllowed - Bumped b3HullData version - Bumped b3CompoundData version
This commit is contained in:
parent
d421e45c82
commit
c37cfe4cd7
84 changed files with 3898 additions and 1151 deletions
|
|
@ -22,6 +22,7 @@ set(BOX3D_TEST_FILES
|
|||
test_mover.c
|
||||
test_name_cache.c
|
||||
test_recording.c
|
||||
test_sat.c
|
||||
test_shape.c
|
||||
test_table.c
|
||||
test_world.c
|
||||
|
|
|
|||
|
|
@ -44,6 +44,7 @@ extern int MathTest( void );
|
|||
extern int MoverTest( void );
|
||||
extern int NameCacheTest( void );
|
||||
extern int RecordingTest( void );
|
||||
extern int SeparatingAxisTest( void );
|
||||
extern int ShapeTest( void );
|
||||
extern int TableTest( void );
|
||||
extern int WorldTest( void );
|
||||
|
|
@ -116,6 +117,7 @@ int main( int argc, char** argv )
|
|||
MAYBE_RUN_TEST( MoverTest );
|
||||
MAYBE_RUN_TEST( NameCacheTest );
|
||||
MAYBE_RUN_TEST( RecordingTest );
|
||||
MAYBE_RUN_TEST( SeparatingAxisTest );
|
||||
MAYBE_RUN_TEST( ShapeTest );
|
||||
MAYBE_RUN_TEST( TableTest );
|
||||
MAYBE_RUN_TEST( WorldTest );
|
||||
|
|
|
|||
|
|
@ -2,8 +2,11 @@
|
|||
// SPDX-License-Identifier: MIT
|
||||
|
||||
#include "block_allocator.h"
|
||||
#include "core.h"
|
||||
#include "test_macros.h"
|
||||
|
||||
#include "box3d/types.h"
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int value1;
|
||||
|
|
@ -40,10 +43,57 @@ static int TestBlockClear( void )
|
|||
return 0;
|
||||
}
|
||||
|
||||
// The free list link is a pointer stored in the element, so a stride that is not a multiple
|
||||
// of the alignment leaves odd elements on a bad boundary. See issue #78.
|
||||
static int TestBlockAlignment( void )
|
||||
{
|
||||
// The manifold sizes are the reported case
|
||||
int elementSizes[] = { 12, (int)sizeof( b3Manifold ), 3 * (int)sizeof( b3Manifold ) };
|
||||
|
||||
for ( int i = 0; i < ARRAY_COUNT( elementSizes ); ++i )
|
||||
{
|
||||
b3BlockAllocator allocator = b3CreateBlockAllocator( elementSizes[i], 0 );
|
||||
|
||||
// Hold the elements so the indices advance and a block boundary is crossed
|
||||
void* elements[B3_BLOCK_SIZE + 8];
|
||||
int count = ARRAY_COUNT( elements );
|
||||
|
||||
for ( int j = 0; j < count; ++j )
|
||||
{
|
||||
elements[j] = b3AllocateElement( &allocator );
|
||||
ENSURE( elements[j] != NULL );
|
||||
ENSURE( ( (uintptr_t)elements[j] & ( B3_ALIGNMENT - 1 ) ) == 0 );
|
||||
}
|
||||
|
||||
for ( int j = 0; j < count; ++j )
|
||||
{
|
||||
b3FreeElement( &allocator, elements[j] );
|
||||
}
|
||||
|
||||
// Now drain the free list
|
||||
for ( int j = 0; j < count; ++j )
|
||||
{
|
||||
elements[j] = b3AllocateElement( &allocator );
|
||||
ENSURE( elements[j] != NULL );
|
||||
ENSURE( ( (uintptr_t)elements[j] & ( B3_ALIGNMENT - 1 ) ) == 0 );
|
||||
}
|
||||
|
||||
for ( int j = 0; j < count; ++j )
|
||||
{
|
||||
b3FreeElement( &allocator, elements[j] );
|
||||
}
|
||||
|
||||
b3DestroyBlockAllocator( &allocator );
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int AllocatorTest( void )
|
||||
{
|
||||
RUN_SUBTEST( TestBlockAllocate );
|
||||
RUN_SUBTEST( TestBlockClear );
|
||||
RUN_SUBTEST( TestBlockAlignment );
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -3,6 +3,7 @@
|
|||
|
||||
#include "box3d/box3d.h"
|
||||
#include "determinism.h"
|
||||
#include "stability.h"
|
||||
#include "test_macros.h"
|
||||
|
||||
#include <stdio.h>
|
||||
|
|
@ -19,9 +20,25 @@
|
|||
#if defined( BOX3D_DOUBLE_PRECISION )
|
||||
#define EXPECTED_SLEEP_STEP 297
|
||||
#define EXPECTED_HASH 0x27FF38C1
|
||||
#define WAVE_PILE_SLEEP_STEP 287
|
||||
#define WAVE_PILE_HASH 0xFFC8DA49
|
||||
#define QUERY_SPAWN_SLEEP_STEP 242
|
||||
#define QUERY_SPAWN_HASH 0x1737F5BC
|
||||
#define QUERY_SPAWN_HIT_COUNT 59
|
||||
#define QUERY_SPAWN_QUERY_HASH 0x31F090DC
|
||||
#define MESH_DROP_SLEEP_STEP 206
|
||||
#define MESH_DROP_HASH 0xB6A9E1DE
|
||||
#else
|
||||
#define EXPECTED_SLEEP_STEP 308
|
||||
#define EXPECTED_HASH 0x1E5EDD79
|
||||
#define WAVE_PILE_SLEEP_STEP 285
|
||||
#define WAVE_PILE_HASH 0x4057173C
|
||||
#define QUERY_SPAWN_SLEEP_STEP 242
|
||||
#define QUERY_SPAWN_HASH 0xB9F993A5
|
||||
#define QUERY_SPAWN_HIT_COUNT 59
|
||||
#define QUERY_SPAWN_QUERY_HASH 0xB9D3863D
|
||||
#define MESH_DROP_SLEEP_STEP 205
|
||||
#define MESH_DROP_HASH 0x8F55FB2D
|
||||
#endif
|
||||
|
||||
static int SingleMultithreadingTest( int workerCount )
|
||||
|
|
@ -111,10 +128,168 @@ static int CrossPlatformTest( void )
|
|||
return 0;
|
||||
}
|
||||
|
||||
static int SingleWavePileTest( int workerCount )
|
||||
{
|
||||
b3WorldDef worldDef = b3DefaultWorldDef();
|
||||
worldDef.workerCount = workerCount;
|
||||
|
||||
b3WorldId worldId = b3CreateWorld( &worldDef );
|
||||
|
||||
WavePileData data = CreateWavePile( worldId );
|
||||
|
||||
float timeStep = 1.0f / 60.0f;
|
||||
|
||||
// Rolling resistance must put the pile to sleep within 500 steps
|
||||
bool done = false;
|
||||
for ( int i = 0; i < 500 && done == false; ++i )
|
||||
{
|
||||
int subStepCount = 4;
|
||||
b3World_Step( worldId, timeStep, subStepCount );
|
||||
TracyCFrameMark;
|
||||
|
||||
done = UpdateWavePile( worldId, &data );
|
||||
}
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
|
||||
if ( data.sleepStep != WAVE_PILE_SLEEP_STEP || data.hash != WAVE_PILE_HASH )
|
||||
{
|
||||
printf( " wave pile workers=%d sleepStep=%d hash=0x%08X\n", workerCount, data.sleepStep, data.hash );
|
||||
}
|
||||
|
||||
ENSURE( done == true );
|
||||
ENSURE( data.sleepStep == WAVE_PILE_SLEEP_STEP );
|
||||
ENSURE( data.hash == WAVE_PILE_HASH );
|
||||
|
||||
DestroyWavePile( &data );
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Test multithreaded determinism of a mixed convex pile on a wave height field.
|
||||
static int WavePileTest( void )
|
||||
{
|
||||
for ( int workerCount = 1; workerCount <= 4; ++workerCount )
|
||||
{
|
||||
int result = SingleWavePileTest( workerCount );
|
||||
ENSURE( result == 0 );
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int SingleQuerySpawnTest( int workerCount )
|
||||
{
|
||||
b3WorldDef worldDef = b3DefaultWorldDef();
|
||||
worldDef.workerCount = workerCount;
|
||||
|
||||
b3WorldId worldId = b3CreateWorld( &worldDef );
|
||||
|
||||
QuerySpawnData data = CreateQuerySpawn( worldId );
|
||||
|
||||
float timeStep = 1.0f / 60.0f;
|
||||
|
||||
bool done = false;
|
||||
for ( int i = 0; i < 1000 && done == false; ++i )
|
||||
{
|
||||
int subStepCount = 4;
|
||||
b3World_Step( worldId, timeStep, subStepCount );
|
||||
TracyCFrameMark;
|
||||
|
||||
done = UpdateQuerySpawn( worldId, &data );
|
||||
}
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
|
||||
if ( data.sleepStep != QUERY_SPAWN_SLEEP_STEP || data.hash != QUERY_SPAWN_HASH || data.queryHitCount != QUERY_SPAWN_HIT_COUNT ||
|
||||
data.queryHash != QUERY_SPAWN_QUERY_HASH )
|
||||
{
|
||||
printf( " query spawn workers=%d sleepStep=%d hash=0x%08X hits=%d queryHash=0x%08X\n", workerCount, data.sleepStep,
|
||||
data.hash, data.queryHitCount, data.queryHash );
|
||||
}
|
||||
|
||||
ENSURE( done == true );
|
||||
ENSURE( data.spawnCount == QUERY_SPAWN_COUNT );
|
||||
ENSURE( data.sleepStep == QUERY_SPAWN_SLEEP_STEP );
|
||||
ENSURE( data.hash == QUERY_SPAWN_HASH );
|
||||
ENSURE( data.queryHitCount == QUERY_SPAWN_HIT_COUNT );
|
||||
ENSURE( data.queryHash == QUERY_SPAWN_QUERY_HASH );
|
||||
|
||||
DestroyQuerySpawn( &data );
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Test determinism of world queries by feeding their results back into the simulation.
|
||||
static int QuerySpawnTest( void )
|
||||
{
|
||||
for ( int workerCount = 1; workerCount <= 4; ++workerCount )
|
||||
{
|
||||
int result = SingleQuerySpawnTest( workerCount );
|
||||
ENSURE( result == 0 );
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int SingleMeshDropTest( int workerCount )
|
||||
{
|
||||
b3WorldDef worldDef = b3DefaultWorldDef();
|
||||
worldDef.workerCount = workerCount;
|
||||
|
||||
b3WorldId worldId = b3CreateWorld( &worldDef );
|
||||
|
||||
MeshDropData data = CreateMeshDrop( worldId, b3Pos_zero );
|
||||
|
||||
float timeStep = 1.0f / 60.0f;
|
||||
|
||||
bool done = false;
|
||||
for ( int i = 0; i < 400 && done == false; ++i )
|
||||
{
|
||||
int subStepCount = 4;
|
||||
b3World_Step( worldId, timeStep, subStepCount );
|
||||
TracyCFrameMark;
|
||||
|
||||
done = UpdateMeshDrop( worldId, &data );
|
||||
}
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
|
||||
if ( data.sleepStep != MESH_DROP_SLEEP_STEP || data.hash != MESH_DROP_HASH )
|
||||
{
|
||||
printf( " mesh drop workers=%d sleepStep=%d hash=0x%08X\n", workerCount, data.sleepStep, data.hash );
|
||||
}
|
||||
|
||||
ENSURE( done == true );
|
||||
ENSURE( data.sleepStep == MESH_DROP_SLEEP_STEP );
|
||||
ENSURE( data.hash == MESH_DROP_HASH );
|
||||
|
||||
DestroyMeshDrop( &data );
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Test continuous collision determinism. Thin fast boxes need CCD against the wave mesh.
|
||||
// The scene is large, so only the single threaded and widest schedules run.
|
||||
static int MeshDropTest( void )
|
||||
{
|
||||
int workerCounts[2] = { 1, 4 };
|
||||
for ( int i = 0; i < 2; ++i )
|
||||
{
|
||||
int result = SingleMeshDropTest( workerCounts[i] );
|
||||
ENSURE( result == 0 );
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int DeterminismTest( void )
|
||||
{
|
||||
RUN_SUBTEST( MultithreadingTest );
|
||||
RUN_SUBTEST( CrossPlatformTest );
|
||||
RUN_SUBTEST( WavePileTest );
|
||||
RUN_SUBTEST( QuerySpawnTest );
|
||||
RUN_SUBTEST( MeshDropTest );
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -146,13 +146,13 @@ static int CreateHullMaxVertexTest( void )
|
|||
// Below the floor: clamps up to 4.
|
||||
b3HullData* h2 = b3CreateHull( points, count, 1 );
|
||||
ENSURE( h2 != NULL );
|
||||
ENSURE( h2->vertexCount >= 4 && h2->vertexCount <= 255 );
|
||||
ENSURE( h2->vertexCount >= 4 && h2->vertexCount <= B3_MAX_HULL_VERTICES );
|
||||
b3DestroyHull( h2 );
|
||||
|
||||
// Above the ceiling: clamps down to 255.
|
||||
// Above the ceiling: clamps down to B3_MAX_HULL_VERTICES.
|
||||
b3HullData* h3 = b3CreateHull( points, count, 1000 );
|
||||
ENSURE( h3 != NULL );
|
||||
ENSURE( h3->vertexCount >= 4 && h3->vertexCount <= 255 );
|
||||
ENSURE( h3->vertexCount >= 4 && h3->vertexCount <= B3_MAX_HULL_VERTICES );
|
||||
b3DestroyHull( h3 );
|
||||
|
||||
return 0;
|
||||
|
|
@ -341,9 +341,8 @@ static void FillCubeSample( b3Vec3* points, int count, uint32_t seed )
|
|||
// 24*M - 48 in src/hull.c::b3ComputeHullWorkSizes) close to their peak by sweeping M
|
||||
// on a dense random sphere. If the peak face/edge count exceeds the cap and the free
|
||||
// list fails to reclaim slots in time, the assertion at src/hull.c::NewFace/NewEdge
|
||||
// fires (exit code 3). M is held <= 80 because the finalization stage caps the final
|
||||
// hull's face/edge count at B3_HULL_LIMIT (255), and a fully-triangulated hull with
|
||||
// V > 87 vertices has 3V - 6 > 255 edges (src/hull.c:2010 returns NULL).
|
||||
// fires (exit code 3). A dense sphere hull is fully triangulated, so F = 2M - 4 and the
|
||||
// final face limit B3_MAX_HULL_FACES binds well before the vertex or edge limits.
|
||||
static int CreateHullSphereStressTest( void )
|
||||
{
|
||||
enum { N = 512 };
|
||||
|
|
@ -352,8 +351,8 @@ static int CreateHullSphereStressTest( void )
|
|||
// Multiple seeds exercise different horizon-size / merge-cascade sequences.
|
||||
const uint32_t seeds[] = { 12345u, 1u, 0xdeadbeefu, 0xcafef00du };
|
||||
|
||||
// M kept <= 40: random sphere inputs at higher M exceed the half edge limit of UINT8_MAX
|
||||
const int M_values[] = { 16, 24, 32, 40 };
|
||||
// M kept <= 32 so the final face count 2M - 4 stays under B3_MAX_HULL_FACES
|
||||
const int M_values[] = { 16, 24, 32 };
|
||||
|
||||
for ( int s = 0; s < ARRAY_COUNT( seeds ); ++s )
|
||||
{
|
||||
|
|
|
|||
486
test/test_sat.c
Normal file
486
test/test_sat.c
Normal file
|
|
@ -0,0 +1,486 @@
|
|||
// SPDX-FileCopyrightText: 2026 Erin Catto
|
||||
// SPDX-License-Identifier: MIT
|
||||
|
||||
#include "test_macros.h"
|
||||
|
||||
#include "manifold.h"
|
||||
|
||||
#include "box3d/collision.h"
|
||||
#include "box3d/constants.h"
|
||||
#include "box3d/math_functions.h"
|
||||
|
||||
#include <float.h>
|
||||
#include <math.h>
|
||||
|
||||
static const float kRoot2 = 1.41421356f;
|
||||
static const b3Vec3 kAxisY = { 0.0f, 1.0f, 0.0f };
|
||||
static const b3Vec3 kAxisZ = { 0.0f, 0.0f, 1.0f };
|
||||
|
||||
// b3ComputeCosSin is a coarse rational approximation, so the fixtures build their rotations from
|
||||
// libm to keep the analytic edge and face positions exact.
|
||||
static b3Quat ExactQuat( b3Vec3 axis, float radians )
|
||||
{
|
||||
float half = 0.5f * radians;
|
||||
float s = sinf( half );
|
||||
return (b3Quat){ { s * axis.x, s * axis.y, s * axis.z }, cosf( half ) };
|
||||
}
|
||||
|
||||
static b3Transform ExactRotation( b3Vec3 axis, float radians )
|
||||
{
|
||||
return (b3Transform){ b3Vec3_zero, ExactQuat( axis, radians ) };
|
||||
}
|
||||
|
||||
// Directed separation along a unit axis n pointing from A to B. Positive means B clears A along n.
|
||||
// This is the projection gap min over B minus max over A, which is exactly what a valid separating
|
||||
// axis measures, so the winning face or edge axis must reproduce it.
|
||||
static float SepAlong( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, b3Vec3 n )
|
||||
{
|
||||
const b3Vec3* pointsA = b3GetHullPoints( hullA );
|
||||
float maxA = -FLT_MAX;
|
||||
for ( int i = 0; i < hullA->vertexCount; ++i )
|
||||
{
|
||||
maxA = b3MaxFloat( maxA, b3Dot( n, pointsA[i] ) );
|
||||
}
|
||||
|
||||
const b3Vec3* pointsB = b3GetHullPoints( hullB );
|
||||
float minB = FLT_MAX;
|
||||
for ( int i = 0; i < hullB->vertexCount; ++i )
|
||||
{
|
||||
minB = b3MinFloat( minB, b3Dot( n, b3TransformPoint( xfB, pointsB[i] ) ) );
|
||||
}
|
||||
|
||||
return minB - maxA;
|
||||
}
|
||||
|
||||
static void TryAxis( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, b3Vec3 n, float* best,
|
||||
b3Vec3* bestNormal )
|
||||
{
|
||||
float s = SepAlong( hullA, hullB, xfB, n );
|
||||
if ( s > *best )
|
||||
{
|
||||
*best = s;
|
||||
*bestNormal = n;
|
||||
}
|
||||
}
|
||||
|
||||
// Brute force SAT. The maximum separation over every face normal and every edge cross product is
|
||||
// the true answer the SIMD query has to match. Each undirected axis is tried both ways so the
|
||||
// winner comes out oriented from A to B with no sign bookkeeping.
|
||||
static float OracleSeparation( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, b3Vec3* outNormal )
|
||||
{
|
||||
float best = -FLT_MAX;
|
||||
b3Vec3 bestNormal = { 0.0f, 0.0f, 0.0f };
|
||||
|
||||
const b3Plane* planesA = b3GetHullPlanes( hullA );
|
||||
for ( int i = 0; i < hullA->faceCount; ++i )
|
||||
{
|
||||
TryAxis( hullA, hullB, xfB, planesA[i].normal, &best, &bestNormal );
|
||||
TryAxis( hullA, hullB, xfB, b3Neg( planesA[i].normal ), &best, &bestNormal );
|
||||
}
|
||||
|
||||
const b3Plane* planesB = b3GetHullPlanes( hullB );
|
||||
for ( int i = 0; i < hullB->faceCount; ++i )
|
||||
{
|
||||
b3Vec3 n = b3RotateVector( xfB.q, planesB[i].normal );
|
||||
TryAxis( hullA, hullB, xfB, n, &best, &bestNormal );
|
||||
TryAxis( hullA, hullB, xfB, b3Neg( n ), &best, &bestNormal );
|
||||
}
|
||||
|
||||
const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA );
|
||||
const b3Vec3* pointsA = b3GetHullPoints( hullA );
|
||||
const b3HullHalfEdge* edgesB = b3GetHullEdges( hullB );
|
||||
const b3Vec3* pointsB = b3GetHullPoints( hullB );
|
||||
|
||||
// Half edges are stored as adjacent twin pairs, so the even index and its successor bound one edge.
|
||||
for ( int i = 0; i < hullA->edgeCount; i += 2 )
|
||||
{
|
||||
b3Vec3 dirA = b3Sub( pointsA[edgesA[i + 1].origin], pointsA[edgesA[i].origin] );
|
||||
for ( int j = 0; j < hullB->edgeCount; j += 2 )
|
||||
{
|
||||
b3Vec3 dirB = b3RotateVector( xfB.q, b3Sub( pointsB[edgesB[j + 1].origin], pointsB[edgesB[j].origin] ) );
|
||||
b3Vec3 cross = b3Cross( dirA, dirB );
|
||||
|
||||
// Near parallel edges give no useful axis and cannot be the true maximum.
|
||||
if ( b3Dot( cross, cross ) < 1e-10f )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
cross = b3Normalize( cross );
|
||||
TryAxis( hullA, hullB, xfB, cross, &best, &bestNormal );
|
||||
TryAxis( hullA, hullB, xfB, b3Neg( cross ), &best, &bestNormal );
|
||||
}
|
||||
}
|
||||
|
||||
*outNormal = bestNormal;
|
||||
return best;
|
||||
}
|
||||
|
||||
// Two axis aligned cubes pushed apart along x. A's plus x face is the separating axis, the gap is
|
||||
// large enough to trip the speculative early out, and the reference vertex on B is its minus x side.
|
||||
static int FaceAxisASeparatedTest( void )
|
||||
{
|
||||
b3BoxHull hullA = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
|
||||
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 );
|
||||
|
||||
ENSURE( q.type == b3_faceAxisA );
|
||||
ENSURE_SMALL( q.separation - 0.2f, 1e-5f );
|
||||
ENSURE_SMALL( q.normal.x - 1.0f, 1e-6f );
|
||||
ENSURE_SMALL( q.normal.y, 1e-6f );
|
||||
ENSURE_SMALL( q.normal.z, 1e-6f );
|
||||
|
||||
const b3Plane* planesA = b3GetHullPlanes( &hullA.base );
|
||||
ENSURE_SMALL( planesA[q.indexA].normal.x - 1.0f, 1e-6f );
|
||||
|
||||
const b3Vec3* pointsB = b3GetHullPoints( &hullB.base );
|
||||
ENSURE_SMALL( pointsB[q.indexB].x + 0.5f, 1e-6f );
|
||||
|
||||
ENSURE_SMALL( SepAlong( &hullA.base, &hullB.base, xfB, q.normal ) - q.separation, 1e-5f );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// A yawed 45 degrees about z so all of its faces sit oblique to x. Only B still has a face square to
|
||||
// the gap, so the reference has to be B's minus x face. This is the case that ties when both hulls
|
||||
// are aligned, and the tie is what a face A bias would hide.
|
||||
static int FaceAxisBSeparatedTest( void )
|
||||
{
|
||||
b3BoxHull hullA = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, ExactRotation( kAxisZ, 0.25f * B3_PI ) );
|
||||
b3BoxHull hullB = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
|
||||
|
||||
float aExtent = 0.5f * kRoot2;
|
||||
float gap = 0.2f;
|
||||
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 );
|
||||
|
||||
ENSURE( q.type == b3_faceAxisB );
|
||||
ENSURE_SMALL( q.separation - gap, 1e-5f );
|
||||
ENSURE_SMALL( q.normal.x - 1.0f, 1e-5f );
|
||||
ENSURE_SMALL( q.normal.y, 1e-5f );
|
||||
ENSURE_SMALL( q.normal.z, 1e-5f );
|
||||
|
||||
const b3Plane* planesB = b3GetHullPlanes( &hullB.base );
|
||||
ENSURE_SMALL( planesB[q.indexB].normal.x + 1.0f, 1e-5f );
|
||||
|
||||
const b3Vec3* pointsA = b3GetHullPoints( &hullA.base );
|
||||
ENSURE_SMALL( pointsA[q.indexA].x - aExtent, 1e-4f );
|
||||
|
||||
ENSURE_SMALL( SepAlong( &hullA.base, &hullB.base, xfB, q.normal ) - q.separation, 1e-4f );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Well beyond the speculative distance the face phase must report the axis and separation without
|
||||
// running the edge phase. A sign slip in the early out would read as deep overlap.
|
||||
static int FaceFarSeparatedTest( void )
|
||||
{
|
||||
b3BoxHull hullA = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
|
||||
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 );
|
||||
|
||||
ENSURE( q.type == b3_faceAxisA );
|
||||
ENSURE_SMALL( q.separation - 2.0f, 1e-5f );
|
||||
ENSURE_SMALL( q.normal.x - 1.0f, 1e-6f );
|
||||
|
||||
b3Vec3 oracleNormal;
|
||||
float oracleSep = OracleSeparation( &hullA.base, &hullB.base, xfB, &oracleNormal );
|
||||
ENSURE_SMALL( q.separation - oracleSep, 1e-5f );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// The support bias is derived from each hull's AABB, not from an origin assumed to sit inside it. A
|
||||
// box built far from its own local origin is the case the old diagonal bias got wrong. A is yawed
|
||||
// and pushed out to x = 5, so the face B query has to run getSupport over vertices clustered near
|
||||
// x = 5 and still pick the right one.
|
||||
static int OffsetFaceAxisBTest( void )
|
||||
{
|
||||
float cx = 5.0f;
|
||||
b3Transform placeA = { { cx, 0.0f, 0.0f }, ExactQuat( kAxisZ, 0.25f * B3_PI ) };
|
||||
b3BoxHull hullA = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, placeA );
|
||||
b3BoxHull hullB = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
|
||||
|
||||
float aExtent = cx + 0.5f * kRoot2;
|
||||
float gap = 0.2f;
|
||||
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 );
|
||||
|
||||
ENSURE( q.type == b3_faceAxisB );
|
||||
ENSURE_SMALL( q.separation - gap, 1e-4f );
|
||||
ENSURE_SMALL( q.normal.x - 1.0f, 1e-5f );
|
||||
ENSURE_SMALL( q.normal.y, 1e-5f );
|
||||
ENSURE_SMALL( q.normal.z, 1e-5f );
|
||||
|
||||
const b3Plane* planesB = b3GetHullPlanes( &hullB.base );
|
||||
ENSURE_SMALL( planesB[q.indexB].normal.x + 1.0f, 1e-5f );
|
||||
|
||||
const b3Vec3* pointsA = b3GetHullPoints( &hullA.base );
|
||||
ENSURE_SMALL( pointsA[q.indexA].x - aExtent, 1e-4f );
|
||||
|
||||
b3Vec3 oracleNormal;
|
||||
float oracleSep = OracleSeparation( &hullA.base, &hullB.base, xfB, &oracleNormal );
|
||||
ENSURE_SMALL( q.separation - oracleSep, 1e-4f );
|
||||
ENSURE_SMALL( SepAlong( &hullA.base, &hullB.base, xfB, q.normal ) - q.separation, 1e-4f );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Cube A yawed 45 about y presents an edge along y at x = h*root2. Cube B rolled 45 about z presents
|
||||
// an edge along z at x = -h*root2. Sliding B along x makes those edges the closest features, so the
|
||||
// axis is x and the separation is d - root2. The sweep straddles contact into overlap while staying
|
||||
// under the speculative distance, so no face axis can short circuit the edge phase. The overlap rows
|
||||
// pin the maximum separation, which is where a premature edge early out shows up.
|
||||
static int EdgePairSweepTest( void )
|
||||
{
|
||||
b3BoxHull hullA = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, ExactRotation( kAxisY, 0.25f * B3_PI ) );
|
||||
b3BoxHull hullB = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, ExactRotation( kAxisZ, 0.25f * B3_PI ) );
|
||||
|
||||
float distances[] = { 1.43f, 1.42f, kRoot2, 1.40f, 1.38f, 1.35f, 1.30f };
|
||||
|
||||
for ( int i = 0; i < ARRAY_COUNT( distances ); ++i )
|
||||
{
|
||||
float d = distances[i];
|
||||
float expected = d - kRoot2;
|
||||
|
||||
b3Transform xfB = { { d, 0.0f, 0.0f }, b3Quat_identity };
|
||||
b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis );
|
||||
|
||||
ENSURE( q.type == b3_edgePairAxis );
|
||||
ENSURE_SMALL( q.separation - expected, 1e-4f );
|
||||
ENSURE_SMALL( q.normal.x - 1.0f, 1e-4f );
|
||||
ENSURE_SMALL( q.normal.y, 1e-4f );
|
||||
ENSURE_SMALL( q.normal.z, 1e-4f );
|
||||
|
||||
// Half edge indices are the even twin of each pair.
|
||||
ENSURE( ( q.indexA & 1 ) == 0 );
|
||||
ENSURE( ( q.indexB & 1 ) == 0 );
|
||||
|
||||
b3Vec3 oracleNormal;
|
||||
float oracleSep = OracleSeparation( &hullA.base, &hullB.base, xfB, &oracleNormal );
|
||||
ENSURE_SMALL( q.separation - oracleSep, 1e-4f );
|
||||
ENSURE_SMALL( SepAlong( &hullA.base, &hullB.base, xfB, q.normal ) - q.separation, 1e-4f );
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static uint32_t g_seed = 987654321u;
|
||||
|
||||
static float NextFloat( float lower, float upper )
|
||||
{
|
||||
g_seed = 1664525u * g_seed + 1013904223u;
|
||||
float t = (float)( g_seed >> 8 ) / (float)( 1 << 24 );
|
||||
return lower + t * ( upper - lower );
|
||||
}
|
||||
|
||||
static b3Vec3 NextDirection( void )
|
||||
{
|
||||
b3Vec3 v = { NextFloat( -1.0f, 1.0f ), NextFloat( -1.0f, 1.0f ), NextFloat( -1.0f, 1.0f ) };
|
||||
return b3Normalize( v );
|
||||
}
|
||||
|
||||
// A wide net over randomly oriented box pairs, from clearly separated to deep overlap. The brute
|
||||
// force oracle is the source of truth. Whatever axis the query returns it must be a unit vector that
|
||||
// reproduces its own separation, it must not exceed the true maximum, and it must not fall short of
|
||||
// it. The short fall bound is what a suboptimal edge pick trips. Thin boxes are mixed in because a
|
||||
// crossed pair of long edges stays the axis of minimum penetration even deep in overlap, which is
|
||||
// the hardest case for the edge phase.
|
||||
static int SeparatingAxisOracleTest( void )
|
||||
{
|
||||
g_seed = 987654321u;
|
||||
|
||||
int separated = 0;
|
||||
int penetrating = 0;
|
||||
int edgeWins = 0;
|
||||
float worstBandShortfall = 0.0f;
|
||||
float worstExcess = 0.0f;
|
||||
|
||||
for ( int i = 0; i < 4000; ++i )
|
||||
{
|
||||
b3Vec3 halfA, halfB;
|
||||
float reach;
|
||||
|
||||
if ( i & 1 )
|
||||
{
|
||||
// Thin crossed beams driven into deep overlap.
|
||||
halfA = (b3Vec3){ NextFloat( 1.0f, 1.6f ), NextFloat( 0.08f, 0.15f ), NextFloat( 0.08f, 0.15f ) };
|
||||
halfB = (b3Vec3){ NextFloat( 1.0f, 1.6f ), NextFloat( 0.08f, 0.15f ), NextFloat( 0.08f, 0.15f ) };
|
||||
reach = NextFloat( 0.0f, 0.4f );
|
||||
}
|
||||
else
|
||||
{
|
||||
// Chunky boxes across the whole separated to overlapping range.
|
||||
halfA = (b3Vec3){ NextFloat( 0.3f, 0.8f ), NextFloat( 0.3f, 0.8f ), NextFloat( 0.3f, 0.8f ) };
|
||||
halfB = (b3Vec3){ NextFloat( 0.3f, 0.8f ), NextFloat( 0.3f, 0.8f ), NextFloat( 0.3f, 0.8f ) };
|
||||
reach = NextFloat( 0.0f, 1.7f );
|
||||
}
|
||||
|
||||
// Rotate A in place so it stays centered on the origin, which keeps the support bias valid.
|
||||
b3BoxHull hullA =
|
||||
b3MakeTransformedBoxHull( halfA.x, halfA.y, halfA.z, ExactRotation( NextDirection(), NextFloat( 0.0f, B3_PI ) ) );
|
||||
b3BoxHull hullB = b3MakeBoxHull( halfB.x, halfB.y, halfB.z );
|
||||
|
||||
b3Transform xfB = { b3MulSV( reach, NextDirection() ), ExactQuat( NextDirection(), NextFloat( 0.0f, B3_PI ) ) };
|
||||
|
||||
b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis );
|
||||
|
||||
b3Vec3 oracleNormal;
|
||||
float oracleSep = OracleSeparation( &hullA.base, &hullB.base, xfB, &oracleNormal );
|
||||
|
||||
ENSURE_SMALL( b3Length( q.normal ) - 1.0f, 1e-3f );
|
||||
ENSURE_SMALL( SepAlong( &hullA.base, &hullB.base, xfB, q.normal ) - q.separation, 2e-3f );
|
||||
|
||||
worstExcess = b3MaxFloat( worstExcess, q.separation - oracleSep );
|
||||
|
||||
// Never overstate the true maximum. This holds on every path since the reported axis is real.
|
||||
ENSURE( q.separation <= oracleSep + 2e-3f );
|
||||
|
||||
// The scan stops at the first axis that clears the speculative band, so a widely separated pair
|
||||
// reports some separating axis rather than the deepest one. That is all the collide path needs,
|
||||
// since past the band it drops the pair either way. Inside the band nothing stops the scan, so
|
||||
// the result there is the exhaustive maximum, and that is what a manifold gets built from.
|
||||
if ( q.separation <= B3_SPECULATIVE_DISTANCE )
|
||||
{
|
||||
worstBandShortfall = b3MaxFloat( worstBandShortfall, oracleSep - q.separation );
|
||||
|
||||
// A suboptimal axis pick lands a discrete step below the true maximum. The bound sits well
|
||||
// under that step yet clear of the edge normalization noise floor near 6e-4.
|
||||
ENSURE( q.separation >= oracleSep - 4e-3f );
|
||||
}
|
||||
|
||||
if ( oracleSep > 0.0f )
|
||||
{
|
||||
separated += 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
penetrating += 1;
|
||||
}
|
||||
|
||||
if ( q.type == b3_edgePairAxis )
|
||||
{
|
||||
edgeWins += 1;
|
||||
}
|
||||
}
|
||||
|
||||
printf( " oracle: separated=%d penetrating=%d edgeWins=%d worstBandShortfall=%.2e worstExcess=%.2e\n", separated,
|
||||
penetrating, edgeWins, worstBandShortfall, worstExcess );
|
||||
|
||||
// The sweep has to cover both regimes and actually drive the edge path.
|
||||
ENSURE( separated > 100 );
|
||||
ENSURE( penetrating > 100 );
|
||||
ENSURE( edgeWins > 20 );
|
||||
return 0;
|
||||
}
|
||||
|
||||
// The oracle over hulls whose vertices sit far from their own local origin, which the AABB based
|
||||
// bias must handle. Half the pairs offset A in its own frame, half offset B, and B is placed so the
|
||||
// two centers land near each other across the separated to overlapping range. Any bias that assumed
|
||||
// the origin was inside the hull would pick the wrong support and blow the separation.
|
||||
static int OffsetHullOracleTest( void )
|
||||
{
|
||||
g_seed = 24681012u;
|
||||
|
||||
int separated = 0;
|
||||
int penetrating = 0;
|
||||
int edgeWins = 0;
|
||||
float worstBandShortfall = 0.0f;
|
||||
float worstExcess = 0.0f;
|
||||
float worstConsistency = 0.0f;
|
||||
|
||||
for ( int i = 0; i < 3000; ++i )
|
||||
{
|
||||
b3Vec3 halfA = { NextFloat( 0.3f, 0.8f ), NextFloat( 0.3f, 0.8f ), NextFloat( 0.3f, 0.8f ) };
|
||||
b3Vec3 halfB = { NextFloat( 0.3f, 0.8f ), NextFloat( 0.3f, 0.8f ), NextFloat( 0.3f, 0.8f ) };
|
||||
|
||||
b3Vec3 offset = b3MulSV( NextFloat( 2.0f, 6.0f ), NextDirection() );
|
||||
float reach = NextFloat( 0.0f, 1.7f );
|
||||
b3Vec3 dir = NextDirection();
|
||||
|
||||
b3BoxHull hullA;
|
||||
b3BoxHull hullB;
|
||||
b3Transform xfB;
|
||||
|
||||
if ( i & 1 )
|
||||
{
|
||||
// A carries the offset in its own frame. B is centered and dropped near A's center.
|
||||
b3Transform placeA = { offset, ExactQuat( NextDirection(), NextFloat( 0.0f, B3_PI ) ) };
|
||||
hullA = b3MakeTransformedBoxHull( halfA.x, halfA.y, halfA.z, placeA );
|
||||
hullB = b3MakeBoxHull( halfB.x, halfB.y, halfB.z );
|
||||
xfB = (b3Transform){ b3MulAdd( offset, reach, dir ), ExactQuat( NextDirection(), NextFloat( 0.0f, B3_PI ) ) };
|
||||
}
|
||||
else
|
||||
{
|
||||
// B carries the offset in its own frame. Place it so its world center lands near the origin.
|
||||
b3Quat qB = ExactQuat( NextDirection(), NextFloat( 0.0f, B3_PI ) );
|
||||
hullA = b3MakeBoxHull( halfA.x, halfA.y, halfA.z );
|
||||
hullB = b3MakeOffsetBoxHull( halfB.x, halfB.y, halfB.z, offset );
|
||||
xfB = (b3Transform){ b3Sub( b3MulSV( reach, dir ), b3RotateVector( qB, offset ) ), qB };
|
||||
}
|
||||
|
||||
b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis );
|
||||
|
||||
b3Vec3 oracleNormal;
|
||||
float oracleSep = OracleSeparation( &hullA.base, &hullB.base, xfB, &oracleNormal );
|
||||
|
||||
float consistency = b3AbsFloat( SepAlong( &hullA.base, &hullB.base, xfB, q.normal ) - q.separation );
|
||||
worstConsistency = b3MaxFloat( worstConsistency, consistency );
|
||||
worstExcess = b3MaxFloat( worstExcess, q.separation - oracleSep );
|
||||
|
||||
// Consistency is the sharp bias check: a wrong support pick on an offset hull would throw the
|
||||
// returned normal off its own separation by a vertex spacing, not a noise floor. It holds on
|
||||
// every path, early out or not, so it stays unconditional. The oracle bounds are looser since
|
||||
// differencing coordinates out at radius six costs a few more digits.
|
||||
ENSURE_SMALL( b3Length( q.normal ) - 1.0f, 1e-3f );
|
||||
ENSURE( consistency < 1e-3f );
|
||||
ENSURE( q.separation <= oracleSep + 3e-3f );
|
||||
|
||||
// Only the in band result feeds a manifold, and only there does the scan run to completion.
|
||||
if ( q.separation <= B3_SPECULATIVE_DISTANCE )
|
||||
{
|
||||
worstBandShortfall = b3MaxFloat( worstBandShortfall, oracleSep - q.separation );
|
||||
ENSURE( q.separation >= oracleSep - 8e-3f );
|
||||
}
|
||||
|
||||
if ( oracleSep > 0.0f )
|
||||
{
|
||||
separated += 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
penetrating += 1;
|
||||
}
|
||||
|
||||
if ( q.type == b3_edgePairAxis )
|
||||
{
|
||||
edgeWins += 1;
|
||||
}
|
||||
}
|
||||
|
||||
printf(
|
||||
" offset: separated=%d penetrating=%d edgeWins=%d worstBandShortfall=%.2e worstExcess=%.2e worstConsistency=%.2e\n",
|
||||
separated, penetrating, edgeWins, worstBandShortfall, worstExcess, worstConsistency );
|
||||
|
||||
ENSURE( separated > 100 );
|
||||
ENSURE( penetrating > 100 );
|
||||
ENSURE( edgeWins > 20 );
|
||||
return 0;
|
||||
}
|
||||
|
||||
int SeparatingAxisTest( void )
|
||||
{
|
||||
RUN_SUBTEST( FaceAxisASeparatedTest );
|
||||
RUN_SUBTEST( FaceAxisBSeparatedTest );
|
||||
RUN_SUBTEST( FaceFarSeparatedTest );
|
||||
RUN_SUBTEST( OffsetFaceAxisBTest );
|
||||
RUN_SUBTEST( EdgePairSweepTest );
|
||||
RUN_SUBTEST( SeparatingAxisOracleTest );
|
||||
RUN_SUBTEST( OffsetHullOracleTest );
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -3,7 +3,6 @@
|
|||
|
||||
#include "benchmarks.h"
|
||||
#include "overflow_color.h"
|
||||
#include "stability.h"
|
||||
#include "test_macros.h"
|
||||
|
||||
#include "box3d/box3d.h"
|
||||
|
|
@ -659,47 +658,6 @@ static int TestSetWorkerCount( void )
|
|||
return 0;
|
||||
}
|
||||
|
||||
// This tests continuous collision and mesh contact stability
|
||||
static int TestMeshDrop( void )
|
||||
{
|
||||
b3WorldDef worldDef = b3DefaultWorldDef();
|
||||
// todo GetCoreCount
|
||||
worldDef.workerCount = 4;
|
||||
|
||||
b3WorldId worldId = b3CreateWorld( &worldDef );
|
||||
|
||||
MeshDropData data = CreateMeshDrop( worldId, b3Pos_zero );
|
||||
|
||||
float timeStep = 1.0f / 60.0f;
|
||||
|
||||
int stepIndex = 0;
|
||||
int stepLimit = 400;
|
||||
|
||||
while ( stepIndex < stepLimit )
|
||||
{
|
||||
int subStepCount = 4;
|
||||
b3World_Step( worldId, timeStep, subStepCount );
|
||||
|
||||
b3BodyEvents events = b3World_GetBodyEvents( worldId );
|
||||
if ( events.moveCount == 0 )
|
||||
{
|
||||
// All bodies sleeping
|
||||
break;
|
||||
}
|
||||
|
||||
stepIndex += 1;
|
||||
}
|
||||
|
||||
printf( " TestMeshDrop stepIndex = %d\n", stepIndex );
|
||||
|
||||
DestroyMeshDrop( &data );
|
||||
b3DestroyWorld( worldId );
|
||||
|
||||
ENSURE( stepIndex < stepLimit );
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Verifies the b3*_Overflow solver path. The scene puts >B3_DYNAMIC_COLOR_COUNT
|
||||
// dyn-dyn contacts on a single hub body so several land in the overflow color.
|
||||
// The new Prepare/Store contactId-pairing asserts in contact_solver.c fire here
|
||||
|
|
@ -1080,7 +1038,6 @@ int WorldTest( void )
|
|||
RUN_SUBTEST( TestContactEvents );
|
||||
RUN_SUBTEST( TestHitEvents );
|
||||
RUN_SUBTEST( TestCompoundHitEvents );
|
||||
RUN_SUBTEST( TestMeshDrop );
|
||||
RUN_SUBTEST( TestOverflowColorPile );
|
||||
RUN_SUBTEST( SetBulletDriftTest );
|
||||
RUN_SUBTEST( EnableSleepFlagSyncTest );
|
||||
|
|
|
|||
Loading…
Reference in a new issue