From 2c939c287bfcffcd9ae12ce34db486f6e0bd17e2 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Mon, 2 Dec 2024 09:25:03 -0800 Subject: [PATCH] Faster continuous (#847) Re-organized fast body handling in solver, cutting the cost of continuous in half. Fixes for Linux samples. b2World_GetAwakeBodyCount Removed unused code from dynamic tree --- benchmark/amd7950x_avx2/joint_grid.csv | 16 +- benchmark/amd7950x_avx2/large_pyramid.csv | 16 +- benchmark/amd7950x_avx2/many_pyramids.csv | 16 +- benchmark/amd7950x_avx2/rain.csv | 16 +- benchmark/amd7950x_avx2/smash.csv | 16 +- benchmark/amd7950x_avx2/spinner.csv | 16 +- benchmark/amd7950x_avx2/tumbler.csv | 16 +- benchmark/amd7950x_avx2_nc/joint_grid.csv | 16 +- benchmark/amd7950x_avx2_nc/large_pyramid.csv | 16 +- benchmark/amd7950x_avx2_nc/many_pyramids.csv | 16 +- benchmark/amd7950x_avx2_nc/rain.csv | 16 +- benchmark/amd7950x_avx2_nc/smash.csv | 16 +- benchmark/amd7950x_avx2_nc/spinner.csv | 16 +- benchmark/amd7950x_avx2_nc/tumbler.csv | 16 +- benchmark/main.c | 2 + build.sh | 5 +- include/box2d/base.h | 26 +- include/box2d/box2d.h | 8 +- include/box2d/collision.h | 27 +- include/box2d/math_functions.h | 24 +- samples/draw.cpp | 2 + samples/draw.h | 1 + samples/main.cpp | 3 +- samples/sample.cpp | 3 +- samples/sample_bodies.cpp | 71 + samples/sample_continuous.cpp | 68 + samples/sample_determinism.cpp | 14 +- samples/sample_joints.cpp | 2 +- shared/benchmarks.h | 25 +- shared/human.h | 23 +- shared/random.h | 15 +- src/CMakeLists.txt | 3 +- src/aabb.c | 4 +- src/body.c | 32 +- src/body.h | 3 +- src/constants.h | 48 + src/constraint_graph.h | 1 + src/core.c | 7 +- src/core.h | 70 +- src/distance.c | 1 + src/dynamic_tree.c | 161 +- src/geometry.c | 10 +- src/hull.c | 1 + src/joint.c | 2 +- src/manifold.c | 18 +- src/math_functions.c | 6 +- src/mouse_joint.c | 8 +- src/shape.c | 29 +- src/shape.h | 1 - src/solver.c | 1612 +++++++++--------- src/solver.h | 4 - src/timer.c | 2 +- src/types.c | 1 + src/weld_joint.c | 10 +- src/world.c | 62 +- test/test_collision.c | 4 +- test/test_determinism.c | 19 +- test/test_math.c | 14 +- test/test_world.c | 11 +- 59 files changed, 1339 insertions(+), 1348 deletions(-) create mode 100644 src/constants.h diff --git a/benchmark/amd7950x_avx2/joint_grid.csv b/benchmark/amd7950x_avx2/joint_grid.csv index a65edb128..2d8c56e25 100644 --- a/benchmark/amd7950x_avx2/joint_grid.csv +++ b/benchmark/amd7950x_avx2/joint_grid.csv @@ -1,9 +1,9 @@ threads,fps -1,200.145 -2,340.128 -3,496.707 -4,607.243 -5,698.173 -6,782.174 -7,856.148 -8,916.514 +1,197.415 +2,338.192 +3,485.363 +4,590.58 +5,678.894 +6,755.337 +7,830.03 +8,889.992 diff --git a/benchmark/amd7950x_avx2/large_pyramid.csv b/benchmark/amd7950x_avx2/large_pyramid.csv index 9c41fa922..cc36cf696 100644 --- a/benchmark/amd7950x_avx2/large_pyramid.csv +++ b/benchmark/amd7950x_avx2/large_pyramid.csv @@ -1,9 +1,9 @@ threads,fps -1,325.321 -2,615.892 -3,878.262 -4,1111.8 -5,1322.39 -6,1511.06 -7,1719 -8,1738.61 +1,337.398 +2,599.766 +3,853.652 +4,1095.24 +5,1288.22 +6,1467.1 +7,1669.81 +8,1704.72 diff --git a/benchmark/amd7950x_avx2/many_pyramids.csv b/benchmark/amd7950x_avx2/many_pyramids.csv index e8bd18097..ec8d10f1e 100644 --- a/benchmark/amd7950x_avx2/many_pyramids.csv +++ b/benchmark/amd7950x_avx2/many_pyramids.csv @@ -1,9 +1,9 @@ threads,fps -1,83.1212 -2,162.42 -3,238.035 -4,307.18 -5,374.76 -6,432.28 -7,501.138 -8,544.638 +1,82.5412 +2,156.573 +3,233.64 +4,294.31 +5,357.121 +6,420.298 +7,487.163 +8,537.355 diff --git a/benchmark/amd7950x_avx2/rain.csv b/benchmark/amd7950x_avx2/rain.csv index 9265f9fb1..03752372d 100644 --- a/benchmark/amd7950x_avx2/rain.csv +++ b/benchmark/amd7950x_avx2/rain.csv @@ -1,9 +1,9 @@ threads,fps -1,154.443 -2,249.524 -3,323.318 -4,384.953 -5,434.035 -6,483.406 -7,527.558 -8,556.645 +1,158.379 +2,253.381 +3,337.197 +4,408.427 +5,475.14 +6,530.178 +7,583.798 +8,633.102 diff --git a/benchmark/amd7950x_avx2/smash.csv b/benchmark/amd7950x_avx2/smash.csv index 610681bac..93622d5fd 100644 --- a/benchmark/amd7950x_avx2/smash.csv +++ b/benchmark/amd7950x_avx2/smash.csv @@ -1,9 +1,9 @@ threads,fps -1,197.528 -2,300.994 -3,385.163 -4,455.355 -5,508.735 -6,553.648 -7,589.206 -8,614.934 +1,194.531 +2,299.76 +3,389.501 +4,469.022 +5,532.045 +6,580.977 +7,620.559 +8,655.217 diff --git a/benchmark/amd7950x_avx2/spinner.csv b/benchmark/amd7950x_avx2/spinner.csv index 94a09855f..a8a572a52 100644 --- a/benchmark/amd7950x_avx2/spinner.csv +++ b/benchmark/amd7950x_avx2/spinner.csv @@ -1,9 +1,9 @@ threads,fps -1,346.759 -2,538.038 -3,710.87 -4,871.698 -5,983.994 -6,1084.08 -7,1178.67 -8,1242.13 +1,342.697 +2,549.335 +3,711.477 +4,889.473 +5,1011.82 +6,1141.73 +7,1261.39 +8,1338.51 diff --git a/benchmark/amd7950x_avx2/tumbler.csv b/benchmark/amd7950x_avx2/tumbler.csv index 68c900c8c..c9d771aa0 100644 --- a/benchmark/amd7950x_avx2/tumbler.csv +++ b/benchmark/amd7950x_avx2/tumbler.csv @@ -1,9 +1,9 @@ threads,fps -1,464.164 -2,705.539 -3,932.064 -4,1133.55 -5,1297.75 -6,1444.39 -7,1563.34 -8,1633.51 +1,461.248 +2,726.229 +3,947.347 +4,1152.31 +5,1335.66 +6,1483.59 +7,1613.59 +8,1715.86 diff --git a/benchmark/amd7950x_avx2_nc/joint_grid.csv b/benchmark/amd7950x_avx2_nc/joint_grid.csv index b6c8e677e..e704c818d 100644 --- a/benchmark/amd7950x_avx2_nc/joint_grid.csv +++ b/benchmark/amd7950x_avx2_nc/joint_grid.csv @@ -1,9 +1,9 @@ threads,fps -1,196.348 -2,335.206 -3,493.878 -4,598.194 -5,691.697 -6,776.717 -7,848.272 -8,910.073 +1,191.305 +2,335.535 +3,485.589 +4,591.62 +5,682.079 +6,762.346 +7,834.071 +8,896.952 diff --git a/benchmark/amd7950x_avx2_nc/large_pyramid.csv b/benchmark/amd7950x_avx2_nc/large_pyramid.csv index 416dbb891..2670595c9 100644 --- a/benchmark/amd7950x_avx2_nc/large_pyramid.csv +++ b/benchmark/amd7950x_avx2_nc/large_pyramid.csv @@ -1,9 +1,9 @@ threads,fps -1,326.839 -2,605.397 -3,874.098 -4,1108.62 -5,1311.67 -6,1500.33 -7,1684.32 -8,1736.04 +1,337.272 +2,606.512 +3,867.572 +4,1096.58 +5,1297.5 +6,1495.5 +7,1668.46 +8,1700.88 diff --git a/benchmark/amd7950x_avx2_nc/many_pyramids.csv b/benchmark/amd7950x_avx2_nc/many_pyramids.csv index ff59a49c3..f31c3fef2 100644 --- a/benchmark/amd7950x_avx2_nc/many_pyramids.csv +++ b/benchmark/amd7950x_avx2_nc/many_pyramids.csv @@ -1,9 +1,9 @@ threads,fps -1,84.1153 -2,163.509 -3,240.109 -4,302.481 -5,368.738 -6,428.312 -7,498.302 -8,545.101 +1,82.3111 +2,160.585 +3,236.095 +4,300.733 +5,362.227 +6,423.2 +7,496.346 +8,544.392 diff --git a/benchmark/amd7950x_avx2_nc/rain.csv b/benchmark/amd7950x_avx2_nc/rain.csv index 51b0b4c8f..b9525cbdd 100644 --- a/benchmark/amd7950x_avx2_nc/rain.csv +++ b/benchmark/amd7950x_avx2_nc/rain.csv @@ -1,9 +1,9 @@ threads,fps -1,161.863 -2,276.688 -3,358.757 -4,434.589 -5,500.599 -6,563.324 -7,620.21 -8,672.701 +1,161.455 +2,275.894 +3,354.968 +4,429.525 +5,495.056 +6,556.892 +7,612.256 +8,664.778 diff --git a/benchmark/amd7950x_avx2_nc/smash.csv b/benchmark/amd7950x_avx2_nc/smash.csv index 644e4f612..a7380ff14 100644 --- a/benchmark/amd7950x_avx2_nc/smash.csv +++ b/benchmark/amd7950x_avx2_nc/smash.csv @@ -1,9 +1,9 @@ threads,fps -1,190.875 -2,302.452 -3,391.787 -4,472.507 -5,531.519 -6,582.197 -7,623.562 -8,654.127 +1,191.697 +2,296.907 +3,387.265 +4,466.151 +5,524.415 +6,571.353 +7,614.959 +8,650.434 diff --git a/benchmark/amd7950x_avx2_nc/spinner.csv b/benchmark/amd7950x_avx2_nc/spinner.csv index 61e1da8d9..298c95923 100644 --- a/benchmark/amd7950x_avx2_nc/spinner.csv +++ b/benchmark/amd7950x_avx2_nc/spinner.csv @@ -1,9 +1,9 @@ threads,fps -1,344.368 -2,562.345 -3,738.154 -4,916.335 -5,1049.29 -6,1167.4 -7,1280.44 -8,1362.46 +1,339.941 +2,558.533 +3,730.695 +4,913.45 +5,1038.29 +6,1153.58 +7,1265.97 +8,1355.59 diff --git a/benchmark/amd7950x_avx2_nc/tumbler.csv b/benchmark/amd7950x_avx2_nc/tumbler.csv index 8bdc720ad..1ec85c98b 100644 --- a/benchmark/amd7950x_avx2_nc/tumbler.csv +++ b/benchmark/amd7950x_avx2_nc/tumbler.csv @@ -1,9 +1,9 @@ threads,fps -1,449.556 -2,717.776 -3,934.15 -4,1136.18 -5,1315.11 -6,1470.1 -7,1599.57 -8,1700.46 +1,440.223 +2,707.306 +3,929.712 +4,1127.08 +5,1313.66 +6,1461.68 +7,1588.84 +8,1690.27 diff --git a/benchmark/main.c b/benchmark/main.c index 28eb4f3c1..88cb0732e 100644 --- a/benchmark/main.c +++ b/benchmark/main.c @@ -62,6 +62,8 @@ int GetNumberOfCores() return (int)sysconf( _SC_NPROCESSORS_ONLN ); #elif defined( __linux__ ) return (int)sysconf( _SC_NPROCESSORS_ONLN ); +#elif defined( __EMSCRIPTEN__ ) + return (int)sysconf( _SC_NPROCESSORS_ONLN ); #else return 1; #endif diff --git a/build.sh b/build.sh index 343359500..c9200f36e 100755 --- a/build.sh +++ b/build.sh @@ -4,5 +4,8 @@ rm -rf build mkdir build cd build -cmake -DBOX2D_BUILD_DOCS=OFF .. + +# I haven't been able to get Wayland working on WSL but X11 works. +# https://www.glfw.org/docs/latest/compile.html +cmake -DBOX2D_BUILD_DOCS=OFF -DGLFW_BUILD_WAYLAND=OFF .. cmake --build . diff --git a/include/box2d/base.h b/include/box2d/base.h index 7f83f8bac..ac4c0970e 100644 --- a/include/box2d/base.h +++ b/include/box2d/base.h @@ -15,7 +15,7 @@ // using the Windows DLL #define BOX2D_EXPORT __declspec( dllimport ) #elif defined( box2d_EXPORTS ) - // building or using the Box2D shared library + // building or using the shared library #define BOX2D_EXPORT __attribute__( ( visibility( "default" ) ) ) #else // static library @@ -66,6 +66,30 @@ B2_API int b2GetByteCount( void ); /// @param assertFcn a non-null assert callback B2_API void b2SetAssertFcn( b2AssertFcn* assertFcn ); +// see https://github.com/scottt/debugbreak +#if defined( _MSC_VER ) +#define B2_BREAKPOINT __debugbreak() +#elif defined( __GNUC__ ) || defined( __clang__ ) +#define B2_BREAKPOINT __builtin_trap() +#else +// Unknown compiler +#include +#define B2_BREAKPOINT assert( 0 ) +#endif + +#if !defined( NDEBUG ) || defined( B2_ENABLE_ASSERT ) +B2_API int b2InternalAssertFcn( const char* condition, const char* fileName, int lineNumber ); +#define B2_ASSERT( condition ) \ + do \ + { \ + if ( !( condition ) && b2InternalAssertFcn( #condition, __FILE__, (int)__LINE__ ) ) \ + B2_BREAKPOINT; \ + } \ + while ( 0 ) +#else +#define B2_ASSERT( ... ) ( (void)0 ) +#endif + /// Version numbering scheme. /// See https://semver.org/ typedef struct b2Version diff --git a/include/box2d/box2d.h b/include/box2d/box2d.h index 9e02c30fc..2b0ba1760 100644 --- a/include/box2d/box2d.h +++ b/include/box2d/box2d.h @@ -183,6 +183,9 @@ B2_API void b2World_EnableWarmStarting( b2WorldId worldId, bool flag ); /// Is constraint warm starting enabled? B2_API bool b2World_IsWarmStartingEnabled( b2WorldId worldId ); +/// Get the number of awake bodies. +B2_API int b2World_GetAwakeBodyCount( b2WorldId worldId ); + /// Get the current world performance profile B2_API b2Profile b2World_GetProfile( b2WorldId worldId ); @@ -444,7 +447,10 @@ B2_API int b2Body_GetJoints( b2BodyId bodyId, b2JointId* jointArray, int capacit /// Get the maximum capacity required for retrieving all the touching contacts on a body B2_API int b2Body_GetContactCapacity( b2BodyId bodyId ); -/// Get the touching contact data for a body +/// Get the touching contact data for a body. +/// @note Box2D uses speculative collision so some contact points may be separated. +/// @returns the number of elements filled in the provided array +/// @warning do not ignore the return value, it specifies the valid number of elements B2_API int b2Body_GetContactData( b2BodyId bodyId, b2ContactData* contactData, int capacity ); /// Get the current world AABB that contains all the attached shapes. Note that this may not encompass the body origin. diff --git a/include/box2d/collision.h b/include/box2d/collision.h index 49a59d022..2047a2446 100644 --- a/include/box2d/collision.h +++ b/include/box2d/collision.h @@ -492,9 +492,11 @@ B2_API b2TOIOutput b2TimeOfImpact( const b2TOIInput* input ); * @{ */ -/// A manifold point is a contact point belonging to a contact -/// manifold. It holds details related to the geometry and dynamics -/// of the contact points. +/// A manifold point is a contact point belonging to a contact manifold. +/// It holds details related to the geometry and dynamics of the contact points. +/// Box2D uses speculative collision so some contact points may be separated. +/// You may use the maxNormalImpulse to determine if there was an interaction during +/// the time step. typedef struct b2ManifoldPoint { /// Location of the contact point in world space. Subject to precision loss at large coordinates. @@ -518,8 +520,8 @@ typedef struct b2ManifoldPoint /// The friction impulse float tangentImpulse; - /// The maximum normal impulse applied during sub-stepping - /// This could be a bool to indicate the point is confirmed (may be a speculative point) + /// The maximum normal impulse applied during sub-stepping. This is important + /// to identify speculative contact points that had an interaction in the time step. float maxNormalImpulse; /// Relative normal velocity pre-solve. Used for hit events. If the normal impulse is @@ -533,7 +535,8 @@ typedef struct b2ManifoldPoint bool persisted; } b2ManifoldPoint; -/// A contact manifold describes the contact points between colliding shapes +/// A contact manifold describes the contact points between colliding shapes. +/// @note Box2D uses speculative collision so some contact points may be separated. typedef struct b2Manifold { /// The manifold points, up to two are possible in 2D @@ -779,27 +782,15 @@ B2_API void b2DynamicTree_Validate( const b2DynamicTree* tree ); /// called often. B2_API int b2DynamicTree_GetHeight( const b2DynamicTree* tree ); -/// Get the maximum balance of the tree. The balance is the difference in height of the two children of a node. -B2_API int b2DynamicTree_GetMaxBalance( const b2DynamicTree* tree ); - /// Get the ratio of the sum of the node areas to the root area. B2_API float b2DynamicTree_GetAreaRatio( const b2DynamicTree* tree ); -/// Build an optimal tree. Very expensive. For testing. -B2_API void b2DynamicTree_RebuildBottomUp( b2DynamicTree* tree ); - /// Get the number of proxies created B2_API int b2DynamicTree_GetProxyCount( const b2DynamicTree* tree ); /// Rebuild the tree while retaining subtrees that haven't changed. Returns the number of boxes sorted. B2_API int b2DynamicTree_Rebuild( b2DynamicTree* tree, bool fullBuild ); -/// Shift the world origin. Useful for large worlds. -/// The shift formula is: position -= newOrigin -/// @param tree the tree to shift -/// @param newOrigin the new origin with respect to the old origin -B2_API void b2DynamicTree_ShiftOrigin( b2DynamicTree* tree, b2Vec2 newOrigin ); - /// Get the number of bytes used by this tree B2_API int b2DynamicTree_GetByteCount( const b2DynamicTree* tree ); diff --git a/include/box2d/math_functions.h b/include/box2d/math_functions.h index 3f7c34815..320e37a4f 100644 --- a/include/box2d/math_functions.h +++ b/include/box2d/math_functions.h @@ -15,9 +15,6 @@ * @{ */ -/// https://en.wikipedia.org/wiki/Pi -#define b2_pi 3.14159265359f - /// 2D vector /// This can be used to represent a point or free vector typedef struct b2Vec2 @@ -71,6 +68,9 @@ typedef struct b2AABB * @{ */ +/// https://en.wikipedia.org/wiki/Pi +#define b2_pi 3.14159265359f + static const b2Vec2 b2Vec2_zero = { 0.0f, 0.0f }; static const b2Rot b2Rot_identity = { 1.0f, 0.0f }; static const b2Transform b2Transform_identity = { { 0.0f, 0.0f }, { 1.0f, 0.0f } }; @@ -191,11 +191,11 @@ B2_INLINE b2Vec2 b2Lerp( b2Vec2 a, b2Vec2 b, float t ) return B2_LITERAL( b2Vec2 ){ ( 1.0f - t ) * a.x + t * b.x, ( 1.0f - t ) * a.y + t * b.y }; } -/// Component-wise multiplication -B2_INLINE b2Vec2 b2Mul( b2Vec2 a, b2Vec2 b ) -{ - return B2_LITERAL( b2Vec2 ){ a.x * b.x, a.y * b.y }; -} +///// Component-wise multiplication +//B2_INLINE b2Vec2 b2Mul( b2Vec2 a, b2Vec2 b ) +//{ +// return B2_LITERAL( b2Vec2 ){ a.x * b.x, a.y * b.y }; +//} /// Multiply a scalar and vector B2_INLINE b2Vec2 b2MulSV( float s, b2Vec2 v ) @@ -605,16 +605,16 @@ B2_INLINE b2AABB b2AABB_Union( b2AABB a, b2AABB b ) } /// Is this a valid number? Not NaN or infinity. -B2_API bool b2Float_IsValid( float a ); +B2_API bool b2IsValidFloat( float a ); /// Is this a valid vector? Not NaN or infinity. -B2_API bool b2Vec2_IsValid( b2Vec2 v ); +B2_API bool b2IsValidVec2( b2Vec2 v ); /// Is this a valid rotation? Not NaN or infinity. Is normalized. -B2_API bool b2Rot_IsValid( b2Rot q ); +B2_API bool b2IsValidRotation( b2Rot q ); /// Is this a valid bounding box? Not Nan or infinity. Upper bound greater than or equal to lower bound. -B2_API bool b2AABB_IsValid( b2AABB aabb ); +B2_API bool b2IsValidAABB( b2AABB aabb ); /// Box2D bases all length units on meters, but you may need different units for your game. /// You can set this value to use different units. This should be done at application startup diff --git a/samples/draw.cpp b/samples/draw.cpp index 2efc85335..53776cbf6 100644 --- a/samples/draw.cpp +++ b/samples/draw.cpp @@ -1487,8 +1487,10 @@ void Draw::DrawString( int x, int y, const char* string, ... ) ImGui::Begin( "Overlay", nullptr, ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoInputs | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoScrollbar ); + ImGui::PushFont( g_draw.m_regularFont ); ImGui::SetCursorPos( ImVec2( float( x ), float( y ) ) ); ImGui::TextColoredV( ImColor( 230, 153, 153, 255 ), string, arg ); + ImGui::PopFont(); ImGui::End(); va_end( arg ); } diff --git a/samples/draw.h b/samples/draw.h index a6a1c03b7..c6d7bdb82 100644 --- a/samples/draw.h +++ b/samples/draw.h @@ -68,6 +68,7 @@ class Draw b2DebugDraw m_debugDraw; ImFont* m_smallFont; + ImFont* m_regularFont; ImFont* m_mediumFont; ImFont* m_largeFont; }; diff --git a/samples/main.cpp b/samples/main.cpp index 32f27cae7..af7f08ae6 100644 --- a/samples/main.cpp +++ b/samples/main.cpp @@ -152,6 +152,7 @@ static void CreateUI( GLFWwindow* window, const char* glslVersion ) ImFontConfig fontConfig; fontConfig.RasterizerMultiply = s_windowScale * s_framebufferScale; g_draw.m_smallFont = ImGui::GetIO().Fonts->AddFontFromFileTTF( fontPath, 14.0f, &fontConfig ); + g_draw.m_regularFont = ImGui::GetIO().Fonts->AddFontFromFileTTF( fontPath, 18.0f, &fontConfig ); g_draw.m_mediumFont = ImGui::GetIO().Fonts->AddFontFromFileTTF( fontPath, 40.0f, &fontConfig ); g_draw.m_largeFont = ImGui::GetIO().Fonts->AddFontFromFileTTF( fontPath, 64.0f, &fontConfig ); } @@ -528,7 +529,7 @@ int main( int, char** ) // How to break at the leaking allocation, in the watch window enter this variable // and set it to the allocation number in {}. Do this at the first line in main. - // {,,ucrtbased.dll}_crtBreakAlloc = 3970 + // {,,ucrtbased.dll}_crtBreakAlloc = #endif // Install memory hooks diff --git a/samples/sample.cpp b/samples/sample.cpp index 124924d57..20717279a 100644 --- a/samples/sample.cpp +++ b/samples/sample.cpp @@ -4,6 +4,7 @@ #include "sample.h" #include "draw.h" +#include "imgui.h" #include "random.h" #include "settings.h" @@ -91,7 +92,7 @@ Sample::Sample( Settings& settings ) m_worldId = b2CreateWorld( &worldDef ); m_textLine = 30; - m_textIncrement = 18; + m_textIncrement = 22; m_mouseJointId = b2_nullJointId; m_stepCount = 0; diff --git a/samples/sample_bodies.cpp b/samples/sample_bodies.cpp index 94cd0b82d..b99dc1431 100644 --- a/samples/sample_bodies.cpp +++ b/samples/sample_bodies.cpp @@ -842,3 +842,74 @@ class BadBody : public Sample }; static int sampleBadBody = RegisterSample( "Bodies", "Bad", BadBody::Create ); + +class Pivot : public Sample +{ +public: + explicit Pivot( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.8f, 6.4f }; + g_camera.m_zoom = 25.0f * 0.4f; + } + + b2BodyId groundId = b2_nullBodyId; + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Segment segment = { { -20.0f, 0.0f }, { 20.0f, 0.0f } }; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + // Create a separate body on the ground + { + b2Vec2 v = { 5.0f, 0.0f }; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { 0.0f, 3.0f }; + bodyDef.gravityScale = 0.0f; + bodyDef.linearVelocity = v; + + m_bodyId = b2CreateBody( m_worldId, &bodyDef ); + + m_lever = 3.0f; + b2Vec2 r = { 0.0f, -m_lever }; + + float omega = b2Cross(v, r) / b2Dot(r, r); + b2Body_SetAngularVelocity( m_bodyId, omega ); + + b2Polygon box = b2MakeBox( 0.1f, m_lever ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape( m_bodyId, &shapeDef, &box ); + } + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + b2Vec2 v = b2Body_GetLinearVelocity( m_bodyId ); + float omega = b2Body_GetAngularVelocity( m_bodyId ); + b2Vec2 r = b2Body_GetWorldVector( m_bodyId, { 0.0f, -m_lever } ); + + b2Vec2 vp = v + b2CrossSV( omega, r ); + g_draw.DrawString( 5, m_textLine, "pivot velocity = (%g, %g)", vp.x, vp.y); + m_textLine += m_textIncrement; + } + + static Sample* Create( Settings& settings ) + { + return new Pivot( settings ); + } + + b2BodyId m_bodyId; + float m_lever; +}; + +static int samplePivot = RegisterSample( "Bodies", "Pivot", Pivot::Create ); diff --git a/samples/sample_continuous.cpp b/samples/sample_continuous.cpp index 1e46c923a..a00aa8450 100644 --- a/samples/sample_continuous.cpp +++ b/samples/sample_continuous.cpp @@ -779,6 +779,74 @@ class SpeculativeGhost : public Sample static int sampleSpeculativeGhost = RegisterSample( "Continuous", "Speculative Ghost", SpeculativeGhost::Create ); +// This shows that while Box2D uses speculative collision, it does not lead to speculative ghost collisions at small distances +class PixelImperfect : public Sample +{ +public: + explicit PixelImperfect( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 7.0f, 5.0f }; + g_camera.m_zoom = 6.0f; + } + + float pixelsPerMeter = 30.f; + + { + b2BodyDef block4BodyDef = b2DefaultBodyDef(); + block4BodyDef.type = b2_staticBody; + block4BodyDef.position = { 175.f / pixelsPerMeter, 150.f / pixelsPerMeter }; + b2BodyId block4BodyId = b2CreateBody( m_worldId, &block4BodyDef ); + b2Polygon block4Shape = b2MakeBox( 20.f / pixelsPerMeter, 10.f / pixelsPerMeter ); + b2ShapeDef block4ShapeDef = b2DefaultShapeDef(); + block4ShapeDef.friction = 0.f; + b2CreatePolygonShape( block4BodyId, &block4ShapeDef, &block4Shape ); + } + + { + b2BodyDef ballBodyDef = b2DefaultBodyDef(); + ballBodyDef.type = b2_dynamicBody; + ballBodyDef.position = { 200.0f / pixelsPerMeter, 275.f / pixelsPerMeter }; + ballBodyDef.gravityScale = 0.0f; + + m_ballId = b2CreateBody( m_worldId, &ballBodyDef ); + // Ball shape + //b2Polygon ballShape = b2MakeBox( 5.f / pixelsPerMeter, 5.f / pixelsPerMeter ); + b2Polygon ballShape = b2MakeRoundedBox( 4.0f / pixelsPerMeter, 4.0f / pixelsPerMeter, 0.9f / pixelsPerMeter ); + b2ShapeDef ballShapeDef = b2DefaultShapeDef(); + ballShapeDef.friction = 0.f; + //ballShapeDef.restitution = 1.f; + b2CreatePolygonShape( m_ballId, &ballShapeDef, &ballShape ); + b2Body_SetLinearVelocity( m_ballId, { 0.f, -5.0f } ); + b2Body_SetFixedRotation( m_ballId, true ); + } + } + + void Step( Settings& settings ) override + { + b2ContactData data; + b2Body_GetContactData( m_ballId, &data, 1 ); + + b2Vec2 p = b2Body_GetPosition( m_ballId ); + b2Vec2 v = b2Body_GetLinearVelocity( m_ballId ); + g_draw.DrawString( 5, m_textLine, "p.x = %.9f, v.y = %.9f", p.x, v.y ); + m_textLine += m_textIncrement; + + Sample::Step( settings ); + } + + static Sample* Create( Settings& settings ) + { + return new PixelImperfect( settings ); + } + + b2BodyId m_ballId; +}; + +static int samplePixelImperfect = RegisterSample( "Continuous", "Pixel Imperfect", PixelImperfect::Create ); + class Drop : public Sample { public: diff --git a/samples/sample_determinism.cpp b/samples/sample_determinism.cpp index 29abdd43c..3c9c8d2e2 100644 --- a/samples/sample_determinism.cpp +++ b/samples/sample_determinism.cpp @@ -144,20 +144,12 @@ class FallingHinges : public Sample if (m_hash == 0) { - bool sleeping = true; - int bodyCount = e_rows * e_columns; - for (int i = 0; i < bodyCount; ++i) - { - if ( b2Body_IsAwake( m_bodies[i] ) == true ) - { - sleeping = false; - break; - } - } + b2BodyEvents bodyEvents = b2World_GetBodyEvents( m_worldId ); - if (sleeping == true) + if ( bodyEvents.moveCount == 0 ) { uint32_t hash = B2_HASH_INIT; + int bodyCount = e_rows * e_columns; for ( int i = 0; i < bodyCount; ++i ) { b2Transform xf = b2Body_GetTransform( m_bodies[i] ); diff --git a/samples/sample_joints.cpp b/samples/sample_joints.cpp index eda071e93..98dc17971 100644 --- a/samples/sample_joints.cpp +++ b/samples/sample_joints.cpp @@ -735,7 +735,7 @@ class PrismaticJoint : public Sample m_textLine += m_textIncrement; float speed = b2PrismaticJoint_GetSpeed( m_jointId ); - g_draw.DrawString( 5, m_textLine, "Speed = %4.1f", speed ); + g_draw.DrawString( 5, m_textLine, "Speed = %4.8f", speed ); m_textLine += m_textIncrement; } diff --git a/shared/benchmarks.h b/shared/benchmarks.h index e94fdf671..d6efd50aa 100644 --- a/shared/benchmarks.h +++ b/shared/benchmarks.h @@ -6,11 +6,20 @@ // This allows benchmarks to be tested on the benchmark app and also visualized in the samples app -B2_API void CreateJointGrid( b2WorldId worldId ); -B2_API void CreateLargePyramid( b2WorldId worldId ); -B2_API void CreateManyPyramids( b2WorldId worldId ); -B2_API void CreateRain( b2WorldId worldId ); -B2_API void StepRain( b2WorldId worldId, int stepCount ); -B2_API void CreateSpinner( b2WorldId worldId ); -B2_API void CreateSmash( b2WorldId worldId ); -B2_API void CreateTumbler( b2WorldId worldId ); +#ifdef __cplusplus +extern "C" +{ +#endif + +void CreateJointGrid( b2WorldId worldId ); +void CreateLargePyramid( b2WorldId worldId ); +void CreateManyPyramids( b2WorldId worldId ); +void CreateRain( b2WorldId worldId ); +void StepRain( b2WorldId worldId, int stepCount ); +void CreateSpinner( b2WorldId worldId ); +void CreateSmash( b2WorldId worldId ); +void CreateTumbler( b2WorldId worldId ); + +#ifdef __cplusplus +} +#endif diff --git a/shared/human.h b/shared/human.h index 295d27239..a6494c49a 100644 --- a/shared/human.h +++ b/shared/human.h @@ -36,12 +36,21 @@ typedef struct Human bool isSpawned; } Human; -B2_API void CreateHuman( Human* human, b2WorldId worldId, b2Vec2 position, float scale, float frictionTorque, float hertz, float dampingRatio, - int groupIndex, void* userData, bool colorize ); +#ifdef __cplusplus +extern "C" +{ +#endif + +void CreateHuman( Human* human, b2WorldId worldId, b2Vec2 position, float scale, float frictionTorque, float hertz, + float dampingRatio, int groupIndex, void* userData, bool colorize ); + +void DestroyHuman( Human* human ); -B2_API void DestroyHuman( Human* human ); +void Human_ApplyRandomAngularImpulse( Human* human, float magnitude ); +void Human_SetJointFrictionTorque( Human* human, float torque ); +void Human_SetJointSpringHertz( Human* human, float hertz ); +void Human_SetJointDampingRatio( Human* human, float dampingRatio ); -B2_API void Human_ApplyRandomAngularImpulse( Human* human, float magnitude ); -B2_API void Human_SetJointFrictionTorque( Human* human, float torque ); -B2_API void Human_SetJointSpringHertz( Human* human, float hertz ); -B2_API void Human_SetJointDampingRatio( Human* human, float dampingRatio ); +#ifdef __cplusplus +} +#endif diff --git a/shared/random.h b/shared/random.h index dfdd2f6dd..d4446e734 100644 --- a/shared/random.h +++ b/shared/random.h @@ -10,7 +10,18 @@ #define RAND_SEED 12345 // Global seed for simple random number generator. -B2_API uint32_t g_seed; + +#ifdef __cplusplus +extern "C" +{ +#endif + +extern uint32_t g_seed; +b2Polygon RandomPolygon( float extent ); + +#ifdef __cplusplus +} +#endif // Simple random number generator. Using this instead of rand() for cross platform determinism. B2_INLINE int RandomInt() @@ -58,5 +69,3 @@ B2_INLINE b2Vec2 RandomVec2( float lo, float hi ) v.y = RandomFloatRange( lo, hi ); return v; } - -B2_API b2Polygon RandomPolygon( float extent ); diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 0bc507726..a245dbfde 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -9,6 +9,7 @@ set(BOX2D_SOURCE_FILES body.h broad_phase.c broad_phase.h + constants.h constraint_graph.c constraint_graph.h contact.c @@ -102,7 +103,7 @@ if (BOX2D_PROFILE) FetchContent_Declare( tracy GIT_REPOSITORY https://github.com/wolfpld/tracy.git - GIT_TAG master + GIT_TAG v0.11.1 GIT_SHALLOW TRUE GIT_PROGRESS TRUE ) diff --git a/src/aabb.c b/src/aabb.c index 2fabfa50d..1d58bc489 100644 --- a/src/aabb.c +++ b/src/aabb.c @@ -7,11 +7,11 @@ #include -bool b2AABB_IsValid( b2AABB a ) +bool b2IsValidAABB( b2AABB a ) { b2Vec2 d = b2Sub( a.upperBound, a.lowerBound ); bool valid = d.x >= 0.0f && d.y >= 0.0f; - valid = valid && b2Vec2_IsValid( a.lowerBound ) && b2Vec2_IsValid( a.upperBound ); + valid = valid && b2IsValidVec2( a.lowerBound ) && b2IsValidVec2( a.upperBound ); return valid; } diff --git a/src/body.c b/src/body.c index d5d78a6d8..94e8cca56 100644 --- a/src/body.c +++ b/src/body.c @@ -168,14 +168,14 @@ static void b2DestroyBodyContacts( b2World* world, b2Body* body, bool wakeBodies b2BodyId b2CreateBody( b2WorldId worldId, const b2BodyDef* def ) { b2CheckDef( def ); - B2_ASSERT( b2Vec2_IsValid( def->position ) ); - B2_ASSERT( b2Rot_IsValid( def->rotation ) ); - B2_ASSERT( b2Vec2_IsValid( def->linearVelocity ) ); - B2_ASSERT( b2Float_IsValid( def->angularVelocity ) ); - B2_ASSERT( b2Float_IsValid( def->linearDamping ) && def->linearDamping >= 0.0f ); - B2_ASSERT( b2Float_IsValid( def->angularDamping ) && def->angularDamping >= 0.0f ); - B2_ASSERT( b2Float_IsValid( def->sleepThreshold ) && def->sleepThreshold >= 0.0f ); - B2_ASSERT( b2Float_IsValid( def->gravityScale ) ); + B2_ASSERT( b2IsValidVec2( def->position ) ); + B2_ASSERT( b2IsValidRotation( def->rotation ) ); + B2_ASSERT( b2IsValidVec2( def->linearVelocity ) ); + B2_ASSERT( b2IsValidFloat( def->angularVelocity ) ); + B2_ASSERT( b2IsValidFloat( def->linearDamping ) && def->linearDamping >= 0.0f ); + B2_ASSERT( b2IsValidFloat( def->angularDamping ) && def->angularDamping >= 0.0f ); + B2_ASSERT( b2IsValidFloat( def->sleepThreshold ) && def->sleepThreshold >= 0.0f ); + B2_ASSERT( b2IsValidFloat( def->gravityScale ) ); b2World* world = b2GetWorldFromId( worldId ); B2_ASSERT( world->locked == false ); @@ -667,8 +667,8 @@ b2Vec2 b2Body_GetWorldVector( b2BodyId bodyId, b2Vec2 localVector ) void b2Body_SetTransform( b2BodyId bodyId, b2Vec2 position, b2Rot rotation ) { - B2_ASSERT( b2Vec2_IsValid( position ) ); - B2_ASSERT( b2Rot_IsValid( rotation ) ); + B2_ASSERT( b2IsValidVec2( position ) ); + B2_ASSERT( b2IsValidRotation( rotation ) ); B2_ASSERT( b2Body_IsValid( bodyId ) ); b2World* world = b2GetWorld( bodyId.world0 ); B2_ASSERT( world->locked == false ); @@ -1223,9 +1223,9 @@ b2Vec2 b2Body_GetWorldCenterOfMass( b2BodyId bodyId ) void b2Body_SetMassData( b2BodyId bodyId, b2MassData massData ) { - B2_ASSERT( b2Float_IsValid( massData.mass ) && massData.mass >= 0.0f ); - B2_ASSERT( b2Float_IsValid( massData.rotationalInertia ) && massData.rotationalInertia >= 0.0f ); - B2_ASSERT( b2Vec2_IsValid( massData.center ) ); + B2_ASSERT( b2IsValidFloat( massData.mass ) && massData.mass >= 0.0f ); + B2_ASSERT( b2IsValidFloat( massData.rotationalInertia ) && massData.rotationalInertia >= 0.0f ); + B2_ASSERT( b2IsValidVec2( massData.center ) ); b2World* world = b2GetWorldLocked( bodyId.world0 ); if ( world == NULL ) @@ -1271,7 +1271,7 @@ void b2Body_ApplyMassFromShapes( b2BodyId bodyId ) void b2Body_SetLinearDamping( b2BodyId bodyId, float linearDamping ) { - B2_ASSERT( b2Float_IsValid( linearDamping ) && linearDamping >= 0.0f ); + B2_ASSERT( b2IsValidFloat( linearDamping ) && linearDamping >= 0.0f ); b2World* world = b2GetWorldLocked( bodyId.world0 ); if ( world == NULL ) @@ -1294,7 +1294,7 @@ float b2Body_GetLinearDamping( b2BodyId bodyId ) void b2Body_SetAngularDamping( b2BodyId bodyId, float angularDamping ) { - B2_ASSERT( b2Float_IsValid( angularDamping ) && angularDamping >= 0.0f ); + B2_ASSERT( b2IsValidFloat( angularDamping ) && angularDamping >= 0.0f ); b2World* world = b2GetWorldLocked( bodyId.world0 ); if ( world == NULL ) @@ -1318,7 +1318,7 @@ float b2Body_GetAngularDamping( b2BodyId bodyId ) void b2Body_SetGravityScale( b2BodyId bodyId, float gravityScale ) { B2_ASSERT( b2Body_IsValid( bodyId ) ); - B2_ASSERT( b2Float_IsValid( gravityScale ) ); + B2_ASSERT( b2IsValidFloat( gravityScale ) ); b2World* world = b2GetWorldLocked( bodyId.world0 ); if ( world == NULL ) diff --git a/src/body.h b/src/body.h index b36e410ae..45b91ae65 100644 --- a/src/body.h +++ b/src/body.h @@ -126,8 +126,9 @@ typedef struct b2BodySim // body data can be moved around, the id is stable (used in b2BodyId) int bodyId; - // todo eliminate + // This flag is used for debug draw bool isFast; + bool isBullet; bool isSpeedCapped; bool allowFastRotation; diff --git a/src/constants.h b/src/constants.h new file mode 100644 index 000000000..447ed21e9 --- /dev/null +++ b/src/constants.h @@ -0,0 +1,48 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +extern float b2_lengthUnitsPerMeter; + +// Used to detect bad values. Positions greater than about 16km will have precision +// problems, so 100km as a limit should be fine in all cases. +#define b2_huge ( 100000.0f * b2_lengthUnitsPerMeter ) + +// Maximum parallel workers. Used to size some static arrays. +#define b2_maxWorkers 64 + +// Maximum number of colors in the constraint graph. Constraints that cannot +// find a color are added to the overflow set which are solved single-threaded. +#define b2_graphColorCount 12 + +// A small length used as a collision and constraint tolerance. Usually it is +// chosen to be numerically significant, but visually insignificant. In meters. +// @warning modifying this can have a significant impact on stability +#define b2_linearSlop ( 0.005f * b2_lengthUnitsPerMeter ) + +// Maximum number of simultaneous worlds that can be allocated +#define b2_maxWorlds 128 + +// The maximum rotation of a body per time step. This limit is very large and is used +// to prevent numerical problems. You shouldn't need to adjust this. +// @warning increasing this to 0.5f * b2_pi or greater will break continuous collision. +#define b2_maxRotation ( 0.25f * b2_pi ) + +// @warning modifying this can have a significant impact on performance and stability +#define b2_speculativeDistance ( 4.0f * b2_linearSlop ) + +// This is used to fatten AABBs in the dynamic tree. This allows proxies +// to move by a small amount without triggering a tree adjustment. This is in meters. +// @warning modifying this can have a significant impact on performance +#define b2_aabbMargin ( 0.05f * b2_lengthUnitsPerMeter ) + +// The time that a body must be still before it will go to sleep. In seconds. +#define b2_timeToSleep 0.5f + +enum b2TreeNodeFlags +{ + b2_allocatedNode = 0x0001, + b2_enlargedNode = 0x0002, + b2_leafNode = 0x0004, +}; diff --git a/src/constraint_graph.h b/src/constraint_graph.h index 36b1961c0..6b96f8215 100644 --- a/src/constraint_graph.h +++ b/src/constraint_graph.h @@ -5,6 +5,7 @@ #include "array.h" #include "bitset.h" +#include "constants.h" typedef struct b2Body b2Body; typedef struct b2ContactSim b2ContactSim; diff --git a/src/core.c b/src/core.c index 25d9d727e..ce2fbf34e 100644 --- a/src/core.c +++ b/src/core.c @@ -36,7 +36,7 @@ float b2_lengthUnitsPerMeter = 1.0f; void b2SetLengthUnitsPerMeter( float lengthUnits ) { - B2_ASSERT( b2Float_IsValid( lengthUnits ) && lengthUnits > 0.0f ); + B2_ASSERT( b2IsValidFloat( lengthUnits ) && lengthUnits > 0.0f ); b2_lengthUnitsPerMeter = lengthUnits; } @@ -61,6 +61,11 @@ void b2SetAssertFcn( b2AssertFcn* assertFcn ) b2AssertHandler = assertFcn; } +int b2InternalAssertFcn( const char* condition, const char* fileName, int lineNumber ) +{ + return b2AssertHandler( condition, fileName, lineNumber ); +} + b2Version b2GetVersion( void ) { return ( b2Version ){ 3, 1, 0 }; diff --git a/src/core.h b/src/core.h index c76ad4ce1..38bc81738 100644 --- a/src/core.h +++ b/src/core.h @@ -3,7 +3,7 @@ #pragma once -#include "box2d/base.h" +#include "box2d/math_functions.h" // clang-format off @@ -90,30 +90,6 @@ #define B2_COMPILER_MSVC #endif -// see https://github.com/scottt/debugbreak -#if defined( B2_COMPILER_MSVC ) - #define B2_BREAKPOINT __debugbreak() -#elif defined( B2_COMPILER_GCC ) || defined( B2_COMPILER_CLANG ) - #define B2_BREAKPOINT __builtin_trap() -#else - // Unknown compiler - #include - #definef B2_BREAKPOINT assert(0) -#endif - -#if !defined( NDEBUG ) || defined( B2_ENABLE_ASSERT ) - extern b2AssertFcn* b2AssertHandler; - #define B2_ASSERT( condition ) \ - do \ - { \ - if ( !( condition ) && b2AssertHandler( #condition, __FILE__, (int)__LINE__ ) ) \ - B2_BREAKPOINT; \ - } \ - while ( 0 ) -#else - #define B2_ASSERT( ... ) ( (void)0 ) -#endif - /// Tracy profiler instrumentation /// https://github.com/wolfpld/tracy #ifdef BOX2D_PROFILE @@ -129,43 +105,6 @@ // clang-format on -extern float b2_lengthUnitsPerMeter; - -// Used to detect bad values. Positions greater than about 16km will have precision -// problems, so 100km as a limit should be fine in all cases. -#define b2_huge ( 100000.0f * b2_lengthUnitsPerMeter ) - -// Maximum parallel workers. Used to size some static arrays. -#define b2_maxWorkers 64 - -// Maximum number of colors in the constraint graph. Constraints that cannot -// find a color are added to the overflow set which are solved single-threaded. -#define b2_graphColorCount 12 - -// A small length used as a collision and constraint tolerance. Usually it is -// chosen to be numerically significant, but visually insignificant. In meters. -// @warning modifying this can have a significant impact on stability -#define b2_linearSlop ( 0.005f * b2_lengthUnitsPerMeter ) - -// Maximum number of simultaneous worlds that can be allocated -#define b2_maxWorlds 128 - -// The maximum rotation of a body per time step. This limit is very large and is used -// to prevent numerical problems. You shouldn't need to adjust this. -// @warning increasing this to 0.5f * b2_pi or greater will break continuous collision. -#define b2_maxRotation ( 0.25f * b2_pi ) - -// @warning modifying this can have a significant impact on performance and stability -#define b2_speculativeDistance ( 4.0f * b2_linearSlop ) - -// This is used to fatten AABBs in the dynamic tree. This allows proxies -// to move by a small amount without triggering a tree adjustment. This is in meters. -// @warning modifying this can have a significant impact on performance -#define b2_aabbMargin ( 0.05f * b2_lengthUnitsPerMeter ) - -// The time that a body must be still before it will go to sleep. In seconds. -#define b2_timeToSleep 0.5f - // Returns the number of elements of an array #define B2_ARRAY_COUNT( A ) (int)( sizeof( A ) / sizeof( A[0] ) ) @@ -177,13 +116,6 @@ extern float b2_lengthUnitsPerMeter; #define b2CheckDef( DEF ) B2_ASSERT( DEF->internalValue == B2_SECRET_COOKIE ) -enum b2TreeNodeFlags -{ - b2_allocatedNode = 0x0001, - b2_enlargedNode = 0x0002, - b2_leafNode = 0x0004, -}; - void* b2Alloc( int size ); void b2Free( void* mem, int size ); void* b2GrowAlloc( void* oldMem, int oldSize, int newSize ); diff --git a/src/distance.c b/src/distance.c index 94a7ef4e3..4332a19d9 100644 --- a/src/distance.c +++ b/src/distance.c @@ -2,6 +2,7 @@ // SPDX-FileCopyrightText: 2023 Erin Catto // SPDX-License-Identifier: MIT +#include "constants.h" #include "core.h" #include "box2d/collision.h" diff --git a/src/dynamic_tree.c b/src/dynamic_tree.c index 05ba86272..967e64d52 100644 --- a/src/dynamic_tree.c +++ b/src/dynamic_tree.c @@ -2,6 +2,7 @@ // SPDX-License-Identifier: MIT #include "aabb.h" +#include "constants.h" #include "core.h" #include "box2d/collision.h" @@ -22,12 +23,17 @@ static b2TreeNode b2_defaultTreeNode = { .flags = b2_allocatedNode, }; -static inline bool b2IsLeaf( const b2TreeNode* node ) +static bool b2IsLeaf( const b2TreeNode* node ) { return node->flags & b2_leafNode; } -static inline uint16_t b2MaxUInt16( uint16_t a, uint16_t b ) +static bool b2IsAllocated( const b2TreeNode* node ) +{ + return node->flags & b2_allocatedNode; +} + +static uint16_t b2MaxUInt16( uint16_t a, uint16_t b ) { return a > b ? a : b; } @@ -83,12 +89,13 @@ static int32_t b2AllocateNode( b2DynamicTree* tree ) // The free list is empty. Rebuild a bigger pool. b2TreeNode* oldNodes = tree->nodes; - int32_t oldCapcity = tree->nodeCapacity; - tree->nodeCapacity += oldCapcity >> 1; + int32_t oldCapacity = tree->nodeCapacity; + tree->nodeCapacity += oldCapacity >> 1; tree->nodes = (b2TreeNode*)b2Alloc( tree->nodeCapacity * sizeof( b2TreeNode ) ); B2_ASSERT( oldNodes != NULL ); memcpy( tree->nodes, oldNodes, tree->nodeCount * sizeof( b2TreeNode ) ); - b2Free( oldNodes, oldCapcity * sizeof( b2TreeNode ) ); + memset( tree->nodes + tree->nodeCount, 0, ( tree->nodeCapacity - tree->nodeCount ) * sizeof( b2TreeNode ) ); + b2Free( oldNodes, oldCapacity * sizeof( b2TreeNode ) ); // Build a linked list for the free list. The parent pointer becomes the "next" pointer. // todo avoid building freelist? @@ -738,7 +745,7 @@ int32_t b2DynamicTree_CreateProxy( b2DynamicTree* tree, b2AABB aabb, uint64_t ca node->userData = userData; node->categoryBits = categoryBits; node->height = 0; - node->flags = b2_leafNode; + node->flags = b2_allocatedNode | b2_leafNode; bool shouldRotate = true; b2InsertLeaf( tree, proxyId, shouldRotate ); @@ -767,7 +774,7 @@ int32_t b2DynamicTree_GetProxyCount( const b2DynamicTree* tree ) void b2DynamicTree_MoveProxy( b2DynamicTree* tree, int32_t proxyId, b2AABB aabb ) { - B2_ASSERT( b2AABB_IsValid( aabb ) ); + B2_ASSERT( b2IsValidAABB( aabb ) ); B2_ASSERT( aabb.upperBound.x - aabb.lowerBound.x < b2_huge ); B2_ASSERT( aabb.upperBound.y - aabb.lowerBound.y < b2_huge ); B2_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); @@ -785,7 +792,7 @@ void b2DynamicTree_EnlargeProxy( b2DynamicTree* tree, int32_t proxyId, b2AABB aa { b2TreeNode* nodes = tree->nodes; - B2_ASSERT( b2AABB_IsValid( aabb ) ); + B2_ASSERT( b2IsValidAABB( aabb ) ); B2_ASSERT( aabb.upperBound.x - aabb.lowerBound.x < b2_huge ); B2_ASSERT( aabb.upperBound.y - aabb.lowerBound.y < b2_huge ); B2_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); @@ -846,9 +853,8 @@ float b2DynamicTree_GetAreaRatio( const b2DynamicTree* tree ) for ( int32_t i = 0; i < tree->nodeCapacity; ++i ) { const b2TreeNode* node = tree->nodes + i; - if ( node->height < 0 || b2IsLeaf( node ) || i == tree->root ) + if ( b2IsAllocated(node) == false || b2IsLeaf( node ) || i == tree->root ) { - // Free node in pool continue; } @@ -881,7 +887,7 @@ int b2DynamicTree_ComputeHeight( const b2DynamicTree* tree ) } #if B2_VALIDATE -static void b2ValidateStructure( const b2DynamicTree* tree, int32_t index ) +static void b2ValidateStructure( const b2DynamicTree* tree, int index ) { if ( index == B2_NULL_INDEX ) { @@ -895,6 +901,8 @@ static void b2ValidateStructure( const b2DynamicTree* tree, int32_t index ) const b2TreeNode* node = tree->nodes + index; + B2_ASSERT( node->flags == 0 || ( node->flags & b2_allocatedNode ) != 0 ); + if ( b2IsLeaf( node ) ) { B2_ASSERT( node->height == 0 ); @@ -974,8 +982,8 @@ void b2DynamicTree_Validate( const b2DynamicTree* tree ) b2ValidateStructure( tree, tree->root ); b2ValidateMetrics( tree, tree->root ); - int32_t freeCount = 0; - int32_t freeIndex = tree->freeList; + int freeCount = 0; + int freeIndex = tree->freeList; while ( freeIndex != B2_NULL_INDEX ) { B2_ASSERT( 0 <= freeIndex && freeIndex < tree->nodeCapacity ); @@ -983,8 +991,8 @@ void b2DynamicTree_Validate( const b2DynamicTree* tree ) ++freeCount; } - int32_t height = b2DynamicTree_GetHeight( tree ); - int32_t computedHeight = b2DynamicTree_ComputeHeight( tree ); + int height = b2DynamicTree_GetHeight( tree ); + int computedHeight = b2DynamicTree_ComputeHeight( tree ); B2_ASSERT( height == computedHeight ); B2_ASSERT( tree->nodeCount + freeCount == tree->nodeCapacity ); @@ -993,117 +1001,6 @@ void b2DynamicTree_Validate( const b2DynamicTree* tree ) #endif } -int32_t b2DynamicTree_GetMaxBalance( const b2DynamicTree* tree ) -{ - int maxBalance = 0; - for ( int i = 0; i < tree->nodeCapacity; ++i ) - { - const b2TreeNode* node = tree->nodes + i; - if ( node->height <= 1 ) - { - continue; - } - - B2_ASSERT( b2IsLeaf( node ) == false ); - - int child1 = node->child1; - int child2 = node->child2; - int balance = b2AbsInt( tree->nodes[child2].height - tree->nodes[child1].height ); - maxBalance = b2MaxInt( maxBalance, balance ); - } - - return maxBalance; -} - -void b2DynamicTree_RebuildBottomUp( b2DynamicTree* tree ) -{ - int* nodes = b2Alloc( tree->nodeCount * sizeof( int ) ); - int count = 0; - - // Build array of leaves. Free the rest. - for ( int i = 0; i < tree->nodeCapacity; ++i ) - { - if ( tree->nodes[i].height < 0 ) - { - // free node in pool - continue; - } - - if ( b2IsLeaf( tree->nodes + i ) ) - { - tree->nodes[i].parent = B2_NULL_INDEX; - nodes[count] = i; - ++count; - } - else - { - b2FreeNode( tree, i ); - } - } - - while ( count > 1 ) - { - float minCost = FLT_MAX; - int32_t iMin = -1, jMin = -1; - for ( int32_t i = 0; i < count; ++i ) - { - b2AABB aabbi = tree->nodes[nodes[i]].aabb; - - for ( int32_t j = i + 1; j < count; ++j ) - { - b2AABB aabbj = tree->nodes[nodes[j]].aabb; - b2AABB b = b2AABB_Union( aabbi, aabbj ); - float cost = b2Perimeter( b ); - if ( cost < minCost ) - { - iMin = i; - jMin = j; - minCost = cost; - } - } - } - - int32_t index1 = nodes[iMin]; - int32_t index2 = nodes[jMin]; - b2TreeNode* child1 = tree->nodes + index1; - b2TreeNode* child2 = tree->nodes + index2; - - int32_t parentIndex = b2AllocateNode( tree ); - b2TreeNode* parent = tree->nodes + parentIndex; - parent->child1 = index1; - parent->child2 = index2; - parent->aabb = b2AABB_Union( child1->aabb, child2->aabb ); - parent->categoryBits = child1->categoryBits | child2->categoryBits; - parent->height = 1 + b2MaxUInt16( child1->height, child2->height ); - parent->parent = B2_NULL_INDEX; - - child1->parent = parentIndex; - child2->parent = parentIndex; - - nodes[jMin] = nodes[count - 1]; - nodes[iMin] = parentIndex; - --count; - } - - tree->root = nodes[0]; - b2Free( nodes, tree->nodeCount * sizeof( b2TreeNode ) ); - - b2DynamicTree_Validate( tree ); -} - -void b2DynamicTree_ShiftOrigin( b2DynamicTree* tree, b2Vec2 newOrigin ) -{ - // shift all AABBs - for ( int32_t i = 0; i < tree->nodeCapacity; ++i ) - { - b2TreeNode* n = tree->nodes + i; - n->aabb.lowerBound.x -= newOrigin.x; - n->aabb.lowerBound.y -= newOrigin.y; - n->aabb.upperBound.x -= newOrigin.x; - n->aabb.upperBound.y -= newOrigin.y; - } -} - int b2DynamicTree_GetByteCount( const b2DynamicTree* tree ) { size_t size = sizeof( b2DynamicTree ) + sizeof( b2TreeNode ) * tree->nodeCapacity + @@ -1112,12 +1009,12 @@ int b2DynamicTree_GetByteCount( const b2DynamicTree* tree ) return (int)size; } -b2TreeStats b2DynamicTree_Query( const b2DynamicTree* tree, b2AABB aabb, uint64_t maskBits, - b2TreeQueryCallbackFcn* callback, void* context ) +b2TreeStats b2DynamicTree_Query( const b2DynamicTree* tree, b2AABB aabb, uint64_t maskBits, b2TreeQueryCallbackFcn* callback, + void* context ) { b2TreeStats result = { 0 }; - if (tree->nodeCount == 0) + if ( tree->nodeCount == 0 ) { return result; } @@ -1269,11 +1166,11 @@ b2TraversalResult b2DynamicTree_RayCast( const b2DynamicTree* tree, const b2RayC #else b2TreeStats b2DynamicTree_RayCast( const b2DynamicTree* tree, const b2RayCastInput* input, uint64_t maskBits, - b2TreeRayCastCallbackFcn* callback, void* context ) + b2TreeRayCastCallbackFcn* callback, void* context ) { b2TreeStats result = { 0 }; - if (tree->nodeCount == 0) + if ( tree->nodeCount == 0 ) { return result; } @@ -1386,7 +1283,7 @@ b2TreeStats b2DynamicTree_RayCast( const b2DynamicTree* tree, const b2RayCastInp #endif b2TreeStats b2DynamicTree_ShapeCast( const b2DynamicTree* tree, const b2ShapeCastInput* input, uint64_t maskBits, - b2TreeShapeCastCallbackFcn* callback, void* context ) + b2TreeShapeCastCallbackFcn* callback, void* context ) { b2TreeStats stats = { 0 }; diff --git a/src/geometry.c b/src/geometry.c index 8becdc305..7679e8f38 100644 --- a/src/geometry.c +++ b/src/geometry.c @@ -15,7 +15,7 @@ _Static_assert( b2_maxPolygonVertices > 2, "must be 3 or more" ); bool b2IsValidRay( const b2RayCastInput* input ) { - bool isValid = b2Vec2_IsValid( input->origin ) && b2Vec2_IsValid( input->translation ) && b2Float_IsValid( input->maxFraction ) && + bool isValid = b2IsValidVec2( input->origin ) && b2IsValidVec2( input->translation ) && b2IsValidFloat( input->maxFraction ) && 0.0f <= input->maxFraction && input->maxFraction < b2_huge; return isValid; } @@ -138,8 +138,8 @@ b2Polygon b2MakeSquare( float h ) b2Polygon b2MakeBox( float hx, float hy ) { - B2_ASSERT( b2Float_IsValid( hx ) && hx > 0.0f ); - B2_ASSERT( b2Float_IsValid( hy ) && hy > 0.0f ); + B2_ASSERT( b2IsValidFloat( hx ) && hx > 0.0f ); + B2_ASSERT( b2IsValidFloat( hy ) && hy > 0.0f ); b2Polygon shape = { 0 }; shape.count = 4; @@ -158,7 +158,7 @@ b2Polygon b2MakeBox( float hx, float hy ) b2Polygon b2MakeRoundedBox( float hx, float hy, float radius ) { - B2_ASSERT( b2Float_IsValid( radius ) && radius >= 0.0f ); + B2_ASSERT( b2IsValidFloat( radius ) && radius >= 0.0f ); b2Polygon shape = b2MakeBox( hx, hy ); shape.radius = radius; return shape; @@ -185,7 +185,7 @@ b2Polygon b2MakeOffsetBox( float hx, float hy, b2Vec2 center, b2Rot rotation ) b2Polygon b2MakeOffsetRoundedBox( float hx, float hy, b2Vec2 center, b2Rot rotation, float radius ) { - B2_ASSERT( b2Float_IsValid( radius ) && radius >= 0.0f ); + B2_ASSERT( b2IsValidFloat( radius ) && radius >= 0.0f ); b2Transform xf = { center, rotation }; b2Polygon shape = { 0 }; diff --git a/src/hull.c b/src/hull.c index eb0f4c5c7..a50ba271f 100644 --- a/src/hull.c +++ b/src/hull.c @@ -1,6 +1,7 @@ // SPDX-FileCopyrightText: 2023 Erin Catto // SPDX-License-Identifier: MIT +#include "constants.h" #include "core.h" #include "box2d/collision.h" diff --git a/src/joint.c b/src/joint.c index 0239a1d72..f4cb992fa 100644 --- a/src/joint.c +++ b/src/joint.c @@ -356,7 +356,7 @@ b2JointId b2CreateDistanceJoint( b2WorldId worldId, const b2DistanceJointDef* de B2_ASSERT( b2Body_IsValid( def->bodyIdA ) ); B2_ASSERT( b2Body_IsValid( def->bodyIdB ) ); - B2_ASSERT( b2Float_IsValid( def->length ) && def->length > 0.0f ); + B2_ASSERT( b2IsValidFloat( def->length ) && def->length > 0.0f ); b2Body* bodyA = b2GetBodyFullId( world, def->bodyIdA ); b2Body* bodyB = b2GetBodyFullId( world, def->bodyIdB ); diff --git a/src/manifold.c b/src/manifold.c index 38f7c94d2..ecd2d2408 100644 --- a/src/manifold.c +++ b/src/manifold.c @@ -1,6 +1,7 @@ // SPDX-FileCopyrightText: 2023 Erin Catto // SPDX-License-Identifier: MIT +#include "constants.h" #include "core.h" #include "box2d/collision.h" @@ -511,18 +512,15 @@ b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const } // Convert manifold to world space - if ( manifold.pointCount > 0 ) + manifold.normal = b2RotateVector( xfA.q, manifold.normal ); + for ( int i = 0; i < manifold.pointCount; ++i ) { - manifold.normal = b2RotateVector( xfA.q, manifold.normal ); - for ( int i = 0; i < manifold.pointCount; ++i ) - { - b2ManifoldPoint* mp = manifold.points + i; + b2ManifoldPoint* mp = manifold.points + i; - // anchor points relative to shape origin in world space - mp->anchorA = b2RotateVector( xfA.q, b2Add( mp->anchorA, origin ) ); - mp->anchorB = b2Add( mp->anchorA, b2Sub( xfA.p, xfB.p ) ); - mp->point = b2Add( xfA.p, mp->anchorA ); - } + // anchor points relative to shape origin in world space + mp->anchorA = b2RotateVector( xfA.q, b2Add( mp->anchorA, origin ) ); + mp->anchorB = b2Add( mp->anchorA, b2Sub( xfA.p, xfB.p ) ); + mp->point = b2Add( xfA.p, mp->anchorA ); } return manifold; diff --git a/src/math_functions.c b/src/math_functions.c index 8743c612b..38bfa5847 100644 --- a/src/math_functions.c +++ b/src/math_functions.c @@ -7,7 +7,7 @@ #include -bool b2Float_IsValid( float a ) +bool b2IsValidFloat( float a ) { if ( isnan( a ) ) { @@ -22,7 +22,7 @@ bool b2Float_IsValid( float a ) return true; } -bool b2Vec2_IsValid( b2Vec2 v ) +bool b2IsValidVec2( b2Vec2 v ) { if ( isnan( v.x ) || isnan( v.y ) ) { @@ -37,7 +37,7 @@ bool b2Vec2_IsValid( b2Vec2 v ) return true; } -bool b2Rot_IsValid( b2Rot q ) +bool b2IsValidRotation( b2Rot q ) { if ( isnan( q.s ) || isnan( q.c ) ) { diff --git a/src/mouse_joint.c b/src/mouse_joint.c index b62916dca..6042fd35d 100644 --- a/src/mouse_joint.c +++ b/src/mouse_joint.c @@ -13,7 +13,7 @@ void b2MouseJoint_SetTarget( b2JointId jointId, b2Vec2 target ) { - B2_ASSERT( b2Vec2_IsValid( target ) ); + B2_ASSERT( b2IsValidVec2( target ) ); b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint ); base->mouseJoint.targetA = target; } @@ -26,7 +26,7 @@ b2Vec2 b2MouseJoint_GetTarget( b2JointId jointId ) void b2MouseJoint_SetSpringHertz( b2JointId jointId, float hertz ) { - B2_ASSERT( b2Float_IsValid( hertz ) && hertz >= 0.0f ); + B2_ASSERT( b2IsValidFloat( hertz ) && hertz >= 0.0f ); b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint ); base->mouseJoint.hertz = hertz; } @@ -39,7 +39,7 @@ float b2MouseJoint_GetSpringHertz( b2JointId jointId ) void b2MouseJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ) { - B2_ASSERT( b2Float_IsValid( dampingRatio ) && dampingRatio >= 0.0f ); + B2_ASSERT( b2IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f ); b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint ); base->mouseJoint.dampingRatio = dampingRatio; } @@ -52,7 +52,7 @@ float b2MouseJoint_GetSpringDampingRatio( b2JointId jointId ) void b2MouseJoint_SetMaxForce( b2JointId jointId, float maxForce ) { - B2_ASSERT( b2Float_IsValid( maxForce ) && maxForce >= 0.0f ); + B2_ASSERT( b2IsValidFloat( maxForce ) && maxForce >= 0.0f ); b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint ); base->mouseJoint.maxForce = maxForce; } diff --git a/src/shape.c b/src/shape.c index 25abccbf8..0698fc60e 100644 --- a/src/shape.c +++ b/src/shape.c @@ -58,9 +58,9 @@ static void b2UpdateShapeAABBs( b2Shape* shape, b2Transform transform, b2BodyTyp static b2Shape* b2CreateShapeInternal( b2World* world, b2Body* body, b2Transform transform, const b2ShapeDef* def, const void* geometry, b2ShapeType shapeType ) { - B2_ASSERT( b2Float_IsValid( def->density ) && def->density >= 0.0f ); - B2_ASSERT( b2Float_IsValid( def->friction ) && def->friction >= 0.0f ); - B2_ASSERT( b2Float_IsValid( def->restitution ) && def->restitution >= 0.0f ); + B2_ASSERT( b2IsValidFloat( def->density ) && def->density >= 0.0f ); + B2_ASSERT( b2IsValidFloat( def->friction ) && def->friction >= 0.0f ); + B2_ASSERT( b2IsValidFloat( def->restitution ) && def->restitution >= 0.0f ); int shapeId = b2AllocId( &world->shapeIdPool ); @@ -117,7 +117,6 @@ static b2Shape* b2CreateShapeInternal( b2World* world, b2Body* body, b2Transform shape->enableContactEvents = def->enableContactEvents; shape->enableHitEvents = def->enableHitEvents; shape->enablePreSolveEvents = def->enablePreSolveEvents; - shape->isFast = false; shape->proxyKey = B2_NULL_INDEX; shape->localCentroid = b2GetShapeCentroid( shape ); shape->aabb = ( b2AABB ){ b2Vec2_zero, b2Vec2_zero }; @@ -150,9 +149,9 @@ static b2Shape* b2CreateShapeInternal( b2World* world, b2Body* body, b2Transform b2ShapeId b2CreateShape( b2BodyId bodyId, const b2ShapeDef* def, const void* geometry, b2ShapeType shapeType ) { b2CheckDef( def ); - B2_ASSERT( b2Float_IsValid( def->density ) && def->density >= 0.0f ); - B2_ASSERT( b2Float_IsValid( def->friction ) && def->friction >= 0.0f ); - B2_ASSERT( b2Float_IsValid( def->restitution ) && def->restitution >= 0.0f ); + B2_ASSERT( b2IsValidFloat( def->density ) && def->density >= 0.0f ); + B2_ASSERT( b2IsValidFloat( def->friction ) && def->friction >= 0.0f ); + B2_ASSERT( b2IsValidFloat( def->restitution ) && def->restitution >= 0.0f ); b2World* world = b2GetWorldLocked( bodyId.world0 ); if ( world == NULL ) @@ -195,7 +194,7 @@ b2ShapeId b2CreateCapsuleShape( b2BodyId bodyId, const b2ShapeDef* def, const b2 b2ShapeId b2CreatePolygonShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Polygon* polygon ) { - B2_ASSERT( b2Float_IsValid( polygon->radius ) && polygon->radius >= 0.0f ); + B2_ASSERT( b2IsValidFloat( polygon->radius ) && polygon->radius >= 0.0f ); return b2CreateShape( bodyId, def, polygon, b2_polygonShape ); } @@ -283,8 +282,8 @@ void b2DestroyShape( b2ShapeId shapeId, bool updateBodyMass ) b2ChainId b2CreateChain( b2BodyId bodyId, const b2ChainDef* def ) { b2CheckDef( def ); - B2_ASSERT( b2Float_IsValid( def->friction ) && def->friction >= 0.0f ); - B2_ASSERT( b2Float_IsValid( def->restitution ) && def->restitution >= 0.0f ); + B2_ASSERT( b2IsValidFloat( def->friction ) && def->friction >= 0.0f ); + B2_ASSERT( b2IsValidFloat( def->restitution ) && def->restitution >= 0.0f ); B2_ASSERT( def->count >= 4 ); b2World* world = b2GetWorldLocked( bodyId.world0 ); @@ -916,7 +915,7 @@ b2CastOutput b2Shape_RayCast( b2ShapeId shapeId, const b2RayCastInput* input ) void b2Shape_SetDensity( b2ShapeId shapeId, float density, bool updateBodyMass ) { - B2_ASSERT( b2Float_IsValid( density ) && density >= 0.0f ); + B2_ASSERT( b2IsValidFloat( density ) && density >= 0.0f ); b2World* world = b2GetWorldLocked( shapeId.world0 ); if ( world == NULL ) @@ -949,7 +948,7 @@ float b2Shape_GetDensity( b2ShapeId shapeId ) void b2Shape_SetFriction( b2ShapeId shapeId, float friction ) { - B2_ASSERT( b2Float_IsValid( friction ) && friction >= 0.0f ); + B2_ASSERT( b2IsValidFloat( friction ) && friction >= 0.0f ); b2World* world = b2GetWorld( shapeId.world0 ); B2_ASSERT( world->locked == false ); @@ -971,7 +970,7 @@ float b2Shape_GetFriction( b2ShapeId shapeId ) void b2Shape_SetRestitution( b2ShapeId shapeId, float restitution ) { - B2_ASSERT( b2Float_IsValid( restitution ) && restitution >= 0.0f ); + B2_ASSERT( b2IsValidFloat( restitution ) && restitution >= 0.0f ); b2World* world = b2GetWorld( shapeId.world0 ); B2_ASSERT( world->locked == false ); @@ -1288,7 +1287,7 @@ b2ChainId b2Shape_GetParentChain( b2ShapeId shapeId ) void b2Chain_SetFriction( b2ChainId chainId, float friction ) { - B2_ASSERT( b2Float_IsValid( friction ) ); + B2_ASSERT( b2IsValidFloat( friction ) ); b2World* world = b2GetWorldLocked( chainId.world0 ); if ( world == NULL ) @@ -1318,7 +1317,7 @@ float b2Chain_GetFriction(b2ChainId chainId) void b2Chain_SetRestitution( b2ChainId chainId, float restitution ) { - B2_ASSERT( b2Float_IsValid( restitution ) ); + B2_ASSERT( b2IsValidFloat( restitution ) ); b2World* world = b2GetWorldLocked( chainId.world0 ); if ( world == NULL ) diff --git a/src/shape.h b/src/shape.h index 35ebd2f56..0b2051ab4 100644 --- a/src/shape.h +++ b/src/shape.h @@ -46,7 +46,6 @@ typedef struct b2Shape bool enableHitEvents; bool enablePreSolveEvents; bool enlargedAABB; - bool isFast; } b2Shape; typedef struct b2ChainShape diff --git a/src/solver.c b/src/solver.c index 8382daf60..f851bded7 100644 --- a/src/solver.c +++ b/src/solver.c @@ -188,943 +188,924 @@ static void b2IntegratePositionsTask( int startIndex, int endIndex, b2StepContex b2TracyCZoneEnd( integrate_positions ); } -static void b2FinalizeBodiesTask( int startIndex, int endIndex, uint32_t threadIndex, void* context ) +struct b2ContinuousContext { - b2TracyCZoneNC( finalize_bodies, "FinalizeBodies", b2_colorViolet, true ); - - b2StepContext* stepContext = context; - b2World* world = stepContext->world; - bool enableSleep = world->enableSleep; - b2BodyState* states = stepContext->states; - b2BodySim* sims = stepContext->sims; - b2Body* bodies = world->bodies.data; - float timeStep = stepContext->dt; - float invTimeStep = stepContext->inv_dt; - - uint16_t worldId = world->worldId; - - // The body move event array has should already have the correct size - B2_ASSERT( endIndex <= world->bodyMoveEvents.count ); - b2BodyMoveEvent* moveEvents = world->bodyMoveEvents.data; - - b2BitSet* enlargedSimBitSet = &world->taskContexts.data[threadIndex].enlargedSimBitSet; - b2BitSet* awakeIslandBitSet = &world->taskContexts.data[threadIndex].awakeIslandBitSet; - b2TaskContext* taskContext = world->taskContexts.data + threadIndex; - - bool enableContinuous = world->enableContinuous; + b2World* world; + b2BodySim* fastBodySim; + b2Shape* fastShape; + b2Vec2 centroid1, centroid2; + b2Sweep sweep; + float fraction; +}; - const float speculativeDistance = b2_speculativeDistance; - const float aabbMargin = b2_aabbMargin; +// This is called from b2DynamicTree_Query for continuous collision +static bool b2ContinuousQueryCallback( int proxyId, int shapeId, void* context ) +{ + B2_MAYBE_UNUSED( proxyId ); - B2_ASSERT( startIndex <= endIndex ); + struct b2ContinuousContext* continuousContext = context; + b2Shape* fastShape = continuousContext->fastShape; + b2BodySim* fastBodySim = continuousContext->fastBodySim; - for ( int simIndex = startIndex; simIndex < endIndex; ++simIndex ) + // Skip same shape + if ( shapeId == fastShape->id ) { - b2BodyState* state = states + simIndex; - b2BodySim* sim = sims + simIndex; - - b2Vec2 v = state->linearVelocity; - float w = state->angularVelocity; - - B2_ASSERT( b2Vec2_IsValid( v ) ); - B2_ASSERT( b2Float_IsValid( w ) ); - - sim->center = b2Add( sim->center, state->deltaPosition ); - sim->transform.q = b2NormalizeRot( b2MulRot( state->deltaRotation, sim->transform.q ) ); - - // Use the velocity of the farthest point on the body to account for rotation. - float maxVelocity = b2Length( v ) + b2AbsFloat( w ) * sim->maxExtent; - - // Sleep needs to observe position correction as well as true velocity. - float maxDeltaPosition = b2Length( state->deltaPosition ) + b2AbsFloat( state->deltaRotation.s ) * sim->maxExtent; - - // Position correction is not as important for sleep as true velocity. - float positionSleepFactor = 0.5f; + return true; + } - float sleepVelocity = b2MaxFloat( maxVelocity, positionSleepFactor * invTimeStep * maxDeltaPosition ); + b2World* world = continuousContext->world; - // reset state deltas - state->deltaPosition = b2Vec2_zero; - state->deltaRotation = b2Rot_identity; + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); - sim->transform.p = b2Sub( sim->center, b2RotateVector( sim->transform.q, sim->localCenter ) ); + // Skip same body + if ( shape->bodyId == fastShape->bodyId ) + { + return true; + } - // cache miss here, however I need the shape list below - b2Body* body = bodies + sim->bodyId; - body->bodyMoveIndex = simIndex; - moveEvents[simIndex].transform = sim->transform; - moveEvents[simIndex].bodyId = ( b2BodyId ){ sim->bodyId + 1, worldId, body->revision }; - moveEvents[simIndex].userData = body->userData; - moveEvents[simIndex].fellAsleep = false; + // Skip sensors + if ( shape->isSensor == true ) + { + return true; + } - // reset applied force and torque - sim->force = b2Vec2_zero; - sim->torque = 0.0f; + // Skip filtered shapes + bool canCollide = b2ShouldShapesCollide( fastShape->filter, shape->filter ); + if ( canCollide == false ) + { + return true; + } - body->isSpeedCapped = sim->isSpeedCapped; - sim->isSpeedCapped = false; + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); - sim->isFast = false; + b2BodySim* bodySim = b2GetBodySim( world, body ); + B2_ASSERT( body->type == b2_staticBody || fastBodySim->isBullet ); - if ( enableSleep == false || body->enableSleep == false || sleepVelocity > body->sleepThreshold ) - { - // Body is not sleepy - body->sleepTime = 0.0f; + // Skip bullets + if ( bodySim->isBullet ) + { + return true; + } - const float saftetyFactor = 0.5f; - if ( body->type == b2_dynamicBody && enableContinuous && maxVelocity * timeStep > saftetyFactor * sim->minExtent ) - { - // Store in fast array for the continuous collision stage - // This is deterministic because the order of TOI sweeps doesn't matter - if ( sim->isBullet ) - { - int bulletIndex = atomic_fetch_add( &stepContext->bulletBodyCount, 1 ); - stepContext->bulletBodies[bulletIndex] = simIndex; - } - else - { - int fastIndex = atomic_fetch_add( &stepContext->fastBodyCount, 1 ); - stepContext->fastBodies[fastIndex] = simIndex; - } + // Skip filtered bodies + b2Body* fastBody = b2BodyArray_Get( &world->bodies, fastBodySim->bodyId ); + canCollide = b2ShouldBodiesCollide( world, fastBody, body ); + if ( canCollide == false ) + { + return true; + } - sim->isFast = true; - } - else - { - // Body is safe to advance - sim->center0 = sim->center; - sim->rotation0 = sim->transform.q; - } - } - else + // Custom user filtering + b2CustomFilterFcn* customFilterFcn = world->customFilterFcn; + if ( customFilterFcn != NULL ) + { + b2ShapeId idA = { shape->id + 1, world->worldId, shape->revision }; + b2ShapeId idB = { fastShape->id + 1, world->worldId, fastShape->revision }; + canCollide = customFilterFcn( idA, idB, world->customFilterContext ); + if ( canCollide == false ) { - // Body is safe to advance and is falling asleep - sim->center0 = sim->center; - sim->rotation0 = sim->transform.q; - body->sleepTime += timeStep; + return true; } + } - // Any single body in an island can keep it awake - b2Island* island = b2IslandArray_Get( &world->islands, body->islandId ); - if ( body->sleepTime < b2_timeToSleep ) - { - // keep island awake - int islandIndex = island->localIndex; - b2SetBit( awakeIslandBitSet, islandIndex ); - } - else if ( island->constraintRemoveCount > 0 ) - { - // body wants to sleep but its island needs splitting first - if ( body->sleepTime > taskContext->splitSleepTime ) - { - // pick the sleepiest candidate - taskContext->splitIslandId = body->islandId; - taskContext->splitSleepTime = body->sleepTime; - } - } + // Prevent pausing on chain segment junctions + if ( shape->type == b2_chainSegmentShape ) + { + b2Transform transform = bodySim->transform; + b2Vec2 p1 = b2TransformPoint( transform, shape->chainSegment.segment.point1 ); + b2Vec2 p2 = b2TransformPoint( transform, shape->chainSegment.segment.point2 ); + b2Vec2 e = b2Sub( p2, p1 ); + b2Vec2 c1 = continuousContext->centroid1; + b2Vec2 c2 = continuousContext->centroid2; + float offset1 = b2Cross( b2Sub( c1, p1 ), e ); + float offset2 = b2Cross( b2Sub( c2, p1 ), e ); - // Update shapes AABBs - b2Transform transform = sim->transform; - bool isFast = sim->isFast; - int shapeId = body->headShapeId; - while ( shapeId != B2_NULL_INDEX ) + if ( offset1 < 0.0f || offset2 > 0.0f ) { - b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); - - B2_ASSERT( shape->isFast == false ); - - if ( isFast ) - { - // The AABB is updated after continuous collision. - // Add to moved shapes regardless of AABB changes. - shape->isFast = true; + // Started behind or finished in front + return true; + } + } - // Bit-set to keep the move array sorted - b2SetBit( enlargedSimBitSet, simIndex ); - } - else - { - b2AABB aabb = b2ComputeShapeAABB( shape, transform ); - aabb.lowerBound.x -= speculativeDistance; - aabb.lowerBound.y -= speculativeDistance; - aabb.upperBound.x += speculativeDistance; - aabb.upperBound.y += speculativeDistance; - shape->aabb = aabb; + b2TOIInput input; + input.proxyA = b2MakeShapeDistanceProxy( shape ); + input.proxyB = b2MakeShapeDistanceProxy( fastShape ); + input.sweepA = b2MakeSweep( bodySim ); + input.sweepB = continuousContext->sweep; + input.tMax = continuousContext->fraction; - B2_ASSERT( shape->enlargedAABB == false ); + float hitFraction = continuousContext->fraction; - if ( b2AABB_Contains( shape->fatAABB, aabb ) == false ) - { - b2AABB fatAABB; - fatAABB.lowerBound.x = aabb.lowerBound.x - aabbMargin; - fatAABB.lowerBound.y = aabb.lowerBound.y - aabbMargin; - fatAABB.upperBound.x = aabb.upperBound.x + aabbMargin; - fatAABB.upperBound.y = aabb.upperBound.y + aabbMargin; - shape->fatAABB = fatAABB; + bool didHit = false; + b2TOIOutput output = b2TimeOfImpact( &input ); + if ( 0.0f < output.t && output.t < continuousContext->fraction ) + { + hitFraction = output.t; + didHit = true; + } + else if ( 0.0f == output.t ) + { + // fallback to TOI of a small circle around the fast shape centroid + b2Vec2 centroid = b2GetShapeCentroid( fastShape ); + input.proxyB = b2MakeProxy( ¢roid, 1, b2_speculativeDistance ); + output = b2TimeOfImpact( &input ); + if ( 0.0f < output.t && output.t < continuousContext->fraction ) + { + hitFraction = output.t; + didHit = true; + } + } - shape->enlargedAABB = true; + if ( didHit && ( shape->enablePreSolveEvents || fastShape->enablePreSolveEvents ) ) + { + // Pre-solve is expensive because I need to compute a temporary manifold + b2Transform transformA = b2GetSweepTransform( &input.sweepA, hitFraction ); + b2Transform transformB = b2GetSweepTransform( &input.sweepB, hitFraction ); + b2Manifold manifold = b2ComputeManifold( shape, transformA, fastShape, transformB ); + b2ShapeId shapeIdA = { shape->id + 1, world->worldId, shape->revision }; + b2ShapeId shapeIdB = { fastShape->id + 1, world->worldId, fastShape->revision }; - // Bit-set to keep the move array sorted - b2SetBit( enlargedSimBitSet, simIndex ); - } - } + // The user may modify the temporary manifold here but it doesn't matter. They will be able to + // modify the real manifold in the discrete solver. + didHit = world->preSolveFcn( shapeIdA, shapeIdB, &manifold, world->preSolveContext ); + } - shapeId = shape->nextShapeId; - } + if ( didHit ) + { + continuousContext->fraction = hitFraction; } - b2TracyCZoneEnd( finalize_bodies ); + return true; } -/* - typedef enum b2SolverStageType +// Continuous collision of dynamic versus static +static void b2SolveContinuous( b2World* world, int bodySimIndex ) { - b2_stagePrepareJoints, - b2_stagePrepareContacts, - b2_stageIntegrateVelocities, - b2_stageWarmStart, - b2_stageSolve, - b2_stageIntegratePositions, - b2_stageRelax, - b2_stageRestitution, - b2_stageStoreImpulses -} b2SolverStageType; + b2TracyCZoneNC( fast_body, "Fast Body", b2_colorDarkGoldenrod, true ); -typedef enum b2SolverBlockType -{ - b2_bodyBlock, - b2_jointBlock, - b2_contactBlock, - b2_graphJointBlock, - b2_graphContactBlock -} b2SolverBlockType; -*/ + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + b2BodySim* fastBodySim = b2BodySimArray_Get( &awakeSet->bodySims, bodySimIndex ); + B2_ASSERT( fastBodySim->isFast ); -static void b2ExecuteBlock( b2SolverStage* stage, b2StepContext* context, b2SolverBlock* block ) -{ - b2SolverStageType stageType = stage->type; - b2SolverBlockType blockType = block->blockType; - int startIndex = block->startIndex; - int endIndex = startIndex + block->count; + b2Sweep sweep = b2MakeSweep( fastBodySim ); - switch ( stageType ) - { - case b2_stagePrepareJoints: - b2PrepareJointsTask( startIndex, endIndex, context ); - break; + b2Transform xf1; + xf1.q = sweep.q1; + xf1.p = b2Sub( sweep.c1, b2RotateVector( sweep.q1, sweep.localCenter ) ); - case b2_stagePrepareContacts: - b2PrepareContactsTask( startIndex, endIndex, context ); - break; + b2Transform xf2; + xf2.q = sweep.q2; + xf2.p = b2Sub( sweep.c2, b2RotateVector( sweep.q2, sweep.localCenter ) ); - case b2_stageIntegrateVelocities: - b2IntegrateVelocitiesTask( startIndex, endIndex, context ); - break; + b2DynamicTree* staticTree = world->broadPhase.trees + b2_staticBody; + b2DynamicTree* kinematicTree = world->broadPhase.trees + b2_kinematicBody; + b2DynamicTree* dynamicTree = world->broadPhase.trees + b2_dynamicBody; + b2Body* fastBody = b2BodyArray_Get( &world->bodies, fastBodySim->bodyId ); - case b2_stageWarmStart: - if ( context->world->enableWarmStarting ) - { - if ( blockType == b2_graphContactBlock ) - { - b2WarmStartContactsTask( startIndex, endIndex, context, stage->colorIndex ); - } - else if ( blockType == b2_graphJointBlock ) - { - b2WarmStartJointsTask( startIndex, endIndex, context, stage->colorIndex ); - } - } - break; + struct b2ContinuousContext context; + context.world = world; + context.sweep = sweep; + context.fastBodySim = fastBodySim; + context.fraction = 1.0f; - case b2_stageSolve: - if ( blockType == b2_graphContactBlock ) - { - b2SolveContactsTask( startIndex, endIndex, context, stage->colorIndex, true ); - } - else if ( blockType == b2_graphJointBlock ) - { - b2SolveJointsTask( startIndex, endIndex, context, stage->colorIndex, true ); - } - break; + bool isBullet = fastBodySim->isBullet; - case b2_stageIntegratePositions: - b2IntegratePositionsTask( startIndex, endIndex, context ); - break; + int shapeId = fastBody->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* fastShape = b2ShapeArray_Get( &world->shapes, shapeId ); + shapeId = fastShape->nextShapeId; - case b2_stageRelax: - if ( blockType == b2_graphContactBlock ) - { - b2SolveContactsTask( startIndex, endIndex, context, stage->colorIndex, false ); - } - else if ( blockType == b2_graphJointBlock ) - { - b2SolveJointsTask( startIndex, endIndex, context, stage->colorIndex, false ); - } - break; + context.fastShape = fastShape; + context.centroid1 = b2TransformPoint( xf1, fastShape->localCentroid ); + context.centroid2 = b2TransformPoint( xf2, fastShape->localCentroid ); - case b2_stageRestitution: - if ( blockType == b2_graphContactBlock ) - { - b2ApplyRestitutionTask( startIndex, endIndex, context, stage->colorIndex ); - } - break; + b2AABB box1 = fastShape->aabb; + b2AABB box2 = b2ComputeShapeAABB( fastShape, xf2 ); + b2AABB box = b2AABB_Union( box1, box2 ); - case b2_stageStoreImpulses: - b2StoreImpulsesTask( startIndex, endIndex, context ); - break; - } -} + // Store this to avoid double computation in the case there is no impact event + fastShape->aabb = box2; -static inline int GetWorkerStartIndex( int workerIndex, int blockCount, int workerCount ) -{ - if ( blockCount <= workerCount ) - { - return workerIndex < blockCount ? workerIndex : B2_NULL_INDEX; - } + // No continuous collision for sensors + if ( fastShape->isSensor ) + { + continue; + } - int blocksPerWorker = blockCount / workerCount; - int remainder = blockCount - blocksPerWorker * workerCount; - return blocksPerWorker * workerIndex + b2MinInt( remainder, workerIndex ); -} + b2DynamicTree_Query( staticTree, box, b2_defaultMaskBits, b2ContinuousQueryCallback, &context ); -static void b2ExecuteStage( b2SolverStage* stage, b2StepContext* context, int previousSyncIndex, int syncIndex, int workerIndex ) -{ - int completedCount = 0; - b2SolverBlock* blocks = stage->blocks; - int blockCount = stage->blockCount; + if ( isBullet ) + { + b2DynamicTree_Query( kinematicTree, box, b2_defaultMaskBits, b2ContinuousQueryCallback, &context ); + b2DynamicTree_Query( dynamicTree, box, b2_defaultMaskBits, b2ContinuousQueryCallback, &context ); + } + } - int expectedSyncIndex = previousSyncIndex; + const float speculativeDistance = b2_speculativeDistance; + const float aabbMargin = b2_aabbMargin; - int startIndex = GetWorkerStartIndex( workerIndex, blockCount, context->workerCount ); - if ( startIndex == B2_NULL_INDEX ) + if ( context.fraction < 1.0f ) { - return; - } + // Handle time of impact event + b2Rot q = b2NLerp( sweep.q1, sweep.q2, context.fraction ); + b2Vec2 c = b2Lerp( sweep.c1, sweep.c2, context.fraction ); + b2Vec2 origin = b2Sub( c, b2RotateVector( q, sweep.localCenter ) ); - B2_ASSERT( 0 <= startIndex && startIndex < blockCount ); + // Advance body + b2Transform transform = { origin, q }; + fastBodySim->transform = transform; + fastBodySim->center = c; + fastBodySim->rotation0 = q; + fastBodySim->center0 = c; - int blockIndex = startIndex; + // Prepare AABBs for broad-phase. + // Even though a body is fast, it may not move much. So the + // AABB may not need enlargement. - // Caution: this can change expectedSyncIndex - while ( atomic_compare_exchange_strong( &blocks[blockIndex].syncIndex, &expectedSyncIndex, syncIndex ) == true ) - { - B2_ASSERT( stage->type != b2_stagePrepareContacts || syncIndex < 2 ); + shapeId = fastBody->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); - B2_ASSERT( completedCount < blockCount ); + // Must recompute aabb at the interpolated transform + b2AABB aabb = b2ComputeShapeAABB( shape, transform ); + aabb.lowerBound.x -= speculativeDistance; + aabb.lowerBound.y -= speculativeDistance; + aabb.upperBound.x += speculativeDistance; + aabb.upperBound.y += speculativeDistance; + shape->aabb = aabb; - b2ExecuteBlock( stage, context, blocks + blockIndex ); + if ( b2AABB_Contains( shape->fatAABB, aabb ) == false ) + { + b2AABB fatAABB; + fatAABB.lowerBound.x = aabb.lowerBound.x - aabbMargin; + fatAABB.lowerBound.y = aabb.lowerBound.y - aabbMargin; + fatAABB.upperBound.x = aabb.upperBound.x + aabbMargin; + fatAABB.upperBound.y = aabb.upperBound.y + aabbMargin; + shape->fatAABB = fatAABB; - completedCount += 1; - blockIndex += 1; - if ( blockIndex >= blockCount ) - { - // Keep looking for work - blockIndex = 0; + shape->enlargedAABB = true; + fastBodySim->enlargeAABB = true; + } + + shapeId = shape->nextShapeId; } + } + else + { + // No time of impact event - expectedSyncIndex = previousSyncIndex; - } + // Advance body + fastBodySim->rotation0 = fastBodySim->transform.q; + fastBodySim->center0 = fastBodySim->center; - // Search backwards for blocks - blockIndex = startIndex - 1; - while ( true ) - { - if ( blockIndex < 0 ) + // Prepare AABBs for broad-phase + shapeId = fastBody->headShapeId; + while ( shapeId != B2_NULL_INDEX ) { - blockIndex = blockCount - 1; - } + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); - expectedSyncIndex = previousSyncIndex; + // shape->aabb is still valid from above - // Caution: this can change expectedSyncIndex - if ( atomic_compare_exchange_strong( &blocks[blockIndex].syncIndex, &expectedSyncIndex, syncIndex ) == false ) - { - break; - } + if ( b2AABB_Contains( shape->fatAABB, shape->aabb ) == false ) + { + b2AABB fatAABB; + fatAABB.lowerBound.x = shape->aabb.lowerBound.x - aabbMargin; + fatAABB.lowerBound.y = shape->aabb.lowerBound.y - aabbMargin; + fatAABB.upperBound.x = shape->aabb.upperBound.x + aabbMargin; + fatAABB.upperBound.y = shape->aabb.upperBound.y + aabbMargin; + shape->fatAABB = fatAABB; - b2ExecuteBlock( stage, context, blocks + blockIndex ); - completedCount += 1; - blockIndex -= 1; + shape->enlargedAABB = true; + fastBodySim->enlargeAABB = true; + } + + shapeId = shape->nextShapeId; + } } - (void)atomic_fetch_add( &stage->completionCount, completedCount ); + b2TracyCZoneEnd( fast_body ); } -static void b2ExecuteMainStage( b2SolverStage* stage, b2StepContext* context, uint32_t syncBits ) +static void b2FinalizeBodiesTask( int startIndex, int endIndex, uint32_t threadIndex, void* context ) { - int blockCount = stage->blockCount; - if ( blockCount == 0 ) - { - return; - } + b2TracyCZoneNC( finalize_bodies, "FinalizeBodies", b2_colorViolet, true ); - if ( blockCount == 1 ) - { - b2ExecuteBlock( stage, context, stage->blocks ); - } - else - { - atomic_store( &context->atomicSyncBits, syncBits ); + b2StepContext* stepContext = context; + b2World* world = stepContext->world; + bool enableSleep = world->enableSleep; + b2BodyState* states = stepContext->states; + b2BodySim* sims = stepContext->sims; + b2Body* bodies = world->bodies.data; + float timeStep = stepContext->dt; + float invTimeStep = stepContext->inv_dt; - int syncIndex = ( syncBits >> 16 ) & 0xFFFF; - B2_ASSERT( syncIndex > 0 ); - int previousSyncIndex = syncIndex - 1; + uint16_t worldId = world->worldId; - b2ExecuteStage( stage, context, previousSyncIndex, syncIndex, 0 ); + // The body move event array has should already have the correct size + B2_ASSERT( endIndex <= world->bodyMoveEvents.count ); + b2BodyMoveEvent* moveEvents = world->bodyMoveEvents.data; - // todo consider using the cycle counter as well - while ( atomic_load( &stage->completionCount ) != blockCount ) - { - b2Pause(); - } + b2BitSet* enlargedSimBitSet = &world->taskContexts.data[threadIndex].enlargedSimBitSet; + b2BitSet* awakeIslandBitSet = &world->taskContexts.data[threadIndex].awakeIslandBitSet; + b2TaskContext* taskContext = world->taskContexts.data + threadIndex; - atomic_store( &stage->completionCount, 0 ); - } -} + bool enableContinuous = world->enableContinuous; -// This should not use the thread index because thread 0 can be called twice by enkiTS. -void b2SolverTask( int startIndex, int endIndex, uint32_t threadIndexDontUse, void* taskContext ) -{ - B2_MAYBE_UNUSED( startIndex ); - B2_MAYBE_UNUSED( endIndex ); - B2_MAYBE_UNUSED( threadIndexDontUse ); + const float speculativeDistance = b2_speculativeDistance; + const float aabbMargin = b2_aabbMargin; - b2WorkerContext* workerContext = taskContext; - int workerIndex = workerContext->workerIndex; - b2StepContext* context = workerContext->context; - int activeColorCount = context->activeColorCount; - b2SolverStage* stages = context->stages; - b2Profile* profile = &context->world->profile; + B2_ASSERT( startIndex <= endIndex ); - if ( workerIndex == 0 ) + for ( int simIndex = startIndex; simIndex < endIndex; ++simIndex ) { - // Main thread synchronizes the workers and does work itself. - // - // Stages are re-used by loops so that I don't need more stages for large iteration counts. - // The sync indices grow monotonically for the body/graph/constraint groupings because they share solver blocks. - // The stage index and sync indices are combined in to sync bits for atomic synchronization. - // The workers need to compute the previous sync index for a given stage so that CAS works correctly. This - // setup makes this easy to do. + b2BodyState* state = states + simIndex; + b2BodySim* sim = sims + simIndex; - /* - b2_stagePrepareJoints, - b2_stagePrepareContacts, - b2_stageIntegrateVelocities, - b2_stageWarmStart, - b2_stageSolve, - b2_stageIntegratePositions, - b2_stageRelax, - b2_stageRestitution, - b2_stageStoreImpulses - */ + b2Vec2 v = state->linearVelocity; + float w = state->angularVelocity; - b2Timer timer = b2CreateTimer(); + B2_ASSERT( b2IsValidVec2( v ) ); + B2_ASSERT( b2IsValidFloat( w ) ); - int bodySyncIndex = 1; - int stageIndex = 0; + sim->center = b2Add( sim->center, state->deltaPosition ); + sim->transform.q = b2NormalizeRot( b2MulRot( state->deltaRotation, sim->transform.q ) ); - // This stage loops over all awake joints - uint32_t jointSyncIndex = 1; - uint32_t syncBits = ( jointSyncIndex << 16 ) | stageIndex; - B2_ASSERT( stages[stageIndex].type == b2_stagePrepareJoints ); - b2ExecuteMainStage( stages + stageIndex, context, syncBits ); - stageIndex += 1; - jointSyncIndex += 1; + // Use the velocity of the farthest point on the body to account for rotation. + float maxVelocity = b2Length( v ) + b2AbsFloat( w ) * sim->maxExtent; - // This stage loops over all contact constraints - uint32_t contactSyncIndex = 1; - syncBits = ( contactSyncIndex << 16 ) | stageIndex; - B2_ASSERT( stages[stageIndex].type == b2_stagePrepareContacts ); - b2ExecuteMainStage( stages + stageIndex, context, syncBits ); - stageIndex += 1; - contactSyncIndex += 1; + // Sleep needs to observe position correction as well as true velocity. + float maxDeltaPosition = b2Length( state->deltaPosition ) + b2AbsFloat( state->deltaRotation.s ) * sim->maxExtent; - int graphSyncIndex = 1; + // Position correction is not as important for sleep as true velocity. + float positionSleepFactor = 0.5f; - // Single-threaded overflow work. These constraints don't fit in the graph coloring. - b2PrepareOverflowJoints( context ); - b2PrepareOverflowContacts( context ); + float sleepVelocity = b2MaxFloat( maxVelocity, positionSleepFactor * invTimeStep * maxDeltaPosition ); - profile->prepareConstraints += b2GetMillisecondsAndReset( &timer ); + // reset state deltas + state->deltaPosition = b2Vec2_zero; + state->deltaRotation = b2Rot_identity; - int subStepCount = context->subStepCount; - for ( int i = 0; i < subStepCount; ++i ) - { - // stage index restarted each iteration - // syncBits still increases monotonically because the upper bits increase each iteration - int iterStageIndex = stageIndex; + sim->transform.p = b2Sub( sim->center, b2RotateVector( sim->transform.q, sim->localCenter ) ); - // integrate velocities - syncBits = ( bodySyncIndex << 16 ) | iterStageIndex; - B2_ASSERT( stages[iterStageIndex].type == b2_stageIntegrateVelocities ); - b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); - iterStageIndex += 1; - bodySyncIndex += 1; + // cache miss here, however I need the shape list below + b2Body* body = bodies + sim->bodyId; + body->bodyMoveIndex = simIndex; + moveEvents[simIndex].transform = sim->transform; + moveEvents[simIndex].bodyId = ( b2BodyId ){ sim->bodyId + 1, worldId, body->revision }; + moveEvents[simIndex].userData = body->userData; + moveEvents[simIndex].fellAsleep = false; - profile->integrateVelocities += b2GetMillisecondsAndReset( &timer ); + // reset applied force and torque + sim->force = b2Vec2_zero; + sim->torque = 0.0f; - // warm start constraints - b2WarmStartOverflowJoints( context ); - b2WarmStartOverflowContacts( context ); + body->isSpeedCapped = sim->isSpeedCapped; + sim->isSpeedCapped = false; - for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + sim->isFast = false; + + if ( enableSleep == false || body->enableSleep == false || sleepVelocity > body->sleepThreshold ) + { + // Body is not sleepy + body->sleepTime = 0.0f; + + const float saftetyFactor = 0.5f; + if ( body->type == b2_dynamicBody && enableContinuous && maxVelocity * timeStep > saftetyFactor * sim->minExtent ) { - syncBits = ( graphSyncIndex << 16 ) | iterStageIndex; - B2_ASSERT( stages[iterStageIndex].type == b2_stageWarmStart ); - b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); - iterStageIndex += 1; + // This flag is only retained for debug draw + sim->isFast = true; + + // Store in fast array for the continuous collision stage + // This is deterministic because the order of TOI sweeps doesn't matter + if ( sim->isBullet ) + { + int bulletIndex = atomic_fetch_add( &stepContext->bulletBodyCount, 1 ); + stepContext->bulletBodies[bulletIndex] = simIndex; + } + else + { + b2SolveContinuous( world, simIndex ); + } } - graphSyncIndex += 1; + else + { + // Body is safe to advance + sim->center0 = sim->center; + sim->rotation0 = sim->transform.q; + } + } + else + { + // Body is safe to advance and is falling asleep + sim->center0 = sim->center; + sim->rotation0 = sim->transform.q; + body->sleepTime += timeStep; + } - profile->warmStart += b2GetMillisecondsAndReset( &timer ); + // Any single body in an island can keep it awake + b2Island* island = b2IslandArray_Get( &world->islands, body->islandId ); + if ( body->sleepTime < b2_timeToSleep ) + { + // keep island awake + int islandIndex = island->localIndex; + b2SetBit( awakeIslandBitSet, islandIndex ); + } + else if ( island->constraintRemoveCount > 0 ) + { + // body wants to sleep but its island needs splitting first + if ( body->sleepTime > taskContext->splitSleepTime ) + { + // pick the sleepiest candidate + taskContext->splitIslandId = body->islandId; + taskContext->splitSleepTime = body->sleepTime; + } + } - // solve constraints - bool useBias = true; - b2SolveOverflowJoints( context, useBias ); - b2SolveOverflowContacts( context, useBias ); + // Update shapes AABBs + b2Transform transform = sim->transform; + bool isFast = sim->isFast; + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); - for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + if ( isFast ) { - syncBits = ( graphSyncIndex << 16 ) | iterStageIndex; - B2_ASSERT( stages[iterStageIndex].type == b2_stageSolve ); - b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); - iterStageIndex += 1; - } - graphSyncIndex += 1; + // For fast non-bullet bodies the AABB has already been updated in b2SolveContinuous + // For fast bullet bodies the AABB will be updated at a later stage - profile->solveVelocities += b2GetMillisecondsAndReset( &timer ); + // Add to enlarged shapes regardless of AABB changes. + // Bit-set to keep the move array sorted + b2SetBit( enlargedSimBitSet, simIndex ); + } + else + { + b2AABB aabb = b2ComputeShapeAABB( shape, transform ); + aabb.lowerBound.x -= speculativeDistance; + aabb.lowerBound.y -= speculativeDistance; + aabb.upperBound.x += speculativeDistance; + aabb.upperBound.y += speculativeDistance; + shape->aabb = aabb; - // integrate positions - B2_ASSERT( stages[iterStageIndex].type == b2_stageIntegratePositions ); - syncBits = ( bodySyncIndex << 16 ) | iterStageIndex; - b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); - iterStageIndex += 1; - bodySyncIndex += 1; + B2_ASSERT( shape->enlargedAABB == false ); - profile->integratePositions += b2GetMillisecondsAndReset( &timer ); + if ( b2AABB_Contains( shape->fatAABB, aabb ) == false ) + { + b2AABB fatAABB; + fatAABB.lowerBound.x = aabb.lowerBound.x - aabbMargin; + fatAABB.lowerBound.y = aabb.lowerBound.y - aabbMargin; + fatAABB.upperBound.x = aabb.upperBound.x + aabbMargin; + fatAABB.upperBound.y = aabb.upperBound.y + aabbMargin; + shape->fatAABB = fatAABB; - // relax constraints - useBias = false; - b2SolveOverflowJoints( context, useBias ); - b2SolveOverflowContacts( context, useBias ); + shape->enlargedAABB = true; - for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) - { - syncBits = ( graphSyncIndex << 16 ) | iterStageIndex; - B2_ASSERT( stages[iterStageIndex].type == b2_stageRelax ); - b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); - iterStageIndex += 1; + // Bit-set to keep the move array sorted + b2SetBit( enlargedSimBitSet, simIndex ); + } } - graphSyncIndex += 1; - profile->relaxVelocities += b2GetMillisecondsAndReset( &timer ); + shapeId = shape->nextShapeId; } + } - // advance the stage according to the sub-stepping tasks just completed - // integrate velocities / warm start / solve / integrate positions / relax - stageIndex += 1 + activeColorCount + activeColorCount + 1 + activeColorCount; - - // Restitution - { - b2ApplyOverflowRestitution( context ); + b2TracyCZoneEnd( finalize_bodies ); +} - int iterStageIndex = stageIndex; - for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) - { - syncBits = ( graphSyncIndex << 16 ) | iterStageIndex; - B2_ASSERT( stages[iterStageIndex].type == b2_stageRestitution ); - b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); - iterStageIndex += 1; - } - // graphSyncIndex += 1; - stageIndex += activeColorCount; - } +/* + typedef enum b2SolverStageType +{ + b2_stagePrepareJoints, + b2_stagePrepareContacts, + b2_stageIntegrateVelocities, + b2_stageWarmStart, + b2_stageSolve, + b2_stageIntegratePositions, + b2_stageRelax, + b2_stageRestitution, + b2_stageStoreImpulses +} b2SolverStageType; - profile->applyRestitution += b2GetMillisecondsAndReset( &timer ); +typedef enum b2SolverBlockType +{ + b2_bodyBlock, + b2_jointBlock, + b2_contactBlock, + b2_graphJointBlock, + b2_graphContactBlock +} b2SolverBlockType; +*/ - b2StoreOverflowImpulses( context ); +static void b2ExecuteBlock( b2SolverStage* stage, b2StepContext* context, b2SolverBlock* block ) +{ + b2SolverStageType stageType = stage->type; + b2SolverBlockType blockType = block->blockType; + int startIndex = block->startIndex; + int endIndex = startIndex + block->count; - syncBits = ( contactSyncIndex << 16 ) | stageIndex; - B2_ASSERT( stages[stageIndex].type == b2_stageStoreImpulses ); - b2ExecuteMainStage( stages + stageIndex, context, syncBits ); + switch ( stageType ) + { + case b2_stagePrepareJoints: + b2PrepareJointsTask( startIndex, endIndex, context ); + break; - profile->storeImpulses += b2GetMillisecondsAndReset( &timer ); + case b2_stagePrepareContacts: + b2PrepareContactsTask( startIndex, endIndex, context ); + break; - // Signal workers to finish - atomic_store( &context->atomicSyncBits, UINT_MAX ); + case b2_stageIntegrateVelocities: + b2IntegrateVelocitiesTask( startIndex, endIndex, context ); + break; - B2_ASSERT( stageIndex + 1 == context->stageCount ); - return; - } + case b2_stageWarmStart: + if ( context->world->enableWarmStarting ) + { + if ( blockType == b2_graphContactBlock ) + { + b2WarmStartContactsTask( startIndex, endIndex, context, stage->colorIndex ); + } + else if ( blockType == b2_graphJointBlock ) + { + b2WarmStartJointsTask( startIndex, endIndex, context, stage->colorIndex ); + } + } + break; - // Worker spins and waits for work - uint32_t lastSyncBits = 0; - // uint64_t maxSpinTime = 10; - while ( true ) - { - // Spin until main thread bumps changes the sync bits. This can waste significant time overall, but it is necessary for - // parallel simulation with graph coloring. - uint32_t syncBits; - int spinCount = 0; - while ( ( syncBits = atomic_load( &context->atomicSyncBits ) ) == lastSyncBits ) - { - if ( spinCount > 5 ) + case b2_stageSolve: + if ( blockType == b2_graphContactBlock ) { - b2Yield(); - spinCount = 0; + b2SolveContactsTask( startIndex, endIndex, context, stage->colorIndex, true ); } - else + else if ( blockType == b2_graphJointBlock ) { - // Using the cycle counter helps to account for variation in mm_pause timing across different - // CPUs. However, this is X64 only. - // uint64_t prev = __rdtsc(); - // do - //{ - // b2Pause(); - //} - // while ((__rdtsc() - prev) < maxSpinTime); - // maxSpinTime += 10; - b2Pause(); - b2Pause(); - spinCount += 1; + b2SolveJointsTask( startIndex, endIndex, context, stage->colorIndex, true ); } - } - - if ( syncBits == UINT_MAX ) - { - // sentinel hit break; - } - - int stageIndex = syncBits & 0xFFFF; - B2_ASSERT( stageIndex < context->stageCount ); - int syncIndex = ( syncBits >> 16 ) & 0xFFFF; - B2_ASSERT( syncIndex > 0 ); + case b2_stageIntegratePositions: + b2IntegratePositionsTask( startIndex, endIndex, context ); + break; - int previousSyncIndex = syncIndex - 1; + case b2_stageRelax: + if ( blockType == b2_graphContactBlock ) + { + b2SolveContactsTask( startIndex, endIndex, context, stage->colorIndex, false ); + } + else if ( blockType == b2_graphJointBlock ) + { + b2SolveJointsTask( startIndex, endIndex, context, stage->colorIndex, false ); + } + break; - b2SolverStage* stage = stages + stageIndex; - b2ExecuteStage( stage, context, previousSyncIndex, syncIndex, workerIndex ); + case b2_stageRestitution: + if ( blockType == b2_graphContactBlock ) + { + b2ApplyRestitutionTask( startIndex, endIndex, context, stage->colorIndex ); + } + break; - lastSyncBits = syncBits; + case b2_stageStoreImpulses: + b2StoreImpulsesTask( startIndex, endIndex, context ); + break; } } -struct b2ContinuousContext -{ - b2World* world; - b2BodySim* fastBodySim; - b2Shape* fastShape; - b2Vec2 centroid1, centroid2; - b2Sweep sweep; - float fraction; -}; - -// todo this may lead to pauses for scenarios where pre-solve would disable collision -static bool b2ContinuousQueryCallback( int proxyId, int shapeId, void* context ) +static inline int GetWorkerStartIndex( int workerIndex, int blockCount, int workerCount ) { - B2_MAYBE_UNUSED( proxyId ); - - struct b2ContinuousContext* continuousContext = context; - b2Shape* fastShape = continuousContext->fastShape; - b2BodySim* fastBodySim = continuousContext->fastBodySim; - - // Skip same shape - if ( shapeId == fastShape->id ) + if ( blockCount <= workerCount ) { - return true; + return workerIndex < blockCount ? workerIndex : B2_NULL_INDEX; } - b2World* world = continuousContext->world; - - b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + int blocksPerWorker = blockCount / workerCount; + int remainder = blockCount - blocksPerWorker * workerCount; + return blocksPerWorker * workerIndex + b2MinInt( remainder, workerIndex ); +} - // Skip same body - if ( shape->bodyId == fastShape->bodyId ) - { - return true; - } +static void b2ExecuteStage( b2SolverStage* stage, b2StepContext* context, int previousSyncIndex, int syncIndex, int workerIndex ) +{ + int completedCount = 0; + b2SolverBlock* blocks = stage->blocks; + int blockCount = stage->blockCount; - // Skip sensors - if ( shape->isSensor == true ) - { - return true; - } + int expectedSyncIndex = previousSyncIndex; - // Skip filtered shapes - bool canCollide = b2ShouldShapesCollide( fastShape->filter, shape->filter ); - if ( canCollide == false ) + int startIndex = GetWorkerStartIndex( workerIndex, blockCount, context->workerCount ); + if ( startIndex == B2_NULL_INDEX ) { - return true; + return; } - b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + B2_ASSERT( 0 <= startIndex && startIndex < blockCount ); - b2BodySim* bodySim = b2GetBodySim( world, body ); - B2_ASSERT( body->type == b2_staticBody || fastBodySim->isBullet ); + int blockIndex = startIndex; - // Skip bullets - if ( bodySim->isBullet ) + // Caution: this can change expectedSyncIndex + while ( atomic_compare_exchange_strong( &blocks[blockIndex].syncIndex, &expectedSyncIndex, syncIndex ) == true ) { - return true; - } + B2_ASSERT( stage->type != b2_stagePrepareContacts || syncIndex < 2 ); - // Skip filtered bodies - b2Body* fastBody = b2BodyArray_Get( &world->bodies, fastBodySim->bodyId ); - canCollide = b2ShouldBodiesCollide( world, fastBody, body ); - if ( canCollide == false ) - { - return true; - } + B2_ASSERT( completedCount < blockCount ); - // Custom user filtering - b2CustomFilterFcn* customFilterFcn = world->customFilterFcn; - if ( customFilterFcn != NULL ) - { - b2ShapeId idA = { shape->id + 1, world->worldId, shape->revision }; - b2ShapeId idB = { fastShape->id + 1, world->worldId, fastShape->revision }; - canCollide = customFilterFcn( idA, idB, world->customFilterContext ); - if ( canCollide == false ) + b2ExecuteBlock( stage, context, blocks + blockIndex ); + + completedCount += 1; + blockIndex += 1; + if ( blockIndex >= blockCount ) { - return true; + // Keep looking for work + blockIndex = 0; } + + expectedSyncIndex = previousSyncIndex; } - // Prevent pausing on chain segment junctions - if ( shape->type == b2_chainSegmentShape ) + // Search backwards for blocks + blockIndex = startIndex - 1; + while ( true ) { - b2Transform transform = bodySim->transform; - b2Vec2 p1 = b2TransformPoint( transform, shape->chainSegment.segment.point1 ); - b2Vec2 p2 = b2TransformPoint( transform, shape->chainSegment.segment.point2 ); - b2Vec2 e = b2Sub( p2, p1 ); - b2Vec2 c1 = continuousContext->centroid1; - b2Vec2 c2 = continuousContext->centroid2; - float offset1 = b2Cross( b2Sub( c1, p1 ), e ); - float offset2 = b2Cross( b2Sub( c2, p1 ), e ); + if ( blockIndex < 0 ) + { + blockIndex = blockCount - 1; + } - if ( offset1 < 0.0f || offset2 > 0.0f ) + expectedSyncIndex = previousSyncIndex; + + // Caution: this can change expectedSyncIndex + if ( atomic_compare_exchange_strong( &blocks[blockIndex].syncIndex, &expectedSyncIndex, syncIndex ) == false ) { - // Started behind or finished in front - return true; + break; } + + b2ExecuteBlock( stage, context, blocks + blockIndex ); + completedCount += 1; + blockIndex -= 1; } - b2TOIInput input; - input.proxyA = b2MakeShapeDistanceProxy( shape ); - input.proxyB = b2MakeShapeDistanceProxy( fastShape ); - input.sweepA = b2MakeSweep( bodySim ); - input.sweepB = continuousContext->sweep; - input.tMax = continuousContext->fraction; + (void)atomic_fetch_add( &stage->completionCount, completedCount ); +} - float hitFraction = continuousContext->fraction; +static void b2ExecuteMainStage( b2SolverStage* stage, b2StepContext* context, uint32_t syncBits ) +{ + int blockCount = stage->blockCount; + if ( blockCount == 0 ) + { + return; + } - bool didHit = false; - b2TOIOutput output = b2TimeOfImpact( &input ); - if ( 0.0f < output.t && output.t < continuousContext->fraction ) + if ( blockCount == 1 ) { - hitFraction = output.t; - didHit = true; + b2ExecuteBlock( stage, context, stage->blocks ); } - else if ( 0.0f == output.t ) + else { - // fallback to TOI of a small circle around the fast shape centroid - b2Vec2 centroid = b2GetShapeCentroid( fastShape ); - input.proxyB = b2MakeProxy( ¢roid, 1, b2_speculativeDistance ); - output = b2TimeOfImpact( &input ); - if ( 0.0f < output.t && output.t < continuousContext->fraction ) + atomic_store( &context->atomicSyncBits, syncBits ); + + int syncIndex = ( syncBits >> 16 ) & 0xFFFF; + B2_ASSERT( syncIndex > 0 ); + int previousSyncIndex = syncIndex - 1; + + b2ExecuteStage( stage, context, previousSyncIndex, syncIndex, 0 ); + + // todo consider using the cycle counter as well + while ( atomic_load( &stage->completionCount ) != blockCount ) { - hitFraction = output.t; - didHit = true; + b2Pause(); } + + atomic_store( &stage->completionCount, 0 ); } +} - if ( didHit && (shape->enablePreSolveEvents || fastShape->enablePreSolveEvents) ) - { - // Pre-solve is expensive because I need to compute a temporary manifold - b2Transform transformA = b2GetSweepTransform( &input.sweepA, hitFraction ); - b2Transform transformB = b2GetSweepTransform( &input.sweepB, hitFraction ); - b2Manifold manifold = b2ComputeManifold( shape, transformA, fastShape, transformB ); - b2ShapeId shapeIdA = { shape->id + 1, world->worldId, shape->revision }; - b2ShapeId shapeIdB = { fastShape->id + 1, world->worldId, fastShape->revision }; +// This should not use the thread index because thread 0 can be called twice by enkiTS. +void b2SolverTask( int startIndex, int endIndex, uint32_t threadIndexDontUse, void* taskContext ) +{ + B2_MAYBE_UNUSED( startIndex ); + B2_MAYBE_UNUSED( endIndex ); + B2_MAYBE_UNUSED( threadIndexDontUse ); - // The user may modify the temporary manifold here but it doesn't matter. They will be able to - // modify the real manifold in the discrete solver. - didHit = world->preSolveFcn( shapeIdA, shapeIdB, &manifold, world->preSolveContext ); - } + b2WorkerContext* workerContext = taskContext; + int workerIndex = workerContext->workerIndex; + b2StepContext* context = workerContext->context; + int activeColorCount = context->activeColorCount; + b2SolverStage* stages = context->stages; + b2Profile* profile = &context->world->profile; - if (didHit) + if ( workerIndex == 0 ) { - continuousContext->fraction = hitFraction; - } + // Main thread synchronizes the workers and does work itself. + // + // Stages are re-used by loops so that I don't need more stages for large iteration counts. + // The sync indices grow monotonically for the body/graph/constraint groupings because they share solver blocks. + // The stage index and sync indices are combined in to sync bits for atomic synchronization. + // The workers need to compute the previous sync index for a given stage so that CAS works correctly. This + // setup makes this easy to do. - return true; -} + /* + b2_stagePrepareJoints, + b2_stagePrepareContacts, + b2_stageIntegrateVelocities, + b2_stageWarmStart, + b2_stageSolve, + b2_stageIntegratePositions, + b2_stageRelax, + b2_stageRestitution, + b2_stageStoreImpulses + */ -// Continuous collision of dynamic versus static -static void b2SolveContinuous( b2World* world, int bodySimIndex ) -{ - b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); - b2BodySim* fastBodySim = b2BodySimArray_Get( &awakeSet->bodySims, bodySimIndex ); - B2_ASSERT( fastBodySim->isFast ); + b2Timer timer = b2CreateTimer(); - b2Sweep sweep = b2MakeSweep( fastBodySim ); + int bodySyncIndex = 1; + int stageIndex = 0; - b2Transform xf1; - xf1.q = sweep.q1; - xf1.p = b2Sub( sweep.c1, b2RotateVector( sweep.q1, sweep.localCenter ) ); + // This stage loops over all awake joints + uint32_t jointSyncIndex = 1; + uint32_t syncBits = ( jointSyncIndex << 16 ) | stageIndex; + B2_ASSERT( stages[stageIndex].type == b2_stagePrepareJoints ); + b2ExecuteMainStage( stages + stageIndex, context, syncBits ); + stageIndex += 1; + jointSyncIndex += 1; - b2Transform xf2; - xf2.q = sweep.q2; - xf2.p = b2Sub( sweep.c2, b2RotateVector( sweep.q2, sweep.localCenter ) ); + // This stage loops over all contact constraints + uint32_t contactSyncIndex = 1; + syncBits = ( contactSyncIndex << 16 ) | stageIndex; + B2_ASSERT( stages[stageIndex].type == b2_stagePrepareContacts ); + b2ExecuteMainStage( stages + stageIndex, context, syncBits ); + stageIndex += 1; + contactSyncIndex += 1; - b2DynamicTree* staticTree = world->broadPhase.trees + b2_staticBody; - b2DynamicTree* kinematicTree = world->broadPhase.trees + b2_kinematicBody; - b2DynamicTree* dynamicTree = world->broadPhase.trees + b2_dynamicBody; - b2Body* fastBody = b2BodyArray_Get( &world->bodies, fastBodySim->bodyId ); + int graphSyncIndex = 1; - struct b2ContinuousContext context; - context.world = world; - context.sweep = sweep; - context.fastBodySim = fastBodySim; - context.fraction = 1.0f; + // Single-threaded overflow work. These constraints don't fit in the graph coloring. + b2PrepareOverflowJoints( context ); + b2PrepareOverflowContacts( context ); - bool isBullet = fastBodySim->isBullet; + profile->prepareConstraints += b2GetMillisecondsAndReset( &timer ); - int shapeId = fastBody->headShapeId; - while ( shapeId != B2_NULL_INDEX ) - { - b2Shape* fastShape = b2ShapeArray_Get( &world->shapes, shapeId ); - B2_ASSERT( fastShape->isFast == true ); + int subStepCount = context->subStepCount; + for ( int i = 0; i < subStepCount; ++i ) + { + // stage index restarted each iteration + // syncBits still increases monotonically because the upper bits increase each iteration + int iterStageIndex = stageIndex; - shapeId = fastShape->nextShapeId; + // integrate velocities + syncBits = ( bodySyncIndex << 16 ) | iterStageIndex; + B2_ASSERT( stages[iterStageIndex].type == b2_stageIntegrateVelocities ); + b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); + iterStageIndex += 1; + bodySyncIndex += 1; + + profile->integrateVelocities += b2GetMillisecondsAndReset( &timer ); + + // warm start constraints + b2WarmStartOverflowJoints( context ); + b2WarmStartOverflowContacts( context ); + + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterStageIndex; + B2_ASSERT( stages[iterStageIndex].type == b2_stageWarmStart ); + b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); + iterStageIndex += 1; + } + graphSyncIndex += 1; + + profile->warmStart += b2GetMillisecondsAndReset( &timer ); + + // solve constraints + bool useBias = true; + b2SolveOverflowJoints( context, useBias ); + b2SolveOverflowContacts( context, useBias ); + + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterStageIndex; + B2_ASSERT( stages[iterStageIndex].type == b2_stageSolve ); + b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); + iterStageIndex += 1; + } + graphSyncIndex += 1; + + profile->solveVelocities += b2GetMillisecondsAndReset( &timer ); - // Clear flag (keep set on body) - fastShape->isFast = false; + // integrate positions + B2_ASSERT( stages[iterStageIndex].type == b2_stageIntegratePositions ); + syncBits = ( bodySyncIndex << 16 ) | iterStageIndex; + b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); + iterStageIndex += 1; + bodySyncIndex += 1; - context.fastShape = fastShape; - context.centroid1 = b2TransformPoint( xf1, fastShape->localCentroid ); - context.centroid2 = b2TransformPoint( xf2, fastShape->localCentroid ); + profile->integratePositions += b2GetMillisecondsAndReset( &timer ); - b2AABB box1 = fastShape->aabb; - b2AABB box2 = b2ComputeShapeAABB( fastShape, xf2 ); - b2AABB box = b2AABB_Union( box1, box2 ); + // relax constraints + useBias = false; + b2SolveOverflowJoints( context, useBias ); + b2SolveOverflowContacts( context, useBias ); - // Store this for later - fastShape->aabb = box2; + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterStageIndex; + B2_ASSERT( stages[iterStageIndex].type == b2_stageRelax ); + b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); + iterStageIndex += 1; + } + graphSyncIndex += 1; - // No continuous collision for sensors - if ( fastShape->isSensor ) - { - continue; + profile->relaxVelocities += b2GetMillisecondsAndReset( &timer ); } - b2DynamicTree_Query( staticTree, box, b2_defaultMaskBits, b2ContinuousQueryCallback, &context ); + // advance the stage according to the sub-stepping tasks just completed + // integrate velocities / warm start / solve / integrate positions / relax + stageIndex += 1 + activeColorCount + activeColorCount + 1 + activeColorCount; - if ( isBullet ) + // Restitution { - b2DynamicTree_Query( kinematicTree, box, b2_defaultMaskBits, b2ContinuousQueryCallback, &context ); - b2DynamicTree_Query( dynamicTree, box, b2_defaultMaskBits, b2ContinuousQueryCallback, &context ); - } - } - - const float speculativeDistance = b2_speculativeDistance; - const float aabbMargin = b2_aabbMargin; + b2ApplyOverflowRestitution( context ); - if ( context.fraction < 1.0f ) - { - // Handle time of impact event - b2Rot q = b2NLerp( sweep.q1, sweep.q2, context.fraction ); - b2Vec2 c = b2Lerp( sweep.c1, sweep.c2, context.fraction ); - b2Vec2 origin = b2Sub( c, b2RotateVector( q, sweep.localCenter ) ); + int iterStageIndex = stageIndex; + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterStageIndex; + B2_ASSERT( stages[iterStageIndex].type == b2_stageRestitution ); + b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); + iterStageIndex += 1; + } + // graphSyncIndex += 1; + stageIndex += activeColorCount; + } - // Advance body - b2Transform transform = { origin, q }; - fastBodySim->transform = transform; - fastBodySim->center = c; - fastBodySim->rotation0 = q; - fastBodySim->center0 = c; + profile->applyRestitution += b2GetMillisecondsAndReset( &timer ); - // Prepare AABBs for broad-phase - shapeId = fastBody->headShapeId; - while ( shapeId != B2_NULL_INDEX ) - { - b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + b2StoreOverflowImpulses( context ); - // Must recompute aabb at the interpolated transform - b2AABB aabb = b2ComputeShapeAABB( shape, transform ); - aabb.lowerBound.x -= speculativeDistance; - aabb.lowerBound.y -= speculativeDistance; - aabb.upperBound.x += speculativeDistance; - aabb.upperBound.y += speculativeDistance; - shape->aabb = aabb; + syncBits = ( contactSyncIndex << 16 ) | stageIndex; + B2_ASSERT( stages[stageIndex].type == b2_stageStoreImpulses ); + b2ExecuteMainStage( stages + stageIndex, context, syncBits ); - if ( b2AABB_Contains( shape->fatAABB, aabb ) == false ) - { - b2AABB fatAABB; - fatAABB.lowerBound.x = aabb.lowerBound.x - aabbMargin; - fatAABB.lowerBound.y = aabb.lowerBound.y - aabbMargin; - fatAABB.upperBound.x = aabb.upperBound.x + aabbMargin; - fatAABB.upperBound.y = aabb.upperBound.y + aabbMargin; - shape->fatAABB = fatAABB; + profile->storeImpulses += b2GetMillisecondsAndReset( &timer ); - shape->enlargedAABB = true; - fastBodySim->enlargeAABB = true; - } + // Signal workers to finish + atomic_store( &context->atomicSyncBits, UINT_MAX ); - shapeId = shape->nextShapeId; - } + B2_ASSERT( stageIndex + 1 == context->stageCount ); + return; } - else - { - // No time of impact event - - // Advance body - fastBodySim->rotation0 = fastBodySim->transform.q; - fastBodySim->center0 = fastBodySim->center; - // Prepare AABBs for broad-phase - shapeId = fastBody->headShapeId; - while ( shapeId != B2_NULL_INDEX ) + // Worker spins and waits for work + uint32_t lastSyncBits = 0; + // uint64_t maxSpinTime = 10; + while ( true ) + { + // Spin until main thread bumps changes the sync bits. This can waste significant time overall, but it is necessary for + // parallel simulation with graph coloring. + uint32_t syncBits; + int spinCount = 0; + while ( ( syncBits = atomic_load( &context->atomicSyncBits ) ) == lastSyncBits ) { - b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); - - // shape->aabb is still valid - - if ( b2AABB_Contains( shape->fatAABB, shape->aabb ) == false ) + if ( spinCount > 5 ) { - b2AABB fatAABB; - fatAABB.lowerBound.x = shape->aabb.lowerBound.x - aabbMargin; - fatAABB.lowerBound.y = shape->aabb.lowerBound.y - aabbMargin; - fatAABB.upperBound.x = shape->aabb.upperBound.x + aabbMargin; - fatAABB.upperBound.y = shape->aabb.upperBound.y + aabbMargin; - shape->fatAABB = fatAABB; - - shape->enlargedAABB = true; - fastBodySim->enlargeAABB = true; + b2Yield(); + spinCount = 0; + } + else + { + // Using the cycle counter helps to account for variation in mm_pause timing across different + // CPUs. However, this is X64 only. + // uint64_t prev = __rdtsc(); + // do + //{ + // b2Pause(); + //} + // while ((__rdtsc() - prev) < maxSpinTime); + // maxSpinTime += 10; + b2Pause(); + b2Pause(); + spinCount += 1; } + } - shapeId = shape->nextShapeId; + if ( syncBits == UINT_MAX ) + { + // sentinel hit + break; } - } -} -static void b2FastBodyTask( int startIndex, int endIndex, uint32_t threadIndex, void* taskContext ) -{ - B2_MAYBE_UNUSED( threadIndex ); + int stageIndex = syncBits & 0xFFFF; + B2_ASSERT( stageIndex < context->stageCount ); - b2TracyCZoneNC( fast_body_task, "Fast Body Task", b2_colorCyan, true ); + int syncIndex = ( syncBits >> 16 ) & 0xFFFF; + B2_ASSERT( syncIndex > 0 ); - b2StepContext* stepContext = taskContext; + int previousSyncIndex = syncIndex - 1; - B2_ASSERT( startIndex <= endIndex ); + b2SolverStage* stage = stages + stageIndex; + b2ExecuteStage( stage, context, previousSyncIndex, syncIndex, workerIndex ); - for ( int i = startIndex; i < endIndex; ++i ) - { - int simIndex = stepContext->fastBodies[i]; - b2SolveContinuous( stepContext->world, simIndex ); + lastSyncBits = syncBits; } - - b2TracyCZoneEnd( fast_body_task ); } static void b2BulletBodyTask( int startIndex, int endIndex, uint32_t threadIndex, void* taskContext ) @@ -1179,11 +1160,9 @@ void b2Solve( b2World* world, b2StepContext* stepContext ) return; } - b2TracyCZoneNC( solve, "Solve", b2_colorMistyRose, true ); + b2TracyCZoneNC( solve, "Solve", b2_colorIndigo, true ); // Prepare buffers for continuous collision (fast bodies) - stepContext->fastBodyCount = 0; - stepContext->fastBodies = b2AllocateStackItem( &world->stackAllocator, awakeBodyCount * sizeof( int ), "fast bodies" ); stepContext->bulletBodyCount = 0; stepContext->bulletBodies = b2AllocateStackItem( &world->stackAllocator, awakeBodyCount * sizeof( int ), "bullet bodies" ); @@ -1790,19 +1769,20 @@ void b2Solve( b2World* world, b2StepContext* stepContext ) b2TracyCZoneNC( enlarge_proxies, "Enlarge Proxies", b2_colorDarkTurquoise, true ); // Gather bits for all sim bodies that have enlarged AABBs - b2BitSet* simBitSet = &world->taskContexts.data[0].enlargedSimBitSet; + b2BitSet* enlargedBodyBitSet = &world->taskContexts.data[0].enlargedSimBitSet; for ( int i = 1; i < world->workerCount; ++i ) { - b2InPlaceUnion( simBitSet, &world->taskContexts.data[i].enlargedSimBitSet ); + b2InPlaceUnion( enlargedBodyBitSet, &world->taskContexts.data[i].enlargedSimBitSet ); } // Enlarge broad-phase proxies and build move array // Apply shape AABB changes to broad-phase. This also create the move array which must be // in deterministic order. I'm tracking sim bodies because the number of shape ids can be huge. + // This has to happen before bullets are processed. { b2BroadPhase* broadPhase = &world->broadPhase; - uint32_t wordCount = simBitSet->blockCount; - uint64_t* bits = simBitSet->bits; + uint32_t wordCount = enlargedBodyBitSet->blockCount; + uint64_t* bits = enlargedBodyBitSet->bits; // Fast array access is important here b2Body* bodyArray = world->bodies.data; @@ -1818,28 +1798,42 @@ void b2Solve( b2World* world, b2StepContext* stepContext ) uint32_t bodySimIndex = 64 * k + ctz; b2BodySim* bodySim = bodySimArray + bodySimIndex; + b2Body* body = bodyArray + bodySim->bodyId; int shapeId = body->headShapeId; - while ( shapeId != B2_NULL_INDEX ) + if (bodySim->isBullet && bodySim->isFast) { - b2Shape* shape = shapeArray + shapeId; - - if ( shape->enlargedAABB ) + // Fast bullet bodies don't have their final AABB yet + while ( shapeId != B2_NULL_INDEX ) { - B2_ASSERT( shape->isFast == false ); + b2Shape* shape = shapeArray + shapeId; - b2BroadPhase_EnlargeProxy( broadPhase, shape->proxyKey, shape->fatAABB ); - shape->enlargedAABB = false; - } - else if ( shape->isFast ) - { // Shape is fast. It's aabb will be enlarged in continuous collision. - // todo_erin can this be deferred? This breaks AABB extension benefits. + // Update the move array here for determinism because bullets are processed + // below in non-deterministic order. b2BufferMove( broadPhase, shape->proxyKey ); - } - shapeId = shape->nextShapeId; + shapeId = shape->nextShapeId; + } + } + else + { + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = shapeArray + shapeId; + + // The AABB may not have been enlarged, despite the body being flagged as enlarged. + // For example, a body with multiple shapes may have not have all shapes enlarged. + // A fast body may have been flagged as enlarged despite having no shapes enlarged. + if ( shape->enlargedAABB ) + { + b2BroadPhase_EnlargeProxy( broadPhase, shape->proxyKey, shape->fatAABB ); + shape->enlargedAABB = false; + } + + shapeId = shape->nextShapeId; + } } // Clear the smallest set bit @@ -1848,95 +1842,19 @@ void b2Solve( b2World* world, b2StepContext* stepContext ) } } - b2TracyCZoneEnd( enlarge_proxies ); - b2ValidateBroadphase( &world->broadPhase ); world->profile.broadphase = b2GetMillisecondsAndReset( &timer ); + b2TracyCZoneEnd( enlarge_proxies ); b2TracyCZoneEnd( broad_phase ); - b2TracyCZoneNC( continuous_collision, "Continuous", b2_colorDarkGoldenrod, true ); - - // Parallel continuous collision - if ( stepContext->fastBodyCount > 0 ) - { - // fast bodies - int minRange = 8; - void* userFastBodyTask = - world->enqueueTaskFcn( &b2FastBodyTask, stepContext->fastBodyCount, minRange, stepContext, world->userTaskContext ); - world->taskCount += 1; - if ( userFastBodyTask != NULL ) - { - world->finishTaskFcn( userFastBodyTask, world->userTaskContext ); - } - } - - // Serially enlarge broad-phase proxies for fast shapes - // Doing this here so that bullet shapes see them - { - b2TracyCZoneNC( continuous_enlarge, "Enlarge Proxies", b2_colorDarkTurquoise, true ); - - b2BroadPhase* broadPhase = &world->broadPhase; - b2DynamicTree* dynamicTree = broadPhase->trees + b2_dynamicBody; - - // Fast array access is important here - b2Body* bodyArray = world->bodies.data; - b2BodySim* bodySimArray = awakeSet->bodySims.data; - b2Shape* shapeArray = world->shapes.data; - - int* fastBodySimIndices = stepContext->fastBodies; - int fastBodyCount = stepContext->fastBodyCount; - - // This loop has non-deterministic order but it shouldn't affect the result - for ( int i = 0; i < fastBodyCount; ++i ) - { - b2BodySim* fastBodySim = bodySimArray + fastBodySimIndices[i]; - if ( fastBodySim->enlargeAABB == false ) - { - continue; - } - - // clear flag - fastBodySim->enlargeAABB = false; - - int bodyId = fastBodySim->bodyId; - - B2_ASSERT( 0 <= bodyId && bodyId < world->bodies.count ); - b2Body* fastBody = bodyArray + bodyId; - - int shapeId = fastBody->headShapeId; - while ( shapeId != B2_NULL_INDEX ) - { - b2Shape* shape = shapeArray + shapeId; - if ( shape->enlargedAABB == false ) - { - shapeId = shape->nextShapeId; - continue; - } - - // clear flag - shape->enlargedAABB = false; - - int proxyKey = shape->proxyKey; - int proxyId = B2_PROXY_ID( proxyKey ); - B2_ASSERT( B2_PROXY_TYPE( proxyKey ) == b2_dynamicBody ); - - // all fast shapes should already be in the move buffer - B2_ASSERT( b2ContainsKey( &broadPhase->moveSet, proxyKey + 1 ) ); - - b2DynamicTree_EnlargeProxy( dynamicTree, proxyId, shape->fatAABB ); - - shapeId = shape->nextShapeId; - } - } - - b2TracyCZoneEnd( continuous_enlarge ); - } - if ( stepContext->bulletBodyCount > 0 ) { - // bullet bodies + b2TracyCZoneNC( bullets, "Bullets", b2_colorDarkGoldenrod, true ); + + // Fast bullet bodies + // Note: a bullet body may be moving slow int minRange = 8; void* userBulletBodyTask = world->enqueueTaskFcn( &b2BulletBodyTask, stepContext->bulletBodyCount, minRange, stepContext, world->userTaskContext ); @@ -1945,10 +1863,15 @@ void b2Solve( b2World* world, b2StepContext* stepContext ) { world->finishTaskFcn( userBulletBodyTask, world->userTaskContext ); } + + b2TracyCZoneEnd( bullets ); } // Serially enlarge broad-phase proxies for bullet shapes { + b2TracyCZoneNC( broad_phase_bullet, "Broadphase", b2_colorPurple, true ); + b2TracyCZoneNC( enlarge_proxies_bullet, "Enlarge Proxies", b2_colorDarkTurquoise, true ); + b2BroadPhase* broadPhase = &world->broadPhase; b2DynamicTree* dynamicTree = broadPhase->trees + b2_dynamicBody; @@ -1994,7 +1917,7 @@ void b2Solve( b2World* world, b2StepContext* stepContext ) int proxyId = B2_PROXY_ID( proxyKey ); B2_ASSERT( B2_PROXY_TYPE( proxyKey ) == b2_dynamicBody ); - // all fast shapes should already be in the move buffer + // all fast bullet shapes should already be in the move buffer B2_ASSERT( b2ContainsKey( &broadPhase->moveSet, proxyKey + 1 ) ); b2DynamicTree_EnlargeProxy( dynamicTree, proxyId, shape->fatAABB ); @@ -2002,18 +1925,15 @@ void b2Solve( b2World* world, b2StepContext* stepContext ) shapeId = shape->nextShapeId; } } - } - b2TracyCZoneEnd( continuous_collision ); + b2TracyCZoneEnd( enlarge_proxies_bullet ); + b2TracyCZoneEnd( broad_phase_bullet ); + } b2FreeStackItem( &world->stackAllocator, stepContext->bulletBodies ); stepContext->bulletBodies = NULL; stepContext->bulletBodyCount = 0; - b2FreeStackItem( &world->stackAllocator, stepContext->fastBodies ); - stepContext->fastBodies = NULL; - stepContext->fastBodyCount = 0; - world->profile.continuous = b2GetMillisecondsAndReset( &timer ); // Island sleeping diff --git a/src/solver.h b/src/solver.h index 7d5cedd22..a3a8609a2 100644 --- a/src/solver.h +++ b/src/solver.h @@ -104,10 +104,6 @@ typedef struct b2StepContext int* enlargedShapes; int enlargedShapeCount; - // Array of fast bodies that need continuous collision handling - int* fastBodies; - _Atomic int fastBodyCount; - // Array of bullet bodies that need continuous collision handling int* bulletBodies; _Atomic int bulletBodyCount; diff --git a/src/timer.c b/src/timer.c index fe2c53d6f..383a9f038 100644 --- a/src/timer.c +++ b/src/timer.c @@ -76,7 +76,7 @@ void b2Yield() SwitchToThread(); } -#elif defined( __linux__ ) || defined( __APPLE__ ) +#elif defined( __linux__ ) || defined( __APPLE__ ) || defined( __EMSCRIPTEN__ ) #include #include diff --git a/src/types.c b/src/types.c index 1d7c1f03f..2cb28e964 100644 --- a/src/types.c +++ b/src/types.c @@ -3,6 +3,7 @@ #include "box2d/types.h" +#include "constants.h" #include "core.h" b2WorldDef b2DefaultWorldDef( void ) diff --git a/src/weld_joint.c b/src/weld_joint.c index 84bcff0a1..15f9b3344 100644 --- a/src/weld_joint.c +++ b/src/weld_joint.c @@ -19,14 +19,14 @@ float b2WeldJoint_GetReferenceAngle( b2JointId jointId ) void b2WeldJoint_SetReferenceAngle( b2JointId jointId, float angleInRadians ) { - B2_ASSERT( b2Float_IsValid( angleInRadians ) ); + B2_ASSERT( b2IsValidFloat( angleInRadians ) ); b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); joint->weldJoint.referenceAngle = b2ClampFloat(angleInRadians, -b2_pi, b2_pi); } void b2WeldJoint_SetLinearHertz( b2JointId jointId, float hertz ) { - B2_ASSERT( b2Float_IsValid( hertz ) && hertz >= 0.0f ); + B2_ASSERT( b2IsValidFloat( hertz ) && hertz >= 0.0f ); b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); joint->weldJoint.linearHertz = hertz; } @@ -39,7 +39,7 @@ float b2WeldJoint_GetLinearHertz( b2JointId jointId ) void b2WeldJoint_SetLinearDampingRatio( b2JointId jointId, float dampingRatio ) { - B2_ASSERT( b2Float_IsValid( dampingRatio ) && dampingRatio >= 0.0f ); + B2_ASSERT( b2IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f ); b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); joint->weldJoint.linearDampingRatio = dampingRatio; } @@ -52,7 +52,7 @@ float b2WeldJoint_GetLinearDampingRatio( b2JointId jointId ) void b2WeldJoint_SetAngularHertz( b2JointId jointId, float hertz ) { - B2_ASSERT( b2Float_IsValid( hertz ) && hertz >= 0.0f ); + B2_ASSERT( b2IsValidFloat( hertz ) && hertz >= 0.0f ); b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); joint->weldJoint.angularHertz = hertz; } @@ -65,7 +65,7 @@ float b2WeldJoint_GetAngularHertz( b2JointId jointId ) void b2WeldJoint_SetAngularDampingRatio( b2JointId jointId, float dampingRatio ) { - B2_ASSERT( b2Float_IsValid( dampingRatio ) && dampingRatio >= 0.0f ); + B2_ASSERT( b2IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f ); b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); joint->weldJoint.angularDampingRatio = dampingRatio; } diff --git a/src/world.c b/src/world.c index 4d0fa0c8f..3302a385d 100644 --- a/src/world.c +++ b/src/world.c @@ -12,6 +12,7 @@ #include "bitset.h" #include "body.h" #include "broad_phase.h" +#include "constants.h" #include "constraint_graph.h" #include "contact.h" #include "core.h" @@ -710,7 +711,7 @@ void b2World_Step( b2WorldId worldId, float timeStep, int subStepCount ) return; } - b2TracyCZoneNC( world_step, "Step", b2_colorChartreuse, true ); + b2TracyCZoneNC( world_step, "Step", b2_colorBox2DGreen, true ); world->locked = true; world->activeTaskCount = 0; @@ -939,7 +940,7 @@ static bool DrawQueryCallback( int proxyId, int shapeId, void* context ) // solution: display order by shape id modulus 3, keep 3 buckets in GLSolid* and flush in 3 passes. static void b2DrawWithBounds( b2World* world, b2DebugDraw* draw ) { - B2_ASSERT( b2AABB_IsValid( draw->drawingBounds ) ); + B2_ASSERT( b2IsValidAABB( draw->drawingBounds ) ); const float k_impulseScale = 1.0f; const float k_axisScale = 0.3f; @@ -1647,6 +1648,13 @@ bool b2World_IsWarmStartingEnabled( b2WorldId worldId ) return world->enableWarmStarting; } +int b2World_GetAwakeBodyCount(b2WorldId worldId) +{ + b2World* world = b2GetWorldFromId( worldId ); + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + return awakeSet->bodySims.count; +} + void b2World_EnableContinuous( b2WorldId worldId, bool flag ) { b2World* world = b2GetWorldFromId( worldId ); @@ -1730,7 +1738,7 @@ void b2World_SetJointTuning( b2WorldId worldId, float hertz, float dampingRatio void b2World_SetMaximumLinearVelocity( b2WorldId worldId, float maximumLinearVelocity ) { - B2_ASSERT( b2Float_IsValid( maximumLinearVelocity ) && maximumLinearVelocity > 0.0f ); + B2_ASSERT( b2IsValidFloat( maximumLinearVelocity ) && maximumLinearVelocity > 0.0f ); b2World* world = b2GetWorldFromId( worldId ); B2_ASSERT( world->locked == false ); @@ -1936,7 +1944,7 @@ b2TreeStats b2World_OverlapAABB( b2WorldId worldId, b2AABB aabb, b2QueryFilter f return treeStats; } - B2_ASSERT( b2AABB_IsValid( aabb ) ); + B2_ASSERT( b2IsValidAABB( aabb ) ); WorldQueryContext worldContext = { world, fcn, filter, context }; @@ -2021,8 +2029,8 @@ b2TreeStats b2World_OverlapCircle( b2WorldId worldId, const b2Circle* circle, b2 return treeStats; } - B2_ASSERT( b2Vec2_IsValid( transform.p ) ); - B2_ASSERT( b2Rot_IsValid( transform.q ) ); + B2_ASSERT( b2IsValidVec2( transform.p ) ); + B2_ASSERT( b2IsValidRotation( transform.q ) ); b2AABB aabb = b2ComputeCircleAABB( circle, transform ); WorldOverlapContext worldContext = { @@ -2053,8 +2061,8 @@ b2TreeStats b2World_OverlapCapsule( b2WorldId worldId, const b2Capsule* capsule, return treeStats; } - B2_ASSERT( b2Vec2_IsValid( transform.p ) ); - B2_ASSERT( b2Rot_IsValid( transform.q ) ); + B2_ASSERT( b2IsValidVec2( transform.p ) ); + B2_ASSERT( b2IsValidRotation( transform.q ) ); b2AABB aabb = b2ComputeCapsuleAABB( capsule, transform ); WorldOverlapContext worldContext = { @@ -2085,8 +2093,8 @@ b2TreeStats b2World_OverlapPolygon( b2WorldId worldId, const b2Polygon* polygon, return treeStats; } - B2_ASSERT( b2Vec2_IsValid( transform.p ) ); - B2_ASSERT( b2Rot_IsValid( transform.q ) ); + B2_ASSERT( b2IsValidVec2( transform.p ) ); + B2_ASSERT( b2IsValidRotation( transform.q ) ); b2AABB aabb = b2ComputePolygonAABB( polygon, transform ); WorldOverlapContext worldContext = { @@ -2157,8 +2165,8 @@ b2TreeStats b2World_CastRay( b2WorldId worldId, b2Vec2 origin, b2Vec2 translatio return treeStats; } - B2_ASSERT( b2Vec2_IsValid( origin ) ); - B2_ASSERT( b2Vec2_IsValid( translation ) ); + B2_ASSERT( b2IsValidVec2( origin ) ); + B2_ASSERT( b2IsValidVec2( translation ) ); b2RayCastInput input = { origin, translation, 1.0f }; @@ -2205,8 +2213,8 @@ b2RayResult b2World_CastRayClosest( b2WorldId worldId, b2Vec2 origin, b2Vec2 tra return result; } - B2_ASSERT( b2Vec2_IsValid( origin ) ); - B2_ASSERT( b2Vec2_IsValid( translation ) ); + B2_ASSERT( b2IsValidVec2( origin ) ); + B2_ASSERT( b2IsValidVec2( translation ) ); b2RayCastInput input = { origin, translation, 1.0f }; WorldRayCastContext worldContext = { world, b2RayCastClosestFcn, filter, 1.0f, &result }; @@ -2273,9 +2281,9 @@ b2TreeStats b2World_CastCircle( b2WorldId worldId, const b2Circle* circle, b2Tra return treeStats; } - B2_ASSERT( b2Vec2_IsValid( originTransform.p ) ); - B2_ASSERT( b2Rot_IsValid( originTransform.q ) ); - B2_ASSERT( b2Vec2_IsValid( translation ) ); + B2_ASSERT( b2IsValidVec2( originTransform.p ) ); + B2_ASSERT( b2IsValidRotation( originTransform.q ) ); + B2_ASSERT( b2IsValidVec2( translation ) ); b2ShapeCastInput input; input.points[0] = b2TransformPoint( originTransform, circle->center ); @@ -2316,9 +2324,9 @@ b2TreeStats b2World_CastCapsule( b2WorldId worldId, const b2Capsule* capsule, b2 return treeStats; } - B2_ASSERT( b2Vec2_IsValid( originTransform.p ) ); - B2_ASSERT( b2Rot_IsValid( originTransform.q ) ); - B2_ASSERT( b2Vec2_IsValid( translation ) ); + B2_ASSERT( b2IsValidVec2( originTransform.p ) ); + B2_ASSERT( b2IsValidRotation( originTransform.q ) ); + B2_ASSERT( b2IsValidVec2( translation ) ); b2ShapeCastInput input; input.points[0] = b2TransformPoint( originTransform, capsule->center1 ); @@ -2360,9 +2368,9 @@ b2TreeStats b2World_CastPolygon( b2WorldId worldId, const b2Polygon* polygon, b2 return treeStats; } - B2_ASSERT( b2Vec2_IsValid( originTransform.p ) ); - B2_ASSERT( b2Rot_IsValid( originTransform.q ) ); - B2_ASSERT( b2Vec2_IsValid( translation ) ); + B2_ASSERT( b2IsValidVec2( originTransform.p ) ); + B2_ASSERT( b2IsValidRotation( originTransform.q ) ); + B2_ASSERT( b2IsValidVec2( translation ) ); b2ShapeCastInput input; for ( int i = 0; i < polygon->count; ++i ) @@ -2603,10 +2611,10 @@ void b2World_Explode( b2WorldId worldId, const b2ExplosionDef* explosionDef ) float falloff = explosionDef->falloff; float impulsePerLength = explosionDef->impulsePerLength; - B2_ASSERT( b2Vec2_IsValid( position ) ); - B2_ASSERT( b2Float_IsValid( radius ) && radius >= 0.0f ); - B2_ASSERT( b2Float_IsValid( falloff ) && falloff >= 0.0f ); - B2_ASSERT( b2Float_IsValid( impulsePerLength ) ); + B2_ASSERT( b2IsValidVec2( position ) ); + B2_ASSERT( b2IsValidFloat( radius ) && radius >= 0.0f ); + B2_ASSERT( b2IsValidFloat( falloff ) && falloff >= 0.0f ); + B2_ASSERT( b2IsValidFloat( impulsePerLength ) ); b2World* world = b2GetWorldFromId( worldId ); B2_ASSERT( world->locked == false ); diff --git a/test/test_collision.c b/test/test_collision.c index cf980bb6b..60ae3f258 100644 --- a/test/test_collision.c +++ b/test/test_collision.c @@ -12,10 +12,10 @@ static int AABBTest( void ) a.lowerBound = ( b2Vec2 ){ -1.0f, -1.0f }; a.upperBound = ( b2Vec2 ){ -2.0f, -2.0f }; - ENSURE( b2AABB_IsValid( a ) == false ); + ENSURE( b2IsValidAABB( a ) == false ); a.upperBound = ( b2Vec2 ){ 1.0f, 1.0f }; - ENSURE( b2AABB_IsValid( a ) == true ); + ENSURE( b2IsValidAABB( a ) == true ); b2AABB b = { { 2.0f, 2.0f }, { 4.0f, 4.0f } }; ENSURE( b2AABB_Overlaps( a, b ) == false ); diff --git a/test/test_determinism.c b/test/test_determinism.c index c12df870b..24585a2c7 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -289,29 +289,24 @@ static int CrossPlatformTest(void) uint32_t hash = 0; int sleepStep = -1; float timeStep = 1.0f / 60.0f; - int subStepCount = 4; int stepCount = 0; int maxSteps = 500; while ( stepCount < maxSteps ) { + int subStepCount = 4; b2World_Step( worldId, timeStep, subStepCount ); TracyCFrameMark; if ( hash == 0 ) { - bool sleeping = true; - for ( int i = 0; i < bodyCount; ++i ) - { - if ( b2Body_IsAwake( bodies[i] ) == true ) - { - sleeping = false; - break; - } - } + b2BodyEvents bodyEvents = b2World_GetBodyEvents( worldId ); - if ( sleeping == true ) + if ( bodyEvents.moveCount == 0 ) { + int awakeCount = b2World_GetAwakeBodyCount( worldId ); + ENSURE( awakeCount == 0 ); + hash = B2_HASH_INIT; for ( int i = 0; i < bodyCount; ++i ) { @@ -330,7 +325,7 @@ static int CrossPlatformTest(void) } ENSURE( stepCount < maxSteps ); - ENSURE( sleepStep == 281 ); + ENSURE( sleepStep == 282 ); ENSURE( hash == 0x7efc22e7 ); free( bodies ); diff --git a/test/test_math.c b/test/test_math.c index 47047926e..63fc55fe6 100644 --- a/test/test_math.c +++ b/test/test_math.c @@ -27,7 +27,7 @@ int MathTest( void ) float xn = b2UnwindLargeAngle( angle ); float a = b2Atan2( s, c ); - ENSURE( b2Float_IsValid( a ) ); + ENSURE( b2IsValidFloat( a ) ); float diff = b2AbsFloat( a - xn ); @@ -48,7 +48,7 @@ int MathTest( void ) float a1 = b2Atan2( y, x ); float a2 = atan2f( y, x ); float diff = b2AbsFloat( a1 - a2 ); - ENSURE( b2Float_IsValid( a1 ) ); + ENSURE( b2IsValidFloat( a1 ) ); ENSURE_SMALL( diff, ATAN_TOL ); } } @@ -57,7 +57,7 @@ int MathTest( void ) float a1 = b2Atan2( 1.0f, 0.0f ); float a2 = atan2f( 1.0f, 0.0f ); float diff = b2AbsFloat( a1 - a2 ); - ENSURE( b2Float_IsValid( a1 ) ); + ENSURE( b2IsValidFloat( a1 ) ); ENSURE_SMALL( diff, ATAN_TOL ); } @@ -65,7 +65,7 @@ int MathTest( void ) float a1 = b2Atan2( -1.0f, 0.0f ); float a2 = atan2f( -1.0f, 0.0f ); float diff = b2AbsFloat( a1 - a2 ); - ENSURE( b2Float_IsValid( a1 ) ); + ENSURE( b2IsValidFloat( a1 ) ); ENSURE_SMALL( diff, ATAN_TOL ); } @@ -73,7 +73,7 @@ int MathTest( void ) float a1 = b2Atan2( 0.0f, 1.0f ); float a2 = atan2f( 0.0f, 1.0f ); float diff = b2AbsFloat( a1 - a2 ); - ENSURE( b2Float_IsValid( a1 ) ); + ENSURE( b2IsValidFloat( a1 ) ); ENSURE_SMALL( diff, ATAN_TOL ); } @@ -81,7 +81,7 @@ int MathTest( void ) float a1 = b2Atan2( 0.0f, -1.0f ); float a2 = atan2f( 0.0f, -1.0f ); float diff = b2AbsFloat( a1 - a2 ); - ENSURE( b2Float_IsValid( a1 ) ); + ENSURE( b2IsValidFloat( a1 ) ); ENSURE_SMALL( diff, ATAN_TOL ); } @@ -89,7 +89,7 @@ int MathTest( void ) float a1 = b2Atan2( 0.0f, 0.0f ); float a2 = atan2f( 0.0f, 0.0f ); float diff = b2AbsFloat( a1 - a2 ); - ENSURE( b2Float_IsValid( a1 ) ); + ENSURE( b2IsValidFloat( a1 ) ); ENSURE_SMALL( diff, ATAN_TOL ); } diff --git a/test/test_world.c b/test/test_world.c index 530da6b2a..f5dcb8396 100644 --- a/test/test_world.c +++ b/test/test_world.c @@ -1,10 +1,9 @@ // SPDX-FileCopyrightText: 2023 Erin Catto // SPDX-License-Identifier: MIT +#include "constants.h" #include "test_macros.h" -#include "core.h" - #include "box2d/box2d.h" #include "box2d/collision.h" #include "box2d/math_functions.h" @@ -297,7 +296,7 @@ int TestForAmy( void ) return 0; } -#define WORLD_COUNT (b2_maxWorlds/2) +#define WORLD_COUNT ( b2_maxWorlds / 2 ) int TestWorldRecycle( void ) { @@ -307,10 +306,10 @@ int TestWorldRecycle( void ) b2WorldId worldIds[WORLD_COUNT]; - for (int i = 0; i < count; ++i) + for ( int i = 0; i < count; ++i ) { b2WorldDef worldDef = b2DefaultWorldDef(); - for (int j = 0; j < WORLD_COUNT; ++j) + for ( int j = 0; j < WORLD_COUNT; ++j ) { worldIds[j] = b2CreateWorld( &worldDef ); ENSURE( b2World_IsValid( worldIds[j] ) == true ); @@ -319,7 +318,7 @@ int TestWorldRecycle( void ) b2CreateBody( worldIds[j], &bodyDef ); } - for (int j = 0; j < WORLD_COUNT; ++j) + for ( int j = 0; j < WORLD_COUNT; ++j ) { float timeStep = 1.0f / 60.0f; int subStepCount = 1;