diff --git a/benchmark/ghost_baseline.csv b/benchmark/ghost_baseline.csv index e143a9d..738ce7b 100644 --- a/benchmark/ghost_baseline.csv +++ b/benchmark/ghost_baseline.csv @@ -99,3 +99,7 @@ peel,3,1,capsule,0,2 peel,3,1,upright,0,2 peel,3,1,sphere,0,2 peel,3,1,cylinder,0,2 +chamfer,0,0,box,0,2 +chamfer,0,0,cylinder,0,2 +chamfer,0,1,box,0,2 +chamfer,0,1,cylinder,0,2 diff --git a/benchmark/ghost_baseline_hf.csv b/benchmark/ghost_baseline_hf.csv index 0309718..00672e0 100644 --- a/benchmark/ghost_baseline_hf.csv +++ b/benchmark/ghost_baseline_hf.csv @@ -54,7 +54,7 @@ tjunction,0,0,capsule,0,2 tjunction,0,0,upright,0,2 tjunction,0,0,sphere,0,2 tjunction,0,0,cylinder,3,2 -tjunction,0,1,box,2,0 +tjunction,0,1,box,1,0 tjunction,0,1,capsule,0,2 tjunction,0,1,upright,0,2 tjunction,0,1,sphere,0,2 @@ -99,3 +99,7 @@ peel,3,1,capsule,0,2 peel,3,1,upright,0,2 peel,3,1,sphere,0,2 peel,3,1,cylinder,0,2 +chamfer,0,0,box,0,2 +chamfer,0,0,cylinder,0,2 +chamfer,0,1,box,0,2 +chamfer,0,1,cylinder,0,2 diff --git a/benchmark/ghost_scan.c b/benchmark/ghost_scan.c index 784a9d3..c5e0def 100644 --- a/benchmark/ghost_scan.c +++ b/benchmark/ghost_scan.c @@ -18,7 +18,7 @@ #include // Command line and result file keys -static const char* s_sceneKeys[e_ghostSceneCount] = { "flat", "ramp", "cliff", "crest", "bumps", "tjunction", "peel" }; +static const char* s_sceneKeys[e_ghostSceneCount] = { "flat", "ramp", "cliff", "crest", "bumps", "tjunction", "peel", "chamfer" }; static const char* s_bodyKeys[e_ghostBodyCount] = { "box", "capsule", "upright", "sphere", "cylinder" }; #define MAX_CONFIGS 256 @@ -86,6 +86,14 @@ static void RunConfig( Config* config, GhostSettings settings, int stepCount, bo settings.mirror = config->mirror; settings.body = config->body; + // The s&box character is a fixed rotation hull without speculative contact. Speculative contact or + // rotation hide the ghost. + if ( config->scene == e_ghostChamfer ) + { + settings.fixedRotation = true; + settings.speculative = false; + } + b3WorldDef worldDef = b3DefaultWorldDef(); b3WorldId worldId = b3CreateWorld( &worldDef ); GhostCollisionsData data = CreateGhostCollisions( worldId, settings ); @@ -270,7 +278,8 @@ int main( int argc, char** argv ) { printf( "Usage: ghost_scan [options]\n" "Runs the Ghost Collisions sample headless and counts ghost manifolds per scene and body.\n" - " -s, --scene= flat, ramp, cliff, crest, bumps, tjunction, peel (default all)\n" + " -s, --scene= flat, ramp, cliff, crest, bumps, tjunction, peel, chamfer (default all)\n" + " chamfer runs only the box and cylinder, fixed rotation without speculative\n" " -b, --body= box, capsule, upright, sphere, cylinder (default all)\n" " -m, --mirror=<0|1|all> mirrored mesh (default all)\n" " -p, --preset=<0-3|all> PEEL preset (default all)\n" @@ -316,6 +325,12 @@ int main( int argc, char** argv ) continue; } + // Only a flat bottom can rest on the chamfer edges + if ( scene == e_ghostChamfer && body != e_ghostBox && body != e_ghostCylinder ) + { + continue; + } + configs[configCount++] = (Config){ .scene = (GhostScene)scene, .preset = preset, @@ -365,6 +380,7 @@ int main( int argc, char** argv ) int totalGhosts = 0; int grewCount = 0; int shrankCount = 0; + int column = 0; for ( int i = 0; i < configCount; ++i ) { const Config* c = configs + i; @@ -375,8 +391,18 @@ int main( int argc, char** argv ) char row[64]; FormatRow( row, sizeof( row ), c ); printf( "%s%-18s", i == 0 ? "" : "\n", row ); + column = 0; } + // A scene that skips a body leaves its column empty + int bodyColumn = bodyFilter >= 0 ? 0 : (int)c->body; + while ( column < bodyColumn ) + { + printf( "%10s", "-" ); + column += 1; + } + column += 1; + char cell[32]; if ( c->baseGhostCount >= 0 && c->baseGhostCount != c->ghostCount ) { diff --git a/include/box3d/types.h b/include/box3d/types.h index c7029c8..7b81955 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -2506,7 +2506,10 @@ typedef struct b3HeightFieldData uint8_t clockwise; /// Explicit padding. - uint8_t padding[7]; + uint8_t padding[3]; + + // Offet to the vertex ghost collision flag. Array of bits. + int32_t vertexFlagOffset; } b3HeightFieldData; /**@}*/ // height_field diff --git a/samples/sample_issues.cpp b/samples/sample_issues.cpp index 5006bd2..9be63dd 100644 --- a/samples/sample_issues.cpp +++ b/samples/sample_issues.cpp @@ -751,7 +751,7 @@ public: bodyDef.motionLocks.angularY = true; bodyDef.motionLocks.angularZ = true; bodyDef.enableSleep = false; - bodyDef.enableContactRecycling = false; + //bodyDef.enableContactRecycling = false; bodyDef.gravityScale = 2.03f; // s&box gravity: 800 inch/s^2 bodyDef.name = "character"; bodyDef.safetyFactor = 0.05f; @@ -764,7 +764,7 @@ public: float volume = 8.0f * m_bodyHalfWidth * m_bodyHalfHeight * m_bodyHalfWidth; shapeDef.density = m_characterMass / volume; - shapeDef.enableSpeculativeContact = false; + //shapeDef.enableSpeculativeContact = false; b3BoxHull box = b3MakeBoxHull( m_bodyHalfWidth, m_bodyHalfHeight, m_bodyHalfWidth ); b3CreateHullShape( m_characterId, &shapeDef, &box.base ); @@ -1981,3 +1981,284 @@ public: static int sampleThinWallPushOut = RegisterSample( "Issues", "Thin Wall Push Out", ThinWallPushOut::Create ); + +// Two crates resting on a rolling deck, one each side of the roll axis: the orange crate with +// contact recycling (the default), the blue crate with it off for its body. +// +// A recycled contact keeps the normal and anchors it was made with, fixed in the world, while each +// body turns less than B3_CONTACT_RECYCLE_ANGULAR_DISTANCE (10 degrees) from where the contact was +// made and the two stay within the recycle distance (5 cm). Only the separation is updated. A deck +// and the cargo resting on it turn together, so their contact is recycled through the roll. Its +// normal stays the deck's up as it was, and its anchors stay at the same world offsets from each +// body's center of mass, so the orange crate moves as the deck moves at that fixed spot in space, +// not as the deck under it moves. It goes straight up and down instead of along its arc, and slides +// out across the deck by about offset * (1 - cos(roll)). While that stays under the recycle +// distance the crate sways out and back. Past it (8 m and 8 degrees here, 78 mm) the contact is +// made again partway through, at the deck's angle then, and frozen there, so the crate walks off +// across the deck roll by roll. The angular tolerance doesn't stop that: at 12 or 15 degrees the +// contact is made again more often and the crate walks all the same. The blue crate rides with the +// deck. The slide arrows on the deck are drawn ten times longer. +class RollingDeck : public Sample +{ +public: + static constexpr float m_deckHalfLength = 2.0f; + static constexpr float m_deckHalfWidth = 10.0f; + static constexpr float m_deckHalfThickness = 0.25f; + static constexpr float m_crateHalf = 0.5f; + static constexpr float m_crateOffset = 8.0f; + static constexpr float m_friction = 1.0f; + static constexpr float m_settleTime = 1.0f; + static constexpr float m_slideScale = 10.0f; + + explicit RollingDeck( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 75.0f, 15.0f, 32.0f, { 0.0f, 0.5f, 0.0f } ); + } + + m_rollDegrees = 8.0f; + m_period = 6.0f; + m_deckId = {}; + m_crateIds[0] = {}; + m_crateIds[1] = {}; + + CreateScene(); + } + + void CreateScene() + { + if ( B3_IS_NON_NULL( m_deckId ) ) + { + b3DestroyBody( m_deckId ); + b3DestroyBody( m_crateIds[0] ); + b3DestroyBody( m_crateIds[1] ); + } + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = m_friction; + + // The deck rolls about the world x axis through its center + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_kinematicBody; + m_deckId = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( m_deckHalfLength, m_deckHalfThickness, m_deckHalfWidth ); + shapeDef.baseMaterial.customColor = b3_colorSlateGray; + b3CreateHullShape( m_deckId, &shapeDef, &box.base ); + } + + // The crates rest on the level deck, m_crateOffset either side of the roll axis + b3BoxHull box = b3MakeBoxHull( m_crateHalf, m_crateHalf, m_crateHalf ); + for ( int i = 0; i < 2; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, m_deckHalfThickness + m_crateHalf, + i == 0 ? m_crateOffset : -m_crateOffset }; + + // The orange crate recycles its contact with the deck, the blue crate doesn't. Both + // stay awake, so a small roll can't put them to sleep. + bodyDef.enableContactRecycling = i == 0; + bodyDef.enableSleep = false; + m_crateIds[i] = b3CreateBody( m_worldId, &bodyDef ); + + shapeDef.baseMaterial.customColor = i == 0 ? b3_colorOrange : b3_colorDodgerBlue; + b3CreateHullShape( m_crateIds[i], &shapeDef, &box.base ); + + m_start[i] = b3Vec3_zero; + m_maxSlide[i] = 0.0f; + m_maxNormalError[i] = 0.0f; + } + + m_time = 0.0f; + m_started = false; + } + + // The roll angle in radians at a time since the start: still while the crates settle, then + // rolling, eased in over the first quarter roll so the crates aren't jolted by a deck starting + // at full speed. + float RollAngle( float time ) const + { + float t = time - m_settleTime; + if ( t <= 0.0f ) + { + return 0.0f; + } + + float ease = 1.0f; + if ( t < 0.25f * m_period ) + { + float u = t / ( 0.25f * m_period ); + ease = u * u * ( 3.0f - 2.0f * u ); + } + + return ease * m_rollDegrees * B3_PI / 180.0f * sinf( 2.0f * B3_PI * t / m_period ); + } + + bool DrawControls() override + { + // Changing the roll starts the scene again, so the contacts are made with the deck level. + ImGui::SliderFloat( "Roll (deg)", &m_rollDegrees, 0.0f, 15.0f, "%.1f" ); + if ( ImGui::IsItemDeactivatedAfterEdit() ) + { + CreateScene(); + } + + ImGui::SliderFloat( "Period (s)", &m_period, 2.0f, 12.0f, "%.1f" ); + if ( ImGui::IsItemDeactivatedAfterEdit() ) + { + CreateScene(); + } + + if ( ImGui::Button( "Reset" ) ) + { + CreateScene(); + } + + return true; + } + + void Step() override + { + float timeStep = m_context->hertz > 0.0f ? 1.0f / m_context->hertz : 0.0f; + if ( m_context->pause && m_context->singleStep == 0 ) + { + timeStep = 0.0f; + } + + if ( timeStep > 0.0f ) + { + b3Quat rotation = + b3MakeQuatFromAxisAngle( b3Vec3_axisX, RollAngle( m_time + timeStep ) ); + b3Body_SetTargetTransform( m_deckId, { { 0.0f, 0.0f, 0.0f }, rotation }, timeStep, + true ); + } + + Sample::Step(); + + m_time += timeStep; + + // Where the crates settled, in the deck's frame, measured from as the deck starts to roll + if ( m_started == false && m_time >= m_settleTime ) + { + for ( int i = 0; i < 2; ++i ) + { + m_start[i] = b3Body_GetLocalPoint( m_deckId, b3Body_GetPosition( m_crateIds[i] ) ); + } + m_started = true; + } + + b3Vec3 deckUp = b3Body_GetWorldVector( m_deckId, b3Vec3_axisY ); + float slide[2] = { 0.0f, 0.0f }; + float normalError[2] = { 0.0f, 0.0f }; + + for ( int i = 0; i < 2; ++i ) + { + Vec4 color = i == 0 ? MakeColor( b3_colorOrange ) : MakeColor( b3_colorDodgerBlue ); + b3Pos center = b3Body_GetPosition( m_crateIds[i] ); + + // The deck's up (white) and the crate's contact normal (in the crate's color), from the + // crate's center + DrawArrow( center, b3OffsetPos( center, b3MulSV( 2.5f, deckUp ) ), + MakeColor( b3_colorWhite ) ); + + b3ContactData contactData; + if ( b3Body_GetContactData( m_crateIds[i], &contactData, 1 ) == 1 && + contactData.manifoldCount > 0 ) + { + b3Vec3 normal = contactData.manifolds[0].normal; + if ( b3Dot( normal, deckUp ) < 0.0f ) + { + normal = b3Neg( normal ); + } + + DrawArrow( center, b3OffsetPos( center, b3MulSV( 2.5f, normal ) ), color ); + normalError[i] = + acosf( b3MinFloat( b3Dot( normal, deckUp ), 1.0f ) ) * 180.0f / B3_PI; + } + + DrawString3D( b3OffsetPos( center, b3MulSV( 3.0f, deckUp ) ), color, "%s", + i == 0 ? "recycling on" : "recycling off" ); + + if ( m_started == false ) + { + continue; + } + + // Where the crate settled, outlined on the deck's top, and how far it has slid from + // there across the deck, drawn m_slideScale times longer + b3Vec3 local = b3Body_GetLocalPoint( m_deckId, center ); + b3Vec3 offset = b3Sub( local, m_start[i] ); + offset.y = 0.0f; + slide[i] = b3Length( offset ); + + float top = m_deckHalfThickness + 0.01f; + b3Vec3 mark = { m_start[i].x, top, m_start[i].z }; + b3Vec3 corners[4] = { + { mark.x - m_crateHalf, top, mark.z - m_crateHalf }, + { mark.x + m_crateHalf, top, mark.z - m_crateHalf }, + { mark.x + m_crateHalf, top, mark.z + m_crateHalf }, + { mark.x - m_crateHalf, top, mark.z + m_crateHalf }, + }; + for ( int j = 0; j < 4; ++j ) + { + DrawLine( b3Body_GetWorldPoint( m_deckId, corners[j] ), + b3Body_GetWorldPoint( m_deckId, corners[( j + 1 ) % 4] ), color ); + } + + b3Vec3 tip = b3MulAdd( mark, m_slideScale, offset ); + DrawArrow( b3Body_GetWorldPoint( m_deckId, mark ), + b3Body_GetWorldPoint( m_deckId, tip ), color ); + + m_maxSlide[i] = b3MaxFloat( m_maxSlide[i], slide[i] ); + m_maxNormalError[i] = b3MaxFloat( m_maxNormalError[i], normalError[i] ); + } + + float roll = RollAngle( m_time ) * 180.0f / B3_PI; + float expected = m_crateOffset * ( 1.0f - cosf( m_rollDegrees * B3_PI / 180.0f ) ); + + DrawTextLine( "deck rolls %.1f deg each way every %.1f s (now %+.1f deg), crates %.0f m " + "from the roll axis, friction %.0f", + m_rollDegrees, m_period, roll, m_crateOffset, m_friction ); + DrawTextLine( "a contact is recycled while each body turns < 10 deg from where it was made " + "(B3_CONTACT_RECYCLE_ANGULAR_DISTANCE)" ); + DrawTextLine( "contacts recycled this step: %d (only the orange crate's can be)", + b3World_GetCounters( m_worldId ).recycledContactCount ); + DrawTextLine( "orange, recycling on : normal %4.1f deg off the deck's up (most %4.1f), " + "slid %5.1f mm (most %5.1f)", + normalError[0], m_maxNormalError[0], 1000.0f * slide[0], + 1000.0f * m_maxSlide[0] ); + DrawTextLine( "blue, recycling off: normal %4.1f deg off the deck's up (most %4.1f), " + "slid %5.1f mm (most %5.1f)", + normalError[1], m_maxNormalError[1], 1000.0f * slide[1], + 1000.0f * m_maxSlide[1] ); + DrawTextLine( + "recycled, the crate moves straight up and down: offset * (1 - cos(roll)) = %.1f mm at " + "each end; past the " + "%.0f cm recycle distance the contact is made again tilted and the crate walks off", + 1000.0f * expected, 100.0f * b3World_GetContactRecycleDistance( m_worldId ) ); + DrawTextLine( "arrows: white is the deck's up, colored is the contact normal; slide arrows " + "on the deck are drawn %.0fx", + m_slideScale ); + } + + static Sample* Create( SampleContext* context ) + { + return new RollingDeck( context ); + } + + b3BodyId m_deckId; + b3BodyId m_crateIds[2]; + b3Vec3 m_start[2]; + float m_maxSlide[2]; + float m_maxNormalError[2]; + float m_rollDegrees; + float m_period; + float m_time; + bool m_started; +}; + +static int sampleRollingDeck = RegisterSample( "Issues", "Rolling Deck", RollingDeck::Create ); diff --git a/samples/sample_mesh.cpp b/samples/sample_mesh.cpp index f3e8450..23f9c67 100644 --- a/samples/sample_mesh.cpp +++ b/samples/sample_mesh.cpp @@ -1631,14 +1631,14 @@ public: static constexpr int m_markerCapacity = 256; explicit GhostCollisions( SampleContext* context ) - : Sample( context ) + : Sample(context), m_markers{} { - if ( m_context->restart == false ) + if (m_context->restart == false) { - m_camera->SetView( 90.0f, 20.0f, 30.0f, { 0.0f, 1.0f, 0.0f } ); + m_camera->SetView(90.0f, 20.0f, 30.0f, {0.0f, 1.0f, 0.0f}); } - m_data = CreateGhostCollisions( m_worldId, GetDefaultGhostSettings() ); + m_data = CreateGhostCollisions(m_worldId, GetDefaultGhostSettings()); } ~GhostCollisions() override @@ -1688,6 +1688,18 @@ public: settings.scene = (GhostScene)scene; rebuild = true; + // The chamfer ghost needs the s&box character, a fixed rotation hull without speculative contact + if ( settings.scene == e_ghostChamfer ) + { + settings.fixedRotation = true; + settings.speculative = false; + if ( settings.body != e_ghostBox && settings.body != e_ghostCylinder ) + { + settings.body = e_ghostBox; + } + relaunch = true; + } + if ( settings.scene == e_ghostInternal ) { m_camera->SetView( 60.0f, 20.0f, 160.0f, { 0.0f, 30.0f, 0.0f } ); diff --git a/shared/ghost_collisions.c b/shared/ghost_collisions.c index 3207139..115ea98 100644 --- a/shared/ghost_collisions.c +++ b/shared/ghost_collisions.c @@ -14,6 +14,10 @@ #define GHOST_HALF_WIDTH 3.0f #define GHOST_START_X -16.0f +// A 34 degree chamfer like the s&box beams +#define GHOST_CHAMFER_WIDTH 0.05f +#define GHOST_CHAMFER_DROP ( GHOST_CHAMFER_WIDTH / 1.5f ) + typedef struct PeelPreset { int columnCount; @@ -29,7 +33,7 @@ static const PeelPreset s_peelPresets[GHOST_PEEL_PRESET_COUNT] = { }; const char* g_ghostSceneNames[e_ghostSceneCount] = { - "Flat", "Concave", "Cliff", "Crest", "Bumps", "T-Junction", "Internal", + "Flat", "Concave", "Cliff", "Crest", "Bumps", "T-Junction", "Internal", "Chamfer", }; const char* g_ghostBodyNames[e_ghostBodyCount] = { "Box", "H Capsule", "V Capsule", "Sphere", "Cylinder" }; @@ -355,6 +359,21 @@ void CreateGhostGround( b3WorldId worldId, GhostCollisionsData* data ) } break; + case e_ghostChamfer: + // Flat tops at y = 0 with a chamfered notch every meter. A body crossing a notch overhangs both + // chamfers while its bottom rests on the convex top edges. + PushProfile( data, -h, 0.0f ); + for ( int i = 0; i < 30; ++i ) + { + float x = -8.0f + (float)i; + PushProfile( data, x, 0.0f ); + PushProfile( data, x + GHOST_CHAMFER_WIDTH, -GHOST_CHAMFER_DROP ); + PushProfile( data, x + 2.0f * GHOST_CHAMFER_WIDTH, -GHOST_CHAMFER_DROP ); + PushProfile( data, x + 3.0f * GHOST_CHAMFER_WIDTH, 0.0f ); + } + PushProfile( data, h, 0.0f ); + break; + default: PushProfile( data, -h, 0.0f ); PushProfile( data, h, 0.0f ); @@ -391,6 +410,12 @@ void CreateGhostGround( b3WorldId worldId, GhostCollisionsData* data ) b3ShapeDef shapeDef = b3DefaultShapeDef(); b3CreateHullShape( data->groundId, &shapeDef, &box.base ); } + else if ( settings->scene == e_ghostChamfer ) + { + // One cell per chamfer puts every notch vertex on the grid + AddHeightField( worldId, data, -h, h, GetCellCount( 2.0f * h, GHOST_CHAMFER_WIDTH ), groundHalfWidth, cellCountZ, + false ); + } else { AddHeightField( worldId, data, -h, h, GetCellCount( 2.0f * h, cellSize ), groundHalfWidth, cellCountZ, false ); @@ -445,6 +470,7 @@ void LaunchGhostBody( b3WorldId worldId, GhostCollisionsData* data ) b3BodyDef bodyDef = b3DefaultBodyDef(); bodyDef.type = b3_dynamicBody; + bodyDef.enableContactRecycling = false; b3Quat yaw = b3MakeQuatFromAxisAngle( b3Vec3_axisY, settings->yaw * B3_PI / 180.0f ); if ( peel ) @@ -586,6 +612,9 @@ static bool IsGhost( const GhostCollisionsData* data, b3Vec3 normal, float xMin, return false; } + GhostBody body = data->settings.body; + bool chamferHull = data->settings.scene == e_ghostChamfer && ( body == e_ghostBox || body == e_ghostCylinder ); + float lower = B3_PI; float upper = -B3_PI; int segmentCount = data->profileCount - 1; @@ -598,6 +627,13 @@ static bool IsGhost( const GhostCollisionsData* data, b3Vec3 normal, float xMin, continue; } + // A flat hull bottom resting on the top edges must keep its own normal, so for a hull only the tops + // count. A round body's normal comes from the closest ground feature, which can be a top edge. + if ( chamferHull && ( data->profile[k].y < 0.0f || data->profile[k + 1].y < 0.0f ) ) + { + continue; + } + float angle = SegmentAngle( data, k ); lower = b3MinFloat( lower, angle ); upper = b3MaxFloat( upper, angle ); diff --git a/shared/ghost_collisions.h b/shared/ghost_collisions.h index 70b8696..572e262 100644 --- a/shared/ghost_collisions.h +++ b/shared/ghost_collisions.h @@ -28,6 +28,7 @@ typedef enum GhostScene e_ghostBumps, e_ghostTJunction, e_ghostInternal, + e_ghostChamfer, e_ghostSceneCount } GhostScene; @@ -44,8 +45,8 @@ typedef enum GhostBody // Mesh variants of the PEEL InternalEdges presets #define GHOST_PEEL_PRESET_COUNT 4 -// Bumps has the longest profile -#define GHOST_MAX_PROFILE_POINTS 96 +// Chamfer has the longest profile +#define GHOST_MAX_PROFILE_POINTS 128 extern const char* g_ghostSceneNames[e_ghostSceneCount]; extern const char* g_ghostBodyNames[e_ghostBodyCount]; diff --git a/src/box3d.natvis b/src/box3d.natvis index 68d36e1..4283388 100644 --- a/src/box3d.natvis +++ b/src/box3d.natvis @@ -83,6 +83,10 @@ + + s: {separation} + + type: {type} diff --git a/src/height_field.c b/src/height_field.c index 4186f6b..d0b35fd 100644 --- a/src/height_field.c +++ b/src/height_field.c @@ -99,6 +99,53 @@ 21 3 22 */ +static b3Vec3 b3GetGridPoint( const float* heights, int columnCount, b3Vec3 scale, int row, int column ) +{ + return b3Mul( scale, (b3Vec3){ (float)column, heights[row * columnCount + column], (float)row } ); +} + +static b3Vec3 b3GetGridTriangleNormal( const float* heights, int columnCount, b3Vec3 scale, int row, int column, bool odd ) +{ + b3Vec3 point11 = b3GetGridPoint( heights, columnCount, scale, row, column ); + b3Vec3 point12 = b3GetGridPoint( heights, columnCount, scale, row, column + 1 ); + b3Vec3 point21 = b3GetGridPoint( heights, columnCount, scale, row + 1, column ); + b3Vec3 point22 = b3GetGridPoint( heights, columnCount, scale, row + 1, column + 1 ); + + b3Vec3 v1 = odd ? point22 : point11; + b3Vec3 v2 = odd ? point12 : point21; + b3Vec3 v3 = odd ? point21 : point12; + return b3Normalize( b3Cross( b3Sub( v2, v1 ), b3Sub( v3, v1 ) ) ); +} + +// Mark vertices as flat or concave. +static void b3MarkGridEdge( uint8_t* vertexFlags, int index1, int index2, b3Vec3 point1, b3Vec3 point2, b3Vec3 normal1, + b3Vec3 normal2, bool clockwise ) +{ + b3Vec3 outward = b3Cross( b3Normalize( b3Sub( point2, point1 ) ), normal1 ); + float angle = b3Atan2( b3Dot( normal2, outward ), b3Dot( normal2, normal1 ) ); + + // Quantize to match the mesh rule. + int8_t quantized = b3QuantizeEdgeAngle( clockwise ? -angle : angle ); + + if ( quantized != 0 ) + { + vertexFlags[index1] = (uint8_t)( vertexFlags[index1] & ~b3_flatVertex ); + vertexFlags[index2] = (uint8_t)( vertexFlags[index2] & ~b3_flatVertex ); + } + + if ( quantized < 0 ) + { + vertexFlags[index1] |= b3_concaveVertex; + vertexFlags[index2] |= b3_concaveVertex; + } +} + +static void b3MarkGridBoundary( uint8_t* vertexFlags, int index1, int index2 ) +{ + vertexFlags[index1] = (uint8_t)( vertexFlags[index1] & ~b3_flatVertex ); + vertexFlags[index2] = (uint8_t)( vertexFlags[index2] & ~b3_flatVertex ); +} + b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data ) { int columnCount = data->countX; @@ -119,6 +166,8 @@ b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data ) byteCount += b3AlignUp8( cellCount * sizeof( uint8_t ) ); int flagsOffset = (int)byteCount; byteCount += b3AlignUp8( triangleCount * sizeof( uint8_t ) ); + int vertexFlagOffset = (int)byteCount; + byteCount += b3AlignUp8( (size_t)( ( heightCount + 7 ) / 8 ) ); // Zero the whole blob so alignment padding is defined. The construction-time hash // sweeps raw bytes and would otherwise pick up uninitialized padding. @@ -133,6 +182,7 @@ b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data ) hf->heightsOffset = heightsOffset; hf->materialOffset = materialOffset; hf->flagsOffset = flagsOffset; + hf->vertexFlagOffset = vertexFlagOffset; hf->clockwise = data->clockwiseWinding; uint16_t* compressedHeights = (uint16_t*)( (intptr_t)hf + heightsOffset ); @@ -466,6 +516,79 @@ b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data ) B3_ASSERT( triangleIndex == triangleCount ); + // b3MeshVertexFlags + uint8_t* vertexFlags = (uint8_t*)b3Alloc( heightCount * sizeof( uint8_t ) ); + memset( vertexFlags, b3_flatVertex, heightCount * sizeof( uint8_t ) ); + + bool clockwise = hf->clockwise != 0; + int cellColumnCount = columnCount - 1; + int cellRowCount = rowCount - 1; + + for ( int row = 0; row < rowCount; ++row ) + { + for ( int column = 0; column < columnCount; ++column ) + { + int index = row * columnCount + column; + b3Vec3 point = b3GetGridPoint( heights, columnCount, scale, row, column ); + + if ( column < cellColumnCount ) + { + bool below = row < cellRowCount && materialIndices[row * cellColumnCount + column] != B3_HEIGHT_FIELD_HOLE; + bool above = + row > 0 && materialIndices[( row - 1 ) * cellColumnCount + column] != B3_HEIGHT_FIELD_HOLE; + if ( below && above ) + { + b3Vec3 next = b3GetGridPoint( heights, columnCount, scale, row, column + 1 ); + b3Vec3 normal1 = b3GetGridTriangleNormal( heights, columnCount, scale, row, column, false ); + b3Vec3 normal2 = b3GetGridTriangleNormal( heights, columnCount, scale, row - 1, column, true ); + b3MarkGridEdge( vertexFlags, index + 1, index, next, point, normal1, normal2, clockwise ); + } + else if ( below || above ) + { + b3MarkGridBoundary( vertexFlags, index, index + 1 ); + } + } + + if ( row < cellRowCount ) + { + bool right = + column < cellColumnCount && materialIndices[row * cellColumnCount + column] != B3_HEIGHT_FIELD_HOLE; + bool left = column > 0 && materialIndices[row * cellColumnCount + column - 1] != B3_HEIGHT_FIELD_HOLE; + if ( right && left ) + { + b3Vec3 next = b3GetGridPoint( heights, columnCount, scale, row + 1, column ); + b3Vec3 normal1 = b3GetGridTriangleNormal( heights, columnCount, scale, row, column, false ); + b3Vec3 normal2 = b3GetGridTriangleNormal( heights, columnCount, scale, row, column - 1, true ); + b3MarkGridEdge( vertexFlags, index, index + columnCount, point, next, normal1, normal2, clockwise ); + } + else if ( right || left ) + { + b3MarkGridBoundary( vertexFlags, index, index + columnCount ); + } + } + + if ( row < cellRowCount && column < cellColumnCount && + materialIndices[row * cellColumnCount + column] != B3_HEIGHT_FIELD_HOLE ) + { + b3Vec3 point21 = b3GetGridPoint( heights, columnCount, scale, row + 1, column ); + b3Vec3 point12 = b3GetGridPoint( heights, columnCount, scale, row, column + 1 ); + b3Vec3 normal1 = b3GetGridTriangleNormal( heights, columnCount, scale, row, column, false ); + b3Vec3 normal2 = b3GetGridTriangleNormal( heights, columnCount, scale, row, column, true ); + b3MarkGridEdge( vertexFlags, index + columnCount, index + 1, point21, point12, normal1, normal2, clockwise ); + } + } + } + + uint8_t* vertexBits = (uint8_t*)( (intptr_t)hf + vertexFlagOffset ); + for ( int i = 0; i < heightCount; ++i ) + { + if ( vertexFlags[i] == 0 ) + { + vertexBits[i >> 3] |= (uint8_t)( 1 << ( i & 7 ) ); + } + } + + b3Free( vertexFlags, heightCount * sizeof( uint8_t ) ); b3Free( decompressedHeights, heightCount * sizeof( float ) ); hf->hash = 0; @@ -633,6 +756,36 @@ bool b3GetHeightFieldNeighborNormal( const b3HeightFieldData* heightField, int t return true; } +bool b3IsHeightFieldVertexConvex( const b3HeightFieldData* heightField, int triangleIndex, int vertexIndex ) +{ + B3_ASSERT( 0 <= triangleIndex && triangleIndex < b3GetHeightFieldTriangleCount( heightField ) ); + B3_ASSERT( 0 <= vertexIndex && vertexIndex < 3 ); + + if ( heightField->vertexFlagOffset == 0 ) + { + return false; + } + + if ( heightField->clockwise ) + { + vertexIndex = ( 3 - vertexIndex ) % 3; + } + + int columnCount = heightField->columnCount; + int cellIndex = triangleIndex >> 1; + int row = cellIndex / ( columnCount - 1 ); + int column = cellIndex - row * ( columnCount - 1 ); + + int offsets[2][3] = { + { 0, columnCount, 1 }, + { columnCount + 1, 1, columnCount }, + }; + + int index = row * columnCount + column + offsets[triangleIndex & 1][vertexIndex]; + const uint8_t* vertexBits = (const uint8_t*)( (intptr_t)heightField + heightField->vertexFlagOffset ); + return ( vertexBits[index >> 3] & ( 1 << ( index & 7 ) ) ) != 0; +} + b3AABB b3ComputeHeightFieldAABB( const b3HeightFieldData* shape, b3Transform transform ) { return b3AABB_Transform( transform, shape->aabb ); diff --git a/src/mesh.c b/src/mesh.c index 5b439cb..0960a64 100644 --- a/src/mesh.c +++ b/src/mesh.c @@ -1056,7 +1056,7 @@ typedef struct #define B3_EDGE_ANGLE_STEPS 127.0f -static int8_t b3QuantizeEdgeAngle( float radians ) +int8_t b3QuantizeEdgeAngle( float radians ) { float scaled = radians * ( B3_EDGE_ANGLE_STEPS / B3_PI ); int quantized = scaled >= 0.0f ? (int)( scaled + 0.5f ) : -(int)( 0.5f - scaled ); diff --git a/src/mesh_contact.c b/src/mesh_contact.c index 6801d1b..a11457f 100644 --- a/src/mesh_contact.c +++ b/src/mesh_contact.c @@ -426,6 +426,7 @@ typedef enum b3NeighborResult b3_neighborSnap, } b3NeighborResult; +// 1.0027 degrees #define B3_NEIGHBOR_TOLERANCE 0.0175f static bool b3GetNeighborDirection( const b3Shape* shape, int triangleIndex, const b3Vec3* vertices, @@ -463,8 +464,7 @@ static bool b3AdmitsVertexNormal( const b3Shape* shape, int triangleIndex, const return false; } - int flags = b3GetVertexFlag( shape, triangleIndex, vertexIndex ); - return ( flags & ( b3_flatVertex | b3_concaveVertex ) ) == 0; + return b3IsConvexVertex( shape, triangleIndex, vertexIndex ); } // Voronoi test of an edge or vertex normal against the neighbor normals. @@ -564,8 +564,8 @@ static b3NeighborResult b3TestHullFaceNeighbors( const b3Shape* shape, int trian b3Vec3 normal = manifold->normal; b3Vec3 triangleNormal = b3MakeNormalFromPoints( vertices[0], vertices[1], vertices[2] ); float tolerance = B3_NEIGHBOR_TOLERANCE; - if ( b3Dot( normal, triangleNormal ) > 0.0f && - b3Length( b3Cross( normal, triangleNormal ) ) <= tolerance ) + float cosAngle = b3Dot( normal, triangleNormal ); + if ( cosAngle > 0.0f && cosAngle * cosAngle >= 1.0f - tolerance * tolerance ) { return b3_neighborAdmit; } @@ -838,7 +838,7 @@ static int b3CollideMeshTriangles( b3Cluster** clustersOut, const b3Shape* shape float restOffset = B3_MESH_REST_OFFSET; const b3HullData* hullB = shapeB->type == b3_hullShape ? shapeB->hull : NULL; - float hullSpeculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f; + float hullSpeculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : restOffset; b3Vec3 hullMargin = { hullSpeculativeDistance, hullSpeculativeDistance, hullSpeculativeDistance }; @@ -899,7 +899,7 @@ static int b3CollideMeshTriangles( b3Cluster** clustersOut, const b3Shape* shape b3Vec3 v2 = meshVertices[meshFlipped ? meshTriangle.index3 : meshTriangle.index2]; b3Vec3 v3 = meshVertices[meshFlipped ? meshTriangle.index2 : meshTriangle.index3]; - // todo comment + // This bool gets predicted, so worth it. if ( unitScale == false ) { v1 = b3Mul( meshScale, v1 ); @@ -1078,12 +1078,17 @@ static int b3CollideMeshTriangles( b3Cluster** clustersOut, const b3Shape* shape if ( validateNeighbors && manifold->pointCount > 0 && manifold->feature != b3_featureTriangleFace ) { - b3NeighborResult result = - manifold->feature == b3_featureHullFace - ? b3TestHullFaceNeighbors( shapeA, triangleIndex, vertices, relativeMatrix, - manifold, &nearFeature ) - : b3TestNeighbors( shapeA, triangleIndex, vertices, relativeMatrix, - manifold->feature, manifold->normal ); + b3NeighborResult result; + if ( manifold->feature == b3_featureHullFace ) + { + result = b3TestHullFaceNeighbors( shapeA, triangleIndex, vertices, + relativeMatrix, manifold, &nearFeature ); + } + else + { + result = b3TestNeighbors( shapeA, triangleIndex, vertices, relativeMatrix, + manifold->feature, manifold->normal ); + } // A vertex test only sees the two edges of this triangle at the vertex, so an admitted // vertex normal can still lean across another edge at the vertex, as out of a sliver tip. @@ -1201,7 +1206,7 @@ static int b3CollideMeshTriangles( b3Cluster** clustersOut, const b3Shape* shape continue; } - // todo comment + // todo explain this better const b3Vec3* targetVertices = survivorVertices + 3 * j; float planeDistance = b3Dot( blockTargets[j].normal, b3Sub( source->point, targetVertices[0] ) ); if ( b3AbsFloat( planeDistance ) > blockTargets[j].distanceTolerance ) @@ -1264,6 +1269,53 @@ static int b3CollideMeshTriangles( b3Cluster** clustersOut, const b3Shape* shape } } + if ( hullB != NULL && validateNeighbors ) + { + // Cull contacts below hull face when it rests on a triangle. + float tolerance = B3_NEIGHBOR_TOLERANCE; + float cosSquaredTolerance = 1.0f - tolerance * tolerance; + const b3LocalManifold* resting = NULL; + for ( int i = 0; i < manifoldCount && resting == NULL; ++i ) + { + const b3LocalManifold* manifold = manifoldBuffer + i; + float cosAngle = b3Dot( manifold->normal, manifold->triangleNormal ); + if ( manifold->pointCount > 0 && manifold->feature == b3_featureHullFace && cosAngle > 0.0f && + cosAngle * cosAngle >= cosSquaredTolerance ) + { + resting = manifold; + } + } + + if ( resting != NULL ) + { + b3Vec3 normal = resting->triangleNormal; + const b3Vec3* hullPoints = b3GetHullPoints( hullB ); + float hullBottom = FLT_MAX; + for ( int i = 0; i < hullB->vertexCount; ++i ) + { + hullBottom = b3MinFloat( hullBottom, b3Dot( normal, hullPoints[i] ) ); + } + + for ( int i = 0; i < survivorCount; ++i ) + { + if ( survivorManifolds[i] == B3_NULL_INDEX ) + { + continue; + } + + b3LocalManifold* manifold = manifoldBuffer + survivorManifolds[i]; + const b3Vec3* vertices = survivorVertices + 3 * i; + float cosAngle = b3Dot( manifold->normal, normal ); + float height = b3MaxFloat( b3Dot( normal, vertices[0] ), + b3MaxFloat( b3Dot( normal, vertices[1] ), b3Dot( normal, vertices[2] ) ) ); + if ( ( cosAngle <= 0.0f || cosAngle * cosAngle < cosSquaredTolerance ) && height < hullBottom ) + { + manifold->pointCount = 0; + } + } + } + } + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) { b3LocalManifold* manifold = manifoldBuffer + manifoldIndex; diff --git a/src/shape.h b/src/shape.h index 5727559..61769b9 100644 --- a/src/shape.h +++ b/src/shape.h @@ -130,6 +130,7 @@ b3Triangle b3GetHeightFieldTriangle( const b3HeightFieldData* heightField, int t int b3GetHeightFieldMaterial( const b3HeightFieldData* heightField, int triangleIndex ); bool b3GetHeightFieldNeighborNormal( const b3HeightFieldData* heightField, int triangleIndex, int edgeIndex, b3Vec3* normal ); +bool b3IsHeightFieldVertexConvex( const b3HeightFieldData* heightField, int triangleIndex, int vertexIndex ); static inline int b3GetHeightFieldTriangleCount( const b3HeightFieldData* heightField ) { @@ -200,16 +201,18 @@ static inline bool b3GetNeighborNormal( const b3Shape* shape, int triangleIndex, } int b3GetMeshVertexFlag( const b3Mesh* mesh, int triangleIndex, int vertexIndex ); +int8_t b3QuantizeEdgeAngle( float radians ); -static inline int b3GetVertexFlag( const b3Shape* shape, int triangleIndex, int vertexIndex ) +static inline bool b3IsConvexVertex( const b3Shape* shape, int triangleIndex, int vertexIndex ) { if ( shape->type == b3_meshShape ) { - return b3GetMeshVertexFlag( &shape->mesh, triangleIndex, vertexIndex ); + int flags = b3GetMeshVertexFlag( &shape->mesh, triangleIndex, vertexIndex ); + return ( flags & ( b3_flatVertex | b3_concaveVertex ) ) == 0; } B3_ASSERT( shape->type == b3_heightShape ); - return b3_flatVertex; + return b3IsHeightFieldVertexConvex( shape->heightField, triangleIndex, vertexIndex ); } bool b3IsValidMesh( const b3MeshData* meshData ); diff --git a/src/triangle_manifold.c b/src/triangle_manifold.c index c3bb1f5..8a69d15 100644 --- a/src/triangle_manifold.c +++ b/src/triangle_manifold.c @@ -747,7 +747,7 @@ static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, co b3ClipVertex* clipPoint = input + i; minSeparation = b3MinFloat( minSeparation, clipPoint->separation ); - if ( enableSpeculative == false && clipPoint->separation > 0.0f ) + if ( enableSpeculative == false && clipPoint->separation > B3_MESH_REST_OFFSET ) { continue; } @@ -763,7 +763,7 @@ static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, co finalPointCount += 1; } - float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f; + float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : B3_MESH_REST_OFFSET; if ( minSeparation > speculativeDistance ) { // This can occur with a stale SAT cache @@ -863,7 +863,7 @@ static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity b3ClipVertex* clipPoint = input + i; minSeparation = b3MinFloat( minSeparation, clipPoint->separation ); - if ( enableSpeculative == false && clipPoint->separation > 0.0f ) + if ( enableSpeculative == false && clipPoint->separation > B3_MESH_REST_OFFSET ) { continue; } @@ -880,7 +880,7 @@ static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity finalPointCount += 1; } - float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f; + float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : B3_MESH_REST_OFFSET; if ( minSeparation >= speculativeDistance ) { // This can happens if the objects move a part while re-using a cached axis @@ -1036,7 +1036,7 @@ void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v const b3Plane* hullPlanes = b3GetHullPlanes( hullB ); const b3Vec3* hullPoints = b3GetHullPoints( hullB ); - float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f; + float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : B3_MESH_REST_OFFSET; cache->hit = 1; // Attempt to use the cache to speed up collision @@ -1340,7 +1340,7 @@ void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v b3SimplexCache simplexCache = { 0 }; b3DistanceOutput output = b3ShapeDistance( &input, &simplexCache, NULL, 0 ); - if ( output.distance > 0.0f ) + if ( 0.0f < output.distance && output.distance <= speculativeDistance ) { B3_ASSERT( 0 < simplexCache.count && simplexCache.count <= 3 ); diff --git a/test/test_determinism.c b/test/test_determinism.c index 4fbfd30..cc6c18c 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -19,27 +19,27 @@ // Double precision accumulates body positions in double, so the settle/sleep step and the // state hash differ from the float build. Both modes are internally deterministic. #if defined( BOX3D_DOUBLE_PRECISION ) -#define RAGDOLL_SLEEP_STEP 278 -#define RAGDOLL_HASH 0x8851ED6D -#define WAVE_PILE_SLEEP_STEP 274 -#define WAVE_PILE_HASH 0xAEDE433C +#define RAGDOLL_SLEEP_STEP 281 +#define RAGDOLL_HASH 0x176A6CCA +#define WAVE_PILE_SLEEP_STEP 282 +#define WAVE_PILE_HASH 0x7691DA9C #define QUERY_SPAWN_SLEEP_STEP 242 #define QUERY_SPAWN_HASH 0x489585E6 #define QUERY_SPAWN_HIT_COUNT 59 #define QUERY_SPAWN_QUERY_HASH 0x5B4429DC -#define MESH_DROP_SLEEP_STEP 218 -#define MESH_DROP_HASH 0xD378CF04 +#define MESH_DROP_SLEEP_STEP 197 +#define MESH_DROP_HASH 0xBC6C5827 #else #define RAGDOLL_SLEEP_STEP 289 #define RAGDOLL_HASH 0xF290ED4B -#define WAVE_PILE_SLEEP_STEP 296 -#define WAVE_PILE_HASH 0x8A4A5802 +#define WAVE_PILE_SLEEP_STEP 303 +#define WAVE_PILE_HASH 0x83B2DAF9 #define QUERY_SPAWN_SLEEP_STEP 242 #define QUERY_SPAWN_HASH 0xF3AD9BA6 #define QUERY_SPAWN_HIT_COUNT 59 #define QUERY_SPAWN_QUERY_HASH 0xE3271F3D #define MESH_DROP_SLEEP_STEP 197 -#define MESH_DROP_HASH 0x70483620 +#define MESH_DROP_HASH 0x5D593538 #endif // The goldens above pin exact values for the default four point manifold. A build that