// SPDX-FileCopyrightText: 2025 Erin Catto // SPDX-License-Identifier: MIT #include "gfx/draw.h" #include "gfx/keycodes.h" #include "sample.h" #include "box3d/box3d.h" #include "box3d/constants.h" #include #include #include static void DrawGizmoControls( bool* show, bool* local ) { ImGui::Checkbox( "Gizmo (G)", show ); 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. static void DrawTransformGizmo( const Camera* camera, b3WorldTransform* transform, bool local ) { ImGuizmo::BeginFrame(); // Behind the panels like the in-world labels ImGuizmo::SetDrawlist( ImGui::GetBackgroundDrawList() ); ImGuiViewport* viewport = ImGui::GetMainViewport(); ImGuizmo::SetRect( viewport->Pos.x, viewport->Pos.y, viewport->Size.x, viewport->Size.y ); ImGuizmo::SetOrthographic( false ); // Alt drag orbits the camera ImGuizmo::Enable( ImGui::GetIO().KeyAlt == false ); ImGuizmo::OPERATION operation = ImGuizmo::TRANSLATE | ImGuizmo::ROTATE; b3Pos origin = GetDrawOrigin(); b3Vec3 offset = b3SubPos( transform->p, origin ); b3Matrix3 r = b3MakeMatrixFromQuat( transform->q ); Mat4 matrix = { { r.cx.x, r.cx.y, r.cx.z, 0.0f }, { r.cy.x, r.cy.y, r.cy.z, 0.0f }, { r.cz.x, r.cz.y, r.cz.z, 0.0f }, { offset.x, offset.y, offset.z, 1.0f }, }; Mat4 view = camera->View(); Mat4 proj = camera->Proj(); ImGuizmo::MODE space = local ? ImGuizmo::LOCAL : ImGuizmo::WORLD; if ( ImGuizmo::Manipulate( &view.cx.x, &proj.cx.x, operation, space, &matrix.cx.x ) ) { // Rotation drags accumulate in the matrix, the quaternion keeps it orthonormal b3Matrix3 m = { { matrix.cx.x, matrix.cx.y, matrix.cx.z }, { matrix.cy.x, matrix.cy.y, matrix.cy.z }, { matrix.cz.x, matrix.cz.y, matrix.cz.z }, }; transform->q = b3NormalizeQuat( b3MakeQuatFromMatrix( &m ) ); transform->p = b3OffsetPos( origin, { matrix.cw.x, matrix.cw.y, matrix.cw.z } ); } } class Manifold : public Sample { public: explicit Manifold( SampleContext* sampleContext ) : Sample( sampleContext ) { if ( m_context->restart == false ) { m_camera->SetView( 35.0f, 30.0f, 50.0f, { 0.0f, 5.0f, 0.0f } ); } memset( m_points, 0, sizeof( m_points ) ); m_manifold = {}; m_manifold.points = m_points; { m_transformA.p = { 3.5f, 0.5f, 0.0f }; m_transformA.q = b3MakeQuatFromAxisAngle( { 0.0f, 1.0f, 0.0f }, 0.5f * B3_PI ); } { m_transformB.p = { 0.0f, 1.5f, 3.5f }; m_transformB.q = b3Quat_identity; } m_simplexCache = {}; m_satCache = {}; m_manualFeature = 0; m_drawPoints = true; m_useCache = false; m_showGizmo = true; m_gizmoLocal = false; } void Render() override { DrawTextLine( "count = %d", m_manifold.pointCount ); DrawAxes( b3WorldTransform_identity, 0.5f ); DrawGroundGrid( 80 ); if ( m_manifold.pointCount == 0 || m_drawPoints == false ) { return; } float length = 0.5f * b3GetLengthUnitsPerMeter(); b3Vec3 normal = b3RotateVector( m_transformA.q, m_manifold.normal ); for ( int pointIndex = 0; pointIndex < m_manifold.pointCount; ++pointIndex ) { const b3LocalManifoldPoint& manifoldPoint = m_manifold.points[pointIndex]; b3Pos point = b3TransformWorldPoint( m_transformA, manifoldPoint.point ); DrawLine( point, point + length * normal, MakeColor( b3_colorWhite ) ); if ( manifoldPoint.separation > 0.0f ) { DrawPoint( point, 10.0f, MakeColor( b3_colorWhite ) ); } else { DrawPoint( point, 10.0f, MakeColor( b3_colorYellow ) ); } DrawString3D( point, MakeColor( b3_colorWhite ), " %.3f", manifoldPoint.separation ); 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 ); } Sample::Render(); } bool HasSolverControls() const override { return false; } bool DrawControls() override { ImGui::Checkbox( "Use cache", &m_useCache ); if ( m_useCache ) { ImGui::RadioButton( "auto", &m_manualFeature, 0 ); ImGui::RadioButton( "faceA", &m_manualFeature, 1 ); ImGui::RadioButton( "faceB", &m_manualFeature, 2 ); ImGui::RadioButton( "edgePair", &m_manualFeature, 3 ); } ImGui::Checkbox( "Draw Points", &m_drawPoints ); DrawGizmoControls( &m_showGizmo, &m_gizmoLocal ); 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_transformB, m_gizmoLocal ); } } // The gizmo moves the shape, so keep the picking in Sample off void MouseDown( b3Vec2 p, int button, int modifiers ) override { } static constexpr int m_pointCapacity = 64; b3LocalManifold m_manifold; b3LocalManifoldPoint m_points[m_pointCapacity]; b3WorldTransform m_transformA; b3WorldTransform m_transformB; b3SimplexCache m_simplexCache; b3SATCache m_satCache; int m_manualFeature; bool m_useCache; bool m_drawPoints; bool m_showGizmo; bool m_gizmoLocal; }; class TriangleManifold : public Sample { public: explicit TriangleManifold( SampleContext* sampleContext ) : Sample( sampleContext ) { if ( m_context->restart == false ) { m_camera->SetView( 35.0f, 30.0f, 50.0f, { 0.0f, 5.0f, 0.0f } ); } m_manifold = {}; memset( m_points, 0, sizeof( m_points ) ); m_manifold.points = m_points; { m_transformA.p = { 3.5f, 0.5f, 0.0f }; m_transformA.q = b3MakeQuatFromAxisAngle( { 0.0f, 1.0f, 0.0f }, 0.5f * B3_PI ); } { m_transformB.p = { 0.0f, 1.5f, 3.5f }; m_transformB.q = b3Quat_identity; } m_simplexCache = {}; m_satCache = {}; m_useCache = false; m_manualFeature = 0; m_showGizmo = true; m_gizmoLocal = false; } void Render() override { DrawTextLine( "count = %d", m_manifold.pointCount ); DrawTextLine( "feature = %d", m_manifold.feature ); DrawTextLine( "cache hit = %d", m_satCache.hit ); DrawAxes( b3WorldTransform_identity, 1.0f ); DrawGroundGrid( 10 ); if ( m_manifold.pointCount > 0 ) { float length = 0.5f * b3GetLengthUnitsPerMeter(); b3Vec3 normal = b3RotateVector( m_transformB.q, m_manifold.normal ); for ( int pointIndex = 0; pointIndex < m_manifold.pointCount; ++pointIndex ) { const b3LocalManifoldPoint& manifoldPoint = m_manifold.points[pointIndex]; b3Pos point = b3TransformWorldPoint( m_transformB, manifoldPoint.point ); DrawLine( point, point + length * normal, MakeColor( b3_colorWhite ) ); if ( manifoldPoint.separation > 0.0f ) { DrawPoint( point, 10.0f, MakeColor( b3_colorWhite ) ); } else { DrawPoint( point, 10.0f, MakeColor( b3_colorYellow ) ); } 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 ); } } 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] ); b3Pos p3 = b3TransformWorldPoint( m_transformA, m_triangle[2] ); DrawString3D( p1, MakeColor( b3_colorWhite ), "0" ); DrawString3D( p2, MakeColor( b3_colorWhite ), "1" ); DrawString3D( p3, MakeColor( b3_colorWhite ), "2" ); b3Vec3 normal = b3Normalize( b3Cross( p2 - p1, p3 - p1 ) ); b3Pos center = p1 + ( 1.0f / 3.0f ) * ( ( p2 - p1 ) + ( p3 - p1 ) ); DrawArrow( center, center + 0.5f * normal, MakeColor( b3_colorMediumPurple ) ); Sample::Render(); } bool HasSolverControls() const override { return false; } bool DrawControls() override { ImGui::Checkbox( "Use cache", &m_useCache ); if ( m_useCache ) { ImGui::RadioButton( "auto", &m_manualFeature, 0 ); ImGui::RadioButton( "faceA", &m_manualFeature, 1 ); ImGui::RadioButton( "faceB", &m_manualFeature, 2 ); ImGui::RadioButton( "edgePair", &m_manualFeature, 3 ); } DrawGizmoControls( &m_showGizmo, &m_gizmoLocal ); 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_transformB, m_gizmoLocal ); } } // The gizmo moves the shape, so keep the picking in Sample off void MouseDown( b3Vec2 p, int button, int modifiers ) override { } static constexpr int m_pointCapacity = 8; b3LocalManifold m_manifold; b3LocalManifoldPoint m_points[m_pointCapacity]; // Triangle transform b3WorldTransform m_transformA; // Convex shape transform b3WorldTransform m_transformB; b3Vec3 m_triangle[3] = {}; b3SimplexCache m_simplexCache; b3SATCache m_satCache; int m_manualFeature; bool m_useCache; bool m_showGizmo; bool m_gizmoLocal; }; class SphereAndSphere : public Manifold { public: explicit SphereAndSphere( SampleContext* sampleContext ) : Manifold( sampleContext ) { m_sphere = { { 0.5f, 0.0f, -0.25f }, 2.0f }; } void Render() override { DrawSolidSphere( m_transformA, m_sphere, MakeColor( b3_colorGreen ) ); DrawSolidSphere( m_transformB, m_sphere, MakeColor( b3_colorCyan ) ); Manifold::Render(); } void Step() override { b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB ); b3CollideSpheres( &m_manifold, m_pointCapacity, &m_sphere, &m_sphere, transformBtoA ); } static Sample* Create( SampleContext* sampleContext ) { return new SphereAndSphere( sampleContext ); } b3Sphere m_sphere; }; static int sampleCollideSpheres = RegisterSample( "Manifold", "Sphere vs Sphere", SphereAndSphere::Create ); class CapsuleAndSphere : public Manifold { public: static Sample* Create( SampleContext* sampleContext ) { return new CapsuleAndSphere( sampleContext ); } explicit CapsuleAndSphere( SampleContext* sampleContext ) : Manifold( sampleContext ) { m_capsule = { { -2.0f, 0.0f, 0.0f }, { 2.0f, 0.0f, 0.0f }, 1.0f }; m_sphere = { { 0.0f, 0.0f, 0.0f }, 2.0f }; m_transformA = { { 0.0f, 0.0f, 0.0f }, b3Quat_identity }; m_transformB = { { -4.0f, 0.0f, 0.0f }, b3Quat_identity }; } void Render() override { DrawSolidCapsule( m_transformA, m_capsule, MakeColor( b3_colorCyan ) ); DrawSolidSphere( m_transformB, m_sphere, MakeColor( b3_colorGreen ) ); Manifold::Render(); } void Step() override { b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB ); 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 ); class HullAndSphere : public Manifold { public: explicit HullAndSphere( SampleContext* sampleContext ) : Manifold( sampleContext ) { m_sphere = { { 0.0f, 0.0f, 0.0f }, 1.0f }; m_hull = b3MakeBoxHull( 2.0f, 0.5f, 0.5f ); m_transformA = { { 0.0f, 0.0f, 0.0f }, b3Quat_identity }; m_transformB = { { 1.5f, 0.0f, 0.0f }, b3Quat_identity }; } void Render() override { DrawHull( m_transformA, &m_hull.base, MakeColor( b3_colorCyan ) ); DrawSolidSphere( m_transformB, m_sphere, MakeColor( b3_colorGreen ) ); Manifold::Render(); } void Step() override { if ( m_useCache == false ) { m_simplexCache = {}; } b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB ); b3CollideHullAndSphere( &m_manifold, m_pointCapacity, &m_hull.base, &m_sphere, transformBtoA, &m_simplexCache ); } static Sample* Create( SampleContext* sampleContext ) { return new HullAndSphere( sampleContext ); } b3Sphere m_sphere; b3BoxHull m_hull; }; static int sampleSphereAndHull = RegisterSample( "Manifold", "Hull vs Sphere", HullAndSphere::Create ); class TriangleAndSphere : public TriangleManifold { public: static Sample* Create( SampleContext* sampleContext ) { return new TriangleAndSphere( sampleContext ); } explicit TriangleAndSphere( SampleContext* sampleContext ) : TriangleManifold( sampleContext ) { if ( m_context->restart == false ) { m_camera->SetView( 0.0f, 30.0f, 10.0f, b3Pos_zero ); } m_sphere = { { 0.0f, 0.0f, 0.0f }, 0.25f }; m_triangle[0] = { 0.0f, 0.0f, 0.0f }; m_triangle[1] = { 4.0f, 0.0f, 4.0f }; m_triangle[2] = { 4.0f, 0.0f, 0.0f }; // b3Quat qA = b3MakeQuatFromAxisAngle( { 0.0f, 1.0f, 0.0f }, 2.0f ); // m_transformA = { { 1.0f, 1.0f, 0.0f }, qA }; m_transformA = b3WorldTransform_identity; m_transformB = { { 2.0f, 0.5f, 1.0f }, b3Quat_identity }; } void Render() override { DrawSolidSphere( m_transformB, m_sphere, MakeColorAlpha( b3_colorGreen, 0.5f ) ); TriangleManifold::Render(); } void Step() override { // Convert triangle to frame B b3Transform xf = b3InvMulWorldTransforms( m_transformB, m_transformA ); b3Vec3 localTriangle[3] = { b3TransformPoint( xf, m_triangle[0] ), b3TransformPoint( xf, m_triangle[1] ), b3TransformPoint( xf, m_triangle[2] ), }; b3CollideTriangleAndSphere( &m_manifold, m_pointCapacity, localTriangle, &m_sphere ); } b3Sphere m_sphere; }; static int sampleSphereAndTriangle = RegisterSample( "Manifold", "Triangle vs Sphere", TriangleAndSphere::Create ); class CapsuleAndCapsule : public Manifold { public: explicit CapsuleAndCapsule( SampleContext* sampleContext ) : Manifold( sampleContext ) { m_capsule = { { -2.0f, 0.0f, 0.0f }, { 2.0f, 0.0f, 0.0f }, 1.0f }; m_transformA = { { 1.0f, 1.0f, 0.0f }, b3Quat_identity }; m_transformB = { { -4.0f, 1.0f, 0.0f }, b3Quat_identity }; } void Render() override { DrawSolidCapsule( m_transformA, m_capsule, MakeColor( b3_colorGreen ) ); DrawSolidCapsule( m_transformB, m_capsule, MakeColor( b3_colorCyan ) ); Manifold::Render(); } void Step() override { b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB ); b3CollideCapsules( &m_manifold, m_pointCapacity, &m_capsule, &m_capsule, transformBtoA ); } static Sample* Create( SampleContext* sampleContext ) { return new CapsuleAndCapsule( sampleContext ); } b3Capsule m_capsule; }; static int sampleCapsuleAndCapsule = RegisterSample( "Manifold", "Capsule vs Capsule", CapsuleAndCapsule::Create ); class CapsuleAndHull : public Manifold { public: explicit CapsuleAndHull( SampleContext* sampleContext ) : Manifold( sampleContext ) { if ( m_context->restart == false ) { m_camera->SetView( 0.0f, 30.0f, 5.0f, b3Pos_zero ); } m_capsule = { { -1.0f, 0.0f, 0.0f }, { 1.0f, 0.0f, 0.0f }, 0.15f }; m_hull = b3MakeBoxHull( 1.0f, 0.5f, 0.5f ); m_transformA = { { 0.0f, 0.0f, 0.0f }, b3Quat_identity }; m_transformB = { { 0.0f, 1.0f, 0.0f }, b3Quat_identity }; /* p [ 1.58523774, 0.729615569, 0.451690674 ] b3Vec3 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 }; // m_hull = b3CreateBox( { 0.5f, 1.0f, 1.5f } ); m_transformB = { { 1.58523774, 0.729615569, 0.451690674 }, { { -0.00256555085, -0.0201825816, 0.126076236 }, 0.991811991 } }; } void Render() override { DrawHull( m_transformA, &m_hull.base, MakeColor( b3_colorCyan ) ); DrawSolidCapsule( m_transformB, m_capsule, MakeColor( b3_colorGreen ) ); Manifold::Render(); } void Step() override { if ( m_useCache == false ) { m_simplexCache = {}; } b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB ); b3CollideHullAndCapsule( &m_manifold, m_pointCapacity, &m_hull.base, &m_capsule, transformBtoA, &m_simplexCache ); } static Sample* Create( SampleContext* sampleContext ) { return new CapsuleAndHull( sampleContext ); } b3BoxHull m_hull; b3Capsule m_capsule; }; static int sampleCapsuleAndHull = RegisterSample( "Manifold", "Capsule vs Hull", CapsuleAndHull::Create ); class TriangleAndCapsule : public TriangleManifold { public: static Sample* Create( SampleContext* sampleContext ) { return new TriangleAndCapsule( sampleContext ); } explicit TriangleAndCapsule( SampleContext* sampleContext ) : TriangleManifold( sampleContext ) { if ( m_context->restart == false ) { m_camera->SetView( 0.0f, 30.0f, 10.0f, b3Pos_zero ); } m_capsule = { { -0.5f, 0.0f, 0.0f }, { 0.5f, 0.0f, 0.0f }, 0.05f }; m_triangle[0] = { -4.0f, 0.0f, -4.0f }; m_triangle[1] = { -4.0f, 0.0f, 0.0f }; m_triangle[2] = { 0.0f, 0.0f, 0.0f }; m_transformA = b3WorldTransform_identity; m_transformB = { { -1.0f, 0.0f, -1.0f }, b3Quat_identity }; } void Render() override { DrawSolidCapsule( m_transformB, m_capsule, MakeColorAlpha( b3_colorGreen, 0.5f ) ); DrawAxes( m_transformB, 0.1f ); TriangleManifold::Render(); } void Step() override { if ( m_useCache == false ) { m_simplexCache = {}; } // Put triangle into capsule coordinates b3Transform xf = b3InvMulWorldTransforms( m_transformB, m_transformA ); b3Vec3 localTriangle[3] = { b3TransformPoint( xf, m_triangle[0] ), b3TransformPoint( xf, m_triangle[1] ), b3TransformPoint( xf, m_triangle[2] ), }; b3CollideTriangleAndCapsule( &m_manifold, m_pointCapacity, localTriangle, &m_capsule, &m_simplexCache ); } b3Capsule m_capsule; }; static int sampleCapsuleAndTriangle = RegisterSample( "Manifold", "Triangle vs Capsule", TriangleAndCapsule::Create ); class HullAndHull : public Manifold { public: explicit HullAndHull( SampleContext* context ) : Manifold( context ) { if ( m_context->restart == false ) { 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 } ); b3Transform transform = { { 1.0f, 0.5f, 0.0f }, b3Quat_identity }; m_boxA = b3MakeTransformedBoxHull( 0.5f, 1.0f, 1.0f, transform ); // m_hull = CreateConvex( 0.6f, 0.0f, 0.95f, 1.0f, context->arena ); m_hullA = &m_boxA.base; m_boxB = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); // m_hull = CreateConvex( 0.6f, 0.0f, 0.95f, 1.0f, context->arena ); 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 */ m_transformA = { { 0.0f, 0.0f, 0.0f }, b3Quat_identity }; m_transformB = { { 0.0f, 0.0f, 0.0f }, b3Quat_identity }; /* p [ 0.691772044, 0.951781273, 0.0741987228 ] b3Vec3 q [ [ -0.00162522844, -0.0456664972, 0.0355294086 ], 0.998323441 ] b3Quat */ // m_transformB = { { 0.691772044, 0.951781273, 0.0741987228 }, // { { -0.00162522844, -0.0456664972, 0.0355294086 }, 0.998323441 } }; m_satCache = {}; } ~HullAndHull() override { if ( m_hullA != &m_boxA.base ) { b3DestroyHull( m_hullA ); } if ( m_hullB != &m_boxB.base ) { b3DestroyHull( m_hullB ); } } b3HullData* CreateConvex( float radius1, float height1, float radius2, float height2 ) const { constexpr int sideCount = 32; const float deltaAlpha = 2.0f * B3_PI / sideCount; int vertexCount = 2 * sideCount; b3Vec3 vertexBase[2 * sideCount]; float alpha = 0.0f; for ( int sideIndex = 0; sideIndex < sideCount; ++sideIndex ) { b3CosSin cs = b3ComputeCosSin( alpha ); float x1 = radius1 * cs.cosine; float z1 = radius1 * cs.sine; float x2 = radius2 * cs.cosine; float z2 = radius2 * cs.sine; vertexBase[2 * sideIndex + 0] = { x1, height1, z1 }; vertexBase[2 * sideIndex + 1] = { x2, height2, z2 }; alpha += deltaAlpha; } return b3CreateHull( vertexBase, vertexCount, vertexCount ); } void Render() override { DrawHull( m_transformA, m_hullA, MakeColor( b3_colorGreen ) ); DrawHull( m_transformB, m_hullB, MakeColor( b3_colorCyan ) ); Manifold::Render(); } void Step() override { if ( m_useCache == true ) { if ( m_manualFeature == 1 ) { m_satCache.type = b3_manualFaceAxisA; } else if ( m_manualFeature == 2 ) { m_satCache.type = b3_manualFaceAxisB; } else if ( m_manualFeature == 3 ) { m_satCache.type = b3_manualEdgePairAxis; } } else { m_satCache = {}; } b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB ); b3CollideHulls( &m_manifold, m_pointCapacity, m_hullA, m_hullB, transformBtoA, &m_satCache ); DrawTextLine( "SAT type: %d", m_satCache.type ); } static Sample* Create( SampleContext* sampleContext ) { return new HullAndHull( sampleContext ); } b3BoxHull m_boxA; b3BoxHull m_boxB; b3HullData* m_hullA; b3HullData* m_hullB; }; 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. class HullCulling : public Manifold { public: enum FeatureState { e_unreached = 0, e_culled, e_tested, }; enum EdgeBound { e_noBound = 0, e_sphereBound, e_supportBound, e_bothBounds, }; // Mirrors B3_EDGE_PROBE_MIN_TESTS static constexpr int probeMinTests = 10; explicit HullCulling( SampleContext* context ) : Manifold( context ) { if ( m_context->restart == false ) { m_camera->SetView( 20.0f, 20.0f, 14.0f, { 0.0f, 1.8f, 0.0f } ); } m_useCylinders = false; m_cylinderHeight = 2.0f; m_cylinderRadius = 2.0f; m_cylinderSides = 32; m_hullA = nullptr; m_hullB = nullptr; 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_showSpheres = true; m_showCulled = true; m_showNormals = true; m_edgeBound = e_bothBounds; ResetCounts(); } ~HullCulling() override { b3DestroyHull( m_hullA ); b3DestroyHull( m_hullB ); } void CreateHulls() { if ( m_hullA != nullptr ) { b3DestroyHull( m_hullA ); b3DestroyHull( m_hullB ); } 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 ); } else { m_hullA = b3CreateComplexHull( 2.0f ); m_hullB = b3CreateComplexHull( 2.0f ); } } bool DrawControls() override { Manifold::DrawControls(); ImGui::Checkbox( "Inscribed spheres", &m_showSpheres ); ImGui::Checkbox( "Culled features", &m_showCulled ); 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::Combo( "Edge bound", &m_edgeBound, edgeBounds, IM_ARRAYSIZE( edgeBounds ) ); bool rebuild = ImGui::Checkbox( "Cylinders", &m_useCylinders ); if ( m_useCylinders ) { rebuild |= ImGui::SliderFloat( "Height", &m_cylinderHeight, 0.1f, 8.0f, "%.2f" ); rebuild |= ImGui::SliderFloat( "Radius", &m_cylinderRadius, 0.1f, 4.0f, "%.2f" ); rebuild |= ImGui::SliderInt( "Sides", &m_cylinderSides, 3, 32 ); } if ( rebuild ) { m_cylinderHeight = b3ClampFloat( m_cylinderHeight, 0.1f, 8.0f ); m_cylinderRadius = b3ClampFloat( m_cylinderRadius, 0.1f, 4.0f ); m_cylinderSides = b3ClampInt( m_cylinderSides, 3, 32 ); CreateHulls(); } return true; } void ResetCounts() { memset( m_faceStateA, 0, sizeof( m_faceStateA ) ); memset( m_faceStateB, 0, sizeof( m_faceStateB ) ); memset( m_edgeStateA, 0, sizeof( m_edgeStateA ) ); memset( m_edgeStateB, 0, sizeof( m_edgeStateB ) ); m_seedA = 0; m_seedB = 0; m_probeVertexA = 0; m_probeVertexB = 0; m_keptEdgeCountA = 0; m_keptEdgeCountB = 0; m_sphereKeptEdgeCountA = 0; m_sphereKeptEdgeCountB = 0; m_separationA = -FLT_MAX; m_separationB = -FLT_MAX; m_gap = 0.0f; m_probeApplied = false; 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, int* vertexIndex = nullptr ) { b3Plane plane = b3GetHullPlanes( hullA )[faceIndex]; const b3Vec3* points = b3GetHullPoints( hullB ); float separation = FLT_MAX; int bestIndex = 0; for ( int i = 0; i < hullB->vertexCount; ++i ) { b3Vec3 point = b3TransformPoint( transformBtoA, points[i] ); float s = b3Dot( plane.normal, point ) - plane.offset; if ( s < separation ) { separation = s; bestIndex = i; } } if ( vertexIndex != nullptr ) { *vertexIndex = bestIndex; } 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, int* vertexIndex = nullptr ) { b3Plane plane = b3GetHullPlanes( hullB )[faceIndex]; const b3Vec3* points = b3GetHullPoints( hullA ); float separation = FLT_MAX; int bestIndex = 0; for ( int i = 0; i < hullA->vertexCount; ++i ) { b3Vec3 point = b3InvTransformPoint( transformBtoA, points[i] ); float s = b3Dot( plane.normal, point ) - plane.offset; if ( s < separation ) { separation = s; bestIndex = i; } } if ( vertexIndex != nullptr ) { *vertexIndex = bestIndex; } return separation; } // Mirrors b3TestEdgeCandidateSorted static bool ArcCanReachSorted( float a, float b, float c, float bound ) { const float parallelTolerance = 1.0e-4f; float hi = b3MaxFloat( a, b ); float lo = b3MinFloat( a, b ); 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 ); return exterior || interior; } // Arc test of every surviving edge against the plane separations of a point on the other hull. // Mirrors the gather loops and b3FilterEdgeCandidates, edges that fail are culled. static int CullEdges( const b3HullData* hull, b3Vec3 point, float bound, uint8_t* states ) { const b3HullHalfEdge* edges = b3GetHullEdges( hull ); const b3Plane* planes = b3GetHullPlanes( hull ); const float* cosines = b3GetHullEdgeCosines( hull ); int keptCount = 0; for ( int i = 0; i < hull->edgeCount; i += 2 ) { if ( states[i / 2] == e_culled ) { continue; } b3Plane plane1 = planes[edges[i].face]; b3Plane plane2 = planes[edges[i + 1].face]; float d1 = b3Dot( plane1.normal, point ) - plane1.offset; float d2 = b3Dot( plane2.normal, point ) - plane2.offset; bool kept = ArcCanReachSorted( d1, d2, cosines[i >> 1], bound ); states[i / 2] = kept ? e_tested : e_culled; keptCount += kept ? 1 : 0; } return keptCount; } // Mirrors the culling in b3ComputeSeparatingAxis with early return enabled void ComputeCulling( b3Transform transformBtoA ) { ResetCounts(); const b3HullData* hullA = m_hullA; const b3HullData* hullB = m_hullB; const b3Plane* planesA = b3GetHullPlanes( hullA ); const b3Plane* planesB = b3GetHullPlanes( hullB ); float speculativeDistance = B3_SPECULATIVE_DISTANCE; 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 ) ); m_gap = centerDistance - radius; float dotA[B3_MAX_HULL_FACES]; for ( int i = 0; i < hullA->faceCount; ++i ) { dotA[i] = b3Dot( planesA[i].normal, deltaCenter ); m_seedA = dotA[i] > dotA[m_seedA] ? i : m_seedA; } // 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 floorA = b3MinFloat( seedSeparationA, speculativeDistance ); for ( int i = 0; i < hullA->faceCount; ++i ) { if ( dotA[i] - radiusBound < b3MaxFloat( floorA, m_separationA ) ) { m_faceStateA[i] = e_culled; continue; } m_faceStateA[i] = e_tested; int vertexIndex; float separation = FaceSeparationA( hullA, hullB, transformBtoA, i, &vertexIndex ); if ( separation > m_separationA ) { m_separationA = separation; m_probeVertexB = vertexIndex; if ( separation > speculativeDistance ) { m_separatedFeature = b3_faceAxisA; return; } } } b3Vec3 deltaCenterB = b3Neg( b3InvRotateVector( transformBtoA.q, deltaCenter ) ); float dotB[B3_MAX_HULL_FACES]; for ( int i = 0; i < hullB->faceCount; ++i ) { dotB[i] = b3Dot( planesB[i].normal, deltaCenterB ); m_seedB = dotB[i] > dotB[m_seedB] ? i : m_seedB; } 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 ) { if ( dotB[i] - radiusBound < b3MaxFloat( floorB, m_separationB ) ) { m_faceStateB[i] = e_culled; continue; } m_faceStateB[i] = e_tested; int vertexIndex; float separation = FaceSeparationB( hullA, hullB, transformBtoA, i, &vertexIndex ); if ( separation > m_separationB ) { m_separationB = separation; m_probeVertexA = vertexIndex; if ( separation > speculativeDistance ) { m_separatedFeature = b3_faceAxisB; return; } } } bool useSphere = m_edgeBound == e_sphereBound || m_edgeBound == e_bothBounds; bool useSupport = m_edgeBound == e_supportBound || m_edgeBound == e_bothBounds; float maxFaceSeparation = b3MaxFloat( m_separationA, m_separationB ); if ( useSphere ) { // 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; b3Vec3 centerBinA = b3TransformPoint( transformBtoA, hullB->center ); b3Vec3 centerAinB = b3InvTransformPoint( transformBtoA, hullA->center ); m_keptEdgeCountA = CullEdges( hullA, centerBinA, thresholdA, m_edgeStateA ); m_keptEdgeCountB = CullEdges( hullB, centerAinB, thresholdB, m_edgeStateB ); } else { m_keptEdgeCountA = hullA->edgeCount / 2; m_keptEdgeCountB = hullB->edgeCount / 2; memset( m_edgeStateA, e_tested, m_keptEdgeCountA ); memset( m_edgeStateB, e_tested, m_keptEdgeCountB ); } m_sphereKeptEdgeCountA = m_keptEdgeCountA; m_sphereKeptEdgeCountB = m_keptEdgeCountB; // The probe is gated on the edge pair work in SIMD groups of A edges int testCount = m_keptEdgeCountB * ( ( m_keptEdgeCountA + 3 ) >> 2 ); 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] ); m_keptEdgeCountA = CullEdges( hullA, probeB, probeBound, m_edgeStateA ); m_keptEdgeCountB = CullEdges( hullB, probeA, probeBound, m_edgeStateB ); } } void Step() override { // 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 ); ComputeCulling( transformBtoA ); } static int CountState( const uint8_t* states, int count, FeatureState state ) { int n = 0; for ( int i = 0; i < count; ++i ) { n += states[i] == state ? 1 : 0; } return n; } void DrawEdges( b3WorldTransform transform, const b3HullData* hull, const uint8_t* states, Vec4 keptColor ) { const b3HullHalfEdge* edges = b3GetHullEdges( hull ); const b3Vec3* points = b3GetHullPoints( hull ); for ( int i = 0; i < hull->edgeCount; i += 2 ) { b3Pos p1 = b3TransformWorldPoint( transform, points[edges[i].origin] ); b3Pos p2 = b3TransformWorldPoint( transform, points[edges[i + 1].origin] ); uint8_t state = states[i / 2]; if ( state == e_tested ) { DrawLineEx( p1, p2, keptColor, 4.0f, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DIM ); } else if ( state == e_culled && m_showCulled ) { DrawLine( p1, p2, MakeColor( b3_colorSlateGray ) ); } else if ( state == e_unreached ) { 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 ) { const b3HullFace* faces = b3GetHullFaces( hull ); const b3HullHalfEdge* edges = b3GetHullEdges( hull ); const b3Plane* planes = b3GetHullPlanes( hull ); const b3Vec3* points = b3GetHullPoints( hull ); float length = 0.4f * b3GetLengthUnitsPerMeter(); for ( int i = 0; i < hull->faceCount; ++i ) { uint8_t state = states[i]; if ( state == e_unreached || ( state == e_culled && m_showCulled == false ) ) { continue; } b3Vec3 centroid = b3Vec3_zero; int count = 0; int edgeIndex = faces[i].edge; do { centroid = b3Add( centroid, points[edges[edgeIndex].origin] ); count += 1; edgeIndex = edges[edgeIndex].next; } while ( edgeIndex != faces[i].edge ); centroid = b3MulSV( 1.0f / (float)count, centroid ); b3Pos p1 = b3TransformWorldPoint( transform, centroid ); 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 ); } else if ( state == e_tested ) { DrawArrowEx( p1, p2, testedColor, 2.0f, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DIM, 0.25f ); } else { DrawLine( p1, p2, MakeColor( b3_colorSlateGray ) ); } } } void Render() override { const b3HullData* hullA = m_hullA; const b3HullData* hullB = m_hullB; DrawEdges( m_transformA, hullA, m_edgeStateA, MakeColor( b3_colorOrange ) ); DrawEdges( m_transformB, hullB, m_edgeStateB, MakeColor( b3_colorDeepSkyBlue ) ); 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 ) ); } b3Pos centerA = b3TransformWorldPoint( m_transformA, hullA->center ); b3Pos centerB = b3TransformWorldPoint( m_transformB, hullB->center ); DrawLine( centerA, centerB, MakeColor( b3_colorWhite ) ); if ( m_showSpheres ) { b3Sphere sphereA = { hullA->center, hullA->innerRadius }; b3Sphere sphereB = { hullB->center, hullB->innerRadius }; DrawWireSphere( m_transformA, &sphereA, 32, MakeColor( b3_colorOrange ) ); DrawWireSphere( m_transformB, &sphereB, 32, MakeColor( b3_colorDeepSkyBlue ) ); } if ( m_probeApplied ) { 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 ) ); } int faceCountA = hullA->faceCount; int faceCountB = hullB->faceCount; int edgeCountA = hullA->edgeCount / 2; int edgeCountB = hullB->edgeCount / 2; int testedA = CountState( m_faceStateA, faceCountA, e_tested ); int testedB = CountState( m_faceStateB, faceCountB, e_tested ); int culledA = CountState( m_faceStateA, faceCountA, e_culled ); int culledB = CountState( m_faceStateB, faceCountB, e_culled ); 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( "|d| - rA - rB = %.3f", m_gap ); DrawTextLine( "faces A: tested %d, culled %d of %d, best separation %.4f", testedA, culledA, faceCountA, m_separationA ); if ( m_separatedFeature == b3_faceAxisA ) { DrawTextLine( "separated by face A, B faces and edges not reached" ); } else { DrawTextLine( "faces B: tested %d, culled %d of %d, best separation %.4f", testedB, culledB, faceCountB, m_separationB ); } if ( m_separatedFeature == b3_faceAxisB ) { DrawTextLine( "separated by face B, edges not reached" ); } 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 ); 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 ); } if ( m_probeApplied ) { DrawTextLine( "magenta points are the support probes" ); } else if ( m_edgeBound == e_supportBound || m_edgeBound == e_bothBounds ) { DrawTextLine( "probe skipped, fewer than %d edge tests", probeMinTests ); } } DrawTextLine( "SAT type: %d", m_satCache.type ); Manifold::Render(); } static Sample* Create( SampleContext* context ) { return new HullCulling( context ); } b3HullData* m_hullA; b3HullData* m_hullB; uint8_t m_faceStateA[B3_MAX_HULL_FACES]; uint8_t m_faceStateB[B3_MAX_HULL_FACES]; uint8_t m_edgeStateA[B3_MAX_HULL_EDGES]; uint8_t m_edgeStateB[B3_MAX_HULL_EDGES]; 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 int m_probeVertexA; int m_probeVertexB; int m_keptEdgeCountA; int m_keptEdgeCountB; int m_sphereKeptEdgeCountA; int m_sphereKeptEdgeCountB; float m_separationA; float m_separationB; float m_gap; b3SeparatingFeature m_separatedFeature; float m_cylinderHeight; float m_cylinderRadius; int m_cylinderSides; bool m_useCylinders; bool m_showSpheres; bool m_showCulled; bool m_showNormals; int m_edgeBound; bool m_probeApplied; }; 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. class EdgeAxisWedge : public Manifold { public: struct EdgeAxis { b3Vec3 normal; float separation; int edgeA; int edgeB; }; // Gauss map arc of one edge in frame A, with the peak of dot(n, d) on its great circle struct Arc { b3Vec3 u; b3Vec3 v; b3Vec3 peak; float a; float b; float c; float separationU; float separationV; // Angle from the peak to the nearest end of the arc, negative when the peak is on the arc float outsideDegrees; bool inside; }; explicit EdgeAxisWedge( SampleContext* context ) : Manifold( context ) { if ( m_context->restart == false ) { m_camera->SetView( 0.0f, 15.0f, 22.0f, { -4.5f, 1.0f, 0.0f } ); } m_transformA = { { 0.0f, 0.0f, 0.0f }, b3Quat_identity }; m_transformB = { { 0.0f, 3.6f, 0.0f }, b3Quat_identity }; m_gaussCenter = { -10.0f, 1.0f, 0.0f }; m_gaussRadius = 2.0f; m_searchCount = 0; m_searchFound = false; m_hullType = e_slab; m_createdHull = nullptr; // Start with an example SetHull( m_hullType ); } ~EdgeAxisWedge() override { if ( m_createdHull != nullptr ) { b3DestroyHull( m_createdHull ); } } enum HullType { e_slab = 0, e_tetrahedron, e_complex, }; // Both hulls are the same shape void SetHull( int type ) { if ( m_createdHull != nullptr ) { b3DestroyHull( m_createdHull ); m_createdHull = nullptr; } if ( type == e_slab ) { m_box = b3MakeBoxHull( 2.0f, 0.25f, 1.0f ); m_hullA = &m_box.base; } 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 } }; m_createdHull = b3CreateHull( points, 4, 4 ); m_hullA = m_createdHull; } else { m_createdHull = b3CreateComplexHull( 2.0f ); m_hullA = m_createdHull; } m_hullB = m_hullA; m_hullType = type; m_seed = 12345; Search(); } float RandomFloat() { m_seed = m_seed * 1664525u + 1013904223u; return (float)( m_seed >> 8 ) / 16777216.0f; } b3Vec3 RandomDirection() { b3Vec3 v; float lengthSquared; do { 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 ); 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 ) { const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA ); const b3Plane* planesA = b3GetHullPlanes( hullA ); const b3Vec3* pointsA = b3GetHullPoints( hullA ); const b3HullHalfEdge* edgesB = b3GetHullEdges( hullB ); const b3Plane* planesB = b3GetHullPlanes( hullB ); const b3Vec3* pointsB = b3GetHullPoints( hullB ); const float eps = -0.0001f; float squaredTolerance = B3_PARALLEL_EDGE_TOL * B3_PARALLEL_EDGE_TOL; EdgeAxis best = { b3Vec3_zero, -FLT_MAX, B3_NULL_INDEX, B3_NULL_INDEX }; for ( int j = 0; j < hullB->edgeCount; j += 2 ) { // 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 bv0 = b3Neg( b3TransformPoint( transformBtoA, pointsB[edgesB[j].origin] ) ); b3Vec3 bv1 = b3Neg( b3TransformPoint( transformBtoA, pointsB[edgesB[j + 1].origin] ) ); b3Vec3 DC = b3Sub( bv1, bv0 ); for ( int i = 0; i < hullA->edgeCount; i += 2 ) { b3Vec3 n0 = planesA[edgesA[i].face].normal; b3Vec3 n1 = planesA[edgesA[i + 1].face].normal; b3Vec3 av0 = pointsA[edgesA[i].origin]; b3Vec3 edge = b3Sub( pointsA[edgesA[i + 1].origin], av0 ); float CBA = b3Dot( C, edge ); float DBA = b3Dot( D, edge ); float ADC = b3Dot( n0, DC ); float BDC = b3Dot( n1, DC ); if ( CBA * DBA >= eps || ADC * BDC >= eps || CBA * BDC >= eps ) { continue; } if ( b3MaxFloat( CBA * CBA, DBA * DBA ) <= squaredTolerance * b3Dot( edge, edge ) ) { continue; } float t = -CBA / ( DBA - CBA ); b3Vec3 normal = b3Normalize( b3MulAdd( C, t, b3Sub( D, C ) ) ); float separation = -b3Dot( normal, b3Add( av0, bv0 ) ); if ( separation > best.separation ) { best = { normal, separation, i, j }; } } } return best; } // The peak of dot(n, d) over the great circle through u and v, and whether it lies on the arc static Arc MakeArc( b3Vec3 u, b3Vec3 v, b3Vec3 d, float separationU, float separationV ) { Arc arc; arc.u = u; arc.v = v; arc.a = b3Dot( u, d ); arc.b = b3Dot( v, d ); arc.c = b3Dot( u, v ); arc.inside = arc.a >= arc.c * arc.b && arc.b >= arc.c * arc.a; arc.separationU = separationU; arc.separationV = separationV; b3Vec3 axis = b3Normalize( b3Cross( u, v ) ); b3Vec3 inPlane = b3MulSub( d, b3Dot( d, axis ), axis ); arc.peak = b3LengthSquared( inPlane ) > 1.0e-12f ? b3Normalize( inPlane ) : u; float arcAngle = acosf( b3ClampFloat( arc.c, -1.0f, 1.0f ) ); float angleU = acosf( b3ClampFloat( b3Dot( u, arc.peak ), -1.0f, 1.0f ) ); float angleV = acosf( b3ClampFloat( b3Dot( v, arc.peak ), -1.0f, 1.0f ) ); float nearest = b3MinFloat( angleU, angleV ) * 180.0f / B3_PI; arc.outsideDegrees = ( angleU <= arcAngle && angleV <= arcAngle ) ? -nearest : nearest; return arc; } 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 ) ); } float separationB = -FLT_MAX; for ( int i = 0; i < m_hullB->faceCount; ++i ) { separationB = b3MaxFloat( separationB, HullCulling::FaceSeparationB( m_hullA, m_hullB, transformBtoA, i ) ); } EdgeAxis edge = FindBestEdgeAxis( m_hullA, m_hullB, transformBtoA ); if ( faceSeparationA != nullptr ) { *faceSeparationA = separationA; *faceSeparationB = separationB; *edgeAxis = edge; } return b3MaxFloat( edge.separation, b3MaxFloat( separationA, separationB ) ); } // Random poses placed just into contact until an edge axis wins with an edge outside its wedge void Search() { m_searchFound = false; m_searchCount = 0; for ( int trial = 0; trial < 500 && m_searchFound == false; ++trial ) { m_searchCount += 1; b3Vec3 direction = RandomDirection(); b3Vec3 axis = RandomDirection(); float angle = 2.0f * B3_PI * RandomFloat(); b3Quat rotation = b3MakeQuatFromAxisAngle( axis, angle ); // 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 ) ) ); for ( int iteration = 0; iteration < 24; ++iteration ) { float mid = 0.5f * ( lower + upper ); b3Transform transform = { b3MulSV( mid, direction ), rotation }; if ( MaxSeparation( transform, nullptr, nullptr, nullptr ) < target ) { lower = mid; } else { upper = mid; } } m_transformB = { m_transformA.p + b3MulSV( upper, direction ), rotation }; Compute(); // 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(); } } void Compute() { 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_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 ) { return; } const b3HullHalfEdge* edgesA = b3GetHullEdges( m_hullA ); const b3Plane* planesA = b3GetHullPlanes( m_hullA ); int faceA1 = edgesA[m_edge.edgeA].face; int faceA2 = edgesA[m_edge.edgeA + 1].face; m_arcA = MakeArc( planesA[faceA1].normal, planesA[faceA2].normal, m_centerOffset, HullCulling::FaceSeparationA( m_hullA, m_hullB, transformBtoA, faceA1 ), HullCulling::FaceSeparationA( m_hullA, m_hullB, transformBtoA, faceA2 ) ); const b3HullHalfEdge* edgesB = b3GetHullEdges( m_hullB ); const b3Plane* planesB = b3GetHullPlanes( m_hullB ); int faceB1 = edgesB[m_edge.edgeB].face; 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 ), HullCulling::FaceSeparationB( m_hullA, m_hullB, transformBtoA, faceB2 ) ); } bool DrawControls() override { const char* hullNames[] = { "Slab", "Tetrahedron", "Complex Hull" }; int hullType = m_hullType; if ( ImGui::Combo( "Hull", &hullType, hullNames, IM_ARRAYSIZE( hullNames ) ) ) { SetHull( hullType ); } if ( ImGui::Button( "Find Example" ) ) { Search(); } DrawGizmoControls( &m_showGizmo, &m_gizmoLocal ); return true; } void Step() override { Compute(); } b3Pos GaussPoint( b3Vec3 directionInA ) const { return m_gaussCenter + b3MulSV( m_gaussRadius, b3RotateVector( m_transformA.q, directionInA ) ); } 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 ) ) ) ); DrawLineEx( previous, point, color, thickness, OVERLAY_THICKNESS_PIXELS, mode ); previous = point; } } // The full great circle through the arc, dashed void DrawGreatCircle( b3Vec3 u, b3Vec3 v, Vec4 color ) const { b3Vec3 axis = b3Normalize( b3Cross( u, v ) ); b3Vec3 w = b3Cross( axis, u ); constexpr int segmentCount = 96; b3CosSin cs = b3ComputeCosSin( 2.0f * B3_PI / segmentCount ); float x1 = 1.0f, y1 = 0.0f; b3Pos previous = GaussPoint( u ); for ( int i = 0; i < segmentCount; ++i ) { 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 ); x1 = x2; y1 = y2; previous = point; } } void DrawEdge( b3WorldTransform transform, const b3HullData* hull, int edgeIndex, Vec4 color ) const { const b3HullHalfEdge* edges = b3GetHullEdges( hull ); const b3Vec3* points = b3GetHullPoints( hull ); b3Pos p1 = b3TransformWorldPoint( transform, points[edges[edgeIndex].origin] ); b3Pos p2 = b3TransformWorldPoint( transform, points[edges[edgeIndex + 1].origin] ); DrawLineEx( p1, p2, color, 5.0f, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DIM ); } void DrawArcText( const char* name, const Arc& arc, float edgeBound ) { if ( arc.inside ) { DrawTextLine( "%s edge: d peak inside wedge", name ); } else { 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 ); } void Render() override { DrawHull( m_transformA, m_hullA, MakeColor( b3_colorSlateGray ) ); DrawHull( m_transformB, m_hullB, MakeColor( b3_colorSlateGray ) ); b3Pos centerA = b3TransformWorldPoint( m_transformA, m_hullA->center ); 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" ); if ( m_edge.edgeA == B3_NULL_INDEX ) { Manifold::Render(); return; } Vec4 colorA = MakeColor( b3_colorOrange ); Vec4 colorB = MakeColor( b3_colorDeepSkyBlue ); Vec4 colorN = MakeColor( b3_colorGold ); Vec4 colorPeak = MakeColor( b3_colorMagenta ); DrawEdge( m_transformA, m_hullA, m_edge.edgeA, colorA ); DrawEdge( m_transformB, m_hullB, m_edge.edgeB, colorB ); // Winning axis from the midpoint of edge A { 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 ); 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 ); } // 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 ) ); DrawGreatCircle( m_arcA.u, m_arcA.v, colorA ); DrawGreatCircle( m_arcB.u, m_arcB.v, colorB ); DrawGreatArc( m_arcA.u, m_arcA.v, colorA, 5.0f, OVERLAY_OCCLUSION_DIM ); DrawGreatArc( m_arcB.u, m_arcB.v, colorB, 5.0f, OVERLAY_OCCLUSION_DIM ); DrawPoint( GaussPoint( m_arcA.u ), 8.0f, colorA ); DrawPoint( GaussPoint( m_arcA.v ), 8.0f, colorA ); DrawPoint( GaussPoint( m_arcB.u ), 8.0f, colorB ); DrawPoint( GaussPoint( m_arcB.v ), 8.0f, colorB ); DrawString3D( GaussPoint( m_arcA.u ), colorA, " uA" ); DrawString3D( GaussPoint( m_arcA.v ), colorA, " vA" ); DrawString3D( GaussPoint( m_arcB.u ), colorB, " uB" ); DrawString3D( GaussPoint( m_arcB.v ), colorB, " vB" ); DrawPoint( GaussPoint( m_arcA.peak ), 10.0f, colorPeak ); DrawString3D( GaussPoint( m_arcA.peak ), colorPeak, " peak A" ); DrawPoint( GaussPoint( m_arcB.peak ), 10.0f, colorPeak ); DrawString3D( GaussPoint( m_arcB.peak ), colorPeak, " peak B" ); DrawPoint( GaussPoint( m_edge.normal ), 12.0f, colorN ); DrawString3D( GaussPoint( m_edge.normal ), colorN, " n" ); b3Vec3 dHat = b3Normalize( m_centerOffset ); DrawPoint( GaussPoint( dHat ), 10.0f, MakeColor( b3_colorWhite ) ); DrawString3D( GaussPoint( dHat ), MakeColor( b3_colorWhite ), " d" ); float edgeBound = b3Dot( m_edge.normal, m_centerOffset ) - m_radius; DrawTextLine( "|d| - rA - rB = %.3f", b3Length( m_centerOffset ) - m_radius ); DrawArcText( "A", m_arcA, edgeBound ); DrawArcText( "B", m_arcB, edgeBound ); Manifold::Render(); } static Sample* Create( SampleContext* context ) { return new EdgeAxisWedge( context ); } b3HullData* m_hullA; b3HullData* m_hullB; EdgeAxis m_edge; Arc m_arcA; Arc m_arcB; b3Vec3 m_centerOffset; b3Pos m_gaussCenter; float m_gaussRadius; float m_radius; float m_faceSeparationA; float m_faceSeparationB; uint32_t m_seed; int m_searchCount; bool m_searchFound; bool m_edgeWins; b3BoxHull m_box; b3HullData* m_createdHull; int m_hullType; }; static int sampleEdgeAxisWedge = RegisterSample( "Manifold", "Edge Axis Wedge", EdgeAxisWedge::Create ); class TriangleAndHull : public TriangleManifold { public: static Sample* Create( SampleContext* sampleContext ) { return new TriangleAndHull( sampleContext ); } explicit TriangleAndHull( SampleContext* sampleContext ) : TriangleManifold( sampleContext ) { if ( m_context->restart == false ) { m_camera->SetView( 0.0f, 30.0f, 3.0f, b3Pos_zero ); } // m_triangle[0] = { 1.00000000, 0, 1.00000000 }; // m_triangle[1] = { 1.00000000, 0, 0.00000000 }; // m_triangle[2] = { 0.00000000, 0, 0.00000000 }; m_triangle[0] = { 0.299769998f, -1.01549578f, -0.744717002f }; m_triangle[1] = { 0.299769998f, -1.01549578f, 1.28728306f }; m_triangle[2] = { 0.299769998f, -0.913895786f, 0.271283031f }; float bodyHalfWidth = 0.304800004f; float bodyHalfHeight = 0.914399981f; m_boxHull = b3MakeBoxHull( bodyHalfWidth, bodyHalfHeight, bodyHalfWidth ); m_transformA = b3WorldTransform_identity; m_transformB = b3WorldTransform_identity; // m_transformB.p = { -2.16650009f, 0.912535489f, 0.00000000f }; // b3MeshEdgeFlags m_flags = 0; /* * separation -0.0415397286 float type 2 '\x2' unsigned char indexA 0 '\0' unsigned char indexB 1 '\x1' unsigned char hit 1 '\x1' unsigned char */ m_satCache = { .separation = -0.0107582733f, .type = 1, .indexA = 0, .indexB = 4, }; m_satCache = {}; m_useCache = false; m_cylinder = b3CreateCylinder( 0.4f, 0.05f, 0.0f, 6 ); m_hull = &m_boxHull.base; } ~TriangleAndHull() override { b3DestroyHull( m_cylinder ); } void Render() override { DrawHull( m_transformB, m_hull, MakeColor( b3_colorGreen ) ); b3WorldTransform xf; xf.p = b3TransformWorldPoint( m_transformB, m_hull->center ); xf.q = m_transformB.q; DrawAxes( xf, 0.1f ); TriangleManifold::Render(); } void Step() override { if ( m_useCache == true ) { if ( m_manualFeature == 1 ) { m_satCache.type = b3_manualFaceAxisA; } else if ( m_manualFeature == 2 ) { m_satCache.type = b3_manualFaceAxisB; } else if ( m_manualFeature == 3 ) { m_satCache.type = b3_manualEdgePairAxis; } } else { m_satCache = {}; } b3Transform xf = b3InvMulWorldTransforms( m_transformB, m_transformA ); b3Vec3 localTriangle[3] = { b3TransformPoint( xf, m_triangle[0] ), b3TransformPoint( xf, m_triangle[1] ), b3TransformPoint( xf, m_triangle[2] ), }; b3CollideTriangleAndHull( &m_manifold, m_pointCapacity, localTriangle[0], localTriangle[1], localTriangle[2], m_flags, m_hull, &m_satCache, true ); } int m_flags; b3BoxHull m_boxHull; b3HullData* m_cylinder; b3HullData* m_hull; }; static int sampleHullAndTriangle = RegisterSample( "Manifold", "Triangle vs Hull", TriangleAndHull::Create ); // 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 );