diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml new file mode 100644 index 0000000..65f0511 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -0,0 +1,5 @@ +blank_issues_enabled: false +contact_links: + - name: Box2D discord + url: https://discord.gg/NKYgCBP + about: Ask for help here first. diff --git a/.github/ISSUE_TEMPLATE/issue.md b/.github/ISSUE_TEMPLATE/issue.md new file mode 100644 index 0000000..f6acf9c --- /dev/null +++ b/.github/ISSUE_TEMPLATE/issue.md @@ -0,0 +1,16 @@ +--- +name: Bug report or feature request +about: Please check these boxes before submitting +--- + +Make sure these boxes are checked before submitting your issue - thank you! + +- [ ] Ask for help on [discord](https://discord.gg/NKYgCBP) +- [ ] Consider providing a recording. +- [ ] Bug reports by LLMs are welcome. +- [ ] Feature requests and enhancements must come from humans. + +Use these to create a recording. +```c +b3CreateRecording, b3World_StartRecording, b3World_StopRecording, b3SaveRecordingToFile +``` diff --git a/.github/issue_template.md b/.github/issue_template.md deleted file mode 100644 index 5c930c6..0000000 --- a/.github/issue_template.md +++ /dev/null @@ -1,10 +0,0 @@ - -## Reporting bugs - -Bugs can be filed by humans or LLMs. Please help me reproduce your bug. Include a sample, unit test, or recording if possible. Recordings -should make note of the Box3D commit used (the SHA). - -## Feature requests - -Please don't disguise feature requests as bugs. Feature requests must be human typed because they usually involve discussion. -I'm likely to close feature requests otherwise. diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md index 9fdfc2c..ecc9d2a 100644 --- a/.github/pull_request_template.md +++ b/.github/pull_request_template.md @@ -1 +1,2 @@ -Pull requests for Box3D code are not accepted. Please consider filing an issue instead. +- [ ] run unit tests +- [ ] run all samples diff --git a/.gitignore b/.gitignore index 9e9caaf..fd1da23 100644 --- a/.gitignore +++ b/.gitignore @@ -8,10 +8,7 @@ settings.ini CMakeUserPresets.json .cache/ .idea/ -.claude/ .tmp/ .fetchcontent-cache/ *.b3rec -Superluminal/ -build-dp/ -AGENTS.md +*.zip diff --git a/include/box3d/base.h b/include/box3d/base.h index 30cee46..1a52fb9 100644 --- a/include/box3d/base.h +++ b/include/box3d/base.h @@ -171,6 +171,10 @@ B3_API b3Version b3GetVersion( void ); /// @return true if the library was built with BOX3D_DOUBLE_PRECISION (large world mode) B3_API bool b3IsDoublePrecision( void ); +/// @return the value of B3_MAX_MANIFOLD_POINTS the library was compiled with. Useful for +/// an ABI test. +B3_API int b3GetMaxManifoldPoints( void ); + /**@}*/ //! @cond diff --git a/include/box3d/box3d.h b/include/box3d/box3d.h index 2e82590..ad9933d 100644 --- a/include/box3d/box3d.h +++ b/include/box3d/box3d.h @@ -691,6 +691,12 @@ B3_API void b3Body_SetSleepThreshold( b3BodyId bodyId, float sleepThreshold ); /// Get the sleep threshold, usually in meters per second. B3_API float b3Body_GetSleepThreshold( b3BodyId bodyId ); +/// Set the continuous collision safety factor. Smaller is safer but can lead to hitching. Recommended range [0.01, 0.5]. Non-dimensional. +B3_API void b3Body_SetSafetyFactor( b3BodyId bodyId, float safetyFactor ); + +/// Get the continuous collision safety factor. Non-dimensional. +B3_API float b3Body_GetSafetyFactor( b3BodyId bodyId ); + /// Returns true if this body is enabled B3_API bool b3Body_IsEnabled( b3BodyId bodyId ); diff --git a/include/box3d/collision.h b/include/box3d/collision.h index 809afb9..266998a 100644 --- a/include/box3d/collision.h +++ b/include/box3d/collision.h @@ -221,7 +221,8 @@ B3_API b3HullData* b3CreateCone( float height, float radius1, float radius2, int /// Create a rock shaped hull. B3_API b3HullData* b3CreateRock( float radius ); -/// Create a generic convex hull. +/// Create a generic convex hull. This can fail if B3_MAX_HULL_VERTICES, B3_MAX_HULL_FACES, +/// or B3_MAX_HULL_EDGES is exceeded. B3_API b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCount ); /// Deep clone a hull. @@ -647,6 +648,21 @@ B3_API void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b B3_API void b3CollideTriangleAndSphere( b3LocalManifold* manifold, int capacity, const b3Vec3* triangleA, const b3Sphere* sphereB ); +/// This is used for computing 2D hulls in service of contact manifold simplification. +/// Placed here for testing. Internal. +typedef struct b3Point2D +{ + b3Vec2 p; // 2D point + float separation; // separation of the associated contact point + int originalIndex; // original index of the associated contact point +} b3Point2D; + +/// Compute a 2D hull. Used internally for contact manifold simplification. Here for testing. +B3_API int b3Hull2D( b3Point2D* pts, int count, b3Point2D* hull ); + +/// Simplify a 2D hull. Used internally for contact manifold simplification. Here for testing. +B3_API int b3SimplifyHull2D( b3Point2D* hull, int count, int target ); + /**@}*/ // collision /** diff --git a/include/box3d/constants.h b/include/box3d/constants.h index 855d018..320e725 100644 --- a/include/box3d/constants.h +++ b/include/box3d/constants.h @@ -76,7 +76,9 @@ B3_API float b3GetStallThreshold( void ); #define B3_MAX_ROTATION ( 0.25f * B3_PI ) /// @warning modifying this can have a significant impact on performance and stability +#ifndef B3_SPECULATIVE_DISTANCE #define B3_SPECULATIVE_DISTANCE ( 4.0f * B3_LINEAR_SLOP ) +#endif /// The rest offset is used for mesh contact to reduce ghost collisions and assist with CCD. /// The rest offset adjusts the contact point separation value, making the solver push the shapes @@ -104,7 +106,14 @@ B3_API float b3GetStallThreshold( void ); #define B3_TIME_TO_SLEEP 0.5f /// The maximum number of contact points between two touching shapes. +/// The default and minimum is 4 and this case uses a fast approximate hull to reduce the +/// point count. A larger value builds a 2D convex hull of the candidate points and then +/// simplifies it to the target by dropping the vertices that contribute the least area. +/// Useful for testing, but I don't recommend shipping with it adjusted. +/// WARNING: if you change this you risk breaking ABI and a corrupted runtime. +#ifndef B3_MAX_MANIFOLD_POINTS #define B3_MAX_MANIFOLD_POINTS 4 +#endif /// The number of iterations for gyroscopic torques. #ifndef B3_GYROSCOPIC_ITERATIONS diff --git a/include/box3d/types.h b/include/box3d/types.h index 19850d2..847521e 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -303,6 +303,13 @@ typedef struct b3BodyDef /// Sleep speed threshold, default is 0.05 meters per second float sleepThreshold; + /// Continuous collision safety factor. The solver only uses continuous collision if there is a + /// risk of tunneling. If the body is moving fast enough to risk tunneling then it is considered a "fast body". + /// This improves performance and prevents movement hitches. If a body moving N meter risks tunneling, then the + /// body will be considered fast if it moves more than a safetyFactor times N meters over one full time step. + /// Non-dimensional. Recommended range [0.01, 0.5]. Default is 0.5 for high performance with low tunneling risk. + float safetyFactor; + /// Optional body name for debugging. const char* name; @@ -1940,8 +1947,7 @@ typedef struct b3Sphere * @{ */ -/// A solid capsule can be viewed as two hemispheres connected -/// by a rectangle. +/// A solid capsule can be viewed as two hemispheres connected by a cylinder. typedef struct b3Capsule { /// Local center of the first hemisphere diff --git a/samples/sample_collision.cpp b/samples/sample_collision.cpp index e6dc8e0..5cf7796 100644 --- a/samples/sample_collision.cpp +++ b/samples/sample_collision.cpp @@ -2796,3 +2796,228 @@ public: }; static int sampleCapsuleCastRay = RegisterSample( "Collision", "Capsule Cast Ray", CapsuleCastRay::Create ); + +// The 2D manifold point reduction that runs when B3_MAX_MANIFOLD_POINTS is more than 4. +// Cyan is the convex hull and yellow the simplified version. +class Hull2D : public Sample +{ +public: + enum SourceMode + { + e_random = 0, + e_circle = 1, + e_sliver = 2, + }; + + static Sample* Create( SampleContext* context ) + { + return new Hull2D( context ); + } + + explicit Hull2D( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 0.0f, 20.0f, b3Pos_zero ); + } + + m_sourceMode = e_random; + m_inputCount = 0; + m_hullCount = 0; + m_simplifiedCount = 0; + m_targetCount = 6; + m_pointCount = 20; + m_drawScale = 1.0f; + m_drawLabels = true; + + Generate(); + } + + void Generate() + { + if ( m_sourceMode == e_random ) + { + m_drawScale = 1.0f; + m_inputCount = m_pointCount; + + for ( int i = 0; i < m_inputCount; ++i ) + { + m_inputPoints[i].p = { RandomFloatRange( -5.0f, 5.0f ), RandomFloatRange( -5.0f, 5.0f ) }; + m_inputPoints[i].separation = 0.0f; + m_inputPoints[i].originalIndex = i; + } + } + else if ( m_sourceMode == e_circle ) + { + // Every vertex of a regular polygon contributes the same area, so this is the case + // where the simplification has nothing but its pecking order to break ties with. + // libm rather than b3ComputeCosSin, whose rational approximation is coarse enough + // to make the areas unequal and hide that. + m_drawScale = 1.0f; + m_inputCount = m_pointCount; + + float radius = 5.0f; + float delta = 2.0f * B3_PI / (float)m_inputCount; + + for ( int i = 0; i < m_inputCount; ++i ) + { + float angle = delta * (float)i; + m_inputPoints[i].p = { radius * cosf( angle ), radius * sinf( angle ) }; + m_inputPoints[i].separation = 0.0f; + m_inputPoints[i].originalIndex = i; + } + } + else + { + // A near degenerate sliver with a duplicate pair. Welding and collinear removal have + // to survive this without producing a zero area hull. + b3Vec2 points[] = { + { 0.0f, 0.0f }, + { -0.00305632968f, 0.0000554900616f }, + { -0.00305632968f, 0.0246563386f }, + { -0.00305632968f, 0.0246562902f }, + { -0.00305632968f, 0.0800221488f }, + }; + + m_drawScale = 50.0f; + m_inputCount = int( sizeof( points ) / sizeof( points[0] ) ); + + for ( int i = 0; i < m_inputCount; ++i ) + { + m_inputPoints[i].p = points[i]; + m_inputPoints[i].separation = 0.0f; + m_inputPoints[i].originalIndex = i; + } + } + + m_targetCount = b3ClampInt( m_targetCount, 3, m_inputCount ); + } + + void Step() override + { + m_targetCount = b3ClampInt( m_targetCount, 3, m_inputCount ); + + // b3Hull2D sorts and welds in place, so it needs a scratch copy of the input + b3Point2D scratch[m_pointCapacity]; + for ( int i = 0; i < m_inputCount; ++i ) + { + scratch[i] = m_inputPoints[i]; + } + + m_hullCount = b3Hull2D( scratch, m_inputCount, m_hullPoints ); + + for ( int i = 0; i < m_hullCount; ++i ) + { + m_simplifiedPoints[i] = m_hullPoints[i]; + } + + m_simplifiedCount = b3SimplifyHull2D( m_simplifiedPoints, m_hullCount, m_targetCount ); + } + + bool DrawControls() override + { + const char* modes[] = { "Random", "Circle", "Sliver" }; + if ( ImGui::Combo( "Source", &m_sourceMode, modes, IM_ARRAYSIZE( modes ) ) ) + { + Generate(); + } + + if ( m_sourceMode != e_sliver ) + { + if ( ImGui::SliderInt( "Point Count", &m_pointCount, 4, m_pointCapacity ) ) + { + Generate(); + } + } + + if ( m_sourceMode == e_random ) + { + if ( ImGui::Button( "Regenerate" ) ) + { + Generate(); + } + } + + ImGui::SliderInt( "Target Count", &m_targetCount, 3, m_inputCount ); + ImGui::Checkbox( "Draw Labels", &m_drawLabels ); + + return true; + } + + void Render() override + { + Sample::Render(); + + DrawTextLine( "input = %d, hull = %d, simplified = %d", m_inputCount, m_hullCount, m_simplifiedCount ); + + DrawAxes( b3WorldTransform_identity, 0.4f ); + + bool onHull[m_pointCapacity] = {}; + for ( int i = 0; i < m_hullCount; ++i ) + { + onHull[m_hullPoints[i].originalIndex] = true; + } + + for ( int i = 0; i < m_inputCount; ++i ) + { + if ( onHull[i] ) + { + continue; + } + + b3Pos p = ToWorld( m_inputPoints[i].p, 0.0f ); + DrawPoint( p, 4.0f, MakeColor( b3_colorGray ) ); + + if ( m_drawLabels ) + { + DrawString3D( p, MakeColor( b3_colorGray ), " %d", i ); + } + } + + for ( int i = 0; i < m_hullCount; ++i ) + { + int next = ( i + 1 ) % m_hullCount; + b3Pos p1 = ToWorld( m_hullPoints[i].p, 0.0f ); + b3Pos p2 = ToWorld( m_hullPoints[next].p, 0.0f ); + DrawLine( p1, p2, MakeColor( b3_colorCyan ) ); + DrawPoint( p1, 6.0f, MakeColor( b3_colorCyan ) ); + + if ( m_drawLabels ) + { + DrawString3D( p1, MakeColor( b3_colorCyan ), " %d", m_hullPoints[i].originalIndex ); + } + } + + // Lifted so the survivors read on top of the hull they came from + for ( int i = 0; i < m_simplifiedCount; ++i ) + { + int next = ( i + 1 ) % m_simplifiedCount; + b3Pos p1 = ToWorld( m_simplifiedPoints[i].p, 0.01f ); + b3Pos p2 = ToWorld( m_simplifiedPoints[next].p, 0.01f ); + DrawLine( p1, p2, MakeColor( b3_colorYellow ) ); + DrawPoint( p1, 10.0f, MakeColor( b3_colorYellow ) ); + } + } + + b3Pos ToWorld( b3Vec2 p, float z ) const + { + return b3Pos{ m_drawScale * p.x, m_drawScale * p.y, z }; + } + + static constexpr int m_pointCapacity = 64; + + b3Point2D m_inputPoints[m_pointCapacity]; + b3Point2D m_hullPoints[2 * m_pointCapacity]; + b3Point2D m_simplifiedPoints[2 * m_pointCapacity]; + float m_drawScale; + int m_sourceMode; + int m_inputCount; + int m_hullCount; + int m_simplifiedCount; + int m_targetCount; + int m_pointCount; + bool m_drawLabels; +}; + +static int sampleHull2D = RegisterSample( "Collision", "Hull 2D", Hull2D::Create ); diff --git a/samples/sample_continuous.cpp b/samples/sample_continuous.cpp index 94d4a48..4911b7b 100644 --- a/samples/sample_continuous.cpp +++ b/samples/sample_continuous.cpp @@ -1027,3 +1027,118 @@ public: }; static int sampleStall = RegisterSample( "Continuous", "Stall", Stall::Create ); + +// This shows how adjusting the CCD safety factor can engage continuous collision at +// lower speeds to avoid the slight overlap that happens with discrete collision detection. +// The best way to run this sample is: +// 1. press pause (P) +// 2. restart (R) or press the Drop button +// 3. then single step (O) +// Then look at the metrics and overlap. The shape is drawn orange while it is using +// continuous collision detection. +class SafetyFactor : public Sample +{ +public: + explicit SafetyFactor( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 3.0f, 8.0f, { 0.0f, 1.0f, 0.0f } ); + } + + AddGroundBox( 20.0f ); + + m_bodyId = b3_nullBodyId; + m_extent = 0.5f; + m_height = 0.3f; + m_safetyFactor = 0.1f; + m_overlap = 0.0f; + + Launch(); + } + + void Launch() + { + if ( B3_IS_NON_NULL( m_bodyId ) ) + { + b3DestroyBody( m_bodyId ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, m_height + m_extent, 0.0f }; + bodyDef.safetyFactor = m_safetyFactor; + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeBoxHull( m_extent, m_extent, m_extent ); + b3CreateHullShape( m_bodyId, &shapeDef, &box.base ); + + m_overlap = 0.0f; + } + + void Step() override + { + Sample::Step(); + + if ( m_didStep ) + { + // Contacts are computed at the beginning of the step, so the manifold holds the initial separation. + b3ContactData data; + int count = b3Body_GetContactData( m_bodyId, &data, 1 ); + if ( count == 1 ) + { + for ( int i = 0; i < data.manifoldCount; ++i ) + { + const b3Manifold* manifold = data.manifolds + i; + for ( int j = 0; j < manifold->pointCount; ++j ) + { + m_overlap = b3MaxFloat( m_overlap, -manifold->points[j].separation ); + } + } + } + } + + // The overlap line sits below the ground surface, so dash it where the ground hides it + DrawLineEx( { -3.0f, -m_overlap, 0.0f }, { 3.0f, -m_overlap, 0.0f }, MakeColor( b3_colorRed ), + DEFAULT_LINE_THICKNESS_PX, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DASHED ); + + float timeStep = m_context->hertz > 0.0f ? 1.0f / m_context->hertz : 0.0f; + float motion = timeStep * b3Length( b3Body_GetLinearVelocity( m_bodyId ) ); + + DrawTextLine( "actual movement = %.3f m", motion ); + DrawTextLine( "fast movement = %.3f m", m_safetyFactor * m_extent ); + DrawTextLine( "overlap = %.4f m", m_overlap ); + } + + bool DrawControls() override + { + bool changed = false; + + ImGui::PushItemWidth( 10.0f * ImGui::GetFontSize() ); + changed |= ImGui::SliderFloat( "Height", &m_height, 0.0f, 2.0f, "%.2f" ); + changed |= ImGui::SliderFloat( "Safety", &m_safetyFactor, 0.0f, 1.0f, "%.2f" ); + ImGui::PopItemWidth(); + + if ( ImGui::Button( "Drop" ) || changed ) + { + Launch(); + } + + return true; + } + + static Sample* Create( SampleContext* context ) + { + return new SafetyFactor( context ); + } + + b3BodyId m_bodyId; + float m_extent; + float m_height; + float m_safetyFactor; + float m_overlap; +}; + +static int sampleSafetyFactor = RegisterSample( "Continuous", "Safety Factor", SafetyFactor::Create ); diff --git a/samples/sample_events.cpp b/samples/sample_events.cpp index dfe2ad9..db08b42 100644 --- a/samples/sample_events.cpp +++ b/samples/sample_events.cpp @@ -1,13 +1,12 @@ // SPDX-FileCopyrightText: 2025 Erin Catto // SPDX-License-Identifier: MIT +#include "gfx/draw.h" +#include "gfx/keycodes.h" #include "imgui.h" #include "sample.h" -#include "gfx/draw.h" #include "utils.h" -#include "gfx/keycodes.h" - #include "box3d/box3d.h" #include @@ -254,7 +253,6 @@ public: if ( context->restart == false ) { m_camera->SetView( 0.0f, 30.0f, 40.0f, { 0.0f, 5.0f, 0.0f } ); - } AddGroundBox( 40.0f ); @@ -343,7 +341,6 @@ public: if ( m_context->restart == false ) { m_camera->SetView( 0.0f, 30.0f, 40.0f, { 0.0f, 5.0f, 0.0f } ); - } AddGroundBox( 20.0f ); @@ -594,7 +591,7 @@ public: } m_groundExtent = 80.0f; - AddGroundBox( m_groundExtent ); + b3BodyId groundId = AddGroundBox( m_groundExtent ); // The walls live on their own body so a contact can be told apart from the floor. The player // rides on the floor at all times, so treating every non-debris body as a wall would strip @@ -610,8 +607,7 @@ public: float height = 5.0f; float thickness = 0.5f; - b3BoxHull wall = - b3MakeTransformedBoxHull( thickness, height, extent, { { extent, height, 0.0f }, b3Quat_identity } ); + b3BoxHull wall = b3MakeTransformedBoxHull( thickness, height, extent, { { extent, height, 0.0f }, b3Quat_identity } ); b3CreateHullShape( m_wallsId, &shapeDef, &wall.base ); wall = b3MakeTransformedBoxHull( thickness, height, extent, { { -extent, height, 0.0f }, b3Quat_identity } ); @@ -631,6 +627,7 @@ public: bodyDef.name = "player"; bodyDef.position = { 0.0f, 1.0f, 0.0f }; bodyDef.angularDamping = 0.1f; + bodyDef.enableSleep = false; m_playerId = b3CreateBody( m_worldId, &bodyDef ); b3ShapeDef shapeDef = b3DefaultShapeDef(); @@ -646,6 +643,15 @@ public: b3Sphere sphere = { b3Vec3_zero, 1.0f }; m_coreShapeId = b3CreateSphereShape( m_playerId, &shapeDef, &sphere ); + + m_torque = 30000.0f; + + b3MotorJointDef jointDef = b3DefaultMotorJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = m_playerId; + jointDef.base.collideConnected = true; + jointDef.maxVelocityTorque = m_torque; + m_motorId = b3CreateMotorJoint( m_worldId, &jointDef ); } for ( int i = 0; i < e_count; ++i ) @@ -658,7 +664,6 @@ public: b3Vec3 extents = b3AABB_Extents( bounds ); float mass = b3Body_GetMass( m_playerId ); m_massExtent = ( mass / 3.0f ) * ( extents.x + extents.y + extents.z ); - m_torque = 30000.0f; m_wait = 0.5f; } @@ -825,7 +830,14 @@ public: // Rolling without slipping spins about up cross velocity b3Vec3 axis = b3Cross( b3Vec3_axisY, b3Normalize( direction ) ); - b3Body_ApplyTorque( m_playerId, (scale * m_torque) * axis, true ); + float speed = 5.0f; + b3MotorJoint_SetMaxVelocityTorque( m_motorId, scale * m_torque ); + b3MotorJoint_SetAngularVelocity( m_motorId, speed * axis ); + } + else + { + b3MotorJoint_SetAngularVelocity( m_motorId, b3Vec3_zero ); + } Sample::Step(); @@ -921,6 +933,7 @@ public: static constexpr int m_capacity = 200; b3BodyId m_playerId; + b3JointId m_motorId; b3BodyId m_wallsId; b3ShapeId m_coreShapeId; b3BodyId m_debrisIds[m_capacity]; @@ -1042,7 +1055,6 @@ public: if ( m_context->restart == false ) { m_camera->SetView( 0.0f, 30.0f, 40.0f, { 0.0f, 5.0f, 0.0f } ); - } AddGroundBox( 10.0f ); diff --git a/samples/sample_issues.cpp b/samples/sample_issues.cpp index 6d2cd4b..e03138c 100644 --- a/samples/sample_issues.cpp +++ b/samples/sample_issues.cpp @@ -1,4 +1,4 @@ -// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-FileCopyrightText: 2026 Erin Catto // SPDX-License-Identifier: MIT #include "gfx/draw.h" @@ -1386,3 +1386,127 @@ public: b3HeightFieldData* m_heightField; }; static int sampleIssueIndex = RegisterSample( "Issues", "Heightfield", HeightfieldIssue::Create ); + +// Issue #135: the quasi static push out idiom, zero the velocity then step, drives a deeply +// overlapping body out through a thin static wall instead of back into the room. The solver +// resolves along the shallow exit, so once the mover center crosses the wall mid plane every +// step compounds the error outward. Interior is -x, exterior is +x. The wire box marks where +// a mover resting flush against the interior face belongs. +// This is NOT a bug. This is just how the separating axis works. I'm keeping this sample +// in case there is further discussion. +class ThinWallPushOut : public Sample +{ +public: + static constexpr float m_wallHalfThickness = 0.1f; + static constexpr float m_wallHalfHeight = 1.5f; + static constexpr float m_moverHalfExtent = 0.5f; + static constexpr float m_tolerance = 0.01f; + + explicit ThinWallPushOut( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 20.0f, 15.0f, 6.0f, { 0.0f, m_wallHalfHeight, 0.0f } ); + } + + // Furniture is dragged into place, never dropped + b3World_SetGravity( m_worldId, b3Vec3_zero ); + + AddGroundBox( 20.0f ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BodyDef bodyDef = b3DefaultBodyDef(); + + bodyDef.name = "wall"; + bodyDef.position = { 0.0f, m_wallHalfHeight, 0.0f }; + b3BodyId wallId = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull wall = b3MakeBoxHull( m_wallHalfThickness, m_wallHalfHeight, 2.0f ); + b3CreateHullShape( wallId, &shapeDef, &wall.base ); + + bodyDef.name = "mover"; + bodyDef.type = b3_dynamicBody; + bodyDef.position = { m_startX, m_wallHalfHeight, 0.0f }; + + // Velocity is zeroed every step, so without this the mover falls asleep on the first one + bodyDef.enableSleep = false; + + // A room planner slides items on the floor plane and yaws them to face a wall + bodyDef.motionLocks = { false, true, false, true, false, true }; + m_moverId = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull mover = b3MakeBoxHull( m_moverHalfExtent, m_moverHalfExtent, m_moverHalfExtent ); + shapeDef.baseMaterial.friction = 0.0f; + b3CreateHullShape( m_moverId, &shapeDef, &mover.base ); + } + + void PlaceMover() + { + b3Body_SetTransform( m_moverId, { m_startX, m_wallHalfHeight, 0.0f }, b3Quat_identity ); + b3Body_SetLinearVelocity( m_moverId, b3Vec3_zero ); + b3Body_SetAngularVelocity( m_moverId, b3Vec3_zero ); + } + + bool DrawControls() override + { + ImGui::PushItemWidth( 6.0f * ImGui::GetFontSize() ); + + // Anywhere past the wall mid plane tunnels out, even while the near face is far deeper + bool changed = ImGui::SliderFloat( "Start X", &m_startX, -0.6f, 0.6f, "%.2f" ); + changed = changed || ImGui::Button( "Reset Mover" ); + + if ( changed ) + { + PlaceMover(); + } + + ImGui::PopItemWidth(); + return true; + } + + void Step() override + { + // The reported idiom. Zeroing the velocity leaves the penetration correction as the + // only thing that can move the body. + b3Body_SetLinearVelocity( m_moverId, b3Vec3_zero ); + b3Body_SetAngularVelocity( m_moverId, b3Vec3_zero ); + + Sample::Step(); + + float interiorRestX = -m_wallHalfThickness - m_moverHalfExtent; + float exteriorRestX = m_wallHalfThickness + m_moverHalfExtent; + + b3Vec3 lower = { interiorRestX - m_moverHalfExtent, m_wallHalfHeight - m_moverHalfExtent, -m_moverHalfExtent }; + b3Vec3 upper = { interiorRestX + m_moverHalfExtent, m_wallHalfHeight + m_moverHalfExtent, m_moverHalfExtent }; + DrawAabb( lower, upper, MakeColor( b3_colorLime ) ); + + float x = (float)b3Body_GetPosition( m_moverId ).x; + DrawTextLine( "start x = %.3f m", m_startX ); + DrawTextLine( "current x = %.3f m", x ); + DrawTextLine( "interior rest x = %.3f m", interiorRestX ); + + if ( b3AbsFloat( x - exteriorRestX ) < m_tolerance ) + { + DrawTextLine( "FAIL: mover pushed out of the room" ); + } + else if ( b3AbsFloat( x - interiorRestX ) < m_tolerance ) + { + DrawTextLine( "PASS: mover pushed back into the room" ); + } + else + { + DrawTextLine( "relaxing ..." ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new ThinWallPushOut( context ); + } + + b3BodyId m_moverId = {}; + float m_startX = 0.2f; +}; + +static int sampleThinWallPushOut = RegisterSample( "Issues", "Thin Wall Push Out", ThinWallPushOut::Create ); diff --git a/shared/human.c b/shared/human.c index e6d3964..1e37931 100644 --- a/shared/human.c +++ b/shared/human.c @@ -36,8 +36,7 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Pos position, float frictio bodyDef.userData = userData; b3ShapeDef shapeDef = b3DefaultShapeDef(); - // shapeDef.friction = 0.2f; - shapeDef.baseMaterial.rollingResistance = 0.2f; + float defaultDensity = shapeDef.density; b3HexColor shirtColor = b3_colorMediumTurquoise; b3HexColor pantColor = b3_colorDodgerBlue; @@ -46,19 +45,20 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Pos position, float frictio { Bone* bone = human->bones + bone_pelvis; - bone->parentIndex = -1; bodyDef.name = "pelvis"; - bone->referenceFrame = (b3Transform){ { 0.0f, 0.932087f, -0.051708f }, { { 0.739169f, 0.0f, 0.0f }, 0.673520f } }; + bone->referenceFrame = (b3Transform){ { 0.000000f, 0.996219f, -0.023868f }, { { 1.000000f, 0.000000f, 0.000000f }, 0.000000f } }; bodyDef.rotation = bone->referenceFrame.q; bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p ); bone->bodyId = b3CreateBody( worldId, &bodyDef ); - b3Capsule capsule = { { 0.07f, 0.0f, -0.08f }, { -0.07f, 0.0f, -0.08f }, 0.13f }; shapeDef.filter.groupIndex = 0; shapeDef.baseMaterial.customColor = colorize ? pantColor : 0; + shapeDef.baseMaterial.friction = 0.6f; + shapeDef.baseMaterial.rollingResistance = 0.1f; + b3Capsule capsule = { { 0.040000f, 0.000001f, 0.000000f }, { -0.040000f, -0.000001f, 0.000000f }, 0.150000f }; b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); } @@ -67,117 +67,74 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Pos position, float frictio bone->parentIndex = bone_pelvis; bodyDef.name = "spine_01"; - bone->referenceFrame = (b3Transform){ { 0.0f, 1.113505f, -0.03481f }, { { 0.739973f, 0.0f, 0.0f }, 0.672637f } }; + bone->referenceFrame = (b3Transform){ { 0.000000f, 1.017288f, -0.024882f }, { { 1.000000f, 0.000000f, 0.000000f }, 0.000000f } }; bodyDef.rotation = bone->referenceFrame.q; bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p ); - // bodyDef.type = b3_staticBody; bone->bodyId = b3CreateBody( worldId, &bodyDef ); - bodyDef.type = b3_dynamicBody; - b3Capsule capsule = { { 0.06f, -0.0f, -0.052264f }, { -0.06f, 0.0f, -0.052264f }, 0.12f }; shapeDef.filter.groupIndex = -groupIndex; shapeDef.baseMaterial.customColor = colorize ? shirtColor : 0; + + shapeDef.baseMaterial.friction = 0.5f; + shapeDef.baseMaterial.rollingResistance = 0.1f; + b3Capsule capsule = { { 0.029876f, -0.146581f, 0.006260f }, { -0.029876f, -0.146574f, 0.006260f }, 0.145663f }; b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); bone->jointType = b3_sphericalJoint; - bone->localFrameA = (b3Transform){ { 0.000000, 0.000000, -0.182204 }, { { -0.999999, 0.000000, -0.000000 }, 0.001194 } }; - bone->localFrameB = (b3Transform){ { 0.000000, 0.000000, -0.007736 }, { { -1.000000, 0.000000, -0.000000 }, 0.000000 } }; - bone->swingLimit = 25.0f * B3_DEG_TO_RAD; - bone->twistLimit = (b3Vec2){ -15.0f * B3_DEG_TO_RAD, 15.0f * B3_DEG_TO_RAD }; - } - - { - Bone* bone = human->bones + bone_spine_02; - bone->parentIndex = bone_spine_01; - - // bodyDef.name = "spine_02"; - bone->referenceFrame = (b3Transform){ { 0.0f, 1.194336f, -0.027087f }, { { 0.703611f, 0.0f, 0.0f }, 0.710586f } }; - bodyDef.rotation = bone->referenceFrame.q; - bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p ); - bone->bodyId = b3CreateBody( worldId, &bodyDef ); - - b3Capsule capsule = { { 0.08f, -0.015133f, -0.091801f }, { -0.08f, -0.015133f, -0.091801f }, 0.10f }; - shapeDef.filter.groupIndex = 0; - shapeDef.baseMaterial.customColor = colorize ? shirtColor : 0; - b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); - - bone->jointType = b3_sphericalJoint; - bone->localFrameA = - (b3Transform){ { 0.000000, -0.000000, -0.088935 }, { { -0.998619, -0.000000, 0.000000 }, -0.052540 } }; - bone->localFrameB = (b3Transform){ { -0.000000, 0.000000, -0.008199 }, { { -1.000000, 0.000000, -0.000000 }, 0.000000 } }; - bone->swingLimit = 25.0f * B3_DEG_TO_RAD; - bone->twistLimit = (b3Vec2){ -15.0f * B3_DEG_TO_RAD, 15.0f * B3_DEG_TO_RAD }; + bone->localFrameA = (b3Transform){ { 0.000000f, -0.021069f, 0.001014f }, { { -0.642737f, 0.000000f, 0.000000f }, -0.766087f } }; + bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { -0.707107f, 0.000000f, 0.000000f }, -0.707107f } }; + bone->swingLimit = 35.0f * B3_DEG_TO_RAD; + bone->twistLimit = (b3Vec2){ -17.5f * B3_DEG_TO_RAD, 17.5f * B3_DEG_TO_RAD }; } { Bone* bone = human->bones + bone_spine_03; - bone->parentIndex = bone_spine_02; + bone->parentIndex = bone_spine_01; bodyDef.name = "spine_03"; - bone->referenceFrame = - (b3Transform){ { -0.0f, 1.31043f, -0.028232f }, { { 0.669856f, 0.000001f, -0.000001f }, 0.742491f } }; + bone->referenceFrame = (b3Transform){ { 0.000000f, 1.267766f, -0.022320f }, { { 1.000000f, 0.000000f, 0.000000f }, 0.000000f } }; bodyDef.rotation = bone->referenceFrame.q; bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p ); bone->bodyId = b3CreateBody( worldId, &bodyDef ); - b3Capsule capsule = { { 0.11f, -0.039753f, -0.13f }, { -0.11f, -0.039753f, -0.13f }, 0.145f }; shapeDef.filter.groupIndex = 0; shapeDef.baseMaterial.customColor = colorize ? shirtColor : 0; + + shapeDef.baseMaterial.friction = 0.5f; + shapeDef.baseMaterial.rollingResistance = 0.1f; + b3Capsule capsule = { { 0.063996f, -0.117434f, -0.040199f }, { -0.063996f, -0.117432f, -0.040199f }, 0.165004f }; b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); bone->jointType = b3_sphericalJoint; - bone->localFrameA = - (b3Transform){ { -0.000000, 0.000000, -0.124298 }, { { -0.998921, 0.000001, -0.000001 }, -0.046434 } }; - bone->localFrameB = (b3Transform){ { 0.000000, 0.000000, 0.000000 }, { { -1.000000, 0.000000, -0.000001 }, 0.000000 } }; - bone->swingLimit = 15.0f * B3_DEG_TO_RAD; - bone->twistLimit = (b3Vec2){ -10.0f * B3_DEG_TO_RAD, 10.0f * B3_DEG_TO_RAD }; + bone->localFrameA = (b3Transform){ { 0.000000f, -0.250478f, -0.002562f }, { { -0.642766f, 0.000000f, 0.000000f }, -0.766063f } }; + bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { -0.707107f, 0.000000f, 0.000000f }, -0.707107f } }; + bone->swingLimit = 35.0f * B3_DEG_TO_RAD; + bone->twistLimit = (b3Vec2){ -17.5f * B3_DEG_TO_RAD, 17.5f * B3_DEG_TO_RAD }; } { Bone* bone = human->bones + bone_neck; bone->parentIndex = bone_spine_03; - bodyDef.name = "neck"; - bone->referenceFrame = (b3Transform){ { 0.0f, 1.575582f, -0.055837f }, { { 0.879922f, 0.0f, 0.0f }, 0.475118f } }; + bodyDef.name = "neck_01"; + bone->referenceFrame = (b3Transform){ { 0.000000f, 1.498783f, 0.024462f }, { { 0.987879f, 0.000000f, 0.000000f }, 0.155228f } }; bodyDef.rotation = bone->referenceFrame.q; bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p ); bone->bodyId = b3CreateBody( worldId, &bodyDef ); - b3Capsule capsule = { { -0.000001f, -0.0f, -0.02f }, { 0.0f, -0.005f, -0.08f }, 0.07f }; shapeDef.filter.groupIndex = 0; shapeDef.baseMaterial.customColor = colorize ? skinColor : 0; - b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + shapeDef.baseMaterial.friction = 0.2f; + shapeDef.baseMaterial.rollingResistance = 0.05f; + b3Capsule head = { { 0.000000f, -0.212340f, 0.000000f }, { 0.000000f, -0.087340f, 0.000000f }, 0.110000f }; + b3CreateCapsuleShape( bone->bodyId, &shapeDef, &head ); bone->jointType = b3_sphericalJoint; - bone->localFrameA = (b3Transform){ { 0.000001, -0.000259, -0.266585 }, { { -0.942192, -0.000001, 0.000000 }, 0.335074 } }; - bone->localFrameB = (b3Transform){ { 0.000000, 0.000000, 0.000000 }, { { -1.000000, 0.000000, -0.000001 }, 0.000000 } }; - bone->swingLimit = 45.0f * B3_DEG_TO_RAD; - bone->twistLimit = (b3Vec2){ -15.0f * B3_DEG_TO_RAD, 15.0f * B3_DEG_TO_RAD }; - bone->jointFriction = 0.8f; - } - - { - Bone* bone = human->bones + bone_head; - bone->parentIndex = bone_neck; - - bodyDef.name = "head"; - bone->referenceFrame = (b3Transform){ { 0.0f, 1.653348f, -0.003241f }, { { 0.750288f, 0.0f, 0.0f }, 0.661111f } }; - bodyDef.rotation = bone->referenceFrame.q; - bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p ); - bone->bodyId = b3CreateBody( worldId, &bodyDef ); - - b3Capsule capsule = { { -0.000001f, 0.016892f, -0.05869f }, { 0.0f, -0.003629f, -0.115072f }, 0.0975f }; - shapeDef.filter.groupIndex = 0; - shapeDef.baseMaterial.customColor = colorize ? skinColor : 0; - b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); - - bone->jointType = b3_sphericalJoint; - bone->localFrameA = - (b3Transform){ { 0.000000, 0.001321, -0.093873 }, { { -0.974301, -0.000000, -0.000000 }, -0.225251 } }; - bone->localFrameB = (b3Transform){ { 0.000000, 0.001268, -0.005104 }, { { -1.000000, 0.000000, -0.00000 }, 0.000000 } }; - bone->swingLimit = 15.0f * B3_DEG_TO_RAD; - bone->twistLimit = (b3Vec2){ -15.0f * B3_DEG_TO_RAD, 15.0f * B3_DEG_TO_RAD }; - bone->jointFriction = 0.4f; + bone->localFrameA = (b3Transform){ { 0.000000f, -0.231017f, -0.046781f }, { { -0.516157f, -0.000002f, 0.000003f }, -0.856494f } }; + bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { -0.707108f, -0.000002f, 0.000002f }, -0.707106f } }; + bone->swingLimit = 30.0f * B3_DEG_TO_RAD; + bone->twistLimit = (b3Vec2){ -17.5f * B3_DEG_TO_RAD, 17.5f * B3_DEG_TO_RAD }; } { @@ -185,22 +142,24 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Pos position, float frictio bone->parentIndex = bone_pelvis; bodyDef.name = "thigh_l"; - bone->referenceFrame = - (b3Transform){ { 0.090416f, 0.986104f, -0.035090f }, { { -0.703287f, -0.070715f, 0.053866f }, 0.705327f } }; + bone->referenceFrame = (b3Transform){ { 0.092175f, 0.971562f, -0.011177f }, { { 0.471412f, 0.529510f, -0.499888f }, -0.497495f } }; bodyDef.rotation = bone->referenceFrame.q; bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p ); bone->bodyId = b3CreateBody( worldId, &bodyDef ); - b3Capsule capsule = { { 0.023719f, 0.006008f, -0.039068f }, { -0.064492f, -0.004664f, -0.424718f }, 0.09f }; shapeDef.filter.groupIndex = -groupIndex; shapeDef.baseMaterial.customColor = colorize ? pantColor : 0; + + shapeDef.baseMaterial.friction = 0.6f; + shapeDef.baseMaterial.rollingResistance = 0.1f; + b3Capsule capsule = { { -0.047269f, 0.000001f, 0.000000f }, { -0.379769f, 0.000005f, 0.000000f }, 0.091539f }; b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); bone->jointType = b3_sphericalJoint; - bone->localFrameA = (b3Transform){ { 0.05f, 0.011537f, -0.055325f }, { { -0.714896, -0.022305, -0.698361 }, -0.026790 } }; - bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { -0.002064, 0.758987, 0.017046 }, 0.650880 } }; - bone->swingLimit = 10.0f * B3_DEG_TO_RAD; - bone->twistLimit = (b3Vec2){ -60.0f * B3_DEG_TO_RAD, 40.0f * B3_DEG_TO_RAD }; + bone->localFrameA = (b3Transform){ { 0.092175f, 0.024656f, -0.012691f }, { { -0.460186f, -0.365368f, -0.637662f }, 0.498118f } }; + bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { -0.499997f, -0.500003f, 0.500002f }, 0.499998f } }; + bone->swingLimit = 30.0f * B3_DEG_TO_RAD; + bone->twistLimit = (b3Vec2){ -10.0f * B3_DEG_TO_RAD, 10.0f * B3_DEG_TO_RAD }; } { @@ -208,21 +167,28 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Pos position, float frictio bone->parentIndex = bone_thigh_l; bodyDef.name = "calf_l"; - bone->referenceFrame = - (b3Transform){ { 0.101198f, 0.527027f, -0.037374f }, { { -0.653328f, -0.066860f, 0.058582f }, 0.751838f } }; + bone->referenceFrame = (b3Transform){ { 0.118720f, 0.534541f, -0.035362f }, { { 0.521188f, 0.480597f, -0.546380f }, -0.445935f } }; bodyDef.rotation = bone->referenceFrame.q; bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p ); bone->bodyId = b3CreateBody( worldId, &bodyDef ); - b3Capsule capsule = { { 0.001778f, 0.0f, 0.009841f }, { -0.078577f, 0.014707f, -0.41816f }, 0.075f }; shapeDef.filter.groupIndex = 0; shapeDef.baseMaterial.customColor = colorize ? pantColor : 0; - b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + shapeDef.baseMaterial.friction = 0.1f; + shapeDef.baseMaterial.rollingResistance = 0.0f; + b3Capsule shin = { { -0.445000f, 0.000001f, 0.000000f }, { 0.005000f, -0.000001f, 0.000000f }, 0.080000f }; + b3CreateCapsuleShape( bone->bodyId, &shapeDef, &shin ); + + shapeDef.density = 0.5f * defaultDensity; + b3Capsule foot = { { -0.456371f, 0.129321f, 0.000013f }, { -0.359977f, 0.014394f, 0.000013f }, 0.070000f }; + b3CreateCapsuleShape( bone->bodyId, &shapeDef, &foot ); + shapeDef.density = defaultDensity; bone->jointType = b3_revoluteJoint; - bone->localFrameA = (b3Transform){ { -0.069989, 0.000253, -0.453844 }, { { -0.000677, 0.760087, 0.105674 }, 0.641171 } }; - bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { -0.044589, 0.765540, 0.053368 }, 0.639619 } }; - bone->twistLimit = (b3Vec2){ -5.0f * B3_DEG_TO_RAD, 45.0f * B3_DEG_TO_RAD }; + bone->localFrameA = (b3Transform){ { -0.438494f, -0.000175f, 0.000000f }, { { 0.344866f, -0.938652f, 0.000002f }, -0.000008f } }; + bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { -0.008182f, -0.999967f, 0.000001f }, -0.000007f } }; + bone->twistLimit = (b3Vec2){ -30.0f * B3_DEG_TO_RAD, 30.0f * B3_DEG_TO_RAD }; } { @@ -230,22 +196,24 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Pos position, float frictio bone->parentIndex = bone_pelvis; bodyDef.name = "thigh_r"; - bone->referenceFrame = - (b3Transform){ { -0.090416f, 0.986104f, -0.03509f }, { { -0.703287f, 0.070715f, -0.053865f }, 0.705326f } }; + bone->referenceFrame = (b3Transform){ { -0.092175f, 0.971562f, -0.011177f }, { { -0.497495f, 0.499888f, 0.529510f }, -0.471412f } }; bodyDef.rotation = bone->referenceFrame.q; bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p ); bone->bodyId = b3CreateBody( worldId, &bodyDef ); - b3Capsule capsule = { { -0.023719f, 0.006008f, -0.039068f }, { 0.064492f, -0.004664f, -0.424718f }, 0.09f }; shapeDef.filter.groupIndex = -groupIndex; shapeDef.baseMaterial.customColor = colorize ? pantColor : 0; + + shapeDef.baseMaterial.friction = 0.6f; + shapeDef.baseMaterial.rollingResistance = 0.1f; + b3Capsule capsule = { { 0.047269f, -0.000001f, 0.000000f }, { 0.379769f, -0.000005f, 0.000000f }, 0.092587f }; b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); bone->jointType = b3_sphericalJoint; - bone->localFrameA = (b3Transform){ { -0.05, 0.011537, -0.055326 }, { { -0.039089, -0.714094, 0.043177 }, 0.697623 } }; - bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { 0.758805, -0.019886, -0.651012 }, -0.001759 } }; - bone->swingLimit = 10.0f * B3_DEG_TO_RAD; - bone->twistLimit = (b3Vec2){ -30.0f * B3_DEG_TO_RAD, 60.0f * B3_DEG_TO_RAD }; + bone->localFrameA = (b3Transform){ { -0.092175f, 0.024656f, -0.012691f }, { { 0.380666f, 0.491846f, 0.488644f }, -0.611889f } }; + bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { -0.500002f, 0.499998f, -0.499998f }, 0.500002f } }; + bone->swingLimit = 30.0f * B3_DEG_TO_RAD; + bone->twistLimit = (b3Vec2){ -10.0f * B3_DEG_TO_RAD, 10.0f * B3_DEG_TO_RAD }; } { @@ -253,115 +221,132 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Pos position, float frictio bone->parentIndex = bone_thigh_r; bodyDef.name = "calf_r"; - bone->referenceFrame = - (b3Transform){ { -0.101198f, 0.527027f, -0.037373f }, { { -0.653327f, 0.06686f, -0.058582f }, 0.751839f } }; + bone->referenceFrame = (b3Transform){ { -0.118720f, 0.534541f, -0.035362f }, { { -0.445935f, 0.546380f, 0.480597f }, -0.521188f } }; bodyDef.rotation = bone->referenceFrame.q; bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p ); bone->bodyId = b3CreateBody( worldId, &bodyDef ); - b3Capsule capsule = { { -0.001820f, 0.0f, 0.010071f }, { 0.077883f, 0.014825f, -0.418047f }, 0.075f }; shapeDef.filter.groupIndex = 0; shapeDef.baseMaterial.customColor = colorize ? pantColor : 0; - b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + shapeDef.baseMaterial.friction = 0.1f; + shapeDef.baseMaterial.rollingResistance = 0.0f; + b3Capsule shin = { { 0.445000f, -0.000002f, 0.000003f }, { -0.005000f, 0.000000f, 0.000003f }, 0.080000f }; + b3CreateCapsuleShape( bone->bodyId, &shapeDef, &shin ); + + shapeDef.density = 0.5f * defaultDensity; + b3Capsule foot = { { 0.369113f, -0.006830f, 0.000017f }, { 0.465507f, -0.121757f, 0.000017f }, 0.070000f }; + b3CreateCapsuleShape( bone->bodyId, &shapeDef, &foot ); + shapeDef.density = defaultDensity; bone->jointType = b3_revoluteJoint; - bone->localFrameA = (b3Transform){ { 0.069988, 0.000253, -0.453844 }, { { 0.760086, -0.000675, -0.641171 }, -0.105676 } }; - bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { 0.765540, -0.044589, -0.639619 }, -0.053368 } }; - bone->twistLimit = (b3Vec2){ -45.0f * B3_DEG_TO_RAD, 5.0f * B3_DEG_TO_RAD }; + bone->localFrameA = (b3Transform){ { 0.438494f, 0.000175f, 0.000000f }, { { 0.000007f, 0.000005f, -0.344860f }, -0.938654f } }; + bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { 0.000006f, 0.000005f, 0.008188f }, -0.999966f } }; + bone->twistLimit = (b3Vec2){ -30.0f * B3_DEG_TO_RAD, 30.0f * B3_DEG_TO_RAD }; } { Bone* bone = human->bones + bone_upper_arm_l; bone->parentIndex = bone_spine_03; - bodyDef.name = "upper_arm_l"; - bone->referenceFrame = - (b3Transform){ { 0.20378f, 1.484275f, -0.115897f }, { { 0.143082f, 0.695980f, -0.690130f }, 0.13733f } }; + bodyDef.name = "upperarm_l"; + bone->referenceFrame = (b3Transform){ { 0.185817f, 1.443630f, 0.031306f }, { { 0.560220f, -0.307179f, 0.366233f }, -0.676512f } }; bodyDef.rotation = bone->referenceFrame.q; bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p ); bone->bodyId = b3CreateBody( worldId, &bodyDef ); - b3Capsule capsule = { { 0.0f, 0.0f, 0.0f }, { -0.091118f, 0.037775f, 0.229719f }, 0.075f }; shapeDef.filter.groupIndex = 0; shapeDef.baseMaterial.customColor = colorize ? shirtColor : 0; + + shapeDef.baseMaterial.friction = 0.6f; + shapeDef.baseMaterial.rollingResistance = 0.1f; + b3Capsule capsule = { { 0.296038f, 0.004386f, -0.000794f }, { -0.016086f, 0.004384f, -0.016127f }, 0.065000f }; b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); bone->jointType = b3_sphericalJoint; - bone->localFrameA = (b3Transform){ { 0.203780, -0.069369, -0.181921 }, { { -0.278486, 0.445600, -0.097014 }, 0.845266 } }; - bone->localFrameB = (b3Transform){ { 0.000000, 0.000000, 0.000000 }, { { -0.201396, -0.001586, 0.901850 }, 0.382234 } }; - bone->swingLimit = 60.0f * B3_DEG_TO_RAD; - bone->twistLimit = (b3Vec2){ -5.0f * B3_DEG_TO_RAD, 5.0f * B3_DEG_TO_RAD }; + bone->localFrameA = (b3Transform){ { 0.185817f, -0.175865f, -0.053625f }, { { -0.559466f, -0.213784f, -0.670199f }, 0.438323f } }; + bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { -0.500953f, -0.502617f, -0.509318f }, -0.486844f } }; + bone->swingLimit = 45.0f * B3_DEG_TO_RAD; + bone->twistLimit = (b3Vec2){ -20.0f * B3_DEG_TO_RAD, 20.0f * B3_DEG_TO_RAD }; + bone->jointFriction = 0.8f; } { Bone* bone = human->bones + bone_lower_arm_l; bone->parentIndex = bone_upper_arm_l; - bodyDef.name = "lower_arm_l"; - bone->referenceFrame = - (b3Transform){ { 0.305614f, 1.242908f, -0.117599f }, { { 0.165048f, 0.563437f, -0.802002f }, 0.109959f } }; + bodyDef.name = "lowerarm_l"; + bone->referenceFrame = (b3Transform){ { 0.347683f, 1.193455f, 0.029378f }, { { 0.406736f, -0.493012f, 0.089135f }, -0.763911f } }; bodyDef.rotation = bone->referenceFrame.q; bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p ); bone->bodyId = b3CreateBody( worldId, &bodyDef ); - b3Capsule capsule = { { 0.0f, 0.0f, 0.0f }, { -0.142406f, 0.039392f, 0.261092f }, 0.05f }; shapeDef.filter.groupIndex = 0; shapeDef.baseMaterial.customColor = colorize ? skinColor : 0; + + shapeDef.baseMaterial.friction = 0.3f; + shapeDef.baseMaterial.rollingResistance = 0.1f; + b3Capsule capsule = { { 0.312329f, -0.008256f, -0.016454f }, { 0.033153f, 0.008259f, -0.002741f }, 0.060000f }; b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); bone->jointType = b3_revoluteJoint; - bone->localFrameA = (b3Transform){ { -0.095482, 0.039584, 0.240723 }, { { 0.512487, -0.180629, 0.839474 }, 0.003742 } }; - bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { 0.503803, -0.029831, 0.858168 }, 0.094017 } }; - bone->twistLimit = (b3Vec2){ -5.0f * B3_DEG_TO_RAD, 60.0f * B3_DEG_TO_RAD }; + bone->localFrameA = (b3Transform){ { 0.297979f, 0.000361f, 0.000000f }, { { 0.916726f, 0.399352f, 0.000839f }, -0.011456f } }; + bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { 0.999922f, -0.003857f, 0.004659f }, -0.010908f } }; + bone->twistLimit = (b3Vec2){ -35.0f * B3_DEG_TO_RAD, 35.0f * B3_DEG_TO_RAD }; + bone->jointFriction = 0.06f; } { Bone* bone = human->bones + bone_upper_arm_r; bone->parentIndex = bone_spine_03; - bodyDef.name = "upper_arm_r"; - bone->referenceFrame = - (b3Transform){ { -0.20378f, 1.484276f, -0.115899f }, { { 0.143083f, -0.695978f, 0.690132f }, 0.137329f } }; + bodyDef.name = "upperarm_r"; + bone->referenceFrame = (b3Transform){ { -0.185817f, 1.443630f, 0.031306f }, { { 0.676512f, 0.366233f, 0.307179f }, 0.560220f } }; bodyDef.rotation = bone->referenceFrame.q; bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p ); bone->bodyId = b3CreateBody( worldId, &bodyDef ); - b3Capsule capsule = { { 0.0f, 0.0f, 0.0f }, { 0.091118f, 0.037775f, 0.229718f }, 0.075f }; shapeDef.filter.groupIndex = 0; shapeDef.baseMaterial.customColor = colorize ? shirtColor : 0; + + shapeDef.baseMaterial.friction = 0.6f; + shapeDef.baseMaterial.rollingResistance = 0.1f; + b3Capsule capsule = { { -0.300237f, -0.000001f, -0.000319f }, { 0.011887f, 0.000001f, 0.015013f }, 0.065000f }; b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); bone->jointType = b3_sphericalJoint; - bone->localFrameA = - (b3Transform){ { -0.203779, -0.069371, -0.181922 }, { { -0.253621, -0.414842, 0.106962 }, 0.867261 } }; - bone->localFrameB = (b3Transform){ { 0.000000, 0.000000, 0.000000 }, { { -0.201397, 0.001587, -0.901850 }, 0.382233 } }; - bone->swingLimit = 60.0f * B3_DEG_TO_RAD; - bone->twistLimit = (b3Vec2){ -5.0f * B3_DEG_TO_RAD, 5.0f * B3_DEG_TO_RAD }; + bone->localFrameA = (b3Transform){ { -0.185817f, -0.175865f, -0.053625f }, { { 0.213793f, 0.559449f, 0.438334f }, -0.670204f } }; + bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { -0.509319f, 0.486843f, 0.500954f }, -0.502617f } }; + bone->swingLimit = 45.0f * B3_DEG_TO_RAD; + bone->twistLimit = (b3Vec2){ -20.0f * B3_DEG_TO_RAD, 20.0f * B3_DEG_TO_RAD }; + bone->jointFriction = 0.8f; } { Bone* bone = human->bones + bone_lower_arm_r; bone->parentIndex = bone_upper_arm_r; - bodyDef.name = "lower_arm_r"; - bone->referenceFrame = - (b3Transform){ { -0.305614f, 1.242907f, -0.117599f }, { { 0.165048f, -0.563437f, 0.802002f }, 0.109959f } }; + bodyDef.name = "lowerarm_r"; + bone->referenceFrame = (b3Transform){ { -0.347683f, 1.193455f, 0.029378f }, { { 0.763911f, 0.089135f, 0.493012f }, 0.406736f } }; bodyDef.rotation = bone->referenceFrame.q; bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p ); bone->bodyId = b3CreateBody( worldId, &bodyDef ); - b3Capsule capsule = { { 0.0f, 0.0f, 0.0f }, { 0.142406f, 0.039392f, 0.261092f }, 0.05f }; shapeDef.filter.groupIndex = 0; shapeDef.baseMaterial.customColor = colorize ? skinColor : 0; + + shapeDef.baseMaterial.friction = 0.3f; + shapeDef.baseMaterial.rollingResistance = 0.1f; + b3Capsule capsule = { { -0.312649f, -0.000003f, 0.016471f }, { -0.032986f, 0.000000f, 0.002732f }, 0.060000f }; b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); bone->jointType = b3_revoluteJoint; - bone->localFrameA = (b3Transform){ { 0.095484, 0.039585, 0.240723 }, { { -0.180627, 0.512487, -0.003744 }, -0.839474 } }; - bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { -0.029831, 0.503803, -0.094017 }, -0.858169 } }; - bone->twistLimit = (b3Vec2){ -60.0f * B3_DEG_TO_RAD, 5.0f * B3_DEG_TO_RAD }; + bone->localFrameA = (b3Transform){ { -0.297979f, -0.000361f, 0.000000f }, { { -0.405444f, 0.914059f, 0.010541f }, 0.000852f } }; + bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { -0.002794f, 0.999936f, 0.010042f }, 0.004364f } }; + bone->twistLimit = (b3Vec2){ -35.0f * B3_DEG_TO_RAD, 35.0f * B3_DEG_TO_RAD }; + bone->jointFriction = 0.06f; } - // float dampingRatio = 0.9f; for ( int i = 1; i < bone_count; ++i ) { Bone* bone = human->bones + i; @@ -412,11 +397,17 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Pos position, float frictio } // Disable some collisions + human->filterJointCount = 0; b3FilterJointDef filterDef = b3DefaultFilterJointDef(); filterDef.base.bodyIdA = human->bones[bone_thigh_l].bodyId; filterDef.base.bodyIdB = human->bones[bone_thigh_r].bodyId; - human->filterJoints[0] = b3CreateFilterJoint( worldId, &filterDef ); - human->filterJointCount = 1; + human->filterJoints[human->filterJointCount++] = b3CreateFilterJoint( worldId, &filterDef ); + filterDef.base.bodyIdA = human->bones[bone_neck].bodyId; + filterDef.base.bodyIdB = human->bones[bone_upper_arm_l].bodyId; + human->filterJoints[human->filterJointCount++] = b3CreateFilterJoint( worldId, &filterDef ); + filterDef.base.bodyIdA = human->bones[bone_neck].bodyId; + filterDef.base.bodyIdB = human->bones[bone_upper_arm_r].bodyId; + human->filterJoints[human->filterJointCount++] = b3CreateFilterJoint( worldId, &filterDef ); human->isSpawned = true; } diff --git a/shared/human.h b/shared/human.h index b966a2a..8ca2742 100644 --- a/shared/human.h +++ b/shared/human.h @@ -9,10 +9,8 @@ typedef enum BoneId { bone_pelvis, bone_spine_01, - bone_spine_02, bone_spine_03, bone_neck, - bone_head, bone_thigh_l, bone_calf_l, bone_thigh_r, @@ -40,7 +38,7 @@ typedef struct Bone int parentIndex; } Bone; -#define FILTER_JOINT_COUNT 8 +#define FILTER_JOINT_COUNT 9 // This must be zero initialized typedef struct Human diff --git a/src/body.c b/src/body.c index f517e7b..f07b210 100644 --- a/src/body.c +++ b/src/body.c @@ -161,6 +161,7 @@ b3BodyId b3CreateBody( b3WorldId worldId, const b3BodyDef* def ) B3_ASSERT( b3IsValidFloat( def->linearDamping ) && def->linearDamping >= 0.0f ); B3_ASSERT( b3IsValidFloat( def->angularDamping ) && def->angularDamping >= 0.0f ); B3_ASSERT( b3IsValidFloat( def->sleepThreshold ) && def->sleepThreshold >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->safetyFactor ) && def->safetyFactor >= 0.0f ); B3_ASSERT( b3IsValidFloat( def->gravityScale ) ); b3World* world = b3GetUnlockedWorldFromId( worldId ); @@ -283,6 +284,7 @@ b3BodyId b3CreateBody( b3WorldId worldId, const b3BodyDef* def ) body->sleepThreshold = def->sleepThreshold; body->sleepTime = 0.0f; body->sleepVelocity = 0.0f; + body->safetyFactor = def->safetyFactor; body->mass = 0.0f; body->inertia = b3Mat3_zero; body->nameId = b3AddName( &world->names, def->name ); @@ -1000,6 +1002,19 @@ void b3UpdateBodyMassData( b3World* world, b3Body* body ) shapeId = s->nextShapeId; } + // When the center of mass changes, any cached contact manifold becomes invalid. + int edgeKey = body->headContactKey; + while ( edgeKey != B3_NULL_INDEX ) + { + int contactId = edgeKey >> 1; + int edgeIndex = edgeKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + contact->flags &= ~b3_relativeTransformValid; + + edgeKey = contact->edges[edgeIndex].nextKey; + } + // Apply fixed rotation if ( ( bodySim->flags & b3_fixedRotation ) == b3_fixedRotation ) { @@ -1890,6 +1905,19 @@ void b3Body_SetMassData( b3BodyId bodyId, b3MassData massData ) bodySim->maxExtent = b3Max( bodySim->maxExtent, extent.maxExtent ); shapeId = s->nextShapeId; } + + // When the center of mass changes, any cached contact manifold becomes invalid. + int edgeKey = body->headContactKey; + while ( edgeKey != B3_NULL_INDEX ) + { + int contactId = edgeKey >> 1; + int edgeIndex = edgeKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + contact->flags &= ~b3_relativeTransformValid; + + edgeKey = contact->edges[edgeIndex].nextKey; + } } b3MassData b3Body_GetMassData( b3BodyId bodyId ) @@ -2063,6 +2091,29 @@ float b3Body_GetSleepThreshold( b3BodyId bodyId ) return body->sleepThreshold; } +void b3Body_SetSafetyFactor( b3BodyId bodyId, float safetyFactor ) +{ + B3_ASSERT( b3IsValidFloat( safetyFactor ) && safetyFactor >= 0.0f ); + + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodySetSafetyFactor, bodyId, safetyFactor ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + body->safetyFactor = safetyFactor; +} + +float b3Body_GetSafetyFactor( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->safetyFactor; +} + void b3Body_EnableSleep( b3BodyId bodyId, bool enableSleep ) { b3World* world = b3GetUnlockedWorld( bodyId.world0 ); diff --git a/src/body.h b/src/body.h index ae90105..484fecc 100644 --- a/src/body.h +++ b/src/body.h @@ -108,6 +108,7 @@ typedef struct b3Body float sleepThreshold; float sleepTime; float sleepVelocity; + float safetyFactor; float mass; // local space inertia diff --git a/src/broad_phase.c b/src/broad_phase.c index a369ca6..fa4f7f3 100644 --- a/src/broad_phase.c +++ b/src/broad_phase.c @@ -395,6 +395,23 @@ static void b3UpdateTreesTask( void* context ) b3TracyCZoneEnd( tree_task ); } +// Task that can be done in parallel with the narrow-phase +// - rebuild the collision tree for dynamic and kinematic bodies to keep their query performance good +static void b3EnqueueTreeUpdate( b3World* world ) +{ + if ( world->taskCount < B3_MAX_TASKS ) + { + world->userTreeTask = world->enqueueTaskFcn( &b3UpdateTreesTask, world, world->userTaskContext, "rebuild tree" ); + world->taskCount += 1; + world->activeTaskCount += world->userTreeTask == NULL ? 0 : 1; + } + else + { + world->userTreeTask = NULL; + b3UpdateTreesTask( world ); + } +} + void b3UpdateBroadPhasePairs( b3World* world ) { b3BroadPhase* bp = &world->broadPhase; @@ -403,6 +420,8 @@ void b3UpdateBroadPhasePairs( b3World* world ) if ( moveCount == 0 ) { + // A destroyed shape may lead to no moves, but the tree could still be enlarged. + b3EnqueueTreeUpdate( world ); return; } @@ -427,19 +446,8 @@ void b3UpdateBroadPhasePairs( b3World* world ) b3TracyCZoneNC( create_contacts, "Create Contacts", b3_colorCoral, true ); - // Task that can be done in parallel with the narrow-phase - // - rebuild the collision tree for dynamic and kinematic bodies to keep their query performance good - if ( world->taskCount < B3_MAX_TASKS ) - { - world->userTreeTask = world->enqueueTaskFcn( &b3UpdateTreesTask, world, world->userTaskContext, "rebuild tree" ); - world->taskCount += 1; - world->activeTaskCount += world->userTreeTask == NULL ? 0 : 1; - } - else - { - world->userTreeTask = NULL; - b3UpdateTreesTask( world ); - } + // Update stale trees. + b3EnqueueTreeUpdate( world ); // Single-threaded work // - Clear move flags diff --git a/src/compound.c b/src/compound.c index 597b78b..886935c 100644 --- a/src/compound.c +++ b/src/compound.c @@ -237,7 +237,7 @@ static inline uint64_t b3HashMaterial( const b3SurfaceMaterial* material ) static bool b3CompareMaterials( const b3SurfaceMaterial* mat1, const b3SurfaceMaterial* mat2 ) { - B3_ASSERT( mat1->padding == 0 && mat2->padding == 0); + B3_ASSERT( mat1->padding == 0 && mat2->padding == 0 ); if ( mat1 == mat2 ) { @@ -684,8 +684,7 @@ b3AABB b3ComputeCompoundAABB( const b3CompoundData* shape, b3Transform transform struct b3CompoundOverlapContext { const b3CompoundData* compound; - // transform of the compound - b3Transform transform; + // proxy in the compound frame b3ShapeProxy proxy; bool overlap; }; @@ -698,25 +697,23 @@ static bool b3CompoundOverlapCallback( int proxyId, uint64_t userData, void* con struct b3CompoundOverlapContext* overlapContext = context; b3ChildShape child = b3GetCompoundChild( overlapContext->compound, childIndex ); - b3Transform transform = b3MulTransforms( overlapContext->transform, child.transform ); - bool overlap = false; switch ( child.type ) { case b3_capsuleShape: - overlap = b3OverlapCapsule( &child.capsule, transform, &overlapContext->proxy ); + overlap = b3OverlapCapsule( &child.capsule, child.transform, &overlapContext->proxy ); break; case b3_hullShape: - overlap = b3OverlapHull( child.hull, transform, &overlapContext->proxy ); + overlap = b3OverlapHull( child.hull, child.transform, &overlapContext->proxy ); break; case b3_meshShape: - overlap = b3OverlapMesh( &child.mesh, transform, &overlapContext->proxy ); + overlap = b3OverlapMesh( &child.mesh, child.transform, &overlapContext->proxy ); break; case b3_sphereShape: - overlap = b3OverlapSphere( &child.sphere, transform, &overlapContext->proxy ); + overlap = b3OverlapSphere( &child.sphere, child.transform, &overlapContext->proxy ); break; default: @@ -735,26 +732,24 @@ static bool b3CompoundOverlapCallback( int proxyId, uint64_t userData, void* con return true; } +// This is dealing with multiple transforms: +// - the compound shape transform +// - the compound child shape transforms bool b3OverlapCompound( const b3CompoundData* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ) { + B3_ASSERT( 0 < proxy->count && proxy->count <= B3_MAX_SHAPE_CAST_POINTS ); + + // Use local proxy. + b3Vec3 buffer[B3_MAX_SHAPE_CAST_POINTS]; struct b3CompoundOverlapContext context = { .compound = shape, - .transform = shapeTransform, - .proxy = *proxy, + .proxy = b3MakeLocalProxy( proxy, shapeTransform, buffer ), .overlap = false, }; - b3AABB aabb = { proxy->points[0], proxy->points[0] }; - for ( int i = 1; i < proxy->count; ++i ) - { - aabb.lowerBound = b3Min( aabb.lowerBound, proxy->points[i] ); - aabb.upperBound = b3Max( aabb.upperBound, proxy->points[i] ); - } - - b3Vec3 r = { proxy->radius, proxy->radius, proxy->radius }; - aabb.lowerBound = b3Sub( aabb.lowerBound, r ); - aabb.upperBound = b3Add( aabb.upperBound, r ); + b3AABB aabb = b3ComputeProxyAABB( &context.proxy ); + // This query must be in the compound frame. (void)b3DynamicTree_Query( &shape->tree, aabb, ~0ull, false, b3CompoundOverlapCallback, &context ); return context.overlap; @@ -779,6 +774,7 @@ static float b3CompoundRayCastCallback( const b3RayCastInput* input, int proxyId b3ChildShape child = b3GetCompoundChild( compound, childIndex ); + // Get the input in child local space. b3RayCastInput localInput = *input; localInput.origin = b3InvTransformPoint( child.transform, input->origin ); localInput.translation = b3InvRotateVector( child.transform.q, input->translation ); @@ -852,23 +848,13 @@ static float b3CompoundShapeCastCallback( const b3BoxCastInput* input, int proxy b3ChildShape child = b3GetCompoundChild( compound, childIndex ); - // Rebuild from the carried shape cast input, taking only the advancing fraction from the tree - b3ShapeCastInput localInput = *shapeInput; - localInput.maxFraction = input->maxFraction; + // Get the input in the child local space. + b3ShapeCastInput localInput = { 0 }; b3Vec3 localPoints[B3_MAX_SHAPE_CAST_POINTS]; - - localInput.proxy.count = b3MinInt( shapeInput->proxy.count, B3_MAX_SHAPE_CAST_POINTS ); - - b3Transform invTransform = b3InvertTransform( child.transform ); - b3Matrix3 R = b3MakeMatrixFromQuat( invTransform.q ); - - for ( int i = 0; i < localInput.proxy.count; ++i ) - { - localPoints[i] = b3Add( b3MulMV( R, shapeInput->proxy.points[i] ), invTransform.p ); - } - - localInput.proxy.points = localPoints; - localInput.translation = b3MulMV( R, shapeInput->translation ); + localInput.proxy = b3MakeLocalProxy( &shapeInput->proxy, child.transform, localPoints ); + localInput.translation = b3InvRotateVector( child.transform.q, shapeInput->translation ); + localInput.maxFraction = input->maxFraction; + localInput.canEncroach = shapeInput->canEncroach; b3CastOutput output = { 0 }; diff --git a/src/contact.c b/src/contact.c index 1506bdb..8897706 100644 --- a/src/contact.c +++ b/src/contact.c @@ -485,7 +485,7 @@ static bool b3ComputeConvexManifold( b3World* world, int workerIndex, b3Contact* b3ContactCache* cache = &contact->convexContact.cache; - int pointCapacity = 32; + int pointCapacity = B3_MAX_MANIFOLD_POINTS; b3LocalManifoldPoint* pointBuffer = (b3LocalManifoldPoint*)b3Bump( &arena, pointCapacity * sizeof( b3LocalManifoldPoint ) ); b3LocalManifold geomManifold = { 0 }; @@ -560,6 +560,7 @@ static bool b3ComputeConvexManifold( b3World* world, int workerIndex, b3Contact* } b3Manifold* manifold = contact->manifolds; + B3_ASSERT( 0 < geomManifold.pointCount && geomManifold.pointCount <= B3_MAX_MANIFOLD_POINTS ); manifold->pointCount = geomManifold.pointCount; b3Matrix3 matrixA = b3MakeMatrixFromQuat( xfA.q ); diff --git a/src/contact.h b/src/contact.h index ad9fa74..4a29423 100644 --- a/src/contact.h +++ b/src/contact.h @@ -133,6 +133,7 @@ typedef struct b3Contact // b3ContactFlags uint32_t flags; + // todo embed single manifold with ability to allocate overfloat, use union. b3Manifold* manifolds; int manifoldCount; diff --git a/src/contact_solver.c b/src/contact_solver.c index 48de409..f796549 100644 --- a/src/contact_solver.c +++ b/src/contact_solver.c @@ -630,7 +630,7 @@ void b3ApplyRestitution_Mesh( b3SolverBlock block, b3StepContext* context ) b3Vec3 normal = cm->normal; int pointCount = cm->pointCount; - B3_ASSERT( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); + B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) { diff --git a/src/contact_solver.h b/src/contact_solver.h index d85f233..0c24903 100644 --- a/src/contact_solver.h +++ b/src/contact_solver.h @@ -19,7 +19,7 @@ typedef struct b3ManifoldConstraintPoint typedef struct b3ManifoldConstraint { - b3ManifoldConstraintPoint points[4]; + b3ManifoldConstraintPoint points[B3_MAX_MANIFOLD_POINTS]; int pointCount; b3Vec3 normal; b3Vec3 tangent1; diff --git a/src/convex_manifold.c b/src/convex_manifold.c index 2f71e54..00a5495 100644 --- a/src/convex_manifold.c +++ b/src/convex_manifold.c @@ -203,6 +203,10 @@ static b3SeparatingAxis b3QueryEdgeDirectionHullAndCapsule( const b3HullData* hu }; } +_Static_assert( B3_MAX_MANIFOLD_POINTS >= 4, "must be 4 or more" ); + +#if B3_MAX_MANIFOLD_POINTS == 4 + // Reduce the manifold points to a maximum of 4 points. // Note: this modifies the input point array to improve performance static void b3ReduceManifoldPoints( b3LocalManifold* manifold, int capacity, b3LocalManifoldPoint* points, int count ) @@ -367,6 +371,61 @@ static void b3ReduceManifoldPoints( b3LocalManifold* manifold, int capacity, b3L } } +#else + +static void b3ReduceManifoldPoints( b3LocalManifold* manifold, int capacity, b3LocalManifoldPoint* points, int count ) +{ + if ( capacity < 4 ) + { + return; + } + + B3_ASSERT( count <= B3_MAX_CLIP_POINTS ); + + int target = b3MinInt( capacity, B3_MAX_MANIFOLD_POINTS ); + + if ( count <= target ) + { + for ( int i = 0; i < count; ++i ) + { + manifold->points[i] = points[i]; + } + + manifold->pointCount = count; + return; + } + + b3Vec3 normal = manifold->normal; + b3Vec3 u = b3Perp( normal ); + b3Vec3 v = b3Cross( normal, u ); + b3Vec3 origin = points[0].point; + + b3Point2D pts[B3_MAX_CLIP_POINTS]; + for ( int i = 0; i < count; ++i ) + { + b3Vec3 d = b3Sub( points[i].point, origin ); + pts[i].p = (b3Vec2){ b3Dot( d, u ), b3Dot( d, v ) }; + pts[i].separation = points[i].separation; + pts[i].originalIndex = i; + } + + b3Point2D hull[2 * B3_MAX_CLIP_POINTS]; + int hullCount = b3Hull2D( pts, count, hull ); + int finalCount = b3SimplifyHull2D( hull, hullCount, target ); + B3_ASSERT( 0 < finalCount && finalCount <= target ); + + for ( int i = 0; i < finalCount; ++i ) + { + int index = hull[i].originalIndex; + B3_ASSERT( 0 <= index && index < count ); + manifold->points[i] = points[index]; + } + + manifold->pointCount = finalCount; +} + +#endif + void b3CollideSpheres( b3LocalManifold* manifold, int capacity, const b3Sphere* sphereA, const b3Sphere* sphereB, b3Transform transformBtoA ) { @@ -984,6 +1043,7 @@ static int b3BuildPolygon( b3ClipVertex* out, b3Transform transform, const b3Hul static bool b3BuildFaceAContact( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA, b3SeparatingAxis query, b3SATCache* cache ) { + B3_ASSERT( capacity > 0 ); B3_VALIDATE( query.type == b3_faceAxisA ); B3_VALIDATE( 0 <= query.indexA && query.indexA < hullA->faceCount ); B3_VALIDATE( 0 <= query.indexB && query.indexB < hullB->vertexCount ); diff --git a/src/core.c b/src/core.c index 9ba7ecb..910a752 100644 --- a/src/core.c +++ b/src/core.c @@ -36,6 +36,8 @@ #include +_Static_assert( B3_MAX_MANIFOLD_POINTS >= 4, "B3_MAX_MANIFOLD_POINTS must be at least 4" ); + // This allows the user to change the length units at runtime static float b3_lengthUnitsPerMeter = 1.0f; @@ -128,6 +130,11 @@ bool b3IsDoublePrecision( void ) #endif } +int b3GetMaxManifoldPoints( void ) +{ + return B3_MAX_MANIFOLD_POINTS; +} + static b3AllocFcn* b3_allocFcn = NULL; static b3FreeFcn* b3_freeFcn = NULL; diff --git a/src/hull.c b/src/hull.c index 84cd8ed..33954d4 100644 --- a/src/hull.c +++ b/src/hull.c @@ -2945,3 +2945,186 @@ b3BoxHull b3MakeScaledBoxHull( b3Vec3 halfWidths, b3Transform transform, b3Vec3 b3ScaleBox( &h, &xf, postScale, 4.0f * B3_LINEAR_SLOP ); return b3MakeTransformedBoxHull( h.x, h.y, h.z, xf ); } + +// 2D hull (Andrew's monotone chain) + +// cross(b - a, c - a) +static inline float b3Cross2D( b3Vec2 a, b3Vec2 b, b3Vec2 c ) +{ + return ( b.x - a.x ) * ( c.y - a.y ) - ( b.y - a.y ) * ( c.x - a.x ); +} + +// Iteratively remove the vertices contributing the least area. +int b3SimplifyHull2D( b3Point2D* hull, int count1, int target ) +{ + B3_ASSERT( target >= 3 ); + + if ( count1 <= 3 ) + { + return count1; + } + + float linearSlop = B3_LINEAR_SLOP; + float areaTol = 0.25f * linearSlop * linearSlop; + int count2 = count1; + + for ( ;; ) + { + float minArea = FLT_MAX; + int minIndex = 0; + + for ( int i = 0; i < count2; i++ ) + { + int prev = ( i + count2 - 1 ) % count2; + int next = ( i + 1 ) % count2; + float area = b3Cross2D( hull[prev].p, hull[i].p, hull[next].p ); + B3_VALIDATE( area >= -FLT_EPSILON ); + + if ( area + areaTol < minArea ) + { + minArea = area; + minIndex = i; + } + } + + if ( count2 <= 3 || ( minArea > areaTol && count2 <= target ) ) + { + break; + } + + count2 -= 1; + if ( minIndex < count2 ) + { + memmove( hull + minIndex, hull + ( minIndex + 1 ), ( count2 - minIndex ) * sizeof( b3Point2D ) ); + } + } + + return count2; +} + +// Sort comparator: by x, then by y. Insertion since n is typically 20 or less. +static void b3Sort2D( b3Point2D* pts, int count ) +{ + for ( int i = 1; i < count; ++i ) + { + b3Point2D base = pts[i]; + int j = i - 1; + while ( j >= 0 && ( pts[j].p.x > base.p.x || ( pts[j].p.x == base.p.x && pts[j].p.y > base.p.y ) ) ) + { + pts[j + 1] = pts[j]; + j -= 1; + } + pts[j + 1] = base; + } +} + +// Welds points that have been sorted O(n) time +static int b3Weld2D( b3Point2D* pts, int count1 ) +{ + if ( count1 <= 1 ) + { + return count1; + } + + float linearSlop = 0.25f * B3_LINEAR_SLOP; + float tolSqr = linearSlop * linearSlop; + + int count2 = 1; + int baseIndex = 0; + for ( int i = 1; i < count1; i++ ) + { + float distanceSqr = b3DistanceSquared2( pts[i].p, pts[baseIndex].p ); + if ( distanceSqr < tolSqr ) + { + if ( pts[i].originalIndex < pts[baseIndex].originalIndex ) + { + pts[baseIndex] = pts[i]; + } + } + else + { + pts[count2] = pts[i]; + baseIndex = count2; + count2 += 1; + } + } + + return count2; +} + +// Andrew's monotone chain convex hull algorithm +// hull must have space for 2 * count1 points +int b3Hull2D( b3Point2D* pts, int count, b3Point2D* hull ) +{ + int count1 = count; + + if ( count1 <= 0 ) + { + return 0; + } + + if ( count1 == 1 ) + { + hull[0] = pts[0]; + return 1; + } + + b3Sort2D( pts, count1 ); + count1 = b3Weld2D( pts, count1 ); + + if ( count1 == 1 ) + { + hull[0] = pts[0]; + return 1; + } + + if ( count1 == 2 ) + { + hull[0] = pts[0]; + hull[1] = pts[1]; + return 2; + } + + int count2 = 0; + + for ( int i = 0; i < count1; i++ ) + { + while ( count2 >= 2 ) + { + float area = b3Cross2D( hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p ); + if ( area > 0.0f ) + { + break; + } + + count2 -= 1; + } + + B3_VALIDATE( count2 < 2 * count1 ); + hull[count2] = pts[i]; + count2 += 1; + } + + int lowerCount = count2 + 1; + + for ( int i = count1 - 2; i >= 0; i-- ) + { + while ( count2 >= lowerCount ) + { + float area = b3Cross2D( hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p ); + if ( area > 0.0f ) + { + break; + } + + count2 -= 1; + } + + B3_VALIDATE( count2 < 2 * count1 ); + hull[count2] = pts[i]; + count2 += 1; + } + + B3_VALIDATE( hull[0].originalIndex == hull[count2 - 1].originalIndex ); + return count2 - 1; +} diff --git a/src/joint.c b/src/joint.c index f2d76c6..0549dc0 100644 --- a/src/joint.c +++ b/src/joint.c @@ -149,6 +149,46 @@ b3JointSim* b3GetJointSimCheckType( b3JointId jointId, b3JointType type ) return jointSim; } +static void b3DestroyContactsBetweenBodies( b3World* world, b3Body* bodyA, b3Body* bodyB ) +{ + int contactKey; + int otherBodyId; + + // use the smaller of the two contact lists + if ( bodyA->contactCount < bodyB->contactCount ) + { + contactKey = bodyA->headContactKey; + otherBodyId = bodyB->id; + } + else + { + contactKey = bodyB->headContactKey; + otherBodyId = bodyA->id; + } + + // no need to wake bodies when a joint removes collision between them + bool wakeBodies = false; + + // destroy the contacts + while ( contactKey != B3_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + int otherEdgeIndex = edgeIndex ^ 1; + if ( contact->edges[otherEdgeIndex].bodyId == otherBodyId ) + { + // Careful, this removes the contact from the current doubly linked list + b3DestroyContact( world, contact, wakeBodies ); + } + } + + b3ValidateSolverSets( world ); +} + typedef struct b3JointPair { b3Joint* joint; @@ -159,10 +199,20 @@ static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3Joint { B3_ASSERT( b3IsValidTransform( def->localFrameA ) ); B3_ASSERT( b3IsValidTransform( def->localFrameB ) ); + B3_ASSERT( world->worldId == def->bodyIdA.world0 ); + B3_ASSERT( world->worldId == def->bodyIdB.world0 ); + B3_ASSERT( B3_ID_EQUALS( def->bodyIdA, def->bodyIdB ) == false ); b3Body* bodyA = b3GetBodyFullId( world, def->bodyIdA ); b3Body* bodyB = b3GetBodyFullId( world, def->bodyIdB ); + // If the joint prevents collisions, then destroy all contacts between attached bodies. + // Do this first so world arrays are not disturbed in the following code. + if ( def->collideConnected == false ) + { + b3DestroyContactsBetweenBodies( world, bodyA, bodyB ); + } + int bodyIdA = bodyA->id; int bodyIdB = bodyB->id; int maxSetIndex = b3MaxInt( bodyA->setIndex, bodyB->setIndex ); @@ -333,46 +383,6 @@ static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3Joint return (b3JointPair){ joint, jointSim }; } -static void b3DestroyContactsBetweenBodies( b3World* world, b3Body* bodyA, b3Body* bodyB ) -{ - int contactKey; - int otherBodyId; - - // use the smaller of the two contact lists - if ( bodyA->contactCount < bodyB->contactCount ) - { - contactKey = bodyA->headContactKey; - otherBodyId = bodyB->id; - } - else - { - contactKey = bodyB->headContactKey; - otherBodyId = bodyA->id; - } - - // no need to wake bodies when a joint removes collision between them - bool wakeBodies = false; - - // destroy the contacts - while ( contactKey != B3_NULL_INDEX ) - { - int contactId = contactKey >> 1; - int edgeIndex = contactKey & 1; - - b3Contact* contact = b3Array_Get( world->contacts, contactId ); - contactKey = contact->edges[edgeIndex].nextKey; - - int otherEdgeIndex = edgeIndex ^ 1; - if ( contact->edges[otherEdgeIndex].bodyId == otherBodyId ) - { - // Careful, this removes the contact from the current doubly linked list - b3DestroyContact( world, contact, wakeBodies ); - } - } - - b3ValidateSolverSets( world ); -} - void b3Joint_SetConstraintTuning( b3JointId jointId, float hertz, float dampingRatio ) { B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f ); diff --git a/src/mesh_contact.c b/src/mesh_contact.c index b3471ce..43a50f9 100644 --- a/src/mesh_contact.c +++ b/src/mesh_contact.c @@ -271,6 +271,8 @@ static inline bool b3AddVertex( b3FoundVertices* vertices, int vertex ) return true; } +#if B3_MAX_MANIFOLD_POINTS == 4 + // Returns true if (score, separation) should replace (bestScore, bestSeparation). static inline bool b3IsBetterCullCandidate( float score, float separation, float bestScore, float bestSeparation, float scoreTol, float separationTol ) @@ -288,13 +290,6 @@ static inline bool b3IsBetterCullCandidate( float score, float separation, float return separation < bestSeparation - separationTol; } -typedef struct b3Point2D -{ - b3Vec2 p; - float separation; - int originalIndex; -} b3Point2D; - static int b3CullPoints( b3Point2D* points, int count ) { if ( count <= 1 ) @@ -475,6 +470,8 @@ static int b3CullPoints( b3Point2D* points, int count ) return 4; } +#endif + static int b3ReduceCluster( b3LocalManifoldPoint* points, int count1, b3Vec3 normal, b3Arena arena ) { int targetCount = 1; @@ -496,13 +493,20 @@ static int b3ReduceCluster( b3LocalManifoldPoint* points, int count1, b3Vec3 nor pts[i].originalIndex = i; } +#if B3_MAX_MANIFOLD_POINTS == 4 + b3Point2D* hull = pts; int count2 = b3CullPoints( pts, count1 ); +#else + b3Point2D* hull = b3Bump( &arena, 2 * count1 * sizeof( b3Point2D ) ); + int hullCount = b3Hull2D( pts, count1, hull ); + int count2 = b3SimplifyHull2D( hull, hullCount, B3_MAX_MANIFOLD_POINTS ); +#endif B3_ASSERT( count2 <= B3_MAX_MANIFOLD_POINTS ); b3LocalManifoldPoint finalPoints[B3_MAX_MANIFOLD_POINTS]; for ( int i = 0; i < count2; ++i ) { - int index = pts[i].originalIndex; + int index = hull[i].originalIndex; B3_ASSERT( 0 <= index && index < count1 ); finalPoints[i] = points[index]; } diff --git a/src/physics_world.c b/src/physics_world.c index 29c13c7..0f5a5b3 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -615,7 +615,6 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* bool isStaticA = bodyA->type == b3_staticBody; bool isStaticB = bodyB->type == b3_staticBody; bool wasTouching = ( contact->flags & b3_simTouchingFlag ); - bool isMeshContact = ( contact->flags & b3_simMeshContact ); b3BodySim* bodySimA; b3BodySim* bodySimB; if ( wasTouching ) @@ -653,7 +652,7 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* // Contact recycling optimization. Please cite this library if you use this optimization. // This is inspired by persistent contact manifolds used in some physics engines, such as PhysX. // However, this allows larger relative motion and has fewer tuning parameters (just one). - if ( ( isFast == false || isMeshContact == false ) && recycleDistance > 0.0f && + if ( isFast == false && recycleDistance > 0.0f && ( contact->flags & b3_relativeTransformValid ) && ( contact->flags & b3_contactRecycleFlag ) ) { // The scalar part of b3InvMulQuat is just the quaternion dot product. @@ -1149,6 +1148,7 @@ void b3World_Step( b3WorldId worldId, float timeStep, int subStepCount ) world->finishTaskFcn( world->userTreeTask, world->userTaskContext ); world->userTreeTask = NULL; world->activeTaskCount -= 1; + b3ValidateNoEnlarged( &world->broadPhase ); } // Update sensors diff --git a/src/platform.h b/src/platform.h index 2bc37a5..399cea5 100644 --- a/src/platform.h +++ b/src/platform.h @@ -8,17 +8,19 @@ #include #include -// Software prefetch hint. T0 brings the line into all cache levels. -// On x86 MSVC exposes _mm_prefetch, ARM MSVC uses __prefetch instead. -// clang/gcc provide __builtin_prefetch on every target. -#if defined( B3_COMPILER_MSVC ) +#if defined( _MSC_VER ) && ( defined( _M_ARM ) || defined( _M_ARM64 ) || defined( _M_ARM64EC ) ) #include -#if defined( B3_CPU_X86_X64 ) +#elif defined( _MSC_VER ) +#include +#endif + +#if defined( _MSC_VER ) +#if defined( _M_X64 ) || defined( __x86_64__ ) || defined( _M_IX86 ) || defined( __i386__ ) #define b3Prefetch( addr ) _mm_prefetch( (const char*)( addr ), _MM_HINT_T0 ) #else #define b3Prefetch( addr ) __prefetch( (const void*)( addr ) ) #endif -#elif defined( B3_COMPILER_CLANG ) || defined( B3_COMPILER_GCC ) +#elif defined( __GNUC__ ) || defined( __clang__ ) #define b3Prefetch( addr ) __builtin_prefetch( (const void*)( addr ), 0, 3 ) #else #define b3Prefetch( addr ) ( (void)( addr ) ) diff --git a/src/recording.c b/src/recording.c index 0f47fc1..d669813 100644 --- a/src/recording.c +++ b/src/recording.c @@ -320,7 +320,7 @@ static void b3RecW_STR( b3RecBuffer* buf, const char* s ) // single-precision and double-precision sizes (equal for most), so either build configuration passes. _Static_assert( sizeof( void* ) != 8 || sizeof( b3ExplosionDef ) == 32 || sizeof( b3ExplosionDef ) == 48, "b3ExplosionDef changed: update b3RecW_EXPLOSIONDEF and b3RecR_EXPLOSIONDEF together" ); -_Static_assert( sizeof( void* ) != 8 || sizeof( b3BodyDef ) == 104 || sizeof( b3BodyDef ) == 120, +_Static_assert( sizeof( void* ) != 8 || sizeof( b3BodyDef ) == 112 || sizeof( b3BodyDef ) == 128, "b3BodyDef changed: update b3RecW_BODYDEF and b3RecR_BODYDEF together" ); _Static_assert( sizeof( void* ) != 8 || sizeof( b3ShapeDef ) == 120, "b3ShapeDef changed: update b3RecW_SHAPEDEF and b3RecR_SHAPEDEF together" ); @@ -363,6 +363,7 @@ void b3RecW_BODYDEF( b3RecBuffer* buf, b3BodyDef v ) b3RecW_F32( buf, v.angularDamping ); b3RecW_F32( buf, v.gravityScale ); b3RecW_F32( buf, v.sleepThreshold ); + b3RecW_F32( buf, v.safetyFactor ); b3RecW_STR( buf, v.name ); // userData: not preserved b3RecW_U64( buf, 0u ); diff --git a/src/recording.h b/src/recording.h index 7870192..a198f70 100644 --- a/src/recording.h +++ b/src/recording.h @@ -47,8 +47,8 @@ typedef struct b3World b3World; #define B3_REC_MAGIC 0x43523342u // Major recording version is bumped when writers change. -// Major version 5 added b3PlaneResult fields. -#define B3_REC_VERSION_MAJOR 5 +// Major version 6 widened the body def with the safety factor. +#define B3_REC_VERSION_MAJOR 6 // Minor tracks op-stream additions that keep the 48 byte header shape. // Minor version 4 added b3Shape_SetMeshMaterial, b3Shape_SetHull, b3Shape_SetMesh diff --git a/src/recording_ops.inl b/src/recording_ops.inl index 3f0cf42..6b6973d 100644 --- a/src/recording_ops.inl +++ b/src/recording_ops.inl @@ -69,6 +69,7 @@ B3_REC_OP( 0x37, BodySetBullet, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) B3_REC_OP( 0x38, BodyEnableContactRecycling, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) ) B3_REC_OP( 0x39, BodyEnableHitEvents, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) ) B3_REC_OP( 0x3A, BodyAllowFastRotation, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) ) +B3_REC_OP( 0x3B, BodySetSafetyFactor, RET_NONE, ARG( BODYID, body ) ARG( F32, value ) ) // Shape create/destroy B3_REC_OP( 0x40, CreateSphereShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( SPHERE, sphere ) ) diff --git a/src/recording_replay.c b/src/recording_replay.c index d1a40bc..dc23de2 100644 --- a/src/recording_replay.c +++ b/src/recording_replay.c @@ -428,6 +428,7 @@ b3BodyDef b3RecR_BODYDEF( b3RecReader* rdr ) def.angularDamping = b3RecR_F32( rdr ); def.gravityScale = b3RecR_F32( rdr ); def.sleepThreshold = b3RecR_F32( rdr ); + def.safetyFactor = b3RecR_F32( rdr ); def.name = b3RecR_STR( rdr ); (void)b3RecR_U64( rdr ); // userData placeholder def.motionLocks = b3RecR_LOCKS( rdr ); @@ -939,6 +940,11 @@ static void b3RecDispatch_BodySetSleepThreshold( const b3RecArgs_BodySetSleepThr b3Body_SetSleepThreshold( b3RecMakeBodyId( rdr, a->body ), a->threshold ); } +static void b3RecDispatch_BodySetSafetyFactor( const b3RecArgs_BodySetSafetyFactor* a, b3RecReader* rdr ) +{ + b3Body_SetSafetyFactor( b3RecMakeBodyId( rdr, a->body ), a->value ); +} + static void b3RecDispatch_BodyDisable( const b3RecArgs_BodyDisable* a, b3RecReader* rdr ) { b3Body_Disable( b3RecMakeBodyId( rdr, a->body ) ); diff --git a/src/solver.c b/src/solver.c index d8aace0..ad5d003 100644 --- a/src/solver.c +++ b/src/solver.c @@ -756,11 +756,11 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex, // Body is not sleepy body->sleepTime = 0.0f; - const float safetyFactor = 0.5f; + float safetyFactor = body->safetyFactor; float maxMotion = b3MaxFloat( maxDeltaPosition, maxVelocity * timeStep ); if ( body->type == b3_dynamicBody && enableContinuous && maxMotion > safetyFactor * sim->minExtent ) { - // This flag is only retained for debug draw + // This flag is used for debug draw and contact recycling. sim->flags |= b3_isFast; // Store in fast array for the continuous collision stage @@ -1444,7 +1444,6 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) int awakeBodyCount = awakeSet->bodySims.count; if ( awakeBodyCount == 0 ) { - b3ValidateNoEnlarged( &world->broadPhase ); return; } diff --git a/src/types.c b/src/types.c index 7e2cd07..13d947b 100644 --- a/src/types.c +++ b/src/types.c @@ -35,6 +35,7 @@ b3BodyDef b3DefaultBodyDef( void ) def.type = b3_staticBody; def.rotation = b3Quat_identity; def.sleepThreshold = 0.05f * b3GetLengthUnitsPerMeter(); + def.safetyFactor = 0.5f; def.gravityScale = 1.0f; def.enableSleep = true; def.isAwake = true; diff --git a/src/world_snapshot.c b/src/world_snapshot.c index 9efaee9..88e5be9 100644 --- a/src/world_snapshot.c +++ b/src/world_snapshot.c @@ -33,7 +33,7 @@ // Snapshot image magic 'BNS3' and version #define B3_SNAP_MAGIC 0x33534E42u -#define B3_SNAP_VERSION 2u +#define B3_SNAP_VERSION 3u // added b3Body::safetyFactor #define B3_SNAP_FLAG_VALIDATION 0x1u #define B3_SNAP_FLAG_DOUBLE_PRECISION 0x2u diff --git a/test/test_body_query.c b/test/test_body_query.c index ca05c35..586a21b 100644 --- a/test/test_body_query.c +++ b/test/test_body_query.c @@ -362,6 +362,61 @@ static bool CountOverlapCallback( b3ShapeId shapeId, void* context ) return true; } +// A baked compound keeps its tree and its child placements in the compound frame while the query +// proxy is in world space, so a compound on a turned body only answers correctly if the proxy is +// brought back into that frame first. +static int OverlapCompoundRotatedBody( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + b3CompoundSphereDef spheres[2] = { + { .sphere = { { -3.0f, 0.0f, 0.0f }, 0.5f }, .material = mat }, + { .sphere = { { 3.0f, 0.0f, 0.0f }, 0.5f }, .material = mat }, + }; + b3CompoundDef compoundDef = { .spheres = spheres, .sphereCount = 2 }; + b3CompoundData* compound = b3CreateCompound( &compoundDef ); + ENSURE( compound != NULL ); + + b3Transform pose = { .p = { 10.0f, 20.0f, 30.0f }, .q = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.5f * B3_PI ) }; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_staticBody; + bodyDef.position = (b3Pos){ pose.p.x, pose.p.y, pose.p.z }; + bodyDef.rotation = pose.q; + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateBakedCompoundShape( bodyId, &shapeDef, compound ); + + b3QueryFilter filter = b3DefaultQueryFilter(); + + // Sitting on the second child, which the body pose has swung onto the +Y side + b3Vec3 onChild = b3TransformPoint( pose, (b3Vec3){ 3.0f, 0.0f, 0.0f } ); + b3ShapeProxy hitProxy = { &onChild, 1, 0.1f }; + + int hits = 0; + b3World_OverlapShape( worldId, b3Pos_zero, &hitProxy, filter, CountOverlapCallback, &hits ); + ENSURE( hits == 1 ); + + b3WorldTransform bodyTransform = { .p = bodyDef.position, .q = bodyDef.rotation }; + ENSURE( b3Body_OverlapShape( bodyId, b3Pos_zero, &hitProxy, filter, bodyTransform ) ); + + // The gap between the two children stays empty + b3Vec3 inGap = b3TransformPoint( pose, (b3Vec3){ 0.0f, 0.0f, 0.0f } ); + b3ShapeProxy gapProxy = { &inGap, 1, 0.25f }; + + hits = 0; + b3World_OverlapShape( worldId, b3Pos_zero, &gapProxy, filter, CountOverlapCallback, &hits ); + ENSURE( hits == 0 ); + ENSURE( b3Body_OverlapShape( bodyId, b3Pos_zero, &gapProxy, filter, bodyTransform ) == false ); + + b3DestroyWorld( worldId ); + b3DestroyCompound( compound ); + return 0; +} + // A box hull proxy built around a world target with a zero origin must hit the same shapes as the // same box built at the local origin and queried with the target as origin. This is the origin // relative equivalence the world query promises, and the pattern users reach for when they bake a @@ -990,6 +1045,7 @@ int BodyQueryTest( void ) RUN_SUBTEST( OverlapFilter ); RUN_SUBTEST( OverlapHullProxyEquivalence ); RUN_SUBTEST( OverlapHullProxyRotation ); + RUN_SUBTEST( OverlapCompoundRotatedBody ); RUN_SUBTEST( MoverTouchesBox ); RUN_SUBTEST( MoverSeparated ); diff --git a/test/test_compound.c b/test/test_compound.c index f0aaa1f..40447bc 100644 --- a/test/test_compound.c +++ b/test/test_compound.c @@ -583,6 +583,140 @@ static int CompoundShapeCastClosest( void ) return 0; } +static int CompoundShapeCastMiss( void ) +{ + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + b3CompoundSphereDef sph = { .sphere = { { 5, 0, 0 }, 1.0f }, .material = mat }; + b3CompoundDef def = { .spheres = &sph, .sphereCount = 1 }; + b3CompoundData* c = b3CreateCompound( &def ); + + b3Vec3 point = { 0, 5, 0 }; + b3ShapeCastInput input = { + .proxy = { .points = &point, .count = 1, .radius = 0.25f }, + .translation = { 20, 0, 0 }, + .maxFraction = 1.0f, + .canEncroach = false, + }; + ENSURE( b3ShapeCastCompound( c, &input ).hit == false ); + + // An empty proxy has nothing to sweep + input.proxy.count = 0; + ENSURE( b3ShapeCastCompound( c, &input ).hit == false ); + + b3DestroyCompound( c ); + return 0; +} + +static int CompoundShapeCastHullNormalRotation( void ) +{ + // Same setup as the ray cast rotation test. The result comes back in compound space, so the + // hull local face the sweep lands on says nothing about the reported normal and point. + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + b3BoxHull box = b3MakeBoxHull( 1, 1, 1 ); + + b3CompoundHullDef hull = { + .hull = &box.base, + .transform = { .p = { 5, 0, 0 }, .q = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.5f * B3_PI ) }, + .material = mat, + }; + b3CompoundDef def = { .hulls = &hull, .hullCount = 1 }; + b3CompoundData* c = b3CreateCompound( &def ); + + b3Vec3 point = { 0, 0, 0 }; + b3ShapeCastInput input = { + .proxy = { .points = &point, .count = 1, .radius = 0.25f }, + .translation = { 20, 0, 0 }, + .maxFraction = 1.0f, + .canEncroach = false, + }; + b3CastOutput out = b3ShapeCastCompound( c, &input ); + ENSURE( out.hit == true ); + + // Near face at x=4, caster radius 0.25 so contact at x=3.75 + ENSURE_SMALL( out.fraction - 3.75f / 20.0f, 1e-3f ); + ENSURE_SMALL( out.normal.x + 1.0f, 1e-3f ); + ENSURE_SMALL( out.normal.y, 1e-3f ); + ENSURE_SMALL( out.normal.z, 1e-3f ); + ENSURE_SMALL( out.point.x - 4.0f, 1e-3f ); + ENSURE_SMALL( out.point.y, 1e-3f ); + ENSURE_SMALL( out.point.z, 1e-3f ); + ENSURE( out.childIndex == 0 ); + + b3DestroyCompound( c ); + return 0; +} + +// Two separated upward facing triangles on the y = 0 plane, one per material. The bake may +// reorder triangles but always keeps a triangle paired with its material index. +static b3MeshData* MakeTwoMaterialMesh( void ) +{ + b3Vec3 vertices[6] = { + { -3.0f, 0.0f, -1.0f }, { -2.0f, 0.0f, 1.0f }, { -1.0f, 0.0f, -1.0f }, + { 1.0f, 0.0f, -1.0f }, { 2.0f, 0.0f, 1.0f }, { 3.0f, 0.0f, -1.0f }, + }; + int32_t indices[6] = { 0, 1, 2, 3, 4, 5 }; + uint8_t materialIndices[2] = { 0, 1 }; + + b3MeshDef def = { 0 }; + def.vertices = vertices; + def.stride = sizeof( b3Vec3 ); + def.indices = indices; + def.materialIndices = materialIndices; + def.vertexCount = 6; + def.triangleCount = 2; + + return b3CreateMesh( &def, NULL, 0 ); +} + +static int CompoundMeshMaterialRemap( void ) +{ + // A mesh child carries its own small material table. Casts report the triangle material + // after it has been remapped into the compound material array. + b3SurfaceMaterial matA = MakeMaterial( 0.3f, 100 ); + b3SurfaceMaterial matB = MakeMaterial( 0.7f, 200 ); + b3SurfaceMaterial materials[2] = { matA, matB }; + + b3MeshData* md = MakeTwoMaterialMesh(); + b3CompoundMeshDef mesh = { + .meshData = md, + .transform = b3Transform_identity, + .scale = { 1, 1, 1 }, + .materials = materials, + .materialCount = 2, + }; + b3CompoundDef def = { .meshes = &mesh, .meshCount = 1 }; + b3CompoundData* c = b3CreateCompound( &def ); + + const b3SurfaceMaterial* table = b3GetCompoundMaterials( c ); + + // Straight down onto the material 0 triangle + b3RayCastInput rayA = { .origin = { -2, 5, 0 }, .translation = { 0, -10, 0 }, .maxFraction = 1.0f }; + b3CastOutput outA = b3RayCastCompound( c, &rayA ); + ENSURE( outA.hit == true ); + ENSURE( table[outA.materialIndex].userMaterialId == 100 ); + + b3RayCastInput rayB = { .origin = { 2, 5, 0 }, .translation = { 0, -10, 0 }, .maxFraction = 1.0f }; + b3CastOutput outB = b3RayCastCompound( c, &rayB ); + ENSURE( outB.hit == true ); + ENSURE( table[outB.materialIndex].userMaterialId == 200 ); + + // The shape cast path does the same remap + b3Vec3 point = { 2, 5, 0 }; + b3ShapeCastInput castInput = { + .proxy = { .points = &point, .count = 1, .radius = 0.1f }, + .translation = { 0, -10, 0 }, + .maxFraction = 1.0f, + .canEncroach = false, + }; + b3CastOutput outCast = b3ShapeCastCompound( c, &castInput ); + ENSURE( outCast.hit == true ); + ENSURE( table[outCast.materialIndex].userMaterialId == 200 ); + + b3DestroyCompound( c ); + b3DestroyMesh( md ); + return 0; +} + static int CompoundOverlap( void ) { b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); @@ -607,6 +741,132 @@ static int CompoundOverlap( void ) return 0; } +static int CompoundOverlapTransformed( void ) +{ + // The tree and the child geometry live in the compound frame while the proxy arrives in world + // space, so a compound that is moved and turned has to pull the proxy back before querying. + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + b3CompoundSphereDef spheres[2] = { + { .sphere = { { -3, 0, 0 }, 0.5f }, .material = mat }, + { .sphere = { { 3, 0, 0 }, 0.5f }, .material = mat }, + }; + b3CompoundDef def = { .spheres = spheres, .sphereCount = 2 }; + b3CompoundData* c = b3CreateCompound( &def ); + + b3Transform transform = { .p = { 10, 20, 30 }, .q = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.5f * B3_PI ) }; + + b3Vec3 gapPoint = b3TransformPoint( transform, (b3Vec3){ 0, 0, 0 } ); + b3ShapeProxy gap = { .points = &gapPoint, .count = 1, .radius = 0.25f }; + ENSURE( b3OverlapCompound( c, transform, &gap ) == false ); + + b3Vec3 hitPoint = b3TransformPoint( transform, (b3Vec3){ 3, 0, 0 } ); + b3ShapeProxy hit = { .points = &hitPoint, .count = 1, .radius = 0.1f }; + ENSURE( b3OverlapCompound( c, transform, &hit ) == true ); + + // A proxy left in the compound frame must not register against the moved compound + b3Vec3 localPoint = { 3, 0, 0 }; + b3ShapeProxy stale = { .points = &localPoint, .count = 1, .radius = 0.1f }; + ENSURE( b3OverlapCompound( c, transform, &stale ) == false ); + + b3DestroyCompound( c ); + return 0; +} + +static int CompoundOverlapChildTypes( void ) +{ + // One child of every type, spaced far enough apart that a hit names the type it came from. + // Hull and mesh children add a child transform underneath the compound transform. + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3MeshData* md = b3CreateBoxMesh( b3Vec3_zero, (b3Vec3){ 0.5f, 0.5f, 0.5f }, false ); + + b3CompoundCapsuleDef capsule = { .capsule = { { -10, 0, 0 }, { -9, 0, 0 }, 0.25f }, .material = mat }; + b3CompoundHullDef hull = { + .hull = &box.base, + .transform = { .p = { 0, 0, 0 }, .q = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.25f * B3_PI ) }, + .material = mat, + }; + b3CompoundMeshDef mesh = { + .meshData = md, + .transform = { .p = { 10, 0, 0 }, .q = b3MakeQuatFromAxisAngle( b3Vec3_axisY, 0.5f * B3_PI ) }, + .scale = { 1, 1, 1 }, + .materials = &mat, + .materialCount = 1, + }; + b3CompoundSphereDef sphere = { .sphere = { { 20, 0, 0 }, 0.5f }, .material = mat }; + + b3CompoundDef def = { + .capsules = &capsule, + .capsuleCount = 1, + .hulls = &hull, + .hullCount = 1, + .meshes = &mesh, + .meshCount = 1, + .spheres = &sphere, + .sphereCount = 1, + }; + b3CompoundData* c = b3CreateCompound( &def ); + + b3Transform transform = { .p = { 100, 200, 300 }, .q = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.5f * B3_PI ) }; + + // Points sitting on or inside each child, then carried out to world space + b3Vec3 localPoints[4] = { + { -9.5f, 0, 0 }, // capsule axis + { 0, 0, 0 }, // hull center + { 10, 0.45f, 0 }, // just under a mesh face + { 20, 0, 0 }, // sphere center + }; + + for ( int i = 0; i < 4; ++i ) + { + b3Vec3 worldPoint = b3TransformPoint( transform, localPoints[i] ); + b3ShapeProxy proxy = { .points = &worldPoint, .count = 1, .radius = 0.1f }; + ENSURE( b3OverlapCompound( c, transform, &proxy ) == true ); + } + + // A gap between the hull and the mesh + b3Vec3 gapPoint = b3TransformPoint( transform, (b3Vec3){ 5, 0, 0 } ); + b3ShapeProxy gap = { .points = &gapPoint, .count = 1, .radius = 0.1f }; + ENSURE( b3OverlapCompound( c, transform, &gap ) == false ); + + b3DestroyCompound( c ); + b3DestroyMesh( md ); + return 0; +} + +static int CompoundOverlapSegmentProxy( void ) +{ + // A multi point proxy has to be carried into the compound frame point by point + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + b3CompoundSphereDef spheres[2] = { + { .sphere = { { -3, 0, 0 }, 0.5f }, .material = mat }, + { .sphere = { { 3, 0, 0 }, 0.5f }, .material = mat }, + }; + b3CompoundDef def = { .spheres = spheres, .sphereCount = 2 }; + b3CompoundData* c = b3CreateCompound( &def ); + + b3Transform transform = { .p = { -40, 15, 7 }, .q = b3MakeQuatFromAxisAngle( b3Vec3_axisY, 0.5f * B3_PI ) }; + + // Segment that stays inside the gap + b3Vec3 inGap[2] = { + b3TransformPoint( transform, (b3Vec3){ -1, 0, 0 } ), + b3TransformPoint( transform, (b3Vec3){ 1, 0, 0 } ), + }; + b3ShapeProxy gap = { .points = inGap, .count = 2, .radius = 0.25f }; + ENSURE( b3OverlapCompound( c, transform, &gap ) == false ); + + // Same segment stretched until one end reaches the second sphere + b3Vec3 reaching[2] = { + b3TransformPoint( transform, (b3Vec3){ -1, 0, 0 } ), + b3TransformPoint( transform, (b3Vec3){ 2.9f, 0, 0 } ), + }; + b3ShapeProxy hit = { .points = reaching, .count = 2, .radius = 0.25f }; + ENSURE( b3OverlapCompound( c, transform, &hit ) == true ); + + b3DestroyCompound( c ); + return 0; +} + struct QueryAccumulator { int childIndices[8]; @@ -704,6 +964,41 @@ static int CompoundMover( void ) return 0; } +static int CompoundMoverRotatedChild( void ) +{ + // The child solves the mover in its own frame, so the plane it returns has to be carried + // back into the compound frame before the caller sees it. + b3SurfaceMaterial mat = MakeMaterial( 0.25f, 77 ); + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + + // Quarter turn about z, so the compound space top face is a side face of the hull + b3CompoundHullDef hull = { + .hull = &box.base, + .transform = { .p = { 0, 0, 0 }, .q = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.5f * B3_PI ) }, + .material = mat, + }; + b3CompoundDef def = { .hulls = &hull, .hullCount = 1 }; + b3CompoundData* c = b3CreateCompound( &def ); + + b3Capsule mover = { { -0.1f, 0.6f, 0 }, { 0.1f, 0.6f, 0 }, 0.2f }; + + b3PlaneResult planes[8] = { 0 }; + int planeCount = b3CollideMoverAndCompound( planes, 8, c, &mover ); + ENSURE( planeCount >= 1 ); + + ENSURE_SMALL( planes[0].plane.normal.x, 1e-3f ); + ENSURE_SMALL( planes[0].plane.normal.y - 1.0f, 1e-3f ); + ENSURE_SMALL( planes[0].plane.normal.z, 1e-3f ); + ENSURE_SMALL( planes[0].point.y - 0.5f, 1e-3f ); + + ENSURE( planes[0].childIndex == 0 ); + const b3SurfaceMaterial* table = b3GetCompoundMaterials( c ); + ENSURE( table[planes[0].materialIndex].userMaterialId == 77 ); + + b3DestroyCompound( c ); + return 0; +} + static b3CompoundData* BuildSerializableCompound( b3MeshData* md ) { b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); @@ -841,11 +1136,18 @@ int CompoundTest( void ) RUN_SUBTEST( CompoundRayCastClosest ); RUN_SUBTEST( CompoundRayCastHullNormalRotation ); RUN_SUBTEST( CompoundShapeCastClosest ); + RUN_SUBTEST( CompoundShapeCastMiss ); + RUN_SUBTEST( CompoundShapeCastHullNormalRotation ); + RUN_SUBTEST( CompoundMeshMaterialRemap ); RUN_SUBTEST( CompoundOverlap ); + RUN_SUBTEST( CompoundOverlapTransformed ); + RUN_SUBTEST( CompoundOverlapChildTypes ); + RUN_SUBTEST( CompoundOverlapSegmentProxy ); RUN_SUBTEST( CompoundQuery ); RUN_SUBTEST( CompoundMover ); + RUN_SUBTEST( CompoundMoverRotatedChild ); RUN_SUBTEST( CompoundSerializeRoundtrip ); RUN_SUBTEST( CompoundSerializeBadVersion ); diff --git a/test/test_determinism.c b/test/test_determinism.c index 0b29d45..02861e8 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -18,10 +18,10 @@ // 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 297 -#define RAGDOLL_HASH 0xF4036C3A -#define WAVE_PILE_SLEEP_STEP 297 -#define WAVE_PILE_HASH 0x658C00CF +#define RAGDOLL_SLEEP_STEP 272 +#define RAGDOLL_HASH 0xBF43ECD7 +#define WAVE_PILE_SLEEP_STEP 291 +#define WAVE_PILE_HASH 0x8B6E2CD2 #define QUERY_SPAWN_SLEEP_STEP 242 #define QUERY_SPAWN_HASH 0x1737F5BC #define QUERY_SPAWN_HIT_COUNT 59 @@ -29,10 +29,10 @@ #define MESH_DROP_SLEEP_STEP 251 #define MESH_DROP_HASH 0x465381C5 #else -#define RAGDOLL_SLEEP_STEP 308 -#define RAGDOLL_HASH 0x4D94E049 -#define WAVE_PILE_SLEEP_STEP 273 -#define WAVE_PILE_HASH 0x0BA28F3A +#define RAGDOLL_SLEEP_STEP 286 +#define RAGDOLL_HASH 0x0211682A +#define WAVE_PILE_SLEEP_STEP 282 +#define WAVE_PILE_HASH 0xFC161D3D #define QUERY_SPAWN_SLEEP_STEP 242 #define QUERY_SPAWN_HASH 0xB9F993A5 #define QUERY_SPAWN_HIT_COUNT 59 @@ -41,7 +41,44 @@ #define MESH_DROP_HASH 0xE58C7240 #endif -static int SingleMultithreadingTest( int workerCount ) +// The goldens above pin exact values for the default four point manifold. A build that +// overrides B3_MAX_MANIFOLD_POINTS produces a different contact set, so those checks drop +// out and only the run to run agreement below applies. +#if B3_MAX_MANIFOLD_POINTS == 4 +#define ENSURE_GOLDEN( condition ) ENSURE( condition ) +#else +#define ENSURE_GOLDEN( condition ) ( (void)0 ) +#endif + +typedef struct DeterminismResult +{ + int sleepStep; + uint32_t hash; + int queryHitCount; + uint32_t queryHash; + bool seeded; +} DeterminismResult; + +// Every worker count has to reproduce the first run exactly. Scenarios without queries leave +// the query fields zero in both the reference and the result. +static int EnsureRepeatable( DeterminismResult* reference, DeterminismResult result ) +{ + if ( reference->seeded == false ) + { + result.seeded = true; + *reference = result; + return 0; + } + + ENSURE( result.sleepStep == reference->sleepStep ); + ENSURE( result.hash == reference->hash ); + ENSURE( result.queryHitCount == reference->queryHitCount ); + ENSURE( result.queryHash == reference->queryHash ); + + return 0; +} + +static int SingleMultithreadingTest( int workerCount, DeterminismResult* reference ) { b3WorldDef worldDef = b3DefaultWorldDef(); worldDef.workerCount = workerCount; @@ -73,8 +110,9 @@ static int SingleMultithreadingTest( int workerCount ) printf( " workers=%d sleepStep=%d hash=0x%08X\n", workerCount, data.sleepStep, data.hash ); } - ENSURE( data.sleepStep == RAGDOLL_SLEEP_STEP ); - ENSURE( data.hash == RAGDOLL_HASH ); + ENSURE_GOLDEN( data.sleepStep == RAGDOLL_SLEEP_STEP ); + ENSURE_GOLDEN( data.hash == RAGDOLL_HASH ); + ENSURE( EnsureRepeatable( reference, (DeterminismResult){ .sleepStep = data.sleepStep, .hash = data.hash } ) == 0 ); DestroyFallingRagdolls( &data ); @@ -84,9 +122,10 @@ static int SingleMultithreadingTest( int workerCount ) // Test multithreaded determinism. static int MultithreadingTest( void ) { + DeterminismResult reference = { 0 }; for ( int workerCount = 1; workerCount < 6; ++workerCount ) { - int result = SingleMultithreadingTest( workerCount ); + int result = SingleMultithreadingTest( workerCount, &reference ); ENSURE( result == 0 ); } @@ -118,8 +157,8 @@ static int CrossPlatformTest( void ) printf( " cross-platform sleepStep=%d hash=0x%08X\n", data.sleepStep, data.hash ); } - ENSURE( data.sleepStep == RAGDOLL_SLEEP_STEP ); - ENSURE( data.hash == RAGDOLL_HASH ); + ENSURE_GOLDEN( data.sleepStep == RAGDOLL_SLEEP_STEP ); + ENSURE_GOLDEN( data.hash == RAGDOLL_HASH ); DestroyFallingRagdolls( &data ); @@ -128,7 +167,7 @@ static int CrossPlatformTest( void ) return 0; } -static int SingleWavePileTest( int workerCount ) +static int SingleWavePileTest( int workerCount, DeterminismResult* reference ) { b3WorldDef worldDef = b3DefaultWorldDef(); worldDef.workerCount = workerCount; @@ -158,8 +197,9 @@ static int SingleWavePileTest( int workerCount ) } ENSURE( done == true ); - ENSURE( data.sleepStep == WAVE_PILE_SLEEP_STEP ); - ENSURE( data.hash == WAVE_PILE_HASH ); + ENSURE_GOLDEN( data.sleepStep == WAVE_PILE_SLEEP_STEP ); + ENSURE_GOLDEN( data.hash == WAVE_PILE_HASH ); + ENSURE( EnsureRepeatable( reference, (DeterminismResult){ .sleepStep = data.sleepStep, .hash = data.hash } ) == 0 ); DestroyWavePile( &data ); @@ -169,16 +209,17 @@ static int SingleWavePileTest( int workerCount ) // Test multithreaded determinism of a mixed convex pile on a wave height field. static int WavePileTest( void ) { + DeterminismResult reference = { 0 }; for ( int workerCount = 1; workerCount <= 4; ++workerCount ) { - int result = SingleWavePileTest( workerCount ); + int result = SingleWavePileTest( workerCount, &reference ); ENSURE( result == 0 ); } return 0; } -static int SingleQuerySpawnTest( int workerCount ) +static int SingleQuerySpawnTest( int workerCount, DeterminismResult* reference ) { b3WorldDef worldDef = b3DefaultWorldDef(); worldDef.workerCount = workerCount; @@ -210,10 +251,14 @@ static int SingleQuerySpawnTest( int workerCount ) ENSURE( done == true ); ENSURE( data.spawnCount == QUERY_SPAWN_COUNT ); - ENSURE( data.sleepStep == QUERY_SPAWN_SLEEP_STEP ); - ENSURE( data.hash == QUERY_SPAWN_HASH ); - ENSURE( data.queryHitCount == QUERY_SPAWN_HIT_COUNT ); - ENSURE( data.queryHash == QUERY_SPAWN_QUERY_HASH ); + ENSURE_GOLDEN( data.sleepStep == QUERY_SPAWN_SLEEP_STEP ); + ENSURE_GOLDEN( data.hash == QUERY_SPAWN_HASH ); + ENSURE_GOLDEN( data.queryHitCount == QUERY_SPAWN_HIT_COUNT ); + ENSURE_GOLDEN( data.queryHash == QUERY_SPAWN_QUERY_HASH ); + ENSURE( EnsureRepeatable( reference, (DeterminismResult){ .sleepStep = data.sleepStep, + .hash = data.hash, + .queryHitCount = data.queryHitCount, + .queryHash = data.queryHash } ) == 0 ); DestroyQuerySpawn( &data ); @@ -223,16 +268,17 @@ static int SingleQuerySpawnTest( int workerCount ) // Test determinism of world queries by feeding their results back into the simulation. static int QuerySpawnTest( void ) { + DeterminismResult reference = { 0 }; for ( int workerCount = 1; workerCount <= 4; ++workerCount ) { - int result = SingleQuerySpawnTest( workerCount ); + int result = SingleQuerySpawnTest( workerCount, &reference ); ENSURE( result == 0 ); } return 0; } -static int SingleMeshDropTest( int workerCount ) +static int SingleMeshDropTest( int workerCount, DeterminismResult* reference ) { b3WorldDef worldDef = b3DefaultWorldDef(); worldDef.workerCount = workerCount; @@ -261,8 +307,9 @@ static int SingleMeshDropTest( int workerCount ) } ENSURE( done == true ); - ENSURE( data.sleepStep == MESH_DROP_SLEEP_STEP ); - ENSURE( data.hash == MESH_DROP_HASH ); + ENSURE_GOLDEN( data.sleepStep == MESH_DROP_SLEEP_STEP ); + ENSURE_GOLDEN( data.hash == MESH_DROP_HASH ); + ENSURE( EnsureRepeatable( reference, (DeterminismResult){ .sleepStep = data.sleepStep, .hash = data.hash } ) == 0 ); DestroyMeshDrop( &data ); @@ -273,10 +320,11 @@ static int SingleMeshDropTest( int workerCount ) // The scene is large, so only the single threaded and widest schedules run. static int MeshDropTest( void ) { + DeterminismResult reference = { 0 }; int workerCounts[2] = { 1, 4 }; for ( int i = 0; i < 2; ++i ) { - int result = SingleMeshDropTest( workerCounts[i] ); + int result = SingleMeshDropTest( workerCounts[i], &reference ); ENSURE( result == 0 ); } diff --git a/test/test_hull.c b/test/test_hull.c index e4f2458..d2a27be 100644 --- a/test/test_hull.c +++ b/test/test_hull.c @@ -4,10 +4,12 @@ #include "test_macros.h" #include "box3d/collision.h" +#include "box3d/constants.h" #include "box3d/math_functions.h" #include #include +#include #include static const b3Vec3 s_cubeCorners[8] = { @@ -544,6 +546,201 @@ static int CreateHullDegenerateTest( void ) return 0; } +static void FillPoints2D( b3Point2D* pts, const b3Vec2* input, int count ) +{ + for ( int i = 0; i < count; ++i ) + { + pts[i].p = input[i]; + pts[i].separation = 0.0f; + pts[i].originalIndex = i; + } +} + +static float Hull2DArea( const b3Point2D* hull, int count ) +{ + float sum = 0.0f; + for ( int i = 0; i < count; ++i ) + { + int next = ( i + 1 ) % count; + sum += hull[i].p.x * hull[next].p.y - hull[i].p.y * hull[next].p.x; + } + + return 0.5f * sum; +} + +// Interior points contribute no area, so they must not survive the hull. +static int Hull2DSquareTest( void ) +{ + b3Vec2 input[8] = { + { -1.0f, -1.0f }, { 1.0f, -1.0f }, { 1.0f, 1.0f }, { -1.0f, 1.0f }, + { 0.0f, 0.0f }, { 0.5f, 0.25f }, { -0.3f, 0.6f }, { 0.1f, -0.4f }, + }; + + b3Point2D pts[8]; + b3Point2D hull[16]; + FillPoints2D( pts, input, 8 ); + + int count = b3Hull2D( pts, 8, hull ); + ENSURE( count == 4 ); + + // The reduction code assumes counter clockwise winding + ENSURE( Hull2DArea( hull, count ) > 0.0f ); + + bool onHull[8] = { false }; + for ( int i = 0; i < count; ++i ) + { + ENSURE( 0 <= hull[i].originalIndex && hull[i].originalIndex < 8 ); + onHull[hull[i].originalIndex] = true; + } + + for ( int i = 0; i < 4; ++i ) + { + ENSURE( onHull[i] == true ); + } + + for ( int i = 4; i < 8; ++i ) + { + ENSURE( onHull[i] == false ); + } + + return 0; +} + +static int Hull2DCollinearTest( void ) +{ + b3Vec2 input[5] = { + { 0.0f, 0.0f }, { 1.0f, 0.0f }, { 2.0f, 0.0f }, { 3.0f, 0.0f }, { 4.0f, 0.0f }, + }; + + b3Point2D pts[5]; + b3Point2D hull[10]; + FillPoints2D( pts, input, 5 ); + + ENSURE( b3Hull2D( pts, 5, hull ) == 2 ); + + return 0; +} + +// Coincident points weld and the lower input index wins, so a sort order change cannot +// swap which of the two survives. +static int Hull2DWeldTest( void ) +{ + float offset = 0.1f * B3_LINEAR_SLOP; + + b3Vec2 input[5] = { + { -1.0f, -1.0f }, { 1.0f, -1.0f }, { 1.0f, 1.0f }, { -1.0f, 1.0f }, { -1.0f + offset, -1.0f + offset }, + }; + + b3Point2D pts[5]; + b3Point2D hull[10]; + FillPoints2D( pts, input, 5 ); + + int count = b3Hull2D( pts, 5, hull ); + ENSURE( count == 4 ); + + for ( int i = 0; i < count; ++i ) + { + ENSURE( hull[i].originalIndex != 4 ); + } + + return 0; +} + +static int SimplifyHull2DTargetTest( void ) +{ + b3Point2D pts[20]; + b3Point2D hull[40]; + + for ( int i = 0; i < 20; ++i ) + { + float angle = 2.0f * B3_PI * (float)i / 20.0f; + pts[i].p = (b3Vec2){ cosf( angle ), sinf( angle ) }; + pts[i].separation = 0.0f; + pts[i].originalIndex = i; + } + + int hullCount = b3Hull2D( pts, 20, hull ); + ENSURE( hullCount == 20 ); + + int count = b3SimplifyHull2D( hull, hullCount, 6 ); + ENSURE( count == 6 ); + ENSURE( Hull2DArea( hull, count ) > 0.0f ); + + return 0; +} + +static int SimplifyHull2DNoopTest( void ) +{ + b3Vec2 input[4] = { + { -1.0f, -1.0f }, + { 1.0f, -1.0f }, + { 1.0f, 1.0f }, + { -1.0f, 1.0f }, + }; + + b3Point2D pts[4]; + b3Point2D hull[8]; + FillPoints2D( pts, input, 4 ); + + int hullCount = b3Hull2D( pts, 4, hull ); + ENSURE( hullCount == 4 ); + + ENSURE( b3SimplifyHull2D( hull, hullCount, 8 ) == 4 ); + ENSURE( b3SimplifyHull2D( hull, hullCount, 4 ) == 4 ); + + return 0; +} + +// The surviving point set must not depend on the order the points arrive in. The reduction +// runs inside the solver, so an input order dependence would show up as a determinism bug. +static int Hull2DOrderTest( void ) +{ + b3Vec2 input[8] = { + { -2.0f, -1.0f }, { 0.0f, -1.5f }, { 2.0f, -1.0f }, { 2.5f, 0.5f }, + { 1.0f, 2.0f }, { -1.0f, 1.8f }, { -2.5f, 0.4f }, { 0.2f, 0.1f }, + }; + + int permutation[8] = { 5, 2, 7, 0, 4, 6, 1, 3 }; + + b3Point2D pts[8]; + b3Point2D hull[16]; + FillPoints2D( pts, input, 8 ); + + int count1 = b3Hull2D( pts, 8, hull ); + ENSURE( count1 == 7 ); + + bool onHull1[8] = { false }; + for ( int i = 0; i < count1; ++i ) + { + onHull1[hull[i].originalIndex] = true; + } + + b3Point2D shuffled[8]; + b3Point2D shuffledHull[16]; + for ( int i = 0; i < 8; ++i ) + { + shuffled[i].p = input[permutation[i]]; + shuffled[i].separation = 0.0f; + shuffled[i].originalIndex = i; + } + + int count2 = b3Hull2D( shuffled, 8, shuffledHull ); + ENSURE( count2 == count1 ); + + bool onHull2[8] = { false }; + for ( int i = 0; i < count2; ++i ) + { + onHull2[permutation[shuffledHull[i].originalIndex]] = true; + } + + for ( int i = 0; i < 8; ++i ) + { + ENSURE( onHull1[i] == onHull2[i] ); + } + + return 0; +} + int HullTest( void ) { RUN_SUBTEST( CreateHullCubeTest ); @@ -558,6 +755,12 @@ int HullTest( void ) RUN_SUBTEST( CreateHullMergeChurnStressTest ); RUN_SUBTEST( CreateHullDegenerateTest ); RUN_SUBTEST( TransformedBoxHullTest ); + RUN_SUBTEST( Hull2DSquareTest ); + RUN_SUBTEST( Hull2DCollinearTest ); + RUN_SUBTEST( Hull2DWeldTest ); + RUN_SUBTEST( Hull2DOrderTest ); + RUN_SUBTEST( SimplifyHull2DTargetTest ); + RUN_SUBTEST( SimplifyHull2DNoopTest ); return 0; } diff --git a/test/test_joint.c b/test/test_joint.c index 582e68c..8057a13 100644 --- a/test/test_joint.c +++ b/test/test_joint.c @@ -565,6 +565,49 @@ static int TestWheelJoint( void ) return FinishJoint( jointId, f.worldId ); } +// Issue #117: a joint created with collideConnected false must clear contacts +// that already exist between the two bodies. +static int TestCreateJointClearsContacts( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.gravity = b3Vec3_zero; + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + bodyDef.position = (b3Pos){ 0.0f, 0.0f, 0.0f }; + b3BodyId bodyIdA = b3CreateBody( worldId, &bodyDef ); + + // Overlapping so a touching contact forms + bodyDef.position = (b3Pos){ 0.5f, 0.0f, 0.0f }; + b3BodyId bodyIdB = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3CreateHullShape( bodyIdA, &shapeDef, &box.base ); + b3CreateHullShape( bodyIdB, &shapeDef, &box.base ); + + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + ENSURE( b3Body_GetContactCapacity( bodyIdA ) > 0 ); + + b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef(); + jointDef.base.bodyIdA = bodyIdA; + jointDef.base.bodyIdB = bodyIdB; + jointDef.base.collideConnected = false; + b3CreateRevoluteJoint( worldId, &jointDef ); + + ENSURE( b3Body_GetContactCapacity( bodyIdA ) == 0 ); + ENSURE( b3Body_GetContactCapacity( bodyIdB ) == 0 ); + + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + ENSURE( b3Body_GetContactCapacity( bodyIdA ) == 0 ); + + b3DestroyWorld( worldId ); + return 0; +} + int JointTest( void ) { RUN_SUBTEST( TestParallelJoint ); @@ -576,6 +619,7 @@ int JointTest( void ) RUN_SUBTEST( TestSphericalJoint ); RUN_SUBTEST( TestWeldJoint ); RUN_SUBTEST( TestWheelJoint ); + RUN_SUBTEST( TestCreateJointClearsContacts ); return 0; } diff --git a/test/test_manifold.c b/test/test_manifold.c index 117af56..d061c80 100644 --- a/test/test_manifold.c +++ b/test/test_manifold.c @@ -10,6 +10,11 @@ #include #include +// A face contact keeps the whole clipped polygon when the manifold has room for it, so a build +// with a larger B3_MAX_MANIFOLD_POINTS legitimately reports more than four points. At the default +// this is still an exact count. +#define ENSURE_FACE_POINTS( count ) ENSURE( 4 <= ( count ) && ( count ) <= B3_MAX_MANIFOLD_POINTS ) + static const float kRoot2 = 1.41421356f; static const float kHalfRoot2 = 0.70710678f; @@ -270,7 +275,7 @@ static int ParallelEdgeTest( void ) b3SATCache cache = { 0 }; b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache ); - ENSURE( manifold.pointCount == 4 ); + ENSURE_FACE_POINTS( manifold.pointCount ); ENSURE( cache.type == b3_faceAxisA || cache.type == b3_faceAxisB ); ENSURE( b3Dot( manifold.normal, kAxisY ) > 0.998f ); @@ -488,7 +493,7 @@ static int TriangleParallelEdgeTest( void ) b3SATCache cache = { 0 }; b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, 0, &hull.base, &cache, true ); - ENSURE( manifold.pointCount == 4 ); + ENSURE_FACE_POINTS( manifold.pointCount ); ENSURE( cache.type == b3_faceAxisA ); ENSURE( b3Dot( manifold.normal, kAxisY ) > 0.99f ); @@ -505,7 +510,7 @@ static int TriangleParallelEdgeTest( void ) // 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_FACE_POINTS( manifold->pointCount ); ENSURE( cache->type == b3_faceAxisA || cache->type == b3_faceAxisB ); // A roof face of one hull, so 45 degrees off the vertical @@ -1244,8 +1249,19 @@ static int HullCapsuleFaceDeepTest( void ) return 0; } +// The library reports the point count it was built with. A mismatch against the manifold +// this translation unit sees means the app and the library disagree on b3Manifold. +static int MaxPointCountTest( void ) +{ + b3Manifold manifold; + ENSURE( b3GetMaxManifoldPoints() == (int)( sizeof( manifold.points ) / sizeof( manifold.points[0] ) ) ); + + return 0; +} + int ManifoldTest( void ) { + RUN_SUBTEST( MaxPointCountTest ); RUN_SUBTEST( CrossedEdgeTest ); RUN_SUBTEST( EdgeAxisScaleTest ); RUN_SUBTEST( EdgeCacheTest ); diff --git a/test/test_recording.c b/test/test_recording.c index 99b4f25..594d865 100644 --- a/test/test_recording.c +++ b/test/test_recording.c @@ -1011,6 +1011,22 @@ static int EmptyWorldRoundTrip( void ) return 0; } +// Safety factor of the named body in a replay world, or -1 if the body is not in the list +static float ReplaySafetyFactor( b3RecPlayer* player, const char* name ) +{ + int count = b3RecPlayer_GetBodyCount( player ); + for ( int i = 0; i < count; ++i ) + { + b3BodyId bodyId = b3RecPlayer_GetBodyId( player, i ); + if ( b3Body_IsValid( bodyId ) && strcmp( b3Body_GetName( bodyId ), name ) == 0 ) + { + return b3Body_GetSafetyFactor( bodyId ); + } + } + + return -1.0f; +} + // Exercise every recorded op in a single session, then validate replay at two worker // counts, round-trip through a file, and drive the incremental player. Mirrors the // comprehensive RecordingTest in Box2D's test suite (box2d/test/test_recording.c). @@ -1229,11 +1245,27 @@ static int AllOps( void ) b3BodyDef disableDef = b3DefaultBodyDef(); disableDef.type = b3_dynamicBody; disableDef.position = (b3Pos){ 9.0f, 5.0f, 0.0f }; + disableDef.name = "dropBody"; b3BodyId disableId = b3CreateBody( worldId, &disableDef ); b3Sphere disableSphere = { { 0.0f, 0.0f, 0.0f }, 0.3f }; b3CreateSphereShape( disableId, &sphereShapeDef, &disableSphere ); b3Body_Disable( disableId ); b3Body_Enable( disableId ); + b3Body_SetSafetyFactor( disableId, 0.25f ); + + // A body that falls fast enough to stay on the continuous path for the whole session. The + // non-default factor is what proves the widened body def payload round-trips, since every + // other body records the default. Named so the replay check can find it. + b3BodyDef fastDef = b3DefaultBodyDef(); + fastDef.type = b3_dynamicBody; + fastDef.position = (b3Pos){ -14.0f, 30.0f, 0.0f }; + fastDef.linearVelocity = (b3Vec3){ 0.0f, -8.0f, 0.0f }; + fastDef.safetyFactor = 0.1f; + fastDef.name = "fastBody"; + b3BodyId fastId = b3CreateBody( worldId, &fastDef ); + ENSURE( b3Body_GetSafetyFactor( fastId ) == 0.1f ); + b3BoxHull fastBox = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3CreateHullShape( fastId, &sphereShapeDef, &fastBox.base ); // Force/impulse/torque (Vec3 args in 3D) b3Body_ApplyForce( bodyId, (b3Vec3){ 0.0f, 50.0f, 0.0f }, (b3Pos){ 1.0f, 6.0f, 0.0f }, true ); @@ -1446,6 +1478,7 @@ static int AllOps( void ) { b3Body_ApplyLinearImpulseToCenter( capsuleBodyId, (b3Vec3){ 2.0f, 0.0f, 0.0f }, true ); b3Body_SetGravityScale( bodyId, 1.0f ); + b3Body_SetSafetyFactor( fastId, 0.4f ); } // Issue queries mid-loop to exercise recording across steps @@ -1532,6 +1565,26 @@ static int AllOps( void ) b3DestroyPlayer( player ); } + // The state hash only proves the op stream stayed aligned. Read the safety factor back out of + // the replay world so a value that is written but never restored is caught too. + { + b3RecPlayer* player = b3CreatePlayer( recData, recSize, 1 ); + ENSURE( player != NULL ); + + // Frame 0 replays the pre-step creates, so the def value is in place + ENSURE( b3RecPlayer_StepFrame( player ) ); + ENSURE( ReplaySafetyFactor( player, "fastBody" ) == 0.1f ); + ENSURE( ReplaySafetyFactor( player, "dropBody" ) == 0.25f ); + + // The setter is injected at frame 6 + while ( b3RecPlayer_StepFrame( player ) ) + { + } + ENSURE( ReplaySafetyFactor( player, "fastBody" ) == 0.4f ); + + b3DestroyPlayer( player ); + } + b3DestroyRecording( rec ); remove( s_recPath ); return 0; diff --git a/test/test_world.c b/test/test_world.c index d5a3561..f26a442 100644 --- a/test/test_world.c +++ b/test/test_world.c @@ -3,6 +3,7 @@ #include "benchmarks.h" #include "overflow_color.h" +#include "physics_world.h" #include "test_macros.h" #include "box3d/box3d.h" @@ -970,6 +971,71 @@ static int EnableContactRecyclingTest( void ) return 0; } +static int CountEnlargedNodes( const b3DynamicTree* tree ) +{ + int count = 0; + for ( int i = 0; i < tree->nodeCapacity; ++i ) + { + const b3TreeNode* node = tree->nodes + i; + if ( ( node->flags & b3_allocatedNode ) != 0 && ( node->flags & b3_enlargedNode ) != 0 ) + { + count += 1; + } + } + + return count; +} + +// A proxy can be enlarged in one step and destroyed before the next, emptying the move buffer. +// The trees must still be rebuilt or the enlarged nodes survive into the next step. Issue #149. +static int TestEnlargedProxyDestroyed( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.gravity = b3Vec3_zero; + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3World* world = b3GetWorldFromId( worldId ); + const b3DynamicTree* tree = world->broadPhase.trees + b3_dynamicBody; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3Sphere sphere = { b3Vec3_zero, 0.25f }; + + // Resting bodies keep the tree deep enough that the mover has an ancestor that outlives it + for ( int i = 0; i < 8; ++i ) + { + bodyDef.position = (b3Pos){ 2.0f * i, 0.0f, 0.0f }; + b3BodyId restingId = b3CreateBody( worldId, &bodyDef ); + b3CreateSphereShape( restingId, &shapeDef, &sphere ); + } + + // This body outruns its AABB margin every step + bodyDef.position = (b3Pos){ 7.0f, 1.0f, 0.0f }; + bodyDef.linearVelocity = (b3Vec3){ 6.0f, 0.0f, 0.0f }; + b3BodyId moverId = b3CreateBody( worldId, &bodyDef ); + b3CreateSphereShape( moverId, &shapeDef, &sphere ); + + float timeStep = 1.0f / 60.0f; + b3World_Step( worldId, timeStep, 4 ); + + ENSURE( CountEnlargedNodes( tree ) > 0 ); + + b3DestroyBody( moverId ); + + // The mover was the only proxy in the move buffer + ENSURE( world->broadPhase.moveArray.count == 0 ); + ENSURE( CountEnlargedNodes( tree ) > 0 ); + + b3World_Step( worldId, timeStep, 4 ); + + ENSURE( CountEnlargedNodes( tree ) == 0 ); + + b3DestroyWorld( worldId ); + return 0; +} + // Identical hull data is shared through a reference counted world database. static int TestHullDatabase( void ) { @@ -1180,6 +1246,7 @@ int WorldTest( void ) RUN_SUBTEST( EnableContactRecyclingTest ); RUN_SUBTEST( TestSetWorkerCount ); RUN_SUBTEST( TestHullDatabase ); + RUN_SUBTEST( TestEnlargedProxyDestroyed ); return 0; }