From 23861418d877fdd97990f35645e845e44a7eb9ec Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Mon, 20 Jul 2026 22:05:11 -0700 Subject: [PATCH] Fixes 07 (#97) #84 #82 Added B3_RESTITUTION_ITERATIONS to override restitution iterations. Fix regression in hull stacking stability. --- CMakeLists.txt | 10 +- docs/collision.md | 10 +- include/box3d/collision.h | 21 +- include/box3d/config.h | 2 + include/box3d/constants.h | 6 + samples/mover.cpp | 4 +- samples/sample_continuous.cpp | 30 ++- samples/sample_determinism.cpp | 2 +- samples/sample_issues.cpp | 157 +++++++++++++ samples/sample_manifold.cpp | 8 +- samples/sample_ragdoll.cpp | 10 +- samples/sample_shapes.cpp | 2 +- samples/sample_stacking.cpp | 160 ++++++++++--- shared/utils.c | 3 +- src/CMakeLists.txt | 3 +- src/contact_solver.c | 6 +- src/convex_manifold.c | 399 ++++++++++++++++++--------------- src/core.h | 5 + src/manifold.c | 2 + src/manifold.h | 51 +++-- src/mesh_contact.c | 6 +- src/solver.c | 11 +- src/timer.c | 3 +- src/triangle_manifold.c | 242 +++++++++++--------- test/test_determinism.c | 8 +- test/test_manifold.c | 145 +++++++----- test/test_sat.c | 21 +- 27 files changed, 878 insertions(+), 449 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 69a167d..2c4d9aa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -84,6 +84,14 @@ option(BOX3D_DISABLE_SIMD "Disable SIMD math (slower)" OFF) option(BOX3D_COMPILE_WARNING_AS_ERROR "Compile warnings as errors" OFF) option(BOX3D_DOUBLE_PRECISION "Enable double precision for large worlds" OFF) +# These are read by the library build, so they must precede add_subdirectory(src), +# otherwise the first configure and a re-configure produce different libraries. +# Top level only, so they stay hidden from FetchContent consumers. +if(PROJECT_IS_TOP_LEVEL) + option(BOX3D_PROFILE "Enable profiling with Tracy" OFF) + option(BOX3D_VALIDATE "Enable heavy validation" ON) +endif() + # Emscripten maps the SSE2 path onto wasm SIMD128. simd.h is a header included by # non-library targets like the tests, so these flags must be directory-wide, not # PRIVATE to the box3d library. @@ -127,8 +135,6 @@ if(PROJECT_IS_TOP_LEVEL) option(BOX3D_SAMPLES "Build the Box3D samples" ON) option(BOX3D_BENCHMARKS "Build the Box3D benchmarks" OFF) option(BOX3D_DOCS "Build the Box3D documentation" OFF) - option(BOX3D_PROFILE "Enable profiling with Tracy" OFF) - option(BOX3D_VALIDATE "Enable heavy validation" ON) option(BOX3D_UNIT_TESTS "Build the Box3D unit tests" ON) # Enable this for shader development using sokol-shdc. diff --git a/docs/collision.md b/docs/collision.md index 91ee199..07dbef1 100644 --- a/docs/collision.md +++ b/docs/collision.md @@ -1,6 +1,7 @@ # Collision Box3D provides geometric types and functions. These include: + - primitives: spheres, capsules, and convex hulls - triangle meshes and height fields for static terrain - convex hull construction from point clouds @@ -24,6 +25,7 @@ physics simulation. At a minimum, you should understand how to create primitives that can later be attached to rigid bodies. Box3D shape primitives support several operations: + - Test a point or proxy for overlap with the primitive - Perform a ray cast against the primitive - Compute the primitive's bounding box @@ -381,6 +383,7 @@ this for stable stacking. Normally you do not compute manifolds directly — you use the contact data returned by the simulation. The `b3Manifold` struct contains: + - `normal`: unit normal from shape A toward shape B - `points[B3_MAX_MANIFOLD_POINTS]`: contact points with separation, impulses, and feature ids @@ -401,15 +404,16 @@ b3CollideHulls(&manifold, B3_MAX_MANIFOLD_POINTS, ``` Available collide functions: + - `b3CollideSpheres` - `b3CollideCapsuleAndSphere` - `b3CollideCapsules` - `b3CollideHullAndSphere` - `b3CollideHullAndCapsule` - `b3CollideHulls` -- `b3CollideCapsuleAndTriangle` -- `b3CollideHullAndTriangle` -- `b3CollideSphereAndTriangle` +- `b3CollideTriangleAndCapsule` +- `b3CollideTriangleAndHull` +- `b3CollideTriangleAndSphere` The SAT cache in `b3CollideHulls` warm-starts the separating axis search between frames, the same idea as the simplex cache for GJK distance. diff --git a/include/box3d/collision.h b/include/box3d/collision.h index 3854cb2..809afb9 100644 --- a/include/box3d/collision.h +++ b/include/box3d/collision.h @@ -44,7 +44,8 @@ B3_API uint64_t b3DynamicTree_GetCategoryBits( b3DynamicTree* tree, int proxyId B3_API b3TreeStats b3DynamicTree_Query( const b3DynamicTree* tree, b3AABB aabb, uint64_t maskBits, bool requireAllBits, b3TreeQueryCallbackFcn* callback, void* context ); -/// Query an AABB for the closest object. The callback function is called for each proxy that might be closest to the supplied point. +/// Query an AABB for the closest object. The callback function is called for each proxy that might be closest to the supplied +/// point. /// @param tree the dynamic tree to query /// @param point the query point /// @param maskBits nodes are skipped if the bit-wise AND with the node category bits is zero @@ -634,17 +635,17 @@ B3_API void b3CollideHullAndCapsule( b3LocalManifold* manifold, int capacity, co B3_API void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA, b3SATCache* cache ); -/// Collide a capsule and a triangle. -B3_API void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA, - const b3Vec3* triangleB, b3SimplexCache* cache ); +/// Collide a triangle and capsule. Normal points from triangle to capsule. +B3_API void b3CollideTriangleAndCapsule( b3LocalManifold* manifold, int capacity, const b3Vec3* triangleA, + const b3Capsule* capsuleB, b3SimplexCache* cache ); -/// Collide a hull and a triangle. -B3_API void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, b3Vec3 v1, b3Vec3 v2, - b3Vec3 v3, int triangleFlags, b3SATCache* cache, bool enableSpeculative ); +/// Collide a triangle and hull. Normal points from triangle to hull. +B3_API void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, int triangleFlags, + const b3HullData* hullB, b3SATCache* cache, bool enableSpeculative ); -/// Collide a sphere and a triangle. -B3_API void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const b3Sphere* sphereA, - const b3Vec3* triangleB ); +/// Collide a triangle and sphere. Normal points from triangle to sphere. +B3_API void b3CollideTriangleAndSphere( b3LocalManifold* manifold, int capacity, const b3Vec3* triangleA, + const b3Sphere* sphereB ); /**@}*/ // collision diff --git a/include/box3d/config.h b/include/box3d/config.h index f39eb7e..1309827 100644 --- a/include/box3d/config.h +++ b/include/box3d/config.h @@ -31,3 +31,5 @@ // Enable assertions when NDEBUG is defined // #define B3_ENABLE_ASSERT + +// #define B3_RESTITUTION_ITERATIONS diff --git a/include/box3d/constants.h b/include/box3d/constants.h index de8da49..186ac53 100644 --- a/include/box3d/constants.h +++ b/include/box3d/constants.h @@ -131,3 +131,9 @@ B3_API float b3GetStallThreshold( void ); #define B3_CHILD_POWER ( 64 - 2 * B3_SHAPE_POWER ) #define B3_MAX_SHAPES ( 1 << B3_SHAPE_POWER ) #define B3_MAX_CHILD_SHAPES ( 1 << B3_CHILD_POWER ) + +/// Increase this if your application needs more accurate restitution. Doing so will +/// slow down the simulation. Must be 1 or more. +#ifndef B3_RESTITUTION_ITERATIONS +#define B3_RESTITUTION_ITERATIONS 1 +#endif diff --git a/samples/mover.cpp b/samples/mover.cpp index 9a4d361..e2efd4c 100644 --- a/samples/mover.cpp +++ b/samples/mover.cpp @@ -98,8 +98,8 @@ void CharacterMover::SolveMove( float timeStep, b3Vec3 forward, b3Vec3 right, b3 float drop = control * m_friction * timeStep; float newSpeed = b3MaxFloat( 0.0f, speed - drop ); float ratio = newSpeed / speed; - m_velocity.x *= newSpeed / speed; - m_velocity.z *= newSpeed / speed; + m_velocity.x *= ratio; + m_velocity.z *= ratio; } float maxSpeed = m_sprint ? 1.5f * m_maxSpeed : m_maxSpeed; diff --git a/samples/sample_continuous.cpp b/samples/sample_continuous.cpp index 76afe53..94d4a48 100644 --- a/samples/sample_continuous.cpp +++ b/samples/sample_continuous.cpp @@ -1,13 +1,12 @@ // SPDX-FileCopyrightText: 2025 Erin Catto // SPDX-License-Identifier: MIT +#include "gfx/debug_adapter.h" +#include "gfx/draw.h" #include "imgui.h" #include "sample.h" -#include "gfx/draw.h" #include "utils.h" -#include "gfx/debug_adapter.h" - #include "box3d/box3d.h" #include @@ -442,7 +441,7 @@ public: m_runCount = 0; m_failure = false; m_autoGenerate = false; - + m_collide = true; m_stepWhilePaused = true; Generate(); @@ -549,13 +548,16 @@ public: // Don't allow shapes to collide with each other. This // makes isolating failures easier. - shapeDef.filter.categoryBits = 2; - shapeDef.filter.maskBits = 1; + if ( m_collide == false ) + { + shapeDef.filter.categoryBits = 2; + shapeDef.filter.maskBits = 1; + } g_randomSeed = (uint32_t)b3GetTicks(); bool simulateAll = true; - //g_randomSeed = 1910133196; + // g_randomSeed = 1910133196; m_runCount += 1; m_stepCount = 0; @@ -624,6 +626,11 @@ public: Generate(); } + if ( ImGui::Checkbox( "Collide", &m_collide ) ) + { + Generate(); + } + if ( ImGui::Button( "Generate" ) ) { Generate(); @@ -645,8 +652,7 @@ public: { PickRay pickRay = m_camera->BuildPickRay( m_context->mouseX, m_context->mouseY ); - b3RayResult result = - b3World_CastRayClosest( m_worldId, pickRay.origin, pickRay.translation, b3DefaultQueryFilter() ); + b3RayResult result = b3World_CastRayClosest( m_worldId, pickRay.origin, pickRay.translation, b3DefaultQueryFilter() ); if ( result.hit ) { @@ -736,6 +742,7 @@ public: int m_runCount; bool m_failure; bool m_autoGenerate; + bool m_collide; }; static int sampleMeshDrop = RegisterSample( "Continuous", "Mesh Drop", MeshDrop::Create ); @@ -862,13 +869,12 @@ public: if ( context->restart == false ) { m_camera->SetView( 0.0f, 15.0f, 50.0f, { 0.0f, 15.0f, 0.0f } ); - } AddGroundBox( 40.0f ); b3BoxHull box = b3MakeBoxHull( 0.5f, 10.0f, 0.5f ); - b3Capsule capsule = {{0.0f, -9.5f, 0.0f}, {0.0f, 9.5f, 0.0f}, 0.5f}; + b3Capsule capsule = { { 0.0f, -9.5f, 0.0f }, { 0.0f, 9.5f, 0.0f }, 0.5f }; bool useCapsule = false; @@ -881,7 +887,7 @@ public: b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); b3ShapeDef shapeDef = b3DefaultShapeDef(); - if (useCapsule) + if ( useCapsule ) { b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); } diff --git a/samples/sample_determinism.cpp b/samples/sample_determinism.cpp index 7720f99..6370e86 100644 --- a/samples/sample_determinism.cpp +++ b/samples/sample_determinism.cpp @@ -35,7 +35,7 @@ public: { Sample::Step( ); - if ( m_done == false ) + if ( m_done == false && (m_context->pause == false || m_context->singleStep > 0) ) { m_done = UpdateFallingRagdolls( m_worldId, &m_data ); if (m_done) diff --git a/samples/sample_issues.cpp b/samples/sample_issues.cpp index d3d17e7..06695a8 100644 --- a/samples/sample_issues.cpp +++ b/samples/sample_issues.cpp @@ -1146,3 +1146,160 @@ public: }; static int sampleWheelStack = RegisterSample( "Issues", "GMod Wheel Stack", WheelStack::Create ); + +class RestitutionOvershoot : public Sample +{ +public: + static constexpr float m_boxHalf = 0.5f; + static constexpr float m_floorHalfXZ = 0.375f; + static constexpr float m_floorHalfY = 0.25f; + static constexpr float m_dropHeight = 10.0f; + static constexpr float m_tolerance = 0.05f; + + explicit RestitutionOvershoot( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 20.0f, 0.0f, 28.0f, { 0.0f, m_dropHeight + m_boxHalf, 0.0f } ); + } + + b3BoxHull floor = b3MakeBoxHull( m_floorHalfXZ, m_floorHalfY, m_floorHalfXZ ); + b3BodyDef floorDef = b3DefaultBodyDef(); + floorDef.type = b3_staticBody; + floorDef.position = { 0.0f, -m_floorHalfY, 0.0f }; + b3BodyId floorBody = b3CreateBody( m_worldId, &floorDef ); + + b3ShapeDef floorShape = b3DefaultShapeDef(); + b3CreateHullShape( floorBody, &floorShape, &floor.base ); + + b3BoxHull box = b3MakeBoxHull( m_boxHalf, m_boxHalf, m_boxHalf ); + b3BodyDef boxDef = b3DefaultBodyDef(); + boxDef.type = b3_dynamicBody; + boxDef.position = { 0.0f, m_dropHeight, 0.0f }; + m_boxBody = b3CreateBody( m_worldId, &boxDef ); + + b3ShapeDef boxShape = b3DefaultShapeDef(); + boxShape.baseMaterial.restitution = 1.0f; + b3CreateHullShape( m_boxBody, &boxShape, &box.base ); + + m_currentY = m_dropHeight; + m_maxBounceY = 0.0f; + m_bounced = false; + m_failed = false; + } + + void Step() override + { + Sample::Step(); + + if ( B3_IS_NON_NULL( m_boxBody ) == false ) + { + return; + } + + b3Pos position = b3Body_GetPosition( m_boxBody ); + m_currentY = position.y; + + b3Vec3 velocity = b3Body_GetLinearVelocity( m_boxBody ); + if ( m_bounced == false && velocity.y > 0.0f ) + { + m_bounced = true; + } + + if ( m_bounced ) + { + if ( position.y > m_maxBounceY ) + { + m_maxBounceY = position.y; + } + if ( position.y > m_dropHeight + m_tolerance ) + { + m_failed = true; + } + } + + b3Pos markerPoint = { 0.0f, m_dropHeight + m_boxHalf, 0.0f }; + DrawPlane( b3Vec3_axisY, markerPoint, MakeColor( b3_colorYellow ) ); + + DrawTextLine( "drop height = %.2f m", m_dropHeight ); + DrawTextLine( "current y = %.2f m", m_currentY ); + DrawTextLine( "max bounce = %.2f m", m_maxBounceY ); + + if ( m_bounced == false ) + { + DrawTextLine( "waiting for first bounce..." ); + } + else if ( m_failed ) + { + DrawTextLine( "FAIL: box exceeded drop height" ); + } + else + { + DrawTextLine( "PASS: bounce stays at or below drop height" ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new RestitutionOvershoot( context ); + } + + b3BodyId m_boxBody = {}; + float m_currentY = 0.0f; + float m_maxBounceY = 0.0f; + bool m_bounced = false; + bool m_failed = false; +}; + +static int sampleRestitutionOvershoot = RegisterSample( "Issues", "Restitution Overshoot", RestitutionOvershoot::Create ); + +class SlideTwistOffCenterShape : public Sample +{ +public: + explicit SlideTwistOffCenterShape( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( -30.0f, 17.0f, 30.0f, { 0.0f, 5.0f, 0.0f } ); + } + + AddGroundBox( 50.0f ); + + b3Quat orientation = b3MakeQuatFromAxisAngle( b3Vec3_axisX, 20.0f * B3_DEG_TO_RAD ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + bodyDef.position = { 0.0f, 4.0f, 0.0f }; + bodyDef.rotation = orientation; + b3BodyId planeBody = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull plane = b3MakeBoxHull( 10.0f, 0.5f, 10.0f ); + shapeDef.baseMaterial.friction = 0.6f; + b3CreateHullShape( planeBody, &shapeDef, &plane.base ); + + b3Vec3 boxLocalCenter = { 1.0f, 0.5f, 1.0f }; + b3Vec3 boxOffset = b3RotateVector( orientation, boxLocalCenter ); + + bodyDef.type = b3_dynamicBody; + bodyDef.position = { -boxOffset.x, 5.0f - boxOffset.y, -boxOffset.z }; + bodyDef.rotation = orientation; + //bodyDef.angularVelocity = 25.0f * b3RotateVector( orientation, b3Vec3_axisY ); + b3BodyId boxBody = b3CreateBody( m_worldId, &bodyDef ); + b3BoxHull mBox = b3MakeOffsetBoxHull( 1.0f, 0.5f, 1.0f, boxLocalCenter ); + shapeDef.baseMaterial.friction = 0.3f; + b3CreateHullShape( boxBody, &shapeDef, &mBox.base ); + + b3Body_SetAngularVelocity( boxBody, 25.0f * b3RotateVector( orientation, b3Vec3_axisY ) ); + } + + static Sample* Create( SampleContext* context ) + { + return new SlideTwistOffCenterShape( context ); + } +}; + +static int sampleSlideTwistOffCenterShape = + RegisterSample( "Issues", "Slide Twist Off Center Shape", SlideTwistOffCenterShape::Create ); diff --git a/samples/sample_manifold.cpp b/samples/sample_manifold.cpp index 75bf01b..ba366f5 100644 --- a/samples/sample_manifold.cpp +++ b/samples/sample_manifold.cpp @@ -517,7 +517,7 @@ public: b3TransformPoint( xf, m_triangle[2] ), }; - b3CollideSphereAndTriangle( &m_manifold, m_pointCapacity, &m_sphere, localTriangle ); + b3CollideTriangleAndSphere( &m_manifold, m_pointCapacity, localTriangle, &m_sphere ); } b3Sphere m_sphere; @@ -668,7 +668,7 @@ public: b3TransformPoint( xf, m_triangle[2] ), }; - b3CollideCapsuleAndTriangle( &m_manifold, m_pointCapacity, &m_capsule, localTriangle, &m_simplexCache ); + b3CollideTriangleAndCapsule( &m_manifold, m_pointCapacity, localTriangle, &m_capsule, &m_simplexCache ); } b3Capsule m_capsule; @@ -913,8 +913,8 @@ public: b3TransformPoint( xf, m_triangle[2] ), }; - b3CollideHullAndTriangle( &m_manifold, m_pointCapacity, m_hull, localTriangle[0], localTriangle[1], localTriangle[2], - m_flags, &m_satCache, true ); + b3CollideTriangleAndHull( &m_manifold, m_pointCapacity, localTriangle[0], localTriangle[1], localTriangle[2], + m_flags, m_hull, &m_satCache, true ); } int m_flags; diff --git a/samples/sample_ragdoll.cpp b/samples/sample_ragdoll.cpp index 6dbaf4e..9d32a46 100644 --- a/samples/sample_ragdoll.cpp +++ b/samples/sample_ragdoll.cpp @@ -1,10 +1,11 @@ // SPDX-FileCopyrightText: 2025 Erin Catto // SPDX-License-Identifier: MIT +#include "gfx/draw.h" #include "human.h" #include "imgui.h" #include "sample.h" -#include "gfx/draw.h" +#include "utils.h" #include "box3d/box3d.h" @@ -232,9 +233,14 @@ public: m_groundMesh = b3CreateGridMesh( 20, 20, 1.0f, 1, true ); b3CreateMeshShape( groundId, &shapeDef, m_groundMesh, b3Vec3_one ); + g_randomSeed = 42; + float a = 0.1f * e_count; + b3Vec3 lower = { -a, -a, -a }; + b3Vec3 upper = { a, a, a }; for ( int i = 0; i < e_count; ++i ) { - b3Pos position = { 0.1f * i, 2.0f + 0.5f * i, -0.1f * i }; + b3Vec3 offset = RandomVec3( lower, upper ); + b3Pos position = { offset.x, 2.0f, offset.z }; float torque = 10.0f; float hertz = 0.5f; float damping = 0.7f; diff --git a/samples/sample_shapes.cpp b/samples/sample_shapes.cpp index 8d07830..72b7cdc 100644 --- a/samples/sample_shapes.cpp +++ b/samples/sample_shapes.cpp @@ -64,7 +64,7 @@ public: m_camera->SetView( -140.0f, 17.0f, 60.0f, { 0.0f, 7.5f, 0.0f } ); } - AddGroundBox( 50.0f ); + AddGroundBox( 80.0f ); b3BodyDef bodyDef = b3DefaultBodyDef(); b3ShapeDef shapeDef = b3DefaultShapeDef(); diff --git a/samples/sample_stacking.cpp b/samples/sample_stacking.cpp index 6462d35..4730110 100644 --- a/samples/sample_stacking.cpp +++ b/samples/sample_stacking.cpp @@ -210,7 +210,7 @@ public: bodyDef.name = "cube"; bodyDef.type = b3_dynamicBody; bodyDef.position = { 0.0f, 0.5f, 0.0f }; - //bodyDef.angularVelocity = { 0.0f, 10.0f, 0.0f }; + // bodyDef.angularVelocity = { 0.0f, 10.0f, 0.0f }; m_bodyId = b3CreateBody( m_worldId, &bodyDef ); b3ShapeDef shapeDef = b3DefaultShapeDef(); @@ -409,7 +409,7 @@ public: { if ( m_context->restart == false ) { - m_camera->SetView( 35.0f, 15.0f, 30.0f, { 0.0f, 10.0f, 0.0f } ); + m_camera->SetView( 35.0f, 15.0f, 12.0f, { 0.0f, 2.0f, 0.0f } ); } m_shapeType = b3_hullShape; @@ -418,42 +418,43 @@ public: void CreateStack() { - AddGroundBox( 60.0f ); + AddGroundBox( 20.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.rollingResistance = m_shapeType == b3_capsuleShape ? 0.1f : 0.05f; + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + float h = 1.0f; + float r = 0.1f; + + b3BoxHull box = b3MakeBoxHull( h, r, r ); + b3Capsule capsule = { { -h, 0.0f, 0.0f }, { h, 0.0f, 0.0f }, r }; + int count = 30; + for ( int i = 0; i < count; ++i ) { - b3ShapeDef shapeDef = b3DefaultShapeDef(); - shapeDef.baseMaterial.rollingResistance = m_shapeType == b3_capsuleShape ? 0.1f : 0.01f; - b3BodyDef bodyDef = b3DefaultBodyDef(); - bodyDef.type = b3_dynamicBody; + float alpha = ( i & 1 ) == 1 ? 0.0f : 0.5f * B3_PI; - b3BoxHull box = b3MakeBoxHull( 2.5f, 0.25f, 0.25f ); - b3Capsule capsule = { { -2.5f, 0.0f, 0.0f }, { 2.5f, 0.0f, 0.0f }, 0.25f }; + float x = ( i & 1 ) == 0 ? h - 2.0f * r : 0.0f; + float z = ( i & 1 ) == 0 ? 0.0f : h - 2.0f * r; - for ( int i = 0; i < m_size; ++i ) + bodyDef.position = { x, ( 2.1f * i + 0.5f ) * r, z }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisY, alpha ); + b3BodyId boxBody1 = b3CreateBody( m_worldId, &bodyDef ); + + bodyDef.position = { -x, ( 2.1f * i + 0.5f ) * r, -z }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisY, alpha ); + b3BodyId boxBody2 = b3CreateBody( m_worldId, &bodyDef ); + + if ( m_shapeType == b3_capsuleShape ) { - float alpha = ( i & 1 ) == 1 ? 0.0f : 0.5f * B3_PI; - - float x = ( i & 1 ) == 0 ? 1.75f : 0.0f; - float z = ( i & 1 ) == 0 ? 0.0f : 1.75f; - - bodyDef.position = { x, 0.5f * i + 0.25f, z }; - bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisY, alpha ); - b3BodyId boxBody1 = b3CreateBody( m_worldId, &bodyDef ); - - bodyDef.position = { -x, 0.5f * i + 0.25f, -z }; - bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisY, alpha ); - b3BodyId boxBody2 = b3CreateBody( m_worldId, &bodyDef ); - - if ( m_shapeType == b3_capsuleShape ) - { - b3CreateCapsuleShape( boxBody1, &shapeDef, &capsule ); - b3CreateCapsuleShape( boxBody2, &shapeDef, &capsule ); - } - else - { - b3CreateHullShape( boxBody1, &shapeDef, &box.base ); - b3CreateHullShape( boxBody2, &shapeDef, &box.base ); - } + b3CreateCapsuleShape( boxBody1, &shapeDef, &capsule ); + b3CreateCapsuleShape( boxBody2, &shapeDef, &capsule ); + } + else + { + b3CreateHullShape( boxBody1, &shapeDef, &box.base ); + b3CreateHullShape( boxBody2, &shapeDef, &box.base ); } } } @@ -484,7 +485,6 @@ public: return new JengaStack( context ); } - static constexpr int m_size = 40; b3ShapeType m_shapeType; }; @@ -811,7 +811,7 @@ public: { if ( m_context->restart == false ) { - m_camera->SetView( 0.0f, 30.0f, 50.0f, { 0.0f, 5.0f, 0.0f } ); + m_camera->SetView( 40.0f, 15.0f, 50.0f, { 0.0f, 12.0f, 0.0f } ); } AddGroundBox( 40.0f ); @@ -847,3 +847,93 @@ public: }; static int samplePyramid2D = RegisterSample( "Stacking", "Pyramid2D", Pyramid2D::Create ); + +// This stresses the edge-edge handling. +class EdgeCrossing : public Sample +{ +public: + explicit EdgeCrossing( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 25.0f, 10.0f, b3Pos_zero ); + } + + AddGroundBox( 40.0f ); + + b3Vec3 h = { 0.2f, 0.02f, 0.04f }; + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + bodyDef.position.x = -10.0f; + bodyDef.position.z = -2.0f; + + b3BoxHull box1 = b3MakeBoxHull( h.x, h.y, h.z ); + b3BoxHull box2 = b3MakeBoxHull( h.x, h.z, h.y ); + + b3Vec3 axis = b3Normalize( { 0.1f, 0.9f, 0.0f } ); + for ( float angle = -B3_PI; angle < B3_PI + 0.001f; angle += 0.1f * B3_PI ) + { + bodyDef.position.y = h.y; + bodyDef.rotation = b3Quat_identity; + b3BodyId bodyId1 = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId1, &shapeDef, &box1.base ); + + bodyDef.position.y = 20.0f * h.y; + bodyDef.rotation = b3MakeQuatFromAxisAngle( axis, angle ); + b3BodyId boxBody2 = b3CreateBody( m_worldId, &bodyDef ); + + b3CreateHullShape( boxBody2, &shapeDef, &box1.base ); + + bodyDef.position.x += 1.0f; + } + + bodyDef.position.x = -10.0f; + bodyDef.position.z = 0.0f; + + for ( float angle = -B3_PI; angle < B3_PI + 0.001f; angle += 0.1f * B3_PI ) + { + bodyDef.position.y = h.z; + bodyDef.rotation = b3Quat_identity; + b3BodyId bodyId1 = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId1, &shapeDef, &box2.base ); + + bodyDef.position.y = 20.0f * h.z; + bodyDef.rotation = b3MakeQuatFromAxisAngle( axis, angle ); + b3BodyId boxBody2 = b3CreateBody( m_worldId, &bodyDef ); + + b3CreateHullShape( boxBody2, &shapeDef, &box2.base ); + + bodyDef.position.x += 1.0f; + } + + bodyDef.position.x = -10.0f; + bodyDef.position.z = 2.0f; + + for ( float angle = -B3_PI; angle < B3_PI + 0.001f; angle += 0.1f * B3_PI ) + { + bodyDef.position.y = h.y; + bodyDef.rotation = b3Quat_identity; + b3BodyId bodyId1 = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId1, &shapeDef, &box1.base ); + + bodyDef.position.y = 20.0f * h.y; + bodyDef.rotation = b3MakeQuatFromAxisAngle( axis, angle ); + b3BodyId boxBody2 = b3CreateBody( m_worldId, &bodyDef ); + + b3CreateHullShape( boxBody2, &shapeDef, &box2.base ); + + bodyDef.position.x += 1.0f; + } + } + + static Sample* Create( SampleContext* context ) + { + return new EdgeCrossing( context ); + } +}; + +static int sampleEdgeCrossing = RegisterSample( "Stacking", "Edge Crossing", EdgeCrossing::Create ); diff --git a/shared/utils.c b/shared/utils.c index ac19aff..bdba729 100644 --- a/shared/utils.c +++ b/shared/utils.c @@ -4,7 +4,8 @@ #include "utils.h" #if defined( _WIN64 ) -#include +// Lower-case windows.h intentionally for cross compiling on mingw. +#include #elif defined( __APPLE__ ) #include #elif defined( __linux__ ) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 71b76d1..61da44a 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -154,7 +154,8 @@ if (BOX3D_PROFILE) endif() if(BOX3D_VALIDATE) - message(STATUS "Box3D validation ON") + # Only effective when NDEBUG is undefined, see base.h + message(STATUS "Box3D validation ON (debug builds)") target_compile_definitions(box3d PRIVATE BOX3D_VALIDATE) endif() diff --git a/src/contact_solver.c b/src/contact_solver.c index 25c4f0a..48de409 100644 --- a/src/contact_solver.c +++ b/src/contact_solver.c @@ -1558,7 +1558,7 @@ void b3WarmStartContacts_Convex( b3SolverBlock block, b3StepContext* context ) int pointCount1 = b3MaxInt( c->pointCounts[0], c->pointCounts[1] ); int pointCount2 = b3MaxInt( c->pointCounts[2], c->pointCounts[3] ); int pointCount = b3MaxInt( pointCount1, pointCount2 ); - B3_VALIDATE( 0 < pointCount && pointCount <= B3_SIMD_WIDTH ); + B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); // Normal impulses for ( int j = 0; j < pointCount; ++j ) @@ -1632,7 +1632,7 @@ void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context, bool u int pointCount1 = b3MaxInt( c->pointCounts[0], c->pointCounts[1] ); int pointCount2 = b3MaxInt( c->pointCounts[2], c->pointCounts[3] ); int pointCount = b3MaxInt( pointCount1, pointCount2 ); - B3_VALIDATE( 0 < pointCount && pointCount <= B3_SIMD_WIDTH ); + B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); b3BodyStateW bA = b3GatherBodies( states, c->indexA ); b3BodyStateW bB = b3GatherBodies( states, c->indexB ); @@ -1847,7 +1847,7 @@ void b3ApplyRestitution_Convex( b3SolverBlock block, b3StepContext* context ) int pointCount1 = b3MaxInt( c->pointCounts[0], c->pointCounts[1] ); int pointCount2 = b3MaxInt( c->pointCounts[2], c->pointCounts[3] ); int pointCount = b3MaxInt( pointCount1, pointCount2 ); - B3_VALIDATE( 0 < pointCount && pointCount <= B3_SIMD_WIDTH ); + B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); // Single gather for all manifolds b3BodyStateW bA = b3GatherBodies( states, c->indexA ); diff --git a/src/convex_manifold.c b/src/convex_manifold.c index bb3fb08..dd53247 100644 --- a/src/convex_manifold.c +++ b/src/convex_manifold.c @@ -1,4 +1,4 @@ -// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-FileCopyrightText: 2026 Erin Catto // SPDX-License-Identifier: MIT #include "algorithm.h" @@ -82,8 +82,8 @@ static int b3ClipSegmentToHullFace( b3ClipVertex segment[2], const b3HullData* h return 2; } -static b3FaceQuery b3QueryFaceDirectionHullAndCapsule( const b3HullData* hull, const b3Capsule* capsule, - b3Transform capsuleTransform ) +static b3SeparatingAxis b3QueryFaceDirectionHullAndCapsule( const b3HullData* hull, const b3Capsule* capsule, + b3Transform capsuleTransform ) { int maxFaceIndex = -1; int maxVertexIndex = -1; @@ -110,15 +110,16 @@ static b3FaceQuery b3QueryFaceDirectionHullAndCapsule( const b3HullData* hull, c } } - return (b3FaceQuery){ + return (b3SeparatingAxis){ + .normal = planes[maxFaceIndex].normal, .separation = maxFaceSeparation, - .faceIndex = (uint8_t)maxFaceIndex, - .vertexIndex = (uint8_t)maxVertexIndex, + .indexA = (uint8_t)maxFaceIndex, + .indexB = (uint8_t)maxVertexIndex, }; } -static b3EdgeQuery b3QueryEdgeDirectionHullAndCapsule( const b3HullData* hull, const b3Capsule* capsule, - b3Transform capsuleTransform ) +static b3SeparatingAxis b3QueryEdgeDirectionHullAndCapsule( const b3HullData* hull, const b3Capsule* capsule, + b3Transform capsuleTransform ) { // Find axis of minimum penetration b3Vec3 maxNormal = b3Vec3_zero; @@ -194,7 +195,7 @@ static b3EdgeQuery b3QueryEdgeDirectionHullAndCapsule( const b3HullData* hull, c } // Save result - return (b3EdgeQuery){ + return (b3SeparatingAxis){ .normal = maxNormal, .separation = maxSeparation, .indexA = maxIndexA, @@ -704,13 +705,13 @@ void b3CollideCapsules( b3LocalManifold* manifold, int capacity, const b3Capsule } static bool b3BuildHullFaceAndCapsuleContact( b3LocalManifold* manifold, const b3HullData* hullA, const b3Capsule* capsuleB, - b3Transform transformBtoA, b3FaceQuery query ) + b3Transform transformBtoA, b3SeparatingAxis query ) { // Work in shapeA coordinates const b3Plane* planes = b3GetHullPlanes( hullA ); // Clip the capsule edge against the side planes of the reference face - int refFace = query.faceIndex; + int refFace = query.indexA; b3Plane refPlane = planes[refFace]; b3ClipVertex segmentB[2]; @@ -771,7 +772,7 @@ static inline float b3DeepestPointSeparation( const b3LocalManifold* manifold ) } static bool b3BuildHullAndCapsuleEdgeContact( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, - const b3Capsule* capsuleB, b3Transform transformBtoA, b3EdgeQuery query ) + const b3Capsule* capsuleB, b3Transform transformBtoA, b3SeparatingAxis query ) { if ( capacity < 1 ) { @@ -916,14 +917,14 @@ void b3CollideHullAndCapsule( b3LocalManifold* manifold, int capacity, const b3H // Deep penetration - b3FaceQuery faceQuery = b3QueryFaceDirectionHullAndCapsule( hullA, capsuleB, transformBtoA ); + b3SeparatingAxis faceQuery = b3QueryFaceDirectionHullAndCapsule( hullA, capsuleB, transformBtoA ); if ( faceQuery.separation > capsuleB->radius ) { // We found a separating axis return; } - b3EdgeQuery edgeQuery = b3QueryEdgeDirectionHullAndCapsule( hullA, capsuleB, transformBtoA ); + b3SeparatingAxis edgeQuery = b3QueryEdgeDirectionHullAndCapsule( hullA, capsuleB, transformBtoA ); if ( edgeQuery.separation > capsuleB->radius ) { // We found a separating axis @@ -999,20 +1000,24 @@ static int b3BuildPolygon( b3ClipVertex* out, b3Transform transform, const b3Hul } static bool b3BuildFaceAContact( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB, - b3Transform transformBtoA, b3FaceQuery query, b3SATCache* cache ) + b3Transform transformBtoA, b3SeparatingAxis query, b3SATCache* cache ) { + B3_VALIDATE( query.type == b3_faceAxisA ); + B3_VALIDATE( 0 <= query.indexA && query.indexA < hullA->faceCount ); + B3_VALIDATE( 0 <= query.indexB && query.indexB < hullB->vertexCount ); + const b3HullFace* facesA = b3GetHullFaces( hullA ); const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA ); const b3Plane* planesA = b3GetHullPlanes( hullA ); const b3Vec3* pointsA = b3GetHullPoints( hullA ); // Reference face - int refFace = query.faceIndex; + int refFace = query.indexA; b3Plane refPlane = planesA[refFace]; // Find incident face b3Vec3 refNormalInB = b3InvRotateVector( transformBtoA.q, refPlane.normal ); - int incFace = b3FindIncidentFace( hullB, refNormalInB, query.vertexIndex ); + int incFace = b3FindIncidentFace( hullB, refNormalInB, query.indexB ); // Build clip polygon from incident face in frame A b3ClipVertex buffer1[B3_MAX_CLIP_POINTS], buffer2[B3_MAX_CLIP_POINTS]; @@ -1089,19 +1094,30 @@ static bool b3BuildFaceAContact( b3LocalManifold* manifold, int capacity, const // Save cache cache->separation = minSeparation; cache->type = (uint8_t)b3_faceAxisA; - cache->indexA = (uint8_t)query.faceIndex; - cache->indexB = (uint8_t)query.vertexIndex; + cache->indexA = (uint8_t)query.indexA; + cache->indexB = (uint8_t)query.indexB; return true; } static bool b3BuildFaceBContact( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB, - b3Transform transformBtoA, b3FaceQuery query, b3SATCache* cache ) + b3Transform transformBtoA, b3SeparatingAxis query, b3SATCache* cache ) { + B3_VALIDATE( query.type == b3_faceAxisB ); + b3Transform transformAtoB = b3InvertTransform( transformBtoA ); - bool touching = b3BuildFaceAContact( manifold, capacity, hullB, hullA, transformAtoB, query, cache ); + b3SeparatingAxis flippedQuery = { + .normal = b3Neg( query.normal ), + .separation = query.separation, + .indexA = query.indexB, + .indexB = query.indexA, + .type = b3_faceAxisA, + }; + + bool touching = b3BuildFaceAContact( manifold, capacity, hullB, hullA, transformAtoB, flippedQuery, cache ); if ( touching == false ) { + *cache = (b3SATCache){ 0 }; return false; } @@ -1110,9 +1126,6 @@ static bool b3BuildFaceBContact( b3LocalManifold* manifold, int capacity, const // Transform and flip normal so it points from A to B, even though the B has the reference face. manifold->normal = b3Neg( b3MulMV( matrix, manifold->normal ) ); - cache->type = (uint8_t)b3_faceAxisB; - cache->indexA = (uint8_t)query.vertexIndex; - cache->indexB = (uint8_t)query.faceIndex; // Transform points from frame B to frame A. // Also flip the pairs to ensure correct matches. @@ -1123,12 +1136,20 @@ static bool b3BuildFaceBContact( b3LocalManifold* manifold, int capacity, const pt->pair = b3FlipPair( pt->pair ); } + cache->type = (uint8_t)b3_faceAxisB; + cache->indexA = (uint8_t)query.indexA; + cache->indexB = (uint8_t)query.indexB; + return true; } static bool b3BuildEdgeContact( b3LocalManifold* manifold, const b3HullData* hullA, const b3HullData* hullB, - b3Transform transformBtoA, b3EdgeQuery query, b3SATCache* cache ) + b3Transform transformBtoA, b3SeparatingAxis query, b3SATCache* cache ) { + B3_VALIDATE( query.type == b3_edgePairAxis ); + B3_VALIDATE( 0 <= query.indexA && query.indexA < hullA->edgeCount ); + B3_VALIDATE( 0 <= query.indexB && query.indexB < hullB->edgeCount ); + // Work in shapeA coordinates const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA ); const b3Vec3* pointsA = b3GetHullPoints( hullA ); @@ -1295,22 +1316,39 @@ static inline void b3GetSupportWide( b3Vec3 normal, const float* vx, const float // SIMD separating axis test based on an implementation developed by Cairn Overturf. // See his article: https://cairno.substack.com/p/improvements-to-the-separating-axis -b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, int axisOverride ) +b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, bool earlyReturn ) { - B3_VALIDATE( axisOverride == b3_invalidAxis || axisOverride == b3_manualFaceAxisA || axisOverride == b3_manualFaceAxisB || - axisOverride == b3_manualEdgePairAxis ); - b3Matrix3 R = b3MakeMatrixFromQuat( xfB.q ); b3Matrix3 invR = b3Transpose( R ); float speculativeDistance = B3_SPECULATIVE_DISTANCE; b3AxisQuery res = { - .normal = b3Vec3_zero, - .separation = -INFINITY, - .indexA = B3_NULL_INDEX, - .indexB = B3_NULL_INDEX, - .type = b3_invalidAxis, + .faceA = + { + .normal = b3Vec3_zero, + .separation = -INFINITY, + .indexA = B3_NULL_INDEX, + .indexB = B3_NULL_INDEX, + .type = b3_faceAxisA, + }, + .faceB = + { + .normal = b3Vec3_zero, + .separation = -INFINITY, + .indexA = B3_NULL_INDEX, + .indexB = B3_NULL_INDEX, + .type = b3_faceAxisB, + }, + .edge = + { + .normal = b3Vec3_zero, + .separation = -INFINITY, + .indexA = B3_NULL_INDEX, + .indexB = B3_NULL_INDEX, + .type = b3_edgePairAxis, + }, + .separatedFeature = b3_invalidAxis, }; int faceCountA = hullA->faceCount; @@ -1325,38 +1363,30 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* b3Vec3 hB = b3AABB_Extents( hullB->aabb ); // Test A's face planes against B's vertices. - if ( axisOverride != b3_manualFaceAxisB && axisOverride != b3_manualEdgePairAxis ) + for ( int i = 0; i < faceCountA; ++i ) { - for ( int i = 0; i < faceCountA; ++i ) + b3Plane plane = planesA[i]; + b3Vec3 direction = b3Neg( b3MulMV( invR, plane.normal ) ); + float planeSeparation = b3Dot( plane.normal, xfB.p ) - plane.offset; + float biasB = b3Dot( direction, cB ) + 1.0625f * b3Dot( b3Abs( direction ), hB ); + float support; + int vertexIndex; + b3GetSupportWide( direction, vxB, vyB, vzB, soaVertexCountB, biasB, &support, &vertexIndex ); + float separation = planeSeparation - support; + if ( separation > res.faceA.separation ) { - b3Plane plane = planesA[i]; - b3Vec3 direction = b3Neg( b3MulMV( invR, plane.normal ) ); - float planeSeparation = b3Dot( plane.normal, xfB.p ) - plane.offset; - float biasB = b3Dot( direction, cB ) + 1.0625f * b3Dot( b3Abs( direction ), hB ); - float support; - int vertexIndex; - b3GetSupportWide( direction, vxB, vyB, vzB, soaVertexCountB, biasB, &support, &vertexIndex ); - float separation = planeSeparation - support; - if ( separation > res.separation ) + res.faceA.normal = plane.normal; + res.faceA.separation = separation; + res.faceA.indexA = i; + res.faceA.indexB = vertexIndex; + if ( separation > speculativeDistance && earlyReturn ) { - res.type = b3_faceAxisA; - res.separation = separation; - res.indexA = i; - res.indexB = vertexIndex; - res.normal = plane.normal; - if ( separation > speculativeDistance ) - { - return res; - } + res.separatedFeature = b3_faceAxisA; + return res; } } } - if ( axisOverride == b3_manualFaceAxisA ) - { - return res; - } - int faceCountB = hullB->faceCount; const b3Plane* planesB = b3GetHullPlanes( hullB ); @@ -1369,39 +1399,31 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* b3Vec3 hA = b3AABB_Extents( hullA->aabb ); // Test B's face planes against A's vertices. - if ( axisOverride != b3_manualEdgePairAxis ) + for ( int i = 0; i < faceCountB; ++i ) { - for ( int i = 0; i < faceCountB; ++i ) + b3Plane plane = planesB[i]; + b3Vec3 direction = b3Neg( b3MulMV( R, plane.normal ) ); + float planeSeparation = b3Dot( direction, xfB.p ) - plane.offset; + float biasA = b3Dot( direction, cA ) + 1.0625f * b3Dot( b3Abs( direction ), hA ); + float support; + int vertexIndex; + b3GetSupportWide( direction, vxA, vyA, vzA, soaVertexCountA, biasA, &support, &vertexIndex ); + float separation = planeSeparation - support; + if ( separation > res.faceB.separation ) { - b3Plane plane = planesB[i]; - b3Vec3 direction = b3Neg( b3MulMV( R, plane.normal ) ); - float planeSeparation = b3Dot( direction, xfB.p ) - plane.offset; - float biasA = b3Dot( direction, cA ) + 1.0625f * b3Dot( b3Abs( direction ), hA ); - float support; - int vertexIndex; - b3GetSupportWide( direction, vxA, vyA, vzA, soaVertexCountA, biasA, &support, &vertexIndex ); - float separation = planeSeparation - support; - if ( separation > res.separation ) + res.faceB.normal = direction; + res.faceB.separation = separation; + res.faceB.indexA = vertexIndex; + res.faceB.indexB = i; + // This points from A to B and is in frame A + if ( separation > speculativeDistance && earlyReturn ) { - res.type = b3_faceAxisB; - res.separation = separation; - res.indexA = vertexIndex; - res.indexB = i; - // This points from A to B and is in frame A - res.normal = direction; - if ( separation > speculativeDistance ) - { - return res; - } + res.separatedFeature = b3_faceAxisB; + return res; } } } - if ( axisOverride == b3_manualFaceAxisB ) - { - return res; - } - // Transform B into A's space once, into SoA arrays. Extra space so // tail can be set to zero in all cases. _Static_assert( ( B3_MAX_HULL_EDGES & 3 ) == 0, "must be multiple of 4" ); @@ -1536,9 +1558,6 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* b3StoreW( aV0z + na, zero ); b3StoreW( aTol + na, zero ); - // Prefer face contact for more contact points. - float absFaceBias = 0.1f * B3_LINEAR_SLOP; - int edgeCountB = halfEdgeCountB / 2; #if defined( B3_SIMD_NONE ) @@ -1608,20 +1627,18 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* float sz = aV0z[i] + bv0z; float separation = -( sx * nx + ( sy * ny + sz * nz ) ); - if ( separation > res.separation + absFaceBias ) + if ( separation > res.edge.separation ) { - res.normal = (b3Vec3){ nx, ny, nz }; - res.separation = separation; - res.type = b3_edgePairAxis; + res.edge.normal = (b3Vec3){ nx, ny, nz }; + res.edge.separation = separation; // Half edge index - res.indexA = 2 * i; - res.indexB = 2 * j; - - // Edge beats face, remove bias - absFaceBias = 0.0f; - if ( separation > speculativeDistance ) + res.edge.indexA = 2 * i; + res.edge.indexB = 2 * j; + + if ( separation > speculativeDistance && earlyReturn ) { + res.separatedFeature = b3_edgePairAxis; return res; } } @@ -1629,7 +1646,7 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* } #else - + // Edge phase, one B edge against four A edges at a time, no transforms in the loop. const b3FloatW EPS = b3SplatW( -0.0001f ); const b3FloatW INF = b3SplatW( INFINITY ); @@ -1718,7 +1735,7 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* // Test all 4 supports against the running best at once. If none beats it, skip the // store and scalar reduction. res->support only turns negative just before returning, // so this never skips a lane that would trigger the early out. - b3FloatW improves = b3GreaterThanW( separation, b3SplatW( res.separation ) ); + b3FloatW improves = b3GreaterThanW( separation, b3SplatW( res.edge.separation ) ); if ( b3AnyTrueW( improves ) == false ) { continue; @@ -1740,20 +1757,18 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* { int ei = i + lane; float s = sA[lane]; - if ( s > res.separation + absFaceBias ) + if ( s > res.edge.separation ) { - res.normal = (b3Vec3){ nxA[lane], nyA[lane], nzA[lane] }; - res.separation = s; - res.type = b3_edgePairAxis; + res.edge.normal = (b3Vec3){ nxA[lane], nyA[lane], nzA[lane] }; + res.edge.separation = s; + // Half edge index - res.indexA = 2 * ei; - res.indexB = 2 * j; + res.edge.indexA = 2 * ei; + res.edge.indexB = 2 * j; - // Edge beats face, remove bias - absFaceBias = 0.0f; - - if ( s > speculativeDistance ) + if ( s > speculativeDistance && earlyReturn ) { + res.separatedFeature = b3_edgePairAxis; return res; } } @@ -1824,10 +1839,12 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* } // Attempt face contact using cached feature - b3FaceQuery faceQuery; + b3SeparatingAxis faceQuery; + faceQuery.normal = plane.normal; faceQuery.separation = 0.0f; - faceQuery.faceIndex = cache->indexA; - faceQuery.vertexIndex = vertexIndex; + faceQuery.indexA = cache->indexA; + faceQuery.indexB = vertexIndex; + faceQuery.type = b3_faceAxisA; b3SATCache localCache = { 0 }; bool touching = b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, &localCache ); @@ -1861,10 +1878,12 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* } // Attempt face contact using cached feature - b3FaceQuery faceQuery; + b3SeparatingAxis faceQuery; + faceQuery.normal = b3Neg( plane.normal ); faceQuery.separation = 0.0f; - faceQuery.faceIndex = cache->indexB; - faceQuery.vertexIndex = vertexIndex; + faceQuery.indexA = vertexIndex; + faceQuery.indexB = cache->indexB; + faceQuery.type = b3_faceAxisB; b3SATCache localCache = { 0 }; bool touching = b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, &localCache ); @@ -1933,11 +1952,12 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* } // Try to rebuild contact from last features - b3EdgeQuery edgeQuery = { 0 }; + b3SeparatingAxis edgeQuery = { 0 }; edgeQuery.normal = b3Neg( axis ); edgeQuery.separation = 0.0f; edgeQuery.indexA = cache->indexA; edgeQuery.indexB = cache->indexB; + edgeQuery.type = b3_edgePairAxis; b3SATCache localCache = { 0 }; bool touching = b3BuildEdgeContact( manifold, hullA, hullB, transformBtoA, edgeQuery, &localCache ); @@ -1957,13 +1977,8 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* // This case is for testing case b3_manualFaceAxisA: { - b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, b3_manualFaceAxisA ); - b3FaceQuery faceQuery = { - .separation = axisQuery.separation, - .faceIndex = axisQuery.indexA, - .vertexIndex = axisQuery.indexB, - }; - + b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, false ); + b3SeparatingAxis faceQuery = axisQuery.faceA; b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, cache ); return; } @@ -1971,12 +1986,8 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* // This case is for testing case b3_manualFaceAxisB: { - b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, b3_manualFaceAxisB ); - b3FaceQuery faceQuery = { - .separation = axisQuery.separation, - .faceIndex = axisQuery.indexB, - .vertexIndex = axisQuery.indexA, - }; + b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, false ); + b3SeparatingAxis faceQuery = axisQuery.faceB; b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, cache ); return; } @@ -1984,14 +1995,8 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* // This case is for testing case b3_manualEdgePairAxis: { - b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, b3_manualEdgePairAxis ); - b3EdgeQuery edgeQuery = { - .normal = axisQuery.normal, - .indexA = axisQuery.indexA, - .indexB = axisQuery.indexB, - .separation = axisQuery.separation, - }; - + b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, false ); + b3SeparatingAxis edgeQuery = axisQuery.edge; if ( edgeQuery.indexA != B3_NULL_INDEX ) { b3BuildEdgeContact( manifold, hullA, hullB, transformBtoA, edgeQuery, cache ); @@ -2007,74 +2012,94 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* manifold->pointCount = 0; *cache = (b3SATCache){ 0 }; - b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, b3_invalidAxis ); + b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, true ); - B3_VALIDATE( 0 <= axisQuery.indexA && axisQuery.indexA <= UINT8_MAX ); - B3_VALIDATE( 0 <= axisQuery.indexB && axisQuery.indexB <= UINT8_MAX ); - B3_ASSERT( axisQuery.type != b3_invalidAxis ); - - cache->separation = axisQuery.separation; - cache->type = (uint8_t)axisQuery.type; - cache->indexA = (uint8_t)axisQuery.indexA; - cache->indexB = (uint8_t)axisQuery.indexB; - - if ( axisQuery.separation > speculativeDistance ) + if ( axisQuery.separatedFeature != b3_invalidAxis ) { // We found a separating axis + cache->type = axisQuery.separatedFeature; + + if ( axisQuery.separatedFeature == b3_faceAxisA ) + { + B3_VALIDATE( axisQuery.faceA.separation > speculativeDistance ); + cache->separation = axisQuery.faceA.separation; + cache->indexA = (uint8_t)axisQuery.faceA.indexA; + cache->indexB = (uint8_t)axisQuery.faceA.indexB; + } + else if ( axisQuery.separatedFeature == b3_faceAxisB ) + { + B3_VALIDATE( axisQuery.faceB.separation > speculativeDistance ); + cache->separation = axisQuery.faceB.separation; + cache->indexA = (uint8_t)axisQuery.faceB.indexA; + cache->indexB = (uint8_t)axisQuery.faceB.indexB; + } + else + { + B3_ASSERT( axisQuery.separatedFeature == b3_edgePairAxis ); + B3_VALIDATE( axisQuery.edge.separation > speculativeDistance ); + cache->separation = axisQuery.edge.separation; + cache->indexA = (uint8_t)axisQuery.edge.indexA; + cache->indexB = (uint8_t)axisQuery.edge.indexB; + } return; } - if ( axisQuery.type == b3_faceAxisA ) - { - B3_ASSERT( axisQuery.indexA < hullA->faceCount ); - B3_ASSERT( axisQuery.indexB < hullB->vertexCount ); + B3_VALIDATE( axisQuery.faceA.separation <= speculativeDistance || axisQuery.faceB.separation <= speculativeDistance || + axisQuery.edge.separation <= speculativeDistance ); - b3FaceQuery faceQuery = { - .separation = axisQuery.separation, - .faceIndex = axisQuery.indexA, - .vertexIndex = axisQuery.indexB, - }; + if ( axisQuery.faceA.separation > axisQuery.faceB.separation ) + { + b3SeparatingAxis faceQuery = axisQuery.faceA; + B3_VALIDATE( 0 <= faceQuery.indexA && faceQuery.indexA < hullA->faceCount ); + B3_VALIDATE( 0 <= faceQuery.indexB && faceQuery.indexB < hullB->vertexCount ); // Face contact A b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, cache ); - return; + B3_VALIDATE( cache->indexA < hullA->faceCount ); + B3_VALIDATE( cache->indexB < hullB->vertexCount ); } - - if ( axisQuery.type == b3_faceAxisB ) + else { - B3_ASSERT( axisQuery.indexA < hullA->vertexCount ); - B3_ASSERT( axisQuery.indexB < hullB->faceCount ); - - b3FaceQuery faceQuery = { - .separation = axisQuery.separation, - .faceIndex = axisQuery.indexB, - .vertexIndex = axisQuery.indexA, - }; + b3SeparatingAxis faceQuery = axisQuery.faceB; + B3_VALIDATE( 0 <= faceQuery.indexA && faceQuery.indexA < hullA->vertexCount ); + B3_VALIDATE( 0 <= faceQuery.indexB && faceQuery.indexB < hullB->faceCount ); // Face contact B b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, cache ); + B3_VALIDATE( cache->indexA < hullA->vertexCount ); + B3_VALIDATE( cache->indexB < hullB->faceCount ); + } + + b3SeparatingAxis edgeQuery = axisQuery.edge; + + if ( edgeQuery.indexA == B3_NULL_INDEX ) + { + // There are no valid edge pairs (all edges parallel) return; } - B3_ASSERT( axisQuery.type == b3_edgePairAxis ); + float clipSeparation = cache->separation; + float edgeTol = linearSlop; + // Face contact can be empty if it does not realize the axis of minimum penetration. + // Create edge contact if face contact fails or edge contact is significantly better! + if ( manifold->pointCount == 0 || edgeQuery.separation > clipSeparation + edgeTol ) { + B3_ASSERT( 0 <= edgeQuery.indexA && edgeQuery.indexA < hullA->edgeCount ); + B3_ASSERT( 0 <= edgeQuery.indexB && edgeQuery.indexB < hullB->edgeCount ); + // Edge contact b3LocalManifold edgeManifold = { 0 }; b3LocalManifoldPoint edgePoint = { 0 }; edgeManifold.points = &edgePoint; - b3EdgeQuery edgeQuery = { - .normal = axisQuery.normal, - .indexA = axisQuery.indexA, - .indexB = axisQuery.indexB, - .separation = axisQuery.separation, - }; - - b3BuildEdgeContact( &edgeManifold, hullA, hullB, transformBtoA, edgeQuery, cache ); + b3SATCache edgeCache = { 0 }; + b3BuildEdgeContact( &edgeManifold, hullA, hullB, transformBtoA, edgeQuery, &edgeCache ); + // It is possible with speculation to have vertex-vertex collision that is missed by SAT, + // so edge contact yields no points. In that case perhaps the face contact has some points. if ( edgeManifold.pointCount == 1 ) { // Copy edge manifold out, being careful to preserve manifold point buffer. @@ -2082,12 +2107,30 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* *manifold = edgeManifold; manifold->points = points; manifold->points[0] = edgePoint; + *cache = edgeCache; } } } #else +// todo this code has gone stale, will be deleted soon + +typedef struct b3FaceQuery +{ + float separation; + int faceIndex; + int vertexIndex; +} b3FaceQuery; + +typedef struct b3EdgeQuery +{ + b3Vec3 normal; + float separation; + int indexA; + int indexB; +} b3EdgeQuery; + // Old non-SIMD version. Keeping this for testing and comparisons static b3FaceQuery b3QueryFaceDirections( const b3HullData* hullA, const b3HullData* hullB, b3Transform relativeTransform ) diff --git a/src/core.h b/src/core.h index 3aaf196..d4fdeb8 100644 --- a/src/core.h +++ b/src/core.h @@ -57,7 +57,12 @@ #define B3_SIMD_WIDTH 4 //#pragma message("B3_SIMD_SSE2") #elif defined( B3_CPU_ARM ) + // ARMv7 Neon doesn't have divide or sqrt so cannot be used. + #if defined( __aarch64__ ) || defined( _M_ARM64 ) #define B3_SIMD_NEON + #else + #define B3_SIMD_NONE + #endif #define B3_SIMD_WIDTH 4 //#pragma message("B3_SIMD_NEON") #elif defined( B3_CPU_WASM ) diff --git a/src/manifold.c b/src/manifold.c index 186d696..f15db6c 100644 --- a/src/manifold.c +++ b/src/manifold.c @@ -17,6 +17,8 @@ // Then from that edge, we select the adjacent face that is most anti-parallel to the reference normal. int b3FindIncidentFace( const b3HullData* hull, b3Vec3 refNormal, int vertexIndex ) { + B3_ASSERT( 0 <= vertexIndex && vertexIndex < hull->vertexCount ); + const b3HullVertex* vertices = b3GetHullVertices( hull ); const b3HullHalfEdge* edges = b3GetHullEdges( hull ); const b3Plane* planes = b3GetHullPlanes( hull ); diff --git a/src/manifold.h b/src/manifold.h index 2eebbda..fca3cde 100644 --- a/src/manifold.h +++ b/src/manifold.h @@ -8,28 +8,21 @@ #define B3_MAX_CLIP_POINTS 64 -typedef struct b3FaceQuery -{ - float separation; - int faceIndex; - int vertexIndex; -} b3FaceQuery; - -typedef struct b3EdgeQuery -{ - b3Vec3 normal; - float separation; - int indexA; - int indexB; -} b3EdgeQuery; - -typedef struct b3AxisQuery +typedef struct b3SeparatingAxis { b3Vec3 normal; float separation; int indexA; int indexB; b3SeparatingFeature type; +} b3SeparatingAxis; + +typedef struct b3AxisQuery +{ + b3SeparatingAxis faceA; + b3SeparatingAxis faceB; + b3SeparatingAxis edge; + b3SeparatingFeature separatedFeature; } b3AxisQuery; typedef struct b3ClipVertex @@ -52,7 +45,7 @@ b3FeaturePair b3FlipPair( b3FeaturePair pair ); int b3ClipPolygon( b3ClipVertex* out, b3ClipVertex* polygon, int count, b3Plane clipPlane, int edge, b3Plane refPlane ); -b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, int axisOverride ); +b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, bool earlyReturn ); #if B3_ENABLE_VALIDATION bool b3ValidatePolygon( b3ClipVertex* polygon, int count ); @@ -66,3 +59,27 @@ static inline uint32_t b3MakeFeatureId( b3FeaturePair pair ) return ( (uint32_t)pair.owner1 << 24 ) | ( (uint32_t)pair.index1 << 16 ) | ( (uint32_t)pair.owner2 << 8 ) | (uint32_t)pair.index2; } + +static inline b3SeparatingAxis b3GetBestAxis( const b3AxisQuery* query ) +{ + B3_VALIDATE( query->faceA.type == b3_faceAxisA ); + B3_VALIDATE( query->edge.type == b3_edgePairAxis ); + B3_VALIDATE( query->faceB.type == b3_faceAxisB ); + + if ( query->faceA.separation > query->faceB.separation ) + { + if ( query->edge.separation > query->faceA.separation ) + { + return query->edge; + } + + return query->faceA; + } + + if ( query->edge.separation > query->faceB.separation ) + { + return query->edge; + } + + return query->faceB; +} diff --git a/src/mesh_contact.c b/src/mesh_contact.c index a716347..60158e4 100644 --- a/src/mesh_contact.c +++ b/src/mesh_contact.c @@ -608,7 +608,7 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact switch ( shapeB->type ) { case b3_capsuleShape: - b3CollideCapsuleAndTriangle( manifold, pointCapacity, &shapeB->capsule, vertices, &cache->simplexCache ); + b3CollideTriangleAndCapsule( manifold, pointCapacity, vertices, &shapeB->capsule, &cache->simplexCache ); break; case b3_hullShape: @@ -619,14 +619,14 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact cache->satCache = (b3SATCache){ 0 }; } - b3CollideHullAndTriangle( manifold, pointCapacity, hullB, vertices[0], vertices[1], vertices[2], triangle.flags, + b3CollideTriangleAndHull( manifold, pointCapacity, vertices[0], vertices[1], vertices[2], triangle.flags, hullB, &cache->satCache, enableSpeculative ); context->satCallCount += 1; context->satCacheHitCount += cache->satCache.hit; break; case b3_sphereShape: - b3CollideSphereAndTriangle( manifold, pointCapacity, &shapeB->sphere, vertices ); + b3CollideTriangleAndSphere( manifold, pointCapacity, vertices, &shapeB->sphere ); break; default: diff --git a/src/solver.c b/src/solver.c index c0db8eb..d8aace0 100644 --- a/src/solver.c +++ b/src/solver.c @@ -24,6 +24,8 @@ #include #include +_Static_assert( B3_RESTITUTION_ITERATIONS >= 1, "must be 1 or more" ); + // these are useful for solver testing #define ITERATIONS 1 #define RELAX_ITERATIONS 1 @@ -1313,6 +1315,7 @@ static void b3SolverTask( void* taskContext ) stageIndex += 1 + activeColorCount + ITERATIONS * activeColorCount + 1 + RELAX_ITERATIONS * activeColorCount; // Restitution + for ( int iteration = 0; iteration < B3_RESTITUTION_ITERATIONS; ++iteration ) { b3ApplyRestitution_Overflow( context ); @@ -1324,7 +1327,7 @@ static void b3SolverTask( void* taskContext ) b3ExecuteMainStage( stages + iterStageIndex, context, syncBits ); iterStageIndex += 1; } - // graphSyncIndex += 1; + graphSyncIndex += 1; stageIndex += activeColorCount; } @@ -1690,7 +1693,7 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) // b3_stageRelax stageCount += RELAX_ITERATIONS * activeColorCount; // b3_stageRestitution - stageCount += activeColorCount; + stageCount += B3_RESTITUTION_ITERATIONS * activeColorCount; // b3_stageStoreWideImpulses stageCount += 1; // b3_stageStoreImpulses @@ -1775,8 +1778,8 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) stage = b3InitColorStages( stage, b3_stageRelax, RELAX_ITERATIONS, activeColorCount, graphColorBlocks, graphBlockCounts, activeColorIndices ); // Note: joint blocks mixed in, could have joint limit restitution - stage = b3InitColorStages( stage, b3_stageRestitution, 1, activeColorCount, graphColorBlocks, graphBlockCounts, - activeColorIndices ); + stage = b3InitColorStages( stage, b3_stageRestitution, B3_RESTITUTION_ITERATIONS, activeColorCount, graphColorBlocks, + graphBlockCounts, activeColorIndices ); stage = b3InitStage( stage, b3_stageStoreWideImpulses, convexBlocks, convexPrepareDim.count, UINT8_MAX ); stage = b3InitStage( stage, b3_stageStoreImpulses, meshBlocks, meshPrepareDim.count, UINT8_MAX ); diff --git a/src/timer.c b/src/timer.c index 726fa18..f779e3b 100644 --- a/src/timer.c +++ b/src/timer.c @@ -23,7 +23,8 @@ #define WIN32_LEAN_AND_MEAN 1 #endif -#include +// Lower-case windows.h intentionally for cross compiling on mingw. +#include #include static double s_invFrequency = 0.0; diff --git a/src/triangle_manifold.c b/src/triangle_manifold.c index b7432b0..c702540 100644 --- a/src/triangle_manifold.c +++ b/src/triangle_manifold.c @@ -50,7 +50,7 @@ static b3TriangleFeature b3GetTriangleFeature( const b3SimplexCache* cache ) return s_triangleFeatures[mask]; } -void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const b3Sphere* sphereA, const b3Vec3* triangleB ) +void b3CollideTriangleAndSphere( b3LocalManifold* manifold, int capacity, const b3Vec3* triangleA, const b3Sphere* sphereB ) { manifold->pointCount = 0; @@ -59,8 +59,8 @@ void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const return; } - b3Vec3 center = sphereA->center; - b3Vec3 v1 = triangleB[0], v2 = triangleB[1], v3 = triangleB[2]; + b3Vec3 center = sphereB->center; + b3Vec3 v1 = triangleA[0], v2 = triangleA[1], v3 = triangleA[2]; b3Plane plane = b3MakePlaneFromPoints( v1, v2, v3 ); float offset = b3PlaneSeparation( plane, center ); @@ -70,19 +70,23 @@ void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const return; } + float radius = sphereB->radius; + // Closest point on triangle to sphere center b3TrianglePoint closest = b3ClosestPointOnTriangle( v1, v2, v3, center ); // Test separating axis float squaredDistance = b3DistanceSquared( closest.point, center ); float speculativeDistance = B3_SPECULATIVE_DISTANCE; - float maxDistance = sphereA->radius + speculativeDistance; + float maxDistance = radius + speculativeDistance; if ( squaredDistance > maxDistance * maxDistance ) { return; } float distance = sqrtf( squaredDistance ); + + // Normal points from triangle to sphere b3Vec3 normal; if ( distance * distance > 1000.0f * FLT_MIN ) { @@ -94,7 +98,8 @@ void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const } // contact point mid-way - b3Vec3 contactPoint = b3MulSV( 0.5f, b3Add( b3Sub( center, b3MulSV( sphereA->radius, normal ) ), closest.point ) ); + // p = 0.5 * (c + q) - 0.5 * r * n + b3Vec3 contactPoint = b3MulSV( 0.5f, b3Add( b3Sub( center, b3MulSV( radius, normal ) ), closest.point ) ); manifold->normal = normal; manifold->pointCount = 1; @@ -103,7 +108,7 @@ void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const b3LocalManifoldPoint* mp = manifold->points + 0; mp->point = contactPoint; - mp->separation = distance - sphereA->radius; + mp->separation = distance - radius; mp->pair = b3FeaturePair_single; } @@ -156,28 +161,30 @@ static bool b3ClipSegmentToTriangleFace( b3ClipVertex segment[2], const b3Vec3* return true; } -static b3FaceQuery b3QueryTriangleFaceAndCapsule( b3Plane plane, const b3Capsule* capsule ) +static b3SeparatingAxis b3QueryTriangleFaceAndCapsule( b3Plane plane, const b3Capsule* capsule ) { float separation1 = b3PlaneSeparation( plane, capsule->center1 ); float separation2 = b3PlaneSeparation( plane, capsule->center2 ); if ( separation1 < separation2 ) { - return (b3FaceQuery){ + return (b3SeparatingAxis){ + .normal = plane.normal, .separation = separation1, - .faceIndex = 0, - .vertexIndex = 0, + .indexA = 0, + .indexB = 0, }; } - return (b3FaceQuery){ + return (b3SeparatingAxis){ + .normal = plane.normal, .separation = separation2, - .faceIndex = 0, - .vertexIndex = 1, + .indexA = 0, + .indexB = 1, }; } -static b3EdgeQuery b3QueryTriangleAndCapsuleEdges( const b3Vec3* vertices, b3Plane plane, const b3Capsule* capsule ) +static b3SeparatingAxis b3QueryTriangleAndCapsuleEdges( const b3Vec3* vertices, b3Plane plane, const b3Capsule* capsule ) { // Work in the local space of the capsule b3Vec3 p1 = capsule->center1; @@ -243,7 +250,7 @@ static b3EdgeQuery b3QueryTriangleAndCapsuleEdges( const b3Vec3* vertices, b3Pla } // Save result - return (b3EdgeQuery){ + return (b3SeparatingAxis){ .normal = maxNormal, .separation = maxSeparation, .indexA = maxIndex1, @@ -300,7 +307,7 @@ static void b3BuildTriangleAndCapsuleFaceContact( b3LocalManifold* manifold, con } static void b3BuildTriangleAndCapsuleEdgeContact( b3LocalManifold* manifold, const b3Vec3* triangle, b3Plane plane, - const b3Capsule* capsule, b3EdgeQuery query ) + const b3Capsule* capsule, b3SeparatingAxis query ) { B3_ASSERT( 0 <= query.indexA && query.indexA < 3 ); @@ -355,7 +362,7 @@ static void b3BuildTriangleAndCapsuleEdgeContact( b3LocalManifold* manifold, con pt->pair = b3MakeFeaturePair( b3_featureShapeA, query.indexA, b3_featureShapeB, query.indexB ); } -void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA, const b3Vec3* triangleB, +void b3CollideTriangleAndCapsule( b3LocalManifold* manifold, int capacity, const b3Vec3* triangleA, const b3Capsule* capsuleB, b3SimplexCache* cache ) { manifold->pointCount = 0; @@ -365,9 +372,9 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const return; } - b3Vec3 v1 = triangleB[0], v2 = triangleB[1], v3 = triangleB[2]; + b3Vec3 v1 = triangleA[0], v2 = triangleA[1], v3 = triangleA[2]; b3Plane plane = b3MakePlaneFromPoints( v1, v2, v3 ); - b3Vec3 capsuleCenter = b3Lerp( capsuleA->center1, capsuleA->center2, 0.5f ); + b3Vec3 capsuleCenter = b3Lerp( capsuleB->center1, capsuleB->center2, 0.5f ); float offset = b3PlaneSeparation( plane, capsuleCenter ); if ( offset < 0.0f ) @@ -377,14 +384,14 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const } b3DistanceInput distanceInput; - distanceInput.proxyA = (b3ShapeProxy){ triangleB, 3, 0.0f }; - distanceInput.proxyB = (b3ShapeProxy){ &capsuleA->center1, 2, 0.0f }; + distanceInput.proxyA = (b3ShapeProxy){ triangleA, 3, 0.0f }; + distanceInput.proxyB = (b3ShapeProxy){ &capsuleB->center1, 2, 0.0f }; distanceInput.transform = b3Transform_identity; distanceInput.useRadii = false; b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, cache, NULL, 0 ); - float radius = capsuleA->radius; + float radius = capsuleB->radius; if ( distanceOutput.distance > radius + B3_SPECULATIVE_DISTANCE ) { // Shapes are separated, persist the cache @@ -403,14 +410,14 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const { // Clip capsule segment against side planes of reference face b3ClipVertex segment[2]; - segment[0].position = capsuleA->center1; + segment[0].position = capsuleB->center1; segment[0].separation = 0.0f; segment[0].pair = b3MakeFeaturePair( b3_featureShapeA, 0, b3_featureShapeA, 0 ); - segment[1].position = capsuleA->center2; + segment[1].position = capsuleB->center2; segment[1].separation = 0.0f; segment[1].pair = b3MakeFeaturePair( b3_featureShapeA, 1, b3_featureShapeA, 1 ); - bool havePoints = b3ClipSegmentToTriangleFace( segment, triangleB, plane ); + bool havePoints = b3ClipSegmentToTriangleFace( segment, triangleA, plane ); if ( havePoints == true ) { @@ -421,6 +428,7 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const b3Vec3 point1 = b3MulSub( segment[0].position, 0.5f * ( radius + distance1 ), normal ); b3Vec3 point2 = b3MulSub( segment[1].position, 0.5f * ( radius + distance2 ), normal ); + // Normal points from triangle to capsule manifold->normal = normal; manifold->feature = b3_featureTriangleFace; manifold->pointCount = 2; @@ -439,9 +447,10 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const } } - // Create contact from closest points + // Create contact from closest points. b3Vec3 point = b3MulSV( 0.5f, b3Add( b3Sub( distanceOutput.pointA, b3MulSV( radius, delta ) ), distanceOutput.pointB ) ); + // Normal points from triangle to capsule. manifold->normal = delta; manifold->pointCount = 1; manifold->feature = b3GetTriangleFeature( cache ); @@ -456,14 +465,14 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const // Deep penetration - b3FaceQuery faceQuery = b3QueryTriangleFaceAndCapsule( plane, capsuleA ); + b3SeparatingAxis faceQuery = b3QueryTriangleFaceAndCapsule( plane, capsuleB ); if ( faceQuery.separation > radius ) { // Shapes are separated return; } - b3EdgeQuery edgeQuery = b3QueryTriangleAndCapsuleEdges( triangleB, plane, capsuleA ); + b3SeparatingAxis edgeQuery = b3QueryTriangleAndCapsuleEdges( triangleA, plane, capsuleB ); if ( edgeQuery.separation > radius ) { // Shapes are separated @@ -472,7 +481,7 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const // Create face contact float faceSeparation = faceQuery.separation - radius; - b3BuildTriangleAndCapsuleFaceContact( manifold, triangleB, plane, capsuleA ); + b3BuildTriangleAndCapsuleFaceContact( manifold, triangleA, plane, capsuleB ); if ( manifold->pointCount == 2 ) { faceSeparation = b3MinFloat( manifold->points[0].separation, manifold->points[1].separation ); @@ -487,7 +496,7 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const if ( manifold->pointCount == 0 || edgeSeparation > kRelEdgeTolerance * faceSeparation + kAbsTolerance ) { // Edge contact - b3BuildTriangleAndCapsuleEdgeContact( manifold, triangleB, plane, capsuleA, edgeQuery ); + b3BuildTriangleAndCapsuleEdgeContact( manifold, triangleA, plane, capsuleB, edgeQuery ); } } @@ -512,31 +521,35 @@ static inline int b3GetTriangleSupport( b3Vec3* points, b3Vec3 direction ) return index; } -static b3FaceQuery b3QueryTriangleFace( const b3TriangleData* triangle, const b3HullData* hull ) +static b3SeparatingAxis b3QueryTriangleFace( const b3TriangleData* triangle, const b3HullData* hull ) { const b3Vec3* hullPoints = b3GetHullPoints( hull ); b3Plane plane = triangle->plane; - int vertexIndex = b3FindHullSupportVertex( hull, b3Neg( plane.normal ) ); + b3Vec3 normal = b3Neg( plane.normal ); + int vertexIndex = b3FindHullSupportVertex( hull, normal ); b3Vec3 support = hullPoints[vertexIndex]; float separation = b3PlaneSeparation( plane, support ); - return (b3FaceQuery){ + return (b3SeparatingAxis){ + .normal = plane.normal, .separation = separation, - .faceIndex = 0, - .vertexIndex = (uint8_t)vertexIndex, + .indexA = 0, + .indexB = vertexIndex, + .type = b3_faceAxisA, }; } -static b3FaceQuery b3QueryHullFace( const b3TriangleData* triangle, const b3HullData* hull ) +static b3SeparatingAxis b3QueryHullFace( const b3TriangleData* triangle, const b3HullData* hull ) { const b3Plane* hullPlanes = b3GetHullPlanes( hull ); int faceCount = hull->faceCount; b3Vec3 trianglePoints[] = { triangle->v1, triangle->v2, triangle->v3 }; - int maxFaceIndex = -1; - int maxVertexIndex = -1; - float maxFaceSeparation = -FLT_MAX; + b3Vec3 maxNormal = b3Vec3_zero; + float maxFaceSeparation = -INFINITY; + int maxFaceIndex = B3_NULL_INDEX; + int maxVertexIndex = B3_NULL_INDEX; for ( int faceIndex = 0; faceIndex < faceCount; ++faceIndex ) { @@ -547,26 +560,32 @@ static b3FaceQuery b3QueryHullFace( const b3TriangleData* triangle, const b3Hull float separation = b3PlaneSeparation( plane, support ); if ( separation > maxFaceSeparation ) { + maxNormal = plane.normal; + maxFaceSeparation = separation; maxFaceIndex = faceIndex; maxVertexIndex = vertexIndex; - maxFaceSeparation = separation; } } - return (b3FaceQuery){ + // Normal points from triangle to hull + return (b3SeparatingAxis){ + .normal = b3Neg( maxNormal ), .separation = maxFaceSeparation, - .faceIndex = maxFaceIndex, - .vertexIndex = maxVertexIndex, + .indexA = maxVertexIndex, + .indexB = maxFaceIndex, + .type = b3_faceAxisB, }; } -static b3EdgeQuery b3QueryTriangleAndHullEdges( const b3TriangleData* triangle, const b3HullData* hull ) +// A: hull, B: triangle +static b3SeparatingAxis b3QueryTriangleAndHullEdges( const b3TriangleData* triangle, const b3HullData* hull ) { - b3EdgeQuery result = { + b3SeparatingAxis result = { .normal = b3Vec3_zero, - .separation = -FLT_MAX, + .separation = -INFINITY, .indexA = B3_NULL_INDEX, .indexB = B3_NULL_INDEX, + .type = b3_edgePairAxis, }; b3Vec3 trianglePoints[] = { triangle->v1, triangle->v2, triangle->v3 }; @@ -645,8 +664,12 @@ static b3EdgeQuery b3QueryTriangleAndHullEdges( const b3TriangleData* triangle, } static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, const b3TriangleData* triangle, - const b3HullData* hull, b3FaceQuery query, b3SATCache* cache, bool enableSpeculative ) + const b3HullData* hull, b3SeparatingAxis query, b3SATCache* cache, bool enableSpeculative ) { + B3_VALIDATE( query.type == b3_faceAxisB ); + B3_VALIDATE( 0 <= query.indexA && query.indexA < 3 ); + B3_VALIDATE( 0 <= query.indexB && query.indexB < hull->faceCount ); + manifold->pointCount = 0; const b3HullFace* hullFaces = b3GetHullFaces( hull ); @@ -655,8 +678,7 @@ static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, co const b3Vec3* hullPoints = b3GetHullPoints( hull ); // Reference hull face - int refFace = query.faceIndex; - b3Plane refPlane = hullPlanes[refFace]; + b3Plane refPlane = hullPlanes[query.indexB]; // Build clip polygon from triangle face (the incident face) b3ClipVertex buffer1[B3_MAX_CLIP_POINTS], buffer2[B3_MAX_CLIP_POINTS]; @@ -679,7 +701,7 @@ static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, co b3ClipVertex* input = buffer1; b3ClipVertex* output = buffer2; - const b3HullFace* face = hullFaces + refFace; + const b3HullFace* face = hullFaces + query.indexB; int edgeIndex = face->edge; do @@ -751,14 +773,17 @@ static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, co // Save cache cache->separation = minSeparation; cache->type = b3_faceAxisB; - cache->indexA = (uint8_t)query.vertexIndex; - cache->indexB = (uint8_t)query.faceIndex; + cache->indexA = (uint8_t)query.indexA; + cache->indexB = (uint8_t)query.indexB; return minSeparation; } static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity, const b3TriangleData* triangle, - const b3HullData* hull, b3FaceQuery query, b3SATCache* cache, bool enableSpeculative ) + const b3HullData* hull, b3SeparatingAxis query, b3SATCache* cache, bool enableSpeculative ) { + B3_VALIDATE( query.type == b3_faceAxisA ); + B3_VALIDATE( query.indexA == 0 ); + B3_VALIDATE( 0 <= query.indexB && query.indexB < hull->vertexCount ); B3_VALIDATE( manifold->pointCount == 0 ); const b3HullFace* hullFaces = b3GetHullFaces( hull ); @@ -766,10 +791,9 @@ static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity const b3Vec3* hullPoints = b3GetHullPoints( hull ); // Find incident face - B3_ASSERT( query.faceIndex == 0 ); b3Plane refPlane = triangle->plane; - int incFace = b3FindIncidentFace( hull, refPlane.normal, query.vertexIndex ); + int incFace = b3FindIncidentFace( hull, refPlane.normal, query.indexB ); // Build clip polygon from incident face b3ClipVertex buffer1[2 * B3_MAX_CLIP_POINTS], buffer2[2 * B3_MAX_CLIP_POINTS]; @@ -865,16 +889,18 @@ static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity // Save cache cache->separation = minSeparation; cache->type = b3_faceAxisA; - cache->indexA = (uint8_t)query.faceIndex; - cache->indexB = (uint8_t)query.vertexIndex; + cache->indexA = (uint8_t)query.indexA; + cache->indexB = (uint8_t)query.indexB; return minSeparation; } -static void b3CollideHullAndTriangleEdges( b3LocalManifold* manifold, int capacity, b3Vec3 trianglePoint, b3Vec3 triangleEdge, - const b3HullData* hull, b3EdgeQuery query, b3SATCache* cache ) +static void b3CollideTriangleAndHullEdges( b3LocalManifold* manifold, int capacity, b3Vec3 trianglePoint, b3Vec3 triangleEdge, + const b3HullData* hull, b3SeparatingAxis query, b3SATCache* cache ) { + B3_VALIDATE( query.type == b3_edgePairAxis ); + B3_VALIDATE( 0 <= query.indexA && query.indexA < 3 ); + B3_VALIDATE( 0 <= query.indexB && query.indexB < hull->edgeCount ); B3_VALIDATE( query.separation <= 2.0f * B3_SPECULATIVE_DISTANCE ); - B3_ASSERT( query.indexA < 3 ); b3Vec3 pA = trianglePoint; b3Vec3 eA = triangleEdge; @@ -927,9 +953,9 @@ static void b3CollideHullAndTriangleEdges( b3LocalManifold* manifold, int capaci b3AtomicInt b3_triangleConvexCalls; b3AtomicInt b3_triangleCacheHits; -// Computes the manifold in the local space of the hull -void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, - int triangleFlags, b3SATCache* cache, bool enableSpeculative ) +// Triangle is in the local space of the hull for efficiency. +void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, int triangleFlags, + const b3HullData* hullB, b3SATCache* cache, bool enableSpeculative ) { manifold->pointCount = 0; manifold->feature = b3_featureNone; @@ -942,7 +968,7 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 b3Plane trianglePlane = b3MakePlaneFromPoints( v1, v2, v3 ); float linearSlop = B3_LINEAR_SLOP; - float offset = b3PlaneSeparation( trianglePlane, hullA->center ); + float offset = b3PlaneSeparation( trianglePlane, hullB->center ); if ( cache->type == b3_backsideAxis ) { // Use hysteresis to avoid jitter on wavy meshes @@ -976,9 +1002,9 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 .flags = triangleFlags, }; - const b3HullHalfEdge* edges = b3GetHullEdges( hullA ); - const b3Plane* hullPlanes = b3GetHullPlanes( hullA ); - const b3Vec3* hullPoints = b3GetHullPoints( hullA ); + const b3HullHalfEdge* edges = b3GetHullEdges( hullB ); + const b3Plane* hullPlanes = b3GetHullPlanes( hullB ); + const b3Vec3* hullPoints = b3GetHullPoints( hullB ); float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f; cache->hit = 1; @@ -990,7 +1016,7 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 { B3_ASSERT( cache->indexA == 0 ); - int vertexIndex = b3FindHullSupportVertex( hullA, b3Neg( trianglePlane.normal ) ); + int vertexIndex = b3FindHullSupportVertex( hullB, b3Neg( trianglePlane.normal ) ); b3Vec3 support = hullPoints[vertexIndex]; float separation = b3PlaneSeparation( trianglePlane, support ); if ( separation > speculativeDistance ) @@ -999,15 +1025,17 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 return; } - b3FaceQuery faceQuery; + b3SeparatingAxis faceQuery; + faceQuery.normal = trianglePlane.normal; faceQuery.separation = separation; - faceQuery.faceIndex = cache->indexA; - faceQuery.vertexIndex = vertexIndex; + faceQuery.indexA = cache->indexA; + faceQuery.indexB = vertexIndex; + faceQuery.type = b3_faceAxisA; // Read cache but don't modify it b3SATCache localCache = *cache; float clippedSeparation = - b3CollideTriangleFace( manifold, capacity, &triangle, hullA, faceQuery, &localCache, enableSpeculative ); + b3CollideTriangleFace( manifold, capacity, &triangle, hullB, faceQuery, &localCache, enableSpeculative ); if ( manifold->pointCount > 0 && b3AbsFloat( cache->separation - clippedSeparation ) < linearSlop ) { @@ -1023,7 +1051,7 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 case b3_faceAxisB: { - B3_ASSERT( cache->indexB < hullA->faceCount ); + B3_ASSERT( cache->indexB < hullB->faceCount ); b3Plane plane = hullPlanes[cache->indexB]; @@ -1058,15 +1086,17 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 if ( isDeep == false ) { // Try to rebuild contact from last features - b3FaceQuery faceQuery; + b3SeparatingAxis faceQuery; + faceQuery.normal = b3Neg( plane.normal ); faceQuery.separation = separation; - faceQuery.faceIndex = cache->indexB; - faceQuery.vertexIndex = vertexIndex; + faceQuery.indexA = vertexIndex; + faceQuery.indexB = cache->indexB; + faceQuery.type = b3_faceAxisB; // Read cache but don't modify it b3SATCache localCache = *cache; float clippedSeparation = - b3CollideHullFace( manifold, capacity, &triangle, hullA, faceQuery, &localCache, enableSpeculative ); + b3CollideHullFace( manifold, capacity, &triangle, hullB, faceQuery, &localCache, enableSpeculative ); // Cache reuse is only successful if it creates contact points and the clipped separation didn't change much. if ( manifold->pointCount > 0 && b3AbsFloat( cache->separation - clippedSeparation ) < linearSlop ) @@ -1090,7 +1120,7 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 b3Vec3 triPoint = trianglePoints[indexA]; b3Vec3 triEdge = triangleEdges[indexA]; - B3_ASSERT( cache->indexB < hullA->edgeCount - 1 ); + B3_ASSERT( cache->indexB < hullB->edgeCount - 1 ); int indexB = cache->indexB; const b3HullHalfEdge* edge2 = edges + indexB; @@ -1134,15 +1164,16 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 { // Try to rebuild contact from last features // Flip normal to point from triangle to hull - b3EdgeQuery edgeQuery; + b3SeparatingAxis edgeQuery; edgeQuery.normal = b3Neg( axis ); edgeQuery.indexA = indexA; edgeQuery.indexB = indexB; edgeQuery.separation = separation; + edgeQuery.type = b3_edgePairAxis; // Read cache but don't modify it b3SATCache localCache = *cache; - b3CollideHullAndTriangleEdges( manifold, capacity, triPoint, triEdge, hullA, edgeQuery, &localCache ); + b3CollideTriangleAndHullEdges( manifold, capacity, triPoint, triEdge, hullB, edgeQuery, &localCache ); if ( manifold->pointCount > 0 ) { @@ -1161,28 +1192,28 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 // This case is for testing case b3_manualFaceAxisA: { - b3FaceQuery faceQueryA = b3QueryTriangleFace( &triangle, hullA ); - b3CollideTriangleFace( manifold, capacity, &triangle, hullA, faceQueryA, cache, enableSpeculative ); + b3SeparatingAxis query = b3QueryTriangleFace( &triangle, hullB ); + b3CollideTriangleFace( manifold, capacity, &triangle, hullB, query, cache, enableSpeculative ); return; } // This case is for testing case b3_manualFaceAxisB: { - b3FaceQuery faceQueryB = b3QueryHullFace( &triangle, hullA ); - b3CollideHullFace( manifold, capacity, &triangle, hullA, faceQueryB, cache, enableSpeculative ); + b3SeparatingAxis query = b3QueryHullFace( &triangle, hullB ); + b3CollideHullFace( manifold, capacity, &triangle, hullB, query, cache, enableSpeculative ); return; } // This case is for testing case b3_manualEdgePairAxis: { - b3EdgeQuery edgeQuery = b3QueryTriangleAndHullEdges( &triangle, hullA ); - if ( edgeQuery.indexA != B3_NULL_INDEX ) + b3SeparatingAxis query = b3QueryTriangleAndHullEdges( &triangle, hullB ); + if ( query.indexA != B3_NULL_INDEX ) { - b3Vec3 trianglePoint = trianglePoints[edgeQuery.indexA]; - b3Vec3 triangleEdge = triangleEdges[edgeQuery.indexA]; - b3CollideHullAndTriangleEdges( manifold, capacity, trianglePoint, triangleEdge, hullA, edgeQuery, cache ); + b3Vec3 trianglePoint = trianglePoints[query.indexA]; + b3Vec3 triangleEdge = triangleEdges[query.indexA]; + b3CollideTriangleAndHullEdges( manifold, capacity, trianglePoint, triangleEdge, hullB, query, cache ); } return; } @@ -1196,29 +1227,29 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 cache->hit = 0; // Find axis of minimum penetration - b3FaceQuery faceQueryA = b3QueryTriangleFace( &triangle, hullA ); + b3SeparatingAxis faceQueryA = b3QueryTriangleFace( &triangle, hullB ); if ( faceQueryA.separation > speculativeDistance ) { // Separating axis found cache->separation = faceQueryA.separation; cache->type = b3_faceAxisA; - cache->indexA = 0; - cache->indexB = UINT8_MAX; + cache->indexA = (uint8_t)faceQueryA.indexA; + cache->indexB = (uint8_t)faceQueryA.indexB; return; } - b3FaceQuery faceQueryB = b3QueryHullFace( &triangle, hullA ); + b3SeparatingAxis faceQueryB = b3QueryHullFace( &triangle, hullB ); if ( faceQueryB.separation > speculativeDistance ) { // Separating axis found cache->separation = faceQueryB.separation; cache->type = b3_faceAxisB; - cache->indexA = UINT8_MAX; - cache->indexB = (uint8_t)faceQueryB.faceIndex; + cache->indexA = (uint8_t)faceQueryB.indexA; + cache->indexB = (uint8_t)faceQueryB.indexB; return; } - b3EdgeQuery edgeQuery = b3QueryTriangleAndHullEdges( &triangle, hullA ); + b3SeparatingAxis edgeQuery = b3QueryTriangleAndHullEdges( &triangle, hullB ); if ( edgeQuery.separation > speculativeDistance ) { // Separating axis found @@ -1229,20 +1260,18 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 return; } - float clippedFaceSeparation; + float clipSeparation; // Don't admit a hull face significantly opposed to the triangle face. // Need a tolerance to avoid ghost collisions. - b3Vec3 hullNormal = hullPlanes[faceQueryB.faceIndex].normal; - bool pushingDown = b3Dot( hullNormal, trianglePlane.normal ) > 0.25f; + bool pushingDown = b3Dot( faceQueryB.normal, trianglePlane.normal ) < -0.25f; if ( faceQueryB.separation >= faceQueryA.separation && pushingDown == false ) { - clippedFaceSeparation = b3CollideHullFace( manifold, capacity, &triangle, hullA, faceQueryB, cache, enableSpeculative ); + clipSeparation = b3CollideHullFace( manifold, capacity, &triangle, hullB, faceQueryB, cache, enableSpeculative ); } else { - clippedFaceSeparation = - b3CollideTriangleFace( manifold, capacity, &triangle, hullA, faceQueryA, cache, enableSpeculative ); + clipSeparation = b3CollideTriangleFace( manifold, capacity, &triangle, hullB, faceQueryA, cache, enableSpeculative ); } // Does an edge axis exist? @@ -1253,13 +1282,13 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 float maxFaceSeparation = b3MaxFloat( faceQueryA.separation, faceQueryB.separation ); if ( ( manifold->pointCount == 0 && edgeQuery.separation > maxFaceSeparation ) || - ( manifold->pointCount == 1 && edgeQuery.separation > clippedFaceSeparation + linearSlop ) ) + ( manifold->pointCount == 1 && edgeQuery.separation > clipSeparation + linearSlop ) ) { B3_ASSERT( 0 <= edgeQuery.indexA && edgeQuery.indexA < 3 ); b3Vec3 trianglePoint = trianglePoints[edgeQuery.indexA]; b3Vec3 triangleEdge = triangleEdges[edgeQuery.indexA]; manifold->pointCount = 0; - b3CollideHullAndTriangleEdges( manifold, capacity, trianglePoint, triangleEdge, hullA, edgeQuery, cache ); + b3CollideTriangleAndHullEdges( manifold, capacity, trianglePoint, triangleEdge, hullB, edgeQuery, cache ); } } @@ -1274,7 +1303,7 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 .count = 3, .radius = 0.0f, }; - input.proxyB = (b3ShapeProxy){ .points = hullPoints, .count = hullA->vertexCount, .radius = 0.0f }; + input.proxyB = (b3ShapeProxy){ .points = hullPoints, .count = hullB->vertexCount, .radius = 0.0f }; input.transform = b3Transform_identity; input.useRadii = false; @@ -1294,5 +1323,8 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 // This feature pair not accurate but maybe it doesn't matter manifold->points[0].pair = b3FeaturePair_single; } + + // No way to cache this scenario + *cache = (b3SATCache){ 0 }; } } diff --git a/test/test_determinism.c b/test/test_determinism.c index af35f95..cb21203 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -20,8 +20,8 @@ #if defined( BOX3D_DOUBLE_PRECISION ) #define RAGDOLL_SLEEP_STEP 297 #define RAGDOLL_HASH 0x27FF38C1 -#define WAVE_PILE_SLEEP_STEP 258 -#define WAVE_PILE_HASH 0xA3CDC61B +#define WAVE_PILE_SLEEP_STEP 297 +#define WAVE_PILE_HASH 0x420CA784 #define QUERY_SPAWN_SLEEP_STEP 242 #define QUERY_SPAWN_HASH 0x1737F5BC #define QUERY_SPAWN_HIT_COUNT 59 @@ -31,8 +31,8 @@ #else #define RAGDOLL_SLEEP_STEP 308 #define RAGDOLL_HASH 0x1E5EDD79 -#define WAVE_PILE_SLEEP_STEP 239 -#define WAVE_PILE_HASH 0xA2F4D472 +#define WAVE_PILE_SLEEP_STEP 273 +#define WAVE_PILE_HASH 0x47233541 #define QUERY_SPAWN_SLEEP_STEP 242 #define QUERY_SPAWN_HASH 0xB9F993A5 #define QUERY_SPAWN_HIT_COUNT 59 diff --git a/test/test_manifold.c b/test/test_manifold.c index 702603f..b178b9a 100644 --- a/test/test_manifold.c +++ b/test/test_manifold.c @@ -420,7 +420,7 @@ static int TriangleEdgeTest( void ) b3LocalManifold manifold = { 0 }; manifold.points = points; b3SATCache cache = { .type = b3_manualEdgePairAxis }; - b3CollideHullAndTriangle( &manifold, 8, &hull.base, v1, v2, v3, 0, &cache, true ); + b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, 0, &hull.base, &cache, true ); b3Vec3 expectedNormal = b3Neg( axis ); b3Vec3 expectedPoint = b3MulAdd( hullPoint, 0.5f * gap, axis ); @@ -453,7 +453,7 @@ static int TriangleEdgeTest( void ) b3LocalManifold manifold = { 0 }; manifold.points = points; b3SATCache cache = { 0 }; - b3CollideHullAndTriangle( &manifold, 8, &hull.base, v1, v2, v3, 0, &cache, true ); + b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, 0, &hull.base, &cache, true ); ENSURE( manifold.pointCount == 0 ); ENSURE( cache.type == b3_edgePairAxis ); @@ -486,7 +486,7 @@ static int TriangleParallelEdgeTest( void ) b3LocalManifold manifold = { 0 }; manifold.points = points; b3SATCache cache = { 0 }; - b3CollideHullAndTriangle( &manifold, 8, &hull.base, v1, v2, v3, 0, &cache, true ); + b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, 0, &hull.base, &cache, true ); ENSURE( manifold.pointCount == 4 ); ENSURE( cache.type == b3_faceAxisA ); @@ -501,72 +501,111 @@ static int TriangleParallelEdgeTest( void ) return 0; } -// Two long roof ridges laid across each other. Unlike the stacked cubes the axis of minimum -// penetration really is the edge pair, so the query has to hand the winning pair to the contact -// builder. Closing the crossing angle drives that pair toward parallel, where the cross product -// the builder needs to orient the contact loses its precision. +// A crossed ridge pair must land on a four point roof face contact. The clipped face +// separation can be no deeper than root2 times the vertical overlap. +static int CheckRoofFaceContact( const b3LocalManifold* manifold, const b3SATCache* cache, float overlap ) +{ + ENSURE( manifold->pointCount == 4 ); + ENSURE( cache->type == b3_faceAxisA || cache->type == b3_faceAxisB ); + + // A roof face of one hull, so 45 degrees off the vertical + ENSURE_SMALL( manifold->normal.y - kHalfRoot2, 1e-4f ); + + float minSeparation = MinSeparation( manifold ); + ENSURE( minSeparation < -kHalfRoot2 * overlap + 1e-4f ); + ENSURE( minSeparation > -kRoot2 * overlap - 1e-4f ); + + return 0; +} + +// Two long roof ridges laid across each other. The axis of minimum penetration is the edge +// pair, but a one point edge contact is weak for stacking. The collider builds the roof face +// contact first and only switches to the edge contact when the edge axis beats the clipped +// face separation by more than the slop. This pins all three regimes of that policy. static int RidgeCrossingTest( void ) { b3BoxHull hullA = b3MakeTransformedBoxHull( 1.5f, 0.1f, 0.1f, ExactRotation( kAxisX, 0.25f * B3_PI ) ); b3BoxHull hullB = b3MakeTransformedBoxHull( 1.5f, 0.1f, 0.1f, ExactRotation( kAxisX, 0.25f * B3_PI ) ); float ridgeY = 0.1f * kRoot2; - float overlap = 0.01f; - float lift = 2.0f * ridgeY - overlap; - // Well clear of the rejection threshold. The ridges cross over the origin and the axis is y. - float crossingAngles[] = { 0.02f, 0.1f, 0.5f }; - - for ( int i = 0; i < ARRAY_COUNT( crossingAngles ); ++i ) + // Shallow overlap. The edge axis is better by only ( root2 - 1 ) * overlap, inside the + // slop, so the four point face contact carries the crossing at every angle. { - b3Transform transform = { { 0.0f, lift, 0.0f }, ExactQuat( kAxisY, crossingAngles[i] ) }; + float overlap = 0.01f; + float lift = 2.0f * ridgeY - overlap; + float crossingAngles[] = { 0.0f, 1e-3f, 0.02f, 0.1f, 0.5f }; - b3LocalManifoldPoint points[8]; - b3LocalManifold manifold = { 0 }; - manifold.points = points; - b3SATCache cache = { 0 }; - b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache ); + for ( int i = 0; i < ARRAY_COUNT( crossingAngles ); ++i ) + { + b3Transform transform = { { 0.0f, lift, 0.0f }, ExactQuat( kAxisY, crossingAngles[i] ) }; - ENSURE( manifold.pointCount == 1 ); - ENSURE( cache.type == b3_edgePairAxis ); - ENSURE_SMALL( manifold.normal.x, 1e-5f ); - ENSURE_SMALL( manifold.normal.y - 1.0f, 1e-5f ); - ENSURE_SMALL( manifold.normal.z, 1e-5f ); - ENSURE_SMALL( manifold.points[0].separation + overlap, 1e-4f ); - ENSURE_SMALL( manifold.points[0].point.y - ( ridgeY - 0.5f * overlap ), 1e-4f ); + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SATCache cache = { 0 }; + b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache ); - // Where the point lands along the ridges is ill conditioned at a shallow crossing since - // the closest point solve divides by the square of the sine of the angle. It only has to - // land near the crossing, not at the end of a three meter beam. - ENSURE_SMALL( manifold.points[0].point.x, 0.01f ); - ENSURE_SMALL( manifold.points[0].point.z, 0.01f ); + if ( CheckRoofFaceContact( &manifold, &cache, overlap ) != 0 ) + { + return 1; + } + } } - // Inside the rejection threshold. A one point edge contact off a parallel pair would have a - // normal built from noise, so the query drops the pair and the roof faces carry the contact. - float shallowAngles[] = { 0.0f, 1e-4f, 1e-3f, 0.003f }; - - for ( int i = 0; i < ARRAY_COUNT( shallowAngles ); ++i ) + // Deep overlap at a clear crossing. The edge axis now beats the clipped face separation + // by more than the slop, so the edge contact replaces the face contact. { - b3Transform transform = { { 0.0f, lift, 0.0f }, ExactQuat( kAxisY, shallowAngles[i] ) }; + float overlap = 0.05f; + float lift = 2.0f * ridgeY - overlap; + float crossingAngles[] = { 0.05f, 0.1f, 0.2f, 0.5f }; - b3LocalManifoldPoint points[8]; - b3LocalManifold manifold = { 0 }; - manifold.points = points; - b3SATCache cache = { 0 }; - b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache ); + for ( int i = 0; i < ARRAY_COUNT( crossingAngles ); ++i ) + { + b3Transform transform = { { 0.0f, lift, 0.0f }, ExactQuat( kAxisY, crossingAngles[i] ) }; - ENSURE( manifold.pointCount == 4 ); - ENSURE( cache.type != b3_edgePairAxis ); + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SATCache cache = { 0 }; + b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache ); - // A roof face of one hull, so 45 degrees off the vertical - ENSURE_SMALL( manifold.normal.x, 1e-4f ); - ENSURE_SMALL( manifold.normal.y - kHalfRoot2, 1e-4f ); - ENSURE_SMALL( b3AbsFloat( manifold.normal.z ) - kHalfRoot2, 1e-4f ); + ENSURE( manifold.pointCount == 1 ); + ENSURE( cache.type == b3_edgePairAxis ); + ENSURE_SMALL( manifold.normal.x, 1e-4f ); + ENSURE_SMALL( manifold.normal.y - 1.0f, 1e-4f ); + ENSURE_SMALL( manifold.normal.z, 1e-4f ); + ENSURE_SMALL( manifold.points[0].separation + overlap, 1e-4f ); + ENSURE_SMALL( manifold.points[0].point.y - ( ridgeY - 0.5f * overlap ), 1e-4f ); - float minSeparation = MinSeparation( &manifold ); - ENSURE( minSeparation < 0.0f ); - ENSURE( minSeparation > -2.0f * overlap ); + // Only has to land near the crossing, not at the end of a three meter beam + ENSURE_SMALL( manifold.points[0].point.x, 0.01f ); + ENSURE_SMALL( manifold.points[0].point.z, 0.01f ); + } + } + + // Deep overlap near parallel. A one point edge contact off a parallel pair would have a + // normal built from noise, so the roof faces keep the contact. + { + float overlap = 0.05f; + float lift = 2.0f * ridgeY - overlap; + float shallowAngles[] = { 0.0f, 1e-4f, 1e-3f, 0.003f }; + + for ( int i = 0; i < ARRAY_COUNT( shallowAngles ); ++i ) + { + b3Transform transform = { { 0.0f, lift, 0.0f }, ExactQuat( kAxisY, shallowAngles[i] ) }; + + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SATCache cache = { 0 }; + b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache ); + + if ( CheckRoofFaceContact( &manifold, &cache, overlap ) != 0 ) + { + return 1; + } + } } return 0; @@ -841,7 +880,7 @@ static int TriangleHullEdgeSweepTest( void ) b3LocalManifold manifold = { 0 }; manifold.points = points; b3SATCache cache = { .type = b3_manualEdgePairAxis }; - b3CollideHullAndTriangle( &manifold, 8, &hull.base, v1, v2, v3, 0, &cache, true ); + b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, 0, &hull.base, &cache, true ); if ( cache.type != b3_edgePairAxis || manifold.pointCount != 1 ) { @@ -912,7 +951,7 @@ static int CapsuleTriangleEdgeDeepTest( void ) b3LocalManifold manifold = { 0 }; manifold.points = points; b3SimplexCache cache = { 0 }; - b3CollideCapsuleAndTriangle( &manifold, 8, &capsule, triangle, &cache ); + b3CollideTriangleAndCapsule( &manifold, 8, triangle, &capsule, &cache ); // Only the edge contacts exercise the new axis. Face contacts are handled elsewhere. if ( manifold.pointCount != 1 || manifold.feature < b3_featureEdge1 || manifold.feature > b3_featureEdge3 ) diff --git a/test/test_sat.c b/test/test_sat.c index 037846f..ac633db 100644 --- a/test/test_sat.c +++ b/test/test_sat.c @@ -124,7 +124,8 @@ static int FaceAxisASeparatedTest( void ) b3BoxHull hullB = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); b3Transform xfB = { { 1.2f, 0.0f, 0.0f }, b3Quat_identity }; - b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis ); + b3AxisQuery aq = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, true ); + b3SeparatingAxis q = b3GetBestAxis( &aq ); ENSURE( q.type == b3_faceAxisA ); ENSURE_SMALL( q.separation - 0.2f, 1e-5f ); @@ -155,7 +156,8 @@ static int FaceAxisBSeparatedTest( void ) float d = aExtent + 0.5f + gap; b3Transform xfB = { { d, 0.0f, 0.0f }, b3Quat_identity }; - b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis ); + b3AxisQuery aq = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, true ); + b3SeparatingAxis q = b3GetBestAxis( &aq ); ENSURE( q.type == b3_faceAxisB ); ENSURE_SMALL( q.separation - gap, 1e-5f ); @@ -181,7 +183,8 @@ static int FaceFarSeparatedTest( void ) b3BoxHull hullB = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); b3Transform xfB = { { 3.0f, 0.0f, 0.0f }, b3Quat_identity }; - b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis ); + b3AxisQuery aq = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, true ); + b3SeparatingAxis q = b3GetBestAxis( &aq ); ENSURE( q.type == b3_faceAxisA ); ENSURE_SMALL( q.separation - 2.0f, 1e-5f ); @@ -209,7 +212,8 @@ static int OffsetFaceAxisBTest( void ) float d = aExtent + 0.5f + gap; b3Transform xfB = { { d, 0.0f, 0.0f }, b3Quat_identity }; - b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis ); + b3AxisQuery aq = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, true ); + b3SeparatingAxis q = b3GetBestAxis( &aq ); ENSURE( q.type == b3_faceAxisB ); ENSURE_SMALL( q.separation - gap, 1e-4f ); @@ -248,7 +252,8 @@ static int EdgePairSweepTest( void ) float expected = d - kRoot2; b3Transform xfB = { { d, 0.0f, 0.0f }, b3Quat_identity }; - b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis ); + b3AxisQuery aq = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, true ); + b3SeparatingAxis q = b3GetBestAxis( &aq ); ENSURE( q.type == b3_edgePairAxis ); ENSURE_SMALL( q.separation - expected, 1e-4f ); @@ -327,7 +332,8 @@ static int SeparatingAxisOracleTest( void ) b3Transform xfB = { b3MulSV( reach, NextDirection() ), ExactQuat( NextDirection(), NextFloat( 0.0f, B3_PI ) ) }; - b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis ); + b3AxisQuery aq = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, true ); + b3SeparatingAxis q = b3GetBestAxis( &aq ); b3Vec3 oracleNormal; float oracleSep = OracleSeparation( &hullA.base, &hullB.base, xfB, &oracleNormal ); @@ -423,7 +429,8 @@ static int OffsetHullOracleTest( void ) xfB = (b3Transform){ b3Sub( b3MulSV( reach, dir ), b3RotateVector( qB, offset ) ), qB }; } - b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis ); + b3AxisQuery aq = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, true ); + b3SeparatingAxis q = b3GetBestAxis( &aq ); b3Vec3 oracleNormal; float oracleSep = OracleSeparation( &hullA.base, &hullB.base, xfB, &oracleNormal );