diff --git a/benchmark/CMakeLists.txt b/benchmark/CMakeLists.txt index 629dfb9..7af976b 100644 --- a/benchmark/CMakeLists.txt +++ b/benchmark/CMakeLists.txt @@ -36,3 +36,18 @@ if (NOT BUILD_SHARED_LIBS) target_include_directories(mesh_soak PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../src) target_link_libraries(mesh_soak PRIVATE box3d) endif() + +# Headless Ghost Collisions sample. Counts ghost manifolds per scene and body. +add_executable(ghost_scan ghost_scan.c) + +set_target_properties(ghost_scan PROPERTIES + C_STANDARD 17 + C_STANDARD_REQUIRED YES + C_EXTENSIONS NO +) + +if (BOX3D_COMPILE_WARNING_AS_ERROR) + set_target_properties(ghost_scan PROPERTIES COMPILE_WARNING_AS_ERROR ON) +endif() + +target_link_libraries(ghost_scan PRIVATE box3d shared) diff --git a/benchmark/ghost_baseline.csv b/benchmark/ghost_baseline.csv new file mode 100644 index 0000000..8d28190 --- /dev/null +++ b/benchmark/ghost_baseline.csv @@ -0,0 +1,111 @@ +scene,preset,mirror,body,ghosts,runs +flat,0,0,box,0,2 +flat,0,0,capsule,0,2 +flat,0,0,upright,0,2 +flat,0,0,sphere,0,2 +flat,0,0,cylinder,0,2 +flat,0,1,box,0,2 +flat,0,1,capsule,0,2 +flat,0,1,upright,0,2 +flat,0,1,sphere,0,2 +flat,0,1,cylinder,0,2 +ramp,0,0,box,0,3 +ramp,0,0,capsule,0,3 +ramp,0,0,upright,0,3 +ramp,0,0,sphere,0,3 +ramp,0,0,cylinder,0,3 +ramp,0,1,box,0,3 +ramp,0,1,capsule,0,3 +ramp,0,1,upright,0,3 +ramp,0,1,sphere,0,3 +ramp,0,1,cylinder,0,3 +cliff,0,0,box,0,2 +cliff,0,0,capsule,0,2 +cliff,0,0,upright,0,2 +cliff,0,0,sphere,0,2 +cliff,0,0,cylinder,2,2 +cliff,0,1,box,2,2 +cliff,0,1,capsule,0,2 +cliff,0,1,upright,0,2 +cliff,0,1,sphere,0,2 +cliff,0,1,cylinder,82,1 +crest,0,0,box,15,1 +crest,0,0,capsule,0,2 +crest,0,0,upright,0,2 +crest,0,0,sphere,0,2 +crest,0,0,cylinder,12,1 +crest,0,1,box,12,1 +crest,0,1,capsule,0,2 +crest,0,1,upright,0,2 +crest,0,1,sphere,0,2 +crest,0,1,cylinder,14,0 +bumps,0,0,box,602,0 +bumps,0,0,capsule,0,1 +bumps,0,0,upright,0,1 +bumps,0,0,sphere,0,1 +bumps,0,0,cylinder,91,0 +bumps,0,1,box,10,0 +bumps,0,1,capsule,0,1 +bumps,0,1,upright,0,1 +bumps,0,1,sphere,0,1 +bumps,0,1,cylinder,20,0 +tjunction,0,0,box,0,2 +tjunction,0,0,capsule,0,2 +tjunction,0,0,upright,0,2 +tjunction,0,0,sphere,0,2 +tjunction,0,0,cylinder,11,2 +tjunction,0,1,box,0,2 +tjunction,0,1,capsule,0,2 +tjunction,0,1,upright,0,2 +tjunction,0,1,sphere,0,2 +tjunction,0,1,cylinder,12,2 +seam,0,0,box,6,1 +seam,0,0,capsule,0,2 +seam,0,0,upright,0,2 +seam,0,0,sphere,0,2 +seam,0,0,cylinder,6,1 +seam,0,1,box,0,2 +seam,0,1,capsule,0,2 +seam,0,1,upright,0,2 +seam,0,1,sphere,0,2 +seam,0,1,cylinder,0,2 +peel,0,0,box,6,2 +peel,0,0,capsule,0,2 +peel,0,0,upright,0,2 +peel,0,0,sphere,0,2 +peel,0,0,cylinder,2,1 +peel,0,1,box,2,2 +peel,0,1,capsule,0,2 +peel,0,1,upright,0,2 +peel,0,1,sphere,0,2 +peel,0,1,cylinder,0,2 +peel,1,0,box,4,2 +peel,1,0,capsule,0,2 +peel,1,0,upright,0,2 +peel,1,0,sphere,0,2 +peel,1,0,cylinder,2,2 +peel,1,1,box,2,2 +peel,1,1,capsule,0,2 +peel,1,1,upright,0,2 +peel,1,1,sphere,0,2 +peel,1,1,cylinder,0,2 +peel,2,0,box,8,1 +peel,2,0,capsule,0,2 +peel,2,0,upright,0,2 +peel,2,0,sphere,0,2 +peel,2,0,cylinder,36,1 +peel,2,1,box,4,2 +peel,2,1,capsule,0,2 +peel,2,1,upright,0,2 +peel,2,1,sphere,0,2 +peel,2,1,cylinder,20,2 +peel,3,0,box,9,1 +peel,3,0,capsule,0,2 +peel,3,0,upright,0,2 +peel,3,0,sphere,0,2 +peel,3,0,cylinder,28,2 +peel,3,1,box,6,2 +peel,3,1,capsule,0,2 +peel,3,1,upright,0,2 +peel,3,1,sphere,0,2 +peel,3,1,cylinder,12,2 diff --git a/benchmark/ghost_scan.c b/benchmark/ghost_scan.c new file mode 100644 index 0000000..296fb71 --- /dev/null +++ b/benchmark/ghost_scan.c @@ -0,0 +1,406 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +// Headless scan of the Mesh / Ghost Collisions sample. Runs every scene, body, mirror and PEEL +// preset for a fixed number of steps and counts the ghost manifolds that pushed on the body. A +// written result file can be compared against a later build, which fails on any count that grew. + +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include "ghost_collisions.h" + +#include "box3d/box3d.h" + +#include +#include +#include + +// Command line and result file keys +static const char* s_sceneKeys[e_ghostSceneCount] = { "flat", "ramp", "cliff", "crest", "bumps", "tjunction", "seam", "peel" }; +static const char* s_bodyKeys[e_ghostBodyCount] = { "box", "capsule", "upright", "sphere", "cylinder" }; + +#define MAX_CONFIGS 256 + +typedef struct Config +{ + GhostScene scene; + int preset; + bool mirror; + GhostBody body; + int ghostCount; + int runCount; + + // From the compare file, -1 when absent + int baseGhostCount; +} Config; + +static const char* MatchValue( const char* arg, const char* shortName, const char* longName ) +{ + size_t shortLength = strlen( shortName ); + if ( strncmp( arg, shortName, shortLength ) == 0 ) + { + return arg + shortLength; + } + + size_t longLength = strlen( longName ); + if ( strncmp( arg, longName, longLength ) == 0 ) + { + return arg + longLength; + } + + return NULL; +} + +// Returns -1 for all, -2 for unknown +static int ParseKey( const char* value, const char** keys, int count ) +{ + if ( strcmp( value, "all" ) == 0 ) + { + return -1; + } + + for ( int i = 0; i < count; ++i ) + { + if ( strcmp( value, keys[i] ) == 0 ) + { + return i; + } + } + + char* end; + long index = strtol( value, &end, 10 ); + if ( *end == '\0' && 0 <= index && index < count ) + { + return (int)index; + } + + return -2; +} + +static void RunConfig( Config* config, GhostSettings settings, int stepCount, bool verbose ) +{ + settings.scene = config->scene; + settings.peelPreset = config->preset; + settings.mirror = config->mirror; + settings.body = config->body; + + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + GhostCollisionsData data = CreateGhostCollisions( worldId, settings ); + + for ( int step = 0; step < stepCount; ++step ) + { + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + + GhostReport reports[16]; + int ghostCount = UpdateGhostCollisions( worldId, &data, reports, 16 ); + if ( verbose ) + { + ghostCount = b3MinInt( ghostCount, 16 ); + for ( int i = 0; i < ghostCount; ++i ) + { + GhostReport* r = reports + i; + printf( " ghost step %d run %d n (%.3f %.3f %.3f) p (%.2f %.2f %.2f) sep %.4f impulse %.3f\n", step + 1, data.runCount, + r->normal.x, r->normal.y, r->normal.z, (float)r->point.x, (float)r->point.y, (float)r->point.z, + r->separation, r->impulse ); + } + } + } + + config->ghostCount = data.ghostCount; + config->runCount = data.runCount; + + b3DestroyWorld( worldId ); + DestroyGhostCollisions( &data ); +} + +static void FormatRow( char* buffer, size_t size, const Config* config ) +{ + if ( config->scene == e_ghostInternalEdges ) + { + snprintf( buffer, size, "%s %d%s", s_sceneKeys[config->scene], config->preset, config->mirror ? " mirror" : "" ); + } + else + { + snprintf( buffer, size, "%s%s", s_sceneKeys[config->scene], config->mirror ? " mirror" : "" ); + } +} + +static bool WriteResults( const char* path, const Config* configs, int configCount ) +{ + FILE* file = fopen( path, "w" ); + if ( file == NULL ) + { + printf( "cannot write %s\n", path ); + return false; + } + + fprintf( file, "scene,preset,mirror,body,ghosts,runs\n" ); + for ( int i = 0; i < configCount; ++i ) + { + const Config* c = configs + i; + fprintf( file, "%s,%d,%d,%s,%d,%d\n", s_sceneKeys[c->scene], c->preset, c->mirror ? 1 : 0, s_bodyKeys[c->body], c->ghostCount, + c->runCount ); + } + + fclose( file ); + return true; +} + +static bool ReadBaseline( const char* path, Config* configs, int configCount ) +{ + FILE* file = fopen( path, "r" ); + if ( file == NULL ) + { + printf( "cannot read %s\n", path ); + return false; + } + + char line[256]; + while ( fgets( line, sizeof( line ), file ) != NULL ) + { + char sceneKey[32], bodyKey[32]; + int preset, mirror, ghosts, runs; + if ( sscanf( line, "%31[^,],%d,%d,%31[^,],%d,%d", sceneKey, &preset, &mirror, bodyKey, &ghosts, &runs ) != 6 ) + { + continue; + } + + int scene = ParseKey( sceneKey, s_sceneKeys, e_ghostSceneCount ); + int body = ParseKey( bodyKey, s_bodyKeys, e_ghostBodyCount ); + for ( int i = 0; i < configCount; ++i ) + { + Config* c = configs + i; + if ( (int)c->scene == scene && c->preset == preset && c->mirror == ( mirror != 0 ) && (int)c->body == body ) + { + c->baseGhostCount = ghosts; + } + } + } + + fclose( file ); + return true; +} + +int main( int argc, char** argv ) +{ + int sceneFilter = -1; + int bodyFilter = -1; + int mirrorFilter = -1; + int presetFilter = -1; + int stepCount = 900; + bool verbose = false; + const char* writePath = NULL; + const char* comparePath = NULL; + GhostSettings settings = GetDefaultGhostSettings(); + + for ( int i = 1; i < argc; ++i ) + { + const char* arg = argv[i]; + const char* value; + + if ( ( value = MatchValue( arg, "-s=", "--scene=" ) ) != NULL ) + { + sceneFilter = ParseKey( value, s_sceneKeys, e_ghostSceneCount ); + } + else if ( ( value = MatchValue( arg, "-b=", "--body=" ) ) != NULL ) + { + bodyFilter = ParseKey( value, s_bodyKeys, e_ghostBodyCount ); + } + else if ( ( value = MatchValue( arg, "-m=", "--mirror=" ) ) != NULL ) + { + mirrorFilter = strcmp( value, "all" ) == 0 ? -1 : atoi( value ) != 0; + } + else if ( ( value = MatchValue( arg, "-p=", "--preset=" ) ) != NULL ) + { + presetFilter = strcmp( value, "all" ) == 0 ? -1 : b3ClampInt( atoi( value ), 0, GHOST_PEEL_PRESET_COUNT - 1 ); + } + else if ( ( value = MatchValue( arg, "-n=", "--steps=" ) ) != NULL ) + { + stepCount = atoi( value ); + } + else if ( ( value = MatchValue( arg, "-w=", "--write=" ) ) != NULL ) + { + writePath = value; + } + else if ( ( value = MatchValue( arg, "-c=", "--compare=" ) ) != NULL ) + { + comparePath = value; + } + else if ( ( value = MatchValue( arg, "--speed=", "--speed=" ) ) != NULL ) + { + settings.speed = (float)atof( value ); + } + else if ( ( value = MatchValue( arg, "--yaw=", "--yaw=" ) ) != NULL ) + { + settings.yaw = (float)atof( value ); + } + else if ( ( value = MatchValue( arg, "--friction=", "--friction=" ) ) != NULL ) + { + settings.friction = (float)atof( value ); + } + else if ( ( value = MatchValue( arg, "--cell=", "--cell=" ) ) != NULL ) + { + settings.cellSize = (float)atof( value ); + } + else if ( strcmp( arg, "--fixed-rotation" ) == 0 ) + { + settings.fixedRotation = true; + } + else if ( strcmp( arg, "--no-speculative" ) == 0 ) + { + settings.speculative = false; + } + else if ( strcmp( arg, "-v" ) == 0 || strcmp( arg, "--verbose" ) == 0 ) + { + verbose = true; + } + else + { + sceneFilter = -2; + } + + if ( sceneFilter == -2 || bodyFilter == -2 ) + { + printf( "Usage: ghost_scan [options]\n" + "Runs the Ghost Collisions sample headless and counts ghost manifolds per scene and body.\n" + " -s, --scene= flat, ramp, cliff, crest, bumps, tjunction, seam, peel (default all)\n" + " -b, --body= box, capsule, upright, sphere, cylinder (default all)\n" + " -m, --mirror=<0|1|all> mirrored mesh (default all)\n" + " -p, --preset=<0-3|all> PEEL preset (default all)\n" + " -n, --steps= steps per config (default 900)\n" + " -w, --write= write results as csv\n" + " -c, --compare= compare with a written csv, fail if any count grew\n" + " --speed=, --yaw=, --friction=, --cell=, --fixed-rotation, --no-speculative\n" + " -v, --verbose print every ghost\n" + "Exit code is 1 when a compared count grew, 0 otherwise.\n" ); + return 2; + } + } + + Config configs[MAX_CONFIGS]; + int configCount = 0; + for ( int scene = 0; scene < e_ghostSceneCount; ++scene ) + { + if ( sceneFilter >= 0 && scene != sceneFilter ) + { + continue; + } + + int presetCount = scene == e_ghostInternalEdges ? GHOST_PEEL_PRESET_COUNT : 1; + for ( int preset = 0; preset < presetCount; ++preset ) + { + if ( scene == e_ghostInternalEdges && presetFilter >= 0 && preset != presetFilter ) + { + continue; + } + + for ( int mirror = 0; mirror < 2; ++mirror ) + { + if ( mirrorFilter >= 0 && mirror != mirrorFilter ) + { + continue; + } + + for ( int body = 0; body < e_ghostBodyCount; ++body ) + { + if ( bodyFilter >= 0 && body != bodyFilter ) + { + continue; + } + + configs[configCount++] = (Config){ + .scene = (GhostScene)scene, + .preset = preset, + .mirror = mirror != 0, + .body = (GhostBody)body, + .baseGhostCount = -1, + }; + } + } + } + } + + if ( comparePath != NULL && ReadBaseline( comparePath, configs, configCount ) == false ) + { + return 2; + } + + printf( "ghost scan: %d configs, %d steps, speed %.1f, yaw %.0f, friction %.2f, cell %.2f\n", configCount, stepCount, + settings.speed, settings.yaw, settings.friction, settings.cellSize ); + fflush( stdout ); + + uint64_t ticks = b3GetTicks(); + for ( int i = 0; i < configCount; ++i ) + { + if ( verbose ) + { + char row[64]; + FormatRow( row, sizeof( row ), configs + i ); + printf( "%s %s\n", row, s_bodyKeys[configs[i].body] ); + } + + RunConfig( configs + i, settings, stepCount, verbose ); + } + float seconds = 0.001f * b3GetMilliseconds( ticks ); + + // One row per ground, one column per body. A compared cell that changed prints as old>new. + printf( "\n%-18s", "" ); + for ( int body = 0; body < e_ghostBodyCount; ++body ) + { + if ( bodyFilter < 0 || body == bodyFilter ) + { + printf( "%10s", s_bodyKeys[body] ); + } + } + printf( "\n" ); + + int totalGhosts = 0; + int grewCount = 0; + int shrankCount = 0; + for ( int i = 0; i < configCount; ++i ) + { + const Config* c = configs + i; + bool newRow = i == 0 || c->scene != configs[i - 1].scene || c->preset != configs[i - 1].preset || + c->mirror != configs[i - 1].mirror; + if ( newRow ) + { + char row[64]; + FormatRow( row, sizeof( row ), c ); + printf( "%s%-18s", i == 0 ? "" : "\n", row ); + } + + char cell[32]; + if ( c->baseGhostCount >= 0 && c->baseGhostCount != c->ghostCount ) + { + snprintf( cell, sizeof( cell ), "%d>%d", c->baseGhostCount, c->ghostCount ); + } + else + { + snprintf( cell, sizeof( cell ), "%d", c->ghostCount ); + } + printf( "%10s", cell ); + + totalGhosts += c->ghostCount; + grewCount += c->baseGhostCount >= 0 && c->ghostCount > c->baseGhostCount; + shrankCount += c->baseGhostCount >= 0 && c->ghostCount < c->baseGhostCount; + } + + printf( "\n\ntotal ghosts %d, time %.1f s\n", totalGhosts, seconds ); + + if ( writePath != NULL && WriteResults( writePath, configs, configCount ) == false ) + { + return 2; + } + + if ( comparePath != NULL ) + { + printf( "compared with %s: %d grew, %d shrank\n", comparePath, grewCount, shrankCount ); + printf( grewCount == 0 ? "PASS\n" : "FAIL\n" ); + return grewCount == 0 ? 0 : 1; + } + + return 0; +} diff --git a/samples/sample_continuous.cpp b/samples/sample_continuous.cpp index 31c94dc..81de7aa 100644 --- a/samples/sample_continuous.cpp +++ b/samples/sample_continuous.cpp @@ -564,8 +564,8 @@ public: // A non-zero seed replays a generation reported by the mesh_soak tool g_randomSeed = m_replaySeed != 0 ? m_replaySeed : (uint32_t)b3GetTicks(); - bool simulateAll = true; - //g_randomSeed = 2772533094; + bool simulateAll = false; + g_randomSeed = 2256131175; m_runCount += 1; m_stepCount = 0; @@ -578,7 +578,7 @@ public: b3Vec3 linearVelocity = RandomVec3Uniform( -1.0f, 1.0f ); b3Vec3 angularVelocity = RandomVec3Uniform( -5.0f, 5.0f ); - if ( simulateAll || ( i == 9 && j == 3 ) ) + if ( simulateAll || ( i == 31 && j == 20 ) ) { bodyDef.position = { 0.5f * ( i - 0.5f * m_gridCount ), 5.0f, 0.5f * ( j - 0.5f * m_gridCount ) }; bodyDef.linearVelocity = linearVelocity; diff --git a/samples/sample_mesh.cpp b/samples/sample_mesh.cpp index 1a76391..604f7e5 100644 --- a/samples/sample_mesh.cpp +++ b/samples/sample_mesh.cpp @@ -1,6 +1,7 @@ // SPDX-FileCopyrightText: 2025 Erin Catto // SPDX-License-Identifier: MIT +#include "ghost_collisions.h" #include "human.h" #include "mesh_loader.h" #include "sample.h" @@ -1622,54 +1623,11 @@ public: static int sampleHollowBox = RegisterSample( "Mesh", "Hollow Box", HollowBox::Create ); -// Ghost collision scenes. Each ground is a profile in the xy-plane extruded along z and finely -// tessellated, so a sliding body crosses many internal edges. A manifold that pushes on the body -// is a ghost when its normal points outside the span of face normals under the body. Those are the -// ones that block or launch a body. +// Ghost collision scenes. The scenes, launch and ghost test live in shared/ghost_collisions.c so the +// headless ghost scan measures the same thing. class GhostCollisions : public Sample { public: - enum Scene - { - e_flat, - e_concaveRamp, - e_cliff, - e_crest, - e_bumps, - e_tJunction, - e_shapeSeam, - e_internalEdges, - e_sceneCount - }; - - enum BodyType - { - e_box, - e_capsule, - e_uprightCapsule, - e_sphere, - e_cylinder, - e_bodyTypeCount - }; - - // Mesh variants of the PEEL InternalEdges presets - struct PeelPreset - { - int columnCount; - float scaleX; - float startZ; - }; - - static constexpr PeelPreset m_peelPresets[] = { - { 4, 0.1f, -55.0f }, - { 4, 0.1f, -54.0f }, - { 4, 0.01f, -55.0f }, - { 16, 0.01f, -54.0f }, - }; - - static constexpr float m_halfLength = 20.0f; - static constexpr float m_halfWidth = 3.0f; - static constexpr float m_startX = -16.0f; static constexpr int m_markerCapacity = 256; explicit GhostCollisions( SampleContext* context ) @@ -1680,440 +1638,28 @@ public: m_camera->SetView( 90.0f, 20.0f, 30.0f, { 0.0f, 1.0f, 0.0f } ); } - m_cylinder = b3CreateCylinder( 0.4f, 0.4f, -0.2f, 16 ); - m_peelCylinder = b3CreateCylinder( 2.0f, 1.0f, -1.0f, 16 ); - m_groundId = b3_nullBodyId; - m_bodyId = b3_nullBodyId; - m_shapeId = b3_nullShapeId; - - CreateGround(); - Launch(); + m_data = CreateGhostCollisions( m_worldId, GetDefaultGhostSettings() ); } ~GhostCollisions() override { - DestroyMeshes(); - b3DestroyHull( m_cylinder ); - b3DestroyHull( m_peelCylinder ); - } - - void DestroyMeshes() - { - for ( int i = 0; i < m_meshCount; ++i ) - { - b3DestroyMesh( m_meshes[i] ); - m_meshes[i] = nullptr; - } - m_meshCount = 0; - } - - // The profile runs in +x with the solid below it. The mirror flips x in the data and the mesh - // scale flips it back, so the world geometry is the same either way. - void AddStrip( std::vector& vertices, std::vector& indices, const b3Vec2* profile, int count, - float zCell ) const - { - std::vector points; - points.push_back( profile[0] ); - for ( int k = 0; k + 1 < count; ++k ) - { - b3Vec2 a = profile[k]; - b3Vec2 b = profile[k + 1]; - float length = sqrtf( ( b.x - a.x ) * ( b.x - a.x ) + ( b.y - a.y ) * ( b.y - a.y ) ); - int n = b3MaxInt( 1, (int)ceilf( length / m_cellSize - 0.01f ) ); - for ( int s = 1; s <= n; ++s ) - { - float t = (float)s / (float)n; - points.push_back( { a.x + t * ( b.x - a.x ), a.y + t * ( b.y - a.y ) } ); - } - } - - int zCount = b3MaxInt( 1, (int)ceilf( 2.0f * m_halfWidth / zCell - 0.01f ) ); - AddGrid( vertices, indices, points, m_halfWidth, zCount, true ); - } - - void AddGrid( std::vector& vertices, std::vector& indices, const std::vector& points, - float halfWidth, int zCount, bool alternate ) const - { - float sign = m_mirror ? -1.0f : 1.0f; - int stride = zCount + 1; - int base = (int)vertices.size(); - - for ( b3Vec2 p : points ) - { - for ( int j = 0; j <= zCount; ++j ) - { - float z = -halfWidth + 2.0f * halfWidth * (float)j / (float)zCount; - vertices.push_back( { sign * p.x, p.y, z } ); - } - } - - bool mirror = m_mirror; - auto emit = [&]( int i1, int i2, int i3 ) { - indices.push_back( i1 ); - indices.push_back( mirror ? i3 : i2 ); - indices.push_back( mirror ? i2 : i3 ); - }; - - int pointCount = (int)points.size(); - for ( int i = 0; i + 1 < pointCount; ++i ) - { - for ( int j = 0; j < zCount; ++j ) - { - int a = base + i * stride + j; - int b = a + 1; - int c = a + stride + 1; - int d = a + stride; - - // Alternate the diagonal like cooked level geometry - if ( alternate && ( ( i + j ) & 1 ) ) - { - emit( a, b, d ); - emit( b, c, d ); - } - else - { - emit( a, b, c ); - emit( c, d, a ); - } - } - } - } - - void AddMesh( std::vector& vertices, std::vector& indices, bool frictionless = false ) - { - b3MeshDef meshDef = {}; - meshDef.vertices = vertices.data(); - meshDef.indices = indices.data(); - meshDef.vertexCount = (int)vertices.size(); - meshDef.triangleCount = (int)( indices.size() / 3 ); - meshDef.weldVertices = true; - meshDef.weldTolerance = 0.001f; - meshDef.identifyEdges = true; - - b3MeshData* mesh = b3CreateMesh( &meshDef, nullptr, 0 ); - m_meshes[m_meshCount++] = mesh; - - b3Vec3 scale = m_mirror ? b3Vec3{ -1.0f, 1.0f, 1.0f } : b3Vec3_one; - b3ShapeDef shapeDef = b3DefaultShapeDef(); - if ( frictionless ) - { - shapeDef.baseMaterial.friction = 0.0f; - } - b3CreateMeshShape( m_groundId, &shapeDef, mesh, scale ); - } - - void CreateGround() - { - if ( B3_IS_NON_NULL( m_groundId ) ) - { - b3DestroyBody( m_groundId ); - } - DestroyMeshes(); - - b3BodyDef bodyDef = b3DefaultBodyDef(); - m_groundId = b3CreateBody( m_worldId, &bodyDef ); - - float h = m_halfLength; - m_profile.clear(); - - switch ( m_scene ) - { - case e_concaveRamp: - { - float slope = 8.0f * B3_PI / 180.0f; - m_profile = { { -h, h * tanf( slope ) }, { 0.0f, 0.0f }, { h, 0.0f } }; - } - break; - - case e_cliff: - m_profile = { { -h, 3.0f }, { 0.0f, 3.0f }, { 0.0f, 0.0f }, { h, 0.0f } }; - break; - - case e_crest: - m_profile = { { -h, 0.0f }, { -10.0f, 0.0f }, { 0.0f, 1.0f }, { 10.0f, 0.0f }, { h, 0.0f } }; - break; - - case e_bumps: - // Gentle folds of a few degrees, the range that only flat edge suppression misses - for ( int i = 0; i <= 80; ++i ) - { - float x = -h + 0.5f * (float)i; - float y = x < -10.0f ? 0.0f : 0.05f * ( 1.0f - cosf( B3_PI * x ) ); - m_profile.push_back( { x, y } ); - } - break; - - case e_internalEdges: - // PEEL's curved mesh: 32 rows 4 m apart, each step up 1.25 times the last, sliding toward +x - for ( int i = 31; i >= 0; --i ) - { - m_profile.push_back( { 62.0f - 4.0f * (float)i, 1.0f + 0.1f * powf( 1.25f, (float)i ) } ); - } - break; - - default: - m_profile = { { -h, 0.0f }, { h, 0.0f } }; - break; - } - - std::vector vertices; - std::vector indices; - b3Vec2 left[2] = { { -h, 0.0f }, { 0.0f, 0.0f } }; - b3Vec2 right[2] = { { 0.0f, 0.0f }, { h, 0.0f } }; - - if ( m_scene == e_tJunction ) - { - // Fine and coarse strips share only every other vertex along x = 0 - AddStrip( vertices, indices, left, 2, 0.5f * m_cellSize ); - AddStrip( vertices, indices, right, 2, m_cellSize ); - AddMesh( vertices, indices ); - } - else if ( m_scene == e_shapeSeam ) - { - // Two mesh shapes meeting at x = 0, so the seam spans two contacts - AddStrip( vertices, indices, left, 2, m_cellSize ); - AddMesh( vertices, indices ); - - vertices.clear(); - indices.clear(); - AddStrip( vertices, indices, right, 2, m_cellSize ); - AddMesh( vertices, indices ); - } - else if ( m_scene == e_internalEdges ) - { - // The mesh has no friction and the ground plane under it does, as in PEEL - const PeelPreset& preset = m_peelPresets[m_peelPreset]; - int zCount = preset.columnCount - 1; - float halfWidth = 20.0f * preset.scaleX * (float)zCount; - AddGrid( vertices, indices, m_profile, halfWidth, zCount, false ); - AddMesh( vertices, indices, true ); - - b3BoxHull box = b3MakeOffsetBoxHull( 140.0f, 1.0f, 30.0f, { 65.0f, -1.0f, 0.0f } ); - b3ShapeDef shapeDef = b3DefaultShapeDef(); - b3CreateHullShape( m_groundId, &shapeDef, &box.base ); - } - else - { - AddStrip( vertices, indices, m_profile.data(), (int)m_profile.size(), m_cellSize ); - AddMesh( vertices, indices ); - } - } - - void Launch() - { - if ( B3_IS_NON_NULL( m_bodyId ) ) - { - b3DestroyBody( m_bodyId ); - } - - // PEEL drops its bodies from rest, sized in meters rather than tenths - bool peel = m_scene == e_internalEdges; - float boxHalf = peel ? 1.0f : 0.2f; - float capsuleRadius = peel ? 1.0f : 0.2f; - float capsuleHalf = peel ? 2.0f : 0.4f; - float sphereRadius = peel ? 1.0f : 0.25f; - - b3BodyDef bodyDef = b3DefaultBodyDef(); - bodyDef.type = b3_dynamicBody; - b3Quat yaw = b3MakeQuatFromAxisAngle( b3Vec3_axisY, m_yaw * B3_PI / 180.0f ); - - if ( peel ) - { - bodyDef.position = { m_peelPresets[m_peelPreset].startZ, 80.0f, 0.0f }; - bodyDef.rotation = yaw; - } - else - { - int segment = 0; - for ( int k = 0; k + 1 < (int)m_profile.size(); ++k ) - { - if ( m_profile[k].x <= m_startX && m_startX < m_profile[k + 1].x ) - { - segment = k; - break; - } - } - - b3Vec2 a = m_profile[segment]; - b3Vec2 b = m_profile[segment + 1]; - float length = sqrtf( ( b.x - a.x ) * ( b.x - a.x ) + ( b.y - a.y ) * ( b.y - a.y ) ); - b3Vec2 d = { ( b.x - a.x ) / length, ( b.y - a.y ) / length }; - b3Vec2 n = { -d.y, d.x }; - float y = a.y + ( m_startX - a.x ) * d.y / d.x; - - float lift = boxHalf; - if ( m_bodyType == e_sphere ) - { - lift = sphereRadius; - } - else if ( m_bodyType == e_uprightCapsule ) - { - lift = capsuleHalf + capsuleRadius; - } - lift += 0.01f; - - bodyDef.position = { m_startX + lift * n.x, y + lift * n.y, 0.0f }; - b3Quat slope = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, atan2f( d.y, d.x ) ); - bodyDef.rotation = b3MulQuat( slope, yaw ); - bodyDef.linearVelocity = { m_speed * d.x, m_speed * d.y, 0.0f }; - } - - bodyDef.motionLocks.angularX = m_fixedRotation; - bodyDef.motionLocks.angularY = m_fixedRotation; - bodyDef.motionLocks.angularZ = m_fixedRotation; - bodyDef.enableSleep = false; - bodyDef.name = "slider"; - m_bodyId = b3CreateBody( m_worldId, &bodyDef ); - - b3ShapeDef shapeDef = b3DefaultShapeDef(); - shapeDef.baseMaterial.friction = m_friction; - shapeDef.enableSpeculativeContact = m_speculative; - - switch ( m_bodyType ) - { - case e_box: - { - b3BoxHull box = peel ? b3MakeBoxHull( 1.0f, 1.0f, 1.0f ) : b3MakeBoxHull( 0.4f, 0.2f, 0.4f ); - m_shapeId = b3CreateHullShape( m_bodyId, &shapeDef, &box.base ); - } - break; - - case e_capsule: - { - b3Capsule capsule = { { 0.0f, 0.0f, -capsuleHalf }, { 0.0f, 0.0f, capsuleHalf }, capsuleRadius }; - m_shapeId = b3CreateCapsuleShape( m_bodyId, &shapeDef, &capsule ); - } - break; - - case e_uprightCapsule: - { - b3Capsule capsule = { { 0.0f, -capsuleHalf, 0.0f }, { 0.0f, capsuleHalf, 0.0f }, capsuleRadius }; - m_shapeId = b3CreateCapsuleShape( m_bodyId, &shapeDef, &capsule ); - } - break; - - case e_sphere: - { - b3Sphere sphere = { b3Vec3_zero, sphereRadius }; - m_shapeId = b3CreateSphereShape( m_bodyId, &shapeDef, &sphere ); - } - break; - - default: - m_shapeId = b3CreateHullShape( m_bodyId, &shapeDef, peel ? m_peelCylinder : m_cylinder ); - break; - } - - m_runGhostCount = 0; - m_runSteps = 0; - } - - float SegmentAngle( int k ) const - { - b3Vec2 a = m_profile[k]; - b3Vec2 b = m_profile[k + 1]; - return atan2f( b.x - a.x, a.y - b.y ); - } - - // The normal points from the ground to the body. A hull face can be the reference face, so any - // normal within the span of face normals under the body is valid. - bool IsGhost( b3Vec3 normal, float xMin, float xMax ) const - { - float tolerance = 5.0f * B3_PI / 180.0f; - if ( b3AbsFloat( normal.z ) > sinf( tolerance ) ) - { - return true; - } - - float lower = B3_PI; - float upper = -B3_PI; - int segmentCount = (int)m_profile.size() - 1; - for ( int k = 0; k < segmentCount; ++k ) - { - float x0 = b3MinFloat( m_profile[k].x, m_profile[k + 1].x ); - float x1 = b3MaxFloat( m_profile[k].x, m_profile[k + 1].x ); - if ( x1 < xMin || xMax < x0 ) - { - continue; - } - - float angle = SegmentAngle( k ); - lower = b3MinFloat( lower, angle ); - upper = b3MaxFloat( upper, angle ); - } - - float angle = atan2f( normal.y, normal.x ); - return angle < lower - tolerance || upper + tolerance < angle; + DestroyGhostCollisions( &m_data ); } void Step() override { Sample::Step(); - b3Pos center = b3Body_GetWorldCenter( m_bodyId ); - bool done = center.x > m_halfLength - 1.0f || center.y < -3.0f; - if ( m_scene == e_internalEdges ) - { - // Run out onto the ground plane, or give up on a body knocked off the side - done = center.x > 100.0f || center.y < -3.0f || m_runSteps > 900; - } - - if ( done ) - { - m_runCount += 1; - Launch(); - center = b3Body_GetWorldCenter( m_bodyId ); - } - if ( m_didStep ) { - m_runSteps += 1; - - b3AABB bounds = b3Shape_GetAABB( m_shapeId ); - float xMin = (float)bounds.lowerBound.x - 0.1f; - float xMax = (float)bounds.upperBound.x + 0.1f; - - b3ContactData contacts[8]; - int contactCount = b3Body_GetContactData( m_bodyId, contacts, 8 ); - for ( int i = 0; i < contactCount; ++i ) + GhostReport reports[16]; + int ghostCount = UpdateGhostCollisions( m_worldId, &m_data, reports, IM_ARRAYSIZE( reports ) ); + ghostCount = b3MinInt( ghostCount, IM_ARRAYSIZE( reports ) ); + for ( int i = 0; i < ghostCount; ++i ) { - bool bodyIsA = B3_ID_EQUALS( contacts[i].shapeIdA, m_shapeId ); - b3ShapeId otherId = bodyIsA ? contacts[i].shapeIdB : contacts[i].shapeIdA; - if ( b3Shape_GetType( otherId ) != b3_meshShape ) - { - continue; - } - - for ( int j = 0; j < contacts[i].manifoldCount; ++j ) - { - const b3Manifold* manifold = contacts[i].manifolds + j; - - // Speculative points that never pushed are harmless - float impulse = 0.0f; - for ( int k = 0; k < manifold->pointCount; ++k ) - { - impulse += manifold->points[k].totalNormalImpulse; - } - - if ( manifold->pointCount == 0 || impulse <= 0.0f ) - { - continue; - } - - b3Vec3 normal = bodyIsA ? b3Neg( manifold->normal ) : manifold->normal; - if ( IsGhost( normal, xMin, xMax ) == false ) - { - continue; - } - - m_ghostCount += 1; - m_runGhostCount += 1; - - b3Vec3 anchor = bodyIsA ? manifold->points[0].anchorA : manifold->points[0].anchorB; - m_markers[m_markerIndex] = { center + anchor, normal }; - m_markerIndex = ( m_markerIndex + 1 ) % m_markerCapacity; - m_markerCount = b3MinInt( m_markerCount + 1, m_markerCapacity ); - } + m_markers[m_markerIndex] = { reports[i].point, reports[i].normal }; + m_markerIndex = ( m_markerIndex + 1 ) % m_markerCapacity; + m_markerCount = b3MinInt( m_markerCount + 1, m_markerCapacity ); } } @@ -2124,32 +1670,25 @@ public: DrawLine( p, p + 0.5f * m_markers[i].normal, MakeColor( b3_colorRed ) ); } - b3Vec3 velocity = b3Body_GetLinearVelocity( m_bodyId ); - DrawTextLine( "ghost manifolds: %d this run, %d total over %d runs", m_runGhostCount, m_ghostCount, m_runCount + 1 ); + b3Vec3 velocity = b3Body_GetLinearVelocity( m_data.bodyId ); + DrawTextLine( "ghost manifolds: %d this run, %d total over %d runs", m_data.runGhostCount, m_data.ghostCount, + m_data.runCount + 1 ); DrawTextLine( "speed: %.2f m/s, vertical velocity: %.2f m/s", b3Length( velocity ), velocity.y ); } bool DrawControls() override { - const char* sceneNames[] = { "Flat", "Concave Ramp", "Cliff", "Crest", "Bumps", "T-Junction", "Shape Seam", - "PEEL Internal Edges" }; - static_assert( IM_ARRAYSIZE( sceneNames ) == e_sceneCount ); - const char* bodyNames[] = { "Box", "Capsule", "Upright Capsule", "Sphere", "Cylinder" }; - static_assert( IM_ARRAYSIZE( bodyNames ) == e_bodyTypeCount ); - const char* presetNames[] = { "4 x 0.1, start -55", "4 x 0.1, start -54", "4 x 0.01, start -55", - "16 x 0.01, start -54" }; - static_assert( IM_ARRAYSIZE( presetNames ) == IM_ARRAYSIZE( m_peelPresets ) ); - + GhostSettings& settings = m_data.settings; bool rebuild = false; bool relaunch = false; - int scene = (int)m_scene; - if ( ImGui::Combo( "Scene", &scene, sceneNames, IM_ARRAYSIZE( sceneNames ) ) ) + int scene = (int)settings.scene; + if ( ImGui::Combo( "Scene", &scene, g_ghostSceneNames, e_ghostSceneCount ) ) { - m_scene = (Scene)scene; + settings.scene = (GhostScene)scene; rebuild = true; - if ( m_scene == e_internalEdges ) + if ( settings.scene == e_ghostInternalEdges ) { m_camera->SetView( 60.0f, 20.0f, 160.0f, { 0.0f, 30.0f, 0.0f } ); } @@ -2159,46 +1698,46 @@ public: } } - if ( m_scene == e_internalEdges ) + if ( settings.scene == e_ghostInternalEdges ) { - rebuild |= ImGui::Combo( "Preset", &m_peelPreset, presetNames, IM_ARRAYSIZE( presetNames ) ); + rebuild |= ImGui::Combo( "Preset", &settings.peelPreset, g_ghostPeelPresetNames, GHOST_PEEL_PRESET_COUNT ); } - int bodyType = (int)m_bodyType; - if ( ImGui::Combo( "Body", &bodyType, bodyNames, IM_ARRAYSIZE( bodyNames ) ) ) + int body = (int)settings.body; + if ( ImGui::Combo( "Body", &body, g_ghostBodyNames, e_ghostBodyCount ) ) { - m_bodyType = (BodyType)bodyType; + settings.body = (GhostBody)body; relaunch = true; } - rebuild |= ImGui::Checkbox( "Mirror Mesh", &m_mirror ); - rebuild |= ImGui::SliderFloat( "Cell Size", &m_cellSize, 0.1f, 1.0f, "%.2f m" ); - relaunch |= ImGui::SliderFloat( "Speed", &m_speed, 0.0f, 15.0f, "%.1f m/s" ); - relaunch |= ImGui::SliderFloat( "Yaw", &m_yaw, 0.0f, 45.0f, "%.0f deg" ); - relaunch |= ImGui::SliderFloat( "Friction", &m_friction, 0.0f, 1.0f, "%.2f" ); - relaunch |= ImGui::Checkbox( "Fixed Rotation", &m_fixedRotation ); - relaunch |= ImGui::Checkbox( "Speculative", &m_speculative ); + rebuild |= ImGui::Checkbox( "Mirror Mesh", &settings.mirror ); + rebuild |= ImGui::SliderFloat( "Cell Size", &settings.cellSize, 0.1f, 1.0f, "%.2f m" ); + relaunch |= ImGui::SliderFloat( "Speed", &settings.speed, 0.0f, 15.0f, "%.1f m/s" ); + relaunch |= ImGui::SliderFloat( "Yaw", &settings.yaw, 0.0f, 45.0f, "%.0f deg" ); + relaunch |= ImGui::SliderFloat( "Friction", &settings.friction, 0.0f, 1.0f, "%.2f" ); + relaunch |= ImGui::Checkbox( "Fixed Rotation", &settings.fixedRotation ); + relaunch |= ImGui::Checkbox( "Speculative", &settings.speculative ); relaunch |= ImGui::Button( "Launch" ); if ( ImGui::Button( "Reset Counters" ) ) { - m_ghostCount = 0; - m_runGhostCount = 0; - m_runCount = 0; + m_data.ghostCount = 0; + m_data.runGhostCount = 0; + m_data.runCount = 0; m_markerCount = 0; m_markerIndex = 0; } if ( rebuild ) { - CreateGround(); + CreateGhostGround( m_worldId, &m_data ); m_markerCount = 0; m_markerIndex = 0; } if ( rebuild || relaunch ) { - Launch(); + LaunchGhostBody( m_worldId, &m_data ); } return true; @@ -2215,30 +1754,7 @@ public: b3Vec3 normal; }; - b3MeshData* m_meshes[2] = {}; - int m_meshCount = 0; - std::vector m_profile; - b3HullData* m_cylinder; - b3HullData* m_peelCylinder; - b3BodyId m_groundId; - b3BodyId m_bodyId; - b3ShapeId m_shapeId; - - Scene m_scene = e_flat; - BodyType m_bodyType = e_box; - bool m_mirror = false; - float m_cellSize = 0.25f; - float m_speed = 6.0f; - float m_yaw = 0.0f; - float m_friction = 0.0f; - bool m_fixedRotation = false; - bool m_speculative = true; - int m_peelPreset = 0; - int m_runSteps = 0; - - int m_ghostCount = 0; - int m_runGhostCount = 0; - int m_runCount = 0; + GhostCollisionsData m_data; Marker m_markers[m_markerCapacity]; int m_markerCount = 0; int m_markerIndex = 0; diff --git a/shared/CMakeLists.txt b/shared/CMakeLists.txt index ff640b4..bc47308 100644 --- a/shared/CMakeLists.txt +++ b/shared/CMakeLists.txt @@ -5,6 +5,8 @@ set(BOX3D_SHARED_FILES benchmarks.h determinism.c determinism.h + ghost_collisions.c + ghost_collisions.h human.c human.h overflow_color.c diff --git a/shared/ghost_collisions.c b/shared/ghost_collisions.c new file mode 100644 index 0000000..ca159fd --- /dev/null +++ b/shared/ghost_collisions.c @@ -0,0 +1,550 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "ghost_collisions.h" + +#include "box3d/box3d.h" + +#include +#include +#include + +#define GHOST_HALF_LENGTH 20.0f +#define GHOST_HALF_WIDTH 3.0f +#define GHOST_START_X -16.0f + +typedef struct PeelPreset +{ + int columnCount; + float scaleX; + float startZ; +} PeelPreset; + +static const PeelPreset s_peelPresets[GHOST_PEEL_PRESET_COUNT] = { + { 4, 0.1f, -55.0f }, + { 4, 0.1f, -54.0f }, + { 4, 0.01f, -55.0f }, + { 16, 0.01f, -54.0f }, +}; + +const char* g_ghostSceneNames[e_ghostSceneCount] = { + "Flat", "Concave Ramp", "Cliff", "Crest", "Bumps", "T-Junction", "Shape Seam", "PEEL Internal Edges", +}; + +const char* g_ghostBodyNames[e_ghostBodyCount] = { "Box", "Capsule", "Upright Capsule", "Sphere", "Cylinder" }; + +const char* g_ghostPeelPresetNames[GHOST_PEEL_PRESET_COUNT] = { + "4 x 0.1, start -55", + "4 x 0.1, start -54", + "4 x 0.01, start -55", + "16 x 0.01, start -54", +}; + +GhostSettings GetDefaultGhostSettings( void ) +{ + GhostSettings settings = { + .scene = e_ghostFlat, + .body = e_ghostBox, + .peelPreset = 0, + .mirror = false, + .cellSize = 0.25f, + .speed = 6.0f, + .yaw = 0.0f, + .friction = 0.0f, + .fixedRotation = false, + .speculative = true, + }; + return settings; +} + +typedef struct MeshBuilder +{ + b3Vec3* vertices; + int vertexCount; + int32_t* indices; + int indexCount; +} MeshBuilder; + +// The profile runs in +x with the solid below it. The mirror flips x in the data and the mesh +// scale flips it back, so the world geometry is the same either way. +static void AddGrid( MeshBuilder* builder, const b3Vec2* points, int pointCount, float halfWidth, int zCount, bool alternate, + bool mirror ) +{ + float sign = mirror ? -1.0f : 1.0f; + int stride = zCount + 1; + int base = builder->vertexCount; + + builder->vertices = realloc( builder->vertices, ( builder->vertexCount + pointCount * stride ) * sizeof( b3Vec3 ) ); + for ( int i = 0; i < pointCount; ++i ) + { + for ( int j = 0; j <= zCount; ++j ) + { + float z = -halfWidth + 2.0f * halfWidth * (float)j / (float)zCount; + builder->vertices[builder->vertexCount++] = (b3Vec3){ sign * points[i].x, points[i].y, z }; + } + } + + int triangleCount = 2 * ( pointCount - 1 ) * zCount; + builder->indices = realloc( builder->indices, ( builder->indexCount + 3 * triangleCount ) * sizeof( int32_t ) ); + for ( int i = 0; i + 1 < pointCount; ++i ) + { + for ( int j = 0; j < zCount; ++j ) + { + int a = base + i * stride + j; + int b = a + 1; + int c = a + stride + 1; + int d = a + stride; + + int corners[6] = { a, b, c, c, d, a }; + + // Alternate the diagonal like cooked level geometry + if ( alternate && ( ( i + j ) & 1 ) ) + { + int flipped[6] = { a, b, d, b, c, d }; + memcpy( corners, flipped, sizeof( corners ) ); + } + + for ( int k = 0; k < 6; k += 3 ) + { + builder->indices[builder->indexCount++] = corners[k]; + builder->indices[builder->indexCount++] = mirror ? corners[k + 2] : corners[k + 1]; + builder->indices[builder->indexCount++] = mirror ? corners[k + 1] : corners[k + 2]; + } + } + } +} + +static void AddStrip( MeshBuilder* builder, const b3Vec2* profile, int count, float cellSize, float zCell, bool mirror ) +{ + int pointCount = 1; + for ( int k = 0; k + 1 < count; ++k ) + { + b3Vec2 a = profile[k]; + b3Vec2 b = profile[k + 1]; + float length = sqrtf( ( b.x - a.x ) * ( b.x - a.x ) + ( b.y - a.y ) * ( b.y - a.y ) ); + pointCount += b3MaxInt( 1, (int)ceilf( length / cellSize - 0.01f ) ); + } + + b3Vec2* points = malloc( pointCount * sizeof( b3Vec2 ) ); + int index = 0; + points[index++] = profile[0]; + for ( int k = 0; k + 1 < count; ++k ) + { + b3Vec2 a = profile[k]; + b3Vec2 b = profile[k + 1]; + float length = sqrtf( ( b.x - a.x ) * ( b.x - a.x ) + ( b.y - a.y ) * ( b.y - a.y ) ); + int n = b3MaxInt( 1, (int)ceilf( length / cellSize - 0.01f ) ); + for ( int s = 1; s <= n; ++s ) + { + float t = (float)s / (float)n; + points[index++] = (b3Vec2){ a.x + t * ( b.x - a.x ), a.y + t * ( b.y - a.y ) }; + } + } + + int zCount = b3MaxInt( 1, (int)ceilf( 2.0f * GHOST_HALF_WIDTH / zCell - 0.01f ) ); + AddGrid( builder, points, pointCount, GHOST_HALF_WIDTH, zCount, true, mirror ); + free( points ); +} + +static void AddMesh( GhostCollisionsData* data, MeshBuilder* builder, bool frictionless ) +{ + b3MeshDef meshDef = { 0 }; + meshDef.vertices = builder->vertices; + meshDef.indices = builder->indices; + meshDef.vertexCount = builder->vertexCount; + meshDef.triangleCount = builder->indexCount / 3; + meshDef.weldVertices = true; + meshDef.weldTolerance = 0.001f; + meshDef.identifyEdges = true; + + b3MeshData* mesh = b3CreateMesh( &meshDef, NULL, 0 ); + data->meshes[data->meshCount++] = mesh; + + b3Vec3 scale = data->settings.mirror ? (b3Vec3){ -1.0f, 1.0f, 1.0f } : b3Vec3_one; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + if ( frictionless ) + { + shapeDef.baseMaterial.friction = 0.0f; + } + b3CreateMeshShape( data->groundId, &shapeDef, mesh, scale ); + + builder->vertexCount = 0; + builder->indexCount = 0; +} + +static void DestroyGhostMeshes( GhostCollisionsData* data ) +{ + for ( int i = 0; i < data->meshCount; ++i ) + { + b3DestroyMesh( data->meshes[i] ); + data->meshes[i] = NULL; + } + data->meshCount = 0; +} + +static void PushProfile( GhostCollisionsData* data, float x, float y ) +{ + if ( data->profileCount < GHOST_MAX_PROFILE_POINTS ) + { + data->profile[data->profileCount++] = (b3Vec2){ x, y }; + } +} + +void CreateGhostGround( b3WorldId worldId, GhostCollisionsData* data ) +{ + if ( B3_IS_NON_NULL( data->groundId ) ) + { + b3DestroyBody( data->groundId ); + } + DestroyGhostMeshes( data ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + data->groundId = b3CreateBody( worldId, &bodyDef ); + + const GhostSettings* settings = &data->settings; + float h = GHOST_HALF_LENGTH; + data->profileCount = 0; + + switch ( settings->scene ) + { + case e_ghostConcaveRamp: + { + float slope = 8.0f * B3_PI / 180.0f; + PushProfile( data, -h, h * tanf( slope ) ); + PushProfile( data, 0.0f, 0.0f ); + PushProfile( data, h, 0.0f ); + } + break; + + case e_ghostCliff: + PushProfile( data, -h, 3.0f ); + PushProfile( data, 0.0f, 3.0f ); + PushProfile( data, 0.0f, 0.0f ); + PushProfile( data, h, 0.0f ); + break; + + case e_ghostCrest: + PushProfile( data, -h, 0.0f ); + PushProfile( data, -10.0f, 0.0f ); + PushProfile( data, 0.0f, 1.0f ); + PushProfile( data, 10.0f, 0.0f ); + PushProfile( data, h, 0.0f ); + break; + + case e_ghostBumps: + // Gentle folds of a few degrees, the range that only flat edge suppression misses + for ( int i = 0; i <= 80; ++i ) + { + float x = -h + 0.5f * (float)i; + float y = x < -10.0f ? 0.0f : 0.05f * ( 1.0f - cosf( B3_PI * x ) ); + PushProfile( data, x, y ); + } + break; + + case e_ghostInternalEdges: + // PEEL's curved mesh: 32 rows 4 m apart, each step up 1.25 times the last, sliding toward +x + for ( int i = 31; i >= 0; --i ) + { + PushProfile( data, 62.0f - 4.0f * (float)i, 1.0f + 0.1f * powf( 1.25f, (float)i ) ); + } + break; + + default: + PushProfile( data, -h, 0.0f ); + PushProfile( data, h, 0.0f ); + break; + } + + MeshBuilder builder = { 0 }; + b3Vec2 left[2] = { { -h, 0.0f }, { 0.0f, 0.0f } }; + b3Vec2 right[2] = { { 0.0f, 0.0f }, { h, 0.0f } }; + float cellSize = settings->cellSize; + bool mirror = settings->mirror; + + if ( settings->scene == e_ghostTJunction ) + { + // Fine and coarse strips share only every other vertex along x = 0 + AddStrip( &builder, left, 2, cellSize, 0.5f * cellSize, mirror ); + AddStrip( &builder, right, 2, cellSize, cellSize, mirror ); + AddMesh( data, &builder, false ); + } + else if ( settings->scene == e_ghostShapeSeam ) + { + // Two mesh shapes meeting at x = 0, so the seam spans two contacts + AddStrip( &builder, left, 2, cellSize, cellSize, mirror ); + AddMesh( data, &builder, false ); + AddStrip( &builder, right, 2, cellSize, cellSize, mirror ); + AddMesh( data, &builder, false ); + } + else if ( settings->scene == e_ghostInternalEdges ) + { + // The mesh has no friction and the ground plane under it does, as in PEEL + PeelPreset preset = s_peelPresets[settings->peelPreset]; + int zCount = preset.columnCount - 1; + float halfWidth = 20.0f * preset.scaleX * (float)zCount; + AddGrid( &builder, data->profile, data->profileCount, halfWidth, zCount, false, mirror ); + AddMesh( data, &builder, true ); + + b3BoxHull box = b3MakeOffsetBoxHull( 140.0f, 1.0f, 30.0f, ( b3Vec3 ){ 65.0f, -1.0f, 0.0f } ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( data->groundId, &shapeDef, &box.base ); + } + else + { + AddStrip( &builder, data->profile, data->profileCount, cellSize, cellSize, mirror ); + AddMesh( data, &builder, false ); + } + + free( builder.vertices ); + free( builder.indices ); +} + +void LaunchGhostBody( b3WorldId worldId, GhostCollisionsData* data ) +{ + if ( B3_IS_NON_NULL( data->bodyId ) ) + { + b3DestroyBody( data->bodyId ); + } + + const GhostSettings* settings = &data->settings; + + // PEEL drops its bodies from rest, sized in meters rather than tenths + bool peel = settings->scene == e_ghostInternalEdges; + float boxHalf = peel ? 1.0f : 0.2f; + float capsuleRadius = peel ? 1.0f : 0.2f; + float capsuleHalf = peel ? 2.0f : 0.4f; + float sphereRadius = peel ? 1.0f : 0.25f; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + b3Quat yaw = b3MakeQuatFromAxisAngle( b3Vec3_axisY, settings->yaw * B3_PI / 180.0f ); + + if ( peel ) + { + bodyDef.position = (b3Pos){ s_peelPresets[settings->peelPreset].startZ, 80.0f, 0.0f }; + bodyDef.rotation = yaw; + } + else + { + int segment = 0; + for ( int k = 0; k + 1 < data->profileCount; ++k ) + { + if ( data->profile[k].x <= GHOST_START_X && GHOST_START_X < data->profile[k + 1].x ) + { + segment = k; + break; + } + } + + b3Vec2 a = data->profile[segment]; + b3Vec2 b = data->profile[segment + 1]; + float length = sqrtf( ( b.x - a.x ) * ( b.x - a.x ) + ( b.y - a.y ) * ( b.y - a.y ) ); + b3Vec2 d = { ( b.x - a.x ) / length, ( b.y - a.y ) / length }; + b3Vec2 n = { -d.y, d.x }; + float y = a.y + ( GHOST_START_X - a.x ) * d.y / d.x; + + float lift = boxHalf; + if ( settings->body == e_ghostSphere ) + { + lift = sphereRadius; + } + else if ( settings->body == e_ghostUprightCapsule ) + { + lift = capsuleHalf + capsuleRadius; + } + lift += 0.01f; + + bodyDef.position = (b3Pos){ GHOST_START_X + lift * n.x, y + lift * n.y, 0.0f }; + b3Quat slope = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, atan2f( d.y, d.x ) ); + bodyDef.rotation = b3MulQuat( slope, yaw ); + bodyDef.linearVelocity = (b3Vec3){ settings->speed * d.x, settings->speed * d.y, 0.0f }; + } + + bodyDef.motionLocks.angularX = settings->fixedRotation; + bodyDef.motionLocks.angularY = settings->fixedRotation; + bodyDef.motionLocks.angularZ = settings->fixedRotation; + bodyDef.enableSleep = false; + bodyDef.name = "slider"; + data->bodyId = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = settings->friction; + shapeDef.enableSpeculativeContact = settings->speculative; + + switch ( settings->body ) + { + case e_ghostBox: + { + b3BoxHull box = peel ? b3MakeBoxHull( 1.0f, 1.0f, 1.0f ) : b3MakeBoxHull( 0.4f, 0.2f, 0.4f ); + data->shapeId = b3CreateHullShape( data->bodyId, &shapeDef, &box.base ); + } + break; + + case e_ghostCapsule: + { + b3Capsule capsule = { { 0.0f, 0.0f, -capsuleHalf }, { 0.0f, 0.0f, capsuleHalf }, capsuleRadius }; + data->shapeId = b3CreateCapsuleShape( data->bodyId, &shapeDef, &capsule ); + } + break; + + case e_ghostUprightCapsule: + { + b3Capsule capsule = { { 0.0f, -capsuleHalf, 0.0f }, { 0.0f, capsuleHalf, 0.0f }, capsuleRadius }; + data->shapeId = b3CreateCapsuleShape( data->bodyId, &shapeDef, &capsule ); + } + break; + + case e_ghostSphere: + { + b3Sphere sphere = { b3Vec3_zero, sphereRadius }; + data->shapeId = b3CreateSphereShape( data->bodyId, &shapeDef, &sphere ); + } + break; + + default: + data->shapeId = b3CreateHullShape( data->bodyId, &shapeDef, peel ? data->peelCylinder : data->cylinder ); + break; + } + + data->runGhostCount = 0; + data->runSteps = 0; +} + +GhostCollisionsData CreateGhostCollisions( b3WorldId worldId, GhostSettings settings ) +{ + GhostCollisionsData data = { 0 }; + data.settings = settings; + data.cylinder = b3CreateCylinder( 0.4f, 0.4f, -0.2f, 16 ); + data.peelCylinder = b3CreateCylinder( 2.0f, 1.0f, -1.0f, 16 ); + data.groundId = b3_nullBodyId; + data.bodyId = b3_nullBodyId; + data.shapeId = b3_nullShapeId; + + CreateGhostGround( worldId, &data ); + LaunchGhostBody( worldId, &data ); + return data; +} + +static float SegmentAngle( const GhostCollisionsData* data, int k ) +{ + b3Vec2 a = data->profile[k]; + b3Vec2 b = data->profile[k + 1]; + return atan2f( b.x - a.x, a.y - b.y ); +} + +// The normal points from the ground to the body. A hull face can be the reference face, so any +// normal within the span of face normals under the body is valid. +static bool IsGhost( const GhostCollisionsData* data, b3Vec3 normal, float xMin, float xMax ) +{ + float tolerance = 5.0f * B3_PI / 180.0f; + if ( b3AbsFloat( normal.z ) > sinf( tolerance ) ) + { + return true; + } + + float lower = B3_PI; + float upper = -B3_PI; + int segmentCount = data->profileCount - 1; + for ( int k = 0; k < segmentCount; ++k ) + { + float x0 = b3MinFloat( data->profile[k].x, data->profile[k + 1].x ); + float x1 = b3MaxFloat( data->profile[k].x, data->profile[k + 1].x ); + if ( x1 < xMin || xMax < x0 ) + { + continue; + } + + float angle = SegmentAngle( data, k ); + lower = b3MinFloat( lower, angle ); + upper = b3MaxFloat( upper, angle ); + } + + float angle = atan2f( normal.y, normal.x ); + return angle < lower - tolerance || upper + tolerance < angle; +} + +int UpdateGhostCollisions( b3WorldId worldId, GhostCollisionsData* data, GhostReport* reports, int capacity ) +{ + b3Pos center = b3Body_GetWorldCenter( data->bodyId ); + bool done = center.x > GHOST_HALF_LENGTH - 1.0f || center.y < -3.0f; + if ( data->settings.scene == e_ghostInternalEdges ) + { + // Run out onto the ground plane, or give up on a body knocked off the side + done = center.x > 100.0f || center.y < -3.0f || data->runSteps > 900; + } + + if ( done ) + { + data->runCount += 1; + LaunchGhostBody( worldId, data ); + center = b3Body_GetWorldCenter( data->bodyId ); + } + + data->runSteps += 1; + + b3AABB bounds = b3Shape_GetAABB( data->shapeId ); + float xMin = (float)bounds.lowerBound.x - 0.1f; + float xMax = (float)bounds.upperBound.x + 0.1f; + + int ghostCount = 0; + b3ContactData contacts[8]; + int contactCount = b3Body_GetContactData( data->bodyId, contacts, 8 ); + for ( int i = 0; i < contactCount; ++i ) + { + bool bodyIsA = B3_ID_EQUALS( contacts[i].shapeIdA, data->shapeId ); + b3ShapeId otherId = bodyIsA ? contacts[i].shapeIdB : contacts[i].shapeIdA; + if ( b3Shape_GetType( otherId ) != b3_meshShape ) + { + continue; + } + + for ( int j = 0; j < contacts[i].manifoldCount; ++j ) + { + const b3Manifold* manifold = contacts[i].manifolds + j; + + // Speculative points that never pushed are harmless + float impulse = 0.0f; + for ( int k = 0; k < manifold->pointCount; ++k ) + { + impulse += manifold->points[k].totalNormalImpulse; + } + + if ( manifold->pointCount == 0 || impulse <= 0.0f ) + { + continue; + } + + b3Vec3 normal = bodyIsA ? b3Neg( manifold->normal ) : manifold->normal; + if ( IsGhost( data, normal, xMin, xMax ) == false ) + { + continue; + } + + if ( ghostCount < capacity ) + { + b3Vec3 anchor = bodyIsA ? manifold->points[0].anchorA : manifold->points[0].anchorB; + reports[ghostCount] = (GhostReport){ + .point = b3OffsetPos( center, anchor ), + .normal = normal, + .separation = manifold->points[0].separation, + .impulse = impulse, + }; + } + + ghostCount += 1; + } + } + + data->runGhostCount += ghostCount; + data->ghostCount += ghostCount; + return ghostCount; +} + +void DestroyGhostCollisions( GhostCollisionsData* data ) +{ + DestroyGhostMeshes( data ); + b3DestroyHull( data->cylinder ); + b3DestroyHull( data->peelCylinder ); + data->cylinder = NULL; + data->peelCylinder = NULL; +} diff --git a/shared/ghost_collisions.h b/shared/ghost_collisions.h new file mode 100644 index 0000000..b988c2c --- /dev/null +++ b/shared/ghost_collisions.h @@ -0,0 +1,122 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT +#pragma once + +#include "box3d/collision.h" +#include "box3d/id.h" +#include "box3d/math_functions.h" + +#include + +#ifdef __cplusplus +extern "C" +{ +#endif + +// Ghost collision scenes, shared by the Mesh / Ghost Collisions sample and the headless ghost scan. +// Each ground is a profile in the xy-plane extruded along z and finely tessellated, so a sliding +// body crosses many internal edges. A manifold that pushes on the body is a ghost when its normal +// points outside the span of face normals under the body. Those are the ones that block or launch +// a body. + +typedef enum GhostScene +{ + e_ghostFlat, + e_ghostConcaveRamp, + e_ghostCliff, + e_ghostCrest, + e_ghostBumps, + e_ghostTJunction, + e_ghostShapeSeam, + e_ghostInternalEdges, + e_ghostSceneCount +} GhostScene; + +typedef enum GhostBody +{ + e_ghostBox, + e_ghostCapsule, + e_ghostUprightCapsule, + e_ghostSphere, + e_ghostCylinder, + e_ghostBodyCount +} GhostBody; + +// Mesh variants of the PEEL InternalEdges presets +#define GHOST_PEEL_PRESET_COUNT 4 + +// Bumps has the longest profile +#define GHOST_MAX_PROFILE_POINTS 96 + +extern const char* g_ghostSceneNames[e_ghostSceneCount]; +extern const char* g_ghostBodyNames[e_ghostBodyCount]; +extern const char* g_ghostPeelPresetNames[GHOST_PEEL_PRESET_COUNT]; + +typedef struct GhostSettings +{ + GhostScene scene; + GhostBody body; + int peelPreset; + bool mirror; + float cellSize; + float speed; + + // Degrees + float yaw; + + float friction; + bool fixedRotation; + bool speculative; +} GhostSettings; + +GhostSettings GetDefaultGhostSettings( void ); + +typedef struct GhostReport +{ + b3Pos point; + b3Vec3 normal; + float separation; + float impulse; +} GhostReport; + +typedef struct GhostCollisionsData +{ + GhostSettings settings; + b3Vec2 profile[GHOST_MAX_PROFILE_POINTS]; + int profileCount; + b3MeshData* meshes[2]; + int meshCount; + b3HullData* cylinder; + b3HullData* peelCylinder; + b3BodyId groundId; + b3BodyId bodyId; + b3ShapeId shapeId; + + // Steps in the current run + int runSteps; + + // Completed runs, each ending when the body reaches the end or falls off + int runCount; + + int runGhostCount; + int ghostCount; +} GhostCollisionsData; + +// Creates the ground and launches the body +GhostCollisionsData CreateGhostCollisions( b3WorldId worldId, GhostSettings settings ); + +// Rebuild the ground after a scene, preset, mirror or cell size change. Relaunch after this. +void CreateGhostGround( b3WorldId worldId, GhostCollisionsData* data ); + +// Launch a new body from the start, after a body, speed, yaw, friction or rotation change +void LaunchGhostBody( b3WorldId worldId, GhostCollisionsData* data ); + +// Call after each world step. Relaunches a body that finished its run, then counts the ghost +// manifolds on the body. Returns the ghost count for this step and writes up to capacity reports. +int UpdateGhostCollisions( b3WorldId worldId, GhostCollisionsData* data, GhostReport* reports, int capacity ); + +void DestroyGhostCollisions( GhostCollisionsData* data ); + +#ifdef __cplusplus +} +#endif diff --git a/src/contact.h b/src/contact.h index c4388b3..cdb4c4c 100644 --- a/src/contact.h +++ b/src/contact.h @@ -10,6 +10,7 @@ #include "box3d/types.h" #define B3_FORCE_GHOST_COLLISIONS 0 +#define B3_VALIDATE_NEIGHBOR_NORMALS 1 typedef struct b3Shape b3Shape; typedef struct b3World b3World; diff --git a/src/manifold.h b/src/manifold.h index 577c5d0..43d807b 100644 --- a/src/manifold.h +++ b/src/manifold.h @@ -51,6 +51,9 @@ b3AxisQuery b3ComputeSeparatingAxisAtWidth( const b3HullData* hullA, const b3Hul void b3CollideHullsAtWidth( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA, b3SATCache* cache, int simdWidth ); +void b3BuildTriangleAndCapsuleFaceContact( b3LocalManifold* manifold, const b3Vec3* triangle, b3Plane plane, + const b3Capsule* capsule ); + b3AxisQuery b3ComputeSeparatingAxisW4( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, bool earlyReturn ); b3AxisQuery b3ComputeSeparatingAxisW8( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, bool earlyReturn ); diff --git a/src/mesh_contact.c b/src/mesh_contact.c index 0bcbc5b..b44e851 100644 --- a/src/mesh_contact.c +++ b/src/mesh_contact.c @@ -41,7 +41,8 @@ typedef struct b3TriangleQueryContext int count; } b3TriangleQueryContext; -static bool b3CollectTriangleIndicesCallback( b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleIndex, void* context ) +static bool b3CollectTriangleIndicesCallback( b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleIndex, + void* context ) { B3_UNUSED( a, b, c ); b3TriangleQueryContext* triangleContext = context; @@ -59,7 +60,8 @@ static bool b3CollectTriangleIndicesCallback( b3Vec3 a, b3Vec3 b, b3Vec3 c, int return true; } -static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3WorldTransform xfA, const b3AABB* bounds, b3Arena arena ) +static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3WorldTransform xfA, + const b3AABB* bounds, b3Arena arena ) { B3_ASSERT( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape ); @@ -73,7 +75,8 @@ static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3WorldTr for ( int i = 0; i < contact->meshContact.triangleCache.count; ++i ) { B3_ASSERT( 0 <= contact->meshContact.triangleCache.data[i].triangleIndex && - contact->meshContact.triangleCache.data[i].triangleIndex < shapeA->mesh.data->triangleCount ); + contact->meshContact.triangleCache.data[i].triangleIndex < + shapeA->mesh.data->triangleCount ); } } @@ -97,7 +100,8 @@ static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3WorldTr // Bounds are in world space. Convert to the local mesh frame. The broadphase bounds are float, // so the demoted mesh transform is the matching float world frame (exact in float mode). b3Transform meshTransform = b3ToRelativeTransform( xfA, b3Pos_zero ); - b3AABB localBounds = b3AABB_Transform( b3InvertTransform( meshTransform ), meshContact->queryBounds ); + b3AABB localBounds = + b3AABB_Transform( b3InvertTransform( meshTransform ), meshContact->queryBounds ); if ( shapeA->type == b3_meshShape ) { b3QueryMesh( &shapeA->mesh, localBounds, b3CollectTriangleIndicesCallback, &queryContext ); @@ -105,7 +109,8 @@ static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3WorldTr else { B3_ASSERT( shapeA->type == b3_heightShape ); - b3QueryHeightField( shapeA->heightField, localBounds, b3CollectTriangleIndicesCallback, &queryContext ); + b3QueryHeightField( shapeA->heightField, localBounds, b3CollectTriangleIndicesCallback, + &queryContext ); } int* triangleIndices = queryContext.indices; @@ -123,13 +128,15 @@ static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3WorldTr contactCache[index1] = (b3ContactCache){ 0 }; while ( index2 < contact->meshContact.triangleCache.count && - contact->meshContact.triangleCache.data[index2].triangleIndex < triangleIndices[index1] ) + contact->meshContact.triangleCache.data[index2].triangleIndex < + triangleIndices[index1] ) { index2 += 1; } if ( index2 < contact->meshContact.triangleCache.count && - contact->meshContact.triangleCache.data[index2].triangleIndex == triangleIndices[index1] ) + contact->meshContact.triangleCache.data[index2].triangleIndex == + triangleIndices[index1] ) { contactCache[index1] = contact->meshContact.triangleCache.data[index2].cache; } @@ -139,12 +146,14 @@ static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3WorldTr b3Array_Resize( contact->meshContact.triangleCache, triangleCount ); for ( int i = 0; i < triangleCount; ++i ) { - contact->meshContact.triangleCache.data[i] = (b3TriangleCache){ triangleIndices[i], contactCache[i] }; + contact->meshContact.triangleCache.data[i] = + (b3TriangleCache){ triangleIndices[i], contactCache[i] }; if ( shapeA->type == b3_meshShape ) { B3_ASSERT( 0 <= contact->meshContact.triangleCache.data[i].triangleIndex && - contact->meshContact.triangleCache.data[i].triangleIndex < shapeA->mesh.data->triangleCount ); + contact->meshContact.triangleCache.data[i].triangleIndex < + shapeA->mesh.data->triangleCount ); } } } @@ -270,7 +279,8 @@ static inline bool b3AddVertex( b3FoundVertices* vertices, int vertex ) #if B3_MAX_MANIFOLD_POINTS == 4 // Returns true if (score, separation) should replace (bestScore, bestSeparation). -static inline bool b3IsBetterCullCandidate( float score, float separation, float bestScore, float bestSeparation, float scoreTol, +static inline bool b3IsBetterCullCandidate( float score, float separation, float bestScore, + float bestSeparation, float scoreTol, float separationTol ) { if ( score > bestScore + scoreTol ) @@ -315,7 +325,8 @@ static int b3CullPoints( b3Point2D* points, int count ) // Separation sum heuristic float separation = points[i].separation + points[j].separation; - if ( b3IsBetterCullCandidate( score, separation, bestScore, bestSeparation, tolSqr, separationTol ) ) + if ( b3IsBetterCullCandidate( score, separation, bestScore, bestSeparation, tolSqr, + separationTol ) ) { bestIndex1 = i; bestIndex2 = j; @@ -379,7 +390,8 @@ static int b3CullPoints( b3Point2D* points, int count ) float signedArea = b3Cross2( ba, b3Sub2( p, a ) ); float score = b3AbsFloat( signedArea ); - if ( b3IsBetterCullCandidate( score, points[i].separation, bestScore, bestSeparation, tolSqr, separationTol ) ) + if ( b3IsBetterCullCandidate( score, points[i].separation, bestScore, bestSeparation, + tolSqr, separationTol ) ) { bestSignedArea = signedArea; bestScore = score; @@ -438,7 +450,8 @@ static int b3CullPoints( b3Point2D* points, int count ) float score = b3MaxFloat( u1, b3MaxFloat( u2, u3 ) ); // Use the area tolerance for collinear points and hysteresis - if ( b3IsBetterCullCandidate( score, points[i].separation, bestScore, bestSeparation, tolSqr, separationTol ) ) + if ( b3IsBetterCullCandidate( score, points[i].separation, bestScore, bestSeparation, + tolSqr, separationTol ) ) { bestScore = score; bestSeparation = points[i].separation; @@ -520,8 +533,112 @@ typedef struct b3Cluster int pointCount; } b3Cluster; -bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact, const b3Shape* shapeA, - const uint16_t* materialMap, b3WorldTransform xfA, const b3Shape* shapeB, b3WorldTransform xfB, +typedef enum b3NeighborResult +{ + // Admit for flat edges + b3_neighborAdmit, + // Unknown for isolated edges + b3_neighborUnknown, + // Skip for convex edges + b3_neighborSkip, + // Snap for concave edges + b3_neighborSnap, +} b3NeighborResult; + +#define B3_NEIGHBOR_TOLERANCE 0.0175f + +static bool b3GetNeighborDirection( const b3Shape* shape, int triangleIndex, const b3Vec3* vertices, + b3Matrix3 meshToLocal, int edgeIndex, b3Vec3* direction ) +{ + b3Vec3 neighborNormal; + + bool haveNeighbor = b3GetNeighborNormal( shape, triangleIndex, edgeIndex, &neighborNormal ); + if ( haveNeighbor == false ) + { + return false; + } + + neighborNormal = b3MulMV( meshToLocal, neighborNormal ); + b3Vec3 edge = b3Normalize( b3Sub( vertices[( edgeIndex + 1 ) % 3], vertices[edgeIndex] ) ); + *direction = b3Cross( edge, neighborNormal ); + return true; +} + +// Voronoi test of an edge or vertex normal against the neighbor normals. +static b3NeighborResult b3TestNeighbors( const b3Shape* shape, int triangleIndex, + const b3Vec3* vertices, b3Matrix3 meshToLocal, + b3TriangleFeature feature, b3Vec3 normal ) +{ + int edges[2] = { 0, 0 }; + int edgeCount = 1; + + switch ( feature ) + { + case b3_featureEdge1: + edges[0] = 0; + break; + + case b3_featureEdge2: + edges[0] = 1; + break; + + case b3_featureEdge3: + edges[0] = 2; + break; + + case b3_featureVertex1: + edges[0] = 0; + edges[1] = 2; + edgeCount = 2; + break; + + case b3_featureVertex2: + edges[0] = 0; + edges[1] = 1; + edgeCount = 2; + break; + + case b3_featureVertex3: + edges[0] = 1; + edges[1] = 2; + edgeCount = 2; + break; + + default: + return b3_neighborUnknown; + } + + b3Vec3 triangleNormal = b3MakeNormalFromPoints( vertices[0], vertices[1], vertices[2] ); + float tolerance = B3_NEIGHBOR_TOLERANCE; + bool admit = true; + bool convex = false; + + for ( int i = 0; i < edgeCount; ++i ) + { + b3Vec3 direction; + bool haveNeighbor = b3GetNeighborDirection( shape, triangleIndex, vertices, meshToLocal, + edges[i], &direction ); + if ( haveNeighbor == false ) + { + // Isolated edge + return b3_neighborUnknown; + } + + admit = admit && b3Dot( normal, direction ) <= tolerance; + convex = convex || b3Dot( triangleNormal, direction ) < -tolerance; + } + + if ( admit ) + { + return b3_neighborAdmit; + } + + return convex ? b3_neighborSkip : b3_neighborSnap; +} + +bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact, + const b3Shape* shapeA, const uint16_t* materialMap, + b3WorldTransform xfA, const b3Shape* shapeB, b3WorldTransform xfB, bool isFast, b3Arena arena ) { B3_ASSERT( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape ); @@ -537,11 +654,14 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact b3MeshContact* meshContact = &contact->meshContact; int triangleCount = meshContact->triangleCache.count; - b3LocalManifold** acceptedManifolds = b3Bump( &arena, triangleCount * sizeof( b3LocalManifold* ) ); + b3LocalManifold** acceptedManifolds = + b3Bump( &arena, triangleCount * sizeof( b3LocalManifold* ) ); int acceptedManifoldCount = 0; - b3LocalManifold** tentativeManifolds = b3Bump( &arena, triangleCount * sizeof( b3LocalManifold* ) ); + b3LocalManifold** tentativeManifolds = + b3Bump( &arena, triangleCount * sizeof( b3LocalManifold* ) ); int tentativeManifoldCount = 0; - b3TentativeTriangle* tentativeTriangles = b3Bump( &arena, triangleCount * sizeof( b3TentativeTriangle ) ); + b3TentativeTriangle* tentativeTriangles = + b3Bump( &arena, triangleCount * sizeof( b3TentativeTriangle ) ); int tentativeTriangleCount = 0; // This transform converts from mesh frame into the shapeB frame @@ -561,7 +681,8 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact const b3HullData* hullB = shapeB->type == b3_hullShape ? shapeB->hull : NULL; float hullSpeculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f; - b3Vec3 hullMargin = { hullSpeculativeDistance, hullSpeculativeDistance, hullSpeculativeDistance }; + b3Vec3 hullMargin = { hullSpeculativeDistance, hullSpeculativeDistance, + hullSpeculativeDistance }; int* survivorIndices = b3Bump( &arena, triangleCount * sizeof( int ) ); b3Vec3* survivorVertices = b3Bump( &arena, 3 * triangleCount * sizeof( b3Vec3 ) ); @@ -615,13 +736,18 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact { b3MeshTriangle meshTriangle = meshTriangles[triangleIndex]; lanes[0][lane] = b3Mul( meshScale, meshVertices[meshTriangle.index1] ); - lanes[1][lane] = b3Mul( meshScale, meshVertices[meshFlipped ? meshTriangle.index3 : meshTriangle.index2] ); - lanes[2][lane] = b3Mul( meshScale, meshVertices[meshFlipped ? meshTriangle.index2 : meshTriangle.index3] ); + lanes[1][lane] = + b3Mul( meshScale, + meshVertices[meshFlipped ? meshTriangle.index3 : meshTriangle.index2] ); + lanes[2][lane] = + b3Mul( meshScale, + meshVertices[meshFlipped ? meshTriangle.index2 : meshTriangle.index3] ); } else { B3_ASSERT( shapeA->type == b3_heightShape ); - b3Triangle triangle = b3GetHeightFieldTriangle( shapeA->heightField, triangleIndex ); + b3Triangle triangle = + b3GetHeightFieldTriangle( shapeA->heightField, triangleIndex ); lanes[0][lane] = triangle.vertices[0]; lanes[1][lane] = triangle.vertices[1]; lanes[2][lane] = triangle.vertices[2]; @@ -636,9 +762,15 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact b3FloatW4 ay = b3SetW4( lanes[k][0].y, lanes[k][1].y, lanes[k][2].y, lanes[k][3].y ); b3FloatW4 az = b3SetW4( lanes[k][0].z, lanes[k][1].z, lanes[k][2].z, lanes[k][3].z ); - vx[k] = b3AddW4( b3AddW4( b3AddW4( b3MulW4( cxx, ax ), b3MulW4( cyx, ay ) ), b3MulW4( czx, az ) ), px ); - vy[k] = b3AddW4( b3AddW4( b3AddW4( b3MulW4( cxy, ax ), b3MulW4( cyy, ay ) ), b3MulW4( czy, az ) ), py ); - vz[k] = b3AddW4( b3AddW4( b3AddW4( b3MulW4( cxz, ax ), b3MulW4( cyz, ay ) ), b3MulW4( czz, az ) ), pz ); + vx[k] = b3AddW4( + b3AddW4( b3AddW4( b3MulW4( cxx, ax ), b3MulW4( cyx, ay ) ), b3MulW4( czx, az ) ), + px ); + vy[k] = b3AddW4( + b3AddW4( b3AddW4( b3MulW4( cxy, ax ), b3MulW4( cyy, ay ) ), b3MulW4( czy, az ) ), + py ); + vz[k] = b3AddW4( + b3AddW4( b3AddW4( b3MulW4( cxz, ax ), b3MulW4( cyz, ay ) ), b3MulW4( czz, az ) ), + pz ); } uint32_t keepMask = ( 1u << laneCount ) - 1u; @@ -651,9 +783,12 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact b3FloatW4 upperY = b3AddW4( b3MaxW4( vy[0], b3MaxW4( vy[1], vy[2] ) ), marginW ); b3FloatW4 upperZ = b3AddW4( b3MaxW4( vz[0], b3MaxW4( vz[1], vz[2] ) ), marginW ); - b3FloatW4 separated = b3OrW4( b3LessThanW4( upperX, hullLowerX ), b3GreaterThanW4( lowerX, hullUpperX ) ); - separated = b3OrW4( separated, b3OrW4( b3LessThanW4( upperY, hullLowerY ), b3GreaterThanW4( lowerY, hullUpperY ) ) ); - separated = b3OrW4( separated, b3OrW4( b3LessThanW4( upperZ, hullLowerZ ), b3GreaterThanW4( lowerZ, hullUpperZ ) ) ); + b3FloatW4 separated = + b3OrW4( b3LessThanW4( upperX, hullLowerX ), b3GreaterThanW4( lowerX, hullUpperX ) ); + separated = b3OrW4( separated, b3OrW4( b3LessThanW4( upperY, hullLowerY ), + b3GreaterThanW4( lowerY, hullUpperY ) ) ); + separated = b3OrW4( separated, b3OrW4( b3LessThanW4( upperZ, hullLowerZ ), + b3GreaterThanW4( lowerZ, hullUpperZ ) ) ); uint32_t cullMask = keepMask & (uint32_t)b3MoveMaskW4( separated ); keepMask &= ~cullMask; @@ -696,13 +831,19 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact // Make room for clip points. Additional capacity for a heavy clip. int pointBufferCapacity = B3_MAX_POINTS_PER_TRIANGLE * survivorCount + 2 * B3_MAX_CLIP_POINTS; - b3LocalManifoldPoint* pointBuffer = b3Bump( &arena, pointBufferCapacity * sizeof( b3LocalManifoldPoint ) ); + b3LocalManifoldPoint* pointBuffer = + b3Bump( &arena, pointBufferCapacity * sizeof( b3LocalManifoldPoint ) ); int totalPointCount = 0; b3LocalManifold* manifoldBuffer = b3Bump( &arena, survivorCount * sizeof( b3LocalManifold ) ); int manifoldCount = 0; - for ( int survivorIndex = 0; survivorIndex < survivorCount && totalPointCount + 3 < pointBufferCapacity; ++survivorIndex ) + bool validateNeighbors = B3_VALIDATE_NEIGHBOR_NORMALS && B3_FORCE_GHOST_COLLISIONS == 0 && + ( shapeB->type == b3_sphereShape || shapeB->type == b3_capsuleShape ); + + for ( int survivorIndex = 0; + survivorIndex < survivorCount && totalPointCount + 3 < pointBufferCapacity; + ++survivorIndex ) { int index = survivorIndices[survivorIndex]; int triangleIndex = triangleCaches[index].triangleIndex; @@ -730,19 +871,21 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact switch ( shapeB->type ) { case b3_capsuleShape: - b3CollideTriangleAndCapsule( manifold, pointCapacity, vertices, &shapeB->capsule, &cache->simplexCache ); + b3CollideTriangleAndCapsule( manifold, pointCapacity, vertices, &shapeB->capsule, + &cache->simplexCache ); break; case b3_hullShape: - // Cached edge contact is dangerous at high speed because the hull can rotate around the edge and tunnel - // through the triangle. + // Cached edge contact is dangerous at high speed because the hull can rotate around + // the edge and tunnel through the triangle. if ( isFast && cache->satCache.type == b3_edgePairAxis ) { cache->satCache = (b3SATCache){ 0 }; } - b3CollideTriangleAndHull( manifold, pointCapacity, vertices[0], vertices[1], vertices[2], triangle.flags, hullB, - &cache->satCache, enableSpeculative ); + b3CollideTriangleAndHull( manifold, pointCapacity, vertices[0], vertices[1], + vertices[2], triangle.flags, hullB, &cache->satCache, + enableSpeculative ); context->satCallCount += 1; context->satCacheHitCount += cache->satCache.hit; break; @@ -756,6 +899,43 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact return false; } + if ( validateNeighbors && manifold->pointCount > 0 && + manifold->feature != b3_featureTriangleFace ) + { + b3NeighborResult result = + b3TestNeighbors( shapeA, triangleIndex, vertices, relativeMatrix, manifold->feature, + manifold->normal ); + + if ( result == b3_neighborSkip ) + { + float minSeparation = manifold->points[0].separation; + for ( int i = 1; i < manifold->pointCount; ++i ) + { + minSeparation = b3MinFloat( minSeparation, manifold->points[i].separation ); + } + + if ( minSeparation < -2.0f * linearSlop ) + { + // In deep overlap the owning neighbor may apply backside culling. Experiments + // show the that in this case the neighbor should snap the normal. + result = shapeB->type == b3_capsuleShape ? b3_neighborSnap : b3_neighborUnknown; + } + } + + if ( result == b3_neighborSkip || result == b3_neighborSnap ) + { + // Skip leaves the normal to the neighbor. A sphere off the face cannot snap. + manifold->pointCount = 0; + } + + if ( result == b3_neighborSnap && shapeB->type == b3_capsuleShape ) + { + // Snap clips the segment to the triangle for a face contact. May clip all points. + b3Plane plane = b3MakePlaneFromPoints( vertices[0], vertices[1], vertices[2] ); + b3BuildTriangleAndCapsuleFaceContact( manifold, vertices, plane, &shapeB->capsule ); + } + } + int manifoldPointCount = manifold->pointCount; if ( manifoldPointCount > 0 ) @@ -765,7 +945,8 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact manifoldCount += 1; totalPointCount += manifoldPointCount; manifold->triangleIndex = triangleIndex; - manifold->triangleNormal = b3MakeNormalFromPoints( vertices[0], vertices[1], vertices[2] ); + manifold->triangleNormal = + b3MakeNormalFromPoints( vertices[0], vertices[1], vertices[2] ); manifold->i1 = triangle.i1; manifold->i2 = triangle.i2; manifold->i3 = triangle.i3; @@ -796,7 +977,8 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact } else { - b3TentativeTriangle tentativeTriangle = { .squaredDistance = manifold->squaredDistance, + b3TentativeTriangle tentativeTriangle = { .squaredDistance = + manifold->squaredDistance, .index = tentativeManifoldCount }; tentativeTriangles[tentativeTriangleCount++] = tentativeTriangle; tentativeManifolds[tentativeManifoldCount++] = manifold; @@ -805,8 +987,8 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact } else { - b3TentativeTriangle tentativeTriangle = { .squaredDistance = manifold->squaredDistance, - .index = tentativeManifoldCount }; + b3TentativeTriangle tentativeTriangle = { + .squaredDistance = manifold->squaredDistance, .index = tentativeManifoldCount }; tentativeTriangles[tentativeTriangleCount++] = tentativeTriangle; tentativeManifolds[tentativeManifoldCount++] = manifold; } @@ -835,21 +1017,23 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact { // Sort triangles so the closest triangles are processed first { -#define LESS( i, j ) tentativeTriangles[(int)i].squaredDistance < tentativeTriangles[(int)j].squaredDistance -#define SWAP( i, j ) \ - do \ - { \ - b3TentativeTriangle tmp = tentativeTriangles[(int)i]; \ - tentativeTriangles[(int)i] = tentativeTriangles[(int)j]; \ - tentativeTriangles[(int)j] = tmp; \ - } \ +#define LESS( i, j ) \ + tentativeTriangles[(int)i].squaredDistance < tentativeTriangles[(int)j].squaredDistance +#define SWAP( i, j ) \ + do \ + { \ + b3TentativeTriangle tmp = tentativeTriangles[(int)i]; \ + tentativeTriangles[(int)i] = tentativeTriangles[(int)j]; \ + tentativeTriangles[(int)j] = tmp; \ + } \ while ( 0 ) QSORT( tentativeTriangleCount, LESS, SWAP ); #undef LESS #undef SWAP } - // Add tentative manifolds in sorted order. Avoid adding manifolds that generate ghost collisions. + // Add tentative manifolds in sorted order. Avoid adding manifolds that generate ghost + // collisions. for ( int i = 0; i < tentativeTriangleCount; ++i ) { b3LocalManifold* m = tentativeManifolds[tentativeTriangles[i].index]; @@ -1048,7 +1232,8 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact } // Setup clusters - b3LocalManifoldPoint* clusterPoints = b3Bump( &arena, clusterPointCount * sizeof( b3LocalManifoldPoint ) ); + b3LocalManifoldPoint* clusterPoints = + b3Bump( &arena, clusterPointCount * sizeof( b3LocalManifoldPoint ) ); int pointOffset = 0; for ( int i = 0; i < clusterCount; ++i ) @@ -1187,7 +1372,8 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact { b3ManifoldPoint* oldPt = matchedManifold->points + k; - if ( target->featureId == oldPt->featureId && target->triangleIndex == oldPt->triangleIndex ) + if ( target->featureId == oldPt->featureId && + target->triangleIndex == oldPt->triangleIndex ) { target->normalImpulse = oldPt->normalImpulse; target->persisted = true; @@ -1246,10 +1432,12 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact materialIndex = b3ClampInt( materialIndex, 0, shapeA->materialCount - 1 ); b3SurfaceMaterial material = materialsA[materialIndex]; - friction += world->frictionCallback( material.friction, material.userMaterialId, materialB->friction, - materialB->userMaterialId ); - restitution += world->restitutionCallback( material.restitution, material.userMaterialId, materialB->restitution, - materialB->userMaterialId ); + friction += + world->frictionCallback( material.friction, material.userMaterialId, + materialB->friction, materialB->userMaterialId ); + restitution += + world->restitutionCallback( material.restitution, material.userMaterialId, + materialB->restitution, materialB->userMaterialId ); tangentVelocityA = b3Add( tangentVelocityA, material.tangentVelocity ); @@ -1271,10 +1459,12 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact else { // Keep these updated in case the values on the shapes are modified - contact->friction = world->frictionCallback( materialsA[0].friction, materialsA[0].userMaterialId, materialB->friction, - materialB->userMaterialId ); - contact->restitution = world->restitutionCallback( materialsA[0].restitution, materialsA[0].userMaterialId, - materialB->restitution, materialB->userMaterialId ); + contact->friction = + world->frictionCallback( materialsA[0].friction, materialsA[0].userMaterialId, + materialB->friction, materialB->userMaterialId ); + contact->restitution = + world->restitutionCallback( materialsA[0].restitution, materialsA[0].userMaterialId, + materialB->restitution, materialB->userMaterialId ); tangentVelocityA = materialsA[0].tangentVelocity; } diff --git a/src/shape.h b/src/shape.h index 3415fbb..048f04c 100644 --- a/src/shape.h +++ b/src/shape.h @@ -187,6 +187,7 @@ static inline b3Triangle b3GetMeshTriangle( const b3Mesh* mesh, int triangleInde bool b3GetMeshNeighborNormal( const b3Mesh* mesh, int triangleIndex, int edgeIndex, b3Vec3* normal ); +// Returns false if edge is isolated. static inline bool b3GetNeighborNormal( const b3Shape* shape, int triangleIndex, int edgeIndex, b3Vec3* normal ) { if ( shape->type == b3_meshShape ) diff --git a/src/triangle_manifold.c b/src/triangle_manifold.c index eb3a25e..ee8414e 100644 --- a/src/triangle_manifold.c +++ b/src/triangle_manifold.c @@ -260,8 +260,8 @@ static b3SeparatingAxis b3QueryTriangleAndCapsuleEdges( const b3Vec3* vertices, }; } -static void b3BuildTriangleAndCapsuleFaceContact( b3LocalManifold* manifold, const b3Vec3* triangle, b3Plane plane, - const b3Capsule* capsule ) +void b3BuildTriangleAndCapsuleFaceContact( b3LocalManifold* manifold, const b3Vec3* triangle, b3Plane plane, + const b3Capsule* capsule ) { B3_ASSERT( manifold->pointCount == 0 ); diff --git a/test/test_determinism.c b/test/test_determinism.c index f2bd7e6..93f70cf 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -19,10 +19,10 @@ // Double precision accumulates body positions in double, so the settle/sleep step and the // state hash differ from the float build. Both modes are internally deterministic. #if defined( BOX3D_DOUBLE_PRECISION ) -#define RAGDOLL_SLEEP_STEP 263 -#define RAGDOLL_HASH 0xA5423F98 -#define WAVE_PILE_SLEEP_STEP 272 -#define WAVE_PILE_HASH 0xC8E5F3D6 +#define RAGDOLL_SLEEP_STEP 278 +#define RAGDOLL_HASH 0x8851ED6D +#define WAVE_PILE_SLEEP_STEP 274 +#define WAVE_PILE_HASH 0xAEDE433C #define QUERY_SPAWN_SLEEP_STEP 242 #define QUERY_SPAWN_HASH 0x489585E6 #define QUERY_SPAWN_HIT_COUNT 59 @@ -30,10 +30,10 @@ #define MESH_DROP_SLEEP_STEP 218 #define MESH_DROP_HASH 0xD378CF04 #else -#define RAGDOLL_SLEEP_STEP 291 -#define RAGDOLL_HASH 0xFF4C605B -#define WAVE_PILE_SLEEP_STEP 272 -#define WAVE_PILE_HASH 0x266F83B0 +#define RAGDOLL_SLEEP_STEP 346 +#define RAGDOLL_HASH 0x27E7E059 +#define WAVE_PILE_SLEEP_STEP 297 +#define WAVE_PILE_HASH 0xC357A993 #define QUERY_SPAWN_SLEEP_STEP 242 #define QUERY_SPAWN_HASH 0xF3AD9BA6 #define QUERY_SPAWN_HIT_COUNT 59 diff --git a/test/test_mesh.c b/test/test_mesh.c index cc0a82a..ae3b0c7 100644 --- a/test/test_mesh.c +++ b/test/test_mesh.c @@ -9,6 +9,8 @@ #include "box3d/math_functions.h" #include +#include +#include #include // Two quads meeting at a concave crease along the shared edge 1-4. The crease exercises @@ -782,6 +784,340 @@ static int MeshWideHullSlide( void ) return 0; } +// A sphere balanced on a convex ridge is held up by edge contacts alone. Their normals lie between +// the two face normals, so neighbor validation must keep them. +static int MeshSphereOnRidge( void ) +{ + b3Vec3 vertices[] = { + { -2.0f, -0.7f, -1.0f }, { 0.0f, 0.0f, -1.0f }, { 2.0f, -0.7f, -1.0f }, + { -2.0f, -0.7f, 1.0f }, { 0.0f, 0.0f, 1.0f }, { 2.0f, -0.7f, 1.0f }, + }; + + int32_t indices[] = { + 0, 3, 1, 3, 4, 1, 1, 4, 2, 4, 5, 2, + }; + + b3MeshDef def = { 0 }; + def.vertices = vertices; + def.vertexCount = ARRAY_COUNT( vertices ); + def.indices = indices; + def.triangleCount = ARRAY_COUNT( indices ) / 3; + def.identifyEdges = true; + b3MeshData* mesh = b3CreateMesh( &def, NULL, 0 ); + ENSURE( mesh != NULL ); + + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateMeshShape( groundId, &shapeDef, mesh, b3Vec3_one ); + + float radius = 0.25f; + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Pos){ 0.0f, radius, 0.0f }; + b3BodyId sphereId = b3CreateBody( worldId, &bodyDef ); + b3Sphere sphere = { b3Vec3_zero, radius }; + b3CreateSphereShape( sphereId, &shapeDef, &sphere ); + + for ( int step = 0; step < 30; ++step ) + { + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + + b3ContactData contacts[4]; + int contactCount = b3Body_GetContactData( sphereId, contacts, ARRAY_COUNT( contacts ) ); + ENSURE( contactCount == 1 ); + ENSURE( contacts[0].manifoldCount >= 1 ); + + for ( int j = 0; j < contacts[0].manifoldCount; ++j ) + { + ENSURE( b3AbsFloat( contacts[0].manifolds[j].normal.y ) > 0.99f ); + } + } + + b3Pos position = b3Body_GetPosition( sphereId ); + ENSURE( position.y > 0.9f * radius ); + + b3DestroyWorld( worldId ); + b3DestroyMesh( mesh ); + return 0; +} + +// The normal points from the mesh to the body. Any normal in the span of the face normals under +// the body is real, anything else is a ghost. +static bool IsProfileGhost( const b3Vec2* profile, int count, b3Vec3 normal, float xMin, float xMax ) +{ + float tolerance = 5.0f * B3_PI / 180.0f; + if ( b3AbsFloat( normal.z ) > sinf( tolerance ) ) + { + return true; + } + + float lower = B3_PI; + float upper = -B3_PI; + for ( int k = 0; k + 1 < count; ++k ) + { + if ( profile[k + 1].x < xMin || xMax < profile[k].x ) + { + continue; + } + + b3Vec2 a = profile[k]; + b3Vec2 b = profile[k + 1]; + float angle = atan2f( b.x - a.x, a.y - b.y ); + lower = b3MinFloat( lower, angle ); + upper = b3MaxFloat( upper, angle ); + } + + float angle = atan2f( normal.y, normal.x ); + return angle < lower - tolerance || upper + tolerance < angle; +} + +// A capsule lying across the slide direction crosses a 5.7 degree crest at speed, like the crest +// scene in the ghost collision sample. The edges are flat, convex and concave, and a capsule edge +// contact on the wrong side of a fold kicks the capsule. +static int CapsuleOverCrest( bool mirror ) +{ + b3Vec2 profile[] = { { -20.0f, 0.0f }, { -10.0f, 0.0f }, { 0.0f, 1.0f }, { 10.0f, 0.0f }, { 20.0f, 0.0f } }; + int profileCount = ARRAY_COUNT( profile ); + + // Quarter meter cells with alternating diagonals, like cooked level geometry + float cellSize = 0.25f; + b3Vec2 points[256]; + int pointCount = 0; + points[pointCount++] = profile[0]; + for ( int k = 0; k + 1 < profileCount; ++k ) + { + b3Vec2 a = profile[k]; + b3Vec2 b = profile[k + 1]; + float length = sqrtf( ( b.x - a.x ) * ( b.x - a.x ) + ( b.y - a.y ) * ( b.y - a.y ) ); + int n = (int)ceilf( length / cellSize - 0.01f ); + for ( int s = 1; s <= n; ++s ) + { + ENSURE( pointCount < ARRAY_COUNT( points ) ); + float t = (float)s / (float)n; + points[pointCount++] = (b3Vec2){ a.x + t * ( b.x - a.x ), a.y + t * ( b.y - a.y ) }; + } + } + + // Mirror the data and flip it back with the mesh scale, so the world geometry is the same + float halfWidth = 3.0f; + int zCount = 24; + int stride = zCount + 1; + float sign = mirror ? -1.0f : 1.0f; + + int vertexCount = pointCount * stride; + b3Vec3* vertices = malloc( vertexCount * sizeof( b3Vec3 ) ); + for ( int i = 0; i < pointCount; ++i ) + { + for ( int j = 0; j <= zCount; ++j ) + { + float z = -halfWidth + 2.0f * halfWidth * (float)j / (float)zCount; + vertices[i * stride + j] = (b3Vec3){ sign * points[i].x, points[i].y, z }; + } + } + + int triangleCount = 2 * ( pointCount - 1 ) * zCount; + int32_t* indices = malloc( 3 * triangleCount * sizeof( int32_t ) ); + int indexCount = 0; + for ( int i = 0; i + 1 < pointCount; ++i ) + { + for ( int j = 0; j < zCount; ++j ) + { + int a = i * stride + j; + int b = a + 1; + int c = a + stride + 1; + int d = a + stride; + + int corners[6] = { a, b, c, c, d, a }; + if ( ( i + j ) & 1 ) + { + int alternate[6] = { a, b, d, b, c, d }; + memcpy( corners, alternate, sizeof( corners ) ); + } + + for ( int k = 0; k < 6; k += 3 ) + { + indices[indexCount++] = corners[k]; + indices[indexCount++] = mirror ? corners[k + 2] : corners[k + 1]; + indices[indexCount++] = mirror ? corners[k + 1] : corners[k + 2]; + } + } + } + + b3MeshDef def = { 0 }; + def.vertices = vertices; + def.vertexCount = vertexCount; + def.indices = indices; + def.triangleCount = triangleCount; + def.identifyEdges = true; + b3MeshData* mesh = b3CreateMesh( &def, NULL, 0 ); + free( vertices ); + free( indices ); + ENSURE( mesh != NULL ); + + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3Vec3 scale = mirror ? (b3Vec3){ -1.0f, 1.0f, 1.0f } : b3Vec3_one; + b3CreateMeshShape( groundId, &shapeDef, mesh, scale ); + + float radius = 0.2f; + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Pos){ -16.0f, radius + 0.01f, 0.0f }; + bodyDef.linearVelocity = (b3Vec3){ 6.0f, 0.0f, 0.0f }; + bodyDef.enableSleep = false; + b3BodyId capsuleId = b3CreateBody( worldId, &bodyDef ); + + b3Capsule capsule = { { 0.0f, 0.0f, -0.4f }, { 0.0f, 0.0f, 0.4f }, radius }; + shapeDef.baseMaterial.friction = 0.0f; + b3ShapeId capsuleShapeId = b3CreateCapsuleShape( capsuleId, &shapeDef, &capsule ); + + int ghostCount = 0; + for ( int step = 0; step < 600 && b3Body_GetPosition( capsuleId ).x < 15.0f; ++step ) + { + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + + b3AABB bounds = b3Shape_GetAABB( capsuleShapeId ); + float xMin = (float)bounds.lowerBound.x - 0.1f; + float xMax = (float)bounds.upperBound.x + 0.1f; + + b3ContactData contacts[4]; + int contactCount = b3Body_GetContactData( capsuleId, contacts, ARRAY_COUNT( contacts ) ); + for ( int i = 0; i < contactCount; ++i ) + { + bool capsuleIsA = B3_ID_EQUALS( contacts[i].shapeIdA, capsuleShapeId ); + for ( int j = 0; j < contacts[i].manifoldCount; ++j ) + { + const b3Manifold* manifold = contacts[i].manifolds + j; + + // Speculative points that never pushed are harmless + float impulse = 0.0f; + for ( int k = 0; k < manifold->pointCount; ++k ) + { + impulse += manifold->points[k].totalNormalImpulse; + } + + b3Vec3 normal = capsuleIsA ? b3Neg( manifold->normal ) : manifold->normal; + if ( impulse > 0.0f && IsProfileGhost( profile, profileCount, normal, xMin, xMax ) ) + { + ghostCount += 1; + } + } + } + } + + ENSURE( ghostCount == 0 ); + ENSURE( b3Body_GetPosition( capsuleId ).x >= 15.0f ); + + b3DestroyWorld( worldId ); + b3DestroyMesh( mesh ); + return 0; +} + +static int MeshCapsuleOverCrest( void ) +{ + ENSURE( CapsuleOverCrest( false ) == 0 ); + ENSURE( CapsuleOverCrest( true ) == 0 ); + return 0; +} + +// Fires a sphere or the end of a capsule at the apex along the given direction, without gravity, +// and returns the deepest separation from the apex. +static float ApexImpact( b3MeshData* mesh, b3Vec3 direction, bool useCapsule ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.gravity = b3Vec3_zero; + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateMeshShape( groundId, &shapeDef, mesh, b3Vec3_one ); + + float radius = 0.25f; + bodyDef.type = b3_dynamicBody; + bodyDef.position = b3ToPos( b3MulSV( radius + 0.1f, direction ) ); + bodyDef.linearVelocity = b3MulSV( -2.0f, direction ); + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + // The capsule points away from the apex, so its lower end is the closest point + b3Vec3 tip = b3MulSV( 0.5f, direction ); + if ( useCapsule ) + { + b3Capsule capsule = { b3Vec3_zero, tip, radius }; + b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + } + else + { + b3Sphere sphere = { b3Vec3_zero, radius }; + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + } + + float minSeparation = FLT_MAX; + for ( int step = 0; step < 60; ++step ) + { + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + + b3Pos p1 = b3Body_GetWorldPoint( bodyId, b3Vec3_zero ); + b3Pos p2 = b3Body_GetWorldPoint( bodyId, useCapsule ? tip : b3Vec3_zero ); + b3Vec3 a = b3ToVec3( p1 ); + b3Vec3 segment = b3Sub( b3ToVec3( p2 ), a ); + float lengthSquared = b3LengthSquared( segment ); + float t = lengthSquared > 0.0f ? b3ClampFloat( -b3Dot( a, segment ) / lengthSquared, 0.0f, 1.0f ) : 0.0f; + float distance = b3Length( b3MulAdd( a, t, segment ) ); + minSeparation = b3MinFloat( minSeparation, distance - radius ); + } + + b3DestroyWorld( worldId ); + return minSeparation; +} + +// A vertex manifold that fails a neighbor test is dropped. That relies on some triangle around a +// convex vertex admitting every normal in the vertex normal cone. On this irregular valence 6 apex +// each direction is inside the cone and exactly one of the six triangles admits it. +static int MeshApexContact( void ) +{ + b3Vec3 vertices[] = { + { 0.0f, 0.0f, 0.0f }, { 1.08f, -1.9f, -0.45f }, { -1.1f, -0.89f, -0.35f }, { -1.73f, -1.91f, -0.06f }, + { -0.8f, -1.45f, 0.27f }, { 0.35f, -1.63f, 0.35f }, { 0.79f, -1.96f, 0.07f }, + }; + + int32_t indices[] = { + 0, 1, 2, 0, 2, 3, 0, 3, 4, 0, 4, 5, 0, 5, 6, 0, 6, 1, + }; + + b3MeshDef def = { 0 }; + def.vertices = vertices; + def.vertexCount = ARRAY_COUNT( vertices ); + def.indices = indices; + def.triangleCount = ARRAY_COUNT( indices ) / 3; + def.identifyEdges = true; + b3MeshData* mesh = b3CreateMesh( &def, NULL, 0 ); + ENSURE( mesh != NULL ); + + b3Vec3 directions[] = { + { -0.5703f, 0.7369f, 0.3629f }, + { 0.8019f, 0.4956f, 0.3337f }, + }; + + // Dropping every vertex manifold here lets the body sink a full step before the deep overlap + // fallback catches it + for ( int i = 0; i < ARRAY_COUNT( directions ); ++i ) + { + b3Vec3 direction = b3Normalize( directions[i] ); + ENSURE( ApexImpact( mesh, direction, false ) > -B3_LINEAR_SLOP ); + ENSURE( ApexImpact( mesh, direction, true ) > -B3_LINEAR_SLOP ); + } + + b3DestroyMesh( mesh ); + return 0; +} + int MeshTest( void ) { RUN_SUBTEST( MeshDenseStride ); @@ -795,6 +1131,9 @@ int MeshTest( void ) RUN_SUBTEST( MeshManyTriangles ); RUN_SUBTEST( MeshMirroredFlags ); RUN_SUBTEST( MeshEdgeAngles ); + RUN_SUBTEST( MeshSphereOnRidge ); + RUN_SUBTEST( MeshCapsuleOverCrest ); + RUN_SUBTEST( MeshApexContact ); return 0; }