diff --git a/include/box3d/collision.h b/include/box3d/collision.h index 1485af6..64f8986 100644 --- a/include/box3d/collision.h +++ b/include/box3d/collision.h @@ -668,6 +668,10 @@ B3_API void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b 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 ); + /// This is used for computing 2D hulls in service of contact manifold simplification. /// Placed here for testing. Internal. typedef struct b3Point2D diff --git a/samples/sample_manifold.cpp b/samples/sample_manifold.cpp index d908f5c..e479a7c 100644 --- a/samples/sample_manifold.cpp +++ b/samples/sample_manifold.cpp @@ -9,6 +9,7 @@ #include "box3d/constants.h" #include +#include #include @@ -18,7 +19,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 +133,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 +261,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 +379,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 +411,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 +451,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 +464,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 +518,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 +555,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 +580,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 +604,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 +617,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 +668,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 +689,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 +703,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 +794,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 +813,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 +858,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 +885,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 +906,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 +950,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 +979,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 +1017,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 +1059,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 +1074,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 +1108,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 +1141,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 +1167,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 +1184,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 +1199,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 +1211,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 +1220,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 +1256,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 +1286,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 +1306,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 +1325,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 +1337,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 +1346,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 +1395,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 +1422,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 +1508,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 +1539,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 +1548,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 +1569,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 +1634,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 +1681,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 +1703,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 +1712,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 +1738,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 +1769,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 +1802,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 +1828,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 +1846,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 +1870,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 +1943,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 { @@ -1967,8 +2050,8 @@ 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_flags, m_hull, &m_satCache, true ); } int m_flags; @@ -1977,4 +2060,743 @@ public: 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; +}; + +// Every surface in these scenes faces up, so wind each triangle upward +static void AddGhostTriangle( GhostMesh& mesh, int a, int b, int c ) +{ + b3Vec3 va = mesh.vertices[a]; + b3Vec3 normal = b3Cross( b3Sub( mesh.vertices[b], va ), b3Sub( mesh.vertices[c], va ) ); + if ( normal.y < 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 = 16; + 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; + 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_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 ); + 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 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]] ); + } + } + } + + 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::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_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 ); +} + +// The flat pair with each triangle in its own mesh +static void BuildGhostSeam( std::vector& meshes ) +{ + GhostMesh left; + left.vertices = { { 0.0f, 0.0f, -2.0f }, { 0.0f, 0.0f, 2.0f }, { -2.0f, 0.0f, 0.0f } }; + AddGhostTriangle( left, 0, 1, 2 ); + meshes.push_back( left ); + + GhostMesh right; + right.vertices = { { 0.0f, 0.0f, -2.0f }, { 0.0f, 0.0f, 2.0f }, { 2.0f, 0.0f, 0.0f } }; + AddGhostTriangle( right, 1, 0, 2 ); + meshes.push_back( right ); +} + +// 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 ) +{ + float inch = 0.0254f; + GhostMesh mesh; + for ( int k = 0; k <= 4; ++k ) + { + float z = ( -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 } ); + } + + 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; + AddGhostTriangle( mesh, low0, low1, top1 ); + AddGhostTriangle( mesh, low0, top1, top0 ); + AddGhostTriangle( mesh, top0, top1, far1 ); + AddGhostTriangle( mesh, top0, far1, far0 ); + } + 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, + }, + { + "T-junction", + "a vertex in the middle of the left triangle edge, no edge matches so all are boundary " + "edges", + "boundary edge, the block pass tests the normal against the other triangles near the box " + "and drops it", + BuildGhostTJunction, + { 0.5f, 0.5f, 0.5f }, + { -0.708f, 0.499f, 0.3f }, + { 0.0f, 45.0f, 0.0f }, + true, + }, + { + "Mesh seam", + "the flat corner case with each triangle in its own mesh", + "none yet, each mesh only sees its own triangles (ghost plan step 4)", + BuildGhostSeam, + { 0.5f, 0.5f, 0.5f }, + { -0.708f, 0.499f, 0.0f }, + { 0.0f, 45.0f, 0.0f }, + true, + }, + { + "s&box 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, + }, +}; + +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], 0, + &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 ); diff --git a/src/mesh_contact.c b/src/mesh_contact.c index d34fdc4..fa0a92c 100644 --- a/src/mesh_contact.c +++ b/src/mesh_contact.c @@ -766,30 +766,18 @@ static bool b3ShouldBlockNormal( const b3Vec3* vertices, b3BlockTarget target, b return onEdge == false || ( infringed && allInfringed ); } -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 ) +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 ) { - B3_ASSERT( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape ); - B3_UNUSED( workerIndex ); - B3_UNUSED( isFast ); - B3_UNUSED( materialMap ); - - b3TaskContext* context = b3Array_Get( world->taskContexts, workerIndex ); - - b3RefreshCache( contact, shapeA, xfA, &shapeB->aabb, 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* ) ); + b3Bump( arena, triangleCount * sizeof( b3LocalManifold* ) ); int acceptedManifoldCount = 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; @@ -799,17 +787,14 @@ 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; - - b3TriangleCache* triangleCaches = meshContact->triangleCache.data; const b3HullData* hullB = shapeB->type == b3_hullShape ? shapeB->hull : NULL; float hullSpeculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f; 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; @@ -956,17 +941,17 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact int pointBufferCapacity = B3_MAX_POINTS_PER_TRIANGLE * survivorCount + 2 * B3_MAX_CLIP_POINTS; b3LocalManifoldPoint* pointBuffer = - b3Bump( &arena, pointBufferCapacity * sizeof( b3LocalManifoldPoint ) ); + b3Bump( arena, pointBufferCapacity * sizeof( b3LocalManifoldPoint ) ); int totalPointCount = 0; - b3LocalManifold* manifoldBuffer = b3Bump( &arena, survivorCount * sizeof( b3LocalManifold ) ); + b3LocalManifold* manifoldBuffer = b3Bump( arena, survivorCount * sizeof( b3LocalManifold ) ); int manifoldCount = 0; - bool validateNeighbors = B3_FORCE_GHOST_COLLISIONS == 0; + bool validateNeighbors = handleGhosts; - int* survivorManifolds = b3Bump( &arena, survivorCount * sizeof( int ) ); + int* survivorManifolds = b3Bump( arena, survivorCount * sizeof( int ) ); memset( survivorManifolds, 0xFF, survivorCount * sizeof( int ) ); - b3BlockSource* blockSources = b3Bump( &arena, survivorCount * sizeof( b3BlockSource ) ); + b3BlockSource* blockSources = b3Bump( arena, survivorCount * sizeof( b3BlockSource ) ); int blockSourceCount = 0; for ( int survivorIndex = 0; @@ -1013,8 +998,11 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact b3CollideTriangleAndHull( manifold, pointCapacity, vertices[0], vertices[1], vertices[2], triangle.flags, 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: @@ -1023,7 +1011,7 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact default: B3_ASSERT( false ); - return false; + return 0; } bool blockSource = false; @@ -1133,10 +1121,10 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact if ( blockSourceCount > 0 ) { - bool* usedAsBlocker = b3Bump( &arena, manifoldCount * sizeof( bool ) ); + bool* usedAsBlocker = b3Bump( arena, manifoldCount * sizeof( bool ) ); memset( usedAsBlocker, 0, manifoldCount * sizeof( bool ) ); - b3BlockTarget* blockTargets = b3Bump( &arena, survivorCount * sizeof( b3BlockTarget ) ); + b3BlockTarget* blockTargets = b3Bump( arena, survivorCount * sizeof( b3BlockTarget ) ); for ( int i = 0; i < survivorCount; ++i ) { blockTargets[i] = b3MakeBlockTarget( survivorVertices + 3 * i ); @@ -1212,17 +1200,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. @@ -1276,12 +1258,12 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact if ( clusterPointCount == 0 ) { - return false; + return 0; } // Setup clusters b3LocalManifoldPoint* clusterPoints = - b3Bump( &arena, clusterPointCount * sizeof( b3LocalManifoldPoint ) ); + b3Bump( arena, clusterPointCount * sizeof( b3LocalManifoldPoint ) ); int pointOffset = 0; for ( int i = 0; i < clusterCount; ++i ) @@ -1324,10 +1306,51 @@ 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 ); + B3_UNUSED( workerIndex ); + B3_UNUSED( isFast ); + B3_UNUSED( materialMap ); + + 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; @@ -1538,3 +1561,81 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact contact->tangentVelocity = b3Sub( tangentVelocityA, tangentVelocityB ); return true; } + +int b3CollideMeshAndHull( b3Manifold* manifolds, int capacity, const b3Mesh* mesh, const b3HullData* hull, + b3Transform transformBtoA, bool enableSpeculative, bool handleGhosts ) +{ + if ( capacity <= 0 ) + { + return 0; + } + + b3Shape meshShape = { + .type = b3_meshShape, + .mesh = *mesh, + }; + + b3Shape hullShape = { + .type = b3_hullShape, + .hull = hull, + }; + + b3Arena arena = b3CreateArena( 64 * 1024 ); + + float margin = B3_SPECULATIVE_DISTANCE; + b3Vec3 extension = { margin, margin, margin }; + b3AABB hullBounds = b3AABB_Transform( transformBtoA, hull->aabb ); + b3AABB queryBounds = { + .lowerBound = b3Sub( hullBounds.lowerBound, extension ), + .upperBound = b3Add( hullBounds.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, &hullShape, 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; + } + } + + b3DestroyArena( &arena ); + return count; +}