diff --git a/.clang-format b/.clang-format index eaea153..4e081ec 100644 --- a/.clang-format +++ b/.clang-format @@ -12,7 +12,7 @@ BraceWrapping: AfterUnion: true BeforeWhile: true -ColumnLimit: 100 +ColumnLimit: 120 PointerAlignment: Left UseTab: Always BreakConstructorInitializers: BeforeComma diff --git a/.editorconfig b/.editorconfig index 2f3b107..a07077e 100644 --- a/.editorconfig +++ b/.editorconfig @@ -1,5 +1,5 @@ root = true [*.{c,h,cpp,inl}] -guidelines = 100 +guidelines = 120 guidelines_style = 1px dotted gray diff --git a/benchmark/CMakeLists.txt b/benchmark/CMakeLists.txt index c3e21cb..2650a34 100644 --- a/benchmark/CMakeLists.txt +++ b/benchmark/CMakeLists.txt @@ -2,6 +2,8 @@ set(BOX3D_BENCHMARK_FILES main.c + mesh_benchmark.c + mesh_benchmark.h ) add_executable(benchmark ${BOX3D_BENCHMARK_FILES}) @@ -18,4 +20,36 @@ endif() target_link_libraries(benchmark PRIVATE box3d shared) -source_group(TREE ${CMAKE_CURRENT_SOURCE_DIR} PREFIX "" FILES ${BOX3D_BENCHMARK_FILES}) \ No newline at end of file +source_group(TREE ${CMAKE_CURRENT_SOURCE_DIR} PREFIX "" FILES ${BOX3D_BENCHMARK_FILES}) +# Mesh drop soak. Uses internal threads, so it needs the static library. +if (NOT BUILD_SHARED_LIBS) + add_executable(mesh_soak mesh_soak.c) + + set_target_properties(mesh_soak PROPERTIES + C_STANDARD 17 + C_STANDARD_REQUIRED YES + C_EXTENSIONS NO + ) + + if (BOX3D_COMPILE_WARNING_AS_ERROR) + set_target_properties(mesh_soak PROPERTIES COMPILE_WARNING_AS_ERROR ON) + endif() + + target_include_directories(mesh_soak PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../src) + target_link_libraries(mesh_soak PRIVATE box3d shared) +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..3d7f972 --- /dev/null +++ b/benchmark/ghost_baseline.csv @@ -0,0 +1,105 @@ +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,0,2 +cliff,0,1,box,0,2 +cliff,0,1,capsule,0,2 +cliff,0,1,upright,0,2 +cliff,0,1,sphere,0,2 +cliff,0,1,cylinder,0,2 +crest,0,0,box,0,2 +crest,0,0,capsule,0,2 +crest,0,0,upright,0,2 +crest,0,0,sphere,0,2 +crest,0,0,cylinder,0,2 +crest,0,1,box,0,2 +crest,0,1,capsule,0,2 +crest,0,1,upright,0,2 +crest,0,1,sphere,0,2 +crest,0,1,cylinder,0,2 +bumps,0,0,box,0,1 +bumps,0,0,capsule,0,1 +bumps,0,0,upright,0,1 +bumps,0,0,sphere,0,1 +bumps,0,0,cylinder,0,1 +bumps,0,1,box,0,1 +bumps,0,1,capsule,0,1 +bumps,0,1,upright,0,1 +bumps,0,1,sphere,0,1 +bumps,0,1,cylinder,0,1 +peel,0,0,box,0,2 +peel,0,0,capsule,0,2 +peel,0,0,upright,0,2 +peel,0,0,sphere,0,2 +peel,0,0,cylinder,0,2 +peel,0,1,box,0,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,0,2 +peel,1,0,capsule,0,2 +peel,1,0,upright,0,2 +peel,1,0,sphere,0,2 +peel,1,0,cylinder,0,2 +peel,1,1,box,0,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,0,2 +peel,2,0,capsule,0,2 +peel,2,0,upright,0,2 +peel,2,0,sphere,0,2 +peel,2,0,cylinder,0,2 +peel,2,1,box,0,2 +peel,2,1,capsule,0,2 +peel,2,1,upright,0,2 +peel,2,1,sphere,0,2 +peel,2,1,cylinder,0,2 +peel,3,0,box,0,2 +peel,3,0,capsule,0,2 +peel,3,0,upright,0,2 +peel,3,0,sphere,0,2 +peel,3,0,cylinder,0,2 +peel,3,1,box,0,2 +peel,3,1,capsule,0,2 +peel,3,1,upright,0,2 +peel,3,1,sphere,0,2 +peel,3,1,cylinder,0,2 +chamfer,0,0,box,0,2 +chamfer,0,0,cylinder,0,2 +chamfer,0,1,box,0,2 +chamfer,0,1,cylinder,0,2 +rail,0,0,box,0,2 +rail,0,0,capsule,0,2 +rail,0,0,upright,0,5 +rail,0,0,sphere,0,4 +rail,0,0,cylinder,0,3 +rail,0,1,box,0,2 +rail,0,1,capsule,0,2 +rail,0,1,upright,0,5 +rail,0,1,sphere,0,4 +rail,0,1,cylinder,0,3 diff --git a/benchmark/ghost_baseline_hf.csv b/benchmark/ghost_baseline_hf.csv new file mode 100644 index 0000000..932a909 --- /dev/null +++ b/benchmark/ghost_baseline_hf.csv @@ -0,0 +1,105 @@ +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,0,2 +cliff,0,1,box,0,2 +cliff,0,1,capsule,0,2 +cliff,0,1,upright,0,2 +cliff,0,1,sphere,0,2 +cliff,0,1,cylinder,0,2 +crest,0,0,box,0,2 +crest,0,0,capsule,0,2 +crest,0,0,upright,0,2 +crest,0,0,sphere,0,2 +crest,0,0,cylinder,0,2 +crest,0,1,box,0,2 +crest,0,1,capsule,0,2 +crest,0,1,upright,0,2 +crest,0,1,sphere,0,2 +crest,0,1,cylinder,0,2 +bumps,0,0,box,0,1 +bumps,0,0,capsule,0,1 +bumps,0,0,upright,0,1 +bumps,0,0,sphere,0,1 +bumps,0,0,cylinder,0,1 +bumps,0,1,box,0,1 +bumps,0,1,capsule,0,1 +bumps,0,1,upright,0,1 +bumps,0,1,sphere,0,1 +bumps,0,1,cylinder,0,1 +peel,0,0,box,0,2 +peel,0,0,capsule,0,2 +peel,0,0,upright,0,2 +peel,0,0,sphere,0,2 +peel,0,0,cylinder,0,2 +peel,0,1,box,0,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,0,2 +peel,1,0,capsule,0,2 +peel,1,0,upright,0,2 +peel,1,0,sphere,0,2 +peel,1,0,cylinder,0,2 +peel,1,1,box,0,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,0,2 +peel,2,0,capsule,0,2 +peel,2,0,upright,0,2 +peel,2,0,sphere,0,2 +peel,2,0,cylinder,0,2 +peel,2,1,box,0,2 +peel,2,1,capsule,0,2 +peel,2,1,upright,0,2 +peel,2,1,sphere,0,2 +peel,2,1,cylinder,0,2 +peel,3,0,box,0,2 +peel,3,0,capsule,0,2 +peel,3,0,upright,0,2 +peel,3,0,sphere,0,2 +peel,3,0,cylinder,0,2 +peel,3,1,box,0,2 +peel,3,1,capsule,0,2 +peel,3,1,upright,0,2 +peel,3,1,sphere,0,2 +peel,3,1,cylinder,0,2 +chamfer,0,0,box,0,2 +chamfer,0,0,cylinder,0,2 +chamfer,0,1,box,0,2 +chamfer,0,1,cylinder,0,2 +rail,0,0,box,0,5 +rail,0,0,capsule,0,4 +rail,0,0,upright,0,5 +rail,0,0,sphere,0,5 +rail,0,0,cylinder,0,4 +rail,0,1,box,0,5 +rail,0,1,capsule,0,4 +rail,0,1,upright,0,5 +rail,0,1,sphere,0,5 +rail,0,1,cylinder,0,4 diff --git a/benchmark/ghost_scan.c b/benchmark/ghost_scan.c new file mode 100644 index 0000000..f61e50c --- /dev/null +++ b/benchmark/ghost_scan.c @@ -0,0 +1,521 @@ +// 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. It +// also fails on a config that completed fewer runs, because a body stopped on an edge or dropped +// through the ground crosses fewer edges and so logs fewer ghosts. + +#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", "peel", "chamfer", "rail" }; +static const char* s_bodyKeys[e_ghostBodyCount] = { "box", "capsule", "upright", "sphere", "cylinder" }; + +#define MAX_CONFIGS 256 + +static bool s_recycling = true; + +typedef struct Config +{ + GhostScene scene; + int preset; + bool mirror; + GhostBody body; + int ghostCount; + int runCount; + + // From the compare file, -1 when absent + int baseGhostCount; + int baseRunCount; +} 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; + + // The s&box character is a fixed rotation hull without speculative contact. Speculative contact or + // rotation hide the ghost. + if ( config->scene == e_ghostChamfer ) + { + settings.fixedRotation = true; + settings.speculative = false; + } + + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + if ( s_recycling == false ) + { + b3World_SetContactRecycleDistance( worldId, 0.0f ); + } + + 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_ghostInternal ) + { + 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 SameConfig( const Config* a, const Config* b ) +{ + return a->scene == b->scene && a->preset == b->preset && a->mirror == b->mirror && a->body == b->body; +} + +// Filters are -1 for all +static int BuildConfigs( Config* configs, int sceneFilter, int presetFilter, int mirrorFilter, int bodyFilter ) +{ + int configCount = 0; + for ( int scene = 0; scene < e_ghostSceneCount; ++scene ) + { + if ( sceneFilter >= 0 && scene != sceneFilter ) + { + continue; + } + + int presetCount = scene == e_ghostInternal ? GHOST_PEEL_PRESET_COUNT : 1; + for ( int preset = 0; preset < presetCount; ++preset ) + { + if ( scene == e_ghostInternal && 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; + } + + // Only a flat bottom can rest on the chamfer edges + if ( scene == e_ghostChamfer && body != e_ghostBox && body != e_ghostCylinder ) + { + continue; + } + + B3_ASSERT( configCount < MAX_CONFIGS ); + configs[configCount++] = (Config){ + .scene = (GhostScene)scene, + .preset = preset, + .mirror = mirror != 0, + .body = (GhostBody)body, + .baseGhostCount = -1, + .baseRunCount = -1, + }; + } + } + } + } + + return configCount; +} + +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; + c->baseRunCount = runs; + } + } + } + + fclose( file ); + return true; +} + +// Replaces only the rows of this run and keeps the rest of an existing file, in canonical order +static bool WriteResults( const char* path, const Config* configs, int configCount ) +{ + Config allConfigs[MAX_CONFIGS]; + int allCount = BuildConfigs( allConfigs, -1, -1, -1, -1 ); + + FILE* existing = fopen( path, "r" ); + if ( existing != NULL ) + { + fclose( existing ); + if ( ReadBaseline( path, allConfigs, allCount ) == false ) + { + return false; + } + } + + 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 < allCount; ++i ) + { + const Config* c = allConfigs + i; + int ghostCount = c->baseGhostCount; + int runCount = c->baseRunCount; + for ( int j = 0; j < configCount; ++j ) + { + if ( SameConfig( c, configs + j ) ) + { + ghostCount = configs[j].ghostCount; + runCount = configs[j].runCount; + break; + } + } + + if ( ghostCount < 0 ) + { + continue; + } + + fprintf( file, "%s,%d,%d,%s,%d,%d\n", s_sceneKeys[c->scene], c->preset, c->mirror ? 1 : 0, s_bodyKeys[c->body], ghostCount, + runCount ); + } + + 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, "--no-recycling" ) == 0 ) + { + s_recycling = false; + } + else if ( strcmp( arg, "--height-field" ) == 0 ) + { + settings.heightField = true; + } + 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, peel, chamfer, rail (default all)\n" + " chamfer runs only the box and cylinder, fixed rotation without speculative\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, replacing only the rows of this run\n" + " -c, --compare= compare with a written csv, fail if any count grew, any config\n" + " completed fewer runs, or has no row\n" + " --speed=, --yaw=, --friction=, --cell=, --fixed-rotation, --no-speculative\n" + " --height-field height field ground, one static body per height field\n" + " --no-recycling disable contact recycling\n" + " -v, --verbose print every ghost\n" + "Exit code is 1 when a compared count grew, completed fewer runs or has no row, 0 otherwise.\n" ); + return 2; + } + } + + Config configs[MAX_CONFIGS]; + int configCount = BuildConfigs( configs, sceneFilter, presetFilter, mirrorFilter, bodyFilter ); + + 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%s\n", configCount, stepCount, + settings.speed, settings.yaw, settings.friction, settings.cellSize, settings.heightField ? ", height field" : "" ); + 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; + int missingCount = 0; + int fewerRunsCount = 0; + int column = 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 ); + column = 0; + } + + // A scene that skips a body leaves its column empty + int bodyColumn = bodyFilter >= 0 ? 0 : (int)c->body; + while ( column < bodyColumn ) + { + printf( "%10s", "-" ); + column += 1; + } + column += 1; + + char cell[32]; + if ( comparePath != NULL && c->baseGhostCount < 0 ) + { + snprintf( cell, sizeof( cell ), "?>%d", c->ghostCount ); + } + else 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; + missingCount += c->baseGhostCount < 0; + fewerRunsCount += c->baseRunCount >= 0 && c->runCount < c->baseRunCount; + } + + printf( "\n\ntotal ghosts %d, time %.1f s\n", totalGhosts, seconds ); + + for ( int i = 0; i < configCount && fewerRunsCount > 0; ++i ) + { + const Config* c = configs + i; + if ( c->baseRunCount >= 0 && c->runCount < c->baseRunCount ) + { + char row[64]; + FormatRow( row, sizeof( row ), c ); + printf( "fewer runs: %s %s %d>%d\n", row, s_bodyKeys[c->body], c->baseRunCount, c->runCount ); + } + } + + if ( writePath != NULL && WriteResults( writePath, configs, configCount ) == false ) + { + return 2; + } + + if ( comparePath != NULL ) + { + bool pass = grewCount == 0 && missingCount == 0 && fewerRunsCount == 0; + printf( "compared with %s: %d grew, %d shrank, %d missing, %d with fewer runs\n", comparePath, grewCount, shrankCount, + missingCount, fewerRunsCount ); + printf( pass ? "PASS\n" : "FAIL\n" ); + return pass ? 0 : 1; + } + + return 0; +} diff --git a/benchmark/main.c b/benchmark/main.c index fd7235f..00747f8 100644 --- a/benchmark/main.c +++ b/benchmark/main.c @@ -6,6 +6,7 @@ #endif #include "benchmarks.h" +#include "mesh_benchmark.h" #include "utils.h" #ifdef BOX3D_INTERNAL_BENCHMARKS @@ -215,6 +216,9 @@ static void PrintBenchmarks( const Benchmark* benchmarks, int count ) // Compare GJK and SAT on hull pairs in the speculative regime. Static Box3D only. // start /affinity 0x5555 .\build\bin\Release\benchmark.exe --sat -r=5 +// Time mesh creation. +// start /affinity 0x5555 .\build\bin\Release\benchmark.exe --mesh -r=5 + int main( int argc, char** argv ) { #ifdef TRACY_ENABLE @@ -286,6 +290,8 @@ int main( int argc, char** argv ) bool enableContinuous = true; bool recordStepTimes = false; bool runSat = false; + bool runMesh = false; + const char* meshCase = NULL; int simdWidth = 0; assert( maxThreadCount <= B3_MAX_WORKERS ); @@ -343,6 +349,15 @@ int main( int argc, char** argv ) exit( 1 ); #endif } + else if ( ( value = MatchValue( arg, "-mesh=", "--mesh=" ) ) != NULL ) + { + runMesh = true; + meshCase = value; + } + else if ( strcmp( arg, "-mesh" ) == 0 || strcmp( arg, "--mesh" ) == 0 ) + { + runMesh = true; + } else if ( ( value = MatchValue( arg, "-simd=", "--simd-width=" ) ) != NULL ) { simdWidth = atoi( value ); @@ -372,6 +387,7 @@ int main( int argc, char** argv ) "-nc, --no-continuous: disable continuous collision\n" "-s, --record-steps: record step times\n" "-sat, --sat: compare GJK and SAT on hull pairs, then exit\n" + "-mesh, --mesh[=]: time b3CreateMesh on several meshes, or one case, then exit\n" "-simd, --simd-width=<4|8>: force the SIMD width (static Box3D only)\n" "-l, --list: list the registered benchmarks\n" "-h, --help: print this help\n" ); @@ -402,6 +418,13 @@ int main( int argc, char** argv ) } #endif + if ( runMesh ) + { + RunMeshBenchmarks( runCount, meshCase ); + free( profiles ); + return 0; + } + printf( "Starting benchmarks\n" ); printf( "======================================\n" ); #ifdef BOX3D_INTERNAL_BENCHMARKS diff --git a/benchmark/mesh_benchmark.c b/benchmark/mesh_benchmark.c new file mode 100644 index 0000000..b1621b1 --- /dev/null +++ b/benchmark/mesh_benchmark.c @@ -0,0 +1,233 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "mesh_benchmark.h" + +#include "box3d/box3d.h" +#include "box3d/collision.h" +#include "box3d/math_functions.h" + +#include +#include +#include +#include +#include + +typedef enum MeshShape +{ + meshShape_wave, + meshShape_torus, + meshShape_soup, +} MeshShape; + +typedef struct MeshCase +{ + const char* name; + MeshShape shape; + int countA; + int countB; + bool useMedianSplit; + bool identifyEdges; + bool weldVertices; +} MeshCase; + +typedef struct MeshSource +{ + b3Vec3* vertices; + int32_t* indices; + int vertexCount; + int triangleCount; +} MeshSource; + +static float WaveHeight( int i, int j ) +{ + return 0.5f * sinf( 0.31f * (float)i ) * cosf( 0.23f * (float)j ) + 0.25f * sinf( 0.07f * (float)( i + 2 * j ) ); +} + +// Shared-vertex grid, or a triangle soup with three vertices per triangle when unshared is set +static MeshSource MakeWave( int xCount, int zCount, bool unshared ) +{ + float cellWidth = 0.5f; + MeshSource source = { 0 }; + source.triangleCount = 2 * xCount * zCount; + source.indices = malloc( 3 * source.triangleCount * sizeof( int32_t ) ); + + if ( unshared == false ) + { + source.vertexCount = ( xCount + 1 ) * ( zCount + 1 ); + source.vertices = malloc( source.vertexCount * sizeof( b3Vec3 ) ); + for ( int i = 0; i <= xCount; ++i ) + { + for ( int j = 0; j <= zCount; ++j ) + { + source.vertices[i * ( zCount + 1 ) + j] = (b3Vec3){ cellWidth * i, WaveHeight( i, j ), cellWidth * j }; + } + } + + int index = 0; + for ( int i = 0; i < xCount; ++i ) + { + for ( int j = 0; j < zCount; ++j ) + { + int a = i * ( zCount + 1 ) + j; + int b = a + 1; + int c = a + ( zCount + 1 ) + 1; + int d = a + ( zCount + 1 ); + source.indices[index++] = a; + source.indices[index++] = b; + source.indices[index++] = c; + source.indices[index++] = a; + source.indices[index++] = c; + source.indices[index++] = d; + } + } + } + else + { + source.vertexCount = 3 * source.triangleCount; + source.vertices = malloc( source.vertexCount * sizeof( b3Vec3 ) ); + int index = 0; + for ( int i = 0; i < xCount; ++i ) + { + for ( int j = 0; j < zCount; ++j ) + { + b3Vec3 a = { cellWidth * i, WaveHeight( i, j ), cellWidth * j }; + b3Vec3 b = { cellWidth * i, WaveHeight( i, j + 1 ), cellWidth * ( j + 1 ) }; + b3Vec3 c = { cellWidth * ( i + 1 ), WaveHeight( i + 1, j + 1 ), cellWidth * ( j + 1 ) }; + b3Vec3 d = { cellWidth * ( i + 1 ), WaveHeight( i + 1, j ), cellWidth * j }; + b3Vec3 tris[6] = { a, b, c, a, c, d }; + for ( int k = 0; k < 6; ++k ) + { + source.vertices[index] = tris[k]; + source.indices[index] = index; + index += 1; + } + } + } + } + + return source; +} + +static MeshSource MakeTorus( int radialCount, int tubularCount ) +{ + float radius = 8.0f; + float thickness = 2.0f; + MeshSource source = { 0 }; + source.vertexCount = radialCount * tubularCount; + source.triangleCount = 2 * radialCount * tubularCount; + source.vertices = malloc( source.vertexCount * sizeof( b3Vec3 ) ); + source.indices = malloc( 3 * source.triangleCount * sizeof( int32_t ) ); + + for ( int i = 0; i < radialCount; ++i ) + { + float u = 2.0f * B3_PI * i / radialCount; + for ( int j = 0; j < tubularCount; ++j ) + { + float v = 2.0f * B3_PI * j / tubularCount; + float r = radius + thickness * cosf( v ); + source.vertices[i * tubularCount + j] = (b3Vec3){ r * cosf( u ), thickness * sinf( v ), r * sinf( u ) }; + } + } + + int index = 0; + for ( int i = 0; i < radialCount; ++i ) + { + int i2 = ( i + 1 ) % radialCount; + for ( int j = 0; j < tubularCount; ++j ) + { + int j2 = ( j + 1 ) % tubularCount; + int a = i * tubularCount + j; + int b = i2 * tubularCount + j; + int c = i2 * tubularCount + j2; + int d = i * tubularCount + j2; + source.indices[index++] = a; + source.indices[index++] = c; + source.indices[index++] = b; + source.indices[index++] = a; + source.indices[index++] = d; + source.indices[index++] = c; + } + } + + return source; +} + +void RunMeshBenchmarks( int runCount, const char* caseName ) +{ + MeshCase cases[] = { + { "wave_64", meshShape_wave, 64, 64, true, true, false }, + { "wave_128", meshShape_wave, 128, 128, true, true, false }, + { "wave_256", meshShape_wave, 256, 256, true, true, false }, + { "wave_128_no_edges", meshShape_wave, 128, 128, true, false, false }, + { "wave_128_sah", meshShape_wave, 128, 128, false, true, false }, + { "torus_128x64", meshShape_torus, 128, 64, false, true, false }, + { "soup_128_weld", meshShape_soup, 128, 128, true, true, true }, + }; + int caseCount = sizeof( cases ) / sizeof( cases[0] ); + +#ifdef NDEBUG + int targetTriangles = 1 << 20; +#else + int targetTriangles = 1 << 14; +#endif + + printf( "benchmark: mesh creation, runs = %d\n", runCount ); + printf( "%-20s %10s %8s %12s %12s\n", "case", "triangles", "builds", "ms / build", "ns / tri" ); + + for ( int caseIndex = 0; caseIndex < caseCount; ++caseIndex ) + { + const MeshCase* mc = cases + caseIndex; + if ( caseName != NULL && strcmp( caseName, mc->name ) != 0 ) + { + continue; + } + + MeshSource source; + if ( mc->shape == meshShape_torus ) + { + source = MakeTorus( mc->countA, mc->countB ); + } + else + { + source = MakeWave( mc->countA, mc->countB, mc->shape == meshShape_soup ); + } + + b3MeshDef def = { 0 }; + def.vertices = source.vertices; + def.vertexCount = source.vertexCount; + def.indices = source.indices; + def.triangleCount = source.triangleCount; + def.useMedianSplit = mc->useMedianSplit; + def.identifyEdges = mc->identifyEdges; + def.weldVertices = mc->weldVertices; + def.weldTolerance = mc->weldVertices ? 0.001f : 0.0f; + + int buildCount = targetTriangles / source.triangleCount; + if ( buildCount < 1 ) + { + buildCount = 1; + } + + float bestMs = FLT_MAX; + for ( int runIndex = 0; runIndex < runCount; ++runIndex ) + { + uint64_t ticks = b3GetTicks(); + for ( int buildIndex = 0; buildIndex < buildCount; ++buildIndex ) + { + b3MeshData* mesh = b3CreateMesh( &def, NULL, 0 ); + b3DestroyMesh( mesh ); + } + float ms = b3GetMilliseconds( ticks ) / buildCount; + bestMs = b3MinFloat( bestMs, ms ); + } + + float nsPerTriangle = 1.0e6f * bestMs / source.triangleCount; + printf( "%-20s %10d %8d %12.3f %12.1f\n", mc->name, source.triangleCount, buildCount, bestMs, nsPerTriangle ); + + free( source.vertices ); + free( source.indices ); + } + + printf( "\n" ); +} diff --git a/benchmark/mesh_benchmark.h b/benchmark/mesh_benchmark.h new file mode 100644 index 0000000..b942ed6 --- /dev/null +++ b/benchmark/mesh_benchmark.h @@ -0,0 +1,6 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +void RunMeshBenchmarks( int runCount, const char* caseName ); diff --git a/benchmark/mesh_soak.c b/benchmark/mesh_soak.c new file mode 100644 index 0000000..f696682 --- /dev/null +++ b/benchmark/mesh_soak.c @@ -0,0 +1,672 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +// Headless soak of the Continuous / Mesh Drop sample. Each generation drops 1024 small bodies on a +// wave mesh and steps until every body sleeps. A generation fails if a body falls through the mesh +// or the pile is still awake at the step limit. Tunnels are rare, so the soak also counts bodies that +// sink into the wave past a few depth levels and can compare those counts against a baseline. +// Generations run in parallel, one single threaded world per thread. The printed seed reproduces a +// generation in the sample. + +#define _CRT_SECURE_NO_WARNINGS + +#include "core.h" +#include "mesh_drop.h" + +#include "box3d/box3d.h" +#include "box3d/collision.h" +#include "box3d/math_functions.h" + +#include +#include +#include +#include +#include + +#define MAX_REPORTED_FAILURES 32 +#define MAX_THREADS 64 + +typedef enum FailureKind +{ + e_tunnel, + e_awake, +} FailureKind; + +typedef struct Failure +{ + FailureKind kind; + MeshDropShape shape; + int generation; + uint32_t seed; + int step; + int index; + b3Pos position; +} Failure; + +typedef struct ShapeStats +{ + int generationCount; + int tunnelCount; + int awakeCount; + int depthCounts[MESH_DROP_DEPTH_LEVEL_COUNT]; + + float maxDepth; + uint32_t maxSeed; + int maxGeneration; + int maxIndex; + int maxStep; +} ShapeStats; + +typedef struct Soak +{ + b3Mutex* mutex; + b3MeshData* groundMesh; + b3HullData* cylinder; + MeshDropSettings settings; + uint32_t baseSeed; + int generationCount; + int nextGeneration; + + // -1 rotates through the shapes + int shapeFilter; + + bool fixedSeed; + bool recycling; + + int completedCount; + int failureCount; + long long totalSteps; + int maxSteps; + ShapeStats stats[e_meshDropShapeCount]; + Failure failures[MAX_REPORTED_FAILURES]; +} Soak; + +static void RecordFailure( Soak* soak, const Failure* failure ) +{ + b3LockMutex( soak->mutex ); + if ( soak->failureCount < MAX_REPORTED_FAILURES ) + { + soak->failures[soak->failureCount] = *failure; + } + soak->failureCount += 1; + b3UnlockMutex( soak->mutex ); + + const char* kind = failure->kind == e_tunnel ? "TUNNEL" : "AWAKE"; + printf( "%s: generation %d, %s, seed %u, step %d, body (%d, %d) at (%.2f, %.2f, %.2f)\n", kind, failure->generation, + g_meshDropShapeNames[failure->shape], failure->seed, failure->step, failure->index / MESH_DROP_GRID_COUNT, + failure->index % MESH_DROP_GRID_COUNT, (float)failure->position.x, (float)failure->position.y, + (float)failure->position.z ); +} + +// Mirrors MeshDrop::Generate and the auto generate step loop. Returns the step count. +static int RunGeneration( Soak* soak, int generation, uint32_t seed, MeshDropShape shape, b3BodyId* bodies, + MeshDropDepth* depth, int* failureKind ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.workerCount = 1; + + // The world registry is global and not thread safe + b3LockMutex( soak->mutex ); + b3WorldId worldId = b3CreateWorld( &worldDef ); + b3UnlockMutex( soak->mutex ); + + if ( soak->recycling == false ) + { + b3World_SetContactRecycleDistance( worldId, 0.0f ); + } + + CreateMeshDropGround( worldId, soak->groundMesh ); + + MeshDropSettings settings = soak->settings; + settings.shape = shape; + CreateMeshDropBodies( worldId, settings, seed, soak->cylinder, bodies ); + ResetMeshDropDepth( depth, settings, soak->cylinder ); + + float timeStep = 1.0f / 60.0f; + int subStepCount = 4; + int maxSteps = GetMeshDropStepLimit( shape ); + float tunnelHeight = GetMeshDropTunnelHeight(); + int step = 0; + bool failed = false; + + while ( failed == false ) + { + b3World_Step( worldId, timeStep, subStepCount ); + UpdateMeshDropDepth( worldId, depth ); + step += 1; + + for ( int i = 0; i < MESH_DROP_BODY_COUNT; ++i ) + { + b3Pos center = b3Body_GetWorldCenter( bodies[i] ); + if ( center.y < tunnelHeight ) + { + Failure failure = { e_tunnel, shape, generation, seed, step, i, center }; + RecordFailure( soak, &failure ); + *failureKind = e_tunnel; + failed = true; + break; + } + } + + if ( b3World_GetBodyEvents( worldId ).moveCount == 0 ) + { + break; + } + + if ( failed == false && step > maxSteps ) + { + int awakeIndex = 0; + for ( int i = 0; i < MESH_DROP_BODY_COUNT; ++i ) + { + if ( b3Body_IsAwake( bodies[i] ) ) + { + awakeIndex = i; + break; + } + } + + b3Pos center = b3Body_GetWorldCenter( bodies[awakeIndex] ); + Failure failure = { e_awake, shape, generation, seed, step, awakeIndex, center }; + RecordFailure( soak, &failure ); + *failureKind = e_awake; + failed = true; + } + } + + b3LockMutex( soak->mutex ); + b3DestroyWorld( worldId ); + b3UnlockMutex( soak->mutex ); + + return step; +} + +static void SoakWorker( void* context ) +{ + Soak* soak = context; + b3BodyId* bodies = malloc( MESH_DROP_BODY_COUNT * sizeof( b3BodyId ) ); + MeshDropDepth* depth = malloc( sizeof( MeshDropDepth ) ); + + while ( true ) + { + b3LockMutex( soak->mutex ); + int generation = soak->nextGeneration; + soak->nextGeneration += 1; + b3UnlockMutex( soak->mutex ); + + if ( generation >= soak->generationCount ) + { + break; + } + + uint32_t seed = soak->fixedSeed ? soak->baseSeed : GetMeshDropSeed( soak->baseSeed, generation ); + MeshDropShape shape = + soak->shapeFilter >= 0 ? (MeshDropShape)soak->shapeFilter : (MeshDropShape)( generation % e_meshDropShapeCount ); + + int failureKind = -1; + int steps = RunGeneration( soak, generation, seed, shape, bodies, depth, &failureKind ); + + int depthCounts[MESH_DROP_DEPTH_LEVEL_COUNT]; + GetMeshDropDepthCounts( depth, depthCounts ); + + b3LockMutex( soak->mutex ); + soak->completedCount += 1; + soak->totalSteps += steps; + soak->maxSteps = b3MaxInt( soak->maxSteps, steps ); + + ShapeStats* stats = soak->stats + shape; + stats->generationCount += 1; + stats->tunnelCount += failureKind == e_tunnel ? 1 : 0; + stats->awakeCount += failureKind == e_awake ? 1 : 0; + for ( int i = 0; i < MESH_DROP_DEPTH_LEVEL_COUNT; ++i ) + { + stats->depthCounts[i] += depthCounts[i]; + } + + // Ties go to the earlier generation so the report does not depend on thread timing + if ( depth->maxDepth > stats->maxDepth || + ( depth->maxDepth == stats->maxDepth && generation < stats->maxGeneration ) ) + { + stats->maxDepth = depth->maxDepth; + stats->maxSeed = seed; + stats->maxGeneration = generation; + stats->maxIndex = depth->maxIndex; + stats->maxStep = depth->maxStep; + } + + int completed = soak->completedCount; + b3UnlockMutex( soak->mutex ); + + if ( completed % b3MaxInt( 100, soak->generationCount / 10 ) == 0 ) + { + printf( "%d generations, %d bodies\n", completed, completed * MESH_DROP_BODY_COUNT ); + fflush( stdout ); + } + } + + free( depth ); + free( bodies ); +} + +static const char* GetFilterName( int shapeFilter ) +{ + return shapeFilter >= 0 ? g_meshDropShapeNames[shapeFilter] : "all"; +} + +// A baseline row starts with the run that made it, so one file can hold several runs +static void GetBaselineKey( const Soak* soak, char* key, int size ) +{ + snprintf( key, size, "%s,%u,%d,", GetFilterName( soak->shapeFilter ), soak->baseSeed, soak->generationCount ); +} + +// Replaces the rows of this run and keeps the rest +static void WriteBaseline( const Soak* soak, const char* path ) +{ + char key[64]; + GetBaselineKey( soak, key, sizeof( key ) ); + size_t keyLength = strlen( key ); + + // Read the whole file so neither a row count nor a row length can drop a row + char* text = NULL; + FILE* file = fopen( path, "rb" ); + if ( file != NULL ) + { + fseek( file, 0, SEEK_END ); + long textLength = ftell( file ); + fseek( file, 0, SEEK_SET ); + text = malloc( textLength + 1 ); + textLength = (long)fread( text, 1, textLength, file ); + text[textLength] = '\0'; + fclose( file ); + } + + file = fopen( path, "w" ); + if ( file == NULL ) + { + printf( "cannot write %s\n", path ); + free( text ); + return; + } + + fprintf( file, "filter,base,generations,shape,tunnels,awake" ); + for ( int i = 0; i < MESH_DROP_DEPTH_LEVEL_COUNT; ++i ) + { + fprintf( file, ",depth%g", g_meshDropDepthLevels[i] ); + } + fprintf( file, "\n" ); + + if ( text != NULL ) + { + bool header = true; + char* line = text; + while ( *line != '\0' ) + { + char* next = strchr( line, '\n' ); + size_t lineLength = next != NULL ? (size_t)( next - line ) : strlen( line ); + if ( lineLength > 0 && line[lineLength - 1] == '\r' ) + { + lineLength -= 1; + } + + if ( header == false && lineLength > 0 && strncmp( line, key, keyLength ) != 0 ) + { + fprintf( file, "%.*s\n", (int)lineLength, line ); + } + + header = false; + line = next != NULL ? next + 1 : line + strlen( line ); + } + + free( text ); + } + + for ( int shape = 0; shape < e_meshDropShapeCount; ++shape ) + { + const ShapeStats* stats = soak->stats + shape; + if ( stats->generationCount == 0 ) + { + continue; + } + + fprintf( file, "%s,%u,%d,%s,%d,%d", GetFilterName( soak->shapeFilter ), soak->baseSeed, soak->generationCount, + g_meshDropShapeNames[shape], stats->tunnelCount, stats->awakeCount ); + for ( int i = 0; i < MESH_DROP_DEPTH_LEVEL_COUNT; ++i ) + { + fprintf( file, ",%d", stats->depthCounts[i] ); + } + fprintf( file, "\n" ); + } + + fclose( file ); + printf( "wrote %s\n", path ); +} + +// Returns 0 when no count grew past its noise band, 1 when one did, 2 when the baseline has no rows for this run +static int CompareBaseline( const Soak* soak, const char* path ) +{ + FILE* file = fopen( path, "r" ); + if ( file == NULL ) + { + printf( "cannot read %s\n", path ); + return 2; + } + + char line[512]; + if ( fgets( line, sizeof( line ), file ) == NULL ) + { + fclose( file ); + printf( "empty baseline %s\n", path ); + return 2; + } + + int result = 0; + int changedCount = 0; + int matchedCount = 0; + + while ( fgets( line, sizeof( line ), file ) != NULL ) + { + char filter[32]; + char shapeName[32]; + unsigned int base; + int generations; + int counts[2 + MESH_DROP_DEPTH_LEVEL_COUNT]; + + _Static_assert( MESH_DROP_DEPTH_LEVEL_COUNT == 4, "update the baseline row format" ); + int fieldCount = sscanf( line, "%31[^,],%u,%d,%31[^,],%d,%d,%d,%d,%d,%d", filter, &base, &generations, shapeName, + counts + 0, counts + 1, counts + 2, counts + 3, counts + 4, counts + 5 ); + if ( fieldCount != 10 ) + { + continue; + } + + if ( strcmp( filter, GetFilterName( soak->shapeFilter ) ) != 0 || base != soak->baseSeed || + generations != soak->generationCount ) + { + continue; + } + + int shape = -1; + for ( int i = 0; i < e_meshDropShapeCount; ++i ) + { + if ( strcmp( shapeName, g_meshDropShapeNames[i] ) == 0 ) + { + shape = i; + } + } + + if ( shape < 0 ) + { + continue; + } + + matchedCount += 1; + + const ShapeStats* stats = soak->stats + shape; + int current[2 + MESH_DROP_DEPTH_LEVEL_COUNT] = { stats->tunnelCount, stats->awakeCount }; + for ( int i = 0; i < MESH_DROP_DEPTH_LEVEL_COUNT; ++i ) + { + current[2 + i] = stats->depthCounts[i]; + } + + for ( int i = 0; i < 2 + MESH_DROP_DEPTH_LEVEL_COUNT; ++i ) + { + if ( current[i] == counts[i] ) + { + continue; + } + + // Any collision change reshuffles the chaotic pile, so a count moves by about its square root + // even when nothing got worse. Flag growth past three of those. A count that was zero flags on any + // growth, since a good build never sinks that deep. + float band = 3.0f * sqrtf( (float)counts[i] ); + bool regressed = i < 2 ? current[i] > counts[i] : current[i] > counts[i] + band; + + char column[32]; + if ( i < 2 ) + { + snprintf( column, sizeof( column ), i == 0 ? "tunnels" : "awake" ); + } + else + { + snprintf( column, sizeof( column ), "> %g slop", g_meshDropDepthLevels[i - 2] ); + } + + printf( " %-9s %-11s %d>%d%s\n", shapeName, column, counts[i], current[i], regressed ? " GREW" : "" ); + changedCount += 1; + result = regressed ? 1 : result; + } + } + + fclose( file ); + + if ( matchedCount == 0 ) + { + printf( "%s has no rows for -s=%s -b=%u -g=%d\n", path, GetFilterName( soak->shapeFilter ), soak->baseSeed, + soak->generationCount ); + return 2; + } + + printf( "compared with %s: %d changed, %s\n", path, changedCount, result == 0 ? "none grew past noise" : "GREW" ); + return result; +} + +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; +} + +int main( int argc, char** argv ) +{ + int generationCount = 1000; + int threadCount = 8; + int shapeFilter = e_meshDropBox; + uint32_t seed = (uint32_t)b3GetTicks(); + bool fixedSeed = false; + bool recycling = true; + const char* writePath = NULL; + const char* comparePath = NULL; + MeshDropSettings settings = GetDefaultMeshDropSettings(); + + for ( int i = 1; i < argc; ++i ) + { + const char* arg = argv[i]; + const char* value; + + if ( ( value = MatchValue( arg, "-g=", "--generations=" ) ) != NULL ) + { + generationCount = atoi( value ); + } + else if ( ( value = MatchValue( arg, "-t=", "--threads=" ) ) != NULL ) + { + threadCount = b3ClampInt( atoi( value ), 1, MAX_THREADS ); + } + else if ( ( value = MatchValue( arg, "-s=", "--shape=" ) ) != NULL ) + { + shapeFilter = -2; + for ( int k = 0; k < e_meshDropShapeCount; ++k ) + { + if ( strcmp( value, g_meshDropShapeNames[k] ) == 0 ) + { + shapeFilter = k; + } + } + + if ( strcmp( value, "all" ) == 0 ) + { + shapeFilter = -1; + } + + if ( shapeFilter == -2 ) + { + printf( "unknown shape: %s\n", value ); + return 2; + } + } + else if ( ( value = MatchValue( arg, "-seed=", "--seed=" ) ) != NULL ) + { + seed = (uint32_t)strtoul( value, NULL, 10 ); + fixedSeed = true; + } + else if ( ( value = MatchValue( arg, "-b=", "--base-seed=" ) ) != NULL ) + { + seed = (uint32_t)strtoul( value, NULL, 10 ); + } + else if ( ( value = MatchValue( arg, "-a=", "--amplitude=" ) ) != NULL ) + { + settings.amplitude = (float)atof( value ); + } + else if ( ( value = MatchValue( arg, "-w=", "--write=" ) ) != NULL ) + { + writePath = value; + } + else if ( ( value = MatchValue( arg, "-c=", "--compare=" ) ) != NULL ) + { + comparePath = value; + } + else if ( strcmp( arg, "--no-recycling" ) == 0 ) + { + recycling = false; + } + else + { + printf( "Usage: mesh_soak [options]\n" + "Drops %d bodies per generation on the Mesh Drop wave mesh until they sleep.\n" + "Fails on a body below y = -2 (tunnel) or a pile awake past the step limit.\n" + "Counts bodies that sink into the wave deeper than each depth level.\n" + " -g, --generations= generation count (default 1000)\n" + " -t, --threads= worker threads, one world each (default 8)\n" + " -s, --shape= box, capsule, cylinder, sphere or all (default box)\n" + " -b, --base-seed= base seed, each generation hashes its own seed from it (default random)\n" + " --seed= run every generation with this exact seed (replay)\n" + " -a, --amplitude= wave amplitude (default 0.5)\n" + " -w, --write= write the counts as a baseline, replacing only the rows of this run\n" + " -c, --compare= compare the counts with the baseline rows from the same -s, -b and -g\n" + " --no-recycling disable contact recycling\n" + "Exit code is 0 when every generation passes and no count grew past noise, 1 otherwise,\n" + "2 for bad arguments or a baseline from a different run.\n", + MESH_DROP_BODY_COUNT ); + return 2; + } + } + + if ( fixedSeed && ( writePath != NULL || comparePath != NULL ) ) + { + printf( "a baseline needs a base seed, not --seed\n" ); + return 2; + } + + Soak soak = { 0 }; + soak.mutex = b3CreateMutex(); + soak.settings = settings; + soak.recycling = recycling; + soak.groundMesh = CreateMeshDropMesh( settings.amplitude ); + soak.cylinder = CreateMeshDropCylinder(); + soak.baseSeed = seed; + soak.fixedSeed = fixedSeed; + soak.generationCount = generationCount; + soak.shapeFilter = shapeFilter; + + for ( int i = 0; i < e_meshDropShapeCount; ++i ) + { + soak.stats[i].maxDepth = -FLT_MAX; + soak.stats[i].maxGeneration = INT32_MAX; + } + + printf( "mesh soak: %d generations, %d threads, shape %s, %s seed %u\n", generationCount, threadCount, + GetFilterName( shapeFilter ), fixedSeed ? "fixed" : "base", seed ); + fflush( stdout ); + + uint64_t ticks = b3GetTicks(); + + b3Thread* threads[MAX_THREADS]; + for ( int i = 0; i < threadCount; ++i ) + { + threads[i] = b3CreateThread( SoakWorker, &soak, "soak" ); + } + + for ( int i = 0; i < threadCount; ++i ) + { + b3JoinThread( threads[i] ); + } + + float seconds = 0.001f * b3GetMilliseconds( ticks ); + double averageSteps = soak.completedCount > 0 ? (double)soak.totalSteps / soak.completedCount : 0.0; + float slop = B3_LINEAR_SLOP; + + printf( "\n%-9s %8s %8s %6s", "shape", "bodies", "tunnels", "awake" ); + for ( int i = 0; i < MESH_DROP_DEPTH_LEVEL_COUNT; ++i ) + { + char label[32]; + snprintf( label, sizeof( label ), "> %g slop", g_meshDropDepthLevels[i] ); + printf( " %10s", label ); + } + printf( " deepest\n" ); + + for ( int shape = 0; shape < e_meshDropShapeCount; ++shape ) + { + const ShapeStats* stats = soak.stats + shape; + if ( stats->generationCount == 0 ) + { + continue; + } + + printf( "%-9s %8d %8d %6d", g_meshDropShapeNames[shape], stats->generationCount * MESH_DROP_BODY_COUNT, + stats->tunnelCount, stats->awakeCount ); + for ( int i = 0; i < MESH_DROP_DEPTH_LEVEL_COUNT; ++i ) + { + printf( " %10d", stats->depthCounts[i] ); + } + + if ( stats->maxIndex >= 0 ) + { + printf( " %.1f slop, seed %u body (%d, %d) step %d\n", stats->maxDepth / slop, stats->maxSeed, + stats->maxIndex / MESH_DROP_GRID_COUNT, stats->maxIndex % MESH_DROP_GRID_COUNT, stats->maxStep ); + } + else + { + printf( "\n" ); + } + } + + printf( "\nsteps to sleep: average %.0f, max %d, time %.1f s\n", averageSteps, soak.maxSteps, seconds ); + + int reported = b3MinInt( soak.failureCount, MAX_REPORTED_FAILURES ); + for ( int i = 0; i < reported; ++i ) + { + Failure* f = soak.failures + i; + printf( " %s %s seed %u body (%d, %d)\n", f->kind == e_tunnel ? "tunnel" : "awake", g_meshDropShapeNames[f->shape], + f->seed, f->index / MESH_DROP_GRID_COUNT, f->index % MESH_DROP_GRID_COUNT ); + } + + int exitCode = soak.failureCount == 0 ? 0 : 1; + + if ( writePath != NULL ) + { + WriteBaseline( &soak, writePath ); + } + + if ( comparePath != NULL ) + { + int compareResult = CompareBaseline( &soak, comparePath ); + exitCode = b3MaxInt( exitCode, compareResult ); + } + + printf( exitCode == 0 ? "PASS\n" : "FAIL\n" ); + + b3DestroyHull( soak.cylinder ); + b3DestroyMesh( soak.groundMesh ); + b3DestroyMutex( soak.mutex ); + + return exitCode; +} diff --git a/benchmark/soak_baseline.csv b/benchmark/soak_baseline.csv new file mode 100644 index 0000000..7db4e34 --- /dev/null +++ b/benchmark/soak_baseline.csv @@ -0,0 +1,9 @@ +filter,base,generations,shape,tunnels,awake,depth4,depth8,depth16,depth32 +all,12345,1000,box,0,0,1538,220,3,0 +all,12345,1000,capsule,0,0,14507,3488,28,0 +all,12345,1000,cylinder,0,0,11210,2353,37,0 +all,12345,1000,sphere,0,0,56,0,0,0 +all,12345,16000,box,0,0,23676,3191,44,0 +all,12345,16000,capsule,0,0,231496,54552,515,0 +all,12345,16000,cylinder,0,0,178981,37441,480,0 +all,12345,16000,sphere,0,0,987,0,0,0 diff --git a/docs/images/build.sh b/docs/images/build.sh new file mode 100644 index 0000000..068f65c --- /dev/null +++ b/docs/images/build.sh @@ -0,0 +1,64 @@ +#!/bin/bash +# Usage: ./build.sh [--force] [name.asy ...] +# https://asymptote.sourceforge.io/doc/colors.png + +asy="/c/Program Files/Asymptote/asy.exe" +if [ ! -x "$asy" ]; then + echo "missing $asy" + exit 1 +fi + +cd "$(dirname "$0")" || exit 1 + +force=0 +sources=() +for arg in "$@"; do + case "$arg" in + --force) force=1 ;; + *) sources+=("$arg") ;; + esac +done + +if [ ${#sources[@]} -eq 0 ]; then + sources=(*.asy) +fi + +# asy can exit 0 after a failed TeX run, so judge success by a fresh svg +marker=$(mktemp) +trap 'rm -f "$marker"' EXIT + +status=0 +failed=0 +for src in "${sources[@]}"; do + name=$(basename "$src" .asy) + svg="$name.svg" + + # Never clobber a hand drawn svg of the same name, delete it first to replace it + if [ -f "$svg" ] && ! head -c 300 "$svg" | grep -q dvisvgm; then + echo "skip $src: $svg was not made by asy" + status=1 + continue + fi + + if [ $force -eq 0 ] && [ "$svg" -nt "$src" ]; then + echo "fresh $svg" + continue + fi + + touch "$marker" + "$asy" -noV -o "$name" "$src" + if [ "$svg" -nt "$marker" ]; then + echo "built $svg" + else + echo "failed $src" + status=1 + failed=1 + fi +done + +# Leftovers from asy's TeX pipe. Keep the log after a failure, it has the TeX error. +rm -f texput.aux +if [ $failed -eq 0 ]; then + rm -f texput.log +fi +exit $status diff --git a/docs/images/saddle_region.asy b/docs/images/saddle_region.asy new file mode 100644 index 0000000..f4ad105 --- /dev/null +++ b/docs/images/saddle_region.asy @@ -0,0 +1,267 @@ +settings.outformat = "svg"; +settings.render = 0; +settings.prc = false; + +import three; + +real px = 0.75bp; + +pen ink = rgb("231f20"); +pen meshPen = ink + 0.6px; +pen regionFill = rgb("f2d4d0"); +pen regionPen = rgb("a8322a") + 1.0px; +pen ridgeColor = rgb("c26a17"); +pen gullyColor = rgb("1f5c99"); +pen sliceFill = gray(0.93) + opacity(0.75); +pen guidePen = gray(0.55) + 0.5px + linetype(new real[] {4, 3}); +pen hullPen = ink + 0.7px + linetype(new real[] {5, 3}); + +real arrowSize = 5px; +triple lightDirection = unit((1, 4, 2)); + +// The Saddle Vertex sample hub, spokes alternate 8 degree ridges and 32 degree gullies +int spokeCount = 6; +real fanRadius = 1.5; +real[] drops = {8, 32, 8, 32, 8, 32}; + +// The slice plane sits this far above the hub. Planes through the hub cut it in lines. +real sliceHeight = 1.6; +real sliceRadius = sliceHeight * Tan(40); + +triple[] spokes; +for (int i = 0; i < spokeCount; ++i) +{ + real angle = 60 * i; + spokes.push(fanRadius * (Cos(angle), -Tan(drops[i]), Sin(angle))); +} + +bool IsRidge(int i) +{ + return i % 2 == 0; +} + +pen SpokePen(int i) +{ + return IsRidge(i) ? ridgeColor : gullyColor; +} + +// Gnomonic coordinates: a normal n maps to (n.x, n.z) / n.y on the slice, so every plane through +// the hub maps to a line and n . e <= 0 maps to a half plane +pair Gnomonic(triple n) +{ + return (n.x / n.y, n.z / n.y); +} + +// Half plane of spoke i on the unit slice: dot(p, u) <= offset +pair SpokeDirection(int i) +{ + return (Cos(60 * i), Sin(60 * i)); +} + +real SpokeOffset(int i) +{ + return Tan(drops[i]); +} + +// Sutherland Hodgman clip of a convex polygon against dot(p, u) <= offset +pair[] ClipPolygon(pair[] poly, pair u, real offset) +{ + pair[] result; + int n = poly.length; + for (int k = 0; k < n; ++k) + { + pair a = poly[k]; + pair b = poly[(k + 1) % n]; + real da = dot(a, u) - offset; + real db = dot(b, u) - offset; + if (da <= 0) + result.push(a); + if (da * db < 0) + result.push(a + da / (da - db) * (b - a)); + } + return result; +} + +pair[] region = { (-10, -10), (10, -10), (10, 10), (-10, 10) }; +for (int i = 0; i < spokeCount; ++i) + region = ClipPolygon(region, SpokeDirection(i), SpokeOffset(i)); + +// Each face normal is perpendicular to its two spokes, so it lies on both of their lines +pair[] faceNormals; +for (int i = 0; i < spokeCount; ++i) +{ + triple n = cross(spokes[(i + 1) % spokeCount], spokes[i]); + if (n.y < 0) + n = -n; + faceNormals.push(Gnomonic(n)); +} + +// A face normal leans past the gully it borders, so sort by angle before drawing the hull +pair[] hullPoints = sort(faceNormals, new bool(pair a, pair b) { return degrees(a, false) < degrees(b, false); }); + +// End points of the line of spoke i, clipped to a disk of the given radius on the unit slice +pair[] SpokeLine(int i, real radius) +{ + pair u = SpokeDirection(i); + pair t = (-u.y, u.x); + real c = SpokeOffset(i); + real half = sqrt(radius * radius - c * c); + return new pair[] { c * u - half * t, c * u + half * t }; +} + +pen Shade(path3 face) +{ + triple n = unit(cross(point(face, 1) - point(face, 0), point(face, 2) - point(face, 0))); + real k = 0.74 + 0.26 * abs(dot(n, lightDirection)); + return rgb(k * 0.843, k * 0.890, k * 0.957); +} + +triple Lift(pair p) +{ + return sliceHeight * (p.x, 1, p.y); +} + +frame[] panels; +string[] titles; + +// 3D view: the fan, the slice above it and the cone of the region +{ + currentprojection = orthographic((3.4, 3.0, 2.6), up = Y); + picture pic; + size(pic, 250px, 250px); + triple hub = (0, 0, 0); + + for (int i = 0; i < spokeCount; ++i) + { + path3 t = hub -- spokes[i] -- spokes[(i + 1) % spokeCount] -- cycle; + draw(pic, surface(t), Shade(t), nolight); + draw(pic, t, meshPen); + } + + triple offset[] = {(0.1, 0, 0.1), (0.3, 0, 0.3), (0, 0, 0), (-0.1, 0, 0), (0, 0, -0.3), (0, 0, 0)}; + for (int i = 0; i < spokeCount; ++i) + { + draw(pic, hub -- spokes[i], SpokePen(i) + 1.4px); + label(pic, "$e_" + string(i) + "$", spokes[i] + offset[i], unit(Gnomonic((spokes[i].x, 1, 4 * spokes[i].z))), SpokePen(i)); + } + + // The boundary plane of one ridge spoke, it holds the hub and its line on the slice + pair[] line0 = SpokeLine(0, Tan(40)); + path3 wall = hub -- Lift(line0[0]) -- Lift(line0[1]) -- cycle; + draw(pic, surface(wall), ridgeColor + opacity(0.18), nolight); + draw(pic, hub -- Lift(line0[0]), ridgeColor + 0.5px + linetype(new real[] {4, 3})); + draw(pic, hub -- Lift(line0[1]), ridgeColor + 0.5px + linetype(new real[] {4, 3})); + + path3 slice = shift((0, sliceHeight, 0)) * rotate(90, X) * scale3(sliceRadius) * unitcircle3; + draw(pic, surface(slice), sliceFill, nolight); + draw(pic, slice, gray(0.6) + 0.5px); + + for (int i = 0; i < spokeCount; ++i) + { + pair[] ends = SpokeLine(i, Tan(40)); + draw(pic, Lift(ends[0]) -- Lift(ends[1]), SpokePen(i) + 0.8px); + } + + path3 top = Lift(region[0]); + for (int k = 1; k < region.length; ++k) + top = top -- Lift(region[k]); + top = top -- cycle; + draw(pic, surface(top), regionFill, nolight); + draw(pic, top, regionPen); + + for (pair p : region) + draw(pic, hub -- Lift(p), regionPen); + + dot(pic, hub, ink + 3px); + label(pic, "slice", Lift((0, -Tan(40))) + (0, 0.1, 0), N, gray(0.35)); + + panels.push(pic.fit()); + titles.push("Saddle Vertex Voronoi"); +} + +// Top view of the slice, in units of tan(tilt) +{ + picture pic; + unitsize(pic, 190px); + real radius = Tan(40); + + // Looking down on the slice: x to the right, z down the page + pair View(pair p) + { + return (p.x, -p.y); + } + + path hull = View(hullPoints[0]); + for (int i = 1; i < spokeCount; ++i) + hull = hull -- View(hullPoints[i]); + hull = hull -- cycle; + fill(pic, hull, gray(0.92)); + + // Excluded side of each line, a band inside the disk + real band = 0.035; + for (int i = 0; i < spokeCount; ++i) + { + pair u = SpokeDirection(i); + pair[] ends = SpokeLine(i, radius - band); + path strip = View(ends[0]) -- View(ends[1]) -- View(ends[1] + band * u) -- View(ends[0] + band * u) -- cycle; + fill(pic, strip, SpokePen(i) + opacity(0.25)); + } + + for (int i = 0; i < spokeCount; ++i) + { + pair[] ends = SpokeLine(i, radius); + draw(pic, View(ends[0]) -- View(ends[1]), SpokePen(i) + 1.0px); + pair u = SpokeDirection(i); + label(pic, "$e_" + string(i) + "$", View((SpokeOffset(i) + (IsRidge(i) ? 0.11 : 0.075)) * u), SpokePen(i)); + } + + draw(pic, hull, hullPen); + + path area = View(region[0]); + for (int k = 1; k < region.length; ++k) + area = area -- View(region[k]); + area = area -- cycle; + filldraw(pic, area, regionFill, regionPen); + + // Tilt from up, drawn over the fills so they read through the region + for (real tilt : new real[] {10, 20, 30, 40}) + { + draw(pic, scale(Tan(tilt)) * unitcircle, tilt == 40 ? gray(0.6) + 0.5px : guidePen); + label(pic, string(tilt) + "$^\circ$", Tan(tilt) * dir(90), N, gray(0.45) + fontsize(7pt)); + } + + for (pair n : faceNormals) + dot(pic, View(n), ink + 3.5px); + label(pic, "face normals", View(faceNormals[1]), NE, ink + fontsize(8pt)); + + dot(pic, (0, 0), ink + 2.5px); + label(pic, "up", (0, 0), SW, ink + fontsize(8pt)); + + frame f = pic.fit(); + panels.push(f); + titles.push("Top View"); +} + +real gap = 36px; +real x = 0; +real bottom = 0; +for (int i = 0; i < panels.length; ++i) +{ + frame f = panels[i]; + pair lo = min(f); + pair hi = max(f); + add(shift(x - lo.x, -lo.y) * f); + label(titles[i], (x + 0.5 * (hi.x - lo.x), -8px), S, ink); + x += hi.x - lo.x + gap; +} + +// Legend +pair legend = (0, -40px); +draw((legend.x, legend.y) -- (legend.x + 18px, legend.y), ridgeColor + 1.4px); +label("ridge spoke, 8$^\circ$ drop", (legend.x + 22px, legend.y), E, ink + fontsize(8pt)); +draw((legend.x + 150px, legend.y) -- (legend.x + 168px, legend.y), gullyColor + 1.4px); +label("gully spoke, 32$^\circ$ drop", (legend.x + 172px, legend.y), E, ink + fontsize(8pt)); +filldraw(shift(legend.x + 300px, legend.y - 5px) * scale(10px) * unitsquare, regionFill, regionPen); +label("vertex region", (legend.x + 314px, legend.y), E, ink + fontsize(8pt)); +draw((legend.x + 410px, legend.y) -- (legend.x + 428px, legend.y), hullPen); +label("cone of face normals", (legend.x + 432px, legend.y), E, ink + fontsize(8pt)); diff --git a/docs/images/saddle_region.svg b/docs/images/saddle_region.svg new file mode 100644 index 0000000..d2c1307 --- /dev/null +++ b/docs/images/saddle_region.svg @@ -0,0 +1,442 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/docs/images/vertex_regions.asy b/docs/images/vertex_regions.asy new file mode 100644 index 0000000..07c8975 --- /dev/null +++ b/docs/images/vertex_regions.asy @@ -0,0 +1,323 @@ +settings.outformat = "svg"; +settings.render = 0; +settings.prc = false; + +import three; + +real px = 0.75bp; + +pen ink = rgb("231f20"); +pen meshPen = ink + 0.6px; +// Half opacity over white matches the opaque f2d4d0 of the other figures +pen regionFill = rgb("e5a9a1") + opacity(0.5); +pen regionShade = rgb("7a1f18"); +pen regionPen = rgb("a8322a") + 0.9px; +pen rayPen = rgb("a8322a") + 0.6px; +pen spokePen = rgb("1f5c99") + 1.0px; +pen cliffFill = gray(0.88) + opacity(0.6); +pen guidePen = gray(0.45) + 0.6px + linetype(new real[] {4, 3}); + +real panelSize = 170px; +real capRadius = 0.55; +real arrowSize = 5px; + +triple lightDirection = unit((1, 4, 2)); + +// Extreme rays of the cone of normals n with n . d <= 0 for every direction d +triple[] RegionRays(triple[] d) +{ + triple[] rays; + for (int i = 0; i < d.length; ++i) + { + for (int j = i + 1; j < d.length; ++j) + { + triple c = cross(d[i], d[j]); + if (abs(c) < 1e-6) + continue; + + for (real s : new real[] {1, -1}) + { + triple r = s * unit(c); + bool inside = true; + for (triple e : d) + inside = inside && dot(r, unit(e)) <= 1e-6; + + bool duplicate = false; + for (triple q : rays) + duplicate = duplicate || dot(r, q) > 1 - 1e-6; + + if (inside && !duplicate) + rays.push(r); + } + } + } + + triple axis = (0, 0, 0); + for (triple r : rays) + axis += r; + axis = unit(axis); + + triple t1 = unit(cross(axis, abs(axis.x) < 0.9 ? X : Y)); + triple t2 = cross(axis, t1); + real[] angles; + for (triple r : rays) + angles.push(atan2(dot(r, t2), dot(r, t1))); + + int[] order = sequence(rays.length); + order = sort(order, new bool(int a, int b) { return angles[a] < angles[b]; }); + triple[] sorted; + for (int k : order) + sorted.push(rays[k]); + return sorted; +} + +triple Arc(triple a, triple b, real t) +{ + return unit((1 - t) * a + t * b); +} + +// Extreme rays of the region, drawn as arrows leaving the rim +void DrawRayTips(picture pic, triple v, triple[] rays, real radius) +{ + for (triple r : rays) + draw(pic, (v + radius * r) -- (v + (radius + 0.3) * r), regionPen, Arrow3(arrowSize)); +} + +// Convex hull by monotone chain +pair[] ConvexHull(pair[] points) +{ + pair[] p = sort(points, new bool(pair a, pair b) { return a.x < b.x || (a.x == b.x && a.y < b.y); }); + pair[] hull; + for (int pass = 0; pass < 2; ++pass) + { + int base = hull.length; + for (pair q : p) + { + while (hull.length >= base + 2 && cross(hull[hull.length - 1] - hull[hull.length - 2], q - hull[hull.length - 2]) <= 0) + hull.pop(); + hull.push(q); + } + hull.pop(); + p = reverse(p); + } + return hull; +} + +// The region is convex, so its outline is the hull of its projected points. Filling that once +// avoids the seams that overlapping translucent patches would show. +void FillProjected(picture pic, triple v, triple[] points) +{ + triple view = unit(currentprojection.camera - currentprojection.target); + triple u1 = unit(cross(currentprojection.up, view)); + triple u2 = cross(view, u1); + + pair[] flat; + for (triple q : points) + flat.push((dot(q - v, u1), dot(q - v, u2))); + + pair[] hull = ConvexHull(flat); + path3 outline = v + hull[0].x * u1 + hull[0].y * u2; + for (int k = 1; k < hull.length; ++k) + outline = outline -- (v + hull[k].x * u1 + hull[k].y * u2); + draw(pic, surface(outline -- cycle, planar = true), regionFill, nolight); +} + +void DrawRegion(picture pic, triple v, triple[] rays, real radius) +{ + int arcSteps = 12; + int n = rays.length; + + triple axis = (0, 0, 0); + for (triple r : rays) + axis += r; + axis = unit(axis); + + triple[] points = { v }; + path3 rim; + for (int i = 0; i < n; ++i) + { + if (n == 2 && i == 1) + break; + + triple a = rays[i]; + triple b = rays[(i + 1) % n]; + for (int k = 0; k <= arcSteps; ++k) + { + triple p = v + radius * Arc(a, b, k / arcSteps); + rim = i == 0 && k == 0 ? p : rim -- p; + points.push(p); + + // Interior points for the silhouette of the spherical cap + for (int j = 1; j < 6 && n > 2; ++j) + points.push(v + radius * Arc(axis, Arc(a, b, k / arcSteps), j / 6)); + } + } + + FillProjected(pic, v, points); + + // Darken the flat sides with the same light as the mesh. Sides on the near half of the cone never + // overlap each other in projection, nor do sides on the far half, so each adds one layer. + for (int i = 0; i < n; ++i) + { + if (n == 2 && i == 1) + break; + + triple a = rays[i]; + triple b = rays[(i + 1) % n]; + triple side = unit(cross(a, b)); + + path3 sector = v + radius * a; + for (int k = 1; k <= arcSteps; ++k) + sector = sector -- (v + radius * Arc(a, b, k / arcSteps)); + sector = v -- sector -- cycle; + + real shade = 1 - abs(dot(side, lightDirection)); + draw(pic, surface(sector, planar = true), regionShade + opacity(0.35 * shade), nolight); + } + + draw(pic, n == 2 ? rim : rim -- cycle, regionPen); + for (triple r : rays) + draw(pic, v -- (v + radius * r), rayPen); + DrawRayTips(pic, v, rays, radius); +} + +// Flat shading in world space, so every panel is lit the same way +pen Shade(path3 face) +{ + triple n = unit(cross(point(face, 1) - point(face, 0), point(face, 2) - point(face, 0))); + real k = 0.74 + 0.26 * abs(dot(n, lightDirection)); + return rgb(k * 0.843, k * 0.890, k * 0.957); +} + +void DrawFan(picture pic, triple v, triple[] rim, bool closed) +{ + int count = closed ? rim.length : rim.length - 1; + for (int i = 0; i < count; ++i) + { + path3 t = v -- rim[i] -- rim[(i + 1) % rim.length] -- cycle; + draw(pic, surface(t), Shade(t), nolight); + draw(pic, t, meshPen); + } +} + +void DrawSpokes(picture pic, triple v, triple[] ends) +{ + for (triple e : ends) + draw(pic, v -- (v + 0.45 * (e - v)), spokePen, Arrow3(arrowSize)); +} + +frame[] panels; +string[] titles; + +// Box corner, three faces meet at a convex vertex +{ + currentprojection = orthographic((3.5, 2.6, 2.8), up = Y); + picture pic; + size(pic, panelSize, panelSize); + triple v = (0, 0, 0); + + path3[] faces = { + (0, 0, 0) -- (0, -1, 0) -- (0, -1, -1) -- (0, 0, -1) -- cycle, + (0, 0, 0) -- (0, 0, -1) -- (-1, 0, -1) -- (-1, 0, 0) -- cycle, + (0, 0, 0) -- (-1, 0, 0) -- (-1, -1, 0) -- (0, -1, 0) -- cycle, + }; + for (path3 f : faces) + { + draw(pic, surface(f), Shade(f), nolight); + draw(pic, f, meshPen); + } + + triple[] ends = { (-1, 0, 0), (0, -1, 0), (0, 0, -1) }; + DrawSpokes(pic, v, ends); + DrawRegion(pic, v, RegionRays(ends), capRadius); + dot(pic, v, ink + 3px); + + panels.push(pic.fit()); + titles.push("convex corner"); +} + +// A vertex on a ridge, the region collapses to a flat wedge +{ + currentprojection = orthographic((3.0, 2.4, 2.2), up = Y); + picture pic; + size(pic, panelSize, panelSize); + triple v = (0, 0, 0); + real h = tan(30 * pi / 180); + + triple[] rim = { (1, 0, 0), (1, -h, 1), (-1, -h, 1), (-1, 0, 0), (-1, -h, -1), (1, -h, -1) }; + DrawFan(pic, v, rim, true); + DrawSpokes(pic, v, rim); + DrawRegion(pic, v, RegionRays(rim), capRadius); + dot(pic, v, ink + 3px); + + panels.push(pic.fit()); + titles.push("ridge"); +} + +// Saddle hub, spokes alternate 8 degree ridges and 32 degree gullies +{ + currentprojection = orthographic((2.0, 4.0, 3.0), up = Y); + picture pic; + size(pic, panelSize, panelSize); + triple v = (0, 0, 0); + + triple[] rim; + for (int i = 0; i < 6; ++i) + { + real angle = i * 60; + real drop = i % 2 == 1 ? 32 : 8; + rim.push((Cos(angle), -Tan(drop), Sin(angle))); + } + + DrawFan(pic, v, rim, true); + DrawSpokes(pic, v, rim); + DrawRegion(pic, v, RegionRays(rim), 1.2); + dot(pic, v, ink + 3px); + + panels.push(pic.fit()); + titles.push("saddle hub"); +} + +// Corner of an open sheet, the cliffs below the boundary edges close the region +{ + currentprojection = orthographic((3.5, 2.6, 2.8), up = Y); + picture pic; + size(pic, panelSize, panelSize); + triple v = (0, 0, 0); + real depth = 0.6; + + path3[] cliffs = { + (0, 0, 0) -- (0, 0, -1) -- (0, -depth, -1) -- (0, -depth, 0) -- cycle, + (0, 0, 0) -- (-1, 0, 0) -- (-1, -depth, 0) -- (0, -depth, 0) -- cycle, + }; + for (path3 c : cliffs) + { + draw(pic, surface(c), cliffFill, nolight); + draw(pic, c, guidePen); + } + + triple[] rim = { (-1, 0, 0), (-1, 0, -1), (0, 0, -1) }; + DrawFan(pic, v, rim, false); + DrawSpokes(pic, v, rim); + + triple[] directions = copy(rim); + directions.push((0, -1, 0)); + DrawRegion(pic, v, RegionRays(directions), capRadius); + dot(pic, v, ink + 3px); + label(pic, "cliff", (0, -depth, -0.5), S, gray(0.35)); + + panels.push(pic.fit()); + titles.push("open corner"); +} + +real gap = 24px; +real x = 0; +for (int i = 0; i < panels.length; ++i) +{ + frame f = panels[i]; + pair lo = min(f); + pair hi = max(f); + add(shift(x - lo.x, -lo.y) * f); + label(titles[i], (x + 0.5 * (hi.x - lo.x), -8px), S, ink); + x += hi.x - lo.x + gap; +} diff --git a/docs/images/vertex_regions.svg b/docs/images/vertex_regions.svg new file mode 100644 index 0000000..e192437 --- /dev/null +++ b/docs/images/vertex_regions.svg @@ -0,0 +1,1050 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/include/box3d/box3d.h b/include/box3d/box3d.h index d9f5fdf..c24c542 100644 --- a/include/box3d/box3d.h +++ b/include/box3d/box3d.h @@ -80,14 +80,13 @@ B3_API b3ContactEvents b3World_GetContactEvents( b3WorldId worldId ); B3_API b3JointEvents b3World_GetJointEvents( b3WorldId worldId ); /// Overlap test for all shapes that *potentially* overlap the provided AABB -B3_API b3TreeStats b3World_OverlapAABB( b3WorldId worldId, b3AABB aabb, b3QueryFilter filter, - b3OverlapResultFcn* fcn, void* context ); +B3_API b3TreeStats b3World_OverlapAABB( b3WorldId worldId, b3AABB aabb, b3QueryFilter filter, b3OverlapResultFcn* fcn, + void* context ); /// Overlap test for all shapes that overlap the provided shape proxy. The proxy points are relative /// to the world origin, which lets the query stay precise far from the world origin. B3_API b3TreeStats b3World_OverlapShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, - b3QueryFilter filter, b3OverlapResultFcn* fcn, - void* context ); + b3QueryFilter filter, b3OverlapResultFcn* fcn, void* context ); /// Cast a ray into the world to collect shapes in the path of the ray. /// Your callback function controls whether you get the closest point, any point, or n-points. @@ -100,23 +99,21 @@ B3_API b3TreeStats b3World_OverlapShape( b3WorldId worldId, b3Pos origin, const /// @param fcn A user implemented callback function /// @param context A user context that is passed along to the callback function /// @return traversal performance counters -B3_API b3TreeStats b3World_CastRay( b3WorldId worldId, b3Pos origin, b3Vec3 translation, - b3QueryFilter filter, b3CastResultFcn* fcn, void* context ); +B3_API b3TreeStats b3World_CastRay( b3WorldId worldId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter, + b3CastResultFcn* fcn, void* context ); /// Cast a ray into the world to collect the closest hit. This is a convenience function. Ignores /// initial overlap. This is less general than b3World_CastRay() and does not allow for custom /// filtering. Ignores back-side collision on meshes and height-fields. -B3_API b3RayResult b3World_CastRayClosest( b3WorldId worldId, b3Pos origin, b3Vec3 translation, - b3QueryFilter filter ); +B3_API b3RayResult b3World_CastRayClosest( b3WorldId worldId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter ); /// Cast a shape through the world. Similar to a cast ray except that a shape is cast instead of a /// point. The proxy points are relative to the origin and the hit points come back as world /// positions, so the cast stays precise far from the world origin. Ignores back-side collision on /// meshes and height-fields. /// @see b3World_CastRay -B3_API b3TreeStats b3World_CastShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, - b3Vec3 translation, b3QueryFilter filter, - b3CastResultFcn* fcn, void* context ); +B3_API b3TreeStats b3World_CastShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation, + b3QueryFilter filter, b3CastResultFcn* fcn, void* context ); /// Cast a capsule mover through the world. This is a special shape cast that handles sliding along /// other shapes while reducing clipping. This is not a good source of information about what the @@ -130,15 +127,14 @@ B3_API b3TreeStats b3World_CastShape( b3WorldId worldId, b3Pos origin, const b3S /// @param fcn Optional callback for custom shape filtering /// @param context A user context that is passed along to the callback function /// @return the translation fraction -B3_API float b3World_CastMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mover, - b3Vec3 translation, b3QueryFilter filter, b3MoverFilterFcn* fcn, - void* context ); +B3_API float b3World_CastMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mover, b3Vec3 translation, + b3QueryFilter filter, b3MoverFilterFcn* fcn, void* context ); /// Collide a capsule mover with the world, gathering collision planes that can be fed to /// b3SolvePlanes. Useful for kinematic character movement. The mover and the returned planes are /// relative to the origin. Ignores back-side collision on meshes and height-fields. -B3_API void b3World_CollideMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mover, - b3QueryFilter filter, b3PlaneResultFcn* fcn, void* context ); +B3_API void b3World_CollideMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mover, b3QueryFilter filter, + b3PlaneResultFcn* fcn, void* context ); /// Enable/disable sleep. If your application does not need sleeping, you can gain some performance /// by disabling sleep completely at the world level. @@ -187,8 +183,7 @@ B3_API void b3World_SetHitEventThreshold( b3WorldId worldId, float value ); B3_API float b3World_GetHitEventThreshold( b3WorldId worldId ); /// Register the custom filter callback. This is optional. -B3_API void b3World_SetCustomFilterCallback( b3WorldId worldId, b3CustomFilterFcn* fcn, - void* context ); +B3_API void b3World_SetCustomFilterCallback( b3WorldId worldId, b3CustomFilterFcn* fcn, void* context ); /// Register the pre-solve callback. This is optional. B3_API void b3World_SetPreSolveCallback( b3WorldId worldId, b3PreSolveFcn* fcn, void* context ); @@ -212,8 +207,7 @@ B3_API void b3World_Explode( b3WorldId worldId, const b3ExplosionDef* explosionD /// @param dampingRatio The contact bounciness with 1 being critical damping (non-dimensional) /// @param contactSpeed The maximum contact constraint push out speed (meters per second) /// @note Advanced feature -B3_API void b3World_SetContactTuning( b3WorldId worldId, float hertz, float dampingRatio, - float contactSpeed ); +B3_API void b3World_SetContactTuning( b3WorldId worldId, float hertz, float dampingRatio, float contactSpeed ); /// Set the contact point recycling distance. Setting this to zero disables contact point recycling. /// Usually in meters. @@ -421,8 +415,7 @@ B3_API void b3RecPlayer_SetWorkerCount( b3RecPlayer* player, int count ); /// @param minIntervalFrames finest spacing between keyframes, in frames /// A zero budget or a non-positive interval keeps that value. Clears the existing ring, so call /// b3RecPlayer_Restart afterward to repopulate it under the new policy. -B3_API void b3RecPlayer_SetKeyframePolicy( b3RecPlayer* player, size_t budgetBytes, - int minIntervalFrames ); +B3_API void b3RecPlayer_SetKeyframePolicy( b3RecPlayer* player, size_t budgetBytes, int minIntervalFrames ); /// @return the keyframe memory budget in bytes B3_API size_t b3RecPlayer_GetKeyframeBudget( const b3RecPlayer* player ); @@ -453,10 +446,8 @@ B3_API b3BodyId b3RecPlayer_GetBodyId( const b3RecPlayer* player, int index ); /// @param createDebugShape called when a replayed shape is added; returns a user draw handle /// @param destroyDebugShape called when a replayed shape is removed; may be NULL /// @param context user context passed to both callbacks -B3_API void b3RecPlayer_SetDebugShapeCallbacks( b3RecPlayer* player, - b3CreateDebugShapeCallback* createDebugShape, - b3DestroyDebugShapeCallback* destroyDebugShape, - void* context ); +B3_API void b3RecPlayer_SetDebugShapeCallbacks( b3RecPlayer* player, b3CreateDebugShapeCallback* createDebugShape, + b3DestroyDebugShapeCallback* destroyDebugShape, void* context ); /// Draw the spatial queries recorded during the most recently replayed frame, layered on top of the /// world. Call after b3World_Draw. NULL draw function pointers are skipped. @@ -465,8 +456,7 @@ B3_API void b3RecPlayer_SetDebugShapeCallbacks( b3RecPlayer* player, /// @param queryIndex index of the frame query to draw, or -1 to draw all of them /// @param selectedIndex index of the query to emphasize (reserved color plus a label), or -1 for /// none -B3_API void b3RecPlayer_DrawFrameQueries( b3RecPlayer* player, b3DebugDraw* draw, int queryIndex, - int selectedIndex ); +B3_API void b3RecPlayer_DrawFrameQueries( b3RecPlayer* player, b3DebugDraw* draw, int queryIndex, int selectedIndex ); /// The kind of a recorded spatial query, matching the public query and cast functions. typedef enum b3RecQueryType @@ -510,8 +500,7 @@ B3_API int b3RecPlayer_GetFrameQueryCount( const b3RecPlayer* player ); B3_API b3RecQueryInfo b3RecPlayer_GetFrameQuery( const b3RecPlayer* player, int index ); /// Get one result of a recorded query from the most recently replayed frame. -B3_API b3RecQueryHit b3RecPlayer_GetFrameQueryHit( const b3RecPlayer* player, int queryIndex, - int hitIndex ); +B3_API b3RecQueryHit b3RecPlayer_GetFrameQueryHit( const b3RecPlayer* player, int queryIndex, int hitIndex ); /**@}*/ // recording @@ -601,8 +590,7 @@ B3_API void b3Body_SetAngularVelocity( b3BodyId bodyId, b3Vec3 angularVelocity ) /// The result will be close but maybe not exact. This is meant for kinematic bodies. /// The target is not applied if the velocity would be below the sleep threshold. /// This will optionally wake the body if asleep, but only if the movement is significant. -B3_API void b3Body_SetTargetTransform( b3BodyId bodyId, b3WorldTransform target, float timeStep, - bool wake ); +B3_API void b3Body_SetTargetTransform( b3BodyId bodyId, b3WorldTransform target, float timeStep, bool wake ); /// Get the linear velocity of a local point attached to a body. Usually in meters per second. B3_API b3Vec3 b3Body_GetLocalPointVelocity( b3BodyId bodyId, b3Vec3 localPoint ); @@ -828,35 +816,30 @@ B3_API float b3Body_GetClosestPoint( b3BodyId bodyId, b3Vec3* result, b3Vec3 tar /// Cast a ray at a specific body using a specified body transform. /// Ignores back-side collision on meshes and height-fields. -B3_API b3BodyCastResult b3Body_CastRay( b3BodyId bodyId, b3Pos origin, b3Vec3 translation, - b3QueryFilter filter, float maxFraction, - b3WorldTransform bodyTransform ); +B3_API b3BodyCastResult b3Body_CastRay( b3BodyId bodyId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter, + float maxFraction, b3WorldTransform bodyTransform ); /// Cast a shape at a specific body using a specified body transform. /// Ignores back-side collision on meshes and height-fields. -B3_API b3BodyCastResult b3Body_CastShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, - b3Vec3 translation, b3QueryFilter filter, - float maxFraction, bool canEncroach, +B3_API b3BodyCastResult b3Body_CastShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation, + b3QueryFilter filter, float maxFraction, bool canEncroach, b3WorldTransform bodyTransform ); /// Overlap a shape with a specific body using a specified body transform. -B3_API bool b3Body_OverlapShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, - b3QueryFilter filter, b3WorldTransform bodyTransform ); +B3_API bool b3Body_OverlapShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, b3QueryFilter filter, + b3WorldTransform bodyTransform ); /// Collide a character mover with a specific body using a specified body transform. /// Only considers convex shapes on the body. -B3_API int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int planeCapacity, - b3Pos origin, const b3Capsule* mover, b3QueryFilter filter, - b3WorldTransform bodyTransform ); +B3_API int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int planeCapacity, b3Pos origin, + const b3Capsule* mover, b3QueryFilter filter, b3WorldTransform bodyTransform ); /// Perform a time of impact between a character mover and a body using specified sweep transforms. /// Initial overlap of any shape on the body is ignored. A non-overlapped shape can still be hit. /// Only considers convex shapes on the body. -B3_API b3BodyTOIResult b3Body_TimeOfImpactMover( b3BodyId bodyId, b3Pos origin, - const b3Capsule* mover, b3Vec3 moverTranslation, - b3QueryFilter filter, - b3WorldTransform bodyTransform1, - b3WorldTransform bodyTransform2 ); +B3_API b3BodyTOIResult b3Body_TimeOfImpactMover( b3BodyId bodyId, b3Pos origin, const b3Capsule* mover, + b3Vec3 moverTranslation, b3QueryFilter filter, + b3WorldTransform bodyTransform1, b3WorldTransform bodyTransform2 ); /** @} */ // body @@ -875,29 +858,25 @@ B3_API b3BodyTOIResult b3Body_TimeOfImpactMover( b3BodyId bodyId, b3Pos origin, /// Create a circle shape and attach it to a body. The shape definition and geometry are fully /// cloned. Contacts are not created until the next time step. /// @return the shape id for accessing the shape -B3_API b3ShapeId b3CreateSphereShape( b3BodyId bodyId, const b3ShapeDef* def, - const b3Sphere* sphere ); +B3_API b3ShapeId b3CreateSphereShape( b3BodyId bodyId, const b3ShapeDef* def, const b3Sphere* sphere ); /// Create a capsule shape and attach it to a body. The shape definition and geometry are fully /// cloned. Contacts are not created until the next time step. /// @return the shape id for accessing the shape -B3_API b3ShapeId b3CreateCapsuleShape( b3BodyId bodyId, const b3ShapeDef* def, - const b3Capsule* capsule ); +B3_API b3ShapeId b3CreateCapsuleShape( b3BodyId bodyId, const b3ShapeDef* def, const b3Capsule* capsule ); /// Create a convex hull shape and attach it to a body. The shape definition is fully cloned. /// Contacts are not created until the next time step. /// @return the shape id for accessing the shape -B3_API b3ShapeId b3CreateHullShape( b3BodyId bodyId, const b3ShapeDef* def, - const b3HullData* hull ); +B3_API b3ShapeId b3CreateHullShape( b3BodyId bodyId, const b3ShapeDef* def, const b3HullData* hull ); /// Create a convex hull shape and attach it to a body. The hull is cloned then transformed with /// scale applied first. Use this for non-uniform or mirrored scale or a baked local transform. The /// baked result is shared through the world hull database. The shape definition and geometry are /// fully cloned. Contacts are not created until the next time step. /// @return the shape id for accessing the shape -B3_API b3ShapeId b3CreateTransformedHullShape( b3BodyId bodyId, const b3ShapeDef* def, - const b3HullData* hull, b3Transform transform, - b3Vec3 scale ); +B3_API b3ShapeId b3CreateTransformedHullShape( b3BodyId bodyId, const b3ShapeDef* def, const b3HullData* hull, + b3Transform transform, b3Vec3 scale ); /// Create a mesh hull shape and attach it to a body. The shape definition is fully cloned but the /// mesh is not. Contacts are not created until the next time step. Mesh collision only creates @@ -905,8 +884,7 @@ B3_API b3ShapeId b3CreateTransformedHullShape( b3BodyId bodyId, const b3ShapeDef /// @warning this holds reference to the input mesh data which must remain valid for the lifetime of /// this shape /// @return the shape id for accessing the shape -B3_API b3ShapeId b3CreateMeshShape( b3BodyId bodyId, const b3ShapeDef* def, const b3MeshData* mesh, - b3Vec3 scale ); +B3_API b3ShapeId b3CreateMeshShape( b3BodyId bodyId, const b3ShapeDef* def, const b3MeshData* mesh, b3Vec3 scale ); /// Create a height-field shape and attach it to a body. The shape definition is fully cloned but /// the height field is not. Contacts are not created until the next time step. Height field is only @@ -920,8 +898,7 @@ B3_API b3ShapeId b3CreateHeightFieldShape( b3BodyId bodyId, const b3ShapeDef* de /// Baked compound shapes are only allowed on static bodies. /// Note: runtime compounds are achieved by adding multiple shapes to a body. /// Runtime compounds can be dynamic and/or kinematic. -B3_API b3ShapeId b3CreateBakedCompoundShape( b3BodyId bodyId, b3ShapeDef* def, - const b3CompoundData* compound ); +B3_API b3ShapeId b3CreateBakedCompoundShape( b3BodyId bodyId, b3ShapeDef* def, const b3CompoundData* compound ); /// Destroy a shape. You may defer the body mass update which can improve performance if several /// shapes on a @@ -987,8 +964,7 @@ B3_API b3SurfaceMaterial b3Shape_GetSurfaceMaterial( b3ShapeId shapeId ); B3_API int b3Shape_GetMeshMaterialCount( b3ShapeId shapeId ); /// Set a surface material for a mesh shape. -B3_API void b3Shape_SetMeshMaterial( b3ShapeId shapeId, b3SurfaceMaterial surfaceMaterial, - int index ); +B3_API void b3Shape_SetMeshMaterial( b3ShapeId shapeId, b3SurfaceMaterial surfaceMaterial, int index ); /// Get a surface material for a mesh shape B3_API b3SurfaceMaterial b3Shape_GetMeshSurfaceMaterial( b3ShapeId shapeId, int index ); @@ -1121,8 +1097,7 @@ B3_API b3Vec3 b3Shape_GetClosestPoint( b3ShapeId shapeId, b3Vec3 target ); /// @param maxSpeed the maximum relative speed. Speed cap is necessary for stability. Typically /// 10m/s or less. /// @param wake should this wake the body -B3_API void b3Shape_ApplyWind( b3ShapeId shapeId, b3Vec3 wind, float drag, float lift, - float maxSpeed, bool wake ); +B3_API void b3Shape_ApplyWind( b3ShapeId shapeId, b3Vec3 wind, float drag, float lift, float maxSpeed, bool wake ); /** @} */ // shape @@ -1271,12 +1246,10 @@ B3_API void b3DistanceJoint_EnableSpring( b3JointId jointId, bool enableSpring ) B3_API bool b3DistanceJoint_IsSpringEnabled( b3JointId jointId ); /// Set the force range for the spring. -B3_API void b3DistanceJoint_SetSpringForceRange( b3JointId jointId, float lowerForce, - float upperForce ); +B3_API void b3DistanceJoint_SetSpringForceRange( b3JointId jointId, float lowerForce, float upperForce ); /// Get the force range for the spring. -B3_API void b3DistanceJoint_GetSpringForceRange( b3JointId jointId, float* lowerForce, - float* upperForce ); +B3_API void b3DistanceJoint_GetSpringForceRange( b3JointId jointId, float* lowerForce, float* upperForce ); /// Set the spring stiffness in Hertz B3_API void b3DistanceJoint_SetSpringHertz( b3JointId jointId, float hertz ); @@ -1651,8 +1624,7 @@ B3_API float b3RevoluteJoint_GetLowerLimit( b3JointId jointId ); B3_API float b3RevoluteJoint_GetUpperLimit( b3JointId jointId ); /// Set the revolute joint limits in radians -B3_API void b3RevoluteJoint_SetLimits( b3JointId jointId, float lowerLimitRadians, - float upperLimitRadians ); +B3_API void b3RevoluteJoint_SetLimits( b3JointId jointId, float lowerLimitRadians, float upperLimitRadians ); /// Enable/disable a revolute joint motor B3_API void b3RevoluteJoint_EnableMotor( b3JointId jointId, bool enableMotor ); @@ -1718,8 +1690,7 @@ B3_API float b3SphericalJoint_GetLowerTwistLimit( b3JointId jointId ); B3_API float b3SphericalJoint_GetUpperTwistLimit( b3JointId jointId ); /// Set the spherical joint limits in radians -B3_API void b3SphericalJoint_SetTwistLimits( b3JointId jointId, float lowerLimitRadians, - float upperLimitRadians ); +B3_API void b3SphericalJoint_SetTwistLimits( b3JointId jointId, float lowerLimitRadians, float upperLimitRadians ); /// Get the spherical joint current twist angle in radians. B3_API float b3SphericalJoint_GetTwistAngle( b3JointId jointId ); @@ -1924,8 +1895,7 @@ B3_API float b3WheelJoint_GetLowerSteeringLimit( b3JointId jointId ); B3_API float b3WheelJoint_GetUpperSteeringLimit( b3JointId jointId ); /// Set the wheel joint steering limits in radians. -B3_API void b3WheelJoint_SetSteeringLimits( b3JointId jointId, float lowerRadians, - float upperRadians ); +B3_API void b3WheelJoint_SetSteeringLimits( b3JointId jointId, float lowerRadians, float upperRadians ); /// Set the wheel joint target steering angle in radians. B3_API void b3WheelJoint_SetTargetSteeringAngle( b3JointId jointId, float radians ); diff --git a/include/box3d/collision.h b/include/box3d/collision.h index 36e4b3f..64b1e56 100644 --- a/include/box3d/collision.h +++ b/include/box3d/collision.h @@ -39,25 +39,28 @@ B3_API void b3DynamicTree_SetCategoryBits( b3DynamicTree* tree, int proxyId, uin /// Get the category bits on a proxy. B3_API uint64_t b3DynamicTree_GetCategoryBits( b3DynamicTree* tree, int proxyId ); -/// Query an AABB for overlapping proxies. The callback function is called for each proxy that overlaps the supplied AABB. +/// Query an AABB for overlapping proxies. The callback function is called for each proxy that overlaps the supplied +/// AABB. /// @return performance data B3_API b3TreeStats b3DynamicTree_Query( const b3DynamicTree* tree, b3AABB aabb, uint64_t maskBits, bool requireAllBits, b3TreeQueryCallbackFcn* callback, void* context ); -/// Query an AABB for the closest object. The callback function is called for each proxy that might be closest to the supplied -/// point. +/// Query an AABB for the closest object. The callback function is called for each proxy that might be closest to the +/// supplied point. /// @param tree the dynamic tree to query /// @param point the query point /// @param maskBits proxies are skipped if the bit-wise AND with the proxy category bits is zero -/// @param requireAllBits proxies are skipped if the bit-wise AND with the proxy category bits does not equal the maskBits +/// @param requireAllBits proxies are skipped if the bit-wise AND with the proxy category bits does not equal the +/// maskBits /// @param callback a user provided instance of b3TreeQueryClosestCallbackFcn /// @param context a user context object that is provided to the callback -/// @param minDistanceSqr the initial and final minimum squared distance. Provide a small initial to restrict the search and -/// improve performance. If the value is large this query has performance that scales linearly with the number of proxies and -/// would be slower than a brute force search. +/// @param minDistanceSqr the initial and final minimum squared distance. Provide a small initial to restrict the search +/// and improve performance. If the value is large this query has performance that scales linearly with the number of +/// proxies and would be slower than a brute force search. /// @return performance data -B3_API b3TreeStats b3DynamicTree_QueryClosest( const b3DynamicTree* tree, b3Vec3 point, uint64_t maskBits, bool requireAllBits, - b3TreeQueryClosestCallbackFcn* callback, void* context, float* minDistanceSqr ); +B3_API b3TreeStats b3DynamicTree_QueryClosest( const b3DynamicTree* tree, b3Vec3 point, uint64_t maskBits, + bool requireAllBits, b3TreeQueryClosestCallbackFcn* callback, + void* context, float* minDistanceSqr ); /// Ray cast against the proxies in the tree. This relies on the callback /// to perform an exact ray cast in the case where the proxy contains a shape. @@ -342,6 +345,28 @@ B3_INLINE const uint8_t* b3GetMeshFlags( const b3MeshData* mesh ) return (const uint8_t*)( (intptr_t)mesh + mesh->flagsOffset ); } +/// Get read only quantized angles between neighboring triangle normals. +B3_INLINE const int8_t* b3GetMeshEdgeAngles( const b3MeshData* mesh ) +{ + if ( mesh->edgeAngleOffset == 0 ) + { + return NULL; + } + + return (const int8_t*)( (intptr_t)mesh + mesh->edgeAngleOffset ); +} + +/// Get read only vertex flags. +B3_INLINE const uint8_t* b3GetMeshVertexFlags( const b3MeshData* mesh ) +{ + if ( mesh->vertexFlagOffset == 0 ) + { + return NULL; + } + + return (const uint8_t*)( (intptr_t)mesh + mesh->vertexFlagOffset ); +} + /// Create a grid mesh along the x and z axes. /// @param xCount the number of rows in the x direction /// @param zCount the number of rows in the z direction @@ -422,8 +447,8 @@ B3_API b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data ); B3_API b3HeightFieldData* b3CreateGrid( int rowCount, int columnCount, b3Vec3 scale, bool makeHoles ); /// Create a wave grid as a height field. -B3_API b3HeightFieldData* b3CreateWave( int rowCount, int columnCount, b3Vec3 scale, float rowFrequency, float columnFrequency, - bool makeHoles ); +B3_API b3HeightFieldData* b3CreateWave( int rowCount, int columnCount, b3Vec3 scale, float rowFrequency, + float columnFrequency, bool makeHoles ); /// Destroy a height field. B3_API void b3DestroyHeightField( b3HeightFieldData* heightField ); @@ -522,7 +547,8 @@ B3_API bool b3OverlapCapsule( const b3Capsule* shape, b3Transform shapeTransform B3_API bool b3OverlapCompound( const b3CompoundData* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ); /// Overlap shape versus height field -B3_API bool b3OverlapHeightField( const b3HeightFieldData* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ); +B3_API bool b3OverlapHeightField( const b3HeightFieldData* shape, b3Transform shapeTransform, + const b3ShapeProxy* proxy ); /// Overlap shape versus hull B3_API bool b3OverlapHull( const b3HullData* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ); @@ -580,29 +606,34 @@ B3_API b3CastOutput b3ShapeCastHeightField( const b3HeightFieldData* shape, cons /// Query callback. typedef bool b3MeshQueryFcn( b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleIndex, void* context ); -/// Query a mesh for triangles overlapping a bounding box in local space. May have false positives. Useful for debug draw. +/// Query a mesh for triangles overlapping a bounding box in local space. May have false positives. Useful for debug +/// draw. /// @param mesh the mesh to query, includes scale /// @param bounds the bounding box in local space /// @param fcn a user function to collect triangles /// @param context the context sent to the user function. B3_API void b3QueryMesh( const b3Mesh* mesh, const b3AABB bounds, b3MeshQueryFcn* fcn, void* context ); -/// Query a height field for triangles overlapping a bounding box in local space. May have false positives. Useful for debug draw. +/// Query a height field for triangles overlapping a bounding box in local space. May have false positives. Useful for +/// debug draw. /// @param heightField the height field to query /// @param bounds the bounding box in local space /// @param fcn a user function to collect triangles /// @param context the context sent to the user function. -B3_API void b3QueryHeightField( const b3HeightFieldData* heightField, b3AABB bounds, b3MeshQueryFcn* fcn, void* context ); +B3_API void b3QueryHeightField( const b3HeightFieldData* heightField, b3AABB bounds, b3MeshQueryFcn* fcn, + void* context ); /// Compute the closest points between two shapes represented as point clouds. /// b3SimplexCache cache is input/output. On the first call set b3SimplexCache.count to zero. /// The query runs in frame A, so the witness points and normal are returned in frame A. -/// The underlying GJK algorithm may be debugged by passing in debug simplexes and capacity. You may pass in NULL and 0 for these. +/// The underlying GJK algorithm may be debugged by passing in debug simplexes and capacity. You may pass in NULL and 0 +/// for these. B3_API b3DistanceOutput b3ShapeDistance( const b3DistanceInput* input, b3SimplexCache* cache, b3Simplex* simplexes, int simplexCapacity ); -/// Perform a linear shape cast of shape B moving and shape A fixed. Determines the hit point, normal, and translation fraction. -/// The query runs in frame A, so the hit point and normal are returned in frame A. Initially touching shapes are a miss. +/// Perform a linear shape cast of shape B moving and shape A fixed. Determines the hit point, normal, and translation +/// fraction. The query runs in frame A, so the hit point and normal are returned in frame A. Initially touching shapes +/// are a miss. B3_API b3CastOutput b3ShapeCast( const b3ShapeCastPairInput* input ); /// Evaluate the transform sweep at a specific time. @@ -630,16 +661,16 @@ B3_API void b3CollideCapsuleAndSphere( b3LocalManifold* manifold, int capacity, const b3Sphere* sphereB, b3Transform transformBtoA ); /// Collide a hull and a sphere. -B3_API void b3CollideHullAndSphere( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3Sphere* sphereB, - b3Transform transformBtoA, b3SimplexCache* cache ); +B3_API void b3CollideHullAndSphere( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, + const b3Sphere* sphereB, b3Transform transformBtoA, b3SimplexCache* cache ); /// Collide two capsules. -B3_API void b3CollideCapsules( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA, const b3Capsule* capsuleB, - b3Transform transformBtoA ); +B3_API void b3CollideCapsules( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA, + const b3Capsule* capsuleB, b3Transform transformBtoA ); /// Collide a hull and a capsule. -B3_API void b3CollideHullAndCapsule( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3Capsule* capsuleB, - b3Transform transformBtoA, b3SimplexCache* cache ); +B3_API void b3CollideHullAndCapsule( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, + const b3Capsule* capsuleB, b3Transform transformBtoA, b3SimplexCache* cache ); /// Collide two hulls. B3_API void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB, @@ -650,24 +681,46 @@ B3_API void b3CollideTriangleAndCapsule( b3LocalManifold* manifold, int capacity const b3Capsule* capsuleB, b3SimplexCache* cache ); /// Collide a triangle and hull. Normal points from triangle to hull. -B3_API void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, int triangleFlags, - const b3HullData* hullB, b3SATCache* cache, bool enableSpeculative ); +B3_API void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, + const b3HullData* hullB, b3SATCache* cache, + bool enableSpeculative ); /// Collide a triangle and sphere. Normal points from triangle to sphere. B3_API void b3CollideTriangleAndSphere( b3LocalManifold* manifold, int capacity, const b3Vec3* triangleA, const b3Sphere* sphereB ); +/// Collide a mesh and hull for testing. +B3_API int b3CollideMeshAndHull( b3Manifold* manifolds, int capacity, const b3Mesh* mesh, const b3HullData* hull, + b3Transform transformBtoA, bool enableSpeculative, bool handleGhosts ); + +/// Collide a mesh and sphere for testing. +B3_API int b3CollideMeshAndSphere( b3Manifold* manifolds, int capacity, const b3Mesh* mesh, const b3Sphere* sphere, + b3Transform transformBtoA, bool enableSpeculative, bool handleGhosts ); + /// This is used for computing 2D hulls in service of contact manifold simplification. /// Placed here for testing. Internal. typedef struct b3Point2D { - b3Vec2 p; // 2D point - float separation; // separation of the associated contact point - int originalIndex; // original index of the associated contact point + b3Vec2 p; ///< 2D point + float separation; ///< separation of the associated contact point + int originalIndex; ///< original index of the associated contact point } b3Point2D; +/// Options for how a 2D hull is built. +typedef enum b3Hull2DFlags +{ + /// Keep collinear points. + b3_hull2DKeepCollinear = 0x01, + + /// Only merge identical points. + b3_hull2DExactMerge = 0x02, + + /// Order the output points by the input order. + b3_hull2DIndexOrder = 0x04, +} b3Hull2DFlags; + /// Compute a 2D hull. Used internally for contact manifold simplification. Here for testing. -B3_API int b3Hull2D( b3Point2D* pts, int count, b3Point2D* hull ); +B3_API int b3Hull2D( b3Point2D* pts, int count, b3Point2D* hull, int flags ); /// Simplify a 2D hull. Used internally for contact manifold simplification. Here for testing. B3_API int b3SimplifyHull2D( b3Point2D* hull, int count, int target ); diff --git a/include/box3d/constants.h b/include/box3d/constants.h index 259dcb9..8e8c3e3 100644 --- a/include/box3d/constants.h +++ b/include/box3d/constants.h @@ -137,10 +137,3 @@ B3_API float b3GetLengthUnitsPerMeter( void ); #define B3_CHILD_POWER ( 64 - 2 * B3_SHAPE_POWER ) #define B3_MAX_SHAPES ( 1 << B3_SHAPE_POWER ) #define B3_MAX_CHILD_SHAPES ( 1 << B3_CHILD_POWER ) - -/// This is the limit on how many mesh or heightfield triangles a single convex shape can collide with. -/// Increasing this will increase stack usage, so be careful. I recommend to simplify your collision data -/// before increasing this. For example, using render mesh for collision often leads to poor performance. -#ifndef B3_MAX_MESH_CONTACT_TRIANGLES -#define B3_MAX_MESH_CONTACT_TRIANGLES 512 -#endif diff --git a/include/box3d/math_functions.h b/include/box3d/math_functions.h index 8f81d93..811eea8 100644 --- a/include/box3d/math_functions.h +++ b/include/box3d/math_functions.h @@ -189,7 +189,48 @@ B3_INLINE float b3LerpFloat( float a, float b, float alpha ) /// This is hand coded for cross-platform determinism. The atan2f /// function in the standard library is not cross-platform deterministic. /// Accurate to around 0.0023 degrees. -B3_API float b3Atan2( float y, float x ); +/// https://stackoverflow.com/questions/46210708/atan2-approximation-with-11bits-in-mantissa-on-x86with-sse2-and-armwith-vfpv4 +B3_INLINE float b3Atan2( float y, float x ) +{ + // Added check for (0,0) to match atan2f and avoid NaN + if ( x == 0.0f && y == 0.0f ) + { + return 0.0f; + } + + float ax = b3AbsFloat( x ); + float ay = b3AbsFloat( y ); + float mx = b3MaxFloat( ay, ax ); + float mn = b3MinFloat( ay, ax ); + float a = mn / mx; + + // Minimax polynomial approximation to atan(a) on [0,1] + float s = a * a; + float c = s * a; + float q = s * s; + float r = 0.024840285f * q + 0.18681418f; + float t = -0.094097948f * q - 0.33213072f; + r = r * s + t; + r = r * c + a; + + // Map to full circle + if ( ay > ax ) + { + r = 1.57079637f - r; + } + + if ( x < 0 ) + { + r = 3.14159274f - r; + } + + if ( y < 0 ) + { + r = -r; + } + + return r; +} /// Compute the cosine and sine of an angle in radians. Implemented /// for cross-platform determinism. diff --git a/include/box3d/types.h b/include/box3d/types.h index 20fd0ce..85d0ced 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -31,8 +31,7 @@ typedef void b3TaskCallback( void* taskContext ); /// @param userContext the scheduler context object that is opaque to Box3D /// @param taskName the Box3D task name that the scheduler can use for diagnostics /// @ingroup world -typedef void* b3EnqueueTaskCallback( b3TaskCallback* task, void* taskContext, void* userContext, - const char* taskName ); +typedef void* b3EnqueueTaskCallback( b3TaskCallback* task, void* taskContext, void* userContext, const char* taskName ); /// Finishes a user task object that wraps a Box3D task. This must block until the task has /// completed. The step blocks here on the tasks it spawned, so b3World_Step holds its stack across @@ -64,8 +63,8 @@ typedef float b3FrictionCallback( float frictionA, uint64_t userMaterialIdA, flo /// this is called from a worker thread. /// @warning This function should not attempt to modify Box3D state or user application state. /// @ingroup world -typedef float b3RestitutionCallback( float restitutionA, uint64_t userMaterialIdA, - float restitutionB, uint64_t userMaterialIdB ); +typedef float b3RestitutionCallback( float restitutionA, uint64_t userMaterialIdA, float restitutionB, + uint64_t userMaterialIdB ); /// Prototype for a contact filter callback. /// This is called when a contact pair is considered for collision. This allows you to @@ -93,8 +92,7 @@ typedef bool b3CustomFilterFcn( b3ShapeId shapeIdA, b3ShapeId shapeIdB, void* co /// full contact manifold. /// @warning Do not attempt to modify the world inside this callback /// @ingroup world -typedef bool b3PreSolveFcn( b3ShapeId shapeIdA, b3ShapeId shapeIdB, b3Pos point, b3Vec3 normal, - void* context ); +typedef bool b3PreSolveFcn( b3ShapeId shapeIdA, b3ShapeId shapeIdB, b3Pos point, b3Vec3 normal, void* context ); /// Prototype callback for overlap queries. /// Called for each shape found in the query. @@ -122,9 +120,8 @@ typedef bool b3OverlapResultFcn( b3ShapeId shapeId, void* context ); /// @return -1 to filter, 0 to terminate, fraction to clip the ray for closest hit, 1 to continue /// @see b3World_CastRay /// @ingroup world -typedef float b3CastResultFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, - uint64_t userMaterialId, int triangleIndex, int childIndex, - void* context ); +typedef float b3CastResultFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, + int triangleIndex, int childIndex, void* context ); /// Optional world capacities that can be use to avoid run-time allocations /// @ingroup world @@ -1628,7 +1625,6 @@ typedef struct b3SimplexCache /// Value use to compare length, area, volume of two simplexes. float metric; - // todo use an index of 0xFF as a sentinel and remove the count /// The number of stored simplex points uint16_t count; @@ -1650,7 +1646,7 @@ typedef struct b3ShapeCastPairInput b3Transform transform; ///< Transform of shape B in shape A's frame, the relative pose B in A b3Vec3 translationB; ///< The translation of shape B, in A's frame float maxFraction; ///< The fraction of the translation to consider, typically 1 - bool canEncroach; ///< Allows shapes with a radius to move slightly closer if already touching + bool canEncroach; ///< Allows shapes with a radius to move slightly closer if already touching } b3ShapeCastPairInput; /// Input for b3ShapeDistance @@ -1905,24 +1901,21 @@ typedef bool b3TreeQueryCallbackFcn( int proxyId, uint64_t userData, void* conte /// This function receives the minimum distance squared so far and proxy to check in the closest /// query. /// @return minimum distance squared to user objects in the proxy -typedef float b3TreeQueryClosestCallbackFcn( float distanceSqrMin, int proxyId, uint64_t userData, - void* context ); +typedef float b3TreeQueryClosestCallbackFcn( float distanceSqrMin, int proxyId, uint64_t userData, void* context ); /// This function receives clipped AABB cast input for a proxy. The function returns the new cast /// fraction. /// - return a value of 0 to terminate the cast /// - return a value less than input->maxFraction to clip the cast /// - return a value of input->maxFraction to continue the cast without clipping -typedef float b3TreeBoxCastCallbackFcn( const b3BoxCastInput* input, int proxyId, uint64_t userData, - void* context ); +typedef float b3TreeBoxCastCallbackFcn( const b3BoxCastInput* input, int proxyId, uint64_t userData, void* context ); /// This function receives clipped ray cast input for a proxy. The function /// returns the new ray fraction. /// - return a value of 0 to terminate the ray cast /// - return a value less than input->maxFraction to clip the ray /// - return a value of input->maxFraction to continue the ray cast without clipping -typedef float b3TreeRayCastCallbackFcn( const b3RayCastInput* input, int proxyId, uint64_t userData, - void* context ); +typedef float b3TreeRayCastCallbackFcn( const b3RayCastInput* input, int proxyId, uint64_t userData, void* context ); /**@}*/ // tree @@ -2009,8 +2002,7 @@ typedef struct b3BodyTOIResult /// Used to collect collision planes for character movers. /// Return true to continue gathering planes. -typedef bool b3PlaneResultFcn( b3ShapeId shapeId, const b3PlaneResult* plane, int planeCount, - void* context ); +typedef bool b3PlaneResultFcn( b3ShapeId shapeId, const b3PlaneResult* plane, int planeCount, void* context ); /// Used to filter shapes for shape casting character movers. /// Return true to accept the collision @@ -2260,7 +2252,7 @@ typedef struct b3MeshDef } b3MeshDef; /// 64-bit mesh version. Useful for validating serialized data. -#define B3_MESH_VERSION 0xAAAB9A00F1A8AAF7ull +#define B3_MESH_VERSION 0x24AA0BF74682501Dull /// Triangle mesh edge flags. typedef enum b3MeshEdgeFlags @@ -2291,6 +2283,16 @@ typedef struct b3MeshTriangle int32_t index3; ///< Index of vertex 3. } b3MeshTriangle; +/// Indicates an isolated edge (treated as a cliff). +#define B3_BOUNDARY_EDGE_ANGLE ( -128 ) + +/// Mesh vertex flags. Used for ghost collision handling. +typedef enum b3MeshVertexFlags +{ + b3_flatVertex = 0x01, ///< All edges are flat. + b3_concaveVertex = 0x02, ///< Any edge is concave. +} b3MeshVertexFlags; + /// A mesh BVH node. typedef struct b3MeshNode { @@ -2327,7 +2329,8 @@ typedef struct b3MeshNode uint32_t triangleOffset; } b3MeshNode; -/// This is a sorted triangle collision bounding volume hierarchy. +/// This is a sorted triangle collision bounding volume hierarchy. Offsets are a byte +/// count offset from the struct address. /// @note This struct has data hanging off the end and cannot be directly copied. typedef struct b3MeshData { @@ -2352,35 +2355,41 @@ typedef struct b3MeshData /// The number of degenerate triangles. Diagnostic. int32_t degenerateCount; - /// Offset of the node array in bytes from the struct address. + /// Offset of the node array. int32_t nodeOffset; /// The number of BVH nodes. int32_t nodeCount; - /// Offset of the vertex array in bytes from the struct address. + /// Offset of the vertex array. int32_t vertexOffset; /// The number of vertices. int32_t vertexCount; - /// Offset of the triangle array in bytes from the struct address. + /// Offset of the triangle array. int32_t triangleOffset; /// The number of triangles. int32_t triangleCount; - /// Offset of the material array in bytes from the struct address. + /// Offset of the material array. int32_t materialOffset; /// The number of materials. int32_t materialCount; - /// Offset of the triangle flag array in bytes from the struct address. + /// Offset of the triangle flag array. int32_t flagsOffset; - /// Explicit padding. - int32_t padding; + // Offset to the edge neighbor angles. + int32_t edgeAngleOffset; + + /// Offset of the vertex flag array. + int32_t vertexFlagOffset; + + /// Offset the vertex neighbor angle ring. + int32_t vertexRingOffset; } b3MeshData; /// This allows mesh data to be re-used with different scales. @@ -2441,7 +2450,7 @@ typedef struct b3HeightFieldDef #define B3_HEIGHT_FIELD_HOLE 0xFF /// 64-bit height-field version. Useful for validating serialized data. -#define B3_HEIGHT_FIELD_VERSION 0x8E41E5FB084848F8ull +#define B3_HEIGHT_FIELD_VERSION 0x5A69A297D7ACE1CDull /// A height field with compressed storage. /// @note This data structure has data hanging off the end and cannot be directly copied. @@ -2489,11 +2498,15 @@ typedef struct b3HeightFieldData /// uint8_t, one per triangle. int32_t flagsOffset; + /// Offset to the vertex ghost collision flag. Array of bits. + int32_t vertexFlagOffset; + /// Triangle winding. uint8_t clockwise; /// Explicit padding. - uint8_t padding[7]; + uint8_t padding[3]; + } b3HeightFieldData; /**@}*/ // height_field @@ -2797,6 +2810,12 @@ typedef struct b3ManifoldPoint /// Did this contact point exist in the previous step? bool persisted; + + // b3TriangleFeature + uint8_t triangleFeature; + + // b3MeshContactFlags + uint8_t meshFlags; } b3ManifoldPoint; /// A contact manifold describes the contact points between colliding shapes. @@ -2824,7 +2843,7 @@ typedef struct b3Manifold } b3Manifold; /// Cached separating axis feature. -typedef enum +typedef enum b3SeparatingFeature { b3_invalidAxis = 0, b3_backsideAxis, @@ -2840,7 +2859,7 @@ typedef enum } b3SeparatingFeature; /// Cached triangle feature. -typedef enum +typedef enum b3TriangleFeature { b3_featureNone = 0, b3_featureTriangleFace, @@ -2856,6 +2875,14 @@ typedef enum b3_featureVertex3 } b3TriangleFeature; +/// This gives debug information about how a contact point was generated. +typedef enum b3MeshContactFlags +{ + b3_meshContactHullFace = 0x01, ///< Normal is a hull face normal. + b3_meshContactSnapped = 0x02, ///< The normal is snapped to the triangle normal. + b3_meshContactDeep = 0x04, ///< The contact is considered deep (2 * linear slop). +} b3MeshContactFlags; + /// Separating axis test cache. Provides temporal acceleration of collision routines. typedef struct { @@ -2907,6 +2934,12 @@ typedef struct b3LocalManifoldPoint /// The triangle index when collide with a mesh or height-field. int triangleIndex; + + // b3TriangleFeature + uint8_t triangleFeature; + + // b3MeshContactFlags + uint8_t meshFlags; } b3LocalManifoldPoint; /// A local manifold with no dynamic information. Used by b3Collide functions. @@ -2927,18 +2960,8 @@ typedef struct b3LocalManifold /// The index of the triangle. int triangleIndex; - int i1; ///< Vertex 1 index. - int i2; ///< Vertex 2 index. - int i3; ///< Vertex 3 index. - - /// The squared distance of a sphere from a triangle. For ghost collision reduction. - float squaredDistance; - /// The triangle feature involved. b3TriangleFeature feature; - - /// b3MeshEdgeFlags. - int triangleFlags; } b3LocalManifold; /**@}*/ // collision @@ -3157,8 +3180,7 @@ typedef struct b3DebugDraw /// Draws a user shape. The userShape pointer is owned by the application and is known to Box3D /// as an opaque pointer returned from b3CreateDebugShapeCallback. When this is called the drawn /// shape has passed a culling test against drawingBounds below. - void ( *DrawShapeFcn )( void* userShape, b3WorldTransform transform, b3HexColor color, - void* context ); + void ( *DrawShapeFcn )( void* userShape, b3WorldTransform transform, b3HexColor color, void* context ); /// Draw a line segment. void ( *DrawSegmentFcn )( b3Pos p1, b3Pos p2, b3HexColor color, void* context ); @@ -3173,15 +3195,13 @@ typedef struct b3DebugDraw void ( *DrawSphereFcn )( b3Pos p, float radius, b3HexColor color, float alpha, void* context ); /// Draw a capsule. - void ( *DrawCapsuleFcn )( b3Pos p1, b3Pos p2, float radius, b3HexColor color, float alpha, - void* context ); + void ( *DrawCapsuleFcn )( b3Pos p1, b3Pos p2, float radius, b3HexColor color, float alpha, void* context ); /// Draw a bounding box. void ( *DrawBoundsFcn )( b3AABB aabb, b3HexColor color, void* context ); /// Draw an oriented box. - void ( *DrawBoxFcn )( b3Vec3 extents, b3WorldTransform transform, b3HexColor color, - void* context ); + void ( *DrawBoxFcn )( b3Vec3 extents, b3WorldTransform transform, b3HexColor color, void* context ); /// Draw a string in world space void ( *DrawStringFcn )( b3Pos p, const char* s, b3HexColor color, void* context ); diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index d9d29de..85081eb 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -34,6 +34,16 @@ FetchContent_Declare(nfd ) FetchContent_MakeAvailable(nfd) +# meshoptimizer for the s&box Ghost Collisions remesh toggle. Static lib, target meshoptimizer. +set(MESHOPT_INSTALL OFF CACHE BOOL "" FORCE) +FetchContent_Declare(meshoptimizer + GIT_REPOSITORY https://github.com/zeux/meshoptimizer.git + GIT_TAG v1.3 + GIT_SHALLOW TRUE + GIT_PROGRESS TRUE +) +FetchContent_MakeAvailable(meshoptimizer) + # The shader compiler is only needed when recompiling shaders. A fresh clone # with BOX3D_BUILD_SHADERS=OFF builds from the committed headers and skips this. if(BOX3D_BUILD_SHADERS) @@ -187,6 +197,7 @@ set(SAMPLE_FILES sample_determinism.cpp sample_events.cpp sample_geometry.cpp + sample_ghost.cpp sample_issues.cpp sample_joint.cpp sample_manifold.cpp @@ -220,7 +231,7 @@ target_include_directories(samples PRIVATE # gfx: the renderer + Draw*/adapter. shared: human, utils (Random*), the # benchmark/determinism helpers the samples lean on. -target_link_libraries(samples PRIVATE gfx shared imgui implot imguizmo sokol nfd) +target_link_libraries(samples PRIVATE gfx shared imgui implot imguizmo sokol nfd meshoptimizer) # MSVC auto-links winmm via a #pragma comment in main.cpp. MinGW ignores that # pragma, so link the timeBeginPeriod/timeEndPeriod library explicitly. diff --git a/samples/gfx/debug_adapter.c b/samples/gfx/debug_adapter.c index 66ffd38..b1b7b0b 100644 --- a/samples/gfx/debug_adapter.c +++ b/samples/gfx/debug_adapter.c @@ -50,11 +50,27 @@ typedef struct float radius; } DebugCapsule; -// Hull/mesh/heightfield. Scale only for mesh. +// A slice of a mesh holding the triangles that share one material color. +// A zero color draws with the shape color. +typedef struct +{ + MeshHandle handle; + uint32_t color; +} DebugMeshPart; + +// Hull/mesh/heightfield. Scale only for mesh. A top-level mesh whose material +// slots carry different custom colors is drawn as parts, one per color. The +// slot colors are snapshot so a runtime material change rebuilds the parts. +// The whole mesh handle is kept for highlight and material overrides. typedef struct { MeshHandle handle; b3Vec3 scale; + const b3MeshData* data; + uint32_t* slotColors; + int slotCount; + DebugMeshPart* parts; + int partCount; } DebugMesh; typedef struct @@ -148,6 +164,138 @@ static const float kBodyTypeRoughness[3] = { 0.70f, 0.55f, 0.40f }; static const float kDebugMaterialMetallic[6] = { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.85f }; static const float kDebugMaterialRoughness[6] = { 0.50f, 0.85f, 0.65f, 0.95f, 0.30f, 0.35f }; +static void ReleaseMeshParts( DebugMesh* geom ) +{ + for ( int i = 0; i < geom->partCount; ++i ) + { + ReleaseMeshReference( geom->parts[i].handle ); + } + free( geom->parts ); + free( geom->slotColors ); + geom->parts = NULL; + geom->partCount = 0; + geom->slotColors = NULL; + geom->slotCount = 0; +} + +// Group the mesh triangles by the custom color of their material slot. Slots +// sharing a color share a part, so a conveyor with many belt slots of one +// color costs one draw. Nothing is split when every used slot has one color. +static void BuildMeshParts( DebugShape* us ) +{ + DebugMesh* geom = &us->geom; + ReleaseMeshParts( geom ); + + const b3MeshData* data = geom->data; + const uint8_t* indices = data != NULL ? b3GetMeshMaterialIndices( data ) : NULL; + const int slotCount = b3Shape_GetMeshMaterialCount( us->shapeId ); + if ( indices == NULL || slotCount <= 1 ) + { + return; + } + + geom->slotColors = (uint32_t*)malloc( (size_t)slotCount * sizeof( uint32_t ) ); + uint8_t* used = (uint8_t*)calloc( (size_t)slotCount, 1 ); + uint8_t* mask = (uint8_t*)malloc( (size_t)slotCount ); + uint32_t* colors = (uint32_t*)malloc( (size_t)slotCount * sizeof( uint32_t ) ); + if ( geom->slotColors == NULL || used == NULL || mask == NULL || colors == NULL ) + { + free( used ); + free( mask ); + free( colors ); + free( geom->slotColors ); + geom->slotColors = NULL; + return; + } + geom->slotCount = slotCount; + + for ( int i = 0; i < slotCount; ++i ) + { + geom->slotColors[i] = b3Shape_GetMeshSurfaceMaterial( us->shapeId, i ).customColor; + } + + for ( int t = 0; t < data->triangleCount; ++t ) + { + const int slot = indices[t] < slotCount ? indices[t] : slotCount - 1; + used[slot] = 1; + } + + int colorCount = 0; + for ( int i = 0; i < slotCount; ++i ) + { + if ( used[i] == 0 ) + { + continue; + } + bool found = false; + for ( int j = 0; j < colorCount; ++j ) + { + if ( colors[j] == geom->slotColors[i] ) + { + found = true; + break; + } + } + if ( found == false ) + { + colors[colorCount++] = geom->slotColors[i]; + } + } + + if ( colorCount > 1 ) + { + geom->parts = (DebugMeshPart*)malloc( (size_t)colorCount * sizeof( DebugMeshPart ) ); + for ( int j = 0; geom->parts != NULL && j < colorCount; ++j ) + { + for ( int i = 0; i < slotCount; ++i ) + { + mask[i] = ( used[i] != 0 && geom->slotColors[i] == colors[j] ) ? 1 : 0; + } + + MeshHandle handle = FindOrAddMeshSubset( data, mask, slotCount ); + if ( IsMeshHandleValid( handle ) == false ) + { + // Fall back to drawing the whole mesh in the shape color + for ( int k = 0; k < geom->partCount; ++k ) + { + ReleaseMeshReference( geom->parts[k].handle ); + } + free( geom->parts ); + geom->parts = NULL; + geom->partCount = 0; + break; + } + + geom->parts[geom->partCount].handle = handle; + geom->parts[geom->partCount].color = colors[j]; + geom->partCount += 1; + } + } + + free( used ); + free( mask ); + free( colors ); +} + +// Rebuild the parts when a slot color changed since they were built +static void RefreshMeshParts( DebugShape* us ) +{ + DebugMesh* geom = &us->geom; + if ( geom->slotCount <= 1 ) + { + return; + } + + for ( int i = 0; i < geom->slotCount; ++i ) + { + if ( b3Shape_GetMeshSurfaceMaterial( us->shapeId, i ).customColor != geom->slotColors[i] ) + { + BuildMeshParts( us ); + return; + } + } +} + void InitAdapter( void ) { for ( int i = 0; i < BOX3D_USER_SHAPE_CAPACITY; ++i ) @@ -183,6 +331,7 @@ void ResetAdapterPool( void ) DebugShape* us = &s_adapter.pool[i]; if ( us->kind == Box3DUS_Hull || us->kind == Box3DUS_Mesh || us->kind == Box3DUS_HeightField ) { + ReleaseMeshParts( &us->geom ); ReleaseMeshReference( us->geom.handle ); } else if ( us->kind == Box3DUS_Compound ) @@ -532,11 +681,13 @@ static int CreateCompoundChild( const b3ChildShape* child, const DebugShape* par break; case b3_hullShape: us->kind = Box3DUS_Hull; + us->geom = (DebugMesh){ 0 }; us->geom.handle = handle; us->geom.scale = b3Vec3_one; break; case b3_meshShape: us->kind = Box3DUS_Mesh; + us->geom = (DebugMesh){ 0 }; us->geom.handle = handle; us->geom.scale = (b3Vec3){ child->mesh.scale.x, child->mesh.scale.y, child->mesh.scale.z }; break; @@ -603,6 +754,7 @@ static void* AdapterCreateDebugShape( const b3DebugShape* debugShape, void* cont DebugShape* us = &s_adapter.pool[index]; us->kind = Box3DUS_Hull; PopulateCommonFields( us, debugShape ); + us->geom = (DebugMesh){ 0 }; us->geom.handle = handle; us->geom.scale = b3Vec3_one; return us; @@ -625,8 +777,11 @@ static void* AdapterCreateDebugShape( const b3DebugShape* debugShape, void* cont DebugShape* us = &s_adapter.pool[index]; us->kind = Box3DUS_Mesh; PopulateCommonFields( us, debugShape ); + us->geom = (DebugMesh){ 0 }; us->geom.handle = handle; us->geom.scale = (b3Vec3){ mesh->scale.x, mesh->scale.y, mesh->scale.z }; + us->geom.data = mesh->data; + BuildMeshParts( us ); return us; } @@ -647,6 +802,7 @@ static void* AdapterCreateDebugShape( const b3DebugShape* debugShape, void* cont DebugShape* us = &s_adapter.pool[index]; us->kind = Box3DUS_HeightField; PopulateCommonFields( us, debugShape ); + us->geom = (DebugMesh){ 0 }; us->geom.handle = handle; us->geom.scale = b3Vec3_one; return us; @@ -740,6 +896,7 @@ static void DestroyDebugShape( void* userShape, void* context ) } else if ( us->kind == Box3DUS_Hull || us->kind == Box3DUS_Mesh || us->kind == Box3DUS_HeightField ) { + ReleaseMeshParts( &us->geom ); ReleaseMeshReference( us->geom.handle ); } @@ -757,8 +914,8 @@ static void DestroyDebugShape( void* userShape, void* context ) // center, capsule frame) layers on top of baseTransform, so the same path // serves a top-level shape (base = body transform) and a compound child // (base = body transform composed with the child transform). -static void AppendResolvedShape( const DebugShape* us, b3Transform baseTransform, Vec4 c, float metallic, float roughness, - TransparentShadowCast shadowCast, HighlightKind hk ) +static void AppendResolvedShape( const DebugShape* us, b3Transform baseTransform, Vec4 c, float metallic, + float roughness, TransparentShadowCast shadowCast, HighlightKind hk ) { if ( us->kind == Box3DUS_Sphere ) { @@ -782,7 +939,24 @@ static void AppendResolvedShape( const DebugShape* us, b3Transform baseTransform { MeshMaterialMode mode = us->isGround ? MESH_MATERIAL_MODE_GROUND_GRID : MESH_MATERIAL_MODE_SOLID; float cell = us->isGround ? BOX3D_GROUND_GRID_CELL_SIZE : 0.0f; - AppendMesh( us->geom.handle, baseTransform, us->geom.scale, c, metallic, roughness, mode, cell, shadowCast ); + if ( us->geom.partCount > 0 && us->hasMaterial == false ) + { + for ( int i = 0; i < us->geom.partCount; ++i ) + { + const DebugMeshPart* part = &us->geom.parts[i]; + Vec4 pc = c; + if ( part->color != 0 ) + { + pc = HexColorToLinear( (b3HexColor)part->color ); + pc.w = c.w; + } + AppendMesh( part->handle, baseTransform, us->geom.scale, pc, metallic, roughness, mode, cell, shadowCast ); + } + } + else + { + AppendMesh( us->geom.handle, baseTransform, us->geom.scale, c, metallic, roughness, mode, cell, shadowCast ); + } if ( hk != HIGHLIGHT_KIND_NONE ) { AppendHighlightGeometry( us->geom.handle, baseTransform, us->geom.scale, hk ); @@ -790,9 +964,8 @@ static void AppendResolvedShape( const DebugShape* us, b3Transform baseTransform } } -// Context for the compound cull query. Carries the resolved material and the -// body relative transform so each visited child appends exactly as the full -// walk would. +// Context for the compound cull query. Carries the resolved material and the body relative transform so each visited +// child appends exactly as the full walk would. typedef struct { const DebugShape* parent; @@ -842,6 +1015,11 @@ static void DrawShape( void* userShape, b3WorldTransform shapeTransform, b3HexCo RefreshMaterialFromOverride( us ); + if ( us->kind == Box3DUS_Mesh ) + { + RefreshMeshParts( us ); + } + Vec4 c; float metallic; float roughness; @@ -964,8 +1142,8 @@ static void DrawTransformFcn( b3WorldTransform transform, void* context ) Vec4 blue = MakeVec4( 0.0f, 0.0f, 1.0f, 1.0f ); OverlayAppendLine( origin, b3MulAdd( origin, L, basis.cx ), red, BOX3D_LINE_THICKNESS_PX, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE ); - OverlayAppendLine( origin, b3MulAdd( origin, L, basis.cy ), green, BOX3D_LINE_THICKNESS_PX, OVERLAY_THICKNESS_PIXELS, - OVERLAY_OCCLUSION_HIDE ); + OverlayAppendLine( origin, b3MulAdd( origin, L, basis.cy ), green, BOX3D_LINE_THICKNESS_PX, + OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE ); OverlayAppendLine( origin, b3MulAdd( origin, L, basis.cz ), blue, BOX3D_LINE_THICKNESS_PX, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE ); } @@ -988,22 +1166,22 @@ static void DrawCapsuleFcn( b3Pos p1, b3Pos p2, float radius, b3HexColor color, { (void)context; - b3Vec3 e = b3SubPos(p2, p1); + b3Vec3 e = b3SubPos( p2, p1 ); float length = b3Length( e ); - if (length < FLT_EPSILON) + if ( length < FLT_EPSILON ) { - DrawSphereEx( (b3WorldTransform){ p1, b3Quat_identity }, radius, HexColorAToVec4( color, alpha ), DEFAULT_METALLIC, - DEFAULT_ROUGHNESS, TRANSPARENT_SHADOW_NONE ); + DrawSphereEx( (b3WorldTransform){ p1, b3Quat_identity }, radius, HexColorAToVec4( color, alpha ), + DEFAULT_METALLIC, DEFAULT_ROUGHNESS, TRANSPARENT_SHADOW_NONE ); return; } b3Vec3 en = b3MulSV( 1.0f / length, e ); b3WorldTransform transform; - transform.p = b3OffsetPos( p1, b3MulSV( 0.5f, e )); + transform.p = b3OffsetPos( p1, b3MulSV( 0.5f, e ) ); transform.q = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisX, en ); DrawCapsuleEx( transform, 0.5f * length, radius, HexColorAToVec4( color, alpha ), DEFAULT_METALLIC, - DEFAULT_ROUGHNESS, TRANSPARENT_SHADOW_NONE ); + DEFAULT_ROUGHNESS, TRANSPARENT_SHADOW_NONE ); } static void DrawBoundsFcn( b3AABB aabb, b3HexColor color, void* context ) diff --git a/samples/gfx/debug_shapes.c b/samples/gfx/debug_shapes.c index 61ffcaf..6cc3a9a 100644 --- a/samples/gfx/debug_shapes.c +++ b/samples/gfx/debug_shapes.c @@ -420,11 +420,14 @@ static inline int MeshEdgeLess( const EdgeRecord* a, const EdgeRecord* b ) return a->v1 < b->v1; } -static MeshHandle BuildMeshData( const b3MeshData* meshData ) +// A NULL slotMask builds every triangle. Otherwise only triangles whose material +// slot has a nonzero mask entry are emitted, and their edges with them. +static MeshHandle BuildMeshData( const b3MeshData* meshData, uint64_t hash, const uint8_t* slotMask, int slotCount ) { const b3Vec3* verts = b3GetMeshVertices( meshData ); const b3MeshTriangle* tris = b3GetMeshTriangles( meshData ); const uint8_t* flags = b3GetMeshFlags( meshData ); + const uint8_t* materialIndices = slotMask != NULL ? b3GetMeshMaterialIndices( meshData ) : NULL; if ( !verts || !tris || meshData->triangleCount <= 0 ) { fprintf( stderr, "error: mesh missing arrays or empty (hash=0x%016" PRIx64 ")\n", meshData->hash ); @@ -436,6 +439,15 @@ static MeshHandle BuildMeshData( const b3MeshData* meshData ) for ( int t = 0; t < meshData->triangleCount; ++t ) { + if ( materialIndices != NULL ) + { + const int slot = materialIndices[t] < slotCount ? materialIndices[t] : slotCount - 1; + if ( slotMask[slot] == 0 ) + { + continue; + } + } + const b3MeshTriangle tri = tris[t]; const b3Vec3 a = verts[tri.index1]; const b3Vec3 b = verts[tri.index2]; @@ -523,7 +535,7 @@ static MeshHandle BuildMeshData( const b3MeshData* meshData ) eb.count = write + 1; } - const MeshHandle h = RegisterMesh( meshData->hash, buf.vertices, buf.vertexCount, buf.indices, buf.indexCount, "geom_mesh" ); + const MeshHandle h = RegisterMesh( hash, buf.vertices, buf.vertexCount, buf.indices, buf.indexCount, "geom_mesh" ); if ( IsMeshHandleValid( h ) ) { @@ -821,7 +833,37 @@ MeshHandle FindOrAddMesh( const b3MeshData* meshData ) return existing; } - return BuildMeshData( meshData ); + return BuildMeshData( meshData, meshData->hash, NULL, 0 ); +} + +MeshHandle FindOrAddMeshSubset( const b3MeshData* meshData, const uint8_t* slotMask, int slotCount ) +{ + if ( !meshData || meshData->hash == 0 || slotMask == NULL || slotCount <= 0 || + b3GetMeshMaterialIndices( meshData ) == NULL ) + { + return FindOrAddMesh( meshData ); + } + + // FNV-1a over the mask, seeded with the mesh hash + uint64_t hash = meshData->hash ^ 0xcbf29ce484222325ull; + for ( int i = 0; i < slotCount; ++i ) + { + hash ^= slotMask[i] != 0 ? 1u : 0u; + hash *= 0x100000001b3ull; + } + if ( hash == 0 || hash == meshData->hash ) + { + hash += 1; + } + + MeshHandle existing = FindMesh( hash ); + if ( IsMeshHandleValid( existing ) ) + { + AddMeshReference( existing ); + return existing; + } + + return BuildMeshData( meshData, hash, slotMask, slotCount ); } MeshHandle FindOrAddHeightField( const b3HeightFieldData* heightField ) diff --git a/samples/gfx/debug_shapes.h b/samples/gfx/debug_shapes.h index ecf024b..1c72a37 100644 --- a/samples/gfx/debug_shapes.h +++ b/samples/gfx/debug_shapes.h @@ -39,6 +39,12 @@ MeshHandle FindOrAddHull( const b3HullData* hull ); // draw time. Two b3Mesh sharing the same b3MeshData share one entry. MeshHandle FindOrAddMesh( const b3MeshData* meshData ); +// Acquire the subset of a mesh whose triangles sit in a material slot with a +// nonzero slotMask entry. Keyed by the mesh hash mixed with the mask, so shapes +// sharing a mesh and a slot grouping share one entry. Falls back to the whole +// mesh when the mesh carries no material indices. +MeshHandle FindOrAddMeshSubset( const b3MeshData* meshData, const uint8_t* slotMask, int slotCount ); + // Acquire for b3HeightFieldData. The `scale` vector is part of the height // field's hash (folded into construction), so two heightfields with // different scales hash to different entries, per-instance scale stays diff --git a/samples/gfx/edges.c b/samples/gfx/edges.c index c11d23e..976b7d3 100644 --- a/samples/gfx/edges.c +++ b/samples/gfx/edges.c @@ -40,7 +40,8 @@ EdgeOverlayParams GetDefaultEdgeParams( void ) p.zBias = 1.0e-6f; p.convexColor = MakeVec4( 0.1f, 0.8f, 0.1f, 0.75f ); p.concaveColor = MakeVec4( 0.8f, 0.10f, 0.10f, 0.75f ); - p.flatColor = MakeVec4( 0.6f, 0.6f, 0.6f, 0.5f ); + p.flatColor = MakeVec4( 0.6f, 0.6f, 0.6f, 0.2f ); + p.alphaScale = 1.0f; return p; } @@ -106,9 +107,11 @@ void ShutdownEdges( void ) s_edges.ready = false; } -static inline Vec4 Premultiply( Vec4 c ) +static inline Vec4 Premultiply( Vec4 c, float alphaScale ) { - return MakeVec4( c.x * c.w, c.y * c.w, c.z * c.w, c.w ); + float a = c.w * alphaScale; + a = a < 1.0f ? a : 1.0f; + return MakeVec4( c.x * a, c.y * a, c.z * a, a ); } // Per-batch policy: does the toggle want this batch drawn at all? @@ -160,13 +163,13 @@ static void SubmitBatches( int width, int height, const Mat4* view, const Mat4* edge_ub_pass_t passMesh = { 0 }; if ( params->showEdgeConvexity ) { - passMesh.convex_color = Premultiply( params->convexColor ); - passMesh.concave_color = Premultiply( params->concaveColor ); - passMesh.flat_color = Premultiply( params->flatColor ); + passMesh.convex_color = Premultiply( params->convexColor, params->alphaScale ); + passMesh.concave_color = Premultiply( params->concaveColor, params->alphaScale ); + passMesh.flat_color = Premultiply( params->flatColor, params->alphaScale ); } else { - Vec4 flat = Premultiply( params->flatColor ); + Vec4 flat = Premultiply( params->flatColor, params->alphaScale ); passMesh.convex_color = flat; passMesh.concave_color = flat; passMesh.flat_color = flat; @@ -175,7 +178,7 @@ static void SubmitBatches( int width, int height, const Mat4* view, const Mat4* edge_ub_pass_t passHull = { 0 }; Vec4 hullEdgeColor = { 0.5f, 0.5f, 0.5f, 0.5f }; - Vec4 hull = Premultiply( hullEdgeColor ); + Vec4 hull = Premultiply( hullEdgeColor, params->alphaScale ); passHull.convex_color = hull; passHull.concave_color = hull; passHull.flat_color = hull; diff --git a/samples/gfx/edges.h b/samples/gfx/edges.h index d38d6fd..86d1c84 100644 --- a/samples/gfx/edges.h +++ b/samples/gfx/edges.h @@ -6,18 +6,14 @@ // into two submission paths to match the silhouette quality of the // rest of the scene: // -// Opaque path (RenderEdgesInMSAA): opaque-arena batches (meshes, -// heightfields, opaque hulls if policy 1b is ever relaxed). Draws -// inside the caller's currently-open MSAA scene pass, picking up the -// same 4x MSAA as the opaque shapes. Pipeline tracks scene_target's -// color/depth formats and sample_count. +// Opaque path (RenderEdgesInMSAA): opaque-arena batches (meshes, heightfields, opaque hulls). Draws inside the caller's +// currently-open MSAA scene pass, picking up the same 4x MSAA as the opaque shapes. Pipeline tracks scene_target's +// color/depth formats and sample_count. // -// Transparent path (RenderEdgesPostTransparent): transparent-arena batches -// (hulls per policy 1b). Opens its own single-sample pass against the -// resolved scene color + GTAO prepass depth, so edges sit on top of the -// transparent draws that have already blended into the resolve. MSAA -// isn't available here (the resolve already happened), the FS's -// fwidth-based AA does what it can. +// Transparent path (RenderEdgesPostTransparent): transparent-arena batches (just hulls). Opens its own single-sample +// pass against the resolved scene color + GTAO prepass depth, so edges sit on top of the transparent draws that have +// already blended into the resolve. MSAA isn't available here (the resolve already happened), the FS's fwidth-based AA +// does what it can. #pragma once @@ -43,6 +39,7 @@ typedef struct EdgeOverlayParams Vec4 convexColor; // straight (non-premultiplied) linear RGBA, premultiplied inside Vec4 concaveColor; Vec4 flatColor; + float alphaScale; // multiplies the alpha of every edge color, clamped to 1 } EdgeOverlayParams; EdgeOverlayParams GetDefaultEdgeParams( void ); diff --git a/samples/sample.cpp b/samples/sample.cpp index 22c0b35..334b8ae 100644 --- a/samples/sample.cpp +++ b/samples/sample.cpp @@ -126,6 +126,7 @@ void SampleContext::Save() fprintf( file, " \"drawDistance\": %g,\n", drawDistance ); fprintf( file, " \"showHullEdges\": %s,\n", GetEdgeOverlayParams().showHulls ? "true" : "false" ); fprintf( file, " \"showEdgeConvexity\": %s,\n", GetEdgeOverlayParams().showEdgeConvexity ? "true" : "false" ); + fprintf( file, " \"edgeAlpha\": %g,\n", GetEdgeOverlayParams().alphaScale ); fprintf( file, " \"showMetrics\": %s,\n", showMetrics ? "true" : "false" ); fprintf( file, " \"showProfile\": %s,\n", showProfile ? "true" : "false" ); fprintf( file, " \"replayKeyframeBudgetMB\": %d,\n", replayKeyframeBudgetMB ); @@ -274,6 +275,12 @@ void SampleContext::Load() p.showEdgeConvexity = strncmp( s, "true", 4 ) == 0; SetEdgeOverlayParams( &p ); } + else if ( jsoneq( data, &tokens[i], "edgeAlpha" ) == 0 ) + { + EdgeOverlayParams p = GetEdgeOverlayParams(); + p.alphaScale = b3ClampFloat( ParseFloat( data, tokens[i + 1] ), 0.0f, 4.0f ); + SetEdgeOverlayParams( &p ); + } else if ( jsoneq( data, &tokens[i], "showMetrics" ) == 0 ) { const char* s = data + tokens[i + 1].start; @@ -1376,8 +1383,52 @@ void Sample::DrawTextLine( const char* text, ... ) char buffer[512]; vsnprintf( buffer, sizeof( buffer ), text, args ); va_end( args ); - DrawScreenString( m_textX, m_textLine, MakeColor( b3_colorWhite ), buffer ); - m_textLine += m_textIncrement; + + // The text arena keeps 63 characters per string, so wrap long lines at spaces and indent the rest + constexpr int capacity = 63; + constexpr int indent = 2; + const char* start = buffer; + bool first = true; + while ( true ) + { + int width = first ? capacity : capacity - indent; + int length = (int)strlen( start ); + int cut = length; + if ( length > width ) + { + cut = width; + while ( cut > 0 && start[cut] != ' ' ) + { + cut -= 1; + } + + if ( cut == 0 ) + { + cut = width; + } + } + + char line[capacity + 1]; + int offset = first ? 0 : indent; + memset( line, ' ', offset ); + memcpy( line + offset, start, cut ); + line[offset + cut] = '\0'; + DrawScreenString( m_textX, m_textLine, MakeColor( b3_colorWhite ), line ); + m_textLine += m_textIncrement; + + start += cut; + while ( *start == ' ' ) + { + start += 1; + } + + if ( *start == '\0' ) + { + break; + } + + first = false; + } } SampleEntry g_sampleEntries[MAX_SAMPLES]; @@ -1712,6 +1763,11 @@ static void DrawRenderMenu( SampleContext& ctx ) ImGui::Separator(); ImGui::PushItemWidth( 8.0f * ImGui::GetFontSize() ); + if ( ImGui::SliderFloat( "Edge Alpha", &edgeParams.alphaScale, 0.0f, 4.0f, "%.2f" ) ) + { + SetEdgeOverlayParams( &edgeParams ); + } + float ev = GetExposure(); if ( ImGui::SliderFloat( "Exposure", &ev, -8.0f, 4.0f, "%.2f" ) ) { diff --git a/samples/sample_collision.cpp b/samples/sample_collision.cpp index 370315c..6251cd2 100644 --- a/samples/sample_collision.cpp +++ b/samples/sample_collision.cpp @@ -2900,6 +2900,7 @@ public: e_random = 0, e_circle = 1, e_sliver = 2, + e_grid = 3, }; static Sample* Create( SampleContext* context ) @@ -2923,6 +2924,7 @@ public: m_pointCount = 20; m_drawScale = 1.0f; m_drawLabels = true; + m_hullFlags = 0; Generate(); } @@ -2961,6 +2963,20 @@ public: m_inputPoints[i].originalIndex = i; } } + else if ( m_sourceMode == e_grid ) + { + // Every border point is collinear with its neighbors, so the hull keeps them only with + // the collinear flag + m_drawScale = 1.0f; + m_inputCount = 36; + + for ( int i = 0; i < m_inputCount; ++i ) + { + m_inputPoints[i].p = { -5.0f + 2.0f * (float)( i % 6 ), -5.0f + 2.0f * (float)( i / 6 ) }; + m_inputPoints[i].separation = 0.0f; + m_inputPoints[i].originalIndex = i; + } + } else { // A near degenerate sliver with a duplicate pair. Welding and collinear removal have @@ -2998,7 +3014,7 @@ public: scratch[i] = m_inputPoints[i]; } - m_hullCount = b3Hull2D( scratch, m_inputCount, m_hullPoints ); + m_hullCount = b3Hull2D( scratch, m_inputCount, m_hullPoints, m_hullFlags ); for ( int i = 0; i < m_hullCount; ++i ) { @@ -3010,13 +3026,13 @@ public: bool DrawControls() override { - const char* modes[] = { "Random", "Circle", "Sliver" }; + const char* modes[] = { "Random", "Circle", "Sliver", "Grid" }; if ( ImGui::Combo( "Source", &m_sourceMode, modes, IM_ARRAYSIZE( modes ) ) ) { Generate(); } - if ( m_sourceMode != e_sliver ) + if ( m_sourceMode == e_random || m_sourceMode == e_circle ) { if ( ImGui::SliderInt( "Point Count", &m_pointCount, 4, m_pointCapacity ) ) { @@ -3033,6 +3049,8 @@ public: } ImGui::SliderInt( "Target Count", &m_targetCount, 3, m_inputCount ); + ImGui::CheckboxFlags( "Keep Collinear", &m_hullFlags, b3_hull2DKeepCollinear ); + ImGui::CheckboxFlags( "Exact Merge", &m_hullFlags, b3_hull2DExactMerge ); ImGui::Checkbox( "Draw Labels", &m_drawLabels ); return true; @@ -3110,6 +3128,7 @@ public: int m_simplifiedCount; int m_targetCount; int m_pointCount; + int m_hullFlags; bool m_drawLabels; }; diff --git a/samples/sample_continuous.cpp b/samples/sample_continuous.cpp index 6ff45dd..fc4d506 100644 --- a/samples/sample_continuous.cpp +++ b/samples/sample_continuous.cpp @@ -4,6 +4,7 @@ #include "gfx/debug_adapter.h" #include "gfx/draw.h" #include "imgui.h" +#include "mesh_drop.h" #include "sample.h" #include "utils.h" @@ -387,38 +388,9 @@ public: static int sampleNeedleMesh = RegisterSample( "Continuous", "Needle Mesh", NeedleMesh::Create ); -struct IndexPair -{ - int index1; - int index2; -}; - -static inline IndexPair ConvertToPair( void* userData ) -{ - static_assert( sizeof( intptr_t ) >= 8 && sizeof( int ) == 4 ); - intptr_t value = (intptr_t)userData; - int index1 = (int)value; - int index2 = (int)( value >> 32 ); - return { index1, index2 }; -} - -static inline void* ConvertToUserData( IndexPair pair ) -{ - intptr_t value = (intptr_t)pair.index2 << 32 | (intptr_t)pair.index1; - return (void*)value; -} - class MeshDrop : public Sample { public: - enum ShapeType - { - e_box, - e_capsule, - e_cylinder, - e_sphere, - }; - explicit MeshDrop( SampleContext* context ) : Sample( context ) { @@ -428,20 +400,21 @@ public: GetGuiDraw()->forceScale = 0.1f; } + m_settings = GetDefaultMeshDropSettings(); m_groundId = b3_nullBodyId; m_groundMesh = nullptr; - m_groundAmplitude = 0.5f; CreateGround(); - m_bodies = new b3BodyId[m_bodyCount]; - memset( m_bodies, 0, m_bodyCount * sizeof( b3BodyId ) ); + for ( int i = 0; i < MESH_DROP_BODY_COUNT; ++i ) + { + m_bodies[i] = b3_nullBodyId; + } - m_cylinder = b3CreateCylinder( 0.4f, 0.05f, 0.0f, 6 ); - m_shapeType = e_box; + m_cylinder = CreateMeshDropCylinder(); + m_seed = 0; m_runCount = 0; m_failure = false; m_autoGenerate = false; - m_collide = true; m_stepWhilePaused = true; Generate(); @@ -449,7 +422,6 @@ public: ~MeshDrop() override { - delete[] m_bodies; b3DestroyHull( m_cylinder ); b3DestroyMesh( m_groundMesh ); } @@ -468,58 +440,13 @@ public: m_groundMesh = nullptr; } - b3BodyDef bodyDef = b3DefaultBodyDef(); - m_groundId = b3CreateBody( m_worldId, &bodyDef ); - - int gridCount = 40; - float cellWidth = 1.0f; - float rowHz = 0.1f; - float columnHz = 0.2f; - - m_groundMesh = b3CreateWaveMesh( gridCount, gridCount, cellWidth, m_groundAmplitude, rowHz, columnHz ); - b3ShapeDef shapeDef = b3DefaultShapeDef(); - shapeDef.filter.categoryBits = 1; - b3CreateMeshShape( m_groundId, &shapeDef, m_groundMesh, b3Vec3_one ); - - float extent = 0.5f * gridCount * cellWidth; - float halfHeight = 1.0f; - - { - b3Transform transform; - transform.p = { 0.0f, halfHeight, -extent }; - transform.q = b3Quat_identity; - b3BoxHull wallBox = b3MakeTransformedBoxHull( extent, halfHeight, 0.1f, transform ); - b3CreateHullShape( m_groundId, &shapeDef, &wallBox.base ); - } - - { - b3Transform transform; - transform.p = { 0.0f, halfHeight, extent }; - transform.q = b3Quat_identity; - b3BoxHull wallBox = b3MakeTransformedBoxHull( extent, halfHeight, 0.1f, transform ); - b3CreateHullShape( m_groundId, &shapeDef, &wallBox.base ); - } - - { - b3Transform transform; - transform.p = { -extent, halfHeight, 0.0f }; - transform.q = b3Quat_identity; - b3BoxHull wallBox = b3MakeTransformedBoxHull( 0.1f, halfHeight, extent, transform ); - b3CreateHullShape( m_groundId, &shapeDef, &wallBox.base ); - } - - { - b3Transform transform; - transform.p = { extent, halfHeight, 0.0f }; - transform.q = b3Quat_identity; - b3BoxHull wallBox = b3MakeTransformedBoxHull( 0.1f, halfHeight, extent, transform ); - b3CreateHullShape( m_groundId, &shapeDef, &wallBox.base ); - } + m_groundMesh = CreateMeshDropMesh( m_settings.amplitude ); + m_groundId = CreateMeshDropGround( m_worldId, m_groundMesh ); } void Generate() { - for ( int i = 0; i < m_bodyCount; ++i ) + for ( int i = 0; i < MESH_DROP_BODY_COUNT; ++i ) { if ( B3_IS_NULL( m_bodies[i] ) ) { @@ -530,107 +457,53 @@ public: m_bodies[i] = b3_nullBodyId; } - // The soft contact solver cannot always resolve this small shape at 4 sub-steps. - // Also this small shape has trouble moving advancing in the time of impact code - // because the size is very close to the tolerances used. - // b3BoxHull box = b3MakeBoxHull( 0.01f, 0.1f, 0.02f ); - b3BoxHull box = b3MakeBoxHull( 0.02f, 0.2f, 0.04f ); - b3Capsule capsule = { { 0.0f, -0.2f, 0.0f }, { 0.0f, 0.2f, 0.0f }, 0.05f }; - b3Sphere sphere = { b3Vec3_zero, 0.05f }; - - int bodyIndex = 0; - - b3BodyDef bodyDef = b3DefaultBodyDef(); - bodyDef.type = b3_dynamicBody; - - b3ShapeDef shapeDef = b3DefaultShapeDef(); - shapeDef.baseMaterial.rollingResistance = m_shapeType == e_capsule ? 0.4f : 0.1f; - - // Don't allow shapes to collide with each other. This - // makes isolating failures easier. - if ( m_collide == false ) - { - shapeDef.filter.categoryBits = 2; - shapeDef.filter.maskBits = 1; - } - - g_randomSeed = (uint32_t)b3GetTicks(); - - bool simulateAll = true; - // g_randomSeed = 1910133196; + // A non-zero seed replays a generation reported by the mesh_soak tool + m_seed = m_replaySeed != 0 ? m_replaySeed : (uint32_t)b3GetTicks(); m_runCount += 1; m_stepCount = 0; - printf( "run %d seed %u\n", m_runCount, g_randomSeed ); + printf( "run %d seed %u\n", m_runCount, m_seed ); - for ( int i = 0; i < m_gridCount; ++i ) + MeshDropSettings settings = m_settings; + if ( m_solo[0] >= 0 && m_solo[1] >= 0 ) { - for ( int j = 0; j < m_gridCount; ++j ) - { - b3Vec3 linearVelocity = RandomVec3Uniform( -1.0f, 1.0f ); - b3Vec3 angularVelocity = RandomVec3Uniform( -5.0f, 5.0f ); - - if ( simulateAll || ( i == 19 && j == 20 ) ) - { - bodyDef.position = { 0.5f * ( i - 0.5f * m_gridCount ), 5.0f, 0.5f * ( j - 0.5f * m_gridCount ) }; - bodyDef.linearVelocity = linearVelocity; - bodyDef.angularVelocity = angularVelocity; - // bodyDef.angularDamping = 0.1f; - bodyDef.userData = ConvertToUserData( { i, j } ); - b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); - - if ( m_shapeType == e_box ) - { - b3CreateHullShape( bodyId, &shapeDef, &box.base ); - } - else if ( m_shapeType == e_capsule ) - { - b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); - } - else if ( m_shapeType == e_cylinder ) - { - b3CreateHullShape( bodyId, &shapeDef, m_cylinder ); - } - else - { - b3CreateSphereShape( bodyId, &shapeDef, &sphere ); - } - - m_bodies[bodyIndex] = bodyId; - } - else - { - m_bodies[bodyIndex] = b3_nullBodyId; - } - bodyIndex += 1; - } + settings.soloIndex = MESH_DROP_GRID_COUNT * m_solo[0] + m_solo[1]; } - assert( bodyIndex == m_bodyCount ); + CreateMeshDropBodies( m_worldId, settings, m_seed, m_cylinder, m_bodies ); + ResetMeshDropDepth( &m_depth, settings, m_cylinder ); } bool DrawControls() override { - const char* shapeTypes[] = { "box", "capsule", "cylinder", "sphere" }; - int shapeType = (int)m_shapeType; - if ( ImGui::Combo( "Type", &shapeType, shapeTypes, IM_ARRAYSIZE( shapeTypes ) ) ) + int shapeType = (int)m_settings.shape; + if ( ImGui::Combo( "Type", &shapeType, g_meshDropShapeNames, e_meshDropShapeCount ) ) { m_stepCount = 0; - m_shapeType = ShapeType( shapeType ); + m_settings.shape = MeshDropShape( shapeType ); Generate(); } - if ( ImGui::SliderFloat( "Amplitude", &m_groundAmplitude, 0.0f, 1.0f ) ) + if ( ImGui::SliderFloat( "Amplitude", &m_settings.amplitude, 0.0f, 1.0f ) ) { CreateGround(); Generate(); } - if ( ImGui::Checkbox( "Collide", &m_collide ) ) + if ( ImGui::Checkbox( "Collide", &m_settings.collide ) ) { Generate(); } + ImGui::InputScalar( "Seed", ImGuiDataType_U32, &m_replaySeed ); + + // Drop only the body at these indices, -1 for all + if ( ImGui::InputInt2( "Solo", m_solo ) ) + { + m_solo[0] = b3ClampInt( m_solo[0], -1, MESH_DROP_GRID_COUNT - 1 ); + m_solo[1] = b3ClampInt( m_solo[1], -1, MESH_DROP_GRID_COUNT - 1 ); + } + if ( ImGui::Button( "Generate" ) ) { Generate(); @@ -647,8 +520,14 @@ public: void Step() override { + int stepCount = m_stepCount; Sample::Step(); + if ( m_stepCount != stepCount ) + { + UpdateMeshDropDepth( m_worldId, &m_depth ); + } + { PickRay pickRay = m_camera->BuildPickRay( m_context->mouseX, m_context->mouseY ); @@ -657,13 +536,25 @@ public: if ( result.hit ) { b3BodyId bodyId = b3Shape_GetBody( result.shapeId ); - void* userData = b3Body_GetUserData( bodyId ); - IndexPair pair = ConvertToPair( userData ); - DrawTextLine( "indices: (%d, %d)\n", pair.index1, pair.index2 ); + int index = GetMeshDropBodyIndex( b3Body_GetUserData( bodyId ) ); + if ( index >= 0 ) + { + DrawTextLine( "indices: (%d, %d)", index / MESH_DROP_GRID_COUNT, index % MESH_DROP_GRID_COUNT ); + } } } - for ( int i = 0; i < m_bodyCount && m_failure == false; i += 1 ) + DrawTextLine( "seed %u", m_seed ); + + if ( m_depth.maxIndex >= 0 ) + { + int index = m_depth.maxIndex; + DrawTextLine( "deepest: (%d, %d) %.1f slop at step %d", index / MESH_DROP_GRID_COUNT, index % MESH_DROP_GRID_COUNT, + m_depth.maxDepth / B3_LINEAR_SLOP, m_depth.maxStep ); + } + + float tunnelHeight = GetMeshDropTunnelHeight(); + for ( int i = 0; i < MESH_DROP_BODY_COUNT && m_failure == false; i += 1 ) { b3BodyId bodyId = m_bodies[i]; if ( B3_IS_NULL( bodyId ) ) @@ -672,19 +563,14 @@ public: } b3Pos massCenter = b3Body_GetWorldCenter( bodyId ); - if ( massCenter.y < -2.0f ) + if ( massCenter.y < tunnelHeight ) { - IndexPair pair = ConvertToPair( b3Body_GetUserData( bodyId ) ); - printf( "index1: %d - index2: %d\n", pair.index1, pair.index2 ); + printf( "index1: %d - index2: %d\n", i / MESH_DROP_GRID_COUNT, i % MESH_DROP_GRID_COUNT ); printf( "(%6.2f, %6.2f, %6.2f)\n", massCenter.x, massCenter.y, massCenter.z ); m_context->pause = true; m_failure = true; m_autoGenerate = false; } - - // This is for debugging the fallback inner sphere - // b3Sphere sphere = {massCenter, 0.015f}; - // DrawSphere( m_scene, b3Transform_identity, sphere, b3_colorLightGray ); } if ( m_autoGenerate ) @@ -709,13 +595,13 @@ public: for ( int i = 0; i < n; ++i ) { b3World_Step( m_worldId, timeStep, m_context->subStepCount ); + UpdateMeshDropDepth( m_worldId, &m_depth ); } m_stepCount += n; } - int maxSteps = m_shapeType == e_capsule ? 3000 : 1000; - if ( m_stepCount > maxSteps ) + if ( m_stepCount > GetMeshDropStepLimit( m_settings.shape ) ) { m_context->pause = true; m_failure = true; @@ -729,20 +615,18 @@ public: return new MeshDrop( context ); } - static constexpr int m_gridCount = 32; - // static constexpr int m_gridCount = 3; - static constexpr int m_bodyCount = m_gridCount * m_gridCount; - + MeshDropSettings m_settings; + MeshDropDepth m_depth; b3MeshData* m_groundMesh; - float m_groundAmplitude; b3BodyId m_groundId; b3HullData* m_cylinder; - b3BodyId* m_bodies; - ShapeType m_shapeType; + b3BodyId m_bodies[MESH_DROP_BODY_COUNT]; + uint32_t m_seed; int m_runCount; bool m_failure; bool m_autoGenerate; - bool m_collide; + uint32_t m_replaySeed = 0; + int m_solo[2] = { -1, -1 }; }; static int sampleMeshDrop = RegisterSample( "Continuous", "Mesh Drop", MeshDrop::Create ); diff --git a/samples/sample_ghost.cpp b/samples/sample_ghost.cpp new file mode 100644 index 0000000..3ce5e77 --- /dev/null +++ b/samples/sample_ghost.cpp @@ -0,0 +1,3749 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "ghost_collisions.h" +#include "sample.h" +#include "utils.h" + +#include "gfx/draw.h" + +#include "box3d/box3d.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Scenes for comparing ghost collision handling across physics engines. Each one isolates a mechanism +// where engines differ: crest angles around the active edge cutoffs, high valence vertex fans, saddle +// vertices, long runs through grid vertices, and large bodies on fine meshes. Energy, vertical velocity +// and drift only need body state, so the same scenes can be measured in any engine. The ghost count +// also needs contact normals and the triangle feature behind each point. + +enum ProbeShape +{ + e_probeBox, + e_probeCylinder, + e_probeSphere, + e_probeCapsule, + e_probeShapeCount +}; + +static const char* s_probeShapeNames[e_probeShapeCount] = { "Box", "Cylinder", "Sphere", "Capsule" }; + +// Box and cylinder slide without friction. Sphere and capsule roll. +static bool IsRoller( int shape ) +{ + return shape == e_probeSphere || shape == e_probeCapsule; +} + +static float GetProbeRadius( int shape ) +{ + return shape == e_probeSphere ? 0.25f : 0.2f; +} + +enum DiagonalMode +{ + e_diagonalForward, + e_diagonalBackward, + e_diagonalAlternate, + e_diagonalModeCount +}; + +static const char* s_diagonalNames[e_diagonalModeCount] = { "Along +x+z", "Along +x-z", "Alternate" }; + +static std::vector MakeSamples( float lower, float upper, int count ) +{ + std::vector samples( count + 1 ); + for ( int i = 0; i <= count; ++i ) + { + samples[i] = lower + ( upper - lower ) * (float)i / (float)count; + } + return samples; +} + +// Rows run along x. Triangles wind counter clockwise seen from above. +static void AddGrid( std::vector& vertices, std::vector& indices, const std::vector& xs, + const std::vector& zs, const std::function& height, int mode ) +{ + int base = (int)vertices.size(); + int stride = (int)zs.size(); + for ( int i = 0; i < (int)xs.size(); ++i ) + { + for ( int j = 0; j < stride; ++j ) + { + vertices.push_back( { xs[i], height( i, j ), zs[j] } ); + } + } + + int xCount = (int)xs.size() - 1; + int zCount = stride - 1; + for ( int i = 0; i < xCount; ++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; + + bool forward = mode == e_diagonalForward || ( mode == e_diagonalAlternate && ( ( i + j ) & 1 ) == 0 ); + if ( forward ) + { + int32_t corners[6] = { a, b, c, c, d, a }; + indices.insert( indices.end(), corners, corners + 6 ); + } + else + { + int32_t corners[6] = { a, b, d, b, c, d }; + indices.insert( indices.end(), corners, corners + 6 ); + } + } + } +} + +static void AddUpTriangle( const std::vector& vertices, std::vector& indices, int a, int b, int c ) +{ + b3Vec3 normal = b3Cross( vertices[b] - vertices[a], vertices[c] - vertices[a] ); + if ( normal.y < 0.0f ) + { + std::swap( b, c ); + } + + indices.push_back( a ); + indices.push_back( b ); + indices.push_back( c ); +} + +// Linear and rotational kinetic energy per unit mass +static float GetSpecificKinetic( b3BodyId bodyId ) +{ + b3Vec3 v = b3Body_GetLinearVelocity( bodyId ); + b3Vec3 w = b3InvRotateVector( b3Body_GetRotation( bodyId ), b3Body_GetAngularVelocity( bodyId ) ); + b3Matrix3 inertia = b3Body_GetLocalRotationalInertia( bodyId ); + return 0.5f * b3Dot( v, v ) + 0.5f * b3Dot( w, b3MulMV( inertia, w ) ) / b3Body_GetMass( bodyId ); +} + +static float GetSpecificEnergy( b3BodyId bodyId, b3Vec3 gravity ) +{ + b3Vec3 center = b3ToVec3( b3Body_GetWorldCenter( bodyId ) ); + return GetSpecificKinetic( bodyId ) - b3Dot( gravity, center ); +} + +static float NextUnit( uint32_t& state ) +{ + state = 1664525u * state + 1013904223u; + return (float)( state >> 8 ) / 16777216.0f; +} + +// Owns the ground mesh and counts ghost manifolds on it. A pushing manifold is a ghost when its normal +// matches no nearby triangle and does not lie in the normal cone of the edge or vertex that produced it. +class GhostSample : public Sample +{ +public: + static constexpr int m_markerCapacity = 256; + static constexpr int m_historyCapacity = 600; + + enum GhostClass + { + e_faceGhost, + e_edgeGhost, + e_vertexGhost, + e_otherGhost, + e_ghostClassCount + }; + + explicit GhostSample( SampleContext* context ) + : Sample( context ) + { + m_mesh = nullptr; + m_groundId = b3_nullBodyId; + m_followId = b3_nullBodyId; + m_cylinder = b3CreateCylinder( 0.4f, 0.4f, -0.2f, 16 ); + m_toleranceDegrees = 5.0f; + m_cosTolerance = 1.0f; + m_sinTolerance = 0.0f; + m_drawGhosts = true; + m_follow = true; + m_showHistory = true; + m_plotScale = 1.0f; + m_lastHeight = 0.0f; + ResetGhostCounts(); + ClearHistory(); + } + + ~GhostSample() override + { + b3DestroyHull( m_cylinder ); + if ( m_mesh != nullptr ) + { + b3DestroyMesh( m_mesh ); + } + } + +protected: + void CreateGround( std::vector& vertices, std::vector& indices ) + { + if ( B3_IS_NON_NULL( m_groundId ) ) + { + b3DestroyBody( m_groundId ); + m_groundId = b3_nullBodyId; + } + + if ( m_mesh != nullptr ) + { + b3DestroyMesh( m_mesh ); + m_mesh = nullptr; + } + + b3MeshDef def = {}; + def.vertices = vertices.data(); + def.vertexCount = (int)vertices.size(); + def.indices = indices.data(); + def.triangleCount = (int)indices.size() / 3; + def.useMedianSplit = true; + def.identifyEdges = true; + m_mesh = b3CreateMesh( &def, nullptr, 0 ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "ground"; + m_groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateMeshShape( m_groundId, &shapeDef, m_mesh, b3Vec3_one ); + + BuildAdjacency(); + ResetGhostCounts(); + } + + // The direction is horizontal. Rollers start with the spin that matches their speed. The yaw turns a + // box or cylinder about the vertical axis relative to the direction of travel. + b3BodyId CreateProbe( int shape, b3Pos position, b3Vec3 direction, float yawDegrees, float speed ) + { + b3Quat heading = b3MakeQuatFromAxisAngle( b3Vec3_axisY, atan2f( -direction.z, direction.x ) ); + b3Quat yaw = b3MakeQuatFromAxisAngle( b3Vec3_axisY, yawDegrees * B3_DEG_TO_RAD ); + bool roller = IsRoller( shape ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = position; + bodyDef.rotation = roller ? heading : b3MulQuat( heading, yaw ); + bodyDef.linearVelocity = speed * direction; + if ( roller ) + { + bodyDef.angularVelocity = ( speed / GetProbeRadius( shape ) ) * b3Cross( b3Vec3_axisY, direction ); + } + + // Recycling reuses a manifold while the body barely moves, which hides the collision under test + bodyDef.enableContactRecycling = false; + bodyDef.enableSleep = false; + bodyDef.name = "probe"; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = roller ? 0.6f : 0.0f; + + switch ( shape ) + { + case e_probeBox: + { + b3BoxHull box = b3MakeBoxHull( 0.4f, 0.2f, 0.4f ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + break; + + case e_probeCylinder: + b3CreateHullShape( bodyId, &shapeDef, m_cylinder ); + break; + + case e_probeSphere: + { + b3Sphere sphere = { b3Vec3_zero, GetProbeRadius( shape ) }; + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + } + break; + + default: + { + // Axis across the direction of travel so it rolls + b3Capsule capsule = { { 0.0f, 0.0f, -0.4f }, { 0.0f, 0.0f, 0.4f }, GetProbeRadius( shape ) }; + b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + } + break; + } + + return bodyId; + } + + void BuildAdjacency() + { + int triangleCount = m_mesh->triangleCount; + int vertexCount = m_mesh->vertexCount; + const b3MeshTriangle* triangles = b3GetMeshTriangles( m_mesh ); + const b3Vec3* vertices = b3GetMeshVertices( m_mesh ); + + m_triangleNormals.resize( triangleCount ); + m_edgeNeighbors.assign( 3 * triangleCount, B3_NULL_INDEX ); + m_vertexRings.assign( vertexCount, {} ); + m_vertexTriangles.assign( vertexCount, {} ); + + // Sorting by vertex pair puts the two sides of a shared edge next to each other + std::vector> edges; + edges.reserve( 3 * triangleCount ); + + for ( int t = 0; t < triangleCount; ++t ) + { + int v[3] = { triangles[t].index1, triangles[t].index2, triangles[t].index3 }; + m_triangleNormals[t] = b3Normalize( b3Cross( vertices[v[1]] - vertices[v[0]], vertices[v[2]] - vertices[v[0]] ) ); + + for ( int e = 0; e < 3; ++e ) + { + int a = v[e]; + int b = v[e < 2 ? e + 1 : 0]; + uint64_t key = ( (uint64_t)b3MinInt( a, b ) << 32 ) | (uint64_t)b3MaxInt( a, b ); + edges.push_back( { key, 3 * t + e } ); + m_vertexRings[a].push_back( b ); + m_vertexRings[b].push_back( a ); + m_vertexTriangles[a].push_back( t ); + } + } + + std::sort( edges.begin(), edges.end() ); + for ( size_t i = 0; i + 1 < edges.size(); ++i ) + { + // A non-manifold edge leaves every side without a neighbor + uint64_t key = edges[i].first; + bool pair = edges[i + 1].first == key; + bool single = ( i + 2 >= edges.size() || edges[i + 2].first != key ) && ( i == 0 || edges[i - 1].first != key ); + if ( pair && single ) + { + m_edgeNeighbors[edges[i].second] = edges[i + 1].second / 3; + m_edgeNeighbors[edges[i + 1].second] = edges[i].second / 3; + } + } + + for ( std::vector& ring : m_vertexRings ) + { + std::sort( ring.begin(), ring.end() ); + ring.erase( std::unique( ring.begin(), ring.end() ), ring.end() ); + } + } + + bool MatchesFace( b3Vec3 normal, int triangleIndex ) const + { + return b3Dot( normal, m_triangleNormals[triangleIndex] ) >= m_cosTolerance; + } + + // Direction in the face plane pointing away from the edge into the face + static b3Vec3 GetInwardDirection( b3Vec3 edgeStart, b3Vec3 axis, b3Vec3 opposite ) + { + b3Vec3 d = opposite - edgeStart; + return b3Normalize( d - b3Dot( d, axis ) * axis ); + } + + // The normal cone of a convex edge is the arc between its two face normals. Neither face may rise + // above the edge along the normal, so a concave edge only admits its face normals. + bool AdmitsEdge( b3Vec3 normal, int triangleIndex, int edgeIndex ) const + { + int neighbor = m_edgeNeighbors[3 * triangleIndex + edgeIndex]; + if ( MatchesFace( normal, triangleIndex ) || ( neighbor != B3_NULL_INDEX && MatchesFace( normal, neighbor ) ) ) + { + return true; + } + + const b3MeshTriangle* triangles = b3GetMeshTriangles( m_mesh ); + const b3Vec3* vertices = b3GetMeshVertices( m_mesh ); + int v[3] = { triangles[triangleIndex].index1, triangles[triangleIndex].index2, triangles[triangleIndex].index3 }; + int ia = v[edgeIndex]; + int ib = v[edgeIndex < 2 ? edgeIndex + 1 : 0]; + int ic = v[edgeIndex > 0 ? edgeIndex - 1 : 2]; + + b3Vec3 a = vertices[ia]; + b3Vec3 axis = b3Normalize( vertices[ib] - a ); + if ( b3AbsFloat( b3Dot( normal, axis ) ) > m_sinTolerance ) + { + return false; + } + + if ( b3Dot( normal, GetInwardDirection( a, axis, vertices[ic] ) ) > m_sinTolerance ) + { + return false; + } + + if ( neighbor != B3_NULL_INDEX ) + { + int w[3] = { triangles[neighbor].index1, triangles[neighbor].index2, triangles[neighbor].index3 }; + for ( int k = 0; k < 3; ++k ) + { + if ( w[k] != ia && w[k] != ib && b3Dot( normal, GetInwardDirection( a, axis, vertices[w[k]] ) ) > m_sinTolerance ) + { + return false; + } + } + } + + return true; + } + + // A vertex normal is valid when the vertex is a local support point in that direction + bool AdmitsVertex( b3Vec3 normal, int vertexIndex ) const + { + for ( int t : m_vertexTriangles[vertexIndex] ) + { + if ( MatchesFace( normal, t ) ) + { + return true; + } + } + + const b3Vec3* vertices = b3GetMeshVertices( m_mesh ); + b3Vec3 vertex = vertices[vertexIndex]; + for ( int other : m_vertexRings[vertexIndex] ) + { + if ( b3Dot( normal, b3Normalize( vertices[other] - vertex ) ) > m_sinTolerance ) + { + return false; + } + } + + return true; + } + + bool AdmitsPoint( b3Vec3 normal, const b3ManifoldPoint& point ) const + { + const b3MeshTriangle& triangle = b3GetMeshTriangles( m_mesh )[point.triangleIndex]; + int v[3] = { triangle.index1, triangle.index2, triangle.index3 }; + + switch ( point.triangleFeature ) + { + case b3_featureTriangleFace: + return MatchesFace( normal, point.triangleIndex ); + + case b3_featureEdge1: + case b3_featureEdge2: + case b3_featureEdge3: + return AdmitsEdge( normal, point.triangleIndex, point.triangleFeature - b3_featureEdge1 ); + + case b3_featureVertex1: + case b3_featureVertex2: + case b3_featureVertex3: + return AdmitsVertex( normal, v[point.triangleFeature - b3_featureVertex1] ); + + default: + // No feature was resolved, so any feature of the triangle may own the normal + if ( MatchesFace( normal, point.triangleIndex ) ) + { + return true; + } + + for ( int k = 0; k < 3; ++k ) + { + if ( AdmitsEdge( normal, point.triangleIndex, k ) || AdmitsVertex( normal, v[k] ) ) + { + return true; + } + } + + return false; + } + } + + static GhostClass Classify( const b3ManifoldPoint& point ) + { + switch ( point.triangleFeature ) + { + case b3_featureTriangleFace: + return e_faceGhost; + + case b3_featureEdge1: + case b3_featureEdge2: + case b3_featureEdge3: + return e_edgeGhost; + + case b3_featureVertex1: + case b3_featureVertex2: + case b3_featureVertex3: + return e_vertexGhost; + + default: + return e_otherGhost; + } + } + + // The center is the body center at the start of the step, where the anchors were computed + int CountGhosts( b3BodyId bodyId, b3Pos center ) + { + // Clustering merges manifolds whose normals agree within its threshold and hands them one normal, + // so a point may carry a normal that far from its own feature + float angle = m_toleranceDegrees * B3_DEG_TO_RAD + acosf( 0.996f ); + m_cosTolerance = cosf( angle ); + m_sinTolerance = sinf( angle ); + + memset( m_stepClassCounts, 0, sizeof( m_stepClassCounts ) ); + int ghostCount = 0; + + // A pile box touches many neighbors besides the ground + m_contacts.resize( b3MaxInt( 1, b3Body_GetContactCapacity( bodyId ) ) ); + b3ContactData* contacts = m_contacts.data(); + int contactCount = b3Body_GetContactData( bodyId, contacts, (int)m_contacts.size() ); + for ( int i = 0; i < contactCount; ++i ) + { + bool bodyIsA = B3_ID_EQUALS( b3Shape_GetBody( contacts[i].shapeIdA ), bodyId ); + 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; + b3Vec3 normal = bodyIsA ? -manifold->normal : manifold->normal; + + // Speculative points that never pushed are harmless. Clustering hands every point the normal + // of one member, so the manifold is sound if any pushing point owns that normal. + int pushCount = 0; + int firstPush = 0; + bool admitted = false; + for ( int k = 0; k < manifold->pointCount && admitted == false; ++k ) + { + const b3ManifoldPoint& point = manifold->points[k]; + if ( point.totalNormalImpulse <= 0.0f ) + { + continue; + } + + firstPush = pushCount == 0 ? k : firstPush; + pushCount += 1; + admitted = AdmitsPoint( normal, point ); + } + + if ( pushCount == 0 ) + { + continue; + } + + m_manifoldCount += 1; + + if ( admitted ) + { + continue; + } + + const b3ManifoldPoint& point = manifold->points[firstPush]; + GhostClass ghostClass = Classify( point ); + m_classCounts[ghostClass] += 1; + m_stepClassCounts[ghostClass] += 1; + m_ghostCount += 1; + ghostCount += 1; + + b3Vec3 anchor = bodyIsA ? point.anchorA : point.anchorB; + m_markers[m_markerIndex] = { b3OffsetPos( center, anchor ), normal }; + m_markerIndex = ( m_markerIndex + 1 ) % m_markerCapacity; + m_markerCount = b3MinInt( m_markerCount + 1, m_markerCapacity ); + } + } + + return ghostCount; + } + + void ResetGhostCounts() + { + memset( m_classCounts, 0, sizeof( m_classCounts ) ); + memset( m_stepClassCounts, 0, sizeof( m_stepClassCounts ) ); + m_ghostCount = 0; + m_manifoldCount = 0; + m_markerCount = 0; + m_markerIndex = 0; + } + + virtual void ResetCounters() + { + ResetGhostCounts(); + } + + void ClearHistory() + { + m_historyCount = 0; + m_historyIndex = 0; + } + + // From the height change over the step, so push out counts and every engine is measured the same way. + // A fast rolling body can report a steady downward velocity that never moves it. + float GetVerticalSpeed( float height ) + { + float speed = ( height - m_lastHeight ) * m_context->hertz; + m_lastHeight = height; + return speed; + } + + void PushHistory( float value ) + { + m_history[m_historyIndex] = value; + m_historyIndex = ( m_historyIndex + 1 ) % m_historyCapacity; + m_historyCount = b3MinInt( m_historyCount + 1, m_historyCapacity ); + } + + void DrawGhosts() + { + if ( m_drawGhosts ) + { + for ( int i = 0; i < m_markerCount; ++i ) + { + b3Pos p = m_markers[i].point; + DrawPoint( p, 8.0f, MakeColor( b3_colorRed ) ); + DrawLine( p, p + 0.5f * m_markers[i].normal, MakeColor( b3_colorRed ) ); + } + } + + if ( m_follow && B3_IS_NON_NULL( m_followId ) ) + { + m_camera->SetPivot( b3Body_GetPosition( m_followId ) ); + } + } + + void DrawGhostSummary() + { + DrawTextLine( "ghost manifolds: %d of %d pushing, face %d, edge %d, vertex %d, unresolved %d", m_ghostCount, + m_manifoldCount, m_classCounts[e_faceGhost], m_classCounts[e_edgeGhost], m_classCounts[e_vertexGhost], + m_classCounts[e_otherGhost] ); + DrawTextLine( "triangles: %d, mesh bytes: %d", m_mesh->triangleCount, m_mesh->byteCount ); + } + + void DrawGhostControls() + { + ImGui::Separator(); + ImGui::SliderFloat( "Ghost Tolerance", &m_toleranceDegrees, 1.0f, 20.0f, "%.0f deg" ); + ImGui::Checkbox( "Draw Ghosts", &m_drawGhosts ); + ImGui::Checkbox( "Follow", &m_follow ); + if ( ImGui::Button( "Reset Counters" ) ) + { + ResetCounters(); + } + + if ( m_showHistory ) + { + ImGui::SliderFloat( "Plot Scale", &m_plotScale, 0.1f, 5.0f, "%.1f m/s" ); + int offset = m_historyCount < m_historyCapacity ? 0 : m_historyIndex; + ImGui::PlotLines( "##vy", m_history, m_historyCount, offset, "vertical velocity", -m_plotScale, m_plotScale, + ImVec2( 0.0f, 80.0f ) ); + } + } + + struct Marker + { + b3Pos point; + b3Vec3 normal; + }; + + b3MeshData* m_mesh; + b3BodyId m_groundId; + b3BodyId m_followId; + b3HullData* m_cylinder; + + std::vector m_triangleNormals; + std::vector m_edgeNeighbors; + std::vector> m_vertexRings; + std::vector> m_vertexTriangles; + std::vector m_contacts; + + float m_toleranceDegrees; + float m_cosTolerance; + float m_sinTolerance; + int m_classCounts[e_ghostClassCount]; + int m_stepClassCounts[e_ghostClassCount]; + int m_ghostCount; + int m_manifoldCount; + + Marker m_markers[m_markerCapacity]; + int m_markerCount; + int m_markerIndex; + + float m_history[m_historyCapacity]; + int m_historyCount; + int m_historyIndex; + float m_plotScale; + float m_lastHeight; + + bool m_drawGhosts; + bool m_follow; + bool m_showHistory; +}; + +// Roof ridges with crest angles from 1 to 20 degrees, separated by flat runs. The angles straddle the +// cutoffs of engines that keep one convex bit per edge, about 5 degrees in Jolt and PhysX, while Box3D +// stores the angle in 1.4 degree steps. Each crest reports, from its foot to the next foot, the energy +// lost and the peak vertical velocity. The slope alone lifts the body at its speed times the sine of +// half the crest angle. +class CrestLadder : public GhostSample +{ +public: + static constexpr int m_crestCount = 9; + static constexpr float m_slopeRun = 1.5f; + static constexpr float m_gap = 2.5f; + static constexpr float m_leadIn = 5.0f; + static constexpr float m_runOut = 5.0f; + static constexpr float m_halfWidth = 2.0f; + + struct CrestStats + { + float startEnergy; + float startKinetic; + float slopeVelocity; + float peakVelocity; + float loss; + int ghosts; + bool valid; + }; + + explicit CrestLadder( SampleContext* context ) + : GhostSample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 60.0f, 20.0f, 12.0f, { 2.0f, 0.5f, 0.0f } ); + } + + m_probe = e_probeBox; + m_speed = 4.0f; + m_yaw = 0.0f; + m_cellSize = 0.25f; + m_diagonal = e_diagonalAlternate; + m_probeId = b3_nullBodyId; + + BuildGround(); + ResetCounters(); + Launch(); + } + + void ResetCounters() override + { + GhostSample::ResetCounters(); + memset( m_current, 0, sizeof( m_current ) ); + memset( m_last, 0, sizeof( m_last ) ); + memset( m_crestGhosts, 0, sizeof( m_crestGhosts ) ); + m_runCount = 0; + } + + void BuildGround() + { + std::vector corners; + corners.push_back( { 0.0f, 0.0f } ); + + float x = m_leadIn; + for ( int i = 0; i < m_crestCount; ++i ) + { + float rise = m_slopeRun * tanf( 0.5f * m_angles[i] * B3_DEG_TO_RAD ); + m_windowStart[i] = x; + m_crestPoints[i] = { x + m_slopeRun, rise }; + corners.push_back( { x, 0.0f } ); + corners.push_back( { x + m_slopeRun, rise } ); + corners.push_back( { x + 2.0f * m_slopeRun, 0.0f } ); + x += 2.0f * m_slopeRun + m_gap; + } + + m_windowEnd = x; + m_length = x + m_runOut; + corners.push_back( { m_length, 0.0f } ); + + // Every corner lands on a vertex row + std::vector xs; + std::vector ys; + for ( size_t k = 0; k + 1 < corners.size(); ++k ) + { + b3Vec2 a = corners[k]; + b3Vec2 b = corners[k + 1]; + int count = b3MaxInt( 1, (int)ceilf( ( b.x - a.x ) / m_cellSize - 0.001f ) ); + for ( int s = 0; s < count; ++s ) + { + float t = (float)s / (float)count; + xs.push_back( a.x + t * ( b.x - a.x ) ); + ys.push_back( a.y + t * ( b.y - a.y ) ); + } + } + xs.push_back( corners.back().x ); + ys.push_back( corners.back().y ); + + int zCount = b3MaxInt( 1, (int)ceilf( 2.0f * m_halfWidth / m_cellSize - 0.001f ) ); + std::vector zs = MakeSamples( -m_halfWidth, m_halfWidth, zCount ); + + std::vector vertices; + std::vector indices; + AddGrid( vertices, indices, xs, zs, [&]( int i, int ) { return ys[i]; }, m_diagonal ); + CreateGround( vertices, indices ); + } + + void Launch() + { + if ( B3_IS_NON_NULL( m_probeId ) ) + { + b3DestroyBody( m_probeId ); + } + + b3Pos position = { 2.0f, GetProbeRadius( m_probe ) + 0.01f, 0.0f }; + m_probeId = CreateProbe( m_probe, position, b3Vec3_axisX, m_yaw, m_speed ); + m_followId = m_probeId; + m_lastHeight = (float)position.y; + m_window = -1; + m_runSteps = 0; + memset( m_current, 0, sizeof( m_current ) ); + ClearHistory(); + } + + int FindWindow( float x ) const + { + for ( int i = 0; i < m_crestCount; ++i ) + { + float end = i + 1 < m_crestCount ? m_windowStart[i + 1] : m_windowEnd; + if ( m_windowStart[i] <= x && x < end ) + { + return i; + } + } + + return -1; + } + + void OpenWindow( int index ) + { + b3Vec3 gravity = b3World_GetGravity( m_worldId ); + float speed = b3Length( b3Body_GetLinearVelocity( m_probeId ) ); + CrestStats& stats = m_current[index]; + stats = {}; + stats.startEnergy = GetSpecificEnergy( m_probeId, gravity ); + stats.startKinetic = GetSpecificKinetic( m_probeId ); + stats.slopeVelocity = speed * sinf( 0.5f * m_angles[index] * B3_DEG_TO_RAD ); + stats.peakVelocity = -FLT_MAX; + } + + void CloseWindow( int index ) + { + b3Vec3 gravity = b3World_GetGravity( m_worldId ); + CrestStats& stats = m_current[index]; + float lost = stats.startEnergy - GetSpecificEnergy( m_probeId, gravity ); + stats.loss = stats.startKinetic > 0.0f ? 100.0f * lost / stats.startKinetic : 0.0f; + stats.valid = true; + } + + void Step() override + { + b3Pos center = b3Body_GetWorldCenter( m_probeId ); + + Sample::Step(); + + if ( m_didStep ) + { + int ghostCount = CountGhosts( m_probeId, center ); + + b3Pos position = b3Body_GetWorldCenter( m_probeId ); + float verticalSpeed = GetVerticalSpeed( (float)position.y ); + PushHistory( verticalSpeed ); + + int window = FindWindow( (float)position.x ); + if ( window != m_window ) + { + if ( m_window >= 0 ) + { + CloseWindow( m_window ); + } + + if ( window >= 0 ) + { + OpenWindow( window ); + } + + m_window = window; + } + + if ( m_window >= 0 ) + { + CrestStats& stats = m_current[m_window]; + stats.ghosts += ghostCount; + stats.peakVelocity = b3MaxFloat( stats.peakVelocity, verticalSpeed ); + m_crestGhosts[m_window] += ghostCount; + } + + m_runSteps += 1; + bool done = (float)position.x > m_length - 3.0f || (float)position.y < -2.0f || m_runSteps > 1800; + if ( done ) + { + if ( m_window >= 0 ) + { + CloseWindow( m_window ); + } + + memcpy( m_last, m_current, sizeof( m_last ) ); + m_runCount += 1; + Launch(); + } + } + + DrawGhosts(); + + for ( int i = 0; i < m_crestCount; ++i ) + { + b3Pos p = { m_crestPoints[i].x, m_crestPoints[i].y + 0.6f, m_halfWidth }; + DrawString3D( p, MakeColor( b3_colorWhite ), "%.0f deg", m_angles[i] ); + } + + DrawTextLine( "completed runs: %d, crest values from the last run, ghosts over all runs", m_runCount ); + DrawGhostSummary(); + + const CrestStats* stats = m_runCount > 0 ? m_last : m_current; + for ( int i = 0; i < m_crestCount; ++i ) + { + if ( stats[i].valid ) + { + DrawTextLine( "crest %2.0f deg: ghosts %d, energy loss %5.1f%%, peak vy %.2f m/s, slope vy %.2f m/s", m_angles[i], + m_crestGhosts[i], stats[i].loss, stats[i].peakVelocity, stats[i].slopeVelocity ); + } + else + { + DrawTextLine( "crest %2.0f deg: ghosts %d", m_angles[i], m_crestGhosts[i] ); + } + } + } + + bool DrawControls() override + { + bool rebuild = false; + bool relaunch = false; + + relaunch |= ImGui::Combo( "Probe", &m_probe, s_probeShapeNames, e_probeShapeCount ); + relaunch |= ImGui::SliderFloat( "Speed", &m_speed, 0.5f, 15.0f, "%.1f m/s" ); + relaunch |= ImGui::SliderFloat( "Yaw", &m_yaw, 0.0f, 45.0f, "%.0f deg" ); + rebuild |= ImGui::SliderFloat( "Cell Size", &m_cellSize, 0.1f, 1.0f, "%.2f m" ); + rebuild |= ImGui::Combo( "Diagonals", &m_diagonal, s_diagonalNames, e_diagonalModeCount ); + relaunch |= ImGui::Button( "Launch" ); + + DrawGhostControls(); + + if ( rebuild ) + { + BuildGround(); + ResetCounters(); + } + + if ( rebuild || relaunch ) + { + Launch(); + } + + return true; + } + + static Sample* Create( SampleContext* context ) + { + return new CrestLadder( context ); + } + + static constexpr float m_angles[m_crestCount] = { 1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 8.0f, 12.0f, 20.0f }; + + b3BodyId m_probeId; + int m_probe; + float m_speed; + float m_yaw; + float m_cellSize; + int m_diagonal; + + float m_windowStart[m_crestCount]; + b3Vec2 m_crestPoints[m_crestCount]; + float m_windowEnd; + float m_length; + + CrestStats m_current[m_crestCount]; + CrestStats m_last[m_crestCount]; + int m_crestGhosts[m_crestCount]; + int m_window; + int m_runSteps; + int m_runCount; +}; + +static int sampleCrestLadder = RegisterSample( "Ghost", "Crest Ladder", CrestLadder::Create ); + +// A cone apex shared by 6 to 48 triangles with uneven spacing, so some triangles near the apex are +// slivers. Every spoke is a crest whose dihedral angle shrinks as the fan grows, so each engine's edge +// cutoff falls somewhere in the range. Vertex handling is where engines differ most. The probe crosses +// the apex along eight headings in turn. +class HubPeak : public GhostSample +{ +public: + static constexpr int m_headingCount = 8; + static constexpr int m_ringCount = 10; + static constexpr float m_ringSpacing = 1.0f; + static constexpr float m_coneRadius = 3.0f; + static constexpr float m_launchDistance = 6.5f; + + enum HubShape + { + e_hubPeak, + e_hubPit, + e_hubSaddle, + e_hubShapeCount + }; + + struct RunStats + { + float startEnergy; + float startKinetic; + float peakVelocity; + float loss; + int ghosts; + int vertexGhosts; + bool valid; + }; + + explicit HubPeak( SampleContext* context ) + : GhostSample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 16.0f, { 0.0f, 0.5f, 0.0f } ); + } + + m_follow = false; + m_hubShape = e_hubPeak; + m_fanIndex = 1; + m_slopeDegrees = 10.0f; + m_jitter = 0.3f; + m_probe = e_probeBox; + m_speed = 5.0f; + m_yaw = 0.0f; + m_offset = 0.0f; + m_heading = 0; + m_probeId = b3_nullBodyId; + + BuildGround(); + ResetCounters(); + Launch(); + } + + void ResetCounters() override + { + GhostSample::ResetCounters(); + memset( m_last, 0, sizeof( m_last ) ); + m_runCount = 0; + } + + float GetHeight( float radius, float angle ) const + { + if ( radius >= m_coneRadius ) + { + return 0.0f; + } + + float slope = tanf( m_slopeDegrees * B3_DEG_TO_RAD ); + switch ( m_hubShape ) + { + case e_hubPeak: + return slope * ( m_coneRadius - radius ); + + case e_hubPit: + return -slope * ( m_coneRadius - radius ); + + default: + // Three ridges and three valleys meet at the center, the slope there is set by the angle + return slope * radius * ( 1.0f - radius / m_coneRadius ) * cosf( 3.0f * angle ); + } + } + + void BuildGround() + { + int fanCount = m_fanCounts[m_fanIndex]; + + // Fixed seed so a fan count always gets the same slivers + uint32_t state = 12345; + std::vector angles( fanCount ); + for ( int i = 0; i < fanCount; ++i ) + { + float u = NextUnit( state ) - 0.5f; + angles[i] = 2.0f * B3_PI * ( (float)i + m_jitter * u ) / (float)fanCount; + } + + std::vector vertices; + std::vector indices; + vertices.push_back( { 0.0f, GetHeight( 0.0f, 0.0f ), 0.0f } ); + for ( int k = 1; k <= m_ringCount; ++k ) + { + float radius = m_ringSpacing * (float)k; + for ( int i = 0; i < fanCount; ++i ) + { + float angle = angles[i]; + vertices.push_back( { radius * cosf( angle ), GetHeight( radius, angle ), radius * sinf( angle ) } ); + } + } + + for ( int i = 0; i < fanCount; ++i ) + { + AddUpTriangle( vertices, indices, 0, 1 + i, 1 + ( i + 1 ) % fanCount ); + } + + // Dihedral angles between neighboring apex triangles + m_minDihedral = FLT_MAX; + m_maxDihedral = 0.0f; + for ( int i = 0; i < fanCount; ++i ) + { + const int32_t* t1 = indices.data() + 3 * i; + const int32_t* t2 = indices.data() + 3 * ( ( i + 1 ) % fanCount ); + b3Vec3 n1 = b3Normalize( b3Cross( vertices[t1[1]] - vertices[t1[0]], vertices[t1[2]] - vertices[t1[0]] ) ); + b3Vec3 n2 = b3Normalize( b3Cross( vertices[t2[1]] - vertices[t2[0]], vertices[t2[2]] - vertices[t2[0]] ) ); + float dihedral = acosf( b3ClampFloat( b3Dot( n1, n2 ), -1.0f, 1.0f ) ) / B3_DEG_TO_RAD; + m_minDihedral = b3MinFloat( m_minDihedral, dihedral ); + m_maxDihedral = b3MaxFloat( m_maxDihedral, dihedral ); + } + + for ( int k = 1; k < m_ringCount; ++k ) + { + int inner = 1 + ( k - 1 ) * fanCount; + int outer = inner + fanCount; + for ( int i = 0; i < fanCount; ++i ) + { + int next = ( i + 1 ) % fanCount; + int a = inner + i; + int b = inner + next; + int c = outer + next; + int d = outer + i; + + if ( ( i + k ) & 1 ) + { + AddUpTriangle( vertices, indices, a, b, c ); + AddUpTriangle( vertices, indices, a, c, d ); + } + else + { + AddUpTriangle( vertices, indices, a, b, d ); + AddUpTriangle( vertices, indices, b, c, d ); + } + } + } + + CreateGround( vertices, indices ); + } + + b3Vec3 GetDirection() const + { + float angle = (float)m_heading * 2.0f * B3_PI / (float)m_headingCount; + return { cosf( angle ), 0.0f, sinf( angle ) }; + } + + void Launch() + { + if ( B3_IS_NON_NULL( m_probeId ) ) + { + b3DestroyBody( m_probeId ); + } + + b3Vec3 d = GetDirection(); + b3Vec3 perp = { -d.z, 0.0f, d.x }; + b3Vec3 start = -m_launchDistance * d + m_offset * perp; + start.y = GetProbeRadius( m_probe ) + 0.01f; + m_start = start; + + m_probeId = CreateProbe( m_probe, b3ToPos( start ), d, m_yaw, m_speed ); + m_followId = m_probeId; + m_lastHeight = start.y; + m_runSteps = 0; + + m_current = {}; + m_current.startEnergy = GetSpecificEnergy( m_probeId, b3World_GetGravity( m_worldId ) ); + m_current.startKinetic = GetSpecificKinetic( m_probeId ); + m_current.peakVelocity = -FLT_MAX; + ClearHistory(); + } + + void Step() override + { + b3Pos center = b3Body_GetWorldCenter( m_probeId ); + + Sample::Step(); + + if ( m_didStep ) + { + int ghostCount = CountGhosts( m_probeId, center ); + m_current.ghosts += ghostCount; + m_current.vertexGhosts += m_stepClassCounts[e_vertexGhost]; + + b3Vec3 position = b3ToVec3( b3Body_GetWorldCenter( m_probeId ) ); + float verticalSpeed = GetVerticalSpeed( position.y ); + m_current.peakVelocity = b3MaxFloat( m_current.peakVelocity, verticalSpeed ); + PushHistory( verticalSpeed ); + + b3Vec3 d = GetDirection(); + float along = b3Dot( position - m_start, d ); + float radius = sqrtf( position.x * position.x + position.z * position.z ); + + m_runSteps += 1; + bool done = along > 2.0f * m_launchDistance || position.y < -4.0f || radius > 9.0f || m_runSteps > 900; + if ( done ) + { + float lost = m_current.startEnergy - GetSpecificEnergy( m_probeId, b3World_GetGravity( m_worldId ) ); + m_current.loss = m_current.startKinetic > 0.0f ? 100.0f * lost / m_current.startKinetic : 0.0f; + m_current.valid = true; + m_last[m_heading] = m_current; + m_runCount += 1; + m_heading = ( m_heading + 1 ) % m_headingCount; + Launch(); + } + } + + DrawGhosts(); + + b3Vec3 d = GetDirection(); + b3Vec3 tail = m_start + 0.5f * b3Vec3_axisY; + DrawArrow( b3ToPos( tail ), b3ToPos( tail + 1.5f * d ), MakeColor( b3_colorYellow ) ); + + DrawTextLine( "fan %d, apex dihedral angles %.1f to %.1f deg, completed runs %d", m_fanCounts[m_fanIndex], m_minDihedral, + m_maxDihedral, m_runCount ); + DrawGhostSummary(); + + for ( int i = 0; i < m_headingCount; ++i ) + { + int degrees = i * 360 / m_headingCount; + const RunStats& stats = m_last[i]; + if ( stats.valid ) + { + DrawTextLine( "heading %3d deg: ghosts %d (vertex %d), energy loss %5.1f%%, peak vy %.2f m/s", degrees, stats.ghosts, + stats.vertexGhosts, stats.loss, stats.peakVelocity ); + } + else + { + DrawTextLine( "heading %3d deg: pending", degrees ); + } + } + } + + bool DrawControls() override + { + bool rebuild = false; + bool relaunch = false; + + const char* shapeNames[e_hubShapeCount] = { "Peak", "Pit", "Saddle" }; + const char* fanNames[] = { "6", "12", "24", "48" }; + rebuild |= ImGui::Combo( "Hub", &m_hubShape, shapeNames, e_hubShapeCount ); + rebuild |= ImGui::Combo( "Fan", &m_fanIndex, fanNames, IM_ARRAYSIZE( fanNames ) ); + rebuild |= ImGui::SliderFloat( "Slope", &m_slopeDegrees, 1.0f, 20.0f, "%.0f deg" ); + rebuild |= ImGui::SliderFloat( "Jitter", &m_jitter, 0.0f, 0.45f, "%.2f" ); + relaunch |= ImGui::Combo( "Probe", &m_probe, s_probeShapeNames, e_probeShapeCount ); + relaunch |= ImGui::SliderFloat( "Speed", &m_speed, 0.5f, 15.0f, "%.1f m/s" ); + relaunch |= ImGui::SliderFloat( "Yaw", &m_yaw, 0.0f, 45.0f, "%.0f deg" ); + relaunch |= ImGui::SliderFloat( "Offset", &m_offset, 0.0f, 0.5f, "%.2f m" ); + relaunch |= ImGui::Button( "Launch" ); + + DrawGhostControls(); + + if ( rebuild ) + { + BuildGround(); + } + + if ( rebuild || relaunch ) + { + ResetCounters(); + m_heading = 0; + Launch(); + } + + return true; + } + + static Sample* Create( SampleContext* context ) + { + return new HubPeak( context ); + } + + static constexpr int m_fanCounts[4] = { 6, 12, 24, 48 }; + + b3BodyId m_probeId; + int m_hubShape; + int m_fanIndex; + float m_slopeDegrees; + float m_jitter; + int m_probe; + float m_speed; + float m_yaw; + float m_offset; + float m_minDihedral; + float m_maxDihedral; + + b3Vec3 m_start; + int m_heading; + int m_runSteps; + int m_runCount; + RunStats m_current; + RunStats m_last[m_headingCount]; +}; + +static int sampleHubPeak = RegisterSample( "Ghost", "Hub Peak", HubPeak::Create ); + +// A single fan whose spokes alternate between ridges and gullies. Each gully is a concave edge, so the +// mesh flags the hub concave, yet every spoke runs downhill and the hub is the closest point for a body +// dropped on it. Every fan triangle has a gully edge, so the edge tests of whichever triangle reports +// the vertex reject a near vertical normal. The contact survives only because the normal is also tested +// against the whole fan. Gullies steeper than the slope lift the ridges above the hub and make a true +// saddle with no normal cone. +class SaddleVertex : public GhostSample +{ +public: + static constexpr float m_fanRadius = 1.5f; + static constexpr int m_maxSpokes = 12; + static constexpr int m_directionCount = 3000; + static constexpr int m_holdSteps = 240; + + // The neighbor tolerance of the mesh collider, about 1 degree + static constexpr float m_edgeTolerance = 0.0175f; + + // Matches the speculative distance of the contact solver + static constexpr float m_reach = 0.02f; + + enum SpokeKind + { + e_convexSpoke, + e_concaveSpoke, + e_flatSpoke + }; + + enum Outcome + { + e_held, + e_fellThrough, + e_rolledOff, + e_outcomeCount + }; + + struct RunStats + { + int outcome; + int reachSteps; + int unsupportedSteps; + float deepest; + bool valid; + }; + + explicit SaddleVertex( SampleContext* context ) + : GhostSample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 6.0f, { 0.0f, 0.6f, 0.0f } ); + } + + m_spokeIndex = 1; + m_slopeDegrees = 20.0f; + m_gullyDegrees = 12.0f; + m_jitter = 0.0f; + m_probe = e_probeSphere; + m_upright = false; + m_dropHeight = 1.0f; + m_offset = 0.0f; + m_yaw = 0.0f; + m_showCone = true; + m_probeId = b3_nullBodyId; + + BuildGround(); + ResetStats(); + Launch(); + } + + void ResetStats() + { + m_last = {}; + memset( m_outcomeCounts, 0, sizeof( m_outcomeCounts ) ); + } + + static b3Vec3 GetUpNormal( b3Vec3 a, b3Vec3 b, b3Vec3 c ) + { + b3Vec3 normal = b3Normalize( b3Cross( b - a, c - a ) ); + return normal.y < 0.0f ? -normal : normal; + } + + void BuildGround() + { + int spokeCount = m_spokeCounts[m_spokeIndex]; + + // Fixed seed so a spoke count always gets the same slivers + uint32_t state = 12345; + + // Even spokes are ridges and odd spokes gullies. The lowest gully end sits at zero. + float hubHeight = m_fanRadius * tanf( ( m_slopeDegrees + m_gullyDegrees ) * B3_DEG_TO_RAD ); + std::vector vertices; + std::vector indices; + vertices.push_back( { 0.0f, hubHeight, 0.0f } ); + for ( int i = 0; i < spokeCount; ++i ) + { + float u = NextUnit( state ) - 0.5f; + float angle = 2.0f * B3_PI * ( (float)i + m_jitter * u ) / (float)spokeCount; + float elevation = ( i & 1 ) ? -( m_slopeDegrees + m_gullyDegrees ) : m_gullyDegrees - m_slopeDegrees; + float height = hubHeight + m_fanRadius * tanf( elevation * B3_DEG_TO_RAD ); + vertices.push_back( { m_fanRadius * cosf( angle ), height, m_fanRadius * sinf( angle ) } ); + } + + for ( int i = 0; i < spokeCount; ++i ) + { + AddUpTriangle( vertices, indices, 0, 1 + i, 1 + ( i + 1 ) % spokeCount ); + } + + CreateGround( vertices, indices ); + + m_spokeCount = spokeCount; + m_hub = vertices[0]; + for ( int i = 0; i < spokeCount; ++i ) + { + m_ring[i] = vertices[1 + i]; + } + + m_minHeight = FLT_MAX; + for ( b3Vec3 v : vertices ) + { + m_minHeight = b3MinFloat( m_minHeight, v.y ); + } + + // A spoke is convex when the next fan triangle bends below the plane of the previous one. The mesh + // stores edge angles in steps of about 1.4 degrees, so a smaller fold counts as flat. + m_convexCount = 0; + m_concaveCount = 0; + m_flatCount = 0; + for ( int i = 0; i < spokeCount; ++i ) + { + b3Vec3 previous = m_ring[( i + spokeCount - 1 ) % spokeCount]; + b3Vec3 next = m_ring[( i + 1 ) % spokeCount]; + b3Vec3 normal1 = GetUpNormal( m_hub, previous, m_ring[i] ); + b3Vec3 normal2 = GetUpNormal( m_hub, m_ring[i], next ); + float dihedral = acosf( b3ClampFloat( b3Dot( normal1, normal2 ), -1.0f, 1.0f ) ) / B3_DEG_TO_RAD; + float lift = b3Dot( normal1, next - m_hub ); + + if ( dihedral < 0.7f ) + { + m_spokeKinds[i] = e_flatSpoke; + m_flatCount += 1; + } + else if ( lift < 0.0f ) + { + m_spokeKinds[i] = e_convexSpoke; + m_convexCount += 1; + } + else + { + m_spokeKinds[i] = e_concaveSpoke; + m_concaveCount += 1; + } + } + + // Spread directions over the sphere and keep the ones the whole fan admits + m_coneDirections.clear(); + float golden = B3_PI * ( 3.0f - sqrtf( 5.0f ) ); + for ( int i = 0; i < m_directionCount; ++i ) + { + float y = 1.0f - 2.0f * ( (float)i + 0.5f ) / (float)m_directionCount; + float r = sqrtf( b3MaxFloat( 0.0f, 1.0f - y * y ) ); + float phi = golden * (float)i; + b3Vec3 direction = { r * cosf( phi ), y, r * sinf( phi ) }; + if ( FanAdmits( direction ) ) + { + m_coneDirections.push_back( direction ); + } + } + } + + // The hub is the closest point for a normal that no spoke leans along + bool FanAdmits( b3Vec3 normal ) const + { + for ( int i = 0; i < m_spokeCount; ++i ) + { + if ( b3Dot( normal, b3Normalize( m_ring[i] - m_hub ) ) > m_edgeTolerance ) + { + return false; + } + } + + return true; + } + + // The two edge tests the collider applies first, against the neighbors across the fan triangle's + // spokes. A normal that leans over either neighbor fails. + bool EdgesAdmit( b3Vec3 normal, int triangle ) const + { + int n = m_spokeCount; + int spokes[2] = { triangle, ( triangle + 1 ) % n }; + int opposites[2] = { ( triangle + n - 1 ) % n, ( triangle + 2 ) % n }; + for ( int j = 0; j < 2; ++j ) + { + b3Vec3 axis = b3Normalize( m_ring[spokes[j]] - m_hub ); + b3Vec3 inward = GetInwardDirection( m_hub, axis, m_ring[opposites[j]] ); + if ( b3Dot( normal, inward ) > m_edgeTolerance ) + { + return false; + } + } + + return true; + } + + // The core radius that the closest point query leaves out + float GetCoreRadius() const + { + return IsRoller( m_probe ) ? GetProbeRadius( m_probe ) : 0.0f; + } + + bool HasGroundSupport() + { + m_contacts.resize( b3MaxInt( 1, b3Body_GetContactCapacity( m_probeId ) ) ); + int contactCount = b3Body_GetContactData( m_probeId, m_contacts.data(), (int)m_contacts.size() ); + for ( int i = 0; i < contactCount; ++i ) + { + const b3ContactData& contact = m_contacts[i]; + for ( int j = 0; j < contact.manifoldCount; ++j ) + { + for ( int k = 0; k < contact.manifolds[j].pointCount; ++k ) + { + if ( contact.manifolds[j].points[k].totalNormalImpulse > 0.0f ) + { + return true; + } + } + } + } + + return false; + } + + void Launch() + { + if ( B3_IS_NON_NULL( m_probeId ) ) + { + b3DestroyBody( m_probeId ); + } + + b3Vec3 start = m_hub + b3Vec3{ m_offset, 0.5f + m_dropHeight, 0.0f }; + m_probeId = CreateProbe( m_probe, b3ToPos( start ), b3Vec3_axisX, m_yaw, 0.0f ); + + // Stands a capsule on its end and a box or cylinder on its side + if ( m_upright ) + { + b3Quat tilt = b3MakeQuatFromAxisAngle( b3Vec3_axisX, 0.5f * B3_PI ); + b3Body_SetTransform( m_probeId, b3ToPos( start ), b3MulQuat( tilt, b3Body_GetRotation( m_probeId ) ) ); + } + + m_runSteps = 0; + m_current = {}; + } + + void Step() override + { + Sample::Step(); + + if ( m_didStep ) + { + b3Vec3 position = b3ToVec3( b3Body_GetWorldCenter( m_probeId ) ); + + b3Vec3 closest; + float distance = b3Body_GetClosestPoint( m_probeId, &closest, m_hub ); + float gap = distance - GetCoreRadius(); + if ( gap <= m_reach ) + { + m_current.reachSteps += 1; + if ( HasGroundSupport() == false ) + { + m_current.unsupportedSteps += 1; + } + + // Inside a hull the query reports zero and the depth is unknown + if ( distance > 0.0f ) + { + m_current.deepest = b3MaxFloat( m_current.deepest, -gap ); + } + } + + m_runSteps += 1; + float radius = sqrtf( position.x * position.x + position.z * position.z ); + bool done = true; + if ( radius < 0.4f * m_fanRadius && position.y < m_hub.y - 0.5f ) + { + m_current.outcome = e_fellThrough; + } + else if ( radius > m_fanRadius + 0.3f || position.y < m_minHeight - 1.0f ) + { + m_current.outcome = e_rolledOff; + } + else if ( m_runSteps >= m_holdSteps ) + { + m_current.outcome = e_held; + } + else + { + done = false; + } + + if ( done ) + { + m_current.valid = true; + m_last = m_current; + m_outcomeCounts[m_current.outcome] += 1; + Launch(); + } + } + + // Direction from the hub to the nearest point of the probe core, straight up once the hub is inside + b3Vec3 closest; + float distance = b3Body_GetClosestPoint( m_probeId, &closest, m_hub ); + b3Vec3 direction = distance > 0.0f ? b3Normalize( closest - m_hub ) : b3Vec3_axisY; + bool fanAdmits = FanAdmits( direction ); + int edgeAdmitCount = 0; + for ( int k = 0; k < m_spokeCount; ++k ) + { + edgeAdmitCount += EdgesAdmit( direction, k ) ? 1 : 0; + } + + if ( m_showCone ) + { + for ( b3Vec3 d : m_coneDirections ) + { + DrawPoint( b3ToPos( m_hub + 0.75f * d ), 4.0f, MakeColor( b3_colorLimeGreen ) ); + } + + b3Vec3 lift = { 0.0f, 0.01f, 0.0f }; + for ( int i = 0; i < m_spokeCount; ++i ) + { + b3HexColor color = m_spokeKinds[i] == e_convexSpoke + ? b3_colorRed + : ( m_spokeKinds[i] == e_concaveSpoke ? b3_colorDodgerBlue : b3_colorGray ); + DrawLine( b3ToPos( m_hub + lift ), b3ToPos( m_ring[i] + lift ), MakeColor( color ) ); + } + + for ( int k = 0; k < m_spokeCount; ++k ) + { + b3Vec3 centroid = ( 1.0f / 3.0f ) * ( m_hub + m_ring[k] + m_ring[( k + 1 ) % m_spokeCount] ) + 2.0f * lift; + b3HexColor color = EdgesAdmit( direction, k ) ? b3_colorLimeGreen : b3_colorRed; + DrawPoint( b3ToPos( centroid ), 10.0f, MakeColor( color ) ); + } + + DrawArrow( b3ToPos( m_hub ), b3ToPos( m_hub + 0.75f * direction ), MakeColor( b3_colorYellow ) ); + } + + DrawTextLine( "hub: %d spokes, %d convex, %d concave, %d flat, the mesh flags it %s", m_spokeCount, m_convexCount, + m_concaveCount, m_flatCount, + m_concaveCount > 0 ? "concave" : ( m_convexCount > 0 ? "convex" : "flat" ) ); + DrawTextLine( "vertex normal cone: %.1f%% of directions, %s", 100.0f * (float)m_coneDirections.size() / (float)m_directionCount, + FanAdmits( b3Vec3_axisY ) ? "contains up" : "does not contain up" ); + DrawTextLine( "probe direction from the hub: the fan %s it, the edge tests admit it on %d of %d fan triangles", + fanAdmits ? "admits" : "rejects", edgeAdmitCount, m_spokeCount ); + DrawTextLine( "green dots: the fan's normal cone, triangle dots: green when the triangle's edge tests admit the arrow" ); + DrawTextLine( "spokes: red convex, blue concave, gray flat" ); + DrawTextLine( "drops: held %d, fell through %d, rolled off %d", m_outcomeCounts[e_held], m_outcomeCounts[e_fellThrough], + m_outcomeCounts[e_rolledOff] ); + + if ( m_last.valid ) + { + const char* outcomeNames[e_outcomeCount] = { "held", "FELL THROUGH", "rolled off" }; + DrawTextLine( "last drop: %s, hub in reach %d steps, %d with no ground contact, deepest %.3f m", + outcomeNames[m_last.outcome], m_last.reachSteps, m_last.unsupportedSteps, m_last.deepest ); + } + else + { + DrawTextLine( "last drop: pending" ); + } + } + + bool DrawControls() override + { + bool rebuild = false; + bool relaunch = false; + + const char* spokeNames[] = { "4", "6", "8", "12" }; + rebuild |= ImGui::Combo( "Spokes", &m_spokeIndex, spokeNames, IM_ARRAYSIZE( spokeNames ) ); + rebuild |= ImGui::SliderFloat( "Slope", &m_slopeDegrees, 5.0f, 30.0f, "%.0f deg" ); + rebuild |= ImGui::SliderFloat( "Gully", &m_gullyDegrees, 0.0f, 40.0f, "%.0f deg" ); + rebuild |= ImGui::SliderFloat( "Jitter", &m_jitter, 0.0f, 0.45f, "%.2f" ); + relaunch |= ImGui::Combo( "Probe", &m_probe, s_probeShapeNames, e_probeShapeCount ); + relaunch |= ImGui::Checkbox( "Upright", &m_upright ); + relaunch |= ImGui::SliderFloat( "Drop Height", &m_dropHeight, 0.0f, 2.0f, "%.2f m" ); + relaunch |= ImGui::SliderFloat( "Offset", &m_offset, 0.0f, 0.5f, "%.2f m" ); + relaunch |= ImGui::SliderFloat( "Yaw", &m_yaw, 0.0f, 45.0f, "%.0f deg" ); + ImGui::Checkbox( "Show Cone", &m_showCone ); + relaunch |= ImGui::Button( "Launch" ); + + if ( rebuild ) + { + BuildGround(); + } + + if ( rebuild || relaunch ) + { + ResetStats(); + Launch(); + } + + return true; + } + + static Sample* Create( SampleContext* context ) + { + return new SaddleVertex( context ); + } + + static constexpr int m_spokeCounts[4] = { 4, 6, 8, 12 }; + + b3BodyId m_probeId; + int m_spokeIndex; + float m_slopeDegrees; + float m_gullyDegrees; + float m_jitter; + int m_probe; + bool m_upright; + float m_dropHeight; + float m_offset; + float m_yaw; + bool m_showCone; + + int m_spokeCount; + b3Vec3 m_hub; + b3Vec3 m_ring[m_maxSpokes]; + int m_spokeKinds[m_maxSpokes]; + int m_convexCount; + int m_concaveCount; + int m_flatCount; + float m_minHeight; + std::vector m_coneDirections; + + int m_runSteps; + RunStats m_current; + RunStats m_last; + int m_outcomeCounts[e_outcomeCount]; +}; + +static int sampleSaddleVertex = RegisterSample( "Ghost", "Saddle Vertex", SaddleVertex::Create ); + +// A long run through grid vertices. At the 45 degree heading the path meets a vertex in every cell, so +// rare vertex misses add up instead of hiding in one crossing. The wave crests and the default diagonals +// both cross that heading, so the path is a mirror line of the mesh and a clean run keeps its line and +// heading. Diagonals along the path fold the wave cells under it into ridges the body rolls off. +class DiagonalRoll : public GhostSample +{ +public: + static constexpr float m_halfExtent = 20.0f; + static constexpr float m_runLength = 50.0f; + static constexpr int m_settleSteps = 10; + + struct RunStats + { + float startEnergy; + float startKinetic; + float sumSquaredVelocity; + float distance; + float loss; + float lateral; + float drift; + int steps; + int ghosts; + bool valid; + }; + + explicit DiagonalRoll( SampleContext* context ) + : GhostSample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 20.0f, 25.0f, 8.0f, { -17.0f, 0.0f, -17.0f } ); + } + + // Gentle enough that a sphere at 5 m/s keeps contact over the crests + m_wave = true; + m_amplitude = 0.1f; + m_wavelength = 6.0f; + m_cellSize = 0.25f; + m_diagonal = e_diagonalBackward; + m_probe = e_probeSphere; + m_speed = 5.0f; + m_headingDegrees = 45.0f; + m_offset = 0.0f; + m_probeId = b3_nullBodyId; + + BuildGround(); + ResetCounters(); + Launch(); + } + + void ResetCounters() override + { + GhostSample::ResetCounters(); + m_last = {}; + m_runCount = 0; + } + + // Crests run along x + z = constant + float GetHeight( float x, float z ) const + { + if ( m_wave == false ) + { + return 0.0f; + } + + return m_amplitude * sinf( 2.0f * B3_PI * ( x + z ) / ( sqrtf( 2.0f ) * m_wavelength ) ); + } + + void BuildGround() + { + // An even count keeps a vertex at the origin, so the heading line through it meets the vertices + int halfCount = b3MaxInt( 1, (int)( m_halfExtent / m_cellSize + 0.5f ) ); + m_actualCellSize = m_halfExtent / (float)halfCount; + std::vector samples = MakeSamples( -m_halfExtent, m_halfExtent, 2 * halfCount ); + + std::vector vertices; + std::vector indices; + AddGrid( vertices, indices, samples, samples, [&]( int i, int j ) { return GetHeight( samples[i], samples[j] ); }, + m_diagonal ); + CreateGround( vertices, indices ); + } + + b3Vec3 GetDirection() const + { + float angle = m_headingDegrees * B3_DEG_TO_RAD; + return { cosf( angle ), 0.0f, sinf( angle ) }; + } + + void Launch() + { + if ( B3_IS_NON_NULL( m_probeId ) ) + { + b3DestroyBody( m_probeId ); + } + + b3Vec3 d = GetDirection(); + b3Vec3 perp = { -d.z, 0.0f, d.x }; + b3Vec3 start = -0.5f * m_runLength * d + m_offset * perp; + start.y = GetHeight( start.x, start.z ) + GetProbeRadius( m_probe ) + 0.01f; + m_start = start; + + m_probeId = CreateProbe( m_probe, b3ToPos( start ), d, 0.0f, m_speed ); + m_followId = m_probeId; + m_lastHeight = start.y; + + m_current = {}; + m_current.startEnergy = GetSpecificEnergy( m_probeId, b3World_GetGravity( m_worldId ) ); + m_current.startKinetic = GetSpecificKinetic( m_probeId ); + ClearHistory(); + } + + void Finish( RunStats& stats ) const + { + b3Vec3 d = GetDirection(); + b3Vec3 perp = { -d.z, 0.0f, d.x }; + b3Vec3 position = b3ToVec3( b3Body_GetWorldCenter( m_probeId ) ); + b3Vec3 velocity = b3Body_GetLinearVelocity( m_probeId ); + + float lost = stats.startEnergy - GetSpecificEnergy( m_probeId, b3World_GetGravity( m_worldId ) ); + stats.loss = stats.startKinetic > 0.0f ? 100.0f * lost / stats.startKinetic : 0.0f; + stats.lateral = b3Dot( position - m_start, perp ); + stats.drift = atan2f( b3Dot( velocity, perp ), b3Dot( velocity, d ) ) / B3_DEG_TO_RAD; + } + + void Step() override + { + b3Pos center = b3Body_GetWorldCenter( m_probeId ); + + Sample::Step(); + + if ( m_didStep ) + { + m_current.ghosts += CountGhosts( m_probeId, center ); + + b3Vec3 position = b3ToVec3( b3Body_GetWorldCenter( m_probeId ) ); + float verticalSpeed = GetVerticalSpeed( position.y ); + m_current.steps += 1; + + // Skip the drop onto the ground + if ( m_current.steps > m_settleSteps ) + { + m_current.sumSquaredVelocity += verticalSpeed * verticalSpeed; + } + m_current.distance = b3Dot( position - m_start, GetDirection() ); + PushHistory( verticalSpeed ); + + float edge = m_halfExtent - 0.5f; + bool done = m_current.distance > m_runLength || b3AbsFloat( position.x ) > edge || b3AbsFloat( position.z ) > edge || + position.y < -3.0f || m_current.steps > 1800; + if ( done ) + { + Finish( m_current ); + m_current.valid = true; + m_last = m_current; + m_runCount += 1; + Launch(); + } + } + + DrawGhosts(); + + b3Vec3 d = GetDirection(); + b3Vec3 lift = { 0.0f, m_wave ? m_amplitude + 0.02f : 0.02f, 0.0f }; + DrawLine( b3ToPos( m_start + lift ), b3ToPos( m_start + m_runLength * d + lift ), MakeColor( b3_colorYellow ) ); + + DrawTextLine( "cell %.3f m, completed runs %d", m_actualCellSize, m_runCount ); + DrawGhostSummary(); + + RunStats current = m_current; + Finish( current ); + DrawRun( "current", current ); + if ( m_last.valid ) + { + DrawRun( "last", m_last ); + } + } + + void DrawRun( const char* label, const RunStats& stats ) + { + int sampleCount = stats.steps - m_settleSteps; + float rms = sampleCount > 0 ? sqrtf( stats.sumSquaredVelocity / (float)sampleCount ) : 0.0f; + float perMeter = stats.distance > 0.0f ? (float)stats.ghosts / stats.distance : 0.0f; + DrawTextLine( "%s: %.1f m, ghosts %d (%.2f per m), energy loss %.1f%%, rms vy %.3f m/s, lateral %.3f m, drift %.2f deg", + label, stats.distance, stats.ghosts, perMeter, stats.loss, rms, stats.lateral, stats.drift ); + } + + bool DrawControls() override + { + bool rebuild = false; + bool relaunch = false; + + rebuild |= ImGui::Checkbox( "Wave", &m_wave ); + rebuild |= ImGui::SliderFloat( "Amplitude", &m_amplitude, 0.0f, 0.5f, "%.2f m" ); + rebuild |= ImGui::SliderFloat( "Wavelength", &m_wavelength, 1.0f, 10.0f, "%.1f m" ); + rebuild |= ImGui::SliderFloat( "Cell Size", &m_cellSize, 0.2f, 1.0f, "%.2f m" ); + rebuild |= ImGui::Combo( "Diagonals", &m_diagonal, s_diagonalNames, e_diagonalModeCount ); + relaunch |= ImGui::Combo( "Probe", &m_probe, s_probeShapeNames, e_probeShapeCount ); + relaunch |= ImGui::SliderFloat( "Speed", &m_speed, 0.5f, 15.0f, "%.1f m/s" ); + relaunch |= ImGui::SliderFloat( "Heading", &m_headingDegrees, 0.0f, 45.0f, "%.0f deg" ); + relaunch |= ImGui::SliderFloat( "Offset", &m_offset, 0.0f, 0.5f, "%.2f m" ); + relaunch |= ImGui::Button( "Launch" ); + + DrawGhostControls(); + + if ( rebuild ) + { + BuildGround(); + } + + if ( rebuild || relaunch ) + { + ResetCounters(); + Launch(); + } + + return true; + } + + static Sample* Create( SampleContext* context ) + { + return new DiagonalRoll( context ); + } + + b3BodyId m_probeId; + bool m_wave; + float m_amplitude; + float m_wavelength; + float m_cellSize; + float m_actualCellSize; + int m_diagonal; + int m_probe; + float m_speed; + float m_headingDegrees; + float m_offset; + + b3Vec3 m_start; + RunStats m_current; + RunStats m_last; + int m_runCount; +}; + +static int sampleDiagonalRoll = RegisterSample( "Ghost", "Diagonal Roll", DiagonalRoll::Create ); + +// Large bodies over thousands of triangles of a fine mesh, and a pile of small boxes for the everyday +// cost. Engines that cap the triangles or features they consider per pair can let ghosts back in where +// the cost is highest. Step time comes from the world profile, so the ghost count is not in it. +class FineMeshLoad : public GhostSample +{ +public: + static constexpr int m_timeCapacity = 60; + static constexpr int m_pileCount = 64; + + enum LoadMode + { + e_largeBodies, + e_pile, + e_loadModeCount + }; + + struct LargeStats + { + int manifolds; + int points; + int ghosts; + float peakSpeed; + }; + + explicit FineMeshLoad( SampleContext* context ) + : GhostSample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 30.0f, 30.0f, 30.0f, { 0.0f, 0.0f, 0.0f } ); + } + + m_follow = false; + m_showHistory = false; + m_mode = e_largeBodies; + m_cellIndex = 0; + m_speed = 3.0f; + m_countGhosts = true; + m_wallsId = b3_nullBodyId; + + Build(); + } + + void ResetCounters() override + { + GhostSample::ResetCounters(); + m_timeCount = 0; + m_timeIndex = 0; + m_maxStepTime = 0.0f; + m_stepGhosts = 0; + memset( m_largeStats, 0, sizeof( m_largeStats ) ); + } + + void DestroyBodies() + { + for ( b3BodyId bodyId : m_bodyIds ) + { + b3DestroyBody( bodyId ); + } + m_bodyIds.clear(); + + if ( B3_IS_NON_NULL( m_wallsId ) ) + { + b3DestroyBody( m_wallsId ); + m_wallsId = b3_nullBodyId; + } + } + + void Build() + { + DestroyBodies(); + + std::vector vertices; + std::vector indices; + + if ( m_mode == e_largeBodies ) + { + float cellSize = m_cellSizes[m_cellIndex]; + int xCount = (int)( 32.0f / cellSize + 0.5f ); + int zCount = (int)( 20.0f / cellSize + 0.5f ); + std::vector xs = MakeSamples( -16.0f, 16.0f, xCount ); + std::vector zs = MakeSamples( -10.0f, 10.0f, zCount ); + AddGrid( vertices, indices, xs, zs, []( int, int ) { return 0.0f; }, e_diagonalAlternate ); + CreateGround( vertices, indices ); + LaunchLarge(); + } + else + { + std::vector samples = MakeSamples( -10.0f, 10.0f, 80 ); + auto height = [&]( int i, int j ) { + float k = 2.0f * B3_PI / 4.0f; + return 0.3f * sinf( k * samples[i] ) * sinf( k * samples[j] ); + }; + AddGrid( vertices, indices, samples, samples, height, e_diagonalAlternate ); + CreateGround( vertices, indices ); + CreatePile(); + } + + ResetCounters(); + } + + void LaunchLarge() + { + DestroyBodies(); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.linearVelocity = { m_speed, 0.0f, 0.0f }; + bodyDef.enableSleep = false; + + // Recycling would skip the collision under test on some steps and make the step time bimodal + bodyDef.enableContactRecycling = false; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.0f; + + // A 4 m box over about 3200 triangles at the finest cells + bodyDef.position = { -13.0f, 0.5f, -5.0f }; + bodyDef.name = "big box"; + b3BodyId boxId = b3CreateBody( m_worldId, &bodyDef ); + b3BoxHull box = b3MakeBoxHull( 2.0f, 0.5f, 2.0f ); + b3CreateHullShape( boxId, &shapeDef, &box.base ); + m_bodyIds.push_back( boxId ); + + // A 6 m capsule lying across the direction of travel + bodyDef.position = { -13.0f, 0.5f, 5.0f }; + bodyDef.name = "long capsule"; + b3BodyId capsuleId = b3CreateBody( m_worldId, &bodyDef ); + b3Capsule capsule = { { 0.0f, 0.0f, -2.5f }, { 0.0f, 0.0f, 2.5f }, 0.5f }; + b3CreateCapsuleShape( capsuleId, &shapeDef, &capsule ); + m_bodyIds.push_back( capsuleId ); + + // Ghosts accumulate over passes, the peak is per pass + m_largeStats[0].peakSpeed = 0.0f; + m_largeStats[1].peakSpeed = 0.0f; + m_passSteps = 0; + } + + void CreatePile() + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "walls"; + m_wallsId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull walls[4] = { + b3MakeOffsetBoxHull( 10.5f, 1.5f, 0.5f, { 0.0f, 1.0f, -10.5f } ), + b3MakeOffsetBoxHull( 10.5f, 1.5f, 0.5f, { 0.0f, 1.0f, 10.5f } ), + b3MakeOffsetBoxHull( 0.5f, 1.5f, 10.5f, { -10.5f, 1.0f, 0.0f } ), + b3MakeOffsetBoxHull( 0.5f, 1.5f, 10.5f, { 10.5f, 1.0f, 0.0f } ), + }; + for ( b3BoxHull& wall : walls ) + { + b3CreateHullShape( m_wallsId, &shapeDef, &wall.base ); + } + + b3BoxHull box = b3MakeBoxHull( 0.1f, 0.1f, 0.1f ); + bodyDef.type = b3_dynamicBody; + bodyDef.name = "pile box"; + float spacing = 0.3f; + float start = -0.5f * spacing * ( m_pileCount - 1 ); + for ( int i = 0; i < m_pileCount; ++i ) + { + for ( int j = 0; j < m_pileCount; ++j ) + { + int index = m_pileCount * i + j; + bodyDef.position = { start + spacing * i, 1.5f, start + spacing * j }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Normalize( { 1.0f, 2.0f, 3.0f } ), 0.37f * (float)index ); + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + m_bodyIds.push_back( bodyId ); + } + } + } + + void Step() override + { + int bodyCount = (int)m_bodyIds.size(); + m_centers.resize( bodyCount ); + for ( int i = 0; i < bodyCount; ++i ) + { + m_centers[i] = b3Body_GetWorldCenter( m_bodyIds[i] ); + } + + Sample::Step(); + + if ( m_didStep ) + { + b3Profile profile = b3World_GetProfile( m_worldId ); + m_stepTimes[m_timeIndex] = profile.step; + m_collideTimes[m_timeIndex] = profile.collide; + m_timeIndex = ( m_timeIndex + 1 ) % m_timeCapacity; + m_timeCount = b3MinInt( m_timeCount + 1, m_timeCapacity ); + m_maxStepTime = b3MaxFloat( m_maxStepTime, profile.step ); + + m_stepGhosts = 0; + if ( m_countGhosts ) + { + for ( int i = 0; i < bodyCount; ++i ) + { + int ghostCount = CountGhosts( m_bodyIds[i], m_centers[i] ); + m_stepGhosts += ghostCount; + if ( m_mode == e_largeBodies ) + { + m_largeStats[i].ghosts += ghostCount; + } + } + } + + if ( m_mode == e_largeBodies ) + { + UpdateLarge(); + } + } + + DrawGhosts(); + + float stepTime = 0.0f; + float collideTime = 0.0f; + for ( int i = 0; i < m_timeCount; ++i ) + { + stepTime += m_stepTimes[i]; + collideTime += m_collideTimes[i]; + } + float scale = m_timeCount > 0 ? 1.0f / (float)m_timeCount : 0.0f; + + DrawTextLine( "step %.2f ms, collide %.2f ms, averaged over %d steps, max step %.2f ms", scale * stepTime, + scale * collideTime, m_timeCount, m_maxStepTime ); + DrawGhostSummary(); + + if ( m_mode == e_largeBodies ) + { + float cellSize = m_cellSizes[m_cellIndex]; + int under = (int)( 16.0f / ( 0.5f * cellSize * cellSize ) ); + DrawTextLine( "triangles under the box: about %d", under ); + const char* names[2] = { "box", "capsule" }; + for ( int i = 0; i < 2; ++i ) + { + const LargeStats& stats = m_largeStats[i]; + DrawTextLine( "%s: manifolds %d, points %d, ghosts %d, peak off plane speed %.3f m/s", names[i], stats.manifolds, + stats.points, stats.ghosts, stats.peakSpeed ); + } + } + else + { + b3Counters counters = b3World_GetCounters( m_worldId ); + DrawTextLine( "bodies %d, awake %d, contacts %d, ghosts this step %d", (int)m_bodyIds.size(), + b3World_GetAwakeBodyCount( m_worldId ), counters.contactCount, m_stepGhosts ); + } + } + + // Off plane speed includes spin, since a ghost on flat ground tips or lifts the body. The first steps + // settle the bodies onto the grid. + void UpdateLarge() + { + m_passSteps += 1; + bool settled = m_passSteps > 10; + bool done = true; + for ( int i = 0; i < 2; ++i ) + { + b3BodyId bodyId = m_bodyIds[i]; + LargeStats& stats = m_largeStats[i]; + + b3ContactData contacts[4]; + int contactCount = b3Body_GetContactData( bodyId, contacts, 4 ); + stats.manifolds = 0; + stats.points = 0; + for ( int j = 0; j < contactCount; ++j ) + { + stats.manifolds += contacts[j].manifoldCount; + for ( int k = 0; k < contacts[j].manifoldCount; ++k ) + { + stats.points += contacts[j].manifolds[k].pointCount; + } + } + + b3Vec3 v = b3Body_GetLinearVelocity( bodyId ); + b3Vec3 w = b3Body_GetAngularVelocity( bodyId ); + float offPlane = b3AbsFloat( v.y ) + b3Length( w ); + stats.peakSpeed = settled ? b3MaxFloat( stats.peakSpeed, offPlane ) : 0.0f; + + done = done && (float)b3Body_GetPosition( bodyId ).x > 13.0f; + } + + if ( done ) + { + LaunchLarge(); + } + } + + bool DrawControls() override + { + bool rebuild = false; + + const char* modeNames[e_loadModeCount] = { "Large Bodies", "Pile" }; + const char* cellNames[] = { "0.1 m", "0.25 m", "0.5 m" }; + rebuild |= ImGui::Combo( "Mode", &m_mode, modeNames, e_loadModeCount ); + if ( m_mode == e_largeBodies ) + { + rebuild |= ImGui::Combo( "Cell Size", &m_cellIndex, cellNames, IM_ARRAYSIZE( cellNames ) ); + if ( ImGui::SliderFloat( "Speed", &m_speed, 0.0f, 10.0f, "%.1f m/s" ) ) + { + LaunchLarge(); + } + } + + rebuild |= ImGui::Button( "Restart" ); + ImGui::Checkbox( "Count Ghosts", &m_countGhosts ); + + DrawGhostControls(); + + if ( rebuild ) + { + Build(); + } + + return true; + } + + static Sample* Create( SampleContext* context ) + { + return new FineMeshLoad( context ); + } + + static constexpr float m_cellSizes[3] = { 0.1f, 0.25f, 0.5f }; + + int m_mode; + int m_cellIndex; + float m_speed; + bool m_countGhosts; + + std::vector m_bodyIds; + std::vector m_centers; + b3BodyId m_wallsId; + + LargeStats m_largeStats[2]; + int m_passSteps; + float m_stepTimes[m_timeCapacity]; + float m_collideTimes[m_timeCapacity]; + int m_timeCount; + int m_timeIndex; + float m_maxStepTime; + int m_stepGhosts; +}; + +static int sampleFineMeshLoad = RegisterSample( "Ghost", "Fine Mesh Load", FineMeshLoad::Create ); + +// 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: + static constexpr int m_markerCapacity = 256; + + explicit GhostCollisions( SampleContext* context ) + : Sample( context ) + , m_markers{} + { + if ( m_context->restart == false ) + { + m_camera->SetView( 90.0f, 20.0f, 30.0f, { 0.0f, 1.0f, 0.0f } ); + } + + m_data = CreateGhostCollisions( m_worldId, GetDefaultGhostSettings() ); + } + + ~GhostCollisions() override + { + DestroyGhostCollisions( &m_data ); + } + + void Step() override + { + Sample::Step(); + + if ( m_didStep ) + { + 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 ) + { + 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 ); + } + } + + for ( int i = 0; i < m_markerCount; ++i ) + { + b3Pos p = m_markers[i].point; + DrawPoint( p, 8.0f, MakeColor( b3_colorRed ) ); + DrawLine( p, p + 0.5f * m_markers[i].normal, MakeColor( b3_colorRed ) ); + } + + 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 + { + GhostSettings& settings = m_data.settings; + bool rebuild = false; + bool relaunch = false; + + int scene = (int)settings.scene; + if ( ImGui::Combo( "Scene", &scene, g_ghostSceneNames, e_ghostSceneCount ) ) + { + settings.scene = (GhostScene)scene; + rebuild = true; + + // The chamfer ghost needs the s&box character, a fixed rotation hull without speculative contact + if ( settings.scene == e_ghostChamfer ) + { + settings.fixedRotation = true; + settings.speculative = false; + if ( settings.body != e_ghostBox && settings.body != e_ghostCylinder ) + { + settings.body = e_ghostBox; + } + relaunch = true; + } + + if ( settings.scene == e_ghostInternal ) + { + m_camera->SetView( 60.0f, 20.0f, 160.0f, { 0.0f, 30.0f, 0.0f } ); + } + else + { + m_camera->SetView( 90.0f, 20.0f, 30.0f, { 0.0f, 1.0f, 0.0f } ); + } + } + + if ( settings.scene == e_ghostInternal ) + { + rebuild |= ImGui::Combo( "Preset", &settings.peelPreset, g_ghostPeelPresetNames, GHOST_PEEL_PRESET_COUNT ); + } + + int body = (int)settings.body; + if ( ImGui::Combo( "Body", &body, g_ghostBodyNames, e_ghostBodyCount ) ) + { + settings.body = (GhostBody)body; + relaunch = true; + } + + rebuild |= ImGui::Checkbox( "Mirror", &settings.mirror ); + rebuild |= ImGui::Checkbox( "Height Field", &settings.heightField ); + 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_data.ghostCount = 0; + m_data.runGhostCount = 0; + m_data.runCount = 0; + m_markerCount = 0; + m_markerIndex = 0; + } + + if ( rebuild ) + { + CreateGhostGround( m_worldId, &m_data ); + m_markerCount = 0; + m_markerIndex = 0; + } + + if ( rebuild || relaunch ) + { + LaunchGhostBody( m_worldId, &m_data ); + } + + return true; + } + + static Sample* Create( SampleContext* context ) + { + return new GhostCollisions( context ); + } + + struct Marker + { + b3Pos point; + b3Vec3 normal; + }; + + GhostCollisionsData m_data; + Marker m_markers[m_markerCapacity]; + int m_markerCount = 0; + int m_markerIndex = 0; +}; + +static int sampleGhostScenarios= RegisterSample( "Ghost", "Scenarios", GhostCollisions::Create ); + +// A closed conveyor circuit swept from a cross section and finely tessellated, so every body slides over +// thousands of internal edges. Belts carry bodies over a washboard, up a spiral lift and a bumpy escalator, +// then off a sharp drop where gravity takes them down a banked trough spiral back to the start. +// A pushing manifold is counted as a ghost when its normal matches no nearby triangle and does not lie +// on the arc between two triangles that meet at a convex crease. +class ConveyorCircuit : public Sample +{ +public: + enum BodyType + { + e_mixedBody, + e_boxBody, + e_capsuleBody, + e_sphereBody, + e_cylinderBody, + e_bodyTypeCount + }; + + static constexpr int m_maxBodies = 300; + static constexpr int m_markerCapacity = 256; + + // Material 0 is the rails and skirts, then the slide, then the belt directions + static constexpr int m_slideMaterial = 1; + static constexpr int m_firstBeltMaterial = 2; + static constexpr int m_yawBinCount = 128; + static constexpr int m_materialCount = m_firstBeltMaterial + m_yawBinCount; + + static constexpr int m_segmentCount = 4; + static constexpr int m_slideSegment = 3; + + // Skirt, lip, inner wall, floor, inner wall, lip, skirt + static constexpr int m_profileSegmentCount = 7; + static constexpr int m_floorSegment = 3; + + // Half turns, so each spiral reverses the heading and the loop closes + static constexpr float m_liftTurns = 2.5f; + static constexpr float m_slideTurns = 1.5f; + + static constexpr float m_radius = 4.5f; + static constexpr float m_straightLength = 16.0f; + static constexpr float m_halfWidth = 1.0f; + static constexpr float m_railHeight = 0.35f; + static constexpr float m_railThickness = 0.08f; + static constexpr float m_skirtDepth = 0.4f; + static constexpr float m_baseHeight = 1.0f; + static constexpr float m_liftHeight = 5.0f; + static constexpr float m_bumpStart = 5.0f; + static constexpr float m_bumpEnd = 11.0f; + static constexpr float m_bumpLength = 1.0f; + static constexpr float m_bumpHeight = 0.06f; + static constexpr float m_escalatorRise = 1.5f; + static constexpr float m_stepLength = 1.5f; + static constexpr float m_stepHeight = 0.15f; + static constexpr float m_dropLength = 2.5f; + static constexpr float m_troughBlendLength = 2.0f; + static constexpr float m_floorArcAngle = 55.0f * B3_DEG_TO_RAD; + static constexpr float m_bankAngle = 20.0f * B3_DEG_TO_RAD; + static constexpr float m_bankRampFraction = 0.2f; + static constexpr float m_spawnDistance = 3.0f; + static constexpr float m_heatBinLength = 1.0f; + static constexpr int m_trackWindow = 6; + + struct Segment + { + float length; + float curvature; + const char* name; + }; + + enum GhostClass + { + e_faceGhost, + e_edgeGhost, + e_vertexGhost, + e_otherGhost, + e_ghostClassCount + }; + + struct Marker + { + b3Pos point; + b3Vec3 normal; + }; + + explicit ConveyorCircuit( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( -135.0f, 35.0f, 27.5f, { 0.0f, -1.0f, 2.0f } ); + } + + AddGroundBox( 30.0f ); + + m_segments[0] = { m_straightLength, 0.0f, "bottom straight" }; + m_segments[1] = { 2.0f * B3_PI * m_radius * m_liftTurns, 1.0f / m_radius, "spiral lift" }; + m_segments[2] = { m_straightLength, 0.0f, "escalator" }; + m_segments[m_slideSegment] = { 2.0f * B3_PI * m_radius * m_slideTurns, 1.0f / m_radius, "slide" }; + + m_totalLength = 0.0f; + for ( int i = 0; i < m_segmentCount; ++i ) + { + m_segmentStart[i] = m_totalLength; + m_totalLength += m_segments[i].length; + } + + m_cylinderHull = b3CreateCylinder( 0.4f, 0.25f, -0.2f, 16 ); + + m_cellSize = 0.4f; + m_beltSpeed = 5.0f; + m_slideFriction = 0.01f; + m_targetCount = 120; + m_spawnInterval = 15; + m_bodyType = e_mixedBody; + m_toleranceDegrees = 10.0f; + m_contactRecycling = true; + m_drawFlow = true; + m_drawGhosts = false; + m_drawHeat = false; + + m_groundId = b3_nullBodyId; + m_bodyCount = 0; + m_spawnIndex = 0; + m_spawnTimer = 0; + + ResetCounters(); + CreateCircuit(); + } + + ~ConveyorCircuit() override + { + DestroyCircuit(); + b3DestroyHull( m_cylinderHull ); + } + + static float SmoothStep( float x ) + { + x = b3ClampFloat( x, 0.0f, 1.0f ); + return x * x * ( 3.0f - 2.0f * x ); + } + + float GetSlideStart() const + { + return m_segmentStart[m_slideSegment]; + } + + float EvaluateHeight( float s ) const + { + float lift0 = m_segmentStart[1]; + float lift1 = m_segmentStart[2]; + float slide0 = GetSlideStart(); + + if ( s < lift0 ) + { + if ( m_bumpStart <= s && s <= m_bumpEnd ) + { + float phase = 2.0f * B3_PI * ( s - m_bumpStart ) / m_bumpLength; + return 0.5f * m_bumpHeight * ( 1.0f - cosf( phase ) ); + } + + return 0.0f; + } + + if ( s < lift1 ) + { + return m_liftHeight * SmoothStep( ( s - lift0 ) / ( lift1 - lift0 ) ); + } + + // Constant grade between parabolic blends, so every step sits on the same slope. The top blend + // levels out at the lip so belt speed bodies fly off the edge instead of following the drop, and + // it shares its length with the trough blend so the steps stay on the flat channel floor. + if ( s < slide0 ) + { + float t = s - lift1; + float length = slide0 - lift1; + float blend = m_troughBlendLength; + float grade = m_escalatorRise / ( length - blend ); + + float height = m_liftHeight; + if ( t < blend ) + { + height += 0.5f * grade * t * t / blend; + } + else if ( t < length - blend ) + { + height += grade * ( t - 0.5f * blend ); + } + else + { + float r = length - t; + height += m_escalatorRise - 0.5f * grade * r * r / blend; + } + + int stepCount = (int)( ( length - 2.0f * blend ) / m_stepLength ); + float stepStart = 0.5f * ( length - stepCount * m_stepLength ); + if ( stepStart <= t && t <= stepStart + stepCount * m_stepLength ) + { + float phase = 2.0f * B3_PI * ( t - stepStart ) / m_stepLength; + height += 0.5f * m_stepHeight * ( 1.0f - cosf( phase ) ); + } + + return height; + } + + // Steepest at the top so slow bodies leaving the belt keep moving, level at the bottom. The drop + // sheds the escalator rise and flattens out where the base curve takes over. + float u = ( s - slide0 ) / m_segments[m_slideSegment].length; + float height = m_liftHeight * ( 1.0f - 1.5f * u + 0.5f * u * u * u ); + + float x = ( s - slide0 ) / m_dropLength; + if ( x < 1.0f ) + { + height += m_escalatorRise * ( 1.0f - sinf( 0.5f * B3_PI * x ) ); + } + + return height; + } + + // Zero for the belt channel and one for the slide trough, blended on the straights at either end + float EvaluateTrough( float s ) const + { + float slide0 = GetSlideStart(); + if ( s >= slide0 ) + { + return 1.0f; + } + + float entry = ( s - ( slide0 - m_troughBlendLength ) ) / m_troughBlendLength; + float exit = 1.0f - s / m_troughBlendLength; + return SmoothStep( b3MaxFloat( entry, exit ) ); + } + + // The slide only turns left, so a positive bank always lowers the inside of the turn + float EvaluateBank( float s ) const + { + float slide0 = GetSlideStart(); + if ( s < slide0 ) + { + return 0.0f; + } + + float u = ( s - slide0 ) / m_segments[m_slideSegment].length; + return m_bankAngle * SmoothStep( b3MinFloat( u, 1.0f - u ) / m_bankRampFraction ); + } + + // Cross section in lateral and up coordinates, traversed with air on the left. The trough bends the + // inner walls and floor onto one circular arc, so channel and trough share a grid and blend smoothly. + b3Vec2 EvaluateProfile( int segment, float alpha, float trough ) const + { + float inner = m_halfWidth; + float outer = m_halfWidth + m_railThickness; + + b3Vec2 channel[m_profileSegmentCount + 1] = { + { -outer, -m_skirtDepth }, { -outer, m_railHeight }, { -inner, m_railHeight }, { -inner, 0.0f }, + { inner, 0.0f }, { inner, m_railHeight }, { outer, m_railHeight }, { outer, -m_skirtDepth }, + }; + + // The lips sit at the top of the arc + b3Vec2 troughCorners[m_profileSegmentCount + 1] = { + { -outer, -m_skirtDepth }, { -outer, inner }, { -inner, inner }, {}, {}, { inner, inner }, { outer, inner }, + { outer, -m_skirtDepth }, + }; + + b3Vec2 a = channel[segment]; + b3Vec2 b = channel[segment + 1]; + b3Vec2 p = { a.x + alpha * ( b.x - a.x ), a.y + alpha * ( b.y - a.y ) }; + + b3Vec2 q; + if ( m_floorSegment - 1 <= segment && segment <= m_floorSegment + 1 ) + { + float angles[4] = { -0.5f * B3_PI, -m_floorArcAngle, m_floorArcAngle, 0.5f * B3_PI }; + int i = segment - ( m_floorSegment - 1 ); + float angle = angles[i] + alpha * ( angles[i + 1] - angles[i] ); + q = { inner * sinf( angle ), inner * ( 1.0f - cosf( angle ) ) }; + } + else + { + b3Vec2 ta = troughCorners[segment]; + b3Vec2 tb = troughCorners[segment + 1]; + q = { ta.x + alpha * ( tb.x - ta.x ), ta.y + alpha * ( tb.y - ta.y ) }; + } + + return { p.x + trough * ( q.x - p.x ), p.y + trough * ( q.y - p.y ) }; + } + + float GetProfileLength( int segment, float trough ) const + { + int sampleCount = 8; + float length = 0.0f; + b3Vec2 a = EvaluateProfile( segment, 0.0f, trough ); + for ( int i = 1; i <= sampleCount; ++i ) + { + b3Vec2 b = EvaluateProfile( segment, (float)i / (float)sampleCount, trough ); + length += sqrtf( ( b.x - a.x ) * ( b.x - a.x ) + ( b.y - a.y ) * ( b.y - a.y ) ); + a = b; + } + + return length; + } + + b3Vec3 EvaluateCenter( float s ) const + { + float x = -0.5f * m_straightLength; + float z = -m_radius; + float yaw = 0.0f; + float remaining = s; + + for ( int i = 0; i < m_segmentCount && remaining > 0.0f; ++i ) + { + float ds = b3MinFloat( remaining, m_segments[i].length ); + float k = m_segments[i].curvature; + if ( k == 0.0f ) + { + x += ds * cosf( yaw ); + z += ds * sinf( yaw ); + } + else + { + float yaw1 = yaw + k * ds; + x += ( sinf( yaw1 ) - sinf( yaw ) ) / k; + z -= ( cosf( yaw1 ) - cosf( yaw ) ) / k; + yaw = yaw1; + } + + remaining -= ds; + } + + return { x, m_baseHeight + EvaluateHeight( s ), z }; + } + + void BuildStations() + { + int count = b3MaxInt( 32, (int)ceilf( m_totalLength / m_cellSize ) ); + m_stations.resize( count ); + m_stationDistances.resize( count ); + m_tangents.resize( count ); + m_laterals.resize( count ); + m_ups.resize( count ); + m_troughs.resize( count ); + + for ( int k = 0; k < count; ++k ) + { + float s = m_totalLength * (float)k / (float)count; + m_stationDistances[k] = s; + m_stations[k] = EvaluateCenter( s ); + m_troughs[k] = EvaluateTrough( s ); + } + + // The chord across each station bisects a crease, so the rails stay square to the belt + for ( int k = 0; k < count; ++k ) + { + b3Vec3 next = m_stations[( k + 1 ) % count]; + b3Vec3 prev = m_stations[( k + count - 1 ) % count]; + b3Vec3 t = b3Normalize( next - prev ); + b3Vec3 l = b3Normalize( b3Cross( t, b3Vec3_axisY ) ); + b3Vec3 u = b3Cross( l, t ); + + float bank = EvaluateBank( m_stationDistances[k] ); + float c = cosf( bank ); + float s = sinf( bank ); + m_tangents[k] = t; + m_laterals[k] = c * l - s * u; + m_ups[k] = c * u + s * l; + } + } + + b3SurfaceMaterial MakeMaterial( int index ) const + { + b3SurfaceMaterial material = b3DefaultSurfaceMaterial(); + if ( index == 0 ) + { + material.friction = 0.2f; + material.customColor = b3_colorCadetBlue; + return material; + } + + if ( index == m_slideMaterial ) + { + material.friction = m_slideFriction; + material.customColor = b3_colorSilver; + return material; + } + + // Horizontal is enough, the solver projects tangent velocity onto the contact plane + float yaw = -B3_PI + 2.0f * B3_PI * (float)( index - m_firstBeltMaterial ) / (float)m_yawBinCount; + material.friction = 0.8f; + material.tangentVelocity = { m_beltSpeed * cosf( yaw ), 0.0f, m_beltSpeed * sinf( yaw ) }; + material.customColor = b3_colorLightSteelBlue; + return material; + } + + int GetStationMaterial( int station, int profileSegment ) const + { + bool slide = m_stationDistances[station] >= GetSlideStart(); + if ( slide && m_floorSegment - 1 <= profileSegment && profileSegment <= m_floorSegment + 1 ) + { + return m_slideMaterial; + } + + if ( slide == false && profileSegment == m_floorSegment ) + { + int stationCount = (int)m_stations.size(); + b3Vec3 direction = m_stations[( station + 1 ) % stationCount] - m_stations[station]; + float yaw = atan2f( direction.z, direction.x ); + int bin = (int)floorf( ( yaw + B3_PI ) / ( 2.0f * B3_PI ) * m_yawBinCount + 0.5f ); + return m_firstBeltMaterial + bin % m_yawBinCount; + } + + return 0; + } + + void CreateCircuit() + { + BuildStations(); + + // The last row repeats the first station and welds to it, closing the loop + int stationCount = (int)m_stations.size(); + + // Every station shares one column layout, sized for the longer of the two sections + int columnCounts[m_profileSegmentCount]; + int columnTotal = 0; + for ( int j = 0; j < m_profileSegmentCount; ++j ) + { + float length = b3MaxFloat( GetProfileLength( j, 0.0f ), GetProfileLength( j, 1.0f ) ); + columnCounts[j] = b3MaxInt( 1, (int)ceilf( length / m_cellSize - 0.01f ) ); + columnTotal += columnCounts[j]; + } + + int stride = columnTotal + 1; + + std::vector vertices; + std::vector indices; + std::vector materialIndices; + vertices.reserve( ( stationCount + 1 ) * stride ); + + m_lipPoints.resize( stationCount ); + for ( int k = 0; k <= stationCount; ++k ) + { + int station = k % stationCount; + b3Vec3 center = m_stations[station]; + b3Vec3 lateral = m_laterals[station]; + b3Vec3 up = m_ups[station]; + float trough = m_troughs[station]; + + b3Vec2 p = EvaluateProfile( 0, 0.0f, trough ); + vertices.push_back( center + p.x * lateral + p.y * up ); + + for ( int j = 0; j < m_profileSegmentCount; ++j ) + { + for ( int i = 1; i <= columnCounts[j]; ++i ) + { + p = EvaluateProfile( j, (float)i / (float)columnCounts[j], trough ); + vertices.push_back( center + p.x * lateral + p.y * up ); + } + } + + if ( k < stationCount ) + { + p = EvaluateProfile( 1, 0.0f, trough ); + m_lipPoints[k] = center + p.x * lateral + p.y * up; + } + } + + for ( int k = 0; k < stationCount; ++k ) + { + int column = 0; + for ( int j = 0; j < m_profileSegmentCount; ++j ) + { + int material = GetStationMaterial( k, j ); + + for ( int i = 0; i < columnCounts[j]; ++i, ++column ) + { + int a = k * stride + column; + int b = a + 1; + int c = a + stride + 1; + int d = a + stride; + + // Air is on the left of the profile direction + b3Vec3 expected = b3Cross( vertices[b] - vertices[a], m_tangents[k] ); + + int corners[6] = { a, b, c, c, d, a }; + if ( ( k + column ) & 1 ) + { + int flipped[6] = { a, b, d, b, c, d }; + memcpy( corners, flipped, sizeof( corners ) ); + } + + for ( int n = 0; n < 6; n += 3 ) + { + b3Vec3 v1 = vertices[corners[n]]; + b3Vec3 v2 = vertices[corners[n + 1]]; + b3Vec3 v3 = vertices[corners[n + 2]]; + bool reversed = b3Dot( b3Cross( v2 - v1, v3 - v1 ), expected ) < 0.0f; + + indices.push_back( corners[n] ); + indices.push_back( reversed ? corners[n + 2] : corners[n + 1] ); + indices.push_back( reversed ? corners[n + 1] : corners[n + 2] ); + materialIndices.push_back( (uint8_t)material ); + } + } + } + } + + b3MeshDef meshDef = {}; + meshDef.vertices = vertices.data(); + meshDef.indices = indices.data(); + meshDef.materialIndices = materialIndices.data(); + meshDef.vertexCount = (int)vertices.size(); + meshDef.triangleCount = (int)( indices.size() / 3 ); + meshDef.weldVertices = true; + meshDef.weldTolerance = 0.001f; + meshDef.identifyEdges = true; + + m_mesh = b3CreateMesh( &meshDef, nullptr, 0 ); + + b3SurfaceMaterial materials[m_materialCount]; + for ( int i = 0; i < m_materialCount; ++i ) + { + materials[i] = MakeMaterial( i ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "circuit"; + m_groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.materials = materials; + shapeDef.materialCount = m_materialCount; + m_shapeId = b3CreateMeshShape( m_groundId, &shapeDef, m_mesh, b3Vec3_one ); + + BuildAdjacency(); + + m_heatBinCount = b3MaxInt( 1, (int)( m_totalLength / m_heatBinLength + 0.5f ) ); + m_binGhosts.assign( m_heatBinCount, 0 ); + m_binManifolds.assign( m_heatBinCount, 0 ); + + // Station indices changed, so every body needs a fresh search + for ( int i = 0; i < m_bodyCount; ++i ) + { + m_bodyStations[i] = B3_NULL_INDEX; + } + + m_markerCount = 0; + m_markerIndex = 0; + } + + // The body holding the mesh shapes must go before the mesh data + void DestroyCircuit() + { + if ( B3_IS_NON_NULL( m_groundId ) ) + { + b3DestroyBody( m_groundId ); + m_groundId = b3_nullBodyId; + } + + if ( m_mesh != nullptr ) + { + b3DestroyMesh( m_mesh ); + m_mesh = nullptr; + } + } + + void UpdateMaterials() + { + for ( int i = 1; i < m_materialCount; ++i ) + { + b3Shape_SetMeshMaterial( m_shapeId, MakeMaterial( i ), i ); + } + } + + b3Pos GetSpawnPosition() const + { + b3Vec3 center = EvaluateCenter( m_spawnDistance ); + float lateral = RandomFloatRange( -0.6f, 0.6f ); + return b3ToPos( { center.x, center.y + 1.2f, center.z + lateral } ); + } + + void SpawnBody() + { + int type = m_bodyType == e_mixedBody ? 1 + m_spawnIndex % ( e_bodyTypeCount - 1 ) : m_bodyType; + m_spawnIndex += 1; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = GetSpawnPosition(); + bodyDef.rotation = RandomQuat(); + + // A jammed body would fall asleep on a moving belt + bodyDef.enableSleep = false; + bodyDef.enableContactRecycling = m_contactRecycling; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.6f; + + // Round bodies would roll back down the helix without it + shapeDef.baseMaterial.rollingResistance = 0.3f; + + switch ( type ) + { + case e_boxBody: + { + float halfLength = ( m_spawnIndex & 1 ) ? 0.4f : 0.25f; + b3BoxHull box = b3MakeBoxHull( halfLength, 0.15f, 0.25f ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + break; + + case e_capsuleBody: + { + b3Capsule capsule = { { -0.3f, 0.0f, 0.0f }, { 0.3f, 0.0f, 0.0f }, 0.15f }; + b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + } + break; + + case e_sphereBody: + { + b3Sphere sphere = { b3Vec3_zero, 0.25f }; + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + } + break; + + default: + b3CreateHullShape( bodyId, &shapeDef, m_cylinderHull ); + break; + } + + m_bodyIds[m_bodyCount] = bodyId; + m_bodyStations[m_bodyCount] = B3_NULL_INDEX; + m_bodyCount += 1; + } + + void DestroyBodies() + { + for ( int i = 0; i < m_bodyCount; ++i ) + { + b3DestroyBody( m_bodyIds[i] ); + } + + m_bodyCount = 0; + m_spawnIndex = 0; + m_spawnTimer = 0; + } + + void ResetCounters() + { + memset( m_classCounts, 0, sizeof( m_classCounts ) ); + memset( m_flagCounts, 0, sizeof( m_flagCounts ) ); + m_ghostCount = 0; + m_manifoldCount = 0; + m_stepGhostCount = 0; + m_recycleCount = 0; + m_markerCount = 0; + m_markerIndex = 0; + std::fill( m_binGhosts.begin(), m_binGhosts.end(), 0 ); + std::fill( m_binManifolds.begin(), m_binManifolds.end(), 0 ); + } + + // Bodies move far less than a station per step, so a local search follows a body along the track and + // keeps it from snapping to a spiral layer above or below. A body that fell off gets a full search. + int FindStation( b3Vec3 point, int hint ) const + { + int count = (int)m_stations.size(); + int best = 0; + float bestDistanceSquared = FLT_MAX; + + if ( hint != B3_NULL_INDEX ) + { + for ( int offset = -m_trackWindow; offset <= m_trackWindow; ++offset ) + { + int k = ( hint + offset + count ) % count; + float distanceSquared = b3LengthSquared( point - m_stations[k] ); + if ( distanceSquared < bestDistanceSquared ) + { + bestDistanceSquared = distanceSquared; + best = k; + } + } + + if ( bestDistanceSquared < 4.0f ) + { + return best; + } + } + + for ( int k = 0; k < count; ++k ) + { + float distanceSquared = b3LengthSquared( point - m_stations[k] ); + if ( distanceSquared < bestDistanceSquared ) + { + bestDistanceSquared = distanceSquared; + best = k; + } + } + + return best; + } + + int GetHeatBin( int station ) const + { + return (int)( (int64_t)station * m_heatBinCount / (int64_t)m_stations.size() ); + } + + const char* GetSegmentName( float s ) const + { + int segment = 0; + while ( segment + 1 < m_segmentCount && s >= m_segmentStart[segment + 1] ) + { + segment += 1; + } + + return m_segments[segment].name; + } + + void BuildAdjacency() + { + int triangleCount = m_mesh->triangleCount; + int vertexCount = m_mesh->vertexCount; + const b3MeshTriangle* triangles = b3GetMeshTriangles( m_mesh ); + const b3Vec3* vertices = b3GetMeshVertices( m_mesh ); + + m_triangleNormals.resize( triangleCount ); + m_edgeNeighbors.assign( 3 * triangleCount, B3_NULL_INDEX ); + m_vertexRings.assign( vertexCount, {} ); + m_vertexTriangles.assign( vertexCount, {} ); + + // Sorting by vertex pair puts the two sides of a shared edge next to each other + std::vector> edges; + edges.reserve( 3 * triangleCount ); + + for ( int t = 0; t < triangleCount; ++t ) + { + int v[3] = { triangles[t].index1, triangles[t].index2, triangles[t].index3 }; + m_triangleNormals[t] = + b3Normalize( b3Cross( vertices[v[1]] - vertices[v[0]], vertices[v[2]] - vertices[v[0]] ) ); + + for ( int e = 0; e < 3; ++e ) + { + int a = v[e]; + int b = v[e < 2 ? e + 1 : 0]; + uint64_t key = ( (uint64_t)b3MinInt( a, b ) << 32 ) | (uint64_t)b3MaxInt( a, b ); + edges.push_back( { key, 3 * t + e } ); + m_vertexRings[a].push_back( b ); + m_vertexRings[b].push_back( a ); + m_vertexTriangles[a].push_back( t ); + } + } + + std::sort( edges.begin(), edges.end() ); + for ( size_t i = 0; i + 1 < edges.size(); ++i ) + { + // A non-manifold edge leaves every side without a neighbor + uint64_t key = edges[i].first; + bool pair = edges[i + 1].first == key; + bool single = + ( i + 2 >= edges.size() || edges[i + 2].first != key ) && ( i == 0 || edges[i - 1].first != key ); + if ( pair && single ) + { + m_edgeNeighbors[edges[i].second] = edges[i + 1].second / 3; + m_edgeNeighbors[edges[i + 1].second] = edges[i].second / 3; + } + } + + for ( std::vector& ring : m_vertexRings ) + { + std::sort( ring.begin(), ring.end() ); + ring.erase( std::unique( ring.begin(), ring.end() ), ring.end() ); + } + } + + bool MatchesFace( b3Vec3 normal, int triangleIndex ) const + { + return b3Dot( normal, m_triangleNormals[triangleIndex] ) >= m_cosTolerance; + } + + // Direction in the face plane pointing away from the edge into the face + static b3Vec3 GetInwardDirection( b3Vec3 edgeStart, b3Vec3 axis, b3Vec3 opposite ) + { + b3Vec3 d = opposite - edgeStart; + return b3Normalize( d - b3Dot( d, axis ) * axis ); + } + + // The normal cone of a convex edge is the arc between its two face normals. Neither face may rise + // above the edge along the normal, so a concave edge only admits its face normals. + bool AdmitsEdge( b3Vec3 normal, int triangleIndex, int edgeIndex ) const + { + int neighbor = m_edgeNeighbors[3 * triangleIndex + edgeIndex]; + if ( MatchesFace( normal, triangleIndex ) || ( neighbor != B3_NULL_INDEX && MatchesFace( normal, neighbor ) ) ) + { + return true; + } + + const b3MeshTriangle* triangles = b3GetMeshTriangles( m_mesh ); + const b3Vec3* vertices = b3GetMeshVertices( m_mesh ); + int v[3] = { triangles[triangleIndex].index1, triangles[triangleIndex].index2, + triangles[triangleIndex].index3 }; + int ia = v[edgeIndex]; + int ib = v[edgeIndex < 2 ? edgeIndex + 1 : 0]; + int ic = v[edgeIndex > 0 ? edgeIndex - 1 : 2]; + + b3Vec3 a = vertices[ia]; + b3Vec3 axis = b3Normalize( vertices[ib] - a ); + if ( b3AbsFloat( b3Dot( normal, axis ) ) > m_sinTolerance ) + { + return false; + } + + if ( b3Dot( normal, GetInwardDirection( a, axis, vertices[ic] ) ) > m_sinTolerance ) + { + return false; + } + + if ( neighbor != B3_NULL_INDEX ) + { + int w[3] = { triangles[neighbor].index1, triangles[neighbor].index2, triangles[neighbor].index3 }; + for ( int k = 0; k < 3; ++k ) + { + if ( w[k] != ia && w[k] != ib && + b3Dot( normal, GetInwardDirection( a, axis, vertices[w[k]] ) ) > m_sinTolerance ) + { + return false; + } + } + } + + return true; + } + + // A vertex normal is valid when the vertex is a local support point in that direction + bool AdmitsVertex( b3Vec3 normal, int vertexIndex ) const + { + for ( int t : m_vertexTriangles[vertexIndex] ) + { + if ( MatchesFace( normal, t ) ) + { + return true; + } + } + + const b3Vec3* vertices = b3GetMeshVertices( m_mesh ); + b3Vec3 vertex = vertices[vertexIndex]; + for ( int other : m_vertexRings[vertexIndex] ) + { + if ( b3Dot( normal, b3Normalize( vertices[other] - vertex ) ) > m_sinTolerance ) + { + return false; + } + } + + return true; + } + + bool AdmitsPoint( b3Vec3 normal, const b3ManifoldPoint& point ) const + { + const b3MeshTriangle& triangle = b3GetMeshTriangles( m_mesh )[point.triangleIndex]; + int v[3] = { triangle.index1, triangle.index2, triangle.index3 }; + + switch ( point.triangleFeature ) + { + case b3_featureTriangleFace: + return MatchesFace( normal, point.triangleIndex ); + + case b3_featureEdge1: + case b3_featureEdge2: + case b3_featureEdge3: + return AdmitsEdge( normal, point.triangleIndex, point.triangleFeature - b3_featureEdge1 ); + + case b3_featureVertex1: + case b3_featureVertex2: + case b3_featureVertex3: + return AdmitsVertex( normal, v[point.triangleFeature - b3_featureVertex1] ); + + default: + // No feature was resolved, so any feature of the triangle may own the normal + if ( MatchesFace( normal, point.triangleIndex ) ) + { + return true; + } + + for ( int k = 0; k < 3; ++k ) + { + if ( AdmitsEdge( normal, point.triangleIndex, k ) || AdmitsVertex( normal, v[k] ) ) + { + return true; + } + } + + return false; + } + } + + static GhostClass Classify( const b3ManifoldPoint& point ) + { + switch ( point.triangleFeature ) + { + case b3_featureTriangleFace: + return e_faceGhost; + + case b3_featureEdge1: + case b3_featureEdge2: + case b3_featureEdge3: + return e_edgeGhost; + + case b3_featureVertex1: + case b3_featureVertex2: + case b3_featureVertex3: + return e_vertexGhost; + + default: + return e_otherGhost; + } + } + + void CountGhosts( b3BodyId bodyId, b3Pos center, int heatBin ) + { + // Clustering merges manifolds whose normals agree within its threshold and hands them one normal, + // so a point may carry a normal that far from its own feature + float angle = m_toleranceDegrees * B3_DEG_TO_RAD + acosf( 0.996f ); + m_cosTolerance = cosf( angle ); + m_sinTolerance = sinf( angle ); + + b3ContactData contacts[16]; + int contactCount = b3Body_GetContactData( bodyId, contacts, 16 ); + + for ( int i = 0; i < contactCount; ++i ) + { + bool bodyIsA = B3_ID_EQUALS( b3Shape_GetBody( contacts[i].shapeIdA ), bodyId ); + 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; + b3Vec3 normal = bodyIsA ? b3Neg( manifold->normal ) : manifold->normal; + + // Speculative points that never pushed are harmless. Clustering hands every point the normal + // of one member, so the manifold is sound if any pushing point owns that normal. + int pushCount = 0; + int firstPush = 0; + bool admitted = false; + for ( int k = 0; k < manifold->pointCount && admitted == false; ++k ) + { + const b3ManifoldPoint& point = manifold->points[k]; + if ( point.totalNormalImpulse <= 0.0f ) + { + continue; + } + + firstPush = pushCount == 0 ? k : firstPush; + pushCount += 1; + admitted = AdmitsPoint( normal, point ); + } + + if ( pushCount == 0 ) + { + continue; + } + + m_manifoldCount += 1; + m_binManifolds[heatBin] += 1; + + if ( admitted ) + { + continue; + } + + const b3ManifoldPoint& point = manifold->points[firstPush]; + m_ghostCount += 1; + m_binGhosts[heatBin] += 1; + m_stepGhostCount += 1; + m_classCounts[Classify( point )] += 1; + for ( int bit = 0; bit < m_flagCount; ++bit ) + { + m_flagCounts[bit] += ( point.meshFlags >> bit ) & 1; + } + + b3Vec3 anchor = bodyIsA ? point.anchorA : point.anchorB; + m_markers[m_markerIndex] = { b3OffsetPos( center, anchor ), normal }; + m_markerIndex = ( m_markerIndex + 1 ) % m_markerCapacity; + m_markerCount = b3MinInt( m_markerCount + 1, m_markerCapacity ); + } + } + } + + void Step() override + { + // Contacts are computed at the start of the step and anchors stay relative to that body position. + // A fast body moves past the margin within a step, so the end position would query the wrong triangles. + for ( int i = 0; i < m_bodyCount; ++i ) + { + m_contactCenters[i] = b3Body_GetWorldCenter( m_bodyIds[i] ); + } + + Sample::Step(); + + if ( m_didStep ) + { + m_stepGhostCount = 0; + + for ( int i = 0; i < m_bodyCount; ++i ) + { + b3BodyId bodyId = m_bodyIds[i]; + b3Pos position = b3Body_GetPosition( bodyId ); + + // Off the belt and on the ground, so drop it back in at the start + if ( position.y < m_baseHeight - 0.5f ) + { + b3Body_SetTransform( bodyId, GetSpawnPosition(), RandomQuat() ); + b3Body_SetLinearVelocity( bodyId, b3Vec3_zero ); + b3Body_SetAngularVelocity( bodyId, b3Vec3_zero ); + m_bodyStations[i] = B3_NULL_INDEX; + m_recycleCount += 1; + continue; + } + + m_bodyStations[i] = FindStation( b3ToVec3( m_contactCenters[i] ), m_bodyStations[i] ); + CountGhosts( bodyId, m_contactCenters[i], GetHeatBin( m_bodyStations[i] ) ); + } + + m_spawnTimer += 1; + if ( m_bodyCount < m_targetCount && m_spawnTimer >= m_spawnInterval ) + { + SpawnBody(); + m_spawnTimer = 0; + } + } + +#if 0 + float rate = m_manifoldCount > 0 ? 1.0e6f * (float)m_ghostCount / (float)m_manifoldCount : 0.0f; + DrawTextLine( "ghost manifolds: %d this step, %d of %d pushing (%.0f ppm)", m_stepGhostCount, m_ghostCount, + m_manifoldCount, rate ); + DrawTextLine( "ghost feature: face %d, edge %d, vertex %d, unresolved %d", m_classCounts[e_faceGhost], + m_classCounts[e_edgeGhost], m_classCounts[e_vertexGhost], m_classCounts[e_otherGhost] ); + DrawTextLine( "ghost origin: hull face %d, snapped %d, deep %d", m_flagCounts[0], m_flagCounts[1], + m_flagCounts[2] ); + int worstBin = 0; + for ( int i = 1; i < m_heatBinCount; ++i ) + { + worstBin = m_binGhosts[i] > m_binGhosts[worstBin] ? i : worstBin; + } + + if ( m_binGhosts[worstBin] > 0 ) + { + float s = ( (float)worstBin + 0.5f ) * m_totalLength / (float)m_heatBinCount; + float binRate = 1.0e6f * (float)m_binGhosts[worstBin] / (float)m_binManifolds[worstBin]; + DrawTextLine( "worst metre: %.0f m along the %s, %d ghosts (%.0f ppm)", s, GetSegmentName( s ), + m_binGhosts[worstBin], binRate ); + } + else + { + DrawTextLine( "worst metre: none" ); + } + + DrawTextLine( "bodies: %d, recycled: %d", m_bodyCount, m_recycleCount ); + DrawTextLine( "triangles: %d, stations: %d, length: %.0f m", m_mesh->triangleCount, (int)m_stations.size(), + m_totalLength ); +#endif + } + + void Render() override + { + Sample::Render(); + + int stationCount = (int)m_stations.size(); + + if ( m_drawFlow ) + { + int stride = b3MaxInt( 1, stationCount / 96 ); + float slide0 = GetSlideStart(); + for ( int k = 0; k < stationCount; k += stride ) + { + if ( m_stationDistances[k] >= slide0 ) + { + continue; + } + + b3Vec3 p = m_stations[k] + 0.05f * m_ups[k]; + b3Vec3 t = m_tangents[k]; + DrawArrow( b3ToPos( p ), b3ToPos( p + 0.25f * m_beltSpeed * t ), MakeColor( b3_colorGold ) ); + } + } + + if ( m_drawHeat ) + { + DrawHeat(); + } + + if ( m_drawGhosts ) + { + for ( int i = 0; i < m_markerCount; ++i ) + { + b3Pos p = m_markers[i].point; + DrawPoint( p, 8.0f, MakeColor( b3_colorRed ) ); + DrawLine( p, p + 0.5f * m_markers[i].normal, MakeColor( b3_colorRed ) ); + } + } + } + + // The strip runs along the outer lip. Each metre of track is a tick, gray before any traffic and green + // while clean. Ghosts raise a bar whose height follows the count and whose color runs from gold to red + // with the rate. + void DrawHeat() + { + int stationCount = (int)m_stations.size(); + + int maxGhosts = 1; + float maxRate = 0.0f; + for ( int i = 0; i < m_heatBinCount; ++i ) + { + maxGhosts = b3MaxInt( maxGhosts, m_binGhosts[i] ); + if ( m_binManifolds[i] > 0 ) + { + maxRate = b3MaxFloat( maxRate, (float)m_binGhosts[i] / (float)m_binManifolds[i] ); + } + } + + Vec4 idle = MakeColor( b3_colorDimGray ); + Vec4 clean = MakeColor( b3_colorLimeGreen ); + Vec4 low = MakeColor( b3_colorGold ); + Vec4 high = MakeColor( b3_colorRed ); + + for ( int i = 0; i < m_heatBinCount; ++i ) + { + int first = (int)( (int64_t)i * stationCount / m_heatBinCount ); + int last = (int)( (int64_t)( i + 1 ) * stationCount / m_heatBinCount ); + b3Vec3 a = m_lipPoints[first] + 0.05f * b3Vec3_axisY; + b3Vec3 b = m_lipPoints[last % stationCount] + 0.05f * b3Vec3_axisY; + + Vec4 color = m_binManifolds[i] > 0 ? clean : idle; + DrawLineEx( b3ToPos( a ), b3ToPos( b ), color, 0.04f, OVERLAY_THICKNESS_WORLD, OVERLAY_OCCLUSION_DIM ); + + if ( m_binGhosts[i] == 0 ) + { + continue; + } + + float rate = (float)m_binGhosts[i] / (float)m_binManifolds[i]; + float t = maxRate > 0.0f ? rate / maxRate : 1.0f; + Vec4 barColor = MakeVec4( low.x + t * ( high.x - low.x ), low.y + t * ( high.y - low.y ), + low.z + t * ( high.z - low.z ), 1.0f ); + + float height = 0.2f + 1.8f * (float)m_binGhosts[i] / (float)maxGhosts; + b3Vec3 base = m_lipPoints[( first + last ) / 2 % stationCount]; + DrawLineEx( b3ToPos( base ), b3ToPos( base + height * b3Vec3_axisY ), barColor, 0.08f, + OVERLAY_THICKNESS_WORLD, OVERLAY_OCCLUSION_DIM ); + } + } + + bool DrawControls() override + { + bool rebuild = false; + bool respawn = false; + + ImGui::SliderFloat( "Cell Size", &m_cellSize, 0.1f, 0.5f, "%.2f m" ); + rebuild |= ImGui::IsItemDeactivatedAfterEdit(); + + if ( ImGui::SliderFloat( "Belt Speed", &m_beltSpeed, 0.0f, 6.0f, "%.1f m/s" ) ) + { + UpdateMaterials(); + } + + if ( ImGui::SliderFloat( "Slide Friction", &m_slideFriction, 0.0f, 0.2f, "%.3f" ) ) + { + UpdateMaterials(); + } + + ImGui::SliderInt( "Body Count", &m_targetCount, 1, m_maxBodies ); + ImGui::SliderInt( "Spawn Interval", &m_spawnInterval, 1, 60 ); + + const char* bodyNames[e_bodyTypeCount] = { "Mixed", "Box", "Capsule", "Sphere", "Cylinder" }; + int bodyType = m_bodyType; + if ( ImGui::Combo( "Bodies", &bodyType, bodyNames, e_bodyTypeCount ) ) + { + m_bodyType = (BodyType)bodyType; + respawn = true; + } + + if ( ImGui::Checkbox( "Contact Recycling", &m_contactRecycling ) ) + { + for ( int i = 0; i < m_bodyCount; ++i ) + { + b3Body_EnableContactRecycling( m_bodyIds[i], m_contactRecycling ); + } + } + + ImGui::SliderFloat( "Ghost Tolerance", &m_toleranceDegrees, 1.0f, 20.0f, "%.0f deg" ); + ImGui::Checkbox( "Draw Flow", &m_drawFlow ); + ImGui::Checkbox( "Draw Ghosts", &m_drawGhosts ); + ImGui::Checkbox( "Draw Heat", &m_drawHeat ); + + respawn |= ImGui::Button( "Respawn" ); + if ( ImGui::Button( "Reset Counters" ) ) + { + ResetCounters(); + } + + if ( rebuild ) + { + DestroyCircuit(); + CreateCircuit(); + ResetCounters(); + } + + if ( respawn ) + { + DestroyBodies(); + ResetCounters(); + } + + // Shrinking the count drops the newest bodies + while ( m_bodyCount > m_targetCount ) + { + m_bodyCount -= 1; + b3DestroyBody( m_bodyIds[m_bodyCount] ); + } + + return true; + } + + static Sample* Create( SampleContext* context ) + { + return new ConveyorCircuit( context ); + } + + Segment m_segments[m_segmentCount]; + float m_segmentStart[m_segmentCount]; + float m_totalLength; + + std::vector m_stations; + std::vector m_stationDistances; + std::vector m_tangents; + std::vector m_laterals; + std::vector m_ups; + std::vector m_troughs; + std::vector m_lipPoints; + + int m_heatBinCount = 1; + std::vector m_binGhosts; + std::vector m_binManifolds; + + std::vector m_triangleNormals; + std::vector m_edgeNeighbors; + std::vector> m_vertexRings; + std::vector> m_vertexTriangles; + + b3BodyId m_groundId; + b3MeshData* m_mesh = nullptr; + b3ShapeId m_shapeId = {}; + b3HullData* m_cylinderHull; + + b3BodyId m_bodyIds[m_maxBodies]; + b3Pos m_contactCenters[m_maxBodies]; + int m_bodyStations[m_maxBodies]; + int m_bodyCount; + int m_spawnIndex; + int m_spawnTimer; + + float m_cellSize; + float m_beltSpeed; + float m_slideFriction; + int m_targetCount; + int m_spawnInterval; + BodyType m_bodyType; + float m_toleranceDegrees; + float m_cosTolerance = 1.0f; + float m_sinTolerance = 0.0f; + bool m_contactRecycling; + bool m_drawFlow; + bool m_drawGhosts; + bool m_drawHeat; + + static constexpr int m_flagCount = 3; + int m_classCounts[e_ghostClassCount]; + int m_flagCounts[m_flagCount]; + int m_ghostCount; + int m_manifoldCount; + int m_stepGhostCount; + int m_recycleCount; + Marker m_markers[m_markerCapacity]; + int m_markerCount; + int m_markerIndex; +}; + +static int sampleConveyorCircuit = RegisterSample( "Ghost", "Conveyor Circuit", ConveyorCircuit::Create ); diff --git a/samples/sample_issues.cpp b/samples/sample_issues.cpp index 05fa85a..84e5f22 100644 --- a/samples/sample_issues.cpp +++ b/samples/sample_issues.cpp @@ -2,8 +2,10 @@ // SPDX-License-Identifier: MIT #include "gfx/draw.h" +#include "gfx/keycodes.h" #include "imgui.h" #include "mesh_loader.h" +#include "meshoptimizer.h" #include "sample.h" #include "box3d/box3d.h" @@ -134,7 +136,8 @@ public: } }; -static int sampleMultiplePrismatic = RegisterSample( "Issues", "Multiple Prismatic", MultiplePrismatic::Create ); +static int sampleMultiplePrismatic = + RegisterSample( "Issues", "Multiple Prismatic", MultiplePrismatic::Create ); // bad hull SM_Waterfall_MED_Wide_01 static const b3Vec3 waterfallPoints[] = { @@ -351,7 +354,8 @@ public: b3AABB bounds = b3MakeAABB( m_points.data(), (int)m_points.size(), 0.0f ); float radius = b3Length( b3AABB_Extents( bounds ) ); - m_camera->SetView( 0.0f, 15.0f, b3MaxFloat( 3.0f * radius, 1.0f ), b3ToPos( b3AABB_Center( bounds ) ) ); + m_camera->SetView( 0.0f, 15.0f, b3MaxFloat( 3.0f * radius, 1.0f ), + b3ToPos( b3AABB_Center( bounds ) ) ); } bool DrawControls() override @@ -360,8 +364,8 @@ public: if ( m_hull != nullptr ) { - DrawTextLine( "hull: vertices = %d, edges = %d, faces = %d", m_hull->vertexCount, m_hull->edgeCount / 2, - m_hull->faceCount ); + DrawTextLine( "hull: vertices = %d, edges = %d, faces = %d", m_hull->vertexCount, + m_hull->edgeCount / 2, m_hull->faceCount ); } else { @@ -732,10 +736,12 @@ public: m_camera->SetView( 90.0f, 25.0f, 10.0f, { 0.0f, 1.0f, 0.0f } ); } - // Two chunks meeting at x = 0, each its own body and mesh shape, so seam contacts live - // in separate contact pairs like s&box world mesh chunks. A beam top straddles the seam. - CreateFloorChunk( 0, -(float)m_halfLengthU, 0.0f ); - CreateFloorChunk( 1, 0.0f, (float)m_halfLengthU ); + m_fixWinding = true; + m_combineChunks = true; + m_remesh = true; + m_simplifyErrorMillimeters = 10.0f; + m_repeatSimplify = true; + CreateFloor(); // Character { @@ -746,9 +752,13 @@ public: bodyDef.motionLocks.angularY = true; bodyDef.motionLocks.angularZ = true; bodyDef.enableSleep = false; - bodyDef.enableContactRecycling = false; + + // With current ghost mitigation this seems unnecessary + //bodyDef.enableContactRecycling = false; + bodyDef.gravityScale = 2.03f; // s&box gravity: 800 inch/s^2 bodyDef.name = "character"; + bodyDef.safetyFactor = 0.05f; m_characterId = b3CreateBody( m_worldId, &bodyDef ); b3ShapeDef shapeDef = b3DefaultShapeDef(); @@ -758,7 +768,9 @@ public: float volume = 8.0f * m_bodyHalfWidth * m_bodyHalfHeight * m_bodyHalfWidth; shapeDef.density = m_characterMass / volume; - shapeDef.enableSpeculativeContact = false; + + // With current ghost mitigation this seems unnecessary + //shapeDef.enableSpeculativeContact = false; b3BoxHull box = b3MakeBoxHull( m_bodyHalfWidth, m_bodyHalfHeight, m_bodyHalfWidth ); b3CreateHullShape( m_characterId, &shapeDef, &box.base ); @@ -778,8 +790,45 @@ public: ~SBoxGhostCollisions() override { - b3DestroyMesh( m_chunkMesh[0] ); - b3DestroyMesh( m_chunkMesh[1] ); + for ( int i = 0; i < 2; ++i ) + { + if ( m_chunkMesh[i] != nullptr ) + { + b3DestroyMesh( m_chunkMesh[i] ); + } + } + } + + void CreateFloor() + { + m_chunkTriangleCount[1] = 0; + m_simplifyPassCount[1] = 0; + + if ( m_combineChunks ) + { + CreateFloorChunk( 0, -(float)m_halfLengthU, (float)m_halfLengthU ); + return; + } + + // Two chunks meeting at x = 0, each its own body and mesh shape, so seam contacts live + // in separate contact pairs like s&box world mesh chunks. A beam top straddles the seam. + CreateFloorChunk( 0, -(float)m_halfLengthU, 0.0f ); + CreateFloorChunk( 1, 0.0f, (float)m_halfLengthU ); + } + + void DestroyFloor() + { + for ( int i = 0; i < 2; ++i ) + { + if ( B3_IS_NON_NULL( m_chunkBodyIds[i] ) ) + { + b3DestroyBody( m_chunkBodyIds[i] ); + b3DestroyMesh( m_chunkMesh[i] ); + } + + m_chunkBodyIds[i] = b3_nullBodyId; + m_chunkMesh[i] = nullptr; + } } // Deterministic integer hash for tile tessellation selection @@ -793,7 +842,8 @@ public: return x; } - static void EmitTriangle( std::vector& vertices, std::vector& indices, b3Vec3 a, b3Vec3 b, b3Vec3 c ) + static void EmitTriangle( std::vector& vertices, std::vector& indices, + b3Vec3 a, b3Vec3 b, b3Vec3 c ) { int32_t base = (int32_t)vertices.size(); vertices.push_back( a ); @@ -805,8 +855,8 @@ public: } // Horizontal patch at height y spanning [x0,x1]x[z0,z1] (inches), normal +y - static void EmitPatch( std::vector& vertices, std::vector& indices, float x0, float x1, float z0, float z1, - float y, float cell ) + static void EmitPatch( std::vector& vertices, std::vector& indices, float x0, + float x1, float z0, float z1, float y, float cell ) { if ( x1 - x0 < 0.01f ) { @@ -846,41 +896,59 @@ public: } // Sloped strip from edge (xLow, yLow) to edge (xHigh, yHigh) spanning the full z width - static void EmitSlope( std::vector& vertices, std::vector& indices, float xLow, float yLow, float xHigh, - float yHigh, float zCell ) + static void EmitSlope( std::vector& vertices, std::vector& indices, float xLow, + float yLow, float xHigh, float yHigh, float zCell, bool fixWinding ) { + // The original winding only faces out for a slope rising toward +x + bool flip = fixWinding && xHigh < xLow; + int countZ = (int)( 2.0f * (float)m_halfWidthU / zCell + 0.99f ); for ( int iz = 0; iz < countZ; ++iz ) { - float z0 = -(float)m_halfWidthU + 2.0f * (float)m_halfWidthU * (float)iz / (float)countZ; - float z1 = -(float)m_halfWidthU + 2.0f * (float)m_halfWidthU * (float)( iz + 1 ) / (float)countZ; + float z0 = + -(float)m_halfWidthU + 2.0f * (float)m_halfWidthU * (float)iz / (float)countZ; + float z1 = -(float)m_halfWidthU + + 2.0f * (float)m_halfWidthU * (float)( iz + 1 ) / (float)countZ; b3Vec3 l0 = { SRC * xLow, SRC * yLow, SRC * z0 }; b3Vec3 l1 = { SRC * xLow, SRC * yLow, SRC * z1 }; b3Vec3 h0 = { SRC * xHigh, SRC * yHigh, SRC * z0 }; b3Vec3 h1 = { SRC * xHigh, SRC * yHigh, SRC * z1 }; - EmitTriangle( vertices, indices, l0, l1, h1 ); - EmitTriangle( vertices, indices, l0, h1, h0 ); + if ( flip ) + { + EmitTriangle( vertices, indices, l0, h1, l1 ); + EmitTriangle( vertices, indices, l0, h0, h1 ); + } + else + { + EmitTriangle( vertices, indices, l0, l1, h1 ); + EmitTriangle( vertices, indices, l0, h1, h0 ); + } } } // Vertical wall at x from y0 (bottom) to y1 (top). facing = +1 faces +x, -1 faces -x - static void EmitWall( std::vector& vertices, std::vector& indices, float x, float y0, float y1, int facing, - float zCell ) + static void EmitWall( std::vector& vertices, std::vector& indices, float x, + float y0, float y1, int facing, float zCell, bool fixWinding ) { + // The original winding faced every wall into the solid + bool faceNegativeX = fixWinding ? facing < 0 : facing > 0; + int countZ = (int)( 2.0f * (float)m_halfWidthU / zCell + 0.99f ); for ( int iz = 0; iz < countZ; ++iz ) { - float z0 = -(float)m_halfWidthU + 2.0f * (float)m_halfWidthU * (float)iz / (float)countZ; - float z1 = -(float)m_halfWidthU + 2.0f * (float)m_halfWidthU * (float)( iz + 1 ) / (float)countZ; + float z0 = + -(float)m_halfWidthU + 2.0f * (float)m_halfWidthU * (float)iz / (float)countZ; + float z1 = -(float)m_halfWidthU + + 2.0f * (float)m_halfWidthU * (float)( iz + 1 ) / (float)countZ; b3Vec3 b0 = { SRC * x, SRC * y0, SRC * z0 }; b3Vec3 b1 = { SRC * x, SRC * y0, SRC * z1 }; b3Vec3 t0 = { SRC * x, SRC * y1, SRC * z0 }; b3Vec3 t1 = { SRC * x, SRC * y1, SRC * z1 }; - if ( facing > 0 ) + if ( faceNegativeX ) { EmitTriangle( vertices, indices, b0, b1, t1 ); EmitTriangle( vertices, indices, b0, t1, t0 ); @@ -901,6 +969,60 @@ public: return *o1 - *o0 > 0.01f; } + // Weld exact duplicates, then let meshoptimizer collapse every edge that leaves the surface in + // place. A t-junction vertex sits on an open border, so it can slide along that border onto a + // corner, which closes the seam. Flat collapses still cost a little from the simplifier's + // vertex regularization, so the error limit has to clear that while staying far below the inch + // scale cost of collapsing a real crease. Regularize options raise that cost. Vertex kinds are + // fixed per call, so a corner locked by a t-junction stays locked after its seam closes. + // Repeating reclassifies, but the error limit applies per pass, so deviation can accumulate. + // Winding is not repaired. Returns the number of passes. + static int Remesh( std::vector& vertices, std::vector& indices, + float errorLimit, unsigned int options, bool repeat ) + { + size_t indexCount = indices.size(); + const unsigned int* sourceIndices = reinterpret_cast( indices.data() ); + + std::vector remap( vertices.size() ); + size_t vertexCount = + meshopt_generateVertexRemap( remap.data(), sourceIndices, indexCount, vertices.data(), + vertices.size(), sizeof( b3Vec3 ) ); + + std::vector welded( indexCount ); + meshopt_remapIndexBuffer( welded.data(), sourceIndices, indexCount, remap.data() ); + + std::vector positions( vertexCount ); + meshopt_remapVertexBuffer( positions.data(), vertices.data(), vertices.size(), + sizeof( b3Vec3 ), remap.data() ); + + std::vector simplified( indexCount ); + int passCount = 0; + for ( ;; ) + { + size_t simplifiedCount = meshopt_simplify( + simplified.data(), welded.data(), welded.size(), &positions[0].x, vertexCount, + sizeof( b3Vec3 ), 0, errorLimit, meshopt_SimplifyErrorAbsolute | options, nullptr ); + passCount += 1; + + bool reduced = simplifiedCount < welded.size(); + welded.assign( simplified.begin(), simplified.begin() + simplifiedCount ); + + if ( repeat == false || reduced == false ) + { + break; + } + } + + size_t usedCount = + meshopt_optimizeVertexFetch( positions.data(), welded.data(), welded.size(), + positions.data(), vertexCount, sizeof( b3Vec3 ) ); + positions.resize( usedCount ); + + vertices = positions; + indices.assign( welded.begin(), welded.end() ); + return passCount; + } + void CreateFloorChunk( int chunk, float x0U, float x1U ) { std::vector vertices; @@ -911,7 +1033,8 @@ public: // --- Concrete slabs at y = 0 outside the beam region --- // Tiles tessellate at a hash-picked resolution so neighbors meet with T-junctions, // like cooked s&box map collision. - float slabSpans[2][2] = { { -(float)m_halfLengthU, m_beamRegion0 }, { m_beamRegion1, (float)m_halfLengthU } }; + float slabSpans[2][2] = { { -(float)m_halfLengthU, m_beamRegion0 }, + { m_beamRegion1, (float)m_halfLengthU } }; for ( int i = 0; i < 2; ++i ) { if ( ClipSpan( slabSpans[i][0], slabSpans[i][1], x0U, x1U, &s0, &s1 ) == false ) @@ -924,10 +1047,15 @@ public: float tx1 = b3MinFloat( tx + (float)m_tileSizeU, s1 ); for ( int tz = -m_halfWidthU; tz < m_halfWidthU; tz += m_tileSizeU ) { - uint32_t h = Hash( (uint32_t)( (int)tx * 73856093 ) ^ (uint32_t)( tz * 19349663 ) ^ - (uint32_t)( chunk * 2654435761u ) ); + // Seeded by side rather than chunk so combined and split floors tessellate the + // same + uint32_t side = tx < 0.0f ? 0u : 1u; + uint32_t h = + Hash( (uint32_t)( (int)tx * 73856093 ) ^ (uint32_t)( tz * 19349663 ) ^ + (uint32_t)( side * 2654435761u ) ); float cells[3] = { 4.0f, 8.0f, 16.0f }; - EmitPatch( vertices, indices, tx, tx1, (float)tz, (float)( tz + m_tileSizeU ), 0.0f, cells[h % 3] ); + EmitPatch( vertices, indices, tx, tx1, (float)tz, (float)( tz + m_tileSizeU ), + 0.0f, cells[h % 3] ); } } } @@ -936,6 +1064,9 @@ public: float pitTop = -m_chamferDropU; float pitBottom = -m_pitDepthU; + // Coarse walls and pit floors leave t-junctions against the 8 inch chamfer cells + float wellCell = 16.0f; + for ( int k = 0; k < m_beamCount; ++k ) { float bx = m_beamRegion0 + m_beamPitchU * (float)k; @@ -947,17 +1078,20 @@ public: float top1 = pitRight ? bx + m_beamWidthU - m_chamferWidthU : bx + m_beamWidthU; if ( ClipSpan( top0, top1, x0U, x1U, &s0, &s1 ) ) { - EmitPatch( vertices, indices, s0, s1, -(float)m_halfWidthU, (float)m_halfWidthU, 0.0f, 8.0f ); + EmitPatch( vertices, indices, s0, s1, -(float)m_halfWidthU, (float)m_halfWidthU, + 0.0f, 8.0f ); } // Chamfers sloping below the walkable plane if ( pitLeft && bx >= x0U && bx < x1U ) { - EmitSlope( vertices, indices, bx, pitTop, bx + m_chamferWidthU, 0.0f, 8.0f ); + EmitSlope( vertices, indices, bx, pitTop, bx + m_chamferWidthU, 0.0f, 8.0f, + m_fixWinding ); } if ( pitRight && bx + m_beamWidthU > x0U && bx + m_beamWidthU <= x1U ) { - EmitSlope( vertices, indices, bx + m_beamWidthU, pitTop, bx + m_beamWidthU - m_chamferWidthU, 0.0f, 8.0f ); + EmitSlope( vertices, indices, bx + m_beamWidthU, pitTop, + bx + m_beamWidthU - m_chamferWidthU, 0.0f, 8.0f, m_fixWinding ); } // Pit to the right of this beam @@ -967,19 +1101,30 @@ public: float pitR = bx + m_beamPitchU; if ( pitL >= x0U && pitL < x1U ) { - EmitWall( vertices, indices, pitL, pitBottom, pitTop, +1, 16.0f ); + EmitWall( vertices, indices, pitL, pitBottom, pitTop, +1, wellCell, + m_fixWinding ); } if ( pitR > x0U && pitR <= x1U ) { - EmitWall( vertices, indices, pitR, pitBottom, pitTop, -1, 16.0f ); + EmitWall( vertices, indices, pitR, pitBottom, pitTop, -1, wellCell, + m_fixWinding ); } if ( ClipSpan( pitL, pitR, x0U, x1U, &s0, &s1 ) ) { - EmitPatch( vertices, indices, s0, s1, -(float)m_halfWidthU, (float)m_halfWidthU, pitBottom, 16.0f ); + EmitPatch( vertices, indices, s0, s1, -(float)m_halfWidthU, (float)m_halfWidthU, + pitBottom, wellCell ); } } } + if ( m_remesh ) + { + m_simplifyPassCount[chunk] = Remesh( + vertices, indices, 0.001f * m_simplifyErrorMillimeters, 0, m_repeatSimplify ); + } + + m_chunkTriangleCount[chunk] = (int)( indices.size() / 3 ); + b3MeshDef meshDef = {}; meshDef.vertices = vertices.data(); meshDef.indices = indices.data(); @@ -992,10 +1137,10 @@ public: m_chunkMesh[chunk] = b3CreateMesh( &meshDef, nullptr, 0 ); b3BodyDef bodyDef = b3DefaultBodyDef(); - b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + m_chunkBodyIds[chunk] = b3CreateBody( m_worldId, &bodyDef ); b3ShapeDef shapeDef = b3DefaultShapeDef(); - b3CreateMeshShape( body, &shapeDef, m_chunkMesh[chunk], b3Vec3_one ); + b3CreateMeshShape( m_chunkBodyIds[chunk], &shapeDef, m_chunkMesh[chunk], b3Vec3_one ); } void Step() override @@ -1060,9 +1205,25 @@ public: } b3Vec3 currentVelocity = b3Body_GetLinearVelocity( m_characterId ); - DrawTextLine( "ghost launches: %d, worst: %.2f m/s (%.0f inch/s)", m_launchCount, m_maxLaunchSpeed, - m_maxLaunchSpeed / SRC ); + DrawTextLine( "ghost launches: %d, worst: %.2f m/s (%.0f inch/s)", m_launchCount, + m_maxLaunchSpeed, m_maxLaunchSpeed / SRC ); DrawTextLine( "vertical velocity: %.2f m/s", currentVelocity.y ); + if ( m_combineChunks ) + { + DrawTextLine( "floor triangles: %d", m_chunkTriangleCount[0] ); + } + else + { + DrawTextLine( "floor triangles: %d (chunks %d + %d)", + m_chunkTriangleCount[0] + m_chunkTriangleCount[1], + m_chunkTriangleCount[0], m_chunkTriangleCount[1] ); + } + + if ( m_remesh ) + { + DrawTextLine( m_combineChunks ? "simplify passes: %d" : "simplify passes: %d + %d", + m_simplifyPassCount[0], m_simplifyPassCount[1] ); + } } bool DrawControls() override @@ -1079,7 +1240,24 @@ public: m_walkSpeedZ = speedUZ * SRC; } - if ( ImGui::Button( "Reset Counters" ) ) + bool rebuild = ImGui::Checkbox( "Fix Winding", &m_fixWinding ); + rebuild = ImGui::Checkbox( "Combine Chunks", &m_combineChunks ) || rebuild; + rebuild = ImGui::Checkbox( "Remesh (meshoptimizer)", &m_remesh ) || rebuild; + + ImGui::SliderFloat( "Simplify Error (mm)", &m_simplifyErrorMillimeters, 0.1f, 50.0f, "%.1f", + ImGuiSliderFlags_Logarithmic ); + rebuild = ( ImGui::IsItemDeactivatedAfterEdit() && m_remesh ) || rebuild; + + rebuild = + ( ImGui::Checkbox( "Repeat Simplify", &m_repeatSimplify ) && m_remesh ) || rebuild; + + if ( rebuild ) + { + DestroyFloor(); + CreateFloor(); + } + + if ( ImGui::Button( "Reset Counters" ) || rebuild ) { m_launchCount = 0; m_maxLaunchSpeed = 0.0f; @@ -1095,12 +1273,21 @@ public: return new SBoxGhostCollisions( context ); } - static constexpr float m_walkRangeX = 3.5f; // turn around beyond +/- this x (meters) - static constexpr float m_walkRangeZ = 0.5f; // turn around beyond +/- this x (meters) - static constexpr float m_launchThreshold = 0.5f; // upward m/s counted as a ghost launch (~20 inch/s) + static constexpr float m_walkRangeX = 3.5f; // turn around beyond +/- this x (meters) + static constexpr float m_walkRangeZ = 0.5f; // turn around beyond +/- this x (meters) + static constexpr float m_launchThreshold = + 0.5f; // upward m/s counted as a ghost launch (~20 inch/s) static constexpr int m_markerCapacity = 64; b3MeshData* m_chunkMesh[2] = {}; + b3BodyId m_chunkBodyIds[2] = { b3_nullBodyId, b3_nullBodyId }; + bool m_fixWinding; + bool m_combineChunks; + bool m_remesh; + float m_simplifyErrorMillimeters; + bool m_repeatSimplify; + int m_simplifyPassCount[2] = {}; + int m_chunkTriangleCount[2] = {}; b3BodyId m_characterId; float m_walkDirectionX; float m_walkDirectionZ; @@ -1113,115 +1300,169 @@ public: int m_launchMarkerCount; }; -static int sampleSBoxGhostCollisions = RegisterSample( "Issues", "s&box Ghost Collisions", SBoxGhostCollisions::Create ); +static int sampleSBoxGhostCollisions = + RegisterSample( "Issues", "s&box Ghost Collisions", SBoxGhostCollisions::Create ); static const b3Vec3 s_metalWheel1Verts[317] = { - { 0.010279f, 0.086341f, 0.051768f }, { -0.010314f, -0.084708f, 0.051804f }, { 0.010279f, -0.084708f, 0.051768f }, - { -0.010314f, 0.086341f, 0.051804f }, { 0.029138f, -0.084708f, -0.043911f }, { 0.043724f, -0.084708f, -0.029376f }, - { 0.010098f, -0.084708f, -0.051759f }, { -0.010494f, -0.084708f, -0.051723f }, { 0.051638f, -0.084708f, -0.010364f }, - { 0.051674f, -0.084708f, 0.010229f }, { 0.043827f, -0.084708f, 0.029268f }, { 0.029291f, -0.084708f, 0.043855f }, - { -0.029353f, -0.084708f, 0.043957f }, { -0.051889f, -0.084708f, -0.010183f }, { -0.051853f, -0.084708f, 0.010409f }, - { -0.043939f, -0.084708f, 0.029421f }, { -0.029506f, -0.084708f, -0.043809f }, { -0.044042f, -0.084708f, -0.029222f }, - { -0.029353f, 0.086341f, 0.043957f }, { 0.043724f, 0.086341f, -0.029376f }, { 0.029138f, 0.086341f, -0.043911f }, - { 0.010098f, 0.086341f, -0.051759f }, { 0.051638f, 0.086341f, -0.010364f }, { 0.051674f, 0.086341f, 0.010229f }, - { -0.044042f, 0.086341f, -0.029222f }, { 0.043827f, 0.086341f, 0.029268f }, { -0.051889f, 0.086341f, -0.010183f }, - { -0.043939f, 0.086341f, 0.029421f }, { -0.029506f, 0.086341f, -0.043809f }, { 0.035354f, 0.070897f, 0.035658f }, - { 0.035354f, -0.069397f, 0.035658f }, { -0.035498f, 0.070897f, 0.035658f }, { -0.035498f, -0.069397f, 0.035658f }, - { 0.035354f, -0.069397f, 0.365503f }, { 0.035354f, 0.070897f, 0.365503f }, { -0.035498f, 0.070897f, 0.365503f }, - { -0.035498f, -0.069397f, 0.365503f }, { 0.035354f, 0.070897f, -0.365033f }, { 0.035354f, -0.069397f, -0.365033f }, - { -0.035498f, 0.070897f, -0.365033f }, { -0.035498f, -0.069397f, -0.365033f }, { 0.035354f, -0.069397f, -0.035187f }, - { 0.035354f, 0.070897f, -0.035187f }, { -0.035498f, 0.070897f, -0.035187f }, { -0.035498f, -0.069397f, -0.035187f }, - { -0.035494f, -0.069397f, -0.035191f }, { -0.365340f, -0.069397f, -0.035191f }, { -0.035494f, 0.070897f, -0.035191f }, - { -0.365340f, 0.070897f, -0.035191f }, { -0.365340f, -0.069397f, 0.035661f }, { -0.035494f, -0.069397f, 0.035661f }, - { -0.035494f, 0.070897f, 0.035661f }, { -0.365340f, 0.070897f, 0.035661f }, { 0.035350f, 0.070897f, -0.035191f }, - { 0.365196f, 0.070897f, -0.035191f }, { 0.035350f, -0.069397f, -0.035191f }, { 0.365196f, -0.069397f, -0.035191f }, - { 0.365196f, 0.070897f, 0.035661f }, { 0.035350f, 0.070897f, 0.035661f }, { 0.035350f, -0.069397f, 0.035661f }, - { 0.365196f, -0.069397f, 0.035661f }, { -0.070802f, 0.086337f, 0.355420f }, { -0.070802f, -0.084705f, 0.355420f }, - { -0.097081f, 0.086335f, 0.487537f }, { -0.097081f, -0.084704f, 0.487537f }, { -0.000108f, 0.086337f, 0.362383f }, - { -0.000108f, 0.086335f, 0.497088f }, { -0.000108f, -0.084704f, 0.497088f }, { -0.000108f, -0.084705f, 0.362383f }, - { -0.138787f, -0.084705f, 0.334799f }, { -0.138787f, 0.086337f, 0.334799f }, { -0.070810f, -0.084705f, 0.355419f }, - { -0.070810f, 0.086337f, 0.355419f }, { -0.097090f, 0.086335f, 0.487536f }, { -0.190336f, 0.086335f, 0.459250f }, - { -0.190336f, -0.084704f, 0.459250f }, { -0.097090f, -0.084704f, 0.487536f }, { -0.138795f, 0.086337f, 0.334796f }, - { -0.138795f, -0.084705f, 0.334796f }, { -0.201442f, 0.086337f, 0.301310f }, { -0.201442f, -0.084705f, 0.301310f }, - { -0.276280f, -0.084704f, 0.413313f }, { -0.276280f, 0.086335f, 0.413313f }, { -0.190344f, 0.086335f, 0.459247f }, - { -0.190344f, -0.084704f, 0.459247f }, { -0.201450f, -0.084705f, 0.301306f }, { -0.256361f, -0.084705f, 0.256241f }, - { -0.201450f, 0.086337f, 0.301306f }, { -0.256361f, 0.086337f, 0.256241f }, { -0.351612f, 0.086335f, 0.351492f }, - { -0.351612f, -0.084704f, 0.351492f }, { -0.276288f, 0.086335f, 0.413309f }, { -0.276288f, -0.084704f, 0.413309f }, - { -0.301431f, -0.084705f, 0.201325f }, { -0.413435f, -0.084704f, 0.276163f }, { -0.413435f, 0.086335f, 0.276163f }, - { -0.301431f, 0.086337f, 0.201325f }, { -0.256367f, -0.084705f, 0.256236f }, { -0.256367f, 0.086337f, 0.256236f }, - { -0.351618f, 0.086335f, 0.351487f }, { -0.351618f, -0.084704f, 0.351487f }, { -0.334922f, -0.084705f, 0.138670f }, - { -0.459374f, -0.084704f, 0.190220f }, { -0.459374f, 0.086335f, 0.190220f }, { -0.334922f, 0.086337f, 0.138670f }, - { -0.301436f, 0.086337f, 0.201318f }, { -0.301436f, -0.084705f, 0.201318f }, { -0.413440f, 0.086335f, 0.276156f }, - { -0.413440f, -0.084704f, 0.276156f }, { -0.487663f, -0.084704f, 0.096966f }, { -0.355546f, -0.084705f, 0.070686f }, - { -0.459377f, -0.084704f, 0.190212f }, { -0.334926f, -0.084705f, 0.138663f }, { -0.355546f, 0.086337f, 0.070686f }, - { -0.334926f, 0.086337f, 0.138663f }, { -0.487663f, 0.086335f, 0.096966f }, { -0.459377f, 0.086335f, 0.190212f }, - { -0.362511f, -0.084705f, -0.000016f }, { -0.355549f, -0.084705f, 0.070678f }, { -0.497217f, -0.084704f, -0.000016f }, - { -0.487665f, -0.084704f, 0.096957f }, { -0.497217f, 0.086335f, -0.000016f }, { -0.362511f, 0.086337f, -0.000016f }, - { -0.355549f, 0.086337f, 0.070678f }, { -0.487665f, 0.086335f, 0.096957f }, { -0.362512f, 0.086337f, -0.000024f }, - { -0.362512f, -0.084705f, -0.000024f }, { -0.355549f, 0.086337f, -0.070717f }, { -0.355549f, -0.084705f, -0.070717f }, - { -0.497217f, -0.084704f, -0.000024f }, { -0.487666f, -0.084704f, -0.096997f }, { -0.497217f, 0.086335f, -0.000024f }, - { -0.487666f, 0.086335f, -0.096997f }, { -0.334927f, -0.084705f, -0.138702f }, { -0.355548f, 0.086337f, -0.070726f }, - { -0.355548f, -0.084705f, -0.070726f }, { -0.334927f, 0.086337f, -0.138702f }, { -0.487665f, -0.084704f, -0.097005f }, - { -0.459379f, -0.084704f, -0.190252f }, { -0.487665f, 0.086335f, -0.097005f }, { -0.459379f, 0.086335f, -0.190252f }, - { -0.413442f, -0.084704f, -0.276196f }, { -0.301439f, -0.084705f, -0.201358f }, { -0.334925f, -0.084705f, -0.138710f }, - { -0.459376f, -0.084704f, -0.190260f }, { -0.334925f, 0.086337f, -0.138710f }, { -0.301439f, 0.086337f, -0.201358f }, - { -0.459376f, 0.086335f, -0.190260f }, { -0.413442f, 0.086335f, -0.276196f }, { -0.256370f, -0.084705f, -0.256276f }, - { -0.301434f, 0.086337f, -0.201365f }, { -0.301434f, -0.084705f, -0.201365f }, { -0.256370f, 0.086337f, -0.256276f }, - { -0.413438f, 0.086335f, -0.276203f }, { -0.351621f, 0.086335f, -0.351527f }, { -0.413438f, -0.084704f, -0.276203f }, - { -0.351621f, -0.084704f, -0.351527f }, { -0.256364f, -0.084705f, -0.256283f }, { -0.201453f, 0.086337f, -0.301347f }, - { -0.256364f, 0.086337f, -0.256283f }, { -0.201453f, -0.084705f, -0.301347f }, { -0.276292f, -0.084704f, -0.413350f }, - { -0.351615f, -0.084704f, -0.351534f }, { -0.351615f, 0.086335f, -0.351534f }, { -0.276292f, 0.086335f, -0.413350f }, - { -0.201447f, -0.084705f, -0.301352f }, { -0.276285f, -0.084704f, -0.413355f }, { -0.138799f, -0.084705f, -0.334838f }, - { -0.190349f, -0.084704f, -0.459289f }, { -0.276285f, 0.086335f, -0.413355f }, { -0.201447f, 0.086337f, -0.301352f }, - { -0.190349f, 0.086335f, -0.459289f }, { -0.138799f, 0.086337f, -0.334838f }, { -0.190341f, 0.086335f, -0.459293f }, - { -0.190341f, -0.084704f, -0.459293f }, { -0.138791f, -0.084705f, -0.334842f }, { -0.138791f, 0.086337f, -0.334842f }, - { -0.070815f, 0.086337f, -0.355462f }, { -0.070815f, -0.084705f, -0.355462f }, { -0.097095f, 0.086335f, -0.487579f }, - { -0.097095f, -0.084704f, -0.487579f }, { -0.070807f, 0.086337f, -0.355464f }, { -0.097086f, 0.086335f, -0.487581f }, - { -0.097086f, -0.084704f, -0.487581f }, { -0.070807f, -0.084705f, -0.355464f }, { -0.000113f, -0.084705f, -0.362427f }, - { -0.000113f, 0.086337f, -0.362427f }, { -0.000113f, 0.086335f, -0.497132f }, { -0.000113f, -0.084704f, -0.497132f }, - { 0.070588f, 0.086337f, -0.355465f }, { 0.096868f, -0.084704f, -0.487582f }, { 0.096868f, 0.086335f, -0.487582f }, - { 0.070588f, -0.084705f, -0.355465f }, { -0.000105f, 0.086337f, -0.362427f }, { -0.000105f, 0.086335f, -0.497133f }, - { -0.000105f, -0.084704f, -0.497133f }, { -0.000105f, -0.084705f, -0.362427f }, { 0.190123f, -0.084704f, -0.459294f }, - { 0.190123f, 0.086335f, -0.459294f }, { 0.138573f, -0.084705f, -0.334843f }, { 0.138573f, 0.086337f, -0.334843f }, - { 0.070597f, 0.086337f, -0.355463f }, { 0.070597f, -0.084705f, -0.355463f }, { 0.096876f, -0.084704f, -0.487580f }, - { 0.096876f, 0.086335f, -0.487580f }, { 0.201229f, -0.084705f, -0.301354f }, { 0.276067f, -0.084704f, -0.413358f }, - { 0.201229f, 0.086337f, -0.301354f }, { 0.138581f, -0.084705f, -0.334840f }, { 0.138581f, 0.086337f, -0.334840f }, - { 0.190131f, 0.086335f, -0.459292f }, { 0.276067f, 0.086335f, -0.413358f }, { 0.190131f, -0.084704f, -0.459292f }, - { 0.201236f, 0.086337f, -0.301350f }, { 0.256147f, -0.084705f, -0.256286f }, { 0.256147f, 0.086337f, -0.256286f }, - { 0.201236f, -0.084705f, -0.301350f }, { 0.351398f, -0.084704f, -0.351537f }, { 0.351398f, 0.086335f, -0.351537f }, - { 0.276074f, -0.084704f, -0.413353f }, { 0.276074f, 0.086335f, -0.413353f }, { 0.301218f, -0.084705f, -0.201369f }, - { 0.301218f, 0.086337f, -0.201369f }, { 0.256154f, -0.084705f, -0.256280f }, { 0.256154f, 0.086337f, -0.256280f }, - { 0.413221f, 0.086335f, -0.276207f }, { 0.413221f, -0.084704f, -0.276207f }, { 0.351405f, 0.086335f, -0.351531f }, - { 0.351405f, -0.084704f, -0.351531f }, { 0.334709f, 0.086337f, -0.138715f }, { 0.301223f, 0.086337f, -0.201362f }, - { 0.334709f, -0.084705f, -0.138715f }, { 0.301223f, -0.084705f, -0.201362f }, { 0.459160f, -0.084704f, -0.190264f }, - { 0.459160f, 0.086335f, -0.190264f }, { 0.413226f, 0.086335f, -0.276200f }, { 0.413226f, -0.084704f, -0.276200f }, - { 0.334713f, -0.084705f, -0.138707f }, { 0.355333f, -0.084705f, -0.070731f }, { 0.334713f, 0.086337f, -0.138707f }, - { 0.355333f, 0.086337f, -0.070731f }, { 0.459164f, 0.086335f, -0.190257f }, { 0.487450f, 0.086335f, -0.097010f }, - { 0.487450f, -0.084704f, -0.097010f }, { 0.459164f, -0.084704f, -0.190257f }, { 0.355335f, -0.084705f, -0.070722f }, - { 0.362298f, -0.084705f, -0.000029f }, { 0.355335f, 0.086337f, -0.070722f }, { 0.362298f, 0.086337f, -0.000029f }, - { 0.497003f, -0.084704f, -0.000029f }, { 0.497003f, 0.086335f, -0.000029f }, { 0.487452f, 0.086335f, -0.097002f }, - { 0.487452f, -0.084704f, -0.097002f }, { 0.362298f, -0.084705f, -0.000021f }, { 0.497003f, -0.084704f, -0.000021f }, - { 0.355336f, -0.084705f, 0.070673f }, { 0.487453f, -0.084704f, 0.096952f }, { 0.497003f, 0.086335f, -0.000021f }, - { 0.362298f, 0.086337f, -0.000021f }, { 0.355336f, 0.086337f, 0.070673f }, { 0.487453f, 0.086335f, 0.096952f }, - { 0.355334f, -0.084705f, 0.070681f }, { 0.355334f, 0.086337f, 0.070681f }, { 0.487451f, 0.086335f, 0.096961f }, - { 0.487451f, -0.084704f, 0.096961f }, { 0.334714f, -0.084705f, 0.138658f }, { 0.459165f, -0.084704f, 0.190207f }, - { 0.334714f, 0.086337f, 0.138658f }, { 0.459165f, 0.086335f, 0.190207f }, { 0.334711f, 0.086337f, 0.138666f }, - { 0.301225f, 0.086337f, 0.201313f }, { 0.459163f, 0.086335f, 0.190215f }, { 0.413229f, 0.086335f, 0.276151f }, - { 0.334711f, -0.084705f, 0.138666f }, { 0.301225f, -0.084705f, 0.201313f }, { 0.413229f, -0.084704f, 0.276151f }, - { 0.459163f, -0.084704f, 0.190215f }, { 0.301221f, -0.084705f, 0.201321f }, { 0.256157f, -0.084705f, 0.256232f }, - { 0.301221f, 0.086337f, 0.201321f }, { 0.256157f, 0.086337f, 0.256232f }, { 0.413224f, -0.084704f, 0.276159f }, - { 0.413224f, 0.086335f, 0.276159f }, { 0.351408f, 0.086335f, 0.351483f }, { 0.351408f, -0.084704f, 0.351483f }, - { 0.201240f, -0.084705f, 0.301302f }, { 0.201240f, 0.086337f, 0.301302f }, { 0.256151f, -0.084705f, 0.256238f }, - { 0.256151f, 0.086337f, 0.256238f }, { 0.351402f, 0.086335f, 0.351489f }, { 0.351402f, -0.084704f, 0.351489f }, - { 0.276078f, 0.086335f, 0.413305f }, { 0.276078f, -0.084704f, 0.413305f }, { 0.138586f, -0.084705f, 0.334793f }, - { 0.201233f, -0.084705f, 0.301307f }, { 0.190135f, -0.084704f, 0.459244f }, { 0.276071f, -0.084704f, 0.413311f }, - { 0.201233f, 0.086337f, 0.301307f }, { 0.138586f, 0.086337f, 0.334793f }, { 0.276071f, 0.086335f, 0.413311f }, - { 0.190135f, 0.086335f, 0.459244f }, { 0.138578f, -0.084705f, 0.334797f }, { 0.070602f, -0.084705f, 0.355417f }, - { 0.138578f, 0.086337f, 0.334797f }, { 0.070602f, 0.086337f, 0.355417f }, { 0.190127f, 0.086335f, 0.459248f }, - { 0.190127f, -0.084704f, 0.459248f }, { 0.096881f, 0.086335f, 0.487534f }, { 0.096881f, -0.084704f, 0.487534f }, - { 0.070593f, -0.084705f, 0.355419f }, { 0.096873f, 0.086335f, 0.487536f }, { 0.096873f, -0.084704f, 0.487536f }, - { 0.070593f, 0.086337f, 0.355419f }, { -0.000100f, 0.086337f, 0.362382f }, { -0.000100f, 0.086335f, 0.497087f }, - { -0.000100f, -0.084704f, 0.497087f }, { -0.000100f, -0.084705f, 0.362382f }, + { 0.010279f, 0.086341f, 0.051768f }, { -0.010314f, -0.084708f, 0.051804f }, + { 0.010279f, -0.084708f, 0.051768f }, { -0.010314f, 0.086341f, 0.051804f }, + { 0.029138f, -0.084708f, -0.043911f }, { 0.043724f, -0.084708f, -0.029376f }, + { 0.010098f, -0.084708f, -0.051759f }, { -0.010494f, -0.084708f, -0.051723f }, + { 0.051638f, -0.084708f, -0.010364f }, { 0.051674f, -0.084708f, 0.010229f }, + { 0.043827f, -0.084708f, 0.029268f }, { 0.029291f, -0.084708f, 0.043855f }, + { -0.029353f, -0.084708f, 0.043957f }, { -0.051889f, -0.084708f, -0.010183f }, + { -0.051853f, -0.084708f, 0.010409f }, { -0.043939f, -0.084708f, 0.029421f }, + { -0.029506f, -0.084708f, -0.043809f }, { -0.044042f, -0.084708f, -0.029222f }, + { -0.029353f, 0.086341f, 0.043957f }, { 0.043724f, 0.086341f, -0.029376f }, + { 0.029138f, 0.086341f, -0.043911f }, { 0.010098f, 0.086341f, -0.051759f }, + { 0.051638f, 0.086341f, -0.010364f }, { 0.051674f, 0.086341f, 0.010229f }, + { -0.044042f, 0.086341f, -0.029222f }, { 0.043827f, 0.086341f, 0.029268f }, + { -0.051889f, 0.086341f, -0.010183f }, { -0.043939f, 0.086341f, 0.029421f }, + { -0.029506f, 0.086341f, -0.043809f }, { 0.035354f, 0.070897f, 0.035658f }, + { 0.035354f, -0.069397f, 0.035658f }, { -0.035498f, 0.070897f, 0.035658f }, + { -0.035498f, -0.069397f, 0.035658f }, { 0.035354f, -0.069397f, 0.365503f }, + { 0.035354f, 0.070897f, 0.365503f }, { -0.035498f, 0.070897f, 0.365503f }, + { -0.035498f, -0.069397f, 0.365503f }, { 0.035354f, 0.070897f, -0.365033f }, + { 0.035354f, -0.069397f, -0.365033f }, { -0.035498f, 0.070897f, -0.365033f }, + { -0.035498f, -0.069397f, -0.365033f }, { 0.035354f, -0.069397f, -0.035187f }, + { 0.035354f, 0.070897f, -0.035187f }, { -0.035498f, 0.070897f, -0.035187f }, + { -0.035498f, -0.069397f, -0.035187f }, { -0.035494f, -0.069397f, -0.035191f }, + { -0.365340f, -0.069397f, -0.035191f }, { -0.035494f, 0.070897f, -0.035191f }, + { -0.365340f, 0.070897f, -0.035191f }, { -0.365340f, -0.069397f, 0.035661f }, + { -0.035494f, -0.069397f, 0.035661f }, { -0.035494f, 0.070897f, 0.035661f }, + { -0.365340f, 0.070897f, 0.035661f }, { 0.035350f, 0.070897f, -0.035191f }, + { 0.365196f, 0.070897f, -0.035191f }, { 0.035350f, -0.069397f, -0.035191f }, + { 0.365196f, -0.069397f, -0.035191f }, { 0.365196f, 0.070897f, 0.035661f }, + { 0.035350f, 0.070897f, 0.035661f }, { 0.035350f, -0.069397f, 0.035661f }, + { 0.365196f, -0.069397f, 0.035661f }, { -0.070802f, 0.086337f, 0.355420f }, + { -0.070802f, -0.084705f, 0.355420f }, { -0.097081f, 0.086335f, 0.487537f }, + { -0.097081f, -0.084704f, 0.487537f }, { -0.000108f, 0.086337f, 0.362383f }, + { -0.000108f, 0.086335f, 0.497088f }, { -0.000108f, -0.084704f, 0.497088f }, + { -0.000108f, -0.084705f, 0.362383f }, { -0.138787f, -0.084705f, 0.334799f }, + { -0.138787f, 0.086337f, 0.334799f }, { -0.070810f, -0.084705f, 0.355419f }, + { -0.070810f, 0.086337f, 0.355419f }, { -0.097090f, 0.086335f, 0.487536f }, + { -0.190336f, 0.086335f, 0.459250f }, { -0.190336f, -0.084704f, 0.459250f }, + { -0.097090f, -0.084704f, 0.487536f }, { -0.138795f, 0.086337f, 0.334796f }, + { -0.138795f, -0.084705f, 0.334796f }, { -0.201442f, 0.086337f, 0.301310f }, + { -0.201442f, -0.084705f, 0.301310f }, { -0.276280f, -0.084704f, 0.413313f }, + { -0.276280f, 0.086335f, 0.413313f }, { -0.190344f, 0.086335f, 0.459247f }, + { -0.190344f, -0.084704f, 0.459247f }, { -0.201450f, -0.084705f, 0.301306f }, + { -0.256361f, -0.084705f, 0.256241f }, { -0.201450f, 0.086337f, 0.301306f }, + { -0.256361f, 0.086337f, 0.256241f }, { -0.351612f, 0.086335f, 0.351492f }, + { -0.351612f, -0.084704f, 0.351492f }, { -0.276288f, 0.086335f, 0.413309f }, + { -0.276288f, -0.084704f, 0.413309f }, { -0.301431f, -0.084705f, 0.201325f }, + { -0.413435f, -0.084704f, 0.276163f }, { -0.413435f, 0.086335f, 0.276163f }, + { -0.301431f, 0.086337f, 0.201325f }, { -0.256367f, -0.084705f, 0.256236f }, + { -0.256367f, 0.086337f, 0.256236f }, { -0.351618f, 0.086335f, 0.351487f }, + { -0.351618f, -0.084704f, 0.351487f }, { -0.334922f, -0.084705f, 0.138670f }, + { -0.459374f, -0.084704f, 0.190220f }, { -0.459374f, 0.086335f, 0.190220f }, + { -0.334922f, 0.086337f, 0.138670f }, { -0.301436f, 0.086337f, 0.201318f }, + { -0.301436f, -0.084705f, 0.201318f }, { -0.413440f, 0.086335f, 0.276156f }, + { -0.413440f, -0.084704f, 0.276156f }, { -0.487663f, -0.084704f, 0.096966f }, + { -0.355546f, -0.084705f, 0.070686f }, { -0.459377f, -0.084704f, 0.190212f }, + { -0.334926f, -0.084705f, 0.138663f }, { -0.355546f, 0.086337f, 0.070686f }, + { -0.334926f, 0.086337f, 0.138663f }, { -0.487663f, 0.086335f, 0.096966f }, + { -0.459377f, 0.086335f, 0.190212f }, { -0.362511f, -0.084705f, -0.000016f }, + { -0.355549f, -0.084705f, 0.070678f }, { -0.497217f, -0.084704f, -0.000016f }, + { -0.487665f, -0.084704f, 0.096957f }, { -0.497217f, 0.086335f, -0.000016f }, + { -0.362511f, 0.086337f, -0.000016f }, { -0.355549f, 0.086337f, 0.070678f }, + { -0.487665f, 0.086335f, 0.096957f }, { -0.362512f, 0.086337f, -0.000024f }, + { -0.362512f, -0.084705f, -0.000024f }, { -0.355549f, 0.086337f, -0.070717f }, + { -0.355549f, -0.084705f, -0.070717f }, { -0.497217f, -0.084704f, -0.000024f }, + { -0.487666f, -0.084704f, -0.096997f }, { -0.497217f, 0.086335f, -0.000024f }, + { -0.487666f, 0.086335f, -0.096997f }, { -0.334927f, -0.084705f, -0.138702f }, + { -0.355548f, 0.086337f, -0.070726f }, { -0.355548f, -0.084705f, -0.070726f }, + { -0.334927f, 0.086337f, -0.138702f }, { -0.487665f, -0.084704f, -0.097005f }, + { -0.459379f, -0.084704f, -0.190252f }, { -0.487665f, 0.086335f, -0.097005f }, + { -0.459379f, 0.086335f, -0.190252f }, { -0.413442f, -0.084704f, -0.276196f }, + { -0.301439f, -0.084705f, -0.201358f }, { -0.334925f, -0.084705f, -0.138710f }, + { -0.459376f, -0.084704f, -0.190260f }, { -0.334925f, 0.086337f, -0.138710f }, + { -0.301439f, 0.086337f, -0.201358f }, { -0.459376f, 0.086335f, -0.190260f }, + { -0.413442f, 0.086335f, -0.276196f }, { -0.256370f, -0.084705f, -0.256276f }, + { -0.301434f, 0.086337f, -0.201365f }, { -0.301434f, -0.084705f, -0.201365f }, + { -0.256370f, 0.086337f, -0.256276f }, { -0.413438f, 0.086335f, -0.276203f }, + { -0.351621f, 0.086335f, -0.351527f }, { -0.413438f, -0.084704f, -0.276203f }, + { -0.351621f, -0.084704f, -0.351527f }, { -0.256364f, -0.084705f, -0.256283f }, + { -0.201453f, 0.086337f, -0.301347f }, { -0.256364f, 0.086337f, -0.256283f }, + { -0.201453f, -0.084705f, -0.301347f }, { -0.276292f, -0.084704f, -0.413350f }, + { -0.351615f, -0.084704f, -0.351534f }, { -0.351615f, 0.086335f, -0.351534f }, + { -0.276292f, 0.086335f, -0.413350f }, { -0.201447f, -0.084705f, -0.301352f }, + { -0.276285f, -0.084704f, -0.413355f }, { -0.138799f, -0.084705f, -0.334838f }, + { -0.190349f, -0.084704f, -0.459289f }, { -0.276285f, 0.086335f, -0.413355f }, + { -0.201447f, 0.086337f, -0.301352f }, { -0.190349f, 0.086335f, -0.459289f }, + { -0.138799f, 0.086337f, -0.334838f }, { -0.190341f, 0.086335f, -0.459293f }, + { -0.190341f, -0.084704f, -0.459293f }, { -0.138791f, -0.084705f, -0.334842f }, + { -0.138791f, 0.086337f, -0.334842f }, { -0.070815f, 0.086337f, -0.355462f }, + { -0.070815f, -0.084705f, -0.355462f }, { -0.097095f, 0.086335f, -0.487579f }, + { -0.097095f, -0.084704f, -0.487579f }, { -0.070807f, 0.086337f, -0.355464f }, + { -0.097086f, 0.086335f, -0.487581f }, { -0.097086f, -0.084704f, -0.487581f }, + { -0.070807f, -0.084705f, -0.355464f }, { -0.000113f, -0.084705f, -0.362427f }, + { -0.000113f, 0.086337f, -0.362427f }, { -0.000113f, 0.086335f, -0.497132f }, + { -0.000113f, -0.084704f, -0.497132f }, { 0.070588f, 0.086337f, -0.355465f }, + { 0.096868f, -0.084704f, -0.487582f }, { 0.096868f, 0.086335f, -0.487582f }, + { 0.070588f, -0.084705f, -0.355465f }, { -0.000105f, 0.086337f, -0.362427f }, + { -0.000105f, 0.086335f, -0.497133f }, { -0.000105f, -0.084704f, -0.497133f }, + { -0.000105f, -0.084705f, -0.362427f }, { 0.190123f, -0.084704f, -0.459294f }, + { 0.190123f, 0.086335f, -0.459294f }, { 0.138573f, -0.084705f, -0.334843f }, + { 0.138573f, 0.086337f, -0.334843f }, { 0.070597f, 0.086337f, -0.355463f }, + { 0.070597f, -0.084705f, -0.355463f }, { 0.096876f, -0.084704f, -0.487580f }, + { 0.096876f, 0.086335f, -0.487580f }, { 0.201229f, -0.084705f, -0.301354f }, + { 0.276067f, -0.084704f, -0.413358f }, { 0.201229f, 0.086337f, -0.301354f }, + { 0.138581f, -0.084705f, -0.334840f }, { 0.138581f, 0.086337f, -0.334840f }, + { 0.190131f, 0.086335f, -0.459292f }, { 0.276067f, 0.086335f, -0.413358f }, + { 0.190131f, -0.084704f, -0.459292f }, { 0.201236f, 0.086337f, -0.301350f }, + { 0.256147f, -0.084705f, -0.256286f }, { 0.256147f, 0.086337f, -0.256286f }, + { 0.201236f, -0.084705f, -0.301350f }, { 0.351398f, -0.084704f, -0.351537f }, + { 0.351398f, 0.086335f, -0.351537f }, { 0.276074f, -0.084704f, -0.413353f }, + { 0.276074f, 0.086335f, -0.413353f }, { 0.301218f, -0.084705f, -0.201369f }, + { 0.301218f, 0.086337f, -0.201369f }, { 0.256154f, -0.084705f, -0.256280f }, + { 0.256154f, 0.086337f, -0.256280f }, { 0.413221f, 0.086335f, -0.276207f }, + { 0.413221f, -0.084704f, -0.276207f }, { 0.351405f, 0.086335f, -0.351531f }, + { 0.351405f, -0.084704f, -0.351531f }, { 0.334709f, 0.086337f, -0.138715f }, + { 0.301223f, 0.086337f, -0.201362f }, { 0.334709f, -0.084705f, -0.138715f }, + { 0.301223f, -0.084705f, -0.201362f }, { 0.459160f, -0.084704f, -0.190264f }, + { 0.459160f, 0.086335f, -0.190264f }, { 0.413226f, 0.086335f, -0.276200f }, + { 0.413226f, -0.084704f, -0.276200f }, { 0.334713f, -0.084705f, -0.138707f }, + { 0.355333f, -0.084705f, -0.070731f }, { 0.334713f, 0.086337f, -0.138707f }, + { 0.355333f, 0.086337f, -0.070731f }, { 0.459164f, 0.086335f, -0.190257f }, + { 0.487450f, 0.086335f, -0.097010f }, { 0.487450f, -0.084704f, -0.097010f }, + { 0.459164f, -0.084704f, -0.190257f }, { 0.355335f, -0.084705f, -0.070722f }, + { 0.362298f, -0.084705f, -0.000029f }, { 0.355335f, 0.086337f, -0.070722f }, + { 0.362298f, 0.086337f, -0.000029f }, { 0.497003f, -0.084704f, -0.000029f }, + { 0.497003f, 0.086335f, -0.000029f }, { 0.487452f, 0.086335f, -0.097002f }, + { 0.487452f, -0.084704f, -0.097002f }, { 0.362298f, -0.084705f, -0.000021f }, + { 0.497003f, -0.084704f, -0.000021f }, { 0.355336f, -0.084705f, 0.070673f }, + { 0.487453f, -0.084704f, 0.096952f }, { 0.497003f, 0.086335f, -0.000021f }, + { 0.362298f, 0.086337f, -0.000021f }, { 0.355336f, 0.086337f, 0.070673f }, + { 0.487453f, 0.086335f, 0.096952f }, { 0.355334f, -0.084705f, 0.070681f }, + { 0.355334f, 0.086337f, 0.070681f }, { 0.487451f, 0.086335f, 0.096961f }, + { 0.487451f, -0.084704f, 0.096961f }, { 0.334714f, -0.084705f, 0.138658f }, + { 0.459165f, -0.084704f, 0.190207f }, { 0.334714f, 0.086337f, 0.138658f }, + { 0.459165f, 0.086335f, 0.190207f }, { 0.334711f, 0.086337f, 0.138666f }, + { 0.301225f, 0.086337f, 0.201313f }, { 0.459163f, 0.086335f, 0.190215f }, + { 0.413229f, 0.086335f, 0.276151f }, { 0.334711f, -0.084705f, 0.138666f }, + { 0.301225f, -0.084705f, 0.201313f }, { 0.413229f, -0.084704f, 0.276151f }, + { 0.459163f, -0.084704f, 0.190215f }, { 0.301221f, -0.084705f, 0.201321f }, + { 0.256157f, -0.084705f, 0.256232f }, { 0.301221f, 0.086337f, 0.201321f }, + { 0.256157f, 0.086337f, 0.256232f }, { 0.413224f, -0.084704f, 0.276159f }, + { 0.413224f, 0.086335f, 0.276159f }, { 0.351408f, 0.086335f, 0.351483f }, + { 0.351408f, -0.084704f, 0.351483f }, { 0.201240f, -0.084705f, 0.301302f }, + { 0.201240f, 0.086337f, 0.301302f }, { 0.256151f, -0.084705f, 0.256238f }, + { 0.256151f, 0.086337f, 0.256238f }, { 0.351402f, 0.086335f, 0.351489f }, + { 0.351402f, -0.084704f, 0.351489f }, { 0.276078f, 0.086335f, 0.413305f }, + { 0.276078f, -0.084704f, 0.413305f }, { 0.138586f, -0.084705f, 0.334793f }, + { 0.201233f, -0.084705f, 0.301307f }, { 0.190135f, -0.084704f, 0.459244f }, + { 0.276071f, -0.084704f, 0.413311f }, { 0.201233f, 0.086337f, 0.301307f }, + { 0.138586f, 0.086337f, 0.334793f }, { 0.276071f, 0.086335f, 0.413311f }, + { 0.190135f, 0.086335f, 0.459244f }, { 0.138578f, -0.084705f, 0.334797f }, + { 0.070602f, -0.084705f, 0.355417f }, { 0.138578f, 0.086337f, 0.334797f }, + { 0.070602f, 0.086337f, 0.355417f }, { 0.190127f, 0.086335f, 0.459248f }, + { 0.190127f, -0.084704f, 0.459248f }, { 0.096881f, 0.086335f, 0.487534f }, + { 0.096881f, -0.084704f, 0.487534f }, { 0.070593f, -0.084705f, 0.355419f }, + { 0.096873f, 0.086335f, 0.487536f }, { 0.096873f, -0.084704f, 0.487536f }, + { 0.070593f, 0.086337f, 0.355419f }, { -0.000100f, 0.086337f, 0.362382f }, + { -0.000100f, 0.086335f, 0.497087f }, { -0.000100f, -0.084704f, 0.497087f }, + { -0.000100f, -0.084705f, 0.362382f }, }; typedef struct WheelHullSpan @@ -1230,10 +1471,11 @@ typedef struct WheelHullSpan int count; } WheelHullSpan; static const WheelHullSpan s_metalWheel1Hulls[37] = { - { 0, 29 }, { 29, 8 }, { 37, 8 }, { 45, 8 }, { 53, 8 }, { 61, 8 }, { 69, 8 }, { 77, 8 }, { 85, 8 }, { 93, 8 }, - { 101, 8 }, { 109, 8 }, { 117, 8 }, { 125, 8 }, { 133, 8 }, { 141, 8 }, { 149, 8 }, { 157, 8 }, { 165, 8 }, { 173, 8 }, - { 181, 8 }, { 189, 8 }, { 197, 8 }, { 205, 8 }, { 213, 8 }, { 221, 8 }, { 229, 8 }, { 237, 8 }, { 245, 8 }, { 253, 8 }, - { 261, 8 }, { 269, 8 }, { 277, 8 }, { 285, 8 }, { 293, 8 }, { 301, 8 }, { 309, 8 }, + { 0, 29 }, { 29, 8 }, { 37, 8 }, { 45, 8 }, { 53, 8 }, { 61, 8 }, { 69, 8 }, { 77, 8 }, + { 85, 8 }, { 93, 8 }, { 101, 8 }, { 109, 8 }, { 117, 8 }, { 125, 8 }, { 133, 8 }, { 141, 8 }, + { 149, 8 }, { 157, 8 }, { 165, 8 }, { 173, 8 }, { 181, 8 }, { 189, 8 }, { 197, 8 }, { 205, 8 }, + { 213, 8 }, { 221, 8 }, { 229, 8 }, { 237, 8 }, { 245, 8 }, { 253, 8 }, { 261, 8 }, { 269, 8 }, + { 277, 8 }, { 285, 8 }, { 293, 8 }, { 301, 8 }, { 309, 8 }, }; #define s_metalWheel1HullCount 37 @@ -1315,14 +1557,14 @@ public: b3Profile p = b3World_GetProfile( m_worldId ); float substepRate = m_context->hertz * m_context->subStepCount; - DrawTextLine( "wheels %d, hull pieces/wheel %d (%d total shapes)", m_wheelCount, s_metalWheel1HullCount, - m_wheelCount * s_metalWheel1HullCount ); - DrawTextLine( "step %.0f hz, sub-steps %d -> substep rate %.0f hz", m_context->hertz, m_context->subStepCount, - substepRate ); + DrawTextLine( "wheels %d, hull pieces/wheel %d (%d total shapes)", m_wheelCount, + s_metalWheel1HullCount, m_wheelCount * s_metalWheel1HullCount ); + DrawTextLine( "step %.0f hz, sub-steps %d -> substep rate %.0f hz", m_context->hertz, + m_context->subStepCount, substepRate ); DrawTextLine( "eff contact hz = min(240, %.0f) = %.0f", 0.125f * substepRate, b3MinFloat( 240.0f, 0.125f * substepRate ) ); - DrawTextLine( "step %.3f ms | collide %.3f ms (%.0f%%) | solve %.3f ms (%.0f%%)", p.step, p.collide, - p.step > 0.0f ? 100.0f * p.collide / p.step : 0.0f, p.solve, + DrawTextLine( "step %.3f ms | collide %.3f ms (%.0f%%) | solve %.3f ms (%.0f%%)", p.step, + p.collide, p.step > 0.0f ? 100.0f * p.collide / p.step : 0.0f, p.solve, p.step > 0.0f ? 100.0f * p.solve / p.step : 0.0f ); } @@ -1337,7 +1579,8 @@ public: static int sampleWheelStack = RegisterSample( "Issues", "GMod Wheel Stack", WheelStack::Create ); -// There was a problem where this bouncing box could gain energy. This has been fixed by the deferred restitution update. +// There was a problem where this bouncing box could gain energy. This has been fixed by the +// deferred restitution update. class RestitutionOvershoot : public Sample { public: @@ -1417,9 +1660,9 @@ public: b3Pos markerPoint = { 0.0f, m_dropHeight + m_boxHalf, 0.0f }; DrawPlane( b3Vec3_axisY, markerPoint, MakeColor( b3_colorYellow ) ); - // Perfect restitution and no friction, so the total energy may only decrease. It is the honest - // invariant here: the height check cannot tell a bounce that gained potential energy from one - // that converted it into spin, and this drop does both. + // Perfect restitution and no friction, so the total energy may only decrease. It is the + // honest invariant here: the height check cannot tell a bounce that gained potential energy + // from one that converted it into spin, and this drop does both. MechanicalEnergy energy = MeasureEnergy( m_worldId, &m_boxBody, 1 ); float total = energy.Total(); @@ -1436,10 +1679,11 @@ public: DrawTextLine( "drop height = %.2f m", m_dropHeight ); DrawTextLine( "current y = %.2f m", m_currentY ); DrawTextLine( "max bounce = %.2f m", m_maxBounceY ); - DrawTextLine( "kinetic = %.0f J linear + %.0f J angular", energy.linear, energy.angular ); + DrawTextLine( "kinetic = %.0f J linear + %.0f J angular", energy.linear, + energy.angular ); DrawTextLine( "potential = %.0f J", energy.potential ); - DrawTextLine( "total = %.0f J (%.2f%% of start, peak %.2f%%)", total, 100.0f * total / m_startEnergy, - 100.0f * m_maxEnergy / m_startEnergy ); + DrawTextLine( "total = %.0f J (%.2f%% of start, peak %.2f%%)", total, + 100.0f * total / m_startEnergy, 100.0f * m_maxEnergy / m_startEnergy ); if ( m_bounced == false ) { @@ -1481,7 +1725,8 @@ public: bool m_energyFailed = false; }; -static int sampleRestitutionOvershoot = RegisterSample( "Issues", "Restitution Overshoot", RestitutionOvershoot::Create ); +static int sampleRestitutionOvershoot = + RegisterSample( "Issues", "Restitution Overshoot", RestitutionOvershoot::Create ); class SlideTwistOffCenterShape : public Sample { @@ -1573,9 +1818,7 @@ public: static int sampleHugeBox = RegisterSample( "Issues", "Huge Box", HugeBox::Create ); -// This demonstrates this cap on mesh complexity. B3_MAX_MESH_CONTACT_TRIANGLES -// It generates this warning: -// Box3D: WARNING: complex mesh detected, triangle buffer capacity of 256 reached +// A tilted cylinder whose bounds cover more than 512 height field triangles class HeightfieldIssue : public Sample { public: @@ -1707,8 +1950,10 @@ public: float interiorRestX = -m_wallHalfThickness - m_moverHalfExtent; float exteriorRestX = m_wallHalfThickness + m_moverHalfExtent; - b3Vec3 lower = { interiorRestX - m_moverHalfExtent, m_wallHalfHeight - m_moverHalfExtent, -m_moverHalfExtent }; - b3Vec3 upper = { interiorRestX + m_moverHalfExtent, m_wallHalfHeight + m_moverHalfExtent, m_moverHalfExtent }; + b3Vec3 lower = { interiorRestX - m_moverHalfExtent, m_wallHalfHeight - m_moverHalfExtent, + -m_moverHalfExtent }; + b3Vec3 upper = { interiorRestX + m_moverHalfExtent, m_wallHalfHeight + m_moverHalfExtent, + m_moverHalfExtent }; DrawAabb( lower, upper, MakeColor( b3_colorLime ) ); float x = (float)b3Body_GetPosition( m_moverId ).x; @@ -1739,4 +1984,600 @@ public: float m_startX = 0.2f; }; -static int sampleThinWallPushOut = RegisterSample( "Issues", "Thin Wall Push Out", ThinWallPushOut::Create ); +static int sampleThinWallPushOut = + RegisterSample( "Issues", "Thin Wall Push Out", ThinWallPushOut::Create ); + +// Two crates resting on a rolling deck, one each side of the roll axis: the orange crate with +// contact recycling (the default), the blue crate with it off for its body. +// +// A recycled contact keeps the normal and anchors it was made with, fixed in the world, while each +// body turns less than B3_CONTACT_RECYCLE_ANGULAR_DISTANCE (10 degrees) from where the contact was +// made and the two stay within the recycle distance (5 cm). Only the separation is updated. A deck +// and the cargo resting on it turn together, so their contact is recycled through the roll. Its +// normal stays the deck's up as it was, and its anchors stay at the same world offsets from each +// body's center of mass, so the orange crate moves as the deck moves at that fixed spot in space, +// not as the deck under it moves. It goes straight up and down instead of along its arc, and slides +// out across the deck by about offset * (1 - cos(roll)). While that stays under the recycle +// distance the crate sways out and back. Past it (8 m and 8 degrees here, 78 mm) the contact is +// made again partway through, at the deck's angle then, and frozen there, so the crate walks off +// across the deck roll by roll. The angular tolerance doesn't stop that: at 12 or 15 degrees the +// contact is made again more often and the crate walks all the same. The blue crate rides with the +// deck. The slide arrows on the deck are drawn ten times longer. +class RollingDeck : public Sample +{ +public: + static constexpr float m_deckHalfLength = 2.0f; + static constexpr float m_deckHalfWidth = 10.0f; + static constexpr float m_deckHalfThickness = 0.25f; + static constexpr float m_crateHalf = 0.5f; + static constexpr float m_crateOffset = 8.0f; + static constexpr float m_friction = 1.0f; + static constexpr float m_settleTime = 1.0f; + static constexpr float m_slideScale = 10.0f; + + explicit RollingDeck( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 75.0f, 15.0f, 32.0f, { 0.0f, 0.5f, 0.0f } ); + } + + m_rollDegrees = 8.0f; + m_period = 6.0f; + m_deckId = {}; + m_crateIds[0] = {}; + m_crateIds[1] = {}; + + CreateScene(); + } + + void CreateScene() + { + if ( B3_IS_NON_NULL( m_deckId ) ) + { + b3DestroyBody( m_deckId ); + b3DestroyBody( m_crateIds[0] ); + b3DestroyBody( m_crateIds[1] ); + } + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = m_friction; + + // The deck rolls about the world x axis through its center + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_kinematicBody; + m_deckId = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( m_deckHalfLength, m_deckHalfThickness, m_deckHalfWidth ); + shapeDef.baseMaterial.customColor = b3_colorSlateGray; + b3CreateHullShape( m_deckId, &shapeDef, &box.base ); + } + + // The crates rest on the level deck, m_crateOffset either side of the roll axis + b3BoxHull box = b3MakeBoxHull( m_crateHalf, m_crateHalf, m_crateHalf ); + for ( int i = 0; i < 2; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, m_deckHalfThickness + m_crateHalf, + i == 0 ? m_crateOffset : -m_crateOffset }; + + // The orange crate recycles its contact with the deck, the blue crate doesn't. Both + // stay awake, so a small roll can't put them to sleep. + bodyDef.enableContactRecycling = i == 0; + bodyDef.enableSleep = false; + m_crateIds[i] = b3CreateBody( m_worldId, &bodyDef ); + + shapeDef.baseMaterial.customColor = i == 0 ? b3_colorOrange : b3_colorDodgerBlue; + b3CreateHullShape( m_crateIds[i], &shapeDef, &box.base ); + + m_start[i] = b3Vec3_zero; + m_maxSlide[i] = 0.0f; + m_maxNormalError[i] = 0.0f; + } + + m_time = 0.0f; + m_started = false; + } + + // The roll angle in radians at a time since the start: still while the crates settle, then + // rolling, eased in over the first quarter roll so the crates aren't jolted by a deck starting + // at full speed. + float RollAngle( float time ) const + { + float t = time - m_settleTime; + if ( t <= 0.0f ) + { + return 0.0f; + } + + float ease = 1.0f; + if ( t < 0.25f * m_period ) + { + float u = t / ( 0.25f * m_period ); + ease = u * u * ( 3.0f - 2.0f * u ); + } + + return ease * m_rollDegrees * B3_PI / 180.0f * sinf( 2.0f * B3_PI * t / m_period ); + } + + bool DrawControls() override + { + // Changing the roll starts the scene again, so the contacts are made with the deck level. + ImGui::SliderFloat( "Roll (deg)", &m_rollDegrees, 0.0f, 15.0f, "%.1f" ); + if ( ImGui::IsItemDeactivatedAfterEdit() ) + { + CreateScene(); + } + + ImGui::SliderFloat( "Period (s)", &m_period, 2.0f, 12.0f, "%.1f" ); + if ( ImGui::IsItemDeactivatedAfterEdit() ) + { + CreateScene(); + } + + if ( ImGui::Button( "Reset" ) ) + { + CreateScene(); + } + + return true; + } + + void Step() override + { + float timeStep = m_context->hertz > 0.0f ? 1.0f / m_context->hertz : 0.0f; + if ( m_context->pause && m_context->singleStep == 0 ) + { + timeStep = 0.0f; + } + + if ( timeStep > 0.0f ) + { + b3Quat rotation = + b3MakeQuatFromAxisAngle( b3Vec3_axisX, RollAngle( m_time + timeStep ) ); + b3Body_SetTargetTransform( m_deckId, { { 0.0f, 0.0f, 0.0f }, rotation }, timeStep, + true ); + } + + Sample::Step(); + + m_time += timeStep; + + // Where the crates settled, in the deck's frame, measured from as the deck starts to roll + if ( m_started == false && m_time >= m_settleTime ) + { + for ( int i = 0; i < 2; ++i ) + { + m_start[i] = b3Body_GetLocalPoint( m_deckId, b3Body_GetPosition( m_crateIds[i] ) ); + } + m_started = true; + } + + b3Vec3 deckUp = b3Body_GetWorldVector( m_deckId, b3Vec3_axisY ); + float slide[2] = { 0.0f, 0.0f }; + float normalError[2] = { 0.0f, 0.0f }; + + for ( int i = 0; i < 2; ++i ) + { + Vec4 color = i == 0 ? MakeColor( b3_colorOrange ) : MakeColor( b3_colorDodgerBlue ); + b3Pos center = b3Body_GetPosition( m_crateIds[i] ); + + // The deck's up (white) and the crate's contact normal (in the crate's color), from the + // crate's center + DrawArrow( center, b3OffsetPos( center, b3MulSV( 2.5f, deckUp ) ), + MakeColor( b3_colorWhite ) ); + + b3ContactData contactData; + if ( b3Body_GetContactData( m_crateIds[i], &contactData, 1 ) == 1 && + contactData.manifoldCount > 0 ) + { + b3Vec3 normal = contactData.manifolds[0].normal; + if ( b3Dot( normal, deckUp ) < 0.0f ) + { + normal = b3Neg( normal ); + } + + DrawArrow( center, b3OffsetPos( center, b3MulSV( 2.5f, normal ) ), color ); + normalError[i] = + acosf( b3MinFloat( b3Dot( normal, deckUp ), 1.0f ) ) * 180.0f / B3_PI; + } + + DrawString3D( b3OffsetPos( center, b3MulSV( 3.0f, deckUp ) ), color, "%s", + i == 0 ? "recycling on" : "recycling off" ); + + if ( m_started == false ) + { + continue; + } + + // Where the crate settled, outlined on the deck's top, and how far it has slid from + // there across the deck, drawn m_slideScale times longer + b3Vec3 local = b3Body_GetLocalPoint( m_deckId, center ); + b3Vec3 offset = b3Sub( local, m_start[i] ); + offset.y = 0.0f; + slide[i] = b3Length( offset ); + + float top = m_deckHalfThickness + 0.01f; + b3Vec3 mark = { m_start[i].x, top, m_start[i].z }; + b3Vec3 corners[4] = { + { mark.x - m_crateHalf, top, mark.z - m_crateHalf }, + { mark.x + m_crateHalf, top, mark.z - m_crateHalf }, + { mark.x + m_crateHalf, top, mark.z + m_crateHalf }, + { mark.x - m_crateHalf, top, mark.z + m_crateHalf }, + }; + for ( int j = 0; j < 4; ++j ) + { + DrawLine( b3Body_GetWorldPoint( m_deckId, corners[j] ), + b3Body_GetWorldPoint( m_deckId, corners[( j + 1 ) % 4] ), color ); + } + + b3Vec3 tip = b3MulAdd( mark, m_slideScale, offset ); + DrawArrow( b3Body_GetWorldPoint( m_deckId, mark ), + b3Body_GetWorldPoint( m_deckId, tip ), color ); + + m_maxSlide[i] = b3MaxFloat( m_maxSlide[i], slide[i] ); + m_maxNormalError[i] = b3MaxFloat( m_maxNormalError[i], normalError[i] ); + } + + float roll = RollAngle( m_time ) * 180.0f / B3_PI; + float expected = m_crateOffset * ( 1.0f - cosf( m_rollDegrees * B3_PI / 180.0f ) ); + + DrawTextLine( "deck rolls %.1f deg each way every %.1f s (now %+.1f deg), crates %.0f m " + "from the roll axis, friction %.0f", + m_rollDegrees, m_period, roll, m_crateOffset, m_friction ); + DrawTextLine( "a contact is recycled while each body turns < 10 deg from where it was made " + "(B3_CONTACT_RECYCLE_ANGULAR_DISTANCE)" ); + DrawTextLine( "contacts recycled this step: %d (only the orange crate's can be)", + b3World_GetCounters( m_worldId ).recycledContactCount ); + DrawTextLine( "orange, recycling on : normal %4.1f deg off the deck's up (most %4.1f), " + "slid %5.1f mm (most %5.1f)", + normalError[0], m_maxNormalError[0], 1000.0f * slide[0], + 1000.0f * m_maxSlide[0] ); + DrawTextLine( "blue, recycling off: normal %4.1f deg off the deck's up (most %4.1f), " + "slid %5.1f mm (most %5.1f)", + normalError[1], m_maxNormalError[1], 1000.0f * slide[1], + 1000.0f * m_maxSlide[1] ); + DrawTextLine( + "recycled, the crate moves straight up and down: offset * (1 - cos(roll)) = %.1f mm at " + "each end; past the " + "%.0f cm recycle distance the contact is made again tilted and the crate walks off", + 1000.0f * expected, 100.0f * b3World_GetContactRecycleDistance( m_worldId ) ); + DrawTextLine( "arrows: white is the deck's up, colored is the contact normal; slide arrows " + "on the deck are drawn %.0fx", + m_slideScale ); + } + + static Sample* Create( SampleContext* context ) + { + return new RollingDeck( context ); + } + + b3BodyId m_deckId; + b3BodyId m_crateIds[2]; + b3Vec3 m_start[2]; + float m_maxSlide[2]; + float m_maxNormalError[2]; + float m_rollDegrees; + float m_period; + float m_time; + bool m_started; +}; + +static int sampleRollingDeck = RegisterSample( "Issues", "Rolling Deck", RollingDeck::Create ); + +// A car made of one dynamic box (the chassis) and four ray cast 'wheels'. +// There are no wheel bodies and no joints. + +// W/S to drive, A/D to steer and Space to brake. T toggles a camera that follows the car. + +// Each step every wheel casts a ray down the chassis' own down axis. Where a ray hits the ground +// the wheel applies two forces to the chassis at the contact point: +// - a spring and damper force along the contact normal, which holds the chassis up +// - a tyre force in the contact plane, which drives, brakes and stops the wheel sliding sideways +// The tyre force is limited by friction times the spring force, so a wheel that is lightly loaded +// grips less. Weight transfer, body roll and rollover come from where these forces are applied. +class RaycastVehicle : public Sample +{ +public: + static constexpr int m_wheelCount = 4; + static constexpr uint64_t m_vehicleBit = 0x2; + + struct Wheel + { + b3Vec3 localMount; + bool steered; + + // Distance from the mount down to the wheel's center + float suspensionLength; + bool grounded; + + // Rotation about the axle, for drawing only + float spinAngle; + float spinSpeed; + }; + + explicit RaycastVehicle( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 25.0f, 20.0f, 12.0f, { 0.0f, 1.0f, 0.0f } ); + } + + m_suspensionHertz = 1.5f; + m_suspensionDampingRatio = 0.45f; + m_friction = 1.0f; + m_wheelRadius = 0.35f; + m_suspensionTravel = 0.3f; + m_engineAcceleration = 5.0f; + m_brakeDeceleration = 8.0f; + m_maxSteeringAngle = 0.15f * B3_PI; + m_steeringAngle = 0.0f; + + AddGroundBox( 100.0f ); + + // A ramp to show the suspension working + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 15.0f, -0.5f, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.1f ); + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeBoxHull( 6.0f, 0.5f, 4.0f ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + + // The chassis is the only body. Its shape has its own category so the wheel rays can skip + // it. + float halfLength = 2.2f; + float halfHeight = 0.7f; + float halfWidth = 0.9f; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 1.2f, 0.0f }; + m_chassisId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 130.0f; + shapeDef.filter.categoryBits = m_vehicleBit; + b3BoxHull box = b3MakeBoxHull( halfLength, halfHeight, halfWidth ); + b3CreateHullShape( m_chassisId, &shapeDef, &box.base ); + + // A car tips over when its lateral acceleration exceeds half the track width divided by + // the height of its center of mass. At the middle of this box the center of mass would + // be about as high as the half track, and the car would roll before its tyres slide. + // Lower it to near the floor, where the heavy parts of a car are. + b3MassData massData = b3Body_GetMassData( m_chassisId ); + massData.center = { 0.0f, -0.45f, 0.0f }; + b3Body_SetMassData( m_chassisId, massData ); + } + + // The mounts are the top of each wheel's travel, inside the chassis at the sides. +x is + // forward. + float axle = 0.75f * halfLength; + float mountHeight = -0.3f; + m_wheels[0] = { { axle, mountHeight, halfWidth }, true, m_suspensionTravel }; + m_wheels[1] = { { axle, mountHeight, -halfWidth }, true, m_suspensionTravel }; + m_wheels[2] = { { -axle, mountHeight, halfWidth }, false, m_suspensionTravel }; + m_wheels[3] = { { -axle, mountHeight, -halfWidth }, false, m_suspensionTravel }; + + // A cylinder along its y axis, centered, for drawing the wheels + float wheelWidth = 0.25f; + m_wheelHull = b3CreateCylinder( wheelWidth, m_wheelRadius, -0.5f * wheelWidth, 12 ); + } + + ~RaycastVehicle() override + { + b3DestroyHull( m_wheelHull ); + + if ( m_camera->m_thirdPerson ) + { + ToggleThirdPerson(); + } + } + + void Keyboard( int key, int action, int mods ) override + { + if ( key == KEY_T && action == ACTION_PRESS ) + { + ToggleThirdPerson(); + } + } + + bool DrawControls() override + { + ImGui::SliderFloat( "Suspension Hertz", &m_suspensionHertz, 0.5f, 5.0f, "%.1f" ); + ImGui::SliderFloat( "Suspension Damping", &m_suspensionDampingRatio, 0.0f, 2.0f, "%.2f" ); + ImGui::SliderFloat( "Tyre Friction", &m_friction, 0.0f, 2.0f, "%.2f" ); + + bool thirdPerson = m_camera->m_thirdPerson; + if ( ImGui::Checkbox( "Third Person (T)", &thirdPerson ) ) + { + ToggleThirdPerson(); + } + + return true; + } + + // Apply the forces of every wheel to the chassis for the coming step + void UpdateWheels( float timeStep, float throttle, float brake, float steer ) + { + b3WorldTransform transform = b3Body_GetTransform( m_chassisId ); + b3Vec3 forward = b3RotateVector( transform.q, b3Vec3_axisX ); + + // The acceleration the driver asks for along the chassis' nose. The brake opposes the + // chassis' motion and is capped at what stops it this step, so it never drives it + // backwards. + float speed = b3Dot( b3Body_GetLinearVelocity( m_chassisId ), forward ); + float acceleration = m_engineAcceleration * throttle; + float stopping = b3MinFloat( m_brakeDeceleration * brake, b3AbsFloat( speed ) / timeStep ); + acceleration -= speed > 0.0f ? stopping : -stopping; + + for ( int i = 0; i < m_wheelCount; ++i ) + { + UpdateWheel( m_wheels[i], transform, timeStep, acceleration, steer ); + } + } + + // Cast one wheel's ray and apply its suspension and tyre forces to the chassis + void UpdateWheel( Wheel& wheel, b3WorldTransform transform, float timeStep, float acceleration, + float steer ) + { + b3Vec3 up = b3RotateVector( transform.q, b3Vec3_axisY ); + b3Vec3 forward = b3RotateVector( transform.q, b3Vec3_axisX ); + + // The ray reaches from the mount down through the spring's travel and the wheel's radius + float reach = m_suspensionTravel + m_wheelRadius; + b3Vec3 translation = b3MulSV( -reach, up ); + + b3QueryFilter filter = b3DefaultQueryFilter(); + filter.categoryBits = m_vehicleBit; + filter.maskBits = ~m_vehicleBit; + + b3Pos mount = b3TransformWorldPoint( transform, wheel.localMount ); + b3RayResult hit = b3World_CastRayClosest( m_worldId, mount, translation, filter ); + wheel.grounded = hit.hit; + float fraction = hit.hit ? hit.fraction : 1.0f; + wheel.suspensionLength = fraction * reach - m_wheelRadius; + + // A wheel turns at the speed of the ground under it, and keeps that speed in the air + wheel.spinAngle = b3UnwindAngle( wheel.spinAngle + wheel.spinSpeed * timeStep ); + + if ( hit.hit == false ) + { + return; + } + + // Each wheel carries an equal share of the mass. The spring rate gives that share the + // requested natural frequency. + float share = b3Body_GetMass( m_chassisId ) / m_wheelCount; + float omega = 2.0f * B3_PI * m_suspensionHertz; + float stiffness = share * omega * omega; + float damping = 2.0f * m_suspensionDampingRatio * share * omega; + + // Suspension. A spring only pushes: a negative load would pull the chassis onto the + // ground and hold it there through a jump. + float compression = ( 1.0f - hit.fraction ) * reach; + float compressionSpeed = -b3Dot( b3Body_GetWorldPointVelocity( m_chassisId, mount ), up ); + float load = b3MaxFloat( stiffness * compression + damping * compressionSpeed, 0.0f ); + b3Body_ApplyForce( m_chassisId, b3MulSV( load, hit.normal ), hit.point, true ); + + // The wheel's rolling direction in the contact plane, and the direction across it + b3Vec3 heading = forward; + if ( wheel.steered ) + { + heading = b3RotateVector( b3MakeQuatFromAxisAngle( up, steer ), forward ); + } + b3Vec3 rolling = + b3Normalize( b3MulAdd( heading, -b3Dot( heading, hit.normal ), hit.normal ) ); + b3Vec3 lateral = b3Cross( hit.normal, rolling ); + + // The lateral force cancels the contact point's sideways sliding in one step. The rolling + // force gives this wheel's share of the mass the requested acceleration, resolved onto the + // wheel, so a steered wheel puts less of it into the nose direction. + b3Vec3 contactVelocity = b3Body_GetWorldPointVelocity( m_chassisId, hit.point ); + float lateralForce = -share * b3Dot( contactVelocity, lateral ) / timeStep; + float rollingForce = share * acceleration * b3Dot( forward, rolling ); + + // Rolling forward along +x turns the wheel backwards about its axle, the chassis' +z + wheel.spinSpeed = -b3Dot( contactVelocity, rolling ) / m_wheelRadius; + + // Friction circle: the tyre cannot give more than friction times the load on it + float maxForce = m_friction * load; + float magnitude = sqrtf( lateralForce * lateralForce + rollingForce * rollingForce ); + if ( magnitude > maxForce ) + { + lateralForce *= maxForce / magnitude; + rollingForce *= maxForce / magnitude; + } + + b3Vec3 tyreForce = + b3Add( b3MulSV( rollingForce, rolling ), b3MulSV( lateralForce, lateral ) ); + b3Body_ApplyForce( m_chassisId, tyreForce, hit.point, true ); + } + + void Step() override + { + float throttle = 0.0f; + throttle += IsKeyDown( KEY_W ) ? 1.0f : 0.0f; + throttle -= IsKeyDown( KEY_S ) ? 1.0f : 0.0f; + float brake = IsKeyDown( KEY_SPACE ) ? 1.0f : 0.0f; + + m_steeringAngle = 0.0f; + m_steeringAngle += IsKeyDown( KEY_A ) ? m_maxSteeringAngle : 0.0f; + m_steeringAngle -= IsKeyDown( KEY_D ) ? m_maxSteeringAngle : 0.0f; + + // Forces accumulate until the world steps, so apply them only on a step that runs + bool stepping = m_context->pause == false || m_context->singleStep > 0; + if ( stepping && m_context->hertz > 0.0f ) + { + UpdateWheels( 1.0f / m_context->hertz, throttle, brake, m_steeringAngle ); + } + + if ( m_camera->m_thirdPerson ) + { + m_camera->m_pivot = b3Body_GetPosition( m_chassisId ); + m_camera->UpdateTransform(); + } + + Sample::Step(); + } + + void Render() override + { + Sample::Render(); + + b3WorldTransform transform = b3Body_GetTransform( m_chassisId ); + b3Vec3 down = b3RotateVector( transform.q, b3Neg( b3Vec3_axisY ) ); + + // The wheel cylinder is along y. This turns it onto the axle, the chassis' z. + b3Quat cylinderToAxle = b3MakeQuatFromAxisAngle( b3Vec3_axisX, 0.5f * B3_PI ); + + for ( int i = 0; i < m_wheelCount; ++i ) + { + const Wheel& wheel = m_wheels[i]; + b3Pos mount = b3TransformWorldPoint( transform, wheel.localMount ); + b3Pos center = b3OffsetPos( mount, b3MulSV( wheel.suspensionLength, down ) ); + + float steer = wheel.steered ? m_steeringAngle : 0.0f; + b3Quat q = b3MulQuat( transform.q, b3MakeQuatFromAxisAngle( b3Vec3_axisY, steer ) ); + q = b3MulQuat( q, b3MakeQuatFromAxisAngle( b3Vec3_axisZ, wheel.spinAngle ) ); + q = b3MulQuat( q, cylinderToAxle ); + + b3HexColor color = wheel.grounded ? b3_colorGreen : b3_colorRed; + DrawLine( mount, center, MakeColor( color ) ); + DrawHull( { center, q }, m_wheelHull, MakeColor( b3_colorGray ) ); + } + + b3Vec3 forward = b3RotateVector( b3Body_GetRotation( m_chassisId ), b3Vec3_axisX ); + float speed = b3Dot( b3Body_GetLinearVelocity( m_chassisId ), forward ); + DrawTextLine( "speed = %.1f m/s", speed ); + } + + static Sample* Create( SampleContext* context ) + { + return new RaycastVehicle( context ); + } + + b3BodyId m_chassisId; + Wheel m_wheels[m_wheelCount]; + b3HullData* m_wheelHull; + + float m_suspensionHertz; + float m_suspensionDampingRatio; + float m_friction; + float m_wheelRadius; + float m_suspensionTravel; + float m_engineAcceleration; + float m_brakeDeceleration; + float m_maxSteeringAngle; + float m_steeringAngle; +}; + +static int sampleRaycastVehicle = + RegisterSample( "Vehicle", "Raycast Vehicle", RaycastVehicle::Create ); diff --git a/samples/sample_joint.cpp b/samples/sample_joint.cpp index 1139019..ecf8ede 100644 --- a/samples/sample_joint.cpp +++ b/samples/sample_joint.cpp @@ -2320,6 +2320,8 @@ public: m_upperSteeringDegrees = 45.0f; m_maxSteeringTorque = 5.0f; m_targetSteeringDegrees = 0.0f; + m_wheelType = WheelType::sphere; + m_cylinderSides = 16; b3BodyId groundId; { @@ -2371,15 +2373,10 @@ public: b3CreateParallelJoint( m_worldId, ¶llelJointDef ); } - shapeDef.density = 2.0f; - shapeDef.baseMaterial.friction = 3.0f; - bodyDef.type = b3_dynamicBody; bodyDef.allowFastRotation = true; bodyDef.rotation = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisY, b3Vec3_axisZ ); - // b3HullData* hull = b3CreateCylinder( 0.25f, 0.4f, 0.0f, 16 ); - b3WheelJointDef jointDef = b3DefaultWheelJointDef(); jointDef.base.bodyIdA = m_chassisId; jointDef.base.localFrameA.q = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisX, b3Vec3_axisY ); @@ -2401,15 +2398,11 @@ public: jointDef.lowerSteeringLimit = B3_PI / 180.0f * m_lowerSteeringDegrees; jointDef.upperSteeringLimit = B3_PI / 180.0f * m_upperSteeringDegrees; - b3Sphere sphere = { b3Vec3_zero, 0.4f }; - { bodyDef.position = { 1.5f, 2.0f, 0.8f }; - b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); - b3CreateSphereShape( bodyId, &shapeDef, &sphere ); - // b3CreateHullShape( bodyId, &shapeDef, hull ); + m_wheelIds[0] = b3CreateBody( m_worldId, &bodyDef ); - jointDef.base.bodyIdB = bodyId; + jointDef.base.bodyIdB = m_wheelIds[0]; jointDef.base.localFrameA.p = { 1.5f, -0.5f, 0.8f }; jointDef.enableSteering = true; jointDef.enableSpinMotor = false; @@ -2418,11 +2411,9 @@ public: { bodyDef.position = { 1.5f, 2.0f, -0.8f }; - b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); - b3CreateSphereShape( bodyId, &shapeDef, &sphere ); - // b3CreateHullShape( bodyId, &shapeDef, hull ); + m_wheelIds[1] = b3CreateBody( m_worldId, &bodyDef ); - jointDef.base.bodyIdB = bodyId; + jointDef.base.bodyIdB = m_wheelIds[1]; jointDef.base.localFrameA.p = { 1.5f, -0.5f, -0.8f }; jointDef.enableSteering = true; jointDef.enableSpinMotor = false; @@ -2431,11 +2422,9 @@ public: { bodyDef.position = { -1.5f, 2.0f, 0.8f }; - b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); - b3CreateSphereShape( bodyId, &shapeDef, &sphere ); - // b3CreateHullShape( bodyId, &shapeDef, hull ); + m_wheelIds[2] = b3CreateBody( m_worldId, &bodyDef ); - jointDef.base.bodyIdB = bodyId; + jointDef.base.bodyIdB = m_wheelIds[2]; jointDef.base.localFrameA.p = { -1.5f, -0.5f, 0.8f }; jointDef.enableSteering = false; jointDef.enableSpinMotor = true; @@ -2444,18 +2433,21 @@ public: { bodyDef.position = { -1.5f, 2.0f, -0.8f }; - b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); - b3CreateSphereShape( bodyId, &shapeDef, &sphere ); - // b3CreateHullShape( bodyId, &shapeDef, hull ); + m_wheelIds[3] = b3CreateBody( m_worldId, &bodyDef ); - jointDef.base.bodyIdB = bodyId; + jointDef.base.bodyIdB = m_wheelIds[3]; jointDef.base.localFrameA.p = { -1.5f, -0.5f, -0.8f }; jointDef.enableSteering = false; jointDef.enableSpinMotor = true; m_rearRightId = b3CreateWheelJoint( m_worldId, &jointDef ); } - // b3DestroyHull( hull ); + for ( int i = 0; i < 4; ++i ) + { + m_wheelShapeIds[i] = b3_nullShapeId; + } + + CreateWheelShapes(); m_camera->m_thirdPerson = false; @@ -2474,6 +2466,59 @@ public: b3DestroyHeightField( m_heightField ); } + void CreateWheelShapes() + { + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 2.0f; + shapeDef.baseMaterial.friction = 3.0f; + + float radius = 0.4f; + float width = 0.25f; + + b3HullData* cylinder = nullptr; + if ( m_wheelType == WheelType::cylinder ) + { + float w = 1.5f * width; + cylinder = b3CreateCylinder( w, radius, -0.5f * w, m_cylinderSides ); + } + + for ( int i = 0; i < 4; ++i ) + { + if ( B3_IS_NON_NULL( m_wheelShapeIds[i] ) ) + { + b3DestroyShape( m_wheelShapeIds[i], false ); + } + + b3BodyId bodyId = m_wheelIds[i]; + + switch ( m_wheelType ) + { + case WheelType::sphere: + { + b3Sphere sphere = { b3Vec3_zero, radius }; + m_wheelShapeIds[i] = b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + } + break; + + case WheelType::capsule: + { + b3Capsule capsule = { { 0.0f, -0.5f * width, 0.0f }, { 0.0f, 0.5f * width, 0.0f }, radius }; + m_wheelShapeIds[i] = b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + } + break; + + case WheelType::cylinder: + m_wheelShapeIds[i] = b3CreateHullShape( bodyId, &shapeDef, cylinder ); + break; + } + } + + if ( cylinder != nullptr ) + { + b3DestroyHull( cylinder ); + } + } + void Keyboard( int key, int action, int mods ) override { if ( key == KEY_T && action == ACTION_PRESS ) @@ -2484,6 +2529,32 @@ public: bool DrawControls() override { + ImGui::Text( "Wheels" ); + if ( ImGui::RadioButton( "Sphere", m_wheelType == WheelType::sphere ) ) + { + m_wheelType = WheelType::sphere; + CreateWheelShapes(); + } + + if ( ImGui::RadioButton( "Capsule", m_wheelType == WheelType::capsule ) ) + { + m_wheelType = WheelType::capsule; + CreateWheelShapes(); + } + + if ( ImGui::RadioButton( "Cylinder", m_wheelType == WheelType::cylinder ) ) + { + m_wheelType = WheelType::cylinder; + CreateWheelShapes(); + } + + if ( ImGui::SliderInt( "Sides", &m_cylinderSides, 3, 32 ) && m_wheelType == WheelType::cylinder ) + { + CreateWheelShapes(); + } + + ImGui::Separator(); + ImGui::Text( "Suspension" ); if ( ImGui::SliderFloat( "Min##Suspension", &m_lowerTranslation, -10.0f, 10.0f, "%.1f" ) ) { @@ -2659,8 +2730,19 @@ public: return new Driving( context ); } + enum class WheelType + { + sphere, + capsule, + cylinder, + }; + b3HeightFieldData* m_heightField; b3BodyId m_chassisId; + b3BodyId m_wheelIds[4]; + b3ShapeId m_wheelShapeIds[4]; + WheelType m_wheelType; + int m_cylinderSides; b3JointId m_frontLeftId; b3JointId m_frontRightId; b3JointId m_rearLeftId; diff --git a/samples/sample_manifold.cpp b/samples/sample_manifold.cpp index d908f5c..a3875f9 100644 --- a/samples/sample_manifold.cpp +++ b/samples/sample_manifold.cpp @@ -1,6 +1,7 @@ // SPDX-FileCopyrightText: 2025 Erin Catto // SPDX-License-Identifier: MIT +#include "ghost_oracle.h" #include "gfx/draw.h" #include "gfx/keycodes.h" #include "sample.h" @@ -9,6 +10,8 @@ #include "box3d/constants.h" #include +#include +#include #include @@ -18,7 +21,8 @@ static void DrawGizmoControls( bool* show, bool* local ) ImGui::Checkbox( "Local axes", local ); } -// Must run inside the ImGui frame. The view is eye relative, so the shape is placed against the draw origin. +// Must run inside the ImGui frame. The view is eye relative, so the shape is placed against the +// draw origin. static void DrawTransformGizmo( const Camera* camera, b3WorldTransform* transform, bool local ) { ImGuizmo::BeginFrame(); @@ -131,8 +135,8 @@ public: b3Vec3 perp = b3Perp( normal ); b3FeaturePair pair = manifoldPoint.pair; - DrawString3D( point + 0.025f * normal + 0.05f * perp, MakeColor( b3_colorPapayaWhip ), " %X:%X %X:%X", pair.owner1, - pair.index1, pair.owner2, pair.index2 ); + DrawString3D( point + 0.025f * normal + 0.05f * perp, MakeColor( b3_colorPapayaWhip ), + " %X:%X %X:%X", pair.owner1, pair.index1, pair.owner2, pair.index2 ); } Sample::Render(); @@ -259,15 +263,17 @@ public: DrawPoint( point, 10.0f, MakeColor( b3_colorYellow ) ); } - DrawString3D( point, MakeColor( b3_colorWhite ), " %.2f", 100.0f * manifoldPoint.separation ); + DrawString3D( point, MakeColor( b3_colorWhite ), " %.2f", + 100.0f * manifoldPoint.separation ); b3FeaturePair pair = manifoldPoint.pair; - DrawString3D( point + 0.025f * normal, MakeColor( b3_colorPapayaWhip ), " %X:%X %X:%X", pair.owner1, pair.index1, - pair.owner2, pair.index2 ); + DrawString3D( point + 0.025f * normal, MakeColor( b3_colorPapayaWhip ), + " %X:%X %X:%X", pair.owner1, pair.index1, pair.owner2, pair.index2 ); } } - DrawTriangle( m_transformA, m_triangle[0], m_triangle[1], m_triangle[2], MakeColor( b3_colorCyan ) ); + DrawTriangle( m_transformA, m_triangle[0], m_triangle[1], m_triangle[2], + MakeColor( b3_colorCyan ) ); b3Pos p1 = b3TransformWorldPoint( m_transformA, m_triangle[0] ); b3Pos p2 = b3TransformWorldPoint( m_transformA, m_triangle[1] ); @@ -375,7 +381,8 @@ public: b3Sphere m_sphere; }; -static int sampleCollideSpheres = RegisterSample( "Manifold", "Sphere vs Sphere", SphereAndSphere::Create ); +static int sampleCollideSpheres = + RegisterSample( "Manifold", "Sphere vs Sphere", SphereAndSphere::Create ); class CapsuleAndSphere : public Manifold { @@ -406,14 +413,16 @@ public: void Step() override { b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB ); - b3CollideCapsuleAndSphere( &m_manifold, m_pointCapacity, &m_capsule, &m_sphere, transformBtoA ); + b3CollideCapsuleAndSphere( &m_manifold, m_pointCapacity, &m_capsule, &m_sphere, + transformBtoA ); } b3Sphere m_sphere; b3Capsule m_capsule; }; -static int sampleSphereAndCapsule = RegisterSample( "Manifold", "Capsule vs Sphere", CapsuleAndSphere::Create ); +static int sampleSphereAndCapsule = + RegisterSample( "Manifold", "Capsule vs Sphere", CapsuleAndSphere::Create ); class HullAndSphere : public Manifold { @@ -444,7 +453,8 @@ public: } b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB ); - b3CollideHullAndSphere( &m_manifold, m_pointCapacity, &m_hull.base, &m_sphere, transformBtoA, &m_simplexCache ); + b3CollideHullAndSphere( &m_manifold, m_pointCapacity, &m_hull.base, &m_sphere, + transformBtoA, &m_simplexCache ); } static Sample* Create( SampleContext* sampleContext ) @@ -456,7 +466,8 @@ public: b3BoxHull m_hull; }; -static int sampleSphereAndHull = RegisterSample( "Manifold", "Hull vs Sphere", HullAndSphere::Create ); +static int sampleSphereAndHull = + RegisterSample( "Manifold", "Hull vs Sphere", HullAndSphere::Create ); class TriangleAndSphere : public TriangleManifold { @@ -509,7 +520,8 @@ public: b3Sphere m_sphere; }; -static int sampleSphereAndTriangle = RegisterSample( "Manifold", "Triangle vs Sphere", TriangleAndSphere::Create ); +static int sampleSphereAndTriangle = + RegisterSample( "Manifold", "Triangle vs Sphere", TriangleAndSphere::Create ); class CapsuleAndCapsule : public Manifold { @@ -545,7 +557,8 @@ public: b3Capsule m_capsule; }; -static int sampleCapsuleAndCapsule = RegisterSample( "Manifold", "Capsule vs Capsule", CapsuleAndCapsule::Create ); +static int sampleCapsuleAndCapsule = + RegisterSample( "Manifold", "Capsule vs Capsule", CapsuleAndCapsule::Create ); class CapsuleAndHull : public Manifold { @@ -569,7 +582,8 @@ public: q [ [ -0.00256555085, -0.0201825816, 0.126076236 ], 0.991811991 ] b3Quat */ // m_capsule = { - // { 0.0799999982, -0.0151330000, -0.0918010026 }, { -0.0799999982, -0.0151330000, -0.0918010026 }, 0.100000001 }; + // { 0.0799999982, -0.0151330000, -0.0918010026 }, { -0.0799999982, -0.0151330000, + //-0.0918010026 }, 0.100000001 }; // m_hull = b3CreateBox( { 0.5f, 1.0f, 1.5f } ); m_transformB = { { 1.58523774, 0.729615569, 0.451690674 }, @@ -592,7 +606,8 @@ public: } b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB ); - b3CollideHullAndCapsule( &m_manifold, m_pointCapacity, &m_hull.base, &m_capsule, transformBtoA, &m_simplexCache ); + b3CollideHullAndCapsule( &m_manifold, m_pointCapacity, &m_hull.base, &m_capsule, + transformBtoA, &m_simplexCache ); } static Sample* Create( SampleContext* sampleContext ) @@ -604,7 +619,8 @@ public: b3Capsule m_capsule; }; -static int sampleCapsuleAndHull = RegisterSample( "Manifold", "Capsule vs Hull", CapsuleAndHull::Create ); +static int sampleCapsuleAndHull = + RegisterSample( "Manifold", "Capsule vs Hull", CapsuleAndHull::Create ); class TriangleAndCapsule : public TriangleManifold { @@ -654,13 +670,15 @@ public: b3TransformPoint( xf, m_triangle[2] ), }; - b3CollideTriangleAndCapsule( &m_manifold, m_pointCapacity, localTriangle, &m_capsule, &m_simplexCache ); + b3CollideTriangleAndCapsule( &m_manifold, m_pointCapacity, localTriangle, &m_capsule, + &m_simplexCache ); } b3Capsule m_capsule; }; -static int sampleCapsuleAndTriangle = RegisterSample( "Manifold", "Triangle vs Capsule", TriangleAndCapsule::Create ); +static int sampleCapsuleAndTriangle = + RegisterSample( "Manifold", "Triangle vs Capsule", TriangleAndCapsule::Create ); class HullAndHull : public Manifold { @@ -673,7 +691,8 @@ public: m_camera->SetView( 0.0f, 15.0f, 4.0f, b3Pos_zero ); } - // m_boxA = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, { { 0.0f, -0.5f, 0.0f }, b3Quat_identity } ); + // m_boxA = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, { { 0.0f, -0.5f, 0.0f }, + // b3Quat_identity } ); b3Transform transform = { { 1.0f, 0.5f, 0.0f }, b3Quat_identity }; m_boxA = b3MakeTransformedBoxHull( 0.5f, 1.0f, 1.0f, transform ); @@ -686,12 +705,12 @@ public: m_hullB = &m_boxB.base; /* -- xfA {p=[ -10.0000000, 0.00000000, -10.0000000 ] q=[ [ 0.00000000, 0.00000000, 0.00000000 ], 1.00000000 ] } b3Transform -+ p [ -10.0000000, 0.00000000, -10.0000000 ] b3Vec3 -+ q [ [ 0.00000000, 0.00000000, 0.00000000 ], 1.00000000 ] b3Quat -- xfB {p=[ -10.0000000, 1.00000000, -10.0000000 ] q=[ [ 0.00000000, 0.00000000, 0.00000000 ], 1.00000000 ] } b3Transform -+ p [ -10.0000000, 1.00000000, -10.0000000 ] b3Vec3 -+ q [ [ 0.00000000, 0.00000000, 0.00000000 ], 1.00000000 ] b3Quat +- xfA {p=[ -10.0000000, 0.00000000, -10.0000000 ] q=[ [ 0.00000000, 0.00000000, 0.00000000 +], 1.00000000 ] } b3Transform + p [ -10.0000000, 0.00000000, -10.0000000 ] b3Vec3 + +q [ [ 0.00000000, 0.00000000, 0.00000000 ], 1.00000000 ] b3Quat - xfB {p=[ +-10.0000000, 1.00000000, -10.0000000 ] q=[ [ 0.00000000, 0.00000000, 0.00000000 ], 1.00000000 ] } +b3Transform + p [ -10.0000000, 1.00000000, -10.0000000 ] b3Vec3 + q [ [ +0.00000000, 0.00000000, 0.00000000 ], 1.00000000 ] b3Quat */ m_transformA = { { 0.0f, 0.0f, 0.0f }, b3Quat_identity }; @@ -777,7 +796,8 @@ public: } b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB ); - b3CollideHulls( &m_manifold, m_pointCapacity, m_hullA, m_hullB, transformBtoA, &m_satCache ); + b3CollideHulls( &m_manifold, m_pointCapacity, m_hullA, m_hullB, transformBtoA, + &m_satCache ); DrawTextLine( "SAT type: %d", m_satCache.type ); } @@ -795,13 +815,13 @@ public: static int sampleCollideHulls = RegisterSample( "Manifold", "Hull vs Hull", HullAndHull::Create ); -// Shows the candidate axes culled in the separating axis test. No axis n can separate the hulls by more -// than dot(n, centerB - centerA) - innerRadiusA - innerRadiusB, so any face whose bound is below the best -// separation found so far is skipped. An edge survives only if the plane separation of the other hull's -// inscribed sphere can beat the best face separation somewhere over the Gauss map arc of the edge. The -// support points of the best faces lie on the other hull, so they give a second, tighter arc test. -// This recomputes the culling decisions of b3ComputeSeparatingAxis from public hull data and must be -// kept in sync with it. +// Shows the candidate axes culled in the separating axis test. No axis n can separate the hulls by +// more than dot(n, centerB - centerA) - innerRadiusA - innerRadiusB, so any face whose bound is +// below the best separation found so far is skipped. An edge survives only if the plane separation +// of the other hull's inscribed sphere can beat the best face separation somewhere over the Gauss +// map arc of the edge. The support points of the best faces lie on the other hull, so they give a +// second, tighter arc test. This recomputes the culling decisions of b3ComputeSeparatingAxis from +// public hull data and must be kept in sync with it. class HullCulling : public Manifold { public: @@ -840,7 +860,8 @@ public: CreateHulls(); m_transformA = { { 0.0f, 0.0f, 0.0f }, b3Quat_identity }; - m_transformB = { { 0.4f, 3.6f, 0.2f }, b3MakeQuatFromAxisAngle( b3Normalize( { 1.0f, 0.0f, 1.0f } ), 0.3f ) }; + m_transformB = { { 0.4f, 3.6f, 0.2f }, + b3MakeQuatFromAxisAngle( b3Normalize( { 1.0f, 0.0f, 1.0f } ), 0.3f ) }; m_showSpheres = true; m_showCulled = true; @@ -866,8 +887,10 @@ public: if ( m_useCylinders ) { float yOffset = -0.5f * m_cylinderHeight; - m_hullA = b3CreateCylinder( m_cylinderHeight, m_cylinderRadius, yOffset, m_cylinderSides ); - m_hullB = b3CreateCylinder( m_cylinderHeight, m_cylinderRadius, yOffset, m_cylinderSides ); + m_hullA = + b3CreateCylinder( m_cylinderHeight, m_cylinderRadius, yOffset, m_cylinderSides ); + m_hullB = + b3CreateCylinder( m_cylinderHeight, m_cylinderRadius, yOffset, m_cylinderSides ); } else { @@ -885,7 +908,8 @@ public: ImGui::Checkbox( "Face normals", &m_showNormals ); const char* edgeBounds[] = { "None", "Sphere", "Support", "Sphere + Support" }; ImGuiStyle& style = ImGui::GetStyle(); - ImGui::SetNextItemWidth( ImGui::CalcTextSize( edgeBounds[3] ).x + ImGui::GetFrameHeight() + 2.0f * style.FramePadding.x ); + ImGui::SetNextItemWidth( ImGui::CalcTextSize( edgeBounds[3] ).x + ImGui::GetFrameHeight() + + 2.0f * style.FramePadding.x ); ImGui::Combo( "Edge bound", &m_edgeBound, edgeBounds, IM_ARRAYSIZE( edgeBounds ) ); bool rebuild = ImGui::Checkbox( "Cylinders", &m_useCylinders ); @@ -928,8 +952,10 @@ public: m_separatedFeature = b3_invalidAxis; } - // Face of A against the vertices of B, in frame A. The deepest vertex of B is the support point. - static float FaceSeparationA( const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA, int faceIndex, + // Face of A against the vertices of B, in frame A. The deepest vertex of B is the support + // point. + static float FaceSeparationA( const b3HullData* hullA, const b3HullData* hullB, + b3Transform transformBtoA, int faceIndex, int* vertexIndex = nullptr ) { b3Plane plane = b3GetHullPlanes( hullA )[faceIndex]; @@ -955,8 +981,10 @@ public: return separation; } - // Face of B against the vertices of A, in frame B. The deepest vertex of A is the support point. - static float FaceSeparationB( const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA, int faceIndex, + // Face of B against the vertices of A, in frame B. The deepest vertex of A is the support + // point. + static float FaceSeparationB( const b3HullData* hullA, const b3HullData* hullB, + b3Transform transformBtoA, int faceIndex, int* vertexIndex = nullptr ) { b3Plane plane = b3GetHullPlanes( hullB )[faceIndex]; @@ -991,7 +1019,8 @@ public: float u = lo - c * hi; float s = 1.0f - c * c; bool exterior = hi >= bound; - bool interior = u >= 0.0f && ( u * u >= ( bound - hi ) * ( bound + hi ) * s || s < parallelTolerance ); + bool interior = + u >= 0.0f && ( u * u >= ( bound - hi ) * ( bound + hi ) * s || s < parallelTolerance ); return exterior || interior; } @@ -1032,7 +1061,8 @@ public: const b3Plane* planesB = b3GetHullPlanes( hullB ); float speculativeDistance = B3_SPECULATIVE_DISTANCE; - b3Vec3 deltaCenter = b3Sub( b3TransformPoint( transformBtoA, hullB->center ), hullA->center ); + b3Vec3 deltaCenter = + b3Sub( b3TransformPoint( transformBtoA, hullB->center ), hullA->center ); float centerDistance = b3Length( deltaCenter ); float radius = hullA->innerRadius + hullB->innerRadius; float radiusBound = radius - ( B3_LINEAR_SLOP + 0.001f * ( centerDistance + radius ) ); @@ -1046,7 +1076,8 @@ public: } // The seed support vertex stands in for the probe if no face improves on it - float seedSeparationA = FaceSeparationA( hullA, hullB, transformBtoA, m_seedA, &m_probeVertexB ); + float seedSeparationA = + FaceSeparationA( hullA, hullB, transformBtoA, m_seedA, &m_probeVertexB ); float floorA = b3MinFloat( seedSeparationA, speculativeDistance ); for ( int i = 0; i < hullA->faceCount; ++i ) { @@ -1079,7 +1110,8 @@ public: m_seedB = dotB[i] > dotB[m_seedB] ? i : m_seedB; } - float seedSeparationB = FaceSeparationB( hullA, hullB, transformBtoA, m_seedB, &m_probeVertexA ); + float seedSeparationB = + FaceSeparationB( hullA, hullB, transformBtoA, m_seedB, &m_probeVertexA ); float floorB = b3MaxFloat( seedSeparationB, m_separationA ); floorB = b3MinFloat( floorB, speculativeDistance ); for ( int i = 0; i < hullB->faceCount; ++i ) @@ -1111,7 +1143,8 @@ public: if ( useSphere ) { - // Inscribed sphere bound, the arc inputs are plane separations of the other hull's center + // Inscribed sphere bound, the arc inputs are plane separations of the other hull's + // center float boundSlack = radius - radiusBound; float thresholdA = maxFaceSeparation + hullB->innerRadius - boundSlack; float thresholdB = maxFaceSeparation + hullA->innerRadius - boundSlack; @@ -1136,9 +1169,13 @@ public: m_probeApplied = useSupport && testCount >= probeMinTests; if ( m_probeApplied ) { - float probeBound = maxFaceSeparation - ( 0.1f * B3_LINEAR_SLOP + 0.001f * b3AbsFloat( centerDistance + radius ) ); - b3Vec3 probeB = b3TransformPoint( transformBtoA, b3GetHullPoints( hullB )[m_probeVertexB] ); - b3Vec3 probeA = b3InvTransformPoint( transformBtoA, b3GetHullPoints( hullA )[m_probeVertexA] ); + float probeBound = + maxFaceSeparation - + ( 0.1f * B3_LINEAR_SLOP + 0.001f * b3AbsFloat( centerDistance + radius ) ); + b3Vec3 probeB = + b3TransformPoint( transformBtoA, b3GetHullPoints( hullB )[m_probeVertexB] ); + b3Vec3 probeA = + b3InvTransformPoint( transformBtoA, b3GetHullPoints( hullA )[m_probeVertexA] ); m_keptEdgeCountA = CullEdges( hullA, probeB, probeBound, m_edgeStateA ); m_keptEdgeCountB = CullEdges( hullB, probeA, probeBound, m_edgeStateB ); } @@ -1149,7 +1186,8 @@ public: // Clear the cache so every step runs the full separating axis test m_satCache = {}; b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB ); - b3CollideHulls( &m_manifold, m_pointCapacity, m_hullA, m_hullB, transformBtoA, &m_satCache ); + b3CollideHulls( &m_manifold, m_pointCapacity, m_hullA, m_hullB, transformBtoA, + &m_satCache ); ComputeCulling( transformBtoA ); } @@ -1163,7 +1201,8 @@ public: return n; } - void DrawEdges( b3WorldTransform transform, const b3HullData* hull, const uint8_t* states, Vec4 keptColor ) + void DrawEdges( b3WorldTransform transform, const b3HullData* hull, const uint8_t* states, + Vec4 keptColor ) { const b3HullHalfEdge* edges = b3GetHullEdges( hull ); const b3Vec3* points = b3GetHullPoints( hull ); @@ -1174,7 +1213,8 @@ public: uint8_t state = states[i / 2]; if ( state == e_tested ) { - DrawLineEx( p1, p2, keptColor, 4.0f, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DIM ); + DrawLineEx( p1, p2, keptColor, 4.0f, OVERLAY_THICKNESS_PIXELS, + OVERLAY_OCCLUSION_DIM ); } else if ( state == e_culled && m_showCulled ) { @@ -1182,12 +1222,14 @@ public: } else if ( state == e_unreached ) { - DrawLineEx( p1, p2, MakeColor( b3_colorDimGray ), 1.0f, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DASHED ); + DrawLineEx( p1, p2, MakeColor( b3_colorDimGray ), 1.0f, OVERLAY_THICKNESS_PIXELS, + OVERLAY_OCCLUSION_DASHED ); } } } - void DrawFaceNormals( b3WorldTransform transform, const b3HullData* hull, const uint8_t* states, int seed, Vec4 testedColor ) + void DrawFaceNormals( b3WorldTransform transform, const b3HullData* hull, const uint8_t* states, + int seed, Vec4 testedColor ) { const b3HullFace* faces = b3GetHullFaces( hull ); const b3HullHalfEdge* edges = b3GetHullEdges( hull ); @@ -1216,15 +1258,18 @@ public: centroid = b3MulSV( 1.0f / (float)count, centroid ); b3Pos p1 = b3TransformWorldPoint( transform, centroid ); - b3Pos p2 = b3TransformWorldPoint( transform, b3MulAdd( centroid, length, planes[i].normal ) ); + b3Pos p2 = + b3TransformWorldPoint( transform, b3MulAdd( centroid, length, planes[i].normal ) ); if ( i == seed ) { - DrawArrowEx( p1, p2, MakeColor( b3_colorGold ), 3.0f, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DIM, 0.25f ); + DrawArrowEx( p1, p2, MakeColor( b3_colorGold ), 3.0f, OVERLAY_THICKNESS_PIXELS, + OVERLAY_OCCLUSION_DIM, 0.25f ); } else if ( state == e_tested ) { - DrawArrowEx( p1, p2, testedColor, 2.0f, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DIM, 0.25f ); + DrawArrowEx( p1, p2, testedColor, 2.0f, OVERLAY_THICKNESS_PIXELS, + OVERLAY_OCCLUSION_DIM, 0.25f ); } else { @@ -1243,8 +1288,10 @@ public: if ( m_showNormals ) { - DrawFaceNormals( m_transformA, hullA, m_faceStateA, m_seedA, MakeColor( b3_colorOrange ) ); - DrawFaceNormals( m_transformB, hullB, m_faceStateB, m_seedB, MakeColor( b3_colorDeepSkyBlue ) ); + DrawFaceNormals( m_transformA, hullA, m_faceStateA, m_seedA, + MakeColor( b3_colorOrange ) ); + DrawFaceNormals( m_transformB, hullB, m_faceStateB, m_seedB, + MakeColor( b3_colorDeepSkyBlue ) ); } b3Pos centerA = b3TransformWorldPoint( m_transformA, hullA->center ); @@ -1261,8 +1308,10 @@ public: if ( m_probeApplied ) { - b3Pos probeA = b3TransformWorldPoint( m_transformA, b3GetHullPoints( hullA )[m_probeVertexA] ); - b3Pos probeB = b3TransformWorldPoint( m_transformB, b3GetHullPoints( hullB )[m_probeVertexB] ); + b3Pos probeA = + b3TransformWorldPoint( m_transformA, b3GetHullPoints( hullA )[m_probeVertexA] ); + b3Pos probeB = + b3TransformWorldPoint( m_transformB, b3GetHullPoints( hullB )[m_probeVertexB] ); DrawPoint( probeA, 12.0f, MakeColor( b3_colorMagenta ) ); DrawPoint( probeB, 12.0f, MakeColor( b3_colorMagenta ) ); } @@ -1278,9 +1327,11 @@ public: bool edgesReached = m_separatedFeature == b3_invalidAxis; int seedCount = m_separatedFeature == b3_faceAxisA ? 1 : 2; - DrawTextLine( "vertices %d, faces %d, edges %d", hullA->vertexCount, faceCountA, edgeCountA ); + DrawTextLine( "vertices %d, faces %d, edges %d", hullA->vertexCount, faceCountA, + edgeCountA ); DrawTextLine( "|d| - rA - rB = %.3f", m_gap ); - DrawTextLine( "faces A: tested %d, culled %d of %d, best separation %.4f", testedA, culledA, faceCountA, m_separationA ); + DrawTextLine( "faces A: tested %d, culled %d of %d, best separation %.4f", testedA, culledA, + faceCountA, m_separationA ); if ( m_separatedFeature == b3_faceAxisA ) { @@ -1288,8 +1339,8 @@ public: } else { - DrawTextLine( "faces B: tested %d, culled %d of %d, best separation %.4f", testedB, culledB, faceCountB, - m_separationB ); + DrawTextLine( "faces B: tested %d, culled %d of %d, best separation %.4f", testedB, + culledB, faceCountB, m_separationB ); } if ( m_separatedFeature == b3_faceAxisB ) @@ -1297,21 +1348,24 @@ public: DrawTextLine( "separated by face B, edges not reached" ); } - DrawTextLine( "support queries: %d of %d (including %d seed)", testedA + testedB + seedCount, faceCountA + faceCountB, - seedCount ); + DrawTextLine( "support queries: %d of %d (including %d seed)", + testedA + testedB + seedCount, faceCountA + faceCountB, seedCount ); if ( edgesReached ) { int pairCount = m_keptEdgeCountA * m_keptEdgeCountB; int totalPairCount = edgeCountA * edgeCountB; - DrawTextLine( "edges kept: A %d of %d, B %d of %d", m_keptEdgeCountA, edgeCountA, m_keptEdgeCountB, edgeCountB ); - DrawTextLine( "edge pairs: %d of %d (%.1f%%)", pairCount, totalPairCount, 100.0f * pairCount / totalPairCount ); + DrawTextLine( "edges kept: A %d of %d, B %d of %d", m_keptEdgeCountA, edgeCountA, + m_keptEdgeCountB, edgeCountB ); + DrawTextLine( "edge pairs: %d of %d (%.1f%%)", pairCount, totalPairCount, + 100.0f * pairCount / totalPairCount ); if ( m_probeApplied && m_edgeBound == e_bothBounds ) { int spherePairCount = m_sphereKeptEdgeCountA * m_sphereKeptEdgeCountB; - DrawTextLine( "sphere bound: A %d, B %d, edge pairs %d (%.1f%%)", m_sphereKeptEdgeCountA, m_sphereKeptEdgeCountB, - spherePairCount, 100.0f * spherePairCount / totalPairCount ); + DrawTextLine( "sphere bound: A %d, B %d, edge pairs %d (%.1f%%)", + m_sphereKeptEdgeCountA, m_sphereKeptEdgeCountB, spherePairCount, + 100.0f * spherePairCount / totalPairCount ); } if ( m_probeApplied ) @@ -1343,7 +1397,8 @@ public: int m_seedA; int m_seedB; - // Support vertex on each hull for the best face of the other, it probes the edges of the other hull + // Support vertex on each hull for the best face of the other, it probes the edges of the other + // hull int m_probeVertexA; int m_probeVertexB; @@ -1369,10 +1424,11 @@ public: static int sampleHullCulling = RegisterSample( "Manifold", "Hull Culling", HullCulling::Create ); // Shows that an edge pair can win the separating axis test even when the direction between the hull -// centers peaks outside the Gauss map arc (wedge) of one of its edges. The inscribed sphere bound of such -// an edge peaks at a face normal at the end of its arc, yet the edge axis beats the real separation of -// those faces. So outside wedge edges can only be culled by their sphere bound, not by the face results. -// Everything here is brute force over all faces and all edge pairs, independent of the pruning in the SAT. +// centers peaks outside the Gauss map arc (wedge) of one of its edges. The inscribed sphere bound +// of such an edge peaks at a face normal at the end of its arc, yet the edge axis beats the real +// separation of those faces. So outside wedge edges can only be culled by their sphere bound, not +// by the face results. Everything here is brute force over all faces and all edge pairs, +// independent of the pruning in the SAT. class EdgeAxisWedge : public Manifold { public: @@ -1454,7 +1510,10 @@ public: } else if ( type == e_tetrahedron ) { - b3Vec3 points[4] = { { 1.0f, 1.0f, 1.0f }, { 1.0f, -1.0f, -1.0f }, { -1.0f, 1.0f, -1.0f }, { -1.0f, -1.0f, 1.0f } }; + b3Vec3 points[4] = { { 1.0f, 1.0f, 1.0f }, + { 1.0f, -1.0f, -1.0f }, + { -1.0f, 1.0f, -1.0f }, + { -1.0f, -1.0f, 1.0f } }; m_createdHull = b3CreateHull( points, 4, 4 ); m_hullA = m_createdHull; } @@ -1482,7 +1541,8 @@ public: float lengthSquared; do { - v = { 2.0f * RandomFloat() - 1.0f, 2.0f * RandomFloat() - 1.0f, 2.0f * RandomFloat() - 1.0f }; + v = { 2.0f * RandomFloat() - 1.0f, 2.0f * RandomFloat() - 1.0f, + 2.0f * RandomFloat() - 1.0f }; lengthSquared = b3Dot( v, v ); } while ( lengthSquared > 1.0f || lengthSquared < 0.01f ); @@ -1490,8 +1550,10 @@ public: return b3Normalize( v ); } - // Mirrors the Gauss map test and edge axis of the scalar path in b3ComputeSeparatingAxis, without pruning - static EdgeAxis FindBestEdgeAxis( const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA ) + // Mirrors the Gauss map test and edge axis of the scalar path in b3ComputeSeparatingAxis, + // without pruning + static EdgeAxis FindBestEdgeAxis( const b3HullData* hullA, const b3HullData* hullB, + b3Transform transformBtoA ) { const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA ); const b3Plane* planesA = b3GetHullPlanes( hullA ); @@ -1509,7 +1571,8 @@ public: { // B face normals and vertices in frame A, negated b3Vec3 C = b3Neg( b3RotateVector( transformBtoA.q, planesB[edgesB[j].face].normal ) ); - b3Vec3 D = b3Neg( b3RotateVector( transformBtoA.q, planesB[edgesB[j + 1].face].normal ) ); + b3Vec3 D = + b3Neg( b3RotateVector( transformBtoA.q, planesB[edgesB[j + 1].face].normal ) ); b3Vec3 bv0 = b3Neg( b3TransformPoint( transformBtoA, pointsB[edgesB[j].origin] ) ); b3Vec3 bv1 = b3Neg( b3TransformPoint( transformBtoA, pointsB[edgesB[j + 1].origin] ) ); b3Vec3 DC = b3Sub( bv1, bv0 ); @@ -1573,18 +1636,21 @@ public: return arc; } - float MaxSeparation( b3Transform transformBtoA, float* faceSeparationA, float* faceSeparationB, EdgeAxis* edgeAxis ) const + float MaxSeparation( b3Transform transformBtoA, float* faceSeparationA, float* faceSeparationB, + EdgeAxis* edgeAxis ) const { float separationA = -FLT_MAX; for ( int i = 0; i < m_hullA->faceCount; ++i ) { - separationA = b3MaxFloat( separationA, HullCulling::FaceSeparationA( m_hullA, m_hullB, transformBtoA, i ) ); + separationA = b3MaxFloat( + separationA, HullCulling::FaceSeparationA( m_hullA, m_hullB, transformBtoA, i ) ); } float separationB = -FLT_MAX; for ( int i = 0; i < m_hullB->faceCount; ++i ) { - separationB = b3MaxFloat( separationB, HullCulling::FaceSeparationB( m_hullA, m_hullB, transformBtoA, i ) ); + separationB = b3MaxFloat( + separationB, HullCulling::FaceSeparationB( m_hullA, m_hullB, transformBtoA, i ) ); } EdgeAxis edge = FindBestEdgeAxis( m_hullA, m_hullB, transformBtoA ); @@ -1617,7 +1683,8 @@ public: // Bisect on the center distance to reach a slight overlap float target = -0.02f * b3GetLengthUnitsPerMeter(); float lower = 0.0f; - float upper = 1.1f * ( b3Length( b3AABB_Extents( m_hullA->aabb ) ) + b3Length( b3AABB_Extents( m_hullB->aabb ) ) ); + float upper = 1.1f * ( b3Length( b3AABB_Extents( m_hullA->aabb ) ) + + b3Length( b3AABB_Extents( m_hullB->aabb ) ) ); for ( int iteration = 0; iteration < 24; ++iteration ) { float mid = 0.5f * ( lower + upper ); @@ -1638,7 +1705,8 @@ public: // Only accept clear examples, well outside the wedge with a clear edge win float margin = m_edge.separation - b3MaxFloat( m_faceSeparationA, m_faceSeparationB ); float outside = b3MaxFloat( m_arcA.outsideDegrees, m_arcB.outsideDegrees ); - m_searchFound = m_edgeWins && outside >= 10.0f && margin >= 0.02f * b3GetLengthUnitsPerMeter(); + m_searchFound = + m_edgeWins && outside >= 10.0f && margin >= 0.02f * b3GetLengthUnitsPerMeter(); } } @@ -1646,9 +1714,11 @@ public: { b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB ); MaxSeparation( transformBtoA, &m_faceSeparationA, &m_faceSeparationB, &m_edge ); - m_edgeWins = m_edge.edgeA != B3_NULL_INDEX && m_edge.separation > b3MaxFloat( m_faceSeparationA, m_faceSeparationB ); + m_edgeWins = m_edge.edgeA != B3_NULL_INDEX && + m_edge.separation > b3MaxFloat( m_faceSeparationA, m_faceSeparationB ); - m_centerOffset = b3Sub( b3TransformPoint( transformBtoA, m_hullB->center ), m_hullA->center ); + m_centerOffset = + b3Sub( b3TransformPoint( transformBtoA, m_hullB->center ), m_hullA->center ); m_radius = m_hullA->innerRadius + m_hullB->innerRadius; if ( m_edge.edgeA == B3_NULL_INDEX ) @@ -1670,7 +1740,8 @@ public: int faceB2 = edgesB[m_edge.edgeB + 1].face; b3Vec3 C = b3Neg( b3RotateVector( transformBtoA.q, planesB[faceB1].normal ) ); b3Vec3 D = b3Neg( b3RotateVector( transformBtoA.q, planesB[faceB2].normal ) ); - m_arcB = MakeArc( C, D, m_centerOffset, HullCulling::FaceSeparationB( m_hullA, m_hullB, transformBtoA, faceB1 ), + m_arcB = MakeArc( C, D, m_centerOffset, + HullCulling::FaceSeparationB( m_hullA, m_hullB, transformBtoA, faceB1 ), HullCulling::FaceSeparationB( m_hullA, m_hullB, transformBtoA, faceB2 ) ); } @@ -1700,17 +1771,20 @@ public: b3Pos GaussPoint( b3Vec3 directionInA ) const { - return m_gaussCenter + b3MulSV( m_gaussRadius, b3RotateVector( m_transformA.q, directionInA ) ); + return m_gaussCenter + + b3MulSV( m_gaussRadius, b3RotateVector( m_transformA.q, directionInA ) ); } - void DrawGreatArc( b3Vec3 u, b3Vec3 v, Vec4 color, float thickness, OverlayOcclusionMode mode ) const + void DrawGreatArc( b3Vec3 u, b3Vec3 v, Vec4 color, float thickness, + OverlayOcclusionMode mode ) const { constexpr int segmentCount = 32; b3Pos previous = GaussPoint( u ); for ( int i = 1; i <= segmentCount; ++i ) { float s = (float)i / segmentCount; - b3Pos point = GaussPoint( b3Normalize( b3Add( b3MulSV( 1.0f - s, u ), b3MulSV( s, v ) ) ) ); + b3Pos point = + GaussPoint( b3Normalize( b3Add( b3MulSV( 1.0f - s, u ), b3MulSV( s, v ) ) ) ); DrawLineEx( previous, point, color, thickness, OVERLAY_THICKNESS_PIXELS, mode ); previous = point; } @@ -1730,14 +1804,16 @@ public: float x2 = cs.cosine * x1 - cs.sine * y1; float y2 = cs.sine * x1 + cs.cosine * y1; b3Pos point = GaussPoint( b3Add( b3MulSV( x2, u ), b3MulSV( y2, w ) ) ); - DrawLineEx( previous, point, color, 1.0f, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DASHED ); + DrawLineEx( previous, point, color, 1.0f, OVERLAY_THICKNESS_PIXELS, + OVERLAY_OCCLUSION_DASHED ); x1 = x2; y1 = y2; previous = point; } } - void DrawEdge( b3WorldTransform transform, const b3HullData* hull, int edgeIndex, Vec4 color ) const + void DrawEdge( b3WorldTransform transform, const b3HullData* hull, int edgeIndex, + Vec4 color ) const { const b3HullHalfEdge* edges = b3GetHullEdges( hull ); const b3Vec3* points = b3GetHullPoints( hull ); @@ -1754,10 +1830,13 @@ public: } else { - DrawTextLine( "%s edge: d peak OUTSIDE wedge by %.1f degrees", name, arc.outsideDegrees ); + DrawTextLine( "%s edge: d peak OUTSIDE wedge by %.1f degrees", name, + arc.outsideDegrees ); } - DrawTextLine( " sphere bound u %.3f v %.3f n %.3f", arc.a - m_radius, arc.b - m_radius, edgeBound ); - DrawTextLine( " separation u %.4f v %.4f n %.4f", arc.separationU, arc.separationV, m_edge.separation ); + DrawTextLine( " sphere bound u %.3f v %.3f n %.3f", arc.a - m_radius, arc.b - m_radius, + edgeBound ); + DrawTextLine( " separation u %.4f v %.4f n %.4f", arc.separationU, arc.separationV, + m_edge.separation ); } void Render() override @@ -1769,9 +1848,11 @@ public: b3Pos centerB = b3TransformWorldPoint( m_transformB, m_hullB->center ); DrawLine( centerA, centerB, MakeColor( b3_colorWhite ) ); - DrawTextLine( "search: %s after %d poses", m_searchFound ? "found" : "not found", m_searchCount ); - DrawTextLine( "best separation: face A %.4f, face B %.4f, edge %.4f -> %s", m_faceSeparationA, m_faceSeparationB, - m_edge.separation, m_edgeWins ? "EDGE WINS" : "face wins" ); + DrawTextLine( "search: %s after %d poses", m_searchFound ? "found" : "not found", + m_searchCount ); + DrawTextLine( "best separation: face A %.4f, face B %.4f, edge %.4f -> %s", + m_faceSeparationA, m_faceSeparationB, m_edge.separation, + m_edgeWins ? "EDGE WINS" : "face wins" ); if ( m_edge.edgeA == B3_NULL_INDEX ) { @@ -1791,16 +1872,19 @@ public: { const b3HullHalfEdge* edges = b3GetHullEdges( m_hullA ); const b3Vec3* points = b3GetHullPoints( m_hullA ); - b3Vec3 mid = b3Lerp( points[edges[m_edge.edgeA].origin], points[edges[m_edge.edgeA + 1].origin], 0.5f ); + b3Vec3 mid = b3Lerp( points[edges[m_edge.edgeA].origin], + points[edges[m_edge.edgeA + 1].origin], 0.5f ); b3Pos p = b3TransformWorldPoint( m_transformA, mid ); b3Vec3 n = b3RotateVector( m_transformA.q, m_edge.normal ); - DrawArrowEx( p, p + b3MulSV( 1.0f, n ), colorN, 3.0f, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DIM, 0.2f ); + DrawArrowEx( p, p + b3MulSV( 1.0f, n ), colorN, 3.0f, OVERLAY_THICKNESS_PIXELS, + OVERLAY_OCCLUSION_DIM, 0.2f ); } - // Gauss map: both arcs cross at the edge axis n. The magenta dots are the peaks of dot(n, d) on each - // great circle. The peak of an arc is only reachable when it lies on the arc. + // Gauss map: both arcs cross at the edge axis n. The magenta dots are the peaks of dot(n, + // d) on each great circle. The peak of an arc is only reachable when it lies on the arc. b3Sphere unitSphere = { b3Vec3_zero, m_gaussRadius }; - DrawWireSphere( { m_gaussCenter, b3Quat_identity }, &unitSphere, 24, MakeColor( b3_colorDimGray ) ); + DrawWireSphere( { m_gaussCenter, b3Quat_identity }, &unitSphere, 24, + MakeColor( b3_colorDimGray ) ); DrawGreatCircle( m_arcA.u, m_arcA.v, colorA ); DrawGreatCircle( m_arcB.u, m_arcB.v, colorB ); @@ -1861,7 +1945,8 @@ public: int m_hullType; }; -static int sampleEdgeAxisWedge = RegisterSample( "Manifold", "Edge Axis Wedge", EdgeAxisWedge::Create ); +static int sampleEdgeAxisWedge = + RegisterSample( "Manifold", "Edge Axis Wedge", EdgeAxisWedge::Create ); class TriangleAndHull : public TriangleManifold { @@ -1883,22 +1968,19 @@ public: // m_triangle[1] = { 1.00000000, 0, 0.00000000 }; // m_triangle[2] = { 0.00000000, 0, 0.00000000 }; - m_triangle[0] = { 0.299769998f, -1.01549578f, -0.744717002f }; - m_triangle[1] = { 0.299769998f, -1.01549578f, 1.28728306f }; - m_triangle[2] = { 0.299769998f, -0.913895786f, 0.271283031f }; + // Deep overlap where the hull face normal crosses the triangle edge, so SAT tries the edge pair, + // but the edge closest points fall outside the segments. Only the hull face clip keeps a contact, + // GJK sees overlap and returns nothing. + m_triangle[0] = { 0.259390f, -0.178787f, -0.292602f }; + m_triangle[1] = { 0.861452f, 0.228978f, 0.393873f }; + m_triangle[2] = { -0.539058f, 0.127502f, 0.225728f }; - float bodyHalfWidth = 0.304800004f; - float bodyHalfHeight = 0.914399981f; - - m_boxHull = b3MakeBoxHull( bodyHalfWidth, bodyHalfHeight, bodyHalfWidth ); + m_boxHull = b3MakeBoxHull( 0.4f, 0.2f, 0.4f ); m_transformA = b3WorldTransform_identity; m_transformB = b3WorldTransform_identity; // m_transformB.p = { -2.16650009f, 0.912535489f, 0.00000000f }; - // b3MeshEdgeFlags - m_flags = 0; - /* * separation -0.0415397286 float type 2 '\x2' unsigned char @@ -1967,14 +2049,1603 @@ public: b3TransformPoint( xf, m_triangle[2] ), }; - b3CollideTriangleAndHull( &m_manifold, m_pointCapacity, localTriangle[0], localTriangle[1], localTriangle[2], m_flags, - m_hull, &m_satCache, true ); + b3CollideTriangleAndHull( &m_manifold, m_pointCapacity, localTriangle[0], localTriangle[1], + localTriangle[2], m_hull, &m_satCache, true ); } - int m_flags; b3BoxHull m_boxHull; b3HullData* m_cylinder; b3HullData* m_hull; }; -static int sampleHullAndTriangle = RegisterSample( "Manifold", "Triangle vs Hull", TriangleAndHull::Create ); +static int sampleHullAndTriangle = + RegisterSample( "Manifold", "Triangle vs Hull", TriangleAndHull::Create ); + +// Minimal meshes that each show one kind of ghost collision and how ghost handling treats it. +// Contacts are computed fresh every step with the same triangle pipeline the world uses, so a +// breakpoint in the mesh collide function stops on this scene. Derive per shape type. +struct GhostMesh +{ + std::vector vertices; + std::vector indices; +}; + +typedef void GhostMeshBuildFcn( std::vector& meshes ); + +struct GhostScene +{ + const char* name; + const char* ghost; + const char* handling; + GhostMeshBuildFcn* build; + + // Interpreted by the derived sample, for example box half extents + b3Vec3 extents; + + b3Vec3 position; + + // Degrees, applied as yaw * pitch * roll + b3Vec3 angles; + + bool speculative; +}; + +// Most surfaces in these scenes face up, so wind each triangle upward unless told otherwise +static void AddGhostTriangle( GhostMesh& mesh, int a, int b, int c, b3Vec3 facing = b3Vec3_axisY ) +{ + b3Vec3 va = mesh.vertices[a]; + b3Vec3 normal = b3Cross( b3Sub( mesh.vertices[b], va ), b3Sub( mesh.vertices[c], va ) ); + if ( b3Dot( normal, facing ) < 0.0f ) + { + int temp = b; + b = c; + c = temp; + } + + mesh.indices.push_back( a ); + mesh.indices.push_back( b ); + mesh.indices.push_back( c ); +} + +class GhostManifold : public Sample +{ +public: + static constexpr int m_maxScenes = 20; + static constexpr int m_manifoldCapacity = 32; + static constexpr int m_triangleCapacity = 64; + static constexpr int m_trianglePointCapacity = 64; + + enum EdgeType + { + e_boundaryEdge, + e_flatEdge, + e_convexEdge, + e_concaveEdge, + }; + + struct MeshView + { + GhostMesh source; + std::vector edgeTypes; + + // By engine triangle index, which differs from the source order + std::vector triangleCenters; + b3MeshData* data; + }; + + struct TriangleResult + { + b3LocalManifold manifold; + b3LocalManifoldPoint points[m_trianglePointCapacity]; + }; + + explicit GhostManifold( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 30.0f, 25.0f, 4.0f, { 0.0f, 0.3f, 0.0f } ); + } + + m_scenes = nullptr; + m_sceneCount = 0; + m_sceneIndex = 0; + m_handleGhosts = true; + m_speculative = true; + m_drawTriangleManifolds = false; + m_drawTriangleIndices = false; + m_drawVertexIndices = false; + m_cullDistance = 0.5f; + m_showGizmo = false; + m_gizmoLocal = false; + m_transform = b3WorldTransform_identity; + m_manifoldCount = 0; + m_triangleManifoldCount = 0; + } + + ~GhostManifold() override + { + DestroyMeshes(); + } + + // Derived samples call this from their constructor + void SetScenes( const GhostScene* scenes, int count ) + { + m_scenes = scenes; + m_sceneCount = count; + LoadScene( 0 ); + } + + virtual void CreateShape( const GhostScene& scene ) = 0; + + virtual int CollideMesh( b3Manifold* manifolds, int capacity, const b3Mesh* mesh, + b3Transform transformBtoA, bool speculative, bool handleGhosts ) = 0; + + // Triangle in the shape frame, before any ghost handling + virtual void CollideTriangle( b3LocalManifold* manifold, int capacity, const b3Vec3* triangle, + bool speculative ) = 0; + + virtual void DrawShape( b3WorldTransform transform ) = 0; + + virtual b3AABB ComputeShapeBounds( b3Transform transform ) = 0; + + void DestroyMeshes() + { + for ( MeshView& mesh : m_meshes ) + { + b3DestroyMesh( mesh.data ); + } + m_meshes.clear(); + } + + void LoadScene( int index ) + { + DestroyMeshes(); + + m_sceneIndex = index; + const GhostScene& scene = m_scenes[index]; + + std::vector sources; + scene.build( sources ); + + for ( GhostMesh& source : sources ) + { + b3MeshDef meshDef = {}; + meshDef.vertices = source.vertices.data(); + meshDef.indices = source.indices.data(); + meshDef.vertexCount = (int)source.vertices.size(); + meshDef.triangleCount = (int)source.indices.size() / 3; + meshDef.weldVertices = true; + meshDef.weldTolerance = 0.001f; + meshDef.identifyEdges = true; + + MeshView mesh; + mesh.source = source; + mesh.data = b3CreateMesh( &meshDef, nullptr, 0 ); + ClassifyEdges( mesh ); + + b3AABB bounds = { source.vertices[0], source.vertices[0] }; + for ( b3Vec3 vertex : source.vertices ) + { + bounds.lowerBound = b3Min( bounds.lowerBound, vertex ); + bounds.upperBound = b3Max( bounds.upperBound, vertex ); + } + + b3Vec3 margin = { 0.1f, 0.1f, 0.1f }; + bounds.lowerBound = b3Sub( bounds.lowerBound, margin ); + bounds.upperBound = b3Add( bounds.upperBound, margin ); + + mesh.triangleCenters.assign( meshDef.triangleCount, b3Vec3_zero ); + b3Mesh query = { mesh.data, b3Vec3_one }; + b3QueryMesh( &query, bounds, StoreTriangleCenter, &mesh.triangleCenters ); + m_meshes.push_back( mesh ); + } + + ResetPose(); + m_speculative = scene.speculative; + CreateShape( scene ); + } + + void ResetPose() + { + const GhostScene& scene = m_scenes[m_sceneIndex]; + float toRadians = B3_PI / 180.0f; + b3Quat qx = b3MakeQuatFromAxisAngle( b3Vec3_axisX, scene.angles.x * toRadians ); + b3Quat qy = b3MakeQuatFromAxisAngle( b3Vec3_axisY, scene.angles.y * toRadians ); + b3Quat qz = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, scene.angles.z * toRadians ); + m_transform.q = b3MulQuat( qy, b3MulQuat( qx, qz ) ); + m_transform.p = b3OffsetPos( b3Pos_zero, scene.position ); + } + + // Matches edges by vertex index, so a t-junction or a seam between meshes is a boundary edge, + // as in the mesh builder + static void ClassifyEdges( MeshView& mesh ) + { + const std::vector& v = mesh.source.vertices; + const std::vector& idx = mesh.source.indices; + int triangleCount = (int)idx.size() / 3; + mesh.edgeTypes.assign( 3 * triangleCount, e_boundaryEdge ); + + for ( int i = 0; i < triangleCount; ++i ) + { + const int32_t* t = idx.data() + 3 * i; + b3Vec3 normal = + b3Normalize( b3Cross( b3Sub( v[t[1]], v[t[0]] ), b3Sub( v[t[2]], v[t[0]] ) ) ); + + for ( int k = 0; k < 3; ++k ) + { + int a = t[k]; + int b = t[k < 2 ? k + 1 : 0]; + + for ( int j = 0; j < triangleCount; ++j ) + { + const int32_t* u = idx.data() + 3 * j; + for ( int m = 0; m < 3 && j != i; ++m ) + { + if ( u[m] != b || u[m < 2 ? m + 1 : 0] != a ) + { + continue; + } + + int opposite = u[m > 0 ? m - 1 : 2]; + float distance = b3Dot( normal, b3Sub( v[opposite], v[a] ) ); + float tolerance = 0.001f; + EdgeType type = distance < -tolerance + ? e_convexEdge + : ( distance > tolerance ? e_concaveEdge : e_flatEdge ); + mesh.edgeTypes[3 * i + k] = type; + } + } + } + } + } + + static bool StoreTriangleCenter( b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleIndex, void* context ) + { + std::vector* centers = (std::vector*)context; + if ( triangleIndex < (int)centers->size() ) + { + ( *centers )[triangleIndex] = b3MulSV( 1.0f / 3.0f, b3Add( a, b3Add( b, c ) ) ); + } + return true; + } + + static bool CollectTriangle( b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleIndex, void* context ) + { + GhostManifold* self = (GhostManifold*)context; + if ( self->m_triangleManifoldCount == m_triangleCapacity ) + { + return false; + } + + self->m_queryTriangles[self->m_triangleManifoldCount] = { a, b, c }; + self->m_queryIndices[self->m_triangleManifoldCount] = triangleIndex; + self->m_triangleManifoldCount += 1; + return true; + } + + void Step() override + { + // Mesh frame is the world frame + b3Transform transformBtoA = + b3InvMulWorldTransforms( b3WorldTransform_identity, m_transform ); + + m_manifoldCount = 0; + for ( int i = 0; i < (int)m_meshes.size(); ++i ) + { + b3Mesh mesh = { m_meshes[i].data, b3Vec3_one }; + int count = + CollideMesh( m_manifolds + m_manifoldCount, m_manifoldCapacity - m_manifoldCount, + &mesh, transformBtoA, m_speculative, m_handleGhosts ); + for ( int j = 0; j < count; ++j ) + { + m_manifoldMesh[m_manifoldCount + j] = i; + } + m_manifoldCount += count; + } + + m_triangleManifoldCount = 0; + if ( m_drawTriangleManifolds == false ) + { + return; + } + + b3AABB bounds = ComputeShapeBounds( transformBtoA ); + float margin = B3_SPECULATIVE_DISTANCE; + bounds.lowerBound = b3Sub( bounds.lowerBound, { margin, margin, margin } ); + bounds.upperBound = b3Add( bounds.upperBound, { margin, margin, margin } ); + + b3Transform transformAtoB = + b3InvMulWorldTransforms( m_transform, b3WorldTransform_identity ); + for ( MeshView& drawMesh : m_meshes ) + { + b3Mesh mesh = { drawMesh.data, b3Vec3_one }; + int base = m_triangleManifoldCount; + b3QueryMesh( &mesh, bounds, CollectTriangle, this ); + + for ( int i = base; i < m_triangleManifoldCount; ++i ) + { + TriangleQuery& query = m_queryTriangles[i]; + b3Vec3 triangle[3] = { + b3TransformPoint( transformAtoB, query.a ), + b3TransformPoint( transformAtoB, query.b ), + b3TransformPoint( transformAtoB, query.c ), + }; + + TriangleResult& tm = m_triangleManifolds[i]; + tm.manifold = {}; + tm.manifold.points = tm.points; + CollideTriangle( &tm.manifold, m_trianglePointCapacity, triangle, m_speculative ); + tm.manifold.triangleIndex = m_queryIndices[i]; + } + } + } + + void Render() override + { + const GhostScene& scene = m_scenes[m_sceneIndex]; + DrawTextLine( "%s", scene.name ); + DrawTextLine( "ghost: %s", scene.ghost ); + DrawTextLine( "handling: %s", scene.handling ); + DrawTextLine( "manifolds: %d", m_manifoldCount ); + + DrawGroundGrid( 80 ); + + Vec4 edgeColors[] = { + MakeColor( b3_colorRed ), + MakeColor( b3_colorGray ), + MakeColor( b3_colorOrange ), + MakeColor( b3_colorDodgerBlue ), + }; + + Vec4 faceColors[] = { MakeColor( b3_colorDarkSlateGray ), + MakeColor( b3_colorDarkOliveGreen ) }; + + for ( int i = 0; i < (int)m_meshes.size(); ++i ) + { + const MeshView& mesh = m_meshes[i]; + const std::vector& v = mesh.source.vertices; + const std::vector& idx = mesh.source.indices; + int triangleCount = (int)idx.size() / 3; + + for ( int t = 0; t < triangleCount; ++t ) + { + b3Vec3 a = v[idx[3 * t]], b = v[idx[3 * t + 1]], c = v[idx[3 * t + 2]]; + DrawTriangle( b3WorldTransform_identity, a, b, c, faceColors[i & 1] ); + + b3Vec3 corners[3] = { a, b, c }; + for ( int k = 0; k < 3; ++k ) + { + b3Pos p1 = b3OffsetPos( b3Pos_zero, corners[k] ); + b3Pos p2 = b3OffsetPos( b3Pos_zero, corners[k < 2 ? k + 1 : 0] ); + DrawLine( p1, p2, edgeColors[mesh.edgeTypes[3 * t + k]] ); + } + } + } + + if ( m_drawTriangleIndices ) + { + bool multipleMeshes = m_meshes.size() > 1; + for ( int i = 0; i < (int)m_meshes.size(); ++i ) + { + const std::vector& centers = m_meshes[i].triangleCenters; + for ( int t = 0; t < (int)centers.size(); ++t ) + { + b3Pos center = b3OffsetPos( b3Pos_zero, centers[t] ); + if ( multipleMeshes ) + { + DrawString3D( center, MakeColor( b3_colorLightGray ), "m%d t%d", i, t ); + } + else + { + DrawString3D( center, MakeColor( b3_colorLightGray ), "t%d", t ); + } + } + } + } + + if ( m_drawVertexIndices ) + { + bool multipleMeshes = m_meshes.size() > 1; + for ( int i = 0; i < (int)m_meshes.size(); ++i ) + { + // Welded engine vertices, which differ from the source vertices + const b3MeshData* data = m_meshes[i].data; + const b3Vec3* vertices = b3GetMeshVertices( data ); + for ( int v = 0; v < data->vertexCount; ++v ) + { + b3Pos point = b3OffsetPos( b3Pos_zero, vertices[v] ); + if ( multipleMeshes ) + { + DrawString3D( point, MakeColor( b3_colorAquamarine ), "m%d v%d", i, v ); + } + else + { + DrawString3D( point, MakeColor( b3_colorAquamarine ), "v%d", v ); + } + } + } + } + + DrawShape( m_transform ); + + float length = 0.3f * b3GetLengthUnitsPerMeter(); + for ( int i = 0; i < m_manifoldCount; ++i ) + { + b3Vec3 offset = {0.0f, 0.01f * i, 0.0f}; + const b3Manifold& manifold = m_manifolds[i]; + for ( int j = 0; j < manifold.pointCount; ++j ) + { + const b3ManifoldPoint& mp = manifold.points[j]; + if ( mp.separation > 0.01f * m_cullDistance ) + { + continue; + } + b3Pos point = b3OffsetPos( b3Pos_zero, mp.anchorA ); + point = b3OffsetPos( point, offset ); + Vec4 color = + mp.separation > 0.0f ? MakeColor( b3_colorWhite ) : MakeColor( b3_colorYellow ); + DrawArrow( point, point + length * manifold.normal, color ); + DrawPoint( point, 8.0f, color ); + DrawString3D( point, MakeColor( b3_colorWhite ), " %.1f mm", + 1000.0f * mp.separation ); + } + + DrawTextLine( " mesh %d normal (%.3f %.3f %.3f) points %d", m_manifoldMesh[i], + manifold.normal.x, manifold.normal.y, manifold.normal.z, + manifold.pointCount ); + } + + if ( m_drawTriangleManifolds ) + { + const char* featureNames[] = { "none", "tri face", "hull face", "edge1", "edge2", + "edge3", "vertex1", "vertex2", "vertex3" }; + b3Matrix3 rotation = b3MakeMatrixFromQuat( m_transform.q ); + + for ( int i = 0; i < m_triangleManifoldCount; ++i ) + { + const b3LocalManifold& manifold = m_triangleManifolds[i].manifold; + if ( manifold.pointCount == 0 ) + { + continue; + } + + b3Vec3 normal = b3MulMV( rotation, manifold.normal ); + for ( int j = 0; j < manifold.pointCount; ++j ) + { + b3Pos point = b3TransformWorldPoint( m_transform, manifold.points[j].point ); + DrawArrow( point, point + 0.5f * length * normal, + MakeColor( b3_colorOrange ) ); + } + + b3Pos point = b3TransformWorldPoint( m_transform, manifold.points[0].point ); + DrawString3D( point + 0.5f * length * normal, MakeColor( b3_colorOrange ), + " t%d %s", manifold.triangleIndex, featureNames[manifold.feature] ); + } + } + + DrawAxes( b3WorldTransform_identity, 0.25f ); + } + + bool HasSolverControls() const override + { + return false; + } + + bool HasProfile() const override + { + return false; + } + + bool DrawControls() override + { + const char* names[m_maxScenes]; + int count = b3MinInt( m_sceneCount, m_maxScenes ); + for ( int i = 0; i < count; ++i ) + { + names[i] = m_scenes[i].name; + } + + ImGui::PushItemWidth( 220.0f ); + int index = m_sceneIndex; + if ( ImGui::Combo( "Scene", &index, names, count ) ) + { + LoadScene( index ); + } + ImGui::PopItemWidth(); + + ImGui::Checkbox( "Ghost Handling", &m_handleGhosts ); + ImGui::Checkbox( "Speculative", &m_speculative ); + ImGui::Checkbox( "Triangle Manifolds", &m_drawTriangleManifolds ); + ImGui::Checkbox( "Triangle Indices", &m_drawTriangleIndices ); + ImGui::Checkbox( "Vertex Indices", &m_drawVertexIndices ); + ImGui::SliderFloat( "Cull Distance", &m_cullDistance, 0.0f, 200.0f, "%0.1f cm" ); + if ( ImGui::Button( "Reset Pose" ) ) + { + ResetPose(); + } + + DrawGizmoControls( &m_showGizmo, &m_gizmoLocal ); + + ImGui::TextColored( ImVec4( 1.0f, 0.0f, 0.0f, 1.0f ), "boundary" ); + ImGui::SameLine(); + ImGui::TextColored( ImVec4( 0.5f, 0.5f, 0.5f, 1.0f ), "flat" ); + ImGui::SameLine(); + ImGui::TextColored( ImVec4( 1.0f, 0.65f, 0.0f, 1.0f ), "convex" ); + ImGui::SameLine(); + ImGui::TextColored( ImVec4( 0.12f, 0.56f, 1.0f, 1.0f ), "concave" ); + return true; + } + + void Keyboard( int key, int action, int modifiers ) override + { + if ( key == KEY_G && action == ACTION_PRESS && modifiers == 0 ) + { + m_showGizmo = !m_showGizmo; + } + } + + void DrawSampleWindows() override + { + if ( m_showGizmo ) + { + DrawTransformGizmo( m_camera, &m_transform, m_gizmoLocal ); + } + } + + // The gizmo moves the shape, so keep the picking in Sample off + void MouseDown( b3Vec2 p, int button, int modifiers ) override + { + } + + struct TriangleQuery + { + b3Vec3 a, b, c; + }; + + const GhostScene* m_scenes; + int m_sceneCount; + int m_sceneIndex; + std::vector m_meshes; + + b3WorldTransform m_transform; + bool m_handleGhosts; + float m_cullDistance; + bool m_speculative; + bool m_drawTriangleManifolds; + bool m_drawTriangleIndices; + bool m_drawVertexIndices; + bool m_showGizmo; + bool m_gizmoLocal; + + b3Manifold m_manifolds[m_manifoldCapacity] = {}; + int m_manifoldMesh[m_manifoldCapacity] = {}; + int m_manifoldCount; + + TriangleQuery m_queryTriangles[m_triangleCapacity] = {}; + int m_queryIndices[m_triangleCapacity] = {}; + TriangleResult m_triangleManifolds[m_triangleCapacity] = {}; + int m_triangleManifoldCount; +}; + +// Two triangles sharing the edge x = 0, optionally folded along it +static void BuildGhostFold( std::vector& meshes, float crestHeight, float sideHeight ) +{ + float w = 2.0f, l = 2.0f; + GhostMesh mesh; + mesh.vertices = { { 0.0f, crestHeight, -w }, + { 0.0f, crestHeight, w }, + { -l, sideHeight, 0.0f }, + { l, sideHeight, 0.0f } }; + AddGhostTriangle( mesh, 0, 1, 2 ); + AddGhostTriangle( mesh, 1, 0, 3 ); + meshes.push_back( mesh ); +} + +static void BuildGhostFlat( std::vector& meshes ) +{ + BuildGhostFold( meshes, 0.0f, 0.0f ); +} + +// 15 degree slopes +static void BuildGhostCrest( std::vector& meshes ) +{ + BuildGhostFold( meshes, 2.0f * tanf( 15.0f * B3_PI / 180.0f ), 0.0f ); +} + +static void BuildGhostValley( std::vector& meshes ) +{ + BuildGhostFold( meshes, 0.0f, 2.0f * tanf( 15.0f * B3_PI / 180.0f ) ); +} + +// Rotated so the box corner arrives in the middle of a wedge +static void BuildGhostFan( std::vector& meshes ) +{ + GhostMesh mesh; + mesh.vertices.push_back( b3Vec3_zero ); + for ( int i = 0; i < 6; ++i ) + { + float angle = ( i + 0.5f ) * B3_PI / 3.0f; + mesh.vertices.push_back( { 2.0f * cosf( angle ), 0.0f, 2.0f * sinf( angle ) } ); + } + + for ( int i = 0; i < 6; ++i ) + { + AddGhostTriangle( mesh, 0, 1 + i, 1 + ( i + 1 ) % 6 ); + } + meshes.push_back( mesh ); +} + +// The right side is split at a vertex in the middle of the left triangle edge +static void BuildGhostTJunction( std::vector& meshes ) +{ + GhostMesh mesh; + mesh.vertices = { { 0.0f, 0.0f, -2.0f }, + { 0.0f, 0.0f, 2.0f }, + { -2.0f, 0.0f, 0.0f }, + { 2.0f, 0.0f, 0.0f }, + b3Vec3_zero }; + AddGhostTriangle( mesh, 0, 1, 2 ); + AddGhostTriangle( mesh, 4, 1, 3 ); + AddGhostTriangle( mesh, 0, 4, 3 ); + meshes.push_back( mesh ); +} + +// A 90 degree convex ridge along z, two cells along the ridge +static void BuildGhostRidge( std::vector& meshes ) +{ + GhostMesh mesh; + b3Vec2 profile[3] = { { -1.5f, -1.5f }, { 0.0f, 0.0f }, { 1.5f, -1.5f } }; + b3Vec3 facings[2] = { { -1.0f, 1.0f, 0.0f }, { 1.0f, 1.0f, 0.0f } }; + for ( int k = 0; k < 3; ++k ) + { + for ( int i = 0; i < 3; ++i ) + { + mesh.vertices.push_back( { profile[i].x, profile[i].y, -1.0f + k } ); + } + } + + for ( int k = 0; k < 2; ++k ) + { + for ( int i = 0; i < 2; ++i ) + { + int a = 3 * k + i; + int b = a + 1; + int c = a + 4; + int d = a + 3; + AddGhostTriangle( mesh, a, b, c, facings[i] ); + AddGhostTriangle( mesh, a, c, d, facings[i] ); + } + } + meshes.push_back( mesh ); +} + +// A welded strip with an open boundary along x = 0, like a PEEL slab lying on other ground +static void BuildGhostStrip( std::vector& meshes ) +{ + GhostMesh mesh; + for ( int row = 0; row < 5; ++row ) + { + for ( int column = 0; column < 3; ++column ) + { + mesh.vertices.push_back( { 0.5f * column, 0.0f, -1.0f + 0.5f * row } ); + } + } + + for ( int row = 0; row < 4; ++row ) + { + for ( int column = 0; column < 2; ++column ) + { + int a = 3 * row + column; + int b = a + 1; + int c = a + 4; + int d = a + 3; + if ( ( row + column ) & 1 ) + { + AddGhostTriangle( mesh, a, d, b ); + AddGhostTriangle( mesh, d, c, b ); + } + else + { + AddGhostTriangle( mesh, a, d, c ); + AddGhostTriangle( mesh, c, b, a ); + } + } + } + meshes.push_back( mesh ); +} + +// s&box units are inches. Beam top at y = 0 for x > 0, chamfer dropping 1 inch over 1.5 inch for x +// < 0, 8 inch cells along z with the chamfer quads split like the s&box sample. A side depth in inches +// adds a vertical face below the chamfer, so the chamfer bottom edge is convex instead of a boundary. +static void BuildChamferBeam( std::vector& meshes, float sideDepth ) +{ + float inch = 0.0254f; + bool hasSide = sideDepth > 0.0f; + int rowSize = hasSide ? 4 : 3; + GhostMesh mesh; + for ( int k = 0; k <= 4; ++k ) + { + float z = 4.0f * ( -16.0f + 8.0f * k ) * inch; + mesh.vertices.push_back( { -1.5f * inch, -1.0f * inch, z } ); + mesh.vertices.push_back( { 0.0f, 0.0f, z } ); + mesh.vertices.push_back( { 48.0f * inch, 0.0f, z } ); + if ( hasSide ) + { + mesh.vertices.push_back( { -1.5f * inch, ( -1.0f - sideDepth ) * inch, z } ); + } + } + + for ( int k = 0; k < 4; ++k ) + { + int low0 = rowSize * k, top0 = low0 + 1, far0 = low0 + 2; + int low1 = low0 + rowSize, top1 = top0 + rowSize, far1 = far0 + rowSize; + AddGhostTriangle( mesh, low0, low1, top1 ); + AddGhostTriangle( mesh, low0, top1, top0 ); + AddGhostTriangle( mesh, top0, top1, far1 ); + AddGhostTriangle( mesh, top0, far1, far0 ); + + if ( hasSide ) + { + int side0 = low0 + 3, side1 = low1 + 3; + b3Vec3 facing = { -1.0f, 0.0f, 0.0f }; + AddGhostTriangle( mesh, side0, side1, low1, facing ); + AddGhostTriangle( mesh, side0, low1, low0, facing ); + } + } + meshes.push_back( mesh ); +} + +static void BuildGhostChamfer( std::vector& meshes ) +{ + BuildChamferBeam( meshes, 0.0f ); +} + +static void BuildGhostChamferExtended( std::vector& meshes ) +{ + BuildChamferBeam( meshes, 6.0f ); +} + +// A conveyor side rail running along x, from the Conveyor Circuit sample. The upper half of the inner +// wall facing -z and the 8 cm rail top meet at a convex edge, two cells along x so the vertex at x = 0 +// is interior. Quad diagonals alternate as in the conveyor mesh. +static void BuildGhostRail( std::vector& meshes ) +{ + GhostMesh mesh; + b3Vec2 section[3] = { { 0.0f, 0.175f }, { 0.0f, 0.35f }, { 0.08f, 0.35f } }; + for ( int row = 0; row < 3; ++row ) + { + for ( int column = 0; column < 3; ++column ) + { + mesh.vertices.push_back( { -0.25f + 0.25f * column, section[row].y, section[row].x } ); + } + } + + b3Vec3 facings[2] = { { 0.0f, 0.0f, -1.0f }, b3Vec3_axisY }; + for ( int row = 0; row < 2; ++row ) + { + for ( int column = 0; column < 2; ++column ) + { + int a = 3 * row + column; + int b = a + 1; + int c = a + 4; + int d = a + 3; + if ( ( row + column ) & 1 ) + { + AddGhostTriangle( mesh, a, d, b, facings[row] ); + AddGhostTriangle( mesh, d, c, b, facings[row] ); + } + else + { + AddGhostTriangle( mesh, a, d, c, facings[row] ); + AddGhostTriangle( mesh, c, b, a, facings[row] ); + } + } + } + meshes.push_back( mesh ); +} + +// A 35 degree ridge along x that runs into a wall rising toward x = 1, so the ridge end at the origin +// is a saddle vertex, convex across the ridge and concave along it. The ridge sags 4 mm toward x = -2. +static void BuildGhostSaddleRidge( std::vector& meshes ) +{ + GhostMesh mesh; + float xs[3] = { -2.0f, 0.0f, 1.0f }; + float crests[3] = { -0.004f, 0.0f, 0.3f }; + float sides[3] = { -0.7f, -0.7f, -0.4f }; + for ( int i = 0; i < 3; ++i ) + { + mesh.vertices.push_back( { xs[i], sides[i], -1.0f } ); + mesh.vertices.push_back( { xs[i], crests[i], 0.0f } ); + mesh.vertices.push_back( { xs[i], sides[i], 1.0f } ); + } + + for ( int i = 0; i < 2; ++i ) + { + for ( int k = 0; k < 2; ++k ) + { + int a = 3 * i + k; + int b = a + 1; + int c = a + 3; + int d = a + 4; + AddGhostTriangle( mesh, a, b, c ); + AddGhostTriangle( mesh, c, b, d ); + } + } + meshes.push_back( mesh ); +} + +// The ghost oracle wave, 3 m square with 25 cm cells +static void BuildGhostWave( std::vector& meshes ) +{ + b3MeshData* data = CreateOracleMesh( e_oracleWave ); + const b3Vec3* vertices = b3GetMeshVertices( data ); + const b3MeshTriangle* triangles = b3GetMeshTriangles( data ); + + GhostMesh mesh; + mesh.vertices.assign( vertices, vertices + data->vertexCount ); + for ( int i = 0; i < data->triangleCount; ++i ) + { + mesh.indices.push_back( triangles[i].index1 ); + mesh.indices.push_back( triangles[i].index2 ); + mesh.indices.push_back( triangles[i].index3 ); + } + + b3DestroyMesh( data ); + meshes.push_back( mesh ); +} + +static const float s_inch = 0.0254f; + +static const GhostScene s_hullGhostScenes[] = { + { + "Flat edge, side face", + "box 1 cm deep, leading face 5 mm past the shared edge, the right triangle picks the box " + "side face", + "flat edge, the hull face snaps to the triangle normal", + BuildGhostFlat, + { 0.5f, 0.5f, 0.5f }, + { -0.495f, 0.49f, 0.0f }, + { 0.0f, 0.0f, 0.0f }, + true, + }, + { + "Flat edge, corner", + "box yawed 45 degrees, a corner 1 mm deep stops 1 mm short of the shared edge, edge pair " + "normal", + "flat edge, snap to the triangle face, which has no points here", + BuildGhostFlat, + { 0.5f, 0.5f, 0.5f }, + { -0.708f, 0.499f, 0.0f }, + { 0.0f, 45.0f, 0.0f }, + true, + }, + { + "Flat vertex", + "the corner stops 3 mm short of the fan center, triangles ahead see the center vertex and " + "the box side faces", + "flat vertex and edges, snap", + BuildGhostFan, + { 0.5f, 0.5f, 0.5f }, + { -0.71f, 0.499f, 0.0f }, + { 0.0f, 45.0f, 0.0f }, + true, + }, + { + "Convex edge", + "box on the right slope tilted 8 degrees, the left triangle crest edge gives a normal past " + "the right slope", + "convex edge, the normal is outside the edge cone so the left triangle skips and the right " + "one owns the contact", + BuildGhostCrest, + { 0.5f, 0.5f, 0.5f }, + { 0.57f, 0.959f, 0.0f }, + { 0.0f, 0.0f, -8.0f }, + true, + }, + { + "Concave edge", + "box on the left slope tilted 20 degrees, its corner in the fold, the right triangle picks " + "the box side face", + "concave edge, snap to the triangle face", + BuildGhostValley, + { 0.5f, 0.5f, 0.5f }, + { -0.3f, 0.64f, 0.0f }, + { 0.0f, 0.0f, -20.0f }, + true, + }, + { + "Concave edge, deep", + "box on the left slope tilted 18 degrees and 2 cm deep, the right triangle picks the box side " + "face 11 mm deep", + "concave edge, snap to the triangle face, which has no points here, so the left triangle owns " + "the contact", + BuildGhostValley, + { 0.5f, 0.5f, 0.5f }, + { -0.314f, 0.61f, 0.0f }, + { 0.0f, 0.0f, -18.0f }, + true, + }, + { + "Ridge, deep", + "box corner jammed on a 90 degree ridge, the apex is 3 cm inside the box, 28 mm into the left " + "slope and 34 mm into the right", + "deep, the right triangle picks an edge pair, a convex skip that snaps when deep, and its face " + "contact is empty. The left triangles snap to their own face, which pushes the box off the apex", + BuildGhostRidge, + { 0.5f, 0.5f, 0.5f }, + { -0.564f, 0.579f, -0.016f }, + { -32.0f, -20.0f, 8.0f }, + true, + }, + { + "T-junction", + "a vertex in the middle of the left triangle edge, the left edge matches neither right edge " + "so all three are boundary edges", + "unsupported, the mesh must be welded. Boundary edges are 90 degree convex edges, so the " + "T-junction vertex admits the box side face normal", + BuildGhostTJunction, + { 0.5f, 0.5f, 0.5f }, + { -0.708f, 0.499f, 0.3f }, + { 0.0f, 45.0f, 0.0f }, + true, + }, + { + "Open boundary", + "box hanging over the open edge of a welded strip, yawed 5 degrees and 1 cm deep, its leading " + "face 5 mm past the row edge at the boundary vertex", + "boundary edge, a 90 degree convex edge. The normal leans over the next row triangle at the " + "boundary vertex, so it snaps to the triangle face", + BuildGhostStrip, + { 0.5f, 0.5f, 0.5f }, + { -0.25f, 0.49f, 0.019f }, + { 0.0f, 5.0f, 0.0f }, + true, + }, + { + "Chamfer", + "none without handling, SAT picks the box bottom for the chamfer triangles, 0.07 inch deep", + "causes the ghost, the deepest point is on a flat chamfer vertex or diagonal next to the " + "convex top edge so the " + "hull face snaps to the 34 degree chamfer normal (ghost plan step 2c)", + BuildGhostChamfer, + { 16.0f * s_inch, 36.0f * s_inch, 16.0f * s_inch }, + { 4.0f * s_inch, 36.0f * s_inch - 0.073f * s_inch, 3.0f * s_inch }, + { 0.0f, 0.0f, 0.0f }, + false, + }, + { + "Chamfer extended", + "the chamfer scene with 6 inches of vertical beam side below the chamfer, 0.07 inch deep", + "the chamfer bottom edge is convex instead of a boundary", + BuildGhostChamferExtended, + { 16.0f * s_inch, 36.0f * s_inch, 16.0f * s_inch }, + { 4.0f * s_inch, 36.0f * s_inch - 0.073f * s_inch, 3.0f * s_inch }, + { 0.0f, 0.0f, 0.0f }, + false, + }, + { + "Rail edge", + "box pitched 60 degrees across the rail, its face cuts the convex rail edge 7 cm past the x = 0 " + "vertex, the real contact is edge on edge and perpendicular to the rail", + "the edge pair is shallower than the hull face and the hull face normal crosses the rail edge, so " + "SAT takes the edge pair. Without that the hull face is admitted at a rail edge vertex and leans 60 " + "degrees along the rail.", + BuildGhostRail, + { 0.4f, 0.2f, 0.4f }, + { 0.1f, 0.77f, -0.15f }, + { -15.0f, 0.0f, 60.0f }, + true, + }, + { + "Saddle ridge", + "box resting flat on a ridge, its footprint covers the saddle where the ridge meets a wall, the " + "saddle is 4 mm deep and the ridge under the box sags to 2 mm", + "the deepest points within linear slop span the ridge edge, which admits the up normal. Taking " + "only the deepest point picks the saddle vertex, whose cone rejects up, and snaps to the 35 degree " + "slope", + BuildGhostSaddleRidge, + { 0.46f, 0.1f, 0.5f }, + { -0.44f, 0.096f, 0.0f }, + { 0.0f, 0.0f, 0.0f }, + true, + }, + { + "Open boundary, snapped neighbors", + "box rolled 44 degrees over the open edge of the strip, its low edge crosses the boundary 1.6 mm " + "up. Its own triangle gives a face contact and the neighbor triangles pick a box side face", + "the neighbors snap to the strip normal and join the face cluster with points 19 to 21 cm up the " + "box side, so the cluster reduction keeps those over the touching point unless the deepest point " + "is put back", + BuildGhostStrip, + { 0.145f, 0.225f, 0.155f }, + { -0.071f, 0.26f, 0.023f }, + { 5.0f, -23.0f, 44.0f }, + true, + }, + { + "Open boundary, rolled", + "box rolled 40 degrees over the open edge of the strip, its low edge 4 mm from the boundary. The " + "edge axis wins SAT by 0.1 mm but the box bottom face is kept for its points, leaning 12 degrees " + "along the boundary edge", + "the leaning face snaps to the strip normal, whose clip reaches 29 cm up the box face. A boundary " + "edge has no neighbor, so the GJK contact at the edge replaces the snapped clip with one point " + "and the edge normal", + BuildGhostStrip, + { 0.2f, 0.1f, 0.2f }, + { 0.0f, 0.184f, 0.25f }, + { -15.0f, 0.0f, 40.0f }, + true, + }, + { + "Rail end", + "box rolled 30 degrees against the open end of the rail, its corner edge 4 mm from the wall end " + "edge, SAT picks a box face whose normal is not perpendicular to that edge", + "the face normal fails the edge test and snaps to the wall face, whose clip is empty beside the " + "rail. A boundary edge has no neighbor to take the contact, so the GJK normal at the edge " + "replaces it", + BuildGhostRail, + { 0.2f, 0.2f, 0.2f }, + { 0.527f, 0.3f, 0.0f }, + { 0.0f, 0.0f, -30.0f }, + true, + }, + { + "Wave, resting cull", + "box on a wave resting within 1 degree of one triangle 12 mm away, the closest triangles are 7 mm " + "away, tilted about 5 degrees and below the lowest box corner along the resting normal", + "the closest triangles snap to their own faces. The resting cull only drops triangles farther " + "than the resting contact, otherwise these drop and the only contact left is 5 mm too far", + BuildGhostWave, + { 0.35f, 0.17f, 0.15f }, + { 0.532f, 0.198f, -1.175f }, + { -3.0f, 77.0f, -6.0f }, + true, + }, +}; + +class HullGhostManifold : public GhostManifold +{ +public: + explicit HullGhostManifold( SampleContext* context ) + : GhostManifold( context ) + { + m_box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + SetScenes( s_hullGhostScenes, IM_ARRAYSIZE( s_hullGhostScenes ) ); + } + + void CreateShape( const GhostScene& scene ) override + { + m_box = b3MakeBoxHull( scene.extents.x, scene.extents.y, scene.extents.z ); + } + + int CollideMesh( b3Manifold* manifolds, int capacity, const b3Mesh* mesh, + b3Transform transformBtoA, bool speculative, bool handleGhosts ) override + { + return b3CollideMeshAndHull( manifolds, capacity, mesh, &m_box.base, transformBtoA, + speculative, handleGhosts ); + } + + void CollideTriangle( b3LocalManifold* manifold, int capacity, const b3Vec3* triangle, + bool speculative ) override + { + b3SATCache cache = {}; + b3CollideTriangleAndHull( manifold, capacity, triangle[0], triangle[1], triangle[2], + &m_box.base, &cache, speculative ); + } + + void DrawShape( b3WorldTransform transform ) override + { + DrawHull( transform, &m_box.base, MakeColor( b3_colorGreen ) ); + } + + b3AABB ComputeShapeBounds( b3Transform transform ) override + { + return b3ComputeHullAABB( &m_box.base, transform ); + } + + static Sample* Create( SampleContext* context ) + { + return new HullGhostManifold( context ); + } + + b3BoxHull m_box; +}; + +static int sampleHullGhostManifold = + RegisterSample( "Manifold", "Hull Ghost Manifold", HullGhostManifold::Create ); + +// A flat floor for x > 0 meeting a 10 degree ramp at the concave edge x = 0 +static void BuildGhostRamp( std::vector& meshes ) +{ + GhostMesh mesh; + mesh.vertices = { { 0.0f, 0.0f, -2.0f }, + { 0.0f, 0.0f, 2.0f }, + { -2.0f, 2.0f * tanf( 10.0f * B3_PI / 180.0f ), 0.0f }, + { 2.0f, 0.0f, 0.0f } }; + AddGhostTriangle( mesh, 0, 1, 2 ); + AddGhostTriangle( mesh, 1, 0, 3 ); + meshes.push_back( mesh ); +} + +// A hub whose six spokes alternate 8 degree ridges and 32 degree gullies, so every fan triangle has a +// concave edge and the mesh flags the hub concave. Every spoke still runs downhill, so the hub is the +// closest point for a body resting on it. +static void BuildGhostSaddle( std::vector& meshes ) +{ + float radius = 1.5f; + float toRadians = B3_PI / 180.0f; + float hubHeight = radius * tanf( 32.0f * toRadians ); + + GhostMesh mesh; + mesh.vertices.push_back( { 0.0f, hubHeight, 0.0f } ); + for ( int i = 0; i < 6; ++i ) + { + float angle = i * B3_PI / 3.0f; + float drop = ( i & 1 ) ? 32.0f : 8.0f; + mesh.vertices.push_back( + { radius * cosf( angle ), hubHeight - radius * tanf( drop * toRadians ), radius * sinf( angle ) } ); + } + + for ( int i = 0; i < 6; ++i ) + { + AddGhostTriangle( mesh, 0, 1 + i, 1 + ( i + 1 ) % 6 ); + } + meshes.push_back( mesh ); +} + +// Sphere radius in the x extent +static const GhostScene s_sphereGhostScenes[] = { + { + "Flat edge", + "sphere 1 mm deep over the left triangle, 5 cm from the shared edge, the right triangle sees the edge " + "and tilts its normal 5.7 degrees", + "flat edge, the normal leans over the left triangle and a sphere off the face cannot snap, so the " + "right triangle drops it", + BuildGhostFlat, + { 0.5f, 0.5f, 0.5f }, + { 0.0f, 0.499f, 0.0f }, + { 0.0f, 0.0f, 0.0f }, + true, + }, + { + "Flat vertex", + "sphere 1 mm deep, 5 cm from the fan center inside one wedge, the other triangles see the center vertex " + "or a spoke and tilt their normals 5.7 degrees", + "flat vertex and edges, the normal is not the face normal, so they drop it", + BuildGhostFan, + { 0.5f, 0.5f, 0.5f }, + { 0.0f, 0.499f, 0.0f }, + { 0.0f, 0.0f, 0.0f }, + true, + }, + { + "Convex edge", + "sphere 1 mm deep on the right slope 5 cm below the crest, the left triangle sees the crest edge and " + "gives a normal 5.7 degrees past the right slope", + "convex edge, the normal is outside the edge cone so the left triangle skips and the right one owns " + "the contact", + BuildGhostCrest, + { 0.5f, 0.5f, 0.5f }, + { 0.1774f, 1.005f, 0.0f }, + { 0.0f, 0.0f, 0.0f }, + true, + }, + { + "Concave edge", + "sphere 1 mm deep on the floor 10 cm from a 10 degree ramp, the ramp triangle sees the crease edge 9 mm " + "away and tilts its normal 11.3 degrees away from the ramp", + "concave edge, the normal is neither face normal so it snaps, and a sphere off the face cannot snap, so " + "the ramp triangle drops it", + BuildGhostRamp, + { 0.5f, 0.5f, 0.5f }, + { 0.1f, 0.499f, 0.0f }, + { 0.0f, 0.0f, 0.0f }, + true, + }, + { + "Saddle vertex", + "none, sphere 1 mm deep on the hub. The spokes alternate 8 degree ridges and 32 degree gullies, so " + "every fan triangle has a concave edge, yet the hub is the closest point", + "the edge tests of every triangle reject the vertical normal across its gully. The normal is in the hub " + "fan cone, so every triangle keeps it", + BuildGhostSaddle, + { 0.5f, 0.5f, 0.5f }, + { 0.0f, 1.4363f, 0.0f }, + { 0.0f, 0.0f, 0.0f }, + true, + }, +}; + +class SphereGhostManifold : public GhostManifold +{ +public: + explicit SphereGhostManifold( SampleContext* context ) + : GhostManifold( context ) + { + m_sphere = { b3Vec3_zero, 0.5f }; + SetScenes( s_sphereGhostScenes, IM_ARRAYSIZE( s_sphereGhostScenes ) ); + } + + void CreateShape( const GhostScene& scene ) override + { + m_sphere = { b3Vec3_zero, scene.extents.x }; + } + + int CollideMesh( b3Manifold* manifolds, int capacity, const b3Mesh* mesh, b3Transform transformBtoA, + bool speculative, bool handleGhosts ) override + { + return b3CollideMeshAndSphere( manifolds, capacity, mesh, &m_sphere, transformBtoA, speculative, + handleGhosts ); + } + + void CollideTriangle( b3LocalManifold* manifold, int capacity, const b3Vec3* triangle, bool ) override + { + b3CollideTriangleAndSphere( manifold, capacity, triangle, &m_sphere ); + } + + void DrawShape( b3WorldTransform transform ) override + { + DrawWireSphere( transform, &m_sphere, 32, MakeColor( b3_colorGreen ) ); + } + + b3AABB ComputeShapeBounds( b3Transform transform ) override + { + b3Vec3 center = b3TransformPoint( transform, m_sphere.center ); + b3Vec3 extent = { m_sphere.radius, m_sphere.radius, m_sphere.radius }; + return { b3Sub( center, extent ), b3Add( center, extent ) }; + } + + static Sample* Create( SampleContext* context ) + { + return new SphereGhostManifold( context ); + } + + b3Sphere m_sphere; +}; + +static int sampleSphereGhostManifold = + RegisterSample( "Manifold", "Sphere Ghost Manifold", SphereGhostManifold::Create ); + +/// Separated poses judged against the union of the triangles, see shared/ghost_oracle.h. Pose N of a scene +// is the pose N the MeshGhostOracle unit test runs, so a failure index from the test replays here. +class GhostOracle : public Sample +{ +public: + explicit GhostOracle( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 30.0f, 25.0f, 5.0f, { 0.0f, 0.2f, 0.0f } ); + } + + m_scene = e_oracleFlat; + m_poseIndex = 0; + m_handleGhosts = true; + m_mesh = nullptr; + m_valid = false; + m_scanned = false; + m_searching = false; + m_status[0] = 0; + LoadScene( e_oracleFlat ); + } + + ~GhostOracle() override + { + if ( m_mesh != nullptr ) + { + b3DestroyMesh( m_mesh ); + } + } + + void LoadScene( int scene ) + { + if ( m_mesh != nullptr ) + { + b3DestroyMesh( m_mesh ); + } + + m_scene = scene; + m_mesh = CreateOracleMesh( (OracleScene)scene ); + m_scanned = false; + m_searching = false; + Evaluate(); + } + + void Evaluate() + { + m_valid = EvaluateOraclePose( &m_pose, m_mesh, (OracleScene)m_scene, m_poseIndex ); + } + + const OracleResult& Shown() const + { + return m_handleGhosts ? m_pose.handled : m_pose.raw; + } + + void StartSearch( bool ghost ) + { + m_searching = true; + m_searchGhost = ghost; + m_searchStart = m_poseIndex; + m_searchIndex = m_poseIndex + 1; + } + + // A pose costs up to 21 GJK sweeps of the mesh and most searches find nothing, so the search takes + // a time slice per frame + void UpdateSearch() + { + uint64_t ticks = b3GetTicks(); + int end = m_searchStart + m_searchCount; + while ( m_searchIndex <= end && b3GetMilliseconds( ticks ) < 8.0f ) + { + int index = m_searchIndex; + m_searchIndex += 1; + + if ( EvaluateOraclePose( &m_searchPose, m_mesh, (OracleScene)m_scene, index ) == false ) + { + continue; + } + + const OracleResult& result = m_handleGhosts ? m_searchPose.handled : m_searchPose.raw; + bool found = m_searchGhost ? result.ghostCount > 0 : IsOracleCullingMiss( &m_searchPose ); + if ( found ) + { + m_pose = m_searchPose; + m_poseIndex = index; + m_valid = true; + m_searching = false; + snprintf( m_status, sizeof( m_status ), "found pose %d", index ); + return; + } + } + + if ( m_searchIndex > end ) + { + m_searching = false; + snprintf( m_status, sizeof( m_status ), "none in poses %d to %d", m_searchStart + 1, end ); + } + else + { + snprintf( m_status, sizeof( m_status ), "searching poses %d to %d, at %d", m_searchStart + 1, end, m_searchIndex ); + } + } + + void CancelSearch() + { + m_searching = false; + m_status[0] = 0; + } + + void Step() override + { + if ( m_searching ) + { + UpdateSearch(); + } + } + + bool FocusBounds( b3AABB* bounds ) override + { + if ( m_valid == false ) + { + return false; + } + + *bounds = b3ComputeHullAABB( &m_pose.box.base, m_pose.transform ); + return true; + } + + void Render() override + { + const OracleResult& result = Shown(); + const OracleReference& reference = m_pose.reference; + + DrawTextLine( "%s, pose %d, ghost handling %s", g_oracleSceneNames[m_scene], m_poseIndex, + m_handleGhosts ? "on" : "off" ); + if ( m_valid == false ) + { + DrawTextLine( "no separated pose at this index" ); + } + else + { + DrawTextLine( "distance %.2f mm", 1000.0f * reference.distance ); + DrawTextLine( "ghost manifolds %d", result.ghostCount ); + if ( result.missed ) + { + bool cullingMiss = m_handleGhosts && IsOracleCullingMiss( &m_pose ); + DrawTextLine( "closest pair missed%s", cullingMiss ? ", by ghost handling" : "" ); + } + } + + if ( m_scanned ) + { + DrawTextLine( + "scan of %d poses: %d separated, ghost poses %d unhandled and %d handled, %d misses by handling", + ORACLE_SCAN_POSES, m_scan.validCount, m_scan.rawGhostPoses, m_scan.ghostPoses, m_scan.missedPoses ); + } + + if ( m_status[0] != 0 ) + { + DrawTextLine( "%s", m_status ); + } + + const b3Vec3* vertices = b3GetMeshVertices( m_mesh ); + const b3MeshTriangle* triangles = b3GetMeshTriangles( m_mesh ); + for ( int i = 0; i < m_mesh->triangleCount; ++i ) + { + b3Vec3 a = vertices[triangles[i].index1]; + b3Vec3 b = vertices[triangles[i].index2]; + b3Vec3 c = vertices[triangles[i].index3]; + DrawTriangle( b3WorldTransform_identity, a, b, c, MakeColor( b3_colorDarkSlateGray ) ); + + b3Vec3 corners[3] = { a, b, c }; + for ( int k = 0; k < 3; ++k ) + { + DrawLine( b3OffsetPos( b3Pos_zero, corners[k] ), b3OffsetPos( b3Pos_zero, corners[k < 2 ? k + 1 : 0] ), + MakeColor( b3_colorGray ) ); + } + } + + DrawAxes( b3WorldTransform_identity, 0.25f ); + + if ( m_valid == false ) + { + return; + } + + b3WorldTransform transform = { b3OffsetPos( b3Pos_zero, m_pose.transform.p ), m_pose.transform.q }; + DrawHull( transform, &m_pose.box.base, MakeColor( b3_colorGreen ) ); + + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + for ( int i = 0; i < reference.pairCount; ++i ) + { + const OraclePair& pair = reference.pairs[i]; + if ( pair.distance > speculativeDistance ) + { + continue; + } + + b3Pos meshPoint = b3OffsetPos( b3Pos_zero, pair.meshPoint ); + b3Pos hullPoint = b3OffsetPos( b3Pos_zero, pair.hullPoint ); + if ( pair.owned ) + { + DrawLineEx( meshPoint, hullPoint, MakeColor( m_ownedColor ), 3.0f, OVERLAY_THICKNESS_PIXELS, + OVERLAY_OCCLUSION_DIM ); + DrawPoint( meshPoint, 8.0f, MakeColor( m_ownedColor ) ); + } + else + { + DrawLine( meshPoint, hullPoint, MakeColor( m_dominatedColor ) ); + } + } + + DrawPoint( b3OffsetPos( b3Pos_zero, reference.closestMeshPoint ), 12.0f, MakeColor( b3_colorWhite ) ); + + float length = 0.15f * b3GetLengthUnitsPerMeter(); + for ( int i = 0; i < result.pointCount; ++i ) + { + const OraclePoint& point = result.points[i]; + b3Pos origin = b3OffsetPos( b3Pos_zero, point.meshPoint ); + Vec4 color = MakeColor( m_verdictColors[point.verdict] ); + DrawArrow( origin, b3OffsetPos( origin, b3MulSV( length, point.normal ) ), color ); + DrawPoint( origin, 8.0f, color ); + DrawString3D( origin, color, " t%d %.1f mm", point.triangleIndex, 1000.0f * point.separation ); + } + } + + bool HasSolverControls() const override + { + return false; + } + + bool HasProfile() const override + { + return false; + } + + static void Legend( b3HexColor hexColor, const char* label ) + { + Vec4 color = MakeColor( hexColor ); + ImGui::TextColored( ImVec4( color.x, color.y, color.z, 1.0f ), "%s", label ); + } + + bool DrawControls() override + { + ImGui::PushItemWidth( 160.0f ); + int scene = m_scene; + if ( ImGui::Combo( "Scene", &scene, g_oracleSceneNames, e_oracleSceneCount ) ) + { + m_status[0] = 0; + LoadScene( scene ); + } + + if ( ImGui::InputInt( "Pose", &m_poseIndex ) ) + { + m_poseIndex = b3MaxInt( m_poseIndex, 0 ); + CancelSearch(); + Evaluate(); + } + ImGui::PopItemWidth(); + + // The search judges the shown result, so it does not survive a switch + if ( ImGui::Checkbox( "Ghost Handling", &m_handleGhosts ) ) + { + CancelSearch(); + } + + if ( m_searching ) + { + if ( ImGui::Button( "Cancel Search" ) ) + { + CancelSearch(); + } + } + else + { + if ( ImGui::Button( "Next Ghost" ) ) + { + StartSearch( true ); + } + ImGui::SameLine(); + if ( ImGui::Button( "Next Miss" ) ) + { + StartSearch( false ); + } + } + + if ( ImGui::Button( "Scan" ) ) + { + m_scan = ScanGhostOracle( m_mesh, (OracleScene)m_scene, ORACLE_SCAN_POSES ); + m_scanned = true; + } + ImGui::SameLine(); + if ( ImGui::Button( "Frame" ) ) + { + FrameSelection( m_context ); + } + + Legend( m_ownedColor, "union contact" ); + ImGui::SameLine(); + Legend( m_dominatedColor, "dominated pair" ); + Legend( m_verdictColors[e_oracleFace], "face" ); + ImGui::SameLine(); + Legend( m_verdictColors[e_oracleFeature], "edge or vertex" ); + ImGui::SameLine(); + Legend( m_verdictColors[e_oracleShared], "shared" ); + ImGui::SameLine(); + Legend( m_verdictColors[e_oracleGhost], "ghost" ); + return true; + } + + static Sample* Create( SampleContext* context ) + { + return new GhostOracle( context ); + } + + static constexpr int m_searchCount = 5000; + static constexpr b3HexColor m_ownedColor = b3_colorLimeGreen; + static constexpr b3HexColor m_dominatedColor = b3_colorDimGray; + + // Indexed by OracleVerdict + static constexpr b3HexColor m_verdictColors[4] = { b3_colorDodgerBlue, b3_colorCyan, b3_colorSilver, b3_colorRed }; + + b3MeshData* m_mesh; + int m_scene; + int m_poseIndex; + bool m_handleGhosts; + bool m_valid; + OraclePose m_pose; + + bool m_scanned; + OracleScan m_scan; + + bool m_searching; + bool m_searchGhost; + int m_searchStart; + int m_searchIndex; + OraclePose m_searchPose; + char m_status[128]; +}; + +static int sampleGhostOracle = RegisterSample( "Manifold", "Ghost Oracle", GhostOracle::Create ); diff --git a/samples/sample_mesh.cpp b/samples/sample_mesh.cpp index 9438735..983d802 100644 --- a/samples/sample_mesh.cpp +++ b/samples/sample_mesh.cpp @@ -10,9 +10,13 @@ #include "box3d/box3d.h" +#include +#include #include #include #include +#include +#include enum class ShapeType { @@ -1621,3 +1625,4 @@ public: }; static int sampleHollowBox = RegisterSample( "Mesh", "Hollow Box", HollowBox::Create ); + diff --git a/shared/CMakeLists.txt b/shared/CMakeLists.txt index ff640b4..f5c323f 100644 --- a/shared/CMakeLists.txt +++ b/shared/CMakeLists.txt @@ -5,8 +5,14 @@ set(BOX3D_SHARED_FILES benchmarks.h determinism.c determinism.h + ghost_collisions.c + ghost_collisions.h + ghost_oracle.c + ghost_oracle.h human.c human.h + mesh_drop.c + mesh_drop.h overflow_color.c overflow_color.h stability.c diff --git a/shared/ghost_collisions.c b/shared/ghost_collisions.c new file mode 100644 index 0000000..226ccd3 --- /dev/null +++ b/shared/ghost_collisions.c @@ -0,0 +1,867 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "ghost_collisions.h" + +#include "box3d/box3d.h" + +#include +#include +#include +#include + +#define GHOST_HALF_LENGTH 20.0f +#define GHOST_HALF_WIDTH 3.0f +#define GHOST_START_X -16.0f + +// The rail deflects the body sideways and the floor is frictionless, so a narrow floor ends the run off +// the side instead of at the far end +#define GHOST_RAIL_HALF_LENGTH ( 0.5f * GHOST_HALF_LENGTH ) +#define GHOST_RAIL_HALF_WIDTH ( 2.0f * GHOST_HALF_WIDTH ) + +// A run takes about 144 steps, clear of 150 where the 900 step scan would flip between 5 and 6 runs +#define GHOST_RAIL_START_X -5.3f + +// A 34 degree chamfer like the s&box beams +#define GHOST_CHAMFER_WIDTH 0.05f +#define GHOST_CHAMFER_DROP ( GHOST_CHAMFER_WIDTH / 1.5f ) + +#define GHOST_RAIL_SKIRT 0.4f + +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", "Cliff", "Crest", "Bumps", "Internal", "Chamfer", "Rail", +}; + +const char* g_ghostBodyNames[e_ghostBodyCount] = { "Box", "H Capsule", "V Capsule", "Sphere", "Cylinder" }; + +const char* g_ghostPeelPresetNames[GHOST_PEEL_PRESET_COUNT] = { + "4.1:-55", + "4.1:-54", + "4.01:-55", + "16.01:-54", +}; + +GhostSettings GetDefaultGhostSettings( void ) +{ + GhostSettings settings = { + .scene = e_ghostFlat, + .body = e_ghostBox, + .peelPreset = 0, + .mirror = false, + .heightField = 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 ); +} + +// The rail cross section is a profile in zy extruded along x. Each quad side is its edge direction along +// the section crossed with x, which is the outward normal for a section traversed with air on the left. +static void AddRail( MeshBuilder* builder, const b3Vec2* section, int count, float halfLength, float cellSize, bool mirror ) +{ + float sign = mirror ? -1.0f : 1.0f; + int xCount = b3MaxInt( 1, (int)ceilf( 2.0f * halfLength / cellSize - 0.01f ) ); + int stride = xCount + 1; + + int pointCount = 1; + for ( int k = 0; k + 1 < count; ++k ) + { + float dz = section[k + 1].x - section[k].x; + float dy = section[k + 1].y - section[k].y; + pointCount += b3MaxInt( 1, (int)ceilf( sqrtf( dz * dz + dy * dy ) / cellSize - 0.01f ) ); + } + + builder->vertices = realloc( builder->vertices, pointCount * stride * sizeof( b3Vec3 ) ); + int index = 0; + for ( int k = 0; k < count; ++k ) + { + int n = 1; + if ( k + 1 < count ) + { + float dz = section[k + 1].x - section[k].x; + float dy = section[k + 1].y - section[k].y; + n = b3MaxInt( 1, (int)ceilf( sqrtf( dz * dz + dy * dy ) / cellSize - 0.01f ) ); + } + + // The last point only closes the final segment + int last = k + 1 < count ? n : 1; + for ( int s = 0; s < last; ++s ) + { + float t = k + 1 < count ? (float)s / (float)n : 0.0f; + b3Vec2 a = section[k]; + b3Vec2 b = k + 1 < count ? section[k + 1] : a; + float z = a.x + t * ( b.x - a.x ); + float y = a.y + t * ( b.y - a.y ); + for ( int j = 0; j <= xCount; ++j ) + { + float x = -halfLength + 2.0f * halfLength * (float)j / (float)xCount; + builder->vertices[index++] = (b3Vec3){ sign * x, y, z }; + } + } + } + + builder->vertexCount = index; + + int triangleCount = 2 * ( pointCount - 1 ) * xCount; + builder->indices = realloc( builder->indices, 3 * triangleCount * sizeof( int32_t ) ); + builder->indexCount = 0; + for ( int i = 0; i + 1 < pointCount; ++i ) + { + for ( int j = 0; j < xCount; ++j ) + { + int a = i * stride + j; + int b = a + 1; + int c = a + stride + 1; + int d = a + stride; + + int corners[6] = { a, d, c, c, b, a }; + if ( ( i + j ) & 1 ) + { + int flipped[6] = { a, d, b, d, c, b }; + 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 float EvaluateRail( float z, float cellZ ) +{ + if ( z < -0.5f * cellZ ) + { + return 0.0f; + } + + return z < 0.5f * cellZ ? GHOST_RAIL_HEIGHT : -GHOST_RAIL_SKIRT; +} + +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; +} + +// The bodies holding height field shapes must go first +static void DestroyGhostGroundData( GhostCollisionsData* data ) +{ + for ( int i = 0; i < data->meshCount; ++i ) + { + b3DestroyMesh( data->meshes[i] ); + data->meshes[i] = NULL; + } + data->meshCount = 0; + + for ( int i = 0; i < data->heightFieldCount; ++i ) + { + b3DestroyHeightField( data->heightFields[i] ); + data->heightFields[i] = NULL; + } + data->heightFieldCount = 0; +} + +static void PushProfile( GhostCollisionsData* data, float x, float y ) +{ + if ( data->profileCount < GHOST_MAX_PROFILE_POINTS ) + { + data->profile[data->profileCount++] = (b3Vec2){ x, y }; + } +} + +// The profile is a function of x, a vertical step has two points at the same x +static float EvaluateProfile( const GhostCollisionsData* data, float x ) +{ + const b3Vec2* profile = data->profile; + int count = data->profileCount; + for ( int k = 0; k + 1 < count; ++k ) + { + if ( x <= profile[k + 1].x ) + { + float dx = profile[k + 1].x - profile[k].x; + float t = dx > 0.0f ? b3ClampFloat( ( x - profile[k].x ) / dx, 0.0f, 1.0f ) : 1.0f; + return profile[k].y + t * ( profile[k + 1].y - profile[k].y ); + } + } + + return profile[count - 1].y; +} + +static int GetCellCount( float length, float cellSize ) +{ + return b3MaxInt( 1, (int)ceilf( length / cellSize - 0.01f ) ); +} + +// Samples the profile on a grid spanning x0 to x1. The mirror samples in reverse and turns the body half +// way around y, so the world geometry is the same either way. All tiles of a scene share one height range +// so their quantized heights line up at a seam. +static void AddHeightField( b3WorldId worldId, GhostCollisionsData* data, float x0, float x1, int cellCountX, float halfWidth, + int cellCountZ, bool frictionless ) +{ + float minHeight = FLT_MAX; + float maxHeight = -FLT_MAX; + for ( int k = 0; k < data->profileCount; ++k ) + { + minHeight = b3MinFloat( minHeight, data->profile[k].y ); + maxHeight = b3MaxFloat( maxHeight, data->profile[k].y ); + } + + bool mirror = data->settings.mirror; + int countX = cellCountX + 1; + int countZ = cellCountZ + 1; + float cellX = ( x1 - x0 ) / (float)cellCountX; + float cellZ = 2.0f * halfWidth / (float)cellCountZ; + + // The mirror turns the body half way around y, which also reverses z + bool rail = data->settings.scene == e_ghostRail; + float* heights = malloc( countX * countZ * sizeof( float ) ); + for ( int row = 0; row < countZ; ++row ) + { + float z = mirror ? halfWidth - cellZ * (float)row : -halfWidth + cellZ * (float)row; + for ( int column = 0; column < countX; ++column ) + { + float x = mirror ? x1 - cellX * (float)column : x0 + cellX * (float)column; + heights[row * countX + column] = rail ? EvaluateRail( z, cellZ ) : EvaluateProfile( data, x ); + } + } + + b3HeightFieldDef def = { 0 }; + def.heights = heights; + def.scale = (b3Vec3){ cellX, 1.0f, cellZ }; + def.countX = countX; + def.countZ = countZ; + def.globalMinimumHeight = minHeight; + def.globalMaximumHeight = maxHeight; + b3HeightFieldData* heightField = b3CreateHeightField( &def ); + free( heights ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + if ( mirror ) + { + bodyDef.position = (b3Pos){ x1, 0.0f, halfWidth }; + bodyDef.rotation = (b3Quat){ { 0.0f, 1.0f, 0.0f }, 0.0f }; + } + else + { + bodyDef.position = (b3Pos){ x0, 0.0f, -halfWidth }; + } + + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + if ( frictionless ) + { + shapeDef.baseMaterial.friction = 0.0f; + } + b3CreateHeightFieldShape( bodyId, &shapeDef, heightField ); + + data->heightFields[data->heightFieldCount] = heightField; + data->heightBodyIds[data->heightFieldCount] = bodyId; + data->heightFieldCount += 1; +} + +void CreateGhostGround( b3WorldId worldId, GhostCollisionsData* data ) +{ + if ( B3_IS_NON_NULL( data->groundId ) ) + { + b3DestroyBody( data->groundId ); + } + + for ( int i = 0; i < data->heightFieldCount; ++i ) + { + b3DestroyBody( data->heightBodyIds[i] ); + } + + DestroyGhostGroundData( data ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + data->groundId = b3CreateBody( worldId, &bodyDef ); + + const GhostSettings* settings = &data->settings; + bool rail = settings->scene == e_ghostRail; + float h = rail ? GHOST_RAIL_HALF_LENGTH : GHOST_HALF_LENGTH; + data->profileCount = 0; + data->halfLength = h; + data->halfWidth = rail ? GHOST_RAIL_HALF_WIDTH : GHOST_HALF_WIDTH; + + switch ( settings->scene ) + { + case e_ghostConcave: + { + 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: + // A height field cannot hold a vertical wall, so there the drop takes one cell + PushProfile( data, -h, 3.0f ); + PushProfile( data, 0.0f, 3.0f ); + PushProfile( data, settings->heightField ? settings->cellSize : 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_ghostInternal: + // 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; + + case e_ghostChamfer: + // Flat tops at y = 0 with a chamfered notch every meter. A body crossing a notch overhangs both + // chamfers while its bottom rests on the convex top edges. + PushProfile( data, -h, 0.0f ); + for ( int i = 0; i < 30; ++i ) + { + float x = -8.0f + (float)i; + PushProfile( data, x, 0.0f ); + PushProfile( data, x + GHOST_CHAMFER_WIDTH, -GHOST_CHAMFER_DROP ); + PushProfile( data, x + 2.0f * GHOST_CHAMFER_WIDTH, -GHOST_CHAMFER_DROP ); + PushProfile( data, x + 3.0f * GHOST_CHAMFER_WIDTH, 0.0f ); + } + PushProfile( data, h, 0.0f ); + break; + + case e_ghostRail: + // Cross section in zy, from the floor up the inner wall, over the top and down the outer skirt + PushProfile( data, -data->halfWidth, 0.0f ); + PushProfile( data, 0.0f, 0.0f ); + PushProfile( data, 0.0f, GHOST_RAIL_HEIGHT ); + PushProfile( data, GHOST_RAIL_THICKNESS, GHOST_RAIL_HEIGHT ); + PushProfile( data, GHOST_RAIL_THICKNESS, -GHOST_RAIL_SKIRT ); + break; + + default: + PushProfile( data, -h, 0.0f ); + PushProfile( data, h, 0.0f ); + break; + } + + MeshBuilder builder = { 0 }; + float cellSize = settings->cellSize; + bool mirror = settings->mirror; + float groundHalfWidth = data->halfWidth; + int cellCountZ = GetCellCount( 2.0f * groundHalfWidth, cellSize ); + + if ( settings->heightField ) + { + if ( settings->scene == e_ghostInternal ) + { + // One grid line per profile row, as in the mesh + PeelPreset preset = s_peelPresets[settings->peelPreset]; + int zCount = preset.columnCount - 1; + data->halfWidth = 20.0f * preset.scaleX * (float)zCount; + float x0 = data->profile[0].x; + float x1 = data->profile[data->profileCount - 1].x; + AddHeightField( worldId, data, x0, x1, data->profileCount - 1, data->halfWidth, zCount, 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 if ( settings->scene == e_ghostChamfer ) + { + // One cell per chamfer puts every notch vertex on the grid + AddHeightField( worldId, data, -h, h, GetCellCount( 2.0f * h, GHOST_CHAMFER_WIDTH ), groundHalfWidth, cellCountZ, + false ); + } + else + { + AddHeightField( worldId, data, -h, h, GetCellCount( 2.0f * h, cellSize ), groundHalfWidth, cellCountZ, false ); + } + } + else if ( settings->scene == e_ghostRail ) + { + AddRail( &builder, data->profile, data->profileCount, h, cellSize, mirror ); + AddMesh( data, &builder, false ); + } + else if ( settings->scene == e_ghostInternal ) + { + // 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 ); + data->halfWidth = halfWidth; + 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_ghostInternal; + 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; + bodyDef.enableContactRecycling = false; + 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 if ( settings->scene == e_ghostRail ) + { + // Dropped across the rail top, pitched so a box face sweeps into the rail edge at an angle. The real + // contact is edge on edge and perpendicular to the rail, a face normal there leans along the rail + // and catches the body. Found in the Conveyor Circuit sample. + b3Quat pitch = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 60.0f * B3_PI / 180.0f ); + bodyDef.position = (b3Pos){ GHOST_RAIL_START_X, GHOST_RAIL_HEIGHT + 0.45f, -0.15f }; + bodyDef.rotation = b3MulQuat( pitch, yaw ); + bodyDef.linearVelocity = (b3Vec3){ settings->speed, 0.0f, 0.0f }; + } + 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_ghostVCapsule ) + { + 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_ghostHCapsule: + { + b3Capsule capsule = { { 0.0f, 0.0f, -capsuleHalf }, { 0.0f, 0.0f, capsuleHalf }, capsuleRadius }; + data->shapeId = b3CreateCapsuleShape( data->bodyId, &shapeDef, &capsule ); + } + break; + + case e_ghostVCapsule: + { + 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->contactBounds = b3Shape_GetAABB( data->shapeId ); + 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. At a side edge of the ground the +// normal may also lean off that side, perpendicular to the edge. +static bool IsGhost( const GhostCollisionsData* data, b3Vec3 normal, float xMin, float xMax, float zMin, float zMax ) +{ + float tolerance = 5.0f * B3_PI / 180.0f; + float sinTolerance = sinf( tolerance ); + if ( b3AbsFloat( normal.z ) > sinTolerance ) + { + // The manifold must reach the side it leans off + float margin = 0.05f; + bool atSide = normal.z < 0.0f ? zMin < -data->halfWidth + margin : zMax > data->halfWidth - margin; + if ( atSide == false ) + { + return true; + } + } + + // A normal leaning off a side has a short projection, which leaves little room to lean along the edge + float planarLength = sqrtf( normal.x * normal.x + normal.y * normal.y ); + if ( planarLength < sinTolerance ) + { + return false; + } + + GhostBody body = data->settings.body; + bool chamferHull = data->settings.scene == e_ghostChamfer && ( body == e_ghostBox || body == e_ghostCylinder ); + + 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; + } + + // A flat hull bottom resting on the top edges must keep its own normal, so for a hull only the tops + // count. A round body's normal comes from the closest ground feature, which can be a top edge. + if ( chamferHull && ( data->profile[k].y < 0.0f || data->profile[k + 1].y < 0.0f ) ) + { + continue; + } + + float angle = SegmentAngle( data, k ); + lower = b3MinFloat( lower, angle ); + upper = b3MaxFloat( upper, angle ); + } + + float angle = atan2f( normal.y, normal.x ); + float deviation = b3MaxFloat( lower - angle, angle - upper ); + if ( deviation <= 0.0f ) + { + return false; + } + + // Measure the lean outside the span in 3D so a steep side normal is not over counted + return deviation > 0.5f * B3_PI || planarLength * sinf( deviation ) > sinTolerance; +} + +int UpdateGhostCollisions( b3WorldId worldId, GhostCollisionsData* data, GhostReport* reports, int capacity ) +{ + b3Pos center = b3Body_GetWorldCenter( data->bodyId ); + bool done = center.x > data->halfLength - 1.0f || center.y < -3.0f; + if ( data->settings.scene == e_ghostInternal ) + { + // 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 = data->contactBounds; + float xMin = (float)bounds.lowerBound.x - 0.1f; + float xMax = (float)bounds.upperBound.x + 0.1f; + data->contactBounds = b3Shape_GetAABB( data->shapeId ); + + 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; + b3ShapeType otherType = b3Shape_GetType( otherId ); + if ( otherType != b3_meshShape && otherType != b3_heightShape ) + { + 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; + } + + float zMin = FLT_MAX; + float zMax = -FLT_MAX; + for ( int k = 0; k < manifold->pointCount; ++k ) + { + b3Vec3 anchor = bodyIsA ? manifold->points[k].anchorA : manifold->points[k].anchorB; + float z = (float)center.z + anchor.z; + zMin = b3MinFloat( zMin, z ); + zMax = b3MaxFloat( zMax, z ); + } + + b3Vec3 normal = bodyIsA ? b3Neg( manifold->normal ) : manifold->normal; + if ( data->settings.scene == e_ghostRail ) + { + // The rail is the same all along x, so no valid normal leans along it + if ( b3AbsFloat( normal.x ) <= sinf( 5.0f * B3_PI / 180.0f ) ) + { + continue; + } + } + else if ( IsGhost( data, normal, xMin, xMax, zMin, zMax ) == 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 ) +{ + DestroyGhostGroundData( 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..c307bc4 --- /dev/null +++ b/shared/ghost_collisions.h @@ -0,0 +1,143 @@ +// 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. + +// A conveyor side rail along x, the floor is on the -z side +#define GHOST_RAIL_HEIGHT 0.35f +#define GHOST_RAIL_THICKNESS 0.08f + +typedef enum GhostScene +{ + e_ghostFlat, + e_ghostConcave, + e_ghostCliff, + e_ghostCrest, + e_ghostBumps, + e_ghostInternal, + e_ghostChamfer, + e_ghostRail, + e_ghostSceneCount +} GhostScene; + +typedef enum GhostBody +{ + e_ghostBox, + e_ghostHCapsule, + e_ghostVCapsule, + e_ghostSphere, + e_ghostCylinder, + e_ghostBodyCount +} GhostBody; + +// Mesh variants of the PEEL InternalEdges presets +#define GHOST_PEEL_PRESET_COUNT 4 + +// Chamfer has the longest profile +#define GHOST_MAX_PROFILE_POINTS 128 + +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; + + // Build the ground from height fields instead of meshes. Each height field is its own shape. + bool heightField; + + 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; + + // The ground spans x in [-halfLength, halfLength] and z in [-halfWidth, halfWidth] + float halfLength; + float halfWidth; + b3MeshData* meshes[2]; + int meshCount; + + // A height field shape has no local transform, so each one gets its own static body + b3HeightFieldData* heightFields[2]; + b3BodyId heightBodyIds[2]; + int heightFieldCount; + b3HullData* cylinder; + b3HullData* peelCylinder; + b3BodyId groundId; + b3BodyId bodyId; + b3ShapeId shapeId; + + // Body bounds at the start of the step, where the contacts were computed. A fast body can cross a fold + // within a step, so the bounds after the step can miss the face a contact came from. + b3AABB contactBounds; + + // 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, height field 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/shared/ghost_oracle.c b/shared/ghost_oracle.c new file mode 100644 index 0000000..5b257fe --- /dev/null +++ b/shared/ghost_oracle.c @@ -0,0 +1,779 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "ghost_oracle.h" + +#include "ghost_collisions.h" + +#include "box3d/box3d.h" +#include "box3d/constants.h" + +#include +#include +#include + +const char* g_oracleSceneNames[e_oracleSceneCount] = { + "flat", "fan", "crest", "valley", "ridge", "saddle ridge", "rail", "wave", +}; + +// The wave scene is the largest, 169 vertices and 288 triangles +#define ORACLE_MAX_VERTICES 256 +#define ORACLE_MAX_INDICES ( 3 * 512 ) +#define ORACLE_MAX_MANIFOLDS 128 + +typedef struct OracleBuilder +{ + b3Vec3 vertices[ORACLE_MAX_VERTICES]; + int32_t indices[ORACLE_MAX_INDICES]; + int vertexCount; + int indexCount; +} OracleBuilder; + +static int AddVertex( OracleBuilder* builder, b3Vec3 vertex ) +{ + B3_ASSERT( builder->vertexCount < ORACLE_MAX_VERTICES ); + if ( builder->vertexCount == ORACLE_MAX_VERTICES ) + { + return 0; + } + + builder->vertices[builder->vertexCount] = vertex; + return builder->vertexCount++; +} + +// Winds the triangle to face the given direction +static void AddTriangle( OracleBuilder* builder, int a, int b, int c, b3Vec3 facing ) +{ + B3_ASSERT( builder->indexCount + 3 <= ORACLE_MAX_INDICES ); + if ( builder->indexCount + 3 > ORACLE_MAX_INDICES ) + { + return; + } + + b3Vec3 va = builder->vertices[a]; + b3Vec3 normal = b3Cross( b3Sub( builder->vertices[b], va ), b3Sub( builder->vertices[c], va ) ); + if ( b3Dot( normal, facing ) < 0.0f ) + { + int temp = b; + b = c; + c = temp; + } + + builder->indices[builder->indexCount++] = a; + builder->indices[builder->indexCount++] = b; + builder->indices[builder->indexCount++] = c; +} + +typedef float OracleHeightFcn( float x, float z ); + +// Up facing grid with alternating diagonals +static void BuildGrid( OracleBuilder* builder, int countX, int countZ, float cellSize, OracleHeightFcn* height ) +{ + float x0 = -0.5f * cellSize * countX; + float z0 = -0.5f * cellSize * countZ; + int base = builder->vertexCount; + for ( int k = 0; k <= countZ; ++k ) + { + for ( int i = 0; i <= countX; ++i ) + { + float x = x0 + cellSize * i; + float z = z0 + cellSize * k; + AddVertex( builder, (b3Vec3){ x, height( x, z ), z } ); + } + } + + int stride = countX + 1; + for ( int k = 0; k < countZ; ++k ) + { + for ( int i = 0; i < countX; ++i ) + { + int a = base + k * stride + i; + int b = a + 1; + int c = a + stride + 1; + int d = a + stride; + if ( ( i + k ) & 1 ) + { + AddTriangle( builder, a, d, b, b3Vec3_axisY ); + AddTriangle( builder, b, d, c, b3Vec3_axisY ); + } + else + { + AddTriangle( builder, a, d, c, b3Vec3_axisY ); + AddTriangle( builder, a, c, b, b3Vec3_axisY ); + } + } + } +} + +static float FlatHeight( float x, float z ) +{ + (void)x; + (void)z; + return 0.0f; +} + +static float CrestHeight( float x, float z ) +{ + (void)z; + return -0.26795f * fabsf( x ); +} + +static float ValleyHeight( float x, float z ) +{ + (void)z; + return 0.26795f * fabsf( x ); +} + +static float RidgeHeight( float x, float z ) +{ + (void)z; + return -fabsf( x ); +} + +// libm trig differs in the last bit between platforms, which would make each a different mesh +static float WaveHeight( float x, float z ) +{ + float s1 = b3ComputeCosSin( 2.3f * x + 0.4f ).sine; + float c1 = b3ComputeCosSin( 1.7f * z ).cosine; + float s2 = b3ComputeCosSin( 4.1f * z - 1.3f * x ).sine; + return 0.12f * s1 * c1 + 0.05f * s2; +} + +static void BuildFan( OracleBuilder* builder ) +{ + int center = AddVertex( builder, b3Vec3_zero ); + for ( int i = 0; i < 6; ++i ) + { + b3CosSin cs = b3ComputeCosSin( ( i + 0.5f ) * B3_PI / 3.0f ); + AddVertex( builder, (b3Vec3){ 2.0f * cs.cosine, 0.0f, 2.0f * cs.sine } ); + } + + for ( int i = 0; i < 6; ++i ) + { + AddTriangle( builder, center, 1 + i, 1 + ( i + 1 ) % 6, b3Vec3_axisY ); + } +} + +// A ridge along x running into a rising wall, so the ridge end is a saddle vertex +static void BuildSaddleRidge( OracleBuilder* builder ) +{ + float xs[3] = { -2.0f, 0.0f, 1.0f }; + float crests[3] = { -0.004f, 0.0f, 0.3f }; + float sides[3] = { -0.7f, -0.7f, -0.4f }; + for ( int i = 0; i < 3; ++i ) + { + AddVertex( builder, (b3Vec3){ xs[i], sides[i], -1.0f } ); + AddVertex( builder, (b3Vec3){ xs[i], crests[i], 0.0f } ); + AddVertex( builder, (b3Vec3){ xs[i], sides[i], 1.0f } ); + } + + for ( int i = 0; i < 2; ++i ) + { + for ( int k = 0; k < 2; ++k ) + { + int a = 3 * i + k; + AddTriangle( builder, a, a + 1, a + 3, b3Vec3_axisY ); + AddTriangle( builder, a + 3, a + 1, a + 4, b3Vec3_axisY ); + } + } +} + +// A wall facing -z meeting a narrow top at a convex edge, like a conveyor side rail +static void BuildRail( OracleBuilder* builder ) +{ + float sectionZ[4] = { 0.0f, 0.0f, GHOST_RAIL_THICKNESS, GHOST_RAIL_THICKNESS }; + float sectionY[4] = { 0.0f, GHOST_RAIL_HEIGHT, GHOST_RAIL_HEIGHT, 0.0f }; + int columns = 5; + for ( int row = 0; row < 4; ++row ) + { + for ( int column = 0; column < columns; ++column ) + { + AddVertex( builder, (b3Vec3){ -0.5f + 0.25f * column, sectionY[row], sectionZ[row] } ); + } + } + + b3Vec3 facings[3] = { { 0.0f, 0.0f, -1.0f }, b3Vec3_axisY, b3Vec3_axisZ }; + for ( int row = 0; row < 3; ++row ) + { + for ( int column = 0; column < columns - 1; ++column ) + { + int a = columns * row + column; + int b = a + 1; + int c = a + columns + 1; + int d = a + columns; + if ( ( row + column ) & 1 ) + { + AddTriangle( builder, a, d, b, facings[row] ); + AddTriangle( builder, d, c, b, facings[row] ); + } + else + { + AddTriangle( builder, a, d, c, facings[row] ); + AddTriangle( builder, c, b, a, facings[row] ); + } + } + } +} + +b3MeshData* CreateOracleMesh( OracleScene scene ) +{ + static OracleBuilder builder; + builder.vertexCount = 0; + builder.indexCount = 0; + + switch ( scene ) + { + case e_oracleFlat: + BuildGrid( &builder, 6, 6, 0.5f, FlatHeight ); + break; + case e_oracleFan: + BuildFan( &builder ); + break; + case e_oracleCrest: + BuildGrid( &builder, 8, 4, 0.5f, CrestHeight ); + break; + case e_oracleValley: + BuildGrid( &builder, 8, 4, 0.5f, ValleyHeight ); + break; + case e_oracleRidge: + BuildGrid( &builder, 6, 4, 0.5f, RidgeHeight ); + break; + case e_oracleSaddleRidge: + BuildSaddleRidge( &builder ); + break; + case e_oracleRail: + BuildRail( &builder ); + break; + case e_oracleWave: + BuildGrid( &builder, 12, 12, 0.25f, WaveHeight ); + break; + default: + return NULL; + } + + b3MeshDef def = { 0 }; + def.vertices = builder.vertices; + def.stride = sizeof( b3Vec3 ); + def.vertexCount = builder.vertexCount; + def.indices = builder.indices; + def.triangleCount = builder.indexCount / 3; + def.weldVertices = true; + def.weldTolerance = 0.001f; + def.identifyEdges = true; + return b3CreateMesh( &def, NULL, 0 ); +} + +// Local state and 24 bits per draw, which the global 15 bit generator in utils.h does not give +typedef struct OracleRandom +{ + uint32_t state; +} OracleRandom; + +static float RandomUnit( OracleRandom* random ) +{ + uint32_t x = random->state; + x ^= x << 13; + x ^= x >> 17; + x ^= x << 5; + random->state = x; + return (float)( x >> 8 ) / 16777216.0f; +} + +static float RandomRange( OracleRandom* random, float lower, float upper ) +{ + return lower + ( upper - lower ) * RandomUnit( random ); +} + +static b3Vec3 RandomDirection( OracleRandom* random ) +{ + for ( int i = 0; i < 16; ++i ) + { + b3Vec3 v = { RandomRange( random, -1.0f, 1.0f ), RandomRange( random, -1.0f, 1.0f ), + RandomRange( random, -1.0f, 1.0f ) }; + float lengthSquared = b3LengthSquared( v ); + if ( 0.01f < lengthSquared && lengthSquared <= 1.0f ) + { + return b3MulSV( 1.0f / sqrtf( lengthSquared ), v ); + } + } + + return b3Vec3_axisY; +} + +// Direction within the given angle of the axis +static b3Vec3 RandomTilt( OracleRandom* random, b3Vec3 axis, float maxAngle ) +{ + b3Vec3 perp = b3Cross( axis, RandomDirection( random ) ); + float length = b3Length( perp ); + if ( length < 1.0e-4f ) + { + return axis; + } + + perp = b3MulSV( 1.0f / length, perp ); + b3CosSin cs = b3ComputeCosSin( RandomRange( random, 0.0f, maxAngle ) ); + return b3Add( b3MulSV( cs.cosine, axis ), b3MulSV( cs.sine, perp ) ); +} + +static bool CollectPair( b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleIndex, void* context ) +{ + OracleReference* reference = context; + + // A truncated union would misjudge the pose + B3_ASSERT( reference->pairCount < ORACLE_MAX_TRIANGLES ); + if ( reference->pairCount == ORACLE_MAX_TRIANGLES ) + { + return false; + } + + OraclePair* pair = reference->pairs + reference->pairCount; + *pair = (OraclePair){ 0 }; + pair->triangleIndex = triangleIndex; + pair->vertices[0] = a; + pair->vertices[1] = b; + pair->vertices[2] = c; + pair->faceNormal = b3Normalize( b3Cross( b3Sub( b, a ), b3Sub( c, a ) ) ); + pair->owned = true; + pair->dominatedBy = -1; + reference->pairCount += 1; + return true; +} + +// From Real-time Collision Detection, p141 +static b3Vec3 ClosestPointOnTriangle( b3Vec3 p, b3Vec3 a, b3Vec3 b, b3Vec3 c ) +{ + b3Vec3 ab = b3Sub( b, a ); + b3Vec3 ac = b3Sub( c, a ); + b3Vec3 ap = b3Sub( p, a ); + float d1 = b3Dot( ab, ap ); + float d2 = b3Dot( ac, ap ); + if ( d1 <= 0.0f && d2 <= 0.0f ) + { + return a; + } + + b3Vec3 bp = b3Sub( p, b ); + float d3 = b3Dot( ab, bp ); + float d4 = b3Dot( ac, bp ); + if ( d3 >= 0.0f && d4 <= d3 ) + { + return b; + } + + float vc = d1 * d4 - d3 * d2; + if ( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f ) + { + return b3MulAdd( a, d1 / ( d1 - d3 ), ab ); + } + + b3Vec3 cp = b3Sub( p, c ); + float d5 = b3Dot( ab, cp ); + float d6 = b3Dot( ac, cp ); + if ( d6 >= 0.0f && d5 <= d6 ) + { + return c; + } + + float vb = d5 * d2 - d1 * d6; + if ( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f ) + { + return b3MulAdd( a, d2 / ( d2 - d6 ), ac ); + } + + float va = d3 * d6 - d5 * d4; + if ( va <= 0.0f && ( d4 - d3 ) >= 0.0f && ( d5 - d6 ) >= 0.0f ) + { + float w = ( d4 - d3 ) / ( ( d4 - d3 ) + ( d5 - d6 ) ); + return b3MulAdd( b, w, b3Sub( c, b ) ); + } + + float denom = 1.0f / ( va + vb + vc ); + float v = vb * denom; + float w = vc * denom; + return b3Add( a, b3Add( b3MulSV( v, ab ), b3MulSV( w, ac ) ) ); +} + +// The closest pair of each nearby triangle and the union distance +static float ComputePairs( OracleReference* reference, const b3MeshData* mesh, const b3HullData* hull, b3Transform transform ) +{ + reference->pairCount = 0; + + b3AABB bounds = b3ComputeHullAABB( hull, transform ); + // Collided points can be well past the speculative distance and the proximal test looks around them + float margin = 0.25f; + bounds.lowerBound = b3Sub( bounds.lowerBound, (b3Vec3){ margin, margin, margin } ); + bounds.upperBound = b3Add( bounds.upperBound, (b3Vec3){ margin, margin, margin } ); + + b3Mesh meshShape = { .data = (b3MeshData*)mesh, .scale = b3Vec3_one }; + b3QueryMesh( &meshShape, bounds, CollectPair, reference ); + + float minDistance = FLT_MAX; + for ( int i = 0; i < reference->pairCount; ++i ) + { + OraclePair* pair = reference->pairs + i; + b3DistanceInput input = { 0 }; + input.proxyA = (b3ShapeProxy){ pair->vertices, 3, 0.0f }; + input.proxyB = (b3ShapeProxy){ b3GetHullPoints( hull ), hull->vertexCount, 0.0f }; + input.transform = transform; + input.useRadii = false; + b3SimplexCache cache = { 0 }; + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + pair->meshPoint = output.pointA; + pair->hullPoint = output.pointB; + pair->normal = output.normal; + pair->distance = output.distance; + + if ( output.distance < minDistance ) + { + minDistance = output.distance; + reference->closestMeshPoint = output.pointA; + reference->closestHullPoint = output.pointB; + } + } + + reference->distance = minDistance; + return minDistance; +} + +#define ORACLE_CONTAIN_TOLERANCE 1.0e-4f + +static bool TriangleContains( const OraclePair* pair, b3Vec3 point ) +{ + b3Vec3 closest = ClosestPointOnTriangle( point, pair->vertices[0], pair->vertices[1], pair->vertices[2] ); + return b3DistanceSquared( closest, point ) < ORACLE_CONTAIN_TOLERANCE * ORACLE_CONTAIN_TOLERANCE; +} + +// A closest pair is a contact of the union unless a closer triangle also contains its mesh point +static void ComputeOwnership( OracleReference* reference ) +{ + // Ties along shared features differ only by GJK round off + float tieTolerance = 0.02f * B3_LINEAR_SLOP; + + for ( int i = 0; i < reference->pairCount; ++i ) + { + OraclePair* pair = reference->pairs + i; + pair->owned = true; + pair->dominatedBy = -1; + for ( int j = 0; j < reference->pairCount; ++j ) + { + const OraclePair* other = reference->pairs + j; + if ( j == i || other->distance >= pair->distance - tieTolerance ) + { + continue; + } + + if ( TriangleContains( other, pair->meshPoint ) ) + { + pair->owned = false; + pair->dominatedBy = other->triangleIndex; + break; + } + } + } +} + +// On success the reference holds the closest pairs of the returned pose +static bool MakeOraclePose( OracleReference* reference, const b3MeshData* mesh, OracleScene scene, int index, b3BoxHull* box, + b3Transform* transform ) +{ + OracleRandom random = { 0x9E3779B9u ^ ( 2654435761u * (uint32_t)( index + 1 ) ) ^ ( 40503u * (uint32_t)scene ) }; + for ( int i = 0; i < 4; ++i ) + { + RandomUnit( &random ); + } + + const b3Vec3* vertices = b3GetMeshVertices( mesh ); + const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh ); + int triangleIndex = (int)( RandomUnit( &random ) * mesh->triangleCount ); + triangleIndex = triangleIndex < mesh->triangleCount ? triangleIndex : mesh->triangleCount - 1; + b3Vec3 a = vertices[triangles[triangleIndex].index1]; + b3Vec3 b = vertices[triangles[triangleIndex].index2]; + b3Vec3 c = vertices[triangles[triangleIndex].index3]; + b3Vec3 faceNormal = b3Normalize( b3Cross( b3Sub( b, a ), b3Sub( c, a ) ) ); + + float u = RandomUnit( &random ); + float v = RandomUnit( &random ); + if ( u + v > 1.0f ) + { + u = 1.0f - u; + v = 1.0f - v; + } + + b3Vec3 target = b3Add( a, b3Add( b3MulSV( u, b3Sub( b, a ) ), b3MulSV( v, b3Sub( c, a ) ) ) ); + + float hx = RandomRange( &random, 0.05f, 0.4f ); + float hy = RandomRange( &random, 0.05f, 0.4f ); + float hz = RandomRange( &random, 0.05f, 0.4f ); + *box = b3MakeBoxHull( hx, hy, hz ); + + // Half the boxes rest nearly flat on the face, where parallel faces make ties + b3Quat rotation; + if ( RandomUnit( &random ) < 0.5f ) + { + b3Vec3 up = RandomTilt( &random, faceNormal, 0.2f ); + b3Quat align = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisY, up ); + b3Quat yaw = b3MakeQuatFromAxisAngle( b3Vec3_axisY, RandomRange( &random, -B3_PI, B3_PI ) ); + rotation = b3MulQuat( align, yaw ); + } + else + { + b3Vec3 axis = RandomDirection( &random ); + rotation = b3MakeQuatFromAxisAngle( axis, RandomRange( &random, -B3_PI, B3_PI ) ); + } + + b3Vec3 approach = RandomTilt( &random, faceNormal, 1.0f ); + + // Put the hull support point against the target + const b3Vec3* hullPoints = b3GetHullPoints( &box->base ); + b3Vec3 localDirection = b3InvRotateVector( rotation, b3Neg( approach ) ); + b3Vec3 support = hullPoints[0]; + for ( int i = 1; i < box->base.vertexCount; ++i ) + { + if ( b3Dot( hullPoints[i], localDirection ) > b3Dot( support, localDirection ) ) + { + support = hullPoints[i]; + } + } + + float linearSlop = B3_LINEAR_SLOP; + float gap = linearSlop * ( 0.2f + 3.4f * RandomUnit( &random ) * RandomUnit( &random ) ); + b3Transform pose = { b3Sub( target, b3RotateVector( rotation, support ) ), rotation }; + pose.p = b3MulAdd( pose.p, gap, approach ); + + for ( int iteration = 0; iteration < 20; ++iteration ) + { + *transform = pose; + + float distance = ComputePairs( reference, mesh, &box->base, pose ); + if ( reference->pairCount == 0 ) + { + return false; + } + + if ( distance <= 0.0f ) + { + pose.p = b3MulAdd( pose.p, 2.0f * linearSlop, approach ); + continue; + } + + if ( b3AbsFloat( distance - gap ) < 0.05f * linearSlop ) + { + return true; + } + + b3Vec3 direction = b3Normalize( b3Sub( reference->closestHullPoint, reference->closestMeshPoint ) ); + pose.p = b3MulAdd( pose.p, gap - distance, direction ); + } + + *transform = pose; + float distance = ComputePairs( reference, mesh, &box->base, pose ); + return reference->pairCount > 0 && ORACLE_MIN_DISTANCE <= distance && distance <= B3_SPECULATIVE_DISTANCE; +} + +// Smallest squared sine between the normal and the face normal of a triangle that contains the mesh +// point and faces the normal. FLT_MAX if there is no such triangle. +static float FacingSineSquared( const OracleReference* reference, b3Vec3 meshPoint, b3Vec3 normal ) +{ + float best = FLT_MAX; + for ( int i = 0; i < reference->pairCount; ++i ) + { + const OraclePair* pair = reference->pairs + i; + if ( b3Dot( pair->faceNormal, normal ) <= 0.0f || TriangleContains( pair, meshPoint ) == false ) + { + continue; + } + + best = b3MinFloat( best, b3LengthSquared( b3Cross( pair->faceNormal, normal ) ) ); + } + + return best; +} + +// The mesh point is the closest union point to a point a little way out along the normal +static bool IsProximalNormal( const OracleReference* reference, b3Vec3 meshPoint, b3Vec3 normal, float sinTolerance ) +{ + float probeDistance = 0.01f; + b3Vec3 probe = b3MulAdd( meshPoint, probeDistance, normal ); + float bestDistanceSquared = FLT_MAX; + b3Vec3 best = meshPoint; + for ( int i = 0; i < reference->pairCount; ++i ) + { + const OraclePair* pair = reference->pairs + i; + b3Vec3 closest = ClosestPointOnTriangle( probe, pair->vertices[0], pair->vertices[1], pair->vertices[2] ); + float distanceSquared = b3DistanceSquared( closest, probe ); + if ( distanceSquared < bestDistanceSquared ) + { + bestDistanceSquared = distanceSquared; + best = closest; + } + } + + float offset = sinTolerance * probeDistance; + return b3DistanceSquared( best, meshPoint ) < offset * offset; +} + +// Collides the hull and judges every manifold against the reference +static void JudgeCollision( OracleResult* result, const OracleReference* reference, const b3MeshData* mesh, + const b3HullData* hull, b3Transform transform, bool handleGhosts ) +{ + result->pointCount = 0; + result->ghostCount = 0; + result->missed = false; + + b3Manifold manifolds[ORACLE_MAX_MANIFOLDS]; + b3Mesh meshShape = { .data = (b3MeshData*)mesh, .scale = b3Vec3_one }; + int manifoldCount = + b3CollideMeshAndHull( manifolds, ORACLE_MAX_MANIFOLDS, &meshShape, hull, transform, true, handleGhosts ); + B3_ASSERT( manifoldCount < ORACLE_MAX_MANIFOLDS ); + + // Clustering merges manifolds whose normals are within about 5 degrees + float sinTolerance = 0.1f; + float minSeparation = FLT_MAX; + for ( int i = 0; i < manifoldCount; ++i ) + { + const b3Manifold* manifold = manifolds + i; + int firstPoint = result->pointCount; + for ( int j = 0; j < manifold->pointCount; ++j ) + { + B3_ASSERT( result->pointCount < ORACLE_MAX_POINTS ); + if ( result->pointCount == ORACLE_MAX_POINTS ) + { + break; + } + + const b3ManifoldPoint* manifoldPoint = manifold->points + j; + OraclePoint* point = result->points + result->pointCount; + point->point = manifoldPoint->anchorA; + point->meshPoint = manifoldPoint->anchorA; + point->normal = manifold->normal; + point->separation = manifoldPoint->separation + B3_MESH_REST_OFFSET; + point->triangleIndex = manifoldPoint->triangleIndex; + point->verdict = e_oracleGhost; + result->pointCount += 1; + + minSeparation = b3MinFloat( minSeparation, point->separation ); + + const OraclePair* pair = NULL; + for ( int k = 0; k < reference->pairCount; ++k ) + { + if ( reference->pairs[k].triangleIndex == point->triangleIndex ) + { + pair = reference->pairs + k; + break; + } + } + + // The reference bounds contain the collider bounds, so every collided triangle has a pair + B3_ASSERT( pair != NULL ); + if ( pair == NULL ) + { + continue; + } + + b3Vec3 meshPoint = + ClosestPointOnTriangle( point->point, pair->vertices[0], pair->vertices[1], pair->vertices[2] ); + point->meshPoint = meshPoint; + + // A proximal normal also has to be in front of a face here, or an inverted face normal would pass + float sineSquared = FacingSineSquared( reference, meshPoint, point->normal ); + if ( sineSquared < sinTolerance * sinTolerance ) + { + point->verdict = e_oracleFace; + } + else if ( sineSquared < FLT_MAX && IsProximalNormal( reference, meshPoint, point->normal, sinTolerance ) ) + { + point->verdict = e_oracleFeature; + } + } + + float deepest = FLT_MAX; + for ( int j = firstPoint; j < result->pointCount; ++j ) + { + deepest = b3MinFloat( deepest, result->points[j].separation ); + } + + bool valid = false; + for ( int j = firstPoint; j < result->pointCount; ++j ) + { + const OraclePoint* point = result->points + j; + if ( point->separation <= deepest + B3_LINEAR_SLOP && point->verdict != e_oracleGhost ) + { + valid = true; + break; + } + } + + for ( int j = firstPoint; j < result->pointCount && valid; ++j ) + { + OraclePoint* point = result->points + j; + point->verdict = point->verdict == e_oracleGhost ? e_oracleShared : point->verdict; + } + + if ( valid == false && result->pointCount > firstPoint ) + { + result->ghostCount += 1; + } + } + + // The union closest pair has to be represented. SAT separation is a lower bound on distance. + float linearSlop = B3_LINEAR_SLOP; + float distance = reference->distance; + if ( distance < B3_SPECULATIVE_DISTANCE - linearSlop && minSeparation > distance + linearSlop ) + { + result->missed = true; + } +} + +bool EvaluateOraclePose( OraclePose* pose, const b3MeshData* mesh, OracleScene scene, int index ) +{ + if ( MakeOraclePose( &pose->reference, mesh, scene, index, &pose->box, &pose->transform ) == false ) + { + return false; + } + + B3_ASSERT( pose->reference.distance >= ORACLE_MIN_DISTANCE ); + ComputeOwnership( &pose->reference ); + + const b3HullData* hull = &pose->box.base; + JudgeCollision( &pose->raw, &pose->reference, mesh, hull, pose->transform, false ); + JudgeCollision( &pose->handled, &pose->reference, mesh, hull, pose->transform, true ); + return true; +} + +bool IsOracleCullingMiss( const OraclePose* pose ) +{ + return pose->handled.missed && pose->raw.missed == false; +} + +OracleScan ScanGhostOracle( const b3MeshData* mesh, OracleScene scene, int poseCount ) +{ + static OraclePose pose; + OracleScan scan = { 0 }; + scan.firstGhost = -1; + scan.firstMissed = -1; + + for ( int i = 0; i < poseCount; ++i ) + { + if ( EvaluateOraclePose( &pose, mesh, scene, i ) == false ) + { + continue; + } + + scan.validCount += 1; + scan.rawGhostPoses += pose.raw.ghostCount > 0 ? 1 : 0; + + if ( pose.handled.ghostCount > 0 ) + { + scan.ghostPoses += 1; + scan.firstGhost = scan.firstGhost < 0 ? i : scan.firstGhost; + } + + if ( IsOracleCullingMiss( &pose ) ) + { + scan.missedPoses += 1; + scan.firstMissed = scan.firstMissed < 0 ? i : scan.firstMissed; + } + } + + return scan; +} diff --git a/shared/ghost_oracle.h b/shared/ghost_oracle.h new file mode 100644 index 0000000..04b3a50 --- /dev/null +++ b/shared/ghost_oracle.h @@ -0,0 +1,168 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT +#pragma once + +#include "box3d/collision.h" +#include "box3d/math_functions.h" + +#include + +#ifdef __cplusplus +extern "C" +{ +#endif + +// Separated-pose oracle for mesh ghost handling. With the hull clear of the mesh the union of the +// triangles is the reference surface, and none of the Voronoi logic under test is needed to judge a +// contact against it. +// +// A normal is valid at a mesh location when the union has it there: the face normal of a triangle +// containing the location, or a proximal normal, meaning the location is the closest union point to a +// point a little way out along the normal. That second test gives the convex edge wedge and the vertex +// cone exactly, and nothing at a concave crease beyond the two face normals. A manifold shares one normal +// across its points, so it is a ghost when the normal is valid at none of its points within linear slop of +// its deepest. +// +// The contacts the union itself has are the local minima of the distance between the hull and the +// union. A triangle's closest pair (GJK) is one unless a triangle that also contains its mesh point is +// closer to the hull. The distance to a triangle is convex over the triangle, so that test is exact. The +// union closest pair must be represented by a collided point, otherwise the contact was missed. + +typedef enum OracleScene +{ + e_oracleFlat, + e_oracleFan, + e_oracleCrest, + e_oracleValley, + e_oracleRidge, + e_oracleSaddleRidge, + e_oracleRail, + e_oracleWave, + e_oracleSceneCount +} OracleScene; + +extern const char* g_oracleSceneNames[e_oracleSceneCount]; + +#define ORACLE_MAX_TRIANGLES 256 +#define ORACLE_MAX_POINTS 128 + +// Poses per scene in the unit test, and in the sample scan so the two report the same counts +#define ORACLE_SCAN_POSES 500 + +typedef enum OracleVerdict +{ + // Face normal of a triangle containing the point + e_oracleFace, + + // Proximal normal at an edge or vertex + e_oracleFeature, + + // Not valid here, but valid at a deep point of the same manifold + e_oracleShared, + + // Point of a manifold whose normal is valid at none of its deep points + e_oracleGhost, +} OracleVerdict; + +typedef struct OraclePair +{ + int triangleIndex; + b3Vec3 vertices[3]; + b3Vec3 faceNormal; + b3Vec3 meshPoint; + b3Vec3 hullPoint; + b3Vec3 normal; + float distance; + bool owned; + + // Triangle index of a closer triangle containing the mesh point, or -1 + int dominatedBy; +} OraclePair; + +// The union of the triangles near the hull. Depends only on the pose, not on the collider. +typedef struct OracleReference +{ + // Distance between the hull and the mesh + float distance; + b3Vec3 closestMeshPoint; + b3Vec3 closestHullPoint; + + OraclePair pairs[ORACLE_MAX_TRIANGLES]; + int pairCount; +} OracleReference; + +typedef struct OraclePoint +{ + b3Vec3 point; + + // Closest point on the point's triangle + b3Vec3 meshPoint; + b3Vec3 normal; + + // Geometric separation, without the mesh rest offset the collider subtracts + float separation; + int triangleIndex; + OracleVerdict verdict; +} OraclePoint; + +// The collider output judged against the reference +typedef struct OracleResult +{ + OraclePoint points[ORACLE_MAX_POINTS]; + int pointCount; + + // Ghost manifolds + int ghostCount; + + // The union closest pair has no collided point + bool missed; +} OracleResult; + +typedef struct OraclePose +{ + b3BoxHull box; + b3Transform transform; + OracleReference reference; + + // Collided without and with ghost handling + OracleResult raw; + OracleResult handled; +} OraclePose; + +typedef struct OracleScan +{ + // Separated poses + int validCount; + + // Poses with a ghost manifold, without and with ghost handling + int rawGhostPoses; + int ghostPoses; + + // Poses where ghost handling lost the closest pair + int missedPoses; + + // Pose indices, -1 for none + int firstGhost; + int firstMissed; +} OracleScan; + +// The poses keep the hull separated by at least this much, where the closest direction is well defined +#define ORACLE_MIN_DISTANCE ( 0.1f * B3_LINEAR_SLOP ) + +// Caller destroys with b3DestroyMesh +b3MeshData* CreateOracleMesh( OracleScene scene ); + +// Deterministic random box and pose for the scene and index, separated from the mesh by +// [ORACLE_MIN_DISTANCE, B3_SPECULATIVE_DISTANCE], collided and judged with and without ghost handling. +// Returns false if no separated pose was found, in which case the reference and results mean nothing. +bool EvaluateOraclePose( OraclePose* pose, const b3MeshData* mesh, OracleScene scene, int index ); + +// A miss counts against ghost handling only when the collider represents the closest pair without it +bool IsOracleCullingMiss( const OraclePose* pose ); + +// Poses [0, poseCount) of the scene +OracleScan ScanGhostOracle( const b3MeshData* mesh, OracleScene scene, int poseCount ); + +#ifdef __cplusplus +} +#endif diff --git a/shared/mesh_drop.c b/shared/mesh_drop.c new file mode 100644 index 0000000..bf5df6e --- /dev/null +++ b/shared/mesh_drop.c @@ -0,0 +1,355 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "mesh_drop.h" + +#include "box3d/box3d.h" + +#include +#include + +#define WAVE_CELL_WIDTH 1.0f +#define WAVE_ROW_FREQUENCY 0.1f +#define WAVE_COLUMN_FREQUENCY 0.2f +#define DROP_SPACING 0.5f +#define DROP_HEIGHT 5.0f +#define BODY_RADIUS 0.05f +#define CAPSULE_HALF_LENGTH 0.2f + +const char* g_meshDropShapeNames[e_meshDropShapeCount] = { "box", "capsule", "cylinder", "sphere" }; + +const float g_meshDropDepthLevels[MESH_DROP_DEPTH_LEVEL_COUNT] = { 4.0f, 8.0f, 16.0f, 32.0f }; + +// Same XorShift32 and mapping as the samples, but with local state so worlds can run on many threads +static float RandomFloatRange( uint32_t* state, float lo, float hi ) +{ + uint32_t x = *state; + x ^= x << 13; + x ^= x >> 17; + x ^= x << 5; + *state = x; + + int i = (int)( x % 32768 ); + float r = (float)( i & 32767 ) / 32767.0f; + return ( hi - lo ) * r + lo; +} + +static b3Vec3 RandomVec3Uniform( uint32_t* state, float lo, float hi ) +{ + b3Vec3 v; + v.x = RandomFloatRange( state, lo, hi ); + v.y = RandomFloatRange( state, lo, hi ); + v.z = RandomFloatRange( state, lo, hi ); + return v; +} + +static uint32_t HashSeed( uint32_t x ) +{ + x ^= x >> 16; + x *= 0x7feb352du; + x ^= x >> 15; + x *= 0x846ca68bu; + x ^= x >> 16; + return x != 0 ? x : 1; +} + +static b3BoxHull MakeDropBox( void ) +{ + // The soft contact solver cannot always resolve a smaller box at 4 sub-steps, and its size gets + // close to the time of impact tolerances + return b3MakeBoxHull( 0.02f, 0.2f, 0.04f ); +} + +MeshDropSettings GetDefaultMeshDropSettings( void ) +{ + MeshDropSettings settings = { + .shape = e_meshDropBox, + .amplitude = 0.5f, + .collide = false, + .soloIndex = -1, + }; + return settings; +} + +uint32_t GetMeshDropSeed( uint32_t baseSeed, int generation ) +{ + return HashSeed( HashSeed( baseSeed ) + (uint32_t)generation ); +} + +int GetMeshDropStepLimit( MeshDropShape shape ) +{ + return shape == e_meshDropCapsule ? 3000 : 1000; +} + +float GetMeshDropTunnelHeight( void ) +{ + return -2.0f; +} + +b3MeshData* CreateMeshDropMesh( float amplitude ) +{ + return b3CreateWaveMesh( MESH_DROP_WAVE_COUNT, MESH_DROP_WAVE_COUNT, WAVE_CELL_WIDTH, amplitude, WAVE_ROW_FREQUENCY, + WAVE_COLUMN_FREQUENCY ); +} + +b3HullData* CreateMeshDropCylinder( void ) +{ + return b3CreateCylinder( 0.4f, BODY_RADIUS, 0.0f, 6 ); +} + +b3BodyId CreateMeshDropGround( b3WorldId worldId, const b3MeshData* mesh ) +{ + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.filter.categoryBits = 1; + b3CreateMeshShape( groundId, &shapeDef, mesh, b3Vec3_one ); + + float extent = 0.5f * MESH_DROP_WAVE_COUNT * WAVE_CELL_WIDTH; + float halfHeight = 1.0f; + + b3Transform transforms[4] = { + { { 0.0f, halfHeight, -extent }, b3Quat_identity }, + { { 0.0f, halfHeight, extent }, b3Quat_identity }, + { { -extent, halfHeight, 0.0f }, b3Quat_identity }, + { { extent, halfHeight, 0.0f }, b3Quat_identity }, + }; + + for ( int i = 0; i < 4; ++i ) + { + float hx = i < 2 ? extent : 0.1f; + float hz = i < 2 ? 0.1f : extent; + b3BoxHull wallBox = b3MakeTransformedBoxHull( hx, halfHeight, hz, transforms[i] ); + b3CreateHullShape( groundId, &shapeDef, &wallBox.base ); + } + + return groundId; +} + +void CreateMeshDropBodies( b3WorldId worldId, MeshDropSettings settings, uint32_t seed, const b3HullData* cylinder, + b3BodyId* bodies ) +{ + b3BoxHull box = MakeDropBox(); + b3Capsule capsule = { { 0.0f, -CAPSULE_HALF_LENGTH, 0.0f }, { 0.0f, CAPSULE_HALF_LENGTH, 0.0f }, BODY_RADIUS }; + b3Sphere sphere = { b3Vec3_zero, BODY_RADIUS }; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + bool round = settings.shape == e_meshDropCapsule || settings.shape == e_meshDropSphere; + shapeDef.baseMaterial.rollingResistance = round ? 0.4f : 0.1f; + + if ( settings.collide == false ) + { + shapeDef.filter.categoryBits = 2; + shapeDef.filter.maskBits = 1; + } + + uint32_t state = seed; + int index = 0; + for ( int i = 0; i < MESH_DROP_GRID_COUNT; ++i ) + { + for ( int j = 0; j < MESH_DROP_GRID_COUNT; ++j ) + { + // Draw for every body so a solo body gets the same velocity it has in the full drop + b3Vec3 linearVelocity = RandomVec3Uniform( &state, -1.0f, 1.0f ); + b3Vec3 angularVelocity = RandomVec3Uniform( &state, -5.0f, 5.0f ); + + if ( settings.soloIndex >= 0 && settings.soloIndex != index ) + { + bodies[index] = b3_nullBodyId; + index += 1; + continue; + } + + bodyDef.position = (b3Pos){ DROP_SPACING * ( i - 0.5f * MESH_DROP_GRID_COUNT ), DROP_HEIGHT, + DROP_SPACING * ( j - 0.5f * MESH_DROP_GRID_COUNT ) }; + bodyDef.linearVelocity = linearVelocity; + bodyDef.angularVelocity = angularVelocity; + bodyDef.userData = (void*)(intptr_t)( index + 1 ); + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + switch ( settings.shape ) + { + case e_meshDropBox: + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + break; + case e_meshDropCapsule: + b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + break; + case e_meshDropCylinder: + b3CreateHullShape( bodyId, &shapeDef, cylinder ); + break; + default: + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + break; + } + + bodies[index] = bodyId; + index += 1; + } + } +} + +int GetMeshDropBodyIndex( void* userData ) +{ + intptr_t value = (intptr_t)userData; + return 1 <= value && value <= MESH_DROP_BODY_COUNT ? (int)value - 1 : -1; +} + +void ResetMeshDropDepth( MeshDropDepth* depth, MeshDropSettings settings, const b3HullData* cylinder ) +{ + // Same heights as b3CreateWaveMesh + float omegaZ = 2.0f * B3_PI * WAVE_ROW_FREQUENCY * WAVE_CELL_WIDTH; + float omegaX = 2.0f * B3_PI * WAVE_COLUMN_FREQUENCY * WAVE_CELL_WIDTH; + int index = 0; + for ( int ix = 0; ix <= MESH_DROP_WAVE_COUNT; ++ix ) + { + float rowHeight = b3ComputeCosSin( omegaX * ix ).sine; + for ( int iz = 0; iz <= MESH_DROP_WAVE_COUNT; ++iz ) + { + float columnHeight = b3ComputeCosSin( omegaZ * iz ).sine; + depth->heights[index++] = settings.amplitude * rowHeight * columnHeight; + } + } + + depth->amplitude = fabsf( settings.amplitude ); + depth->probeCount = 0; + depth->radius = 0.0f; + + switch ( settings.shape ) + { + case e_meshDropBox: + case e_meshDropCylinder: + { + b3BoxHull box = MakeDropBox(); + const b3HullData* hull = settings.shape == e_meshDropBox ? &box.base : cylinder; + const b3Vec3* points = b3GetHullPoints( hull ); + int count = hull->vertexCount < MESH_DROP_MAX_PROBES ? hull->vertexCount : MESH_DROP_MAX_PROBES; + for ( int i = 0; i < count; ++i ) + { + depth->probes[i] = points[i]; + } + depth->probeCount = count; + } + break; + + case e_meshDropCapsule: + depth->probes[0] = (b3Vec3){ 0.0f, -CAPSULE_HALF_LENGTH, 0.0f }; + depth->probes[1] = (b3Vec3){ 0.0f, CAPSULE_HALF_LENGTH, 0.0f }; + depth->probeCount = 2; + depth->radius = BODY_RADIUS; + break; + + default: + depth->probes[0] = b3Vec3_zero; + depth->probeCount = 1; + depth->radius = BODY_RADIUS; + break; + } + + float maxLength = 0.0f; + for ( int i = 0; i < depth->probeCount; ++i ) + { + maxLength = fmaxf( maxLength, b3Length( depth->probes[i] ) ); + } + depth->boundingRadius = maxLength + depth->radius; + + for ( int i = 0; i < MESH_DROP_BODY_COUNT; ++i ) + { + depth->bodyDepths[i] = -FLT_MAX; + } + + depth->maxDepth = -FLT_MAX; + depth->maxIndex = -1; + depth->maxStep = 0; + depth->stepCount = 0; +} + +// Signed distance to the plane of the wave triangle under the point. Exact away from the folds, where +// it reads a little deep on a crest and shallow in a valley. +static float GetWaveDistance( const MeshDropDepth* depth, b3Vec3 p ) +{ + float origin = -0.5f * MESH_DROP_WAVE_COUNT * WAVE_CELL_WIDTH; + float u = ( p.x - origin ) / WAVE_CELL_WIDTH; + float v = ( p.z - origin ) / WAVE_CELL_WIDTH; + int ix = b3ClampInt( (int)floorf( u ), 0, MESH_DROP_WAVE_COUNT - 1 ); + int iz = b3ClampInt( (int)floorf( v ), 0, MESH_DROP_WAVE_COUNT - 1 ); + + int stride = MESH_DROP_WAVE_COUNT + 1; + float ha = depth->heights[ix * stride + iz]; + float hb = depth->heights[ix * stride + iz + 1]; + float hc = depth->heights[( ix + 1 ) * stride + iz + 1]; + float hd = depth->heights[( ix + 1 ) * stride + iz]; + + // The cell diagonal runs from a to c, matching b3CreateWaveMesh + float w = WAVE_CELL_WIDTH; + b3Vec3 a = { origin + ix * w, ha, origin + iz * w }; + b3Vec3 ac = { w, hc - ha, w }; + b3Vec3 side = v - iz >= u - ix ? (b3Vec3){ 0.0f, hb - ha, w } : (b3Vec3){ w, hd - ha, 0.0f }; + b3Vec3 normal = b3Normalize( b3Cross( side, ac ) ); + if ( normal.y < 0.0f ) + { + normal = b3Neg( normal ); + } + + return b3Dot( normal, b3Sub( p, a ) ); +} + +void UpdateMeshDropDepth( b3WorldId worldId, MeshDropDepth* depth ) +{ + depth->stepCount += 1; + + b3BodyEvents events = b3World_GetBodyEvents( worldId ); + for ( int i = 0; i < events.moveCount; ++i ) + { + const b3BodyMoveEvent* event = events.moveEvents + i; + int index = GetMeshDropBodyIndex( event->userData ); + if ( index < 0 ) + { + continue; + } + + b3WorldTransform transform = event->transform; + if ( (float)transform.p.y - depth->boundingRadius > depth->amplitude ) + { + continue; + } + + float sink = -FLT_MAX; + for ( int j = 0; j < depth->probeCount; ++j ) + { + b3Pos point = b3TransformWorldPoint( transform, depth->probes[j] ); + b3Vec3 p = { (float)point.x, (float)point.y, (float)point.z }; + sink = fmaxf( sink, depth->radius - GetWaveDistance( depth, p ) ); + } + + if ( sink > depth->bodyDepths[index] ) + { + depth->bodyDepths[index] = sink; + } + + if ( sink > depth->maxDepth ) + { + depth->maxDepth = sink; + depth->maxIndex = index; + depth->maxStep = depth->stepCount; + } + } +} + +void GetMeshDropDepthCounts( const MeshDropDepth* depth, int counts[MESH_DROP_DEPTH_LEVEL_COUNT] ) +{ + for ( int level = 0; level < MESH_DROP_DEPTH_LEVEL_COUNT; ++level ) + { + float threshold = g_meshDropDepthLevels[level] * B3_LINEAR_SLOP; + int count = 0; + for ( int i = 0; i < MESH_DROP_BODY_COUNT; ++i ) + { + count += depth->bodyDepths[i] > threshold ? 1 : 0; + } + counts[level] = count; + } +} diff --git a/shared/mesh_drop.h b/shared/mesh_drop.h new file mode 100644 index 0000000..9c19dc0 --- /dev/null +++ b/shared/mesh_drop.h @@ -0,0 +1,115 @@ +// 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 +#include + +#ifdef __cplusplus +extern "C" +{ +#endif + +// Mesh Drop scene, shared by the Continuous / Mesh Drop sample and the headless mesh soak. A generation +// drops a grid of small bodies with random velocities on a wave mesh and steps until they sleep. A body +// that falls out of the world tunneled. The depth tracker measures how far each body sinks into the +// wave, which flags contact trouble far more often than tunnels do. + +typedef enum MeshDropShape +{ + e_meshDropBox, + e_meshDropCapsule, + e_meshDropCylinder, + e_meshDropSphere, + e_meshDropShapeCount +} MeshDropShape; + +#define MESH_DROP_GRID_COUNT 32 +#define MESH_DROP_BODY_COUNT ( MESH_DROP_GRID_COUNT * MESH_DROP_GRID_COUNT ) +#define MESH_DROP_DEPTH_LEVEL_COUNT 4 + +// Wave mesh cells along x and z +#define MESH_DROP_WAVE_COUNT 40 + +// Enough for the cylinder hull +#define MESH_DROP_MAX_PROBES 16 + +extern const char* g_meshDropShapeNames[e_meshDropShapeCount]; + +// Sink depth thresholds in linear slop +extern const float g_meshDropDepthLevels[MESH_DROP_DEPTH_LEVEL_COUNT]; + +typedef struct MeshDropSettings +{ + MeshDropShape shape; + float amplitude; + + // Let the bodies collide with each other. Off isolates mesh collision. + bool collide; + + // Create only this grid index, or -1 for all. Without body collision a body moves the same alone. + int soloIndex; +} MeshDropSettings; + +MeshDropSettings GetDefaultMeshDropSettings( void ); + +// Hashes the base before adding the generation, so nearby base seeds share no generations +uint32_t GetMeshDropSeed( uint32_t baseSeed, int generation ); + +// Steps a generation may take to fall asleep +int GetMeshDropStepLimit( MeshDropShape shape ); + +// A body below this height tunneled +float GetMeshDropTunnelHeight( void ); + +b3MeshData* CreateMeshDropMesh( float amplitude ); +b3HullData* CreateMeshDropCylinder( void ); + +// Creates a static body with the wave mesh and four walls +b3BodyId CreateMeshDropGround( b3WorldId worldId, const b3MeshData* mesh ); + +// Creates the bodies of one generation in grid order. Bodies left out by the solo index are null. The +// body user data encodes the grid index, see GetMeshDropBodyIndex. +void CreateMeshDropBodies( b3WorldId worldId, MeshDropSettings settings, uint32_t seed, const b3HullData* cylinder, + b3BodyId* bodies ); + +// Grid index from body user data, or -1 if the body is not part of the drop +int GetMeshDropBodyIndex( void* userData ); + +typedef struct MeshDropDepth +{ + // Wave heights at the mesh vertices + float heights[( MESH_DROP_WAVE_COUNT + 1 ) * ( MESH_DROP_WAVE_COUNT + 1 )]; + + // Collision points in the body frame, rounded by the radius + b3Vec3 probes[MESH_DROP_MAX_PROBES]; + int probeCount; + float radius; + float boundingRadius; + float amplitude; + + // Deepest sink of each body so far, positive when inside the wave + float bodyDepths[MESH_DROP_BODY_COUNT]; + + float maxDepth; + int maxIndex; + int maxStep; + int stepCount; +} MeshDropDepth; + +// Starts tracking a new generation +void ResetMeshDropDepth( MeshDropDepth* depth, MeshDropSettings settings, const b3HullData* cylinder ); + +// Call after each world step +void UpdateMeshDropDepth( b3WorldId worldId, MeshDropDepth* depth ); + +// Number of bodies that sank deeper than each depth level +void GetMeshDropDepthCounts( const MeshDropDepth* depth, int counts[MESH_DROP_DEPTH_LEVEL_COUNT] ); + +#ifdef __cplusplus +} +#endif diff --git a/src/box3d.natvis b/src/box3d.natvis index 68d36e1..4283388 100644 --- a/src/box3d.natvis +++ b/src/box3d.natvis @@ -83,6 +83,10 @@ + + s: {separation} + + type: {type} diff --git a/src/contact.c b/src/contact.c index d200301..e5c9a60 100644 --- a/src/contact.c +++ b/src/contact.c @@ -581,6 +581,8 @@ static bool b3ComputeConvexManifold( b3World* world, int workerIndex, b3Contact* target->separation = source->separation; target->featureId = b3MakeFeatureId( source->pair ); target->triangleIndex = B3_NULL_INDEX; + target->triangleFeature = b3_featureNone; + target->meshFlags = 0; target->normalVelocity = 0.0f; } diff --git a/src/contact.h b/src/contact.h index c4388b3..5aae9a4 100644 --- a/src/contact.h +++ b/src/contact.h @@ -9,8 +9,6 @@ #include "box3d/collision.h" #include "box3d/types.h" -#define B3_FORCE_GHOST_COLLISIONS 0 - typedef struct b3Shape b3Shape; typedef struct b3World b3World; diff --git a/src/convex_manifold.c b/src/convex_manifold.c index 6f84f44..85337c7 100644 --- a/src/convex_manifold.c +++ b/src/convex_manifold.c @@ -990,7 +990,7 @@ static void b3ReduceManifoldPoints( b3LocalManifold* manifold, int capacity, b3L } b3Point2D hull[2 * B3_MAX_CLIP_POINTS]; - int hullCount = b3Hull2D( pts, count, hull ); + int hullCount = b3Hull2D( pts, count, hull, 0 ); int finalCount = b3SimplifyHull2D( hull, hullCount, target ); B3_ASSERT( 0 < finalCount && finalCount <= target ); diff --git a/src/height_field.c b/src/height_field.c index 37ff65f..13ec97e 100644 --- a/src/height_field.c +++ b/src/height_field.c @@ -99,6 +99,53 @@ 21 3 22 */ +static b3Vec3 b3GetGridPoint( const float* heights, int columnCount, b3Vec3 scale, int row, int column ) +{ + return b3Mul( scale, (b3Vec3){ (float)column, heights[row * columnCount + column], (float)row } ); +} + +static b3Vec3 b3GetGridTriangleNormal( const float* heights, int columnCount, b3Vec3 scale, int row, int column, bool odd ) +{ + b3Vec3 point11 = b3GetGridPoint( heights, columnCount, scale, row, column ); + b3Vec3 point12 = b3GetGridPoint( heights, columnCount, scale, row, column + 1 ); + b3Vec3 point21 = b3GetGridPoint( heights, columnCount, scale, row + 1, column ); + b3Vec3 point22 = b3GetGridPoint( heights, columnCount, scale, row + 1, column + 1 ); + + b3Vec3 v1 = odd ? point22 : point11; + b3Vec3 v2 = odd ? point12 : point21; + b3Vec3 v3 = odd ? point21 : point12; + return b3Normalize( b3Cross( b3Sub( v2, v1 ), b3Sub( v3, v1 ) ) ); +} + +// Mark vertices as flat or concave. +static void b3MarkGridEdge( uint8_t* vertexFlags, int index1, int index2, b3Vec3 point1, b3Vec3 point2, b3Vec3 normal1, + b3Vec3 normal2, bool clockwise ) +{ + b3Vec3 outward = b3Cross( b3Normalize( b3Sub( point2, point1 ) ), normal1 ); + float angle = b3Atan2( b3Dot( normal2, outward ), b3Dot( normal2, normal1 ) ); + + // Quantize to match the mesh rule. + int8_t quantized = b3QuantizeEdgeAngle( clockwise ? -angle : angle ); + + if ( quantized != 0 ) + { + vertexFlags[index1] = (uint8_t)( vertexFlags[index1] & ~b3_flatVertex ); + vertexFlags[index2] = (uint8_t)( vertexFlags[index2] & ~b3_flatVertex ); + } + + if ( quantized < 0 ) + { + vertexFlags[index1] |= b3_concaveVertex; + vertexFlags[index2] |= b3_concaveVertex; + } +} + +static void b3MarkGridBoundary( uint8_t* vertexFlags, int index1, int index2 ) +{ + vertexFlags[index1] = (uint8_t)( vertexFlags[index1] & ~b3_flatVertex ); + vertexFlags[index2] = (uint8_t)( vertexFlags[index2] & ~b3_flatVertex ); +} + b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data ) { int columnCount = data->countX; @@ -119,6 +166,8 @@ b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data ) byteCount += b3AlignUp8( cellCount * sizeof( uint8_t ) ); int flagsOffset = (int)byteCount; byteCount += b3AlignUp8( triangleCount * sizeof( uint8_t ) ); + int vertexFlagOffset = (int)byteCount; + byteCount += b3AlignUp8( (size_t)( ( heightCount + 7 ) / 8 ) ); // Zero the whole blob so alignment padding is defined. The construction-time hash // sweeps raw bytes and would otherwise pick up uninitialized padding. @@ -133,6 +182,7 @@ b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data ) hf->heightsOffset = heightsOffset; hf->materialOffset = materialOffset; hf->flagsOffset = flagsOffset; + hf->vertexFlagOffset = vertexFlagOffset; hf->clockwise = data->clockwiseWinding; uint16_t* compressedHeights = (uint16_t*)( (intptr_t)hf + heightsOffset ); @@ -466,6 +516,79 @@ b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data ) B3_ASSERT( triangleIndex == triangleCount ); + // b3MeshVertexFlags + uint8_t* vertexFlags = (uint8_t*)b3Alloc( heightCount * sizeof( uint8_t ) ); + memset( vertexFlags, b3_flatVertex, heightCount * sizeof( uint8_t ) ); + + bool clockwise = hf->clockwise != 0; + int cellColumnCount = columnCount - 1; + int cellRowCount = rowCount - 1; + + for ( int row = 0; row < rowCount; ++row ) + { + for ( int column = 0; column < columnCount; ++column ) + { + int index = row * columnCount + column; + b3Vec3 point = b3GetGridPoint( heights, columnCount, scale, row, column ); + + if ( column < cellColumnCount ) + { + bool below = row < cellRowCount && materialIndices[row * cellColumnCount + column] != B3_HEIGHT_FIELD_HOLE; + bool above = + row > 0 && materialIndices[( row - 1 ) * cellColumnCount + column] != B3_HEIGHT_FIELD_HOLE; + if ( below && above ) + { + b3Vec3 next = b3GetGridPoint( heights, columnCount, scale, row, column + 1 ); + b3Vec3 normal1 = b3GetGridTriangleNormal( heights, columnCount, scale, row, column, false ); + b3Vec3 normal2 = b3GetGridTriangleNormal( heights, columnCount, scale, row - 1, column, true ); + b3MarkGridEdge( vertexFlags, index + 1, index, next, point, normal1, normal2, clockwise ); + } + else if ( below || above ) + { + b3MarkGridBoundary( vertexFlags, index, index + 1 ); + } + } + + if ( row < cellRowCount ) + { + bool right = + column < cellColumnCount && materialIndices[row * cellColumnCount + column] != B3_HEIGHT_FIELD_HOLE; + bool left = column > 0 && materialIndices[row * cellColumnCount + column - 1] != B3_HEIGHT_FIELD_HOLE; + if ( right && left ) + { + b3Vec3 next = b3GetGridPoint( heights, columnCount, scale, row + 1, column ); + b3Vec3 normal1 = b3GetGridTriangleNormal( heights, columnCount, scale, row, column, false ); + b3Vec3 normal2 = b3GetGridTriangleNormal( heights, columnCount, scale, row, column - 1, true ); + b3MarkGridEdge( vertexFlags, index, index + columnCount, point, next, normal1, normal2, clockwise ); + } + else if ( right || left ) + { + b3MarkGridBoundary( vertexFlags, index, index + columnCount ); + } + } + + if ( row < cellRowCount && column < cellColumnCount && + materialIndices[row * cellColumnCount + column] != B3_HEIGHT_FIELD_HOLE ) + { + b3Vec3 point21 = b3GetGridPoint( heights, columnCount, scale, row + 1, column ); + b3Vec3 point12 = b3GetGridPoint( heights, columnCount, scale, row, column + 1 ); + b3Vec3 normal1 = b3GetGridTriangleNormal( heights, columnCount, scale, row, column, false ); + b3Vec3 normal2 = b3GetGridTriangleNormal( heights, columnCount, scale, row, column, true ); + b3MarkGridEdge( vertexFlags, index + columnCount, index + 1, point21, point12, normal1, normal2, clockwise ); + } + } + } + + uint8_t* vertexBits = (uint8_t*)( (intptr_t)hf + vertexFlagOffset ); + for ( int i = 0; i < heightCount; ++i ) + { + if ( vertexFlags[i] == 0 ) + { + vertexBits[i >> 3] |= (uint8_t)( 1 << ( i & 7 ) ); + } + } + + b3Free( vertexFlags, heightCount * sizeof( uint8_t ) ); b3Free( decompressedHeights, heightCount * sizeof( float ) ); hf->hash = 0; @@ -474,9 +597,6 @@ b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data ) return hf; } -_Static_assert( b3_concaveEdge3 == 4 * b3_concaveEdge1, "bit math" ); -_Static_assert( b3_inverseConcaveEdge3 == 4 * b3_inverseConcaveEdge1, "bit math" ); - // Decode the four corner vertices of a height field cell into local space. // Output order matches the index naming used throughout this file: // corners[0] = (column, row), corners[1] = (column + 1, row), @@ -518,8 +638,6 @@ b3Triangle b3GetHeightFieldTriangle( const b3HeightFieldData* heightField, int t B3_ASSERT( triangleIndex < 2 * ( heightField->columnCount - 1 ) * ( heightField->rowCount - 1 ) ); b3Triangle triangle; - triangle.flags = b3GetHeightFieldFlags( heightField )[triangleIndex]; - int columnCount = heightField->columnCount; int quadIndex = triangleIndex >> 1; int row = quadIndex / ( columnCount - 1 ); @@ -562,15 +680,6 @@ b3Triangle b3GetHeightFieldTriangle( const b3HeightFieldData* heightField, int t { B3_SWAP( triangle.vertices[1], triangle.vertices[2] ); B3_SWAP( triangle.i2, triangle.i3 ); - - // Reversing winding swaps edge1 and edge3; edge2 (the diagonal) is preserved. - int flags = triangle.flags; - int edge1Bits = flags & ( b3_concaveEdge1 | b3_inverseConcaveEdge1 ); - int edge3Bits = flags & ( b3_concaveEdge3 | b3_inverseConcaveEdge3 ); - flags &= ~( b3_concaveEdge1 | b3_concaveEdge3 | b3_inverseConcaveEdge1 | b3_inverseConcaveEdge3 ); - flags |= edge1Bits << 2; - flags |= edge3Bits >> 2; - triangle.flags = flags; } return triangle; @@ -585,6 +694,124 @@ int b3GetHeightFieldMaterial( const b3HeightFieldData* heightField, int triangle return b3GetHeightFieldMaterialIndices( heightField )[cellIndex]; } +bool b3GetHeightFieldNeighborNormal( const b3HeightFieldData* heightField, int triangleIndex, int edgeIndex, b3Vec3* normal ) +{ + B3_ASSERT( 0 <= triangleIndex && triangleIndex < b3GetHeightFieldTriangleCount( heightField ) ); + B3_ASSERT( 0 <= edgeIndex && edgeIndex < 3 ); + + if ( heightField->clockwise ) + { + // Reflected + edgeIndex = 2 - edgeIndex; + } + + int cellColumnCount = heightField->columnCount - 1; + int cellRowCount = heightField->rowCount - 1; + int cellIndex = triangleIndex >> 1; + int row = cellIndex / cellColumnCount; + int column = cellIndex - row * cellColumnCount; + bool odd = ( triangleIndex & 1 ) == 1; + + // Get the row and column for the neighbor cell. + if ( edgeIndex == 0 ) + { + column += odd ? 1 : -1; + } + else if ( edgeIndex == 2 ) + { + row += odd ? 1 : -1; + } + + if ( row < 0 || row >= cellRowCount || column < 0 || column >= cellColumnCount ) + { + return false; + } + + int neighborCell = row * cellColumnCount + column; + + const uint8_t* materialIndices = b3GetHeightFieldMaterialIndices( heightField ); + if ( materialIndices[neighborCell] == B3_HEIGHT_FIELD_HOLE ) + { + return false; + } + + b3Triangle neighbor = b3GetHeightFieldTriangle( heightField, 2 * neighborCell + ( odd ? 0 : 1 ) ); + b3Vec3 e1 = b3Sub( neighbor.vertices[1], neighbor.vertices[0] ); + b3Vec3 e2 = b3Sub( neighbor.vertices[2], neighbor.vertices[0] ); + *normal = b3Normalize( b3Cross( e1, e2 ) ); + return true; +} + +bool b3IsHeightFieldVertexConvex( const b3HeightFieldData* heightField, int triangleIndex, int vertexIndex ) +{ + B3_ASSERT( 0 <= triangleIndex && triangleIndex < b3GetHeightFieldTriangleCount( heightField ) ); + B3_ASSERT( 0 <= vertexIndex && vertexIndex < 3 ); + + if ( heightField->vertexFlagOffset == 0 ) + { + return false; + } + + if ( heightField->clockwise ) + { + vertexIndex = ( 3 - vertexIndex ) % 3; + } + + int columnCount = heightField->columnCount; + int cellIndex = triangleIndex >> 1; + int row = cellIndex / ( columnCount - 1 ); + int column = cellIndex - row * ( columnCount - 1 ); + + int offsets[2][3] = { + { 0, columnCount, 1 }, + { columnCount + 1, 1, columnCount }, + }; + + int index = row * columnCount + column + offsets[triangleIndex & 1][vertexIndex]; + const uint8_t* vertexBits = (const uint8_t*)( (intptr_t)heightField + heightField->vertexFlagOffset ); + return ( vertexBits[index >> 3] & ( 1 << ( index & 7 ) ) ) != 0; +} + +int b3GetHeightFieldVertexTriangles( const b3HeightFieldData* heightField, int vertex, int triangles[6] ) +{ + int columnCount = heightField->columnCount; + int cellColumnCount = columnCount - 1; + int cellRowCount = heightField->rowCount - 1; + int row = vertex / columnCount; + int column = vertex - row * columnCount; + + int rowOffsets[4] = { 0, 0, -1, -1 }; + int columnOffsets[4] = { 0, -1, 0, -1 }; + int firstParity[4] = { 0, 0, 0, 1 }; + int lastParity[4] = { 0, 1, 1, 1 }; + + const uint8_t* materialIndices = b3GetHeightFieldMaterialIndices( heightField ); + int count = 0; + for ( int i = 0; i < 4; ++i ) + { + int cellRow = row + rowOffsets[i]; + int cellColumn = column + columnOffsets[i]; + if ( cellRow < 0 || cellRow >= cellRowCount || cellColumn < 0 || cellColumn >= cellColumnCount ) + { + continue; + } + + int cellIndex = cellRow * cellColumnCount + cellColumn; + if ( materialIndices[cellIndex] == B3_HEIGHT_FIELD_HOLE ) + { + continue; + } + + for ( int parity = firstParity[i]; parity <= lastParity[i]; ++parity ) + { + triangles[count] = 2 * cellIndex + parity; + count += 1; + } + } + + return count; +} + b3AABB b3ComputeHeightFieldAABB( const b3HeightFieldData* shape, b3Transform transform ) { return b3AABB_Transform( transform, shape->aabb ); diff --git a/src/hull.c b/src/hull.c index b0392d8..dad8468 100644 --- a/src/hull.c +++ b/src/hull.c @@ -7,6 +7,7 @@ #include "algorithm.h" #include "math_internal.h" +#include "qsort.h" #include "shape.h" #include "simd.h" @@ -398,7 +399,8 @@ static void b3FindFarthestPointsAlongCardinalAxes( int* index1Out, int* index2Ou } } -static int b3FindFarthestPointFromLine( int index1, int index2, float tolerance, int vertexCount, const b3Vec3* vertexBase ) +static int b3FindFarthestPointFromLine( int index1, int index2, float tolerance, int vertexCount, + const b3Vec3* vertexBase ) { b3Vec3 a = vertexBase[index1]; b3Vec3 b = vertexBase[index2]; @@ -764,7 +766,8 @@ static void b3HullBuilder_RecacheConflicts( b3QHFace* face, float minOutside ) b3QHVertex* maxVertex = NULL; float maxDistance = minOutside; - for ( b3QHListNode* node = face->conflictListHead.link.next; node != &face->conflictListHead.link; node = node->next ) + for ( b3QHListNode* node = face->conflictListHead.link.next; node != &face->conflictListHead.link; + node = node->next ) { b3QHVertex* vertex = (b3QHVertex*)node; float distance = b3PlaneSeparation( face->plane, vertex->position ); @@ -784,7 +787,8 @@ static b3QHVertex* b3HullBuilder_NextConflictVertex( const b3HullBuilder* b ) b3QHVertex* maxVertex = NULL; float maxDistance = b->minOutside; - for ( const b3QHListNode* faceNode = b->faceList.link.next; faceNode != &b->faceList.link; faceNode = faceNode->next ) + for ( const b3QHListNode* faceNode = b->faceList.link.next; faceNode != &b->faceList.link; + faceNode = faceNode->next ) { const b3QHFace* face = (const b3QHFace*)faceNode; if ( face->maxConflict != NULL && face->maxConflictDistance > maxDistance ) @@ -817,7 +821,8 @@ static void b3HullBuilder_DrainConflictList( b3HullBuilder* b, b3QHFace* face ) // `entryEdge` is the half-edge in `face` whose twin lies in the just-deleted parent face, or // NULL for the seed. The frame skips `entryEdge` on recursive entries (it would be ignored // anyway since the parent's mark is now DELETE, but skipping saves one iteration). -static void b3HullBuilder_EnterHorizonFace( b3HullBuilder* b, b3QHFace* face, b3QHHalfEdge* entryEdge, b3HorizonFrame* frameOut ) +static void b3HullBuilder_EnterHorizonFace( b3HullBuilder* b, b3QHFace* face, b3QHHalfEdge* entryEdge, + b3HorizonFrame* frameOut ) { face->mark = B3_MARK_DELETE; b3HullBuilder_DrainConflictList( b, face ); @@ -1357,7 +1362,8 @@ static bool b3HullBuilder_IsConsistent( const b3HullBuilder* b ) return false; } - for ( const b3QHListNode* faceNode = b->faceList.link.next; faceNode != &b->faceList.link; faceNode = faceNode->next ) + for ( const b3QHListNode* faceNode = b->faceList.link.next; faceNode != &b->faceList.link; + faceNode = faceNode->next ) { const b3QHFace* face = (const b3QHFace*)faceNode; if ( face->edge->face != face ) @@ -1427,8 +1433,8 @@ static bool b3HullBuilder_HasHull( const b3HullBuilder* b ) } // Build the entire hull. Returns true iff the result satisfies Euler's identity. -static bool b3HullBuilder_Construct( b3HullBuilder* b, const b3Vec3* points, int pointCount, int maxVertexCount, b3Vec3 origin, - b3Vec3* shiftedPoints ) +static bool b3HullBuilder_Construct( b3HullBuilder* b, const b3Vec3* points, int pointCount, int maxVertexCount, + b3Vec3 origin, b3Vec3* shiftedPoints ) { if ( pointCount < 4 ) { @@ -2195,7 +2201,8 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou int faceCount = 0; int edgeCount = 0; - for ( b3QHListNode* faceNode = builder.faceList.link.next; faceNode != &builder.faceList.link; faceNode = faceNode->next ) + for ( b3QHListNode* faceNode = builder.faceList.link.next; faceNode != &builder.faceList.link; + faceNode = faceNode->next ) { B3_ASSERT( faceCount < B3_HULL_MAX_COUNT ); @@ -3098,20 +3105,20 @@ int b3SimplifyHull2D( b3Point2D* hull, int count1, int target ) return count2; } -// Sort comparator: by x, then by y. Insertion since n is typically 20 or less. -static void b3Sort2D( b3Point2D* pts, int count ) +// Comparator for sorting points for the 2D hull. +static inline bool b3IsPoint2DLess( const b3Point2D* a, const b3Point2D* b ) { - for ( int i = 1; i < count; ++i ) + if ( a->p.x != b->p.x ) { - b3Point2D base = pts[i]; - int j = i - 1; - while ( j >= 0 && ( pts[j].p.x > base.p.x || ( pts[j].p.x == base.p.x && pts[j].p.y > base.p.y ) ) ) - { - pts[j + 1] = pts[j]; - j -= 1; - } - pts[j + 1] = base; + return a->p.x < b->p.x; } + + if ( a->p.y != b->p.y ) + { + return a->p.y < b->p.y; + } + + return a->originalIndex < b->originalIndex; } // Welds points that have been sorted O(n) time @@ -3148,79 +3155,151 @@ static int b3Weld2D( b3Point2D* pts, int count1 ) return count2; } +// Only merge points that line up exactly. +static int b3MergeExact2D( b3Point2D* pts, int count1 ) +{ + float separationTol = B3_LINEAR_SLOP; + + int count2 = 0; + for ( int i = 0; i < count1; i++ ) + { + b3Point2D point = pts[i]; + if ( count2 > 0 && pts[count2 - 1].p.x == point.p.x && pts[count2 - 1].p.y == point.p.y ) + { + if ( point.separation < pts[count2 - 1].separation - separationTol ) + { + pts[count2 - 1] = point; + } + } + else + { + pts[count2] = point; + count2 += 1; + } + } + + return count2; +} + // Andrew's monotone chain convex hull algorithm // hull must have space for 2 * count1 points -int b3Hull2D( b3Point2D* pts, int count, b3Point2D* hull ) +int b3Hull2D( b3Point2D* pts, int count, b3Point2D* hull, int flags ) { - int count1 = count; - - if ( count1 <= 0 ) + if ( count <= 0 ) { return 0; } - if ( count1 == 1 ) +#define LESS( i, j ) b3IsPoint2DLess( pts + (int)( i ), pts + (int)( j ) ) +#define SWAP( i, j ) \ + do \ + { \ + b3Point2D swapPoint = pts[(int)( i )]; \ + pts[(int)( i )] = pts[(int)( j )]; \ + pts[(int)( j )] = swapPoint; \ + } \ + while ( 0 ) + + QSORT( count, LESS, SWAP ); + +#undef LESS +#undef SWAP + + int count1; + if ( flags & b3_hull2DExactMerge ) { - hull[0] = pts[0]; - return 1; + count1 = b3MergeExact2D( pts, count ); + } + else + { + count1 = b3Weld2D( pts, count ); } - b3Sort2D( pts, count1 ); - count1 = b3Weld2D( pts, count1 ); - - if ( count1 == 1 ) - { - hull[0] = pts[0]; - return 1; - } - - if ( count1 == 2 ) - { - hull[0] = pts[0]; - hull[1] = pts[1]; - return 2; - } + bool keepCollinear = ( flags & b3_hull2DKeepCollinear ) != 0; int count2 = 0; - - for ( int i = 0; i < count1; i++ ) + if ( count1 <= 2 ) { - while ( count2 >= 2 ) + for ( int i = 0; i < count1; i++ ) { - float area = b3Cross2D( hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p ); - if ( area > 0.0f ) - { - break; - } - - count2 -= 1; + hull[i] = pts[i]; } - B3_VALIDATE( count2 < 2 * count1 ); - hull[count2] = pts[i]; - count2 += 1; + count2 = count1; } - - int lowerCount = count2 + 1; - - for ( int i = count1 - 2; i >= 0; i-- ) + else { - while ( count2 >= lowerCount ) + for ( int i = 0; i < count1; i++ ) { - float area = b3Cross2D( hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p ); - if ( area > 0.0f ) + while ( count2 >= 2 ) { - break; + float area = b3Cross2D( hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p ); + if ( area > 0.0f || ( keepCollinear && area == 0.0f ) ) + { + break; + } + + count2 -= 1; } - count2 -= 1; + B3_VALIDATE( count2 < 2 * count1 ); + hull[count2] = pts[i]; + count2 += 1; } - B3_VALIDATE( count2 < 2 * count1 ); - hull[count2] = pts[i]; - count2 += 1; + int lowerCount = count2 + 1; + int lowerIndex = count2 - 2; + + for ( int i = count1 - 2; i >= 0; i-- ) + { + if ( lowerIndex >= 0 && hull[lowerIndex].p.x == pts[i].p.x && hull[lowerIndex].p.y == pts[i].p.y ) + { + lowerIndex -= 1; + if ( i > 0 ) + { + continue; + } + } + + while ( count2 >= lowerCount ) + { + float area = b3Cross2D( hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p ); + if ( area > 0.0f || ( keepCollinear && area == 0.0f ) ) + { + break; + } + + count2 -= 1; + } + + B3_VALIDATE( count2 < 2 * count1 ); + hull[count2] = pts[i]; + count2 += 1; + } + + B3_VALIDATE( hull[0].originalIndex == hull[count2 - 1].originalIndex ); + count2 -= 1; } - B3_VALIDATE( hull[0].originalIndex == hull[count2 - 1].originalIndex ); - return count2 - 1; + B3_VALIDATE( count2 <= count1 ); + + if ( flags & b3_hull2DIndexOrder ) + { +#define LESS( i, j ) ( hull[(int)( i )].originalIndex < hull[(int)( j )].originalIndex ) +#define SWAP( i, j ) \ + do \ + { \ + b3Point2D swapPoint = hull[(int)( i )]; \ + hull[(int)( i )] = hull[(int)( j )]; \ + hull[(int)( j )] = swapPoint; \ + } \ + while ( 0 ) + + QSORT( count2, LESS, SWAP ); + +#undef LESS +#undef SWAP + } + + return count2; } diff --git a/src/manifold.h b/src/manifold.h index 577c5d0..9f0e662 100644 --- a/src/manifold.h +++ b/src/manifold.h @@ -51,6 +51,12 @@ 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 ); + +void b3BuildTriangleAndHullFaceContact( b3LocalManifold* manifold, int capacity, const b3Vec3* triangle, const b3HullData* hull, + b3SATCache* cache, bool enableSpeculative ); + 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/math_functions.c b/src/math_functions.c index a339d95..e533fdc 100644 --- a/src/math_functions.c +++ b/src/math_functions.c @@ -166,49 +166,6 @@ bool b3IsValidWorldTransform( b3WorldTransform t ) return b3IsValidPosition( t.p ) && b3IsValidQuat( t.q ); } -// https://stackoverflow.com/questions/46210708/atan2-approximation-with-11bits-in-mantissa-on-x86with-sse2-and-armwith-vfpv4 -float b3Atan2( float y, float x ) -{ - // Added check for (0,0) to match atan2f and avoid NaN - if ( x == 0.0f && y == 0.0f ) - { - return 0.0f; - } - - float ax = b3AbsFloat( x ); - float ay = b3AbsFloat( y ); - float mx = b3MaxFloat( ay, ax ); - float mn = b3MinFloat( ay, ax ); - float a = mn / mx; - - // Minimax polynomial approximation to atan(a) on [0,1] - float s = a * a; - float c = s * a; - float q = s * s; - float r = 0.024840285f * q + 0.18681418f; - float t = -0.094097948f * q - 0.33213072f; - r = r * s + t; - r = r * c + a; - - // Map to full circle - if ( ay > ax ) - { - r = 1.57079637f - r; - } - - if ( x < 0 ) - { - r = 3.14159274f - r; - } - - if ( y < 0 ) - { - r = -r; - } - - return r; -} - // Approximate cosine and sine for determinism. In my testing cosf and sinf produced // the same results on x64 and ARM using MSVC, GCC, and Clang. However, I don't trust // this result. diff --git a/src/math_internal.h b/src/math_internal.h index b272bdc..be8418d 100644 --- a/src/math_internal.h +++ b/src/math_internal.h @@ -28,7 +28,6 @@ typedef struct b3Triangle { b3Vec3 vertices[3]; int i1, i2, i3; - int flags; } b3Triangle; typedef struct b3TrianglePoint diff --git a/src/mesh.c b/src/mesh.c index 7b8d617..fb4484e 100644 --- a/src/mesh.c +++ b/src/mesh.c @@ -128,6 +128,26 @@ static uint8_t* b3GetMeshFlagsWrite( b3MeshData* mesh ) return (uint8_t*)( (intptr_t)mesh + mesh->flagsOffset ); } +static int8_t* b3GetMeshEdgeAnglesWrite( b3MeshData* mesh ) +{ + if ( mesh->edgeAngleOffset == 0 ) + { + return NULL; + } + + return (int8_t*)( (intptr_t)mesh + mesh->edgeAngleOffset ); +} + +static uint8_t* b3GetMeshVertexFlagsWrite( b3MeshData* mesh ) +{ + if ( mesh->vertexFlagOffset == 0 ) + { + return NULL; + } + + return (uint8_t*)( (intptr_t)mesh + mesh->vertexFlagOffset ); +} + static int b3GetNodeHeight( const b3MeshNode* node ) { if ( b3IsLeaf( node ) ) @@ -361,7 +381,8 @@ typedef struct b3SpatialHash float tolerance; } b3SpatialHash; -static void b3CreateSpatialHash( b3SpatialHash* h, const b3Vec3* vertices, int vertexCount, size_t stride, float tolerance ) +static void b3CreateSpatialHash( b3SpatialHash* h, const b3Vec3* vertices, int vertexCount, size_t stride, + float tolerance ) { h->vertices = vertices; h->vertexCount = vertexCount; @@ -553,10 +574,70 @@ typedef struct b3Primitive int triangleIndex; } b3Primitive; +_Static_assert( offsetof( b3Primitive, aabb.upperBound ) == 3 * sizeof( float ) && + offsetof( b3Primitive, center ) == 6 * sizeof( float ), + "primitive floats must be contiguous" ); + +static inline b3FloatW4 b3PrimitiveMinW4( b3FloatW4 a, b3FloatW4 b ) +{ +#if defined( B3_SIMD_SSE2 ) + return b3MinW4( a, b ); +#else + return b3BlendW4( b, a, b3LessThanW4( a, b ) ); +#endif +} + +static inline b3FloatW4 b3PrimitiveMaxW4( b3FloatW4 a, b3FloatW4 b ) +{ +#if defined( B3_SIMD_SSE2 ) + return b3MaxW4( a, b ); +#else + return b3BlendW4( b, a, b3GreaterThanW4( a, b ) ); +#endif +} + +static inline b3Vec3 b3PrimitiveLanes( b3FloatW4 v, int first ) +{ + float lanes[4]; + b3StoreW4( lanes, v ); + return (b3Vec3){ lanes[first], lanes[first + 1], lanes[first + 2] }; +} + +static b3AABB b3PrimitiveBounds( const b3Primitive* primitives, int count ) +{ + b3FloatW4 lower = b3SplatW4( FLT_MAX ); + b3FloatW4 upper = b3SplatW4( -FLT_MAX ); + + for ( int i = 0; i < count; ++i ) + { + const float* p = &primitives[i].aabb.lowerBound.x; + lower = b3PrimitiveMinW4( lower, b3LoadW4( p ) ); + upper = b3PrimitiveMaxW4( upper, b3LoadW4( p + 2 ) ); + } + + return (b3AABB){ b3PrimitiveLanes( lower, 0 ), b3PrimitiveLanes( upper, 1 ) }; +} + +static b3AABB b3PrimitiveCenterBounds( const b3Primitive* primitives, int count ) +{ + b3FloatW4 lower = b3LoadW4( &primitives[0].aabb.lowerBound.x + 5 ); + b3FloatW4 upper = lower; + + for ( int i = 1; i < count; ++i ) + { + b3FloatW4 center = b3LoadW4( &primitives[i].aabb.lowerBound.x + 5 ); + lower = b3PrimitiveMinW4( lower, center ); + upper = b3PrimitiveMaxW4( upper, center ); + } + + return (b3AABB){ b3PrimitiveLanes( lower, 1 ), b3PrimitiveLanes( upper, 1 ) }; +} + typedef struct b3Bucket { + b3FloatW4 lower; + b3FloatW4 upper; int count; - b3AABB bounds; } b3Bucket; typedef struct b3Split @@ -574,11 +655,9 @@ static b3Split b3SplitBinnedSah( int count, b3Primitive* primitives ) split.index = -1; // Compute bounds of primitive centroids and choose split axis - b3AABB bounds = { primitives[0].center, primitives[0].center }; - for ( int i = 1; i < count; ++i ) - { - bounds = b3AABB_AddPoint( bounds, primitives[i].center ); - } + b3AABB bounds = b3PrimitiveCenterBounds( primitives, count ); + b3FloatW4 emptyLower = b3SplatW4( FLT_MAX ); + b3FloatW4 emptyUpper = b3SplatW4( -FLT_MAX ); // Compute costs for splitting after each bucket and keep track of best split // This is a small O(n^2) loop. This can be further optimized, but it is already @@ -586,54 +665,90 @@ static b3Split b3SplitBinnedSah( int count, b3Primitive* primitives ) int bestBucket = -1; float bestCost = FLT_MAX; + b3Vec3 extent = b3AABB_Extents( bounds ); + bool binAxis[3]; + float factors[3]; + b3Bucket axisBuckets[3][B3_BIN_COUNT]; for ( int axis = 0; axis < 3; ++axis ) { - b3Vec3 extent = b3AABB_Extents( bounds ); - if ( b3GetByIndex( extent, axis ) < B3_LINEAR_SLOP ) + binAxis[axis] = b3GetByIndex( extent, axis ) >= B3_LINEAR_SLOP; + factors[axis] = binAxis[axis] ? B3_BIN_COUNT * ( 1.0f - FLT_EPSILON ) / + ( b3GetByIndex( bounds.upperBound, axis ) - b3GetByIndex( bounds.lowerBound, axis ) ) + : 0.0f; + + for ( int i = 0; i < B3_BIN_COUNT; ++i ) + { + axisBuckets[axis][i].lower = emptyLower; + axisBuckets[axis][i].upper = emptyUpper; + axisBuckets[axis][i].count = 0; + } + } + + for ( int i = 0; i < count; ++i ) + { + b3Vec3 center = primitives[i].center; + int indexX = (int)( factors[0] * ( center.x - bounds.lowerBound.x ) ); + int indexY = (int)( factors[1] * ( center.y - bounds.lowerBound.y ) ); + int indexZ = (int)( factors[2] * ( center.z - bounds.lowerBound.z ) ); + B3_ASSERT( 0 <= indexX && indexX < B3_BIN_COUNT ); + B3_ASSERT( 0 <= indexY && indexY < B3_BIN_COUNT ); + B3_ASSERT( 0 <= indexZ && indexZ < B3_BIN_COUNT ); + + const float* p = &primitives[i].aabb.lowerBound.x; + b3FloatW4 lower = b3LoadW4( p ); + b3FloatW4 upper = b3LoadW4( p + 2 ); + + b3Bucket* bucketX = axisBuckets[0] + indexX; + bucketX->count += 1; + bucketX->lower = b3PrimitiveMinW4( bucketX->lower, lower ); + bucketX->upper = b3PrimitiveMaxW4( bucketX->upper, upper ); + + b3Bucket* bucketY = axisBuckets[1] + indexY; + bucketY->count += 1; + bucketY->lower = b3PrimitiveMinW4( bucketY->lower, lower ); + bucketY->upper = b3PrimitiveMaxW4( bucketY->upper, upper ); + + b3Bucket* bucketZ = axisBuckets[2] + indexZ; + bucketZ->count += 1; + bucketZ->lower = b3PrimitiveMinW4( bucketZ->lower, lower ); + bucketZ->upper = b3PrimitiveMaxW4( bucketZ->upper, upper ); + } + + for ( int axis = 0; axis < 3; ++axis ) + { + if ( binAxis[axis] == false ) { continue; } - // Initialize buckets - b3Bucket buckets[B3_BIN_COUNT]; - for ( int i = 0; i < B3_BIN_COUNT; ++i ) - { - buckets[i].count = 0; - buckets[i].bounds = B3_BOUNDS3_EMPTY; - } - - // Fill buckets - float factor = B3_BIN_COUNT * ( 1.0f - FLT_EPSILON ) / - ( b3GetByIndex( bounds.upperBound, axis ) - b3GetByIndex( bounds.lowerBound, axis ) ); - for ( int i = 0; i < count; ++i ) - { - b3Vec3 center = primitives[i].center; - int index = (int)( factor * ( b3GetByIndex( center, axis ) - b3GetByIndex( bounds.lowerBound, axis ) ) ); - B3_ASSERT( 0 <= index && index < B3_BIN_COUNT ); - - buckets[index].count++; - buckets[index].bounds = b3AABB_Union( buckets[index].bounds, primitives[i].aabb ); - } + b3Bucket* buckets = axisBuckets[axis]; // Evaluate splits for ( int i = 0; i < B3_BIN_COUNT - 1; ++i ) { int leftCount = 0; - b3AABB leftBounds = B3_BOUNDS3_EMPTY; + b3FloatW4 leftLower = emptyLower; + b3FloatW4 leftUpper = emptyUpper; for ( int k = 0; k <= i; ++k ) { leftCount += buckets[k].count; - leftBounds = b3AABB_Union( leftBounds, buckets[k].bounds ); + leftLower = b3PrimitiveMinW4( leftLower, buckets[k].lower ); + leftUpper = b3PrimitiveMaxW4( leftUpper, buckets[k].upper ); } int rightCount = 0; - b3AABB rightBounds = B3_BOUNDS3_EMPTY; + b3FloatW4 rightLower = emptyLower; + b3FloatW4 rightUpper = emptyUpper; for ( int k = i + 1; k < B3_BIN_COUNT; ++k ) { rightCount += buckets[k].count; - rightBounds = b3AABB_Union( rightBounds, buckets[k].bounds ); + rightLower = b3PrimitiveMinW4( rightLower, buckets[k].lower ); + rightUpper = b3PrimitiveMaxW4( rightUpper, buckets[k].upper ); } + b3AABB leftBounds = { b3PrimitiveLanes( leftLower, 0 ), b3PrimitiveLanes( leftUpper, 1 ) }; + b3AABB rightBounds = { b3PrimitiveLanes( rightLower, 0 ), b3PrimitiveLanes( rightUpper, 1 ) }; + B3_ASSERT( leftCount + rightCount == count ); if ( leftCount > 0 && rightCount > 0 ) { @@ -685,18 +800,8 @@ static b3Split b3SplitHalf( int count, b3Primitive* primitives ) // Split in the middle int splitIndex = count / 2; - b3AABB leftBounds = B3_BOUNDS3_EMPTY; - for ( int i = 0; i < splitIndex; ++i ) - { - leftBounds = b3AABB_Union( leftBounds, primitives[i].aabb ); - } - - b3AABB rightBounds = B3_BOUNDS3_EMPTY; - for ( int i = splitIndex; i < count; ++i ) - { - - rightBounds = b3AABB_Union( rightBounds, primitives[i].aabb ); - } + b3AABB leftBounds = b3PrimitiveBounds( primitives, splitIndex ); + b3AABB rightBounds = b3PrimitiveBounds( primitives + splitIndex, count - splitIndex ); b3AABB bounds = b3AABB_Union( leftBounds, rightBounds ); int axis = b3MajorAxis( b3AABB_Extents( bounds ) ); @@ -714,14 +819,9 @@ static b3Split b3SplitMedian( int count, b3Primitive* primitives ) { B3_ASSERT( count > 2 ); - b3Vec3 lowerBound = primitives[0].center; - b3Vec3 upperBound = primitives[0].center; - - for ( int i = 1; i < count; ++i ) - { - lowerBound = b3Min( lowerBound, primitives[i].center ); - upperBound = b3Max( upperBound, primitives[i].center ); - } + b3AABB centerBounds = b3PrimitiveCenterBounds( primitives, count ); + b3Vec3 lowerBound = centerBounds.lowerBound; + b3Vec3 upperBound = centerBounds.upperBound; b3Vec3 d = b3Sub( upperBound, lowerBound ); b3Vec3 c = b3MulSV( 0.5f, b3Add( lowerBound, upperBound ) ); @@ -832,21 +932,9 @@ static b3Split b3SplitMedian( int count, b3Primitive* primitives ) i1 = count / 2; } - b3AABB leftBounds = B3_BOUNDS3_EMPTY; - for ( int i = 0; i < i1; ++i ) - { - leftBounds = b3AABB_Union( leftBounds, primitives[i].aabb ); - } - - b3AABB rightBounds = B3_BOUNDS3_EMPTY; - for ( int i = i1; i < count; ++i ) - { - rightBounds = b3AABB_Union( rightBounds, primitives[i].aabb ); - } - split.index = i1; - split.leftBounds = leftBounds; - split.rightBounds = rightBounds; + split.leftBounds = b3PrimitiveBounds( primitives, i1 ); + split.rightBounds = b3PrimitiveBounds( primitives + i1, count - i1 ); return split; } @@ -905,11 +993,7 @@ static int b3BuildRecursive( b3Array( b3MeshNode ) * nodes, int count, b3Primiti } else { - b3AABB bounds = B3_BOUNDS3_EMPTY; - for ( int i = 0; i < count; ++i ) - { - bounds = b3AABB_Union( bounds, primitives[i].aabb ); - } + b3AABB bounds = b3PrimitiveBounds( primitives, count ); // We have only a few triangles left. Create a leaf. int index = b3Array_AddIndex( *nodes ); @@ -924,8 +1008,8 @@ static int b3BuildRecursive( b3Array( b3MeshNode ) * nodes, int count, b3Primiti int index = b3Array_AddIndex( *nodes ); int heightLeft = 0, heightRight = 0; int leftIndex = b3BuildRecursive( nodes, split.index, primitives, base, useMedianSplit, &heightLeft ); - int rightIndex = - b3BuildRecursive( nodes, count - split.index, primitives + split.index, base, useMedianSplit, &heightRight ); + int rightIndex = b3BuildRecursive( nodes, count - split.index, primitives + split.index, base, useMedianSplit, + &heightRight ); *height = b3MaxInt( heightLeft, heightRight ) + 1; @@ -945,11 +1029,7 @@ static int b3BuildRecursive( b3Array( b3MeshNode ) * nodes, int count, b3Primiti return index; } - b3AABB aabb = B3_BOUNDS3_EMPTY; - for ( int i = 0; i < count; ++i ) - { - aabb = b3AABB_Union( aabb, primitives[i].aabb ); - } + b3AABB aabb = b3PrimitiveBounds( primitives, count ); int index = b3Array_AddIndex( *nodes ); b3StoreLeaf( &nodes->data[index], &aabb, count, (int)( primitives - base ) ); @@ -1034,44 +1114,570 @@ typedef struct uint8_t triangleEdgeIndex2; } b3MeshEdge; -#define NAME b3EdgeMap -#define KEY_TY uint64_t -#define VAL_TY int -#define HASH_FN vt_hash_integer -#define CMPR_FN vt_cmpr_integer -#define MALLOC_FN b3Alloc -#define FREE_FN b3Free -#include "verstable.h" +#define B3_EDGE_ANGLE_STEPS 127.0f -#if 0 -// todo this is for testing other hash tables -struct b3TestHash -{ - size_t operator()( uint64_t key ) const - { - key ^= key >> 23; - key *= 0x2127599bf4325c37ull; - key ^= key >> 47; - return (size_t)key; - } +// Lookup table for 8-bit compressed triangle neighbor angles. +static const b3CosSin s_quantizedCosSin[255] = { + { 1.0f, 0.0f }, + { 0.9999235f, 0.012368159f }, + { 0.99969405f, 0.024734428f }, + { 0.9993117f, 0.03709691f }, + { 0.99877644f, 0.04945372f }, + { 0.99808836f, 0.061802965f }, + { 0.99724764f, 0.074142754f }, + { 0.9962543f, 0.0864712f }, + { 0.99510866f, 0.09878642f }, + { 0.9938107f, 0.111086525f }, + { 0.9923608f, 0.12336964f }, + { 0.990759f, 0.13563387f }, + { 0.9890057f, 0.14787737f }, + { 0.9871011f, 0.16009824f }, + { 0.9850455f, 0.17229462f }, + { 0.98283917f, 0.18446463f }, + { 0.98048246f, 0.19660644f }, + { 0.97797585f, 0.20871817f }, + { 0.97531956f, 0.22079796f }, + { 0.9725141f, 0.23284398f }, + { 0.96955985f, 0.24485438f }, + { 0.9664573f, 0.25682732f }, + { 0.9632069f, 0.26876098f }, + { 0.9598091f, 0.2806535f }, + { 0.95626456f, 0.29250312f }, + { 0.9525737f, 0.304308f }, + { 0.9487371f, 0.3160663f }, + { 0.9447554f, 0.32777625f }, + { 0.9406291f, 0.33943605f }, + { 0.936359f, 0.35104394f }, + { 0.9319456f, 0.36259815f }, + { 0.9273896f, 0.37409684f }, + { 0.9226918f, 0.38553834f }, + { 0.9178528f, 0.39692086f }, + { 0.91287345f, 0.40824264f }, + { 0.9077544f, 0.419502f }, + { 0.9024965f, 0.43069714f }, + { 0.89710057f, 0.44182643f }, + { 0.89156735f, 0.4528881f }, + { 0.88589776f, 0.4638805f }, + { 0.8800927f, 0.47480196f }, + { 0.8741529f, 0.48565078f }, + { 0.8680794f, 0.4964253f }, + { 0.8618732f, 0.5071239f }, + { 0.8555351f, 0.51774484f }, + { 0.8490661f, 0.52828664f }, + { 0.8424672f, 0.5387476f }, + { 0.83573943f, 0.5491262f }, + { 0.8288838f, 0.55942076f }, + { 0.82190144f, 0.5696297f }, + { 0.8147933f, 0.57975155f }, + { 0.8075605f, 0.58978474f }, + { 0.8002042f, 0.59972763f }, + { 0.79272544f, 0.60957885f }, + { 0.78512543f, 0.6193367f }, + { 0.7774054f, 0.62899995f }, + { 0.7695663f, 0.6385669f }, + { 0.76160955f, 0.6480362f }, + { 0.7535363f, 0.65740633f }, + { 0.74534774f, 0.66667587f }, + { 0.73704517f, 0.6758435f }, + { 0.7286299f, 0.6849077f }, + { 0.7201031f, 0.6938671f }, + { 0.71146613f, 0.70272034f }, + { 0.70272034f, 0.71146613f }, + { 0.6938671f, 0.7201031f }, + { 0.6849077f, 0.7286299f }, + { 0.6758435f, 0.73704517f }, + { 0.66667587f, 0.74534774f }, + { 0.65740633f, 0.7535363f }, + { 0.6480362f, 0.76160955f }, + { 0.6385669f, 0.7695663f }, + { 0.62899995f, 0.7774054f }, + { 0.6193367f, 0.78512543f }, + { 0.60957885f, 0.79272544f }, + { 0.59972763f, 0.8002042f }, + { 0.58978474f, 0.8075605f }, + { 0.57975155f, 0.8147933f }, + { 0.5696297f, 0.82190144f }, + { 0.55942076f, 0.8288838f }, + { 0.5491262f, 0.83573943f }, + { 0.5387476f, 0.8424672f }, + { 0.52828664f, 0.8490661f }, + { 0.51774484f, 0.8555351f }, + { 0.5071239f, 0.8618732f }, + { 0.4964253f, 0.8680794f }, + { 0.48565078f, 0.8741529f }, + { 0.47480196f, 0.8800927f }, + { 0.4638805f, 0.88589776f }, + { 0.4528881f, 0.89156735f }, + { 0.44182643f, 0.89710057f }, + { 0.43069714f, 0.9024965f }, + { 0.419502f, 0.9077544f }, + { 0.40824264f, 0.91287345f }, + { 0.39692086f, 0.9178528f }, + { 0.38553834f, 0.9226918f }, + { 0.37409684f, 0.9273896f }, + { 0.36259815f, 0.9319456f }, + { 0.35104394f, 0.936359f }, + { 0.33943605f, 0.9406291f }, + { 0.32777625f, 0.9447554f }, + { 0.3160663f, 0.9487371f }, + { 0.304308f, 0.9525737f }, + { 0.29250312f, 0.95626456f }, + { 0.2806535f, 0.9598091f }, + { 0.26876098f, 0.9632069f }, + { 0.25682732f, 0.9664573f }, + { 0.24485438f, 0.96955985f }, + { 0.23284398f, 0.9725141f }, + { 0.22079796f, 0.97531956f }, + { 0.20871817f, 0.97797585f }, + { 0.19660644f, 0.98048246f }, + { 0.18446463f, 0.98283917f }, + { 0.17229462f, 0.9850455f }, + { 0.16009824f, 0.9871011f }, + { 0.14787737f, 0.9890057f }, + { 0.13563387f, 0.990759f }, + { 0.12336964f, 0.9923608f }, + { 0.111086525f, 0.9938107f }, + { 0.09878642f, 0.99510866f }, + { 0.0864712f, 0.9962543f }, + { 0.074142754f, 0.99724764f }, + { 0.061802965f, 0.99808836f }, + { 0.04945372f, 0.99877644f }, + { 0.03709691f, 0.9993117f }, + { 0.024734428f, 0.99969405f }, + { 0.012368159f, 0.9999235f }, + { 0.0f, 1.0f }, + { -0.012368159f, 0.9999235f }, + { -0.024734428f, 0.99969405f }, + { -0.03709691f, 0.9993117f }, + { -0.04945372f, 0.99877644f }, + { -0.061802965f, 0.99808836f }, + { -0.074142754f, 0.99724764f }, + { -0.0864712f, 0.9962543f }, + { -0.09878642f, 0.99510866f }, + { -0.111086525f, 0.9938107f }, + { -0.12336964f, 0.9923608f }, + { -0.13563387f, 0.990759f }, + { -0.14787737f, 0.9890057f }, + { -0.16009824f, 0.9871011f }, + { -0.17229462f, 0.9850455f }, + { -0.18446463f, 0.98283917f }, + { -0.19660644f, 0.98048246f }, + { -0.20871817f, 0.97797585f }, + { -0.22079796f, 0.97531956f }, + { -0.23284398f, 0.9725141f }, + { -0.24485438f, 0.96955985f }, + { -0.25682732f, 0.9664573f }, + { -0.26876098f, 0.9632069f }, + { -0.2806535f, 0.9598091f }, + { -0.29250312f, 0.95626456f }, + { -0.304308f, 0.9525737f }, + { -0.3160663f, 0.9487371f }, + { -0.32777625f, 0.9447554f }, + { -0.33943605f, 0.9406291f }, + { -0.35104394f, 0.936359f }, + { -0.36259815f, 0.9319456f }, + { -0.37409684f, 0.9273896f }, + { -0.38553834f, 0.9226918f }, + { -0.39692086f, 0.9178528f }, + { -0.40824264f, 0.91287345f }, + { -0.419502f, 0.9077544f }, + { -0.43069714f, 0.9024965f }, + { -0.44182643f, 0.89710057f }, + { -0.4528881f, 0.89156735f }, + { -0.4638805f, 0.88589776f }, + { -0.47480196f, 0.8800927f }, + { -0.48565078f, 0.8741529f }, + { -0.4964253f, 0.8680794f }, + { -0.5071239f, 0.8618732f }, + { -0.51774484f, 0.8555351f }, + { -0.52828664f, 0.8490661f }, + { -0.5387476f, 0.8424672f }, + { -0.5491262f, 0.83573943f }, + { -0.55942076f, 0.8288838f }, + { -0.5696297f, 0.82190144f }, + { -0.57975155f, 0.8147933f }, + { -0.58978474f, 0.8075605f }, + { -0.59972763f, 0.8002042f }, + { -0.60957885f, 0.79272544f }, + { -0.6193367f, 0.78512543f }, + { -0.62899995f, 0.7774054f }, + { -0.6385669f, 0.7695663f }, + { -0.6480362f, 0.76160955f }, + { -0.65740633f, 0.7535363f }, + { -0.66667587f, 0.74534774f }, + { -0.6758435f, 0.73704517f }, + { -0.6849077f, 0.7286299f }, + { -0.6938671f, 0.7201031f }, + { -0.70272034f, 0.71146613f }, + { -0.71146613f, 0.70272034f }, + { -0.7201031f, 0.6938671f }, + { -0.7286299f, 0.6849077f }, + { -0.73704517f, 0.6758435f }, + { -0.74534774f, 0.66667587f }, + { -0.7535363f, 0.65740633f }, + { -0.76160955f, 0.6480362f }, + { -0.7695663f, 0.6385669f }, + { -0.7774054f, 0.62899995f }, + { -0.78512543f, 0.6193367f }, + { -0.79272544f, 0.60957885f }, + { -0.8002042f, 0.59972763f }, + { -0.8075605f, 0.58978474f }, + { -0.8147933f, 0.57975155f }, + { -0.82190144f, 0.5696297f }, + { -0.8288838f, 0.55942076f }, + { -0.83573943f, 0.5491262f }, + { -0.8424672f, 0.5387476f }, + { -0.8490661f, 0.52828664f }, + { -0.8555351f, 0.51774484f }, + { -0.8618732f, 0.5071239f }, + { -0.8680794f, 0.4964253f }, + { -0.8741529f, 0.48565078f }, + { -0.8800927f, 0.47480196f }, + { -0.88589776f, 0.4638805f }, + { -0.89156735f, 0.4528881f }, + { -0.89710057f, 0.44182643f }, + { -0.9024965f, 0.43069714f }, + { -0.9077544f, 0.419502f }, + { -0.91287345f, 0.40824264f }, + { -0.9178528f, 0.39692086f }, + { -0.9226918f, 0.38553834f }, + { -0.9273896f, 0.37409684f }, + { -0.9319456f, 0.36259815f }, + { -0.936359f, 0.35104394f }, + { -0.9406291f, 0.33943605f }, + { -0.9447554f, 0.32777625f }, + { -0.9487371f, 0.3160663f }, + { -0.9525737f, 0.304308f }, + { -0.95626456f, 0.29250312f }, + { -0.9598091f, 0.2806535f }, + { -0.9632069f, 0.26876098f }, + { -0.9664573f, 0.25682732f }, + { -0.96955985f, 0.24485438f }, + { -0.9725141f, 0.23284398f }, + { -0.97531956f, 0.22079796f }, + { -0.97797585f, 0.20871817f }, + { -0.98048246f, 0.19660644f }, + { -0.98283917f, 0.18446463f }, + { -0.9850455f, 0.17229462f }, + { -0.9871011f, 0.16009824f }, + { -0.9890057f, 0.14787737f }, + { -0.990759f, 0.13563387f }, + { -0.9923608f, 0.12336964f }, + { -0.9938107f, 0.111086525f }, + { -0.99510866f, 0.09878642f }, + { -0.9962543f, 0.0864712f }, + { -0.99724764f, 0.074142754f }, + { -0.99808836f, 0.061802965f }, + { -0.99877644f, 0.04945372f }, + { -0.9993117f, 0.03709691f }, + { -0.99969405f, 0.024734428f }, + { -0.9999235f, 0.012368159f }, + { -1.0f, 0.0f }, }; -#endif -// Results for MeshCreationBenchmark -// eastl::hash_map : 4.9703 ms and 5.1445ms with FastHash function -// std::unordered_map : 4.8755 ms and 4.4780ms with FastHash function -// verstable : 3.3968 ms with default FastHash -// no edge identification : 1.7396 ms -static void b3IdentifyEdges( b3MeshData* mesh ) +static b3CosSin b3GetQuantizedCosSin( int code ) +{ + B3_ASSERT( -254 <= code && code <= 254 ); + int index = b3MinInt( code < 0 ? -code : code, 254 ); + b3CosSin cs = s_quantizedCosSin[index]; + if ( code < 0 ) + { + cs.sine = -cs.sine; + } + + return cs; +} + +int8_t b3QuantizeEdgeAngle( float radians ) +{ + float scaled = radians * ( B3_EDGE_ANGLE_STEPS / B3_PI ); + int quantized = scaled >= 0.0f ? (int)( scaled + 0.5f ) : -(int)( 0.5f - scaled ); + return (int8_t)b3ClampInt( quantized, -127, 127 ); +} + +// Ring of mesh-space edge directions in 16-bit octahedral encoding. +typedef struct b3VertexRing +{ + // Number of boundary edges with 90 degree cliffs. + uint16_t cliffCount; +} b3VertexRing; + +typedef struct b3VertexRingEntry +{ + // Octohedral encoded edge direction. + uint16_t code; +} b3VertexRingEntry; + +static b3Vec3 b3FoldOctahedral( float x, float y ) +{ + return (b3Vec3){ + ( 1.0f - b3AbsFloat( y ) ) * ( x >= 0.0f ? 1.0f : -1.0f ), + ( 1.0f - b3AbsFloat( x ) ) * ( y >= 0.0f ? 1.0f : -1.0f ), + 0.0f, + }; +} + +static b3Vec3 b3DecodeRingDirection( uint16_t code ) +{ + float x = (float)( code & 0xFF ) * ( 2.0f / 255.0f ) - 1.0f; + float y = (float)( code >> 8 ) * ( 2.0f / 255.0f ) - 1.0f; + float z = 1.0f - b3AbsFloat( x ) - b3AbsFloat( y ); + if ( z < 0.0f ) + { + b3Vec3 folded = b3FoldOctahedral( x, y ); + x = folded.x; + y = folded.y; + } + + return b3Normalize( (b3Vec3){ x, y, z } ); +} + +typedef struct b3RingEncoder +{ + float* x; + float* y; + float* z; + b3VertexRingEntry* entries; + int count; + int capacity; +} b3RingEncoder; + +static void b3PushRingEntry( b3RingEncoder* encoder, b3Vec3 direction ) +{ + int index = encoder->count; + B3_ASSERT( index < encoder->capacity ); + encoder->x[index] = direction.x; + encoder->y[index] = direction.y; + encoder->z[index] = direction.z; + encoder->count = index + 1; +} + +static void b3EncodeRingEntries( b3RingEncoder* encoder ) +{ + float* xs = encoder->x; + float* ys = encoder->y; + float* zs = encoder->z; + int count = encoder->count; + + int paddedCount = ( count + 3 ) & ~3; + B3_ASSERT( paddedCount <= encoder->capacity ); + for ( int i = count; i < paddedCount; ++i ) + { + xs[i] = 0.0f; + ys[i] = 0.0f; + zs[i] = 1.0f; + } + + for ( int i = 0; i < count; i += 4 ) + { + b3FloatW4 x = b3LoadW4( xs + i ); + b3FloatW4 y = b3LoadW4( ys + i ); + b3FloatW4 z = b3LoadW4( zs + i ); + b3FloatW4 zero = b3ZeroW4(); + b3FloatW4 one = b3SplatW4( 1.0f ); + b3FloatW4 minusOne = b3SplatW4( -1.0f ); + b3FloatW4 sum = b3AddW4( b3AddW4( b3AbsW4( x ), b3AbsW4( y ) ), b3AbsW4( z ) ); + b3FloatW4 ox = b3DivW4( x, sum ); + b3FloatW4 oy = b3DivW4( y, sum ); + b3FloatW4 fx = b3MulW4( b3SubW4( one, b3AbsW4( oy ) ), b3BlendW4( one, minusOne, b3LessThanW4( ox, zero ) ) ); + b3FloatW4 fy = b3MulW4( b3SubW4( one, b3AbsW4( ox ) ), b3BlendW4( one, minusOne, b3LessThanW4( oy, zero ) ) ); + b3FloatW4 below = b3LessThanW4( z, zero ); + ox = b3BlendW4( ox, fx, below ); + oy = b3BlendW4( oy, fy, below ); + b3FloatW4 scale = b3SplatW4( 127.5f ); + b3FloatW4 half = b3SplatW4( 0.5f ); + b3StoreW4( xs + i, b3AddW4( b3MulW4( scale, b3AddW4( ox, one ) ), half ) ); + b3StoreW4( ys + i, b3AddW4( b3MulW4( scale, b3AddW4( oy, one ) ), half ) ); + } + + for ( int i = 0; i < count; ++i ) + { + int u = b3ClampInt( (int)xs[i], 0, 255 ); + int v = b3ClampInt( (int)ys[i], 0, 255 ); + encoder->entries[i].code = (uint16_t)( u | ( v << 8 ) ); + } +} + +bool b3GetMeshNeighborNormal( const b3Mesh* mesh, int triangleIndex, int edgeIndex, b3Vec3* normal ) +{ + const b3MeshData* data = mesh->data; + B3_ASSERT( 0 <= triangleIndex && triangleIndex < data->triangleCount ); + B3_ASSERT( 0 <= edgeIndex && edgeIndex < 3 ); + + const int8_t* edgeAngles = b3GetMeshEdgeAngles( data ); + if ( edgeAngles == NULL ) + { + return false; + } + + b3Vec3 scale = mesh->scale; + if ( scale.x * scale.y * scale.z < 0.0f ) + { + // Reflected mesh. + edgeIndex = 2 - edgeIndex; + } + + int8_t quantized = edgeAngles[3 * triangleIndex + edgeIndex]; + if ( quantized == B3_BOUNDARY_EDGE_ANGLE ) + { + return false; + } + + const b3MeshTriangle* triangles = b3GetMeshTriangles( data ); + const b3Vec3* vertices = b3GetMeshVertices( data ); + b3MeshTriangle triangle = triangles[triangleIndex]; + int indices[3] = { triangle.index1, triangle.index2, triangle.index3 }; + + b3Vec3 a = vertices[indices[edgeIndex]]; + b3Vec3 b = vertices[indices[( edgeIndex + 1 ) % 3]]; + b3Vec3 c = vertices[indices[( edgeIndex + 2 ) % 3]]; + b3Vec3 n = b3Normalize( b3Cross( b3Sub( b, a ), b3Sub( c, a ) ) ); + b3Vec3 outward = b3Cross( b3Normalize( b3Sub( b, a ) ), n ); + + b3CosSin cs = b3GetQuantizedCosSin( 2 * quantized ); + b3Vec3 unscaled = b3Add( b3MulSV( cs.cosine, n ), b3MulSV( cs.sine, outward ) ); + + b3Vec3 absScale = b3Abs( scale ); + if ( absScale.y == absScale.x && absScale.z == absScale.x ) + { + // Shortcut for uniform scale. Common case. + *normal = (b3Vec3){ + scale.x < 0.0f ? -unscaled.x : unscaled.x, + scale.y < 0.0f ? -unscaled.y : unscaled.y, + scale.z < 0.0f ? -unscaled.z : unscaled.z, + }; + return true; + } + + // Handle non-uniform scale. + b3Vec3 invScale = { 1.0f / scale.x, 1.0f / scale.y, 1.0f / scale.z }; + *normal = b3Normalize( b3Mul( invScale, unscaled ) ); + return true; +} + +static int b3GetMeshVertexIndex( const b3Mesh* mesh, b3MeshTriangle triangle, int vertexIndex ) +{ + b3Vec3 scale = mesh->scale; + if ( scale.x * scale.y * scale.z < 0.0f ) + { + // Handle reflection. + vertexIndex = ( 3 - vertexIndex ) % 3; + } + + int indices[3] = { triangle.index1, triangle.index2, triangle.index3 }; + return indices[vertexIndex]; +} + +int b3GetMeshVertexFlag( const b3Mesh* mesh, int triangleIndex, int vertexIndex ) +{ + const b3MeshData* data = mesh->data; + B3_ASSERT( 0 <= triangleIndex && triangleIndex < data->triangleCount ); + B3_ASSERT( 0 <= vertexIndex && vertexIndex < 3 ); + + const uint8_t* vertexFlags = b3GetMeshVertexFlags( data ); + if ( vertexFlags == NULL ) + { + return b3_flatVertex; + } + + b3MeshTriangle triangle = b3GetMeshTriangles( data )[triangleIndex]; + int index = b3GetMeshVertexIndex( mesh, triangle, vertexIndex ); + return vertexFlags[index]; +} + +// Is the candidate manifold normal in vertex Voronoi region? +bool b3IsInMeshVertexRing( const b3Mesh* mesh, int triangleIndex, int vertexIndex, b3Vec3 normal, float tolerance ) +{ + const b3MeshData* data = mesh->data; + B3_ASSERT( 0 <= triangleIndex && triangleIndex < data->triangleCount ); + B3_ASSERT( 0 <= vertexIndex && vertexIndex < 3 ); + + if ( data->vertexRingOffset == 0 ) + { + return true; + } + + const b3MeshTriangle* triangles = b3GetMeshTriangles( data ); + b3MeshTriangle triangle = triangles[triangleIndex]; + int vertex = b3GetMeshVertexIndex( mesh, triangle, vertexIndex ); + + // Account for the mesh scale, including reflection. The cliff scale is separate + // to maintain the 90 degree angle that all cliffs use. + b3Vec3 scale = mesh->scale; + b3Vec3 edgeScale, cliffScale; + b3Vec3 absScale = b3Abs( scale ); + bool uniform = absScale.y == absScale.x && absScale.z == absScale.x; + if ( uniform ) + { + edgeScale = + (b3Vec3){ scale.x < 0.0f ? -1.0f : 1.0f, scale.y < 0.0f ? -1.0f : 1.0f, scale.z < 0.0f ? -1.0f : 1.0f }; + cliffScale = edgeScale; + } + else + { + edgeScale = scale; + cliffScale = (b3Vec3){ 1.0f / scale.x, 1.0f / scale.y, 1.0f / scale.z }; + } + + const int* starts = (const int*)( (intptr_t)data + data->vertexRingOffset ); + int byteCount = starts[vertex + 1] - starts[vertex]; + if ( byteCount == 0 ) + { + // Flat vertex + const b3Vec3* vertices = b3GetMeshVertices( data ); + b3Vec3 faceNormal = + b3MakeNormalFromPoints( vertices[triangle.index1], vertices[triangle.index2], vertices[triangle.index3] ); + faceNormal = b3Normalize( b3Mul( cliffScale, faceNormal ) ); + float cosAngle = b3Dot( normal, faceNormal ); + return cosAngle > 0.0f && cosAngle * cosAngle >= 1.0f - tolerance * tolerance; + } + + const b3VertexRing* ring = (const b3VertexRing*)( (intptr_t)data + starts[vertex] ); + const b3VertexRingEntry* entries = (const b3VertexRingEntry*)( ring + 1 ); + int entryCount = ( byteCount - (int)sizeof( b3VertexRing ) ) / (int)sizeof( b3VertexRingEntry ); + int edgeCount = entryCount - ring->cliffCount; + + b3Vec3 edgeNormal = b3Mul( edgeScale, normal ); + b3Vec3 cliffNormal = b3Mul( cliffScale, normal ); + + for ( int i = 0; i < entryCount; ++i ) + { + bool isCliff = i >= edgeCount; + b3Vec3 direction = b3DecodeRingDirection( entries[i].code ); + float lean = b3Dot( isCliff ? cliffNormal : edgeNormal, direction ); + + float limit = tolerance; + if ( uniform == false ) + { + limit *= b3Length( b3Mul( isCliff ? cliffScale : edgeScale, direction ) ); + } + + if ( lean > limit ) + { + return false; + } + } + + return true; +} + +static void b3IdentifyEdges( b3MeshData* mesh, b3Vec3* normals ) { b3MeshTriangle* triangles = b3GetMeshTrianglesWrite( mesh ); const b3Vec3* vertices = b3GetMeshVertices( mesh ); uint8_t* flags = b3GetMeshFlagsWrite( mesh ); + int8_t* edgeAngles = b3GetMeshEdgeAnglesWrite( mesh ); + B3_ASSERT( edgeAngles != NULL ); int triangleCount = mesh->triangleCount; int edgeCount = 3 * triangleCount; + + for ( int i = 0; i < edgeCount; ++i ) + { + edgeAngles[i] = B3_BOUNDARY_EDGE_ANGLE; + } + b3MeshEdge* edges = B3_ALLOC( b3MeshEdge, edgeCount ); - b3Vec3* normals = B3_ALLOC( b3Vec3, triangleCount ); for ( int i = 0; i < triangleCount; ++i ) { @@ -1115,27 +1721,61 @@ static void b3IdentifyEdges( b3MeshData* mesh ) normals[i] = b3Normalize( n ); } - b3EdgeMap map; - b3EdgeMap_init( &map ); - b3EdgeMap_reserve( &map, edgeCount ); + // Counting sort for edges. Faster than a hashtable. + int vertexCount = mesh->vertexCount; + int* edgeStarts = B3_ALLOC( int, ( vertexCount + 1 ) ); + int* sortedEdges = B3_ALLOC( int, edgeCount ); + int* stamps = B3_ALLOC( int, vertexCount ); + int* firstEdges = B3_ALLOC( int, vertexCount ); - uint64_t key = (uint64_t)edges[0].vertex1 << 32 | (uint64_t)edges[0].vertex2; - b3EdgeMap_insert( &map, key, 0 ); + for ( int i = 0; i <= vertexCount; ++i ) + { + edgeStarts[i] = 0; + } + + for ( int i = 0; i < vertexCount; ++i ) + { + stamps[i] = B3_NULL_INDEX; + } + + for ( int i = 0; i < edgeCount; ++i ) + { + B3_ASSERT( 0 <= edges[i].vertex1 && edges[i].vertex2 < vertexCount ); + edgeStarts[edges[i].vertex1 + 1] += 1; + } + + for ( int i = 0; i < vertexCount; ++i ) + { + edgeStarts[i + 1] += edgeStarts[i]; + } + + for ( int i = 0; i < edgeCount; ++i ) + { + int vertex1 = edges[i].vertex1; + sortedEdges[edgeStarts[vertex1]] = i; + edgeStarts[vertex1] += 1; + } // Find unique edges and assign adjacency - for ( int i = 1; i < edgeCount; ++i ) + int begin = 0; + for ( int vertex1 = 0; vertex1 < vertexCount; ++vertex1 ) { - b3MeshEdge* edge = edges + i; - key = (uint64_t)edge->vertex1 << 32 | (uint64_t)edge->vertex2; - b3EdgeMap_itr itr = b3EdgeMap_get( &map, key ); + int end = edgeStarts[vertex1]; + for ( int j = begin; j < end; ++j ) + { + int i = sortedEdges[j]; + b3MeshEdge* edge = edges + i; + int vertex2 = edge->vertex2; - if ( b3EdgeMap_is_end( itr ) ) - { - b3EdgeMap_insert( &map, key, i ); - } - else - { - int otherIndex = itr.data->val; + // Establish twin. + if ( stamps[vertex2] != vertex1 ) + { + stamps[vertex2] = vertex1; + firstEdges[vertex2] = i; + continue; + } + + int otherIndex = firstEdges[vertex2]; B3_ASSERT( otherIndex < i ); b3MeshEdge* base = edges + otherIndex; @@ -1147,9 +1787,21 @@ static void b3IdentifyEdges( b3MeshData* mesh ) base->triangleCount += 1; } + + begin = end; } - b3EdgeMap_cleanup( &map ); + B3_FREE( firstEdges, int, vertexCount ); + B3_FREE( stamps, int, vertexCount ); + B3_FREE( sortedEdges, int, edgeCount ); + B3_FREE( edgeStarts, int, ( vertexCount + 1 ) ); + + int angleCapacity = ( edgeCount / 2 + 3 ) & ~3; + float* angleYs = B3_ALLOC( float, angleCapacity ); + float* angleXs = B3_ALLOC( float, angleCapacity ); + float* angleLengthSqs = B3_ALLOC( float, angleCapacity ); + int* angleSlots = B3_ALLOC( int, 2 * angleCapacity ); + int angleCount = 0; for ( int i = 0; i < edgeCount; ++i ) { @@ -1199,11 +1851,31 @@ static void b3IdentifyEdges( b3MeshData* mesh ) b3Vec3 v2 = vertices[i2]; b3Vec3 v3 = vertices[i3]; b3Vec3 p = vertices[opposite]; + b3Vec3 n1 = normals[edge->triangle1]; + b3Vec3 n2 = normals[edge->triangle2]; + + // Quantize the angles between neighboring normals. + int indices1[3] = { i1, i2, i3 }; + int indices2[3] = { j1, j2, j3 }; + int k1 = edge->triangleEdgeIndex1; + int k2 = edge->triangleEdgeIndex2; + if ( indices1[k1] == indices2[( k2 + 1 ) % 3] ) + { + b3Vec3 a = vertices[indices1[k1]]; + b3Vec3 b = vertices[indices1[( k1 + 1 ) % 3]]; + b3Vec3 edgeVector = b3Sub( b, a ); + b3Vec3 outward = b3Cross( edgeVector, n1 ); + B3_ASSERT( angleCount < angleCapacity ); + angleYs[angleCount] = b3Dot( n2, outward ); + angleXs[angleCount] = b3Dot( n2, n1 ); + angleLengthSqs[angleCount] = b3Dot( edgeVector, edgeVector ); + angleSlots[2 * angleCount + 0] = 3 * edge->triangle1 + k1; + angleSlots[2 * angleCount + 1] = 3 * edge->triangle2 + k2; + angleCount += 1; + } float cos5Deg = 0.9962f; float signedVolume = b3SignedVolume( v1, v2, v3, p ); - b3Vec3 n1 = normals[edge->triangle1]; - b3Vec3 n2 = normals[edge->triangle2]; float cosAngle = b3Dot( n1, n2 ); if ( signedVolume > 0.0f || cosAngle > cos5Deg ) { @@ -1220,10 +1892,288 @@ static void b3IdentifyEdges( b3MeshData* mesh ) } } - B3_FREE( normals, b3Vec3, triangleCount ); + int paddedAngleCount = ( angleCount + 3 ) & ~3; + B3_ASSERT( paddedAngleCount <= angleCapacity ); + for ( int i = angleCount; i < paddedAngleCount; ++i ) + { + angleYs[i] = 0.0f; + angleXs[i] = 0.0f; + angleLengthSqs[i] = 0.0f; + } + + for ( int i = 0; i < angleCount; i += 4 ) + { + b3FloatW4 x = b3MulW4( b3LoadW4( angleXs + i ), b3SqrtW4( b3LoadW4( angleLengthSqs + i ) ) ); + b3FloatW4 angle = b3Atan2W4( b3LoadW4( angleYs + i ), x ); + b3StoreW4( angleYs + i, angle ); + } + + for ( int i = 0; i < angleCount; ++i ) + { + int8_t quantized = b3QuantizeEdgeAngle( angleYs[i] ); + edgeAngles[angleSlots[2 * i + 0]] = quantized; + edgeAngles[angleSlots[2 * i + 1]] = quantized; + } + + B3_FREE( angleSlots, int, 2 * angleCapacity ); + B3_FREE( angleLengthSqs, float, angleCapacity ); + B3_FREE( angleXs, float, angleCapacity ); + B3_FREE( angleYs, float, angleCapacity ); + + uint8_t* vertexFlags = b3GetMeshVertexFlagsWrite( mesh ); + B3_ASSERT( vertexFlags != NULL ); + + for ( int i = 0; i < mesh->vertexCount; ++i ) + { + vertexFlags[i] = b3_flatVertex; + } + + // Vertex flags for ghost collision handling. + for ( int i = 0; i < triangleCount; ++i ) + { + int indices[3] = { triangles[i].index1, triangles[i].index2, triangles[i].index3 }; + for ( int k = 0; k < 3; ++k ) + { + int8_t angle = edgeAngles[3 * i + k]; + int a = indices[k]; + int b = indices[k < 2 ? k + 1 : 0]; + + // Edge not flat? + if ( angle != 0 ) + { + vertexFlags[a] = (uint8_t)( vertexFlags[a] & ~b3_flatVertex ); + vertexFlags[b] = (uint8_t)( vertexFlags[b] & ~b3_flatVertex ); + } + + // Edge concave? + if ( angle < 0 && angle != B3_BOUNDARY_EDGE_ANGLE ) + { + vertexFlags[a] |= b3_concaveVertex; + vertexFlags[b] |= b3_concaveVertex; + } + } + } + B3_FREE( edges, b3MeshEdge, edgeCount ); } +// Push the ring entry directions of a vertex to the encoder. Returns the cliff count. +static int b3BuildVertexRing( const b3MeshData* mesh, const b3Vec3* normals, const int* fan, int fanCount, int vertex, + int* stamps, b3RingEncoder* encoder ) +{ + const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh ); + const b3Vec3* vertices = b3GetMeshVertices( mesh ); + const int8_t* edgeAngles = b3GetMeshEdgeAngles( mesh ); + + for ( int i = 0; i < fanCount; ++i ) + { + b3MeshTriangle triangle = triangles[fan[i]]; + int indices[3] = { triangle.index1, triangle.index2, triangle.index3 }; + int k = indices[0] == vertex ? 0 : ( indices[1] == vertex ? 1 : 2 ); + + // The other vertex indices on this triangle that are not the input vertex index. + int others[2] = { indices[k < 2 ? k + 1 : 0], indices[k > 0 ? k - 1 : 2] }; + + for ( int j = 0; j < 2; ++j ) + { + int other = others[j]; + if ( stamps[other] == vertex ) + { + continue; + } + + stamps[other] = vertex; + + // This encodes every edge. The intersection of all planes perpendicular to the edge axis builds + // the Voronoi region. That process can identify edges that don't contribute to the Voronoi region. + // That culling could save storage but is skipped for simplicity. + b3PushRingEntry( encoder, b3Sub( vertices[other], vertices[vertex] ) ); + } + } + + int cliffCount = 0; + for ( int i = 0; i < fanCount; ++i ) + { + int triangleIndex = fan[i]; + b3MeshTriangle triangle = triangles[triangleIndex]; + int indices[3] = { triangle.index1, triangle.index2, triangle.index3 }; + int k = indices[0] == vertex ? 0 : ( indices[1] == vertex ? 1 : 2 ); + int edgeIndices[2] = { k, k > 0 ? k - 1 : 2 }; + + for ( int j = 0; j < 2; ++j ) + { + if ( edgeAngles[3 * triangleIndex + edgeIndices[j]] != B3_BOUNDARY_EDGE_ANGLE ) + { + continue; + } + + // The cliff is a 90 degree drop. + b3PushRingEntry( encoder, b3Neg( normals[triangleIndex] ) ); + cliffCount += 1; + } + } + + return cliffCount; +} + +// Build a neighbor angle ring for each vertex. Used to mitigate ghost collisions with minimal tolerances +// and fudge factors. +static b3MeshData* b3AppendVertexRings( b3MeshData* mesh, const b3Vec3* normals ) +{ + const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh ); + const uint8_t* vertexFlags = b3GetMeshVertexFlags( mesh ); + B3_ASSERT( vertexFlags != NULL ); + + int vertexCount = mesh->vertexCount; + int triangleCount = mesh->triangleCount; + + // Triangle fan counts per vertex via counting sort and prefix sum. + int* fanStarts = B3_ALLOC( int, ( vertexCount + 1 ) ); + + // Sub-arrays of triangle fans for each vertex. + int* fans = B3_ALLOC( int, ( 3 * triangleCount ) ); + + // Cursors for walking fan sub-arrays. + int* cursors = B3_ALLOC( int, vertexCount ); + + // Edge stamps to avoid visiting the same edge twice when + // iterating triangles. + int* stamps = B3_ALLOC( int, vertexCount ); + + // Byte offsets for each sub-array of ring entries per vertex. + int* ringStarts = B3_ALLOC( int, ( vertexCount + 1 ) ); + + // Counting sort + memset( fanStarts, 0, ( vertexCount + 1 ) * sizeof( int ) ); + for ( int i = 0; i < triangleCount; ++i ) + { + fanStarts[triangles[i].index1 + 1] += 1; + fanStarts[triangles[i].index2 + 1] += 1; + fanStarts[triangles[i].index3 + 1] += 1; + } + + for ( int i = 0; i < vertexCount; ++i ) + { + fanStarts[i + 1] += fanStarts[i]; + } + + memcpy( cursors, fanStarts, vertexCount * sizeof( int ) ); + for ( int i = 0; i < triangleCount; ++i ) + { + fans[cursors[triangles[i].index1]++] = i; + fans[cursors[triangles[i].index2]++] = i; + fans[cursors[triangles[i].index3]++] = i; + } + + for ( int i = 0; i < vertexCount; ++i ) + { + stamps[i] = B3_NULL_INDEX; + } + + // Hey dude, I got an offset to the array of offsets. + size_t startsOffset = mesh->byteCount; + + // After the array of offsets, the ring sub-arrays begin. + size_t ringOffset = startsOffset + b3AlignUp8( ( vertexCount + 1 ) * sizeof( int ) ); + + // Upper bound on entries: two edges per fan triangle and two cliffs per boundary edge. + const int8_t* edgeAngles = b3GetMeshEdgeAngles( mesh ); + int entryBound = 0; + for ( int i = 0; i < vertexCount; ++i ) + { + if ( ( vertexFlags[i] & b3_flatVertex ) == 0 ) + { + entryBound += 2 * ( fanStarts[i + 1] - fanStarts[i] ); + } + } + + for ( int i = 0; i < 3 * triangleCount; ++i ) + { + if ( edgeAngles[i] == B3_BOUNDARY_EDGE_ANGLE ) + { + entryBound += 2; + } + } + + b3RingEncoder encoder = { 0 }; + encoder.capacity = ( entryBound + 3 ) & ~3; + encoder.x = B3_ALLOC( float, encoder.capacity ); + encoder.y = B3_ALLOC( float, encoder.capacity ); + encoder.z = B3_ALLOC( float, encoder.capacity ); + encoder.entries = B3_ALLOC( b3VertexRingEntry, encoder.capacity ); + + int* entryStarts = B3_ALLOC( int, ( vertexCount + 1 ) ); + int* cliffCounts = B3_ALLOC( int, vertexCount ); + + // The vertex ring offset for the current vertex. + size_t offset = ringOffset; + + for ( int i = 0; i < vertexCount; ++i ) + { + ringStarts[i] = (int)offset; + entryStarts[i] = encoder.count; + cliffCounts[i] = 0; + if ( vertexFlags[i] & b3_flatVertex ) + { + // Flat vertices don't store ring data. + continue; + } + + const int* fan = fans + fanStarts[i]; + int fanCount = fanStarts[i + 1] - fanStarts[i]; + cliffCounts[i] = b3BuildVertexRing( mesh, normals, fan, fanCount, i, stamps, &encoder ); + int entryCount = encoder.count - entryStarts[i]; + offset += sizeof( b3VertexRing ) + entryCount * sizeof( b3VertexRingEntry ); + } + + ringStarts[vertexCount] = (int)offset; + entryStarts[vertexCount] = encoder.count; + + b3EncodeRingEntries( &encoder ); + + // We now have the required space, grow the blob. + size_t byteCount = ringOffset + b3AlignUp8( offset - ringOffset ); + b3MeshData* result = b3Alloc( byteCount ); + memset( result, 0, byteCount ); + memcpy( result, mesh, mesh->byteCount ); + b3Free( mesh, mesh->byteCount ); + + result->byteCount = (int)byteCount; + result->vertexRingOffset = (int)startsOffset; + + // Copy the ring sub-array offsets per vertex. The vertexCount + 1 entry is the end offset. + memcpy( (uint8_t*)result + startsOffset, ringStarts, ( vertexCount + 1 ) * sizeof( int ) ); + + for ( int i = 0; i < vertexCount; ++i ) + { + if ( ringStarts[i + 1] == ringStarts[i] ) + { + continue; + } + + b3VertexRing* ring = (b3VertexRing*)( (uint8_t*)result + ringStarts[i] ); + B3_ASSERT( cliffCounts[i] <= UINT16_MAX ); + ring->cliffCount = (uint16_t)cliffCounts[i]; + + int entryCount = entryStarts[i + 1] - entryStarts[i]; + memcpy( ring + 1, encoder.entries + entryStarts[i], entryCount * sizeof( b3VertexRingEntry ) ); + } + + B3_FREE( cliffCounts, int, vertexCount ); + B3_FREE( entryStarts, int, ( vertexCount + 1 ) ); + B3_FREE( encoder.entries, b3VertexRingEntry, encoder.capacity ); + B3_FREE( encoder.z, float, encoder.capacity ); + B3_FREE( encoder.y, float, encoder.capacity ); + B3_FREE( encoder.x, float, encoder.capacity ); + B3_FREE( ringStarts, int, ( vertexCount + 1 ) ); + B3_FREE( stamps, int, vertexCount ); + B3_FREE( cursors, int, vertexCount ); + B3_FREE( fans, int, ( 3 * triangleCount ) ); + B3_FREE( fanStarts, int, ( vertexCount + 1 ) ); + + return result; +} + b3MeshData* b3CreateGridMesh( int xCount, int zCount, float cellWidth, int materialCount, bool identifyEdges ) { B3_ASSERT( 0 <= materialCount && materialCount <= UINT8_MAX ); @@ -1468,7 +2418,8 @@ b3MeshData* b3CreateBoxMesh( b3Vec3 center, b3Vec3 extent, bool identifyEdges ) float y = extent.y; float z = extent.z; b3Vec3 vertices[] = { - { x, y, z }, { -x, y, z }, { -x, -y, z }, { x, -y, z }, { x, y, -z }, { -x, y, -z }, { -x, -y, -z }, { x, -y, -z }, + { x, y, z }, { -x, y, z }, { -x, -y, z }, { x, -y, z }, + { x, y, -z }, { -x, y, -z }, { -x, -y, -z }, { x, -y, -z }, }; for ( int i = 0; i < 8; ++i ) @@ -1502,7 +2453,8 @@ b3MeshData* b3CreateHollowBoxMesh( b3Vec3 center, b3Vec3 extent ) float y = extent.y; float z = extent.z; b3Vec3 vertices[] = { - { x, y, z }, { -x, y, z }, { -x, -y, z }, { x, -y, z }, { x, y, -z }, { -x, y, -z }, { -x, -y, -z }, { x, -y, -z }, + { x, y, z }, { -x, y, z }, { -x, -y, z }, { x, -y, z }, + { x, y, -z }, { -x, y, -z }, { -x, -y, -z }, { x, -y, -z }, }; for ( int i = 0; i < 8; ++i ) @@ -1740,6 +2692,15 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, byteCount += b3AlignUp8( triangleCount * sizeof( uint8_t ) ); int flagsOffset = (int)byteCount; byteCount += b3AlignUp8( triangleCount * sizeof( uint8_t ) ); + int edgeAngleOffset = 0; + int vertexFlagOffset = 0; + if ( def->identifyEdges ) + { + edgeAngleOffset = (int)byteCount; + byteCount += b3AlignUp8( 3 * triangleCount * sizeof( int8_t ) ); + vertexFlagOffset = (int)byteCount; + byteCount += b3AlignUp8( vertexCount * sizeof( uint8_t ) ); + } b3MeshData* mesh = b3Alloc( byteCount ); @@ -1761,6 +2722,8 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, mesh->materialOffset = materialIndicesOffset; mesh->materialCount = materialCount; mesh->flagsOffset = flagsOffset; + mesh->edgeAngleOffset = edgeAngleOffset; + mesh->vertexFlagOffset = vertexFlagOffset; b3MeshNode* nodes = b3GetMeshNodesWrite( mesh ); b3MeshTriangle* triangles = b3GetMeshTrianglesWrite( mesh ); @@ -1812,7 +2775,14 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, if ( def->identifyEdges ) { - b3IdentifyEdges( mesh ); + int meshTriangleCount = mesh->triangleCount; + b3Vec3* normals = B3_ALLOC( b3Vec3, meshTriangleCount ); + + b3IdentifyEdges( mesh, normals ); + + mesh = b3AppendVertexRings( mesh, normals ); + + B3_FREE( normals, b3Vec3, meshTriangleCount ); } B3_VALIDATE( b3IsNonDegenerate( mesh, minArea ) ); @@ -2154,7 +3124,7 @@ b3CastOutput b3ShapeCastMesh( const b3Mesh* mesh, const b3ShapeCastInput* input // Test extended triangle vs ray bounds overlap in scaled space. Skip edge tests. bool overlap = b3TestBoundsOverlap( triangleMin, triangleMax, rayMin, rayMax ); - if (overlap == false) + if ( overlap == false ) { continue; } @@ -2336,8 +3306,8 @@ int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* sh else if ( distanceOutput.distance <= mover->radius ) { b3Plane plane = { distanceOutput.normal, mover->radius - distanceOutput.distance }; - planes[planeCount] = - (b3PlaneResult){ plane, distanceOutput.pointA, triangleIndex, 0, materialIndices[triangleIndex] }; + planes[planeCount] = (b3PlaneResult){ plane, distanceOutput.pointA, triangleIndex, 0, + materialIndices[triangleIndex] }; planeCount += 1; if ( planeCount == capacity ) @@ -2448,10 +3418,12 @@ void b3QueryMesh( const b3Mesh* mesh, b3AABB bounds, b3MeshQueryFcn* fcn, void* const b3MeshNode* leftChild = b3GetLeftChild( node ); const b3MeshNode* rightChild = b3GetRightChild( node ); - bool overlapLeft = b3TestBoundsOverlap( b3LoadV( &leftChild->lowerBound.x ), b3LoadV( &leftChild->upperBound.x ), - invScaledBoundsMin, invScaledBoundsMax ); - bool overlapRight = b3TestBoundsOverlap( b3LoadV( &rightChild->lowerBound.x ), - b3LoadV( &rightChild->upperBound.x ), invScaledBoundsMin, invScaledBoundsMax ); + bool overlapLeft = + b3TestBoundsOverlap( b3LoadV( &leftChild->lowerBound.x ), b3LoadV( &leftChild->upperBound.x ), + invScaledBoundsMin, invScaledBoundsMax ); + bool overlapRight = + b3TestBoundsOverlap( b3LoadV( &rightChild->lowerBound.x ), b3LoadV( &rightChild->upperBound.x ), + invScaledBoundsMin, invScaledBoundsMax ); if ( overlapLeft ) { diff --git a/src/mesh_contact.c b/src/mesh_contact.c index 86f7ed0..b5d4396 100644 --- a/src/mesh_contact.c +++ b/src/mesh_contact.c @@ -5,14 +5,15 @@ #include "ctz.h" #include "manifold.h" #include "physics_world.h" -#include "qsort.h" #include "shape.h" #include "simd.h" #include "box3d/types.h" #include +#include +#define B3_FORCE_GHOST_COLLISIONS 0 #define B3_MAX_POINTS_PER_TRIANGLE 32 #if B3_ENABLE_VALIDATION @@ -30,8 +31,11 @@ static bool b3IsSorted( const int* array, int count ) } #endif +#define B3_INITIAL_TRIANGLE_CAPACITY 512 + typedef struct b3TriangleQueryContext { + b3Arena* arena; int* indices; int capacity; int count; @@ -40,41 +44,23 @@ typedef struct b3TriangleQueryContext static bool b3CollectTriangleIndicesCallback( b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleIndex, void* context ) { B3_UNUSED( a, b, c ); - b3TriangleQueryContext* triangleContext = (b3TriangleQueryContext*)context; + b3TriangleQueryContext* triangleContext = context; if ( triangleContext->count == triangleContext->capacity ) { - return false; + int capacity = 2 * triangleContext->capacity; + int* indices = b3Bump( triangleContext->arena, capacity * (int)sizeof( int ) ); + memcpy( indices, triangleContext->indices, triangleContext->count * sizeof( int ) ); + triangleContext->indices = indices; + triangleContext->capacity = capacity; } + triangleContext->indices[triangleContext->count] = triangleIndex; triangleContext->count += 1; - return triangleContext->count < triangleContext->capacity; + return true; } -static int b3QueryMeshTriangles( int* indices, int capacity, const b3Mesh* mesh, b3AABB bounds ) -{ - b3TriangleQueryContext context = { - .indices = indices, - .capacity = capacity, - .count = 0, - }; - - b3QueryMesh( mesh, bounds, b3CollectTriangleIndicesCallback, &context ); - return context.count; -} - -static int b3QueryHeightFieldTriangles( int* indices, int capacity, const b3HeightFieldData* heightField, b3AABB bounds ) -{ - b3TriangleQueryContext context = { - .indices = indices, - .capacity = capacity, - .count = 0, - }; - - b3QueryHeightField( heightField, bounds, b3CollectTriangleIndicesCallback, &context ); - return context.count; -} - -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 ); @@ -88,7 +74,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 ); } } @@ -102,33 +89,29 @@ static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3WorldTr meshContact->queryBounds.upperBound = b3Add( bounds->upperBound, extension ); // Query triangles - int triangleCapacity = B3_MAX_MESH_CONTACT_TRIANGLES; - int* triangleIndices = b3Bump( &arena, B3_MAX_MESH_CONTACT_TRIANGLES * sizeof( int ) ); + b3TriangleQueryContext queryContext = { + .arena = &arena, + .indices = b3Bump( &arena, B3_INITIAL_TRIANGLE_CAPACITY * (int)sizeof( int ) ), + .capacity = B3_INITIAL_TRIANGLE_CAPACITY, + .count = 0, + }; // 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 ); - int triangleCount; if ( shapeA->type == b3_meshShape ) { - triangleCount = b3QueryMeshTriangles( triangleIndices, triangleCapacity, &shapeA->mesh, localBounds ); + b3QueryMesh( &shapeA->mesh, localBounds, b3CollectTriangleIndicesCallback, &queryContext ); } else { B3_ASSERT( shapeA->type == b3_heightShape ); - triangleCount = b3QueryHeightFieldTriangles( triangleIndices, triangleCapacity, shapeA->heightField, localBounds ); + b3QueryHeightField( shapeA->heightField, localBounds, b3CollectTriangleIndicesCallback, &queryContext ); } - if ( triangleCount == triangleCapacity ) - { - static bool s_once = false; - if ( s_once == false ) - { - b3Log( "WARNING: complex mesh detected, triangle buffer capacity of %d reached", triangleCapacity ); - s_once = true; - } - } + int* triangleIndices = queryContext.indices; + int triangleCount = queryContext.count; // Triangle indices must be sorted to match caches. B3_VALIDATE( b3IsSorted( triangleIndices, triangleCount ) ); @@ -168,115 +151,11 @@ static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3WorldTr } } -typedef struct b3TentativeTriangle -{ - float squaredDistance; - int index; -} b3TentativeTriangle; - -#define B3_MAX_EDGE_COUNT 64 - -typedef struct b3FoundEdges -{ - uint64_t keys[B3_MAX_EDGE_COUNT]; - int count; -} b3FoundEdges; - -static inline bool b3AddEdge( b3FoundEdges* edges, int vertex1, int vertex2 ) -{ - uint64_t i1 = (uint64_t)b3MinInt( vertex1, vertex2 ); - uint64_t i2 = (uint64_t)b3MaxInt( vertex1, vertex2 ); - uint64_t key = i1 << 32 | i2; - - int count = edges->count; - for ( int i = 0; i < count; ++i ) - { - if ( edges->keys[i] == key ) - { - return false; - } - } - - if ( count == B3_MAX_EDGE_COUNT ) - { - // This will lead to a potential ghost collision - return true; - } - - edges->keys[count] = key; - edges->count += 1; - - return true; -} - -static inline bool b3FindEdge( b3FoundEdges* edges, int vertex1, int vertex2 ) -{ - uint64_t i1 = (uint64_t)b3MinInt( vertex1, vertex2 ); - uint64_t i2 = (uint64_t)b3MaxInt( vertex1, vertex2 ); - uint64_t key = i1 << 32 | i2; - - int count = edges->count; - for ( int i = 0; i < count; ++i ) - { - if ( edges->keys[i] == key ) - { - return true; - } - } - - return false; -} - -#if 0 -// Two triangles share an edge iff they share at least two vertex indices. -static inline bool b3TrianglesShareEdge( int a1, int a2, int a3, int b1, int b2, int b3 ) -{ - int matches = 0; - matches += ( a1 == b1 || a1 == b2 || a1 == b3 ); - matches += ( a2 == b1 || a2 == b2 || a2 == b3 ); - matches += ( a3 == b1 || a3 == b2 || a3 == b3 ); - return matches >= 2; -} -#endif - -#define B3_MAX_VERTEX_COUNT 64 - -typedef struct b3FoundVertices -{ - int keys[B3_MAX_VERTEX_COUNT]; - int count; -} b3FoundVertices; - -static inline bool b3AddVertex( b3FoundVertices* vertices, int vertex ) -{ - int key = vertex; - - int count = vertices->count; - for ( int i = 0; i < count; ++i ) - { - if ( vertices->keys[i] == key ) - { - return false; - } - } - - if ( count == B3_MAX_VERTEX_COUNT ) - { - // This will lead to a potential ghost collision - return true; - } - - vertices->keys[count] = key; - vertices->count += 1; - - return true; -} - #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, - float separationTol ) +static inline bool b3IsBetterCullCandidate( float score, float separation, float bestScore, float bestSeparation, + float scoreTol, float separationTol ) { if ( score > bestScore + scoreTol ) { @@ -291,7 +170,34 @@ static inline bool b3IsBetterCullCandidate( float score, float separation, float return separation < bestSeparation - separationTol; } -static int b3CullPoints( b3Point2D* points, int count ) +// Find the farthest point from the base point. +static int b3FindFarthestCullPoint( const b3Point2D* points, int count, int index, float scoreTol, float separationTol ) +{ + b3Vec2 origin = points[index].p; + float bestScore = 0.0f; + float bestSeparation = FLT_MAX; + int bestIndex = B3_NULL_INDEX; + for ( int i = 0; i < count; ++i ) + { + if ( i == index ) + { + continue; + } + + float score = b3DistanceSquared2( origin, points[i].p ); + if ( b3IsBetterCullCandidate( score, points[i].separation, bestScore, bestSeparation, scoreTol, + separationTol ) ) + { + bestIndex = i; + bestScore = score; + bestSeparation = points[i].separation; + } + } + + return bestIndex; +} + +static int b3CullPoints( b3Point2D* points, int count, bool sweep ) { if ( count <= 1 ) { @@ -303,29 +209,56 @@ static int b3CullPoints( b3Point2D* points, int count ) float separationTol = B3_LINEAR_SLOP; b3Point2D finalPoints[4]; - int count1 = count; - // Step 1: the two points with the largest distance, ties broken by deepest combined separation float bestScore = 0.0f; float bestSeparation = FLT_MAX; int bestIndex1 = B3_NULL_INDEX; int bestIndex2 = B3_NULL_INDEX; - for ( int i = 0; i < count1; ++i ) + if ( sweep ) { - b3Vec2 p1 = points[i].p; - for ( int j = i + 1; j < count1; ++j ) + // Arbitrary search direction. + b3Vec2 axis = { 0.92106099f, 0.38941834f }; + int startIndex = 0; + float startScore = b3Dot2( axis, points[0].p ); + float startSeparation = points[0].separation; + for ( int i = 1; i < count; ++i ) { - float score = b3DistanceSquared2( p1, points[j].p ); - // Separation sum heuristic - float separation = points[i].separation + points[j].separation; - - if ( b3IsBetterCullCandidate( score, separation, bestScore, bestSeparation, tolSqr, separationTol ) ) + float score = b3Dot2( axis, points[i].p ); + if ( b3IsBetterCullCandidate( score, points[i].separation, startScore, startSeparation, tol, + separationTol ) ) { - bestIndex1 = i; - bestIndex2 = j; - bestScore = score; - bestSeparation = separation; + startIndex = i; + startScore = score; + startSeparation = points[i].separation; + } + } + + bestIndex1 = b3FindFarthestCullPoint( points, count, startIndex, tolSqr, separationTol ); + B3_ASSERT( bestIndex1 != B3_NULL_INDEX ); + bestIndex2 = b3FindFarthestCullPoint( points, count, bestIndex1, tolSqr, separationTol ); + B3_ASSERT( bestIndex2 != B3_NULL_INDEX ); + bestScore = b3DistanceSquared2( points[bestIndex1].p, points[bestIndex2].p ); + } + else + { + // Step 1: the two points with the largest distance, ties broken by deepest combined separation + for ( int i = 0; i < count; ++i ) + { + b3Vec2 p1 = points[i].p; + for ( int j = i + 1; j < count; ++j ) + { + float score = b3DistanceSquared2( p1, points[j].p ); + // Separation sum heuristic + float separation = points[i].separation + points[j].separation; + + if ( b3IsBetterCullCandidate( score, separation, bestScore, bestSeparation, tolSqr, separationTol ) ) + { + bestIndex1 = i; + bestIndex2 = j; + bestScore = score; + bestSeparation = separation; + } } } } @@ -334,7 +267,7 @@ static int b3CullPoints( b3Point2D* points, int count ) { // Choose deepest point int deepestIndex = 0; - for ( int i = 1; i < count1; ++i ) + for ( int i = 1; i < count; ++i ) { if ( points[i].separation < points[deepestIndex].separation ) { @@ -352,12 +285,7 @@ static int b3CullPoints( b3Point2D* points, int count ) finalPoints[0] = points[bestIndex1]; finalPoints[1] = points[bestIndex2]; - // Cull - points[bestIndex2] = points[count1 - 1]; - points[bestIndex1] = points[count1 - 2]; - count1 -= 2; - - if ( count1 == 0 ) + if ( count == 2 ) { points[0] = finalPoints[0]; points[1] = finalPoints[1]; @@ -376,10 +304,16 @@ static int b3CullPoints( b3Point2D* points, int count ) // Step 2: find the point with the maximum triangular area, ties broken by deepest separation bestScore = 0.0f; bestSeparation = FLT_MAX; - int bestIndex = B3_NULL_INDEX; + int bestIndex3 = B3_NULL_INDEX; float bestSignedArea = 0.0f; - for ( int i = 0; i < count1; ++i ) + for ( int i = 0; i < count; ++i ) { + // Skip best points 1 and 2 rather than culling them. This preserves order under octogon culling. + if ( i == bestIndex1 || i == bestIndex2 ) + { + continue; + } + b3Vec2 p = points[i].p; float signedArea = b3Cross2( ba, b3Sub2( p, a ) ); float score = b3AbsFloat( signedArea ); @@ -389,11 +323,11 @@ static int b3CullPoints( b3Point2D* points, int count ) bestSignedArea = signedArea; bestScore = score; bestSeparation = points[i].separation; - bestIndex = i; + bestIndex3 = i; } } - if ( bestIndex == B3_NULL_INDEX ) + if ( bestIndex3 == B3_NULL_INDEX ) { // All points collinear points[0] = finalPoints[0]; @@ -402,9 +336,9 @@ static int b3CullPoints( b3Point2D* points, int count ) } // Store best point - finalPoints[2] = points[bestIndex]; + finalPoints[2] = points[bestIndex3]; - if ( count1 == 1 ) + if ( count == 3 ) { points[0] = finalPoints[0]; points[1] = finalPoints[1]; @@ -412,10 +346,6 @@ static int b3CullPoints( b3Point2D* points, int count ) return 3; } - // Cull - points[bestIndex] = points[count1 - 1]; - count1 -= 1; - // Step 4: get the point that adds the most area outside the current triangle // Third anchor @@ -433,9 +363,14 @@ static int b3CullPoints( b3Point2D* points, int count ) bestScore = 0.0f; bestSeparation = FLT_MAX; - bestIndex = B3_NULL_INDEX; - for ( int i = 0; i < count1; ++i ) + int bestIndex4 = B3_NULL_INDEX; + for ( int i = 0; i < count; ++i ) { + if ( i == bestIndex1 || i == bestIndex2 || i == bestIndex3 ) + { + continue; + } + b3Vec2 p = points[i].p; float u1 = b3Cross2( b3Sub2( p, a ), ba ); float u2 = b3Cross2( b3Sub2( p, b ), cb ); @@ -447,11 +382,11 @@ static int b3CullPoints( b3Point2D* points, int count ) { bestScore = score; bestSeparation = points[i].separation; - bestIndex = i; + bestIndex4 = i; } } - if ( bestIndex == B3_NULL_INDEX ) + if ( bestIndex4 == B3_NULL_INDEX ) { // No additional area points[0] = finalPoints[0]; @@ -461,7 +396,7 @@ static int b3CullPoints( b3Point2D* points, int count ) } // Store best point - finalPoints[3] = points[bestIndex]; + finalPoints[3] = points[bestIndex4]; // Full quad points[0] = finalPoints[0]; @@ -471,8 +406,155 @@ static int b3CullPoints( b3Point2D* points, int count ) return 4; } +// Above this point count it is worth culling interior points prior to the regular culling algorithm. +#define B3_CULL_FILTER_THRESHOLD 64 + +// If the cull filter leaves this many points then fallback to a linear tests for the first to points. +#define B3_CULL_SWEEP_THRESHOLD 256 + +// Drop interior points before computing the farthest two points. This is faster than brute-force O(N^2). +static int b3DropInteriorPoints( b3Point2D* points, int count, float margin ) +{ + int extremes[8] = { 0 }; + float bests[8] = { + points[0].p.x, points[0].p.x + points[0].p.y, points[0].p.y, points[0].p.y - points[0].p.x, + -points[0].p.x, -points[0].p.x - points[0].p.y, -points[0].p.y, points[0].p.x - points[0].p.y, + }; + + // Find the maximum offsets in the octogon. + for ( int i = 1; i < count; ++i ) + { + b3Vec2 p = points[i].p; + float values[8] = { p.x, p.x + p.y, p.y, p.y - p.x, -p.x, -p.x - p.y, -p.y, p.x - p.y }; + for ( int k = 0; k < 8; ++k ) + { + if ( values[k] > bests[k] ) + { + bests[k] = values[k]; + extremes[k] = i; + } + } + } + + // Build 8 planes as an approximate convex hull for culling. + b3Vec2 normals[8]; + float offsets[8]; + int edgeCount = 0; + for ( int k = 0; k < 8; ++k ) + { + b3Vec2 a = points[extremes[k]].p; + b3Vec2 b = points[extremes[( k + 1 ) & 7]].p; + b3Vec2 normal = { a.y - b.y, b.x - a.x }; + float length = b3Length2( normal ); + if ( length == 0.0f ) + { + continue; + } + + normals[edgeCount] = normal; + offsets[edgeCount] = b3Dot2( normal, a ) + margin * length; + edgeCount += 1; + } + + if ( edgeCount < 3 ) + { + return count; + } + + // Cull points. + int keptCount = 0; + for ( int i = 0; i < count; ++i ) + { + b3Vec2 p = points[i].p; + bool interior = true; + for ( int k = 0; k < edgeCount; ++k ) + { + if ( b3Dot2( normals[k], p ) <= offsets[k] ) + { + interior = false; + break; + } + } + + if ( interior == false ) + { + points[keptCount] = points[i]; + keptCount += 1; + } + } + + return keptCount; +} + #endif +// Fallback if culling leads to all points separated. Rare. +static void b3KeepDeepestPoint( b3Point2D* kept, int keptCount, const b3LocalManifoldPoint* points, int count, + b3Vec3 origin, b3Vec3 u, b3Vec3 v ) +{ + int deepest = 0; + for ( int i = 1; i < count; ++i ) + { + if ( points[i].separation < points[deepest].separation ) + { + deepest = i; + } + } + + float keptSeparation = FLT_MAX; + for ( int i = 0; i < keptCount; ++i ) + { + if ( kept[i].originalIndex == deepest ) + { + return; + } + + keptSeparation = b3MinFloat( keptSeparation, points[kept[i].originalIndex].separation ); + } + + float separation = points[deepest].separation; + if ( separation <= 0.0f || separation >= keptSeparation - B3_LINEAR_SLOP ) + { + return; + } + + int replaceIndex = 0; + float bestArea = -1.0f; + for ( int candidate = 0; candidate < keptCount; ++candidate ) + { + b3Vec2 ring[B3_MAX_MANIFOLD_POINTS]; + int ringCount = 0; + for ( int i = 0; i < keptCount; ++i ) + { + int index = i == candidate ? deepest : kept[i].originalIndex; + b3Vec3 d = b3Sub( points[index].point, origin ); + ring[ringCount] = (b3Vec2){ b3Dot( d, u ), b3Dot( d, v ) }; + ringCount += 1; + } + + float area = 0.0f; + for ( int i = 0; i < ringCount; ++i ) + { + for ( int j = i + 1; j < ringCount; ++j ) + { + for ( int k = j + 1; k < ringCount; ++k ) + { + float signedArea = b3Cross2( b3Sub2( ring[j], ring[i] ), b3Sub2( ring[k], ring[i] ) ); + area = b3MaxFloat( area, b3AbsFloat( signedArea ) ); + } + } + } + + if ( area > bestArea ) + { + bestArea = area; + replaceIndex = candidate; + } + } + + kept[replaceIndex].originalIndex = deepest; +} + static int b3ReduceCluster( b3LocalManifoldPoint* points, int count1, b3Vec3 normal, b3Arena arena ) { int targetCount = 1; @@ -496,14 +578,26 @@ static int b3ReduceCluster( b3LocalManifoldPoint* points, int count1, b3Vec3 nor #if B3_MAX_MANIFOLD_POINTS == 4 b3Point2D* hull = pts; - int count2 = b3CullPoints( pts, count1 ); + int count2; + if ( count1 <= B3_CULL_FILTER_THRESHOLD ) + { + count2 = b3CullPoints( pts, count1, false ); + } + else + { + // Many points. Reduce point count using bounding octogon. + int keptCount = b3DropInteriorPoints( pts, count1, 0.25f * B3_LINEAR_SLOP ); + count2 = b3CullPoints( pts, keptCount, keptCount > B3_CULL_SWEEP_THRESHOLD ); + } #else b3Point2D* hull = b3Bump( &arena, 2 * count1 * sizeof( b3Point2D ) ); - int hullCount = b3Hull2D( pts, count1, hull ); + int hullCount = b3Hull2D( pts, count1, hull, 0 ); int count2 = b3SimplifyHull2D( hull, hullCount, B3_MAX_MANIFOLD_POINTS ); #endif B3_ASSERT( count2 <= B3_MAX_MANIFOLD_POINTS ); + b3KeepDeepestPoint( hull, count2, points, count1, origin, u, v ); + b3LocalManifoldPoint finalPoints[B3_MAX_MANIFOLD_POINTS]; for ( int i = 0; i < count2; ++i ) { @@ -525,37 +619,550 @@ 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, - bool isFast, b3Arena arena ) +typedef enum b3NeighborResult { - B3_ASSERT( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape ); - B3_UNUSED( workerIndex ); - B3_UNUSED( isFast ); - B3_UNUSED( materialMap ); + // Admit for flat edges. + b3_neighborAdmit, + // Skip for convex edges. + b3_neighborSkip, + // Snap for concave edges. + b3_neighborSnap, + // Drop for a cliff. + b3_neighborDrop, +} b3NeighborResult; - b3TaskContext* context = b3Array_Get( world->taskContexts, workerIndex ); +// 1.0027 degrees +#define B3_NEIGHBOR_TOLERANCE 0.0175f - b3RefreshCache( contact, shapeA, xfA, &shapeB->aabb, arena ); +// Cross product of triangle edge with neighbor triangle normal. A vector pointing away from the edge +// in the neighbor triangle plane towards its center. +// tangent = cross(e, n) +static bool b3GetNeighborTangent( const b3Shape* shape, int triangleIndex, const b3Vec3* vertices, + b3Matrix3 meshToLocal, int edgeIndex, b3Vec3* tangent ) +{ + b3Vec3 neighborNormal; + + bool haveNeighbor = b3GetNeighborNormal( shape, triangleIndex, edgeIndex, &neighborNormal ); + if ( haveNeighbor == false ) + { + // Cliff edge axis along negative triangle normal. + *tangent = b3Neg( b3MakeNormalFromPoints( vertices[0], vertices[1], vertices[2] ) ); + return false; + } + + neighborNormal = b3MulMV( meshToLocal, neighborNormal ); + b3Vec3 edge = b3Normalize( b3Sub( vertices[( edgeIndex + 1 ) % 3], vertices[edgeIndex] ) ); + *tangent = b3Cross( edge, neighborNormal ); + return true; +} + +// Is the normal in the vertex Voronoi region? +static bool b3IsInVertexRegion( const b3Vec3* vertices, int vertexIndex, b3Vec3 normal ) +{ + b3Vec3 vertex = vertices[vertexIndex]; + b3Vec3 next = vertices[vertexIndex < 2 ? vertexIndex + 1 : 0]; + b3Vec3 previous = vertices[vertexIndex > 0 ? vertexIndex - 1 : 2]; + b3Vec3 e1 = b3Sub( next, vertex ); + b3Vec3 e2 = b3Sub( previous, vertex ); + return b3Dot( normal, e1 ) <= 0.0f && b3Dot( normal, e2 ) <= 0.0f; +} + +// This is used to avoid ghost collisions at vertices. It is called rarely. +// trees25 : 2.6% +// mesh_drop : 0.04% +static bool b3IsInVertexCone( const b3Shape* shape, int triangleIndex, int vertexIndex, b3Matrix3 meshToLocal, + b3Vec3 normal ) +{ + b3Vec3 meshNormal = b3MulMV( b3Transpose( meshToLocal ), normal ); + float tolerance = B3_NEIGHBOR_TOLERANCE; + + if ( shape->type == b3_meshShape ) + { + return b3IsInMeshVertexRing( &shape->mesh, triangleIndex, vertexIndex, meshNormal, tolerance ); + } + + B3_ASSERT( shape->type == b3_heightShape ); + float toleranceSquared = tolerance * tolerance; + + b3Triangle triangle = b3GetHeightFieldTriangle( shape->heightField, triangleIndex ); + int indices[3] = { triangle.i1, triangle.i2, triangle.i3 }; + int vertex = indices[vertexIndex]; + + int fanTriangles[6]; + int fanCount = b3GetHeightFieldVertexTriangles( shape->heightField, vertex, fanTriangles ); + + for ( int i = 0; i < fanCount; ++i ) + { + int fanIndex = fanTriangles[i]; + b3Triangle fan = b3GetHeightFieldTriangle( shape->heightField, fanIndex ); + int fanIndices[3] = { fan.i1, fan.i2, fan.i3 }; + int k = fanIndices[0] == vertex ? 0 : ( fanIndices[1] == vertex ? 1 : 2 ); + B3_ASSERT( fanIndices[k] == vertex ); + + int next = k < 2 ? k + 1 : 0; + int previous = k > 0 ? k - 1 : 2; + b3Vec3 edges[2] = { b3Sub( fan.vertices[next], fan.vertices[k] ), + b3Sub( fan.vertices[previous], fan.vertices[k] ) }; + int edgeIndices[2] = { k, previous }; + + for ( int j = 0; j < 2; ++j ) + { + float lean = b3Dot( meshNormal, edges[j] ); + if ( lean > 0.0f && lean * lean > toleranceSquared * b3LengthSquared( edges[j] ) ) + { + return false; + } + + b3Vec3 neighborNormal; + if ( b3GetNeighborNormal( shape, fanIndex, edgeIndices[j], &neighborNormal ) == false ) + { + // Cliff at boundary or hole. + b3Vec3 fanNormal = b3MakeNormalFromPoints( fan.vertices[0], fan.vertices[1], fan.vertices[2] ); + if ( b3Dot( meshNormal, fanNormal ) < -tolerance ) + { + return false; + } + } + } + } + + return true; +} + +// Check the vertex flags to determined if this normal is admissible. +static bool b3AdmitsVertexNormal( const b3Shape* shape, int triangleIndex, const b3Vec3* vertices, int vertexIndex, + b3Matrix3 meshToLocal, b3Vec3 normal ) +{ + if ( b3IsInVertexRegion( vertices, vertexIndex, normal ) == false ) + { + // Not in the vertex region so this vertex cannot admit the normal. + return false; + } + + if ( b3IsConvexVertex( shape, triangleIndex, vertexIndex ) == false ) + { + return false; + } + + return b3IsInVertexCone( shape, triangleIndex, vertexIndex, meshToLocal, normal ); +} + +// 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 candidateNormal, + int* ownerEdge ) +{ + *ownerEdge = B3_NULL_INDEX; + + int edges[2] = { 0, 0 }; + int edgeCount = 1; + int vertexIndex = B3_NULL_INDEX; + + 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; + vertexIndex = 0; + break; + + case b3_featureVertex2: + edges[0] = 0; + edges[1] = 1; + edgeCount = 2; + vertexIndex = 1; + break; + + case b3_featureVertex3: + edges[0] = 1; + edges[1] = 2; + edgeCount = 2; + vertexIndex = 2; + break; + + default: + return b3_neighborAdmit; + } + + 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 ) + { + // neighborTangent = cross(edgeAxis, neighborTriangleNormal) + b3Vec3 neighborTangent; + bool haveNeighbor = + b3GetNeighborTangent( shape, triangleIndex, vertices, meshToLocal, edges[i], &neighborTangent ); + + // Does the candidate normal lean into the neighbor's face region? + bool admitted = b3Dot( candidateNormal, neighborTangent ) <= tolerance; + if ( admitted == false && haveNeighbor && *ownerEdge == B3_NULL_INDEX ) + { + *ownerEdge = edges[i]; + } + + admit = admit && admitted; + + // Convex if at least one edge is convex. + convex = convex || b3Dot( triangleNormal, neighborTangent ) < -tolerance; + } + + if ( admit ) + { + if ( vertexIndex == B3_NULL_INDEX || + b3IsInVertexCone( shape, triangleIndex, vertexIndex, meshToLocal, candidateNormal ) ) + { + return b3_neighborAdmit; + } + + bool inVertexRegion = b3IsInVertexRegion( vertices, vertexIndex, candidateNormal ); + return inVertexRegion ? b3_neighborSkip : b3_neighborSnap; + } + + // Check vertex area first to avoid a saddle tunnel. + if ( vertexIndex != B3_NULL_INDEX && b3IsInVertexRegion( vertices, vertexIndex, candidateNormal ) && + b3IsInVertexCone( shape, triangleIndex, vertexIndex, meshToLocal, candidateNormal ) ) + { + return b3_neighborAdmit; + } + + if ( convex ) + { + // Edge is convex. Drop for a cliff with no deep consideration. + return *ownerEdge == B3_NULL_INDEX ? b3_neighborDrop : b3_neighborSkip; + } + + return b3_neighborSnap; +} + +// Classify a clipped hull point relative to the triangle features. ORs into the vertex and edge masks. +// Returns false if the point isn't close to an edge or vertex. +static bool b3ClassifyHullFacePoint( const b3Vec3* vertices, b3Vec3 triangleNormal, b3Vec3 point, b3FeaturePair pair, + int* vertexMask, int* edgeMask ) +{ + B3_ASSERT( pair.owner1 != pair.owner2 || pair.index1 != pair.index2 ); + + float linearSlop = B3_LINEAR_SLOP; + + int edges[3]; + int edgeCount = 0; + if ( pair.owner1 == b3_featureShapeA ) + { + edges[edgeCount] = pair.index1; + edgeCount += 1; + } + + if ( pair.owner2 == b3_featureShapeA && ( edgeCount == 0 || pair.index2 != edges[0] ) ) + { + edges[edgeCount] = pair.index2; + edgeCount += 1; + } + + if ( edgeCount == 0 ) + { + // Close to an edge? + for ( int i = 0; i < 3; ++i ) + { + b3Vec3 vertex = vertices[i]; + b3Vec3 edge = b3Normalize( b3Sub( vertices[i < 2 ? i + 1 : 0], vertex ) ); + b3Vec3 inwardTangent = b3Cross( triangleNormal, edge ); + if ( b3Dot( inwardTangent, b3Sub( point, vertex ) ) <= linearSlop ) + { + edges[edgeCount] = i; + edgeCount += 1; + } + } + } + + if ( edgeCount == 0 || edgeCount == 3 ) + { + // Not close to any triangle edge or triangle is very small. + return false; + } + + if ( edgeCount == 2 ) + { + // Close to two edges, mask the shared vertex. + int a = edges[0]; + int b = edges[1]; + int vertex = ( a < 2 ? a + 1 : 0 ) == b ? b : a; + *vertexMask |= 1 << vertex; + return true; + } + + B3_ASSERT( edgeCount == 1 ); + + // Close to only one edge. Determine if it should be classified as a vertex or edge point. + int edge = edges[0]; + int next = edge < 2 ? edge + 1 : 0; + if ( b3DistanceSquared( point, vertices[edge] ) <= linearSlop * linearSlop ) + { + *vertexMask |= 1 << edge; + } + else if ( b3DistanceSquared( point, vertices[next] ) <= linearSlop * linearSlop ) + { + *vertexMask |= 1 << next; + } + else + { + *edgeMask |= 1 << edge; + } + + return true; +} + +static inline bool b3IsSubset( int mask, int allowed ) +{ + return ( mask & ~allowed ) == 0; +} + +// Voronoi test of neighbors based on deepest point. +// Ghost collisions arise when the chose manifold normal does not align with what GJK would give with slight separation. +// A non-ghosting normal must be within the Voronoi regions the triangles face, edges, and vertices. However, even +// normals within the Voronoi regions can ghost. So we use the deepest point as a proxy for what GJK would yield with +// slight separation. +static b3NeighborResult b3TestHullFaceNeighbors( const b3Shape* shape, int triangleIndex, const b3Vec3* vertices, + b3Matrix3 meshToLocal, const b3LocalManifold* manifold, + b3TriangleFeature* nearFeature ) +{ + *nearFeature = b3_featureNone; + b3Vec3 normal = manifold->normal; + b3Vec3 triangleNormal = b3MakeNormalFromPoints( vertices[0], vertices[1], vertices[2] ); + float tolerance = B3_NEIGHBOR_TOLERANCE; + float cosAngle = b3Dot( normal, triangleNormal ); + if ( cosAngle > 0.0f && cosAngle * cosAngle >= 1.0f - tolerance * tolerance ) + { + // Face normal (flipped to point from A to B) is parallel to triangle normal. + return b3_neighborAdmit; + } + + float minSeparation = manifold->points[0].separation; + for ( int i = 1; i < manifold->pointCount; ++i ) + { + minSeparation = b3MinFloat( minSeparation, manifold->points[i].separation ); + } + + // Determine if the overlapped points are on the triangle face or boundary. Classify + // considering all points within slop of deepest point. + float depthLimit = minSeparation + B3_LINEAR_SLOP; + int vertexMask = 0; + int edgeMask = 0; + bool onFace = false; + for ( int i = 0; i < manifold->pointCount; ++i ) + { + const b3LocalManifoldPoint* manifoldPoint = manifold->points + i; + if ( manifoldPoint->separation > depthLimit ) + { + continue; + } + + // Clip points were moved onto the hull face, step back along the normal to the triangle. + b3Vec3 point = b3MulSub( manifoldPoint->point, manifoldPoint->separation, normal ); + bool onBoundary = + b3ClassifyHullFacePoint( vertices, triangleNormal, point, manifoldPoint->pair, &vertexMask, &edgeMask ); + + onFace = onFace || onBoundary == false; + } + + b3TriangleFeature vertexFeatures[3] = { b3_featureVertex1, b3_featureVertex2, b3_featureVertex3 }; + b3TriangleFeature edgeFeatures[3] = { b3_featureEdge1, b3_featureEdge2, b3_featureEdge3 }; + + // If the hull overlaps this triangle then determine if a deepest point is close to a vertex. + if ( minSeparation < 0.0f ) + { + for ( int i = 0; i < 3; ++i ) + { + if ( ( vertexMask & ( 1 << i ) ) == 0 ) + { + continue; + } + + bool admit = b3AdmitsVertexNormal( shape, triangleIndex, vertices, i, meshToLocal, normal ); + if ( admit ) + { + // This vertex tentatively accepts the normal. + *nearFeature = vertexFeatures[i]; + return b3_neighborAdmit; + } + } + } + + if ( onFace ) + { + // A tilted hull face over the triangle interior, the triangle face owns the normal. + return b3_neighborSnap; + } + + B3_ASSERT( edgeMask != 0 || vertexMask != 0 ); + + b3TriangleFeature feature = b3_featureNone; + int edges[2] = { 0, 0 }; + int edgeCount = 0; + + // Is the contact point close to only one vertex? + if ( edgeMask == 0 && vertexMask != 0 && ( vertexMask & ( vertexMask - 1 ) ) == 0 ) + { + int vertex = vertexMask == 1 ? 0 : ( vertexMask == 2 ? 1 : 2 ); + feature = vertexFeatures[vertex]; + edges[0] = vertex; + edges[1] = vertex > 0 ? vertex - 1 : 2; + edgeCount = 2; + } + else + { + // Is the contact point close to an edge + for ( int i = 0; i < 3; ++i ) + { + int next = i < 2 ? i + 1 : 0; + int edgeBit = 1 << i; + int vertexBits = ( 1 << i ) | ( 1 << next ); + + // Is this edge and its vertices a subset? One mask can be zero and always a subset. + if ( b3IsSubset( edgeMask, edgeBit ) && b3IsSubset( vertexMask, vertexBits ) ) + { + feature = edgeFeatures[i]; + edges[0] = i; + edgeCount = 1; + break; + } + } + } + + if ( feature == b3_featureNone ) + { + return b3_neighborSnap; + } + + for ( int i = 0; i < edgeCount; ++i ) + { + int edgeIndex = edges[i]; + b3Vec3 vertex = vertices[edgeIndex]; + b3Vec3 edge = b3Normalize( b3Sub( vertices[edgeIndex < 2 ? edgeIndex + 1 : 0], vertex ) ); + b3Vec3 inwardTangent = b3Cross( triangleNormal, edge ); + + // An edge or vertex normal cone tilts outward from the triangle and an edge cone is perpendicular to the + // edge, a hull face normal need be neither. + if ( b3Dot( normal, inwardTangent ) > tolerance ) + { + return b3_neighborSnap; + } + + if ( edgeCount == 1 && b3AbsFloat( b3Dot( normal, edge ) ) > tolerance ) + { + return b3_neighborSnap; + } + } + + *nearFeature = feature; + + // Never skip a hull face, its normal can miss the whole vertex cone so every triangle would + // skip, the face contact is empty when the hull is off this triangle. + int ownerEdge; + b3NeighborResult result = + b3TestNeighbors( shape, triangleIndex, vertices, meshToLocal, feature, normal, &ownerEdge ); + return result == b3_neighborSkip || result == b3_neighborDrop ? b3_neighborSnap : result; +} + +// Special handling if a boundary collision drops all points. Rare. +static void b3CollideBoundaryEdge( b3LocalManifold* manifold, const b3Shape* shapeA, int triangleIndex, + const b3Vec3* vertices, b3Matrix3 meshToLocal, const b3HullData* hull, + float speculativeDistance ) +{ + float linearSlop = B3_LINEAR_SLOP; + + b3DistanceInput input = { 0 }; + input.proxyA = (b3ShapeProxy){ vertices, 3, 0.0f }; + input.proxyB = (b3ShapeProxy){ b3GetHullPoints( hull ), hull->vertexCount, 0.0f }; + input.transform = b3Transform_identity; + input.useRadii = false; + b3SimplexCache simplexCache = { 0 }; + b3DistanceOutput output = b3ShapeDistance( &input, &simplexCache, NULL, 0 ); + + if ( output.distance < B3_OVERLAP_SLOP || output.distance > speculativeDistance ) + { + return; + } + + float tolerance = 0.01f * linearSlop; + b3TriangleFeature feature = b3_featureNone; + bool onBoundary = false; + b3TriangleFeature vertexFeatures[] = { b3_featureVertex1, b3_featureVertex2, b3_featureVertex3 }; + b3TriangleFeature edgeFeatures[] = { b3_featureEdge1, b3_featureEdge2, b3_featureEdge3 }; + for ( int i = 0; i < 3; ++i ) + { + b3Vec3 a = vertices[i]; + b3Vec3 e = b3Sub( vertices[i < 2 ? i + 1 : 0], a ); + float t = b3ClampFloat( b3Dot( b3Sub( output.pointA, a ), e ) / b3LengthSquared( e ), 0.0f, 1.0f ); + b3Vec3 closest = b3MulAdd( a, t, e ); + if ( b3DistanceSquared( closest, output.pointA ) >= tolerance * tolerance ) + { + continue; + } + + b3Vec3 neighborNormal; + onBoundary = onBoundary || b3GetNeighborNormal( shapeA, triangleIndex, i, &neighborNormal ) == false; + + if ( b3DistanceSquared( a, output.pointA ) < tolerance * tolerance ) + { + feature = vertexFeatures[i]; + } + else if ( b3DistanceSquared( b3Add( a, e ), output.pointA ) < tolerance * tolerance ) + { + feature = vertexFeatures[i < 2 ? i + 1 : 0]; + } + else if ( feature == b3_featureNone ) + { + feature = edgeFeatures[i]; + } + } + + if ( onBoundary == false || feature == b3_featureNone ) + { + return; + } + + int ownerEdge; + if ( b3TestNeighbors( shapeA, triangleIndex, vertices, meshToLocal, feature, output.normal, &ownerEdge ) != + b3_neighborAdmit ) + { + return; + } + + b3LocalManifoldPoint* point = manifold->points + 0; + point->point = b3Lerp( output.pointA, output.pointB, 0.5f ); + point->separation = output.distance; + point->pair = b3MakeFeaturePair( b3_featureShapeA, (int)feature, b3_featureShapeA, (int)feature ); + manifold->normal = output.normal; + manifold->pointCount = 1; + manifold->feature = feature; +} + +static int b3CollideMeshTriangles( b3Cluster** clustersOut, const b3Shape* shapeA, const b3Shape* shapeB, + b3Transform transformAtoB, b3TriangleCache* triangleCaches, int triangleCount, + bool enableSpeculative, bool handleGhosts, bool isFast, b3TaskContext* context, + b3Arena* arena ) +{ + *clustersOut = NULL; // Collide with triangles and build manifolds - 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* ) ); - int tentativeManifoldCount = 0; - b3TentativeTriangle* tentativeTriangles = b3Bump( &arena, triangleCount * sizeof( b3TentativeTriangle ) ); - int tentativeTriangleCount = 0; - b3FoundEdges foundEdges; - b3FoundVertices foundVertices; - foundEdges.count = 0; - foundVertices.count = 0; - - // This transform converts from mesh frame into the shapeB frame - b3Transform transformAtoB = b3InvMulWorldTransforms( xfB, xfA ); b3Matrix3 relativeMatrix = b3MakeMatrixFromQuat( transformAtoB.q ); float linearSlop = B3_LINEAR_SLOP; @@ -565,37 +1172,27 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact // It leads to a small visual gap but seems to improve the quality of mesh // collision, especially for hull versus mesh. float restOffset = B3_MESH_REST_OFFSET; - bool enableSpeculative = contact->flags & b3_enableSpeculativePoints; - - // Make room for clip points - int pointBufferCapacity = B3_MAX_POINTS_PER_TRIANGLE * triangleCount; - - b3LocalManifoldPoint* pointBuffer = b3Bump( &arena, pointBufferCapacity * sizeof( b3LocalManifoldPoint ) ); - int totalPointCount = 0; - - b3LocalManifold* manifoldBuffer = b3Bump( &arena, triangleCount * sizeof( b3LocalManifold ) ); - int manifoldCount = 0; - - b3TriangleCache* triangleCaches = meshContact->triangleCache.data; const b3HullData* hullB = shapeB->type == b3_hullShape ? shapeB->hull : NULL; - float hullSpeculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f; + float hullSpeculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : restOffset; b3Vec3 hullMargin = { hullSpeculativeDistance, hullSpeculativeDistance, hullSpeculativeDistance }; - int* survivorIndices = b3Bump( &arena, triangleCount * sizeof( int ) ); - b3Vec3* survivorVertices = b3Bump( &arena, 3 * triangleCount * sizeof( b3Vec3 ) ); + int* survivorIndices = b3Bump( arena, triangleCount * sizeof( int ) ); + b3Vec3* survivorVertices = b3Bump( arena, 3 * triangleCount * sizeof( b3Vec3 ) ); int survivorCount = 0; const b3MeshTriangle* meshTriangles = NULL; const b3Vec3* meshVertices = NULL; b3Vec3 meshScale = b3Vec3_one; bool meshFlipped = false; + bool unitScale = true; if ( shapeA->type == b3_meshShape ) { meshTriangles = b3GetMeshTriangles( shapeA->mesh.data ); meshVertices = b3GetMeshVertices( shapeA->mesh.data ); meshScale = shapeA->mesh.scale; meshFlipped = meshScale.x * meshScale.y * meshScale.z < 0.0f; + unitScale = meshScale.x == 1.0f && meshScale.y == 1.0f && meshScale.z == 1.0f; } b3FloatW4 cxx = b3SplatW4( relativeMatrix.cx.x ); @@ -633,9 +1230,21 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact if ( meshVertices != NULL ) { 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] ); + b3Vec3 v1 = meshVertices[meshTriangle.index1]; + b3Vec3 v2 = meshVertices[meshFlipped ? meshTriangle.index3 : meshTriangle.index2]; + b3Vec3 v3 = meshVertices[meshFlipped ? meshTriangle.index2 : meshTriangle.index3]; + + // This bool gets predicted, so worth it. + if ( unitScale == false ) + { + v1 = b3Mul( meshScale, v1 ); + v2 = b3Mul( meshScale, v2 ); + v3 = b3Mul( meshScale, v3 ); + } + + lanes[0][lane] = v1; + lanes[1][lane] = v2; + lanes[2][lane] = v3; } else { @@ -671,8 +1280,10 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact 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 ) ) ); + 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; @@ -712,49 +1323,59 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact } } - for ( int survivorIndex = 0; survivorIndex < survivorCount && totalPointCount + 3 < pointBufferCapacity; ++survivorIndex ) + if ( survivorCount == 0 ) + { + return 0; + } + + // 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 ) ); + int totalPointCount = 0; + + b3LocalManifold* manifoldBuffer = b3Bump( arena, survivorCount * sizeof( b3LocalManifold ) ); + int manifoldCount = 0; + + int* survivorManifolds = b3Bump( arena, survivorCount * sizeof( int ) ); + memset( survivorManifolds, 0xFF, survivorCount * sizeof( int ) ); + + for ( int survivorIndex = 0; survivorIndex < survivorCount && totalPointCount + 3 < pointBufferCapacity; + ++survivorIndex ) { int index = survivorIndices[survivorIndex]; int triangleIndex = triangleCaches[index].triangleIndex; const b3Vec3* vertices = survivorVertices + 3 * survivorIndex; - b3Triangle triangle; - if ( shapeA->type == b3_meshShape ) - { - triangle = b3GetMeshTriangle( &shapeA->mesh, triangleIndex ); - } - else - { - B3_ASSERT( shapeA->type == b3_heightShape ); - triangle = b3GetHeightFieldTriangle( shapeA->heightField, triangleIndex ); - } - b3ContactCache* cache = &triangleCaches[index].cache; int pointCapacity = pointBufferCapacity - totalPointCount; b3LocalManifold* manifold = manifoldBuffer + manifoldCount; manifold->points = pointBuffer + totalPointCount; manifold->pointCount = 0; - manifold->triangleFlags = triangle.flags; manifold->feature = b3_featureNone; 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, + b3CollideTriangleAndHull( manifold, pointCapacity, vertices[0], vertices[1], vertices[2], hullB, &cache->satCache, enableSpeculative ); - context->satCallCount += 1; - context->satCacheHitCount += cache->satCache.hit; + if ( context != NULL ) + { + context->satCallCount += 1; + context->satCacheHitCount += cache->satCache.hit; + } break; case b3_sphereShape: @@ -763,7 +1384,103 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact default: B3_ASSERT( false ); - return false; + return 0; + } + + b3TriangleFeature nearFeature = manifold->feature; + b3NeighborResult result = b3_neighborAdmit; + int ownerEdge = B3_NULL_INDEX; + uint8_t meshFlags = manifold->feature == b3_featureHullFace ? b3_meshContactHullFace : 0; + if ( handleGhosts && manifold->pointCount > 0 && manifold->feature != b3_featureTriangleFace ) + { + if ( manifold->feature == b3_featureHullFace ) + { + result = + b3TestHullFaceNeighbors( shapeA, triangleIndex, vertices, relativeMatrix, manifold, &nearFeature ); + } + else + { + result = b3TestNeighbors( shapeA, triangleIndex, vertices, relativeMatrix, manifold->feature, + manifold->normal, &ownerEdge ); + } + + // Check deep overlap before skipping. + if ( result == b3_neighborSkip ) + { + bool deep = false; + for ( int i = 0; i < manifold->pointCount; ++i ) + { + if ( manifold->points[i].separation < -2.0f * linearSlop ) + { + deep = true; + break; + } + } + + if ( deep ) + { + meshFlags |= b3_meshContactDeep; + + // Handle deep overlap. + if ( shapeB->type == b3_sphereShape ) + { + // No way to snap a deep sphere normal. + b3Vec3 neighborNormal; + if ( ownerEdge != B3_NULL_INDEX && + b3GetNeighborNormal( shapeA, triangleIndex, ownerEdge, &neighborNormal ) ) + { + neighborNormal = b3MulMV( relativeMatrix, neighborNormal ); + float neighborSeparation = + b3Dot( neighborNormal, b3Sub( shapeB->sphere.center, vertices[ownerEdge] ) ); + if ( neighborSeparation < 0.0f ) + { + // Neighbor would cull, so better keep this one. + result = b3_neighborAdmit; + } + } + } + else + { + // In deep overlap the owning neighbor may apply backside culling. Experiments show that in this + // case the neighbor should snap the normal. + result = b3_neighborSnap; + } + } + } + + if ( result == b3_neighborSnap ) + { + meshFlags |= b3_meshContactSnapped; + } + + float rawSeparation = FLT_MAX; + for ( int i = 0; i < manifold->pointCount; ++i ) + { + rawSeparation = b3MinFloat( rawSeparation, manifold->points[i].separation ); + } + + if ( result == b3_neighborSkip || result == b3_neighborSnap || result == b3_neighborDrop ) + { + // 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 ); + } + else if ( result == b3_neighborSnap && shapeB->type == b3_hullShape ) + { + b3BuildTriangleAndHullFaceContact( manifold, pointCapacity, vertices, hullB, &cache->satCache, + enableSpeculative ); + if ( rawSeparation > 0.0f ) + { + b3CollideBoundaryEdge( manifold, shapeA, triangleIndex, vertices, relativeMatrix, hullB, + hullSpeculativeDistance ); + } + } } int manifoldPointCount = manifold->pointCount; @@ -772,195 +1489,96 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact { B3_ASSERT( manifold->feature != b3_featureNone ); - manifoldCount += 1; - totalPointCount += manifoldPointCount; + b3TriangleFeature pointFeature = manifold->feature; + if ( pointFeature == b3_featureHullFace && nearFeature != b3_featureHullFace && + nearFeature != b3_featureNone ) + { + pointFeature = nearFeature; + } + else if ( pointFeature == b3_featureHullFace && nearFeature == b3_featureNone && + result == b3_neighborAdmit ) + { + pointFeature = b3_featureTriangleFace; + } + + for ( int i = 0; i < manifoldPointCount; ++i ) + { + manifold->points[i].triangleFeature = (uint8_t)pointFeature; + manifold->points[i].meshFlags = meshFlags; + } + manifold->triangleIndex = triangleIndex; manifold->triangleNormal = b3MakeNormalFromPoints( vertices[0], vertices[1], vertices[2] ); - manifold->i1 = triangle.i1; - manifold->i2 = triangle.i2; - manifold->i3 = triangle.i3; - if ( manifold->feature == b3_featureTriangleFace || B3_FORCE_GHOST_COLLISIONS ) + survivorManifolds[survivorIndex] = manifoldCount; + manifoldCount += 1; + totalPointCount += manifoldPointCount; + } + } + + if ( hullB != NULL && handleGhosts ) + { + // Cull triangles fully below the hull face when it rests on a triangle and has a tilted manifold normal. This + // helps when a box slides across a chamfered channel. + float tolerance = B3_NEIGHBOR_TOLERANCE; + float cosSquaredTolerance = 1.0f - tolerance * tolerance; + const b3LocalManifold* resting = NULL; + for ( int i = 0; i < manifoldCount && resting == NULL; ++i ) + { + const b3LocalManifold* manifold = manifoldBuffer + i; + float cosAngle = b3Dot( manifold->normal, manifold->triangleNormal ); + if ( manifold->pointCount > 0 && manifold->feature == b3_featureHullFace && cosAngle > 0.0f && + cosAngle * cosAngle >= cosSquaredTolerance ) { - (void)b3AddEdge( &foundEdges, triangle.i1, triangle.i2 ); - (void)b3AddEdge( &foundEdges, triangle.i2, triangle.i3 ); - (void)b3AddEdge( &foundEdges, triangle.i3, triangle.i1 ); - (void)b3AddVertex( &foundVertices, triangle.i1 ); - (void)b3AddVertex( &foundVertices, triangle.i2 ); - (void)b3AddVertex( &foundVertices, triangle.i3 ); - - acceptedManifolds[acceptedManifoldCount++] = manifold; + resting = manifold; } - else if ( manifold->feature == b3_featureHullFace ) + } + + if ( resting != NULL ) + { + b3Vec3 normal = resting->triangleNormal; + const b3Vec3* hullPoints = b3GetHullPoints( hullB ); + float hullBottom = FLT_MAX; + for ( int i = 0; i < hullB->vertexCount; ++i ) { - float cosNormalAngle = b3Dot( manifold->triangleNormal, manifold->normal ); - if ( cosNormalAngle > 0.5f ) - { - (void)b3AddEdge( &foundEdges, triangle.i1, triangle.i2 ); - (void)b3AddEdge( &foundEdges, triangle.i2, triangle.i3 ); - (void)b3AddEdge( &foundEdges, triangle.i3, triangle.i1 ); - (void)b3AddVertex( &foundVertices, triangle.i1 ); - (void)b3AddVertex( &foundVertices, triangle.i2 ); - (void)b3AddVertex( &foundVertices, triangle.i3 ); - - acceptedManifolds[acceptedManifoldCount++] = manifold; - } - else - { - float minSeparation = manifold->points[0].separation; - for ( int i = 1; i < manifoldPointCount; ++i ) - { - minSeparation = b3MinFloat( minSeparation, manifold->points[i].separation ); - } - - if ( minSeparation < -2.0f * linearSlop ) - { - // Deep overlap - (void)b3AddEdge( &foundEdges, triangle.i1, triangle.i2 ); - (void)b3AddEdge( &foundEdges, triangle.i2, triangle.i3 ); - (void)b3AddEdge( &foundEdges, triangle.i3, triangle.i1 ); - (void)b3AddVertex( &foundVertices, triangle.i1 ); - (void)b3AddVertex( &foundVertices, triangle.i2 ); - (void)b3AddVertex( &foundVertices, triangle.i3 ); - acceptedManifolds[acceptedManifoldCount++] = manifold; - } - else - { - b3TentativeTriangle tentativeTriangle = { .squaredDistance = manifold->squaredDistance, - .index = tentativeManifoldCount }; - tentativeTriangles[tentativeTriangleCount++] = tentativeTriangle; - tentativeManifolds[tentativeManifoldCount++] = manifold; - } - } + hullBottom = b3MinFloat( hullBottom, b3Dot( normal, hullPoints[i] ) ); } - else + + float restingSeparation = FLT_MAX; + for ( int i = 0; i < resting->pointCount; ++i ) { - b3TentativeTriangle tentativeTriangle = { .squaredDistance = manifold->squaredDistance, - .index = tentativeManifoldCount }; - tentativeTriangles[tentativeTriangleCount++] = tentativeTriangle; - tentativeManifolds[tentativeManifoldCount++] = manifold; + restingSeparation = b3MinFloat( restingSeparation, resting->points[i].separation ); + } + + // Padding + hullBottom -= b3MaxFloat( 0.0f, restingSeparation ) + linearSlop; + + for ( int i = 0; i < survivorCount; ++i ) + { + if ( survivorManifolds[i] == B3_NULL_INDEX ) + { + continue; + } + + b3LocalManifold* manifold = manifoldBuffer + survivorManifolds[i]; + const b3Vec3* vertices = survivorVertices + 3 * i; + float cosAngle = b3Dot( manifold->normal, normal ); + float height = b3MaxFloat( b3Dot( normal, vertices[0] ), + b3MaxFloat( b3Dot( normal, vertices[1] ), b3Dot( normal, vertices[2] ) ) ); + if ( ( cosAngle <= 0.0f || cosAngle * cosAngle < cosSquaredTolerance ) && height < hullBottom ) + { + manifold->pointCount = 0; + } } } } - B3_ASSERT( acceptedManifoldCount <= triangleCount ); - B3_ASSERT( tentativeManifoldCount <= triangleCount ); - B3_ASSERT( tentativeTriangleCount <= triangleCount ); - - if ( shapeB->type == b3_sphereShape ) + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) { - // Sort triangles so the closest triangles are processed first + b3LocalManifold* manifold = manifoldBuffer + manifoldIndex; + if ( manifold->pointCount > 0 ) { -#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. - for ( int i = 0; i < tentativeTriangleCount; ++i ) - { - b3LocalManifold* m = tentativeManifolds[tentativeTriangles[i].index]; - - bool addedEdge1 = b3AddEdge( &foundEdges, m->i1, m->i2 ); - bool addedEdge2 = b3AddEdge( &foundEdges, m->i2, m->i3 ); - bool addedEdge3 = b3AddEdge( &foundEdges, m->i3, m->i1 ); - bool addedVertex1 = b3AddVertex( &foundVertices, m->i1 ); - bool addedVertex2 = b3AddVertex( &foundVertices, m->i2 ); - bool addedVertex3 = b3AddVertex( &foundVertices, m->i3 ); - - b3TriangleFeature feature = m->feature; - bool shouldCollide = false; - switch ( feature ) - { - case b3_featureNone: - case b3_featureTriangleFace: - B3_ASSERT( false ); - break; - - case b3_featureEdge1: - shouldCollide = addedEdge1; - break; - - case b3_featureEdge2: - shouldCollide = addedEdge2; - break; - - case b3_featureEdge3: - shouldCollide = addedEdge3; - break; - - case b3_featureVertex1: - shouldCollide = addedVertex1; - break; - - case b3_featureVertex2: - shouldCollide = addedVertex2; - break; - - case b3_featureVertex3: - shouldCollide = addedVertex3; - break; - - default: - B3_ASSERT( false ); - break; - } - - if ( shouldCollide == true ) - { - acceptedManifolds[acceptedManifoldCount++] = m; - } - } - } - else - { - // Problem: hull can tunnel if time of impact is at concave edge - // Example: flat box sliding down a ramp to a flat bottom - // Solution: only ignore flat edges - for ( int i = 0; i < tentativeManifoldCount; ++i ) - { - b3LocalManifold* m = tentativeManifolds[i]; - int triangleFlags = m->triangleFlags; - - if ( ( triangleFlags & b3_allFlatEdges ) == b3_allFlatEdges ) - { - continue; - } - - if ( ( triangleFlags & b3_flatEdge1 ) == b3_flatEdge1 ) - { - if ( b3FindEdge( &foundEdges, m->i1, m->i2 ) ) - { - continue; - } - } - - if ( ( triangleFlags & b3_flatEdge2 ) == b3_flatEdge2 ) - { - if ( b3FindEdge( &foundEdges, m->i2, m->i3 ) ) - { - continue; - } - } - - if ( ( triangleFlags & b3_flatEdge3 ) == b3_flatEdge3 ) - { - if ( b3FindEdge( &foundEdges, m->i3, m->i1 ) ) - { - continue; - } - } - - acceptedManifolds[acceptedManifoldCount++] = m; + acceptedManifolds[acceptedManifoldCount++] = manifold; } } @@ -968,17 +1586,11 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact if ( acceptedManifoldCount == 0 ) { - if ( contact->manifoldCount > 0 ) - { - b3FreeManifolds( world, contact->manifolds, contact->manifoldCount ); - contact->manifolds = NULL; - contact->manifoldCount = 0; - } - return false; + return 0; } - b3Cluster* clusters = b3Bump( &arena, acceptedManifoldCount * sizeof( b3Cluster ) ); - int* clusterMemberships = b3Bump( &arena, acceptedManifoldCount * sizeof( int ) ); + b3Cluster* clusters = b3Bump( arena, acceptedManifoldCount * sizeof( b3Cluster ) ); + int* clusterMemberships = b3Bump( arena, acceptedManifoldCount * sizeof( int ) ); // Cluster tolerance is tighter than the warm starting manifold matching tolerance. These // serve different purposes. @@ -1010,37 +1622,9 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact continue; } -#if 0 - // todo there could be later triangles that create the connection - // then failure to cluster breaks greedy impulse warm starting - bool edgeConnected = false; - - for ( int k = 0; k < i; ++k ) - { - if ( clusterMemberships[k] != j ) - { - continue; - } - - const b3LocalManifold* other = acceptedManifolds[k]; - if ( b3TrianglesShareEdge( manifold->i1, manifold->i2, manifold->i3, other->i1, other->i2, other->i3 ) ) - { - edgeConnected = true; - break; - } - } - - if ( edgeConnected ) - { - clusterIndex = j; - break; - } -#else - // Found a cluster clusterIndex = j; break; -#endif } if ( clusterIndex != B3_NULL_INDEX ) @@ -1060,11 +1644,11 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact if ( clusterPointCount == 0 ) { - return false; + return 0; } // 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 ) @@ -1098,6 +1682,8 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact cp->point = ap->point; cp->separation = ap->separation; cp->pair = ap->pair; + cp->triangleFeature = ap->triangleFeature; + cp->meshFlags = ap->meshFlags; cm->pointCount += 1; } } @@ -1107,10 +1693,46 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact { b3Cluster* cm = clusters + i; B3_ASSERT( cm->pointCount == cm->pointCapacity ); - int reducedCount = b3ReduceCluster( cm->points, cm->pointCount, cm->triangleNormal, arena ); + int reducedCount = b3ReduceCluster( cm->points, cm->pointCount, cm->triangleNormal, *arena ); cm->pointCount = reducedCount; } + *clustersOut = clusters; + return clusterCount; +} + +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 ); + b3TaskContext* context = b3Array_Get( world->taskContexts, workerIndex ); + + b3RefreshCache( contact, shapeA, xfA, &shapeB->aabb, arena ); + + b3MeshContact* meshContact = &contact->meshContact; + b3Transform transformAtoB = b3InvMulWorldTransforms( xfB, xfA ); + bool enableSpeculative = contact->flags & b3_enableSpeculativePoints; + bool handleGhosts = B3_FORCE_GHOST_COLLISIONS == 0; + + b3Cluster* clusters; + int clusterCount = b3CollideMeshTriangles( &clusters, shapeA, shapeB, transformAtoB, + meshContact->triangleCache.data, meshContact->triangleCache.count, + enableSpeculative, handleGhosts, isFast, context, &arena ); + + if ( clusterCount == 0 ) + { + if ( contact->manifoldCount > 0 ) + { + b3FreeManifolds( world, contact->manifolds, contact->manifoldCount ); + contact->manifolds = NULL; + contact->manifoldCount = 0; + } + return false; + } + + float restOffset = B3_MESH_REST_OFFSET; + // Make a temporary copy of previous manifolds int oldManifoldCount = contact->manifoldCount; b3Manifold* oldManifolds = NULL; @@ -1194,6 +1816,8 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact target->separation = source->separation - restOffset; target->featureId = b3MakeFeatureId( source->pair ); target->triangleIndex = source->triangleIndex; + target->triangleFeature = source->triangleFeature; + target->meshFlags = source->meshFlags; // Preserve normal impulse if possible if ( matchedManifold != NULL ) @@ -1264,8 +1888,8 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact 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 ); + restitution += world->restitutionCallback( material.restitution, material.userMaterialId, + materialB->restitution, materialB->userMaterialId ); tangentVelocityA = b3Add( tangentVelocityA, material.tangentVelocity ); @@ -1287,8 +1911,8 @@ 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->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; @@ -1316,3 +1940,106 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact contact->tangentVelocity = b3Sub( tangentVelocityA, tangentVelocityB ); return true; } + +// Mesh collision testing interface. +static int b3CollideMeshAndShape( b3Manifold* manifolds, int capacity, const b3Mesh* mesh, const b3Shape* shapeB, + b3AABB localBounds, b3Transform transformBtoA, bool enableSpeculative, + bool handleGhosts ) +{ + if ( capacity <= 0 ) + { + return 0; + } + + b3Shape meshShape = { + .type = b3_meshShape, + .mesh = *mesh, + }; + + b3Arena arena = b3CreateArena( 64 * 1024 ); + + float margin = B3_SPECULATIVE_DISTANCE; + b3Vec3 extension = { margin, margin, margin }; + b3AABB shapeBounds = b3AABB_Transform( transformBtoA, localBounds ); + b3AABB queryBounds = { + .lowerBound = b3Sub( shapeBounds.lowerBound, extension ), + .upperBound = b3Add( shapeBounds.upperBound, extension ), + }; + + b3TriangleQueryContext queryContext = { + .arena = &arena, + .indices = b3Bump( &arena, B3_INITIAL_TRIANGLE_CAPACITY * (int)sizeof( int ) ), + .capacity = B3_INITIAL_TRIANGLE_CAPACITY, + .count = 0, + }; + + b3QueryMesh( mesh, queryBounds, b3CollectTriangleIndicesCallback, &queryContext ); + + int triangleCount = queryContext.count; + b3TriangleCache* triangleCaches = b3Bump( &arena, triangleCount * (int)sizeof( b3TriangleCache ) ); + for ( int i = 0; i < triangleCount; ++i ) + { + triangleCaches[i] = (b3TriangleCache){ .triangleIndex = queryContext.indices[i] }; + } + + b3Transform transformAtoB = b3InvertTransform( transformBtoA ); + + b3Cluster* clusters; + int clusterCount = b3CollideMeshTriangles( &clusters, &meshShape, shapeB, transformAtoB, triangleCaches, + triangleCount, enableSpeculative, handleGhosts, false, NULL, &arena ); + + int count = b3MinInt( clusterCount, capacity ); + b3Matrix3 matrixB = b3MakeMatrixFromQuat( transformBtoA.q ); + float restOffset = B3_MESH_REST_OFFSET; + + for ( int i = 0; i < count; ++i ) + { + const b3Cluster* cluster = clusters + i; + b3Manifold* manifold = manifolds + i; + *manifold = (b3Manifold){ 0 }; + manifold->normal = b3MulMV( matrixB, cluster->manifoldNormal ); + manifold->pointCount = cluster->pointCount; + + for ( int j = 0; j < cluster->pointCount; ++j ) + { + const b3LocalManifoldPoint* source = cluster->points + j; + b3ManifoldPoint* target = manifold->points + j; + target->anchorB = b3MulMV( matrixB, source->point ); + target->anchorA = b3Add( target->anchorB, transformBtoA.p ); + target->separation = source->separation - restOffset; + target->featureId = b3MakeFeatureId( source->pair ); + target->triangleIndex = source->triangleIndex; + target->triangleFeature = source->triangleFeature; + target->meshFlags = source->meshFlags; + } + } + + b3DestroyArena( &arena ); + return count; +} + +int b3CollideMeshAndHull( b3Manifold* manifolds, int capacity, const b3Mesh* mesh, const b3HullData* hull, + b3Transform transformBtoA, bool enableSpeculative, bool handleGhosts ) +{ + b3Shape hullShape = { + .type = b3_hullShape, + .hull = hull, + }; + + return b3CollideMeshAndShape( manifolds, capacity, mesh, &hullShape, hull->aabb, transformBtoA, enableSpeculative, + handleGhosts ); +} + +int b3CollideMeshAndSphere( b3Manifold* manifolds, int capacity, const b3Mesh* mesh, const b3Sphere* sphere, + b3Transform transformBtoA, bool enableSpeculative, bool handleGhosts ) +{ + b3Shape sphereShape = { + .type = b3_sphereShape, + .sphere = *sphere, + }; + + b3Vec3 extent = { sphere->radius, sphere->radius, sphere->radius }; + b3AABB bounds = { b3Sub( sphere->center, extent ), b3Add( sphere->center, extent ) }; + return b3CollideMeshAndShape( manifolds, capacity, mesh, &sphereShape, bounds, transformBtoA, enableSpeculative, + handleGhosts ); +} diff --git a/src/shape.h b/src/shape.h index a9164d4..4f6ee87 100644 --- a/src/shape.h +++ b/src/shape.h @@ -129,6 +129,10 @@ float b3ComputeHullProjectedArea( const b3HullData* hull, b3Vec3 direction ); b3Triangle b3GetHeightFieldTriangle( const b3HeightFieldData* heightField, int triangleIndex ); int b3GetHeightFieldMaterial( const b3HeightFieldData* heightField, int triangleIndex ); +bool b3GetHeightFieldNeighborNormal( const b3HeightFieldData* heightField, int triangleIndex, int edgeIndex, b3Vec3* normal ); +bool b3IsHeightFieldVertexConvex( const b3HeightFieldData* heightField, int triangleIndex, int vertexIndex ); +int b3GetHeightFieldVertexTriangles( const b3HeightFieldData* heightField, int vertex, int triangles[6] ); + static inline int b3GetHeightFieldTriangleCount( const b3HeightFieldData* heightField ) { int cellCount = ( heightField->rowCount - 1 ) * ( heightField->columnCount - 1 ); @@ -143,12 +147,10 @@ static inline b3Triangle b3GetMeshTriangle( const b3Mesh* mesh, int triangleInde B3_ASSERT( 0 <= triangleIndex && triangleIndex < mesh->data->triangleCount ); const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh->data ); - const uint8_t* flags = b3GetMeshFlags( mesh->data ); const b3Vec3* vertices = b3GetMeshVertices( mesh->data ); b3Triangle result; b3MeshTriangle triangle = triangles[triangleIndex]; - uint8_t triangleFlags = flags[triangleIndex]; b3Vec3 scale = mesh->scale; @@ -163,11 +165,6 @@ static inline b3Triangle b3GetMeshTriangle( const b3Mesh* mesh, int triangleInde result.i2 = triangle.index3; result.i3 = triangle.index2; - // mesh is inverted, so concave edges are now convex - result.flags = 0; - result.flags |= ( triangleFlags & b3_inverseConcaveEdge1 ) ? b3_concaveEdge1 : 0; - result.flags |= ( triangleFlags & b3_inverseConcaveEdge2 ) ? b3_concaveEdge2 : 0; - result.flags |= ( triangleFlags & b3_inverseConcaveEdge3 ) ? b3_concaveEdge3 : 0; } else { @@ -176,12 +173,41 @@ static inline b3Triangle b3GetMeshTriangle( const b3Mesh* mesh, int triangleInde result.i2 = triangle.index2; result.i3 = triangle.index3; - result.flags = triangleFlags; } return result; } +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 ) + { + return b3GetMeshNeighborNormal( &shape->mesh, triangleIndex, edgeIndex, normal ); + } + + B3_ASSERT( shape->type == b3_heightShape ); + return b3GetHeightFieldNeighborNormal( shape->heightField, triangleIndex, edgeIndex, normal ); +} + +int b3GetMeshVertexFlag( const b3Mesh* mesh, int triangleIndex, int vertexIndex ); +bool b3IsInMeshVertexRing( const b3Mesh* mesh, int triangleIndex, int vertexIndex, b3Vec3 normal, float tolerance ); +int8_t b3QuantizeEdgeAngle( float radians ); + +static inline bool b3IsConvexVertex( const b3Shape* shape, int triangleIndex, int vertexIndex ) +{ + if ( shape->type == b3_meshShape ) + { + int flags = b3GetMeshVertexFlag( &shape->mesh, triangleIndex, vertexIndex ); + return ( flags & ( b3_flatVertex | b3_concaveVertex ) ) == 0; + } + + B3_ASSERT( shape->type == b3_heightShape ); + return b3IsHeightFieldVertexConvex( shape->heightField, triangleIndex, vertexIndex ); +} + bool b3IsValidMesh( const b3MeshData* meshData ); static inline bool b3ShouldShapesCollide( b3Filter filterA, b3Filter filterB ) diff --git a/src/simd.h b/src/simd.h index e10b9ed..8250f6f 100644 --- a/src/simd.h +++ b/src/simd.h @@ -29,6 +29,30 @@ #endif +static inline b3FloatW4 b3Atan2W4( b3FloatW4 y, b3FloatW4 x ) +{ + b3FloatW4 zero = b3ZeroW4(); + b3FloatW4 ax = b3AbsW4( x ); + b3FloatW4 ay = b3AbsW4( y ); + b3FloatW4 mx = b3MaxW4( ay, ax ); + b3FloatW4 mn = b3MinW4( ay, ax ); + b3FloatW4 a = b3DivW4( mn, mx ); + + b3FloatW4 s = b3MulW4( a, a ); + b3FloatW4 c = b3MulW4( s, a ); + b3FloatW4 q = b3MulW4( s, s ); + b3FloatW4 r = b3MulAddW4( b3SplatW4( 0.18681418f ), b3SplatW4( 0.024840285f ), q ); + b3FloatW4 t = b3SubW4( b3MulW4( b3SplatW4( -0.094097948f ), q ), b3SplatW4( 0.33213072f ) ); + r = b3MulAddW4( t, r, s ); + r = b3MulAddW4( a, r, c ); + + r = b3BlendW4( r, b3SubW4( b3SplatW4( 1.57079637f ), r ), b3GreaterThanW4( ay, ax ) ); + r = b3BlendW4( r, b3SubW4( b3SplatW4( 3.14159274f ), r ), b3LessThanW4( x, zero ) ); + r = b3BlendW4( r, b3NegW4( r ), b3LessThanW4( y, zero ) ); + + return b3BlendW4( r, zero, b3EqualsW4( b3AddW4( ax, ay ), zero ) ); +} + #if !defined( B3_SIMD_SSE2 ) // I don't expect the use case of b3V32 to benefit from Neon code. diff --git a/src/triangle_manifold.c b/src/triangle_manifold.c index eb3a25e..9bf5196 100644 --- a/src/triangle_manifold.c +++ b/src/triangle_manifold.c @@ -14,12 +14,14 @@ #include #include +// cosine of hull normal versus edge axis tolerance. +#define B3_CROSSING_TOLERANCE 0.1f + typedef struct b3TriangleData { b3Vec3 v1, v2, v3; b3Vec3 e1, e2, e3; b3Plane plane; - int flags; } b3TriangleData; // Indexed by the 3-bit vertex mask @@ -104,7 +106,6 @@ void b3CollideTriangleAndSphere( b3LocalManifold* manifold, int capacity, const manifold->normal = normal; manifold->pointCount = 1; manifold->feature = closest.feature; - manifold->squaredDistance = squaredDistance; b3LocalManifoldPoint* mp = manifold->points + 0; mp->point = contactPoint; @@ -260,8 +261,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 ); @@ -597,15 +598,6 @@ static b3SeparatingAxis b3QueryTriangleAndHullEdges( const b3TriangleData* trian b3Vec3 trianglePoints[] = { triangle->v1, triangle->v2, triangle->v3 }; b3Vec3 triangleEdges[] = { triangle->e1, triangle->e2, triangle->e3 }; - // int edgeFlags[] = { b3_concaveEdge1, b3_concaveEdge1, b3_concaveEdge3 }; - -#if B3_FORCE_GHOST_COLLISIONS - int triangleFlags = 0xFF; -#else - int triangleFlags = triangle->flags; -#endif - (void)triangleFlags; - b3Vec3 triNormal = triangle->plane.normal; const b3HullHalfEdge* hullEdges = b3GetHullEdges( hull ); @@ -653,7 +645,6 @@ static b3SeparatingAxis b3QueryTriangleAndHullEdges( const b3TriangleData* trian axis = b3Normalize( axis ); float separation = b3Dot( axis, b3Sub( trianglePoints[j], hullPoint ) ); - // if ( separation > result.separation && ( edgeFlags[j] & triangleFlags ) == 0 ) if ( separation > result.separation ) { // Note: We don't exit early if we find a separating axis here since we want to @@ -748,7 +739,7 @@ static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, co b3ClipVertex* clipPoint = input + i; minSeparation = b3MinFloat( minSeparation, clipPoint->separation ); - if ( enableSpeculative == false && clipPoint->separation > 0.0f ) + if ( enableSpeculative == false && clipPoint->separation > B3_MESH_REST_OFFSET ) { continue; } @@ -764,7 +755,7 @@ static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, co finalPointCount += 1; } - float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f; + float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : B3_MESH_REST_OFFSET; if ( minSeparation > speculativeDistance ) { // This can occur with a stale SAT cache @@ -864,7 +855,7 @@ static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity b3ClipVertex* clipPoint = input + i; minSeparation = b3MinFloat( minSeparation, clipPoint->separation ); - if ( enableSpeculative == false && clipPoint->separation > 0.0f ) + if ( enableSpeculative == false && clipPoint->separation > B3_MESH_REST_OFFSET ) { continue; } @@ -881,7 +872,7 @@ static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity finalPointCount += 1; } - float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f; + float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : B3_MESH_REST_OFFSET; if ( minSeparation >= speculativeDistance ) { // This can happens if the objects move a part while re-using a cached axis @@ -954,11 +945,37 @@ static void b3CollideTriangleAndHullEdges( b3LocalManifold* manifold, int capaci manifold->feature = edgesFeatures[query.indexA]; } +// Triangle face contact wrapper. +void b3BuildTriangleAndHullFaceContact( b3LocalManifold* manifold, int capacity, const b3Vec3* triangle, const b3HullData* hull, + b3SATCache* cache, bool enableSpeculative ) +{ + manifold->pointCount = 0; + + b3TriangleData triangleData = { + .v1 = triangle[0], + .v2 = triangle[1], + .v3 = triangle[2], + .e1 = b3Sub( triangle[1], triangle[0] ), + .e2 = b3Sub( triangle[2], triangle[1] ), + .e3 = b3Sub( triangle[0], triangle[2] ), + .plane = b3MakePlaneFromPoints( triangle[0], triangle[1], triangle[2] ), + }; + + b3SeparatingAxis query = b3QueryTriangleFace( &triangleData, hull ); + b3SATCache faceCache = { 0 }; + b3CollideTriangleFace( manifold, capacity, &triangleData, hull, query, &faceCache, enableSpeculative ); + + if ( manifold->pointCount > 0 ) + { + *cache = faceCache; + } +} + b3AtomicInt b3_triangleConvexCalls; b3AtomicInt b3_triangleCacheHits; // Triangle is in the local space of the hull for efficiency. -void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, int triangleFlags, +void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, const b3HullData* hullB, b3SATCache* cache, bool enableSpeculative ) { manifold->pointCount = 0; @@ -1003,14 +1020,13 @@ void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v .e2 = triangleEdges[1], .e3 = triangleEdges[2], .plane = trianglePlane, - .flags = triangleFlags, }; const b3HullHalfEdge* edges = b3GetHullEdges( hullB ); const b3Plane* hullPlanes = b3GetHullPlanes( hullB ); const b3Vec3* hullPoints = b3GetHullPoints( hullB ); - float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f; + float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : B3_MESH_REST_OFFSET; cache->hit = 1; // Attempt to use the cache to speed up collision @@ -1082,6 +1098,9 @@ void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v return; } + // Note: it is possible for a hull face to lean into an edge direction while mainting + // a valid cache. But this has never been observed. + // Deep overlap may lead to an invalid cache // todo confirm bool isDeep = separation < -2.0f * linearSlop; @@ -1285,14 +1304,29 @@ void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v // If a face axis has positive separation there may be no points. float maxFaceSeparation = b3MaxFloat( faceQueryA.separation, faceQueryB.separation ); + // The edge axis can win if the hull face axis is significantly aligned with the edge axis. + bool crossesEdge = false; + if ( manifold->pointCount >= 1 && manifold->feature == b3_featureHullFace && edgeQuery.separation > clipSeparation ) + { + b3Vec3 edgeAxis = b3Normalize( triangleEdges[edgeQuery.indexA] ); + + // Is the angle less than ~84 degrees? + crossesEdge = b3AbsFloat( b3Dot( faceQueryB.normal, edgeAxis ) ) > B3_CROSSING_TOLERANCE; + } + if ( ( manifold->pointCount == 0 && edgeQuery.separation > maxFaceSeparation ) || - ( manifold->pointCount == 1 && edgeQuery.separation > clipSeparation + linearSlop ) ) + ( manifold->pointCount == 1 && edgeQuery.separation > clipSeparation + linearSlop ) || crossesEdge ) { B3_ASSERT( 0 <= edgeQuery.indexA && edgeQuery.indexA < 3 ); b3Vec3 trianglePoint = trianglePoints[edgeQuery.indexA]; b3Vec3 triangleEdge = triangleEdges[edgeQuery.indexA]; manifold->pointCount = 0; b3CollideTriangleAndHullEdges( manifold, capacity, trianglePoint, triangleEdge, hullB, edgeQuery, cache ); + + if ( crossesEdge && manifold->pointCount == 0 ) + { + b3CollideHullFace( manifold, capacity, &triangle, hullB, faceQueryB, cache, enableSpeculative ); + } } } @@ -1314,7 +1348,7 @@ void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v b3SimplexCache simplexCache = { 0 }; b3DistanceOutput output = b3ShapeDistance( &input, &simplexCache, NULL, 0 ); - if ( output.distance > 0.0f ) + if ( 0.0f < output.distance && output.distance <= speculativeDistance ) { B3_ASSERT( 0 < simplexCache.count && simplexCache.count <= 3 ); diff --git a/test/test_determinism.c b/test/test_determinism.c index f2bd7e6..9a3f5fd 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -19,27 +19,27 @@ // 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 282 +#define RAGDOLL_HASH 0x6E4B13AD +#define WAVE_PILE_SLEEP_STEP 315 +#define WAVE_PILE_HASH 0x15007937 #define QUERY_SPAWN_SLEEP_STEP 242 #define QUERY_SPAWN_HASH 0x489585E6 #define QUERY_SPAWN_HIT_COUNT 59 #define QUERY_SPAWN_QUERY_HASH 0x5B4429DC -#define MESH_DROP_SLEEP_STEP 218 -#define MESH_DROP_HASH 0xD378CF04 +#define MESH_DROP_SLEEP_STEP 197 +#define MESH_DROP_HASH 0x5DA091C3 #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 277 +#define RAGDOLL_HASH 0x5E042B6E +#define WAVE_PILE_SLEEP_STEP 455 +#define WAVE_PILE_HASH 0x854F313D #define QUERY_SPAWN_SLEEP_STEP 242 #define QUERY_SPAWN_HASH 0xF3AD9BA6 #define QUERY_SPAWN_HIT_COUNT 59 #define QUERY_SPAWN_QUERY_HASH 0xE3271F3D -#define MESH_DROP_SLEEP_STEP 226 -#define MESH_DROP_HASH 0xD2FF069D +#define MESH_DROP_SLEEP_STEP 197 +#define MESH_DROP_HASH 0x96D47D4F #endif // The goldens above pin exact values for the default four point manifold. A build that diff --git a/test/test_height_field.c b/test/test_height_field.c index 78ed368..55621e9 100644 --- a/test/test_height_field.c +++ b/test/test_height_field.c @@ -11,6 +11,7 @@ #include "box3d/math_functions.h" #include +#include #include static int HeightFieldCreate( void ) @@ -673,6 +674,146 @@ static int RayCastClockwiseWinding( void ) return 0; } +#define BUMPY_COUNT_X 9 +#define BUMPY_COUNT_Z 7 + +// A bumpy field with holes, so cells have every mix of interior, border and hole neighbors +static b3HeightFieldData* MakeBumpyField( bool clockwise ) +{ + float heights[BUMPY_COUNT_X * BUMPY_COUNT_Z]; + for ( int i = 0; i < BUMPY_COUNT_Z; ++i ) + { + for ( int j = 0; j < BUMPY_COUNT_X; ++j ) + { + heights[i * BUMPY_COUNT_X + j] = sinf( 0.9f * (float)i ) * cosf( 0.7f * (float)j ) + 0.1f * (float)( ( i * j ) % 3 ); + } + } + + uint8_t materials[( BUMPY_COUNT_X - 1 ) * ( BUMPY_COUNT_Z - 1 )] = { 0 }; + materials[9] = B3_HEIGHT_FIELD_HOLE; + materials[20] = B3_HEIGHT_FIELD_HOLE; + materials[47] = B3_HEIGHT_FIELD_HOLE; + + b3HeightFieldDef def = { 0 }; + def.heights = heights; + def.materialIndices = materials; + def.scale = (b3Vec3){ 1.5f, 0.8f, 0.5f }; + def.countX = BUMPY_COUNT_X; + def.countZ = BUMPY_COUNT_Z; + def.globalMinimumHeight = -2.0f; + def.globalMaximumHeight = 2.0f; + def.clockwiseWinding = clockwise; + return b3CreateHeightField( &def ); +} + +static bool IsHole( const b3HeightFieldData* hf, int triangleIndex ) +{ + return b3GetHeightFieldMaterial( hf, triangleIndex ) == B3_HEIGHT_FIELD_HOLE; +} + +static b3Vec3 TriangleNormal( b3Triangle triangle ) +{ + b3Vec3 e1 = b3Sub( triangle.vertices[1], triangle.vertices[0] ); + b3Vec3 e2 = b3Sub( triangle.vertices[2], triangle.vertices[0] ); + return b3Normalize( b3Cross( e1, e2 ) ); +} + +// Neighbors found by brute force over shared grid points must match the grid walk +static int HeightFieldNeighborNormals( void ) +{ + for ( int pass = 0; pass < 2; ++pass ) + { + b3HeightFieldData* hf = MakeBumpyField( pass == 1 ); + int triangleCount = b3GetHeightFieldTriangleCount( hf ); + int interiorCount = 0; + int boundaryCount = 0; + + for ( int i = 0; i < triangleCount; ++i ) + { + if ( IsHole( hf, i ) ) + { + continue; + } + + b3Triangle triangle = b3GetHeightFieldTriangle( hf, i ); + int indices[3] = { triangle.i1, triangle.i2, triangle.i3 }; + + for ( int k = 0; k < 3; ++k ) + { + int a = indices[k]; + int b = indices[( k + 1 ) % 3]; + + int neighbor = B3_NULL_INDEX; + for ( int j = 0; j < triangleCount && neighbor == B3_NULL_INDEX; ++j ) + { + if ( j == i || IsHole( hf, j ) ) + { + continue; + } + + b3Triangle other = b3GetHeightFieldTriangle( hf, j ); + int otherIndices[3] = { other.i1, other.i2, other.i3 }; + for ( int m = 0; m < 3; ++m ) + { + if ( otherIndices[m] == b && otherIndices[( m + 1 ) % 3] == a ) + { + neighbor = j; + } + } + } + + b3Vec3 normal; + bool found = b3GetHeightFieldNeighborNormal( hf, i, k, &normal ); + ENSURE( found == ( neighbor != B3_NULL_INDEX ) ); + + if ( found == false ) + { + boundaryCount += 1; + continue; + } + + interiorCount += 1; + b3Vec3 expected = TriangleNormal( b3GetHeightFieldTriangle( hf, neighbor ) ); + ENSURE( b3Dot( normal, expected ) > 0.99999f ); + } + } + + ENSURE( interiorCount > 0 ); + ENSURE( boundaryCount > 0 ); + b3DestroyHeightField( hf ); + } + + return 0; +} + +// The shape level lookup dispatches to the mesh and height field versions +static int NeighborNormalDispatch( void ) +{ + b3HeightFieldData* hf = MakeBumpyField( false ); + b3Shape heightShape = { 0 }; + heightShape.type = b3_heightShape; + heightShape.heightField = hf; + + b3Vec3 normal1, normal2; + ENSURE( b3GetNeighborNormal( &heightShape, 1, 1, &normal1 ) ); + ENSURE( b3GetHeightFieldNeighborNormal( hf, 1, 1, &normal2 ) ); + ENSURE( normal1.x == normal2.x && normal1.y == normal2.y && normal1.z == normal2.z ); + b3DestroyHeightField( hf ); + + b3MeshData* data = b3CreateWaveMesh( 4, 4, 1.0f, 0.5f, 0.2f, 0.15f ); + b3Shape meshShape = { 0 }; + meshShape.type = b3_meshShape; + meshShape.mesh = (b3Mesh){ data, { 2.0f, 0.5f, -1.0f } }; + + bool found1 = b3GetNeighborNormal( &meshShape, 5, 1, &normal1 ); + bool found2 = b3GetMeshNeighborNormal( &meshShape.mesh, 5, 1, &normal2 ); + ENSURE( found1 && found2 ); + ENSURE( normal1.x == normal2.x && normal1.y == normal2.y && normal1.z == normal2.z ); + b3DestroyMesh( data ); + + return 0; +} + int HeightFieldTest( void ) { RUN_SUBTEST( HeightFieldCreate ); @@ -688,6 +829,8 @@ int HeightFieldTest( void ) RUN_SUBTEST( ShapeCastClockwiseWinding ); RUN_SUBTEST( RayCastBackside ); RUN_SUBTEST( RayCastClockwiseWinding ); + RUN_SUBTEST( HeightFieldNeighborNormals ); + RUN_SUBTEST( NeighborNormalDispatch ); return 0; } diff --git a/test/test_hull.c b/test/test_hull.c index d2a27be..7696f8c 100644 --- a/test/test_hull.c +++ b/test/test_hull.c @@ -580,7 +580,7 @@ static int Hull2DSquareTest( void ) b3Point2D hull[16]; FillPoints2D( pts, input, 8 ); - int count = b3Hull2D( pts, 8, hull ); + int count = b3Hull2D( pts, 8, hull, 0 ); ENSURE( count == 4 ); // The reduction code assumes counter clockwise winding @@ -616,7 +616,7 @@ static int Hull2DCollinearTest( void ) b3Point2D hull[10]; FillPoints2D( pts, input, 5 ); - ENSURE( b3Hull2D( pts, 5, hull ) == 2 ); + ENSURE( b3Hull2D( pts, 5, hull, 0 ) == 2 ); return 0; } @@ -635,7 +635,7 @@ static int Hull2DWeldTest( void ) b3Point2D hull[10]; FillPoints2D( pts, input, 5 ); - int count = b3Hull2D( pts, 5, hull ); + int count = b3Hull2D( pts, 5, hull, 0 ); ENSURE( count == 4 ); for ( int i = 0; i < count; ++i ) @@ -659,7 +659,7 @@ static int SimplifyHull2DTargetTest( void ) pts[i].originalIndex = i; } - int hullCount = b3Hull2D( pts, 20, hull ); + int hullCount = b3Hull2D( pts, 20, hull, 0 ); ENSURE( hullCount == 20 ); int count = b3SimplifyHull2D( hull, hullCount, 6 ); @@ -682,7 +682,7 @@ static int SimplifyHull2DNoopTest( void ) b3Point2D hull[8]; FillPoints2D( pts, input, 4 ); - int hullCount = b3Hull2D( pts, 4, hull ); + int hullCount = b3Hull2D( pts, 4, hull, 0 ); ENSURE( hullCount == 4 ); ENSURE( b3SimplifyHull2D( hull, hullCount, 8 ) == 4 ); @@ -706,7 +706,7 @@ static int Hull2DOrderTest( void ) b3Point2D hull[16]; FillPoints2D( pts, input, 8 ); - int count1 = b3Hull2D( pts, 8, hull ); + int count1 = b3Hull2D( pts, 8, hull, 0 ); ENSURE( count1 == 7 ); bool onHull1[8] = { false }; @@ -724,7 +724,7 @@ static int Hull2DOrderTest( void ) shuffled[i].originalIndex = i; } - int count2 = b3Hull2D( shuffled, 8, shuffledHull ); + int count2 = b3Hull2D( shuffled, 8, shuffledHull, 0 ); ENSURE( count2 == count1 ); bool onHull2[8] = { false }; @@ -741,6 +741,143 @@ static int Hull2DOrderTest( void ) return 0; } +// Float rounding can call the middle of three nearly collinear points a left turn in both chains. +// Listing it twice gave the mesh manifold a duplicate point. +static int Hull2DNearCollinearTest( void ) +{ + b3Vec2 input[3] = { + { 0.42742428183555603f, -0.9914372563362122f }, + { 0.8582707643508911f, -0.7915351390838623f }, + { 1.5856003761291504f, -0.45407217741012573f }, + }; + + b3Point2D pts[3]; + b3Point2D hull[6]; + FillPoints2D( pts, input, 3 ); + + int count = b3Hull2D( pts, 3, hull, 0 ); + ENSURE( 2 <= count && count <= 3 ); + + for ( int i = 0; i < count; ++i ) + { + for ( int j = i + 1; j < count; ++j ) + { + ENSURE( hull[i].originalIndex != hull[j].originalIndex ); + } + } + + return 0; +} + +// A 3 by 3 grid has three points on each side. Only the corners are strict hull vertices. +static int Hull2DKeepCollinearTest( void ) +{ + b3Vec2 input[9]; + for ( int i = 0; i < 9; ++i ) + { + input[i] = (b3Vec2){ (float)( i % 3 ), (float)( i / 3 ) }; + } + + b3Point2D pts[9]; + b3Point2D hull[18]; + FillPoints2D( pts, input, 9 ); + ENSURE( b3Hull2D( pts, 9, hull, 0 ) == 4 ); + + FillPoints2D( pts, input, 9 ); + int count = b3Hull2D( pts, 9, hull, b3_hull2DKeepCollinear ); + ENSURE( count == 8 ); + ENSURE( Hull2DArea( hull, count ) > 0.0f ); + + for ( int i = 0; i < count; ++i ) + { + ENSURE( hull[i].originalIndex != 4 ); + } + + // All collinear keeps every point + b3Vec2 line[4] = { { 0.0f, 0.0f }, { 1.0f, 0.0f }, { 2.0f, 0.0f }, { 3.0f, 0.0f } }; + FillPoints2D( pts, line, 4 ); + ENSURE( b3Hull2D( pts, 4, hull, b3_hull2DKeepCollinear ) == 4 ); + + return 0; +} + +// Coincident points under the exact merge: the earlier point survives unless a later one is deeper by +// more than the linear slop. The default weld always keeps the earlier point. +static int Hull2DExactMergeTest( void ) +{ + b3Vec2 input[5] = { + { -1.0f, -1.0f }, { 1.0f, -1.0f }, { 1.0f, 1.0f }, { -1.0f, 1.0f }, { -1.0f, -1.0f }, + }; + + float depths[2] = { 2.0f * B3_LINEAR_SLOP, 0.5f * B3_LINEAR_SLOP }; + int expected[2] = { 4, 0 }; + + for ( int k = 0; k < 2; ++k ) + { + b3Point2D pts[5]; + b3Point2D hull[10]; + FillPoints2D( pts, input, 5 ); + pts[4].separation = -depths[k]; + + int count = b3Hull2D( pts, 5, hull, b3_hull2DExactMerge ); + ENSURE( count == 4 ); + + int survivor = -1; + for ( int i = 0; i < count; ++i ) + { + if ( hull[i].originalIndex == 0 || hull[i].originalIndex == 4 ) + { + survivor = hull[i].originalIndex; + } + } + ENSURE( survivor == expected[k] ); + + FillPoints2D( pts, input, 5 ); + pts[4].separation = -depths[k]; + count = b3Hull2D( pts, 5, hull, 0 ); + ENSURE( count == 4 ); + + for ( int i = 0; i < count; ++i ) + { + ENSURE( hull[i].originalIndex != 4 ); + } + } + + return 0; +} + +// The index order flag returns the same points sorted by input index +static int Hull2DIndexOrderTest( void ) +{ + b3Vec2 input[8] = { + { 1.0f, 2.0f }, { -2.5f, 0.4f }, { 2.0f, -1.0f }, { 0.2f, 0.1f }, + { -2.0f, -1.0f }, { 2.5f, 0.5f }, { 0.0f, -1.5f }, { -1.0f, 1.8f }, + }; + + b3Point2D pts[8]; + b3Point2D hull[16]; + FillPoints2D( pts, input, 8 ); + int count1 = b3Hull2D( pts, 8, hull, 0 ); + + bool onHull[8] = { false }; + for ( int i = 0; i < count1; ++i ) + { + onHull[hull[i].originalIndex] = true; + } + + FillPoints2D( pts, input, 8 ); + int count2 = b3Hull2D( pts, 8, hull, b3_hull2DIndexOrder ); + ENSURE( count2 == count1 ); + + for ( int i = 0; i < count2; ++i ) + { + ENSURE( onHull[hull[i].originalIndex] ); + ENSURE( i == 0 || hull[i - 1].originalIndex < hull[i].originalIndex ); + } + + return 0; +} + int HullTest( void ) { RUN_SUBTEST( CreateHullCubeTest ); @@ -759,6 +896,10 @@ int HullTest( void ) RUN_SUBTEST( Hull2DCollinearTest ); RUN_SUBTEST( Hull2DWeldTest ); RUN_SUBTEST( Hull2DOrderTest ); + RUN_SUBTEST( Hull2DNearCollinearTest ); + RUN_SUBTEST( Hull2DKeepCollinearTest ); + RUN_SUBTEST( Hull2DExactMergeTest ); + RUN_SUBTEST( Hull2DIndexOrderTest ); RUN_SUBTEST( SimplifyHull2DTargetTest ); RUN_SUBTEST( SimplifyHull2DNoopTest ); diff --git a/test/test_manifold.c b/test/test_manifold.c index a304ef1..819440e 100644 --- a/test/test_manifold.c +++ b/test/test_manifold.c @@ -430,7 +430,7 @@ static int TriangleEdgeTest( void ) b3LocalManifold manifold = { 0 }; manifold.points = points; b3SATCache cache = { .type = b3_manualEdgePairAxis }; - b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, 0, &hull.base, &cache, true ); + b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, &hull.base, &cache, true ); b3Vec3 expectedNormal = b3Neg( axis ); b3Vec3 expectedPoint = b3MulAdd( hullPoint, 0.5f * gap, axis ); @@ -463,7 +463,7 @@ static int TriangleEdgeTest( void ) b3LocalManifold manifold = { 0 }; manifold.points = points; b3SATCache cache = { 0 }; - b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, 0, &hull.base, &cache, true ); + b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, &hull.base, &cache, true ); ENSURE( manifold.pointCount == 0 ); ENSURE( cache.type == b3_edgePairAxis ); @@ -496,7 +496,7 @@ static int TriangleParallelEdgeTest( void ) b3LocalManifold manifold = { 0 }; manifold.points = points; b3SATCache cache = { 0 }; - b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, 0, &hull.base, &cache, true ); + b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, &hull.base, &cache, true ); ENSURE_FACE_POINTS( manifold.pointCount ); ENSURE( cache.type == b3_faceAxisA ); @@ -890,7 +890,7 @@ static int TriangleHullEdgeSweepTest( void ) b3LocalManifold manifold = { 0 }; manifold.points = points; b3SATCache cache = { .type = b3_manualEdgePairAxis }; - b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, 0, &hull.base, &cache, true ); + b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, &hull.base, &cache, true ); if ( cache.type != b3_edgePairAxis || manifold.pointCount != 1 ) { @@ -1340,7 +1340,7 @@ static int SpeculativeMarginTest( void ) manifold.pointCount = 0; b3BoxHull raised = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, SlideY( 0.5f + gap ) ); b3SATCache cache = { 0 }; - b3CollideTriangleAndHull( &manifold, B3_MAX_MANIFOLD_POINTS, triangle[0], triangle[1], triangle[2], 0, &raised.base, + b3CollideTriangleAndHull( &manifold, B3_MAX_MANIFOLD_POINTS, triangle[0], triangle[1], triangle[2], &raised.base, &cache, true ); ENSURE( CheckSpeculativePoints( &manifold, gap ) == 0 ); } diff --git a/test/test_mesh.c b/test/test_mesh.c index 69330ed..6c436cb 100644 --- a/test/test_mesh.c +++ b/test/test_mesh.c @@ -1,12 +1,19 @@ // SPDX-FileCopyrightText: 2026 Erin Catto // SPDX-License-Identifier: MIT +#include "ghost_collisions.h" +#include "ghost_oracle.h" +#include "shape.h" #include "test_macros.h" +#include "box3d/box3d.h" #include "box3d/collision.h" #include "box3d/math_functions.h" #include +#include +#include +#include #include // Two quads meeting at a concave crease along the shared edge 1-4. The crease exercises @@ -341,6 +348,1006 @@ static int MeshCreators( void ) return 0; } +// Brute force search for the triangle across edge a-b, walked b to a with consistent winding +static int FindNeighbor( const b3Mesh* mesh, int triangleIndex, int a, int b, int* edgeIndex ) +{ + for ( int i = 0; i < mesh->data->triangleCount; ++i ) + { + if ( i == triangleIndex ) + { + continue; + } + + b3Triangle triangle = b3GetMeshTriangle( mesh, i ); + int indices[3] = { triangle.i1, triangle.i2, triangle.i3 }; + for ( int j = 0; j < 3; ++j ) + { + if ( indices[j] == b && indices[( j + 1 ) % 3] == a ) + { + *edgeIndex = j; + return i; + } + } + } + + return B3_NULL_INDEX; +} + +// Both sides of an edge store the same angle +static bool EdgeAnglesAreSymmetric( const b3MeshData* data, int* boundaryCount ) +{ + b3Mesh mesh = { .data = data, .scale = b3Vec3_one }; + const int8_t* edgeAngles = b3GetMeshEdgeAngles( data ); + *boundaryCount = 0; + + for ( int i = 0; i < data->triangleCount; ++i ) + { + b3Triangle triangle = b3GetMeshTriangle( &mesh, i ); + int indices[3] = { triangle.i1, triangle.i2, triangle.i3 }; + + for ( int k = 0; k < 3; ++k ) + { + int8_t angle = edgeAngles[3 * i + k]; + if ( angle == B3_BOUNDARY_EDGE_ANGLE ) + { + *boundaryCount += 1; + continue; + } + + int edgeIndex = -1; + int neighbor = FindNeighbor( &mesh, i, indices[k], indices[( k + 1 ) % 3], &edgeIndex ); + if ( neighbor == B3_NULL_INDEX || edgeAngles[3 * neighbor + edgeIndex] != angle ) + { + return false; + } + } + } + + return true; +} + +// The rebuilt neighbor normal must match the real one under any scale. A non-uniform scale +// stretches the quantization error by up to the ratio of its largest and smallest components. +static bool NeighborNormalsMatch( const b3MeshData* data, b3Vec3 scale ) +{ + b3Mesh mesh = { .data = data, .scale = scale }; + + b3Vec3 s = b3Abs( scale ); + float spread = b3MaxFloat( s.x, b3MaxFloat( s.y, s.z ) ) / b3MinFloat( s.x, b3MinFloat( s.y, s.z ) ); + float tolerance = 0.8f * spread * B3_PI / 180.0f; + float cosTolerance = cosf( tolerance ); + + for ( int i = 0; i < data->triangleCount; ++i ) + { + b3Triangle triangle = b3GetMeshTriangle( &mesh, i ); + int indices[3] = { triangle.i1, triangle.i2, triangle.i3 }; + + for ( int k = 0; k < 3; ++k ) + { + int edgeIndex = -1; + int neighbor = FindNeighbor( &mesh, i, indices[k], indices[( k + 1 ) % 3], &edgeIndex ); + + b3Vec3 rebuilt; + bool found = b3GetMeshNeighborNormal( &mesh, i, k, &rebuilt ); + if ( found != ( neighbor != B3_NULL_INDEX ) ) + { + return false; + } + + if ( found == false ) + { + continue; + } + + b3Triangle other = b3GetMeshTriangle( &mesh, neighbor ); + b3Vec3 e1 = b3Sub( other.vertices[1], other.vertices[0] ); + b3Vec3 e2 = b3Sub( other.vertices[2], other.vertices[0] ); + b3Vec3 actual = b3Normalize( b3Cross( e1, e2 ) ); + if ( b3Dot( rebuilt, actual ) < cosTolerance ) + { + return false; + } + } + } + + return true; +} + +// Positive angles are convex, negative are concave. A right angle sits on a rounding boundary. +static bool HasRightAngle( const b3MeshData* data, int sign ) +{ + const int8_t* edgeAngles = b3GetMeshEdgeAngles( data ); + for ( int i = 0; i < 3 * data->triangleCount; ++i ) + { + int angle = sign * edgeAngles[i]; + if ( angle == 63 || angle == 64 ) + { + return true; + } + } + + return false; +} + +static int MeshEdgeAngles( void ) +{ + { + b3MeshData* grid = b3CreateGridMesh( 4, 4, 1.0f, 0, true ); + ENSURE( b3GetMeshEdgeAngles( grid ) != NULL ); + + int boundaryCount = 0; + ENSURE( EdgeAnglesAreSymmetric( grid, &boundaryCount ) ); + ENSURE( boundaryCount == 16 ); + + const int8_t* edgeAngles = b3GetMeshEdgeAngles( grid ); + for ( int i = 0; i < 3 * grid->triangleCount; ++i ) + { + ENSURE( edgeAngles[i] == 0 || edgeAngles[i] == B3_BOUNDARY_EDGE_ANGLE ); + } + + b3DestroyMesh( grid ); + } + + { + b3MeshData* grid = b3CreateGridMesh( 4, 4, 1.0f, 0, false ); + ENSURE( b3GetMeshEdgeAngles( grid ) == NULL ); + b3DestroyMesh( grid ); + } + + b3Vec3 vertices[VALLEY_VERTEX_COUNT]; + int32_t indices[3 * VALLEY_TRIANGLE_COUNT]; + MakeValley( vertices, indices ); + b3MeshDef def = MakeValleyDef( vertices, indices ); + + b3MeshData* meshes[] = { + b3CreateMesh( &def, NULL, 0 ), + b3CreateBoxMesh( b3Vec3_zero, (b3Vec3){ 1.0f, 2.0f, 3.0f }, true ), + b3CreateWaveMesh( 8, 8, 1.0f, 0.5f, 0.2f, 0.15f ), + b3CreateTorusMesh( 8, 6, 2.0f, 0.5f ), + }; + + b3MeshData* valley = meshes[0]; + b3MeshData* box = meshes[1]; + ENSURE( HasRightAngle( valley, -1 ) ); + ENSURE( HasRightAngle( box, 1 ) ); + + int boundaryCount = 0; + ENSURE( EdgeAnglesAreSymmetric( box, &boundaryCount ) ); + ENSURE( boundaryCount == 0 ); + + b3Vec3 scales[] = { + { 1.0f, 1.0f, 1.0f }, { 3.0f, 3.0f, 3.0f }, { -1.0f, 1.0f, 1.0f }, + { 2.0f, 0.5f, 1.5f }, { 0.5f, -2.0f, 1.0f }, { -1.0f, -1.0f, -1.0f }, + }; + + for ( int i = 0; i < ARRAY_COUNT( meshes ); ++i ) + { + ENSURE( meshes[i] != NULL ); + ENSURE( EdgeAnglesAreSymmetric( meshes[i], &boundaryCount ) ); + + for ( int j = 0; j < ARRAY_COUNT( scales ); ++j ) + { + ENSURE( NeighborNormalsMatch( meshes[i], scales[j] ) ); + } + + b3DestroyMesh( meshes[i] ); + } + + { + // Three triangles on edge 0-1 are non-manifold. The last two share edge 5-6 walked the + // same way, which is inconsistent winding. Neither gets an angle. + b3Vec3 finVertices[] = { + { 0.0f, 0.0f, 0.0f }, { 1.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 1.0f }, { 0.0f, 0.0f, -1.0f }, + { 0.0f, 1.0f, 0.0f }, { 5.0f, 0.0f, 0.0f }, { 5.0f, 0.0f, 1.0f }, { 5.0f, 1.0f, 0.0f }, + }; + + int32_t finIndices[] = { + 0, 1, 2, 1, 0, 3, 0, 1, 4, 5, 6, 7, 5, 6, 2, + }; + + b3MeshDef finDef = { 0 }; + finDef.vertices = finVertices; + finDef.vertexCount = ARRAY_COUNT( finVertices ); + finDef.indices = finIndices; + finDef.triangleCount = ARRAY_COUNT( finIndices ) / 3; + finDef.identifyEdges = true; + b3MeshData* fin = b3CreateMesh( &finDef, NULL, 0 ); + ENSURE( fin != NULL ); + + ENSURE( EdgeAnglesAreSymmetric( fin, &boundaryCount ) ); + ENSURE( boundaryCount == 15 ); + b3DestroyMesh( fin ); + } + + return 0; +} + +// A wide box sliding over a fine grid sits on hundreds of flat internal edges. With no friction +// any manifold off the face normal is a ghost that slows or lifts the box. +static int WideHullSlide( b3Vec3 meshScale ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3MeshData* mesh = b3CreateGridMesh( 120, 40, 0.25f, 0, true ); + ENSURE( mesh != NULL ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateMeshShape( groundId, &shapeDef, mesh, meshScale ); + + float speed = 4.0f; + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Pos){ -10.0f, 0.5f, 0.0f }; + bodyDef.linearVelocity = (b3Vec3){ speed, 0.0f, 0.0f }; + b3BodyId boxId = b3CreateBody( worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 1.25f, 0.5f, 1.25f ); + shapeDef.baseMaterial.friction = 0.0f; + b3CreateHullShape( boxId, &shapeDef, &box.base ); + + int ghostCount = 0; + for ( int step = 0; step < 120; ++step ) + { + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + + b3ContactData contacts[4]; + int contactCount = b3Body_GetContactData( boxId, contacts, ARRAY_COUNT( contacts ) ); + for ( int i = 0; i < contactCount; ++i ) + { + for ( int j = 0; j < contacts[i].manifoldCount; ++j ) + { + if ( b3AbsFloat( contacts[i].manifolds[j].normal.y ) < 0.99f ) + { + ghostCount += 1; + } + } + } + } + + b3Vec3 velocity = b3Body_GetLinearVelocity( boxId ); + + ENSURE( ghostCount == 0 ); + ENSURE( velocity.x > 0.99f * speed ); + ENSURE( b3AbsFloat( velocity.y ) < 0.01f ); + + b3DestroyWorld( worldId ); + b3DestroyMesh( mesh ); + return 0; +} + +// The box overlaps more than a thousand grid triangles. All of them must reach the narrow +// phase, so the reduced manifold reaches all four sides of the footprint. +static int MeshManyTriangles( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3MeshData* mesh = b3CreateGridMesh( 40, 40, 0.25f, 0, true ); + ENSURE( mesh != NULL ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateMeshShape( groundId, &shapeDef, mesh, b3Vec3_one ); + + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Pos){ 0.0f, 0.5f, 0.0f }; + b3BodyId boxId = b3CreateBody( worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 3.0f, 0.5f, 3.0f ); + b3ShapeId boxShapeId = b3CreateHullShape( boxId, &shapeDef, &box.base ); + + for ( int step = 0; step < 30; ++step ) + { + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + } + + b3Pos position = b3Body_GetPosition( boxId ); + b3Quat rotation = b3Body_GetRotation( boxId ); + + ENSURE( b3AbsFloat( (float)position.y - 0.5f ) < 0.02f ); + ENSURE( b3AbsFloat( rotation.v.x ) < 0.001f ); + ENSURE( b3AbsFloat( rotation.v.z ) < 0.001f ); + + b3Vec3 lower = { FLT_MAX, FLT_MAX, FLT_MAX }; + b3Vec3 upper = { -FLT_MAX, -FLT_MAX, -FLT_MAX }; + + b3ContactData contacts[4]; + int contactCount = b3Body_GetContactData( boxId, contacts, ARRAY_COUNT( contacts ) ); + for ( int i = 0; i < contactCount; ++i ) + { + bool boxIsA = B3_ID_EQUALS( contacts[i].shapeIdA, boxShapeId ); + for ( int j = 0; j < contacts[i].manifoldCount; ++j ) + { + const b3Manifold* manifold = contacts[i].manifolds + j; + for ( int k = 0; k < manifold->pointCount; ++k ) + { + b3Vec3 anchor = boxIsA ? manifold->points[k].anchorA : manifold->points[k].anchorB; + lower = b3Min( lower, anchor ); + upper = b3Max( upper, anchor ); + } + } + } + + ENSURE( lower.x < -2.5f && upper.x > 2.5f ); + ENSURE( lower.z < -2.5f && upper.z > 2.5f ); + + b3DestroyWorld( worldId ); + b3DestroyMesh( mesh ); + return 0; +} + +// The grid is symmetric in x, so the mirror only changes the winding +static int MeshWideHullSlide( void ) +{ + ENSURE( WideHullSlide( b3Vec3_one ) == 0 ); + ENSURE( WideHullSlide( (b3Vec3){ -1.0f, 1.0f, 1.0f } ) == 0 ); + return 0; +} + +static bool HasVertex( const b3MeshData* mesh, int triangleIndex, b3Vec3 point ) +{ + const b3MeshTriangle* triangle = b3GetMeshTriangles( mesh ) + triangleIndex; + const b3Vec3* vertices = b3GetMeshVertices( mesh ); + int indices[3] = { triangle->index1, triangle->index2, triangle->index3 }; + for ( int i = 0; i < 3; ++i ) + { + if ( b3DistanceSquared( vertices[indices[i]], point ) < 1.0e-8f ) + { + return true; + } + } + + return false; +} + +// A box face rests on a ridge that runs into a rising wall, so the ridge end is a saddle vertex. The +// ridge sags slightly away from the saddle, leaving the saddle the deepest point by less than linear +// slop. The ridge edge admits the up normal and the saddle cone does not, so the deepest points have +// to be taken as the edge they span, not as the single deepest vertex, or the contact snaps to a slope. +static int MeshHullOnSaddleRidge( void ) +{ + b3Vec3 ridgeStart = { -2.0f, -0.004f, 0.0f }; + b3Vec3 saddle = { 0.0f, 0.0f, 0.0f }; + + b3Vec3 vertices[] = { + { -2.0f, -0.7f, -1.0f }, + ridgeStart, + { -2.0f, -0.7f, 1.0f }, + { 0.0f, -0.7f, -1.0f }, + saddle, + { 0.0f, -0.7f, 1.0f }, + { 1.0f, -0.4f, -1.0f }, + { 1.0f, 0.3f, 0.0f }, + { 1.0f, -0.4f, 1.0f }, + }; + + int32_t indices[] = { + 0, 1, 3, 3, 1, 4, 1, 2, 4, 4, 2, 5, 3, 4, 6, 6, 4, 7, 4, 5, 7, 7, 5, 8, + }; + + b3MeshDef def = { 0 }; + def.vertices = vertices; + def.vertexCount = ARRAY_COUNT( vertices ); + def.indices = indices; + def.triangleCount = ARRAY_COUNT( indices ) / 3; + def.identifyEdges = true; + b3MeshData* meshData = b3CreateMesh( &def, NULL, 0 ); + ENSURE( meshData != NULL ); + + // The footprint covers the saddle and a stretch of the ridge, slightly overlapping + float hy = 0.1f; + b3BoxHull box = b3MakeBoxHull( 0.46f, hy, 0.5f ); + b3Transform transform = { { -0.44f, hy - 0.004f, 0.0f }, b3Quat_identity }; + + b3Mesh mesh = { .data = meshData, .scale = b3Vec3_one }; + b3Manifold manifolds[16]; + int manifoldCount = + b3CollideMeshAndHull( manifolds, ARRAY_COUNT( manifolds ), &mesh, &box.base, transform, true, true ); + + int ridgePointCount = 0; + for ( int i = 0; i < manifoldCount; ++i ) + { + const b3Manifold* manifold = manifolds + i; + for ( int j = 0; j < manifold->pointCount; ++j ) + { + int triangleIndex = manifold->points[j].triangleIndex; + if ( HasVertex( meshData, triangleIndex, ridgeStart ) && HasVertex( meshData, triangleIndex, saddle ) ) + { + ridgePointCount += 1; + ENSURE( manifold->normal.y > 0.999f ); + } + } + } + + ENSURE( ridgePointCount > 0 ); + + b3DestroyMesh( meshData ); + 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; +} + +// Runs a Mesh / Ghost Collisions scene with the cylinder and returns the ghost count +static int CountCylinderGhosts( GhostScene scene, int peelPreset, bool mirror, int* runCount ) +{ + GhostSettings settings = GetDefaultGhostSettings(); + settings.scene = scene; + settings.peelPreset = peelPreset; + settings.body = e_ghostCylinder; + settings.mirror = mirror; + + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + GhostCollisionsData data = CreateGhostCollisions( worldId, settings ); + + for ( int step = 0; step < 900; ++step ) + { + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + UpdateGhostCollisions( worldId, &data, NULL, 0 ); + } + + *runCount = data.runCount; + int ghostCount = data.ghostCount; + + b3DestroyWorld( worldId ); + DestroyGhostCollisions( &data ); + return ghostCount; +} + +// The PEEL cylinder slides along its axis on a polygon edge that runs along an internal edge of the +// strip. Deep edge contacts on the far side fail the neighbor test and their face snap is empty, so +// the triangles under the cylinder own the contact. +static int MeshPeelCylinder( void ) +{ + int runCount; + ENSURE( CountCylinderGhosts( e_ghostInternal, 2, false, &runCount ) == 0 ); + ENSURE( runCount >= 1 ); + return 0; +} + +// Exact vertex cone from every triangle at the vertex. Returns the largest lean of the normal past +// an edge at the vertex or past the cliff below a boundary edge. +static float ExactVertexLean( const b3Mesh* mesh, int vertex, b3Vec3 normal ) +{ + float lean = -FLT_MAX; + for ( int i = 0; i < mesh->data->triangleCount; ++i ) + { + b3Triangle triangle = b3GetMeshTriangle( mesh, i ); + int indices[3] = { triangle.i1, triangle.i2, triangle.i3 }; + int k = indices[0] == vertex ? 0 : ( indices[1] == vertex ? 1 : ( indices[2] == vertex ? 2 : -1 ) ); + if ( k < 0 ) + { + continue; + } + + int next = ( k + 1 ) % 3; + int previous = ( k + 2 ) % 3; + b3Vec3 edges[2] = { b3Sub( triangle.vertices[next], triangle.vertices[k] ), + b3Sub( triangle.vertices[previous], triangle.vertices[k] ) }; + int edgeIndices[2] = { k, previous }; + b3Vec3 faceNormal = b3MakeNormalFromPoints( triangle.vertices[0], triangle.vertices[1], triangle.vertices[2] ); + + for ( int j = 0; j < 2; ++j ) + { + lean = b3MaxFloat( lean, b3Dot( normal, b3Normalize( edges[j] ) ) ); + + b3Vec3 neighborNormal; + if ( b3GetMeshNeighborNormal( mesh, i, edgeIndices[j], &neighborNormal ) == false ) + { + lean = b3MaxFloat( lean, -b3Dot( normal, faceNormal ) ); + } + } + } + + return lean; +} + +static uint32_t NextRandom( uint32_t* state ) +{ + *state = *state * 1664525u + 1013904223u; + return *state; +} + +static float RandomFloat( uint32_t* state ) +{ + return (float)( NextRandom( state ) >> 8 ) / (float)( 1 << 24 ) * 2.0f - 1.0f; +} + +// The quantized ring must admit normals well inside the exact cone and reject normals well outside. +// The band between is quantization noise. A non-uniform scale stretches it. +static bool VertexRingMatches( const b3MeshData* data, b3Vec3 scale, int* insideCount, int* outsideCount ) +{ + b3Mesh mesh = { .data = data, .scale = scale }; + + b3Vec3 s = b3Abs( scale ); + float spread = b3MaxFloat( s.x, b3MaxFloat( s.y, s.z ) ) / b3MinFloat( s.x, b3MinFloat( s.y, s.z ) ); + float tolerance = 0.0175f; + float margin = 0.025f * spread; + uint32_t state = 12345; + + for ( int i = 0; i < data->triangleCount; ++i ) + { + b3Triangle triangle = b3GetMeshTriangle( &mesh, i ); + int indices[3] = { triangle.i1, triangle.i2, triangle.i3 }; + b3Vec3 faceNormal = b3MakeNormalFromPoints( triangle.vertices[0], triangle.vertices[1], triangle.vertices[2] ); + + for ( int k = 0; k < 3; ++k ) + { + b3Vec3 samples[24]; + int sampleCount = 0; + samples[sampleCount++] = faceNormal; + + // Fan normals and their blends land near the cone + for ( int j = 0; j < data->triangleCount && sampleCount < 12; ++j ) + { + b3Triangle other = b3GetMeshTriangle( &mesh, j ); + if ( j == i || ( other.i1 != indices[k] && other.i2 != indices[k] && other.i3 != indices[k] ) ) + { + continue; + } + + b3Vec3 otherNormal = b3MakeNormalFromPoints( other.vertices[0], other.vertices[1], other.vertices[2] ); + samples[sampleCount++] = otherNormal; + samples[sampleCount++] = b3Normalize( b3Add( faceNormal, otherNormal ) ); + } + + while ( sampleCount < 24 ) + { + b3Vec3 tilt = { RandomFloat( &state ), RandomFloat( &state ), RandomFloat( &state ) }; + float amount = sampleCount % 2 == 0 ? 0.1f : 1.0f; + samples[sampleCount++] = b3Normalize( b3MulAdd( faceNormal, amount, tilt ) ); + } + + for ( int j = 0; j < sampleCount; ++j ) + { + b3Vec3 normal = samples[j]; + float lean = ExactVertexLean( &mesh, indices[k], normal ); + bool admit = b3IsInMeshVertexRing( &mesh, i, k, normal, tolerance ); + + if ( lean <= tolerance - margin ) + { + *insideCount += 1; + if ( admit == false ) + { + return false; + } + } + else if ( lean > tolerance + margin ) + { + *outsideCount += 1; + if ( admit ) + { + return false; + } + } + } + } + } + + return true; +} + +static int MeshVertexRing( void ) +{ + b3Vec3 vertices[VALLEY_VERTEX_COUNT]; + int32_t indices[3 * VALLEY_TRIANGLE_COUNT]; + MakeValley( vertices, indices ); + b3MeshDef def = MakeValleyDef( vertices, indices ); + + b3MeshData* meshes[] = { + b3CreateMesh( &def, NULL, 0 ), + b3CreateBoxMesh( b3Vec3_zero, (b3Vec3){ 1.0f, 2.0f, 3.0f }, true ), + b3CreateWaveMesh( 8, 8, 1.0f, 0.5f, 0.2f, 0.15f ), + b3CreateTorusMesh( 8, 6, 2.0f, 0.5f ), + b3CreateGridMesh( 4, 4, 1.0f, 0, true ), + }; + + b3Vec3 scales[] = { + { 1.0f, 1.0f, 1.0f }, { 3.0f, 3.0f, 3.0f }, { -1.0f, 1.0f, 1.0f }, + { 2.0f, 0.5f, 1.5f }, { 0.5f, -2.0f, 1.0f }, { -1.0f, -1.0f, -1.0f }, + }; + + for ( int i = 0; i < ARRAY_COUNT( meshes ); ++i ) + { + ENSURE( meshes[i] != NULL ); + + for ( int j = 0; j < ARRAY_COUNT( scales ); ++j ) + { + int insideCount = 0; + int outsideCount = 0; + ENSURE( VertexRingMatches( meshes[i], scales[j], &insideCount, &outsideCount ) ); + ENSURE( insideCount > 0 && outsideCount > 0 ); + } + + b3DestroyMesh( meshes[i] ); + } + + // Without edge data every normal passes + b3MeshData* bare = b3CreateGridMesh( 4, 4, 1.0f, 0, false ); + b3Mesh mesh = { .data = bare, .scale = b3Vec3_one }; + ENSURE( b3IsInMeshVertexRing( &mesh, 0, 0, (b3Vec3){ 1.0f, 0.0f, 0.0f }, 0.0175f ) ); + b3DestroyMesh( bare ); + + return 0; +} + +/// Separated poses have an exact reference, the union of the triangles, see ghost_oracle.h. Without ghost +// handling the oracle has to find ghosts in every scene, which shows it has teeth. +// +// With ghost handling a pose fails as a ghost when a manifold carries a normal the union lacks at its +// deep points, and as a miss when the union closest pair goes unrepresented with ghost handling but not +// without it. Both are rare rather than absent, so each has a budget holding the current count. The +// counts belong to the first ORACLE_SCAN_POSES poses, so a change to a scene or to the pose stream moves +// them without the collider changing. Lower a budget when a case is fixed. +// +// The remaining ghost, wave pose 291, is a hull face normal admitted by a vertex cone with the deepest +// point half a linear slop from the vertex, where the oracle only accepts the normal at the vertex. +static int MeshGhostOracle( void ) +{ + int ghostBudget[e_oracleSceneCount] = { 0, 0, 0, 0, 0, 0, 0, 1 }; + int missedBudget[e_oracleSceneCount] = { 0, 0, 0, 0, 0, 0, 0, 0 }; + + for ( int scene = 0; scene < e_oracleSceneCount; ++scene ) + { + b3MeshData* mesh = CreateOracleMesh( (OracleScene)scene ); + ENSURE( mesh != NULL ); + + OracleScan scan = ScanGhostOracle( mesh, (OracleScene)scene, ORACLE_SCAN_POSES ); + b3DestroyMesh( mesh ); + + if ( scan.validCount <= ORACLE_SCAN_POSES / 2 || scan.rawGhostPoses == 0 || scan.ghostPoses != ghostBudget[scene] || + scan.missedPoses != missedBudget[scene] ) + { + printf( " oracle %s: valid %d, unhandled ghosts %d, ghosts %d of %d (first pose %d), missed %d of %d " + "(first pose %d)\n", + g_oracleSceneNames[scene], scan.validCount, scan.rawGhostPoses, scan.ghostPoses, ghostBudget[scene], + scan.firstGhost, scan.missedPoses, missedBudget[scene], scan.firstMissed ); + } + + ENSURE( scan.validCount > ORACLE_SCAN_POSES / 2 ); + ENSURE( scan.rawGhostPoses > 0 ); + ENSURE( scan.ghostPoses <= ghostBudget[scene] ); + ENSURE( scan.missedPoses <= missedBudget[scene] ); + } + + return 0; +} + int MeshTest( void ) { RUN_SUBTEST( MeshDenseStride ); @@ -350,6 +1357,16 @@ int MeshTest( void ) RUN_SUBTEST( MeshClockWiseIgnored ); RUN_SUBTEST( MeshClockWiseStrideWeld ); RUN_SUBTEST( MeshCreators ); + RUN_SUBTEST( MeshWideHullSlide ); + RUN_SUBTEST( MeshManyTriangles ); + RUN_SUBTEST( MeshEdgeAngles ); + RUN_SUBTEST( MeshVertexRing ); + RUN_SUBTEST( MeshSphereOnRidge ); + RUN_SUBTEST( MeshHullOnSaddleRidge ); + RUN_SUBTEST( MeshCapsuleOverCrest ); + RUN_SUBTEST( MeshApexContact ); + RUN_SUBTEST( MeshPeelCylinder ); + RUN_SUBTEST( MeshGhostOracle ); return 0; }