From b0825bc16793abe7c44173b64e8d2c31d25b2c65 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Mon, 9 Nov 2026 17:38:43 -0800 Subject: [PATCH] ghost oracle and cleanup --- samples/sample_manifold.cpp | 331 +++++++++++++++++ shared/CMakeLists.txt | 2 + shared/ghost_oracle.c | 706 ++++++++++++++++++++++++++++++++++++ shared/ghost_oracle.h | 126 +++++++ src/mesh_contact.c | 10 +- test/test_mesh.c | 81 +++++ 6 files changed, 1252 insertions(+), 4 deletions(-) create mode 100644 shared/ghost_oracle.c create mode 100644 shared/ghost_oracle.h diff --git a/samples/sample_manifold.cpp b/samples/sample_manifold.cpp index 05c62e8..6206166 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" @@ -3232,3 +3233,333 @@ public: 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_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; + Evaluate(); + } + + // Returns true if the pose is separated + bool EvaluatePose( int index ) + { + if ( MakeOraclePose( m_mesh, (OracleScene)m_scene, index, &m_box, &m_transform ) == false ) + { + return false; + } + + if ( EvaluateGhostOracle( &m_rawResult, m_mesh, &m_box.base, m_transform, false ) == false ) + { + return false; + } + + EvaluateGhostOracle( &m_result, m_mesh, &m_box.base, m_transform, true ); + return true; + } + + void Evaluate() + { + m_valid = EvaluatePose( m_poseIndex ); + } + + const OracleResult& Shown() const + { + return m_handleGhosts ? m_result : m_rawResult; + } + + bool IsCullingMiss() const + { + return m_result.missed && m_rawResult.missed == false; + } + + void FindNext( bool ghost ) + { + int start = m_poseIndex; + for ( int i = 1; i <= 5000; ++i ) + { + int index = start + i; + if ( EvaluatePose( index ) == false ) + { + continue; + } + + const OracleResult& result = Shown(); + bool found = ghost ? result.ghostCount > 0 : IsCullingMiss(); + if ( found ) + { + m_poseIndex = index; + m_valid = true; + snprintf( m_status, sizeof( m_status ), "found pose %d", index ); + return; + } + } + + snprintf( m_status, sizeof( m_status ), "none in poses %d to %d", start + 1, start + 5000 ); + Evaluate(); + } + + void Scan() + { + m_scanValid = 0; + m_scanGhosts = 0; + m_scanRawGhosts = 0; + m_scanMissed = 0; + for ( int i = 0; i < m_scanCount; ++i ) + { + if ( EvaluatePose( i ) == false ) + { + continue; + } + + m_scanValid += 1; + m_scanRawGhosts += m_rawResult.ghostCount > 0 ? 1 : 0; + m_scanGhosts += m_result.ghostCount > 0 ? 1 : 0; + m_scanMissed += IsCullingMiss() ? 1 : 0; + } + + m_scanned = true; + Evaluate(); + } + + void Focus() + { + m_camera->SetView( 30.0f, 25.0f, 2.0f, b3OffsetPos( b3Pos_zero, m_transform.p ) ); + } + + void Step() override + { + } + + void Render() override + { + const OracleResult& result = Shown(); + + 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 * result.distance ); + DrawTextLine( "ghost manifolds %d", result.ghostCount ); + if ( result.missed ) + { + DrawTextLine( "closest pair missed%s", m_handleGhosts && IsCullingMiss() ? ", by ghost handling" : "" ); + } + } + + if ( m_scanned ) + { + DrawTextLine( + "scan of %d poses: %d separated, ghost poses %d unhandled and %d handled, %d misses by handling", + m_scanCount, m_scanValid, m_scanRawGhosts, m_scanGhosts, m_scanMissed ); + } + + 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_transform.p ), m_transform.q }; + DrawHull( transform, &m_box.base, MakeColor( b3_colorGreen ) ); + + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + for ( int i = 0; i < result.pairCount; ++i ) + { + const OraclePair& pair = result.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( b3_colorLimeGreen ), 3.0f, OVERLAY_THICKNESS_PIXELS, + OVERLAY_OCCLUSION_DIM ); + DrawPoint( meshPoint, 8.0f, MakeColor( b3_colorLimeGreen ) ); + } + else + { + DrawLine( meshPoint, hullPoint, MakeColor( b3_colorDimGray ) ); + } + } + + DrawPoint( b3OffsetPos( b3Pos_zero, result.closestMeshPoint ), 12.0f, MakeColor( b3_colorWhite ) ); + + Vec4 verdictColors[] = { + MakeColor( b3_colorDodgerBlue ), + MakeColor( b3_colorCyan ), + MakeColor( b3_colorSilver ), + MakeColor( b3_colorRed ), + }; + + 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 = 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; + } + + 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 ); + m_status[0] = 0; + Evaluate(); + } + ImGui::PopItemWidth(); + + if ( ImGui::Checkbox( "Ghost Handling", &m_handleGhosts ) ) + { + m_status[0] = 0; + } + + if ( ImGui::Button( "Next Ghost" ) ) + { + FindNext( true ); + } + ImGui::SameLine(); + if ( ImGui::Button( "Next Miss" ) ) + { + FindNext( false ); + } + + if ( ImGui::Button( "Scan" ) ) + { + Scan(); + } + ImGui::SameLine(); + if ( ImGui::Button( "Focus" ) ) + { + Focus(); + } + + ImGui::TextColored( ImVec4( 0.2f, 0.8f, 0.2f, 1.0f ), "union contact" ); + ImGui::SameLine(); + ImGui::TextColored( ImVec4( 0.41f, 0.41f, 0.41f, 1.0f ), "dominated pair" ); + ImGui::TextColored( ImVec4( 0.12f, 0.56f, 1.0f, 1.0f ), "face" ); + ImGui::SameLine(); + ImGui::TextColored( ImVec4( 0.0f, 1.0f, 1.0f, 1.0f ), "edge or vertex" ); + ImGui::SameLine(); + ImGui::TextColored( ImVec4( 0.75f, 0.75f, 0.75f, 1.0f ), "shared" ); + ImGui::SameLine(); + ImGui::TextColored( ImVec4( 1.0f, 0.0f, 0.0f, 1.0f ), "ghost" ); + return true; + } + + static Sample* Create( SampleContext* context ) + { + return new GhostOracle( context ); + } + + static constexpr int m_scanCount = 500; + + b3MeshData* m_mesh; + int m_scene; + int m_poseIndex; + bool m_handleGhosts; + bool m_valid; + b3BoxHull m_box; + b3Transform m_transform; + OracleResult m_result; + OracleResult m_rawResult; + + bool m_scanned; + int m_scanValid = 0; + int m_scanGhosts = 0; + int m_scanRawGhosts = 0; + int m_scanMissed = 0; + char m_status[128]; +}; + +static int sampleGhostOracle = RegisterSample( "Manifold", "Ghost Oracle", GhostOracle::Create ); diff --git a/shared/CMakeLists.txt b/shared/CMakeLists.txt index f09c241..f5c323f 100644 --- a/shared/CMakeLists.txt +++ b/shared/CMakeLists.txt @@ -7,6 +7,8 @@ set(BOX3D_SHARED_FILES determinism.h ghost_collisions.c ghost_collisions.h + ghost_oracle.c + ghost_oracle.h human.c human.h mesh_drop.c diff --git a/shared/ghost_oracle.c b/shared/ghost_oracle.c new file mode 100644 index 0000000..ea20bdf --- /dev/null +++ b/shared/ghost_oracle.c @@ -0,0 +1,706 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "ghost_oracle.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", +}; + +#define ORACLE_MAX_VERTICES 256 +#define ORACLE_MAX_INDICES ( 3 * 256 ) + +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 ) +{ + 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 ) +{ + 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 ); +} + +static float WaveHeight( float x, float z ) +{ + return 0.12f * sinf( 2.3f * x + 0.4f ) * cosf( 1.7f * z ) + 0.05f * sinf( 4.1f * z - 1.3f * x ); +} + +static void BuildFan( OracleBuilder* builder ) +{ + int center = AddVertex( builder, b3Vec3_zero ); + for ( int i = 0; i < 6; ++i ) + { + float angle = ( i + 0.5f ) * B3_PI / 3.0f; + AddVertex( builder, (b3Vec3){ 2.0f * cosf( angle ), 0.0f, 2.0f * sinf( angle ) } ); + } + + 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, 0.08f, 0.08f }; + float sectionY[4] = { 0.0f, 0.35f, 0.35f, 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 ); +} + +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 ); + float angle = RandomRange( random, 0.0f, maxAngle ); + return b3Add( b3MulSV( cosf( angle ), axis ), b3MulSV( sinf( angle ), perp ) ); +} + +static bool CollectPair( b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleIndex, void* context ) +{ + OracleResult* result = context; + if ( result->pairCount == ORACLE_MAX_TRIANGLES ) + { + return false; + } + + OraclePair* pair = result->pairs + result->pairCount; + *pair = (OraclePair){ 0 }; + pair->triangleIndex = triangleIndex; + pair->vertices[0] = a; + pair->vertices[1] = b; + pair->vertices[2] = c; + pair->dominatedBy = -1; + result->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 ) ) ); +} + +static float ComputePairs( OracleResult* result, const b3MeshData* mesh, const b3HullData* hull, b3Transform transform ) +{ + result->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, result ); + + float minDistance = FLT_MAX; + for ( int i = 0; i < result->pairCount; ++i ) + { + OraclePair* pair = result->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; + result->closestMeshPoint = output.pointA; + result->closestHullPoint = output.pointB; + } + } + + result->distance = minDistance; + return minDistance; +} + +bool MakeOraclePose( 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 ); + + static OracleResult scratch; + for ( int iteration = 0; iteration < 20; ++iteration ) + { + float distance = ComputePairs( &scratch, mesh, &box->base, pose ); + if ( scratch.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 ) + { + *transform = pose; + return true; + } + + b3Vec3 direction = b3Normalize( b3Sub( scratch.closestHullPoint, scratch.closestMeshPoint ) ); + pose.p = b3MulAdd( pose.p, gap - distance, direction ); + } + + float distance = ComputePairs( &scratch, mesh, &box->base, pose ); + *transform = pose; + return ORACLE_MIN_DISTANCE <= distance && distance <= B3_SPECULATIVE_DISTANCE; +} + +bool EvaluateGhostOracle( OracleResult* result, const b3MeshData* mesh, const b3HullData* hull, b3Transform transform, + bool handleGhosts ) +{ + result->pointCount = 0; + result->ghostCount = 0; + result->missed = false; + + float distance = ComputePairs( result, mesh, hull, transform ); + if ( result->pairCount == 0 || distance < ORACLE_MIN_DISTANCE ) + { + result->pairCount = 0; + return false; + } + + // Ties along shared features differ only by GJK round off + float tieTolerance = 0.02f * B3_LINEAR_SLOP; + float containTolerance = 1.0e-4f; + + for ( int i = 0; i < result->pairCount; ++i ) + { + OraclePair* pair = result->pairs + i; + pair->owned = true; + for ( int j = 0; j < result->pairCount; ++j ) + { + const OraclePair* other = result->pairs + j; + if ( j == i || other->distance >= pair->distance - tieTolerance ) + { + continue; + } + + b3Vec3 closest = + ClosestPointOnTriangle( pair->meshPoint, other->vertices[0], other->vertices[1], other->vertices[2] ); + if ( b3DistanceSquared( closest, pair->meshPoint ) < containTolerance * containTolerance ) + { + pair->owned = false; + pair->dominatedBy = other->triangleIndex; + break; + } + } + } + + b3Manifold manifolds[ORACLE_MAX_TRIANGLES]; + b3Mesh meshShape = { .data = (b3MeshData*)mesh, .scale = b3Vec3_one }; + int manifoldCount = + b3CollideMeshAndHull( manifolds, ORACLE_MAX_TRIANGLES, &meshShape, hull, transform, true, handleGhosts ); + + // Clustering merges manifolds whose normals are within about 5 degrees + float sinTolerance = 0.1f; + float probeDistance = 0.01f; + 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 && result->pointCount < ORACLE_MAX_POINTS; ++j ) + { + 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 < result->pairCount; ++k ) + { + if ( result->pairs[k].triangleIndex == point->triangleIndex ) + { + pair = result->pairs + k; + break; + } + } + + if ( pair == NULL ) + { + continue; + } + + b3Vec3 meshPoint = + ClosestPointOnTriangle( point->point, pair->vertices[0], pair->vertices[1], pair->vertices[2] ); + point->meshPoint = meshPoint; + + for ( int k = 0; k < result->pairCount; ++k ) + { + const OraclePair* other = result->pairs + k; + b3Vec3 closest = + ClosestPointOnTriangle( meshPoint, other->vertices[0], other->vertices[1], other->vertices[2] ); + if ( b3DistanceSquared( closest, meshPoint ) > containTolerance * containTolerance ) + { + continue; + } + + b3Vec3 faceNormal = b3Normalize( b3Cross( b3Sub( other->vertices[1], other->vertices[0] ), + b3Sub( other->vertices[2], other->vertices[0] ) ) ); + b3Vec3 cross = b3Cross( faceNormal, point->normal ); + if ( b3Dot( faceNormal, point->normal ) > 0.0f && + b3LengthSquared( cross ) < sinTolerance * sinTolerance ) + { + point->verdict = e_oracleFace; + break; + } + } + + if ( point->verdict == e_oracleGhost ) + { + b3Vec3 probe = b3MulAdd( meshPoint, probeDistance, point->normal ); + float bestDistanceSquared = FLT_MAX; + b3Vec3 best = meshPoint; + for ( int k = 0; k < result->pairCount; ++k ) + { + const OraclePair* other = result->pairs + k; + b3Vec3 closest = + ClosestPointOnTriangle( probe, other->vertices[0], other->vertices[1], other->vertices[2] ); + float distanceSquared = b3DistanceSquared( closest, probe ); + if ( distanceSquared < bestDistanceSquared ) + { + bestDistanceSquared = distanceSquared; + best = closest; + } + } + + // The probe must also be in front of the surface, or an inverted face normal would pass + float offset = sinTolerance * probeDistance; + if ( b3DistanceSquared( best, meshPoint ) < offset * offset ) + { + for ( int k = 0; k < result->pairCount; ++k ) + { + const OraclePair* other = result->pairs + k; + b3Vec3 closest = ClosestPointOnTriangle( meshPoint, other->vertices[0], other->vertices[1], + other->vertices[2] ); + if ( b3DistanceSquared( closest, meshPoint ) > containTolerance * containTolerance ) + { + continue; + } + + b3Vec3 faceNormal = b3Cross( b3Sub( other->vertices[1], other->vertices[0] ), + b3Sub( other->vertices[2], other->vertices[0] ) ); + if ( b3Dot( faceNormal, point->normal ) > 0.0f ) + { + point->verdict = e_oracleFeature; + break; + } + } + } + } + } + + 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; + if ( distance < B3_SPECULATIVE_DISTANCE - linearSlop && minSeparation > distance + linearSlop ) + { + result->missed = true; + } + + return true; +} diff --git a/shared/ghost_oracle.h b/shared/ghost_oracle.h new file mode 100644 index 0000000..fa7ecdf --- /dev/null +++ b/shared/ghost_oracle.h @@ -0,0 +1,126 @@ +// 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 128 +#define ORACLE_MAX_POINTS 128 + +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 meshPoint; + b3Vec3 hullPoint; + b3Vec3 normal; + float distance; + bool owned; + + // Triangle index of a closer triangle containing the mesh point, or -1 + int dominatedBy; +} OraclePair; + +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; + +typedef struct OracleResult +{ + // Distance between the hull and the mesh + float distance; + b3Vec3 closestMeshPoint; + b3Vec3 closestHullPoint; + + OraclePair pairs[ORACLE_MAX_TRIANGLES]; + int pairCount; + + OraclePoint points[ORACLE_MAX_POINTS]; + int pointCount; + + // Ghost manifolds + int ghostCount; + bool missed; +} OracleResult; + +// 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]. Returns false if no separated pose was found. +bool MakeOraclePose( const b3MeshData* mesh, OracleScene scene, int index, b3BoxHull* box, b3Transform* transform ); + +// Returns false if the hull is not separated by ORACLE_MIN_DISTANCE, in which case the result is empty. +bool EvaluateGhostOracle( OracleResult* result, const b3MeshData* mesh, const b3HullData* hull, b3Transform transform, + bool handleGhosts ); + +#ifdef __cplusplus +} +#endif diff --git a/src/mesh_contact.c b/src/mesh_contact.c index 6fde83d..8010941 100644 --- a/src/mesh_contact.c +++ b/src/mesh_contact.c @@ -862,6 +862,10 @@ static inline bool b3IsSubset( int mask, int allowed ) } // 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 ) @@ -1184,8 +1188,6 @@ static int b3CollideMeshTriangles( b3Cluster** clustersOut, const b3Shape* shape b3LocalManifold* manifoldBuffer = b3Bump( arena, survivorCount * sizeof( b3LocalManifold ) ); int manifoldCount = 0; - bool validateNeighbors = handleGhosts; - int* survivorManifolds = b3Bump( arena, survivorCount * sizeof( int ) ); memset( survivorManifolds, 0xFF, survivorCount * sizeof( int ) ); @@ -1251,7 +1253,7 @@ static int b3CollideMeshTriangles( b3Cluster** clustersOut, const b3Shape* shape b3NeighborResult result = b3_neighborAdmit; int ownerEdge = B3_NULL_INDEX; uint8_t meshFlags = manifold->feature == b3_featureHullFace ? b3_meshContactHullFace : 0; - if ( validateNeighbors && manifold->pointCount > 0 && manifold->feature != b3_featureTriangleFace ) + if ( handleGhosts && manifold->pointCount > 0 && manifold->feature != b3_featureTriangleFace ) { if ( manifold->feature == b3_featureHullFace ) { @@ -1365,7 +1367,7 @@ static int b3CollideMeshTriangles( b3Cluster** clustersOut, const b3Shape* shape } } - if ( hullB != NULL && validateNeighbors ) + 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. diff --git a/test/test_mesh.c b/test/test_mesh.c index ec98259..fe929be 100644 --- a/test/test_mesh.c +++ b/test/test_mesh.c @@ -2,6 +2,7 @@ // SPDX-License-Identifier: MIT #include "ghost_collisions.h" +#include "ghost_oracle.h" #include "shape.h" #include "test_macros.h" @@ -11,6 +12,7 @@ #include #include +#include #include #include @@ -1304,6 +1306,84 @@ static int MeshVertexRing( void ) return 0; } +// Separated poses have an exact reference, the union of the triangles, see ghost_oracle.h. With ghost +// handling no manifold may carry a normal the union lacks at its deep points. Without ghost handling the +// oracle has to find ghosts in every scene, which shows it has teeth. +// +// A miss is a pose whose union closest pair goes unrepresented with ghost handling but not without it. +// The budget holds the current counts. The two cases examined so far: the cluster reduction keeps spread +// rather than the deepest point, so a snapped neighbor joining the cluster pushed the deepest point out +// (flat pose 416), and a contact against a boundary edge from the side was dropped (crest pose 377). +// Lower the budget as these are fixed. +static int MeshGhostOracle( void ) +{ + static OracleResult result; + int poseCount = 500; + int missedBudget[e_oracleSceneCount] = { 1, 0, 1, 3, 2, 1, 1, 3 }; + + for ( int scene = 0; scene < e_oracleSceneCount; ++scene ) + { + b3MeshData* mesh = CreateOracleMesh( (OracleScene)scene ); + ENSURE( mesh != NULL ); + + int validCount = 0; + int ghostPoses = 0; + int missedPoses = 0; + int rawGhostPoses = 0; + int firstGhost = -1; + int firstMissed = -1; + + for ( int i = 0; i < poseCount; ++i ) + { + b3BoxHull box; + b3Transform transform; + if ( MakeOraclePose( mesh, (OracleScene)scene, i, &box, &transform ) == false ) + { + continue; + } + + if ( EvaluateGhostOracle( &result, mesh, &box.base, transform, false ) == false ) + { + continue; + } + + validCount += 1; + rawGhostPoses += result.ghostCount > 0 ? 1 : 0; + bool rawMissed = result.missed; + + EvaluateGhostOracle( &result, mesh, &box.base, transform, true ); + if ( result.ghostCount > 0 ) + { + ghostPoses += 1; + firstGhost = firstGhost < 0 ? i : firstGhost; + } + + if ( result.missed && rawMissed == false ) + { + missedPoses += 1; + firstMissed = firstMissed < 0 ? i : firstMissed; + } + } + + b3DestroyMesh( mesh ); + + if ( validCount <= poseCount / 2 || rawGhostPoses == 0 || ghostPoses > 0 || missedPoses > missedBudget[scene] ) + { + printf( " oracle %s: valid %d, unhandled ghosts %d, ghosts %d (first pose %d), missed %d of %d (first " + "pose %d)\n", + g_oracleSceneNames[scene], validCount, rawGhostPoses, ghostPoses, firstGhost, missedPoses, + missedBudget[scene], firstMissed ); + } + + ENSURE( validCount > poseCount / 2 ); + ENSURE( rawGhostPoses > 0 ); + ENSURE( ghostPoses == 0 ); + ENSURE( missedPoses <= missedBudget[scene] ); + } + + return 0; +} + int MeshTest( void ) { RUN_SUBTEST( MeshDenseStride ); @@ -1322,6 +1402,7 @@ int MeshTest( void ) RUN_SUBTEST( MeshCapsuleOverCrest ); RUN_SUBTEST( MeshApexContact ); RUN_SUBTEST( MeshPeelCylinder ); + RUN_SUBTEST( MeshGhostOracle ); return 0; }