diff --git a/samples/sample_manifold.cpp b/samples/sample_manifold.cpp index 5e6dcd5..05c62e8 100644 --- a/samples/sample_manifold.cpp +++ b/samples/sample_manifold.cpp @@ -2843,6 +2843,36 @@ static void BuildGhostRail( std::vector& meshes ) 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 ); +} + static const float s_inch = 0.0254f; static const GhostScene s_hullGhostScenes[] = { @@ -2985,6 +3015,19 @@ static const GhostScene s_hullGhostScenes[] = { { -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, + }, }; class HullGhostManifold : public GhostManifold diff --git a/src/mesh_contact.c b/src/mesh_contact.c index f086f52..6fde83d 100644 --- a/src/mesh_contact.c +++ b/src/mesh_contact.c @@ -780,6 +780,87 @@ static b3NeighborResult b3TestNeighbors( const b3Shape* shape, int triangleIndex 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. static b3NeighborResult b3TestHullFaceNeighbors( const b3Shape* shape, int triangleIndex, const b3Vec3* vertices, b3Matrix3 meshToLocal, const b3LocalManifold* manifold, @@ -796,27 +877,43 @@ static b3NeighborResult b3TestHullFaceNeighbors( const b3Shape* shape, int trian return b3_neighborAdmit; } - int deepest = 0; + float minSeparation = manifold->points[0].separation; for ( int i = 1; i < manifold->pointCount; ++i ) { - if ( manifold->points[i].separation < manifold->points[deepest].separation ) - { - deepest = i; - } + minSeparation = b3MinFloat( minSeparation, manifold->points[i].separation ); } - // 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 ); + // 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; + } - float linearSlop = B3_LINEAR_SLOP; + // 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 ); - // If the hull overlaps this triangle then determine if the deepest point is close to a vertex. - if ( deepestPoint->separation < 0.0f ) + 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 ( b3DistanceSquared( point, vertices[i] ) > linearSlop * linearSlop ) + if ( ( vertexMask & ( 1 << i ) ) == 0 ) { continue; } @@ -825,69 +922,76 @@ static b3NeighborResult b3TestHullFaceNeighbors( const b3Shape* shape, int trian if ( admit ) { // This vertex tentatively accepts the normal. - b3TriangleFeature vertexFeatures[3] = { b3_featureVertex1, b3_featureVertex2, b3_featureVertex3 }; *nearFeature = vertexFeatures[i]; return b3_neighborAdmit; } } } - int nearEdges[3]; - int nearCount = 0; - bool inwardTilt = false; - bool alongEdge = false; - 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 ) - { - nearEdges[nearCount] = i; - nearCount += 1; - - // 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. - inwardTilt = inwardTilt || b3Dot( normal, inwardTangent ) > tolerance; - alongEdge = alongEdge || b3AbsFloat( b3Dot( normal, edge ) ) > tolerance; - } - } - - b3TriangleFeature feature = b3_featureNone; - 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; - } - } - - if ( nearCount == 0 || inwardTilt ) + if ( onFace ) { // A tilted hull face over the triangle interior, the triangle face owns the normal. return b3_neighborSnap; } - else if ( nearCount == 1 ) + + 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 ) { - if ( alongEdge ) + 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; } - b3TriangleFeature edgeFeatures[3] = { b3_featureEdge1, b3_featureEdge2, b3_featureEdge3 }; - feature = edgeFeatures[nearEdges[0]]; - } - else if ( nearCount == 3 ) - { - return b3_neighborSnap; + if ( edgeCount == 1 && b3AbsFloat( b3Dot( normal, edge ) ) > tolerance ) + { + return b3_neighborSnap; + } } *nearFeature = feature; @@ -1125,8 +1229,8 @@ static int b3CollideMeshTriangles( b3Cluster** clustersOut, const b3Shape* shape cache->satCache = (b3SATCache){ 0 }; } - b3CollideTriangleAndHull( manifold, pointCapacity, vertices[0], vertices[1], vertices[2], - hullB, &cache->satCache, enableSpeculative ); + b3CollideTriangleAndHull( manifold, pointCapacity, vertices[0], vertices[1], vertices[2], hullB, + &cache->satCache, enableSpeculative ); if ( context != NULL ) { context->satCallCount += 1; @@ -1223,8 +1327,8 @@ static int b3CollideMeshTriangles( b3Cluster** clustersOut, const b3Shape* shape } else if ( result == b3_neighborSnap && shapeB->type == b3_hullShape ) { - b3BuildTriangleAndHullFaceContact( manifold, pointCapacity, vertices, hullB, - &cache->satCache, enableSpeculative ); + b3BuildTriangleAndHullFaceContact( manifold, pointCapacity, vertices, hullB, &cache->satCache, + enableSpeculative ); } } diff --git a/test/test_determinism.c b/test/test_determinism.c index ce68e31..16a3975 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -21,25 +21,25 @@ #if defined( BOX3D_DOUBLE_PRECISION ) #define RAGDOLL_SLEEP_STEP 282 #define RAGDOLL_HASH 0x6E4B13AD -#define WAVE_PILE_SLEEP_STEP 290 -#define WAVE_PILE_HASH 0x86682E98 +#define WAVE_PILE_SLEEP_STEP 349 +#define WAVE_PILE_HASH 0x9AD0EB48 #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 197 -#define MESH_DROP_HASH 0x71517836 +#define MESH_DROP_HASH 0xBCCE1A08 #else #define RAGDOLL_SLEEP_STEP 277 #define RAGDOLL_HASH 0x5E042B6E -#define WAVE_PILE_SLEEP_STEP 290 -#define WAVE_PILE_HASH 0x889A0741 +#define WAVE_PILE_SLEEP_STEP 353 +#define WAVE_PILE_HASH 0xC1BEDCBA #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 197 -#define MESH_DROP_HASH 0xCB94D984 +#define MESH_DROP_HASH 0x4EC0324E #endif // The goldens above pin exact values for the default four point manifold. A build that diff --git a/test/test_mesh.c b/test/test_mesh.c index d9ab0bb..ec98259 100644 --- a/test/test_mesh.c +++ b/test/test_mesh.c @@ -685,6 +685,87 @@ static int MeshWideHullSlide( void ) 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 ) @@ -1237,6 +1318,7 @@ int MeshTest( void ) RUN_SUBTEST( MeshEdgeAngles ); RUN_SUBTEST( MeshVertexRing ); RUN_SUBTEST( MeshSphereOnRidge ); + RUN_SUBTEST( MeshHullOnSaddleRidge ); RUN_SUBTEST( MeshCapsuleOverCrest ); RUN_SUBTEST( MeshApexContact ); RUN_SUBTEST( MeshPeelCylinder );