diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index d39bbf815..a77b246f5 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -5,6 +5,7 @@ on: branches: [ main ] pull_request: branches: [ main ] + workflow_dispatch: env: BUILD_TYPE: Debug @@ -14,7 +15,7 @@ jobs: name: ${{ matrix.name }} runs-on: ${{ matrix.os }} timeout-minutes: ${{ matrix.timeout || 4 }} - if: github.event_name == 'push' || github.event.pull_request.draft == false + if: github.event_name != 'pull_request' || github.event.pull_request.draft == false strategy: fail-fast: false matrix: @@ -50,6 +51,17 @@ jobs: os: windows-11-arm args: -A arm64 -DBOX2D_DISABLE_SIMD=TRUE -DBOX2D_SANITIZE=ON test: ./bin/Debug/test + # Optimized runs. The determinism golden hash is otherwise only checked at -O0, + # and -O2 is where a value changing float transform would first show up. + - name: windows-relwithdebinfo + os: windows-latest + msvc: true + config: RelWithDebInfo + test: ./bin/RelWithDebInfo/test + - name: macos-relwithdebinfo + os: macos-latest + config: RelWithDebInfo + test: ./bin/test - name: ubuntu-gcc-double os: ubuntu-latest args: -DBOX2D_DOUBLE_PRECISION=ON @@ -69,23 +81,27 @@ jobs: arch: x64 - name: Configure CMake - run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DBOX2D_SAMPLES=OFF -DBUILD_SHARED_LIBS=OFF -DBOX2D_VALIDATE=ON -DCMAKE_COMPILE_WARNING_AS_ERROR=ON ${{ matrix.args }} + run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{ matrix.config || env.BUILD_TYPE }} -DBOX2D_SAMPLES=OFF -DBUILD_SHARED_LIBS=OFF -DBOX2D_VALIDATE=ON -DCMAKE_COMPILE_WARNING_AS_ERROR=ON ${{ matrix.args }} - name: Build - run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} + run: cmake --build ${{github.workspace}}/build --config ${{ matrix.config || env.BUILD_TYPE }} - name: Test working-directory: ${{github.workspace}}/build env: MSAN_OPTIONS: ${{ matrix['msan-options'] || '' }} + UBSAN_OPTIONS: print_stacktrace=1 run: ${{ matrix.test }} # mingw needs some extra setup so keeping it separate from the matrix. + # Debug -O0 copies whole structs and can hide padding bugs. RelWithDebInfo tests + # -O2 codegen while B2_ENABLE_ASSERT keeps asserts alive, heavy validation stays + # Debug only. build-windows-mingw: name: windows-mingw runs-on: windows-latest timeout-minutes: 4 - if: github.event_name == 'push' || github.event.pull_request.draft == false + if: github.event_name != 'pull_request' || github.event.pull_request.draft == false defaults: run: shell: msys2 {0} @@ -103,20 +119,38 @@ jobs: mingw-w64-ucrt-x86_64-ninja - name: Configure CMake - run: cmake -B build -G Ninja -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DBOX2D_SAMPLES=OFF -DBUILD_SHARED_LIBS=OFF -DBOX2D_VALIDATE=ON -DCMAKE_COMPILE_WARNING_AS_ERROR=ON + run: cmake -B build -G Ninja -DCMAKE_BUILD_TYPE=RelWithDebInfo -DBOX2D_SAMPLES=OFF -DBUILD_SHARED_LIBS=OFF -DBOX2D_VALIDATE=ON -DCMAKE_COMPILE_WARNING_AS_ERROR=ON - name: Build - run: cmake --build build --config ${{env.BUILD_TYPE}} + run: cmake --build build --config RelWithDebInfo - name: Test working-directory: build run: ./bin/test + # Build-only: running the wasm test needs a node + pthread/SharedArrayBuffer harness. + # This still catches compile breaks in the box2d library and the test target. + emscripten: + name: emscripten + runs-on: ubuntu-latest + timeout-minutes: 8 + if: github.event_name != 'pull_request' || github.event.pull_request.draft == false + steps: + - uses: actions/checkout@v6 + + - uses: mymindstorm/setup-emsdk@v14 + + - name: Configure CMake + run: emcmake cmake -B build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DBOX2D_SAMPLES=OFF -DBUILD_SHARED_LIBS=OFF -DBOX2D_VALIDATE=ON + + - name: Build + run: cmake --build build --config ${{env.BUILD_TYPE}} + samples: name: ${{ matrix.name }} runs-on: ${{ matrix.os }} timeout-minutes: 8 - if: github.event_name == 'push' || github.event.pull_request.draft == false + if: github.event_name != 'pull_request' || github.event.pull_request.draft == false strategy: fail-fast: false matrix: diff --git a/.gitignore b/.gitignore index 270d94aa2..fec57b0ab 100644 --- a/.gitignore +++ b/.gitignore @@ -11,8 +11,4 @@ CMakeUserPresets.json .idea/ .claude/ .tmp/ -AGENTS.md -GEMINI.md *.b2rec -build-dp/ -build_float/ diff --git a/CMakeLists.txt b/CMakeLists.txt index d5ed6a197..cdb0406fd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,8 +30,13 @@ if (MSVC OR APPLE OR (UNIX AND NOT APPLE)) add_compile_options("$<$:/fsanitize=address>") add_link_options("$<$:/INCREMENTAL:NO>") elseif(APPLE) - add_compile_options(-fsanitize=address -fsanitize-address-use-after-scope -fsanitize=undefined) + # UBSan recovers by default, no-recover makes the first report fatal so CI fails + add_compile_options(-fsanitize=address -fsanitize-address-use-after-scope -fsanitize=undefined -fno-sanitize-recover=all) add_link_options(-fsanitize=address -fsanitize-address-use-after-scope -fsanitize=undefined) + elseif(UNIX) + # use-after-scope is clang only, leave it off so gcc builds work + add_compile_options(-fsanitize=address -fsanitize=undefined -fno-sanitize-recover=all -fno-omit-frame-pointer) + add_link_options(-fsanitize=address -fsanitize=undefined) endif() else() if(MSVC AND CMAKE_CXX_COMPILER_ID STREQUAL "MSVC" AND PROJECT_IS_TOP_LEVEL) @@ -44,12 +49,17 @@ endif() # Deterministic math # https://box2d.org/posts/2024/08/determinism/ -if (MINGW OR UNIX) - add_compile_options(-ffp-contract=off) -elseif (MSVC AND CMAKE_C_COMPILER_ID STREQUAL "Clang") - # clang-cl contracts multiply add into FMA under AVX2, diverging from non FMA builds. - # The plain driver flag is dropped, so force it through to the frontend. - add_compile_options(/clang:-ffp-contract=off) +# Contraction into FMA is the one value changing transform enabled by default, and whether +# it fires depends on the target. Test the compiler, not the platform, so every gcc and +# clang gets the flag including a clang that is neither MinGW nor clang-cl. +if (CMAKE_C_COMPILER_ID MATCHES "GNU|Clang") + if (CMAKE_C_COMPILER_FRONTEND_VARIANT STREQUAL "MSVC") + # clang-cl contracts multiply add into FMA under AVX2, diverging from non FMA builds. + # The plain driver flag is dropped, so force it through to the frontend. + add_compile_options(/clang:-ffp-contract=off) + else() + add_compile_options(-ffp-contract=off) + endif() endif() option(BOX2D_DISABLE_SIMD "Disable SIMD math (slower)" OFF) @@ -60,12 +70,32 @@ if(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64|AMD64") cmake_dependent_option(BOX2D_AVX2 "Enable AVX2" OFF "NOT BOX2D_DISABLE_SIMD" OFF) endif() +# These are read by the library build, so they must precede add_subdirectory(src), +# otherwise the first configure and a re-configure produce different libraries. +# Top level only, so they stay hidden from FetchContent consumers. +if(PROJECT_IS_TOP_LEVEL) + option(BOX2D_PROFILE "Enable profiling with Tracy" OFF) + option(BOX2D_VALIDATE "Enable heavy validation" ON) +endif() + set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set_property(GLOBAL PROPERTY USE_FOLDERS ON) # Leaving this here for testing # set(CMAKE_VERBOSE_MAKEFILE ON) +# Emscripten pthread support. Must precede add_subdirectory(src) so the box2d library +# objects are built with the same atomics/bulk-memory features as the executables that +# link them, otherwise wasm-ld rejects --shared-memory. Top level only, so a FetchContent +# consumer keeps control of its own threading model. +if (EMSCRIPTEN AND PROJECT_IS_TOP_LEVEL) + set(EMSCRIPTEN_PTHREADS_COMPILER_FLAGS "-pthread -s USE_PTHREADS=1") + set(EMSCRIPTEN_PTHREADS_LINKER_FLAGS "${EMSCRIPTEN_PTHREADS_COMPILER_FLAGS} -s ALLOW_MEMORY_GROWTH") + string(APPEND CMAKE_C_FLAGS " ${EMSCRIPTEN_PTHREADS_COMPILER_FLAGS}") + string(APPEND CMAKE_CXX_FLAGS " ${EMSCRIPTEN_PTHREADS_COMPILER_FLAGS}") + string(APPEND CMAKE_EXE_LINKER_FLAGS " ${EMSCRIPTEN_PTHREADS_LINKER_FLAGS}") +endif() + add_subdirectory(src) # This hides samples, test, and doxygen from apps that use box2d via FetchContent @@ -73,8 +103,6 @@ if(PROJECT_IS_TOP_LEVEL) option(BOX2D_SAMPLES "Build the Box2D samples" ON) option(BOX2D_BENCHMARKS "Build the Box2D benchmarks" OFF) option(BOX2D_DOCS "Build the Box2D documentation" OFF) - option(BOX2D_PROFILE "Enable profiling with Tracy" OFF) - option(BOX2D_VALIDATE "Enable heavy validation" ON) option(BOX2D_UNIT_TESTS "Build the Box2D unit tests" ON) # Needed for samples.exe to find box2d.dll @@ -82,15 +110,6 @@ if(PROJECT_IS_TOP_LEVEL) set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/bin") if(BOX2D_UNIT_TESTS OR BOX2D_SAMPLES OR BOX2D_BENCHMARKS) - # Emscripten pthread support - if(EMSCRIPTEN) - set(EMSCRIPTEN_PTHREADS_COMPILER_FLAGS "-pthread -s USE_PTHREADS=1") - set(EMSCRIPTEN_PTHREADS_LINKER_FLAGS "${EMSCRIPTEN_PTHREADS_COMPILER_FLAGS} -s ALLOW_MEMORY_GROWTH") - string(APPEND CMAKE_C_FLAGS " ${EMSCRIPTEN_PTHREADS_COMPILER_FLAGS}") - string(APPEND CMAKE_CXX_FLAGS " ${EMSCRIPTEN_PTHREADS_COMPILER_FLAGS}") - string(APPEND CMAKE_EXE_LINKER_FLAGS " ${EMSCRIPTEN_PTHREADS_LINKER_FLAGS}") - endif() - add_subdirectory(shared) endif() diff --git a/README.md b/README.md index 9e2cbc32d..a92779fb8 100644 --- a/README.md +++ b/README.md @@ -136,6 +136,19 @@ Support development of Box2D through [Github Sponsors](https://github.com/sponso Please consider starring this repository and subscribing to my [YouTube channel](https://www.youtube.com/@erin_catto). +## LLM Usage + +LLMs are used in the following areas: + +- unit tests +- samples app +- migrating code between Box2D and Box3D +- build configuration +- code reviews +- benchmarking + +Elsewhere all code is developed and written by me. I take responsibility for every line of code in Box2D/3D. + ## External ports, wrappers, and bindings (unsupported) - Beef bindings - https://github.com/EnokViking/Box2DBeef diff --git a/benchmark/main.c b/benchmark/main.c index 71a79ec9d..071c297c3 100644 --- a/benchmark/main.c +++ b/benchmark/main.c @@ -13,6 +13,7 @@ #include "box2d/math_functions.h" #include +#include #include #include #include @@ -47,6 +48,74 @@ static void MinProfile( b2Profile* p1, const b2Profile* p2 ) p1->sleepIslands = b2MinFloat( p1->sleepIslands, p2->sleepIslands ); } +// Match a name=value option in either its short or long spelling. Returns the value or NULL. +static const char* MatchValue( const char* arg, const char* shortName, const char* longName ) +{ + size_t n = strlen( shortName ); + if ( strncmp( arg, shortName, n ) == 0 ) + { + return arg + n; + } + n = strlen( longName ); + if ( strncmp( arg, longName, n ) == 0 ) + { + return arg + n; + } + return NULL; +} + +// Distinguish a numeric benchmark index from a name. Empty string is not numeric. +static bool IsAllDigits( const char* s ) +{ + if ( *s == 0 ) + { + return false; + } + for ( ; *s != 0; ++s ) + { + if ( isdigit( (unsigned char)*s ) == 0 ) + { + return false; + } + } + return true; +} + +static bool EqualsIgnoreCase( const char* a, const char* b ) +{ + while ( *a != 0 && *b != 0 ) + { + if ( tolower( (unsigned char)*a ) != tolower( (unsigned char)*b ) ) + { + return false; + } + ++a; + ++b; + } + return *a == *b; +} + +static int FindBenchmark( const Benchmark* benchmarks, int count, const char* name ) +{ + for ( int i = 0; i < count; ++i ) + { + if ( EqualsIgnoreCase( benchmarks[i].name, name ) ) + { + return i; + } + } + return -1; +} + +static void PrintBenchmarks( const Benchmark* benchmarks, int count ) +{ + printf( "Registered benchmarks:\n" ); + for ( int i = 0; i < count; ++i ) + { + printf( " %2d %s\n", i, benchmarks[i].name ); + } +} + // Box2D benchmark application. On Windows it is important to use affinity avoid cross CCD // usage or efficiency cores. Also on Windows create a power plan with Processor power management // Min/Max of 99%. This prevents boosting and makes the benchmarks more repeatable. @@ -58,6 +127,12 @@ static void MinProfile( b2Profile* p1, const b2Profile* p2 ) // Run all benchmarks with 4 workers only. // start /affinity 0x5555 .\build\bin\Release\benchmark.exe -t=4 -w=4 +// List the registered benchmarks. +// .\build\bin\Release\benchmark.exe -l + +// Run a single benchmark by name with 4 workers. +// start /affinity 0x5555 .\build\bin\Release\benchmark.exe -t=4 -w=4 --benchmark=many_pyramids -r=5 + // Run benchmark 3 with 4 workers and repeat 20 times. Record the step times. // start /affinity 0x5555 .\build\bin\Release\benchmark.exe -t=4 -w=4 -b=3 -r=20 -s // start /affinity 0x5555 .\build\bin\Release\benchmark.exe -t=8 -b=7 @@ -135,43 +210,69 @@ int main( int argc, char** argv ) for ( int i = 1; i < argc; ++i ) { const char* arg = argv[i]; - if ( strncmp( arg, "-t=", 3 ) == 0 ) + const char* value; + if ( ( value = MatchValue( arg, "-t=", "--threads=" ) ) != NULL ) { - int threadCount = atoi( arg + 3 ); + int threadCount = atoi( value ); maxThreadCount = b2ClampInt( threadCount, 1, maxThreadCount ); } - else if ( strncmp( arg, "-b=", 3 ) == 0 ) + else if ( ( value = MatchValue( arg, "-b=", "--benchmark=" ) ) != NULL ) { - singleBenchmark = atoi( arg + 3 ); - singleBenchmark = b2ClampInt( singleBenchmark, 0, benchmarkCount - 1 ); + // Accept an index for backward compatibility, otherwise an exact name + if ( IsAllDigits( value ) ) + { + singleBenchmark = b2ClampInt( atoi( value ), 0, benchmarkCount - 1 ); + } + else + { + singleBenchmark = FindBenchmark( benchmarks, benchmarkCount, value ); + if ( singleBenchmark == -1 ) + { + printf( "No benchmark named \"%s\"\n", value ); + PrintBenchmarks( benchmarks, benchmarkCount ); + exit( 1 ); + } + } } - else if ( strncmp( arg, "-w=", 3 ) == 0 ) + else if ( ( value = MatchValue( arg, "-w=", "--workers=" ) ) != NULL ) { - singleWorkerCount = atoi( arg + 3 ); + singleWorkerCount = atoi( value ); } - else if ( strncmp( arg, "-r=", 3 ) == 0 ) + else if ( ( value = MatchValue( arg, "-r=", "--repeats=" ) ) != NULL ) { - runCount = b2ClampInt( atoi( arg + 3 ), 1, 1000 ); + runCount = b2ClampInt( atoi( value ), 1, 1000 ); } - else if ( strncmp( arg, "-nc", 3 ) == 0 ) + else if ( strcmp( arg, "-nc" ) == 0 || strcmp( arg, "--no-continuous" ) == 0 ) { enableContinuous = false; printf( "Continuous disabled\n" ); } - else if ( strncmp( arg, "-s", 3 ) == 0 ) + else if ( strcmp( arg, "-s" ) == 0 || strcmp( arg, "--record-steps" ) == 0 ) { recordStepTimes = true; } - else if ( strcmp( arg, "-h" ) == 0 ) + else if ( strcmp( arg, "-l" ) == 0 || strcmp( arg, "--list" ) == 0 ) + { + PrintBenchmarks( benchmarks, benchmarkCount ); + exit( 0 ); + } + else if ( strcmp( arg, "-h" ) == 0 || strcmp( arg, "--help" ) == 0 ) { printf( "Usage\n" - "-t=: the maximum number of threads to use\n" - "-b=: run a single benchmark\n" - "-w=: run a single worker count\n" - "-r=: number of repeats (default is 4)\n" - "-s: record step times\n" ); + "-t, --threads=: the maximum number of threads to use\n" + "-b, --benchmark=: run a single benchmark by index or name\n" + "-w, --workers=: run a single worker count\n" + "-r, --repeats=: number of repeats (default is 4)\n" + "-nc, --no-continuous: disable continuous collision\n" + "-s, --record-steps: record step times\n" + "-l, --list: list the registered benchmarks\n" + "-h, --help: print this help\n" ); exit( 0 ); } + else + { + printf( "Unknown option \"%s\", use -h for help\n", arg ); + } } if ( singleWorkerCount != -1 ) diff --git a/docs/foundation.md b/docs/foundation.md index f689a5fbc..dd3d17806 100644 --- a/docs/foundation.md +++ b/docs/foundation.md @@ -39,6 +39,7 @@ members are exposed, so you may use them freely in your application. The math library is kept simple to make Box2D easy to port and maintain. ## Multithreading {#multi} + Box2D has been highly optimized for multithreading. Multithreading is not required and by default Box2D will run single-threaded. If performance is important for your application, you should consider using the multithreading interface. Box2D has a built-in task scheduler that creates threads. You can optionally connect your own task scheduler if you want more control. diff --git a/docs/layout.xml b/docs/layout.xml index e93b6cf3b..331a4ace7 100644 --- a/docs/layout.xml +++ b/docs/layout.xml @@ -1,11 +1,11 @@ - - + + - + @@ -51,51 +51,51 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - - - - - - - - - - - + + + + + + + + + + + @@ -113,25 +113,25 @@ - - - - - - - + + + + + + + - + - - - - - - - - + + + + + + + + @@ -141,7 +141,7 @@ - + @@ -160,27 +160,27 @@ - - - - - - - - + + + + + + + + - + - - - - - - - - - + + + + + + + + + @@ -197,42 +197,42 @@ - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + - + - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + @@ -244,13 +244,13 @@ - - - - - + + + + + - + @@ -264,6 +264,6 @@ - + diff --git a/docs/simulation.md b/docs/simulation.md index 4a272a21e..914df655f 100644 --- a/docs/simulation.md +++ b/docs/simulation.md @@ -721,7 +721,7 @@ destroyed when the parent body is destroyed. However, you may wish to store the to change properties on it later. You can create multiple shapes on a single body. They all can contribute -to the mass of the body. These shapes never collide with each other and may overlap. +to the mass properties of the body. These shapes never collide with each other and may overlap. You can destroy a shape on the parent body. You may do this to model a breakable object. Otherwise you can just leave the shape alone and let diff --git a/include/box2d/base.h b/include/box2d/base.h index 694584c16..fa7c678e2 100644 --- a/include/box2d/base.h +++ b/include/box2d/base.h @@ -7,16 +7,26 @@ #include #include +// Compile-time options. Edit box2d/config.h, or define BOX2D_USER_CONFIG to +// point at your own copy. +#ifdef BOX2D_USER_CONFIG +#include BOX2D_USER_CONFIG +#endif +#include "config.h" + /// Used to indicate an unset or invalid index value. #define B2_NULL_INDEX ( -1 ) // clang-format off // // Shared library macros -#if defined( _MSC_VER ) && defined( box2d_EXPORTS ) +// Predefine BOX2D_EXPORT to reuse an existing export/import scheme, for example +// when compiling Box2D into another shared library. +#ifndef BOX2D_EXPORT +#if defined( _WIN32 ) && defined( box2d_EXPORTS ) // build the Windows DLL #define BOX2D_EXPORT __declspec( dllexport ) -#elif defined( _MSC_VER ) && defined( BOX2D_DLL ) +#elif defined( _WIN32 ) && defined( BOX2D_DLL ) // using the Windows DLL #define BOX2D_EXPORT __declspec( dllimport ) #elif defined( box2d_EXPORTS ) @@ -26,6 +36,7 @@ // static library #define BOX2D_EXPORT #endif +#endif // C++ macros #ifdef __cplusplus diff --git a/include/box2d/box2d.h b/include/box2d/box2d.h index 450eded18..2c9f1fb8d 100644 --- a/include/box2d/box2d.h +++ b/include/box2d/box2d.h @@ -143,8 +143,9 @@ B2_API float b2World_GetHitEventThreshold( b2WorldId worldId ); /// Register the custom filter callback. This is optional. B2_API void b2World_SetCustomFilterCallback( b2WorldId worldId, b2CustomFilterFcn* fcn, void* context ); -/// Register the pre-solve callback. This is optional. -B2_API void b2World_SetPreSolveCallback( b2WorldId worldId, b2PreSolveFcn* fcn, void* context ); +/// Register the pre-solve and pre-continuous callback. This is optional. +B2_API void b2World_SetPreSolveCallback( b2WorldId worldId, b2PreSolveFcn* preSolveFcn, b2PreContinuousFcn* preContinuousFcn, + void* context ); /// Set the gravity vector for the entire world. Box2D has no concept of an up direction and this /// is left as a decision for the application. Usually in m/s^2. @@ -226,6 +227,13 @@ B2_API void b2World_RebuildStaticTree( b2WorldId worldId ); /// This is for internal testing B2_API void b2World_EnableSpeculative( b2WorldId worldId, bool flag ); +/// Compute a deterministic hash of the simulation state: body transforms and velocities, contact and +/// joint impulses, and the index bookkeeping that drives the solve. Reproduces exactly across worker +/// counts and ignores struct padding and free slots, so two worlds that simulate identically always +/// agree. Use this to detect desyncs (for example rollback resimulation) instead of comparing +/// serialized world bytes, which carry non-canonical padding. +B2_API uint64_t b2World_GetStateHash( b2WorldId worldId ); + /** @} */ /** @@ -501,6 +509,8 @@ B2_API b2Pos b2Body_GetWorldCenter( b2BodyId bodyId ); /// Override the body's mass properties. Normally this is computed automatically using the /// shape geometry and density. This information is lost if a shape is added or removed or if the /// body type changes. +/// If motion locks are used to fix rotation then the rotational inertia value is ignored and +/// rotational inertia remains fixed at zero. B2_API void b2Body_SetMassData( b2BodyId bodyId, b2MassData massData ); /// Get the mass data for a body @@ -565,7 +575,8 @@ B2_API void b2Body_Disable( b2BodyId bodyId ); /// Enable a body by adding it to the simulation. This is expensive. B2_API void b2Body_Enable( b2BodyId bodyId ); -/// Set the motion locks on this body. +/// Set the motion locks on this body. This causes mass properties to be recomputed because +/// fixed rotation sets the rotational inertia to zero. B2_API void b2Body_SetMotionLocks( b2BodyId bodyId, b2MotionLocks locks ); /// Get the motion locks for this body. @@ -898,8 +909,8 @@ B2_API bool b2Chain_IsValid( b2ChainId id ); * @{ */ -/// Destroy a joint. Optionally wake attached bodies. -B2_API void b2DestroyJoint( b2JointId jointId, bool wakeAttached ); +/// Destroy a joint. +B2_API void b2DestroyJoint( b2JointId jointId ); /// Joint identifier validation. Provides validation for up to 64K allocations. B2_API bool b2Joint_IsValid( b2JointId id ); @@ -1060,6 +1071,21 @@ B2_API float b2DistanceJoint_GetMotorForce( b2JointId jointId ); /** @} */ +/** + * @defgroup filter_joint Filter Joint + * @brief Functions for the filter joint. + * + * The filter joint is used to disable collision between two bodies. As a side effect of being a joint, it also + * keeps the two bodies in the same simulation island. + * @{ + */ + +/// Create a filter joint. +/// @see b2FilterJointDef for details +B2_API b2JointId b2CreateFilterJoint( b2WorldId worldId, const b2FilterJointDef* def ); + +/**@}*/ + /** * @defgroup motor_joint Motor Joint * @brief Functions for the motor joint. @@ -1139,20 +1165,72 @@ B2_API float b2MotorJoint_GetMaxSpringTorque( b2JointId jointId ); /**@}*/ /** - * @defgroup filter_joint Filter Joint - * @brief Functions for the filter joint. + * @defgroup mover_joint Mover Joint + * @brief Functions for the mover joint. * - * The filter joint is used to disable collision between two bodies. As a side effect of being a joint, it also - * keeps the two bodies in the same simulation island. + * The mover joint is designed for a dynamic character mover. * @{ */ -/// Create a filter joint. -/// @see b2FilterJointDef for details -B2_API b2JointId b2CreateFilterJoint( b2WorldId worldId, const b2FilterJointDef* def ); +/// Create a mover joint +/// @see b2MoverJointDef for details +B2_API b2JointId b2CreateMoverJoint( b2WorldId worldId, const b2MoverJointDef* def ); + +/// Set the desired relative linear velocity in meters per second +B2_API void b2MoverJoint_SetLinearVelocity( b2JointId jointId, b2Vec2 velocity ); + +/// Get the desired relative linear velocity in meters per second +B2_API b2Vec2 b2MoverJoint_GetLinearVelocity( b2JointId jointId ); + +/// Set the motor joint maximum force, usually in newtons +B2_API void b2MoverJoint_SetMaxVelocityForce( b2JointId jointId, b2Vec2 maxForce ); + +/// Get the motor joint maximum force, usually in newtons +B2_API b2Vec2 b2MoverJoint_GetMaxVelocityForce( b2JointId jointId ); /**@}*/ +/** + * @defgroup pogo_joint Pogo Joint + * @brief Functions for the pogo joint. Used for dynamic character movers. + * @{ + */ + +/// Create a pog joint +/// @see b2PogoJointDef for details +B2_API b2JointId b2CreatePogoJoint( b2WorldId worldId, const b2PogoJointDef* def ); + +/// Set the rest length of a pogo joint +/// @param jointId The id for a pogo joint +/// @param length The new pogo joint length +B2_API void b2PogoJoint_SetRestLength( b2JointId jointId, float length ); + +/// Get the rest length of a pogo joint +B2_API float b2PogoJoint_GetRestLength( b2JointId jointId ); + +/// Set the spring stiffness in Hertz +B2_API void b2PogoJoint_SetSpringHertz( b2JointId jointId, float hertz ); + +/// Get the spring Hertz +B2_API float b2PogoJoint_GetSpringHertz( b2JointId jointId ); + +/// Set the spring damping ratio, non-dimensional +B2_API void b2PogoJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ); + +/// Get the spring damping ratio +B2_API float b2PogoJoint_GetSpringDampingRatio( b2JointId jointId ); + +/// Get the current length of the pogo. +B2_API float b2PogoJoint_GetLength( b2JointId jointId ); + +/// Get the internal pogo velocity. +B2_API float b2PogoJoint_GetVelocity( b2JointId jointId ); + +/// Get the internal pogo impulse. +B2_API float b2PogoJoint_GetImpulse( b2JointId jointId ); + +/** @} */ + /** * @defgroup prismatic_joint Prismatic Joint * @brief A prismatic joint allows for translation along a single axis with no rotation. @@ -1240,7 +1318,6 @@ B2_API float b2PrismaticJoint_GetSpeed( b2JointId jointId ); * @defgroup revolute_joint Revolute Joint * @brief A revolute joint allows for relative rotation in the 2D plane with no relative translation. * - * The revolute joint is probably the most common joint. It can be used for ragdolls and chains. * Also called a *hinge* or *pin* joint. * @{ */ diff --git a/include/box2d/collision.h b/include/box2d/collision.h index e2cd8b674..f260eeebc 100644 --- a/include/box2d/collision.h +++ b/include/box2d/collision.h @@ -910,10 +910,10 @@ typedef struct b2PlaneResult } b2PlaneResult; /// These are collision planes that can be fed to b2SolvePlanes. Normally -/// this is assembled by the user from plane results in b2PlaneResult +/// this is assembled by the user from plane results in b2PlaneResult. typedef struct b2CollisionPlane { - /// The collision plane between the mover and some shape + /// The collision plane between the mover and some shape. b2Plane plane; /// Setting this to FLT_MAX makes the plane as rigid as possible. Lower values can @@ -927,18 +927,18 @@ typedef struct b2CollisionPlane bool clipVelocity; } b2CollisionPlane; -/// Result returned by b2SolvePlanes +/// Result returned by b2SolvePlanes. typedef struct b2PlaneSolverResult { - /// The translation of the mover - b2Vec2 translation; + /// The final relative translation. + b2Vec2 delta; /// The number of iterations used by the plane solver. For diagnostics. int iterationCount; } b2PlaneSolverResult; /// Solves the position of a mover that satisfies the given collision planes. -/// @param targetDelta the desired movement from the position used to generate the collision planes +/// @param targetDelta the desired translation from the position used to generate the collision planes /// @param planes the collision planes /// @param count the number of collision planes B2_API b2PlaneSolverResult b2SolvePlanes( b2Vec2 targetDelta, b2CollisionPlane* planes, int count ); diff --git a/include/box2d/config.h b/include/box2d/config.h new file mode 100644 index 000000000..b07f602d8 --- /dev/null +++ b/include/box2d/config.h @@ -0,0 +1,30 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +// Box2D compile-time options. +// +// Normally set by the CMake BOX2D_* options. If you build Box2D without its +// CMake (dropping the sources into another project), set them here so the +// library and your code agree. Edit this file, or keep your settings elsewhere +// and point Box2D at them from your build: +// +// #define BOX2D_USER_CONFIG "my_box2d_config.h" +// +// A define passed on the compiler command line still wins over this file. + +// Large world mode. Stores world positions in double precision. Affects ABI. +//#define BOX2D_DOUBLE_PRECISION + +// Build the scalar fallback instead of SSE2/NEON. +//#define BOX2D_DISABLE_SIMD + +// Enable internal validation in debug builds. +//#define BOX2D_VALIDATE + +// Decorate the public API with your own export macro instead of Box2D's +// box2d_EXPORTS/BOX2D_DLL scheme, for example when compiling Box2D into another +// shared library. A single value cannot switch between dllexport and dllimport, +// so this suits embedding more than shipping Box2D as its own DLL. +//#define BOX2D_EXPORT MYENGINE_API diff --git a/include/box2d/math_functions.h b/include/box2d/math_functions.h index 77e1a545b..3d455aa8e 100644 --- a/include/box2d/math_functions.h +++ b/include/box2d/math_functions.h @@ -337,17 +337,16 @@ B2_INLINE float b2Distance( b2Vec2 a, b2Vec2 b ) /// Convert a vector into a unit vector if possible, otherwise returns the zero vector. /// todo MSVC is not inlining this function in several places per warning 4710 -B2_INLINE b2Vec2 b2Normalize( b2Vec2 v ) +B2_INLINE b2Vec2 b2Normalize( b2Vec2 a ) { - float length = sqrtf( v.x * v.x + v.y * v.y ); - if ( length < FLT_EPSILON ) + float lengthSquared = a.x * a.x + a.y * a.y; + if ( lengthSquared > 1000.0f * FLT_MIN ) { - return B2_LITERAL( b2Vec2 ){ 0.0f, 0.0f }; + float s = 1.0f / sqrtf( lengthSquared ); + b2Vec2 u = { s * a.x, s * a.y }; + return u; } - - float invLength = 1.0f / length; - b2Vec2 n = { invLength * v.x, invLength * v.y }; - return n; + return B2_LITERAL( b2Vec2 ){ 0.0f, 0.0f }; } /// Determines if the provided vector is normalized (norm(a) == 1). @@ -359,17 +358,19 @@ B2_INLINE bool b2IsNormalized( b2Vec2 a ) /// Convert a vector into a unit vector if possible, otherwise returns the zero vector. Also /// outputs the length. -B2_INLINE b2Vec2 b2GetLengthAndNormalize( float* length, b2Vec2 v ) +B2_INLINE b2Vec2 b2GetLengthAndNormalize( float* length, b2Vec2 a ) { - *length = sqrtf( v.x * v.x + v.y * v.y ); - if ( *length < FLT_EPSILON ) + float lengthSquared = a.x * a.x + a.y * a.y; + if ( lengthSquared > 1000.0f * FLT_MIN ) { - return B2_LITERAL( b2Vec2 ){ 0.0f, 0.0f }; + *length = sqrtf( lengthSquared ); + float s = 1.0f / *length; + b2Vec2 u = { s * a.x, s * a.y }; + return u; } - float invLength = 1.0f / *length; - b2Vec2 n = { invLength * v.x, invLength * v.y }; - return n; + *length = 0.0f; + return B2_LITERAL( b2Vec2 ){ 0.0f, 0.0f }; } /// Normalize rotation @@ -525,7 +526,7 @@ B2_INLINE b2Rot b2InvMulRot( b2Rot a, b2Rot b ) return r; } -/// Relative angle between a and b +/// Relative angle between a and b. B2_INLINE float b2RelativeAngle( b2Rot a, b2Rot b ) { // sin(b - a) = bs * ac - bc * as @@ -535,26 +536,37 @@ B2_INLINE float b2RelativeAngle( b2Rot a, b2Rot b ) return b2Atan2( s, c ); } -/// Convert any angle into the range [-pi, pi] +/// Convert any angle into the range [-pi, pi]. B2_INLINE float b2UnwindAngle( float radians ) { - // Assuming this is deterministic - return remainderf( radians, 2.0f * B2_PI ); + // This function can be implemented using remainderf. + // However that is a libc dependency not present in WASM. + // This function doesn't work well with huge angles. So unwind your angles regularly. + B2_VALIDATE( b2AbsFloat( radians ) < 1.0e4f ); + float x = b2ClampFloat( radians, -1.0e6f, 1.0e6f ); + + // Work in doubles. Adding and removing this constant rounds to the nearest integer. + // https://stackoverflow.com/questions/17035464/a-fast-method-to-round-a-double-to-a-32-bit-int-explained + double twoPi = 2.0f * B2_PI; + double roundToNearest = 6755399441055744.0; + double a = x; + double k = ( a / twoPi + roundToNearest ) - roundToNearest; + return (float)( a - k * twoPi ); } -/// Rotate a vector +/// Rotate a vector. B2_INLINE b2Vec2 b2RotateVector( b2Rot q, b2Vec2 v ) { return B2_LITERAL( b2Vec2 ){ q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y }; } -/// Inverse rotate a vector +/// Inverse rotate a vector. B2_INLINE b2Vec2 b2InvRotateVector( b2Rot q, b2Vec2 v ) { return B2_LITERAL( b2Vec2 ){ q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y }; } -/// Transform a point (e.g. local space to world space) +/// Transform a point (e.g. local space to world space). B2_INLINE b2Vec2 b2TransformPoint( b2Transform t, const b2Vec2 p ) { float x = ( t.q.c * p.x - t.q.s * p.y ) + t.p.x; @@ -563,7 +575,7 @@ B2_INLINE b2Vec2 b2TransformPoint( b2Transform t, const b2Vec2 p ) return B2_LITERAL( b2Vec2 ){ x, y }; } -/// Inverse transform a point (e.g. world space to local space) +/// Inverse transform a point (e.g. world space to local space). B2_INLINE b2Vec2 b2InvTransformPoint( b2Transform t, const b2Vec2 p ) { float vx = p.x - t.p.x; @@ -678,7 +690,7 @@ B2_INLINE b2Transform b2InvMulWorldTransforms( b2WorldTransform A, b2WorldTransf } /// Convert a local transform B into world space using world transform A. -B2_INLINE b2WorldTransform b2OffsetWorldTransform( b2WorldTransform A, b2Transform B ) +B2_INLINE b2WorldTransform b2MulWorldTransforms( b2WorldTransform A, b2Transform B ) { b2WorldTransform C; C.q = b2MulRot( A.q, B.q ); diff --git a/include/box2d/types.h b/include/box2d/types.h index 78f43c8ab..133b2ce3a 100644 --- a/include/box2d/types.h +++ b/include/box2d/types.h @@ -47,12 +47,21 @@ typedef float b2RestitutionCallback( float restitutionA, uint64_t userMaterialId /// @ingroup world typedef struct b2RayResult { + /// The shape hit. b2ShapeId shapeId; b2Pos point; b2Vec2 normal; + + /// The fraction of the input ray. float fraction; + + /// The number of BVH nodes visited. Diagnostic. int nodeVisits; + + /// The number of BVH leaves visited. Diagnostic. int leafVisits; + + /// Did the ray hit? If false, all other data is invalid. bool hit; } b2RayResult; @@ -443,7 +452,7 @@ typedef struct b2ShapeDef bool invokeContactCreation; /// Should the body update the mass properties when this shape is created. Default is true. - /// Warning: if this is true, you MUST call b2Body_ApplyMassFromShapes before simulating the world. + /// Warning: if this is false, you MUST call b2Body_ApplyMassFromShapes or b2Body_SetMassData before simulating the world. bool updateBodyMass; /// Used internally to detect a valid definition. DO NOT SET. @@ -569,6 +578,8 @@ typedef enum b2JointType b2_distanceJoint, b2_filterJoint, b2_motorJoint, + b2_moverJoint, + b2_pogoJoint, b2_prismaticJoint, b2_revoluteJoint, b2_weldJoint, @@ -670,6 +681,22 @@ typedef struct b2DistanceJointDef /// @ingroup distance_joint B2_API b2DistanceJointDef b2DefaultDistanceJointDef( void ); +/// A filter joint is used to disable collision between two specific bodies. +/// +/// @ingroup filter_joint +typedef struct b2FilterJointDef +{ + /// Base joint definition + b2JointDef base; + + /// Used internally to detect a valid definition. DO NOT SET. + int internalValue; +} b2FilterJointDef; + +/// Use this to initialize your joint definition +/// @ingroup filter_joint +B2_API b2FilterJointDef b2DefaultFilterJointDef( void ); + /// A motor joint is used to control the relative velocity and or transform between two bodies. /// With a velocity of zero this acts like top-down friction. /// @ingroup motor_joint @@ -716,21 +743,68 @@ typedef struct b2MotorJointDef /// @ingroup motor_joint B2_API b2MotorJointDef b2DefaultMotorJointDef( void ); -/// A filter joint is used to disable collision between two specific bodies. -/// -/// @ingroup filter_joint -typedef struct b2FilterJointDef +/// A mover joint is used to move a dynamic character mover through velocity commands. +/// The x and y directions are handled separately. Does not affect rotation. +/// @ingroup mover_joint +typedef struct b2MoverJointDef { /// Base joint definition b2JointDef base; + /// The desired linear velocity + b2Vec2 linearVelocity; + + /// The maximum motor force in newtons. + b2Vec2 maxVelocityForce; + /// Used internally to detect a valid definition. DO NOT SET. int internalValue; -} b2FilterJointDef; +} b2MoverJointDef; /// Use this to initialize your joint definition -/// @ingroup filter_joint -B2_API b2FilterJointDef b2DefaultFilterJointDef( void ); +/// @ingroup mover_joint +B2_API b2MoverJointDef b2DefaultMoverJointDef( void ); + +/// Pogo joint definition +/// @ingroup pogo_joint +typedef struct b2PogoJointDef +{ + /// Base joint definition + b2JointDef base; + + /// Normal vector at the contact + b2Vec2 normal; + + /// The spring stiffness Hertz, cycles per second + float hertz; + + /// The spring damping ratio, non-dimensional + float dampingRatio; + + /// Spring length. + float restLength; + + /// Maximum tension force. Controls how sticky the mover is to ground. + float maxTensionForce; + + /// Maximum compression force. How hard the pogo pushes the mover up from the ground. + float maxCompressionForce; + + /// The initial pogo internal impulse. The pogo is typically recreated every frame. This + /// gives a way to move the internal state forward for smoother spring behavior. + float impulse; + + /// The initial pogo internal velocity. The pogo is typically recreated every frame. This + /// gives a way to move the internal state forward for smoother spring behavior. + float velocity; + + /// Used internally to detect a valid definition. DO NOT SET. + int internalValue; +} b2PogoJointDef; + +/// Use this to initialize your joint definition +/// @ingroup pogo_joint +B2_API b2PogoJointDef b2DefaultPogoJointDef( void ); /// Prismatic joint definition /// Body B may slide along the x-axis in local frame A. Body B cannot rotate relative to body A. @@ -1091,9 +1165,12 @@ typedef struct b2ContactEvents /// @note If sleeping is disabled all dynamic and kinematic bodies will trigger move events. typedef struct b2BodyMoveEvent { + /// The body user data. void* userData; b2WorldTransform transform; b2BodyId bodyId; + + /// Did the body fall asleep this time step? bool fellAsleep; } b2BodyMoveEvent; @@ -1137,9 +1214,17 @@ typedef struct b2JointEvents /// @see b2Shape_GetContactData() and b2Body_GetContactData() typedef struct b2ContactData { + /// The contact id. You may hold onto this to track a contact across time steps. + /// This id may become orphaned. Use b2Contact_IsValid before using it for other functions. b2ContactId contactId; + + /// The first shape id. b2ShapeId shapeIdA; + + /// The second shape id. b2ShapeId shapeIdB; + + /// The manifold copied from the contact. b2Manifold manifold; } b2ContactData; @@ -1160,19 +1245,33 @@ typedef struct b2ContactData typedef bool b2CustomFilterFcn( b2ShapeId shapeIdA, b2ShapeId shapeIdB, void* context ); /// Prototype for a pre-solve callback. -/// This is called after a contact is updated. This allows you to inspect a -/// contact before it goes to the solver. If you are careful, you can modify the -/// contact manifold (e.g. modify the normal). +/// This is called after a contact manifold is updated. This allows you to inspect a +/// manifold before it goes to the solver. If you are careful, you can modify the +/// manifold (e.g. modify the normal). You can set the manifold point count to zero disable +/// collision. +/// Notes: +/// - this function must be thread-safe +/// - this is only called if the shape has enabled pre-solve events +/// - this is called only for awake dynamic bodies +/// - this is not called for sensors +/// - the supplied manifold impulses are not reliable +/// @warning Do not attempt to modify the world inside this callback +/// @ingroup world +typedef void b2PreSolveFcn( b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifold, void* context ); + +/// Prototype for a pre-continuous callback. +/// This is called when a time of impact event is computed in continuous collision. This function +/// can be used to skip the event. Unlike the pre-solve callback, this only supplies the point and normal. +/// At this stage of the solver the manifold may not exist or may be stale. /// Notes: /// - this function must be thread-safe /// - this is only called if the shape has enabled pre-solve events /// - this is called only for awake dynamic bodies /// - this is not called for sensors -/// - the supplied manifold has impulse values from the previous step -/// Return false if you want to disable the contact this step +/// Return false if you want to disable the time of impact event. /// @warning Do not attempt to modify the world inside this callback /// @ingroup world -typedef bool b2PreSolveFcn( b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Pos point, b2Vec2 normal, void* context ); +typedef bool b2PreContinuousFcn( b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Pos point, b2Vec2 normal, void* context ); /// Prototype callback for overlap queries. /// Called for each shape found in the query. diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index 7eabdfc0f..8c160af55 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -148,6 +148,10 @@ set(BOX2D_SAMPLE_COMMON_FILES donut.h doohickey.cpp doohickey.h + dynamic_mover.cpp + dynamic_mover.h + geometric_mover.cpp + geometric_mover.h sample_benchmark.cpp sample_bodies.cpp sample_character.cpp diff --git a/samples/car.cpp b/samples/car.cpp index 10151a649..84253e778 100644 --- a/samples/car.cpp +++ b/samples/car.cpp @@ -65,7 +65,6 @@ void Car::Spawn( b2WorldId worldId, b2Pos position, float scale, float hertz, fl m_frontWheelId = b2CreateBody( worldId, &bodyDef ); b2CreateCircleShape( m_frontWheelId, &shapeDef, &circle ); - b2Vec2 axis = { 0.0f, 1.0f }; b2Pos pivot = b2Body_GetPosition( m_rearWheelId ); // float throttle = 0.0f; @@ -112,8 +111,8 @@ void Car::Despawn() { assert( m_isSpawned == true ); - b2DestroyJoint( m_rearAxleId, false ); - b2DestroyJoint( m_frontAxleId, false ); + b2DestroyJoint( m_rearAxleId ); + b2DestroyJoint( m_frontAxleId ); b2DestroyBody( m_rearWheelId ); b2DestroyBody( m_frontWheelId ); b2DestroyBody( m_chassisId ); @@ -214,7 +213,6 @@ void Truck::Spawn( b2WorldId worldId, b2Pos position, float scale, float hertz, m_frontWheelId = b2CreateBody( worldId, &bodyDef ); b2CreateCircleShape( m_frontWheelId, &shapeDef, &circle ); - b2Vec2 axis = { 0.0f, 1.0f }; b2Pos pivot = b2Body_GetPosition( m_rearWheelId ); // float throttle = 0.0f; @@ -261,8 +259,8 @@ void Truck::Despawn() { assert( m_isSpawned == true ); - b2DestroyJoint( m_rearAxleId, false ); - b2DestroyJoint( m_frontAxleId, false ); + b2DestroyJoint( m_rearAxleId ); + b2DestroyJoint( m_frontAxleId ); b2DestroyBody( m_rearWheelId ); b2DestroyBody( m_frontWheelId ); b2DestroyBody( m_chassisId ); diff --git a/samples/container.h b/samples/container.h index bf516e905..1f849c091 100644 --- a/samples/container.h +++ b/samples/container.h @@ -3,104 +3,160 @@ #pragma once +#include +#include +#include + #define NULL_INDEX -1 -// Array declaration. Works with forward declaration of TYPE. -#define ARRAY_DECLARE( TYPE ) \ - typedef struct \ +#define DeclareArray( T ) \ + typedef struct DynamicArray_##T \ { \ - TYPE* data; \ + struct T* data; \ int count; \ int capacity; \ - } TYPE##Array; \ - TYPE##Array TYPE##Array_Create( int capacity ); \ - void TYPE##Array_Reserve( TYPE##Array* a, int newCapacity ); \ - void TYPE##Array_Destroy( TYPE##Array* a ) - -// Inline array functions that need the TYPE to be defined. -#define ARRAY_INLINE( TYPE ) \ - static inline void TYPE##Array_Resize( TYPE##Array* a, int count ) \ + } DynamicArray_##T + +#define DeclareArrayNative( T ) \ + typedef struct DynamicArray_##T \ { \ - TYPE##Array_Reserve( a, count ); \ - a->count = count; \ - } \ - static inline TYPE* TYPE##Array_Get( TYPE##Array* a, int index ) \ + T* data; \ + int count; \ + int capacity; \ + } DynamicArray_##T + +// Define an array. +// It may be zero initialized: +// Array(int) myArray = { 0 }; +#define Array( T ) DynamicArray_##T + +// Alternative to zero initialization +#define Array_Create( a ) \ + do \ { \ - assert( 0 <= index && index < a->count ); \ - return a->data + index; \ + ( a ).data = NULL; \ + ( a ).count = 0; \ + ( a ).capacity = 0; \ } \ - static inline TYPE* TYPE##Array_Add( TYPE##Array* a ) \ - { \ - if ( a->count == a->capacity ) \ - { \ - int newCapacity = a->capacity < 2 ? 2 : a->capacity + ( a->capacity >> 1 ); \ - TYPE##Array_Reserve( a, newCapacity ); \ - } \ - a->count += 1; \ - return a->data + ( a->count - 1 ); \ - } \ - static inline void TYPE##Array_Push( TYPE##Array* a, TYPE value ) \ + while ( 0 ) + +#define Array_CreateN( a, n ) \ + do \ { \ - if ( a->count == a->capacity ) \ - { \ - int newCapacity = a->capacity < 2 ? 2 : a->capacity + ( a->capacity >> 1 ); \ - TYPE##Array_Reserve( a, newCapacity ); \ - } \ - a->data[a->count] = value; \ - a->count += 1; \ + ( a ).data = ( n ) > 0 ? GrowAlloc( NULL, 0, ( n ) * sizeof( *( a ).data ) ) : NULL; \ + ( a ).count = 0; \ + ( a ).capacity = ( n ); \ } \ - static inline void TYPE##Array_Set( TYPE##Array* a, int index, TYPE value ) \ + while ( 0 ) + +#define Array_Destroy( a ) \ + do \ { \ - assert( 0 <= index && index < a->count ); \ - a->data[index] = value; \ + free( ( a ).data ); \ + ( a ).data = NULL; \ + ( a ).count = 0; \ + ( a ).capacity = 0; \ } \ - static inline int TYPE##Array_RemoveSwap( TYPE##Array* a, int index ) \ + while ( 0 ) + +#define Array_Reserve( a, n ) \ + do \ { \ - assert( 0 <= index && index < a->count ); \ - int movedIndex = NULL_INDEX; \ - if ( index != a->count - 1 ) \ + if ( ( a ).capacity < n ) \ { \ - movedIndex = a->count - 1; \ - a->data[index] = a->data[movedIndex]; \ + int oldSize = ( a ).capacity * sizeof( *( a ).data ); \ + int newSize = ( n ) * sizeof( *( a ).data ); \ + ( a ).data = GrowAlloc( ( a ).data, oldSize, newSize ); \ + ( a ).capacity = ( n ); \ } \ - a->count -= 1; \ - return movedIndex; \ } \ - static inline TYPE TYPE##Array_Pop( TYPE##Array* a ) \ + while ( 0 ) + +#define Array_Resize( a, n ) \ + do \ { \ - assert( a->count > 0 ); \ - TYPE value = a->data[a->count - 1]; \ - a->count -= 1; \ - return value; \ + Array_Reserve( a, n ); \ + ( a ).count = ( n ); \ } \ + while ( 0 ) -// These functions go in the source file -#define ARRAY_SOURCE( TYPE ) \ - TYPE##Array TYPE##Array_Create( int capacity ) \ +#define Array_ResizeAndSetZero( a, n ) \ + do \ { \ - TYPE##Array a = { 0 }; \ - if ( capacity > 0 ) \ - { \ - a.data = malloc( capacity * sizeof( TYPE ) ); \ - a.capacity = capacity; \ - } \ - return a; \ + Array_Reserve( a, n ); \ + memset( ( a ).data, 0, ( n ) * sizeof( *( a ).data ) ); \ + ( a ).count = ( n ); \ } \ - void TYPE##Array_Reserve( TYPE##Array* a, int newCapacity ) \ + while ( 0 ) + +// Push a new element by value +#define Array_Push( a, value ) \ + do \ { \ - if ( newCapacity <= a->capacity ) \ + int elementSize = sizeof( *( a ).data ); \ + if ( ( a ).count >= ( a ).capacity ) \ { \ - return; \ + int oldSize = ( a ).capacity * elementSize; \ + int newCapacity = ( a ).capacity == 0 ? 8 : 2 * ( a ).capacity; \ + int newSize = newCapacity * elementSize; \ + ( a ).data = GrowAlloc( ( a ).data, oldSize, newSize ); \ + ( a ).capacity = newCapacity; \ } \ - a->data = GrowAlloc( a->data, a->capacity * sizeof( TYPE ), newCapacity * sizeof( TYPE ) ); \ - a->capacity = newCapacity; \ + ( a ).data[( a ).count] = ( value ); \ + ( a ).count += 1; \ } \ - void TYPE##Array_Destroy( TYPE##Array* a ) \ - { \ - free( a->data ); \ - a->data = NULL; \ - a->count = 0; \ - a->capacity = 0; \ - } + while ( 0 ) + +// Get a pointer to an element +#define Array_Get( a, index ) ( assert( 0 <= ( index ) && ( index ) < ( a ).count ), ( a ).data + ( index ) ) + +// Create a new uninitialized element and return a pointer to it +#define Array_Emplace( a ) ( EmplaceHelper( (void**)&( a ).data, &( a ).count, &( a ).capacity, sizeof( *( a ).data ) ) ) + +// Remove the last element and return it by value. +#define Array_Pop( a ) ( assert( 0 < ( a ).count ), ( a ).data[-1 + ( a ).count--] ) + +// Remove an element by swapping with the last element. If the index is the last element it returns +// NULL_INDEX, otherwise it returns the index of the last element (which is now out of bounds). +#define Array_RemoveSwap( a, index ) RemoveHelper( ( a ).data, &( a ).count, ( index ), sizeof( *( a ).data ) ) void* GrowAlloc( void* oldMem, int oldSize, int newSize ); + +static inline void* EmplaceHelper( void** data, int* count, int* capacity, int elementSize ) +{ + if ( *count >= *capacity ) + { + int oldCapacity = *capacity; + int oldSize = oldCapacity * elementSize; + int newCapacity = ( oldCapacity == 0 ? 8 : 2 * oldCapacity ); + int newSize = newCapacity * elementSize; + *data = GrowAlloc( *data, oldSize, newSize ); + *capacity = newCapacity; + } + return (char*)*data + ( *count )++ * elementSize; +} + +static inline int RemoveHelper( void* data, int* count, int index, int elementSize ) +{ + assert( 0 <= index && index < *count && "Array index out of bounds" ); + + ( *count )--; + if ( index != *count ) + { + memcpy( (char*)data + index * elementSize, (char*)data + ( *count ) * elementSize, elementSize ); + return *count; + } + + return NULL_INDEX; +} + +#define Array_Clear( a ) \ + do \ + { \ + ( a ).count = 0; \ + } \ + while ( 0 ) + +#define Array_ByteCount( a ) ( ( a ).capacity * (int)sizeof( *( a ).data ) ) + +DeclareArrayNative( int ); diff --git a/samples/data/map03.svg b/samples/data/map03.svg index dd7f564e9..733d3e8a1 100644 --- a/samples/data/map03.svg +++ b/samples/data/map03.svg @@ -7,7 +7,7 @@ viewBox="0 0 297 210" version="1.1" id="svg1" - inkscape:version="1.3.2 (091e20e, 2023-11-25, custom)" + inkscape:version="1.4.3 (0d15f75, 2025-12-25)" sodipodi:docname="map03.svg" xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape" xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd" @@ -25,8 +25,8 @@ inkscape:document-units="mm" showgrid="true" inkscape:zoom="4.8302894" - inkscape:cx="977.06361" - inkscape:cy="573.98217" + inkscape:cx="401.94279" + inkscape:cy="640.02376" inkscape:window-width="3840" inkscape:window-height="2131" inkscape:window-x="-9" @@ -58,7 +58,7 @@ id="layer1"> diff --git a/samples/data/map04.svg b/samples/data/map04.svg new file mode 100644 index 000000000..4044de8b1 --- /dev/null +++ b/samples/data/map04.svg @@ -0,0 +1,66 @@ + + + + + + + + + + + + diff --git a/samples/data/map01.svg b/samples/data/map_left.svg similarity index 60% rename from samples/data/map01.svg rename to samples/data/map_left.svg index caa22d349..13261dc05 100644 --- a/samples/data/map01.svg +++ b/samples/data/map_left.svg @@ -7,8 +7,8 @@ viewBox="0 0 328.58334 95.75" version="1.1" id="svg1" - inkscape:version="1.3.2 (091e20e, 2023-11-25, custom)" - sodipodi:docname="map01.svg" + inkscape:version="1.4.3 (0d15f75, 2025-12-25)" + sodipodi:docname="map_left.svg" xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape" xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd" xmlns="http://www.w3.org/2000/svg" @@ -24,22 +24,22 @@ inkscape:deskcolor="#d1d1d1" inkscape:document-units="mm" showgrid="true" - inkscape:zoom="1.7077652" - inkscape:cx="478.69578" - inkscape:cy="211.68015" - inkscape:window-width="3840" - inkscape:window-height="2131" - inkscape:window-x="-9" - inkscape:window-y="-9" - inkscape:window-maximized="1" + inkscape:zoom="6.0377018" + inkscape:cx="646.27239" + inkscape:cy="348.80822" + inkscape:window-width="1188" + inkscape:window-height="1224" + inkscape:window-x="169" + inkscape:window-y="96" + inkscape:window-maximized="0" inkscape:current-layer="layer1"> + sodipodi:nodetypes="ccccccccccccccccccccccccccccccccccccccccccccccccccccccccc" /> diff --git a/samples/data/map_right.svg b/samples/data/map_right.svg new file mode 100644 index 000000000..5376313aa --- /dev/null +++ b/samples/data/map_right.svg @@ -0,0 +1,66 @@ + + + + + + + + + + + + diff --git a/samples/doohickey.cpp b/samples/doohickey.cpp index 6e0fd7f3a..c1fb349c1 100644 --- a/samples/doohickey.cpp +++ b/samples/doohickey.cpp @@ -89,9 +89,9 @@ void Doohickey::Despawn() { assert( m_isSpawned == true ); - b2DestroyJoint( m_axleId1, false ); - b2DestroyJoint( m_axleId2, false ); - b2DestroyJoint( m_sliderId, false ); + b2DestroyJoint( m_axleId1 ); + b2DestroyJoint( m_axleId2 ); + b2DestroyJoint( m_sliderId ); b2DestroyBody( m_wheelId1 ); b2DestroyBody( m_wheelId2 ); diff --git a/samples/draw.c b/samples/draw.c index 4fe7070fe..726458b82 100644 --- a/samples/draw.c +++ b/samples/draw.c @@ -270,20 +270,18 @@ void RenderBackground( Background* background, Camera* camera ) #define POINT_BATCH_SIZE 2048 -typedef struct +typedef struct PointData { b2Vec2 position; float size; RGBA8 rgba; } PointData; -ARRAY_DECLARE( PointData ); -ARRAY_INLINE( PointData ); -ARRAY_SOURCE( PointData ); +DeclareArray( PointData ); typedef struct { - PointDataArray points; + Array( PointData ) points; GLuint vaoId; GLuint vboId; GLuint programId; @@ -293,7 +291,7 @@ typedef struct PointRender CreatePointDrawData() { PointRender render = { 0 }; - render.points = PointDataArray_Create( POINT_BATCH_SIZE ); + Array_CreateN( render.points, POINT_BATCH_SIZE ); render.programId = CreateProgramFromStrings( k_point_vs, k_point_fs ); render.projectionUniform = glGetUniformLocation( render.programId, "projectionMatrix" ); int vertexAttribute = 0; @@ -341,7 +339,7 @@ void DestroyPointDrawData( PointRender* render ) glDeleteProgram( render->programId ); } - PointDataArray_Destroy( &render->points ); + Array_Destroy( render->points ); *render = (PointRender){ 0 }; } @@ -349,7 +347,8 @@ void DestroyPointDrawData( PointRender* render ) void AddPoint( PointRender* render, b2Vec2 v, float size, b2HexColor c ) { RGBA8 rgba = MakeRGBA8( c, 1.0f ); - PointDataArray_Push( &render->points, (PointData){ v, size, rgba } ); + PointData data = { v, size, rgba }; + Array_Push( render->points, data ); } void FlushPoints( PointRender* render, Camera* camera ) @@ -394,19 +393,17 @@ void FlushPoints( PointRender* render, Camera* camera ) #define LINE_BATCH_SIZE ( 2 * 2048 ) -typedef struct +typedef struct VertexData { b2Vec2 position; RGBA8 rgba; } VertexData; -ARRAY_DECLARE( VertexData ); -ARRAY_INLINE( VertexData ); -ARRAY_SOURCE( VertexData ); +DeclareArray( VertexData ); typedef struct { - VertexDataArray points; + Array( VertexData ) points; GLuint vaoId; GLuint vboId; GLuint programId; @@ -416,7 +413,7 @@ typedef struct LineRender CreateLineRender() { LineRender render = { 0 }; - render.points = VertexDataArray_Create( LINE_BATCH_SIZE ); + Array_CreateN( render.points, LINE_BATCH_SIZE ); render.programId = CreateProgramFromStrings( k_line_vs, k_line_fs ); render.projectionUniform = glGetUniformLocation( render.programId, "projectionMatrix" ); int vertexAttribute = 0; @@ -462,7 +459,7 @@ void DestroyLineRender( LineRender* render ) glDeleteProgram( render->programId ); } - VertexDataArray_Destroy( &render->points ); + Array_Destroy( render->points ); *render = (LineRender){ 0 }; } @@ -470,8 +467,10 @@ void DestroyLineRender( LineRender* render ) void AddLine( LineRender* render, b2Vec2 p1, b2Vec2 p2, b2HexColor c ) { RGBA8 rgba = MakeRGBA8( c, 1.0f ); - VertexDataArray_Push( &render->points, (VertexData){ p1, rgba } ); - VertexDataArray_Push( &render->points, (VertexData){ p2, rgba } ); + VertexData v1 = { p1, rgba }; + VertexData v2 = { p2, rgba }; + Array_Push( render->points, v1 ); + Array_Push( render->points, v2 ); } void FlushLines( LineRender* render, Camera* camera ) @@ -523,20 +522,18 @@ void FlushLines( LineRender* render, Camera* camera ) #define CIRCLE_BATCH_SIZE 2048 -typedef struct +typedef struct CircleData { b2Vec2 position; float radius; RGBA8 rgba; } CircleData; -ARRAY_DECLARE( CircleData ); -ARRAY_INLINE( CircleData ); -ARRAY_SOURCE( CircleData ); +DeclareArray( CircleData ); typedef struct { - CircleDataArray circles; + Array( CircleData ) circles; GLuint vaoId; GLuint vboIds[2]; GLuint programId; @@ -547,7 +544,7 @@ typedef struct CircleRender CreateCircles() { CircleRender render = { 0 }; - render.circles = CircleDataArray_Create( CIRCLE_BATCH_SIZE ); + Array_CreateN( render.circles, CIRCLE_BATCH_SIZE ); render.programId = CreateProgramFromStrings( k_circle_vs, k_circle_fs ); render.projectionUniform = glGetUniformLocation( render.programId, "projectionMatrix" ); render.pixelScaleUniform = glGetUniformLocation( render.programId, "pixelScale" ); @@ -609,7 +606,7 @@ void DestroyCircles( CircleRender* render ) glDeleteProgram( render->programId ); } - CircleDataArray_Destroy( &render->circles ); + Array_Destroy( render->circles ); *render = (CircleRender){ 0 }; } @@ -617,7 +614,8 @@ void DestroyCircles( CircleRender* render ) void AddCircle( CircleRender* render, b2Vec2 center, float radius, b2HexColor color ) { RGBA8 rgba = MakeRGBA8( color, 1.0f ); - CircleDataArray_Push( &render->circles, (CircleData){ center, radius, rgba } ); + CircleData c = { center, radius, rgba }; + Array_Push( render->circles, c ); } void FlushCircles( CircleRender* render, Camera* camera ) @@ -665,16 +663,14 @@ void FlushCircles( CircleRender* render, Camera* camera ) render->circles.count = 0; } -typedef struct +typedef struct SolidCircle { b2Transform transform; float radius; RGBA8 rgba; } SolidCircle; -ARRAY_DECLARE( SolidCircle ); -ARRAY_INLINE( SolidCircle ); -ARRAY_SOURCE( SolidCircle ); +DeclareArray( SolidCircle ); #define SOLID_CIRCLE_BATCH_SIZE 2048 @@ -683,7 +679,7 @@ ARRAY_SOURCE( SolidCircle ); // https://www.g-truc.net/post-0666.html typedef struct { - SolidCircleArray circles; + Array( SolidCircle ) circles; GLuint vaoId; GLuint vboIds[2]; GLuint programId; @@ -694,7 +690,7 @@ typedef struct SolidCircles CreateSolidCircles() { SolidCircles render = { 0 }; - render.circles = SolidCircleArray_Create( SOLID_CIRCLE_BATCH_SIZE ); + Array_CreateN( render.circles, SOLID_CIRCLE_BATCH_SIZE ); render.programId = CreateProgramFromStrings( k_solid_circle_vs, k_solid_circle_fs ); render.projectionUniform = glGetUniformLocation( render.programId, "projectionMatrix" ); render.pixelScaleUniform = glGetUniformLocation( render.programId, "pixelScale" ); @@ -757,7 +753,7 @@ void DestroySolidCircles( SolidCircles* render ) glDeleteProgram( render->programId ); } - SolidCircleArray_Destroy( &render->circles ); + Array_Destroy( render->circles ); *render = (SolidCircles){ 0 }; } @@ -765,7 +761,8 @@ void DestroySolidCircles( SolidCircles* render ) void AddSolidCircle( SolidCircles* render, b2Transform transform, float radius, b2HexColor color ) { RGBA8 rgba = MakeRGBA8( color, 1.0f ); - SolidCircleArray_Push( &render->circles, (SolidCircle){ transform, radius, rgba } ); + SolidCircle c = { transform, radius, rgba }; + Array_Push( render->circles, c ); } void FlushSolidCircles( SolidCircles* render, Camera* camera ) @@ -813,7 +810,7 @@ void FlushSolidCircles( SolidCircles* render, Camera* camera ) render->circles.count = 0; } -typedef struct +typedef struct Capsule { b2Transform transform; float radius; @@ -821,16 +818,14 @@ typedef struct RGBA8 rgba; } Capsule; -ARRAY_DECLARE( Capsule ); -ARRAY_INLINE( Capsule ); -ARRAY_SOURCE( Capsule ); +DeclareArray( Capsule ); #define CAPSULE_BATCH_SIZE 2048 // Draw capsules using SDF-based shader typedef struct { - CapsuleArray capsules; + Array( Capsule ) capsules; GLuint vaoId; GLuint vboIds[2]; GLuint programId; @@ -841,7 +836,7 @@ typedef struct Capsules CreateCapsules() { Capsules render = { 0 }; - render.capsules = CapsuleArray_Create( CAPSULE_BATCH_SIZE ); + Array_CreateN( render.capsules, CAPSULE_BATCH_SIZE ); render.programId = CreateProgramFromStrings( k_solid_capsule_vs, k_solid_capsule_fs ); render.projectionUniform = glGetUniformLocation( render.programId, "projectionMatrix" ); render.pixelScaleUniform = glGetUniformLocation( render.programId, "pixelScale" ); @@ -906,7 +901,7 @@ void DestroyCapsules( Capsules* render ) glDeleteProgram( render->programId ); } - CapsuleArray_Destroy( &render->capsules ); + Array_Destroy( render->capsules ); *render = (Capsules){ 0 }; } @@ -929,7 +924,8 @@ void AddCapsule( Capsules* render, b2Vec2 p1, b2Vec2 p2, float radius, b2HexColo RGBA8 rgba = MakeRGBA8( c, 1.0f ); - CapsuleArray_Push( &render->capsules, (Capsule){ transform, radius, length, rgba } ); + Capsule capsule = { transform, radius, length, rgba }; + Array_Push( render->capsules, capsule ); } void FlushCapsules( Capsules* render, Camera* camera ) @@ -977,7 +973,7 @@ void FlushCapsules( Capsules* render, Camera* camera ) render->capsules.count = 0; } -typedef struct +typedef struct Polygon { b2Transform transform; b2Vec2 p1, p2, p3, p4, p5, p6, p7, p8; @@ -988,16 +984,14 @@ typedef struct RGBA8 color; } Polygon; -ARRAY_DECLARE( Polygon ); -ARRAY_INLINE( Polygon ); -ARRAY_SOURCE( Polygon ); +DeclareArray( Polygon ); #define POLYGON_BATCH_SIZE 2048 // Rounded and non-rounded convex polygons using an SDF-based shader. -typedef struct +typedef struct Polygons { - PolygonArray polygons; + Array( Polygon ) polygons; GLuint vaoId; GLuint vboIds[2]; GLuint programId; @@ -1008,7 +1002,7 @@ typedef struct Polygons CreatePolygons() { Polygons render = { 0 }; - render.polygons = PolygonArray_Create( 10 * POLYGON_BATCH_SIZE ); + Array_CreateN( render.polygons, 10 * POLYGON_BATCH_SIZE ); render.programId = CreateProgramFromStrings( k_solid_polygon_vs, k_solid_polygon_fs ); render.projectionUniform = glGetUniformLocation( render.programId, "projectionMatrix" ); render.pixelScaleUniform = glGetUniformLocation( render.programId, "pixelScale" ); @@ -1090,7 +1084,7 @@ void DestroyPolygons( Polygons* render ) glDeleteProgram( render->programId ); } - PolygonArray_Destroy( &render->polygons ); + Array_Destroy( render->polygons ); *render = (Polygons){ 0 }; } @@ -1111,7 +1105,7 @@ void AddPolygon( Polygons* render, b2Transform transform, const b2Vec2* points, data.radius = radius; data.color = MakeRGBA8( color, 1.0f ); - PolygonArray_Push( &render->polygons, data ); + Array_Push( render->polygons, data ); } void FlushPolygons( Polygons* render, Camera* camera ) @@ -1244,7 +1238,7 @@ void DrawSolidCircle( Draw* draw, b2WorldTransform transform, b2Vec2 center, flo } void DrawSolidPolygon( Draw* draw, b2WorldTransform transform, const b2Vec2* vertices, int vertexCount, float radius, - b2HexColor color ) + b2HexColor color ) { AddPolygon( &draw->polygons, b2ToRelativeTransform( transform, draw->origin ), vertices, vertexCount, radius, color ); } diff --git a/samples/dynamic_mover.cpp b/samples/dynamic_mover.cpp new file mode 100644 index 000000000..4268ee3ca --- /dev/null +++ b/samples/dynamic_mover.cpp @@ -0,0 +1,363 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "dynamic_mover.h" + +#include "box2d/box2d.h" +#include "box2d/math_functions.h" + +struct CastResult +{ + b2Pos point; + b2Vec2 normal; + b2BodyId bodyId; + float fraction; + bool hit; +}; + +static float CastCallback( b2ShapeId shapeId, b2Pos point, b2Vec2 normal, float fraction, void* context ) +{ + CastResult* result = (CastResult*)context; + result->point = point; + result->normal = normal; + result->bodyId = b2Shape_GetBody( shapeId ); + result->fraction = fraction; + result->hit = true; + return fraction; +} + +DynamicMover::DynamicMover() +{ + m_jumpSpeed = 5.0f; + m_maxSpeed = 6.0f; + m_minSpeed = 0.1f; + m_stopSpeed = 3.0f; + m_accelerate = 20.0f; + m_airSteer = 0.2f; + m_friction = 8.0f; + m_gravityScale = 1.5f; + m_maxGroundForce = 70.0f; + m_maxAirForce = 20.0f; + m_pogoRestLength = 0.9f; + m_pogoHertz = 5.0f; + m_pogoDampingRatio = 0.8f; + m_pogoCompressionScale = 100.0f; + m_pogoTensionScale = 100.0f; + m_minGroundNormalY = 0.7f; + + m_capsule = { { 0.0f, -0.5f }, { 0.0f, 0.5f }, 0.3f }; + + m_filter = b2DefaultFilter(); + m_worldId = b2_nullWorldId; + m_groundId = b2_nullBodyId; + m_moverId = b2_nullBodyId; + m_moverJointId = b2_nullJointId; + m_pogoJointId = b2_nullJointId; + + m_velocity = b2Vec2_zero; + m_onGround = false; + m_walkable = false; + m_jumping = false; + m_jumpTicks = 0; + + m_pogoImpulse = 0.0f; + m_pogoVelocity = 0.0f; + m_pogoLength = 0.0f; + + m_pogoOrigin = b2Pos_zero; + m_pogoTranslation = b2Vec2_zero; + m_pogoFraction = 1.0f; + m_pogoHit = false; + + m_castResult = {}; +} + +void DynamicMover::Create( b2WorldId worldId, const DynamicMoverDef* def ) +{ + m_capsule = def->capsule; + m_filter = def->filter; + m_jumpSpeed = def->jumpSpeed; + m_maxSpeed = def->maxSpeed; + m_minSpeed = def->minSpeed; + m_stopSpeed = def->stopSpeed; + m_accelerate = def->accelerate; + m_airSteer = def->airSteer; + m_friction = def->friction; + m_gravityScale = def->gravityScale; + m_maxGroundForce = def->maxGroundForce; + m_maxAirForce = def->maxAirForce; + m_pogoRestLength = def->pogoRestLength; + m_pogoHertz = def->pogoHertz; + m_pogoDampingRatio = def->pogoDampingRatio; + m_pogoCompressionScale = def->pogoCompressionScale; + m_pogoTensionScale = def->pogoTensionScale; + m_minGroundNormalY = 0.7f; + + m_worldId = worldId; + m_velocity = b2Vec2_zero; + m_onGround = false; + m_walkable = false; + m_jumping = false; + m_pogoImpulse = 0.0f; + m_pogoVelocity = 0.0f; + m_pogoLength = 0.0f; + m_pogoJointId = b2_nullJointId; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + m_groundId = b2CreateBody( worldId, &bodyDef ); + + // Position is the center of the capsule. + bodyDef.type = b2_dynamicBody; + bodyDef.position = def->position; + bodyDef.gravityScale = m_gravityScale; + bodyDef.motionLocks.angularZ = true; + bodyDef.enableSleep = false; + bodyDef.name = "mover"; + + m_moverId = b2CreateBody( worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = def->density; + + // The mover joint and the pogo joint handle all the movement, surface friction would fight them + shapeDef.material.friction = 0.0f; + shapeDef.filter = def->filter; + shapeDef.enablePreSolveEvents = def->enablePreSolveEvents; + + b2CreateCapsuleShape( m_moverId, &shapeDef, &m_capsule ); + + b2MoverJointDef moverDef = b2DefaultMoverJointDef(); + moverDef.linearVelocity = m_velocity; + moverDef.maxVelocityForce = { m_maxGroundForce, 0.0f }; + moverDef.base.bodyIdA = m_groundId; + moverDef.base.bodyIdB = m_moverId; + moverDef.base.collideConnected = true; + + m_moverJointId = b2CreateMoverJoint( worldId, &moverDef ); +} + +void DynamicMover::Destroy() +{ + if ( B2_IS_NON_NULL( m_moverId ) ) + { + b2DestroyBody( m_moverId ); + } + + if ( B2_IS_NON_NULL( m_groundId ) ) + { + b2DestroyBody( m_groundId ); + } + + m_groundId = b2_nullBodyId; + m_moverId = b2_nullBodyId; + + // Joints are implicitly destroyed. + m_moverJointId = b2_nullJointId; + m_pogoJointId = b2_nullJointId; +} + +void DynamicMover::SetGravityScale( float gravityScale ) +{ + m_gravityScale = gravityScale; + + if ( B2_IS_NON_NULL( m_moverId ) ) + { + b2Body_SetGravityScale( m_moverId, gravityScale ); + } +} + +bool DynamicMover::Jump() +{ + if ( m_onGround == false || m_walkable == false ) + { + return false; + } + + float surfaceVelocity = 0.0f; + if (b2Body_IsValid(m_castResult.bodyId)) + { + b2Vec2 v = b2Body_GetWorldPointVelocity( m_castResult.bodyId, m_castResult.point ); + surfaceVelocity = v.y; + } + + float mass = b2Body_GetMass( m_moverId ); + float vy = b2Body_GetLinearVelocity( m_moverId ).y; + + // Remove the pogo constraint velocity but add the surface velocity. + float dv = b2MaxFloat( 0.0f, m_jumpSpeed - vy ) + surfaceVelocity; + b2Body_ApplyLinearImpulseToCenter( m_moverId, { 0.0f, mass * dv }, true ); + + // This removes the pogo step down on the next update. + m_onGround = false; + m_walkable = false; + m_jumping = true; + m_jumpTicks = 0; + return true; +} + +// Movement follows the Quake ground and air model: +// https://github.com/id-Software/Quake/blob/master/QW/client/pmove.c#L390 + +// onGround - controls friction, mover max force, and pogo step-down extension +// walkable - controls air steer +// jumping - blocks the pogo from pulling down +void DynamicMover::Update( float timeStep, float throttle ) +{ + // Reach further while grounded so the spring can find the ground over a step + float stepDownLength = m_pogoRestLength; + float rayLength = m_onGround ? m_pogoRestLength + stepDownLength : m_pogoRestLength; + b2Vec2 translation = { 0.0f, -rayLength }; + + b2Pos position = b2Body_GetPosition( m_moverId ); + b2Pos origin = position + m_capsule.center1; + + b2QueryFilter queryFilter = { + .categoryBits = m_filter.categoryBits, + .maskBits = m_filter.maskBits, + }; + + m_castResult = {}; + b2World_CastRay( m_worldId, origin, translation, queryFilter, CastCallback, &m_castResult ); + + m_pogoOrigin = origin; + m_pogoTranslation = translation; + m_pogoFraction = m_castResult.hit ? m_castResult.fraction : 1.0f; + m_pogoHit = m_castResult.hit; + + // Should jumping end? + if ( m_jumping ) + { + m_jumpTicks += 1; + + // Jump ticks allow time for the jump to leave the ground. Otherwise jumping ends + // when the character approaches the current ground. + if ( m_jumpTicks > 2 && m_castResult.hit && b2Dot( m_velocity, m_castResult.normal ) <= 0.0f ) + { + m_jumping = false; + } + } + + if ( m_castResult.hit ) + { + // The cast can still hit while jumping. + m_onGround = m_jumping == false; + + // Is the ground too steep to walk? + m_walkable = m_castResult.normal.y > m_minGroundNormalY; + } + else + { + m_onGround = false; + m_walkable = false; + } + + m_velocity = b2Body_GetLinearVelocity( m_moverId ); + + // Friction + if ( m_onGround ) + { + float speed = b2Length( m_velocity ); + if ( speed < m_minSpeed ) + { + m_velocity.x = 0.0f; + } + else + { + // Linear damping above stopSpeed and fixed reduction below stopSpeed + float control = speed < m_stopSpeed ? m_stopSpeed : speed; + + // friction has units of 1/time + float drop = control * m_friction * timeStep; + float newSpeed = b2MaxFloat( 0.0f, speed - drop ); + m_velocity.x *= newSpeed / speed; + } + } + + // Mover force + if ( m_onGround ) + { + float maxForce = m_walkable ? m_maxGroundForce : 0.0f; + b2MoverJoint_SetMaxVelocityForce( m_moverJointId, { maxForce, 0.0f } ); + } + else + { + b2MoverJoint_SetMaxVelocityForce( m_moverJointId, { m_maxAirForce, 0.0f } ); + } + + b2Vec2 desiredVelocity = { m_maxSpeed * throttle, 0.0f }; + float desiredSpeed; + b2Vec2 desiredDirection = b2GetLengthAndNormalize( &desiredSpeed, desiredVelocity ); + + if ( desiredSpeed > m_maxSpeed ) + { + desiredSpeed = m_maxSpeed; + } + + // Accelerate + float currentSpeed = b2Dot( m_velocity, desiredDirection ); + float addSpeed = desiredSpeed - currentSpeed; + if ( addSpeed > 0.0f ) + { + float steer = m_walkable ? 1.0f : m_airSteer; + float accelSpeed = steer * m_accelerate * m_maxSpeed * timeStep; + if ( accelSpeed > addSpeed ) + { + accelSpeed = addSpeed; + } + + m_velocity.x += accelSpeed * desiredDirection.x; + } + + // The pogo joint is rebuilt every solve because it may land on a different body + if ( b2Joint_IsValid( m_pogoJointId ) ) + { + m_pogoLength = b2PogoJoint_GetLength( m_pogoJointId ); + m_pogoImpulse = b2PogoJoint_GetImpulse( m_pogoJointId ); + m_pogoVelocity = b2PogoJoint_GetVelocity( m_pogoJointId ); + + b2DestroyJoint( m_pogoJointId ); + m_pogoJointId = b2_nullJointId; + } + + if ( m_castResult.hit == true ) + { + float moverMass = b2Body_GetMass( m_moverId ); + float moverWeight = m_gravityScale * b2Length( b2World_GetGravity( m_worldId ) ) * moverMass; + + b2PogoJointDef pogoDef = b2DefaultPogoJointDef(); + pogoDef.base.localFrameA.p = b2Body_GetLocalPoint( m_castResult.bodyId, m_castResult.point ); + pogoDef.base.localFrameB.p = m_capsule.center1; + pogoDef.normal = m_castResult.normal; + pogoDef.base.bodyIdA = m_castResult.bodyId; + pogoDef.base.bodyIdB = m_moverId; + pogoDef.base.collideConnected = true; + pogoDef.restLength = m_pogoRestLength; + pogoDef.hertz = m_pogoHertz; + pogoDef.dampingRatio = m_pogoDampingRatio; + pogoDef.maxCompressionForce = m_pogoCompressionScale * moverWeight; + + // Warm start from the joint that was just destroyed + pogoDef.impulse = m_pogoImpulse; + pogoDef.velocity = m_pogoVelocity; + + if ( m_jumping ) + { + // Don't allow the pogo to pull down + pogoDef.maxTensionForce = 0.0f; + } + else + { + // The pogo can pull down at a multiple of the gravity force. + pogoDef.maxTensionForce = m_pogoTensionScale * moverWeight; + } + + m_pogoJointId = b2CreatePogoJoint( m_worldId, &pogoDef ); + } + else + { + m_pogoImpulse = 0.0f; + m_pogoVelocity = 0.0f; + } + + b2MoverJoint_SetLinearVelocity( m_moverJointId, m_velocity ); +} diff --git a/samples/dynamic_mover.h b/samples/dynamic_mover.h new file mode 100644 index 000000000..ccdd79d69 --- /dev/null +++ b/samples/dynamic_mover.h @@ -0,0 +1,125 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box2d/collision.h" +#include "box2d/types.h" + +struct DynamicMoverDef +{ + b2Pos position = {}; + b2Capsule capsule = {}; + float density = 1.0f; + b2Filter filter = b2DefaultFilter(); + float jumpSpeed = 7.0f; + float maxSpeed = 6.0f; + float minSpeed = 0.1f; + float stopSpeed = 3.0f; + float accelerate = 20.0f; + float airSteer = 0.5f; + float friction = 8.0f; + float gravityScale = 1.5f; + float maxGroundForce = 70.0f; + float maxAirForce = 20.0f; + float pogoRestLength = 0.9f; + float pogoHertz = 5.0f; + float pogoDampingRatio = 0.8f; + float pogoCompressionScale = 100.0f; + float pogoTensionScale = 100.0f; + bool enablePreSolveEvents = false; +}; + +struct DynamicMoverCastResult +{ + b2Pos point; + b2Vec2 normal; + b2BodyId bodyId; + float fraction; + bool hit; +}; + +// Dynamic character mover. The capsule is a rigid body with a locked rotation, so it +// collides and is pushed like everything else in the world. A mover joint drives it to +// the target velocity and a pogo joint holds it above the ground so it can climb +// steps without touching the capsule. +// +// The dynamic mover should be vendored into your project so you can alter it to meet +// your game's specific needs. +class DynamicMover +{ +public: + DynamicMover(); + + void Create( b2WorldId worldId, const DynamicMoverDef* def ); + void Destroy(); + + // Advance the mover. Throttle is the horizontal input on [-1, 1]. + void Update( float timeStep, float throttle ); + + // Jump if grounded. Returns false when airborne. + bool Jump(); + + // Gravity scale is used to size the pogo forces, so change it here. + void SetGravityScale( float gravityScale ); + + float m_jumpSpeed; + float m_maxSpeed; + float m_minSpeed; + float m_stopSpeed; + float m_accelerate; + + // Fraction of the ground acceleration available in the air. + float m_airSteer; + + // Ground friction in units of 1/time. + float m_friction; + + // A character usually wants to fall faster than the rest of the world. + float m_gravityScale; + + // Force budget the mover joint has for reaching the target velocity. The air value + // is small so the character keeps its momentum while jumping. + float m_maxGroundForce; + float m_maxAirForce; + + float m_pogoRestLength; + float m_pogoHertz; + float m_pogoDampingRatio; + + // Pogo force limits as a multiple of the mover weight. + float m_pogoCompressionScale; + float m_pogoTensionScale; + + // Surfaces steeper than this are not ground. + float m_minGroundNormalY; + + b2Capsule m_capsule; + b2Filter m_filter; + + b2WorldId m_worldId; + b2BodyId m_groundId; + b2BodyId m_moverId; + b2JointId m_moverJointId; + b2JointId m_pogoJointId; + + DynamicMoverCastResult m_castResult; + + b2Vec2 m_velocity; + bool m_onGround; + bool m_walkable; + bool m_jumping; + int m_jumpTicks; + + // Pogo joint state, carried across the joint being recreated each solve. + float m_pogoImpulse; + float m_pogoVelocity; + float m_pogoLength; + + // Ground probe from the last solve, kept for debug drawing. The probe ends at + // the origin plus the fraction of the translation. + b2Pos m_pogoOrigin; + b2Vec2 m_pogoTranslation; + float m_pogoFraction; + bool m_pogoHit; +}; diff --git a/samples/geometric_mover.cpp b/samples/geometric_mover.cpp new file mode 100644 index 000000000..99ffb54ab --- /dev/null +++ b/samples/geometric_mover.cpp @@ -0,0 +1,311 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "geometric_mover.h" + +#include "box2d/box2d.h" +#include "box2d/math_functions.h" + +#include +#include + +struct CastResult +{ + b2Pos point; + b2Vec2 normal; + b2BodyId bodyId; + float fraction; + bool hit; +}; + +static float CastCallback( b2ShapeId shapeId, b2Pos point, b2Vec2 normal, float fraction, void* context ) +{ + CastResult* result = (CastResult*)context; + result->point = point; + result->normal = normal; + result->bodyId = b2Shape_GetBody( shapeId ); + result->fraction = fraction; + result->hit = true; + return fraction; +} + +static bool PlaneResultFcn( b2ShapeId shapeId, const b2PlaneResult* planeResult, void* context ) +{ + assert( planeResult->hit == true ); + + GeometricMover* self = static_cast( context ); + float maxPush = FLT_MAX; + bool clipVelocity = true; + MoverShapeUserData* userData = static_cast( (void*)b2Shape_GetUserData( shapeId ) ); + if ( userData != nullptr ) + { + maxPush = userData->maxPush; + clipVelocity = userData->clipVelocity; + } + + if ( self->m_planeCount < GeometricMover::m_planeCapacity ) + { + assert( b2IsValidPlane( planeResult->plane ) ); + self->m_planes[self->m_planeCount] = { planeResult->plane, maxPush, 0.0f, clipVelocity }; + + // The plane point is relative to the origin passed to the collide call + self->m_planeExtras[self->m_planeCount] = { + .point = b2OffsetPos( self->m_position, planeResult->point ), + .shapeId = shapeId, + }; + self->m_planeCount += 1; + } + + return true; +} + +GeometricMover::GeometricMover() +{ + m_jumpSpeed = 10.0f; + m_maxSpeed = 6.0f; + m_minSpeed = 0.1f; + m_stopSpeed = 3.0f; + m_accelerate = 20.0f; + m_airSteer = 0.2f; + m_friction = 8.0f; + m_gravity = 30.0f; + m_pogoRestLength = 0.9f; + m_pogoHertz = 5.0f; + m_pogoDampingRatio = 0.8f; + m_pogoGroundForce = 50.0f; + m_pushBodies = true; + + m_capsule = { { 0.0f, -0.5f }, { 0.0f, 0.5f }, 0.3f }; + + m_filter = b2DefaultQueryFilter(); + + m_worldId = b2_nullWorldId; + m_position = b2Pos_zero; + m_velocity = b2Vec2_zero; + m_pogoVelocity = 0.0f; + m_minGroundNormalY = 0.7f; + m_onGround = false; + + m_planeCount = 0; + m_totalIterations = 0; + + m_pogoOrigin = b2Pos_zero; + m_pogoTranslation = b2Vec2_zero; + m_pogoFraction = 1.0f; + m_pogoHit = false; +} + +void GeometricMover::Create( b2WorldId worldId, const GeometricMoverDef* def ) +{ + m_capsule = def->capsule; + m_filter = def->filter; + m_jumpSpeed = def->jumpSpeed; + m_maxSpeed = def->maxSpeed; + m_minSpeed = def->minSpeed; + m_stopSpeed = def->stopSpeed; + m_accelerate = def->accelerate; + m_airSteer = def->airSteer; + m_friction = def->friction; + m_gravity = def->gravity; + m_pogoRestLength = def->pogoRestLength; + m_pogoHertz = def->pogoHertz; + m_pogoDampingRatio = def->pogoDampingRatio; + m_minGroundNormalY = 0.7f; + + m_worldId = worldId; + m_position = def->position; + m_velocity = b2Vec2_zero; + m_pogoVelocity = 0.0f; + m_onGround = false; + m_planeCount = 0; + m_totalIterations = 0; + m_pogoFraction = 1.0f; + m_pogoHit = false; +} + +bool GeometricMover::Jump() +{ + if ( m_onGround == false ) + { + return false; + } + + m_velocity.y = m_jumpSpeed; + m_onGround = false; + return true; +} + +// Solve a normal constraint between the mover and each touched rigid body. This is the +// same velocity constraint the solver uses for a contact, except the mover is treated as +// having infinite mass, so all of the impulse lands on the body. +void GeometricMover::PushBodies() +{ + for ( int i = 0; i < m_planeCount; ++i ) + { + b2BodyId bodyId = b2Shape_GetBody( m_planeExtras[i].shapeId ); + if ( b2Body_GetType( bodyId ) != b2_dynamicBody ) + { + continue; + } + + // Plane normals point at the mover + b2Pos point = m_planeExtras[i].point; + b2Vec2 normal = -m_planes[i].plane.normal; + + float invMassA = 0.0f; + + float massB = b2Body_GetMass( bodyId ); + float invMassB = massB > 0.0f ? 1.0f / massB : 0.0f; + + float inertiaB = b2Body_GetRotationalInertia( bodyId ); + float invIB = inertiaB > 0.0f ? 1.0f / inertiaB : 0.0f; + + b2Pos centerB = b2Body_GetWorldCenter( bodyId ); + b2Vec2 rB = point - centerB; + + float rnB = b2Cross( rB, normal ); + float kNormal = invMassA + invMassB + invIB * rnB * rnB; + float normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; + + b2Vec2 vB = b2Body_GetLinearVelocity( bodyId ); + float omegaB = b2Body_GetAngularVelocity( bodyId ); + b2Vec2 vrB = vB + b2CrossSV( omegaB, rB ); + float vn = b2Dot( vrB - m_velocity, normal ); + + // Push only, a separating body is left alone + float impulse = b2MaxFloat( -normalMass * vn, 0.0f ); + + b2Vec2 P = impulse * normal; + m_velocity = b2MulSub( m_velocity, invMassA, P ); + + b2Body_ApplyLinearImpulse( bodyId, P, point, true ); + } +} + +// Movement follows the Quake ground and air model: +// https://github.com/id-Software/Quake/blob/master/QW/client/pmove.c#L390 +void GeometricMover::Update( float timeStep, float throttle ) +{ + // Friction + float speed = b2Length( m_velocity ); + if ( speed < m_minSpeed ) + { + m_velocity.x = 0.0f; + m_velocity.y = 0.0f; + } + else if ( m_onGround ) + { + // Linear damping above stopSpeed and fixed reduction below stopSpeed + float control = speed < m_stopSpeed ? m_stopSpeed : speed; + + // friction has units of 1/time + float drop = control * m_friction * timeStep; + float newSpeed = b2MaxFloat( 0.0f, speed - drop ); + m_velocity *= newSpeed / speed; + } + + b2Vec2 desiredVelocity = { m_maxSpeed * throttle, 0.0f }; + float desiredSpeed; + b2Vec2 desiredDirection = b2GetLengthAndNormalize( &desiredSpeed, desiredVelocity ); + + if ( desiredSpeed > m_maxSpeed ) + { + desiredSpeed = m_maxSpeed; + } + + // Accelerate + float currentSpeed = b2Dot( m_velocity, desiredDirection ); + float addSpeed = desiredSpeed - currentSpeed; + if ( addSpeed > 0.0f ) + { + float steer = m_onGround ? 1.0f : m_airSteer; + float accelSpeed = steer * m_accelerate * m_maxSpeed * timeStep; + if ( accelSpeed > addSpeed ) + { + accelSpeed = addSpeed; + } + + m_velocity += accelSpeed * desiredDirection; + } + + // The probe reaches past the rest length so the spring can be stretched + float rayLength = m_onGround ? 2.0f * m_pogoRestLength : m_pogoRestLength; + b2Vec2 translation = { 0.0f, -rayLength }; + b2Pos origin = m_position + m_capsule.center1; + + CastResult castResult = {}; + b2World_CastRay( m_worldId, origin, translation, m_filter, CastCallback, &castResult ); + + m_pogoOrigin = origin; + m_pogoTranslation = translation; + m_pogoFraction = castResult.hit ? castResult.fraction : 1.0f; + m_pogoHit = castResult.hit; + + // Avoid snapping to ground if still going up + bool haveGround = castResult.hit && castResult.normal.y > m_minGroundNormalY; + + if ( m_onGround == false ) + { + m_onGround = haveGround && m_velocity.y <= 0.01f; + } + else + { + m_onGround = haveGround; + } + + if ( m_onGround == false ) + { + m_velocity.y -= m_gravity * timeStep; + m_pogoVelocity = 0.0f; + } + else + { + m_velocity.y = 0.0f; + + float pogoCurrentLength = castResult.fraction * rayLength; + + float offset = pogoCurrentLength - m_pogoRestLength; + m_pogoVelocity = b2SpringDamper( m_pogoHertz, m_pogoDampingRatio, offset, m_pogoVelocity, timeStep ); + + b2Body_ApplyForce( castResult.bodyId, { 0.0f, -m_pogoGroundForce }, castResult.point, true ); + } + + b2Pos target = m_position + timeStep * m_velocity + timeStep * m_pogoVelocity * b2Vec2{ 0.0f, 1.0f }; + + m_totalIterations = 0; + float tolerance = 0.01f; + + // Don't cast against other movers. + b2QueryFilter castFilter = { + .categoryBits = m_filter.categoryBits, + .maskBits = ( m_filter.maskBits & ~m_filter.categoryBits ), + }; + + for ( int iteration = 0; iteration < 5; ++iteration ) + { + m_planeCount = 0; + + b2World_CollideMover( m_worldId, m_position, &m_capsule, m_filter, PlaneResultFcn, this ); + b2PlaneSolverResult result = b2SolvePlanes( target - m_position, m_planes, m_planeCount ); + + m_totalIterations += result.iterationCount; + + float fraction = b2World_CastMover( m_worldId, m_position, &m_capsule, result.delta, castFilter ); + + b2Vec2 delta = fraction * result.delta; + m_position = m_position + delta; + + if ( b2LengthSquared( delta ) < tolerance * tolerance ) + { + break; + } + } + + if ( m_pushBodies ) + { + PushBodies(); + } + + // Stop pushing into surfaces so speed can't accumulate against a wall + m_velocity = b2ClipVector( m_velocity, m_planes, m_planeCount ); +} diff --git a/samples/geometric_mover.h b/samples/geometric_mover.h new file mode 100644 index 000000000..22c1b2490 --- /dev/null +++ b/samples/geometric_mover.h @@ -0,0 +1,126 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box2d/collision.h" +#include "box2d/types.h" + +struct GeometricMoverDef +{ + b2Pos position = {}; + b2Capsule capsule = { { 0.0f, -0.5f }, { 0.0f, 0.5f }, 0.3f }; + b2QueryFilter filter = b2DefaultQueryFilter(); + float jumpSpeed = 10.0f; + float maxSpeed = 6.0f; + float minSpeed = 0.1f; + float stopSpeed = 3.0f; + float accelerate = 20.0f; + float airSteer = 0.2f; + float friction = 8.0f; + float gravity = 30.0f; + float pogoRestLength = 0.9f; + float pogoHertz = 5.0f; + float pogoDampingRatio = 0.8f; + float pogoCompressionScale = 100.0f; + float pogoTensionScale = 100.0f; +}; + +// Optional shape user data read by GeometryMover when it gathers collision planes. +// Shapes without user data are treated as rigid and fully clipping. +struct MoverShapeUserData +{ + // Distance limit on the push this surface may apply to the mover. FLT_MAX is rigid. + // Use a small value for surfaces that must not shove the mover, such as an elevator + // that would otherwise push it through the floor. + float maxPush; + + // False keeps the velocity along this surface, which is what soft collision needs. + bool clipVelocity; +}; + +// Extra data kept alongside each collision plane so the mover can push rigid bodies. +struct PlaneExtra +{ + b2Pos point; + b2ShapeId shapeId; +}; + +// Geometric character mover. The capsule is moved by casting +// and by solving collision planes, so the game keeps full control of the motion. +// +// A pogo spring holds the capsule above the ground so it can move up steps. +class GeometricMover +{ +public: + GeometricMover(); + + // Place the mover. The capsule and tuning may be changed at any time. + void Create( b2WorldId worldId, const GeometricMoverDef* def ); + + // Advance the mover. Throttle is the horizontal input on [-1, 1]. + void Update( float timeStep, float throttle ); + + // Jump if grounded. Returns false when airborne. + bool Jump(); + + static constexpr int m_planeCapacity = 8; + + float m_jumpSpeed; + float m_maxSpeed; + float m_minSpeed; + float m_stopSpeed; + float m_accelerate; + + // Fraction of the ground acceleration available in the air. + float m_airSteer; + + // Ground friction in units of 1/time. + float m_friction; + + // The mover is not simulated so it needs its own gravity. + float m_gravity; + + float m_pogoRestLength; + float m_pogoHertz; + float m_pogoDampingRatio; + + // Downward force applied to whatever the pogo stick is resting on. + float m_pogoGroundForce; + + // Surfaces steeper than this are not ground. + float m_minGroundNormalY; + + // Push touched rigid bodies out of the way by solving a normal constraint against + // each collision plane. Without this the mover slides along a crate instead of + // shoving it. + bool m_pushBodies; + + // The capsule is relative to the mover position. + b2Capsule m_capsule; + + b2QueryFilter m_filter; + + b2WorldId m_worldId; + + // The center of the capsule. + b2Pos m_position; + + b2Vec2 m_velocity; + float m_pogoVelocity; + bool m_onGround; + + // Planes from the last solve, kept for debug drawing. + b2CollisionPlane m_planes[m_planeCapacity] = {}; + PlaneExtra m_planeExtras[m_planeCapacity] = {}; + int m_planeCount; + int m_totalIterations; + + b2Pos m_pogoOrigin; + b2Vec2 m_pogoTranslation; + float m_pogoFraction; + bool m_pogoHit; + +private: + void PushBodies(); +}; diff --git a/samples/main.cpp b/samples/main.cpp index 24c938def..6ea16a37f 100644 --- a/samples/main.cpp +++ b/samples/main.cpp @@ -39,6 +39,7 @@ #if defined( _MSC_VER ) #include +#if 0 static int MyAllocHook( int allocType, void* userData, size_t size, int blockType, long requestNumber, const unsigned char* filename, int lineNumber ) { @@ -51,6 +52,7 @@ static int MyAllocHook( int allocType, void* userData, size_t size, int blockTyp return 1; } #endif +#endif static SampleContext s_context; static bool s_rightMouseDown = false; @@ -325,7 +327,7 @@ static void KeyCallback( GLFWwindow* window, int key, int scancode, int action, } break; - case GLFW_KEY_SPACE: + case GLFW_KEY_P: s_context.pause = !s_context.pause; break; @@ -614,7 +616,7 @@ int main( int argc, char** argv ) glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); - // s_context.draw.DrawBackground(); + // DrawBackground( s_context.draw, &s_context.camera ); ImGui_ImplOpenGL3_NewFrame(); ImGui_ImplGlfw_NewFrame(); diff --git a/samples/sample.cpp b/samples/sample.cpp index 336f7d25b..08f22f50c 100644 --- a/samples/sample.cpp +++ b/samples/sample.cpp @@ -483,7 +483,7 @@ void Sample::MouseUp( b2Pos p, int button ) { if ( B2_IS_NON_NULL( m_mouseJointId ) && button == GLFW_MOUSE_BUTTON_1 ) { - b2DestroyJoint( m_mouseJointId, true ); + b2DestroyJoint( m_mouseJointId ); m_mouseJointId = b2_nullJointId; b2DestroyBody( m_mouseBodyId ); @@ -1491,7 +1491,7 @@ static void DrawMenuBar( SampleContext* context ) { DrawRow( "Tab", "Show / hide UI" ); DrawRow( "M", "Show / hide diagnostics" ); - DrawRow( "Space", "Pause / resume" ); + DrawRow( "P", "Pause / resume" ); DrawRow( "O", "Single step" ); DrawRow( "R", "Restart sample" ); DrawRow( "[ ]", "Previous / next sample" ); @@ -1708,7 +1708,7 @@ static void DrawInfoPanel( SampleContext* context, float frameTime ) { ImGui::TextColored( MakeColor( b2_colorRed ), "PAUSED" ); ImGui::SameLine(); - ImGui::TextDisabled( "(space)" ); + ImGui::TextDisabled( "(P)" ); ImGui::Separator(); } diff --git a/samples/sample_benchmark.cpp b/samples/sample_benchmark.cpp index e35a05825..78c2f4f2e 100644 --- a/samples/sample_benchmark.cpp +++ b/samples/sample_benchmark.cpp @@ -956,7 +956,7 @@ class BenchmarkSleep : public Sample uint64_t ticks = b2GetTicks(); // This will wake the island - b2DestroyJoint( jointId, true ); + b2DestroyJoint( jointId ); m_wakeTotal += b2GetMillisecondsAndReset( &ticks ); // Put the island back to sleep. It must be split because a constraint was removed. @@ -1837,7 +1837,7 @@ class BenchmarkSensor : public Sample float x = -40.0f * gridSize; for ( int i = 0; i < 81; ++i ) { - b2Polygon box = b2MakeOffsetBox( 0.5f * gridSize, 0.5f * gridSize, { x, y }, b2Rot_identity ); + b2Polygon box = b2MakeOffsetBox( 0.45f * gridSize, 0.45f * gridSize, { x, y }, b2Rot_identity ); b2CreatePolygonShape( groundId, &shapeDef, &box ); x += gridSize; } diff --git a/samples/sample_character.cpp b/samples/sample_character.cpp index 43c9fd331..b75eae64b 100644 --- a/samples/sample_character.cpp +++ b/samples/sample_character.cpp @@ -1,7 +1,9 @@ -// SPDX-FileCopyrightText: 2022 Erin Catto +// SPDX-FileCopyrightText: 2026 Erin Catto // SPDX-License-Identifier: MIT #include "draw.h" +#include "dynamic_mover.h" +#include "geometric_mover.h" #include "sample.h" #include "box2d/box2d.h" @@ -10,53 +12,29 @@ #include #include -struct ShapeUserData -{ - float maxPush; - bool clipVelocity; -}; - enum CollisionBits : uint64_t { StaticBit = 0x0001, MoverBit = 0x0002, DynamicBit = 0x0004, - DebrisBit = 0x0008, AllBits = ~0u, }; -enum PogoShape +// The ground probe is a point, a circle, or a segment, all carried by the proxy. +static void DrawPogo( Draw* draw, b2Pos origin, b2Vec2 translation, float fraction, bool hit ) { - PogoPoint, - PogoCircle, - PogoSegment -}; + b2HexColor color = hit ? b2_colorPlum : b2_colorGray; + b2Vec2 delta = fraction * translation; -struct CastResult -{ - b2Pos point; - b2Vec2 normal; - b2BodyId bodyId; - float fraction; - bool hit; -}; - -static float CastCallback( b2ShapeId shapeId, b2Pos point, b2Vec2 normal, float fraction, void* context ) -{ - CastResult* result = (CastResult*)context; - result->point = point; - result->normal = normal; - result->bodyId = b2Shape_GetBody( shapeId ); - result->fraction = fraction; - result->hit = true; - return fraction; + DrawLine( draw, origin, origin + delta, b2_colorGray ); + DrawPoint( draw, origin + delta, 10.0f, color ); } -class Mover : public Sample +class GeometryMoverSample : public Sample { public: - explicit Mover( SampleContext* context ) + explicit GeometryMoverSample( SampleContext* context ) : Sample( context ) { if ( context->restart == false ) @@ -67,23 +45,27 @@ class Mover : public Sample context->debugDraw.drawJoints = false; - // Mover position is center of the capsule. - m_position = { 2.0f, 8.0f }; - m_velocity = { 0.0f, 0.0f }; - m_capsule = { { 0.0f, -0.5f }, { 0.0f, 0.5f }, 0.3f }; + GeometricMoverDef moverDef; + moverDef.position = { 0.0f, 8.0f }; + moverDef.capsule = { { 0.0f, -0.5f }, { 0.0f, 0.5f }, 0.3f }; + moverDef.filter = { MoverBit, StaticBit | DynamicBit | MoverBit }; + m_mover.Create( m_worldId, &moverDef ); b2BodyId groundId1; { b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.name = "svg"; bodyDef.position = { 0.0f, 0.0f }; groundId1 = b2CreateBody( m_worldId, &bodyDef ); const char* path = - "M 2.6458333,201.08333 H 293.68751 v -47.625 h -2.64584 l -10.58333,7.9375 -13.22916,7.9375 -13.24648,5.29167 " - "-31.73269,7.9375 -21.16667,2.64583 -23.8125,10.58333 H 142.875 v -5.29167 h -5.29166 v 5.29167 H 119.0625 v " - "-2.64583 h -2.64583 v -2.64584 h -2.64584 v -2.64583 H 111.125 v -2.64583 H 84.666668 v -2.64583 h -5.291666 v " - "-2.64584 h -5.291667 v -2.64583 H 68.791668 V 174.625 h -5.291666 v -2.64584 H 52.916669 L 39.6875,177.27083 H " - "34.395833 L 23.8125,185.20833 H 15.875 L 5.2916669,187.85416 V 153.45833 H 2.6458333 v 47.625"; + "M -34.395834,201.08333 H 293.68751 v -47.625 h -2.64584 l -10.58333,7.9375 -13.22916,7.9375 -13.24648,5.29167 " + "-31.73269,7.9375 -21.16667,2.64583 -23.8125,10.58333 -15.875,2e-5 v -3.96875 l -17.19792,2e-5 v 2.64582 l " + "-17.19791,0 v -1.32292 h -3.96875 v -1.32292 h -3.96875 v -1.32292 h -3.96875 v -1.32291 h -3.96875 v -1.32292 " + "h -3.96875 v -1.32292 h -3.96875 v -1.32291 l -3.968754,0 v -1.32292 h -3.96875 v -1.32292 h -3.968751 v " + "-1.32291 h -3.96875 v -1.32292 h -3.96875 v -1.32292 h -3.96875 v -1.32291 h -3.96875 v -1.32292 l -3.96875,0 v " + "-1.32292 h -3.968751 v -1.32291 h -3.96875 l -2e-6,-1.32294 H 52.916669 L 39.6875,177.27083 h -5.291667 l " + "-7.937499,5.29167 H 15.875001 l -47.625002,-50.27083 v -26.45834 h -2.645834 l 10e-7,95.25"; b2Vec2 points[64]; @@ -107,13 +89,12 @@ class Mover : public Sample groundId2 = b2CreateBody( m_worldId, &bodyDef ); const char* path = - "M 2.6458333,201.08333 H 293.68751 l 0,-23.8125 h -23.8125 l 21.16667,21.16667 h -23.8125 l -39.68751,-13.22917 " - "-26.45833,7.9375 -23.8125,2.64583 h -13.22917 l -0.0575,2.64584 h -5.29166 v -2.64583 l -7.86855,-1e-5 " - "-0.0114,-2.64583 h -2.64583 l -2.64583,2.64584 h -7.9375 l -2.64584,2.64583 -2.58891,-2.64584 h -13.28609 v " - "-2.64583 h -2.64583 v -2.64584 l -5.29167,1e-5 v -2.64583 h -2.64583 v -2.64583 l -5.29167,-1e-5 v -2.64583 h " - "-2.64583 v -2.64584 h -5.291667 v -2.64583 H 92.60417 V 174.625 h -5.291667 v -2.64584 l -34.395835,1e-5 " - "-7.9375,-2.64584 -7.9375,-2.64583 -5.291667,-5.29167 H 21.166667 L 13.229167,158.75 5.2916668,153.45833 H " - "2.6458334 l -10e-8,47.625"; + "M 2.6458333,201.08333 H 399.52085 v -23.8125 h -23.8125 l 21.16667,18.52084 -232.83335,-1e-5 -0.0575,2.64584 h " + "-5.29166 v -2.64583 l -7.86855,-1e-5 -0.0114,-2.64583 h -2.64583 l -2.64583,2.64584 h -7.9375 l " + "-2.64584,2.64583 -2.58891,-2.64584 -10.64031,2e-5 v -2.64583 l -7.9375,0 v -2.64584 l -7.9375,0 v -2.64583 l " + "-7.9375,0 v -2.64583 l -7.9375,0 v -2.64583 l -7.937501,-1e-5 v -2.64584 l -7.9375,1e-5 v -2.64583 l " + "-7.937501,-2e-5 V 174.625 l -7.937501,1e-5 v -2.64584 l -5.291669,0 -7.9375,-2.64584 -7.9375,-2.64583 " + "-5.291667,-5.29167 H 21.166667 L 13.229167,158.75 5.2916668,153.45833 H 2.6458334 l -10e-8,47.625"; b2Vec2 points[64]; @@ -131,7 +112,21 @@ class Mover : public Sample } { - b2Polygon box = b2MakeBox( 0.5f, 0.125f ); + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { 32.0f, 4.5f }; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + m_friendlyShape.maxPush = 0.025f; + m_friendlyShape.clipVelocity = false; + + shapeDef.filter = { MoverBit, AllBits, 0 }; + shapeDef.userData = &m_friendlyShape; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreateCapsuleShape( bodyId, &shapeDef, &m_mover.m_capsule ); + } + + { + b2Polygon box = b2MakeBox( 0.55f, 0.125f ); b2ShapeDef shapeDef = b2DefaultShapeDef(); @@ -173,33 +168,20 @@ class Mover : public Sample b2CreateRevoluteJoint( m_worldId, &jointDef ); } - { - b2BodyDef bodyDef = b2DefaultBodyDef(); - bodyDef.position = { 32.0f, 4.5f }; - - b2ShapeDef shapeDef = b2DefaultShapeDef(); - m_friendlyShape.maxPush = 0.025f; - m_friendlyShape.clipVelocity = false; - - shapeDef.filter = { MoverBit, AllBits, 0 }; - shapeDef.userData = &m_friendlyShape; - b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); - b2CreateCapsuleShape( bodyId, &shapeDef, &m_capsule ); - } - + for ( int i = 0; i < 5; ++i ) { b2BodyDef bodyDef = b2DefaultBodyDef(); bodyDef.type = b2_dynamicBody; - bodyDef.position = { 7.0f, 7.0f }; + bodyDef.position = { 7.0f, 7.0f + 0.5f * i }; b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); b2ShapeDef shapeDef = b2DefaultShapeDef(); - shapeDef.filter = { DebrisBit, AllBits, 0 }; + shapeDef.filter = { DynamicBit, AllBits, 0 }; shapeDef.material.restitution = 0.7f; shapeDef.material.rollingResistance = 0.2f; - b2Circle circle = { b2Vec2_zero, 0.3f }; - m_ballId = b2CreateCircleShape( bodyId, &shapeDef, &circle ); + b2Circle circle = { b2Vec2_zero, 0.25f }; + b2CreateCircleShape( bodyId, &shapeDef, &circle ); } { @@ -220,282 +202,491 @@ class Mover : public Sample b2CreatePolygonShape( m_elevatorId, &shapeDef, &box ); } - m_totalIterations = 0; - m_pogoVelocity = 0.0f; - m_onGround = false; + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position.x = 140.0f; + float a = 0.25f; + b2Polygon square = b2MakeSquare( a ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.filter = { DynamicBit, AllBits, 0 }; + for ( int i = 0; i < 10; ++i ) + { + bodyDef.position.y = ( 2.0f * i + 1.0f ) * a + 1.0f; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &square ); + } + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { 160.0f, 5.0f }; + bodyDef.type = b2_dynamicBody; + b2BodyId body = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeBox( 0.1f, 4.0f ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + b2CreatePolygonShape( body, &shapeDef, &box ); + + b2Pos pivot = bodyDef.position + b2Vec2{ 0.0f, 4.0f }; + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.base.bodyIdA = groundId2; + jointDef.base.bodyIdB = body; + jointDef.base.localFrameA.p = b2Body_GetLocalPoint( jointDef.base.bodyIdA, pivot ); + jointDef.base.localFrameB.p = b2Body_GetLocalPoint( jointDef.base.bodyIdB, pivot ); + jointDef.enableMotor = true; + jointDef.motorSpeed = 1.0f; + jointDef.maxMotorTorque = 500.0f; + + b2CreateRevoluteJoint( m_worldId, &jointDef ); + } + m_jumpReleased = true; m_lockCamera = true; - m_planeCount = 0; m_time = 0.0f; } - // https://github.com/id-Software/Quake/blob/master/QW/client/pmove.c#L390 - void SolveMove( float timeStep, float throttle ) + bool DrawControls() override { - // Friction - float speed = b2Length( m_velocity ); - if ( speed < m_minSpeed ) - { - m_velocity.x = 0.0f; - m_velocity.y = 0.0f; - } - else if ( m_onGround ) - { - // Linear damping above stopSpeed and fixed reduction below stopSpeed - float control = speed < m_stopSpeed ? m_stopSpeed : speed; + ImGui::SliderFloat( "Jump Speed", &m_mover.m_jumpSpeed, 0.0f, 40.0f, "%.0f" ); + ImGui::SliderFloat( "Min Speed", &m_mover.m_minSpeed, 0.0f, 1.0f, "%.2f" ); + ImGui::SliderFloat( "Max Speed", &m_mover.m_maxSpeed, 0.0f, 20.0f, "%.0f" ); + ImGui::SliderFloat( "Stop Speed", &m_mover.m_stopSpeed, 0.0f, 10.0f, "%.1f" ); + ImGui::SliderFloat( "Accelerate", &m_mover.m_accelerate, 0.0f, 100.0f, "%.0f" ); + ImGui::SliderFloat( "Friction", &m_mover.m_friction, 0.0f, 10.0f, "%.1f" ); + ImGui::SliderFloat( "Gravity", &m_mover.m_gravity, 0.0f, 100.0f, "%.1f" ); + ImGui::SliderFloat( "Air Steer", &m_mover.m_airSteer, 0.0f, 1.0f, "%.2f" ); + ImGui::SliderFloat( "Pogo Hertz", &m_mover.m_pogoHertz, 0.0f, 30.0f, "%.0f" ); + ImGui::SliderFloat( "Pogo Damping", &m_mover.m_pogoDampingRatio, 0.0f, 4.0f, "%.1f" ); - // friction has units of 1/time - float drop = control * m_friction * timeStep; - float newSpeed = b2MaxFloat( 0.0f, speed - drop ); - m_velocity *= newSpeed / speed; - } + ImGui::Separator(); + + ImGui::Checkbox( "Push Bodies", &m_mover.m_pushBodies ); + ImGui::Checkbox( "Lock Camera", &m_lockCamera ); - b2Vec2 desiredVelocity = { m_maxSpeed * throttle, 0.0f }; - float desiredSpeed; - b2Vec2 desiredDirection = b2GetLengthAndNormalize( &desiredSpeed, desiredVelocity ); + return true; + } + + void Step() override + { + DrawScreenTextLine( "left/right/jump = A/D/Space" ); - if ( desiredSpeed > m_maxSpeed ) + bool pause = false; + if ( m_context->pause ) { - desiredSpeed = m_maxSpeed; + pause = m_context->singleStep != true; } - if ( m_onGround ) + float timeStep = m_context->hertz > 0.0f ? 1.0f / m_context->hertz : 0.0f; + if ( pause ) { - m_velocity.y = 0.0f; + timeStep = 0.0f; } - // Accelerate - float currentSpeed = b2Dot( m_velocity, desiredDirection ); - float addSpeed = desiredSpeed - currentSpeed; - if ( addSpeed > 0.0f ) + if ( timeStep > 0.0f ) { - float steer = m_onGround ? 1.0f : m_airSteer; - float accelSpeed = steer * m_accelerate * m_maxSpeed * timeStep; - if ( accelSpeed > addSpeed ) + b2Pos point = { + .x = m_elevatorBase.x, + .y = m_elevatorAmplitude * cosf( 1.0f * m_time + B2_PI ) + m_elevatorBase.y, + }; + + bool wake = true; + b2Body_SetTargetTransform( m_elevatorId, { point, b2Rot_identity }, timeStep, wake ); + } + + m_time += timeStep; + + Sample::Step(); + + if ( pause == false ) + { + float throttle = 0.0f; + + if ( glfwGetKey( m_context->window, GLFW_KEY_A ) ) + { + throttle -= 1.0f; + } + + if ( glfwGetKey( m_context->window, GLFW_KEY_D ) ) + { + throttle += 1.0f; + } + + if ( glfwGetKey( m_context->window, GLFW_KEY_SPACE ) ) + { + if ( m_jumpReleased && m_mover.Jump() ) + { + m_jumpReleased = false; + } + } + else { - accelSpeed = addSpeed; + m_jumpReleased = true; } - m_velocity += accelSpeed * desiredDirection; + m_mover.Update( timeStep, throttle ); } - m_velocity.y -= m_gravity * timeStep; + DrawPogo( m_draw, m_mover.m_pogoOrigin, m_mover.m_pogoTranslation, m_mover.m_pogoFraction, m_mover.m_pogoHit ); - float pogoRestLength = 3.0f * m_capsule.radius; - float rayLength = pogoRestLength + m_capsule.radius; - b2Circle circle = { b2Vec2_zero, 0.5f * m_capsule.radius }; - b2Vec2 segmentOffset = { 0.75f * m_capsule.radius, 0.0f }; - b2Segment segment = { - .point1 = -segmentOffset, - .point2 = segmentOffset, - }; + DrawTransform( m_draw, { m_mover.m_position, b2Rot_identity }, 0.25f ); - b2ShapeProxy proxy = {}; - b2Vec2 translation; - b2QueryFilter pogoFilter = { MoverBit, StaticBit | DynamicBit }; - CastResult castResult = {}; + b2Capsule capsule = m_mover.m_capsule; - if ( m_pogoShape == PogoPoint ) - { - proxy = b2MakeProxy( &b2Vec2_zero, 1, 0.0f ); - translation = { 0.0f, -rayLength }; - } - else if ( m_pogoShape == PogoCircle ) + int count = m_mover.m_planeCount; + for ( int i = 0; i < count; ++i ) { - proxy = b2MakeProxy( &b2Vec2_zero, 1, circle.radius ); - translation = { 0.0f, -rayLength + circle.radius }; + b2Plane plane = m_mover.m_planes[i].plane; + b2Pos p1 = m_mover.m_position + ( plane.offset - capsule.radius ) * plane.normal; + b2Pos p2 = p1 + 0.1f * plane.normal; + DrawPoint( m_draw, p1, 5.0f, b2_colorYellow ); + DrawLine( m_draw, p1, p2, b2_colorYellow ); } - else + + b2Pos p1 = m_mover.m_position + capsule.center1; + b2Pos p2 = m_mover.m_position + capsule.center2; + + b2HexColor color = m_mover.m_onGround ? b2_colorOrange : b2_colorAquamarine; + DrawCapsule( m_draw, p1, p2, capsule.radius, color ); + DrawLine( m_draw, m_mover.m_position, m_mover.m_position + m_mover.m_velocity, b2_colorPurple ); + + b2Pos p = m_mover.m_position; + DrawScreenTextLine( "position %.2f %.2f", p.x, p.y ); + DrawScreenTextLine( "velocity %.2f %.2f", m_mover.m_velocity.x, m_mover.m_velocity.y ); + DrawScreenTextLine( "iterations %d", m_mover.m_totalIterations ); + + if ( m_lockCamera ) { - proxy = b2MakeProxy( &segment.point1, 2, 0.0f ); - translation = { 0.0f, -rayLength }; + m_camera->center.x = m_mover.m_position.x; } + } + + static Sample* Create( SampleContext* context ) + { + return new GeometryMoverSample( context ); + } + + static constexpr b2Vec2 m_elevatorBase = { 112.0f, 10.0f }; + static constexpr float m_elevatorAmplitude = 4.0f; + + GeometricMover m_mover; + b2BodyId m_elevatorId; + MoverShapeUserData m_friendlyShape; + MoverShapeUserData m_elevatorShape; + float m_time; + bool m_jumpReleased; + bool m_lockCamera; +}; - b2Pos origin = m_position + m_capsule.center1; - b2World_CastShape( m_worldId, origin, &proxy, translation, pogoFilter, CastCallback, &castResult ); +static int sampleGeometricMover = RegisterSample( "Character", "Geometric Mover", GeometryMoverSample::Create ); - // Avoid snapping to ground if still going up - if ( m_onGround == false ) +class DynamicMoverSample : public Sample +{ +public: + explicit DynamicMoverSample( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) { - m_onGround = castResult.hit && m_velocity.y <= 0.01f; + m_camera->center = { 20.0f, 9.0f }; + m_camera->zoom = 10.0f; } - else + + context->debugDraw.drawJoints = false; + + // This is the mover the player controls { - m_onGround = castResult.hit; + DynamicMoverDef def; + def.position = { 0.0f, 8.0f }; + def.capsule = { { 0.0f, -0.5f }, { 0.0f, 0.5f }, 0.3f }; + def.filter = { MoverBit, StaticBit | DynamicBit }; + def.enablePreSolveEvents = true; + m_mover.Create( m_worldId, &def ); } - if ( castResult.hit == false ) + m_elevatorId = b2_nullBodyId; + + b2BodyId groundId1; { - m_pogoVelocity = 0.0f; + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.name = "svg"; + bodyDef.position = { 0.0f, 0.0f }; + groundId1 = b2CreateBody( m_worldId, &bodyDef ); - b2Vec2 delta = translation; - DrawLine( m_draw, origin, origin + delta, b2_colorGray ); +#if 0 + const char* path = "M -34.4,201.08 H 293.69 V 34.4 L -21.17,198.44 H -31.75 l -0,-92.6 h -2.65 l 0,95.25"; +#else + const char* path = + "M -34.395834,201.08333 H 293.68751 v -47.625 h -2.64584 l -10.58333,7.9375 -13.22916,7.9375 -13.24648,5.29167 " + "-31.73269,7.9375 -21.16667,2.64583 -23.8125,13.22919 -15.875,2e-5 0,-6.61461 -17.19792,2e-5 v 2.64582 h " + "-17.19791 v -1.32292 h -3.96875 v -1.32292 h -3.96875 v -1.32292 h -3.96875 v -1.32291 h -3.96875 v -1.32292 h " + "-3.96875 v -1.32292 h -3.96875 v -1.32291 h -3.968754 v -1.32292 h -3.96875 v -1.32292 h -3.968751 v -1.32291 h " + "-3.96875 v -1.32292 h -3.96875 v -1.32292 h -3.96875 v -1.32291 h -3.96875 v -1.32292 h -3.96875 v -1.32292 h " + "-3.968751 v -1.32291 h -3.96875 l -2e-6,-1.32294 H 52.916669 L 39.6875,177.27083 h -5.291667 l " + "-7.937499,5.29167 H 15.875001 l -47.625002,-50.27083 v -26.45834 h -2.645834 l 10e-7,95.25"; +#endif - if ( m_pogoShape == PogoPoint ) - { - DrawPoint( m_draw, origin + delta, 10.0f, b2_colorGray ); - } - else if ( m_pogoShape == PogoCircle ) - { - DrawCircle( m_draw, origin + delta, circle.radius, b2_colorGray ); - } - else - { - DrawLine( m_draw, origin + segment.point1 + delta, origin + segment.point2 + delta, b2_colorGray ); - } + b2Vec2 points[64]; + + b2Vec2 offset = { -50.0f, -200.0f }; + float scale = 0.2f; + + int count = ParsePath( path, offset, points, 64, scale, false ); + + b2ChainDef chainDef = b2DefaultChainDef(); + chainDef.points = points; + chainDef.count = count; + chainDef.isLoop = true; + + b2CreateChain( groundId1, &chainDef ); } - else + + b2BodyId groundId2; { - float pogoCurrentLength = castResult.fraction * rayLength; + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { 98.0f, 0.0f }; + groundId2 = b2CreateBody( m_worldId, &bodyDef ); + + const char* path = + "M 2.6458333,201.08333 H 399.52085 v -23.8125 h -23.8125 l 21.16667,18.52084 -232.83335,-1e-5 -0.0575,2.64584 h " + "-5.29166 v -2.64583 l -7.86855,-1e-5 -0.0114,-2.64583 h -2.64583 l -2.64583,2.64584 h -7.9375 l " + "-2.64584,2.64583 -2.58891,-2.64584 -10.64031,2e-5 v -2.64583 l -7.9375,0 v -2.64584 l -7.9375,0 v -2.64583 l " + "-7.9375,0 v -2.64583 l -7.9375,0 v -2.64583 l -7.937501,-1e-5 v -2.64584 l -7.9375,1e-5 v -2.64583 l " + "-7.937501,-2e-5 V 174.625 l -7.937501,1e-5 v -2.64584 l -5.291669,0 -7.9375,-2.64584 -7.9375,-2.64583 " + "-5.291667,-5.29167 H 21.166667 L 13.229167,158.75 5.2916668,153.45833 H 2.6458334 l -10e-8,47.625"; - float offset = pogoCurrentLength - pogoRestLength; - m_pogoVelocity = b2SpringDamper( m_pogoHertz, m_pogoDampingRatio, offset, m_pogoVelocity, timeStep ); + b2Vec2 points[64]; - b2Vec2 delta = castResult.fraction * translation; - DrawLine( m_draw, origin, origin + delta, b2_colorGray ); + b2Vec2 offset = { 0.0f, -200.0f }; + float scale = 0.2f; - if ( m_pogoShape == PogoPoint ) - { - DrawPoint( m_draw, origin + delta, 10.0f, b2_colorPlum ); - } - else if ( m_pogoShape == PogoCircle ) - { - DrawCircle( m_draw, origin + delta, circle.radius, b2_colorPlum ); - } - else - { - DrawLine( m_draw, origin + segment.point1 + delta, origin + segment.point2 + delta, b2_colorPlum ); - } + int count = ParsePath( path, offset, points, 64, scale, false ); + + b2ChainDef chainDef = b2DefaultChainDef(); + chainDef.points = points; + chainDef.count = count; + chainDef.isLoop = true; - b2Body_ApplyForce( castResult.bodyId, { 0.0f, -50.0f }, castResult.point, true ); + b2CreateChain( groundId2, &chainDef ); } - DrawTransform( m_draw, { m_position, b2Rot_identity }, 0.25f ); + { + b2Polygon box = b2MakeBox( 0.55f, 0.125f ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.maxMotorTorque = 10.0f; + jointDef.enableMotor = true; + jointDef.hertz = 3.0f; + jointDef.dampingRatio = 0.8f; + jointDef.enableSpring = true; - b2Pos target = m_position + timeStep * m_velocity + timeStep * m_pogoVelocity * b2Vec2{ 0.0f, 1.0f }; + float xBase = 48.7f; + float yBase = 9.2f; + int count = 50; + b2BodyId prevBodyId = groundId1; + for ( int i = 0; i < count; ++i ) + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { xBase + 0.5f + 1.0f * i, yBase }; + bodyDef.angularDamping = 0.2f; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); - // Mover overlap filter - b2QueryFilter collideFilter = { MoverBit, StaticBit | DynamicBit | MoverBit }; + b2Pos pivot = { xBase + 1.0f * i, yBase }; + jointDef.base.bodyIdA = prevBodyId; + jointDef.base.bodyIdB = bodyId; + jointDef.base.localFrameA.p = b2Body_GetLocalPoint( jointDef.base.bodyIdA, pivot ); + jointDef.base.localFrameB.p = b2Body_GetLocalPoint( jointDef.base.bodyIdB, pivot ); + b2CreateRevoluteJoint( m_worldId, &jointDef ); - // Movers don't sweep against other movers, allows for soft collision - b2QueryFilter castFilter = { MoverBit, StaticBit | DynamicBit }; + prevBodyId = bodyId; + } - m_totalIterations = 0; - float tolerance = 0.01f; + b2Pos pivot = { xBase + 1.0f * count, yBase }; + jointDef.base.bodyIdA = prevBodyId; + jointDef.base.bodyIdB = groundId2; + jointDef.base.localFrameA.p = b2Body_GetLocalPoint( jointDef.base.bodyIdA, pivot ); + jointDef.base.localFrameB.p = b2Body_GetLocalPoint( jointDef.base.bodyIdB, pivot ); + b2CreateRevoluteJoint( m_worldId, &jointDef ); + } - for ( int iteration = 0; iteration < 5; ++iteration ) + for ( int i = 0; i < 5; ++i ) { - m_planeCount = 0; + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { 7.0f, 7.0f + 0.5f * i }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); - b2Capsule mover = m_capsule; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.filter = { DynamicBit, AllBits, 0 }; + shapeDef.material.restitution = 0.7f; + shapeDef.material.rollingResistance = 0.2f; - b2World_CollideMover( m_worldId, m_position, &mover, collideFilter, PlaneResultFcn, this ); - b2PlaneSolverResult result = b2SolvePlanes( target - m_position, m_planes, m_planeCount ); + b2Circle circle = { b2Vec2_zero, 0.25f }; + b2CreateCircleShape( bodyId, &shapeDef, &circle ); + } - m_totalIterations += result.iterationCount; + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_kinematicBody; + bodyDef.position = { m_elevatorBase.x, m_elevatorBase.y - m_elevatorAmplitude }; + m_elevatorId = b2CreateBody( m_worldId, &bodyDef ); - float fraction = b2World_CastMover( m_worldId, m_position, &mover, result.translation, castFilter ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.filter = { DynamicBit, AllBits, 0 }; - b2Vec2 delta = fraction * result.translation; - m_position = m_position + delta; + b2Polygon box = b2MakeBox( 2.0f, 0.1f ); + b2CreatePolygonShape( m_elevatorId, &shapeDef, &box ); + } - if ( b2LengthSquared( delta ) < tolerance * tolerance ) + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position.x = 140.0f; + float a = 0.25f; + b2Polygon square = b2MakeSquare( a ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.filter = { DynamicBit, AllBits, 0 }; + for ( int i = 0; i < 10; ++i ) { - break; + bodyDef.position.y = ( 2.0f * i + 1.0f ) * a + 1.0f; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &square ); } } - m_velocity = b2ClipVector( m_velocity, m_planes, m_planeCount ); - } + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { 160.0f, 4.0f }; + bodyDef.type = b2_dynamicBody; + b2BodyId body = b2CreateBody( m_worldId, &bodyDef ); - bool DrawControls() override - { - ImGui::SliderFloat( "Jump Speed", &m_jumpSpeed, 0.0f, 40.0f, "%.0f" ); - ImGui::SliderFloat( "Min Speed", &m_minSpeed, 0.0f, 1.0f, "%.2f" ); - ImGui::SliderFloat( "Max Speed", &m_maxSpeed, 0.0f, 20.0f, "%.0f" ); - ImGui::SliderFloat( "Stop Speed", &m_stopSpeed, 0.0f, 10.0f, "%.1f" ); - ImGui::SliderFloat( "Accelerate", &m_accelerate, 0.0f, 100.0f, "%.0f" ); - ImGui::SliderFloat( "Friction", &m_friction, 0.0f, 10.0f, "%.1f" ); - ImGui::SliderFloat( "Gravity", &m_gravity, 0.0f, 100.0f, "%.1f" ); - ImGui::SliderFloat( "Air Steer", &m_airSteer, 0.0f, 1.0f, "%.2f" ); - ImGui::SliderFloat( "Pogo Hertz", &m_pogoHertz, 0.0f, 30.0f, "%.0f" ); - ImGui::SliderFloat( "Pogo Damping", &m_pogoDampingRatio, 0.0f, 4.0f, "%.1f" ); + b2Polygon box = b2MakeBox( 0.1f, 4.0f ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + b2CreatePolygonShape( body, &shapeDef, &box ); - ImGui::Separator(); + b2Pos pivot = bodyDef.position + b2Vec2{ 0.0f, 4.0f }; + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.base.bodyIdA = groundId2; + jointDef.base.bodyIdB = body; + jointDef.base.localFrameA.p = b2Body_GetLocalPoint( jointDef.base.bodyIdA, pivot ); + jointDef.base.localFrameB.p = b2Body_GetLocalPoint( jointDef.base.bodyIdB, pivot ); + jointDef.enableMotor = true; + jointDef.motorSpeed = 1.0f; + jointDef.maxMotorTorque = 500.0f; - ImGui::TextUnformatted( "Pogo Shape" ); - ImGui::RadioButton( "Point", &m_pogoShape, PogoPoint ); - ImGui::RadioButton( "Circle", &m_pogoShape, PogoCircle ); - ImGui::RadioButton( "Segment", &m_pogoShape, PogoSegment ); + b2CreateRevoluteJoint( m_worldId, &jointDef ); + } - ImGui::Separator(); + m_preSolve = true; - ImGui::Checkbox( "Lock Camera", &m_lockCamera ); + // This is used for one-way collision on the kinematic elevator. But pre-solve doesn't work with recording. + if ( m_preSolve ) + { + b2World_SetPreSolveCallback( m_worldId, PreSolveStatic, PreContinuousStatic, this ); + } - return true; + m_jumpReleased = true; + m_lockCamera = true; + m_time = 0.0f; } - static bool PlaneResultFcn( b2ShapeId shapeId, const b2PlaneResult* planeResult, void* context ) + void PreSolve( b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifold ) { - assert( planeResult->hit == true ); + b2BodyId idA = b2Shape_GetBody( shapeIdA ); + b2BodyId idB = b2Shape_GetBody( shapeIdB ); - Mover* self = static_cast( context ); - float maxPush = FLT_MAX; - bool clipVelocity = true; - ShapeUserData* userData = static_cast( (void*)b2Shape_GetUserData( shapeId ) ); - if ( userData != nullptr ) + if ( B2_ID_EQUALS( idA, m_elevatorId ) && B2_ID_EQUALS( idB, m_mover.m_moverId ) ) { - maxPush = userData->maxPush; - clipVelocity = userData->clipVelocity; + if (manifold->normal.y < 0.0f) + { + manifold->pointCount = 0; + } } - - if ( self->m_planeCount < m_planeCapacity ) + else if ( B2_ID_EQUALS( idA, m_mover.m_moverId ) && B2_ID_EQUALS( idB, m_elevatorId ) ) { - assert( b2IsValidPlane( planeResult->plane ) ); - self->m_planes[self->m_planeCount] = { planeResult->plane, maxPush, 0.0f, clipVelocity }; - self->m_planeCount += 1; + if ( manifold->normal.y > 0.0f ) + { + manifold->pointCount = 0; + } } + } - return true; + static void PreSolveStatic( b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifold, void* context ) + { + DynamicMoverSample* sample = (DynamicMoverSample*)context; + sample->PreSolve( shapeIdA, shapeIdB, manifold ); } - static bool Kick( b2ShapeId shapeId, void* context ) + bool PreContinuous( b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Pos point, b2Vec2 normal ) { - Mover* self = (Mover*)context; - b2BodyId bodyId = b2Shape_GetBody( shapeId ); - b2BodyType type = b2Body_GetType( bodyId ); + (void)point; - if ( type != b2_dynamicBody ) + b2BodyId idA = b2Shape_GetBody( shapeIdA ); + b2BodyId idB = b2Shape_GetBody( shapeIdB ); + + if ( B2_ID_EQUALS( idA, m_elevatorId ) && B2_ID_EQUALS( idB, m_mover.m_moverId ) ) { + if ( normal.y < 0.0f ) + { + return false; + } + return true; } - b2Pos center = b2Body_GetWorldCenter( bodyId ); - b2Vec2 direction = b2Normalize( center - self->m_position ); - b2Vec2 impulse = b2Vec2{ 2.0f * direction.x, 2.0f }; - b2Body_ApplyLinearImpulseToCenter( bodyId, impulse, true ); + if ( B2_ID_EQUALS( idA, m_mover.m_moverId ) && B2_ID_EQUALS( idB, m_elevatorId ) ) + { + if ( normal.y > 0.0f ) + { + return false; + } + + return true; + } return true; } - void Keyboard( int key ) override + static bool PreContinuousStatic( b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Pos point, b2Vec2 normal, void* context ) + { + DynamicMoverSample* sample = (DynamicMoverSample*)context; + return sample->PreContinuous( shapeIdA, shapeIdB, point, normal ); + } + + bool DrawControls() override { - if ( key == 'K' ) + ImGui::SliderFloat( "Jump Speed", &m_mover.m_jumpSpeed, 0.0f, 40.0f, "%.0f" ); + ImGui::SliderFloat( "Min Speed", &m_mover.m_minSpeed, 0.0f, 1.0f, "%.2f" ); + ImGui::SliderFloat( "Max Speed", &m_mover.m_maxSpeed, 0.0f, 20.0f, "%.0f" ); + ImGui::SliderFloat( "Stop Speed", &m_mover.m_stopSpeed, 0.0f, 10.0f, "%.1f" ); + ImGui::SliderFloat( "Accelerate", &m_mover.m_accelerate, 0.0f, 100.0f, "%.0f" ); + ImGui::SliderFloat( "Friction", &m_mover.m_friction, 0.0f, 10.0f, "%.1f" ); + + float gravityScale = m_mover.m_gravityScale; + if ( ImGui::SliderFloat( "Gravity Scale", &gravityScale, 0.0f, 4.0f, "%.1f" ) ) { - b2Pos origin = { m_position.x, m_position.y + m_capsule.center1.y - 3.0f * m_capsule.radius }; - float radius = 0.5f; - b2ShapeProxy proxy = b2MakeProxy( &b2Vec2_zero, 1, radius ); - b2QueryFilter filter = { MoverBit, DebrisBit }; - b2World_OverlapShape( m_worldId, origin, &proxy, filter, Kick, this ); - DrawCircle( m_draw, origin, radius, b2_colorGoldenRod ); + m_mover.SetGravityScale( gravityScale ); } - Sample::Keyboard( key ); + ImGui::SliderFloat( "Air Steer", &m_mover.m_airSteer, 0.0f, 1.0f, "%.2f" ); + ImGui::SliderFloat( "Pogo Hertz", &m_mover.m_pogoHertz, 0.0f, 30.0f, "%.0f" ); + ImGui::SliderFloat( "Pogo Damping", &m_mover.m_pogoDampingRatio, 0.0f, 4.0f, "%.1f" ); + + ImGui::Separator(); + + ImGui::Checkbox( "Lock Camera", &m_lockCamera ); + + return true; } void Step() override { - DrawScreenTextLine( "left/right/jump = A/D/W" ); + DrawScreenTextLine( "left/right/jump = A/D/Space" ); bool pause = false; if ( m_context->pause ) @@ -509,7 +700,7 @@ class Mover : public Sample timeStep = 0.0f; } - if ( timeStep > 0.0f ) + if ( timeStep > 0.0f && B2_IS_NON_NULL( m_elevatorId ) ) { b2Pos point = { .x = m_elevatorBase.x, @@ -522,8 +713,6 @@ class Mover : public Sample m_time += timeStep; - Sample::Step(); - if ( pause == false ) { float throttle = 0.0f; @@ -538,12 +727,10 @@ class Mover : public Sample throttle += 1.0f; } - if ( glfwGetKey( m_context->window, GLFW_KEY_W ) ) + if ( glfwGetKey( m_context->window, GLFW_KEY_SPACE ) ) { - if ( m_onGround == true && m_jumpReleased ) + if ( m_jumpReleased && m_mover.Jump() ) { - m_velocity.y = m_jumpSpeed; - m_onGround = false; m_jumpReleased = false; } } @@ -552,73 +739,46 @@ class Mover : public Sample m_jumpReleased = true; } - SolveMove( timeStep, throttle ); + m_mover.Update( timeStep, throttle ); } - int count = m_planeCount; - for ( int i = 0; i < count; ++i ) - { - b2Plane plane = m_planes[i].plane; - b2Pos p1 = m_position + ( plane.offset - m_capsule.radius ) * plane.normal; - b2Pos p2 = p1 + 0.1f * plane.normal; - DrawPoint( m_draw, p1, 5.0f, b2_colorYellow ); - DrawLine( m_draw, p1, p2, b2_colorYellow ); - } + Sample::Step(); - b2Pos p1 = m_position + m_capsule.center1; - b2Pos p2 = m_position + m_capsule.center2; + DrawPogo( m_draw, m_mover.m_pogoOrigin, m_mover.m_pogoTranslation, m_mover.m_pogoFraction, m_mover.m_pogoHit ); - b2HexColor color = m_onGround ? b2_colorOrange : b2_colorAquamarine; - DrawCapsule( m_draw, p1, p2, m_capsule.radius, color ); - DrawLine( m_draw, m_position, m_position + m_velocity, b2_colorPurple ); + b2Pos position = b2Body_GetPosition( m_mover.m_moverId ); - b2Pos p = m_position; - DrawScreenTextLine( "position %.2f %.2f", p.x, p.y ); - DrawScreenTextLine( "velocity %.2f %.2f", m_velocity.x, m_velocity.y ); - DrawScreenTextLine( "iterations %d", m_totalIterations ); + b2HexColor color = m_mover.m_onGround ? b2_colorOrange : b2_colorAquamarine; + DrawLine( m_draw, position, position + m_mover.m_velocity, color ); + + DrawScreenTextLine( "position %.2f %.2f", position.x, position.y ); + DrawScreenTextLine( "velocity %.2f %.2f", m_mover.m_velocity.x, m_mover.m_velocity.y ); + DrawScreenTextLine( "pogo: impulse %.3f, velocity %.3f", m_mover.m_pogoImpulse, m_mover.m_pogoVelocity ); + DrawScreenTextLine( "pogo: length = %.2f/%.2f", m_mover.m_pogoLength, m_mover.m_pogoRestLength ); + DrawScreenTextLine( "on ground: %d", (int)m_mover.m_onGround ); + DrawScreenTextLine( "walkable: %d", (int)m_mover.m_walkable ); + DrawScreenTextLine( "jumping/ticks: %d/%d", (int)m_mover.m_jumping, m_mover.m_jumpTicks ); if ( m_lockCamera ) { - m_camera->center.x = m_position.x; + m_camera->center.x = position.x; } } static Sample* Create( SampleContext* context ) { - return new Mover( context ); + return new DynamicMoverSample( context ); } - static constexpr int m_planeCapacity = 8; static constexpr b2Vec2 m_elevatorBase = { 112.0f, 10.0f }; static constexpr float m_elevatorAmplitude = 4.0f; - float m_jumpSpeed = 10.0f; - float m_maxSpeed = 6.0f; - float m_minSpeed = 0.1f; - float m_stopSpeed = 3.0f; - float m_accelerate = 20.0f; - float m_airSteer = 0.2f; - float m_friction = 8.0f; - float m_gravity = 30.0f; - float m_pogoHertz = 5.0f; - float m_pogoDampingRatio = 0.8f; - - int m_pogoShape = PogoSegment; - b2Pos m_position; - b2Vec2 m_velocity; - b2Capsule m_capsule; + DynamicMover m_mover; b2BodyId m_elevatorId; - b2ShapeId m_ballId; - ShapeUserData m_friendlyShape; - ShapeUserData m_elevatorShape; - b2CollisionPlane m_planes[m_planeCapacity] = {}; - int m_planeCount; - int m_totalIterations; - float m_pogoVelocity; float m_time; - bool m_onGround; + bool m_preSolve; bool m_jumpReleased; bool m_lockCamera; }; -static int sampleMover = RegisterSample( "Character", "Mover", Mover::Create ); +static int sampleDynamicMover = RegisterSample( "Character", "Dynamic Mover", DynamicMoverSample::Create ); diff --git a/samples/sample_collision.cpp b/samples/sample_collision.cpp index 8de1158a9..061836fe9 100644 --- a/samples/sample_collision.cpp +++ b/samples/sample_collision.cpp @@ -3117,7 +3117,7 @@ class SmoothManifold : public Sample if ( m_shapeType == e_circleShape ) { float radius = 0.5f; - b2Circle circle = { { 0.0f, 0.0f }, 0.5f }; + b2Circle circle = { { 0.0f, 0.0f }, radius }; DrawSolidCircle( m_draw, transform2, circle.center, circle.radius, color2 ); for ( int i = 0; i < m_count; ++i ) diff --git a/samples/sample_determinism.cpp b/samples/sample_determinism.cpp index b4d531335..45e4ddf38 100644 --- a/samples/sample_determinism.cpp +++ b/samples/sample_determinism.cpp @@ -56,18 +56,24 @@ class FallingHinges : public Sample { m_done = UpdateFallingHinges( m_worldId, &m_data ); - // Issue a few queries each step so the Replay viewer has something to draw b2QueryFilter filter = b2DefaultQueryFilter(); b2AABB scanBox = { { 5.0f, 1.0f }, { 7.0f, 2.5f } }; b2World_OverlapAABB( m_worldId, b2Pos_zero, scanBox, filter, OverlapFcn, nullptr ); b2Pos origin = { 0.0f, 12.0f }; b2Vec2 translation = { 0.0f, -14.0f }; - b2World_CastRayClosest( m_worldId, origin, translation, filter ); - origin = { -10.0f, 2.0f }; - translation = { 20.0f, 0.0f }; - b2World_CastRay( m_worldId, origin, translation, filter, CastFcn, nullptr ); + if ( m_stepCount == 30 ) + { + b2World_CastRayClosest( m_worldId, origin, translation, filter ); + } + + if ( m_stepCount < 5 || 100 < m_stepCount ) + { + origin = { -10.0f, 2.0f }; + translation = { 20.0f, 0.0f }; + b2World_CastRay( m_worldId, origin, translation, filter, CastFcn, nullptr ); + } if ( m_done ) { @@ -113,7 +119,7 @@ class SnapShot : public Sample ~SnapShot() override { - if (m_image != nullptr) + if ( m_image != nullptr ) { free( m_image ); } diff --git a/samples/sample_events.cpp b/samples/sample_events.cpp index c54d5fb3f..1644ebf5d 100644 --- a/samples/sample_events.cpp +++ b/samples/sample_events.cpp @@ -1218,236 +1218,6 @@ class ContactEvent : public Sample static int sampleWeeble = RegisterSample( "Events", "Contact", ContactEvent::Create ); -// Shows how to make a rigid body character mover and use the pre-solve callback. In this -// case the platform should get the pre-solve event, not the player. -class Platform : public Sample -{ -public: - explicit Platform( SampleContext* context ) - : Sample( context ) - { - if ( m_context->restart == false ) - { - m_context->camera.center = { 0.5f, 7.5f }; - m_context->camera.zoom = 25.0f * 0.4f; - } - - b2World_SetPreSolveCallback( m_worldId, PreSolveStatic, this ); - - // Ground - { - b2BodyDef bodyDef = b2DefaultBodyDef(); - b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); - b2ShapeDef shapeDef = b2DefaultShapeDef(); - b2Segment segment = { { -20.0f, 0.0f }, { 20.0f, 0.0f } }; - b2CreateSegmentShape( groundId, &shapeDef, &segment ); - } - - // Static Platform - // This tests pre-solve with continuous collision - { - b2BodyDef bodyDef = b2DefaultBodyDef(); - bodyDef.type = b2_staticBody; - bodyDef.position = { -6.0f, 6.0f }; - b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); - - b2ShapeDef shapeDef = b2DefaultShapeDef(); - - // Need to turn this on to get the callback - shapeDef.enablePreSolveEvents = true; - - b2Polygon box = b2MakeBox( 2.0f, 0.5f ); - b2CreatePolygonShape( bodyId, &shapeDef, &box ); - } - - // Moving Platform - { - b2BodyDef bodyDef = b2DefaultBodyDef(); - bodyDef.type = b2_kinematicBody; - bodyDef.position = { 0.0f, 6.0f }; - bodyDef.linearVelocity = { 2.0f, 0.0f }; - m_movingPlatformId = b2CreateBody( m_worldId, &bodyDef ); - - b2ShapeDef shapeDef = b2DefaultShapeDef(); - - // Need to turn this on to get the callback - shapeDef.enablePreSolveEvents = true; - - b2Polygon box = b2MakeBox( 3.0f, 0.5f ); - b2CreatePolygonShape( m_movingPlatformId, &shapeDef, &box ); - } - - // Player - { - b2BodyDef bodyDef = b2DefaultBodyDef(); - bodyDef.type = b2_dynamicBody; - bodyDef.motionLocks.angularZ = true; - bodyDef.linearDamping = 0.5f; - bodyDef.position = { 0.0f, 1.0f }; - m_playerId = b2CreateBody( m_worldId, &bodyDef ); - - m_radius = 0.5f; - b2Capsule capsule = { { 0.0f, 0.0f }, { 0.0f, 1.0f }, m_radius }; - b2ShapeDef shapeDef = b2DefaultShapeDef(); - shapeDef.material.friction = 0.1f; - - m_playerShapeId = b2CreateCapsuleShape( m_playerId, &shapeDef, &capsule ); - } - - m_force = 25.0f; - m_impulse = 25.0f; - m_jumpDelay = 0.25f; - m_jumping = false; - } - - static bool PreSolveStatic( b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Pos point, b2Vec2 normal, void* context ) - { - Platform* self = static_cast( context ); - return self->PreSolve( shapeIdA, shapeIdB, point , normal ); - } - - // This callback must be thread-safe. It may be called multiple times simultaneously. - // Notice how this method is constant and doesn't change any data. It also - // does not try to access any values in the world that may be changing, such as contact data. - bool PreSolve( b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Pos point, b2Vec2 normal ) const - { - assert( b2Shape_IsValid( shapeIdA ) ); - assert( b2Shape_IsValid( shapeIdB ) ); - - float sign = 0.0f; - if ( B2_ID_EQUALS( shapeIdA, m_playerShapeId ) ) - { - sign = -1.0f; - } - else if ( B2_ID_EQUALS( shapeIdB, m_playerShapeId ) ) - { - sign = 1.0f; - } - else - { - // not colliding with the player, enable contact - return true; - } - - if ( sign * normal.y > 0.95f ) - { - return true; - } - - // normal points down, disable contact - return false; - } - - bool DrawControls() override - { - ImGui::PushItemWidth( 6.0f * ImGui::GetFontSize() ); - ImGui::SliderFloat( "force", &m_force, 0.0f, 50.0f, "%.1f" ); - ImGui::SliderFloat( "impulse", &m_impulse, 0.0f, 50.0f, "%.1f" ); - ImGui::PopItemWidth(); - - return true; - } - - void Step() override - { - bool canJump = false; - b2Vec2 velocity = b2Body_GetLinearVelocity( m_playerId ); - if ( m_jumpDelay == 0.0f && m_jumping == false && velocity.y < 0.01f ) - { - int capacity = b2Body_GetContactCapacity( m_playerId ); - capacity = b2MinInt( capacity, 4 ); - b2ContactData contactData[4]; - int count = b2Body_GetContactData( m_playerId, contactData, capacity ); - for ( int i = 0; i < count; ++i ) - { - b2BodyId bodyIdA = b2Shape_GetBody( contactData[i].shapeIdA ); - float sign = 0.0f; - if ( B2_ID_EQUALS( bodyIdA, m_playerId ) ) - { - // normal points from A to B - sign = -1.0f; - } - else - { - sign = 1.0f; - } - - if ( sign * contactData[i].manifold.normal.y > 0.9f ) - { - canJump = true; - break; - } - } - } - - // A kinematic body is moved by setting its velocity. This - // ensure friction works correctly. - b2Pos platformPosition = b2Body_GetPosition( m_movingPlatformId ); - if ( platformPosition.x < -15.0f ) - { - b2Body_SetLinearVelocity( m_movingPlatformId, { 2.0f, 0.0f } ); - } - else if ( platformPosition.x > 15.0f ) - { - b2Body_SetLinearVelocity( m_movingPlatformId, { -2.0f, 0.0f } ); - } - - if ( glfwGetKey( m_context->window, GLFW_KEY_A ) == GLFW_PRESS ) - { - b2Body_ApplyForceToCenter( m_playerId, { -m_force, 0.0f }, true ); - } - - if ( glfwGetKey( m_context->window, GLFW_KEY_D ) == GLFW_PRESS ) - { - b2Body_ApplyForceToCenter( m_playerId, { m_force, 0.0f }, true ); - } - - int keyState = glfwGetKey( m_context->window, GLFW_KEY_SPACE ); - if ( keyState == GLFW_PRESS ) - { - if ( canJump ) - { - b2Body_ApplyLinearImpulseToCenter( m_playerId, { 0.0f, m_impulse }, true ); - m_jumpDelay = 0.5f; - m_jumping = true; - } - } - else - { - m_jumping = false; - } - - Sample::Step(); - - b2ContactData contactData = {}; - int contactCount = b2Body_GetContactData( m_movingPlatformId, &contactData, 1 ); - DrawScreenTextLine( "Platform contact count = %d, point count = %d", contactCount, contactData.manifold.pointCount ); - DrawScreenTextLine( "Movement: A/D/Space" ); - DrawScreenTextLine( "Can jump = %s", canJump ? "true" : "false" ); - - if ( m_context->hertz > 0.0f ) - { - m_jumpDelay = b2MaxFloat( 0.0f, m_jumpDelay - 1.0f / m_context->hertz ); - } - } - - static Sample* Create( SampleContext* context ) - { - return new Platform( context ); - } - - bool m_jumping; - float m_radius; - float m_force; - float m_impulse; - float m_jumpDelay; - b2BodyId m_playerId; - b2ShapeId m_playerShapeId; - b2BodyId m_movingPlatformId; -}; - -static int samplePlatformer = RegisterSample( "Events", "Platformer", Platform::Create ); - // This shows how to process body events. class BodyMove : public Sample { @@ -2042,7 +1812,7 @@ class JointEvent : public Sample { int index = (int)(intptr_t)event->userData; assert( 0 <= index && index < e_count ); - b2DestroyJoint( event->jointId, true ); + b2DestroyJoint( event->jointId ); m_jointIds[index] = b2_nullJointId; } } @@ -2117,7 +1887,6 @@ class PersistentContact : public Sample b2ContactEvents events = b2World_GetContactEvents( m_worldId ); for ( int i = 0; i < events.beginCount && i < 1; ++i ) { - b2ContactBeginTouchEvent event = events.beginEvents[i]; m_contactId = events.beginEvents[i].contactId; } diff --git a/samples/sample_issues.cpp b/samples/sample_issues.cpp index b91f756d3..d30dfe4a3 100644 --- a/samples/sample_issues.cpp +++ b/samples/sample_issues.cpp @@ -327,6 +327,7 @@ static int staticVsBulletBug = RegisterSample( "Issues", "StaticVsBulletBug", St // This simulations stresses the solver by putting a light mass between two bodies on a prismatic joint with a stiff spring. // This can be made stable by increasing the size of the middle circle and/or increasing the number of sub-steps. +// todo try applying the impulse at the mid point to avoid breaking Newton's 3rd law class UnstablePrismaticJoints : public Sample { public: diff --git a/samples/sample_joints.cpp b/samples/sample_joints.cpp index 1ffe7d5ba..b88b5d4e5 100644 --- a/samples/sample_joints.cpp +++ b/samples/sample_joints.cpp @@ -62,7 +62,7 @@ class DistanceJoint : public Sample { for ( int i = 0; i < m_count; ++i ) { - b2DestroyJoint( m_jointIds[i], false ); + b2DestroyJoint( m_jointIds[i] ); m_jointIds[i] = b2_nullJointId; } @@ -1942,7 +1942,7 @@ class BreakableJoint : public Sample b2Vec2 force = b2Joint_GetConstraintForce( m_jointIds[i] ); if ( b2LengthSquared( force ) > m_breakForce * m_breakForce ) { - b2DestroyJoint( m_jointIds[i], true ); + b2DestroyJoint( m_jointIds[i] ); m_jointIds[i] = b2_nullJointId; } else diff --git a/shared/determinism.c b/shared/determinism.c index f92b13b8c..2169f3782 100644 --- a/shared/determinism.c +++ b/shared/determinism.c @@ -115,8 +115,7 @@ bool UpdateFallingHinges( b2WorldId worldId, FallingHingeData* data ) if ( bodyEvents.moveCount == 0 ) { - int awakeCount = b2World_GetAwakeBodyCount( worldId ); - assert( awakeCount == 0 ); + assert( b2World_GetAwakeBodyCount( worldId ) == 0 ); data->hash = B2_HASH_INIT; for ( int i = 0; i < data->bodyCount; ++i ) diff --git a/shared/human.c b/shared/human.c index 23c5cfa59..85769cc61 100644 --- a/shared/human.c +++ b/shared/human.c @@ -520,7 +520,7 @@ void DestroyHuman( Human* human ) continue; } - b2DestroyJoint( human->bones[i].jointId, false ); + b2DestroyJoint( human->bones[i].jointId ); human->bones[i].jointId = b2_nullJointId; } diff --git a/shared/utils.c b/shared/utils.c index 780ea9b3c..6dce72fba 100644 --- a/shared/utils.c +++ b/shared/utils.c @@ -4,7 +4,8 @@ #include "utils.h" #if defined( _WIN64 ) -#include +// On Windows this is capitalized, but not in Linux mingw. +#include #elif defined( __APPLE__ ) #include #elif defined( __linux__ ) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 9498e61ce..be69d5ab0 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -37,11 +37,13 @@ set(BOX2D_SOURCE_FILES manifold.c math_functions.c motor_joint.c + mover_joint.c mover.c parallel_for.c parallel_for.h physics_world.c physics_world.h + pogo_joint.c prismatic_joint.c recording.c recording.h @@ -73,6 +75,7 @@ set(BOX2D_API_FILES ../include/box2d/base.h ../include/box2d/box2d.h ../include/box2d/collision.h + ../include/box2d/config.h ../include/box2d/constants.h ../include/box2d/id.h ../include/box2d/math_functions.h @@ -129,7 +132,8 @@ if (BOX2D_PROFILE) endif() if (BOX2D_VALIDATE) - message(STATUS "Box2D validation ON") + # Only effective when NDEBUG is undefined, see base.h + message(STATUS "Box2D validation ON (debug builds)") target_compile_definitions(box2d PRIVATE BOX2D_VALIDATE) endif() @@ -144,6 +148,10 @@ if (BOX2D_DOUBLE_PRECISION) target_compile_definitions(box2d PUBLIC BOX2D_DOUBLE_PRECISION) endif() +# Enable asserts in release with debug info. RelWithDebInfo defines NDEBUG, so the +# optimized test runs would otherwise lose assertions. +target_compile_definitions(box2d PUBLIC "$<$:B2_ENABLE_ASSERT>") + if (MSVC) message(STATUS "Box2D on MSVC") if (BUILD_SHARED_LIBS) @@ -154,9 +162,6 @@ if (MSVC) # Visual Studio won't load the natvis unless it is in the project target_sources(box2d PRIVATE box2d.natvis) - # Enable asserts in release with debug info - target_compile_definitions(box2d PUBLIC "$<$:B2_ENABLE_ASSERT>") - if (CMAKE_C_COMPILER_ID STREQUAL "Clang") # clang-cl: /Wall maps to -Weverything which is far too aggressive target_compile_options(box2d PRIVATE /W4 -Wmissing-prototypes) diff --git a/src/atomic.h b/src/atomic.h index 5f2c49ef2..6691dcbc1 100644 --- a/src/atomic.h +++ b/src/atomic.h @@ -12,6 +12,10 @@ #include #endif +#if defined( _M_X64 ) || defined( __x86_64__ ) || defined( _M_IX86 ) || defined( __i386__ ) +#include +#endif + static inline void b2AtomicStoreInt( b2AtomicInt* a, int value ) { #if defined( _MSC_VER ) @@ -25,8 +29,14 @@ static inline void b2AtomicStoreInt( b2AtomicInt* a, int value ) static inline int b2AtomicLoadInt( b2AtomicInt* a ) { -#if defined( _MSC_VER ) - return _InterlockedOr( (long*)&a->value, 0 ); +#if defined( _MSC_VER ) && !defined( __clang__ ) + int value = __iso_volatile_load32( (volatile __int32*)&a->value ); +#if defined( _M_ARM ) || defined( _M_ARM64 ) || defined( _M_ARM64EC ) + __dmb( 0xB ); +#else + _ReadWriteBarrier(); +#endif + return value; #elif defined( __GNUC__ ) || defined( __clang__ ) return __atomic_load_n( &a->value, __ATOMIC_SEQ_CST ); #else @@ -70,8 +80,14 @@ static inline void b2AtomicStoreU32( b2AtomicU32* a, uint32_t value ) static inline uint32_t b2AtomicLoadU32( b2AtomicU32* a ) { -#if defined( _MSC_VER ) - return (uint32_t)_InterlockedOr( (long*)&a->value, 0 ); +#if defined( _MSC_VER ) && !defined( __clang__ ) + uint32_t value = (uint32_t)__iso_volatile_load32( (volatile __int32*)&a->value ); +#if defined( _M_ARM ) || defined( _M_ARM64 ) || defined( _M_ARM64EC ) + __dmb( 0xB ); +#else + _ReadWriteBarrier(); +#endif + return value; #elif defined( __GNUC__ ) || defined( __clang__ ) return __atomic_load_n( &a->value, __ATOMIC_SEQ_CST ); #else @@ -92,7 +108,16 @@ static inline int64_t b2AtomicFetchAddI64( b2AtomicI64* a, int64_t increment ) static inline int64_t b2AtomicLoadI64( b2AtomicI64* a ) { -#if defined( _MSC_VER ) +#if defined( _MSC_VER ) && !defined( __clang__ ) && !defined( _M_ARM ) + int64_t value = __iso_volatile_load64( (volatile __int64*)&a->value ); +#if defined( _M_ARM64 ) || defined( _M_ARM64EC ) + __dmb( 0xB ); +#else + _ReadWriteBarrier(); +#endif + return value; +#elif defined( _MSC_VER ) && !defined( __clang__ ) + // 32-bit ARM has no plain atomic 64-bit load return _InterlockedOr64( (__int64*)&a->value, 0 ); #elif defined( __GNUC__ ) || defined( __clang__ ) return __atomic_load_n( &a->value, __ATOMIC_SEQ_CST ); @@ -100,3 +125,26 @@ static inline int64_t b2AtomicLoadI64( b2AtomicI64* a ) #error "Unsupported platform" #endif } + +// Denormal flushing is a per thread mode the host can turn on, usually by linking a module +// built with fast math. This breaks determinism. +static inline bool b2IsDenormalFlushEnabled( void ) +{ +#if defined( _M_X64 ) || defined( __x86_64__ ) || defined( _M_IX86 ) || defined( __i386__ ) + // FTZ is bit 15, DAZ is bit 6 + uint32_t csr = _mm_getcsr(); + return ( csr & 0x8040 ) != 0; +#elif defined( _M_ARM64 ) || defined( __aarch64__ ) + // FZ is bit 24 and flushes denormal inputs and outputs together + uint64_t fpcr; +#if defined( _MSC_VER ) + fpcr = _ReadStatusReg( ARM64_FPCR ); +#else + __asm__ __volatile__( "mrs %0, fpcr" : "=r"( fpcr ) ); +#endif + return ( fpcr & ( 1ull << 24 ) ) != 0; +#else + // wasm has no flush mode + return false; +#endif +} diff --git a/src/body.c b/src/body.c index 840ada03d..ce6204362 100644 --- a/src/body.c +++ b/src/body.c @@ -3,8 +3,6 @@ #include "body.h" -#include "recording.h" - #include "aabb.h" #include "contact.h" #include "core.h" @@ -12,6 +10,7 @@ #include "island.h" #include "joint.h" #include "physics_world.h" +#include "recording.h" #include "sensor.h" #include "shape.h" #include "solver_set.h" @@ -155,7 +154,7 @@ static void b2RemoveBodyFromIsland( b2World* world, b2Body* body ) body->islandIndex = B2_NULL_INDEX; } -static void b2DestroyBodyContacts( b2World* world, b2Body* body, bool wakeBodies ) +static void b2DestroyBodyContacts( b2World* world, b2Body* body ) { // Destroy the attached contacts int edgeKey = body->headContactKey; @@ -166,7 +165,7 @@ static void b2DestroyBodyContacts( b2World* world, b2Body* body, bool wakeBodies b2Contact* contact = b2Array_Get( world->contacts, contactId ); edgeKey = contact->edges[edgeIndex].nextKey; - b2DestroyContact( world, contact, wakeBodies ); + b2DestroyContact( world, contact ); } b2ValidateSolverSets( world ); @@ -355,9 +354,6 @@ void b2DestroyBody( b2BodyId bodyId ) b2Body* body = b2GetBodyFullId( world, bodyId ); - // Wake bodies attached to this body, even if this body is static. - bool wakeBodies = true; - // Destroy the attached joints int edgeKey = body->headJointKey; while ( edgeKey != B2_NULL_INDEX ) @@ -369,11 +365,11 @@ void b2DestroyBody( b2BodyId bodyId ) edgeKey = joint->edges[edgeIndex].nextKey; // Careful because this modifies the list being traversed - b2DestroyJointInternal( world, joint, wakeBodies ); + b2DestroyJointInternal( world, joint ); } // Destroy all contacts attached to this body. - b2DestroyBodyContacts( world, body, wakeBodies ); + b2DestroyBodyContacts( world, body ); // Destroy the attached shapes and their broad-phase proxies. int shapeId = body->headShapeId; @@ -619,7 +615,7 @@ void b2UpdateBodyMassData( b2World* world, b2Body* body ) B2_ASSERT( body->inertia >= 0.0f ); - if ( body->inertia > 0.0f ) + if ( body->inertia > 0.0f && ( body->flags & b2_fixedRotation ) == 0 ) { bodySim->invInertia = 1.0f / body->inertia; } @@ -1167,9 +1163,8 @@ void b2Body_SetType( b2BodyId bodyId, b2BodyType type ) return; } - // Stage 2: destroy all contacts but don't wake bodies (because we don't need to) - bool wakeBodies = false; - b2DestroyBodyContacts( world, body, wakeBodies ); + // Stage 2: destroy all contacts + b2DestroyBodyContacts( world, body ); // Stage 3: wake this body (does nothing if body is static), otherwise it will also wake // all bodies in the same sleeping solver set. @@ -1419,6 +1414,28 @@ void b2Body_SetMassData( b2BodyId bodyId, b2MassData massData ) bodySim->invMass = body->mass > 0.0f ? 1.0f / body->mass : 0.0f; bodySim->invInertia = body->inertia > 0.0f ? 1.0f / body->inertia : 0.0f; + + // Update extents using supplied mass center. + bodySim->minExtent = B2_HUGE; + bodySim->maxExtent = 0.0f; + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + const b2Shape* s = b2Array_Get( world->shapes, shapeId ); + b2ShapeExtent extent = b2ComputeShapeExtent( s, massData.center ); + bodySim->minExtent = b2MinFloat( bodySim->minExtent, extent.minExtent ); + bodySim->maxExtent = b2MaxFloat( bodySim->maxExtent, extent.maxExtent ); + shapeId = s->nextShapeId; + } + + // Motion locks take priority over mass data. + if ( body->flags & b2_fixedRotation ) + { + body->inertia = 0.0f; + bodySim->invInertia = 0.0f; + } + + body->flags &= ~b2_dirtyMass; } b2MassData b2Body_GetMassData( b2BodyId bodyId ) @@ -1664,8 +1681,7 @@ void b2Body_Disable( b2BodyId bodyId ) // Destroy contacts and wake bodies touching this body. This avoid floating bodies. // This is necessary even for static bodies. - bool wakeBodies = true; - b2DestroyBodyContacts( world, body, wakeBodies ); + b2DestroyBodyContacts( world, body ); // The current solver set of the body b2SolverSet* set = b2Array_Get( world->solverSets, body->setIndex ); @@ -1852,6 +1868,9 @@ void b2Body_SetMotionLocks( b2BodyId bodyId, b2MotionLocks locks ) state->angularVelocity = 0.0f; } } + + // Motion locks can affect mass properties. + b2UpdateBodyMassData( world, body ); } } @@ -1880,7 +1899,7 @@ void b2Body_SetBullet( b2BodyId bodyId, bool flag ) uint32_t newFlag = flag ? b2_isBullet : 0; b2Body* body = b2GetBodyFullId( world, bodyId ); - if ((body->flags & b2_isBullet) == newFlag) + if ( ( body->flags & b2_isBullet ) == newFlag ) { return; } diff --git a/src/body.h b/src/body.h index 759e91e9a..7ac781902 100644 --- a/src/body.h +++ b/src/body.h @@ -58,7 +58,6 @@ enum b2BodyFlags b2_allLocks = b2_lockAngularZ | b2_lockLinearX | b2_lockLinearY, // If this flag is set then the body has fixed rotation - // todo use this to set the inverse inertia to zero b2_fixedRotation = b2_lockAngularZ, // These flags are transient per time step. These may be different across b2Body, b2BodySim, and b2BodyState. diff --git a/src/contact.c b/src/contact.c index 9a9476435..d51684888 100644 --- a/src/contact.c +++ b/src/contact.c @@ -369,7 +369,7 @@ void b2CreateContact( b2World* world, b2Shape* shapeA, b2Shape* shapeB ) // - a body changes type from dynamic to kinematic or static // - a shape is destroyed // - contact filtering is modified -void b2DestroyContact( b2World* world, b2Contact* contact, bool wakeBodies ) +void b2DestroyContact( b2World* world, b2Contact* contact ) { // Remove pair from set uint64_t pairKey = B2_SHAPE_PAIR_KEY( contact->shapeIdA, contact->shapeIdB ); @@ -493,7 +493,7 @@ void b2DestroyContact( b2World* world, b2Contact* contact, bool wakeBodies ) contact->localIndex = B2_NULL_INDEX; b2FreeId( &world->contactIdPool, contactId ); - if ( wakeBodies && touching ) + if ( touching ) { b2WakeBody( world, bodyA ); b2WakeBody( world, bodyB ); @@ -573,29 +573,12 @@ bool b2UpdateContact( b2World* world, b2ContactSim* contactSim, b2Shape* shapeA, b2ShapeId shapeIdA = { shapeA->id + 1, world->worldId, shapeA->generation }; b2ShapeId shapeIdB = { shapeB->id + 1, world->worldId, shapeB->generation }; - b2Manifold* manifold = &contactSim->manifold; - float bestSeparation = manifold->points[0].separation; - b2Pos bestPoint = b2OffsetPos( transformA.p, manifold->points[0].anchorA ); + // This call assumes thread safety. + world->preSolveFcn( shapeIdA, shapeIdB, &contactSim->manifold, world->preSolveContext ); - // Get deepest point - for ( int i = 1; i < manifold->pointCount; ++i ) - { - float separation = manifold->points[i].separation; - if ( separation < bestSeparation ) - { - bestSeparation = separation; - bestPoint = b2OffsetPos( transformA.p, manifold->points[i].anchorA ); - } - } - - // this call assumes thread safety - touching = world->preSolveFcn( shapeIdA, shapeIdB, bestPoint, manifold->normal, world->preSolveContext ); - if ( touching == false ) - { - // disable contact - pointCount = 0; - manifold->pointCount = 0; - } + // Keep these current. + pointCount = contactSim->manifold.pointCount; + touching = pointCount > 0; } // This flag is for testing diff --git a/src/contact.h b/src/contact.h index 70ec5270f..406eeb73e 100644 --- a/src/contact.h +++ b/src/contact.h @@ -145,7 +145,7 @@ void b2InitializeContactRegisters( void ); bool b2CanCollide( b2ShapeType typeA, b2ShapeType typeB ); void b2CreateContact( b2World* world, b2Shape* shapeA, b2Shape* shapeB ); -void b2DestroyContact( b2World* world, b2Contact* contact, bool wakeBodies ); +void b2DestroyContact( b2World* world, b2Contact* contact ); b2ContactSim* b2GetContactSim( b2World* world, b2Contact* contact ); diff --git a/src/distance.c b/src/distance.c index bbce8d1e2..2bf5df021 100644 --- a/src/distance.c +++ b/src/distance.c @@ -498,8 +498,10 @@ b2DistanceOutput b2ShapeDistance( const b2DistanceInput* input, b2SimplexCache* if ( simplex.count == 3 ) { // Overlap + *cache = b2MakeSimplexCache( &simplex ); b2Vec2 localPointA, localPointB; b2ComputeWitnessPoints( &simplex, &localPointA, &localPointB ); + output.distance = 0.0f; output.pointA = localPointA; output.pointB = localPointB; return output; @@ -514,7 +516,7 @@ b2DistanceOutput b2ShapeDistance( const b2DistanceInput* input, b2SimplexCache* #endif // Ensure the search direction is numerically fit. - if ( b2Dot( d, d ) < FLT_EPSILON * FLT_EPSILON ) + if ( b2Dot( d, d ) < 1000.0f * FLT_MIN ) { // This is unlikely but could lead to bad cycling. // The branch predictor seems to make this check have low cost. @@ -522,9 +524,12 @@ b2DistanceOutput b2ShapeDistance( const b2DistanceInput* input, b2SimplexCache* // The origin is probably contained by a line segment // or triangle. Thus the shapes are overlapped. + *cache = b2MakeSimplexCache( &simplex ); + // Must return overlap due to invalid normal. b2Vec2 localPointA, localPointB; b2ComputeWitnessPoints( &simplex, &localPointA, &localPointB ); + output.distance = 0.0f; output.pointA = localPointA; output.pointB = localPointB; return output; diff --git a/src/distance_joint.c b/src/distance_joint.c index 72a5f5295..004f025bc 100644 --- a/src/distance_joint.c +++ b/src/distance_joint.c @@ -62,9 +62,6 @@ void b2DistanceJoint_SetLengthRange( b2JointId jointId, float minLength, float m maxLength = b2ClampFloat( maxLength, B2_LINEAR_SLOP, B2_HUGE ); joint->minLength = b2MinFloat( minLength, maxLength ); joint->maxLength = b2MaxFloat( minLength, maxLength ); - joint->impulse = 0.0f; - joint->lowerImpulse = 0.0f; - joint->upperImpulse = 0.0f; } float b2DistanceJoint_GetMinLength( b2JointId jointId ) @@ -222,10 +219,10 @@ b2Vec2 b2GetDistanceJointForce( b2World* world, b2JointSim* base ) { b2DistanceJoint* joint = &base->distanceJoint; - // Relative to body A so the difference stays in float precision far from the origin - b2WorldTransform wxfA = b2GetBodyTransform( world, base->bodyIdA ); - b2Transform transformA = b2ToRelativeTransform( wxfA, wxfA.p ); - b2Transform transformB = b2ToRelativeTransform( b2GetBodyTransform( world, base->bodyIdB ), wxfA.p ); + b2WorldTransform worldTransformA = b2GetBodyTransform( world, base->bodyIdA ); + b2Transform transformA = b2ToRelativeTransform( worldTransformA, worldTransformA.p ); + b2WorldTransform worldTransformB = b2GetBodyTransform( world, base->bodyIdB ); + b2Transform transformB = b2ToRelativeTransform( worldTransformB, worldTransformA.p ); b2Vec2 pA = b2TransformPoint( transformA, base->localFrameA.p ); b2Vec2 pB = b2TransformPoint( transformB, base->localFrameB.p ); @@ -396,13 +393,13 @@ void b2SolveDistanceJoint( b2JointSim* base, b2StepContext* context, bool useBia { // Cdot = dot(u, v + cross(w, r)) b2Vec2 vr = b2Add( b2Sub( vB, vA ), b2Sub( b2CrossSV( wB, rB ), b2CrossSV( wA, rA ) ) ); - float Cdot = b2Dot( axis, vr ); - float C = length - joint->length; - float bias = joint->distanceSoftness.biasRate * C; + float cdot = b2Dot( axis, vr ); + float c = length - joint->length; + float bias = joint->distanceSoftness.biasRate * c; float m = joint->distanceSoftness.massScale * joint->axialMass; float oldImpulse = joint->impulse; - float impulse = -m * ( Cdot + bias ) - joint->distanceSoftness.impulseScale * oldImpulse; + float impulse = -m * ( cdot + bias ) - joint->distanceSoftness.impulseScale * oldImpulse; float h = context->h; joint->impulse = b2ClampFloat( joint->impulse + impulse, joint->lowerSpringForce * h, joint->upperSpringForce * h ); @@ -437,26 +434,26 @@ void b2SolveDistanceJoint( b2JointSim* base, b2StepContext* context, bool useBia // lower limit { b2Vec2 vr = b2Add( b2Sub( vB, vA ), b2Sub( b2CrossSV( wB, rB ), b2CrossSV( wA, rA ) ) ); - float Cdot = b2Dot( axis, vr ); + float cdot = b2Dot( axis, vr ); - float C = length - joint->minLength; + float c = length - joint->minLength; float bias = 0.0f; - float massCoeff = 1.0f; - float impulseCoeff = 0.0f; - if ( C > 0.0f ) + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( c > 0.0f ) { // speculative - bias = C * context->inv_h; + bias = c * context->inv_h; } else if ( useBias ) { - bias = base->constraintSoftness.biasRate * C; - massCoeff = base->constraintSoftness.massScale; - impulseCoeff = base->constraintSoftness.impulseScale; + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; } - float impulse = -massCoeff * joint->axialMass * ( Cdot + bias ) - impulseCoeff * joint->lowerImpulse; + float impulse = -massScale * joint->axialMass * ( cdot + bias ) - impulseScale * joint->lowerImpulse; float newImpulse = b2MaxFloat( 0.0f, joint->lowerImpulse + impulse ); impulse = newImpulse - joint->lowerImpulse; joint->lowerImpulse = newImpulse; diff --git a/src/joint.c b/src/joint.c index b7b270c6a..6453f3f87 100644 --- a/src/joint.c +++ b/src/joint.c @@ -45,6 +45,14 @@ b2DistanceJointDef b2DefaultDistanceJointDef( void ) return def; } +b2FilterJointDef b2DefaultFilterJointDef( void ) +{ + b2FilterJointDef def = { 0 }; + def.base = b2DefaultJointDef(); + def.internalValue = B2_SECRET_COOKIE; + return def; +} + b2MotorJointDef b2DefaultMotorJointDef( void ) { b2MotorJointDef def = { 0 }; @@ -53,9 +61,17 @@ b2MotorJointDef b2DefaultMotorJointDef( void ) return def; } -b2FilterJointDef b2DefaultFilterJointDef( void ) +b2MoverJointDef b2DefaultMoverJointDef( void ) { - b2FilterJointDef def = { 0 }; + b2MoverJointDef def = { 0 }; + def.base = b2DefaultJointDef(); + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +b2PogoJointDef b2DefaultPogoJointDef( void ) +{ + b2PogoJointDef def = { 0 }; def.base = b2DefaultJointDef(); def.internalValue = B2_SECRET_COOKIE; return def; @@ -106,7 +122,7 @@ b2ExplosionDef b2DefaultExplosionDef( void ) b2Joint* b2GetJointFullId( b2World* world, b2JointId jointId ) { int id = jointId.index1 - 1; - b2Joint* joint = b2Array_Get( world->joints,id ); + b2Joint* joint = b2Array_Get( world->joints, id ); B2_ASSERT( joint->jointId == id && joint->generation == jointId.generation ); return joint; } @@ -117,11 +133,11 @@ b2JointSim* b2GetJointSim( b2World* world, b2Joint* joint ) { B2_ASSERT( 0 <= joint->colorIndex && joint->colorIndex < B2_GRAPH_COLOR_COUNT ); b2GraphColor* color = world->constraintGraph.colors + joint->colorIndex; - return b2Array_Get( color->jointSims,joint->localIndex ); + return b2Array_Get( color->jointSims, joint->localIndex ); } - b2SolverSet* set = b2Array_Get( world->solverSets,joint->setIndex ); - return b2Array_Get( set->jointSims,joint->localIndex ); + b2SolverSet* set = b2Array_Get( world->solverSets, joint->setIndex ); + return b2Array_Get( set->jointSims, joint->localIndex ); } b2JointSim* b2GetJointSimCheckType( b2JointId jointId, b2JointType type ) @@ -159,23 +175,20 @@ static void b2DestroyContactsBetweenBodies( b2World* world, b2Body* bodyA, b2Bod otherBodyId = bodyA->id; } - // no need to wake bodies when a joint removes collision between them - bool wakeBodies = false; - // destroy the contacts while ( contactKey != B2_NULL_INDEX ) { int contactId = contactKey >> 1; int edgeIndex = contactKey & 1; - b2Contact* contact = b2Array_Get( world->contacts,contactId ); + b2Contact* contact = b2Array_Get( world->contacts, contactId ); contactKey = contact->edges[edgeIndex].nextKey; int otherEdgeIndex = edgeIndex ^ 1; if ( contact->edges[otherEdgeIndex].bodyId == otherBodyId ) { // Careful, this removes the contact from the current doubly linked list - b2DestroyContact( world, contact, wakeBodies ); + b2DestroyContact( world, contact ); } } @@ -199,6 +212,13 @@ static b2JointPair b2CreateJoint( b2World* world, const b2JointDef* def, b2Joint b2Body* bodyA = b2GetBodyFullId( world, def->bodyIdA ); b2Body* bodyB = b2GetBodyFullId( world, def->bodyIdB ); + // If the joint prevents collisions, then destroy all contacts between attached bodies. + // Do this first so world arrays are not disturbed in the following code. + if ( def->collideConnected == false ) + { + b2DestroyContactsBetweenBodies( world, bodyA, bodyB ); + } + int bodyIdA = bodyA->id; int bodyIdB = bodyB->id; int maxSetIndex = b2MaxInt( bodyA->setIndex, bodyB->setIndex ); @@ -207,10 +227,10 @@ static b2JointPair b2CreateJoint( b2World* world, const b2JointDef* def, b2Joint int jointId = b2AllocId( &world->jointIdPool ); if ( jointId == world->joints.count ) { - b2Array_Push( world->joints,(b2Joint){ 0 } ); + b2Array_Push( world->joints, (b2Joint){ 0 } ); } - b2Joint* joint = b2Array_Get( world->joints,jointId ); + b2Joint* joint = b2Array_Get( world->joints, jointId ); joint->jointId = jointId; joint->userData = def->userData; joint->generation += 1; @@ -222,7 +242,6 @@ static b2JointPair b2CreateJoint( b2World* world, const b2JointDef* def, b2Joint joint->drawScale = def->drawScale; joint->type = type; joint->collideConnected = def->collideConnected; - //joint->isMarked = false; // Doubly linked list on bodyA joint->edges[0].bodyId = bodyIdA; @@ -232,7 +251,7 @@ static b2JointPair b2CreateJoint( b2World* world, const b2JointDef* def, b2Joint int keyA = ( jointId << 1 ) | 0; if ( bodyA->headJointKey != B2_NULL_INDEX ) { - b2Joint* jointA = b2Array_Get( world->joints,bodyA->headJointKey >> 1 ); + b2Joint* jointA = b2Array_Get( world->joints, bodyA->headJointKey >> 1 ); b2JointEdge* edgeA = jointA->edges + ( bodyA->headJointKey & 1 ); edgeA->prevKey = keyA; } @@ -247,7 +266,7 @@ static b2JointPair b2CreateJoint( b2World* world, const b2JointDef* def, b2Joint int keyB = ( jointId << 1 ) | 1; if ( bodyB->headJointKey != B2_NULL_INDEX ) { - b2Joint* jointB = b2Array_Get( world->joints,bodyB->headJointKey >> 1 ); + b2Joint* jointB = b2Array_Get( world->joints, bodyB->headJointKey >> 1 ); b2JointEdge* edgeB = jointB->edges + ( bodyB->headJointKey & 1 ); edgeB->prevKey = keyB; } @@ -259,7 +278,7 @@ static b2JointPair b2CreateJoint( b2World* world, const b2JointDef* def, b2Joint if ( bodyA->setIndex == b2_disabledSet || bodyB->setIndex == b2_disabledSet ) { // if either body is disabled, create in disabled set - b2SolverSet* set = b2Array_Get( world->solverSets,b2_disabledSet ); + b2SolverSet* set = b2Array_Get( world->solverSets, b2_disabledSet ); joint->setIndex = b2_disabledSet; joint->localIndex = set->jointSims.count; @@ -273,7 +292,7 @@ static b2JointPair b2CreateJoint( b2World* world, const b2JointDef* def, b2Joint else if ( bodyA->type != b2_dynamicBody && bodyB->type != b2_dynamicBody ) { // joint is not attached to a dynamic body - b2SolverSet* set = b2Array_Get( world->solverSets,b2_staticSet ); + b2SolverSet* set = b2Array_Get( world->solverSets, b2_staticSet ); joint->setIndex = b2_staticSet; joint->localIndex = set->jointSims.count; @@ -308,7 +327,7 @@ static b2JointPair b2CreateJoint( b2World* world, const b2JointDef* def, b2Joint // joint should go into the sleeping set (not static set) int setIndex = maxSetIndex; - b2SolverSet* set = b2Array_Get( world->solverSets,setIndex ); + b2SolverSet* set = b2Array_Get( world->solverSets, setIndex ); joint->setIndex = setIndex; joint->localIndex = set->jointSims.count; @@ -330,10 +349,10 @@ static b2JointPair b2CreateJoint( b2World* world, const b2JointDef* def, b2Joint // fix potentially invalid set index setIndex = bodyA->setIndex; - b2SolverSet* mergedSet = b2Array_Get( world->solverSets,setIndex ); + b2SolverSet* mergedSet = b2Array_Get( world->solverSets, setIndex ); // Careful! The joint sim pointer was orphaned by the set merge. - jointSim = b2Array_Get( mergedSet->jointSims,joint->localIndex ); + jointSim = b2Array_Get( mergedSet->jointSims, joint->localIndex ); } B2_ASSERT( joint->setIndex == setIndex ); @@ -366,12 +385,6 @@ static b2JointPair b2CreateJoint( b2World* world, const b2JointDef* def, b2Joint b2LinkJoint( world, joint ); } - // If the joint prevents collisions, then destroy all contacts between attached bodies - if ( def->collideConnected == false ) - { - b2DestroyContactsBetweenBodies( world, bodyA, bodyB ); - } - b2ValidateSolverSets( world ); return (b2JointPair){ joint, jointSim }; @@ -422,6 +435,29 @@ b2JointId b2CreateDistanceJoint( b2WorldId worldId, const b2DistanceJointDef* de return jointId; } +b2JointId b2CreateFilterJoint( b2WorldId worldId, const b2FilterJointDef* def ) +{ + B2_CHECK_DEF( def ); + b2World* world = b2GetWorldFromId( worldId ); + + B2_ASSERT( world->locked == false ); + + if ( world->locked ) + { + return (b2JointId){ 0 }; + } + + b2JointPair pair = b2CreateJoint( world, &def->base, b2_filterJoint ); + + b2JointSim* joint = pair.jointSim; + + b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + + B2_REC_CREATE( world, CreateFilterJoint, jointId, worldId, *def ); + + return jointId; +} + b2JointId b2CreateMotorJoint( b2WorldId worldId, const b2MotorJointDef* def ) { B2_CHECK_DEF( def ); @@ -456,7 +492,7 @@ b2JointId b2CreateMotorJoint( b2WorldId worldId, const b2MotorJointDef* def ) return jointId; } -b2JointId b2CreateFilterJoint( b2WorldId worldId, const b2FilterJointDef* def ) +b2JointId b2CreateMoverJoint( b2WorldId worldId, const b2MoverJointDef* def ) { B2_CHECK_DEF( def ); b2World* world = b2GetWorldFromId( worldId ); @@ -468,13 +504,59 @@ b2JointId b2CreateFilterJoint( b2WorldId worldId, const b2FilterJointDef* def ) return (b2JointId){ 0 }; } - b2JointPair pair = b2CreateJoint( world, &def->base, b2_filterJoint ); - + b2JointPair pair = b2CreateJoint( world, &def->base, b2_moverJoint ); b2JointSim* joint = pair.jointSim; + joint->moverJoint = (b2MoverJoint){ 0 }; + joint->moverJoint.linearVelocity = def->linearVelocity; + joint->moverJoint.maxVelocityForce = def->maxVelocityForce; + b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; - B2_REC_CREATE( world, CreateFilterJoint, jointId, worldId, *def ); + B2_REC_CREATE( world, CreateMoverJoint, jointId, worldId, *def ); + + return jointId; +} + +b2JointId b2CreatePogoJoint( b2WorldId worldId, const b2PogoJointDef* def ) +{ + B2_CHECK_DEF( def ); + B2_ASSERT( b2IsValidFloat( def->restLength ) && def->restLength >= 0.0f ); + B2_ASSERT( b2IsValidFloat( def->hertz ) && def->hertz >= 0.0f ); + B2_ASSERT( b2IsValidFloat( def->dampingRatio ) && def->dampingRatio >= 0.0f ); + B2_ASSERT( b2IsValidFloat( def->maxTensionForce ) && def->maxTensionForce >= 0.0f ); + B2_ASSERT( b2IsValidFloat( def->maxCompressionForce ) && def->maxCompressionForce >= 0.0f ); + + b2World* world = b2GetWorldFromId( worldId ); + + B2_ASSERT( world->locked == false ); + + if ( world->locked ) + { + return (b2JointId){ 0 }; + } + + b2JointPair pair = b2CreateJoint( world, &def->base, b2_pogoJoint ); + + b2JointSim* joint = pair.jointSim; + + joint->pogoJoint = (b2PogoJoint){ 0 }; + joint->pogoJoint.restLength = def->restLength; + joint->pogoJoint.hertz = def->hertz; + joint->pogoJoint.dampingRatio = def->dampingRatio; + joint->pogoJoint.maxTensionForce = def->maxTensionForce; + joint->pogoJoint.maxCompressionForce = def->maxCompressionForce; + joint->pogoJoint.normal = def->normal; + joint->pogoJoint.impulse = def->impulse; + joint->pogoJoint.velocity = def->velocity; + + b2JointId jointId = { + .index1 = joint->jointId + 1, + .world0 = world->worldId, + .generation = pair.joint->generation, + }; + + B2_REC_CREATE( world, CreatePogoJoint, jointId, worldId, *def ); return jointId; } @@ -509,7 +591,11 @@ b2JointId b2CreatePrismaticJoint( b2WorldId worldId, const b2PrismaticJointDef* joint->prismaticJoint.enableLimit = def->enableLimit; joint->prismaticJoint.enableMotor = def->enableMotor; - b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + b2JointId jointId = { + .index1 = joint->jointId + 1, + .world0 = world->worldId, + .generation = pair.joint->generation, + }; B2_REC_CREATE( world, CreatePrismaticJoint, jointId, worldId, *def ); @@ -520,8 +606,6 @@ b2JointId b2CreateRevoluteJoint( b2WorldId worldId, const b2RevoluteJointDef* de { B2_CHECK_DEF( def ); B2_ASSERT( def->lowerAngle <= def->upperAngle ); - B2_ASSERT( def->lowerAngle >= -0.99f * B2_PI ); - B2_ASSERT( def->upperAngle <= 0.99f * B2_PI ); b2World* world = b2GetWorldFromId( worldId ); @@ -542,8 +626,12 @@ b2JointId b2CreateRevoluteJoint( b2WorldId worldId, const b2RevoluteJointDef* de joint->revoluteJoint.targetAngle = b2ClampFloat( def->targetAngle, -B2_PI, B2_PI ); joint->revoluteJoint.hertz = def->hertz; joint->revoluteJoint.dampingRatio = def->dampingRatio; - joint->revoluteJoint.lowerAngle = def->lowerAngle; - joint->revoluteJoint.upperAngle = def->upperAngle; + + float lowerAngle = b2MinFloat( def->lowerAngle, def->upperAngle ); + float upperAngle = b2MaxFloat( def->lowerAngle, def->upperAngle ); + joint->revoluteJoint.lowerAngle = b2ClampFloat( lowerAngle, -0.99f * B2_PI, 0.99f * B2_PI ); + joint->revoluteJoint.upperAngle = b2ClampFloat( upperAngle, -0.99f * B2_PI, 0.99f * B2_PI ); + joint->revoluteJoint.maxMotorTorque = def->maxMotorTorque; joint->revoluteJoint.motorSpeed = def->motorSpeed; joint->revoluteJoint.enableSpring = def->enableSpring; @@ -630,7 +718,7 @@ b2JointId b2CreateWheelJoint( b2WorldId worldId, const b2WheelJointDef* def ) return jointId; } -void b2DestroyJointInternal( b2World* world, b2Joint* joint, bool wakeBodies ) +void b2DestroyJointInternal( b2World* world, b2Joint* joint ) { int jointId = joint->jointId; @@ -639,20 +727,20 @@ void b2DestroyJointInternal( b2World* world, b2Joint* joint, bool wakeBodies ) int idA = edgeA->bodyId; int idB = edgeB->bodyId; - b2Body* bodyA = b2Array_Get( world->bodies,idA ); - b2Body* bodyB = b2Array_Get( world->bodies,idB ); + b2Body* bodyA = b2Array_Get( world->bodies, idA ); + b2Body* bodyB = b2Array_Get( world->bodies, idB ); // Remove from body A if ( edgeA->prevKey != B2_NULL_INDEX ) { - b2Joint* prevJoint = b2Array_Get( world->joints,edgeA->prevKey >> 1 ); + b2Joint* prevJoint = b2Array_Get( world->joints, edgeA->prevKey >> 1 ); b2JointEdge* prevEdge = prevJoint->edges + ( edgeA->prevKey & 1 ); prevEdge->nextKey = edgeA->nextKey; } if ( edgeA->nextKey != B2_NULL_INDEX ) { - b2Joint* nextJoint = b2Array_Get( world->joints,edgeA->nextKey >> 1 ); + b2Joint* nextJoint = b2Array_Get( world->joints, edgeA->nextKey >> 1 ); b2JointEdge* nextEdge = nextJoint->edges + ( edgeA->nextKey & 1 ); nextEdge->prevKey = edgeA->prevKey; } @@ -668,14 +756,14 @@ void b2DestroyJointInternal( b2World* world, b2Joint* joint, bool wakeBodies ) // Remove from body B if ( edgeB->prevKey != B2_NULL_INDEX ) { - b2Joint* prevJoint = b2Array_Get( world->joints,edgeB->prevKey >> 1 ); + b2Joint* prevJoint = b2Array_Get( world->joints, edgeB->prevKey >> 1 ); b2JointEdge* prevEdge = prevJoint->edges + ( edgeB->prevKey & 1 ); prevEdge->nextKey = edgeB->nextKey; } if ( edgeB->nextKey != B2_NULL_INDEX ) { - b2Joint* nextJoint = b2Array_Get( world->joints,edgeB->nextKey >> 1 ); + b2Joint* nextJoint = b2Array_Get( world->joints, edgeB->nextKey >> 1 ); b2JointEdge* nextEdge = nextJoint->edges + ( edgeB->nextKey & 1 ); nextEdge->prevKey = edgeB->prevKey; } @@ -708,14 +796,14 @@ void b2DestroyJointInternal( b2World* world, b2Joint* joint, bool wakeBodies ) } else { - b2SolverSet* set = b2Array_Get( world->solverSets,setIndex ); - int movedIndex = b2Array_RemoveSwap( set->jointSims,localIndex ); + b2SolverSet* set = b2Array_Get( world->solverSets, setIndex ); + int movedIndex = b2Array_RemoveSwap( set->jointSims, localIndex ); if ( movedIndex != B2_NULL_INDEX ) { // Fix moved joint b2JointSim* movedJointSim = set->jointSims.data + localIndex; int movedId = movedJointSim->jointId; - b2Joint* movedJoint = b2Array_Get( world->joints,movedId ); + b2Joint* movedJoint = b2Array_Get( world->joints, movedId ); B2_ASSERT( movedJoint->localIndex == movedIndex ); movedJoint->localIndex = localIndex; } @@ -728,16 +816,13 @@ void b2DestroyJointInternal( b2World* world, b2Joint* joint, bool wakeBodies ) joint->jointId = B2_NULL_INDEX; b2FreeId( &world->jointIdPool, jointId ); - if ( wakeBodies ) - { - b2WakeBody( world, bodyA ); - b2WakeBody( world, bodyB ); - } + b2WakeBody( world, bodyA ); + b2WakeBody( world, bodyB ); b2ValidateSolverSets( world ); } -void b2DestroyJoint( b2JointId jointId, bool wakeAttached ) +void b2DestroyJoint( b2JointId jointId ) { b2World* world = b2GetWorld( jointId.world0 ); B2_ASSERT( world->locked == false ); @@ -747,11 +832,11 @@ void b2DestroyJoint( b2JointId jointId, bool wakeAttached ) return; } - B2_REC( world, DestroyJoint, jointId, wakeAttached ); + B2_REC( world, DestroyJoint, jointId ); b2Joint* joint = b2GetJointFullId( world, jointId ); - b2DestroyJointInternal( world, joint, wakeAttached ); + b2DestroyJointInternal( world, joint ); } b2JointType b2Joint_GetType( b2JointId jointId ) @@ -837,8 +922,8 @@ void b2Joint_SetCollideConnected( b2JointId jointId, bool shouldCollide ) joint->collideConnected = shouldCollide; - b2Body* bodyA = b2Array_Get( world->bodies,joint->edges[0].bodyId ); - b2Body* bodyB = b2Array_Get( world->bodies,joint->edges[1].bodyId ); + b2Body* bodyA = b2Array_Get( world->bodies, joint->edges[0].bodyId ); + b2Body* bodyB = b2Array_Get( world->bodies, joint->edges[1].bodyId ); if ( shouldCollide ) { @@ -850,7 +935,7 @@ void b2Joint_SetCollideConnected( b2JointId jointId, bool shouldCollide ) int shapeId = shapeCountA < shapeCountB ? bodyA->headShapeId : bodyB->headShapeId; while ( shapeId != B2_NULL_INDEX ) { - b2Shape* shape = b2Array_Get( world->shapes,shapeId ); + b2Shape* shape = b2Array_Get( world->shapes, shapeId ); if ( shape->proxyKey != B2_NULL_INDEX ) { @@ -898,8 +983,8 @@ void b2Joint_WakeBodies( b2JointId jointId ) B2_REC( world, JointWakeBodies, jointId ); b2Joint* joint = b2GetJointFullId( world, jointId ); - b2Body* bodyA = b2Array_Get( world->bodies,joint->edges[0].bodyId ); - b2Body* bodyB = b2Array_Get( world->bodies,joint->edges[1].bodyId ); + b2Body* bodyA = b2Array_Get( world->bodies, joint->edges[0].bodyId ); + b2Body* bodyB = b2Array_Get( world->bodies, joint->edges[1].bodyId ); b2WakeBody( world, bodyA ); b2WakeBody( world, bodyB ); @@ -922,11 +1007,25 @@ void b2GetJointReaction( b2JointSim* sim, float invTimeStep, float* force, float case b2_motorJoint: { b2MotorJoint* joint = &sim->motorJoint; - linearImpulse = b2Length( b2Add(joint->linearVelocityImpulse, joint->linearSpringImpulse) ); + linearImpulse = b2Length( b2Add( joint->linearVelocityImpulse, joint->linearSpringImpulse ) ); angularImpulse = b2AbsFloat( joint->angularVelocityImpulse + joint->angularSpringImpulse ); } break; + case b2_moverJoint: + { + b2MoverJoint* joint = &sim->moverJoint; + linearImpulse = b2Length( joint->linearVelocityImpulse ); + } + break; + + case b2_pogoJoint: + { + b2PogoJoint* joint = &sim->pogoJoint; + linearImpulse = b2AbsFloat( joint->impulse ); + } + break; + case b2_prismaticJoint: { b2PrismaticJoint* joint = &sim->prismaticJoint; @@ -984,9 +1083,15 @@ static b2Vec2 b2GetJointConstraintForce( b2World* world, b2Joint* joint ) case b2_motorJoint: return b2GetMotorJointForce( world, base ); + case b2_moverJoint: + return b2GetMoverJointForce( world, base ); + case b2_filterJoint: return b2Vec2_zero; + case b2_pogoJoint: + return b2GetPogoJointForce( world, base ); + case b2_prismaticJoint: return b2GetPrismaticJointForce( world, base ); @@ -1314,9 +1419,17 @@ void b2PrepareJoint( b2JointSim* joint, b2StepContext* context ) b2PrepareMotorJoint( joint, context ); break; + case b2_moverJoint: + b2PrepareMoverJoint( joint, context ); + break; + case b2_filterJoint: break; + case b2_pogoJoint: + b2PreparePogoJoint( joint, context ); + break; + case b2_prismaticJoint: b2PreparePrismaticJoint( joint, context ); break; @@ -1350,9 +1463,17 @@ void b2WarmStartJoint( b2JointSim* joint, b2StepContext* context ) b2WarmStartMotorJoint( joint, context ); break; + case b2_moverJoint: + b2WarmStartMoverJoint( joint, context ); + break; + case b2_filterJoint: break; + case b2_pogoJoint: + b2WarmStartPogoJoint( joint, context ); + break; + case b2_prismaticJoint: b2WarmStartPrismaticJoint( joint, context ); break; @@ -1386,9 +1507,17 @@ void b2SolveJoint( b2JointSim* joint, b2StepContext* context, bool useBias ) b2SolveMotorJoint( joint, context ); break; + case b2_moverJoint: + b2SolveMoverJoint( joint, context ); + break; + case b2_filterJoint: break; + case b2_pogoJoint: + b2SolvePogoJoint( joint, context, useBias ); + break; + case b2_prismaticJoint: b2SolvePrismaticJoint( joint, context, useBias ); break; @@ -1551,8 +1680,8 @@ void b2SolveJointsTask( b2SolverBlock block, b2StepContext* context, bool useBia void b2DrawJoint( b2DebugDraw* draw, b2World* world, b2Joint* joint ) { - b2Body* bodyA = b2Array_Get( world->bodies,joint->edges[0].bodyId ); - b2Body* bodyB = b2Array_Get( world->bodies,joint->edges[1].bodyId ); + b2Body* bodyA = b2Array_Get( world->bodies, joint->edges[0].bodyId ); + b2Body* bodyB = b2Array_Get( world->bodies, joint->edges[1].bodyId ); if ( bodyA->setIndex == b2_disabledSet || bodyB->setIndex == b2_disabledSet ) { return; @@ -1586,6 +1715,14 @@ void b2DrawJoint( b2DebugDraw* draw, b2World* world, b2Joint* joint ) draw->DrawLineFcn( pA, pB, b2_colorLightGray, draw->context ); break; + case b2_moverJoint: + draw->DrawPointFcn( pB, 8.0f, b2_colorPlum, draw->context ); + break; + + case b2_pogoJoint: + draw->DrawLineFcn( pA, pB, b2_colorLightGray, draw->context ); + break; + case b2_prismaticJoint: b2DrawPrismaticJoint( draw, jointSim, xfA, xfB, scale ); break; diff --git a/src/joint.h b/src/joint.h index d733cb354..6963e1ab3 100644 --- a/src/joint.h +++ b/src/joint.h @@ -61,6 +61,28 @@ typedef struct b2Joint } b2Joint; +// The pogo joint is a hybrid between a contact and a joint +typedef struct b2PogoJoint +{ + // Settings + b2Vec2 normal; + float restLength; + float hertz; + float dampingRatio; + float maxTensionForce; + float maxCompressionForce; + + // Runtime + float impulse; + int indexA; + int indexB; + b2Transform frameA; + b2Transform frameB; + b2Vec2 deltaCenter; + float linearMass; + float velocity; +} b2PogoJoint; + typedef struct b2DistanceJoint { float length; @@ -122,6 +144,20 @@ typedef struct b2MotorJoint float angularMass; } b2MotorJoint; +typedef struct b2MoverJoint +{ + b2Vec2 linearVelocity; + b2Vec2 maxVelocityForce; + + b2Vec2 linearVelocityImpulse; + + int indexA; + int indexB; + b2Transform frameA; + b2Transform frameB; + float linearMass; +} b2MoverJoint; + typedef struct b2PrismaticJoint { b2Vec2 impulse; @@ -255,6 +291,8 @@ typedef struct b2JointSim { b2DistanceJoint distanceJoint; b2MotorJoint motorJoint; + b2MoverJoint moverJoint; + b2PogoJoint pogoJoint; b2RevoluteJoint revoluteJoint; b2PrismaticJoint prismaticJoint; b2WeldJoint weldJoint; @@ -262,7 +300,7 @@ typedef struct b2JointSim }; } b2JointSim; -void b2DestroyJointInternal( b2World* world, b2Joint* joint, bool wakeBodies ); +void b2DestroyJointInternal( b2World* world, b2Joint* joint ); b2Joint* b2GetJointFullId( b2World* world, b2JointId jointId ); b2JointSim* b2GetJointSim( b2World* world, b2Joint* joint ); @@ -286,6 +324,8 @@ void b2DrawJoint( b2DebugDraw* draw, b2World* world, b2Joint* joint ); b2Vec2 b2GetDistanceJointForce( b2World* world, b2JointSim* base ); b2Vec2 b2GetMotorJointForce( b2World* world, b2JointSim* base ); +b2Vec2 b2GetMoverJointForce( b2World* world, b2JointSim* base ); +b2Vec2 b2GetPogoJointForce( b2World* world, b2JointSim* base ); b2Vec2 b2GetPrismaticJointForce( b2World* world, b2JointSim* base ); b2Vec2 b2GetRevoluteJointForce( b2World* world, b2JointSim* base ); b2Vec2 b2GetWeldJointForce( b2World* world, b2JointSim* base ); @@ -299,6 +339,8 @@ float b2GetWheelJointTorque( b2World* world, b2JointSim* base ); void b2PrepareDistanceJoint( b2JointSim* base, b2StepContext* context ); void b2PrepareMotorJoint( b2JointSim* base, b2StepContext* context ); +void b2PrepareMoverJoint( b2JointSim* base, b2StepContext* context ); +void b2PreparePogoJoint( b2JointSim* base, b2StepContext* context ); void b2PreparePrismaticJoint( b2JointSim* base, b2StepContext* context ); void b2PrepareRevoluteJoint( b2JointSim* base, b2StepContext* context ); void b2PrepareWeldJoint( b2JointSim* base, b2StepContext* context ); @@ -306,6 +348,8 @@ void b2PrepareWheelJoint( b2JointSim* base, b2StepContext* context ); void b2WarmStartDistanceJoint( b2JointSim* base, b2StepContext* context ); void b2WarmStartMotorJoint( b2JointSim* base, b2StepContext* context ); +void b2WarmStartMoverJoint( b2JointSim* base, b2StepContext* context ); +void b2WarmStartPogoJoint( b2JointSim* base, b2StepContext* context ); void b2WarmStartPrismaticJoint( b2JointSim* base, b2StepContext* context ); void b2WarmStartRevoluteJoint( b2JointSim* base, b2StepContext* context ); void b2WarmStartWeldJoint( b2JointSim* base, b2StepContext* context ); @@ -313,6 +357,8 @@ void b2WarmStartWheelJoint( b2JointSim* base, b2StepContext* context ); void b2SolveDistanceJoint( b2JointSim* base, b2StepContext* context, bool useBias ); void b2SolveMotorJoint( b2JointSim* base, b2StepContext* context ); +void b2SolveMoverJoint( b2JointSim* base, b2StepContext* context ); +void b2SolvePogoJoint( b2JointSim* base, b2StepContext* context, bool useBias ); void b2SolvePrismaticJoint( b2JointSim* base, b2StepContext* context, bool useBias ); void b2SolveRevoluteJoint( b2JointSim* base, b2StepContext* context, bool useBias ); void b2SolveWeldJoint( b2JointSim* base, b2StepContext* context, bool useBias ); diff --git a/src/motor_joint.c b/src/motor_joint.c index 99871c826..1950ffa40 100644 --- a/src/motor_joint.c +++ b/src/motor_joint.c @@ -2,7 +2,6 @@ // SPDX-License-Identifier: MIT #include "body.h" -#include "core.h" #include "joint.h" #include "physics_world.h" #include "recording.h" @@ -435,22 +434,3 @@ void b2SolveMotorJoint( b2JointSim* base, b2StepContext* context ) stateB->angularVelocity = wB; } } - -#if 0 -void b2DumpMotorJoint() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Dump(" b2MotorJointDef jd;\n"); - b2Dump(" jd.bodyA = sims[%d];\n", indexA); - b2Dump(" jd.bodyB = sims[%d];\n", indexB); - b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Dump(" jd.referenceAngle = %.9g;\n", m_referenceAngle); - b2Dump(" jd.stiffness = %.9g;\n", m_stiffness); - b2Dump(" jd.damping = %.9g;\n", m_damping); - b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} -#endif diff --git a/src/mover.c b/src/mover.c index fa4019018..695811743 100644 --- a/src/mover.c +++ b/src/mover.c @@ -48,7 +48,7 @@ b2PlaneSolverResult b2SolvePlanes( b2Vec2 targetDelta, b2CollisionPlane* planes, } return (b2PlaneSolverResult){ - .translation = delta, + .delta = delta, .iterationCount = iteration, }; } diff --git a/src/mover_joint.c b/src/mover_joint.c new file mode 100644 index 000000000..1dff375bf --- /dev/null +++ b/src/mover_joint.c @@ -0,0 +1,170 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "joint.h" +#include "physics_world.h" +#include "recording.h" +#include "solver.h" +#include "solver_set.h" + +// needed for dll export +#include "box2d/box2d.h" + +void b2MoverJoint_SetLinearVelocity( b2JointId jointId, b2Vec2 velocity ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + B2_REC( world, MoverJointSetLinearVelocity, jointId, velocity ); + + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_moverJoint ); + joint->moverJoint.linearVelocity = velocity; +} + +b2Vec2 b2MoverJoint_GetLinearVelocity( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_moverJoint ); + return joint->moverJoint.linearVelocity; +} + +void b2MoverJoint_SetMaxVelocityForce( b2JointId jointId, b2Vec2 maxForce ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + B2_REC( world, MoverJointSetMaxVelocityForce, jointId, maxForce ); + + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_moverJoint ); + joint->moverJoint.maxVelocityForce = maxForce; +} + +b2Vec2 b2MoverJoint_GetMaxVelocityForce( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_moverJoint ); + return joint->moverJoint.maxVelocityForce; +} + +b2Vec2 b2GetMoverJointForce( b2World* world, b2JointSim* base ) +{ + b2Vec2 force = b2MulSV( world->inv_h, base->moverJoint.linearVelocityImpulse ); + return force; +} + +void b2PrepareMoverJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_moverJoint ); + + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b2World* world = context->world; + + b2Body* bodyA = b2Array_Get( world->bodies, idA ); + b2Body* bodyB = b2Array_Get( world->bodies, idB ); + + B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet ); + + b2SolverSet* setA = b2Array_Get( world->solverSets, bodyA->setIndex ); + b2SolverSet* setB = b2Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b2BodySim* bodySimA = b2Array_Get( setA->bodySims, localIndexA ); + b2BodySim* bodySimB = b2Array_Get( setB->bodySims, localIndexB ); + + float mA = bodySimA->invMass; + float mB = bodySimB->invMass; + + base->invMassA = mA; + base->invMassB = mB; + base->invIA = 0.0f; + base->invIB = 0.0f; + + b2MoverJoint* joint = &base->moverJoint; + joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX; + joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX; + + float k = mA + mB; + joint->linearMass = k > 0.0f ? 1.0f / k : 0.0f; + + if ( context->enableWarmStarting == false ) + { + joint->linearVelocityImpulse = b2Vec2_zero; + } +} + +void b2WarmStartMoverJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_moverJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + + b2MoverJoint* joint = &base->moverJoint; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + if ( stateA->flags & b2_dynamicFlag ) + { + stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, joint->linearVelocityImpulse ); + } + + if ( stateB->flags & b2_dynamicFlag ) + { + stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, joint->linearVelocityImpulse ); + } +} + +void b2SolveMoverJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_moverJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2MoverJoint* joint = &base->moverJoint; + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 vA = stateA->linearVelocity; + b2Vec2 vB = stateB->linearVelocity; + + // linear velocity + if ( joint->maxVelocityForce.x > 0.0f || joint->maxVelocityForce.y > 0.0f ) + { + b2Vec2 cdot = b2Sub( vB, vA ); + cdot = b2Sub( cdot, joint->linearVelocity ); + b2Vec2 b = b2MulSV( joint->linearMass, cdot ); + b2Vec2 impulse = { -b.x, -b.y }; + + b2Vec2 oldImpulse = joint->linearVelocityImpulse; + joint->linearVelocityImpulse = b2Add( joint->linearVelocityImpulse, impulse ); + b2Vec2 maxImpulse = b2MulSV( context->h, joint->maxVelocityForce ); + + // clamp x and y separately + joint->linearVelocityImpulse = b2Clamp( joint->linearVelocityImpulse, b2Neg( maxImpulse ), maxImpulse ); + + impulse = b2Sub( joint->linearVelocityImpulse, oldImpulse ); + vA = b2MulSub( vA, mA, impulse ); + vB = b2MulAdd( vB, mB, impulse ); + } + else + { + joint->linearVelocityImpulse = b2Vec2_zero; + } + + if ( stateA->flags & b2_dynamicFlag ) + { + stateA->linearVelocity = vA; + } + + if ( stateB->flags & b2_dynamicFlag ) + { + stateB->linearVelocity = vB; + } +} diff --git a/src/physics_world.c b/src/physics_world.c index 49914b495..8d15f863f 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -9,6 +9,7 @@ #include "aabb.h" #include "arena_allocator.h" +#include "atomic.h" #include "bitset.h" #include "body.h" #include "broad_phase.h" @@ -33,6 +34,12 @@ #include #include +#if defined( _M_X64 ) || defined( __x86_64__ ) || defined( _M_IX86 ) || defined( __i386__ ) +#include +#elif ( defined( _M_ARM64 ) || defined( __aarch64__ ) ) && defined( _MSC_VER ) +#include +#endif + _Static_assert( B2_MAX_WORLDS > 0, "must be 1 or more" ); _Static_assert( B2_MAX_WORLDS < UINT16_MAX, "B2_MAX_WORLDS limit exceeded" ); static b2World b2_worlds[B2_MAX_WORLDS]; @@ -154,6 +161,11 @@ b2WorldId b2CreateWorld( const b2WorldDef* def ) _Static_assert( B2_MAX_WORLDS < UINT16_MAX, "B2_MAX_WORLDS limit exceeded" ); B2_CHECK_DEF( def ); + if ( b2IsDenormalFlushEnabled() ) + { + b2Log( "Denormal flushing is enabled, determinism not supported" ); + } + int worldId = B2_NULL_INDEX; for ( int i = 0; i < B2_MAX_WORLDS; ++i ) { @@ -323,14 +335,17 @@ b2WorldId b2CreateWorld( const b2WorldDef* def ) world->recording = NULL; // add one to worldId so that 0 represents a null b2WorldId - return (b2WorldId){ (uint16_t)( worldId + 1 ), world->generation }; + return (b2WorldId){ + .index1 = (uint16_t)( worldId + 1 ), + .generation = world->generation, + }; } void b2DestroyWorld( b2WorldId worldId ) { b2World* world = b2GetWorldFromId( worldId ); - // Detach any recording before teardown; the host owns and frees the recording buffer + // Detach any recording before teardown. The user owns and frees the recording buffer. b2StopRecordingInternal( world ); if ( world->scheduler != NULL ) @@ -749,7 +764,7 @@ static void b2Collide( b2StepContext* context ) if ( simFlags & b2_simDisjoint ) { // Bounding boxes no longer overlap - b2DestroyContact( world, contact, false ); + b2DestroyContact( world, contact ); contact = NULL; contactSim = NULL; } @@ -938,6 +953,7 @@ void b2World_Step( b2WorldId worldId, float timeStep, int subStepCount ) world->finishTaskFcn( world->userTreeTask, world->userTaskContext ); world->userTreeTask = NULL; world->activeTaskCount -= 1; + b2ValidateNoEnlarged( &world->broadPhase ); } // Update sensors @@ -2004,6 +2020,12 @@ void b2World_StartRecording( b2WorldId worldId, b2Recording* recording ) return; } + if ( world->preSolveFcn != NULL || world->preContinuousFcn != NULL ) + { + printf( "b2World_StartRecording: preSolve not supported when recording\n" ); + return; + } + b2StartRecordingIntoBuffer( world, recording ); } @@ -2468,21 +2490,17 @@ static float RayCastCallback( const b2RayCastInput* input, int proxyId, uint64_t } b2Body* body = b2Array_Get( world->bodies, shape->bodyId ); - b2WorldTransform xf = b2GetBodyTransformQuick( world, body ); + b2WorldTransform bodyTransform = b2GetBodyTransformQuick( world, body ); + b2Transform transform = b2ToRelativeTransform( bodyTransform, worldContext->origin ); - // Re-center on the body so the per-shape cast stays in float precision far from the origin. - // The tree traversal already used the truncated origin in input. Here we re-difference in full - // precision against the body position. - b2Pos base = xf.p; - b2Transform transform = b2ToRelativeTransform( xf, base ); b2RayCastInput localInput = *input; - localInput.origin = b2SubPos( worldContext->origin, base ); + localInput.origin = b2Vec2_zero; b2CastOutput output = b2RayCastShape( &localInput, shape, transform ); if ( output.hit ) { b2ShapeId id = { shapeId + 1, world->worldId, shape->generation }; - b2Pos point = b2OffsetPos( base, output.point ); + b2Pos point = b2OffsetPos( worldContext->origin, output.point ); float fraction = worldContext->fcn( id, point, output.normal, output.fraction, worldContext->userContext ); // The user may return -1 to skip this shape @@ -2722,9 +2740,7 @@ b2TreeStats b2World_CastShape( b2WorldId worldId, b2Pos origin, const b2ShapePro worldContext.input.maxFraction = 1.0f; worldContext.userContext = context; - // Bound the proxy in origin relative space then lift to a conservative world float box. The - // tree node boxes use the same directed rounding, so the swept box never clips a shape far - // from the origin. Per shape casts re-difference at full precision against the carried origin. + // Create a conservative world space box from the proxy and origin. b2AABB localBox = b2MakeAABB( proxy->points, proxy->count, proxy->radius ); b2AABB box = b2OffsetAABB( localBox, origin ); b2BoxCastInput treeInput = { box, translation, 1.0f }; @@ -2784,7 +2800,8 @@ static float MoverCastCallback( const b2BoxCastInput* input, int proxyId, uint64 localInput.maxFraction = input->maxFraction; b2Body* body = b2Array_Get( world->bodies, shape->bodyId ); - b2Transform transform = b2ToRelativeTransform( b2GetBodyTransformQuick( world, body ), worldContext->origin ); + b2WorldTransform bodyTransform = b2GetBodyTransformQuick( world, body ); + b2Transform transform = b2ToRelativeTransform( bodyTransform, worldContext->origin ); b2CastOutput output = b2ShapeCastShape( &localInput, shape, transform ); if ( output.fraction == 0.0f ) @@ -2899,8 +2916,8 @@ static bool TreeCollideCallback( int proxyId, uint64_t userData, void* context ) // It is tempting to use a shape proxy for the mover, but this makes handling deep overlap difficult and the generality may // not be worth it. -void b2World_CollideMover( b2WorldId worldId, b2Pos origin, const b2Capsule* mover, b2QueryFilter filter, - b2PlaneResultFcn* fcn, void* context ) +void b2World_CollideMover( b2WorldId worldId, b2Pos origin, const b2Capsule* mover, b2QueryFilter filter, b2PlaneResultFcn* fcn, + void* context ) { b2World* world = b2GetWorldFromId( worldId ); B2_ASSERT( world->locked == false ); @@ -2962,15 +2979,18 @@ void b2World_SetCustomFilterCallback( b2WorldId worldId, b2CustomFilterFcn* fcn, world->customFilterContext = context; } -void b2World_SetPreSolveCallback( b2WorldId worldId, b2PreSolveFcn* fcn, void* context ) +void b2World_SetPreSolveCallback( b2WorldId worldId, b2PreSolveFcn* preSolveFcn, b2PreContinuousFcn* preContinuousFcn, + void* context ) { b2World* world = b2GetWorldFromId( worldId ); - if ( fcn != NULL && world->recording != NULL ) + if ( (preSolveFcn != NULL || preContinuousFcn != NULL) && world->recording != NULL ) { printf( "b2World_SetPreSolveCallback: preSolve not supported while recording\n" ); B2_ASSERT( false && "preSolve callbacks are not supported while recording" ); } - world->preSolveFcn = fcn; + + world->preSolveFcn = preSolveFcn; + world->preContinuousFcn = preContinuousFcn; world->preSolveContext = context; } diff --git a/src/physics_world.h b/src/physics_world.h index 2f48015d4..15f08cb22 100644 --- a/src/physics_world.h +++ b/src/physics_world.h @@ -30,7 +30,7 @@ b2DeclareArray( b2TaskContext ); typedef struct b2TaskContext { // Collect per thread sensor continuous hit events. - b2Array(b2SensorHit) sensorHits; + b2Array( b2SensorHit ) sensorHits; // These bits align with the contact id capacity and signal a change in contact status b2BitSet contactStateBitSet; @@ -178,6 +178,7 @@ typedef struct b2World b2Capacity maxCapacity; b2PreSolveFcn* preSolveFcn; + b2PreContinuousFcn* preContinuousFcn; void* preSolveContext; b2CustomFilterFcn* customFilterFcn; diff --git a/src/pogo_joint.c b/src/pogo_joint.c new file mode 100644 index 000000000..6fb1f2fd2 --- /dev/null +++ b/src/pogo_joint.c @@ -0,0 +1,281 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "joint.h" +#include "physics_world.h" +#include "recording.h" +#include "solver.h" +#include "solver_set.h" + +// needed for dll export +#include "box2d/box2d.h" + +static const b2Vec2 b2_pogoAxis = { 0.0f, 1.0f }; + +void b2PogoJoint_SetRestLength( b2JointId jointId, float length ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + B2_REC( world, PogoJointSetRestLength, jointId, length ); + + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_pogoJoint ); + joint->pogoJoint.restLength = length; +} + +float b2PogoJoint_GetRestLength( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_pogoJoint ); + return joint->pogoJoint.restLength; +} + +float b2PogoJoint_GetSpringHertz( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_pogoJoint ); + return joint->pogoJoint.hertz; +} + +void b2PogoJoint_SetSpringHertz( b2JointId jointId, float hertz ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + B2_REC( world, PogoJointSetSpringHertz, jointId, hertz ); + + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_pogoJoint ); + joint->pogoJoint.hertz = hertz; +} + +void b2PogoJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + B2_REC( world, PogoJointSetSpringDampingRatio, jointId, dampingRatio ); + + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_pogoJoint ); + joint->pogoJoint.dampingRatio = dampingRatio; +} + +float b2PogoJoint_GetSpringDampingRatio( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_pogoJoint ); + return joint->pogoJoint.dampingRatio; +} + +float b2PogoJoint_GetLength( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2JointSim* jointSim = b2GetJointSimCheckType( jointId, b2_pogoJoint ); + + // Relative to body A so the difference stays in float precision far from the origin + b2WorldTransform wxfA = b2GetBodyTransform( world, jointSim->bodyIdA ); + b2Transform transformA = b2ToRelativeTransform( wxfA, wxfA.p ); + b2Transform transformB = b2ToRelativeTransform( b2GetBodyTransform( world, jointSim->bodyIdB ), wxfA.p ); + + b2Vec2 axis = b2RotateVector( jointSim->localFrameB.q, b2_pogoAxis ); + axis = b2RotateVector( transformB.q, axis ); + b2Vec2 pA = b2TransformPoint( transformA, jointSim->localFrameA.p ); + b2Vec2 pB = b2TransformPoint( transformB, jointSim->localFrameB.p ); + b2Vec2 d = b2Sub( pB, pA ); + float length = b2Dot( d, axis ); + return length; +} + +float b2PogoJoint_GetImpulse( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_pogoJoint ); + return joint->pogoJoint.impulse; +} + +float b2PogoJoint_GetVelocity( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_pogoJoint ); + return joint->pogoJoint.velocity; +} + +b2Vec2 b2GetPogoJointForce( b2World* world, b2JointSim* base ) +{ + b2Vec2 force = b2MulSV( world->inv_h * base->pogoJoint.impulse, base->pogoJoint.normal ); + return force; +} + +void b2PreparePogoJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_pogoJoint ); + + b2World* world = context->world; + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b2Body* bodyA = b2Array_Get( world->bodies, idA ); + b2Body* bodyB = b2Array_Get( world->bodies, idB ); + + B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet ); + + b2SolverSet* setA = b2Array_Get( world->solverSets, bodyA->setIndex ); + b2SolverSet* setB = b2Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b2BodySim* bodySimA = b2Array_Get( setA->bodySims, localIndexA ); + b2BodySim* bodySimB = b2Array_Get( setB->bodySims, localIndexB ); + + float mA = bodySimA->invMass; + float iA = bodySimA->invInertia; + float mB = bodySimB->invMass; + float iB = bodySimB->invInertia; + + base->invMassA = mA; + base->invMassB = mB; + base->invIA = iA; + base->invIB = iB; + + b2PogoJoint* joint = &base->pogoJoint; + joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX; + joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX; + + // Compute joint anchor frames with world space rotation, relative to center of mass + joint->frameA.q = b2MulRot( bodySimA->transform.q, base->localFrameA.q ); + joint->frameA.p = b2RotateVector( bodySimA->transform.q, b2Sub( base->localFrameA.p, bodySimA->localCenter ) ); + joint->frameB.q = b2MulRot( bodySimB->transform.q, base->localFrameB.q ); + joint->frameB.p = b2RotateVector( bodySimB->transform.q, b2Sub( base->localFrameB.p, bodySimB->localCenter ) ); + + // Compute the initial center delta. Incremental position updates are relative to this. + joint->deltaCenter = b2SubPos( bodySimB->center, bodySimA->center ); + + b2Vec2 rA = joint->frameA.p; + b2Vec2 rB = joint->frameB.p; + + // compute effective mass + float crA = b2Cross( rA, joint->normal ); + float crB = b2Cross( rB, joint->normal ); + float k = mA + mB + iA * crA * crA + iB * crB * crB; + joint->linearMass = k > 0.0f ? 1.0f / k : 0.0f; + + if ( context->enableWarmStarting == false ) + { + joint->impulse = 0.0f; + } +} + +void b2WarmStartPogoJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_pogoJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + b2PogoJoint* joint = &base->pogoJoint; + + if ( joint->hertz == 0.0f ) + { + joint->impulse = 0.0f; + return; + } + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->frameA.p ); + b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->frameB.p ); + + b2Vec2 linearImpulse = b2MulSV( joint->impulse, joint->normal ); + + if ( stateA->flags & b2_dynamicFlag ) + { + stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, linearImpulse ); + stateA->angularVelocity -= iA * b2Cross( rA, linearImpulse ); + } + + if ( stateB->flags & b2_dynamicFlag ) + { + stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, linearImpulse ); + stateB->angularVelocity += iB * b2Cross( rB, linearImpulse ); + } +} + +void b2SolvePogoJoint( b2JointSim* base, b2StepContext* context, bool useBias ) +{ + B2_UNUSED( useBias ); + + B2_ASSERT( base->type == b2_pogoJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2PogoJoint* joint = &base->pogoJoint; + if ( joint->hertz == 0.0f ) + { + joint->impulse = 0.0f; + return; + } + + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 vA = stateA->linearVelocity; + float wA = stateA->angularVelocity; + b2Vec2 vB = stateB->linearVelocity; + float wB = stateB->angularVelocity; + + b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->frameA.p ); + b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->frameB.p ); + + + float bias = 0.0f; + if ( useBias ) + { + // This is a custom pogo position correction designed to avoid feeding velocity into + // the mover body. Otherwise the mover can make huge jumps going up stairs. This still + // works with continuous collision. + // However, this position delta is not observer contact forces and can lead to tunnelling. + b2Vec2 dcA = stateA->deltaPosition; + b2Vec2 dcB = stateB->deltaPosition; + b2Vec2 d = b2Add( b2Add( b2Sub( dcB, dcA ), b2Sub( rB, rA ) ), joint->deltaCenter ); + + // The pogo axis is usually fixed as the world y-axis. Different than the ground normal. + b2Vec2 pogoAxis = b2RotateVector( joint->frameB.q, b2_pogoAxis ); + float c = b2Dot( pogoAxis, d ) - joint->restLength; + + // This could be divided by dot(joint->normal, pogoAxis) to account for the constraint + // direction, but I'd rather diminish the pogo recovery rate than risk making it huge. + joint->velocity = b2SpringDamper( joint->hertz, joint->dampingRatio, c, joint->velocity, context->h ); + bias = -joint->velocity; + } + + b2Vec2 vr = b2Sub( b2Add( vB, b2CrossSV( wB, rB ) ), b2Add( vA, b2CrossSV( wA, rA ) ) ); + float cdot = b2Dot( joint->normal, vr ); + + float maxTensionImpulse = context->h * joint->maxTensionForce; + float maxCompressionImpulse = context->h * joint->maxCompressionForce; + float oldImpulse = joint->impulse; + float impulse = -joint->linearMass * (cdot + bias); + joint->impulse = b2ClampFloat( joint->impulse + impulse, -maxTensionImpulse, maxCompressionImpulse ); + impulse = joint->impulse - oldImpulse; + + b2Vec2 P = b2MulSV( impulse, joint->normal ); + vA = b2MulSub( vA, mA, P ); + wA -= iA * b2Cross( rA, P ); + vB = b2MulAdd( vB, mB, P ); + wB += iB * b2Cross( rB, P ); + + if ( stateA->flags & b2_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b2_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} diff --git a/src/prismatic_joint.c b/src/prismatic_joint.c index dcf161c45..368fcd370 100644 --- a/src/prismatic_joint.c +++ b/src/prismatic_joint.c @@ -112,13 +112,8 @@ void b2PrismaticJoint_SetLimits( b2JointId jointId, float lower, float upper ) B2_ASSERT( lower <= upper ); b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); - if ( lower != joint->prismaticJoint.lowerTranslation || upper != joint->prismaticJoint.upperTranslation ) - { - joint->prismaticJoint.lowerTranslation = b2MinFloat( lower, upper ); - joint->prismaticJoint.upperTranslation = b2MaxFloat( lower, upper ); - joint->prismaticJoint.lowerImpulse = 0.0f; - joint->prismaticJoint.upperImpulse = 0.0f; - } + joint->prismaticJoint.lowerTranslation = b2MinFloat( lower, upper ); + joint->prismaticJoint.upperTranslation = b2MaxFloat( lower, upper ); } void b2PrismaticJoint_EnableMotor( b2JointId jointId, bool enableMotor ) @@ -677,8 +672,8 @@ void b2DrawPrismaticJoint( b2DebugDraw* draw, b2JointSim* base, b2WorldTransform b2PrismaticJoint* joint = &base->prismaticJoint; - b2WorldTransform frameA = b2OffsetWorldTransform( transformA, base->localFrameA ); - b2WorldTransform frameB = b2OffsetWorldTransform( transformB, base->localFrameB ); + b2WorldTransform frameA = b2MulWorldTransforms( transformA, base->localFrameA ); + b2WorldTransform frameB = b2MulWorldTransforms( transformB, base->localFrameB ); b2Vec2 axisA = b2RotateVector( frameA.q, (b2Vec2){ 1.0f, 0.0f } ); draw->DrawLineFcn( frameA.p, frameB.p, b2_colorDimGray, draw->context ); diff --git a/src/recording.c b/src/recording.c index 477f8606e..730d0bde7 100644 --- a/src/recording.c +++ b/src/recording.c @@ -364,6 +364,11 @@ void b2RecW_DISTANCEJOINTDEF( b2RecBuffer* buf, b2DistanceJointDef v ) b2RecW_F32( buf, v.motorSpeed ); } +void b2RecW_FILTERJOINTDEF( b2RecBuffer* buf, b2FilterJointDef v ) +{ + b2RecW_JointBase( buf, &v.base ); +} + void b2RecW_MOTORJOINTDEF( b2RecBuffer* buf, b2MotorJointDef v ) { b2RecW_JointBase( buf, &v.base ); @@ -379,9 +384,24 @@ void b2RecW_MOTORJOINTDEF( b2RecBuffer* buf, b2MotorJointDef v ) b2RecW_F32( buf, v.maxSpringTorque ); } -void b2RecW_FILTERJOINTDEF( b2RecBuffer* buf, b2FilterJointDef v ) +void b2RecW_MOVERJOINTDEF( b2RecBuffer* buf, b2MoverJointDef v ) +{ + b2RecW_JointBase( buf, &v.base ); + b2RecW_VEC2( buf, v.linearVelocity ); + b2RecW_VEC2( buf, v.maxVelocityForce ); +} + +void b2RecW_POGOJOINTDEF( b2RecBuffer* buf, b2PogoJointDef v ) { b2RecW_JointBase( buf, &v.base ); + b2RecW_VEC2( buf, v.normal ); + b2RecW_F32( buf, v.hertz ); + b2RecW_F32( buf, v.dampingRatio ); + b2RecW_F32( buf, v.restLength ); + b2RecW_F32( buf, v.maxTensionForce ); + b2RecW_F32( buf, v.maxCompressionForce ); + b2RecW_F32( buf, v.impulse ); + b2RecW_F32( buf, v.velocity ); } void b2RecW_PRISMATICJOINTDEF( b2RecBuffer* buf, b2PrismaticJointDef v ) @@ -622,21 +642,28 @@ void b2RecEndRecord( b2Recording* rec ) #undef ARG // Codegen: full writers wrapping begin, arg writer, end +// Setters and creates may run on threads that each own a distinct object, so hold the lock across +// the whole record. Without it a concurrent writer splices its bytes between our begin and end and +// the record desyncs replay. Same lock the query commit path takes. // Example: // B2_REC_OP( 0x80, Step, RET_NONE, ARG( WORLDID, world ) ARG( F32, dt ) ARG( I32, subStepCount ) ) // Becomes: // void b2RecWrite_Step( b2Recording* rec, const b2RecArgs_Step* a) // { +// b2LockMutex( rec->lock ); // b2RecBeginRecord( rec, (uint8_t)( 0x80 ) ); // b2RecWriteArgs_Step( rec, a ); // b2RecEndRecord( rec ); +// b2UnlockMutex( rec->lock ); // } #define B2_REC_OP( op, Name, RET, ... ) \ void b2RecWrite_##Name( b2Recording* rec, const b2RecArgs_##Name* a ) \ { \ + b2LockMutex( rec->lock ); \ b2RecBeginRecord( rec, (uint8_t)( op ) ); \ b2RecWriteArgs_##Name( rec, a ); \ b2RecEndRecord( rec ); \ + b2UnlockMutex( rec->lock ); \ } #include "recording_ops.inl" #undef B2_REC_OP @@ -648,19 +675,23 @@ void b2RecEndRecord( b2Recording* rec ) // Becomes: // void b2RecWriteRet_CreateCircleShape( b2Recording* rec, const b2RecArgs_CreateCircleShape* a, idType id ) // { +// b2LockMutex( rec->lock ); // b2RecBeginRecord( rec, (uint8_t)( 0x40 ) ); // b2RecWriteArgs_CreateCircleShape( rec, a ); // b2RecW_SHAPEID( &rec->buffer, id ); // b2RecEndRecord( rec ); +// b2UnlockMutex( rec->lock ); // } #define B2_REC_RETWRITE( op, Name, idType, idW ) \ void b2RecWriteRet_##Name( b2Recording* rec, const b2RecArgs_##Name* a, idType id ) \ { \ + b2LockMutex( rec->lock ); \ b2RecBeginRecord( rec, (uint8_t)( op ) ); \ b2RecWriteArgs_##Name( rec, a ); \ idW( &rec->buffer, id ); \ b2RecEndRecord( rec ); \ + b2UnlockMutex( rec->lock ); \ } #define B2_REC_RETWRITE_RET_NONE( op, Name ) #define B2_REC_RETWRITE_RET_BODYID( op, Name ) B2_REC_RETWRITE( op, Name, b2BodyId, b2RecW_BODYID ) diff --git a/src/recording.h b/src/recording.h index 3a1594468..32f2e31b8 100644 --- a/src/recording.h +++ b/src/recording.h @@ -43,24 +43,24 @@ typedef struct b2World b2World; #define B2_REC_MAGIC 0x43523242u // Recording format version. Any mismatch refuses to load. The minor tracks op stream layout -// changes that keep the 32 byte header shape, such as the query origin args. +// changes that keep the 32 byte header shape. #define B2_REC_VERSION_MAJOR 3 -#define B2_REC_VERSION_MINOR 2 +#define B2_REC_VERSION_MINOR 3 // added mover and pogo joints and rearranged opcodes // File header, fixed 32 bytes, little-endian typedef struct b2RecHeader { - uint32_t magic; // 'B2RC' = 0x43523242 - uint16_t versionMajor; // B2_REC_VERSION_MAJOR - uint16_t versionMinor; // B2_REC_VERSION_MINOR + uint32_t magic; // 'B2RC' = 0x43523242 + uint16_t versionMajor; // B2_REC_VERSION_MAJOR + uint16_t versionMinor; // B2_REC_VERSION_MINOR uint32_t reserved2; - float lengthScale; // The world length scale + float lengthScale; // The world length scale uint8_t reserved3; - uint8_t pointerWidth; // sizeof(void*), gates POD-def memcpy - uint8_t bigEndian; // 0 on all supported targets + uint8_t pointerWidth; // sizeof(void*), gates POD-def memcpy + uint8_t bigEndian; // 0 on all supported targets uint8_t validationEnabled; // 1 if built with validation, only for diagnostics on a layout mismatch uint32_t reserved1; - uint64_t snapshotSize; // bytes of snapshot blob after the header + uint64_t snapshotSize; // bytes of snapshot blob after the header } b2RecHeader; _Static_assert( sizeof( b2RecHeader ) == 32, "recording header must be 32 bytes" ); @@ -120,8 +120,10 @@ typedef b2BodyDef b2RecCType_BODYDEF; typedef b2ShapeDef b2RecCType_SHAPEDEF; typedef b2ChainDef b2RecCType_CHAINDEF; typedef b2DistanceJointDef b2RecCType_DISTANCEJOINTDEF; -typedef b2MotorJointDef b2RecCType_MOTORJOINTDEF; typedef b2FilterJointDef b2RecCType_FILTERJOINTDEF; +typedef b2MotorJointDef b2RecCType_MOTORJOINTDEF; +typedef b2MoverJointDef b2RecCType_MOVERJOINTDEF; +typedef b2PogoJointDef b2RecCType_POGOJOINTDEF; typedef b2PrismaticJointDef b2RecCType_PRISMATICJOINTDEF; typedef b2RevoluteJointDef b2RecCType_REVOLUTEJOINTDEF; typedef b2WeldJointDef b2RecCType_WELDJOINTDEF; @@ -195,8 +197,10 @@ void b2RecW_BODYDEF( b2RecBuffer* buf, b2BodyDef v ); void b2RecW_SHAPEDEF( b2RecBuffer* buf, b2ShapeDef v ); void b2RecW_CHAINDEF( b2RecBuffer* buf, b2ChainDef v ); void b2RecW_DISTANCEJOINTDEF( b2RecBuffer* buf, b2DistanceJointDef v ); -void b2RecW_MOTORJOINTDEF( b2RecBuffer* buf, b2MotorJointDef v ); void b2RecW_FILTERJOINTDEF( b2RecBuffer* buf, b2FilterJointDef v ); +void b2RecW_MOTORJOINTDEF( b2RecBuffer* buf, b2MotorJointDef v ); +void b2RecW_MOVERJOINTDEF( b2RecBuffer* buf, b2MoverJointDef v ); +void b2RecW_POGOJOINTDEF( b2RecBuffer* buf, b2PogoJointDef v ); void b2RecW_PRISMATICJOINTDEF( b2RecBuffer* buf, b2PrismaticJointDef v ); void b2RecW_REVOLUTEJOINTDEF( b2RecBuffer* buf, b2RevoluteJointDef v ); void b2RecW_WELDJOINTDEF( b2RecBuffer* buf, b2WeldJointDef v ); diff --git a/src/recording_ops.inl b/src/recording_ops.inl index 24af2210d..4a226c345 100644 --- a/src/recording_ops.inl +++ b/src/recording_ops.inl @@ -105,86 +105,99 @@ B2_REC_OP( 0x70, CreateChain, RET_CHAINID, ARG( BODYID, body ) ARG( CHAINDEF, de B2_REC_OP( 0x71, DestroyChain, RET_NONE, ARG( CHAINID, chain ) ) B2_REC_OP( 0x72, ChainSetSurfaceMaterial, RET_NONE, ARG( CHAINID, chain ) ARG( MATERIAL, material ) ARG( I32, materialIndex ) ) -// Joint create and destroy -B2_REC_OP( 0x90, CreateDistanceJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( DISTANCEJOINTDEF, def ) ) -B2_REC_OP( 0x91, CreateMotorJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( MOTORJOINTDEF, def ) ) -B2_REC_OP( 0x92, CreateFilterJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( FILTERJOINTDEF, def ) ) -B2_REC_OP( 0x93, CreatePrismaticJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( PRISMATICJOINTDEF, def ) ) -B2_REC_OP( 0x94, CreateRevoluteJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( REVOLUTEJOINTDEF, def ) ) -B2_REC_OP( 0x95, CreateWeldJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( WELDJOINTDEF, def ) ) -B2_REC_OP( 0x96, CreateWheelJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( WHEELJOINTDEF, def ) ) -B2_REC_OP( 0x97, DestroyJoint, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, wakeAttached ) ) +// Joint destroy +B2_REC_OP( 0x90, DestroyJoint, RET_NONE, ARG( JOINTID, joint ) ) // Generic joint mutators -B2_REC_OP( 0x98, JointSetLocalFrameA, RET_NONE, ARG( JOINTID, joint ) ARG( XF, localFrame ) ) -B2_REC_OP( 0x99, JointSetLocalFrameB, RET_NONE, ARG( JOINTID, joint ) ARG( XF, localFrame ) ) -B2_REC_OP( 0x9A, JointSetCollideConnected, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, shouldCollide ) ) -B2_REC_OP( 0x9B, JointWakeBodies, RET_NONE, ARG( JOINTID, joint ) ) -B2_REC_OP( 0x9C, JointSetConstraintTuning, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ARG( F32, dampingRatio ) ) -B2_REC_OP( 0x9D, JointSetForceThreshold, RET_NONE, ARG( JOINTID, joint ) ARG( F32, threshold ) ) -B2_REC_OP( 0x9E, JointSetTorqueThreshold, RET_NONE, ARG( JOINTID, joint ) ARG( F32, threshold ) ) +B2_REC_OP( 0x91, JointSetLocalFrameA, RET_NONE, ARG( JOINTID, joint ) ARG( XF, localFrame ) ) +B2_REC_OP( 0x92, JointSetLocalFrameB, RET_NONE, ARG( JOINTID, joint ) ARG( XF, localFrame ) ) +B2_REC_OP( 0x93, JointSetCollideConnected, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, shouldCollide ) ) +B2_REC_OP( 0x94, JointWakeBodies, RET_NONE, ARG( JOINTID, joint ) ) +B2_REC_OP( 0x95, JointSetConstraintTuning, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ARG( F32, dampingRatio ) ) +B2_REC_OP( 0x96, JointSetForceThreshold, RET_NONE, ARG( JOINTID, joint ) ARG( F32, threshold ) ) +B2_REC_OP( 0x97, JointSetTorqueThreshold, RET_NONE, ARG( JOINTID, joint ) ARG( F32, threshold ) ) // Distance joint -B2_REC_OP( 0xA0, DistanceJointSetLength, RET_NONE, ARG( JOINTID, joint ) ARG( F32, length ) ) -B2_REC_OP( 0xA1, DistanceJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) ) -B2_REC_OP( 0xA2, DistanceJointSetSpringForceRange, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lowerForce ) ARG( F32, upperForce ) ) -B2_REC_OP( 0xA3, DistanceJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) -B2_REC_OP( 0xA4, DistanceJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) -B2_REC_OP( 0xA5, DistanceJointEnableLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) ) -B2_REC_OP( 0xA6, DistanceJointSetLengthRange, RET_NONE, ARG( JOINTID, joint ) ARG( F32, minLength ) ARG( F32, maxLength ) ) -B2_REC_OP( 0xA7, DistanceJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) ) -B2_REC_OP( 0xA8, DistanceJointSetMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, motorSpeed ) ) -B2_REC_OP( 0xA9, DistanceJointSetMaxMotorForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, force ) ) +B2_REC_OP( 0x98, CreateDistanceJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( DISTANCEJOINTDEF, def ) ) +B2_REC_OP( 0x99, DistanceJointSetLength, RET_NONE, ARG( JOINTID, joint ) ARG( F32, length ) ) +B2_REC_OP( 0x9A, DistanceJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) ) +B2_REC_OP( 0x9B, DistanceJointSetSpringForceRange, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lowerForce ) ARG( F32, upperForce ) ) +B2_REC_OP( 0x9C, DistanceJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) +B2_REC_OP( 0x9D, DistanceJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) +B2_REC_OP( 0x9E, DistanceJointEnableLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) ) +B2_REC_OP( 0x9F, DistanceJointSetLengthRange, RET_NONE, ARG( JOINTID, joint ) ARG( F32, minLength ) ARG( F32, maxLength ) ) +B2_REC_OP( 0xA0, DistanceJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) ) +B2_REC_OP( 0xA1, DistanceJointSetMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, motorSpeed ) ) +B2_REC_OP( 0xA2, DistanceJointSetMaxMotorForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, force ) ) + +// Filter joint +B2_REC_OP( 0xA3, CreateFilterJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( FILTERJOINTDEF, def ) ) // Motor joint -B2_REC_OP( 0xAA, MotorJointSetLinearVelocity, RET_NONE, ARG( JOINTID, joint ) ARG( VEC2, velocity ) ) -B2_REC_OP( 0xAB, MotorJointSetAngularVelocity, RET_NONE, ARG( JOINTID, joint ) ARG( F32, velocity ) ) -B2_REC_OP( 0xAC, MotorJointSetMaxVelocityForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxForce ) ) -B2_REC_OP( 0xAD, MotorJointSetMaxVelocityTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxTorque ) ) -B2_REC_OP( 0xAE, MotorJointSetLinearHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) -B2_REC_OP( 0xAF, MotorJointSetLinearDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, damping ) ) -B2_REC_OP( 0xB0, MotorJointSetAngularHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) -B2_REC_OP( 0xB1, MotorJointSetAngularDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, damping ) ) -B2_REC_OP( 0xB2, MotorJointSetMaxSpringForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxForce ) ) -B2_REC_OP( 0xB3, MotorJointSetMaxSpringTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxTorque ) ) +B2_REC_OP( 0xA4, CreateMotorJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( MOTORJOINTDEF, def ) ) +B2_REC_OP( 0xA5, MotorJointSetLinearVelocity, RET_NONE, ARG( JOINTID, joint ) ARG( VEC2, velocity ) ) +B2_REC_OP( 0xA6, MotorJointSetAngularVelocity, RET_NONE, ARG( JOINTID, joint ) ARG( F32, velocity ) ) +B2_REC_OP( 0xA7, MotorJointSetMaxVelocityForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxForce ) ) +B2_REC_OP( 0xA8, MotorJointSetMaxVelocityTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxTorque ) ) +B2_REC_OP( 0xA9, MotorJointSetLinearHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) +B2_REC_OP( 0xAA, MotorJointSetLinearDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, damping ) ) +B2_REC_OP( 0xAB, MotorJointSetAngularHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) +B2_REC_OP( 0xAC, MotorJointSetAngularDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, damping ) ) +B2_REC_OP( 0xAD, MotorJointSetMaxSpringForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxForce ) ) +B2_REC_OP( 0xAE, MotorJointSetMaxSpringTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxTorque ) ) + +// Mover joint +B2_REC_OP( 0xAF, CreateMoverJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( MOVERJOINTDEF, def ) ) +B2_REC_OP( 0xB0, MoverJointSetLinearVelocity, RET_NONE, ARG( JOINTID, joint ) ARG( VEC2, velocity ) ) +B2_REC_OP( 0xB1, MoverJointSetMaxVelocityForce, RET_NONE, ARG( JOINTID, joint ) ARG( VEC2, maxForce ) ) + +// Pogo joint +B2_REC_OP( 0xB2, CreatePogoJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( POGOJOINTDEF, def ) ) +B2_REC_OP( 0xB3, PogoJointSetRestLength, RET_NONE, ARG( JOINTID, joint ) ARG( F32, length ) ) +B2_REC_OP( 0xB4, PogoJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) +B2_REC_OP( 0xB5, PogoJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) // Prismatic joint -B2_REC_OP( 0xB4, PrismaticJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) ) -B2_REC_OP( 0xB5, PrismaticJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) -B2_REC_OP( 0xB6, PrismaticJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) -B2_REC_OP( 0xB7, PrismaticJointSetTargetTranslation, RET_NONE, ARG( JOINTID, joint ) ARG( F32, translation ) ) -B2_REC_OP( 0xB8, PrismaticJointEnableLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) ) -B2_REC_OP( 0xB9, PrismaticJointSetLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) ) -B2_REC_OP( 0xBA, PrismaticJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) ) -B2_REC_OP( 0xBB, PrismaticJointSetMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, motorSpeed ) ) -B2_REC_OP( 0xBC, PrismaticJointSetMaxMotorForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, force ) ) +B2_REC_OP( 0xB6, CreatePrismaticJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( PRISMATICJOINTDEF, def ) ) +B2_REC_OP( 0xB7, PrismaticJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) ) +B2_REC_OP( 0xB8, PrismaticJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) +B2_REC_OP( 0xB9, PrismaticJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) +B2_REC_OP( 0xBA, PrismaticJointSetTargetTranslation, RET_NONE, ARG( JOINTID, joint ) ARG( F32, translation ) ) +B2_REC_OP( 0xBB, PrismaticJointEnableLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) ) +B2_REC_OP( 0xBC, PrismaticJointSetLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) ) +B2_REC_OP( 0xBD, PrismaticJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) ) +B2_REC_OP( 0xBE, PrismaticJointSetMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, motorSpeed ) ) +B2_REC_OP( 0xBF, PrismaticJointSetMaxMotorForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, force ) ) // Revolute joint -B2_REC_OP( 0xBD, RevoluteJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) ) -B2_REC_OP( 0xBE, RevoluteJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) -B2_REC_OP( 0xBF, RevoluteJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) -B2_REC_OP( 0xC0, RevoluteJointSetTargetAngle, RET_NONE, ARG( JOINTID, joint ) ARG( F32, angle ) ) -B2_REC_OP( 0xC1, RevoluteJointEnableLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) ) -B2_REC_OP( 0xC2, RevoluteJointSetLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) ) -B2_REC_OP( 0xC3, RevoluteJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) ) -B2_REC_OP( 0xC4, RevoluteJointSetMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, motorSpeed ) ) -B2_REC_OP( 0xC5, RevoluteJointSetMaxMotorTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, torque ) ) +B2_REC_OP( 0xC0, CreateRevoluteJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( REVOLUTEJOINTDEF, def ) ) +B2_REC_OP( 0xC1, RevoluteJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) ) +B2_REC_OP( 0xC2, RevoluteJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) +B2_REC_OP( 0xC3, RevoluteJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) +B2_REC_OP( 0xC4, RevoluteJointSetTargetAngle, RET_NONE, ARG( JOINTID, joint ) ARG( F32, angle ) ) +B2_REC_OP( 0xC5, RevoluteJointEnableLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) ) +B2_REC_OP( 0xC6, RevoluteJointSetLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) ) +B2_REC_OP( 0xC7, RevoluteJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) ) +B2_REC_OP( 0xC8, RevoluteJointSetMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, motorSpeed ) ) +B2_REC_OP( 0xC9, RevoluteJointSetMaxMotorTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, torque ) ) // Weld joint -B2_REC_OP( 0xC6, WeldJointSetLinearHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) -B2_REC_OP( 0xC7, WeldJointSetLinearDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) -B2_REC_OP( 0xC8, WeldJointSetAngularHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) -B2_REC_OP( 0xC9, WeldJointSetAngularDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) +B2_REC_OP( 0xCA, CreateWeldJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( WELDJOINTDEF, def ) ) +B2_REC_OP( 0xCB, WeldJointSetLinearHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) +B2_REC_OP( 0xCC, WeldJointSetLinearDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) +B2_REC_OP( 0xCD, WeldJointSetAngularHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) +B2_REC_OP( 0xCE, WeldJointSetAngularDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) // Wheel joint -B2_REC_OP( 0xCA, WheelJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) ) -B2_REC_OP( 0xCB, WheelJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) -B2_REC_OP( 0xCC, WheelJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) -B2_REC_OP( 0xCD, WheelJointEnableLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) ) -B2_REC_OP( 0xCE, WheelJointSetLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) ) -B2_REC_OP( 0xCF, WheelJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) ) -B2_REC_OP( 0xD0, WheelJointSetMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, motorSpeed ) ) -B2_REC_OP( 0xD1, WheelJointSetMaxMotorTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, torque ) ) +B2_REC_OP( 0xCF, CreateWheelJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( WHEELJOINTDEF, def ) ) +B2_REC_OP( 0xD0, WheelJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) ) +B2_REC_OP( 0xD1, WheelJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) +B2_REC_OP( 0xD2, WheelJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) +B2_REC_OP( 0xD3, WheelJointEnableLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) ) +B2_REC_OP( 0xD4, WheelJointSetLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) ) +B2_REC_OP( 0xD5, WheelJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) ) +B2_REC_OP( 0xD6, WheelJointSetMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, motorSpeed ) ) +B2_REC_OP( 0xD7, WheelJointSetMaxMotorTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, torque ) ) // Spatial queries. Inputs through the manifest (reader side). Hit tail / results hand-written. // Query geometry is relative to the origin so recordings reproduce queries far from the world origin. diff --git a/src/recording_replay.c b/src/recording_replay.c index 8f3c99a50..721378532 100644 --- a/src/recording_replay.c +++ b/src/recording_replay.c @@ -456,6 +456,13 @@ b2DistanceJointDef b2RecR_DISTANCEJOINTDEF( b2RecReader* rdr ) return def; } +b2FilterJointDef b2RecR_FILTERJOINTDEF( b2RecReader* rdr ) +{ + b2FilterJointDef def = b2DefaultFilterJointDef(); + b2RecR_JointBase( rdr, &def.base ); + return def; +} + b2MotorJointDef b2RecR_MOTORJOINTDEF( b2RecReader* rdr ) { b2MotorJointDef def = b2DefaultMotorJointDef(); @@ -473,10 +480,12 @@ b2MotorJointDef b2RecR_MOTORJOINTDEF( b2RecReader* rdr ) return def; } -b2FilterJointDef b2RecR_FILTERJOINTDEF( b2RecReader* rdr ) +b2MoverJointDef b2RecR_MOVERJOINTDEF( b2RecReader* rdr ) { - b2FilterJointDef def = b2DefaultFilterJointDef(); + b2MoverJointDef def = b2DefaultMoverJointDef(); b2RecR_JointBase( rdr, &def.base ); + def.linearVelocity = b2RecR_VEC2( rdr ); + def.maxVelocityForce = b2RecR_VEC2( rdr ); return def; } @@ -497,6 +506,21 @@ b2PrismaticJointDef b2RecR_PRISMATICJOINTDEF( b2RecReader* rdr ) return def; } +b2PogoJointDef b2RecR_POGOJOINTDEF( b2RecReader* rdr ) +{ + b2PogoJointDef def = b2DefaultPogoJointDef(); + b2RecR_JointBase( rdr, &def.base ); + def.normal = b2RecR_VEC2( rdr ); + def.hertz = b2RecR_F32( rdr ); + def.dampingRatio = b2RecR_F32( rdr ); + def.restLength = b2RecR_F32( rdr ); + def.maxTensionForce = b2RecR_F32( rdr ); + def.maxCompressionForce = b2RecR_F32( rdr ); + def.impulse = b2RecR_F32( rdr ); + def.velocity = b2RecR_F32( rdr ); + return def; +} + b2RevoluteJointDef b2RecR_REVOLUTEJOINTDEF( b2RecReader* rdr ) { b2RevoluteJointDef def = b2DefaultRevoluteJointDef(); @@ -1190,6 +1214,15 @@ static void b2RecDispatch_CreateDistanceJoint( const b2RecArgs_CreateDistanceJoi b2RecCheckJointId( rdr, b2CreateDistanceJoint( rdr->replayWorldId, &def ), recId ); } +static void b2RecDispatch_CreateFilterJoint( const b2RecArgs_CreateFilterJoint* a, b2RecReader* rdr ) +{ + b2JointId recId = b2RecR_JOINTID( rdr ); + b2FilterJointDef def = a->def; + def.base.bodyIdA = b2RecMakeBodyId( rdr, def.base.bodyIdA ); + def.base.bodyIdB = b2RecMakeBodyId( rdr, def.base.bodyIdB ); + b2RecCheckJointId( rdr, b2CreateFilterJoint( rdr->replayWorldId, &def ), recId ); +} + static void b2RecDispatch_CreateMotorJoint( const b2RecArgs_CreateMotorJoint* a, b2RecReader* rdr ) { b2JointId recId = b2RecR_JOINTID( rdr ); @@ -1199,13 +1232,13 @@ static void b2RecDispatch_CreateMotorJoint( const b2RecArgs_CreateMotorJoint* a, b2RecCheckJointId( rdr, b2CreateMotorJoint( rdr->replayWorldId, &def ), recId ); } -static void b2RecDispatch_CreateFilterJoint( const b2RecArgs_CreateFilterJoint* a, b2RecReader* rdr ) +static void b2RecDispatch_CreateMoverJoint( const b2RecArgs_CreateMoverJoint* a, b2RecReader* rdr ) { b2JointId recId = b2RecR_JOINTID( rdr ); - b2FilterJointDef def = a->def; + b2MoverJointDef def = a->def; def.base.bodyIdA = b2RecMakeBodyId( rdr, def.base.bodyIdA ); def.base.bodyIdB = b2RecMakeBodyId( rdr, def.base.bodyIdB ); - b2RecCheckJointId( rdr, b2CreateFilterJoint( rdr->replayWorldId, &def ), recId ); + b2RecCheckJointId( rdr, b2CreateMoverJoint( rdr->replayWorldId, &def ), recId ); } static void b2RecDispatch_CreatePrismaticJoint( const b2RecArgs_CreatePrismaticJoint* a, b2RecReader* rdr ) @@ -1217,6 +1250,15 @@ static void b2RecDispatch_CreatePrismaticJoint( const b2RecArgs_CreatePrismaticJ b2RecCheckJointId( rdr, b2CreatePrismaticJoint( rdr->replayWorldId, &def ), recId ); } +static void b2RecDispatch_CreatePogoJoint( const b2RecArgs_CreatePogoJoint* a, b2RecReader* rdr ) +{ + b2JointId recId = b2RecR_JOINTID( rdr ); + b2PogoJointDef def = a->def; + def.base.bodyIdA = b2RecMakeBodyId( rdr, def.base.bodyIdA ); + def.base.bodyIdB = b2RecMakeBodyId( rdr, def.base.bodyIdB ); + b2RecCheckJointId( rdr, b2CreatePogoJoint( rdr->replayWorldId, &def ), recId ); +} + static void b2RecDispatch_CreateRevoluteJoint( const b2RecArgs_CreateRevoluteJoint* a, b2RecReader* rdr ) { b2JointId recId = b2RecR_JOINTID( rdr ); @@ -1246,7 +1288,7 @@ static void b2RecDispatch_CreateWheelJoint( const b2RecArgs_CreateWheelJoint* a, static void b2RecDispatch_DestroyJoint( const b2RecArgs_DestroyJoint* a, b2RecReader* rdr ) { - b2DestroyJoint( b2RecMakeJointId( rdr, a->joint ), a->wakeAttached ); + b2DestroyJoint( b2RecMakeJointId( rdr, a->joint ) ); } // Generic joint mutators @@ -1393,6 +1435,34 @@ static void b2RecDispatch_MotorJointSetMaxSpringTorque( const b2RecArgs_MotorJoi b2MotorJoint_SetMaxSpringTorque( b2RecMakeJointId( rdr, a->joint ), a->maxTorque ); } +// Mover joint +static void b2RecDispatch_MoverJointSetLinearVelocity( const b2RecArgs_MoverJointSetLinearVelocity* a, b2RecReader* rdr ) +{ + b2MoverJoint_SetLinearVelocity( b2RecMakeJointId( rdr, a->joint ), a->velocity ); +} + +static void b2RecDispatch_MoverJointSetMaxVelocityForce( const b2RecArgs_MoverJointSetMaxVelocityForce* a, b2RecReader* rdr ) +{ + b2MoverJoint_SetMaxVelocityForce( b2RecMakeJointId( rdr, a->joint ), a->maxForce ); +} + +// Pogo joint + +static void b2RecDispatch_PogoJointSetRestLength( const b2RecArgs_PogoJointSetRestLength* a, b2RecReader* rdr ) +{ + b2PogoJoint_SetRestLength( b2RecMakeJointId( rdr, a->joint ), a->length ); +} + +static void b2RecDispatch_PogoJointSetSpringHertz( const b2RecArgs_PogoJointSetSpringHertz* a, b2RecReader* rdr ) +{ + b2PogoJoint_SetSpringHertz( b2RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b2RecDispatch_PogoJointSetSpringDampingRatio( const b2RecArgs_PogoJointSetSpringDampingRatio* a, b2RecReader* rdr ) +{ + b2PogoJoint_SetSpringDampingRatio( b2RecMakeJointId( rdr, a->joint ), a->dampingRatio ); +} + // Prismatic joint static void b2RecDispatch_PrismaticJointEnableSpring( const b2RecArgs_PrismaticJointEnableSpring* a, b2RecReader* rdr ) @@ -1974,7 +2044,11 @@ static int b2RecDispatchOne( b2RecPlayer* player ) { \ b2RecArgs_##Name a; \ memset( &a, 0, sizeof( a ) ); \ - __VA_ARGS__ b2RecDispatch_##Name( &a, rdr ); \ + __VA_ARGS__ \ + if ( rdr->ok ) \ + { \ + b2RecDispatch_##Name( &a, rdr ); \ + } \ break; \ } #include "recording_ops.inl" diff --git a/src/recording_replay.h b/src/recording_replay.h index dee78c54c..c099913e2 100644 --- a/src/recording_replay.h +++ b/src/recording_replay.h @@ -181,8 +181,10 @@ b2BodyDef b2RecR_BODYDEF( b2RecReader* rdr ); b2ShapeDef b2RecR_SHAPEDEF( b2RecReader* rdr ); b2ChainDef b2RecR_CHAINDEF( b2RecReader* rdr ); b2DistanceJointDef b2RecR_DISTANCEJOINTDEF( b2RecReader* rdr ); -b2MotorJointDef b2RecR_MOTORJOINTDEF( b2RecReader* rdr ); b2FilterJointDef b2RecR_FILTERJOINTDEF( b2RecReader* rdr ); +b2MotorJointDef b2RecR_MOTORJOINTDEF( b2RecReader* rdr ); +b2MoverJointDef b2RecR_MOVERJOINTDEF( b2RecReader* rdr ); +b2PogoJointDef b2RecR_POGOJOINTDEF( b2RecReader* rdr ); b2PrismaticJointDef b2RecR_PRISMATICJOINTDEF( b2RecReader* rdr ); b2RevoluteJointDef b2RecR_REVOLUTEJOINTDEF( b2RecReader* rdr ); b2WeldJointDef b2RecR_WELDJOINTDEF( b2RecReader* rdr ); diff --git a/src/revolute_joint.c b/src/revolute_joint.c index c1dd50c9e..1174904fd 100644 --- a/src/revolute_joint.c +++ b/src/revolute_joint.c @@ -136,20 +136,16 @@ float b2RevoluteJoint_GetUpperLimit( b2JointId jointId ) void b2RevoluteJoint_SetLimits( b2JointId jointId, float lower, float upper ) { + B2_ASSERT( lower <= upper ); + b2World* world = b2GetWorld( jointId.world0 ); B2_REC( world, RevoluteJointSetLimits, jointId, lower, upper ); - B2_ASSERT( lower <= upper ); - B2_ASSERT( lower >= -0.99f * B2_PI ); - B2_ASSERT( upper <= 0.99f * B2_PI ); b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); - if ( lower != joint->revoluteJoint.lowerAngle || upper != joint->revoluteJoint.upperAngle ) - { - joint->revoluteJoint.lowerAngle = b2MinFloat( lower, upper ); - joint->revoluteJoint.upperAngle = b2MaxFloat( lower, upper ); - joint->revoluteJoint.lowerImpulse = 0.0f; - joint->revoluteJoint.upperImpulse = 0.0f; - } + float lowerAngle = b2MinFloat( lower, upper ); + float upperAngle = b2MaxFloat( lower, upper ); + joint->revoluteJoint.lowerAngle = b2ClampFloat( lowerAngle, -0.99f * B2_PI, 0.99f * B2_PI ); + joint->revoluteJoint.upperAngle = b2ClampFloat( upperAngle, -0.99f * B2_PI, 0.99f * B2_PI ); } void b2RevoluteJoint_EnableMotor( b2JointId jointId, bool enableMotor ) @@ -510,8 +506,8 @@ void b2DrawRevoluteJoint( b2DebugDraw* draw, b2JointSim* base, b2WorldTransform b2RevoluteJoint* joint = &base->revoluteJoint; - b2WorldTransform frameA = b2OffsetWorldTransform( transformA, base->localFrameA ); - b2WorldTransform frameB = b2OffsetWorldTransform( transformB, base->localFrameB ); + b2WorldTransform frameA = b2MulWorldTransforms( transformA, base->localFrameA ); + b2WorldTransform frameB = b2MulWorldTransforms( transformB, base->localFrameB ); const float radius = 0.25f * drawScale; draw->DrawCircleFcn( frameB.p, radius, b2_colorGray, draw->context ); diff --git a/src/shape.c b/src/shape.c index 573ee14e8..62f639988 100644 --- a/src/shape.c +++ b/src/shape.c @@ -319,7 +319,7 @@ b2ShapeId b2CreateChainSegmentShape( b2BodyId bodyId, const b2ShapeDef* def, con } // Destroy a shape on a body. This doesn't need to be called when destroying a body. -static void b2DestroyShapeInternal( b2World* world, b2Shape* shape, b2Body* body, bool wakeBodies ) +static void b2DestroyShapeInternal( b2World* world, b2Shape* shape, b2Body* body ) { int shapeId = shape->id; @@ -358,7 +358,7 @@ static void b2DestroyShapeInternal( b2World* world, b2Shape* shape, b2Body* body if ( contact->shapeIdA == shapeId || contact->shapeIdB == shapeId ) { - b2DestroyContact( world, contact, wakeBodies ); + b2DestroyContact( world, contact ); } } @@ -427,10 +427,8 @@ void b2DestroyShape( b2ShapeId shapeId, bool updateBodyMass ) B2_REC( world, DestroyShape, shapeId, updateBodyMass ); - // need to wake bodies because this might be a static body - bool wakeBodies = true; b2Body* body = b2Array_Get( world->bodies, shape->bodyId ); - b2DestroyShapeInternal( world, shape, body, wakeBodies ); + b2DestroyShapeInternal( world, shape, body ); if ( updateBodyMass == true ) { @@ -630,8 +628,7 @@ void b2DestroyChain( b2ChainId chainId ) { int shapeId = chain->shapeIndices[i]; b2Shape* shape = b2Array_Get( world->shapes, shapeId ); - bool wakeBodies = true; - b2DestroyShapeInternal( world, shape, body, wakeBodies ); + b2DestroyShapeInternal( world, shape, body ); } b2FreeChainData( chain ); @@ -1317,7 +1314,7 @@ b2Filter b2Shape_GetFilter( b2ShapeId shapeId ) return shape->filter; } -static void b2ResetProxy( b2World* world, b2Shape* shape, bool wakeBodies, bool destroyProxy ) +static void b2ResetProxy( b2World* world, b2Shape* shape, bool destroyProxy ) { b2Body* body = b2Array_Get( world->bodies, shape->bodyId ); @@ -1335,7 +1332,7 @@ static void b2ResetProxy( b2World* world, b2Shape* shape, bool wakeBodies, bool if ( contact->shapeIdA == shapeId || contact->shapeIdB == shapeId ) { - b2DestroyContact( world, contact, wakeBodies ); + b2DestroyContact( world, contact ); } } @@ -1389,9 +1386,7 @@ void b2Shape_SetFilter( b2ShapeId shapeId, b2Filter filter ) shape->filter = filter; - // need to wake bodies because a filter change may destroy contacts - bool wakeBodies = true; - b2ResetProxy( world, shape, wakeBodies, destroyProxy ); + b2ResetProxy( world, shape, destroyProxy ); // note: this does not immediately update sensor overlaps. Instead sensor // overlaps are updated the next time step @@ -1543,10 +1538,8 @@ void b2Shape_SetCircle( b2ShapeId shapeId, const b2Circle* circle ) shape->type = b2_circleShape; shape->aabbMargin = b2ComputeShapeMargin( shape ); - // need to wake bodies so they can react to the shape change - bool wakeBodies = true; bool destroyProxy = true; - b2ResetProxy( world, shape, wakeBodies, destroyProxy ); + b2ResetProxy( world, shape, destroyProxy ); } void b2Shape_SetCapsule( b2ShapeId shapeId, const b2Capsule* capsule ) @@ -1570,10 +1563,8 @@ void b2Shape_SetCapsule( b2ShapeId shapeId, const b2Capsule* capsule ) shape->type = b2_capsuleShape; shape->aabbMargin = b2ComputeShapeMargin( shape ); - // need to wake bodies so they can react to the shape change - bool wakeBodies = true; bool destroyProxy = true; - b2ResetProxy( world, shape, wakeBodies, destroyProxy ); + b2ResetProxy( world, shape, destroyProxy ); } void b2Shape_SetSegment( b2ShapeId shapeId, const b2Segment* segment ) @@ -1591,10 +1582,8 @@ void b2Shape_SetSegment( b2ShapeId shapeId, const b2Segment* segment ) shape->type = b2_segmentShape; shape->aabbMargin = b2ComputeShapeMargin( shape ); - // need to wake bodies so they can react to the shape change - bool wakeBodies = true; bool destroyProxy = true; - b2ResetProxy( world, shape, wakeBodies, destroyProxy ); + b2ResetProxy( world, shape, destroyProxy ); } void b2Shape_SetPolygon( b2ShapeId shapeId, const b2Polygon* polygon ) @@ -1612,10 +1601,8 @@ void b2Shape_SetPolygon( b2ShapeId shapeId, const b2Polygon* polygon ) shape->type = b2_polygonShape; shape->aabbMargin = b2ComputeShapeMargin( shape ); - // need to wake bodies so they can react to the shape change - bool wakeBodies = true; bool destroyProxy = true; - b2ResetProxy( world, shape, wakeBodies, destroyProxy ); + b2ResetProxy( world, shape, destroyProxy ); } void b2Shape_SetChainSegment( b2ShapeId shapeId, const b2ChainSegment* chainSegment ) @@ -1648,9 +1635,8 @@ void b2Shape_SetChainSegment( b2ShapeId shapeId, const b2ChainSegment* chainSegm shape->type = b2_chainSegmentShape; shape->aabbMargin = b2ComputeShapeMargin( shape ); - bool wakeBodies = true; bool destroyProxy = true; - b2ResetProxy( world, shape, wakeBodies, destroyProxy ); + b2ResetProxy( world, shape, destroyProxy ); } b2ChainId b2Shape_GetParentChain( b2ShapeId shapeId ) diff --git a/src/solver.c b/src/solver.c index dfa5b5323..d4f0da800 100644 --- a/src/solver.c +++ b/src/solver.c @@ -359,14 +359,14 @@ static bool b2ContinuousQueryCallback( int proxyId, uint64_t userData, void* con } } - if ( didHit && ( shape->enablePreSolveEvents || fastShape->enablePreSolveEvents ) && world->preSolveFcn != NULL ) + if ( didHit && ( shape->enablePreSolveEvents || fastShape->enablePreSolveEvents ) && world->preContinuousFcn != NULL ) { b2ShapeId shapeIdA = { shape->id + 1, world->worldId, shape->generation }; b2ShapeId shapeIdB = { fastShape->id + 1, world->worldId, fastShape->generation }; // TOI runs in the base frame, lift the hit point back to world for the callback b2Pos worldPoint = b2OffsetPos( continuousContext->base, output.point ); - didHit = world->preSolveFcn( shapeIdA, shapeIdB, worldPoint, output.normal, world->preSolveContext ); + didHit = world->preContinuousFcn( shapeIdA, shapeIdB, worldPoint, output.normal, world->preSolveContext ); } if ( didHit ) @@ -518,6 +518,10 @@ static void b2SolveContinuous( b2World* world, int bodySimIndex, b2TaskContext* if ( b2AABB_Contains( shape->fatAABB, shape->aabb ) == false ) { float margin = shape->aabbMargin; + + // Note: far from the origin the margin can be snapped to the nearest ULP. + // This relevant for DP mode. So we lose the broad-phase hysteresis. + // todo consider using b2Expand to ensure at least one ULP of margin. b2AABB fatAABB; fatAABB.lowerBound.x = shape->aabb.lowerBound.x - margin; fatAABB.lowerBound.y = shape->aabb.lowerBound.y - margin; @@ -1279,7 +1283,6 @@ void b2Solve( b2World* world, b2StepContext* stepContext ) int awakeBodyCount = awakeSet->bodySims.count; if ( awakeBodyCount == 0 ) { - b2ValidateNoEnlarged( &world->broadPhase ); return; } diff --git a/src/timer.c b/src/timer.c index 6a2edfde0..b08c6c89e 100644 --- a/src/timer.c +++ b/src/timer.c @@ -20,7 +20,8 @@ #define WIN32_LEAN_AND_MEAN 1 #endif -#include +// On Windows this is capitalized, but not in Linux mingw. +#include #include static double s_invFrequency = 0.0; diff --git a/src/weld_joint.c b/src/weld_joint.c index 48b6e2b93..ac6cc750a 100644 --- a/src/weld_joint.c +++ b/src/weld_joint.c @@ -457,8 +457,8 @@ void b2DrawWeldJoint( b2DebugDraw* draw, b2JointSim* base, b2WorldTransform tran { B2_ASSERT( base->type == b2_weldJoint ); - b2WorldTransform frameA = b2OffsetWorldTransform( transformA, base->localFrameA ); - b2WorldTransform frameB = b2OffsetWorldTransform( transformB, base->localFrameB ); + b2WorldTransform frameA = b2MulWorldTransforms( transformA, base->localFrameA ); + b2WorldTransform frameB = b2MulWorldTransforms( transformB, base->localFrameB ); b2Polygon box = b2MakeBox( 0.25f * drawScale, 0.125f * drawScale ); draw->DrawPolygonFcn( frameA, box.vertices, 4, b2_colorDarkOrange, draw->context ); diff --git a/src/wheel_joint.c b/src/wheel_joint.c index 94a69da3c..88d7e63a5 100644 --- a/src/wheel_joint.c +++ b/src/wheel_joint.c @@ -99,13 +99,8 @@ void b2WheelJoint_SetLimits( b2JointId jointId, float lower, float upper ) B2_ASSERT( lower <= upper ); b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); - if ( lower != joint->wheelJoint.lowerTranslation || upper != joint->wheelJoint.upperTranslation ) - { - joint->wheelJoint.lowerTranslation = b2MinFloat( lower, upper ); - joint->wheelJoint.upperTranslation = b2MaxFloat( lower, upper ); - joint->wheelJoint.lowerImpulse = 0.0f; - joint->wheelJoint.upperImpulse = 0.0f; - } + joint->wheelJoint.lowerTranslation = b2MinFloat( lower, upper ); + joint->wheelJoint.upperTranslation = b2MaxFloat( lower, upper ); } void b2WheelJoint_EnableMotor( b2JointId jointId, bool enableMotor ) @@ -557,8 +552,8 @@ void b2DrawWheelJoint( b2DebugDraw* draw, b2JointSim* base, b2WorldTransform tra b2WheelJoint* joint = &base->wheelJoint; - b2WorldTransform frameA = b2OffsetWorldTransform( transformA, base->localFrameA ); - b2WorldTransform frameB = b2OffsetWorldTransform( transformB, base->localFrameB ); + b2WorldTransform frameA = b2MulWorldTransforms( transformA, base->localFrameA ); + b2WorldTransform frameB = b2MulWorldTransforms( transformB, base->localFrameB ); b2Vec2 axisA = b2RotateVector( frameA.q, (b2Vec2){ 1.0f, 0.0f } ); b2HexColor c1 = b2_colorGray; diff --git a/src/world_snapshot.c b/src/world_snapshot.c index e3932ba35..45bfa5f00 100644 --- a/src/world_snapshot.c +++ b/src/world_snapshot.c @@ -828,6 +828,20 @@ static bool b2DeserializeIntoShell( b2SnapReader* r, b2World* world ) } } + // Event buffers are transient and never serialized. An in-place restore reuses the live + // world's arrays, so end events queued by a between-step mutator (b2Body_Disable and friends) + // would survive the restore and surface after the first resimmed step. Reset to match a fresh + // world from snapshot. The double-buffer parity is restored, but the contents must start empty. + b2Array_Clear( world->bodyMoveEvents ); + b2Array_Clear( world->sensorBeginEvents ); + b2Array_Clear( world->sensorEndEvents[0] ); + b2Array_Clear( world->sensorEndEvents[1] ); + b2Array_Clear( world->contactBeginEvents ); + b2Array_Clear( world->contactEndEvents[0] ); + b2Array_Clear( world->contactEndEvents[1] ); + b2Array_Clear( world->contactHitEvents ); + b2Array_Clear( world->jointEvents ); + return r->ok; } @@ -1097,6 +1111,10 @@ uint64_t b2HashWorldStateDeep( b2World* world ) hash = b2FnvMixFloat( hash, sim->distanceJoint.upperImpulse ); hash = b2FnvMixFloat( hash, sim->distanceJoint.motorImpulse ); break; + case b2_moverJoint: + hash = b2FnvMixFloat( hash, sim->moverJoint.linearVelocityImpulse.x ); + hash = b2FnvMixFloat( hash, sim->moverJoint.linearVelocityImpulse.y ); + break; case b2_motorJoint: hash = b2FnvMixFloat( hash, sim->motorJoint.linearVelocityImpulse.x ); hash = b2FnvMixFloat( hash, sim->motorJoint.linearVelocityImpulse.y ); @@ -1105,6 +1123,10 @@ uint64_t b2HashWorldStateDeep( b2World* world ) hash = b2FnvMixFloat( hash, sim->motorJoint.linearSpringImpulse.y ); hash = b2FnvMixFloat( hash, sim->motorJoint.angularSpringImpulse ); break; + case b2_pogoJoint: + hash = b2FnvMixFloat( hash, sim->pogoJoint.impulse ); + hash = b2FnvMixFloat( hash, sim->pogoJoint.velocity ); + break; case b2_prismaticJoint: hash = b2FnvMixBytes( hash, &sim->prismaticJoint.impulse, sizeof( b2Vec2 ) ); hash = b2FnvMixFloat( hash, sim->prismaticJoint.springImpulse ); @@ -1157,3 +1179,9 @@ uint64_t b2HashWorldStateDeep( b2World* world ) return hash; } + +uint64_t b2World_GetStateHash( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + return b2HashWorldStateDeep( world ); +} diff --git a/test/main.c b/test/main.c index f1a9fc1da..697a872f6 100644 --- a/test/main.c +++ b/test/main.c @@ -53,6 +53,12 @@ int main( int argc, char** argv ) //_CrtSetBreakAlloc(196); _CrtSetReportMode( _CRT_WARN, _CRTDBG_MODE_DEBUG | _CRTDBG_MODE_FILE ); _CrtSetReportFile( _CRT_WARN, _CRTDBG_FILE_STDERR ); + // Route CRT errors and assertions to stderr instead of a modal dialog so a headless run + // (CI, redirected stdout) fails fast rather than blocking on Abort/Retry/Ignore. + _CrtSetReportMode( _CRT_ERROR, _CRTDBG_MODE_DEBUG | _CRTDBG_MODE_FILE ); + _CrtSetReportFile( _CRT_ERROR, _CRTDBG_FILE_STDERR ); + _CrtSetReportMode( _CRT_ASSERT, _CRTDBG_MODE_DEBUG | _CRTDBG_MODE_FILE ); + _CrtSetReportFile( _CRT_ASSERT, _CRTDBG_FILE_STDERR ); //_CrtSetDbgFlag(_CRTDBG_LEAK_CHECK_DF | _CrtSetDbgFlag(_CRTDBG_REPORT_FLAG)); //_CrtSetAllocHook(MyAllocHook); #endif diff --git a/test/test_distance.c b/test/test_distance.c index 415bfb196..b47af00c8 100644 --- a/test/test_distance.c +++ b/test/test_distance.c @@ -4,6 +4,7 @@ #include "test_macros.h" #include "box2d/collision.h" +#include "box2d/constants.h" #include "box2d/math_functions.h" #include @@ -60,12 +61,13 @@ static int ShapeCastTest( void ) ( b2Vec2 ){ 2.0f, 1.0f }, }; - b2ShapeCastPairInput input; + b2ShapeCastPairInput input = { 0 }; input.proxyA = b2MakeProxy( vas, ARRAY_COUNT( vas ), 0.0f ); input.proxyB = b2MakeProxy( vbs, ARRAY_COUNT( vbs ), 0.0f ); input.transform = b2Transform_identity; input.translationB = ( b2Vec2 ){ -2.0f, 0.0f }; input.maxFraction = 1.0f; + input.canEncroach = false; b2CastOutput output = b2ShapeCast( &input ); @@ -75,6 +77,86 @@ static int ShapeCastTest( void ) return 0; } +// A thin plank and a point a linear slop off the middle of its short end face. The closest feature +// is that end face, so GJK finishes on a two point simplex spanning it. +// +// b2SolveSimplex2 hands back a search direction of magnitude 2 * distance * edge^2, which the +// caller weighs against an absolute epsilon. That test is cubic in length, so it gives out on +// short edges: below an edge of about 3.5 mm at default length units it declares the shapes +// overlapped and returns a zero distance with no normal, though they are plainly apart. +// +// Measured cutoff matches 2 * distance * edge^2 < FLT_EPSILON to three digits, and it tracks the +// contact edge alone. Widening the plank from 1 m to 100 m changes nothing. +static int ShapeDistanceShortEdgeTest( void ) +{ + float halfThickness = 0.0016f; + + b2Vec2 corners[] = { + ( b2Vec2 ){ -0.5f, -halfThickness }, + ( b2Vec2 ){ 0.5f, -halfThickness }, + ( b2Vec2 ){ 0.5f, halfThickness }, + ( b2Vec2 ){ -0.5f, halfThickness }, + }; + + b2Vec2 point = { 0.5f + B2_LINEAR_SLOP, 0.0f }; + + b2DistanceInput input = { 0 }; + input.proxyA = b2MakeProxy( corners, ARRAY_COUNT( corners ), 0.0f ); + input.proxyB = b2MakeProxy( &point, 1, 0.0f ); + input.transform = b2Transform_identity; + input.useRadii = false; + + b2SimplexCache cache = { 0 }; + b2DistanceOutput output = b2ShapeDistance( &input, &cache, NULL, 0 ); + + ENSURE( output.distance > 0.0f ); + ENSURE( b2IsNormalized( output.normal ) ); + ENSURE_SMALL( output.distance - B2_LINEAR_SLOP, 1e-6f ); + + return 0; +} + +// The same false overlap seen through b2ShapeCast, which is how it was reported. +// +// Conservative advancement stops the cores a target apart and target is at least a linear slop, so +// the query can never legitimately answer overlap after the first iteration. Here iteration 1 does, +// and b2ShapeCast has no fallback: it trips +// B2_ASSERT( distanceOutput.distance > 0.0f && b2IsNormalized( distanceOutput.normal ) ). +// +// Enable once b2ShapeDistance stops reporting the false overlap. The assert aborts the run, so +// leaving it on would take the rest of the suite with it. +#define B2_SHORT_EDGE_CAST_REPRO 0 +#if B2_SHORT_EDGE_CAST_REPRO +static int ShapeCastShortEdgeTest( void ) +{ + float halfThickness = 0.0002f; + + b2Vec2 corners[] = { + ( b2Vec2 ){ -0.5f, -halfThickness }, + ( b2Vec2 ){ 0.5f, -halfThickness }, + ( b2Vec2 ){ 0.5f, halfThickness }, + ( b2Vec2 ){ -0.5f, halfThickness }, + }; + + // falls past the end of the plank and drifts into it, so the second query lands on the end face + b2Vec2 start = { 0.506f, 0.0035f }; + + b2ShapeCastPairInput input = { 0 }; + input.proxyA = b2MakeProxy( corners, ARRAY_COUNT( corners ), 0.0f ); + input.proxyB = b2MakeProxy( &start, 1, 0.0f ); + input.transform = b2Transform_identity; + input.translationB = ( b2Vec2 ){ -0.01f, -0.5f }; + input.maxFraction = 1.0f; + input.canEncroach = false; + + b2CastOutput output = b2ShapeCast( &input ); + + ENSURE( output.hit == false || b2IsNormalized( output.normal ) ); + + return 0; +} +#endif + static int TimeOfImpactTest( void ) { b2Vec2 vas[] = { { -1.0f, -1.0f }, { 1.0f, -1.0f }, { 1.0f, 1.0f }, { -1.0f, 1.0f } }; @@ -104,6 +186,10 @@ int DistanceTest( void ) RUN_SUBTEST( SegmentDistanceTest ); RUN_SUBTEST( ShapeDistanceTest ); RUN_SUBTEST( ShapeCastTest ); + RUN_SUBTEST( ShapeDistanceShortEdgeTest ); +#if B2_SHORT_EDGE_CAST_REPRO + RUN_SUBTEST( ShapeCastShortEdgeTest ); +#endif RUN_SUBTEST( TimeOfImpactTest ); return 0; diff --git a/test/test_recording.c b/test/test_recording.c index e67d5b7c2..f8010d37f 100644 --- a/test/test_recording.c +++ b/test/test_recording.c @@ -428,7 +428,7 @@ int RecordingTest( void ) tmpJointDef.base.bodyIdB = jb[7]; tmpJointDef.length = 5.0f; b2JointId tmpJointId = b2CreateDistanceJoint( worldId, &tmpJointDef ); - b2DestroyJoint( tmpJointId, true ); + b2DestroyJoint( tmpJointId ); // Exercise world config mutators b2World_SetGravity( worldId, (b2Vec2){ 0.0f, -9.8f } ); diff --git a/test/test_snapshot.c b/test/test_snapshot.c index c56697e12..1db983bc6 100644 --- a/test/test_snapshot.c +++ b/test/test_snapshot.c @@ -548,6 +548,64 @@ int SnapshotTest( void ) b2DestroyRecording( rec ); } + // Phase 11: restore clears the event buffers. End events queued by a between-step mutator + // must not survive a rollback and surface after the first resimmed step. + { + b2WorldDef wd = b2DefaultWorldDef(); + wd.workerCount = 1; + b2WorldId wId = b2CreateWorld( &wd ); + b2World* w = b2GetWorldFromId( wId ); + + b2BodyDef gbd = b2DefaultBodyDef(); + gbd.position = (b2Pos){ 0.0f, -1.0f }; + b2BodyId ground = b2CreateBody( wId, &gbd ); + b2Polygon groundBox = b2MakeBox( 10.0f, 1.0f ); + b2ShapeDef gsd = b2DefaultShapeDef(); + gsd.enableContactEvents = true; + b2CreatePolygonShape( ground, &gsd, &groundBox ); + + b2BodyDef bd = b2DefaultBodyDef(); + bd.type = b2_dynamicBody; + bd.position = (b2Pos){ 0.0f, 0.5f }; + b2BodyId box = b2CreateBody( wId, &bd ); + b2Polygon boxPoly = b2MakeBox( 0.5f, 0.5f ); + b2ShapeDef bsd = b2DefaultShapeDef(); + bsd.enableContactEvents = true; + b2CreatePolygonShape( box, &bsd, &boxPoly ); + + // Settle so the box rests on the ground with a live touching contact + for ( int step = 0; step < 60; ++step ) + { + b2World_Step( wId, dt, subSteps ); + } + + // The public state hash accessor must match the internal deep hash + ENSURE( b2World_GetStateHash( wId ) == b2HashWorldStateDeep( w ) ); + + int snapSize = b2World_Snapshot( wId, NULL, 0 ); + uint8_t* snap = b2Alloc( snapSize ); + b2World_Snapshot( wId, snap, snapSize ); + + // Disabling the resting box destroys its touching contact and queues an end event, + // exactly the between-step mutation a rollback would discard + b2Body_Disable( box ); + ENSURE( w->contactEndEvents[w->endEventArrayIndex].count >= 1 ); + + ENSURE( b2World_Restore( wId, snap, snapSize ) ); + + // Restore must leave no queued end events in either half of the double buffer + ENSURE( w->contactEndEvents[0].count == 0 ); + ENSURE( w->contactEndEvents[1].count == 0 ); + + // The resimmed step keeps the box resting, so no end event should reach the user + b2World_Step( wId, dt, subSteps ); + b2ContactEvents events = b2World_GetContactEvents( wId ); + ENSURE( events.endCount == 0 ); + + b2Free( snap, snapSize ); + b2DestroyWorld( wId ); + } + remove( s_snapPath ); // Clean up diff --git a/test/test_world.c b/test/test_world.c index 82389c7ca..63e571534 100644 --- a/test/test_world.c +++ b/test/test_world.c @@ -252,14 +252,12 @@ static bool CustomFilter( b2ShapeId shapeIdA, b2ShapeId shapeIdB, void* context return true; } -static bool PreSolveStatic( b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Pos point, b2Vec2 normal, void* context ) +static void PreSolveStatic( b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifold, void* context ) { (void)shapeIdA; (void)shapeIdB; - (void)point; - (void)normal; - ENSURE( context == NULL ); - return false; + (void)manifold; + (void)context; } // This test is here to ensure all API functions link correctly. @@ -291,7 +289,7 @@ int TestWorldCoverage( void ) ENSURE( value == 100.0f ); b2World_SetCustomFilterCallback( worldId, CustomFilter, NULL ); - b2World_SetPreSolveCallback( worldId, PreSolveStatic, NULL ); + b2World_SetPreSolveCallback( worldId, PreSolveStatic, NULL, NULL ); b2Vec2 g = { 1.0f, 2.0f }; b2World_SetGravity( worldId, g );