diff --git a/benchmark/ghost_baseline_hf.csv b/benchmark/ghost_baseline_hf.csv new file mode 100644 index 0000000..5ce9643 --- /dev/null +++ b/benchmark/ghost_baseline_hf.csv @@ -0,0 +1,111 @@ +scene,preset,mirror,body,ghosts,runs +flat,0,0,box,0,2 +flat,0,0,capsule,0,2 +flat,0,0,upright,0,2 +flat,0,0,sphere,0,2 +flat,0,0,cylinder,0,2 +flat,0,1,box,0,2 +flat,0,1,capsule,0,2 +flat,0,1,upright,0,2 +flat,0,1,sphere,0,2 +flat,0,1,cylinder,0,2 +ramp,0,0,box,0,3 +ramp,0,0,capsule,0,3 +ramp,0,0,upright,0,3 +ramp,0,0,sphere,0,3 +ramp,0,0,cylinder,0,3 +ramp,0,1,box,0,3 +ramp,0,1,capsule,0,3 +ramp,0,1,upright,0,3 +ramp,0,1,sphere,0,3 +ramp,0,1,cylinder,0,3 +cliff,0,0,box,0,2 +cliff,0,0,capsule,0,2 +cliff,0,0,upright,0,2 +cliff,0,0,sphere,0,2 +cliff,0,0,cylinder,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 +tjunction,0,0,box,0,2 +tjunction,0,0,capsule,0,2 +tjunction,0,0,upright,0,2 +tjunction,0,0,sphere,0,2 +tjunction,0,0,cylinder,3,2 +tjunction,0,1,box,2,0 +tjunction,0,1,capsule,0,2 +tjunction,0,1,upright,0,2 +tjunction,0,1,sphere,0,2 +tjunction,0,1,cylinder,0,2 +seam,0,0,box,0,2 +seam,0,0,capsule,0,2 +seam,0,0,upright,6,2 +seam,0,0,sphere,0,2 +seam,0,0,cylinder,3,2 +seam,0,1,box,2,1 +seam,0,1,capsule,0,2 +seam,0,1,upright,0,2 +seam,0,1,sphere,0,2 +seam,0,1,cylinder,0,2 +peel,0,0,box,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 diff --git a/benchmark/ghost_scan.c b/benchmark/ghost_scan.c index f756662..7429ec2 100644 --- a/benchmark/ghost_scan.c +++ b/benchmark/ghost_scan.c @@ -253,6 +253,10 @@ int main( int argc, char** argv ) { settings.speculative = false; } + else if ( strcmp( arg, "--height-field" ) == 0 ) + { + settings.heightField = true; + } else if ( strcmp( arg, "-v" ) == 0 || strcmp( arg, "--verbose" ) == 0 ) { verbose = true; @@ -274,6 +278,7 @@ int main( int argc, char** argv ) " -w, --write= write results as csv\n" " -c, --compare= compare with a written csv, fail if any count grew\n" " --speed=, --yaw=, --friction=, --cell=, --fixed-rotation, --no-speculative\n" + " --height-field height field ground, the t-junction becomes a fine and coarse tile seam\n" " -v, --verbose print every ghost\n" "Exit code is 1 when a compared count grew, 0 otherwise.\n" ); return 2; @@ -328,8 +333,8 @@ int main( int argc, char** argv ) return 2; } - printf( "ghost scan: %d configs, %d steps, speed %.1f, yaw %.0f, friction %.2f, cell %.2f\n", configCount, stepCount, - settings.speed, settings.yaw, settings.friction, settings.cellSize ); + 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(); diff --git a/include/box3d/collision.h b/include/box3d/collision.h index 64f8986..d5e2e83 100644 --- a/include/box3d/collision.h +++ b/include/box3d/collision.h @@ -353,6 +353,17 @@ B3_INLINE const int8_t* b3GetMeshEdgeAngles( const b3MeshData* mesh ) 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 diff --git a/include/box3d/types.h b/include/box3d/types.h index e7776e8..c7029c8 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -2294,6 +2294,13 @@ typedef struct b3MeshTriangle // Indicates an isolated edge. #define B3_BOUNDARY_EDGE_ANGLE ( -128 ) +// Mesh vertex flags. Used for ghost collision handling. +typedef enum b3MeshVertexFlags +{ + b3_flatVertex = 0x01, + b3_concaveVertex = 0x02, +} b3MeshVertexFlags; + /// A mesh BVH node. typedef struct b3MeshNode { @@ -2384,6 +2391,9 @@ typedef struct b3MeshData // Offset to the array of neighbor quantized normal offsets. int32_t edgeAngleOffset; + + /// Offset of the vertex flag array in bytes from the struct address. + int32_t vertexFlagOffset; } b3MeshData; /// This allows mesh data to be re-used with different scales. diff --git a/samples/sample_manifold.cpp b/samples/sample_manifold.cpp index e479a7c..66ccfbc 100644 --- a/samples/sample_manifold.cpp +++ b/samples/sample_manifold.cpp @@ -2092,12 +2092,12 @@ struct GhostScene bool speculative; }; -// Every surface in these scenes faces up, so wind each triangle upward -static void AddGhostTriangle( GhostMesh& mesh, int a, int b, int c ) +// 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 ( normal.y < 0.0f ) + if ( b3Dot( normal, facing ) < 0.0f ) { int temp = b; b = c; @@ -2630,31 +2630,56 @@ static void BuildGhostSeam( std::vector& meshes ) } // 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. -static void BuildGhostChamfer( std::vector& meshes ) +// < 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 = ( -16.0f + 8.0f * k ) * inch; + 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( { 24.0f * inch, 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 = 3 * k, top0 = 3 * k + 1, far0 = 3 * k + 2; - int low1 = low0 + 3, top1 = top0 + 3, far1 = far0 + 3; + 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 ); +} + static const float s_inch = 0.0254f; static const GhostScene s_hullGhostScenes[] = { @@ -2737,7 +2762,7 @@ static const GhostScene s_hullGhostScenes[] = { true, }, { - "s&box chamfer", + "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 " @@ -2748,6 +2773,16 @@ static const GhostScene s_hullGhostScenes[] = { { 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, + }, }; class HullGhostManifold : public GhostManifold diff --git a/samples/sample_mesh.cpp b/samples/sample_mesh.cpp index a915823..f3e8450 100644 --- a/samples/sample_mesh.cpp +++ b/samples/sample_mesh.cpp @@ -1710,7 +1710,8 @@ public: relaunch = true; } - rebuild |= ImGui::Checkbox( "Mirror Mesh", &settings.mirror ); + 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" ); diff --git a/shared/ghost_collisions.c b/shared/ghost_collisions.c index b39f551..ad84279 100644 --- a/shared/ghost_collisions.c +++ b/shared/ghost_collisions.c @@ -48,6 +48,7 @@ GhostSettings GetDefaultGhostSettings( void ) .body = e_ghostBox, .peelPreset = 0, .mirror = false, + .heightField = false, .cellSize = 0.25f, .speed = 6.0f, .yaw = 0.0f, @@ -173,7 +174,8 @@ static void AddMesh( GhostCollisionsData* data, MeshBuilder* builder, bool frict builder->indexCount = 0; } -static void DestroyGhostMeshes( GhostCollisionsData* data ) +// The bodies holding height field shapes must go first +static void DestroyGhostGroundData( GhostCollisionsData* data ) { for ( int i = 0; i < data->meshCount; ++i ) { @@ -181,6 +183,13 @@ static void DestroyGhostMeshes( GhostCollisionsData* data ) 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 ) @@ -191,13 +200,107 @@ static void PushProfile( GhostCollisionsData* data, float x, float 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; + + float* heights = malloc( countX * countZ * sizeof( float ) ); + for ( int row = 0; row < countZ; ++row ) + { + for ( int column = 0; column < countX; ++column ) + { + float x = mirror ? x1 - cellX * (float)column : x0 + cellX * (float)column; + heights[row * countX + column] = 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 ); } - DestroyGhostMeshes( data ); + + for ( int i = 0; i < data->heightFieldCount; ++i ) + { + b3DestroyBody( data->heightBodyIds[i] ); + } + + DestroyGhostGroundData( data ); b3BodyDef bodyDef = b3DefaultBodyDef(); data->groundId = b3CreateBody( worldId, &bodyDef ); @@ -219,9 +322,10 @@ void CreateGhostGround( b3WorldId worldId, GhostCollisionsData* data ) 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, 0.0f, 0.0f ); + PushProfile( data, settings->heightField ? settings->cellSize : 0.0f, 0.0f ); PushProfile( data, h, 0.0f ); break; @@ -262,8 +366,42 @@ void CreateGhostGround( b3WorldId worldId, GhostCollisionsData* data ) b3Vec2 right[2] = { { 0.0f, 0.0f }, { h, 0.0f } }; float cellSize = settings->cellSize; bool mirror = settings->mirror; + float groundHalfWidth = GHOST_HALF_WIDTH; + int cellCountZ = GetCellCount( 2.0f * groundHalfWidth, cellSize ); - if ( settings->scene == e_ghostTJunction ) + if ( settings->heightField ) + { + if ( settings->scene == e_ghostTJunction ) + { + AddHeightField( worldId, data, -h, 0.0f, GetCellCount( h, cellSize ), groundHalfWidth, + GetCellCount( 2.0f * groundHalfWidth, 0.5f * cellSize ), false ); + AddHeightField( worldId, data, 0.0f, h, GetCellCount( h, cellSize ), groundHalfWidth, cellCountZ, false ); + } + else if ( settings->scene == e_ghostSeam ) + { + AddHeightField( worldId, data, -h, 0.0f, GetCellCount( h, cellSize ), groundHalfWidth, cellCountZ, false ); + AddHeightField( worldId, data, 0.0f, h, GetCellCount( h, cellSize ), groundHalfWidth, cellCountZ, false ); + } + else 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 + { + AddHeightField( worldId, data, -h, h, GetCellCount( 2.0f * h, cellSize ), groundHalfWidth, cellCountZ, false ); + } + } + else if ( settings->scene == e_ghostTJunction ) { // Fine and coarse strips share only every other vertex along x = 0 AddStrip( &builder, left, 2, cellSize, 0.5f * cellSize, mirror ); @@ -520,7 +658,8 @@ int UpdateGhostCollisions( b3WorldId worldId, GhostCollisionsData* data, GhostRe { bool bodyIsA = B3_ID_EQUALS( contacts[i].shapeIdA, data->shapeId ); b3ShapeId otherId = bodyIsA ? contacts[i].shapeIdB : contacts[i].shapeIdA; - if ( b3Shape_GetType( otherId ) != b3_meshShape ) + b3ShapeType otherType = b3Shape_GetType( otherId ); + if ( otherType != b3_meshShape && otherType != b3_heightShape ) { continue; } @@ -579,7 +718,7 @@ int UpdateGhostCollisions( b3WorldId worldId, GhostCollisionsData* data, GhostRe void DestroyGhostCollisions( GhostCollisionsData* data ) { - DestroyGhostMeshes( data ); + DestroyGhostGroundData( data ); b3DestroyHull( data->cylinder ); b3DestroyHull( data->peelCylinder ); data->cylinder = NULL; diff --git a/shared/ghost_collisions.h b/shared/ghost_collisions.h index bddfc47..4161257 100644 --- a/shared/ghost_collisions.h +++ b/shared/ghost_collisions.h @@ -58,6 +58,11 @@ typedef struct GhostSettings GhostBody body; int peelPreset; bool mirror; + + // Build the ground from height fields instead of meshes. Each height field is its own shape, so the + // T-junction scene becomes a seam between a fine and a coarse tile. + bool heightField; + float cellSize; float speed; @@ -89,6 +94,11 @@ typedef struct GhostCollisionsData 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; @@ -112,7 +122,7 @@ typedef struct GhostCollisionsData // Creates the ground and launches the body GhostCollisionsData CreateGhostCollisions( b3WorldId worldId, GhostSettings settings ); -// Rebuild the ground after a scene, preset, mirror or cell size change. Relaunch after this. +// 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 diff --git a/src/mesh.c b/src/mesh.c index 5329946..5439d8e 100644 --- a/src/mesh.c +++ b/src/mesh.c @@ -138,6 +138,16 @@ static int8_t* b3GetMeshEdgeAnglesWrite( b3MeshData* mesh ) 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 ) ) @@ -1110,6 +1120,30 @@ bool b3GetMeshNeighborNormal( const b3Mesh* mesh, int triangleIndex, int edgeInd return true; } +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; + } + + b3Vec3 scale = mesh->scale; + if ( scale.x * scale.y * scale.z < 0.0f ) + { + // Handle reflection. + vertexIndex = ( 3 - vertexIndex ) % 3; + } + + b3MeshTriangle triangle = b3GetMeshTriangles( data )[triangleIndex]; + int indices[3] = { triangle.index1, triangle.index2, triangle.index3 }; + return vertexFlags[indices[vertexIndex]]; +} + #define NAME b3EdgeMap #define KEY_TY uint64_t #define VAL_TY int @@ -1320,6 +1354,40 @@ static void b3IdentifyEdges( b3MeshData* mesh ) } } + 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( normals, b3Vec3, triangleCount ); B3_FREE( edges, b3MeshEdge, edgeCount ); } @@ -1841,10 +1909,13 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, 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 ); @@ -1868,6 +1939,7 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, mesh->materialCount = materialCount; mesh->flagsOffset = flagsOffset; mesh->edgeAngleOffset = edgeAngleOffset; + mesh->vertexFlagOffset = vertexFlagOffset; b3MeshNode* nodes = b3GetMeshNodesWrite( mesh ); b3MeshTriangle* triangles = b3GetMeshTrianglesWrite( mesh ); diff --git a/src/mesh_contact.c b/src/mesh_contact.c index fa0a92c..df66957 100644 --- a/src/mesh_contact.c +++ b/src/mesh_contact.c @@ -445,6 +445,28 @@ static bool b3GetNeighborDirection( const b3Shape* shape, int triangleIndex, con 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]; + return b3Dot( normal, b3Sub( next, vertex ) ) <= 0.0f && b3Dot( normal, b3Sub( previous, vertex ) ) <= 0.0f; +} + +// Check the vertex flags to determined if this normal is admissible. +static bool b3AdmitsVertexNormal( const b3Shape* shape, int triangleIndex, const b3Vec3* vertices, int vertexIndex, + b3Vec3 normal ) +{ + if ( b3IsInVertexRegion( vertices, vertexIndex, normal ) == false ) + { + return false; + } + + int flags = b3GetVertexFlag( shape, triangleIndex, vertexIndex ); + return ( flags & ( b3_flatVertex | b3_concaveVertex ) ) == 0; +} + // 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, @@ -452,6 +474,7 @@ static b3NeighborResult b3TestNeighbors( const b3Shape* shape, int triangleIndex { int edges[2] = { 0, 0 }; int edgeCount = 1; + int vertexIndex = B3_NULL_INDEX; switch ( feature ) { @@ -471,18 +494,21 @@ static b3NeighborResult b3TestNeighbors( const b3Shape* shape, int triangleIndex 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: @@ -514,7 +540,18 @@ static b3NeighborResult b3TestNeighbors( const b3Shape* shape, int triangleIndex return b3_neighborAdmit; } - return convex ? b3_neighborSkip : b3_neighborSnap; + if ( convex ) + { + return b3_neighborSkip; + } + + // Check vertex. + if ( vertexIndex != B3_NULL_INDEX && b3AdmitsVertexNormal( shape, triangleIndex, vertices, vertexIndex, normal ) ) + { + return b3_neighborAdmit; + } + + return b3_neighborSnap; } // Voronoi test of neighbors based on deepest point. @@ -542,6 +579,20 @@ static b3NeighborResult b3TestHullFaceNeighbors( const b3Shape* shape, int trian } } + // Test vertices. + if ( manifold->points[deepest].separation < 0.0f ) + { + for ( int i = 0; i < 3; ++i ) + { + if ( b3AdmitsVertexNormal( shape, triangleIndex, vertices, i, normal ) ) + { + b3TriangleFeature vertexFeatures[3] = { b3_featureVertex1, b3_featureVertex2, b3_featureVertex3 }; + *nearFeature = vertexFeatures[i]; + return b3_neighborAdmit; + } + } + } + // Clip points were moved onto the hull face, step back along the normal to the triangle. const b3LocalManifoldPoint* deepestPoint = manifold->points + deepest; b3Vec3 point = b3MulSub( deepestPoint->point, deepestPoint->separation, normal ); @@ -567,7 +618,34 @@ static b3NeighborResult b3TestHullFaceNeighbors( const b3Shape* shape, int trian } } - b3TriangleFeature feature; + b3TriangleFeature feature = b3_featureNone; + if ( nearCount == 2 ) + { + int vertexIndex; + if ( nearEdges[0] == 0 && nearEdges[1] == 1 ) + { + feature = b3_featureVertex2; + vertexIndex = 1; + } + else if ( nearEdges[0] == 1 ) + { + feature = b3_featureVertex3; + vertexIndex = 2; + } + else + { + feature = b3_featureVertex1; + vertexIndex = 0; + } + + // Test vertex. + if ( b3AdmitsVertexNormal( shape, triangleIndex, vertices, vertexIndex, normal ) ) + { + *nearFeature = feature; + return b3_neighborAdmit; + } + } + if ( nearCount == 0 || inwardTilt ) { // A tilted hull face over the triangle interior, the triangle face owns the normal. @@ -583,22 +661,7 @@ static b3NeighborResult b3TestHullFaceNeighbors( const b3Shape* shape, int trian b3TriangleFeature edgeFeatures[3] = { b3_featureEdge1, b3_featureEdge2, b3_featureEdge3 }; feature = edgeFeatures[nearEdges[0]]; } - else if ( nearCount == 2 ) - { - if ( nearEdges[0] == 0 && nearEdges[1] == 1 ) - { - feature = b3_featureVertex2; - } - else if ( nearEdges[0] == 1 ) - { - feature = b3_featureVertex3; - } - else - { - feature = b3_featureVertex1; - } - } - else + else if ( nearCount == 3 ) { return b3_neighborUnknown; } @@ -1134,6 +1197,7 @@ static int b3CollideMeshTriangles( b3Cluster** clustersOut, const b3Shape* shape { b3BlockSource* source = blockSources + i; b3Vec3 normal = manifoldBuffer[source->manifoldIndex].normal; + float bestAlignment = -FLT_MAX; for ( int j = 0; j < survivorCount; ++j ) { if ( j == source->survivorIndex ) @@ -1143,13 +1207,22 @@ static int b3CollideMeshTriangles( b3Cluster** clustersOut, const b3Shape* shape if ( b3ShouldBlockNormal( survivorVertices + 3 * j, blockTargets[j], source->point, normal ) ) { - source->blockerIndex = j; - int blockerManifold = survivorManifolds[j]; - if ( blockerManifold != B3_NULL_INDEX ) + // todo explain this better + float alignment = b3Dot( normal, blockTargets[j].normal ); + if ( alignment > bestAlignment ) { - usedAsBlocker[blockerManifold] = true; + bestAlignment = alignment; + source->blockerIndex = j; } - break; + } + } + + if ( source->blockerIndex != B3_NULL_INDEX ) + { + int blockerManifold = survivorManifolds[source->blockerIndex]; + if ( blockerManifold != B3_NULL_INDEX ) + { + usedAsBlocker[blockerManifold] = true; } } } diff --git a/src/shape.h b/src/shape.h index 048f04c..5727559 100644 --- a/src/shape.h +++ b/src/shape.h @@ -199,6 +199,19 @@ static inline bool b3GetNeighborNormal( const b3Shape* shape, int triangleIndex, return b3GetHeightFieldNeighborNormal( shape->heightField, triangleIndex, edgeIndex, normal ); } +int b3GetMeshVertexFlag( const b3Mesh* mesh, int triangleIndex, int vertexIndex ); + +static inline int b3GetVertexFlag( const b3Shape* shape, int triangleIndex, int vertexIndex ) +{ + if ( shape->type == b3_meshShape ) + { + return b3GetMeshVertexFlag( &shape->mesh, triangleIndex, vertexIndex ); + } + + B3_ASSERT( shape->type == b3_heightShape ); + return b3_flatVertex; +} + bool b3IsValidMesh( const b3MeshData* meshData ); static inline bool b3ShouldShapesCollide( b3Filter filterA, b3Filter filterB )