From 573e1a9fa3f385312da6328fc72a200d3619a30a Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Sun, 10 May 2026 13:36:52 -0700 Subject: [PATCH] initial commit --- .clang-format | 42 + .gitattributes | 10 + .github/FUNDING.yml | 14 + .github/issue_template.md | 1 + .github/pull_request_template.md | 1 + .github/workflows/build.yml | 119 + .gitignore | 14 + CMakeLists.txt | 155 + README.md | 98 + benchmark/CMakeLists.txt | 13 + benchmark/amd7950x_scalar/washer.csv | 9 + benchmark/amd7950x_sse2/compound_capsules.csv | 9 + benchmark/amd7950x_sse2/compound_hulls.csv | 9 + benchmark/amd7950x_sse2/joint_grid.csv | 9 + benchmark/amd7950x_sse2/junkyard.csv | 9 + benchmark/amd7950x_sse2/large_pyramid.csv | 9 + benchmark/amd7950x_sse2/many_pyramids.csv | 9 + benchmark/amd7950x_sse2/rain.csv | 9 + benchmark/amd7950x_sse2/washer.csv | 9 + benchmark/amd7950x_sse2/welded_hulls.csv | 9 + benchmark/main.c | 344 + build.sh | 9 + build_vs2026.bat | 24 + data/dumps/single_box/box3d_dump.inl | 85 + data/fonts/Roboto-Medium.ttf | Bin 0 -> 168644 bytes data/meshes/building.obj | 4971 ++++++++++ data/meshes/collision_mesh_01.obj | 4056 +++++++++ data/meshes/conveyor.obj | 76 + data/meshes/single_triangle.obj | 4 + data/meshes/stairs.obj | 42 + data/meshes/test_map01.obj | 258 + data/meshes/voxel_mesh_01.obj | 5323 +++++++++++ data/meshes/voxel_mesh_02.obj | 6418 +++++++++++++ data/meshes/voxel_mesh_03.obj | 4373 +++++++++ data/meshes/voxel_mesh_04.obj | 3358 +++++++ data/trees/bounds01.txt | 288 + data/trees/bounds02.txt | 4221 +++++++++ data/trees/bounds03.txt | 268 + extern/glad/include/KHR/khrplatform.h | 311 + extern/glad/include/glad/glad.h | 4133 +++++++++ extern/glad/src/glad.c | 2121 +++++ extern/stb/stb_image.h | 7987 +++++++++++++++++ include/box3d/base.h | 169 + include/box3d/box3d.h | 1492 +++ include/box3d/collision.h | 1543 ++++ include/box3d/constants.h | 82 + include/box3d/id.h | 172 + include/box3d/math_functions.h | 1106 +++ include/box3d/types.h | 1523 ++++ samples/CMakeLists.txt | 149 + samples/camera.cpp | 330 + samples/camera.h | 85 + samples/earcut.h | 820 ++ samples/font.cpp | 276 + samples/font.h | 51 + samples/geo_buffer.cpp | 150 + samples/geo_buffer.h | 17 + samples/imconfig.h | 156 + samples/jsmn.h | 471 + samples/main.cpp | 356 + samples/mesh_loader.cpp | 172 + samples/mesh_loader.h | 21 + samples/renderer.cpp | 783 ++ samples/renderer.h | 44 + samples/sample.cpp | 1757 ++++ samples/sample.h | 215 + samples/sample_benchmark.cpp | 1468 +++ samples/sample_bodies.cpp | 965 ++ samples/sample_character.cpp | 438 + samples/sample_collision.cpp | 2821 ++++++ samples/sample_compound.cpp | 800 ++ samples/sample_continuous.cpp | 1040 +++ samples/sample_determinism.cpp | 59 + samples/sample_events.cpp | 918 ++ samples/sample_geometry.cpp | 651 ++ samples/sample_issues.cpp | 401 + samples/sample_joint.cpp | 2784 ++++++ samples/sample_manifold.cpp | 940 ++ samples/sample_mesh.cpp | 1523 ++++ samples/sample_ragdoll.cpp | 484 + samples/sample_robustness.cpp | 404 + samples/sample_shapes.cpp | 1145 +++ samples/sample_stacking.cpp | 955 ++ samples/sample_tree.cpp | 848 ++ samples/scene.cpp | 2320 +++++ samples/scene.h | 147 + samples/shader.cpp | 465 + samples/shader.h | 39 + samples/ssao_buffer.cpp | 163 + samples/ssao_buffer.h | 21 + samples/stb_image_write.h | 1724 ++++ samples/stb_truetype.h | 5079 +++++++++++ samples/tiny_obj_loader.h | 3499 ++++++++ samples/utility.h | 118 + shaders/common.glsl | 80 + shaders/deferred_lighting_fs.glsl | 253 + shaders/depth_copy_fs.glsl | 10 + shaders/empty.fs | 5 + shaders/forward.fs | 10 + shaders/forward.vs | 17 + shaders/geometry_fs.glsl | 20 + shaders/geometry_vs.glsl | 46 + shaders/mesh.fs | 20 + shaders/mesh.vs | 48 + shaders/mesh_edge.vs | 20 + shaders/mesh_forward.fs | 34 + shaders/mesh_forward.vs | 47 + shaders/mesh_shadow.vs | 14 + shaders/quad_vs.glsl | 11 + shaders/shadow_map.vs | 10 + shaders/ssao_blur_fs.glsl | 47 + shaders/ssao_fs.glsl | 67 + shared/CMakeLists.txt | 25 + shared/benchmarks.c | 1005 +++ shared/benchmarks.h | 47 + shared/determinism.c | 124 + shared/determinism.h | 41 + shared/human.c | 775 ++ shared/human.h | 82 + shared/stability.c | 74 + shared/stability.h | 24 + shared/utils.c | 33 + shared/utils.h | 122 + src/CMakeLists.txt | 240 + src/aabb.c | 124 + src/aabb.h | 80 + src/algorithm.h | 21 + src/arena_allocator.c | 127 + src/arena_allocator.h | 86 + src/bitset.c | 66 + src/bitset.h | 76 + src/block_allocator.c | 99 + src/block_allocator.h | 44 + src/body.c | 2323 +++++ src/body.h | 254 + src/box3d.natvis | 138 + src/broad_phase.c | 534 ++ src/broad_phase.h | 88 + src/capsule.c | 290 + src/compound.c | 1210 +++ src/compound.h | 46 + src/constraint_graph.c | 328 + src/constraint_graph.h | 86 + src/contact.c | 862 ++ src/contact.h | 185 + src/contact_solver.c | 2428 +++++ src/contact_solver.h | 82 + src/container.h | 184 + src/convex_manifold.c | 1603 ++++ src/core.c | 345 + src/core.h | 166 + src/ctz.h | 131 + src/distance.c | 1994 ++++ src/distance_joint.c | 558 ++ src/dynamic_tree.c | 2217 +++++ src/height_field.c | 1601 ++++ src/hull.c | 2910 ++++++ src/id_pool.c | 73 + src/id_pool.h | 43 + src/island.c | 735 ++ src/island.h | 110 + src/joint.c | 1725 ++++ src/joint.h | 424 + src/manifold.c | 246 + src/manifold.h | 66 + src/math_functions.c | 587 ++ src/math_internal.h | 513 ++ src/mesh.c | 2342 +++++ src/mesh_contact.c | 1130 +++ src/motor_joint.c | 437 + src/mover.c | 70 + src/parallel_for.c | 135 + src/parallel_for.h | 32 + src/parallel_joint.c | 244 + src/physics_world.c | 3627 ++++++++ src/physics_world.h | 333 + src/platform.h | 88 + src/prismatic_joint.c | 665 ++ src/qsort.h | 186 + src/revolute_joint.c | 649 ++ src/scheduler.c | 198 + src/scheduler.h | 24 + src/sensor.c | 452 + src/sensor.h | 51 + src/shape.c | 2340 +++++ src/shape.h | 143 + src/simd.c | 202 + src/simd.h | 374 + src/solver.c | 2292 +++++ src/solver.h | 315 + src/solver_set.c | 633 ++ src/solver_set.h | 76 + src/sphere.c | 241 + src/spherical_joint.c | 717 ++ src/table.c | 248 + src/table.h | 62 + src/timer.c | 671 ++ src/triangle_manifold.c | 1256 +++ src/types.c | 135 + src/verstable.h | 2069 +++++ src/weld_joint.c | 305 + src/wheel_joint.c | 1076 +++ test/CMakeLists.txt | 40 + test/main.c | 122 + test/test_allocator.c | 49 + test/test_bitset.c | 63 + test/test_collision.c | 210 + test/test_compound.c | 876 ++ test/test_container.c | 195 + test/test_determinism.c | 113 + test/test_distance.c | 112 + test/test_height_field.c | 246 + test/test_hull.c | 462 + test/test_id.c | 31 + test/test_macros.h | 66 + test/test_math.c | 277 + test/test_mover.c | 60 + test/test_shape.c | 322 + test/test_table.c | 113 + test/test_world.c | 720 ++ 220 files changed, 150440 insertions(+) create mode 100644 .clang-format create mode 100644 .gitattributes create mode 100644 .github/FUNDING.yml create mode 100644 .github/issue_template.md create mode 100644 .github/pull_request_template.md create mode 100644 .github/workflows/build.yml create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 README.md create mode 100644 benchmark/CMakeLists.txt create mode 100644 benchmark/amd7950x_scalar/washer.csv create mode 100644 benchmark/amd7950x_sse2/compound_capsules.csv create mode 100644 benchmark/amd7950x_sse2/compound_hulls.csv create mode 100644 benchmark/amd7950x_sse2/joint_grid.csv create mode 100644 benchmark/amd7950x_sse2/junkyard.csv create mode 100644 benchmark/amd7950x_sse2/large_pyramid.csv create mode 100644 benchmark/amd7950x_sse2/many_pyramids.csv create mode 100644 benchmark/amd7950x_sse2/rain.csv create mode 100644 benchmark/amd7950x_sse2/washer.csv create mode 100644 benchmark/amd7950x_sse2/welded_hulls.csv create mode 100644 benchmark/main.c create mode 100644 build.sh create mode 100644 build_vs2026.bat create mode 100644 data/dumps/single_box/box3d_dump.inl create mode 100644 data/fonts/Roboto-Medium.ttf create mode 100644 data/meshes/building.obj create mode 100644 data/meshes/collision_mesh_01.obj create mode 100644 data/meshes/conveyor.obj create mode 100644 data/meshes/single_triangle.obj create mode 100644 data/meshes/stairs.obj create mode 100644 data/meshes/test_map01.obj create mode 100644 data/meshes/voxel_mesh_01.obj create mode 100644 data/meshes/voxel_mesh_02.obj create mode 100644 data/meshes/voxel_mesh_03.obj create mode 100644 data/meshes/voxel_mesh_04.obj create mode 100644 data/trees/bounds01.txt create mode 100644 data/trees/bounds02.txt create mode 100644 data/trees/bounds03.txt create mode 100644 extern/glad/include/KHR/khrplatform.h create mode 100644 extern/glad/include/glad/glad.h create mode 100644 extern/glad/src/glad.c create mode 100644 extern/stb/stb_image.h create mode 100644 include/box3d/base.h create mode 100644 include/box3d/box3d.h create mode 100644 include/box3d/collision.h create mode 100644 include/box3d/constants.h create mode 100644 include/box3d/id.h create mode 100644 include/box3d/math_functions.h create mode 100644 include/box3d/types.h create mode 100644 samples/CMakeLists.txt create mode 100644 samples/camera.cpp create mode 100644 samples/camera.h create mode 100644 samples/earcut.h create mode 100644 samples/font.cpp create mode 100644 samples/font.h create mode 100644 samples/geo_buffer.cpp create mode 100644 samples/geo_buffer.h create mode 100644 samples/imconfig.h create mode 100644 samples/jsmn.h create mode 100644 samples/main.cpp create mode 100644 samples/mesh_loader.cpp create mode 100644 samples/mesh_loader.h create mode 100644 samples/renderer.cpp create mode 100644 samples/renderer.h create mode 100644 samples/sample.cpp create mode 100644 samples/sample.h create mode 100644 samples/sample_benchmark.cpp create mode 100644 samples/sample_bodies.cpp create mode 100644 samples/sample_character.cpp create mode 100644 samples/sample_collision.cpp create mode 100644 samples/sample_compound.cpp create mode 100644 samples/sample_continuous.cpp create mode 100644 samples/sample_determinism.cpp create mode 100644 samples/sample_events.cpp create mode 100644 samples/sample_geometry.cpp create mode 100644 samples/sample_issues.cpp create mode 100644 samples/sample_joint.cpp create mode 100644 samples/sample_manifold.cpp create mode 100644 samples/sample_mesh.cpp create mode 100644 samples/sample_ragdoll.cpp create mode 100644 samples/sample_robustness.cpp create mode 100644 samples/sample_shapes.cpp create mode 100644 samples/sample_stacking.cpp create mode 100644 samples/sample_tree.cpp create mode 100644 samples/scene.cpp create mode 100644 samples/scene.h create mode 100644 samples/shader.cpp create mode 100644 samples/shader.h create mode 100644 samples/ssao_buffer.cpp create mode 100644 samples/ssao_buffer.h create mode 100644 samples/stb_image_write.h create mode 100644 samples/stb_truetype.h create mode 100644 samples/tiny_obj_loader.h create mode 100644 samples/utility.h create mode 100644 shaders/common.glsl create mode 100644 shaders/deferred_lighting_fs.glsl create mode 100644 shaders/depth_copy_fs.glsl create mode 100644 shaders/empty.fs create mode 100644 shaders/forward.fs create mode 100644 shaders/forward.vs create mode 100644 shaders/geometry_fs.glsl create mode 100644 shaders/geometry_vs.glsl create mode 100644 shaders/mesh.fs create mode 100644 shaders/mesh.vs create mode 100644 shaders/mesh_edge.vs create mode 100644 shaders/mesh_forward.fs create mode 100644 shaders/mesh_forward.vs create mode 100644 shaders/mesh_shadow.vs create mode 100644 shaders/quad_vs.glsl create mode 100644 shaders/shadow_map.vs create mode 100644 shaders/ssao_blur_fs.glsl create mode 100644 shaders/ssao_fs.glsl create mode 100644 shared/CMakeLists.txt create mode 100644 shared/benchmarks.c create mode 100644 shared/benchmarks.h create mode 100644 shared/determinism.c create mode 100644 shared/determinism.h create mode 100644 shared/human.c create mode 100644 shared/human.h create mode 100644 shared/stability.c create mode 100644 shared/stability.h create mode 100644 shared/utils.c create mode 100644 shared/utils.h create mode 100644 src/CMakeLists.txt create mode 100644 src/aabb.c create mode 100644 src/aabb.h create mode 100644 src/algorithm.h create mode 100644 src/arena_allocator.c create mode 100644 src/arena_allocator.h create mode 100644 src/bitset.c create mode 100644 src/bitset.h create mode 100644 src/block_allocator.c create mode 100644 src/block_allocator.h create mode 100644 src/body.c create mode 100644 src/body.h create mode 100644 src/box3d.natvis create mode 100644 src/broad_phase.c create mode 100644 src/broad_phase.h create mode 100644 src/capsule.c create mode 100644 src/compound.c create mode 100644 src/compound.h create mode 100644 src/constraint_graph.c create mode 100644 src/constraint_graph.h create mode 100644 src/contact.c create mode 100644 src/contact.h create mode 100644 src/contact_solver.c create mode 100644 src/contact_solver.h create mode 100644 src/container.h create mode 100644 src/convex_manifold.c create mode 100644 src/core.c create mode 100644 src/core.h create mode 100644 src/ctz.h create mode 100644 src/distance.c create mode 100644 src/distance_joint.c create mode 100644 src/dynamic_tree.c create mode 100644 src/height_field.c create mode 100644 src/hull.c create mode 100644 src/id_pool.c create mode 100644 src/id_pool.h create mode 100644 src/island.c create mode 100644 src/island.h create mode 100644 src/joint.c create mode 100644 src/joint.h create mode 100644 src/manifold.c create mode 100644 src/manifold.h create mode 100644 src/math_functions.c create mode 100644 src/math_internal.h create mode 100644 src/mesh.c create mode 100644 src/mesh_contact.c create mode 100644 src/motor_joint.c create mode 100644 src/mover.c create mode 100644 src/parallel_for.c create mode 100644 src/parallel_for.h create mode 100644 src/parallel_joint.c create mode 100644 src/physics_world.c create mode 100644 src/physics_world.h create mode 100644 src/platform.h create mode 100644 src/prismatic_joint.c create mode 100644 src/qsort.h create mode 100644 src/revolute_joint.c create mode 100644 src/scheduler.c create mode 100644 src/scheduler.h create mode 100644 src/sensor.c create mode 100644 src/sensor.h create mode 100644 src/shape.c create mode 100644 src/shape.h create mode 100644 src/simd.c create mode 100644 src/simd.h create mode 100644 src/solver.c create mode 100644 src/solver.h create mode 100644 src/solver_set.c create mode 100644 src/solver_set.h create mode 100644 src/sphere.c create mode 100644 src/spherical_joint.c create mode 100644 src/table.c create mode 100644 src/table.h create mode 100644 src/timer.c create mode 100644 src/triangle_manifold.c create mode 100644 src/types.c create mode 100644 src/verstable.h create mode 100644 src/weld_joint.c create mode 100644 src/wheel_joint.c create mode 100644 test/CMakeLists.txt create mode 100644 test/main.c create mode 100644 test/test_allocator.c create mode 100644 test/test_bitset.c create mode 100644 test/test_collision.c create mode 100644 test/test_compound.c create mode 100644 test/test_container.c create mode 100644 test/test_determinism.c create mode 100644 test/test_distance.c create mode 100644 test/test_height_field.c create mode 100644 test/test_hull.c create mode 100644 test/test_id.c create mode 100644 test/test_macros.h create mode 100644 test/test_math.c create mode 100644 test/test_mover.c create mode 100644 test/test_shape.c create mode 100644 test/test_table.c create mode 100644 test/test_world.c diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..f0cdb58 --- /dev/null +++ b/.clang-format @@ -0,0 +1,42 @@ +# Reference: https://clang.llvm.org/docs/ClangFormatStyleOptions.html +# /c/Program Files/Microsoft Visual Studio/2022/Community/VC/Tools/Llvm/bin +# default values: +# https://github.com/llvm/llvm-project/blob/main/clang/lib/Format/Format.cpp +--- +Language: Cpp +BasedOnStyle: Microsoft + +BreakBeforeBraces: Custom +BraceWrapping: + AfterCaseLabel: true + AfterUnion: true + BeforeWhile: true + +ColumnLimit: 130 +PointerAlignment: Left +UseTab: Always +BreakConstructorInitializers: BeforeComma + +InsertNewlineAtEOF: true +IncludeBlocks: Regroup + +IncludeCategories: + - Regex: '^"(box3d\/)' + Priority: 2 + - Regex: '^<.*' + Priority: 3 + - Regex: '^".*' + Priority: 1 + +IndentExternBlock: NoIndent +IndentCaseLabels: true +IndentAccessModifiers: false +AccessModifierOffset: -4 + +SpacesInParens: Custom +SpacesInParensOptions: + ExceptDoubleParentheses: false + InConditionalStatements: true + InCStyleCasts: false + InEmptyParentheses: false + Other: true diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..a04f5fa --- /dev/null +++ b/.gitattributes @@ -0,0 +1,10 @@ +* text=auto + +*.c text +*.cpp text +*.h text +*.md text +*.txt text +*.sh text eol=lf + +*.ttf binary diff --git a/.github/FUNDING.yml b/.github/FUNDING.yml new file mode 100644 index 0000000..b31f021 --- /dev/null +++ b/.github/FUNDING.yml @@ -0,0 +1,14 @@ +# These are supported funding model platforms + +github: # Replace with up to 4 GitHub Sponsors-enabled usernames e.g., [user1, user2] +patreon: # Replace with a single Patreon username +open_collective: # Replace with a single Open Collective username +ko_fi: # Replace with a single Ko-fi username +tidelift: # Replace with a single Tidelift platform-name/package-name e.g., npm/babel +community_bridge: # Replace with a single Community Bridge project-name e.g., cloud-foundry +liberapay: # Replace with a single Liberapay username +issuehunt: # Replace with a single IssueHunt username +otechie: # Replace with a single Otechie username +custom: # Replace with up to 4 custom sponsorship URLs e.g., ['link1', 'link2'] + +github: [erincatto] diff --git a/.github/issue_template.md b/.github/issue_template.md new file mode 100644 index 0000000..abbe499 --- /dev/null +++ b/.github/issue_template.md @@ -0,0 +1 @@ +Thank you for filing an issue! diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md new file mode 100644 index 0000000..9fdfc2c --- /dev/null +++ b/.github/pull_request_template.md @@ -0,0 +1 @@ +Pull requests for Box3D code are not accepted. Please consider filing an issue instead. diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml new file mode 100644 index 0000000..05a89a6 --- /dev/null +++ b/.github/workflows/build.yml @@ -0,0 +1,119 @@ +# https://docs.github.com/en/actions +name: CI Build + +on: + push: + branches: [ main ] + pull_request: + branches: [ main ] + +env: + BUILD_TYPE: Debug + +jobs: + build-ubuntu-gcc: + name: ubuntu-gcc + runs-on: ubuntu-latest + if: github.event_name == 'push' || github.event.pull_request.draft == false + timeout-minutes: 4 + steps: + - uses: actions/checkout@v4 + + - name: Configure CMake + run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DBOX3D_SAMPLES=OFF -DBUILD_SHARED_LIBS=OFF -DBOX3D_VALIDATE=ON -DCMAKE_COMPILE_WARNING_AS_ERROR=ON + + - name: Build + run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} + + - name: Test + working-directory: ${{github.workspace}}/build + run: ./bin/test + + build-ubuntu-clang: + name: ubuntu-clang + runs-on: ubuntu-latest + if: github.event_name == 'push' || github.event.pull_request.draft == false + timeout-minutes: 4 + steps: + - uses: actions/checkout@v4 + + - name: Configure CMake + run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DCMAKE_CXX_COMPILER=clang++-18 -DCMAKE_C_COMPILER=clang-18 -DCMAKE_C_FLAGS="-fsanitize=thread -fno-omit-frame-pointer" -DCMAKE_CXX_FLAGS="-fsanitize=thread -fno-omit-frame-pointer" -DCMAKE_EXE_LINKER_FLAGS="-fsanitize=thread" -DBOX3D_SAMPLES=OFF -DBUILD_SHARED_LIBS=OFF -DBOX3D_VALIDATE=ON -DCMAKE_COMPILE_WARNING_AS_ERROR=ON + + - name: Build + run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} + + - name: Test + working-directory: ${{github.workspace}}/build + run: ./bin/test + + build-macos: + name: macos + runs-on: macos-latest + if: github.event_name == 'push' || github.event.pull_request.draft == false + timeout-minutes: 4 + steps: + - uses: actions/checkout@v4 + + - name: Configure CMake + run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DBOX3D_SAMPLES=OFF -DBOX3D_SANITIZE=ON -DBUILD_SHARED_LIBS=OFF -DBOX3D_VALIDATE=ON -DCMAKE_COMPILE_WARNING_AS_ERROR=ON + + - name: Build + run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} + + - name: Test + working-directory: ${{github.workspace}}/build + run: ./bin/test + + build-windows: + name: windows + runs-on: windows-latest + if: github.event_name == 'push' || github.event.pull_request.draft == false + timeout-minutes: 4 + steps: + - uses: actions/checkout@v3 + + - name: Configure CMake + run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DBOX3D_SAMPLES=OFF -DBOX3D_VALIDATE=ON -DCMAKE_COMPILE_WARNING_AS_ERROR=ON + + - name: Build + run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} + + - name: Test + working-directory: ${{github.workspace}}/build + run: ./bin/${{env.BUILD_TYPE}}/test + + samples-windows: + name: samples-windows + runs-on: windows-latest + if: github.event_name == 'push' || github.event.pull_request.draft == false + timeout-minutes: 6 + steps: + + - uses: actions/checkout@v4 + + - name: Setup MSVC dev command prompt + uses: TheMrMilchmann/setup-msvc-dev@v3 + with: + arch: x64 + + - name: Configure CMake + run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=Release -DBOX3D_SAMPLES=ON -DBUILD_SHARED_LIBS=OFF -DBOX3D_UNIT_TESTS=OFF + + - name: Build + run: cmake --build ${{github.workspace}}/build --config Release + + samples-macos: + name: samples-macos + runs-on: macos-latest + if: github.event_name == 'push' || github.event.pull_request.draft == false + timeout-minutes: 6 + steps: + - uses: actions/checkout@v4 + + - name: Configure CMake + run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=Release -DBOX3D_SAMPLES=ON -DBUILD_SHARED_LIBS=OFF -DBOX3D_UNIT_TESTS=OFF + + - name: Build + run: cmake --build ${{github.workspace}}/build --config Release + \ No newline at end of file diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..344933e --- /dev/null +++ b/.gitignore @@ -0,0 +1,14 @@ +build/ +imgui.ini +settings.ini +.vscode/ +.vs/ +.cap +.DS_Store +CMakeUserPresets.json +.cache/ +.idea/ +.claude/ +.tmp/ +AGENTS.md +GEMINI.md diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..9fa66f2 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,155 @@ +cmake_minimum_required(VERSION 3.22) +include(FetchContent) +include(CMakeDependentOption) + +project(box3d + VERSION 0.1.0 + DESCRIPTION "A 3D physics engine for games" + HOMEPAGE_URL "https://box2d.org" + LANGUAGES C CXX +) + +# stuff to help debug cmake +# message(STATUS "cmake tool chain: ${CMAKE_TOOLCHAIN_FILE}") +# message(STATUS "cmake source dir: ${CMAKE_SOURCE_DIR}") +# message(STATUS "library postfix: ${CMAKE_DEBUG_POSTFIX}") +message(STATUS "CMake C compiler: ${CMAKE_C_COMPILER_ID}") +message(STATUS "CMake C++ compiler: ${CMAKE_CXX_COMPILER_ID}") +message(STATUS "CMake system name: ${CMAKE_SYSTEM_NAME}") +message(STATUS "CMake host system processor: ${CMAKE_HOST_SYSTEM_PROCESSOR}") + +# static link to make leak checking easier (_crtBreakAlloc) +set(CMAKE_MSVC_RUNTIME_LIBRARY "MultiThreaded$<$:Debug>") + +if (MSVC OR APPLE OR UNIX) + option(BOX3D_SANITIZE "Enable sanitizers for some builds" OFF) + set(BOX3D_SANITIZER_TYPE "address" CACHE STRING "Type of sanitizer to use (address, thread, undefined, memory)") + set_property(CACHE BOX3D_SANITIZER_TYPE PROPERTY STRINGS "address" "thread" "undefined" "memory") + + if(BOX3D_SANITIZE) + message(STATUS "Box3D Sanitize: ${BOX3D_SANITIZER_TYPE}") + # sanitizers need to apply to all compiled libraries to work well + if(MSVC) + # address sanitizer only in the debug build + add_compile_options("$<$:/fsanitize=address>") + add_link_options("$<$:/INCREMENTAL:NO>") + elseif(APPLE) + # more sanitizers on Apple clang + if(BOX3D_SANITIZER_TYPE STREQUAL "thread") + add_compile_options(-fsanitize=thread -fno-omit-frame-pointer) + add_link_options(-fsanitize=thread) + elseif(BOX3D_SANITIZER_TYPE STREQUAL "undefined") + add_compile_options(-fsanitize=undefined -fno-omit-frame-pointer) + add_link_options(-fsanitize=undefined) + else() + # default to address sanitizer + add_compile_options(-fsanitize=address -fsanitize-address-use-after-scope -fsanitize=undefined) + add_link_options(-fsanitize=address -fsanitize-address-use-after-scope -fsanitize=undefined) + endif() + elseif(UNIX) + # Linux/WSL2 sanitizer support + if(BOX3D_SANITIZER_TYPE STREQUAL "thread") + # Disable ASLR first to avoid crash on launch + # echo 0 | sudo tee /proc/sys/kernel/randomize_va_space + add_compile_options(-fsanitize=thread -fno-omit-frame-pointer -fPIE) + add_link_options(-fsanitize=thread -pie) + elseif(BOX3D_SANITIZER_TYPE STREQUAL "undefined") + add_compile_options(-fsanitize=undefined -fno-omit-frame-pointer) + add_link_options(-fsanitize=undefined) + elseif(BOX3D_SANITIZER_TYPE STREQUAL "memory") + add_compile_options(-fsanitize=memory -fno-omit-frame-pointer) + add_link_options(-fsanitize=memory) + else() + # default to address sanitizer + add_compile_options(-fsanitize=address -fno-omit-frame-pointer) + add_link_options(-fsanitize=address) + endif() + endif() + else() + if(MSVC) + # enable hot reloading + add_compile_options("$<$:/ZI>") + add_link_options("$<$:/INCREMENTAL>") + endif() + endif() +endif() + +# Deterministic math +# https://box2d.org/posts/2024/08/determinism/ +if (MINGW OR APPLE OR UNIX) + add_compile_options(-ffp-contract=off) +endif() + +option(BOX3D_DISABLE_SIMD "Disable SIMD math (slower)" OFF) +option(BOX3D_COMPILE_WARNING_AS_ERROR "Compile warnings as errors" OFF) + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set_property(GLOBAL PROPERTY USE_FOLDERS ON) +set(CMAKE_VERBOSE_MAKEFILE ON) + +# Needed for samples.exe to find box2d.dll +set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/bin") +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/bin") + +add_subdirectory(src) + +# This hides samples, test, and doxygen from apps that use box3d via FetchContent +if(PROJECT_IS_TOP_LEVEL) + option(BOX3D_SAMPLES "Build the Box3D samples" ON) + option(BOX3D_BENCHMARKS "Build the Box3D benchmarks" OFF) + option(BOX3D_DOCS "Build the Box3D documentation" OFF) + option(BOX3D_PROFILE "Enable profiling with Tracy" OFF) + option(BOX3D_VALIDATE "Enable heavy validation" ON) + option(BOX3D_UNIT_TESTS "Build the Box3D unit tests" ON) + + if(BOX3D_UNIT_TESTS OR BOX3D_SAMPLES OR BOX3D_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() + + # Tests need static linkage because they test internal Box3D functions + if(NOT BUILD_SHARED_LIBS AND BOX3D_UNIT_TESTS) + message(STATUS "Adding Box3D unit tests") + add_subdirectory(test) + set_target_properties(test PROPERTIES XCODE_GENERATE_SCHEME TRUE) + else() + message(STATUS "Skipping Box3D unit tests") + endif() + + if(BOX3D_SAMPLES) + add_subdirectory(samples) + + # default startup project for Visual Studio + if(MSVC) + set_property(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY VS_STARTUP_PROJECT samples) + set_property(TARGET samples PROPERTY VS_DEBUGGER_WORKING_DIRECTORY "${CMAKE_SOURCE_DIR}") + endif() + + set_target_properties(samples PROPERTIES + XCODE_GENERATE_SCHEME TRUE + XCODE_SCHEME_WORKING_DIRECTORY "${CMAKE_SOURCE_DIR}") + endif() + + if(BOX3D_BENCHMARKS) + add_subdirectory(benchmark) + set_target_properties(benchmark PROPERTIES XCODE_GENERATE_SCHEME TRUE) + endif() + + if(BOX3D_DOCS) + # todo + # add_subdirectory(docs) + endif() +endif() + +# https://stackoverflow.com/questions/72635402/how-to-set-working-directory-in-cmake-visual-studio-2022 +# launch.vs.json +# "currentDir": "${workspaceRoot}" \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..62bbc3c --- /dev/null +++ b/README.md @@ -0,0 +1,98 @@ +![Box3D Logo](https://box2d.org/images/logo.svg) + +# Build Status +[![Build Status](https://github.com/erincatto/box3d/actions/workflows/build.yml/badge.svg)](https://github.com/erincatto/box3d/actions) + +# Box3D +Box3D is a 3D physics engine for games. + +[![Box3D Version 3.0 Release Demo](https://img.youtube.com/vi/dAoM-xjOWtA/0.jpg)](https://www.youtube.com/watch?v=dAoM-xjOWtA) + +## Features + +### Collision +- Continuous collision detection +- Contact events +- Convex polygons, capsules, circles, rounded polygons, segments, and chains +- Multiple shapes per body +- Collision filtering +- Ray casts, shape casts, and overlap queries +- Sensor system + +### Physics +- Robust _Soft Step_ rigid body solver +- Continuous physics for fast translations and rotations +- Island based sleep +- Revolute, prismatic, distance, mouse joint, weld, and wheel joints +- Joint limits, motors, springs, and friction +- Joint and contact forces +- Body movement events and sleep notification + +### System +- Data-oriented design +- Written in portable C17 +- Extensive multithreading and SIMD +- Optimized for large piles of bodies + +### Samples +- OpenGL with GLFW +- Graphical user interface with imgui +- Many samples to demonstrate features and performance + +## Building All Platforms +- Install [CMake](https://cmake.org/) +- Install [git](https://git-scm.com/) +- Ensure these run from the command line + +## Building for Visual Studio +- Install [Visual Studio](https://visualstudio.microsoft.com/) +- Run `build_vs2026.bat` +- Open and build `build/box3d.slnx` + +## Building for Linux +- Run `build.sh` from a bash shell +- Results are in the build sub-folder + +## Building for Xcode +- mkdir build +- cd build +- cmake -G Xcode .. +- Open `box3d.xcodeproj` +- Select the samples scheme +- Build and run the samples + +## Building and installing +- mkdir build +- cd build +- cmake .. +- cmake --build . --config Release +- cmake --install . (might need sudo) + +## Compatibility +The Box3D library and samples build and run on Windows, Linux, and Mac. + +You will need a compiler that supports C17 to build the Box3D library. + +You will need a compiler that supports C++20 to build the samples. + +Box3D uses SSE2 and Neon SIMD math to improve performance. This can be disabled by defining `BOX3D_DISABLE_SIMD`. + +## Documentation +- [Manual](https://box2d.org/documentation/) + +## Community +- [Discord](https://discord.gg/NKYgCBP) + +## Contributing +Please do not submit pull requests. Instead, please file an issue for bugs or feature requests. For support, please visit the Discord server. + +# Giving Feedback +Please file an issue or start a chat on discord. You can also use [GitHub Discussions](https://github.com/erincatto/box3d/discussions). + +## License +Box3D is developed by Erin Catto and uses the [MIT license](https://en.wikipedia.org/wiki/MIT_License). + +## Sponsorship +Support development of Box3D through [Github Sponsors](https://github.com/sponsors/erincatto). + +Please consider starring this repository and subscribing to my [YouTube channel](https://www.youtube.com/@erin_catto). diff --git a/benchmark/CMakeLists.txt b/benchmark/CMakeLists.txt new file mode 100644 index 0000000..73f7210 --- /dev/null +++ b/benchmark/CMakeLists.txt @@ -0,0 +1,13 @@ +# Benchmark app + +set(BOX3D_BENCHMARK_FILES + main.c +) + +add_executable(benchmark ${BOX3D_BENCHMARK_FILES}) + +set_target_properties(benchmark PROPERTIES C_STANDARD 17) + +target_link_libraries(benchmark PRIVATE box3d shared) + +source_group(TREE ${CMAKE_CURRENT_SOURCE_DIR} PREFIX "" FILES ${BOX3D_BENCHMARK_FILES}) \ No newline at end of file diff --git a/benchmark/amd7950x_scalar/washer.csv b/benchmark/amd7950x_scalar/washer.csv new file mode 100644 index 0000000..83bc876 --- /dev/null +++ b/benchmark/amd7950x_scalar/washer.csv @@ -0,0 +1,9 @@ +threads,ms +1,0 +2,0 +3,0 +4,0 +5,0 +6,0 +7,0 +8,5488.14 diff --git a/benchmark/amd7950x_sse2/compound_capsules.csv b/benchmark/amd7950x_sse2/compound_capsules.csv new file mode 100644 index 0000000..e0179bc --- /dev/null +++ b/benchmark/amd7950x_sse2/compound_capsules.csv @@ -0,0 +1,9 @@ +threads,ms +1,730.575 +2,424.323 +3,324.045 +4,276.202 +5,242.944 +6,226.599 +7,211.694 +8,205.856 diff --git a/benchmark/amd7950x_sse2/compound_hulls.csv b/benchmark/amd7950x_sse2/compound_hulls.csv new file mode 100644 index 0000000..2854909 --- /dev/null +++ b/benchmark/amd7950x_sse2/compound_hulls.csv @@ -0,0 +1,9 @@ +threads,ms +1,462.769 +2,292.812 +3,230.578 +4,197.733 +5,178.241 +6,166.567 +7,161.298 +8,157.091 diff --git a/benchmark/amd7950x_sse2/joint_grid.csv b/benchmark/amd7950x_sse2/joint_grid.csv new file mode 100644 index 0000000..898d654 --- /dev/null +++ b/benchmark/amd7950x_sse2/joint_grid.csv @@ -0,0 +1,9 @@ +threads,ms +1,1796.4 +2,954.375 +3,641.176 +4,481.819 +5,390.785 +6,328.059 +7,287.637 +8,258.124 diff --git a/benchmark/amd7950x_sse2/junkyard.csv b/benchmark/amd7950x_sse2/junkyard.csv new file mode 100644 index 0000000..50a6450 --- /dev/null +++ b/benchmark/amd7950x_sse2/junkyard.csv @@ -0,0 +1,9 @@ +threads,ms +1,24717.7 +2,13513.9 +3,9716.35 +4,7493.39 +5,6140.22 +6,5221.34 +7,4550.24 +8,4106.65 diff --git a/benchmark/amd7950x_sse2/large_pyramid.csv b/benchmark/amd7950x_sse2/large_pyramid.csv new file mode 100644 index 0000000..852f52b --- /dev/null +++ b/benchmark/amd7950x_sse2/large_pyramid.csv @@ -0,0 +1,9 @@ +threads,ms +1,1682.1 +2,878.51 +3,613.252 +4,478.075 +5,387.426 +6,337.026 +7,294.737 +8,270.737 diff --git a/benchmark/amd7950x_sse2/many_pyramids.csv b/benchmark/amd7950x_sse2/many_pyramids.csv new file mode 100644 index 0000000..cdbba91 --- /dev/null +++ b/benchmark/amd7950x_sse2/many_pyramids.csv @@ -0,0 +1,9 @@ +threads,ms +1,2575.34 +2,1278.03 +3,878.515 +4,629.084 +5,507.933 +6,413.473 +7,362.456 +8,320.241 diff --git a/benchmark/amd7950x_sse2/rain.csv b/benchmark/amd7950x_sse2/rain.csv new file mode 100644 index 0000000..ec9581b --- /dev/null +++ b/benchmark/amd7950x_sse2/rain.csv @@ -0,0 +1,9 @@ +threads,ms +1,2627.76 +2,1427.39 +3,1012.48 +4,796.119 +5,664.231 +6,577.576 +7,518.164 +8,483.436 diff --git a/benchmark/amd7950x_sse2/washer.csv b/benchmark/amd7950x_sse2/washer.csv new file mode 100644 index 0000000..e1caa96 --- /dev/null +++ b/benchmark/amd7950x_sse2/washer.csv @@ -0,0 +1,9 @@ +threads,ms +1,31480.4 +2,17010.7 +3,11890.1 +4,9295.49 +5,7696.31 +6,6645.53 +7,5843.28 +8,5249.02 diff --git a/benchmark/amd7950x_sse2/welded_hulls.csv b/benchmark/amd7950x_sse2/welded_hulls.csv new file mode 100644 index 0000000..3b28fcd --- /dev/null +++ b/benchmark/amd7950x_sse2/welded_hulls.csv @@ -0,0 +1,9 @@ +threads,ms +1,726.988 +2,430.503 +3,324.672 +4,268.082 +5,232.24 +6,213.849 +7,199.971 +8,193.328 diff --git a/benchmark/main.c b/benchmark/main.c new file mode 100644 index 0000000..1fc8fd8 --- /dev/null +++ b/benchmark/main.c @@ -0,0 +1,344 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include "benchmarks.h" +#include "utils.h" + +#include "box3d/box3d.h" +#include "box3d/constants.h" +#include "box3d/math_functions.h" + +#include +#include +#include +#include +#include + +#ifdef TRACY_ENABLE +#include +#endif + +#define ARRAY_COUNT( A ) (int)( sizeof( A ) / sizeof( A[0] ) ) +#define MAYBE_UNUSED( x ) ( (void)( x ) ) + +typedef void CapacityFcn( b3Capacity* capacity ); +typedef void CreateFcn( b3WorldId worldId ); +typedef void DestroyFcn( void ); +typedef void StepFcn( b3WorldId worldId, int stepCount ); + +typedef struct Benchmark +{ + const char* name; + CapacityFcn* capacityFcn; + CreateFcn* createFcn; + DestroyFcn* destroyFcn; + StepFcn* stepFcn; + int totalStepCount; +} Benchmark; + +static void MinProfile( b3Profile* p1, const b3Profile* p2 ) +{ + p1->step = b3MinFloat( p1->step, p2->step ); + p1->pairs = b3MinFloat( p1->pairs, p2->pairs ); + p1->collide = b3MinFloat( p1->collide, p2->collide ); + p1->constraints = b3MinFloat( p1->constraints, p2->constraints ); + p1->transforms = b3MinFloat( p1->transforms, p2->transforms ); + p1->refit = b3MinFloat( p1->refit, p2->refit ); + p1->sleepIslands = b3MinFloat( p1->sleepIslands, p2->sleepIslands ); +} + +// Box3D 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. +// Affinity [0x01 0x02 0x04 0x08 0x10 0x20 0x40 0x80] + +// Run all benchmarks with 1 to 8 threads. +// start /affinity 0x5555 .\build\bin\Release\benchmark.exe -t=8 + +// Run all benchmarks with 4 workers only. +// start /affinity 0x5555 .\build\bin\Release\benchmark.exe -t=4 -w=4 + +// start /affinity 0x5555 .\build\bin\Release\benchmark.exe -t=4 -w=4 -b=5 -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 + +// Run benchmark 3 with 4 workers and run once. Disable continuous collision. Record the step times. +// start /affinity 0x5555 .\build\bin\Release\benchmark.exe -t=4 -w=4 -b=3 -r=1 -nc -s + +int main( int argc, char** argv ) +{ +#ifdef TRACY_ENABLE + ___tracy_startup_profiler(); +#endif + + Benchmark benchmarks[] = { + { "compound_capsules", NULL, CreateCompoundCapsules, DestroyCompoundCapsules, NULL, 500 }, + { "compound_hulls", NULL, CreateCompoundHulls, DestroyCompoundHulls, NULL, 500 }, + { "joint_grid", NULL, CreateJointGrid, NULL, NULL, 100 }, + { "junkyard", NULL, CreateJunkyard, NULL, StepJunkyard, 500 }, + { "large_pyramid", NULL, CreateLargePyramid, NULL, NULL, 200 }, + { "many_pyramids", NULL, CreateManyPyramids, NULL, NULL, 100 }, + { "rain", GetRainCapacity, CreateRain, DestroyRain, StepRain, 400 }, + { "washer", GetWasherCapacity, CreateWasher, NULL, NULL, 1000 }, + { "welded_hulls", NULL, CreateWeldedHulls, DestroyWeldedHulls, NULL, 500 }, + //{ "smash", CreateSmash, NULL, 300 }, + //{ "spinner", CreateSpinner, StepSpinner, 1400 }, + //{ "tumbler", CreateTumbler, NULL, 750 }, + }; + + int benchmarkCount = ARRAY_COUNT( benchmarks ); + + int maxSteps = benchmarks[0].totalStepCount; + for ( int i = 1; i < benchmarkCount; ++i ) + { + maxSteps = b3MaxInt( maxSteps, benchmarks[i].totalStepCount ); + } + + b3Profile maxProfile = { + .step = FLT_MAX, + .pairs = FLT_MAX, + .collide = FLT_MAX, + .solve = FLT_MAX, + .solverSetup = FLT_MAX, + .constraints = FLT_MAX, + .prepareConstraints = FLT_MAX, + .integrateVelocities = FLT_MAX, + .warmStart = FLT_MAX, + .solveImpulses = FLT_MAX, + .integratePositions = FLT_MAX, + .relaxImpulses = FLT_MAX, + .applyRestitution = FLT_MAX, + .storeImpulses = FLT_MAX, + .splitIslands = FLT_MAX, + .transforms = FLT_MAX, + .hitEvents = FLT_MAX, + .refit = FLT_MAX, + .bullets = FLT_MAX, + .sleepIslands = FLT_MAX, + }; + + b3Profile* profiles = malloc( maxSteps * sizeof( b3Profile ) ); + for ( int i = 0; i < maxSteps; ++i ) + { + profiles[i] = maxProfile; + } + + int maxThreadCount = GetNumberOfCores(); + int runCount = 4; + int singleBenchmark = -1; + int singleWorkerCount = -1; + b3Counters counters = { 0 }; + bool enableContinuous = true; + bool recordStepTimes = false; + + assert( maxThreadCount <= B3_MAX_WORKERS ); + + for ( int i = 1; i < argc; ++i ) + { + const char* arg = argv[i]; + if ( strncmp( arg, "-t=", 3 ) == 0 ) + { + int threadCount = atoi( arg + 3 ); + maxThreadCount = b3ClampInt( threadCount, 1, maxThreadCount ); + } + else if ( strncmp( arg, "-b=", 3 ) == 0 ) + { + singleBenchmark = atoi( arg + 3 ); + singleBenchmark = b3ClampInt( singleBenchmark, 0, benchmarkCount - 1 ); + } + else if ( strncmp( arg, "-w=", 3 ) == 0 ) + { + singleWorkerCount = atoi( arg + 3 ); + } + else if ( strncmp( arg, "-r=", 3 ) == 0 ) + { + runCount = b3ClampInt( atoi( arg + 3 ), 1, 1000 ); + } + else if ( strncmp( arg, "-nc", 3 ) == 0 ) + { + enableContinuous = false; + printf( "Continuous disabled\n" ); + } + else if ( strncmp( arg, "-s", 3 ) == 0 ) + { + recordStepTimes = true; + } + else if ( strcmp( arg, "-h" ) == 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" ); + exit( 0 ); + } + } + + if ( singleWorkerCount != -1 ) + { + singleWorkerCount = b3ClampInt( singleWorkerCount, 1, maxThreadCount ); + } + + printf( "Starting benchmarks\n" ); + printf( "======================================\n" ); + + for ( int benchmarkIndex = 0; benchmarkIndex < benchmarkCount; ++benchmarkIndex ) + { + if ( singleBenchmark != -1 && benchmarkIndex != singleBenchmark ) + { + continue; + } + +#ifdef NDEBUG + int stepCount = benchmarks[benchmarkIndex].totalStepCount; +#else + int stepCount = 10; +#endif + + Benchmark* benchmark = benchmarks + benchmarkIndex; + + bool countersAcquired = false; + + printf( "benchmark: %s, steps = %d\n", benchmarks[benchmarkIndex].name, stepCount ); + + float minTime[B3_MAX_WORKERS] = { 0 }; + + for ( int threadCount = 1; threadCount <= maxThreadCount; ++threadCount ) + { + if ( singleWorkerCount != -1 && singleWorkerCount != threadCount ) + { + continue; + } + + printf( "thread count: %d\n", threadCount ); + + for ( int runIndex = 0; runIndex < runCount; ++runIndex ) + { + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.enableContinuous = enableContinuous; + worldDef.workerCount = threadCount; + + if (benchmark->capacityFcn != NULL) + { + benchmark->capacityFcn( &worldDef.capacity ); + } + + b3WorldId worldId = b3CreateWorld( &worldDef ); + + benchmark->createFcn( worldId ); + + float timeStep = 1.0f / 60.0f; + int subStepCount = 4; + + // Initial step can be expensive and skew benchmark + if ( benchmark->stepFcn != NULL ) + { + benchmark->stepFcn( worldId, 0 ); + } + + assert( stepCount <= maxSteps ); + + b3World_Step( worldId, timeStep, subStepCount ); +\ + b3Profile profile = b3World_GetProfile( worldId ); + MinProfile( profiles + 0, &profile ); + + uint64_t ticks = b3GetTicks(); + + for ( int stepIndex = 1; stepIndex < stepCount; ++stepIndex ) + { + if ( benchmark->stepFcn != NULL ) + { + benchmark->stepFcn( worldId, stepIndex ); + } + + b3World_Step( worldId, timeStep, subStepCount ); +\ + profile = b3World_GetProfile( worldId ); + MinProfile( profiles + stepIndex, &profile ); + } + + float ms = b3GetMilliseconds( ticks ); + printf( "run %d : %g (ms)\n", runIndex, ms ); + + if ( runIndex == 0 ) + { + minTime[threadCount - 1] = ms; + } + else + { + minTime[threadCount - 1] = b3MinFloat( minTime[threadCount - 1], ms ); + } + + if ( countersAcquired == false ) + { + counters = b3World_GetCounters( worldId ); + countersAcquired = true; + } + + if ( benchmark->destroyFcn != NULL ) + { + benchmark->destroyFcn(); + } + + b3DestroyWorld( worldId ); + } + + if ( recordStepTimes ) + { + char fileName[64] = { 0 }; + snprintf( fileName, 64, "%s_t%d.dat", benchmarks[benchmarkIndex].name, threadCount ); + FILE* file = fopen( fileName, "w" ); + if ( file == NULL ) + { + continue; + } + + for ( int stepIndex = 0; stepIndex < stepCount; ++stepIndex ) + { + b3Profile p = profiles[stepIndex]; + fprintf( file, "%g %g %g %g %g %g %g\n", p.step, p.pairs, p.collide, p.constraints, p.transforms, + p.refit, p.sleepIslands ); + } + + fclose( file ); + } + } + + printf( "body %d / shape %d / contact %d / joint %d / stack %d\n\n", counters.bodyCount, counters.shapeCount, + counters.contactCount, counters.jointCount, counters.stackUsed ); + + char fileName[64] = { 0 }; + snprintf( fileName, 64, "%s.csv", benchmarks[benchmarkIndex].name ); + FILE* file = fopen( fileName, "w" ); + if ( file == NULL ) + { + continue; + } + + fprintf( file, "threads,ms\n" ); + for ( int threadIndex = 1; threadIndex <= maxThreadCount; ++threadIndex ) + { + fprintf( file, "%d,%g\n", threadIndex, minTime[threadIndex - 1] ); + } + + fclose( file ); + } + + printf( "======================================\n" ); + printf( "All benchmarks complete!\n" ); + + free( profiles ); + +#ifdef TRACY_ENABLE + ___tracy_shutdown_profiler(); +#endif + + return 0; +} diff --git a/build.sh b/build.sh new file mode 100644 index 0000000..e678e44 --- /dev/null +++ b/build.sh @@ -0,0 +1,9 @@ +#!/usr/bin/env bash + +# Use this to build Box3D on any system with a bash shell +rm -rf build + +# I haven't been able to get Wayland working on WSL but X11 works. +# https://www.glfw.org/docs/latest/compile.html +cmake -S . -B build -DGLFW_BUILD_WAYLAND=OFF +cmake --build build diff --git a/build_vs2026.bat b/build_vs2026.bat new file mode 100644 index 0000000..bc38e44 --- /dev/null +++ b/build_vs2026.bat @@ -0,0 +1,24 @@ +@echo off +rem Use this batch file to generate Visual Studio 2026 project files for box3d. +rem +rem Usage: +rem build_vs2026.bat Configure or update project files in build\ +rem build_vs2026.bat fresh Delete build\ first, then configure from scratch +rem +rem Re-running without "fresh" updates the existing project files in place. +rem CMake rewrites .vcxproj/.sln files atomically, so this works even while +rem Visual Studio has the solution open -- VS will detect the change and +rem prompt to reload. + +if /i "%~1"=="fresh" ( + if exist build ( + echo Fresh build requested, removing build\ ... + rmdir /s /q build + if exist build ( + echo ERROR: failed to remove build\ -- is something holding files open? + exit /b 1 + ) + ) +) + +cmake -S . -B build -G "Visual Studio 18 2026" diff --git a/data/dumps/single_box/box3d_dump.inl b/data/dumps/single_box/box3d_dump.inl new file mode 100644 index 0000000..f60be18 --- /dev/null +++ b/data/dumps/single_box/box3d_dump.inl @@ -0,0 +1,85 @@ +b3Vec3 gravity = {0, -10, 0}; +b3World_SetGravity(m_worldId, gravity); +std::vector bodies; +std::vector joints; + +{ + b3BodyDef bd = b3DefaultBodyDef(); + bd.name = "cube"; + bd.type = b3BodyType(2); + bd.position = {-7.98573353e-07, 0.499929696, -9.86034479e-07}; + bd.rotation = {{-4.03613463e-08, 0.825855613, 4.68083634e-08}, 0.563881755}; + bd.linearVelocity = {2.47239775e-08, -1.7695136e-08, 8.65693384e-09}; + bd.angularVelocity = {1.73163173e-08, -4.2026933e-15, -4.94549042e-08}; + bd.linearDamping = 0; + bd.angularDamping = 0; + bd.enableSleep = bool(1); + bd.isAwake = bool(1); + bd.gravityScale = 1; + b3BodyId bodyId = b3CreateBody(m_worldId, &bd); + bodies.push_back(bodyId); + + { + b3ShapeDef sd = b3DefaultShapeDef(); + sd.density = 1000; + sd.isSensor = bool(0); + sd.filter.categoryBits = 0x1; + sd.filter.maskBits = 0xffffffffffffffff; + sd.filter.groupIndex = 0; + sd.baseMaterial.friction = 0.600000024; + sd.baseMaterial.restitution = 0; + sd.baseMaterial.rollingResistance = 0; + b3Vec3 vs[8]; + vs[0] = {0.5, 0.5, 0.5}; + vs[1] = {-0.5, 0.5, 0.5}; + vs[2] = {-0.5, -0.5, 0.5}; + vs[3] = {0.5, -0.5, 0.5}; + vs[4] = {0.5, 0.5, -0.5}; + vs[5] = {-0.5, 0.5, -0.5}; + vs[6] = {-0.5, -0.5, -0.5}; + vs[7] = {0.5, -0.5, -0.5}; + b3Hull* hull = b3CreateHull(vs, 8, 8); + b3CreateHullShape(bodyId, &sd, hull); + b3DestroyHull(hull); + } +} +{ + b3BodyDef bd = b3DefaultBodyDef(); + bd.name = "ground"; + bd.type = b3BodyType(0); + bd.position = {0, -1, 0}; + bd.rotation = {{0, 0, 0}, 1}; + bd.linearVelocity = {0, 0, 0}; + bd.angularVelocity = {0, 0, 0}; + bd.linearDamping = 0; + bd.angularDamping = 0; + bd.enableSleep = bool(1); + bd.isAwake = bool(0); + bd.gravityScale = 1; + b3BodyId bodyId = b3CreateBody(m_worldId, &bd); + bodies.push_back(bodyId); + + { + b3ShapeDef sd = b3DefaultShapeDef(); + sd.density = 1000; + sd.isSensor = bool(0); + sd.filter.categoryBits = 0x1; + sd.filter.maskBits = 0xffffffffffffffff; + sd.filter.groupIndex = 0; + sd.baseMaterial.friction = 0.600000024; + sd.baseMaterial.restitution = 0; + sd.baseMaterial.rollingResistance = 0; + b3Vec3 vs[8]; + vs[0] = {15, 1, 15}; + vs[1] = {-15, 1, 15}; + vs[2] = {-15, -1, 15}; + vs[3] = {15, -1, 15}; + vs[4] = {15, 1, -15}; + vs[5] = {-15, 1, -15}; + vs[6] = {-15, -1, -15}; + vs[7] = {15, -1, -15}; + b3Hull* hull = b3CreateHull(vs, 8, 8); + b3CreateHullShape(bodyId, &sd, hull); + b3DestroyHull(hull); + } +} diff --git a/data/fonts/Roboto-Medium.ttf b/data/fonts/Roboto-Medium.ttf new file mode 100644 index 0000000000000000000000000000000000000000..e89b0b79a2910f0ac309a1845a9f733bcb568792 GIT binary patch literal 168644 zcmbS!2YeJ&7w_De+1>Q)rfm`eX(XX1BqRX@q)G1}MS8EHhZ=hCy(kDsF9MqpklsN+ zgn)pGe255GXi5~3?B2Zpy|a^@LB8+3-&3-?vt{Pqb5B42b4MV8AOzt-7GmnxeXriW z+K#D$7`j0a#2)oqG;jUS;KnlrF*Ul`k8%$S zqEroM>DDu=`_S{xT4o8N*FHfoPwhEsWK6{^%>eXYnHR*Euuj^mR3vPO=1@nzG$@%cSL2&p`1NYCz1FAUx;kO#8`!KXx4 z_c22)HKafs_rv#N26xZuU1McdD~xY5=6hx6kP#ybvxHt4&q((9(BZv@Mv9wC3!?Q; zjH$I?!e@e1%H$9v!7O+SenOxyMDerQY(Z5VepZK&#~#i6(VIW|=Lrtsz4i_U!WB^x0&BE^z) z5-XK*RTIx<&}Z~8@p~XjloHY9GC5vvHd$N~#L)#dag3{r*k!iZMG%EWg)hh^K7N0p znqm$JWFt3Q9pr7}^usv)FwR`Fzr&|&``B-b{n-KEbdcnfiX*Yl4tu~kyWw<4F_U6Kw6I7$Cm*jy zWGgOU)eboMF%wSyNPH5>$Y0z~WYaI6oGVD43Cekn;(QVD=$q&8P0sbwed>!>gzztiieB99X%J~%!-mj9(mX~Fmljpu)O42~DC-fObG$tx{ZB!YZKLj0y+=NxeaJ`aNR{wo|u@P77~`0 znjxhPa@PCx((Vm=j=vbzz1yIHL|ED@@tMAO4xpnJ;`crpon`5fmu65(a_1d&)^k)NkcmwFGwxF8L zbXJIh1XdJ2QYxgTF^W~NI;?r@u>`2ZG?jsIih(}GG{ylv3Ws*pn1f zial9NF>X?V)z56f;p`|PJeWO&*&^`dC}zzq?%^IKx;WRH-qze?&%2y_nP3`~Qs5yA z$w*JLTI{h&=9rSE)J$Sw3?_xxGcz+lRnV^O;EnIs$#bm#vQz(_B%=D)6K~N+t=7^O z)J5)(A4pF}kC__LcSv-NwzccGA#-}~7&&ldy^eV593>r z(P164dry>hwwcr%Xqhm!GznM%ciW|*c z1*>3HEVdxbUU10C4u7b&;7UP3nRc@%S*$^JOQI-F&iQ?2%7VRKWY zC-rMD97r3|hi&OKnvgTAHfcyc#(7KOye)CwAa>s15beAHOl5|Mq9rLK$d+yulaeyS zf`aT~%WDW9r(_&&% zX(V)NfDoh3RK}9aTv+lV971xq!yC`u$>F$KLZ(SdlM?KvV2dOrktXzNCaDnkSs*E$ zK`&icvQJqmU2eZ4o3y7JM||3zz8pYe=&ya)Rq6^$`H{Yo0Vy_l z3>FBUl)?0q#cWnr0O|)fy+`Ql9#S>=S z9Ch<(*7MbL8phZhD;0_{mKKI7mNI3F#u$h(25Mu}w5vh_*(~5!Dba)sQ`QsGS$P+) z9D&wci)VZeV$Ch(IaESR5%x2v)i}?)SkSb$O##+9M2?Ru>9)p9bHmJ|NHCo&-^J_!=}!jxk#!{h7amDVpzwa2m5}1rpNAbr5)qW{&Med zw=rXekH`T>R>E4e2mks&XEaim!C*kR0%10Tc+2V(_09%!l?n5*6fZNAUB#b&i%lUk zo(X>vNh7f~i?nI~{(RRwi#W2twU|r@5-n%wQBrrg^sTd<*ewS1o&tJT#W;!!SxRJy zXq-zO3<14~MsI%{_%n$B9rgTXA^vP^mgL+>4;>YgC6c}CZ_N$Sy=x0$?<&P3neK&d z35J@Iz$QVVAp)qIlol2SUq`h|mOag-0N2q{y(i`UTqyi-f&SzBI3TCj&vF zD=@Q8btx9XJuA$Fy~+*sOoPp;nCB1#4~;jQEk*MXU_^viDGnZ7NLXryHBstb;_{#N z>;tEMpanN>(?3Wy>GGt-jOf|lPMmD|{GxRG!g%`oH+Sh%Qk$4slSxEzHTRW4e*KO7^bB-% zKetY_3$a33VYK31t~{fbx788J?24nLTQ`Pc@k2G5aUNxCS=u|4eY-RyuQVBESbr(9 zG(J)SOLI1Wg^KnN42zMQ;5h{Spd^YU4DPVzz)0NfV>I-!I(u4GAT{?>PkS4`k<=30WDR20=3Qa4G+l0nW4j_)yf8N% zlSS3&fH9jw5iEz246&K>i1eN~`d&WCK1D|6gN92;FX_2-3%q2(nRy(V=a3K->S$oG z=E|N!oR9e>_>-PHH%eXDpX7JRweTVv&17MVbnh*r;dWgeO#&NBy7-%GrPR&2LCpE- zIh}us&VP>4R4e>bT8$MA7vhC(ig(Eb#-CWxDEEpQ;@UVcbsSbSH_GF5Fn>x&R%F4- zerYw@JwGeYIE3;KF%7Ju=0%6byUm@&Vzx+{`;FMXOYJj}r8t|^93-QLJjj*es_=Wubwum(b82x)=6gux0%weQt*sE)66z{ik>`6uPNSs z^GNj7HlNo_tbVmWrOP{Z@%6h)ro8-z^i`aaf1{Qe-Aa^CO&&5|G{2@leAJ}L?-w@_ z^P)1fU09` ztZ%9fooa#gt*1Um zN%WO#E!p}tX-#({v(!S27CRvkCM<_mXW66xAq9S3M`t{^DZ;nB|L{I71M07sWMA5sG67H5SBM7!EJ!!my`Uz zi8!d=s44WHzvyoyV)V>k==E2lW{jWu(&RXQw%3MoF}XQk-4U12Dx>=UWI8;s#~>c@ z^?^-!3>hpg)K(;&H(^LH75Uf3R0;F1b#4UXI_4ONN$HgsI|=sqM8-(WurjWaABWK! z|LmcEFP4@P+h>1}#BgWOvV~i6#T}bgtPCNyf29RmNXo_8S4h>J^!4+P=l${Plqaud zA|Wbr`dfORfCYe5aoEu#>U9JndM~9 zDN=Ux7JBvE)-7M$6tA2$IXLp*OpITL;EqFXu`+TQsqg`d79xZ z@&|p*+#f`CO|NEULoN!5$y_h}(BKQQsF43jKf88{N1P3$TvrpZtvHv~ykv4*rh}OV zW@D&`F?d7kaGmm2xp=!MI4}lXEXqWJd7|vH()X?dF6m=eH?fh}-?fTet~stY0NnCB zxTS>9Uh$1)SrGN2zEEVo+Qf^v#rJt&Q zV#{P!NdCP!{YCklKKf81)kv{?Qlj{)GW4IFxzZQ8r(4k*VyR<&2W%zjSB8^DWW?|H ziRG`SL!Z!s_N4epH@!^Ze9fA^Wz7sxr6#=|vNBceZRSB1l!`8u$OAY=Jjf}Mx#3P% zw#y~XbBeQEV@(d%MkJ&`y-j#nk84YiHhxkq{N)GQ<@mYM$a<#}U&XJ=7e0|5;ANnY zqL_I8UH!4aGv+ZThNowDtY?~y1wvX{GF|+|Nega0c;y_C)d%BpO4nXK`f6nMtkHVP z_zA7m3m7QQfVwpr|qp;$O|GNErtteCOI z<(f}+l2q4EVl^yU_FjsR#`XSgLS5sXEK|YwIuWxrA+Gc+z?%cmOH@k51jf2)WQn-K zIoNdxGNPWxJOHl(gu!nin_&Sof#sLeOfx^wssjoUm?-b-jhFyaw5Ue|EaNOV@^K&e zjPqd=_7)jG1jC&V#xuD?Hj_=HzB7Wh+D}_YU~LOF%bi}gAos+n1?%MQu1Pco!&FCX zvg4(<&_p5bJP&gg3~2?!a%h~x$Er!6BAU4WW8qJXsR&yE0`|1KT%#F#7RcTOe`6r| zvOhE|EP<&6WZMeqZlK6qx}hPE_g4vLun0x8f<@59j7*~#gDs|7HIon{BMVrGrC?wZ zSPCZHRAM=O`dKc0yo#(M!K4=nUP)Y^Z{K`KJhFBBeo?takL}(~YF=w~lhoV2i=Mb? zdI2}P@Nt{p>1!gqblZg+pq3A6Yt0p(5FX?BSRH=uQFC%=Y@-EViZ9{1=M9w%bEYc< zfM-E4Zg%DP1a5B=lVZsdmstK!5>0=0(XUC$tc8mw(u-no*H0z~y>{!_my^bfS|DOv z-3nz>B&Zn*-?o9`rTVt0qQtnzWys(ZF&n~>2#*zDUSN#p5CaMkEp|%+^Vu{b%Sj3= z>mg~xv{A1&(@WLsIoxE;-N(pE4{qOdNIbM{2grJrUh8mX`CtA<(!Qj$@VED$(3c*{ z-UVd?1iR2lF@^KIHtG#l_Y4f`LDL9Ipva9~pdq&idh_ZV>@_M%;1RoFR!mUb?hOfr zKgN>^NwMNC`p6{;P7<_WBB@XRaGfUd@DHhbKylaRJi^EtIX9`O!&SF3%z?&x*5&P+LhoWJWN*w<#%4Kf^ z!<7=@KBrfLyJM&cX#ujPdZh)k=!Sk@km!f2pUrkInvpYSIcamH8~ye9I{NoA*X9qW z&mg0_^&3!Y!bQh}p;spkpE96pi-EnTY#Vyuo9ruda%K<2?2s6h9as^IkgQJ2;BRP4 zz+4G~qro%GjKr{>4qCN<){v*cjfs*UTxK+z0Gb`cS*=2K1#9qyjbtNfU7e8BW4nfBxlqiHNhhJmPf=oR!F>{TgY=Ud zylx>U6?_M+dswcA(Ur!U{)*B0YF5`UEQ3Fyk)5k~3z*o#tuA2#rZGWo z;TD$}-8m$cl%x&FY5E=g0$J_bL zg5jhZFY6f@VLlJ%ke@Jl)Q942LeUAAWyT*Ef%Mo=GF9>^I4l0)3X!8PUD_BYLm~{cd^^=CdWTtAE#bf326k_4F4VN)7$(O*5;K=>T6;gH=Df5-Q zm8OZ`xJu(}QeiE&7??+tx(1qP=?Rlk(ZrHJtDOU-6(>&MR~6-JBKQlx0v(aZ4@3&z zA+a>lzCbBjf+wU32d|?ZmkF8kNR@L@%vI~ zm#e{~m}#?ScaE+%_VAqDhw8WK-MVInHSJIArr}H5+4k0Z=W>^}^;$Cub0;kAsC9gTN)9xh2ftRZ}+wSxk6M zlE;W;UJFB~lhhYm4$UA}rXFEqZ;i40g36(Y7-uSBJU`Mh=M-hwY1wOlD8dBMb4Q%#S3UbNxIb01qJ7>RAdU$cqQM@Ieg= zWP*uTGvYzE&{$RiVp&89L3%6zFk*+F>5mic&->3GuIj!YkKWdQTCMLk_Zn{_SG|UZ zkoXrRHWn_Vk0|XibM34N-3LnR=T8V4Hv#;_ioUnZp{N7BuUJCEiuj4okb$R|OmJQB z=5!(0@JaQaI|FWE*l(nIkhxzUR&zY39D3`_2X( z_xh~cKIb?_{Z_QC><8*cK-0}s;t~=W!^W{9o=#nA!{Jh?9dHjK$USPV&Y03NNjz(1 zOp7RiHukO(s(uJPrmH|ODK->5RnE$E$Mt|dumoWG8%);f>DfaAe7~oE|2X=mcSa1| zJa&Hny~qD}F=N!S`tL0pGeb;plJZls3Vy!%vS*8>Gp0>ym`y6Y+;n6jxikJavod3$ z++RUrtU?{d$L$eXRY~E7S({~1kx}m&G2`arVIvrTjI%e{&NTo^4H}AY+?MrqY9ywNl|r@A6!h7;*LKo- zy1dCVE_mnglQKXRV^&5J2!s4ZXFu9 zBE<_QMmv9(tFn1p|CT>( z^xLO-yj5UOAin~@fPXjdhGh145tgl1ThpN@}rHVTNqKG zP*VY3qDUeQ(Hvh5s4hh?@oa7&XIYPR9sZfhH$00%JZ%BZJ0?hCuTFIUh7xtm#}TCb%9- z%}<|RFMf0OBen{a>1uI_xjM4jnMx4PZVT`p`GRws9gdlr1s3%oOw2WT0wpEL#FA{7 zYDp=*Qha)9dODybc2?)M01F>x7rWE-mQ$ys{f?B~=H2*5SN9~{ZvLp;zp?Lb>io*D zzx}aer=M=Vu8dLa!K%EEj1JEw{Rw!`nVLk`_dg{RP zMl+VwNvhLE3Ud5Vc}{GIW#*7NR z3VmfCQ=$+fbXAJQ#&IgJW#DxkjcvIoLuVS;R`TJ!0H{%^oj^9;S`(72gxjE&Iegbp zLgN_`AQoAJiHOiUNNqq-^CFTZBus4HVT}2Kd~WH5&o5v8{KC3J@&n7Lj*qO!iu7wS z`>Uwmev2c~ccN~Rn7AMBMSZ)rTQllV>$zk)K7`zh!&M{L9kUpT839M z{08IXm8X;dPhq?$;Op~hh`~x&9f5gDnEG*$p`?W+w})6_kt<-x-6Ytqq)HhvMfTW^ z?5)dEQ#?oter}ai*7C_3uh(SdFMsv#(xI<6$rRf9^YlyY`>j z@5L7^X6#(}(9~T11NpR4;7hS|88$Td4AE|0&@>1FItOA|Sg#n5bTH^xqUR7B@34i z>@jblbIjo5caCRe9lLuxYv81b(-+TKd+x%PWs8@L$X>o=)%r`P)-G5gjvsR+d(@R@ zm$S2ZCB_%4Q3<*r3`lB$N1*Cz7?SUvz|NOpiWCds;?7#iAZ1Wgx(+S`Lf*PVe{`O) zrE#E&2hh5*c6o4{($i3vKOBcl}0@yQSBh^fIYLFTbKY zh)rrPc6Vj0p1TTl(DJM1t`;w`b(jr(mc{j%1zJkO`Ev^jeqsfavJf`-2h0=y#oF0p7tB0z^mykTwd#`KN52!l z{3Ta(eBYTdfM>ej8OKCzg;9S{Qdu{x2E zu?%6U5}jF@5iZ(_UK#e7=9aZIEKwSZa?9sA$|oyg8iy&1s1E>5hx&kbdESrZoxD%* zwtC|Ye{bfg=UP1oMkb(k&!G~j!gb3dhwtT?5ipSno`~dHX144xV=F1XYcst^ zKc)A!?IbbVE|Cwu_=0YwgXq?a7sv-#sRiO^66;QjL{@RB;;&{E{XMAH5Ku3KF}Re# zfcKDT)kY!7!H$uF=5PBXJ3^4-)X~W5!yI$vvXykM8SH0`vX-+}vo^7IvJSH5Sm#^4 zJ1d8+XtaPO)&oh=G864N325jpgJh&3UCInI6f}#NPy|tHAZGBl*v-~7XVkRqyZel+ z)o}OT*>gUMq#H`LDK?-0=>^O>tm2VZs^TgMXu~(cs_I;;aqBk(!(vrTalbR4RKHy|v|-c;r^~e8%KY&w+1n zC)2#P8l<$1mSt*N$|r)N0m*?01U~*;LnVjoZ+q&deGa_1+;^pEXFgG&1dk9*9tg z%wm4tLZ<5{2J^7BY2<1t?lt+X1Fuh#_KNes>)xo8zM}|kbdFn1a4(xJg4&L1@^B$9 zMR*QzUZ$%MUslQp=0u1yhlG(?13p?kC~M8f1JmDoZ$N{FrnBqrtXXqs9UW6Yy;74V z{TkrK~rD|#mnt8Gh=4xk>d0S zfwvgunYtnhtQMl6lb`VtbFFnB`tf&BN@E{#>Z5^_<=NftG*~E5j1?6inu`z1s8qG~ z;T^LUtsT%~J{cjdnLtc^>y$~aoiXym$s-2#X*Uex%oG=kJ21|YK(LqKnn~wLYx2VD z85pNR>`dY0Kup6A)FdrPHzEyF7PE3N2x!)g(_8I2LViPsSyzI9NW;{M@w2c3ZVAs% zFqyQ}H>SeuFk^zEwyvaj;@5I|MvatOhqp{!v}x9g+(ol?9e%IcyVYlQlC!$U!@umA zGq(Tew9*R)Oq?_zCgIb$RxpWl+gYu@%B%K zE=ePy^zj+9K(FEVOOgGHMywhnbXWY{ovCa!`2uRbzag!{;WJv+uZaEFZ;F%;NTy5v zD8BwGtby2AR*EN7m|{+XY9Q0a4g5y~o}u6L8CJhYYM17 z5Fv}!A{U`W0)%wMJCNn?it4aM>2WiHBWC8=85MsE1MMh;V|U=2t9+28YQs!Y!*}JU zYb>?v*KZLcWfWW5f7G1hGUek<^Vqc}L+(V=9%PxS0vEyNsPZ!pX$(|g{48^*Br`dZ zffM6x7S@|79=b?|1mlcVQI7{{{`f z|HH-MghM3c=5e)`p5xIW&7)6{jFU${UUz(N=N8R6lFrSWb(XttICFYygY{?5ZZh>4 zJg9q%?t`=XF#FmW_Vq(0XkNCG*t`k?}ENMQV`+zQ;d$rEZyLfQRrp^sUq!UkuKrDO-;VyK3zC5_ zTC9N+S&mrA(#|}X{HS`m#u+!yjrR(!nKD@B$#l#=j_B#SIfd5T5mZW08-7XfB4K@&Bx z6YvgcN~+Pb%gH;W+H!i9KhSez5c(-=ucXKM13kWy)S@R?)X~526H}b&8Ej_^@Iw6* ztBM!m+=0N|k?7`;w?Y(^NGng3#`7m^>cO_lK(w!~u}8AbNhbA)p70MW!ja1W%w#44 ziNg9>?3rnnuwYY&omDzSi>Xo)focYbISwG^eVeCu+_B`$N)n=+ThwmbvX6 zc-DlBxI7h!jU+Pbk3UHeeU|?sKmX-~CG*Dt-2BV43tv2^fLhZ88aegubdI;34SdQP zpf<`0ot1dq{w!R-YGq+V$+pPk1ekcO7Tu*$9)DYVHF^xIl~k_H@;Qxn9ZHHg6P3`mPQ{M1XSZm7v1!Zhe>1LYQ24|=U2Xx6t0Ytt z@)WUp4K^^)A3-f1E4x?0fK}5nW$u8@&&2T_e#y!rKX@4MaIBuGG6)iQu~@QFS>P@4 zm6P{HS5B_X06LWm*;CibL)g=a%2z8R#+Y9j;k-U)1-MSU?sP_yF@#dGC!@t0=hHo3 z2By#59LQp24erJRJ$r^74c=~G4@35Ng1#`qK|V-pJaWyib!(>M!n0pbbZy^p#0%HC zABIf3aO0V)q4(;Ua|Tv#Snry1b=BGB&-NSBSt{Fel9imz3i+gI8^_vppVsf#w?pmV zrH5wB5~=I)oO$c2#%--!^WZyWuZdmj)oL#OJa{eQG1<{ zz$oRWd<1MlgvMhZU)ZPKJ_73RrFK-P_xtIJ0? zIxPWfBqy?z85=1W8;|ksl`^0Xit32rGRFbN@F$k!i{MIFm-}{vTOF}^Y{_(t&iy2| zFiaZ*T*I7Vrlf;ay=-=?S7|_1{l0IhvPw4zXgC%{g{)s!=TIt{Es{s70hpYo@T(x3`|(`$e#ST?x684aZ7Ytz=l9;^c7 z2!$+Um1BiPO0?QHk8Ba`xy++AxrhY4Bew!CL!!Jn*6Q1;u1)+V;2-A0DbSOm9gDq>6{(n@oI zH7n{yT20Ld8i@I63*j>i#7k7Y)fpQ^&rt5xR+cYwQBlFHw(po>i_FPpphOp$VRrO; zZTJv(@sl|Wn4H>P@O;RN7w;nlP>yydhv=8&Q`$zZN?QVJ5zV(;YcfGtM&PrOAB(7v*0!Bcr0Lkzb8mnXCUX*51id=G{`mz6YOh~Kq z5nvo%+6Y+4$gcm<^5yw6XXZ;|)0d{ZBE-eXOOwUB?AnWIW0N=M&VGuS=f%_^7`(cF zyR@2K(U3!<p1Pd{|vAUh(JEYb7YaogPx5hzv8Dp3ZvP&YCuOkoqlKu!^kY2B;p(_rUaGz(w#;(#e0sS;u~IWb1oX14_M<)4BhW z#^C;yJ8xZg729{(cHYk+em7=lU*|o@frvWo6H9r-=ypVwD;1(fmZ*6}x^0tz4LKn- zp43z~C$T1QjsSxbrvwQsH{l3M){md1FLiiuHaoow0wt&mKUjc&fEW616 zumK#$jI$l8EajaaZyURf{=HmWLVUN6-zIfpz2K}bykJ7le#1L{vX>|!%ZUF@QuR|3 zv@!(k;TLGlD1W(fz}2Jl(y43EBpupPe@ZFT@M-85t_m%;t``5vTX)f@S@_*ZSzh$-t7=kCUuj% z_Q09YL%xR#utBRRv~+jhhGBKOD1zF3Yj-b*!ER}nQA}!1*z_;N#T)F0$UtX<;rqv+ zw<4B#vKExMficC_HVAf+P|J*kYbze4cjz%G&-BCPZL2C(jCI_`#%BXJ(uX@%9uIOQ z9}YpDx8a<7zgG%AAJ~A-qn{5vL(lBJar;>GA;jyX@BwWsehpfc5JoB?x;+AT7^u;T zYc@?&@PKW43HLom;(QMB+OilZ(%aW~aFV zs@aKH8JXi1_1-N{=W0r$$Y=!mv%CWm{JCMCvJcCP>!Mwp#pv0pIftU&UT{Bh-PrR3 zUDH@CPm8aWB^-*{dkZ%*fZ(NVgDdZZZ0ZQw0)x zri47FCdfRKi@2)!ga^xioSUAqt$p8j#qmF{_=!GwQ3NXf#5HF{@POenKTDXft~i4h zS0IFAi?dDS_>r#De9XJzH$;>pBTD0+@xd@}#pq5FL zmnmC(omcf}&DoaPO`1TeS%1Vb?pdZ7G$5y!Kz9-cICwidO6f9szDTNt5cFS-IjA z30x#U_FcQypK!lyIIdQSUk$rWyRW8}&i4h9Xtqm*AoIP4;3)ImrZdw*QO^Rl=laq- z`s+&i_=>5|8Xx*A3HN4I0+KIV%1@?BcZKx1G>WXQKz;Gnl12O3XrWO_7l?b?a88#b&BshuMKKxU!q##<<% zNbbEZyg)K9BXotw{O-<~DRn2-#XnPr^!ljt)~g5386z(c)68XXmJnnjK{X}Ngi!+A z6pSF%howQ(4ZUGQ{5^+^3AC~@V01i_2uv4g+j;VYZrvwLvemATT)IEV8{BdKAm(R}lfReRnf^jf1^f8``oWj&K%iH&SF)GHh^2@ zDiyp2ul%42@6l~2Uqk({5MHB2sQG`bk}`9Q&={@=Qlp$Z*hnp4VGCzrbRc5~-FUIz zGy3w};$QC`G8NM6qyoK;)tgEOkSB%5!C`jnZU=?!;6S#Tfr4a}Vx$zQrqn{}CRsW& zYY%(s%@bbgf@0dCoipL8FdVGWg1&lS1v4cj*!1EY=O162wMvNVbbTSAwQxYnkS)FS{4kJ ztddrVMof;9EsMi*Ejkn6PuzhX6dABPnOtHlp{|yehP75&cI;}_Kz_8;6cyiKe82Y9 zn)#qaEIR$<2ZfbTU195-$@;kwco$7rT~fZD2bi=cn;8qjU1!?XA>)fOUtUmxEikp@ z(VN(LF7C`;IZZB{2aTxiI&AiU(%_A1_%m0OT3Y)9f;|9oO@#CE zujIbD7RF|0^kgXk(7-@LFcp!0n7`Oui*CpF`T_-a*t{37{2)Qq1SEBe6fWlW?`2@l z4p&#XxgJTzvTZwX3P?=^vRfPQlHHvFnwg!C z8^|}$r_YxtW`27GHa8NR0GR%ee6hysvE+U69$n2gySpTzyNSn%reMU(67*@DFjnzW z{X*DcFf$%COsye3QDl)tFILmf!GeYF>n08b;BQ}wSd-!Y1QC1R>2gV|5T_4}q0@}n zL6$T7-x~-?n`ljPk_=_r2s!Ap4EpYPz?2*DX1dwLG5yX_>^zUs`XZhRx5 z=ftGjNC?~(1BwQLqGoN+3myh*^ek#KWt7(HJc=MO+X{j*^eli9tLZB2aYFwhh4gp1 zZUJuEz)%yC0M#y6;HzbUo@=JA7pItZer^5ri;uvX;|5IVY4!MYO!XTYY~ir*(yoN( zLDFewjb7qQmrpzCLucPhm!ziWrP3T-4E{5PUD?H9G9#27C&p;Q1NkNw-e7JU^-AFD<-2q_PLUGl@1bttQ0TvLq%@ zlHnpy_t9_2gm53jjWlN8(L90lQ9K5nkb%Ys75j$G#q8jrG__rVLA3cEj~3wD#UDnF zCm%0fxMtF5YWa5Hn1#f&c?&Tw9zE~|wO)-O)}OO3-Tv;YUXSVH5?8H8(qy=@C0gY}#7ib{^Iq7s~^&dt#2X9!!gn3GRb<3Ym)-9Ev5pNUZZ z#Ofvbm^%1P1+nQv4y1VH?}qGj-(?$tBgZ`Glu6*_htv)R~_ioZa6ym`05o zHNRfHxg#dYRh_TL%lk@f+DMAO@+UqdW7Ra*iFLaHLth9;kSMIAqvZjx1R+3t_fwM8 zu8Jfp7J~Ks{M+hi^HY-4#;K%aLr12$BrgymQq!w_OwzE|DZFw-W_+hpHZ$-bDzUiz z3ah;G{YtRRtaBt)?0@w|&-P=dzPPq!+>4y=NRwfmyG?2T@vu4L`i^gK!*uprrR$oM za>u5A^`teeY}zs6)KHrB!KjJ-oDW;g8Jx9*9Aa9ZcEvs>RpFap|0!<w7KQ zG+_C(AQv0(uviV^f<8nHlB9_NPYURwe7dNBydxGC{7%zJp6tu$*amr)%H|k=3(I1c z^DwoRjS5-3n`5|s)N*57f|&royVa-j1HyRf9Dt6}9&4Z!FU>R1VQ-|8)H}1-Lpjw# z>|u}ycfti*cB70PtOYztapt@mpEn%Vd{zHu9~`}RR~+}_iu?50!8u~bexv4$9X)GF z*4R1nZ%ej>6+4q&y;A4e&FOEKqt?8f1^-vtQW$3{HODn!T*ffeJK*-tMXcW6Q+x0oeVTZR+)84`kZz0cq4^O$_o^Tz_WgJO?E!wQQ zsr%cRr=djo!yWd;gCoJfNlI)9me|=R$A<6*V34F}vvN<*MjDEBT17vN_jz>d-=kgk zSJzJzz8Ke`%kXafzqodNLZ2Q}Ur^_lH|Z;q+G$cu$*~PqZrQ)2TJxswR%^UlTJ~PiNcREpGJ5QFt?u}{=Q zMS4b361yRhm1M@IOP6T3w)Ltv{!jPj<62GVEDr8>wow(5aL0Alc6#FYr_@mS0)JG{=MSrX0BPy#qcKgSWkywHH50n7^xsEH><2tX4A z>QB0@9$8gKuaO{lIv4`RZ@6y=_QO(@k%^_tkuZ|Wz53~g*QS?`?!Pp-s*)Syh7B1< zZqRb$?~NA4;kf0@0aA;U{}sEkH^ITz@hMln{PN24GI`f7T>|ZQ6#gl8=b1J4PNxPs zb7&<5MeoOBK>7x7vNp9wnX--A!X{UBzO0_XyS{NR5S%-#@R{7mRDfuql_F`mMpbWX z-T+tP+>>W22fh8e;p3*U{VO%lk9aF*kEecMw1rG3}Px~Qah$~YLyxmw54_GncUUZZM z)r}Yl4KQv*S(rNFGSC@iu+Vy!RPid$JN>*2C^x0J`YuWcdPHPNZ}Z8SSSFYJZfg!IWj*^>+Z5k)8@7tH=9JwnNQbEArV93TZ-dbOiQouE_F4TG-}i0 z!8t_y>B9-~?UHR9A3RTg>M^@^LU`q@21nlMRi#Dg`0{mQ(m!9mWG zz@mmiZxIm78{Q-G6M7HMSeL>&U>gopp4>KGklZOaX6MDW_X_Tv`fNZxk*wGvYeLTO zKBMJk^h^3Y@CAK{-D{=qUyxYm&FtNu9Xxkx1LO5pbguL(#vF%zV2kk>K-FP}vGN)Q z!w&QO1!HMFyGO-y<%@kJj2KBcB%W?E8NM*w6O~)QEZFTlBSWlw_UVJHZo{T6Cl$I? zB$Jv?)BS(*| z{S3JW+=Bv{dfbCXuwthq#J@$>g7c|1$|4Si8?PVbe_>iSP*thHosE%rlh+fvx@;Pn za9a$bPfT==v|AeMa=i0S7`ivBh;2?`B$MHZdtoF&0=9AeZ=Dz#JVNhhv-Q_9;x;Qw zizZ5GVKysvFh!6;hJP|_i8T0+5!Ctfm)8h7EJjh+uN!_Q#UIi8^iO)~{TnkbXkD7%Cx6W)+gsQa+4dFcG)+odolc{zcv$1g@Y(=355IXDz9P4p+kSZ%F zu=a0&GLf#ftB6=EE^aIpAB>QVRdhpfzbYsFY2t|04Vs7_Enko$pnZ4Cj6W9K{PLGhogRI8YXJ~Yzdk+o<;TlOJ-T<{>MxHyC!xQ% zF3{_D?~)2)nJ4c|%tE<+1ee-Kd zpG+OxBkLNf0_V1DJKDI6Yv+W4a#p1}V!IIE?AtD4x_zn`_ zd^fhj!lN(<0dzA%4~g$yFFUSVz1oTXrD{df@0|4e*qUX1N>*#scS1SILSFUi7J1tA zo@=1%nP?SPy=OXAtQV_8rh%sbUV~0a6oxBaYP}y>&l2tx`L{r8cDhLAL2z{{Pw}$x z)k66K&DYo?E(}mLgNLa^TY0NQNhT=qVWCCrWvi44n@zCqI1_nrpSt%0UYOfV&b)N= zS*K3Vj$OzYVveQfJ~*Y%xU&$5G%(k<33UA1tbb&f1XnZqhhXlB?(5=?@$A% zjuHQXTT8I6Y#3ZQ9#jCYBJHE@RUc1QiIbhA?Am9O=ui2sd#=e&r<8+PFI}F4J8%tj zT_61AjIaI{$GK+WE*D-jeCE(0)0q9Lh>Y$9%r#VKuR3~K^FLp`w=N|tt5NW*9E^ke zO1|MnKt~4ig@c&I$e|c7i&`}qTS{%y77;&UaZdPX{!AyGD|d%@tts~8AMyMBR*(#k^x--| zwZ0T*g4&U^5ADr35&N-O)^JXos1jG}>EsJ(z_=O84MpQ-GG|!)BDff~_+ZY8^^N`3 zaVJdPjmfa{xGiR|cAqweD`y2|R#Jn=sQ`1iaFcTy<{_T zjh+`ry5_S#$s+D>Z4kThzYfm7F|US&PvuGQHcAU~lmy+~NewGe1ZxFt^>@oG<;&8J zIgV1v;kfBDK?^*+S1$IVJVABoa!!XaFN*}V(e_^Nw8d+O}4sRt|=}Tt8vQy1^JNP7mSl&t@ssfh{I)F zF41sdLpjysU>L~HJP94-6W7M24c5ox?=+ekK9)DF3&vChapHf#-GM@*fA4Rn&0phQ zMF>>=H7fXU*ZJC)}QwMJ>-ytp{MA)>v#{tX2iEt6O-o5~l9u z#u8w8N`iWjU`Tbbln-+MdZV3S#a(9L?enr2woyR!3q^EGJT?$xICI|2iZ&Z`^RY%Y zE@LVJQ`!@@odkdQA^l@}WPbP#`efB=60|cspZ>M|*wIfz^TW3vIkL-?xQag8wUhp_ zk{mz2iUjP~MS?y&PlsGTwC}1E;CylYv(K-C;!Ei#remPEFGFFt+oTK}V6YmR@-`R> zv&T>Xi(yj@g(Kvbf@`XwD2Q_1B-r7DcfCBrwQLNj^e-MaC^C)jlmNp0RTNWnDKwSE zkbgj(iq$+>6O#PQxzRxj=%2(%Ca&7EnX#Byz4W7B$Q7D0Z&Vp^8%BV;C|D#uTiD2d zjextJ8kZSFPUHMH>*8$cFhb1Z;D2N#IqD>bod2wFPo3LIT##LxZnW&i%Ru4%qRjve zl>YCR*LBMcQnPa9eNy8TYFVhX9_cqfVF3w_>djkFSnd=5n1hZ7j4SW6d z;EOZijLA=3Uuu?HVqT}2-QAh(6ca8CQA8dMF|ZK?438QL)nGRa{UQc$t*8(xc<~In zfUO@qsT_uT&?QE()}9#vD(7)5lgMf|3Dx~@$tRu8f|}|)q-mP}!ESS-_dztT$rnCI zb0t9aK?0aAQ>ky5fyRKSeQM*>lO7sa=T#cVS>*YX1-dc5IXchCfCWID_6LvCrW)cj zgA=bMXcX^YmXyhX@<0X0dnon^+n48kk3FS$*JQhf3El|I^j;G3wtDL=n0dVvNpszl zlt6D*V8>L{;0*C53q8%2_zd)$;BHz>S0X2pMEZrSTDEqV*+oUsD|f^E6+zB$lJMd) zVqHcS(LWup=#4P)-MX0%zPbGEgZb;85wGp^!dK(3(l53X(=U*M4d^_wV77K(AEqcl zYHuujL2XY9!>kPs7FUgLCKb@nukN1sGC=3YVPXRuzQ zf8CGGVqrk4ZS>W;fCEnFXXcMc;BMt@H)UH*qK>(6KUCMSizI!^Tkp&UJeAxP62*{s z*wX{4B<2KKp;lBr4DQls+VpF#;z-Aaa)EM>O34TeQyX3iZA3vTyTa0nt69F3KK& zp0j&p*#OwcL|zn49%c*vW!Q_F%PS>{<1W9Xe?9zxK6~|YZp*W^XK!7-zSxpwqh?MY zH+IGhiPGo4`Ta)kl2SydN4k*JX;JTH^q$x}K4$Zh6=&9OS-WSiWBw+18|WmJt0JEj zfOUux#^bJM>~1Ygry&ue(`*LBgSH+@7{`?I`)BW~8P+0hoTK98m1yMjmL_m}UrDSh@-*abYGlM+O z`@a9rAI)svz31F>PcJ|H*1PAhjB8&a$nJOeD1UnPV$RoUoee8nPmBzm*K_S=_BnUW zo4;?$43*^GT&PlQ0hcwO((p@rm027FlQ~E%OlXm9d4-4aS!F`rB76n*J)R#`PYEmq zLnAA&+*DAjrz@~Hmd5uRV$1yHPPn^~%?}L@(i71e-Cn~2EW92YE(|qV1|0WfuQ}wI zgPJ;u$`95&bhrAOp{*S{4?2pT9D89i+nl$+oyQvUqp~V*zsSD<3EM|AwXU zJ*@oWbl14QS$V#f4MpDf2fQu)$%(RqcV#>2&zw)&C8t&fA}z=riD(}|v>9<7H(8-3+EhYCM+k`A0MoOc&hbi-AnF`Xi6f>Lqax^t zH1sVLRU)clRNW|*2AEPxlJJ+mXr)SZfa2Kh0O%V9{P{O^JN?kav5CZei#~{*}85Vp>(`7l}Iz!Y>wl!h zm;R#ABEYbbQ%qS<8L<7s4}5LEiHeW|ui87|MhmiA(|s`}kL)U}$|HkxBi}vMeX*bQ6fXg+M_CKV~dL@G9eh!P&*z3{6Ax)g>)jvL5rjNIF(u-_gEkKJ&mgA2(WG zUzUC#!MB00oUR>%+I66eZvs8P+!s!f<-Tv@d9&#TVJw)NZojC3F5c7SV(+^cs>l!& zdxd42Xx*BR{9l(zmbj@cmiK3()M75)!=)@S_&Gy7R#I|-5eF)r|BVsv;lUT0q=QpM z4GWn(LPnsKWiEfm#owpEbKZPq@(D=O&OFsd6L6?|YXbB-N7ZVeY2x;+eP7TYtbHFn zOl&!y7Ovuzou<8n?DV>Yg`GLi#kRRt1^<}$6@tMFlt~bSZq^9D)fYQ^U9Agizoo^$ zWD$Y_sRk_>%t0$6jQ$M72EwiRR3KNNOs|j>R)3;$9jR$Q{q7n%qW?e_8yb4~43f7F zOz0mb7uIRlt_XO|_P+6Z;@BGxEKJ)t_GWz+6eoa(I;5W$+_K56ql~;2*$?2i{iQZu)x?79|NQg+$=(-9-`=11nE7M%C43V6i1u0j8O>Lk7bPEx?q6Ar@c|fS1YPcTEgeEA1&3DorX}>Gq|eHYlYj2}>)9LQ|~} zE?FfCG&ACZrcf+8k?0w|A;BS{GcU|1-K(w__nJKFNRox2FS9NaZjC>mew7uPe0Am2 zv!iZJcx`ZIVW_B`ie@T{ho%hNKZJjk7qKC4LE53$dry38;f^`Wf%%ol38)1agF*8P zv=9+vc7_o3yhLdd+}!|NDBy-+>!llpR(qs0p#=CqF1wC0-2>D>m#iw2mk3A1T*3@P z!qPW{CDw!u?;Xt*FSIJKdfl0eLe@E-m0*4sj1gU5aaYKBh~ZBb{k>r6By_==fANVi zrc1rV>w(t3qgh>4PY-(tT=tqc+o7=DgLNMsl6uG<@;W#TAPoj9Uh(Rp#UxR9jNT|F zB}{WDVrm4=yD18<>tfMLbda8Wsd&xzYY%VU&dVP-{Lbl52cQ6y+ zDHHi)F%|KXZO!WV$Jd=Ih9hp?>|%w~y>~DnF^_9GD3w1Z-xqTd)xEoS>>$k#l?BRE zP-ZDIYT`7nI*1H|3>sTagA)Os-E8N|Z_G$NvJ`-shL&kHv_RQPccbPrfYzV@d`) z2eNo3)9}`x52n7pdg%^+4^&8&?s`yRD9+t9%~wBnz6IV`i`+Ay6dP{jrFmK#1!MYtVV;E1$l-<%BHEiHCnM*c1D2vM=1H_=#?9TXbN66}F!}3$PYL z4(MFe0f?dW$a^qyK!wg~oH&bJZ`fil03Ikjx~OAT^?TG)-@^5YW=)6CiRh;(145|5 zFz3Bs!xlltgAQ*rG?e#U2FTFv<#qRa3?A51ew%GDU}Zn@XfPFk*rRo`&N!ccfTz&; z42KuW=+Qw%XoyN$&Zps4v%+&M1jLp=7k|2h`-}de;V+y`RXE}Rm|582Fd`>9oN!4{ z>4&i(AHq0;i}?-HWYWcpU961y;a}$Qr0R#e-g!spyX%FWUWlfbzf9k?=wJp%6!5%E z1tjv)l|M3@fZA;?{-KL&y4v;pG==R!8BAJ`f7ebK5!4~WZtATg&Vk`e6;j4{AnHtv z2Zp&P7GfM1^adn;mSu8MfpyW61iLcp#V8BZlXP*%e&Xp@w?>Y=bJ68a>oYW+v5^D% zjY2pm|C>MFfB(T{l|9dUeb&s0s`5N<{OswIRfV|KnEbo8`+(%7>o;iIqB)B78?XF_ z^@se2^^3M6FRUMvbD`j4{kHq;%8@gvgD-r1+10sOr@@bUrL^h^a&33w>at}&X3P8C z9nwdPNH4gB#+!c~8d#J)T%6JIT8Mr|p$8})sVKOP;T<-1S}W!+VvP`lrtE!9)}N)i zc2R?l(0~XvS&k$EF#kG|#(@OclEGjc$#{7y|LLEcTept$V*B>FSXutl{hoscbOrM{ zr+l$*@7JnYXE#gapSyeVS%bQF8H#KxL=W@t+dsFRLX2XiR@79V8ADZ=2Hgk(HXaxO z8bL&5@wT$$29nCgfQXP?pp}u>Sj{I!WYnVJ#tY{VK2h*tlRuntIT5%q$xba?d>9m{ zM=2y@1h_Y&F42>5r)xZ`KPsSs`l1HSVHW^c+|hin}V78)d^j|{!pA{@VS1|Mj7JwYwix- z3}!v{r+gB#{MX%!mI>jDmR}SjkP#t}Y+)h}S`?F73K)g4`(DqKEDpwgn@j3>KAUm4ANz@HNDkbmi&-E+u4Z0BvZ`ybxx z$-syR7KUay_!Ej~4zmv&NVf1EWB^nJ-h!a3Ow;iu7G&VfGEwcNd`#flun@!p!)Ve% zgpakLVA~w@M3ml6p>_0SB{m4`3cMm=Lf{zDaT4263fsYku!;i1%QAmX2fo+wAJFo# zKkKwpPE%SwYj5D3?dF(BnD;Sz3b-~wD+veIgCMnMnBX$7F5ogA{9oac0(4t|`XO1& zf3)O4AO*@#FeOZDVZ}V4CDO=G4`|Vt-%Oy;wR z5O{oeu2w>S2YOOlGl;3Y9;Nn3`l9?y3P?0K2XUsv*! zXUw0^MHcO{J3K)tr+)t(U&=bj0YBgEsC1Oau=jPd+h5F zC1M!mVKQowsYbFJMX4ooktw1@>2HEQZHd80A)Q|wM-i!h$({oZ@~OJ~ZHTI>e7<^% z&oOr1mN$LnI7Rlk!>h2=cLurFBlJF1p6j0E-F#p7Ub)xU48BsS&2|w^;4N}Yz`pA^ zDW&72)C)K<_T3V;_&?*MkQXN=T6hsB`kVfRlO!};0~wGa^PfmVev>8fk07?*=l6&} zP92pWvOeRgm;~& zeW6E&A@e5yzUI=>P)*e<*OA!`D-_pokF{Vi)V%TqUO$s5rUk2%uatpKR@5!f?3AEN zfRY8M0_aAL52biwykVFa*bo<|z&8V3(H8daE=lh9wjA`4KlM4V7rFE?u^hfU3DBo1)Aj zYHJ7!Vcr=J43N-2R4C~PC0biHge6mLT9_l+?tnHKCHE^);+Le2Q!2~ho#XksM@SAX z!WO5GmjBtamVGN{JYNj~hLup>R5*9*8v!R9ba!*%EMZ~TG6irqQ=J1gRzHR<%^xpStTo;*X= zF!x|_bY^BW8^=m@K_>Q;=&|Y1d>;Sa{>9_y)PB*AFSa@3j!o?w{r5${m6E>zRD1w% z#Y#OiMbFNI05`NEiv!0{#7V*fv;ZOPUkJ%UQw3;dCPhvZjCm9Q=?#cT?U9qHy^G_j8UEdmfj{u?OAQ}UhQt?8a$om%#HSk}PT}qxp)3$1*3kf|w8Wl$r0e8ZcQ8U+BGl2$QR!YdKSkR-8 zICu42nv_}F`efr=XX6BCvs`C0l6cKO(+zE++3<5MTLah_4yoY?SM4WI?>haf-Jfp6UclwWpY{ z`(F`X=g?Jl>BCd~2AtHsR<9mz*|XED?fbBMtWSr=oqM)C64PnXiAo_u<^%Jm#_*~F z^J?>3pFR2P_R5vlPch~5+u5&YJkEH1%&_z4hm8?;+3k7JZF~jN+m+yR4Wwh|K>rxJ!Q)D>EK<$ zi)zafFfTU29BYQzkh~TLYH7HrN*k|uH4d_n4AcNG8s zqx)BN9VP+89!Kadz!YbtSLXrEy7goZi&_Z z$F;(7fVG+nbg|Zm7uSl$Z$dSKW4NGANm!zM+4aHKlYe+4l$J3OK1lp{MzymMH_I!8k5SY@jZc^D+2X_D|i83Y21*c`D4&c-I yL< z$8t{=)q?-{;r<)^T5}eAp8wR6MSrsA7Z%%wUuP%w%-F?Guw}bv>}ErD&)SU^2*Y&D>M{J=@#;lnm`8%k4darC6WC!zIWf($0k*&l-T0+-hIc9N$QozFW@=ct5P}9onpMv-FCd^Bm5`h zjY+sepRJszx;hUT(z!}zKW9{e6Qy|Q6$8$mc_Xw zwhlf6bZZI2iBaN%5*3zg3yH;vNwTrz*f1M&*y3ZAcqKWAg#|ej-o6mJRMaVqi(IM* zYs|&sh3yM?tI65CWr1%sJuCkrvt~{C<-D6RKir&Er0(vkd8X}0M+{`;RY?L&@34}r zK66L`>Wuu;bn?BTfY@k}s~FCTPtD_*>MyqM@u@%cPf2U@UGyns{tKTX(IP(Ojg|Em zv9gITiIrI-v#@6j)@=mN$X8;Cm=zImG;hSLFx4+u^wR$!Tt=A||5LckW^;2rTxPTF z*0j78MzBnu$oV~tw4#X6BNthZ{+TRZVKW#&7&$RosOS+4!ZBxGkgZ@1?7;^nTs&Yh zP`-kMiGW;#>dEoRY^7VS{D_r#eagrMlXpEWxgwLFV^ve?j2k7Fa^J*6&M!|)oIh|) zGJ7?t)r6%oS3_rrm_!D;cvXe$2?G6&(ZZrdgbwq{X!09vv?WPWY=@y52az6`@a z^9>+dKr1S4C>6^jHIYSU_4cx09TL&NGcgQ|q!<}>)PVrXB%lhbsa72|Z1j-sV=&L| zy5I1RPv3Yra}o`-YgT>OsP1LuVvp8N88E3P=Gxh=jGD#2=g;i#9;LB%QBwz|b%V^J ze7G8p0G!jBrs)9!G>h48$gzK=53^{@TB4A9qFTepo$8h~mwMlqT5Pjae19gC`kuCN zkW}W5-3E@l@h12wOet+(sGjdsdh$>gyZca2N@lh3jT`uBbrUFvdcXO5dcS#!V}*a+ zARe!?kvhH=hYGC!oMcmd-Sc%%L?-`P`2lW#5cIMvB(ggt%qti8we*1U zy7pdZO`!yficXBIG-+G3wh+>*sOAZ6EI|@taY{2@u_a?Hvd`{uU4GiIteuO>b8hix zKc=2sT54abOEcGs5GD9gC|~bm!xH19E8s^Cxr6_ zwY%HheQPo+0f&FjyvOp$J9pfxaDP4L@?flW8P32yz<{bHeXW*rY}qDbd13<%SB8fRUs z(0K6j75e=EcI0tzIwC8&%rA6?=-mbFTBlK;KdhEpH%Ve*0ztOBddC!rwb6cxlE zn^_D~eCqyONb&Wy9|Zjy^t?pS^E}Y=WkJu6JoJp5GQfr&5RTb+e-UY+j(q`vfp7~w z3#iG1XQ5{%i-rhDC3~A8!ZDfwz-x5zXDpqb8>=!z)&HQfj5z4Ho@ye=9%dj z2nK}&2ElwOh`Ul=tfaZ$#B@xwcI+tk$$lOrV<;vTjrD^M+bI93h&05seq%et`i%$w zVf|vNq@Hf8=h^-rmp7KZFSqAgMIDPgk!9y#{*{QaA~6EbbV(<}^rrQWAsY4iez zUs%w&Ka>AzKLqTCqtjH&Nb;M8f$)`jgXh0K^!@eI^)eoLHGlq7zhlcv?Jc)FZPr>|)2Qo_F%|l*?OTzr zR2>~%LiGuTp#@XmEn1}*EN9TUR|^ncHceJVf+dtm_^8dL2N36`WF!Rc&7EC9Hx#Nv zW7&iPu?$^bbOY1}x9Pr0^q_~9BSy!Q<`WX&2o-YyYGRl?lpuSE?b~Y~tp4aQzqC`C zc4Nul%pbIB<$u2a@k!U=Uc=nW1`T*OwOi*tAOSR+s{hBbLqyBkH6(l9T!-W z3x6UYdW=6i&99sfmLuj|Zn2#CUf#ER|2yyw^+N^oIcR-lr7F@2O--mu^aU+7+N1SZ zLcOBBps~XR7=kbpGLKlVtQMzl0HVmOE`XZiGp0=@M3ps-AOfObgretQ#e$C*h@w`c z#S+nTmz?{F$#LYrAVV`GSYRm(jRu&Z)!aPZ|cU zSsq$IS-~~ydAK76)8hFu;F|G*YxdHgm%kHSvoe3j{t@2qFLjVmF(i(y@RAt>W#OVm z42BqsNn>+D&?Cjmny^FMkH%YwoKI10gM{`i%2EEdJTuQ3eEa#AkWD$ye?nsoy~PQ* zOdLOEqYN#7sjYW4mKcSR@JyD4nel?!k?Cw+fFfF-!bd9R<)Xh}h){ts-vPI*#A7-8 z2cz^#nV6mJZtX3;0=AX`i_~o-PP(CEOYI}D*cVuQ4^>%l5=}0G?xl1QSeya<P#~x9q=PW1o|R84peWF;*Xn zKG}9r>!1r7i}TbXXuRf~Khxv0{-`N4^%8c%6XntHqD)Ie=)zdB^4Qh1+SY?d*+VvK zYu&miGAn9Zv^Xd?i?!#@J!M!S{=i;TRGZBK4sQX66cMzUB7$NK2HK4CD(KX(pwTNq zq-J1(jETijB|a4lMyj5yBiLkf+*>qeXMVDqWv-zvH|~?=Z{On8*hTcXarXdU3C+KQ zUXeM-5R5@@v(Q+P%>d48@D;IdFRn%_tq;g8onY}|0y(H(MCuxthS`Y3QL$vq*l1L# zsJ9M-H#UqWl~T`ja8@+I#UT=0Lb=Mn;=49%|8VJQ=Chgku(EmRwVJi3+N=Fu z>SfL_)|pkT7k+8i!3z6W$t{d;y;%5aX;ONm zJ}Esiz#9{^sMZF)wUUwuB$PzyY{`oysdTpF=#nU%EmJZ9H`?-&NC$aPG9UM|0!{KA zC_sXejslas(ZckvgHVdJGR_$E`=|b3VN~@4i6XK;J$Q)2+&cNYOZ?YhW!Ai%L&`A6 z;UE8Uzfx$^#2smK8eQQd*+rl9^HYMKe_6fMiB)e_J%xHs&il{XIDcBd;qsbdQ>HD! zDOiCj&ikOVP|W%W@tgvhg$`*TM1!%p|K@@uCmbp%z!^00?SSM6z%)aG4qgJ2-a<-_ zsg49^*kkrk?vE5g=4xc1jL}qY`ncU=nZ~N~vUixo|9pgDpEBRapU7j~%R!v)$-Uf* z%J6TWau@&lBpc19psoPc5dU3$6%P zHa|aMuCFl^9elJhJaM`X57L8&AwbQ{KaJ<3;A^`jq|rnDd1V27Y@}tj#!N1)dOxWUQX1hx-DmVANikd)|xH8|sHeoc)K@i{deT^$q3r7%KL^ zTefKcsaQ7jc^5l`3F0WV%0O|tUX=cP?guAm94Wg^W$K2%^f!PI2M>_GON+%Wkw=sg zB;q7tX~7dZS3D2@s-v2CKRSETuG%h%=a=(Ldn56Fm*;za!LHKO=exGj;Hm$0=Bee_ z(Mckz4r9oyZ0+7yw;j z8!N9*ezxWCCH&y<6Z|BsaPJW4BCVd~>BFCy=$_LW@RvX@>q5dmYBJaiIJ^;WtE!}_ zVmh5xqL9uo5+)oC}5@l4GI;8Q6TB--)$7-eD@p5O2+4%y~1z( zzWC?Ny!k^WuXtn}9<;ya(4{$Qy(2t! zkVT4lrMbgLC`=?YELl`qp#DMxqK=m27iR<|LshL7RW^o}3a1mAYt>Ay85fVrIcn+@ zYb;()zsG+TE6;1)XM|0TD)qQboUFP&<5~QhX)BgZaew{AGer)0UR+)@t=0E!`h3>( zQl4^Nwl5n0`WsXs*eCzCeLAX-Xo}Q4Ekf@aiJzyQ6GWWC`C(KaS-7cD8GhlD>xJZf&1a9BK4rqvrgrq6z$CIsa^vSI zDMRItb{@Ul{iE4Cp3YT2`heMfVjYH@_gC_IApnBzu7kB`Q+HRP{~G+J ztWl%HDhTg6d=+L34W4w9-j0zsqH4LU#S5SnFRQ!jf)GB22O|pXCKRc-j?kbGFXY&6 zKyIgjAaT*ZZeKMAy2<-&;4@~=|J%muB@Rjl+>eV_l;tT`7H!vjGScM!pRu;p;#MD!71Yk`u$!o=YnmOf|iz4?5%|0&Lts>x3&K zOkr}n9;u4j=w9V;rr!B<^A?|Xf0a*uav|T&f-n5y{KS5@o4kDbnw|4z)c)+DGIn;0 z=||MrYqI5S{CumbduZ=k@IzuAYFV7lLLxRLVG5X_P|NliHCFIRQ&|;UW&exqgM2V$ zO&|{s>lgb506hDa2!|AOxSGk)`i`w-U#y(T@ABUjR^nWMz z$* z7#rn1e#}?Y$PqlIU3q~VF%yY^#3^i9OknHr?<)L>nvm06)mg`SxzLpsiizp zGth-82n9=ewpmTVKyjkbmqm5(e&I#?eX_AtxhX^ZODfCSG#E8qRU7?Xvb-!$zrG~< zw0hHho&U|hUL>Dae6O*R^0numb{u$M`{C~fpBcRU`J59+Xpb5qTdW86C<>72_MLu` zET+9kk}@FkmV{W_ijzOW#5G{eW|_DEBpulyD*AM5G}mz* z*64wTj`p~*&?nrJCZ$6wW|RRMfM5u!0j18}RYfVz+oQb9>5@A(>oZVM8{Q?3KITj& zUlha+ap;ZwRd&Q(x#M7Y+sgf4^gh-PyX3>>0Jh%n%pQjCItrbOBu{?|FMBYgm7$4H zgx6$!k~(5`6BPHvk8J4Zj09Qg=Bej*i!52w#>j{F@Vgu2^(<`fwFM34y}RjD&gN;e z6(#9$d=Qgo{vNWCh3{ql`&p^Y-9K(}`_p{s;2nAH_%}bx>eg-X=c8y(y5!$gZo@B9 zT56^RnX1l3M8ntyL#sFASF;rovlkG>wlq_S#rkUCctbs+@iTh%nrHcBi1g^tR1UgZ zN>;!6@HYQBKu75a{V!^G77pt z51fGrwBpE zafymTrvbIAH$VS8Z+_=gi1WkWL7bENa-6!JHOarJUH}IQqg9$__!@?VkQd%i%S%^@ zP>^MnA^&leK(DH>h5WFxnD6b_w9fz{*Gdsg{p%!O7$ls#e|8=y&l$gJb4F(JXAg-? zvBsI8(|pirIrtjl#LQRy3|dADd-vO13>6bfNy`}+Ocbt-i$SO`6&G{-M6)Y2?^6qi z70D<-xT(vTRw9P_tpMRd0#I926}`BVYQzdNAc|T*c@i9hX?P!6+)V%It$F+MmU8zN zC|k!^(FwCy0`mo7xVL7k*u@eL9^15)ts9g%v(K!fL+|9cyH?xQ|I`md4^8a;`nUn# zpB*DSF|BQIT)>tWo*22a3ZfUDnASEsb}CL1o*22aGD+AyY3gYC8{4u%(VEX z-SpL~=Jw9T8d_lu8Db5gI@tq3}XQa*kF?d zSym0h*bBJbVGlDV4 zRG~*i5ZTzqdOd5V5Sm(>*1e3S(;wE(7vL|IZ8<3)$RPl7+-(Sk1+rakqWpj}8>T5m z$rX_L(@YH|S$g{$ph{ z106`k;AKD#GX;?J=WXc3?0G(3f1X}f=7CO_)Pvn+Bjx+p!3a$$OnNWh|WO)sJHy-mSGP#}R!$$=F71>B?&jAvog5DcZ8FnK6+ZhqjA75Xbm!kz_d+O>Og-u|77*0yW6deI(ti5+tn z?cTj$?k+ZQntW!OGo^mRR_!v{vNN7L&)c`50A zd7^lLDbOM!2CDEYNC+U&fskV$I8GcTfg*UoB3rLu$|>8l0r977dnV84=eRGsvbVeY zD+LP`oj%nJ%!XZTC=*3@+nvq@|1;sun z_))4O-K?SfF?)WyV|+#V{VnRowYFz$&2Nu~_U)EtXASsi8wlf4mHA!HpZ6WzGkN9Y z74Mw?{mRFEht3-E&Wg#q!1EUk4pUw_rupfUB2mv$hH6CMf}|_XJg!vF zPF&V9JJNzQ!N3ihsUTSFD>6M0a0k|q?1k*E-dRe)7COo3H9d^QuYo;Ka2`_)XbC_X zvI|f$RW_G+HX3-qUmA}(DpCIyL&!w>3`ham8~hcBoe2>H>?Prk^7W{pM^dy6*knmi zn&NE}zr#N_NMmo_*Z7wQTy=fU$F6qqv_JYTmzN~Cw^ppEoZ7N>3opk0pg-JYcX&!~cIXrXi)R3%uXA;&C!HX-f|)OmBd z;7c>hSBP6P$w{JM6g^rZgF=WOL?&dJyajN?QQwG?2`D!+*Tw=(mI=<$a3)i<5qfYK z1@M8X>VfE-Z%;f{9V4APwdKD}D*5RA=4WilC3pDi505?X`^KYRx4>uq-Yd1lP;hzB z-ouI;sRY>{Fb98%8UpDbJ8_8B`S*P40XK0{O-L(;)KyK#tzA%_1J4uP#l!1d1rzJ; zX%(s`5)>7LbifsxjVrbp*UI8r%|n3}+si_9-UK)wS>(zh*-_5%7YMnvY(A09cHSe!-~?vK8i<@xU9u==kEIHWpMXCn9Noc+&!AQb(*?u zS*%PiZ;B2TzWZo6OHE7~J`zi=Q-HIuVB1B-H3(W`!a@I_GC>JJ;O+sY55n*?`M#-a z8N?Dm!~#&^;}#hv(UFqkVNEm~Mt4v+z|}*dlwqj*+PZF3-;$GNYseO8J^GI{BYrTMHxna|7ad2i_qsPlBT#FV7sI9)N| z>9@7QI!`Yw=oeQ=CtM9~E_Tr1Pe%F?#YJNZ0y4UK)-Xtl42X=3tQy%cvVCM~hd1GFI=GBQpE+M$KnOScu|2D!=MJ!v1WQ&d;(( zyfCXal`3JBGFw_~VZIAyZiy?qW!hYJvRsSb>dL%sGN`Q_P+l0EtT-xRLQQp;7G2oO zp`fOk99AR_q51-ZV1zAkg)`pdu?Gf%J&JOcZ^83g5s^brTpauwT_DhU0p&;WR7o9` z90lKfa+LD@l|=vZ)z^kzJ;%Pn1Z;)5$IK3O|B40;)xP7dn#w-)Y3o4;-`^kTzE(u; z@K*0O2RDeB2*_0GAY|EUt%S)VXDOpHT-RDqpwL2t5~y4-EP*E4_O`$WA&nF}C`J&^ zV5WtHT!+)nRG~y79A~SDDDj7AM`bDw859Cb9{|w{2M~u~Dg>1IKu1z?68-`cLy|)r z^hZ;`lhh9K8h&3JzT_(Y<=wzPbmPhI{uR&OjhvgVXSy!zdbTU8`)!Z2Jr?#j+ui+K z&U?}pdzgX~<1c(?82)7PomtY94Z$K9orUXp@ev}%Iu^83Gl!u?EA&t)2o)LVuLDx0 zB-QAoV$_#HDR-SIs*BA`ca5FpQkZ(pr3@?2-{AL-eSGRc1@|!d%?hqFS%+9K8&eU! z44J6TmWX|#{sK3Og`zhE-m$ zVrd4?xqbWgh2aBcM6dkZMWmwBohvkZw>s zz$sDoFKGq1CIjB21ZT1-9Ax@TERn5WUxKbn`T^4Bn^|Yz}CbY!n~HHNYRERC1<^zFTJ~Y z){J*{HL6*o@vFlcvhaQL7wp~6f7`taCS=QYuhehrex50x$y8&DsBaFdrWY!%UXiz^}f-_7uM0D9jvUdWHhi`vZAq z4jEn(&0bjKB@j&%uox#K9K^Oc=Lb{}{_p*_A%RO$oVsE9h`IyL0TD0~_M@@||gImraqIwEk>x z`34O!oSkZ=94BqZVS)N1`V=qi^vvxco)5N(E7Uq%4Gkp@+Z|`wd*gvH8KBWwqKB{Gc#$W=82Ozyh`< z@NHBCqDI6hzrn~m0znajbbwjSM;~Jeq5;z;^cemuvc15g(YFCM8P%CFsIP!1F$hDN zqD9&zGQXAIGU{iHUO02^+|fL6(d$c5`0UFUv+;b1T$0ab)7-zXBfKd)+!t$K1DUfD zQ6QQkHpnzZEY!oxEPg;QFN@I2q9RZqB7AL8-<5dO9m;uDP+_nCNDI;51gj~u;7yc? z07n?jDAj6uIQMKIxkn zTeX}iZ!dPOsLwwhHASf~h2LeR$C9nnJ^!)og6%C#_&c5dy*v-MtEJ~~H#p=N1v;4& zVBMFh)$p0l`H$^GYzL4vRYoc$ucpti9iGn|E%=NPAQzwMiO=-1?Z9WEq{8wl@fp-< ziQmx*K4YXwiO+PyXNudl;xo}wS$UcGOrqyAV+uYaBpHqh+%~e*AJLjuZI1;H?xO|i z9Z;aqMq*z)S!;%ws!4i04jv|1zX$cvYN(p!In?5;$O7dOG*mq-j+g~{pC{_MgxDd9 z-xVtuTnDi!*V;F+_^CM=i%ZoTwH!^Z;-}nPeG~DSM^oq0?7Gm!syglDt)!14D)Dwa zJqcxw+gZ#%MXs@8C%3=1-kE!5z_Qs_vjwiXN#+ zQF0?T>3be_>boyl8mp+zMb$&2Qxp;E126J5oaEx@R8(6UuQ}?}#jiOX!b--qy5~e2 zUV8Dfh9yR(Hgv?$1kDjD+MPlfES8`J>kq5}J-jz7zJP$7RfH%=CRC#Om)z{KNbwU# zJTj;lMfk!TLLMQ?gz66R`Rd-@j%nH@JNsSRrtA+^6pv}#>RdMVBY!_{E$=|Tkr1hYCVQ%%iDrcH zY{{}ih{F=OKp?}CtYh2_&L0fblGvjNVT~Y)0p%V36L{XB9fEF6DGhki zoCrno07@c^8JN3hYlJ(Iv?u09m3Y#e<40c5irBFrXbv^QXiJhquyrU#a9}ggmhC0a z4yrn*amO|rn{}8oXz=t7&DN!KZalwg)ftUDr>tw%Vg8`OvpY1~*rsFSIaQTq)oaOR z+eK!6KP<9MjNG{11w4*vBObRai^n(@E2SCA12r37Ya9rK)XaW1=oa8~4snIF^S4x* z6IxJ$Q;5hd3Ooz)j;6pQleG!Qt@6PA^Nf+boh&Mo_mczWjT&%-e>siqVFQ1uzwO+$ z^Y`oTKYtCpzn&DVELDd~X;LIO@ZyG2?e&QN?a$xr-i}vi;MMEm)v}gXe|B%j?#1A^ z-Nl)e;@#!tZ`n7z0x!?LCF%B#w3~fub3hX-hVy-pB2ZFs)q%APkf+P_)!I1!4ylGF z`3U`)!jweRLS7rbBEe(v0B@qy0tzgsghVRTr9WxYCRTsbCV4vLyRbvpqo(<3tOv7$ zccEvFh=q$VuP4T7x1XBPQaQ&uz14Qy)PX6z4`H2^VHp+2>57wD0dEeemD~ikVs^9| zhUdfJ;d)*3)jeD=4UDW^!G?^}Cm58mYD+1jCA~@5_@qpNb)mk>RDePv1a^k0bd_qO z$WW>;L+3D3QsA8ki~{6G-hZDBJGr3!s+8D8>7xf;zAUG?T*|xyt)?IBRkrxKs{Q&k zIgmF67Ai$%B()OY2!*ax%v4b(!fnR-EzwdhgQflO=BXgkW)@6EDYEPYgE3|7g3hry z;Ef@=OI@rV=d!vDP1b7xnJ3%$j@5Su#SKu)ovJ|Co7H&>Y_HdxP1A3?2ycm~d|*WV15yB5DvuFrkJl0bsB!h(srC3ywT=LNPgme4+xa_|XYR zrz5@ugkxYvC!hj~%9}!ZOl8;!UxJ!qq-k1^DO*|O@ft7+fDI&hiJf5d##fNSYxcpx z+XNzbA+UmG?O^*zD>cbm;`yQ*oi%uZGfgl;Ii%xFIG{>Q^}?}gp4f?|H65`P2j!%5 z&|m4tC~8ta*%LK6@Fen+=V^ev|H159)%Y?%*&5NL8OX(NBc0WnwQYwJqchUx*staW zU=8u0p#iceGcz8v#w)Fj2bPmx39nS5-ESRW5q1A|b;A?}Qkug}SKtxQN;RxRr?(+A zchj?Rl^Wt%t(v$&56-L_rWfmqj|l!A?2+!|`64U3VHwOdNJQc)!z4SB8opgyeh)|7`ib92YvvrQV1 zI&k=tP6zKD?Af;R_xsNML|Ff50g%4yUAf(XZvBULZdzyP;K^;9^=?(aR{aKDr*wVe z!_*!9UhCbmZlfkO%5~emLv*A#cv&)fdb}#NgxByj zsfW}Dy<7&P+uA5;f;0trEVD3gX_2&yoz()CuON&;r;a* z#I?P+b`{s2;@VGK2Z(E$xQ-OpG2%K=T&Ifbba9;{t_#F<354(~x@)W%cE1@E0&Q9o zrJ|NG8J9^jahXvY7r$n&&`QyQM>xu8_tB{j9zUV_Xro11ONMhaDv{e4GFjK{M~jak zj$QC9DTcA8hd4ClbJ2nvvW3+2DhXLNOs~EY;2fUo1Y7{Her{HS0^8j=DAzeO*9l;O ztJ+F{6Y@O?D65wnnL|F&Z4>b>P_9d+?Yl7*? zNdZovYjS{dZmu&UzzL*H3ve#Xbs*p?C=AH1t7Gdm_SG@RNq1PbH|Ig^0Q2L1gTC{)FM@xgSKYqID57 zysEg3HQiT|XAT@ZL(UvBXomdir~T7ASFe%Fm$z!vuvM$Z4O=RQdJXN^ZDj8uh8%x+`Z7$+ppmMr@bfCsnDoaw^K9aTY1B zn|3a~23$rVBwFNMn^(N?_U4r8Qr+X zc_0!RqXChj3}_X7MXpH21npu)YOq9?z7<)zIdH8hB`Oky`BxOBQP2@g{yHEnE3Ux% zBDTe0h;P80Oj*^mLyCY^a7XZ)H|4c1v*XJx@3-N=2OIk@FBdDQ%m)+Mxp| zw>Hp`t1boQ*3p=QItJ-`DG1nr+(f{Z2^*3^;Y%zZK6$WWCD+aKaS+@?m9?tv`E%u5 z4yYy-xc*jM4=+I^EUpq_4lT{{DeS?qIK0L$dE>kYo|yWHVG^FmMX_UKqMhzp0-%Vg znNkL@9JC~N=!aMkQBr^utB^}X7c({Dl%OkL1rC2_@7X_cav!s{tX58o5yeOK?LKm- zy8g_jO(QR9`)@q`sRc`B?N#@#S1MN-KW|+3p2JCpP6ISw0$-(&alL7#kc=fa229Dn$X=-gw-gn@nPm6hwt9nZQdUidKU>ftwAH5oRr_sIA# z;xARP^Y;LWA3Cdb)r$HD;CD#J@b&DAAa?^IVITn_l==zRB1+@T#;+am4!bCaW8~IB zpimbWBAX1}op^Xkq5>UC$Mx%7?jQW*3Qyc$vAV(R)0`(yHptK132Z$a{7kA3?0$-K z87IA?MGAkpPL0AGH`Zdh*PN@jdaSzTn(-8)h8R!?2jfEhzJD}7El1ucE z8q|=q8AAv4@7{5U>%pN5w+Hn5F7Ny)*Q4|v1BTq1ChzRpta;lObygn8+L$sg_LZ+U zee%iX#+|yg*a2KL#{PT@T!fS4Gxg^YjMcz}SMr%6jhYd`;vPW{|*a4ODrv-jDujiZ{sHmK*$D~I3hxPD+yv}$6~8W2>D;35tG zGZ!(|>MRh0rgh04UIJ-oBWal9;-}gviE^nyL&kHf0c#?RxHx6;5URcEOIlZct4BnU;mt+5y*;AXZNJ&g&Rh!B{9JeseKIDtqC zfI>pbEt@1y&1)x*aKEW+IeBuaQZGm3{oCNfs)=9pLDrx6MGO0}{Ji(%O3ZU6^#Cfy zo|Ixm(aWg!Lq3odlBdW)ZU-O1-jVOoj)veDTHzN8Nu6Hu3qtoW05pD~0CZv$`p;>0 z@vE3wOo39(7sWF|U<5@$W+)gmh-Jh-WF^u5dp^0h2+^6yLM@`|(pEz?T%IhVo1opRGJp&gCVO5`b?d&i(-0EEfF$d_{(@ z{^b@a<1N5eN{ST6B6=M*l&64#WBbA>)}q9FsSevT(+i8K5f@{qQ3XztFhk3teoe@+ zQRE@~qO;TA*_gpTX;9x$W0(JU{K;pFZe>1VQoqrxdgGKf{b%=jN zO>Nm*`+DyhdmPSj86FEstqoiifsbQ=W)t3IVsHK)87emBU#KIb8Y+TTR2BjiU0zM- z)F6~ev>{4oDlG+2o-AXOl`}*-JrXEdZgI`U&R+TR1}nz%c#X>=MhqQ&neF6V)ukWp zy4#b-7LteeZPy_cD{hltRULtS2!`g{Lh}(dc zlEefHA1$TzqyXZk(7nW*Ad_vcjuayO^Pxez_ZD8_mC{6|2Jb|bqEIOu;)YCC!WqpRSJNK4A7KDs;fG3zJh11yUzC!b zU$ia9FBXNR$b@dG|FA{d7tF*UHiL-?p5vuFT7_X0U}8L-70sdF_=y|Qrx}Y00xJ!| zEn1UxVC`7_hY@_s4*pKWL#*jTrSt`waCgt_Tz4W-6 zNkiBj@==+S6FKOlJBu#*ULT$EbM z-ht7+(|w`bU-NxHZ|h4>mGrHI`(^`0tdFD<^*~5ehq!c_bzyR8th$s3vIn&4&N$C4 zaGpz|yHs&ghXc!cy+@*`B6_I9;bL}U;5eIA@e=x*N+ejDV#PY_QFO=yki4!>jgCbB zuh7V3q=H7NEyC9D%TM@K7W(MhXA$>{O&-C2*}}>-d&=&v3SZ2sFyBVZmsQF>J`gi( z8h&)B_F2#LRz(gG_@S6GKuq;DB0HV&FrCP-v; zV;YmWA_5uNCSL%hM)>3+N0cyvK}|&|Q^MjF2rEMl0G!mQP@|eHUR1KhK`HgZ#=@RM?gciv7D|C%<=fZQr)5O=UCKEp1G8cA-_%9IJ+Y@yDf4`wh#S z4EZIrSLH2GOgMV~NhS-^VoZ3MuT)rA!#<*`s>o`|&47SFD>!dvBlHMw*)BnZ$0!tr zK7?#o$VKG0z&#);4X8c`{)`YfHIV-dOD8EFj(+gDug>Q4XUBNHA1sr{5BVcjSmjSn z^Jk37%UHhqqg7#5SAPHf%KE;mS0r$oL{CWRF~bDGStW!gp?VD}d`jN5c?oB}z?yzLtIR_t0WN zKe3zOIOkfob&tk@_399mi54kM)*QNj%t4|6<{=Wgba*UQzc4IFa47x+vDk(Ksmd4y zy;Q0y4RNDM3L9&Nl_EFh^a@y>riqAziKHmoe^Wh_TD%9_#*$yEfUs-X6aJeDVz!iO zX%15tgO}p6fPE2=4iT!*kqHXHOmRP+4`d?8C7=IPEJ$4ntL(MbZGlblnRLM(QYK9y zsh_5bdTwkxg+FjLJk=JCZb%@rz|b6iB5VLxowS564cV2 zDbH$2_p;$uWPUk1XIA@TRSpT>5s}@a@2N#78lq3YZYZ#1WtseJUNIJ>_J8Dl?o#e~ zXcpZ|0Qs-@jlxn}O)5ghgZ>*9?ZY?+7T2n70^_Wjvnm0?5{qT@GIkytjvy-WKZ!ghf;m+|ozaioppGQNp#jhn_`}g~2%u zZBO$L5IyVtDO{!b`UsaKrm)yWE|AGPO4MvJjPR|L6;p!DU1}nx3JD;({(-5;wm{W} zez1WtO3Oc4Fu!x(^(PGke&BLHRarPDS4XUv#fqsR{(qm(eda%Mg#Xjr&;Kbh$^8*` zhsYH|p3j}>=l%u`5hkEh03EUmEF4qh4jIsRIc8{KQL&G~RG_|MqWlA4Hl)Xx(*Q-Q zZ-msKnxOXt>3ngrBCK^JK`7DFN4eOLY0q3YpUbW>(_HMw@RQqF0{{Hxs{N(-Z!9bn z<0vEy&-il3@o;%Rtq}>j>Kw?bXiTzkn5y@L+tgU25uFj#W{7FRs#%u-2gkzU_cZs@ z3KTIC>Z^!eL3)XTJ_(j0wbWIgf`Srd?o%?Kym!)w)k_zxo-m$Qbsm}haO3Z+!hr4v zdBWMU=bRNc@7eiYxvVqMXLB~N_|TR2=J1>MBbHukB|HW(s9@;`Tt}c|Y3PexgA6!~ zdPxC?x0m9}n)7HrvM{NUjtS?le?gW7QND?mbXEwlB&007xk)0E0$z3`=V$V5%3i)@ z(b6k_{&{!Ygz>M>oa=&vBj=C!OQ%0Qex&oz^t3qlZQ2F04PV7BltP8PVu};I)F#yY z3yH!St7)i(=+A~EX!#+e?uJ0|Z9w-f;&Nbg$n+5MYKI&PA;p(oT#k!{%R|*NM;b~g zl;#Ks(Y;65n6y`FSTv=MD-gvZq>!vO4&?uQe~;hGytnZirUttTPfLFu%4YBxewni; zWfWm^j$LMzSfM66^A}w?Ic6xon6vur{)nrItV%}+_ACstY>KApvJ8CJn5%6$Ck8hX zduP}}I45S8C{j-&z zk%#d2iWcg&XJri*m)fpyh*J07m1pjTf2F-XVKAG%`zu*F$2a~z^4>eJs$%OK-m_0j zLP9DDJrE!P1OtQ+Kzf%J1f^q;rUaE{C<;;(P^2iu0z&92H5?HY3kcW{1RGeeT*Y2c zj)FkWKHqPx*)wOKfcHN4eV^~2@4F@B?6b1=%$hZ8R-dz0sOXsKEB@8I#=8APthHoZ zzZ*u{o*!RD%rqHN?yz4-DF^LF-BKK(8Acp(5oK`N4P+#++H1hzG4>l|@EEkv^aH<5 z@)yvL5Pd`&Ojtd*oRF*@<1AqJq{CH*KPe^{cN~ZCDb+I~eUfw^rvyc~sSMXc=zvBX z!2t2dSb}{o=_SUBVBp-M_s56cx6&;a?ET-FPUc#1*1El5+k{;wq;U{@+9nbE5qbYs zpxa9vS=zkpYX9;zNaSI1`ozdWaGtt23!+kZgG5D?=SOT+y9V01P3JH9;%9f@uDk3_ z`fKL#cdW<7;2C4>JvzlIcHe?EEQcEqROc7E!Vac+#L;|JZGpkmq3zTPN~ln=CM%Yp zD4CnLSPKL2h?<^&RV?tr(q^tBHV?V$^7~Vut3=5@y*MLd?K3jsz3PlCfezx(VR5}0 zADNVE9@(}RM0EI>h*lL4GfeQ-!s4Q_G{WfUknZvapD#WB=ii@L>8@7R)3;5YFy_4L zov&Pnt*4LDKu6w~KMXzjX|!3^E)VVPc*F*V`IC7?DaV@|JrTRq5i z=X`jw4p0OI%P3-`d2LMIu%w$O)=CB~Yx>N&-8b?$8bM z_uMO9D{U71MciQRGE-dMQfnvVZ)If8rem}fjAFlVa9@OfGa{@QL2VT$;8l(gRH_4< zKi0+PNi3p|vDlUcvDkda@!OtVvHN4ODVN3;$C{Ji3U+E+$g}ZB#j_VHS;*WI_UYE* zPzVaZ@Lw4l`Kur>ywv(Y3`N0b^$Rx!ZoD9>TL;C>c=VBV4gDY8$XZx6#8>qH;ECWy z%(?@7K;Prk$ubuEGaZ?)5p&Z?o!}&gMT#VNYBTH4*Ngw>!vnz9dCIn1~ z9>iyoIKmI7zuliuE0g}XYAQQLB8yGHdopK3rPd%$3Nb_dz>%MrlgdJdOxtGW*AvDK z850N``uNW~hmF6}vQ7`%)pkap+>xnweYIlMo34&s^1HNJ@$6HN4q2F0`;j5V&o#^* zQoMXXw=J#`w`~o&@5HJ{lYYiI{Zb2#@H$4Q#L8%oSZKf7#qQ2%kF5kFJvR~Sl#}SX zClCn!7C^KQHb9eRml+up7Ih;d5Nd+ z+n!zC{a)fJZ?V^$SGfxt-7TvW3r;kvr_h+SZH5~I8x+)0$G zjS-WjltA9}MAB2QLMd&AkVsVNFLbUr*al7=XV?jAP1}}^UY~vRx^D_to8n^!Y+ck+ zWR2(^i{KK7_qJ91tA%^ij=9UN`_8?y@Y5eI=mN;2QwV;=+~4yJrw+7SoApNB|Zsgr;s$Q*N{ zdt}^>dt@TKX%QxqS{i?xtTT;^FjU!4q(p=)yjjNma@?tb`E!2yT?EEYpF1xgwr0Nn z9@_J*oqBsFN+%@`d&~2@JNSL}ylLGd&TIABi1Qh)9av~=Uc+F8Xy@~f&ptZ;yyY+5 z77zsiF&x{5&OUkmF{QnfOA~K&@RzgK8|N6kG=kFlly#hYRBjy29uk}+)W6S3Xnt!v zh<)i%`C=!%)#)1pOSKB#9*Z*qr>{KE*iMx@Az}#n2q+V=kMXfyLW#d+%(@7#CU%8X zNs+(`9ZMg-o+_+|ZZP}){MkjT$(W*vBQDyy=V9oZ)~a#+@<*CTYe!uUWQ^w-j5pJa zqPiTok(zK_4#^&+MdbTkQj5TnC@msFl~JTbW+zuJ!@uEhzLwDudDKuBq+X~8?XF8& zAghCV$h(k3Mv9>Ip&BtbsugUYMr;EujIeF>A00y+-Bo`0tgn+yp@DUQE9(!^#r+dT6t6fe8jqcD@KfuD-;16JJ@Dy1=tomR zzvCv6HLwV38h*c9_6)$LQ1wv}+xMzwAalMJX>n0xtUfTeN8ak$j60KfZ>)6=&2u_rw)8_@J%QUpE9+zJeSbu>={UD)}jF3q8?I%Pw+ z-uCeQePMl_{PMYTFQ_!%O!(SDwt`jhM-g8(3TYze-*6|{J8r1oD|Z|OhU!2N zvE$Tn<4=s{^ti+Cbq3%a^L;;&JLV?T?oIgoAcr8~lVX z-(7plxX(Yw{Yo+Y-`n^iLW3_`om?5+2Z`Iu8E%b2*1Cld07II3edq`=Ay`7k2K6Nti;fJo8!S_#!@2tw=yR9O4*upg) z&|s}y-a9agO6Z9eW&1yD=@_BJs-uUwM^TMGXyY=CTSZZWnYL-9R-_Tb;I}=y()Op3 zT9HwS!b3k@qd>XF^TGF7x82%u5_A1iMsDA+nPKrpRKI?w% zw^vVW3tmN|HHTTFgTEqTdlj9Q&LnG>dj@EbU@Y`QBjc4I%N8dD>rBdwjL;AwXrL5% zq$F|%X&h)kTSadgwY%hIa-QEPvgCA4_+TA0z&sH<#SD2NS>ZC5i<)npSZy6tN|)K! znxJMS)h?O~xZlEDQjl-#aoiKdCa{`Fgs|piRyrc98{`f*61kHtIt8>PcR&Fd{p2UG z9S3`s8pQ_e4tN@&s3kb^W@2G_aIX?fLEdJ}s-nI-Sd8t8>x`Qt8EcZ*X}xD1Y_qW| zEd3o{d;zubaB!FS=$&^+jl^7_J$icMxAMjUf3zdk9I=BV6eP7>c2ddFcuKFueVi5KeVoW=uWj4kTo~olXJcQC--qWQk6M#Z;!()YYk7<} z`=f2`()R8|Sbz~u0JX3YP74TfM{t~67v)ssmKwC$nkO_0ikBzmcnS}vd{NrT+T_|` zZ8yE=Sl1^nap@)U`-HXqH29s|j+&MTk;YcKqN7r~{RuMqnvm@j*0k3XD99?+es$R< zY5r-sITffGAe8@}{Wl&xZU%1bzp?v3sQ;-kN8gHjZ@(zCipPzO4V;Uv2s1!RsUDzO zSxY2hStDGSdL`WnoB@vV7Ds#ond@|+B?8uvndAk48+}S({_J1R28zfr2l3cH^W=Nh zT5${c<-Q_MQ%vZ0-1xl_b>_8jYepQ$FiPMvs%G!Yh!xYjuZFyo6dgPrxdg&jJ zAnuY@dLT*U&!3##{<_!K&t4a0_FM@f{8aAqmxfJ=U3TVP>s#x>O6$ZmN&9LxaQ(J< zQR@7aEO&l)&%ylu?cH)}>BIWq|M}f=X^6&nYdP<&36W(%}Qy3Sm5@lngN zes;C>J-cQ8l!oowz5V!{wK2gxE8H8c(--eveenTNRYa|PbX9}IeHqu3CT(0ob0Lq@ zFRmZJFO`hj{Yn26rO`V-GBBg=m8&E#l0(xBT+bG-Zx~TuL&ldUE(F{YM^B#o$4qR%)z*QO{SQ5T;KlXV_Zi%;YSMH!3J9$E>Th>;rNB_kIWF9f2o`qQW2Gi z3C^aO{+hKp=OnhK7)EQ3uFaG&0+MidXY>up$-OcqMo=q`xh`>!H909$=EXEkdUU;O z?GpltF^5kDZtRD|7<1pU4ynsK=N>v#{`TeNyD!Un_qP*!2j6<{bdu0r;72WSuSJgR zCV!0Mo}~z(tOZ|&?0}kq?tdA$QZ^x@Mh4wx6aD}uSqyJ0?7j#yzf8YL#Dj=G z!kOgft-k^%@E`H>d*^?B%Q{bKSbyI#x#;GLPdxF&-d@j}EkCgK9P{)N?ptr_KbrPR z9_mz|L7i%CW3IotqpJj4pQ=7pb$vmHQ?jXMpk%k=o<7EK1Tpu{MytYghHDr|wA&lr zG{}Kx7Zncf+K#75wK*||dcQlK>icPA@i+8k6>GSnBn$1JV{f>>S(`@f_idiLbmM}R zyO%E5wy$%mwyoz674P3t3le4IjOk;i=G0qKc<-!- zz!E@BQK^p361ngQ$wMk@EfbQ z=Uggioex^?#26~0`Y_JXE0?V+H4tU?L$Epvjp3+J)rS?s0N z%AAaO)#^i=(7q8(#V^+Olr~-OTeNY}oE_$io=HU=J9NxN&E2DquBbBq?rBflKeF%4 z=AG(-hP^QU7hM_fkyS@_N{ZYW$lOuBN>=97Jk&Wvs+(i?I~s1Lgy&HLMt8K7giI`3 z1d4)M(&VVz8NpAp$s-TObsbl<;OY6xxBdRbqYXR6&&q`p=!t?6otNA_^@*oeH-7&I zYe4O^+K+T;hD+!L^Xw*-_Gz4nyA#8srcL0S6KuIiHlZkqEVd>s*;fncK3PrF z>5!5tzV9&j-UVCle`tew`m5xn(?|BM)uCxl`&Ob^i-j|%ZQAnKb*DZN%lmF`+PPc% z-YxPv5vM$CE^sw;b{8zGxw;w#N1AKQ1zSl2ebn8}+<-=ibh=hSh8YFZ(e@dAIepQ( zRY-{mJqeC4*fH7d;^RS_Y@;n2s1z9E4c{%{7n0 zL3XbL@$z?nt(<#j@1D1h&A(yX$fk8_H)>S7PE+s7gJ1L>HM&>#kt2H4Y23I@R!$CP z1uek!98R@z#toAHBMx&d{mOZ{mZ40^4U$hBd5k!roQ^h9VAvhX2DGcdiZOeZzUFEb zd|sRve_KaTWpUU#N3@Euy1T2OKVm)X#*W51e^jT=#J7mrG~(>SmiA7`gldtgn)tBN zMwcCBQp@&|x=70oH$+F^hXU8gXZFXZ+R2BzbHn>E1QnSAijk)@YbePaE!w@K09QKfI*)I&@RHmcqDfl2eXFSmYbclV%yQ;S|Ga;>{* zZ0e|I3X*DNcXx?ZO9pNkv3mOEK~1_%y`k~ig47L1QZ9d++^tcEUN^TJ7j2r)_84<( z@1*Wy#`L27*B?Dg3qaunWUfu{SK-Vxs@9LF3Rje639UY82}s^qy)%tyN=NdS<9~QF zh6zNWi8Dt|o^|*WQRB~ex}5mbq2Sl(jeBw3_HA3TH^2Bg&1p51*mEB0^<5n@rJ_7_ zDkhY;ZEj%|O!fLxt`_!EsWeGHuGJ+jNsFkOurJ~ylP)rs8X6@UX39&5Q(&^lYi#S( zqtoK*w!ndBTR_S%_dnO>IBjw>au-(2f`v37XOGVzY@NUmTWo<^95u7KrDrv}cFQGJM53}t@nI-_iVI+6;UUhS)=5ttpfmzF++ z#*59YShEIh%b5^-7_AMTG)D!y2BR^eIiPGV?p}++$E#jTQ0mPZx`&D82r~%JLeePl~lxV(>XdpCr&n-^CWO zuzX-D`b4a9*tO29jGMOD>Wnt*quV)%$4X1RW+l-)xY?X#y=ATnjsZbtnR9}P!*$Y!a@A&861Gr|+99x_Yg*-HR7qE^fWidI)*x7sb$yVt5p2qHUI&$MSaoKwFruJ(+Tg`5r^SgkL zsJ5M|BYoiIu8!TUSSLcnFk_Q8de!nWe+(C$!Bt+!f!PG~G0C9dx%6Ikvd)*b2N}8XYCNP!$CB zWK?*V78aVNZByOD$c{+K-rF`MKcyfA)SZuj@@hrACPUU0||o`DC+oJvcLR$40Fk2#nil!?n{WojMfS_unLF+4le!};Li z@TL2*eeeaKR4@i0F-+PVPpvFs95iNFvtHEtu(r#9qQ@+ z)HT$5pJOy`AaLUy>i$=Z0`3%{2pz2m4JW4QVqXIwP?U!f0U!RzfPr_Zv1`x@)GVVzoA72ldEn@4HU!lJ7ye_b}SZ#?t?Kv>urO zMO11R-8LparXXf~4Az|Y5H?L5C8V^C%a1FF8y|;F1HTl~eiO3EQb`HV2G4h=IX=SX zt4yvPq6;-eS=yzzNdDTdV>eGsy{2Uas&7GOP2ilWt)Ny4XRXZH@tnr zs*opSy*!5dIen-Tbp2BemQJO>R&$su7zHJ>L8&u01yCwzUh6q*dAL}R*ZPD1I~gOX z@lP@aF5W~iW8#%EhIXn$#*E(k%xq7I5;5-{kSqI+}QiMb@qIrJTBg=N}=+miluP>vz zwO!_0Tim;o@RW?e<*iLyw9KhA#Z0T*Yyd4eurLwdC{{E zxH6pYWc`S6rP%LmhIdL_wVb$^a}+K;eiL~T7=I=CK6c*_E?D0iB5Bkgbr38-1!FTQb&BQN%_Vq1DNnlrjQ$m@}~thlQgM2n*&l5GydL zSqJC8aS4!#Bme(_rC$E_c@et3D* z>U8UKGyUa)Nq*7oy~dx2{Jp<_gG(2`{W0ek>&r&9MU?~C)nsd>u`N-7dV9!RH5?DB zvx{OLruu)SMJGXvHr>b@Mb9ai|7NY#;?=iAd%vs1I&k#x&eMPY@u{a|w{_^P=0~ml zewX{qhJN3%%|O#Pw#l<%Q1WJ-1KpbdFWlMWZyo~ z`|ZZ>h+YStw_bcB>5Ry*KKr50@;~N_=uhRyT^<^F3ZnYxJR_s}IA7^D6zwulBWLa> zK882Xq4$%+rAAl}Cc$Icoo8%zsNpU<_*~DV#~wVp+4`tsn`f<0x0+9hy5csG^|ZhQ zwtg|@x$!$cG2b|1y)xp!iofEX7roxO_JrvB((~4?chU0|ta7&6W9V2@13HE<9>YY= zx)oZ5~*ijI)NM<#=)>26Ru zTwLzbV(j2s?+HvlHSfqz1MakTo7WeMeQig!>o$1W@X0S1KL6z4V%+9V6HEV)f(YT{tTTnnJk8;etCVKj-Pp(#kgm%{XGT^Y zpDjosL5zQGaGz_cwPxFA4_oI--h8*knjM=CZ+*TSV*E?3*_AJpYz*9g>)wGcEPHg> zJ=in3)?s8TE#k;cKgn!-MyPeWswLx`0a*g9r=iOs8+ZnVED*w>2~h$N-ItrFyBvPB zckj92K(D#wQgD>mns)Gz^}4ujh3jHzGjn!*&~J>j4n4gV;Y=RxuVnWbLAz+$1j?** zGLGLTxY7tt5~pnR__+Ha5?LIA9)TNn=vS>o<%D+N#)Y1}NuVP6AvF~d3Ry7NRWG=z z;pq9tf1UT&<}=TAzyFn`+m=l0H>Qdxa;3d*ZEa$j)`c^V2j&*}M?Sk^!G_%HF#3+5 zW&-blOx6`?*nIravOD-4EePy&Bzq5SO%j5817@3G6W1}f_tL8#TrkSo(+Tr}_p}iU zd4@aewH0)Qg%<754Y3@EpM&M_i`1Stgx)Sr-YD8OIX}4|8Kx6-r_>WLhPbtc!&ZV& zspj}tnS>B4F}Q5Z)?t#bqHvlQN~|wRcJ=Jit^30TU7OFTpI-mATRLYy(4cw)Ps!+e z?iroivQ>_V?=h}xlR8y;-QKkcj(=ecwN{CI<2m>iaMuu$SEVPP4H3K({w6KsBk@tLlxXf$Mcgpi+T*^_y$<#DwT=G%bQuRjcNB^b;)YLRHC7{N z=i;tUr_Zob3Wd*SAZ}7IB7hEA6Va>81O0~G%(74&ZFl%W%)HcOXVmA%$6u*5Y3q9z z?)h}VA0p)uE8t&yfA-AxO8P!9>y8BrAAIZ8!6NC~Cw`rcG}-g-erx&tt@dV%AE#~K zyl3~ag(bM7sKn|amKevNdE?G_JBQrf$#5?ML~pl-P!fhA z8elKaWr$uE45epl*{JmAF*u)UyVn9wkH0k@bvQAek-*c4@#NRg5K1U8L@weA1KsO@ zV>D7pofjH@5~;x;bL7fP?b4Y)Eq zhk*+X$&?PRuM)3ev{+fpvHt$uzTjPw3>K=dgOq-7QV8fzsD2Kf_8k#9Z7a z<2BIk3g2J0hVti3`i!TW-jBfwMLdx=iaU6}3)8Y7j2c za2TZinDh!@c39l7C3Z)J&rZzAx#7V~;X^YSF%(L|k=k8T-neVDds&aNZPuvh7J zyLs5Fbf*I}WX?t)caF+(iI7z)%Z03br|f~g@5qLOJftcV+$_kAv667%SPgYMK%RS^ z^-W-HV6I3%SA1^Hxz*PBCsxg$_n2w6v3|4;FA=eCid4~Ni4}P2g*V^Y*WkHhukQiP zTViE@f*v2Ku_-l^Hv`cZD#*V%x?Dd6Km5~d5bWeC@zlF?%Khsh%zp;>s|#Wu@tFS@ zf3loE{OlyF;H}fLO1b+r3FsT`FGWc+R?;6M4V^2kwg}nF{c7RsK}-bNTT)_{OsJ(b zc6SN9dEk!3_XA`7M}H4IbkC|zU7xsnp_%xnb$rR)UL*Ma4C|zIDQe40^Q^oDFVnfy z2je#o>8pm0F=-Bal0$cD&`J-F5VbCD%;Hax%~~SsiokLZusJBZ%%JNBC9|=0$qQ+A z7<{+)jQj2*cYSA_yZc{{ANtdK#y4%~^m+4kj?EuaGD=)yRa$#_!I?b;Q=au#*-^r0 z;xD*&%7DdF6?B$;+34n4ir(2d=WOSwA?x%PqO%UGc-`ba^Oxg)u3`^GSjWjcZuDPC ztd#{(j4&-n&GYCk9&i4HV{^9`20lo<{iS=~xht^vo)ulXEStI5%q$Krnrha&WE4HP zD7adzIXFk0n)w=?YWY@wcN!>N1%0jO`_tt;koKuYgNztHhpE(DSg29xn!r?h6TjC@ z$vLD&J4|PPLK3I!q{=TVMjU%3u=r{_O%Z#BdB_o`4QH!`xjb3tT<&u&=K^U2kzQx}!4DLybq zd~(lgn0rrf*-e;xJS1>uzniTaj7Cd|h`IB0u^pZBqMOrNI2RC(?!Eg<_8JP1K zGQ2(K{`}~7qQo&v1n{XuRDDBx#HK~p#5tN zcX#4QcNIjsE6MxJD$y}4r84u8S_l?q>H{y4S`}qu`@L~o)?beQVdrL;;4}$`?hZL8 zwQ-6prqNcm?^dL1RGYawjel#-?%M;Ol)ru7YiDErcx1-1PMw!eTW%(<4KBRPGCZG7 zoM-*~OIGp0X;#`a)}r$wv=?yCtG+NZE;m83`A1 z!)1`=$>r>#Vh}PZ57k zeuH*OKX6Mq%!4BJv;0v~|Ami0ohcC!Wu@}^t3wn_-U?~VNI1{?IGq5>Hr{ejTtYFSqEvC36dRzWUR-Ig=mj*lER- z*=A-?TmwmvaL%%>ty$9c(G?j}ME3copPwh*oAn&Dp&?jczvnre#1)>UT%KW}vu?Ug zq#C4R6I2g0$Z1MatpUfe@KbdpW~Nl8x>|Dt6qw-1ZGx(jA&EkYX)-DRD-dh+5MK?Mca)2p!NH z!}2DU|A9oo-ODh=e7UIhNz60vxI`UlXU+^KSccFaP*SOn zSv+jgg?>*mR}`Ydp^9!__3cb^r4ZnvdwTs0-0WG=sT`1`b84!I42JMUWC>M7wt+uM z%>j+{Dxjers=e@Ua!uSq;CbBY9BWoC4aQh=@3TsAZ|~IL`Euf5X*-dcA}s6R0l01v zIeyLh^CFrQV}DVk=Y7nL>iQ9nmY!RL6P4$dSxE7dPDC-KX^fkZ;^l9+vDD#ABdge9 zNn^|4ZYt?NL;Ym3-6tt)N?_%w;Q3Qe1_Dovn>=MC+HarOy7P$m#+78PD7dLF@`$P8 z44IQ@ygZ=|)-(tDkClv?GJQ(KJYwjzrW< z#8Z`oQC6~mujja+yZT81pngp+u{YQ2l)nC zxGJkY+?Sj94^o7ws~L@6LC0XtCc-*3LooH-zr!YqhwPo_Pmno&@RfOO&QrlG-tbD| z;tB_LYP(RW;zBV~TQnH;kEb8$^rM)b#kAK8o5unthP zsUp6K8%HhqW0Ij4ff4DdC;aAr=_RrAHF3?R&DQbP+kU(8osZrB?bhL= z*B`gu*lI4a+5~^^^6Mq5@Ba^GC_*1%4O3wiRWzm`dz+3w12=5(ie0JfF@s?T>2${I z3bb1EqhdUJ5-K7AFT8H3WTvE7lI5{sJpoHe`7lVFOqN>*SIsA5??@w5?V%iV;`dYc ziou@1EjKUg7C3Y$@V)y@E4a`q3br=)O&B(}^s@WStxK0;?=-|5N|+COJKUSfHdlKe zCk@99AdnqHg!xwzPb^Cw1xHU-nBVrcW{%>T+#J`7$1i{Ifz`%+pVjiXE2;E1SJGBd z_vlgUV|eO*2>oKt^R$O`S>!hyUB4r4MP+XnkrAn|aNU-<0TpAx^RyGl50!C(5-41b zyFW?#AUNZsCEy|Mt7c=cxaJU-=OKhZ(Q~@OyGA0*BbtENv48)^A2)7W|C)ZitRLQg z-|zRdw=VA4ywwWWcc6;Yt(*4U`&;qpfM($ z=C-bSb}KaeMlnzn;*=+x38&dPsG)XCC{(OxiJL`LJRf7Va{uf(jOSU9fj(opcsq0% z&sTs4d-_`Ac|A!p;|I}N(TqZyp-C$Hu-9f`6t1y2!{4L$k?2&|V20 zdI!*V`)K46_u5ci7+T|wvZ40_dY+F)E};uUBf=xs(B)2Z5{0C>C(TFP(S%3787t4+ zfzV(Dd%R=N{k1yta*mvxXc^`=K3r+MHU0s*NPH58W&?ttYeqm5FH78OjZeeS8h5P- zXd11A-fW!4Xl<{v#+?xXO_g6e!LLGmxc0TNv zlRVzmJYL!XTE^(4h;sH=2BSLsvey9i&T@9f4(vXgc5K0tp?l3gKoOTZ)dmqT z!p7(|*7mv*-?dIy&v^~IIbU4y?uHStM>TBWG`hE#&r?0l#-~eKN-D^C-K^)e(fHbE zdIfy;>5@Dr{0s?CQpxxs^gXDVZ?$s$ zdS2Jig`wVnwpUO?hh@-oSc!i#Z}jC+Z@!6o)KmPqs!i2=?9A4@2K@j*Yk-tuH8ZVa z{S$PNxJj-7PN!-S7-qxQSc^oed^<8})d(4eF%pc|DD0pyvYtzPmv)*X``Br$?Rd{% ztWr0cV7w|?s@ZF8L+M7mzdbtvO>)a?Oz7seIEH9n;YNPOSf z_GP7WTOVqy1>5L-U*lw9i5{N~1;(v~#1bI9Tm;(BX`r=)bei7o=BG1%(s zx(vT(Lv*3o>Q8IYl3$;IgRqR4rLosT0bf03Br`dc4O1Q8ROb({EwgdgrS!VyQ) zrR8v)z_Endt}myBVHjw)1cRFul!TFZdTKmtjn{z38BKR;SP~=c zb;5|fj+_G`O?Pgz(JA4wF6Z~a|*J@gsvoNZ5E6^(g=xKnhwVx%edI3#9T55i3AkriGKjhjy) z*zhZ?zV4ooRaK1p{fUm+t_T^$t9aGjtfZH}vJ^*^vz=9ml;h)YCYPXy0#{VTI5scp zaU&z-R7hNAPv+Uh#jfvzTg{iohqvE?;BwdQU{ABZxzK8NMDMow1)h?l7Lqa%kFxII zcvLOp8siy%VuOYxvyi)8+m=~;r-q8vD0$0bSvd^+W+J;g5v+BHVSn99oUvIqdrx}Z z?7E0m?XP>hF8Ty5t&3PyLEZ86=xp5({@GnE?3W=YLd~#n^4F4B&>SfuxLXGoi9`x- z$O;*YPfC=btUPWkgt}VSrRyh#DbnY-b=fO}TCK~inflaN>-AChB#PrvlU^vy(F7XP zH%=s_ZtyF%^d zLP)#ceI%u*MKrWPoZDJTvG?9-+4V(iEtc{105qgjJH0Ekewk&zTVigkF|RlNxC&-9 z4#w=E{U$N5pL`|ex)GR3{>d?K5SzmCPmfuRLt>`=Mwn^8Wrp{g9<{Pl3WEXoZpn`U z+~@+lPkdu70^Q}zpHW!Mrr5Rjl0<_>ThAUBBn-NJv;j;5o0eaP*M_w)qNVaju!iir z8PKr8nDS|27-2-|j%{m(p2a44*p~wsY58vswGeHTWYJy)pL~C)7|=63X8}!?g3s6- zsuQ|I7}43lmTRG71jABQVw@0a5|*mkmtflyp%(#to98KwaYCp%ZnCw{C9U-<=q~|1 z+D8&r;$9yr49{QVR-=6f&^LI#)VSA&`d`KVu%V4KYoI9hlt|n-B{u!$#i|%qU-7W# zBRuVe)xy(L7ZdEXT3Is4BEP*5gpR=*G}5-b_eX9yRRmYfUJ+z%mm$GH~NL z;r1E5&2s#_A$H6*AKCxc%5_7wyVgwEn@_83OMA+6Gsp2Pk?BUO9I6}oLQ#+<8i_Zx zUsqV2JQaDC**5pBu|DUqWV@TOz5x?}xw4`47M|0YmdV}%H1`1ZBBNe-EZWCt+i=E0 zP<({%5sbW#4P9A0qQ=79M0M1*eTb9CixD| z7urmAwAW=r;60(??x3O-CpIctu}6rbVA@G`Azu90(%g2u_cAJmX$6hap!8>Tf|7B5vDZZV9wRXlFA_$a?WkRK!jhcQn638ig?vf^Gxpzf0hx<+>R~l^9bviv2S4u= zx`5~1aTA;a9=qj1YZ<`@&pP4pypi<)!7~NIBQG|#XbLD#rKCW3yv5i;a{kKS*)Uj2 z@h{8>H0RZuYOMtJsNh9&lBYeMLmlGPdy7ZY(CXxb$E+SgqjJDo?aj?$cw@S8TAl>p zvJzfx;0Fg48;^(StM|IHP@=G6S*V}ZZxzQpq|r$aaRZE|1!LKaxPq&c{TWH(8C6S}+As`Cs8pW;h7 zQj!P~-+JRrc)psS)Y~Oyk_d#EB*I)!+vX_klTy$UH$|T$?v0|0j6PwksX9i*IK8AZ zNt_#vGw=%|Mg^}~7)s(I=^}A$;n;>PT{J}z>7lDqu>!#AzScw1MPl7ze2kGwtbpRQ zcmaLjLH8@r6Vj3OQq^%sEvJpr5|LkpuO(Bi6z-#25ltK1-0f|5w8OAMUs6YA2?c$* z(PSbD#>&_hHH=(j93ggzu%-KzdH1%?SH7WUA;bEtbj8D>HoQPh9zWB%-k?0`8@j2p zi2d1hthC9pRidfu4cL)W;T^hwjQVus##eTXUOUHV^qdl-Mjw$V&*wM!x;&0KAjKA~ zM&a^6nv5gA7cL@BI;dG@BGS)8bAj6n|G2&m8%E1K<6bX$F0gHPn}9Zh7Tq%9-ggg~ zuLQfiJIfV)`3V zFJk%MhDTC_)}eYN+Ky0K48)%60G|Xm+t5|Qv()=cK}*~z7>mTcK@^I%5xbIDS?zen z&AuavdxN09C1v-pf|j^RYD?V3#%hck@&;+-%2O*xOLQZQIQ^hpL_mbjLnIIIbpQnX z?O3%U^Ci(iy;Dn7<@fFe?frgUIjS-7me>%c&lT6QVZ;4youaln**2{luM?#t1NVp7!!t8^-4P0 zFQL`ALto&{&3d+frYTG@S1sG0xEN z8;w$E;bq~mE9veHO^GKYJZChht?hO-{P&*EMh zwfKhk8CefS;c>9-Oyf}TM(ihZh=%qWMag;`Y)=xbiZ?z3*nwV**~Yv!ME!ehPSTiF zyb=1OIg&RG+t$+l!Y!5cL5q0p{XlRyo8}S5k{;-k)Dypr@X`xmPROHH&5m90h{t`F$B+9 zj<3*I7oM}mSNn8HL&=f1Vm{S*KBw&YJQto1pDQ#UrRkN0E`rZYkCx9Yf>yFzLetC$ zH)eK|hSiwW`Npt3ZaI!ZMBnsUY3OjQ(1W#V#^e5g$6X{EhDX9DEsaFwILLjCXt}-f zCyb!}wybUGT^%WZ+%_!foU6i^H8k%cc%P%;Tc-|O6bGDql{jETCbROS^Nat7aXHSy9a(z08AMs9Yn4xBI~iNFsX ziLNAx_~aUi*-MUfa&<@Ag$#pr9i`V*b6D6%0DPWOjciiaqM@0ep*>82u1uX+uNre9)+Al|e-Lx2l1j1Dk}6273L1R$Ttu|I8k7xTeQ+TcM zXZU?uSSiot6brQB_x;PJS(5dHz5cAxiLdyLMKPLYT0>JF<0+PKK|*4NfMvQTSgRBc z_rvlz#cst)*Fxo$RIA5Y6LFgZ19sk|U>Au2=QZgW^1Ln*^TKn{XNH>j6s-TPJPttf zIHu|Kzm+K>u@uAiuVh*v4w_I?Fo^u&m7@ z;(+xki=%iDkHq1TtQ`!6EeW1O9rl#7g~4-R3&SYxs^6|xNXhitR#ztuc;H$X6z7rJ z2tTHfGIL%G;>=8C)UXVDq7+*+U02FKvZf_=xTdle1OtTo@jPzIG>PxA}uop(1 zEP5x?F~d$h_>lO_QT`d+Vas|2+D z9`B=_R!!PR*6$P}=I4kJV}I?m>htpe-c#)D2rP(M^7DqMvk8TsLY{ULZ+in#m=yW^ zUks0*o|gQ)^(grqULU~w@!mi9`92%|7k<7V^r!i$cNOp-VruOQ)d>BJnhfSE9fw4Q zAo~5l8EI!D^W5M;_tM<3_7qpy$e%OyXJn_~d(0Bwvj>Pjw^847Jkv`NaeTj#X35`U zmJ*-+J>s46v$v4HhX!kZPti9oMbq$^<3@-(cqt|&zjuGg-#6vYz{(zcFM0g&J$v5x z`_}v!qjdkspIg!A(0-iY|4nB7f9uJkZtv51wO?YY?-ZaA$^PW*5~Vi}hxa*_VK5r# zO*^rlR3t-5<$JAL++K!x&4#HZVUT^GtgvUW!nb%E*eeV^jQ^S51o@*lbv8y5<=p@X z#NNTxyQ&2g2e8Z`Z)kP$cT*$gENNpd0ma5~rxyGMigF|l5gsqa`S5ueD&y1ndz^Fl zoQhrfDSsY}5elP?b-jDEyF1=f=udW3|L7NzkYl}r-hPE*%~luu&S?3@6;nu0a@_gq zPnJ(n0~b;KpN!uhGSBmRHx-7)V>j8dO7EtW)9eQaRhaUg{hIQHw+9roCPd}aCAB2YnZ~@@pfPhq zhFooUyOgFP*RrXNk+>!<6tLG@`9`r@ybZlg0Q0WIV>Uo#vg8R>eL*|AC)bu>Z{S`3 zp(=)Z{pWF=%Wj4*5_7{j9&BNg|CsehtUkx_&=ocdokbEI_o3-v5iK}hj)tg%->eo5 zd&yrux*C~HxV*qoa8Cuoj`jim42Grq%pz-Wn`YLgs&h2mL<62$GM_Xh)lJ=as0E|y zKjvDzz*=|PnH)pOU_I< ze($6?g#~@ajh^$=guSPV-hFV!1LLr&Wu)1EElrR~;yT092$>|nw;D3N5q&nLw8YvV zpNO&}r*?C#Gw`$p=*>y1ysE5lk6t&GxY{*z?KSloF$Hmozo z#(ct`rKG2^#qrw^3&q&hi)!Jw@##x%Qztv1uaN7M=c3MqvT~g$k;32a)o*vodBAhE znHQWzqbn=-%gA|y^TSzcb!K?Z!Q{`^^XK`PZIb5`P#Whm&Bk~PyUDiucs3-R)NIPi zKz9Ix9%xNioU_6Vo_&a3wXbEPaQgCwwSfEu=nU)V??HKRk|SR{ptRfXbV%Pvf6>7)u04sy1x6B@jSJ68gUeo_T_bqxsw?c-j{4867MTtJ@h_AA*n`d zh=fL5TjOSEqKbkpi@ON@RP~c0_c6wuSZYgj!EMb}bWPY@1g`P-*Ov;+>{ys_2cP{7`*e zrJ|hIaCvd}h#mp2A!V%Dy&l2Oqg{WR?HJArxCT;H=&;d$gpL&`Zbsff_^+3S z&bi(~gg=w+WrptPp^nku#NEi@o9xKpD-N|q4xcGtCLQC?37A*FOP)jM$$0m6_w}N@;$nTyC8~;L>c!%& z=s5xS$`ao7ZRl$S&uQee|H9cI`BsmpQIOu8$p$(Pb4$&4jMXE0D0CUlhok2n%nNkB z-fHQd+rmnb)D<5yBD6HwV$^Lt;h2*sFQb8N(266XcM02TWy@R zig@0gpO>{^YFGt;quzk%BJ!F4iJAM-V(o@A}`W(gHmsZjx3K_kXHimo;GU``9&Q|F*(ZW*IP-&0#8X z9)Y_Ls47=g?w(q}R?nW<|5mlg4d^pp-TN8%aUf$ZG!BY*#gA+akYor~Fb{$@@wYo4 zBU{5G<U|-uZUvmR8Co`q(A^CwU70C~U zVix$p2~YUL@YjcabBFn$*nG&w-?S%!%CkCz z9d*6UvG?s?xqGebZo;6w@aW6a-M3?_ir_MSh(?!~R|Eb!v0Dhq~ z@`S*TMN&xXq@p5!=I?#T=b-QT{mIDvvnj3hh%le%_tW3gx=hyVqID%*lZ-C;zqtSK zRmAMRm)MPOP)lxO=NvfV;jGiRHjt@z16KV9o?S>{AEdEs+SNT>(pW;XCBv&OEtv_h zF51&+h)ALG;Yxjt@)o7`Fd>QammLt&9wN?{SPG5z!kn+qiEva}VkG$?G5-5{SBbrq z#=Zp?cI$nmu_GrjNzRF|pXVGy!tOJ+80&B<*c8+HP8c3%j{7&}&woh@<6<_ApCc?p z;Yz1VG9>;2=-ywkhvA~6@U42+vi?K+Dh$0EQh%T?#>h0jgF=;MG<4&vWiB}=+Ne=! zjj+b)l6vwj(5XFdd-mh+EQ~(OSnTd<|J_~T-JmtY^wYQxlGmTO%SGA|)%uZaw;qeo zWp;StY+Yt`gf6qg-Hvq`c^13`YRj`=o5v$s5l6ryqINXPfA2iNbmlQ_b1#KA7BI*J zQ+_nU)7@uxFG03ep0QulG}5G=s4VK;<)i@6{RkAhd!L9UnT1tTd= z@>XJR+53coc4BX8kDb<>^D99c@D6vl0nhQ;D><#-qHOKK7C3_Q`|u9%9r{tw-wDT^uhe}pObpD;zW z(X?&N-FU7#CfwPmDB5}&=OJG?rYMrVpd)y=59;b#DY`p(po*lsoafgXXPv_dv@CJrw>3SY<}%;68h;DqJ6Bemq3|Piz}?7bRe-$*%MyvbMSCqvGT`{~B}N~6HSDPpUalL-5{bQK2Q5o9cF@K?PndIjw43TNzj5a$;!@%uB^UG@l+M6> z#j*tNAX(C0$`Wvw`WyW&Ro4WI$4vAQ&%wt?Jm;Dwc@9}4Zkt!|_af-M7ch%$m{Fhr z_{jmY0=HB=#k>wY#M7>69NC^`t#wo|((|Fb2gX<{Jk`0v2Kpnxd#Xu2&syeg&3glS zBJYhgAq#Jl`X-%_fTwYKszNH*9!#z8h4U7v_X_P2VBwwgxZEX>kSeo)Fy6vvC1V7C zpTt>NR)a(~qOBXy8UHzRH{k?#_to6Z@`5xFM3-vI_0Hn{QzWA$_U6WCI4N!H+AcwN z6?-?JN)sa{X)UogAE0^LiQSH%NNUhIDY3Ue2P%C|YHC2QvG)RveK_;B#NML6=539= zs@An6b~+~|_LdmCJ|~^n9h7P9CEk|UTMp2??Zj^H9A0@oCxP9da}wRNutv-QSFEUt z#M}L8U3a*_9qMoNyHpHmAl6_jR~J9dTu=mmV2*;;YoO#E(dZ@i`8;7m z55${n4%4_*##UL}okY{C;8qzbWpQ^hppRaGTSZJIZj!um+}k{<$P=+gqQ{+U?@o!E zcwLTrn|n2Qz3jLZ9VPC18uw;!eNzRkaVve2adQ_T!i`g6h_1%c6jA+z24Y8V^2dk7w<{7L2t!1Ib9kz4^wIohU$^u!Za;BOgbkmL%7H7)T_c7eo0IurL4 z1dPf+kQOxQOoRvXuEaVMAk-S=TRYu;us!#DF|y#Dm}hpt8#@|LgzkXH&!1ftJyX-G zQo~#P#+pP!Yn8Jq)4aj##gwRJt#X~<8K0*a!(aLxKVJg)->ef@b8i`^RZ01v7 zJUJnTBI3zNBn~$#@tg7~lngYE#eH3}^D^S(DUNJobZ>RsHiew9S`~1WHATaHxN()c zGUvF^8JOCzXPI|=CF-yHZHD#p+298U4g|mYO4OUZbkPiV*TZ)nnRoPW)7^FJ{{6u> zt)z!0-#Od-|JrOs2);ey1tSi(^h zmz(ugK+|_~mi70!jT_CktokBKyk}*L_qK}thc9EphQ5JC(+RUrq5HDwl@YU#U`<5| z24kT{bSW?CQa+-|kRSFeaWAgdb2}K*$KAqlQwc8mOX3Uo!i1tW;ncHN)D)toIbeI~ z(W9jwee{snE}B`fW)16vIU)G4yY1FD-`pB7M+LhEqcQrOSi9>XTOvoV;(k2(vZO3C z`UD>56{Am3qYv-BlK(t<+&f337w5;H`~LfLhYyd11Fx!8ES6f|iBGI7_e)z>uihH` zmso2h2A>1E7xxgS}0EsfAJ5(I$*))Rm)B^wb_dJu20V zisKnz4W=}!MEJ1V8y=-UrkqS=J;6it*jq*9Q2thqYxwEFyBn;RE(doP-=tg(QbYu@2G;c9Gj5xeFYW5%?jzePmb-HvlM5sSiS(wQspXBoL@ zith&kmlwU(>H9hOz9oOI#GfC;=N_Ktp^;Qhq3`Klux5zE;@&y_JS=nxxjXc^8pA_Z zv%gQ_?+fwyckbtR4Tw(~>MOFN9kTRm+8U7T?nJo=J#3h^m=`Q~#NU<1Nd6vYZ<$V_ zr_k9A+Sj1G(dyKfon0tG{wy%sR)@~{Q~Acy&sY=hXP+Ko+EYc2vW9uv5F5kuN>_7( z265yyX003kHq2?eTfokcWH+8kh=-n z@?Grb@37TD@b5D`viuo-1;gjV7IiT^IEkO{;k`!vaUAfd1L`LGMp6-<85}gs|FIQCzIbc-CD~UGj~Sgx2@;rv;(iqhr}+0*GlC$&BRJJ z<2;sprd*L2hqzfJ%nIvEXO}Hd_VleNWQy=m@0kb z_-)@E;5pkd(`anZ>!eNfwchFMyMs)md3ipO^RiYUmP@MyKIG?h**C~^VEDfBxqC6g zOaIfCfal#{HbXS5pV~coH^6t6E4dqVbAQMdeN&qzlVEjA>87gZIfv160XT28Vbkr`J(*`|zSPwb7{^0ox=Dvp3 zV@^vC&kx!CcUCz2_27Ammh`Xqhz`7~%x3P>paWtm@OU7b+V-cBY^~-a+r1^vY%}7> z95sMls^l+DCfBE)UjZ{1FtA)J{!MdtKqP@ztMM8E3-jbzo?|L_mGa&BFZuZZe%=T? z@GZk%XRqmZ{JbXc9A)?+_VeESybox*h~eAV_+R7aeS!Y~!{2N_A4$)xs_?sv@|?oH z&cPP;iHW2{g0|6EYj}=P=?dP!cy@-SPzSjX&S}>lo|*Ct9E3IZ^akf*0S|&nGkda?~JB_f*I8&toK>V}#%O0eB|y^ABwJ41Vt>JYT}k z12+5@^jwY;6>nY|Cq7RBTnvB49+01@1{dE$65(@uj6Xo1sUpXQr|L$pIK!XkVZ2lM zGpv}1@8JcIpFK-%{ApYVLj(DT2Z4{B9iqnI8z>uq}8#$%BA49+S@CA-scO17ZtOH=SJ zTz|K&y5bB8Dv(n-xXkB-|KIS>WBh*1S-wAlpNQY?;eNSfZo%#2yl>YV!?)IFRboFj~qAj0pq|ef;O0!^YqF()PgWl0dm5Q}6m}#i}=59lPXrX|>|nryd=$ zFst?>L+01+n(_;vtmH7FJsOE zY)^VjHbEEQ8|QsvbR%T>u+eLq;^dgYtz7tjj;BH3aPUvMXe@9Tk-{-_5t@6kl_<`Z z*2Nt2t&YSm;Lza_b)#%6s%f#Fstj*66ZoV2N#*Gd~`3J?5F*X^JaNxQm&YgJ97g1 z#;FQkl{?Oof?#Y(F`)xPz!j=uz0k-!y?6)y!8>eT=y%U>a0%Dy!yf|sZC<8oiQ~iB4cWk@tn;5wuVOJR%{aO zSfA*Jo+r-SicP}J^R;`SV7@qy@!m~1BUo-)gIR9!Nkh+(7a9dRu$=;bFUvtx06l=pi5%~f-nV_-e9!v!``+>W6qOX!IO_4}F3}6312J7=9*Oy&Tuixf z<(?^bxZK~dZDU8qz8w2^T)(*D@<#c=<=-yKx zJYOlV(u7LSRytBSw(<>?msUPkWk8h|t5&Jnvg)a7(bcwBZ&1B?_5SHm=^5!G(zm2v zs&Pk+#WlXHS*~V-nj>mHQuB1pA2Q60su|5P*46UX>Qw7M?WEeX>a?r#VrF9I{h4R7 zhC|$bS~sii#Jaoc`Ra|V_hbF}^`F0{>NWRYbFRUF2HP9@8a8kEWWx^{UcT0KZIf$n zx^~I6FI^kTZkjzk`-$uqvkztm8r5mkzR}=D>l!`X=y;=G<7thzH2$GUT9X@_yq!}k zXK_=r>Exzca;xU9&OP6(U9&ySzRtTYZ+o8Ad|>mvE#g}AZLzFHsAb=l&tBKyx*6A< zYjsnr*IT!0?Qi35)4t8BHW%8CZ2L{Ss_n+K+tEI~{Y~v3ZU141Y8|F_INh;c$90`* zbh@Y0na+JWPwjlHOUo|%x&*p*>bj`w$!?9h6?A*(`qb-tUccn}qump`kLmt)kCYzs zdxUxp>iI#h4!vIJU9b0&-beE5=P%3;^qJo0@(tZ?c&x9h@5H`m`t|Sk)s16sioR)0 z|JeRh`@cNEHDLUJQv;tE)NjzS!Mz4=9DHF&gCX69%owt4$kRh-4E<@CXIPD44-Wg+ z%{SaU=jN>iqM%p7pn}l_6ANY*d@ww7`2WM)d&fsr?En9B%BE0~fQU#lfCZIir5Gs+ zC?Zl-swiEGM4F0#pn!l%QPc=hMWjjypdcloh5#WUgance$%bS%B%9q$L#5<=pK~^W zpm@EX`}qCw+mqLsQ|HXQ`^=m(dGF2GH!r-I{Z^;9mcCUyr0Wpt&{0Dt4V^hOdRXOQ z_Y7+^Y|5~s!~KSTF#OLE{YPvYv3ErD$OlJ$JaXa4Wh2**j2KmW)bvpoM&*q9<8A%z zpts+AJ7#pR(MQL$7!x+;#@Nwglg2$WZsR+acly4w`JEf%Cyk#u{`2vB|HE>4{K@g> z#;1?}J+yXcNa(=O4?{l<{VMd>yOwuHzWdp`-@g0PyC)_roAAqogbCRb{+!rh;?YT? zCS94VPChZE#gy>(M!t9Y{VwmX{h;~>+o#?&b<>A_ACCQS?uV;C{O!ZUX&t7Gnzm-z z?rHhcXHGvnqvMR(GrpR!c1G%qoR4aL)cm7oK3err$;`#G>dsp9aleoI&8|9o%Iuig zMRP*tteN}Z+}GzG__Wcd<325#*Ja++dB^7~^BSsqj zzxVTApRfD;_!pJGc>0T;Uu;=edEs*l_bj@9(cDGhi|mX0FaBcj`6ZQ>ys~8fmw{hS z{4(LIZeJZ*+I#7`rCDD;^YxeCRQ=|QZ(_b}`0dzlw|#p#?9s4cVLyaj`_A{fKHr^O zHgH+^vZCdam#2Q;;rn$fo?6ji#aAmbRyJE1x-w%`=T+-h*IT`PO{+EWYhPHqckPWI z2K})2hn#iqtczdg`0<$^r>}3eK5YHy4K-b=>sX zrZbxxZ(g=JdduBgLbuG`61HX2mIGVjw>Y+3+uD6=#MaWE2mHMM=bUZrw}oxX2yYiY zB0M=ffBU%Y2@zUEjfe*#T19k;=pQjIVrIlw5gQ}6N9>6>7!ebZ7-8M<{ElyS9NyVz z=X*QP?|N<555M^TGVYi3UkCg;^jG`t2D=ySPT$ja&#~WXu>}9NWUsn+*xsM^RoT~Q zUz>eB_YK|m;l73YmhD@=Z~wl;eR=z}{k8W$w*STbZ|48lL4j#C0Ap1Z;q+ew1$QF?=L=K9a7`Y&FZRC!~6Oq=)!YD1O zPSoR39i#e3y&LsO)Uv4UQHP=~M0pMlIA|QIcBs*zHivo~8hL2Op~Z*R9NKZ{@S&7L zd56`*wGKBw-1+dXBd;78eq_p#FOK|hz09}7Qr^w^nWX~&AAeWI&J*N=WG zx^wh^=n2uEL@$fp9vu^Hi!M5DIbQ8}z2nV~w?E$N_^{(29$#{N#qo{DcO5@?{Os|p zdX&siN4*vG>F_ift3yGj?R` zjM%SZH^&}|O^J1$)=uAj`q9%Nr~8~9bNb`cVW-1SN1skRT^u(s?wz>Vam(Yj#O;qe z8+SQg#E*}k6Tdt@B0eVmTzp>qjWadQJbLEoGcTVRdS?8YIcJufS$`(tOw5^cXPjql zB-BWFD4}&ir-auMMkUNhSdg$h;irWC2{8%j37)gQXKSBrdUny-^=J2;J$p7cQAxZf zv0-Af#5ReY6MH4TkvKYWPU07dUnj0e{4sHB;;zK~iANJ-6VD~4C1xk)CzhP6dhUU9 zEzfm1H{jgZb92uvJonAHmFLzcRZDs}seMwfq+v-@k`^ZYm|P>dZt|1KA;}$+2P98O zo|^n|@`B_qlQ$-BPu`P!FgYgqO!9?fTk_@Pg5*EX2c55VKKT6P^V84IIsg6n9q0F+ zKYTv*{H60(F9cn9=tA2IuU;5?VfKaZFYLGwb0O`*^%TF9+9?fFTBmePc|B!l%G8u4 zDH~EEQ=(JiQXDBpYUR{>QX8ces1TQV*x5rWU4NwW`*8 zt<9}3Si4(CSZ7$jvHom5YPDJm(<-GkOlyPcx(Q*1 zS5_vioO`$bLV2LH76ATeG9H&t|7* z=VkwH*X)7zI`)U`&F$aVZ#Y66(;TTe&*aR_Id{3m<@uMFTwZbcr_0+e|9Uy{a`ff6 z%gLA1FFP*(nOixxVQ%Z(p1E)5&dU8RcYAJ3t}XY^ylQz5=XJ>Imp44`-Mr~}^YgyW zTa&jlFDma;-o-pm-gT#+vxYO+*~Hn(+0ogggKgn(F%4wZOI3waK;5mEcNoWx8@*g>HX$HTS*l2JVO5kGWgBJGi^J zlRaHLn>{-`k)AW2Oi!-oN`YDsP*A(z!GgyN+7`T2@LIu;f_Dn07R)X9vS1~#qKp{T zM%A|%$324sLrTQGelmg-M?BS<$@HAk%X#(w8|VB7`pIbBpTN@mX`hQ>-Yv3_tp>oi zn~T&iFL;0H0U-VK>h?#1FXspc_b@a*nAJ;>ouDY84;~XMB)gBQl z*S`%mfS#Z`coNJ4lR+mi3Jg;36CW!M(M+!<+UZlp68#Y|Q_B(|zKz8IOK(x#QbjDa zv=$>QZNyT2E*Q@K1TosOO)OQP6F=zhil&yz9PcBR`trYiEI)&}Twl*j-_%PvmndG+ zQ@O6Cm~Lq*s`J%G3rmoASkDl?`WexK9)#IiUr~d!iZ(_(rF}0nb*XraJp1dj#5BDH z<)00@ir#7xUwX_Jy?E{iYGpB9Rg7JFQ_{-fbDt|*J5hX~FXUQz-)Ew`8c*6lbX7MB zKYfCDPb(2MErp_i?j!EgvPBirUg|c{N>#L23Vi}WzDP#-25s@+8c%U5DC*Ua$UDTevB5d+jJ z;xo%U(Nm2#vMdWkUwtL_trOFE3-3L~J9p_JqMv3LOSNz@L9L789d`8OpA&1NPvgTgPRAi@>2-X*i zv6k&(Iq3@)w|L32Np!Rf5M$uwYx*(qjUK=?^F$w=_LN>*yd}%7uMrc~O=6=yTGZ6< zf(J$Ld!Cr7w}j`5K~v=PO)(SNb+p8Y?mkP!2z`f`qJL=EeBKpLSV~1hOFuEnZ=PuC zw@57a86Y0xxnElR#9|*Galh|c@=X$5EE|x|jmTsr(b{J)borBO=8J{C%SD*)pCZhs zwy5RvC-w0ai!D9G7~ei(wf;Nhoh}CGi^L+WznG(65fAxH5%u+&q8ogetG@$3mWc*F z8tJ#}a}0TFWI0P6&$2&44E9+l=K46r3=1>&`HbhfQ6NP0^qGzBg^Gvtca2o``}mv@ z^?kCqXTIT*`S^B07P=!(BaIAnGt5U3bJ5MZ(4vy>b7G6n(_)2BK6J5*kNqDPKTwB7 zdNB6E15Kui!O*a~rM3u?G%w%x?L?j(#ia7QIX?pV9jF^(9Q++o#_JE^^N&JYrHAio zH=jqKTOGnQ2;`$fhQ;C>C-XnSTTiBb2_0Hlx-^bujsCdwF71^?i!InwL!9+0$ z`!d_-e)xFUI7$EhV*P#cZp^jy(1C3{6FKsEio9#{j2U97&-;`CJ3v>IZpR!6kg{KVt>Akkj`P7G8}ih0^qvB0vP zHyMwM{=Pqnm(&SjtopX-Lmo}BHBT{~_cnHHuumv*xm$$!eMLPQv;8@I`yBgLf-QVf zG}hNrpEr<^>7ubEON3&dUiNE-?E8yxmKx$4=-ChYNVy!~vld&jL=3TfBi8E|D9a~e zr5+)k@#`esP@5Xtu)o7iUs6qkNPnVN5%;SD_=-D>Iu(dc$WK#!gQ%tN6HR>^d10w0 zn)>`I-(7%yEkr-Rr>LLTj?N_Qs@KEz)#JJKL~Fe+_M#rNt|wZ6w%|GN4d}=7I-oxm z{S9%qk1zH5Q21kSM(WRq`>AJJeIT|XO1zBSmT7lAO-#WqOwsnRKY;X{xF1_TN59Da zdgyqRRNgm2w9?iIfAcz-r@WU`3**`6x#nJEVTI^n=_7jRS;*=Vo;Oo;_w9qtd0kAg ze9L|^*S*VjKkP;uQ5zY5j%%lwWje`uWZp7}{m(@Q?7E;{=?OdHUbA$>E;dHbkX3YbIW~C~{FdeGjy;-! zpYc~M_?0!xpCYcW&?}3ndS&Bf(#aB%hVq<5*>mprZ0yt+%UUp(=Vg#);ydbyZ}pGG zJNoM)P(L7^(#K%4nu+ISUdj$J&?k&C?8Juz8R68uH5d(=fDvFQcnXXM{eX;tE=gZ# zq7Ly^9pjoxYStr94AGwuZJ_rw{aN@b^Zp8~L4KBUY!%zYB*Y^UvQ8;u+@&XTd#oj~km#$7V@Dueo*aU6g9 z1L;W+c{ApcaaTpic&h@;cuU4q{{$Il$@)})THDwu;|$VY!RDLsm5i|}g4jyNSLKkg zm4vL5eBR$uSudU^W6L|H#GA-z?4&vG>#7V@qQZCH+mKc+IEajR7W$CMCx<-6%rVjDB#?_=kZuGkq^_7YO4_N4 zkT$#`q>ZomEb=z(PkB03jIQ{c@@*ZPejBj!Ja-A$4E9r&yTLT+2TXq;ZB+%B<&$On zYdNJ~DTnkI6(Q+Tj*4}tuxIlWm~C7eXg{~vds9&(y`QL8E-_W zn~YE8dD%uY^#eYYx67o={E!%an4TxPb+kwR2@|tp4{%b1hE6&sKxDcs*v z(p}c`Zz=5?vkgMK#%z1grYp#?R&QWa4rlBDAB6hjGZ)dfbPwX>I zUrifjC%(VjCzksqFORB1>lcx01$%NutzDW~Mzfmup2z7ccn6>HF1{mBPcPkK+7|TB z?EjJSBKv`4TINs7ePelA<{RI?sZZt4mby-#kg3aFUpL{VZ!7a%W~@U0kl9C5mX_;q zxev0ODy?hU{kp~@mfA*3wUyCYKVf{Ozh9c8yNq?_Ibu+0?@b$O+IQl4VtX0q-?ZB@ z{+BkntgT@B9=1(CA=?7jZ~VXhER#*RYXLH?C>GE-H}$HKHdnx9O-k^DGMbn<@LY11KUpHqnFWwcMpa}9#^C!>A%*yxW$qGL+mz(=~8GB@T%%#G`H;+lt^Db~#(h7=8 zx%6I8ey4_{RivF1u(C_M7eaMOIQcV)=G~NQQ#NJo{{EAvym!fhtMH(F|8LiNS;ezt z)lHhPmO2KJ%z~LK6p1_KwQ#zOVCGdfd1A5{nL=lz49jAx8v9;OlNs_?sj{RzdHDO9 zb(C_?zQ4(kTZHq0M=Vk$Djd9R`>)lIl>Vn{&Eq%uN;%~sb;Hy`Dj+w_a(QWC?#P=c zsLYh2-u`T}PO{>XCXkH6QdCn{I3n|v%r47J+7QTKas#=!`3P>68tJtdUd@7HXu5>7 zHZ@pdBT021zP#YSp3D?JViH@bs48ZN)nbd-%9lyUM4YfQ+hQr7+Er5?P##iRD7}@z z%1C8~@{zJg`BGVJ+#Qd^^iYkRa9 z?X;G_SJe;b&Gk-tcYTOHUjIO!tFO{G>f80-^wWB>ZqrLFiY3T$SAC^^yZUqL7t}AR z|L1+(g8hRl2UiQeJGg#u!{CR4TLeE5+$K0AxMT37;E#gW1#b%861+Y5x8VH^EDi2z zP_se91`jr9*5D1^5%Osm*syxT+70VBe6-<<4c}}y{DBiL^~&LEI+J|d;JqvPleG?} z)@!Ks&mu}hi+JW?^9Vx;QmQl0Z)4>#rH}F^|Ciib>($COYJF9$RaWa`)cQQN#^jaN zdN#FQsC}jVpl#QF(@tn-s5SptTyLRw)_dqf^-z7PK2Kk*Z_*=}s~}E4uV?C4;B2+} zqJG=@GwZLeUs%7GT8rRH!9l@y1>X}K9Q**aemuDK&07Buygqm{wcZoF@3vYGD64g~ z3bh_atrcpmQ0wc|8txlcj4b86aY#939K>Ztg5SU|U?+$Go4^+S|Dvy9DgC1~PjN|N zl@2fMs5H89{zmeRm>WB9Y`F2$jkPyc+*m-4rTi~@emCf0HZEQ-y?%}UHuAhqUe|vD zYruD43FqZo=nt^X7*2L(_S)>;*)y^n+0(M8+a@roJYERfSlbxe+qSnZZM5~fw8qxM z*1*PljTwa?Kf|7pov|`wNyeg#+38C&KFH{nzUktY^p)u=(!WXnD*e;+G3kBMd#2{! z^=qZyefFw{m1tAmW#Ml9ve=|X=KOq*la@p3L4}_(Ntsb$_qI)CtlZ^_#jH1YPbpCd z6vM2tZ(Z#Lh5$yZ)MelRI1Mtr*WSKn1oaL-^$^#C|K+ECq|Q`lsUNGKsIwVK`%qn^ zE>@SQU#efJOU1|H6ZLEL8}(Z?O#M#G7IV~P>T>mab%nZ8T_xsQddRhj$kj zs|9MITBH`MCF&LRcdI`*`x>|cw zYoayPnrW4^%36RHh-Pln=JEyMr`kNmnYW7wZLzjQ`%>&c&z6dv+Sl4QVwd)<_=R5Z zU$yVFWn#CsoOw@v)4tbMXe+f<+G_03TI~mMMEg-&FOJendJJ7ZE@H$9aZ;SpeiE_b zw3el96$#qU;;gm}tF>K=5Q+R>!z68|maXj)$=WaCy!NZMTU?+gCPkzQD;mB-q-lG# zecFEQfEKAmX$Qqck**!m4vSoo$2|^l%7g2tbHHlRc!QB>*#fr0m?vSkn#prZN4&Cuct3i#w(%9EahYUbNw^rTYZYc7aZZDN!M`y z9(}hf%EErT6=P0#$9nu1=fQ6@4Of~32X}hAW4Q8W({Qy(xbjGYrr}zX;LhRN1D$*I zZPYJ#LGXeu!xjX04j%Q^uyFkWGv&na1^t=_hl}2Q-)7yXZ-emm{p#G@9Nw>Ao2KEq z%!WI-dO<%j7+sbDsd=dRji%w2CS8NWwFi6j?b$bcX2&|=?K}3X)1X0cr|_sAeZ!+V z)@jhMU(;}(n`NY|)8DS;Ev;{paGyt-hWmMQ=-oHGeVuU8Z@~hYU+=z+8idbWu%ONY zYEZr(b^HEqMf~+Z`-%skK&Rcx%pPP#(x^cld7x2)Mhz%ozm84A{hM^{-M14(Y(Nn! zH3@IhscCrSCgDv zRQ#&6YIc9^YHa?hzS92jMt93C?Zs!BHK-xC^qa>lEgIifGe92m8QQa5(>ij?xBZi4 z_xcTZsm;Up%47bY411+bBe_-Sy&*3@-9T{R@3AUV-t78xJ$bBZll$w|s3Nxl zo^Sc+ebw0#5kUdI6xPe5h?zB%i4oy))uVKrh`?ZO8y_rp<(6Bn?hEGP!E&`ouHxj%kgIm`y5AAjP2vbPzeeesyo<7&IcZvps-@r3 zAJITm*4v00`WGUIHg64|T8ytni6F)_tNYS=@ws@@*9D5-WTeOFj}Z9TvTa4wX3u+E z6#=x`z1v!a<9dZ{i+B)k;Jt=##)%ER+rFYb_S3uVPd_Hp#GB8mENUp#%eJfVeZYfd z+f~ItW_a;l6CnEFFTLAA+)2ab`IAP9~SPxc)Wsu8Eu*&og8Rn(_Q6L`!D-FE0=8fmFz?>0ffI_?)*XMKJjW z^V|uf!^nM{`J6Ev8!tw%u2{2Xx9#12W+>N>gG!@J3h@pCWqivlqqj7&wv%|CEceTt z8_KiZF{v<&r%YyBK7E2&mQid;+J%zCP;!)ex8Ku*BX^=j3zIXGxU&uaPo=qd&s>_3 z&#kh%c6++EDCYWH>|_N3fAjb z6#i%7R^)R#i-Jb&WgW?)h=VMOILx9DN5~lSB#R=W2-fkuQK5*lEDCXg)JhwRBC=T& z+838u=ZR~qZ?Gt|HWZ;KG@V%E>si-SYO}7Z)MXv4G-Q3h@&IpR;Op7Ov$K9$d7AYz z%t56n9h8o&Ur=6T-Bn>?SA2L+)_s(|tntpQhbTi>4^xJ*9;J+8J(?(1;XmQL!#Y$6 zW&Im10ZrMf92BaOpx8N*tK_mSQi@oYC?%|~Gls129SUWZT@kE(RbSTrjDaX>HMJV+ zntYq1sI}=CRMf`s0iU5#a%uV@D6Qn zZw_)TOX0)&crr%?8CJ;GBJLOPQCwxe5?9~NH$eA@TH;=Et;0-L^^oWX#Y3VoGf+H& zUv7d1HAAbUm3$m4^`v-8JdLet#kbsTuvj7DSu{Y}w^>O1Z1Fjf`4VDp;%H{l*vVH; zzldMOZW%9&1Z38Xc}21~FD`haT#+VYT9JVaXNpTA3n{iE#W~_K@#Yi6mo1gn$YBTN zd8L!m1^Ihf=}Uz8t+GN{tNaLuKP0lR&>kVipR0bV&Qs^B3)Iil&($x~g%#T;x7sG^ zFY0dfH+mNis8Q-6^@w^*J+7WmPpPNXcr`&yRFi0Zr>Iu-qMD&*(jLjCEpnMwNIor) z0$Ls=v^TEO)+nWgE_)+=G(TDuRcKWN(c-9~)zoTfwY9oheJxmPsNJtUpgp8LO#7p$ zHrLzQpcmsavqiti+vboh4%yz&zSY8HOG8_3wlTJ9KbN-|v>nLqF6|fDQlRJKx7%9> zv<%K_=d@()0y6B;3h6(ts^3Kf{;@t6$@IR5tBOh;rb;Q7Qlu36#)s4jspVVh_h#E3 z?fgROcWBl&(Qib%DJ<37y(!ZU&DyQ4qE|_1=WKtV{gsdw&!%*!QT4^P5$&QvQmQTt zI1+HWU0A!Ywwdjs+NA`>x3>m89kPIRk9JWVngxCHY^}CE$g@YaTGeabRqL*+HNJ0e zt+BRU*gf;^omxArc0}#nwGY(WUGG4>Kk7HDzdQK3hE*D6wwv-miw9FeT09&gpC7W= zEX|Zhsy`aVo~y~GrlHN$X2Y7l6q4248nU28L<{TVzqE_ui>Vnc_qAToE~-`MR*9`j zTlZ}(LKP0GaVuu=LnV_&&cG@|gg_rH7 zJewkGO8qk1CO%uM?3(H!i{)KNNm(xK)`rw?=L~7lHiG&hQ|;j}bjpG2(udzHR`m#JE`int$jlnEU!~&M4#5>>MJ65nX#~Qd7l~Ih=;7!^|3(4v^%e04 z@#HmeoyfKn$2N+{#v;Zjm6S^29i^&LRg9+>qqYcD8Y&INM0zb+h)L3qi1+X$EyYy4 zNo(;T9_1M^4WH6cOec=%BR--h;x#c7-_lpiqR-(Cz8ILT%wcTwQ)RyRR5`Dl=l#6zqL47PliEpqr@o-RD3(c2 zCBDa3O&2TiS4+hz^*eR7*rl#je-!)a6Wb;Zsyo$vA{u{mK*ZydqQn{e(jk$6Z#p8* z;-BI~qV!4PB3{WVY;H~mS4j#)bF5|Us zh+NfB4SI$kocKcyAc;TmSOMZHUMomk!*f*^*YQ}v;s##p0a1$AdWc}`VeMi1keX|) z6kU5>dqD}%x@p~&yR<%9AEl-?Tzgx&M;oV&SL$l-Gxzp=__)uMV0x=pC=Y6@wbjbg z+B$8W(vk>lz0ykCq-|1KYj`%LjqD9mLhx*Rl(zV@{Yobyvm?rj+A%F!d4=BnQ%Vo| z_s=SQ@NMUm*YI!2%Io+ztI}6X*Rqs>^l{mhA+p~}8H(TYD8uQ4x~7cKN;N|XrFThJ zCh#?N6=jMZqz5VQ>(%vI$_M<1=g!JZ{$Krz%0}h{d0pA057q}OyXi%rtn8sD>pdk> z_CqOAG73}<>9h2YmBaLO%~p=mzcp7mrhlt1SEBV5`U>R~qYbN-SbeR&R*BQs>l>7K zdf_)IXZ0=m&q|^mu7@kh^vXvl=k=ZXZ^{LIpT1AY&?EIo#ik$9k13h@as9aB&`;=O`{cSI<@4dVyY`c=Qszg!hdri&n-FkpPm6 zBT8$~$vC0~fFMvE95jyT?}13JsfEtfAbOLt#MV^qr6=G!Q4@|os5KIrbe`&anvc*a`Ns zzn^o*Nn^lC&Lxm<68PQ7Q#4T3Xs^^Ft!?Be^+_9o*7P7eN7{+>P13i(7%;cgr7Qpo z!6J}nL@D`3l==kt$Y`(51hc@$;1e(#%mK@ci|TUlJy-!&f>mHOSYza=Yrzj-9rzJ! z23x>Z@H5y3!i`*YJBR=~z)mAa-35Lj|6j>xH|ZYI-$?h8?&qFJa1a~@M?o}z4(dq| z3*x{Ta2A{c$spTkq5esF4csvD(Dxc>dku8GhIY4+tK9?c1$96@a35%785i|n(gvgrNgI*gPx=7qgQO3UHYR#^ZM(c9~b}zfx+M{ zFx1G^hl7#eZ7>Fm1LMKF)M+A^4BkWMzM~Atqy9bjtp@A3W+N#wr$=)BFxMRgCygjQ z0puD{wB}kF>7tF1C7xlv?q`iO>_TydP%r^Z0`D2=`drdT(nCg;egqt27NV0PKu-XJ3)BX6L46Rx_fA8=FmRCjyAhQI zVZ(!nzG~2GRvr78gMG}wKIUK_bFhs$^nmd#66tqfFE|d+8*E+nfqq~B7zEw|--C@{Gxb)*6(dQRV}yzJ(8~$EoY2b&y`0d?39X#a z$_cHU(8>v|oY2Y%t(?%x39X#a$_cHU(8>v|oY2JyU7XOx30<7f#R*-U(8UQ|oY2Jy zU7XOx30<7f#R*-U(8UQ|oY2JyU7XOx30?RfdVps`7bmoELJKFfa6$_wv~WTTC$w-v z3n#Q-UM+w&LJKFfa6$_wv~WTTC$w-v3n#R2LJKFfa6$_wv~bdY)7!|z<7L9>T6mgF zJWVE^CKFGSiKod_{{+{-4I>lloQYM=#QJ7pF*C82nOLVxtWze|DHH3IiFL}vI%Q&= zGO(E}%X;6x9cv@U#@-M=#E&;KbK1ond~MhSkrNSSRE!tFxkQ!o$AHwy8W zMfl4id{q(trU*Y$gbyjghZJe4{GV7WNC$cB7a2wPgd%)G5k8>^8(M@7Ey9KtVIPaI zk43b39%B6{cmg~HJ^^2WFt7t0K}XM08b5Fkr~~SO`#=NG0gR?poj_;sBIp9`=fC1?*734h8H`zzzlMP{0la>`=fC1?*734h8H`zzzlMP{0la>`=fC z1?*734h8H`zzzlMP{0la>`=fC1?*734h8H`zzzlMP{0la>`=fC1?*734h8H`zzzlM zP{0la>`=fC1?*734h8H`zzzlMP{0la>`=fC1?*734h8JQrw`x{LyQEx$XO!JEF#V< zaRk{vy>-Dp9h1C1oa~$i|8o`vTjUY!{N2u>S|!SMhh6kw65KMf8$I z@FHl0}4)MRbxyWRgWxl7;s=i}yN<_d1K`I!lz0MH@ZH=!_0` zrlzsfFqRs|ifHtysu7D`cSf%}qt~6$>(10LmKw%V!&qtZ%Y%y8fjtoQ?~IKgcCC%OI<@j|STC{7!x!pZd>PymWRF(?7Q zbNwHrSHU$o~uW?ag2p$A1Nf zx$Y?PISr4Og2zii+C50S2Wj^p?H;7tgLJ!*ZY$DlMY=smw+HEVBi$aP+lo{>(Um|X zIv0udAkiKq`ZN;lMxq@^tOx1zAeC06(t}ibkU|eq=RxW`NSz0-nu1qN!KO5RkpeeTU_}b7 zNP!jZd*Hkq&U@gz2aa3exRw4)-b^6EuL8#%aNGgM9dO(M#~pCl0hb+c*kN+^IB5*{ z-EhE32V8W(MF(7Tz(ogKbihT2GFMbl7J!9d5pO%JF&uEv0S6s$&;bV>aLNIv9B|43 zryOv~0jC^r$^oYwaLNIv9B|43ryOv~0jC^r!vXajP~QRN9Z=Z;g&k1X0d*Zv*8z1M zP}c!P9Z=K(MIBJo0Yx29)B!~uP}BiM9Z=K(MIBJo0Yx29$^n%eP{~1rc>-C^r%iN) z|EyR_>#Yru`8ecU;Ybwg6GVvzxUP!Xi(Rpu^b*Iq(k|#t4zj(}pDPA|ifyL->>n`} z^M*;~o9(9)+*jU)s%7NTdt4bh|5ID43K4b{+EP_$OI4vQRYf@~swzp~JT2u5#Hy*3 zIE|7M`6_uvq~c^dpR}0sC2apeOF9TtGxF8yqN-Yh?b>WVMyv4&BNE>Iy=_MS2|gef zACQX=$i)ZHS0pN{yTJe0Zo|gss@K2`+1Dc~)7KM8Ur!``J(1c|;Azkjv;wVZ4?hn& zf=-|_cmcdDs?rY>Nk32|{Xmgg503Q&y+Ci!9}EBk!5}aWyaUFAiAISw2}}l4!29$L z%p$*!foxOKhoP;fT$@N``w}aM4CZJt9Fy(L1X64#?aeB*H>=Rztb%OjFos_l`Si9q zi`cH%?yN$;h-`ZXk=Edir<$M^_;1>xUAd!yG#bPV$9n9DND#qAL9|k@`ha8^~q5faArWgdUDRNw3oKkXn#Q zbOb%c8^8t73*|}V3f?Fj(;ftk!6V=VxWc_Uh~Zk^mZEj_25=GaJqbDj_PMK&y9&9h zkh==GtB|`2xvP+op8iI$dFLQfS`B!gVl#)@MsdYl@CwCvg<`CDF*z2IV-Y#ZwplTj zyV%S*fiwl=fg(`;q#8ysmbnaoKkdJW!e|bW*B1UgR z#1=wHWUNj^PYK=lN;{>qiv-Rm0iGl-Qga*L*3R`VuDN2wU|Y^8{zf)EM2YxEyAo_% zBDR;canoazfZaJloL>xm-1HtLC@&D%bpcQSdbyP$+&2_$ABOcEYYf9i#o$X%;7d>7 zOHbfSPvA>WsIncflh(^FS}6g<^Fg3GknMp+9FuJV*>=1&TG<7Ff^mU2yIo*BydF zG3=kf2hu9iWcwh6G}SnP-#dZdJ3((%BGj~-ZH6M!5|gTK+6=|;(5>GE_iNC?xtp{m z={=;iNbe;Ik!^%`*_Q2uiKMcvFq!l{+8ZBmUbY(|**?Tv4u?5@g!Cx%od74I;6xOh zSOF)Z;e-cHY$i@mAWlypMo%F2W+adEiAD+BSOGUYB8}~f*l3%0vz)YV<*6|6>#j7Ld%UxS1oLvEDH^bGLLD3ApT8&tGRGA7jEXl$!y}@1h}~w zPHu*ao8jOJIG9a5ngAC!6O$$oeC>pE8%7o+>C>pad1=GDXE7mZ|b4sXm8HF3SI{T!64I? zN!^Tvqd9PM72G@qC%41N?Qn8CoZJp4x5LQ|a54@~#=*%rI2i{gT;=~RfqQ_|&$=87H8#N24RCb>TwQ7EBe&A!9g3Gv%$Foa4sFr+2EWFjvYYv zF2gAsoU*|w8=SJ4mUEnu0S9dAPFfSYKq1>a7f#sVgbhv{fD;Gc!~ra&8>-u|jBcoY z0E*k7xDCtbMt^Kj+=g{@V;S90+6JX&w)cp$h`x(cOdr; zIAllebKp`Ea_@jsNpLC&P9?!9J96(p?j5{g`v5JhhoJnU;0f>)Xk}!;y$q~cy7+|s zuRs`x;MyIeJ4q8b&$tF}odm(f4Bk+z!P|*JWfm?&$o~#o$o`*32Jg`a(f^sl`zArk zJ*4+?tPUx0q;el<09tdb18FBx89ls6DkF%ONV}2t;NG5~FZwYAof&3aR7S8rntR5w z{|?q{Jo}-H*Dw~Mb_Op3IIMoev6)~N_!xWw80S$L=TYZ!elb`Az68{je%2hgov!}E z`Q6|*u#aO0KomFx;sN8YY9dGie}JptIf+a_kR|U1i3R%5b+T2n5pd)iyHp z`Wz1i4Z;23K|l|t{s?FSnt>MJaquK~iF;oLuYew)7w7|C1APJS70~aSL%(kh{k}O! zSUM7xj)bKnVHu`nok}{F{qM+c71!(rksLb&jvyCLi8}bcR_JpZVoPuQ=|Or5%A%YC z^uM?)$|*ws3y_XtGq!Bawu~t|k;-U9#*#ze@-X0yKH#`f0;fDsya0+9K=A@7?t$VS zsO*8d9&}K~i5~Q?2+9_iG2#XGb2(l}O8kd@6{Ax{=u{CpRYXla)YL;wJ=C;-8hWTz z0ktZoR>firdJ|Y?SnA(M2G75(pLS3M0g27R;eM&$icUWuqI?a%H0%Irxh#bRz)W2vi1< z4&lCGU;<^dJyD2t*G8(StzrAP_wWL=OU?q8)h;MBW3D_rSkcH5qx^ zk>5b5YDabhp{$)R`vRaW(Kqcz?)6$Xj$PnbCi}Uhg@860avO-;1~St^fPNQkxx2wV zK*nOccY^E&BD;aeZXmK7i0lR;yMf4VAhH{X>;@vcfyizkvKt78WE3tVa2b6|YnQE0 zB_GC3kj20mxJdyA*B8k}|;n*tVFa~b%eIeUPz#Es@k-Z~uA{H*3gZeR0ew7&| zcOrcOiZUu=M#e*E3o!-b;Xj(QkzAOrf;dSr?a@3mE-1DTJ zNPCddUPs%q(Xwo6nT3{RE1~p#Fd{;&E>WvX)anwo%A!_sghr0eWT9!<)GUjdT}Boz zp=sIFPL9rGp=H@_8oeiYz66=+gKu6g8|N3MC$qzIZ6 zL6ahAQUpDu)yabnMbJT7mLlq0M4gMMa~^dr;t6@Y-Px1KsTb%22Ga67Na_T#f6v9f zhwTDDlqQNn3An(77uZYo47?rkZxiYH}ScL z*j%=_+x>Yu5Sb}vb!8(;-ol3Ayl1?R9n-Z)|318&> zbu6Vftz0>tQa*+v$4|<~OUlPcYSE)m7Y+EQagtQdrGadsHQq*3O3;*Qpf-36tYKW^ zuVW^gjc9cX*b06I+rWOV`ya+j)IYiR8n{7k0Gdi~Y#zO_c^Vo^Z)_gDv3c~q=F$6_ zNAGJMy{~y%f52O1+CadYW%R!0(fgW5?`xhW$5_^p*CrsxR*sR%@fA6?BF9n6$51Mc zpH!j8K#rjVk=7&qx3QD1T-ObB2R#9N&|d|wgNoxSgV~p3D{{TR(K=%F9LV@#9 zjHyI1LUs;IbR3JDEPkiAK?AyxtjM=$NsUQteN-%x=4N0E^Jvsj->04kZ*9-?^ z*be3RY{mfQfKS0ZFdzJXJvKn}r_!IqJEtC%2vCKKymRVNSA#Y5Ppt(%fOX(UaHltc z%Et|U<(@qNZ9pockjg0aAUF(;f@lDpRNfh6Y$%Gcp(w_NqSUkC97qNg-zxh5-xxv^ z5}sK$hLFiSrye9d3Q5mI(xb}85u&tP;|Nt5N2tm;LRIaOsRdDGV+m1=bwz1%Ea6X% zUjqj3P%1zL@(rp0Qu!9u?PCg2`h7@r1JDRO03HGlgGWJA&>TDl=*Q$8QmIil0TNgU(!aBRq zRTsMILPuTbsPtO0JrTfDE}^5+OI^Y{?dEBh&{r2v3qVI*=%@5fzoDbjGhO1z7kF|2 zy6Qq#UFfLvOqVD{0D9s=Ph9AU3q5h6CobgQh5Wmae;4xaLjGOIzYFO!Vm$de1p>B4flu$(UB#f6NxkP#O$;zB;8ce{kubird6Ja)lj z7d&>sV;4Mj!DAOZc3~M^@X-YyUGPwP!%OhZ1+oL=kCV(ZCgBCRA~m^!GPX zi8ioU(7lbp7rWH@B`{R@D#M9J#u*=}JJH%*NOL2wUTOj0jbNe;q#YC?6*WLjPz&(h zDp5uhQAQL|MiixS5otsbX+()eW#d8~N>fZ}X#Wur$;ghlkP^Wk-iE8qlV!9fqqCmu zzagc8H`*$W72O&ik|XY8t_1E%5dPdXnmdl&cE?|?HtUv5 z-S9GC4)_$z1M`h!>XuC1lBruVBV;e|M(m5A3+M`Z8=>e|n0Vc&B?jOFm$LmG*b9z> z81TChhCYSi`$EyDF!U)5T?#{g!qA^E^e0R?Xw*V?!qA;CbSDhm2}5_n@G+r8IiW;3 zp+q^ML^+}8QyBUbhCYR%Phsd&82S{3K82xAVdzsB`V@vfg`rPj=u;T_6ox+uRj+{? zMyO^nYH7Z}A5;ca!Ac_xn;uHk5{ll1Var3&y)bkyOpE2bozw{mfMJB8e_`lf7&bZ- z9Sp-phoXaF=wKK+7^Xi1o(1hd2k<=T1iErvH_#pQ1kjMkC6ve|6#WcCKf}<^Fzjq7 zIvR$4hGADj(a|t;G)zALt?yE>6v{aw)Fj$0=aHOZ@CR?L27&5WkQzkyFJmd*C!I<3 zI*aX((XV)L5!e9Z1iFmNWp3HzRzPlf1Hw4ctgxLxtHC}nV|8(Z=i)@U&Gu0*{pl)eSDJ;!$tm571m zcLV{{SAI+I5@W5e8Rtb`Quz(RI4C`hUZGjWYHD_nnnh8wc(IT3k&M(uarYrOa~KXB zqYp3I_>Ed#rj{qEWeT;7R=$A4sm4jTcoHtg(wor8*v1pw&@NppgXX)DjXhA*D=){9 zk-Ol+QRr|28pKn#S#V)Eb(=%oM!?53>Q<->VSH{FW9MU;D}9`CTuEXqFB=*4<4$js z;g1dk<44+%4kaB9#vwx*HFfd)T<-B;SG{i%?dHBrDD2^$0`B1(LF9usija}TNPaaU zczP8n-2iB@n-ZQdcEY`6?%P92FLQ4MQk1~`$GLwsC6-d1MLwr^_9^l@3FY0;!$C>y z&?5)_58zCav74iX94+8z5p}qL@5yKO^)B5168bFrI)@OK45N(nOWpjYVIKdhwJ&kc ztkPtGor4F_NLFzPm~0=b?}86uGDnaK4A$aQ1ndODQ*2+ECsaw*95EaW;2xt2DKf7Qx2%2K(1^O%uEN&ohv6lFhSvvH1cn?Fh`(B7Lr!)DCoNQpfD*PoGR z#2dwE8$Yf%ZMcnjL?`kmXEPDhM;P7-%A|Nt5f_fAYTa9~3P6 z85?eunEP%XFF*g^r$+d{U+blZAtO}thtA8nO7Hml=dzSjwc>3RYj^8-`Eza^`S&d^ z<-OP4$`!e&NWa2=&j)H2-RUZ%@?U@6JH1b=_}stT7X|<83!j z8g_chg+bq&JY+cVX8-li;J;ECKa^i?&>L@@XMMpKZ5%ZCE(=>$jqf2g8}tjn_p072 zc@oksX<^(8+@_q5Dm%CO&eSb`#@|yT7tdt=cw^>oKjSC#`6r_VrJ85-HTK}21$H~g z*k-jt_>Tg9c4@l~IpL*-r zcfRt!If_j|(^$*54+Zotw_IhE{%`W|UV{`k%$!U+hZG zcJxj$Nd=Fv)7X3GC)2JJ)a|nIv(!xEN7@43Uv<+jCfw;R<3#Cu<@qto=&cx370Zax zeYmGGiy}Q8?H-wvMr8ajadH#dEdG3p*_LCS_+sW>W+3XqJVbH^qI%}sL-ozMhj=fY zrI8rO{EPRSvkpCE&N|eX`G$6qlbmCyDKiWmWF8_plVWq;VL3+|Kbhrm-eE~$4g@Rj zuskU;Xxp@8wp1t2WNe!6fPZIc&pbodX)lxtLp&$vP3C_P%bA8cn==i)V9qr3q9W%Q z>T1q0^olvhP)~D?pdH)j_bVa_fz(wtrBZF6>^vF7YT6lKmFbl99Z=!iLU&{1>dpkvH#m(AQma{i#>=KMi1 z=KMh?%=v>(n)3&pGUpG9HRlgXG3O6THRlhqn)3&xnezu-H0KXWXZE`qB7+(5Y6=^( z;MEeD%!F54Tw*r7x+04i@#>3gbB-b2FVr3p4mqz7vsg2)5c3c*uaGdzd4*JSULj4B z^9uQy^9ofq=M}1A&MQ>ioL8u!Ij>M7b6%nQ&3T0$Fy|F|Q2uWKrLj4q(BtNeLQgTH z&?)5^b1tFh%(;X*nsW(tGUpQNY|bV0k~x=94|6V|p3Eh5P3isr+B@?&Es8Ub*YUi= zTrk7Hz%X3HrHIN69^e5ADlniRpeRJ-@Zu8C>_*r1i0DVxc;O0|>|!)B2jhAJA|eWM zE8e26f)Fo^2O&gIV82iG^m`y=vnKy#yFdNBPghk}S5-gtbag%T{+@s$^s|5>G&Z0J zO$aDLmjo1{O9P6~)PN#1C!h$;4Jbkj*i654w;d3RML}p~nKE(BlD7=*fU6 z^i)6;dL|$WEeeQ2ivyz2vVbV`d_WX>As`B^2#7)}1ESEXfGD&kAPTJwh(hZEqR?vr zQRsD90iw_wAPTJqX-Khz{ySg^eHgHWJ_=YuTLPBQ)_^7SaljJV7O;evgRnD0|7;Ta zXOqxBTZR7FivHP-zIp_EGcdo7ynKCG;=QIO$*AnL$)s zv3jfuIe|1MvNs2Ts28Y{z1fREB07n0%#+#c)93q9g8uAv=>2DaP^tYtJM@2{{|`k+ z3}g48key2!#UU#Wx*!K#a3NoHquF!N3S%g}qLI}JdcmR>9>;$21iJ&D@=40}6uS$W zA}44H37Vo#&=h4sM--tUKBV5;*~`!qpFmYplm<-!<`8?Gpeb6SDfXL|Xo~|_TvT7Q z4EmyF&=(GUQNqk?6Whczx24#abI=_fN!iJEGOcZA^hhasq?;+S-EDVJih6)smKStN zUeGCdXp_^3JssS#hCz>5^vH!=HJX)jMRp9hWeo#vSra>p{{*%Q=w(gpZD^TRLCdrX zTBc3VGIh~1TTENK)owK{&^0@lZTZrE$y_b}SA$nnvlc<`vBjcvXFdReR1Ya*Ig0nALmEEy2TZ7FeCn!Y_48;hzI3t$|zSmXVL* zr8RKN-Ew$EO>5v*xE1hGf&aH!A@7BX_ za2wz^x{dI!;8h{vUIlk8+iiB6DW_`5+@K}%f|e`^TCyZ)$&#QYOYlmtla#yME~t7Y z5O6}jGm902v)#9L4i-T5GEK&X|7_@4=pjC5& zR?Q7sH8*J0+@Mu+gI3KATD2)!wKe~SYvbGCMXN1{UiqMpwlnqp5uO#ais%O4-m{Wc zaoylMpoI(3!kw{wckx|J8{ZX;To5!e|L{X27pO)ic7mT^TKI|nGSgP|G9lo-k$#q+ zWgNJsx0nRDrniAGdOIj$s;PfP_?`Yv(+rf;yNJEp-_0Fo``Osj@AvnUa*m$^uGm~Z zmzvJ=^SJ5(R$><6U*R|K^Zk4ys^xKn0(u-Z0zD4C#@CSBqxe*4?Vs>Zm|R6iCC{f> zz1a?oRHO);)J5EPv0qG@C4LF%m-z_$cR6=_!6Pkx1&CDjz)pRUJHCW(g)G0)uQb`b zEnem>tNm&`Dz5QsOk=R<)^hiCejNx_>pjxqH~0;vls5`?Jg`^aFirKn0tKTEy4i20 z{O|EE#T@@WYYUq2j`;|>)o-OnAG4y=`E7n1ciir`Q#Nqy$ny*Tg=vlth8@4+!4{7N70+p);z5g~$kYKZwt;CU4Oz*MBgL#~Z6}TRmt>AKW^HRb zDUlNR7OZV`(o$NQEGd&R_*T*izBOxIowSiQ@NHT1>hQ_a4*m$%zB)Nlj)ZT|8dxVs z$x-kfSPSdqXgM1G7}mr(IaZE^@5tI%hc};2@SRyB>!gcxf$u6^;k!vU`0mmjzK8UH z?v=*>kyGGLl~dvSNFVsV z(ii?TISu}FIUT;A^n)KD18A#(GLTjpB!l3~r5t{+42B;fL*UPrv*FK?bKr-{Q21fO z8%oZVbK!@}aQG220)C{7ga>O7$|wPU(~J_J(?HbAmn&Jbkt5SV*2|X}{C_k@W`eMn zFIVwD(i|N_u-f<IhZX_IpLRKMzP{>ttWDc=&SwHM# zo-po~2jl_j^`H24ws;DfPZ_{0#DCu-@`!0B3uFN@P$3n>f?o)&ks5M%R30TyPz<4< z7~h zStYCBUzV3mQ^h_s^<}NBWtGS}Sx0W`LH8=qv5WpQ7L>079lMwoI(9K_bnF5T4kF>; zAd&_gM7{(pHT+ZBtZr(4x0LsC>7l+;eiWu(CayL#`fZa;qvv`kT30S3m`03N@>6pd zAst_HltXv@O>MwLGj0LXoFL`~7%$~Oi=nmiGpVFMnTyMPxC~sfL=hLhLl2kuN8wr2 zU0Wx$X*+~KbMdxWb@|8HAk zz8l(-sEBU^W;__vM@RZ~8WwMLNs&3fQ4+2(Tpw-C;-rWDjZ)}!diVG`O`d+I!(N_@ z(A1e_Nk(NbF34U)Ga~-ZC;GJGP3KcmdrJ1>c$KAi&qZ_Tp3hfH`X1@5sNY7{slJN$ z1o}$%9QG`(iw~92^dr)Fsuwbk^fW2CPL(gkwMnrDadBT$U7$TUKGKshT1Go(9@-=0 zBbPcH>S#ooeCnp!ydZgK&Z*X{%kxj|hmxLbWGdQqK1=t;wO7R9nq#~jDlCpm{YI&x za~%1FJ(B$rqlc&?l_DD5Wn|_omKZGd9)BCMmtCs7!4e^|x#^c%;#zJhha$ z_F{LBk;C{)IWwvo{!)ratF5jQnoUdArQ`)@3+%s?H};0Yj7*7`Emg;JM{>pEkcxo6zC$bObf?La>W^kagjo6n|d>{rv2?wT=q+%&Uy#*EHgtbum5Sk6h6{IcNDf)@)uDJ(1;RCrP0%)(m>=M_FxxVwJC`V;FfEUH&Dsp$Hm$BJI(O1(Op z+1I^hul>|rX+L$b)~;a&uB+?9Y+QT$iT%CZZnxQwnZMg&Ke8V( zr}u$2H{Jc*4aM)$4rcoPV860o<8k&| zyAvNwyYRrY+y2>pXZP5>_Io@r?X&x7U@QBb!xs}yv0WD4n6mN3lo4}>-@j^>-`P>Mt_t4rN5a0j(&M=kd1f^epNQfYkYgYA#ciC@-|+C z-<8esp1dy~@D=)@d?Z`&EBvu+lkM_*`9wao2ly`ioUhUy@}>MizLKxy8@^6=;vs4m z->18ANiBF$&XPaN9@#5DxKi0C-@ANQl*miyTloLIGR^%5^vaa#`5%}a?#iFi$K7~0 z!*|v`?mO9SQ7>1?)t(+nFCHY`iF&+zK%f6cCAhZd?^St?dpusZO_1B2NbDhde!8YO zq!wt52Dths_xl_FFFpTn?)N|L_3zTZwUtE_G9$n?({ydJwCHvyOLzj zZj{!yckqrvYd)vNPoch}Vw%m?# z*WyPx&7-rMk2UAF?jiTE`<;8lEpQdC5}Qua!*j~JvwIqw&Jyf5%dpj~z%H{Y_;p{8 zt>snsntQ{&<=(-{@;;v4KXO~~{``si46n~$;_>+#Y%063rR>3Ovd+5=-2v5#zXS!{!Rb3f0r5jubG4YBeUq)QjfXu>zH-DNp4AGC-_$~ z@9iu`3>{2gp4xPtla7yd3}Ja%+PC!!!}6?C;s2BtJ~%D>qExusnij4e`)c#?ucU={ zP7B|Z7G9DHm$SksslqIzP;KpzjCPXxhB9WkvG_{7*o|`+1gHHaV1R0(t2)Xp*w{Rmu*a##nsf&-jTX0e@p zmKkl0%gpyyO@-}1^SuRHf$e~Nu^MQ;4?q`kXdd_RTj}1`oWh+n{NUXFP1DGk8slz< zR!DznwR;a*CBK2HkK1gm8MEn{->0EukHM=a&=BK34>3DJj9RH{I4FVNqT!;pYOG$2 zM57tBw(eUEcYh4&v3QvpxgT2L?u1q`!YyP}+=AJG4%p*+GEeUbt#o(mRaiAh@Awx4 zWgP^qcK1T7unoj5x&*EgImjiSMy8qDujdG|w&+s$8hJ3Jtu4LR@7PM88{(H~JcpLP zUIg277|mFuwC!oK<2=AO<@e zS7)@6$_VQ)LhNRZ#lE6XdL|V42xZij4VBm-wA7h$qLNUFwM$Dj4_bk>OIu5yX_aV5 zB!3IwBKgz0$MRPl>aJ%8=c{$9#A>Da{uWw+)k;g%3tA<=hDP~@mI-pF?HTS6`gKxj zj8DgT^Zq;yXIGSI&JnanWcZKweG*;$7!ODYH9n>!R zJSwq@Y5A!oKNz*l0u(z2p|{Ly+IxMP#tF-Z>VCb8+Qu98#fRDV^-8PIC? zKD0{y1G+FhrJP4drCbE9kx|eJxe!_{W1v+TDbFXQQpQ4SLMmk{v_>w2R>&35YMBhhT8Iw*9h}N~eOAmI#rxxER^TOA z5!4<1)|2%<;7YN^2QOfZf*LUjD#tD|gt3pV=(_+trDL8B<_7j-%uVdaGX9yxx+3+W z+=&s;9q6$+<^gblHnI2Mp0)2NBqYo+Xsf1;dFVbe8M_vY)a;p+so|Xnb%E>g#Lz=U#_J8cb>;uwL zb|5yNT9*+nb}4DFE5ogT(~^i)*;Y>PG}7D$m#m#tU(c2nOAnhG!qj(kDjj~5+40|; zY=dU3n>j|kBPT<03F+uM5#ngoA!#u!Sf$gC)jj>)neHriiM!NY<}PFL@0nw}Iq5!IZjT#Yt`%dtHm)scQYlJ#3MZRi;Pri`JH?&q`nbOCG + * by Khronos client API header files that use its types and defines. + * + * The types in khrplatform.h should only be used to define API-specific types. + * + * Types defined in khrplatform.h: + * khronos_int8_t signed 8 bit + * khronos_uint8_t unsigned 8 bit + * khronos_int16_t signed 16 bit + * khronos_uint16_t unsigned 16 bit + * khronos_int32_t signed 32 bit + * khronos_uint32_t unsigned 32 bit + * khronos_int64_t signed 64 bit + * khronos_uint64_t unsigned 64 bit + * khronos_intptr_t signed same number of bits as a pointer + * khronos_uintptr_t unsigned same number of bits as a pointer + * khronos_ssize_t signed size + * khronos_usize_t unsigned size + * khronos_float_t signed 32 bit floating point + * khronos_time_ns_t unsigned 64 bit time in nanoseconds + * khronos_utime_nanoseconds_t unsigned time interval or absolute time in + * nanoseconds + * khronos_stime_nanoseconds_t signed time interval in nanoseconds + * khronos_boolean_enum_t enumerated boolean type. This should + * only be used as a base type when a client API's boolean type is + * an enum. Client APIs which use an integer or other type for + * booleans cannot use this as the base type for their boolean. + * + * Tokens defined in khrplatform.h: + * + * KHRONOS_FALSE, KHRONOS_TRUE Enumerated boolean false/true values. + * + * KHRONOS_SUPPORT_INT64 is 1 if 64 bit integers are supported; otherwise 0. + * KHRONOS_SUPPORT_FLOAT is 1 if floats are supported; otherwise 0. + * + * Calling convention macros defined in this file: + * KHRONOS_APICALL + * KHRONOS_APIENTRY + * KHRONOS_APIATTRIBUTES + * + * These may be used in function prototypes as: + * + * KHRONOS_APICALL void KHRONOS_APIENTRY funcname( + * int arg1, + * int arg2) KHRONOS_APIATTRIBUTES; + */ + +#if defined(__SCITECH_SNAP__) && !defined(KHRONOS_STATIC) +# define KHRONOS_STATIC 1 +#endif + +/*------------------------------------------------------------------------- + * Definition of KHRONOS_APICALL + *------------------------------------------------------------------------- + * This precedes the return type of the function in the function prototype. + */ +#if defined(KHRONOS_STATIC) + /* If the preprocessor constant KHRONOS_STATIC is defined, make the + * header compatible with static linking. */ +# define KHRONOS_APICALL +#elif defined(_WIN32) +# define KHRONOS_APICALL __declspec(dllimport) +#elif defined (__SYMBIAN32__) +# define KHRONOS_APICALL IMPORT_C +#elif defined(__ANDROID__) +# define KHRONOS_APICALL __attribute__((visibility("default"))) +#else +# define KHRONOS_APICALL +#endif + +/*------------------------------------------------------------------------- + * Definition of KHRONOS_APIENTRY + *------------------------------------------------------------------------- + * This follows the return type of the function and precedes the function + * name in the function prototype. + */ +#if defined(_WIN32) && !defined(_WIN32_WCE) && !defined(__SCITECH_SNAP__) + /* Win32 but not WinCE */ +# define KHRONOS_APIENTRY __stdcall +#else +# define KHRONOS_APIENTRY +#endif + +/*------------------------------------------------------------------------- + * Definition of KHRONOS_APIATTRIBUTES + *------------------------------------------------------------------------- + * This follows the closing parenthesis of the function prototype arguments. + */ +#if defined (__ARMCC_2__) +#define KHRONOS_APIATTRIBUTES __softfp +#else +#define KHRONOS_APIATTRIBUTES +#endif + +/*------------------------------------------------------------------------- + * basic type definitions + *-----------------------------------------------------------------------*/ +#if (defined(__STDC_VERSION__) && __STDC_VERSION__ >= 199901L) || defined(__GNUC__) || defined(__SCO__) || defined(__USLC__) + + +/* + * Using + */ +#include +typedef int32_t khronos_int32_t; +typedef uint32_t khronos_uint32_t; +typedef int64_t khronos_int64_t; +typedef uint64_t khronos_uint64_t; +#define KHRONOS_SUPPORT_INT64 1 +#define KHRONOS_SUPPORT_FLOAT 1 +/* + * To support platform where unsigned long cannot be used interchangeably with + * inptr_t (e.g. CHERI-extended ISAs), we can use the stdint.h intptr_t. + * Ideally, we could just use (u)intptr_t everywhere, but this could result in + * ABI breakage if khronos_uintptr_t is changed from unsigned long to + * unsigned long long or similar (this results in different C++ name mangling). + * To avoid changes for existing platforms, we restrict usage of intptr_t to + * platforms where the size of a pointer is larger than the size of long. + */ +#if defined(__SIZEOF_LONG__) && defined(__SIZEOF_POINTER__) +#if __SIZEOF_POINTER__ > __SIZEOF_LONG__ +#define KHRONOS_USE_INTPTR_T +#endif +#endif + +#elif defined(__VMS ) || defined(__sgi) + +/* + * Using + */ +#include +typedef int32_t khronos_int32_t; +typedef uint32_t khronos_uint32_t; +typedef int64_t khronos_int64_t; +typedef uint64_t khronos_uint64_t; +#define KHRONOS_SUPPORT_INT64 1 +#define KHRONOS_SUPPORT_FLOAT 1 + +#elif defined(_WIN32) && !defined(__SCITECH_SNAP__) + +/* + * Win32 + */ +typedef __int32 khronos_int32_t; +typedef unsigned __int32 khronos_uint32_t; +typedef __int64 khronos_int64_t; +typedef unsigned __int64 khronos_uint64_t; +#define KHRONOS_SUPPORT_INT64 1 +#define KHRONOS_SUPPORT_FLOAT 1 + +#elif defined(__sun__) || defined(__digital__) + +/* + * Sun or Digital + */ +typedef int khronos_int32_t; +typedef unsigned int khronos_uint32_t; +#if defined(__arch64__) || defined(_LP64) +typedef long int khronos_int64_t; +typedef unsigned long int khronos_uint64_t; +#else +typedef long long int khronos_int64_t; +typedef unsigned long long int khronos_uint64_t; +#endif /* __arch64__ */ +#define KHRONOS_SUPPORT_INT64 1 +#define KHRONOS_SUPPORT_FLOAT 1 + +#elif 0 + +/* + * Hypothetical platform with no float or int64 support + */ +typedef int khronos_int32_t; +typedef unsigned int khronos_uint32_t; +#define KHRONOS_SUPPORT_INT64 0 +#define KHRONOS_SUPPORT_FLOAT 0 + +#else + +/* + * Generic fallback + */ +#include +typedef int32_t khronos_int32_t; +typedef uint32_t khronos_uint32_t; +typedef int64_t khronos_int64_t; +typedef uint64_t khronos_uint64_t; +#define KHRONOS_SUPPORT_INT64 1 +#define KHRONOS_SUPPORT_FLOAT 1 + +#endif + + +/* + * Types that are (so far) the same on all platforms + */ +typedef signed char khronos_int8_t; +typedef unsigned char khronos_uint8_t; +typedef signed short int khronos_int16_t; +typedef unsigned short int khronos_uint16_t; + +/* + * Types that differ between LLP64 and LP64 architectures - in LLP64, + * pointers are 64 bits, but 'long' is still 32 bits. Win64 appears + * to be the only LLP64 architecture in current use. + */ +#ifdef KHRONOS_USE_INTPTR_T +typedef intptr_t khronos_intptr_t; +typedef uintptr_t khronos_uintptr_t; +#elif defined(_WIN64) +typedef signed long long int khronos_intptr_t; +typedef unsigned long long int khronos_uintptr_t; +#else +typedef signed long int khronos_intptr_t; +typedef unsigned long int khronos_uintptr_t; +#endif + +#if defined(_WIN64) +typedef signed long long int khronos_ssize_t; +typedef unsigned long long int khronos_usize_t; +#else +typedef signed long int khronos_ssize_t; +typedef unsigned long int khronos_usize_t; +#endif + +#if KHRONOS_SUPPORT_FLOAT +/* + * Float type + */ +typedef float khronos_float_t; +#endif + +#if KHRONOS_SUPPORT_INT64 +/* Time types + * + * These types can be used to represent a time interval in nanoseconds or + * an absolute Unadjusted System Time. Unadjusted System Time is the number + * of nanoseconds since some arbitrary system event (e.g. since the last + * time the system booted). The Unadjusted System Time is an unsigned + * 64 bit value that wraps back to 0 every 584 years. Time intervals + * may be either signed or unsigned. + */ +typedef khronos_uint64_t khronos_utime_nanoseconds_t; +typedef khronos_int64_t khronos_stime_nanoseconds_t; +#endif + +/* + * Dummy value used to pad enum types to 32 bits. + */ +#ifndef KHRONOS_MAX_ENUM +#define KHRONOS_MAX_ENUM 0x7FFFFFFF +#endif + +/* + * Enumerated boolean type + * + * Values other than zero should be considered to be true. Therefore + * comparisons should not be made against KHRONOS_TRUE. + */ +typedef enum { + KHRONOS_FALSE = 0, + KHRONOS_TRUE = 1, + KHRONOS_BOOLEAN_ENUM_FORCE_SIZE = KHRONOS_MAX_ENUM +} khronos_boolean_enum_t; + +#endif /* __khrplatform_h_ */ diff --git a/extern/glad/include/glad/glad.h b/extern/glad/include/glad/glad.h new file mode 100644 index 0000000..e43afbd --- /dev/null +++ b/extern/glad/include/glad/glad.h @@ -0,0 +1,4133 @@ +/* + + OpenGL loader generated by glad 0.1.36 on Sat Apr 12 17:00:07 2025. + + Language/Generator: C/C++ + Specification: gl + APIs: gl=4.1 + Profile: compatibility + Extensions: + + Loader: True + Local files: False + Omit khrplatform: False + Reproducible: False + + Commandline: + --profile="compatibility" --api="gl=4.1" --generator="c" --spec="gl" --extensions="" + Online: + https://glad.dav1d.de/#profile=compatibility&language=c&specification=gl&loader=on&api=gl%3D4.1 +*/ + + +#ifndef __glad_h_ +#define __glad_h_ + +#ifdef __gl_h_ +#error OpenGL header already included, remove this include, glad already provides it +#endif +#define __gl_h_ + +#if defined(_WIN32) && !defined(APIENTRY) && !defined(__CYGWIN__) && !defined(__SCITECH_SNAP__) +#define APIENTRY __stdcall +#endif + +#ifndef APIENTRY +#define APIENTRY +#endif +#ifndef APIENTRYP +#define APIENTRYP APIENTRY * +#endif + +#ifndef GLAPIENTRY +#define GLAPIENTRY APIENTRY +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +struct gladGLversionStruct { + int major; + int minor; +}; + +typedef void* (* GLADloadproc)(const char *name); + +#ifndef GLAPI +# if defined(GLAD_GLAPI_EXPORT) +# if defined(_WIN32) || defined(__CYGWIN__) +# if defined(GLAD_GLAPI_EXPORT_BUILD) +# if defined(__GNUC__) +# define GLAPI __attribute__ ((dllexport)) extern +# else +# define GLAPI __declspec(dllexport) extern +# endif +# else +# if defined(__GNUC__) +# define GLAPI __attribute__ ((dllimport)) extern +# else +# define GLAPI __declspec(dllimport) extern +# endif +# endif +# elif defined(__GNUC__) && defined(GLAD_GLAPI_EXPORT_BUILD) +# define GLAPI __attribute__ ((visibility ("default"))) extern +# else +# define GLAPI extern +# endif +# else +# define GLAPI extern +# endif +#endif + +GLAPI struct gladGLversionStruct GLVersion; + +GLAPI int gladLoadGL(void); + +GLAPI int gladLoadGLLoader(GLADloadproc); + +#include +typedef unsigned int GLenum; +typedef unsigned char GLboolean; +typedef unsigned int GLbitfield; +typedef void GLvoid; +typedef khronos_int8_t GLbyte; +typedef khronos_uint8_t GLubyte; +typedef khronos_int16_t GLshort; +typedef khronos_uint16_t GLushort; +typedef int GLint; +typedef unsigned int GLuint; +typedef khronos_int32_t GLclampx; +typedef int GLsizei; +typedef khronos_float_t GLfloat; +typedef khronos_float_t GLclampf; +typedef double GLdouble; +typedef double GLclampd; +typedef void *GLeglClientBufferEXT; +typedef void *GLeglImageOES; +typedef char GLchar; +typedef char GLcharARB; +#ifdef __APPLE__ +typedef void *GLhandleARB; +#else +typedef unsigned int GLhandleARB; +#endif +typedef khronos_uint16_t GLhalf; +typedef khronos_uint16_t GLhalfARB; +typedef khronos_int32_t GLfixed; +typedef khronos_intptr_t GLintptr; +typedef khronos_intptr_t GLintptrARB; +typedef khronos_ssize_t GLsizeiptr; +typedef khronos_ssize_t GLsizeiptrARB; +typedef khronos_int64_t GLint64; +typedef khronos_int64_t GLint64EXT; +typedef khronos_uint64_t GLuint64; +typedef khronos_uint64_t GLuint64EXT; +typedef struct __GLsync *GLsync; +struct _cl_context; +struct _cl_event; +typedef void (APIENTRY *GLDEBUGPROC)(GLenum source,GLenum type,GLuint id,GLenum severity,GLsizei length,const GLchar *message,const void *userParam); +typedef void (APIENTRY *GLDEBUGPROCARB)(GLenum source,GLenum type,GLuint id,GLenum severity,GLsizei length,const GLchar *message,const void *userParam); +typedef void (APIENTRY *GLDEBUGPROCKHR)(GLenum source,GLenum type,GLuint id,GLenum severity,GLsizei length,const GLchar *message,const void *userParam); +typedef void (APIENTRY *GLDEBUGPROCAMD)(GLuint id,GLenum category,GLenum severity,GLsizei length,const GLchar *message,void *userParam); +typedef unsigned short GLhalfNV; +typedef GLintptr GLvdpauSurfaceNV; +typedef void (APIENTRY *GLVULKANPROCNV)(void); +#define GL_DEPTH_BUFFER_BIT 0x00000100 +#define GL_STENCIL_BUFFER_BIT 0x00000400 +#define GL_COLOR_BUFFER_BIT 0x00004000 +#define GL_FALSE 0 +#define GL_TRUE 1 +#define GL_POINTS 0x0000 +#define GL_LINES 0x0001 +#define GL_LINE_LOOP 0x0002 +#define GL_LINE_STRIP 0x0003 +#define GL_TRIANGLES 0x0004 +#define GL_TRIANGLE_STRIP 0x0005 +#define GL_TRIANGLE_FAN 0x0006 +#define GL_QUADS 0x0007 +#define GL_NEVER 0x0200 +#define GL_LESS 0x0201 +#define GL_EQUAL 0x0202 +#define GL_LEQUAL 0x0203 +#define GL_GREATER 0x0204 +#define GL_NOTEQUAL 0x0205 +#define GL_GEQUAL 0x0206 +#define GL_ALWAYS 0x0207 +#define GL_ZERO 0 +#define GL_ONE 1 +#define GL_SRC_COLOR 0x0300 +#define GL_ONE_MINUS_SRC_COLOR 0x0301 +#define GL_SRC_ALPHA 0x0302 +#define GL_ONE_MINUS_SRC_ALPHA 0x0303 +#define GL_DST_ALPHA 0x0304 +#define GL_ONE_MINUS_DST_ALPHA 0x0305 +#define GL_DST_COLOR 0x0306 +#define GL_ONE_MINUS_DST_COLOR 0x0307 +#define GL_SRC_ALPHA_SATURATE 0x0308 +#define GL_NONE 0 +#define GL_FRONT_LEFT 0x0400 +#define GL_FRONT_RIGHT 0x0401 +#define GL_BACK_LEFT 0x0402 +#define GL_BACK_RIGHT 0x0403 +#define GL_FRONT 0x0404 +#define GL_BACK 0x0405 +#define GL_LEFT 0x0406 +#define GL_RIGHT 0x0407 +#define GL_FRONT_AND_BACK 0x0408 +#define GL_NO_ERROR 0 +#define GL_INVALID_ENUM 0x0500 +#define GL_INVALID_VALUE 0x0501 +#define GL_INVALID_OPERATION 0x0502 +#define GL_OUT_OF_MEMORY 0x0505 +#define GL_CW 0x0900 +#define GL_CCW 0x0901 +#define GL_POINT_SIZE 0x0B11 +#define GL_POINT_SIZE_RANGE 0x0B12 +#define GL_POINT_SIZE_GRANULARITY 0x0B13 +#define GL_LINE_SMOOTH 0x0B20 +#define GL_LINE_WIDTH 0x0B21 +#define GL_LINE_WIDTH_RANGE 0x0B22 +#define GL_LINE_WIDTH_GRANULARITY 0x0B23 +#define GL_POLYGON_MODE 0x0B40 +#define GL_POLYGON_SMOOTH 0x0B41 +#define GL_CULL_FACE 0x0B44 +#define GL_CULL_FACE_MODE 0x0B45 +#define GL_FRONT_FACE 0x0B46 +#define GL_DEPTH_RANGE 0x0B70 +#define GL_DEPTH_TEST 0x0B71 +#define GL_DEPTH_WRITEMASK 0x0B72 +#define GL_DEPTH_CLEAR_VALUE 0x0B73 +#define GL_DEPTH_FUNC 0x0B74 +#define GL_STENCIL_TEST 0x0B90 +#define GL_STENCIL_CLEAR_VALUE 0x0B91 +#define GL_STENCIL_FUNC 0x0B92 +#define GL_STENCIL_VALUE_MASK 0x0B93 +#define GL_STENCIL_FAIL 0x0B94 +#define GL_STENCIL_PASS_DEPTH_FAIL 0x0B95 +#define GL_STENCIL_PASS_DEPTH_PASS 0x0B96 +#define GL_STENCIL_REF 0x0B97 +#define GL_STENCIL_WRITEMASK 0x0B98 +#define GL_VIEWPORT 0x0BA2 +#define GL_DITHER 0x0BD0 +#define GL_BLEND_DST 0x0BE0 +#define GL_BLEND_SRC 0x0BE1 +#define GL_BLEND 0x0BE2 +#define GL_LOGIC_OP_MODE 0x0BF0 +#define GL_DRAW_BUFFER 0x0C01 +#define GL_READ_BUFFER 0x0C02 +#define GL_SCISSOR_BOX 0x0C10 +#define GL_SCISSOR_TEST 0x0C11 +#define GL_COLOR_CLEAR_VALUE 0x0C22 +#define GL_COLOR_WRITEMASK 0x0C23 +#define GL_DOUBLEBUFFER 0x0C32 +#define GL_STEREO 0x0C33 +#define GL_LINE_SMOOTH_HINT 0x0C52 +#define GL_POLYGON_SMOOTH_HINT 0x0C53 +#define GL_UNPACK_SWAP_BYTES 0x0CF0 +#define GL_UNPACK_LSB_FIRST 0x0CF1 +#define GL_UNPACK_ROW_LENGTH 0x0CF2 +#define GL_UNPACK_SKIP_ROWS 0x0CF3 +#define GL_UNPACK_SKIP_PIXELS 0x0CF4 +#define GL_UNPACK_ALIGNMENT 0x0CF5 +#define GL_PACK_SWAP_BYTES 0x0D00 +#define GL_PACK_LSB_FIRST 0x0D01 +#define GL_PACK_ROW_LENGTH 0x0D02 +#define GL_PACK_SKIP_ROWS 0x0D03 +#define GL_PACK_SKIP_PIXELS 0x0D04 +#define GL_PACK_ALIGNMENT 0x0D05 +#define GL_MAX_TEXTURE_SIZE 0x0D33 +#define GL_MAX_VIEWPORT_DIMS 0x0D3A +#define GL_SUBPIXEL_BITS 0x0D50 +#define GL_TEXTURE_1D 0x0DE0 +#define GL_TEXTURE_2D 0x0DE1 +#define GL_TEXTURE_WIDTH 0x1000 +#define GL_TEXTURE_HEIGHT 0x1001 +#define GL_TEXTURE_BORDER_COLOR 0x1004 +#define GL_DONT_CARE 0x1100 +#define GL_FASTEST 0x1101 +#define GL_NICEST 0x1102 +#define GL_BYTE 0x1400 +#define GL_UNSIGNED_BYTE 0x1401 +#define GL_SHORT 0x1402 +#define GL_UNSIGNED_SHORT 0x1403 +#define GL_INT 0x1404 +#define GL_UNSIGNED_INT 0x1405 +#define GL_FLOAT 0x1406 +#define GL_STACK_OVERFLOW 0x0503 +#define GL_STACK_UNDERFLOW 0x0504 +#define GL_CLEAR 0x1500 +#define GL_AND 0x1501 +#define GL_AND_REVERSE 0x1502 +#define GL_COPY 0x1503 +#define GL_AND_INVERTED 0x1504 +#define GL_NOOP 0x1505 +#define GL_XOR 0x1506 +#define GL_OR 0x1507 +#define GL_NOR 0x1508 +#define GL_EQUIV 0x1509 +#define GL_INVERT 0x150A +#define GL_OR_REVERSE 0x150B +#define GL_COPY_INVERTED 0x150C +#define GL_OR_INVERTED 0x150D +#define GL_NAND 0x150E +#define GL_SET 0x150F +#define GL_TEXTURE 0x1702 +#define GL_COLOR 0x1800 +#define GL_DEPTH 0x1801 +#define GL_STENCIL 0x1802 +#define GL_STENCIL_INDEX 0x1901 +#define GL_DEPTH_COMPONENT 0x1902 +#define GL_RED 0x1903 +#define GL_GREEN 0x1904 +#define GL_BLUE 0x1905 +#define GL_ALPHA 0x1906 +#define GL_RGB 0x1907 +#define GL_RGBA 0x1908 +#define GL_POINT 0x1B00 +#define GL_LINE 0x1B01 +#define GL_FILL 0x1B02 +#define GL_KEEP 0x1E00 +#define GL_REPLACE 0x1E01 +#define GL_INCR 0x1E02 +#define GL_DECR 0x1E03 +#define GL_VENDOR 0x1F00 +#define GL_RENDERER 0x1F01 +#define GL_VERSION 0x1F02 +#define GL_EXTENSIONS 0x1F03 +#define GL_NEAREST 0x2600 +#define GL_LINEAR 0x2601 +#define GL_NEAREST_MIPMAP_NEAREST 0x2700 +#define GL_LINEAR_MIPMAP_NEAREST 0x2701 +#define GL_NEAREST_MIPMAP_LINEAR 0x2702 +#define GL_LINEAR_MIPMAP_LINEAR 0x2703 +#define GL_TEXTURE_MAG_FILTER 0x2800 +#define GL_TEXTURE_MIN_FILTER 0x2801 +#define GL_TEXTURE_WRAP_S 0x2802 +#define GL_TEXTURE_WRAP_T 0x2803 +#define GL_REPEAT 0x2901 +#define GL_CURRENT_BIT 0x00000001 +#define GL_POINT_BIT 0x00000002 +#define GL_LINE_BIT 0x00000004 +#define GL_POLYGON_BIT 0x00000008 +#define GL_POLYGON_STIPPLE_BIT 0x00000010 +#define GL_PIXEL_MODE_BIT 0x00000020 +#define GL_LIGHTING_BIT 0x00000040 +#define GL_FOG_BIT 0x00000080 +#define GL_ACCUM_BUFFER_BIT 0x00000200 +#define GL_VIEWPORT_BIT 0x00000800 +#define GL_TRANSFORM_BIT 0x00001000 +#define GL_ENABLE_BIT 0x00002000 +#define GL_HINT_BIT 0x00008000 +#define GL_EVAL_BIT 0x00010000 +#define GL_LIST_BIT 0x00020000 +#define GL_TEXTURE_BIT 0x00040000 +#define GL_SCISSOR_BIT 0x00080000 +#define GL_ALL_ATTRIB_BITS 0xFFFFFFFF +#define GL_QUAD_STRIP 0x0008 +#define GL_POLYGON 0x0009 +#define GL_ACCUM 0x0100 +#define GL_LOAD 0x0101 +#define GL_RETURN 0x0102 +#define GL_MULT 0x0103 +#define GL_ADD 0x0104 +#define GL_AUX0 0x0409 +#define GL_AUX1 0x040A +#define GL_AUX2 0x040B +#define GL_AUX3 0x040C +#define GL_2D 0x0600 +#define GL_3D 0x0601 +#define GL_3D_COLOR 0x0602 +#define GL_3D_COLOR_TEXTURE 0x0603 +#define GL_4D_COLOR_TEXTURE 0x0604 +#define GL_PASS_THROUGH_TOKEN 0x0700 +#define GL_POINT_TOKEN 0x0701 +#define GL_LINE_TOKEN 0x0702 +#define GL_POLYGON_TOKEN 0x0703 +#define GL_BITMAP_TOKEN 0x0704 +#define GL_DRAW_PIXEL_TOKEN 0x0705 +#define GL_COPY_PIXEL_TOKEN 0x0706 +#define GL_LINE_RESET_TOKEN 0x0707 +#define GL_EXP 0x0800 +#define GL_EXP2 0x0801 +#define GL_COEFF 0x0A00 +#define GL_ORDER 0x0A01 +#define GL_DOMAIN 0x0A02 +#define GL_PIXEL_MAP_I_TO_I 0x0C70 +#define GL_PIXEL_MAP_S_TO_S 0x0C71 +#define GL_PIXEL_MAP_I_TO_R 0x0C72 +#define GL_PIXEL_MAP_I_TO_G 0x0C73 +#define GL_PIXEL_MAP_I_TO_B 0x0C74 +#define GL_PIXEL_MAP_I_TO_A 0x0C75 +#define GL_PIXEL_MAP_R_TO_R 0x0C76 +#define GL_PIXEL_MAP_G_TO_G 0x0C77 +#define GL_PIXEL_MAP_B_TO_B 0x0C78 +#define GL_PIXEL_MAP_A_TO_A 0x0C79 +#define GL_CURRENT_COLOR 0x0B00 +#define GL_CURRENT_INDEX 0x0B01 +#define GL_CURRENT_NORMAL 0x0B02 +#define GL_CURRENT_TEXTURE_COORDS 0x0B03 +#define GL_CURRENT_RASTER_COLOR 0x0B04 +#define GL_CURRENT_RASTER_INDEX 0x0B05 +#define GL_CURRENT_RASTER_TEXTURE_COORDS 0x0B06 +#define GL_CURRENT_RASTER_POSITION 0x0B07 +#define GL_CURRENT_RASTER_POSITION_VALID 0x0B08 +#define GL_CURRENT_RASTER_DISTANCE 0x0B09 +#define GL_POINT_SMOOTH 0x0B10 +#define GL_LINE_STIPPLE 0x0B24 +#define GL_LINE_STIPPLE_PATTERN 0x0B25 +#define GL_LINE_STIPPLE_REPEAT 0x0B26 +#define GL_LIST_MODE 0x0B30 +#define GL_MAX_LIST_NESTING 0x0B31 +#define GL_LIST_BASE 0x0B32 +#define GL_LIST_INDEX 0x0B33 +#define GL_POLYGON_STIPPLE 0x0B42 +#define GL_EDGE_FLAG 0x0B43 +#define GL_LIGHTING 0x0B50 +#define GL_LIGHT_MODEL_LOCAL_VIEWER 0x0B51 +#define GL_LIGHT_MODEL_TWO_SIDE 0x0B52 +#define GL_LIGHT_MODEL_AMBIENT 0x0B53 +#define GL_SHADE_MODEL 0x0B54 +#define GL_COLOR_MATERIAL_FACE 0x0B55 +#define GL_COLOR_MATERIAL_PARAMETER 0x0B56 +#define GL_COLOR_MATERIAL 0x0B57 +#define GL_FOG 0x0B60 +#define GL_FOG_INDEX 0x0B61 +#define GL_FOG_DENSITY 0x0B62 +#define GL_FOG_START 0x0B63 +#define GL_FOG_END 0x0B64 +#define GL_FOG_MODE 0x0B65 +#define GL_FOG_COLOR 0x0B66 +#define GL_ACCUM_CLEAR_VALUE 0x0B80 +#define GL_MATRIX_MODE 0x0BA0 +#define GL_NORMALIZE 0x0BA1 +#define GL_MODELVIEW_STACK_DEPTH 0x0BA3 +#define GL_PROJECTION_STACK_DEPTH 0x0BA4 +#define GL_TEXTURE_STACK_DEPTH 0x0BA5 +#define GL_MODELVIEW_MATRIX 0x0BA6 +#define GL_PROJECTION_MATRIX 0x0BA7 +#define GL_TEXTURE_MATRIX 0x0BA8 +#define GL_ATTRIB_STACK_DEPTH 0x0BB0 +#define GL_ALPHA_TEST 0x0BC0 +#define GL_ALPHA_TEST_FUNC 0x0BC1 +#define GL_ALPHA_TEST_REF 0x0BC2 +#define GL_LOGIC_OP 0x0BF1 +#define GL_AUX_BUFFERS 0x0C00 +#define GL_INDEX_CLEAR_VALUE 0x0C20 +#define GL_INDEX_WRITEMASK 0x0C21 +#define GL_INDEX_MODE 0x0C30 +#define GL_RGBA_MODE 0x0C31 +#define GL_RENDER_MODE 0x0C40 +#define GL_PERSPECTIVE_CORRECTION_HINT 0x0C50 +#define GL_POINT_SMOOTH_HINT 0x0C51 +#define GL_FOG_HINT 0x0C54 +#define GL_TEXTURE_GEN_S 0x0C60 +#define GL_TEXTURE_GEN_T 0x0C61 +#define GL_TEXTURE_GEN_R 0x0C62 +#define GL_TEXTURE_GEN_Q 0x0C63 +#define GL_PIXEL_MAP_I_TO_I_SIZE 0x0CB0 +#define GL_PIXEL_MAP_S_TO_S_SIZE 0x0CB1 +#define GL_PIXEL_MAP_I_TO_R_SIZE 0x0CB2 +#define GL_PIXEL_MAP_I_TO_G_SIZE 0x0CB3 +#define GL_PIXEL_MAP_I_TO_B_SIZE 0x0CB4 +#define GL_PIXEL_MAP_I_TO_A_SIZE 0x0CB5 +#define GL_PIXEL_MAP_R_TO_R_SIZE 0x0CB6 +#define GL_PIXEL_MAP_G_TO_G_SIZE 0x0CB7 +#define GL_PIXEL_MAP_B_TO_B_SIZE 0x0CB8 +#define GL_PIXEL_MAP_A_TO_A_SIZE 0x0CB9 +#define GL_MAP_COLOR 0x0D10 +#define GL_MAP_STENCIL 0x0D11 +#define GL_INDEX_SHIFT 0x0D12 +#define GL_INDEX_OFFSET 0x0D13 +#define GL_RED_SCALE 0x0D14 +#define GL_RED_BIAS 0x0D15 +#define GL_ZOOM_X 0x0D16 +#define GL_ZOOM_Y 0x0D17 +#define GL_GREEN_SCALE 0x0D18 +#define GL_GREEN_BIAS 0x0D19 +#define GL_BLUE_SCALE 0x0D1A +#define GL_BLUE_BIAS 0x0D1B +#define GL_ALPHA_SCALE 0x0D1C +#define GL_ALPHA_BIAS 0x0D1D +#define GL_DEPTH_SCALE 0x0D1E +#define GL_DEPTH_BIAS 0x0D1F +#define GL_MAX_EVAL_ORDER 0x0D30 +#define GL_MAX_LIGHTS 0x0D31 +#define GL_MAX_CLIP_PLANES 0x0D32 +#define GL_MAX_PIXEL_MAP_TABLE 0x0D34 +#define GL_MAX_ATTRIB_STACK_DEPTH 0x0D35 +#define GL_MAX_MODELVIEW_STACK_DEPTH 0x0D36 +#define GL_MAX_NAME_STACK_DEPTH 0x0D37 +#define GL_MAX_PROJECTION_STACK_DEPTH 0x0D38 +#define GL_MAX_TEXTURE_STACK_DEPTH 0x0D39 +#define GL_INDEX_BITS 0x0D51 +#define GL_RED_BITS 0x0D52 +#define GL_GREEN_BITS 0x0D53 +#define GL_BLUE_BITS 0x0D54 +#define GL_ALPHA_BITS 0x0D55 +#define GL_DEPTH_BITS 0x0D56 +#define GL_STENCIL_BITS 0x0D57 +#define GL_ACCUM_RED_BITS 0x0D58 +#define GL_ACCUM_GREEN_BITS 0x0D59 +#define GL_ACCUM_BLUE_BITS 0x0D5A +#define GL_ACCUM_ALPHA_BITS 0x0D5B +#define GL_NAME_STACK_DEPTH 0x0D70 +#define GL_AUTO_NORMAL 0x0D80 +#define GL_MAP1_COLOR_4 0x0D90 +#define GL_MAP1_INDEX 0x0D91 +#define GL_MAP1_NORMAL 0x0D92 +#define GL_MAP1_TEXTURE_COORD_1 0x0D93 +#define GL_MAP1_TEXTURE_COORD_2 0x0D94 +#define GL_MAP1_TEXTURE_COORD_3 0x0D95 +#define GL_MAP1_TEXTURE_COORD_4 0x0D96 +#define GL_MAP1_VERTEX_3 0x0D97 +#define GL_MAP1_VERTEX_4 0x0D98 +#define GL_MAP2_COLOR_4 0x0DB0 +#define GL_MAP2_INDEX 0x0DB1 +#define GL_MAP2_NORMAL 0x0DB2 +#define GL_MAP2_TEXTURE_COORD_1 0x0DB3 +#define GL_MAP2_TEXTURE_COORD_2 0x0DB4 +#define GL_MAP2_TEXTURE_COORD_3 0x0DB5 +#define GL_MAP2_TEXTURE_COORD_4 0x0DB6 +#define GL_MAP2_VERTEX_3 0x0DB7 +#define GL_MAP2_VERTEX_4 0x0DB8 +#define GL_MAP1_GRID_DOMAIN 0x0DD0 +#define GL_MAP1_GRID_SEGMENTS 0x0DD1 +#define GL_MAP2_GRID_DOMAIN 0x0DD2 +#define GL_MAP2_GRID_SEGMENTS 0x0DD3 +#define GL_TEXTURE_COMPONENTS 0x1003 +#define GL_TEXTURE_BORDER 0x1005 +#define GL_AMBIENT 0x1200 +#define GL_DIFFUSE 0x1201 +#define GL_SPECULAR 0x1202 +#define GL_POSITION 0x1203 +#define GL_SPOT_DIRECTION 0x1204 +#define GL_SPOT_EXPONENT 0x1205 +#define GL_SPOT_CUTOFF 0x1206 +#define GL_CONSTANT_ATTENUATION 0x1207 +#define GL_LINEAR_ATTENUATION 0x1208 +#define GL_QUADRATIC_ATTENUATION 0x1209 +#define GL_COMPILE 0x1300 +#define GL_COMPILE_AND_EXECUTE 0x1301 +#define GL_2_BYTES 0x1407 +#define GL_3_BYTES 0x1408 +#define GL_4_BYTES 0x1409 +#define GL_EMISSION 0x1600 +#define GL_SHININESS 0x1601 +#define GL_AMBIENT_AND_DIFFUSE 0x1602 +#define GL_COLOR_INDEXES 0x1603 +#define GL_MODELVIEW 0x1700 +#define GL_PROJECTION 0x1701 +#define GL_COLOR_INDEX 0x1900 +#define GL_LUMINANCE 0x1909 +#define GL_LUMINANCE_ALPHA 0x190A +#define GL_BITMAP 0x1A00 +#define GL_RENDER 0x1C00 +#define GL_FEEDBACK 0x1C01 +#define GL_SELECT 0x1C02 +#define GL_FLAT 0x1D00 +#define GL_SMOOTH 0x1D01 +#define GL_S 0x2000 +#define GL_T 0x2001 +#define GL_R 0x2002 +#define GL_Q 0x2003 +#define GL_MODULATE 0x2100 +#define GL_DECAL 0x2101 +#define GL_TEXTURE_ENV_MODE 0x2200 +#define GL_TEXTURE_ENV_COLOR 0x2201 +#define GL_TEXTURE_ENV 0x2300 +#define GL_EYE_LINEAR 0x2400 +#define GL_OBJECT_LINEAR 0x2401 +#define GL_SPHERE_MAP 0x2402 +#define GL_TEXTURE_GEN_MODE 0x2500 +#define GL_OBJECT_PLANE 0x2501 +#define GL_EYE_PLANE 0x2502 +#define GL_CLAMP 0x2900 +#define GL_CLIP_PLANE0 0x3000 +#define GL_CLIP_PLANE1 0x3001 +#define GL_CLIP_PLANE2 0x3002 +#define GL_CLIP_PLANE3 0x3003 +#define GL_CLIP_PLANE4 0x3004 +#define GL_CLIP_PLANE5 0x3005 +#define GL_LIGHT0 0x4000 +#define GL_LIGHT1 0x4001 +#define GL_LIGHT2 0x4002 +#define GL_LIGHT3 0x4003 +#define GL_LIGHT4 0x4004 +#define GL_LIGHT5 0x4005 +#define GL_LIGHT6 0x4006 +#define GL_LIGHT7 0x4007 +#define GL_COLOR_LOGIC_OP 0x0BF2 +#define GL_POLYGON_OFFSET_UNITS 0x2A00 +#define GL_POLYGON_OFFSET_POINT 0x2A01 +#define GL_POLYGON_OFFSET_LINE 0x2A02 +#define GL_POLYGON_OFFSET_FILL 0x8037 +#define GL_POLYGON_OFFSET_FACTOR 0x8038 +#define GL_TEXTURE_BINDING_1D 0x8068 +#define GL_TEXTURE_BINDING_2D 0x8069 +#define GL_TEXTURE_INTERNAL_FORMAT 0x1003 +#define GL_TEXTURE_RED_SIZE 0x805C +#define GL_TEXTURE_GREEN_SIZE 0x805D +#define GL_TEXTURE_BLUE_SIZE 0x805E +#define GL_TEXTURE_ALPHA_SIZE 0x805F +#define GL_DOUBLE 0x140A +#define GL_PROXY_TEXTURE_1D 0x8063 +#define GL_PROXY_TEXTURE_2D 0x8064 +#define GL_R3_G3_B2 0x2A10 +#define GL_RGB4 0x804F +#define GL_RGB5 0x8050 +#define GL_RGB8 0x8051 +#define GL_RGB10 0x8052 +#define GL_RGB12 0x8053 +#define GL_RGB16 0x8054 +#define GL_RGBA2 0x8055 +#define GL_RGBA4 0x8056 +#define GL_RGB5_A1 0x8057 +#define GL_RGBA8 0x8058 +#define GL_RGB10_A2 0x8059 +#define GL_RGBA12 0x805A +#define GL_RGBA16 0x805B +#define GL_CLIENT_PIXEL_STORE_BIT 0x00000001 +#define GL_CLIENT_VERTEX_ARRAY_BIT 0x00000002 +#define GL_CLIENT_ALL_ATTRIB_BITS 0xFFFFFFFF +#define GL_VERTEX_ARRAY_POINTER 0x808E +#define GL_NORMAL_ARRAY_POINTER 0x808F +#define GL_COLOR_ARRAY_POINTER 0x8090 +#define GL_INDEX_ARRAY_POINTER 0x8091 +#define GL_TEXTURE_COORD_ARRAY_POINTER 0x8092 +#define GL_EDGE_FLAG_ARRAY_POINTER 0x8093 +#define GL_FEEDBACK_BUFFER_POINTER 0x0DF0 +#define GL_SELECTION_BUFFER_POINTER 0x0DF3 +#define GL_CLIENT_ATTRIB_STACK_DEPTH 0x0BB1 +#define GL_INDEX_LOGIC_OP 0x0BF1 +#define GL_MAX_CLIENT_ATTRIB_STACK_DEPTH 0x0D3B +#define GL_FEEDBACK_BUFFER_SIZE 0x0DF1 +#define GL_FEEDBACK_BUFFER_TYPE 0x0DF2 +#define GL_SELECTION_BUFFER_SIZE 0x0DF4 +#define GL_VERTEX_ARRAY 0x8074 +#define GL_NORMAL_ARRAY 0x8075 +#define GL_COLOR_ARRAY 0x8076 +#define GL_INDEX_ARRAY 0x8077 +#define GL_TEXTURE_COORD_ARRAY 0x8078 +#define GL_EDGE_FLAG_ARRAY 0x8079 +#define GL_VERTEX_ARRAY_SIZE 0x807A +#define GL_VERTEX_ARRAY_TYPE 0x807B +#define GL_VERTEX_ARRAY_STRIDE 0x807C +#define GL_NORMAL_ARRAY_TYPE 0x807E +#define GL_NORMAL_ARRAY_STRIDE 0x807F +#define GL_COLOR_ARRAY_SIZE 0x8081 +#define GL_COLOR_ARRAY_TYPE 0x8082 +#define GL_COLOR_ARRAY_STRIDE 0x8083 +#define GL_INDEX_ARRAY_TYPE 0x8085 +#define GL_INDEX_ARRAY_STRIDE 0x8086 +#define GL_TEXTURE_COORD_ARRAY_SIZE 0x8088 +#define GL_TEXTURE_COORD_ARRAY_TYPE 0x8089 +#define GL_TEXTURE_COORD_ARRAY_STRIDE 0x808A +#define GL_EDGE_FLAG_ARRAY_STRIDE 0x808C +#define GL_TEXTURE_LUMINANCE_SIZE 0x8060 +#define GL_TEXTURE_INTENSITY_SIZE 0x8061 +#define GL_TEXTURE_PRIORITY 0x8066 +#define GL_TEXTURE_RESIDENT 0x8067 +#define GL_ALPHA4 0x803B +#define GL_ALPHA8 0x803C +#define GL_ALPHA12 0x803D +#define GL_ALPHA16 0x803E +#define GL_LUMINANCE4 0x803F +#define GL_LUMINANCE8 0x8040 +#define GL_LUMINANCE12 0x8041 +#define GL_LUMINANCE16 0x8042 +#define GL_LUMINANCE4_ALPHA4 0x8043 +#define GL_LUMINANCE6_ALPHA2 0x8044 +#define GL_LUMINANCE8_ALPHA8 0x8045 +#define GL_LUMINANCE12_ALPHA4 0x8046 +#define GL_LUMINANCE12_ALPHA12 0x8047 +#define GL_LUMINANCE16_ALPHA16 0x8048 +#define GL_INTENSITY 0x8049 +#define GL_INTENSITY4 0x804A +#define GL_INTENSITY8 0x804B +#define GL_INTENSITY12 0x804C +#define GL_INTENSITY16 0x804D +#define GL_V2F 0x2A20 +#define GL_V3F 0x2A21 +#define GL_C4UB_V2F 0x2A22 +#define GL_C4UB_V3F 0x2A23 +#define GL_C3F_V3F 0x2A24 +#define GL_N3F_V3F 0x2A25 +#define GL_C4F_N3F_V3F 0x2A26 +#define GL_T2F_V3F 0x2A27 +#define GL_T4F_V4F 0x2A28 +#define GL_T2F_C4UB_V3F 0x2A29 +#define GL_T2F_C3F_V3F 0x2A2A +#define GL_T2F_N3F_V3F 0x2A2B +#define GL_T2F_C4F_N3F_V3F 0x2A2C +#define GL_T4F_C4F_N3F_V4F 0x2A2D +#define GL_UNSIGNED_BYTE_3_3_2 0x8032 +#define GL_UNSIGNED_SHORT_4_4_4_4 0x8033 +#define GL_UNSIGNED_SHORT_5_5_5_1 0x8034 +#define GL_UNSIGNED_INT_8_8_8_8 0x8035 +#define GL_UNSIGNED_INT_10_10_10_2 0x8036 +#define GL_TEXTURE_BINDING_3D 0x806A +#define GL_PACK_SKIP_IMAGES 0x806B +#define GL_PACK_IMAGE_HEIGHT 0x806C +#define GL_UNPACK_SKIP_IMAGES 0x806D +#define GL_UNPACK_IMAGE_HEIGHT 0x806E +#define GL_TEXTURE_3D 0x806F +#define GL_PROXY_TEXTURE_3D 0x8070 +#define GL_TEXTURE_DEPTH 0x8071 +#define GL_TEXTURE_WRAP_R 0x8072 +#define GL_MAX_3D_TEXTURE_SIZE 0x8073 +#define GL_UNSIGNED_BYTE_2_3_3_REV 0x8362 +#define GL_UNSIGNED_SHORT_5_6_5 0x8363 +#define GL_UNSIGNED_SHORT_5_6_5_REV 0x8364 +#define GL_UNSIGNED_SHORT_4_4_4_4_REV 0x8365 +#define GL_UNSIGNED_SHORT_1_5_5_5_REV 0x8366 +#define GL_UNSIGNED_INT_8_8_8_8_REV 0x8367 +#define GL_UNSIGNED_INT_2_10_10_10_REV 0x8368 +#define GL_BGR 0x80E0 +#define GL_BGRA 0x80E1 +#define GL_MAX_ELEMENTS_VERTICES 0x80E8 +#define GL_MAX_ELEMENTS_INDICES 0x80E9 +#define GL_CLAMP_TO_EDGE 0x812F +#define GL_TEXTURE_MIN_LOD 0x813A +#define GL_TEXTURE_MAX_LOD 0x813B +#define GL_TEXTURE_BASE_LEVEL 0x813C +#define GL_TEXTURE_MAX_LEVEL 0x813D +#define GL_SMOOTH_POINT_SIZE_RANGE 0x0B12 +#define GL_SMOOTH_POINT_SIZE_GRANULARITY 0x0B13 +#define GL_SMOOTH_LINE_WIDTH_RANGE 0x0B22 +#define GL_SMOOTH_LINE_WIDTH_GRANULARITY 0x0B23 +#define GL_ALIASED_LINE_WIDTH_RANGE 0x846E +#define GL_RESCALE_NORMAL 0x803A +#define GL_LIGHT_MODEL_COLOR_CONTROL 0x81F8 +#define GL_SINGLE_COLOR 0x81F9 +#define GL_SEPARATE_SPECULAR_COLOR 0x81FA +#define GL_ALIASED_POINT_SIZE_RANGE 0x846D +#define GL_TEXTURE0 0x84C0 +#define GL_TEXTURE1 0x84C1 +#define GL_TEXTURE2 0x84C2 +#define GL_TEXTURE3 0x84C3 +#define GL_TEXTURE4 0x84C4 +#define GL_TEXTURE5 0x84C5 +#define GL_TEXTURE6 0x84C6 +#define GL_TEXTURE7 0x84C7 +#define GL_TEXTURE8 0x84C8 +#define GL_TEXTURE9 0x84C9 +#define GL_TEXTURE10 0x84CA +#define GL_TEXTURE11 0x84CB +#define GL_TEXTURE12 0x84CC +#define GL_TEXTURE13 0x84CD +#define GL_TEXTURE14 0x84CE +#define GL_TEXTURE15 0x84CF +#define GL_TEXTURE16 0x84D0 +#define GL_TEXTURE17 0x84D1 +#define GL_TEXTURE18 0x84D2 +#define GL_TEXTURE19 0x84D3 +#define GL_TEXTURE20 0x84D4 +#define GL_TEXTURE21 0x84D5 +#define GL_TEXTURE22 0x84D6 +#define GL_TEXTURE23 0x84D7 +#define GL_TEXTURE24 0x84D8 +#define GL_TEXTURE25 0x84D9 +#define GL_TEXTURE26 0x84DA +#define GL_TEXTURE27 0x84DB +#define GL_TEXTURE28 0x84DC +#define GL_TEXTURE29 0x84DD +#define GL_TEXTURE30 0x84DE +#define GL_TEXTURE31 0x84DF +#define GL_ACTIVE_TEXTURE 0x84E0 +#define GL_MULTISAMPLE 0x809D +#define GL_SAMPLE_ALPHA_TO_COVERAGE 0x809E +#define GL_SAMPLE_ALPHA_TO_ONE 0x809F +#define GL_SAMPLE_COVERAGE 0x80A0 +#define GL_SAMPLE_BUFFERS 0x80A8 +#define GL_SAMPLES 0x80A9 +#define GL_SAMPLE_COVERAGE_VALUE 0x80AA +#define GL_SAMPLE_COVERAGE_INVERT 0x80AB +#define GL_TEXTURE_CUBE_MAP 0x8513 +#define GL_TEXTURE_BINDING_CUBE_MAP 0x8514 +#define GL_TEXTURE_CUBE_MAP_POSITIVE_X 0x8515 +#define GL_TEXTURE_CUBE_MAP_NEGATIVE_X 0x8516 +#define GL_TEXTURE_CUBE_MAP_POSITIVE_Y 0x8517 +#define GL_TEXTURE_CUBE_MAP_NEGATIVE_Y 0x8518 +#define GL_TEXTURE_CUBE_MAP_POSITIVE_Z 0x8519 +#define GL_TEXTURE_CUBE_MAP_NEGATIVE_Z 0x851A +#define GL_PROXY_TEXTURE_CUBE_MAP 0x851B +#define GL_MAX_CUBE_MAP_TEXTURE_SIZE 0x851C +#define GL_COMPRESSED_RGB 0x84ED +#define GL_COMPRESSED_RGBA 0x84EE +#define GL_TEXTURE_COMPRESSION_HINT 0x84EF +#define GL_TEXTURE_COMPRESSED_IMAGE_SIZE 0x86A0 +#define GL_TEXTURE_COMPRESSED 0x86A1 +#define GL_NUM_COMPRESSED_TEXTURE_FORMATS 0x86A2 +#define GL_COMPRESSED_TEXTURE_FORMATS 0x86A3 +#define GL_CLAMP_TO_BORDER 0x812D +#define GL_CLIENT_ACTIVE_TEXTURE 0x84E1 +#define GL_MAX_TEXTURE_UNITS 0x84E2 +#define GL_TRANSPOSE_MODELVIEW_MATRIX 0x84E3 +#define GL_TRANSPOSE_PROJECTION_MATRIX 0x84E4 +#define GL_TRANSPOSE_TEXTURE_MATRIX 0x84E5 +#define GL_TRANSPOSE_COLOR_MATRIX 0x84E6 +#define GL_MULTISAMPLE_BIT 0x20000000 +#define GL_NORMAL_MAP 0x8511 +#define GL_REFLECTION_MAP 0x8512 +#define GL_COMPRESSED_ALPHA 0x84E9 +#define GL_COMPRESSED_LUMINANCE 0x84EA +#define GL_COMPRESSED_LUMINANCE_ALPHA 0x84EB +#define GL_COMPRESSED_INTENSITY 0x84EC +#define GL_COMBINE 0x8570 +#define GL_COMBINE_RGB 0x8571 +#define GL_COMBINE_ALPHA 0x8572 +#define GL_SOURCE0_RGB 0x8580 +#define GL_SOURCE1_RGB 0x8581 +#define GL_SOURCE2_RGB 0x8582 +#define GL_SOURCE0_ALPHA 0x8588 +#define GL_SOURCE1_ALPHA 0x8589 +#define GL_SOURCE2_ALPHA 0x858A +#define GL_OPERAND0_RGB 0x8590 +#define GL_OPERAND1_RGB 0x8591 +#define GL_OPERAND2_RGB 0x8592 +#define GL_OPERAND0_ALPHA 0x8598 +#define GL_OPERAND1_ALPHA 0x8599 +#define GL_OPERAND2_ALPHA 0x859A +#define GL_RGB_SCALE 0x8573 +#define GL_ADD_SIGNED 0x8574 +#define GL_INTERPOLATE 0x8575 +#define GL_SUBTRACT 0x84E7 +#define GL_CONSTANT 0x8576 +#define GL_PRIMARY_COLOR 0x8577 +#define GL_PREVIOUS 0x8578 +#define GL_DOT3_RGB 0x86AE +#define GL_DOT3_RGBA 0x86AF +#define GL_BLEND_DST_RGB 0x80C8 +#define GL_BLEND_SRC_RGB 0x80C9 +#define GL_BLEND_DST_ALPHA 0x80CA +#define GL_BLEND_SRC_ALPHA 0x80CB +#define GL_POINT_FADE_THRESHOLD_SIZE 0x8128 +#define GL_DEPTH_COMPONENT16 0x81A5 +#define GL_DEPTH_COMPONENT24 0x81A6 +#define GL_DEPTH_COMPONENT32 0x81A7 +#define GL_MIRRORED_REPEAT 0x8370 +#define GL_MAX_TEXTURE_LOD_BIAS 0x84FD +#define GL_TEXTURE_LOD_BIAS 0x8501 +#define GL_INCR_WRAP 0x8507 +#define GL_DECR_WRAP 0x8508 +#define GL_TEXTURE_DEPTH_SIZE 0x884A +#define GL_TEXTURE_COMPARE_MODE 0x884C +#define GL_TEXTURE_COMPARE_FUNC 0x884D +#define GL_POINT_SIZE_MIN 0x8126 +#define GL_POINT_SIZE_MAX 0x8127 +#define GL_POINT_DISTANCE_ATTENUATION 0x8129 +#define GL_GENERATE_MIPMAP 0x8191 +#define GL_GENERATE_MIPMAP_HINT 0x8192 +#define GL_FOG_COORDINATE_SOURCE 0x8450 +#define GL_FOG_COORDINATE 0x8451 +#define GL_FRAGMENT_DEPTH 0x8452 +#define GL_CURRENT_FOG_COORDINATE 0x8453 +#define GL_FOG_COORDINATE_ARRAY_TYPE 0x8454 +#define GL_FOG_COORDINATE_ARRAY_STRIDE 0x8455 +#define GL_FOG_COORDINATE_ARRAY_POINTER 0x8456 +#define GL_FOG_COORDINATE_ARRAY 0x8457 +#define GL_COLOR_SUM 0x8458 +#define GL_CURRENT_SECONDARY_COLOR 0x8459 +#define GL_SECONDARY_COLOR_ARRAY_SIZE 0x845A +#define GL_SECONDARY_COLOR_ARRAY_TYPE 0x845B +#define GL_SECONDARY_COLOR_ARRAY_STRIDE 0x845C +#define GL_SECONDARY_COLOR_ARRAY_POINTER 0x845D +#define GL_SECONDARY_COLOR_ARRAY 0x845E +#define GL_TEXTURE_FILTER_CONTROL 0x8500 +#define GL_DEPTH_TEXTURE_MODE 0x884B +#define GL_COMPARE_R_TO_TEXTURE 0x884E +#define GL_BLEND_COLOR 0x8005 +#define GL_BLEND_EQUATION 0x8009 +#define GL_CONSTANT_COLOR 0x8001 +#define GL_ONE_MINUS_CONSTANT_COLOR 0x8002 +#define GL_CONSTANT_ALPHA 0x8003 +#define GL_ONE_MINUS_CONSTANT_ALPHA 0x8004 +#define GL_FUNC_ADD 0x8006 +#define GL_FUNC_REVERSE_SUBTRACT 0x800B +#define GL_FUNC_SUBTRACT 0x800A +#define GL_MIN 0x8007 +#define GL_MAX 0x8008 +#define GL_BUFFER_SIZE 0x8764 +#define GL_BUFFER_USAGE 0x8765 +#define GL_QUERY_COUNTER_BITS 0x8864 +#define GL_CURRENT_QUERY 0x8865 +#define GL_QUERY_RESULT 0x8866 +#define GL_QUERY_RESULT_AVAILABLE 0x8867 +#define GL_ARRAY_BUFFER 0x8892 +#define GL_ELEMENT_ARRAY_BUFFER 0x8893 +#define GL_ARRAY_BUFFER_BINDING 0x8894 +#define GL_ELEMENT_ARRAY_BUFFER_BINDING 0x8895 +#define GL_VERTEX_ATTRIB_ARRAY_BUFFER_BINDING 0x889F +#define GL_READ_ONLY 0x88B8 +#define GL_WRITE_ONLY 0x88B9 +#define GL_READ_WRITE 0x88BA +#define GL_BUFFER_ACCESS 0x88BB +#define GL_BUFFER_MAPPED 0x88BC +#define GL_BUFFER_MAP_POINTER 0x88BD +#define GL_STREAM_DRAW 0x88E0 +#define GL_STREAM_READ 0x88E1 +#define GL_STREAM_COPY 0x88E2 +#define GL_STATIC_DRAW 0x88E4 +#define GL_STATIC_READ 0x88E5 +#define GL_STATIC_COPY 0x88E6 +#define GL_DYNAMIC_DRAW 0x88E8 +#define GL_DYNAMIC_READ 0x88E9 +#define GL_DYNAMIC_COPY 0x88EA +#define GL_SAMPLES_PASSED 0x8914 +#define GL_SRC1_ALPHA 0x8589 +#define GL_VERTEX_ARRAY_BUFFER_BINDING 0x8896 +#define GL_NORMAL_ARRAY_BUFFER_BINDING 0x8897 +#define GL_COLOR_ARRAY_BUFFER_BINDING 0x8898 +#define GL_INDEX_ARRAY_BUFFER_BINDING 0x8899 +#define GL_TEXTURE_COORD_ARRAY_BUFFER_BINDING 0x889A +#define GL_EDGE_FLAG_ARRAY_BUFFER_BINDING 0x889B +#define GL_SECONDARY_COLOR_ARRAY_BUFFER_BINDING 0x889C +#define GL_FOG_COORDINATE_ARRAY_BUFFER_BINDING 0x889D +#define GL_WEIGHT_ARRAY_BUFFER_BINDING 0x889E +#define GL_FOG_COORD_SRC 0x8450 +#define GL_FOG_COORD 0x8451 +#define GL_CURRENT_FOG_COORD 0x8453 +#define GL_FOG_COORD_ARRAY_TYPE 0x8454 +#define GL_FOG_COORD_ARRAY_STRIDE 0x8455 +#define GL_FOG_COORD_ARRAY_POINTER 0x8456 +#define GL_FOG_COORD_ARRAY 0x8457 +#define GL_FOG_COORD_ARRAY_BUFFER_BINDING 0x889D +#define GL_SRC0_RGB 0x8580 +#define GL_SRC1_RGB 0x8581 +#define GL_SRC2_RGB 0x8582 +#define GL_SRC0_ALPHA 0x8588 +#define GL_SRC2_ALPHA 0x858A +#define GL_BLEND_EQUATION_RGB 0x8009 +#define GL_VERTEX_ATTRIB_ARRAY_ENABLED 0x8622 +#define GL_VERTEX_ATTRIB_ARRAY_SIZE 0x8623 +#define GL_VERTEX_ATTRIB_ARRAY_STRIDE 0x8624 +#define GL_VERTEX_ATTRIB_ARRAY_TYPE 0x8625 +#define GL_CURRENT_VERTEX_ATTRIB 0x8626 +#define GL_VERTEX_PROGRAM_POINT_SIZE 0x8642 +#define GL_VERTEX_ATTRIB_ARRAY_POINTER 0x8645 +#define GL_STENCIL_BACK_FUNC 0x8800 +#define GL_STENCIL_BACK_FAIL 0x8801 +#define GL_STENCIL_BACK_PASS_DEPTH_FAIL 0x8802 +#define GL_STENCIL_BACK_PASS_DEPTH_PASS 0x8803 +#define GL_MAX_DRAW_BUFFERS 0x8824 +#define GL_DRAW_BUFFER0 0x8825 +#define GL_DRAW_BUFFER1 0x8826 +#define GL_DRAW_BUFFER2 0x8827 +#define GL_DRAW_BUFFER3 0x8828 +#define GL_DRAW_BUFFER4 0x8829 +#define GL_DRAW_BUFFER5 0x882A +#define GL_DRAW_BUFFER6 0x882B +#define GL_DRAW_BUFFER7 0x882C +#define GL_DRAW_BUFFER8 0x882D +#define GL_DRAW_BUFFER9 0x882E +#define GL_DRAW_BUFFER10 0x882F +#define GL_DRAW_BUFFER11 0x8830 +#define GL_DRAW_BUFFER12 0x8831 +#define GL_DRAW_BUFFER13 0x8832 +#define GL_DRAW_BUFFER14 0x8833 +#define GL_DRAW_BUFFER15 0x8834 +#define GL_BLEND_EQUATION_ALPHA 0x883D +#define GL_MAX_VERTEX_ATTRIBS 0x8869 +#define GL_VERTEX_ATTRIB_ARRAY_NORMALIZED 0x886A +#define GL_MAX_TEXTURE_IMAGE_UNITS 0x8872 +#define GL_FRAGMENT_SHADER 0x8B30 +#define GL_VERTEX_SHADER 0x8B31 +#define GL_MAX_FRAGMENT_UNIFORM_COMPONENTS 0x8B49 +#define GL_MAX_VERTEX_UNIFORM_COMPONENTS 0x8B4A +#define GL_MAX_VARYING_FLOATS 0x8B4B +#define GL_MAX_VERTEX_TEXTURE_IMAGE_UNITS 0x8B4C +#define GL_MAX_COMBINED_TEXTURE_IMAGE_UNITS 0x8B4D +#define GL_SHADER_TYPE 0x8B4F +#define GL_FLOAT_VEC2 0x8B50 +#define GL_FLOAT_VEC3 0x8B51 +#define GL_FLOAT_VEC4 0x8B52 +#define GL_INT_VEC2 0x8B53 +#define GL_INT_VEC3 0x8B54 +#define GL_INT_VEC4 0x8B55 +#define GL_BOOL 0x8B56 +#define GL_BOOL_VEC2 0x8B57 +#define GL_BOOL_VEC3 0x8B58 +#define GL_BOOL_VEC4 0x8B59 +#define GL_FLOAT_MAT2 0x8B5A +#define GL_FLOAT_MAT3 0x8B5B +#define GL_FLOAT_MAT4 0x8B5C +#define GL_SAMPLER_1D 0x8B5D +#define GL_SAMPLER_2D 0x8B5E +#define GL_SAMPLER_3D 0x8B5F +#define GL_SAMPLER_CUBE 0x8B60 +#define GL_SAMPLER_1D_SHADOW 0x8B61 +#define GL_SAMPLER_2D_SHADOW 0x8B62 +#define GL_DELETE_STATUS 0x8B80 +#define GL_COMPILE_STATUS 0x8B81 +#define GL_LINK_STATUS 0x8B82 +#define GL_VALIDATE_STATUS 0x8B83 +#define GL_INFO_LOG_LENGTH 0x8B84 +#define GL_ATTACHED_SHADERS 0x8B85 +#define GL_ACTIVE_UNIFORMS 0x8B86 +#define GL_ACTIVE_UNIFORM_MAX_LENGTH 0x8B87 +#define GL_SHADER_SOURCE_LENGTH 0x8B88 +#define GL_ACTIVE_ATTRIBUTES 0x8B89 +#define GL_ACTIVE_ATTRIBUTE_MAX_LENGTH 0x8B8A +#define GL_FRAGMENT_SHADER_DERIVATIVE_HINT 0x8B8B +#define GL_SHADING_LANGUAGE_VERSION 0x8B8C +#define GL_CURRENT_PROGRAM 0x8B8D +#define GL_POINT_SPRITE_COORD_ORIGIN 0x8CA0 +#define GL_LOWER_LEFT 0x8CA1 +#define GL_UPPER_LEFT 0x8CA2 +#define GL_STENCIL_BACK_REF 0x8CA3 +#define GL_STENCIL_BACK_VALUE_MASK 0x8CA4 +#define GL_STENCIL_BACK_WRITEMASK 0x8CA5 +#define GL_VERTEX_PROGRAM_TWO_SIDE 0x8643 +#define GL_POINT_SPRITE 0x8861 +#define GL_COORD_REPLACE 0x8862 +#define GL_MAX_TEXTURE_COORDS 0x8871 +#define GL_PIXEL_PACK_BUFFER 0x88EB +#define GL_PIXEL_UNPACK_BUFFER 0x88EC +#define GL_PIXEL_PACK_BUFFER_BINDING 0x88ED +#define GL_PIXEL_UNPACK_BUFFER_BINDING 0x88EF +#define GL_FLOAT_MAT2x3 0x8B65 +#define GL_FLOAT_MAT2x4 0x8B66 +#define GL_FLOAT_MAT3x2 0x8B67 +#define GL_FLOAT_MAT3x4 0x8B68 +#define GL_FLOAT_MAT4x2 0x8B69 +#define GL_FLOAT_MAT4x3 0x8B6A +#define GL_SRGB 0x8C40 +#define GL_SRGB8 0x8C41 +#define GL_SRGB_ALPHA 0x8C42 +#define GL_SRGB8_ALPHA8 0x8C43 +#define GL_COMPRESSED_SRGB 0x8C48 +#define GL_COMPRESSED_SRGB_ALPHA 0x8C49 +#define GL_CURRENT_RASTER_SECONDARY_COLOR 0x845F +#define GL_SLUMINANCE_ALPHA 0x8C44 +#define GL_SLUMINANCE8_ALPHA8 0x8C45 +#define GL_SLUMINANCE 0x8C46 +#define GL_SLUMINANCE8 0x8C47 +#define GL_COMPRESSED_SLUMINANCE 0x8C4A +#define GL_COMPRESSED_SLUMINANCE_ALPHA 0x8C4B +#define GL_COMPARE_REF_TO_TEXTURE 0x884E +#define GL_CLIP_DISTANCE0 0x3000 +#define GL_CLIP_DISTANCE1 0x3001 +#define GL_CLIP_DISTANCE2 0x3002 +#define GL_CLIP_DISTANCE3 0x3003 +#define GL_CLIP_DISTANCE4 0x3004 +#define GL_CLIP_DISTANCE5 0x3005 +#define GL_CLIP_DISTANCE6 0x3006 +#define GL_CLIP_DISTANCE7 0x3007 +#define GL_MAX_CLIP_DISTANCES 0x0D32 +#define GL_MAJOR_VERSION 0x821B +#define GL_MINOR_VERSION 0x821C +#define GL_NUM_EXTENSIONS 0x821D +#define GL_CONTEXT_FLAGS 0x821E +#define GL_COMPRESSED_RED 0x8225 +#define GL_COMPRESSED_RG 0x8226 +#define GL_CONTEXT_FLAG_FORWARD_COMPATIBLE_BIT 0x00000001 +#define GL_RGBA32F 0x8814 +#define GL_RGB32F 0x8815 +#define GL_RGBA16F 0x881A +#define GL_RGB16F 0x881B +#define GL_VERTEX_ATTRIB_ARRAY_INTEGER 0x88FD +#define GL_MAX_ARRAY_TEXTURE_LAYERS 0x88FF +#define GL_MIN_PROGRAM_TEXEL_OFFSET 0x8904 +#define GL_MAX_PROGRAM_TEXEL_OFFSET 0x8905 +#define GL_CLAMP_READ_COLOR 0x891C +#define GL_FIXED_ONLY 0x891D +#define GL_MAX_VARYING_COMPONENTS 0x8B4B +#define GL_TEXTURE_1D_ARRAY 0x8C18 +#define GL_PROXY_TEXTURE_1D_ARRAY 0x8C19 +#define GL_TEXTURE_2D_ARRAY 0x8C1A +#define GL_PROXY_TEXTURE_2D_ARRAY 0x8C1B +#define GL_TEXTURE_BINDING_1D_ARRAY 0x8C1C +#define GL_TEXTURE_BINDING_2D_ARRAY 0x8C1D +#define GL_R11F_G11F_B10F 0x8C3A +#define GL_UNSIGNED_INT_10F_11F_11F_REV 0x8C3B +#define GL_RGB9_E5 0x8C3D +#define GL_UNSIGNED_INT_5_9_9_9_REV 0x8C3E +#define GL_TEXTURE_SHARED_SIZE 0x8C3F +#define GL_TRANSFORM_FEEDBACK_VARYING_MAX_LENGTH 0x8C76 +#define GL_TRANSFORM_FEEDBACK_BUFFER_MODE 0x8C7F +#define GL_MAX_TRANSFORM_FEEDBACK_SEPARATE_COMPONENTS 0x8C80 +#define GL_TRANSFORM_FEEDBACK_VARYINGS 0x8C83 +#define GL_TRANSFORM_FEEDBACK_BUFFER_START 0x8C84 +#define GL_TRANSFORM_FEEDBACK_BUFFER_SIZE 0x8C85 +#define GL_PRIMITIVES_GENERATED 0x8C87 +#define GL_TRANSFORM_FEEDBACK_PRIMITIVES_WRITTEN 0x8C88 +#define GL_RASTERIZER_DISCARD 0x8C89 +#define GL_MAX_TRANSFORM_FEEDBACK_INTERLEAVED_COMPONENTS 0x8C8A +#define GL_MAX_TRANSFORM_FEEDBACK_SEPARATE_ATTRIBS 0x8C8B +#define GL_INTERLEAVED_ATTRIBS 0x8C8C +#define GL_SEPARATE_ATTRIBS 0x8C8D +#define GL_TRANSFORM_FEEDBACK_BUFFER 0x8C8E +#define GL_TRANSFORM_FEEDBACK_BUFFER_BINDING 0x8C8F +#define GL_RGBA32UI 0x8D70 +#define GL_RGB32UI 0x8D71 +#define GL_RGBA16UI 0x8D76 +#define GL_RGB16UI 0x8D77 +#define GL_RGBA8UI 0x8D7C +#define GL_RGB8UI 0x8D7D +#define GL_RGBA32I 0x8D82 +#define GL_RGB32I 0x8D83 +#define GL_RGBA16I 0x8D88 +#define GL_RGB16I 0x8D89 +#define GL_RGBA8I 0x8D8E +#define GL_RGB8I 0x8D8F +#define GL_RED_INTEGER 0x8D94 +#define GL_GREEN_INTEGER 0x8D95 +#define GL_BLUE_INTEGER 0x8D96 +#define GL_RGB_INTEGER 0x8D98 +#define GL_RGBA_INTEGER 0x8D99 +#define GL_BGR_INTEGER 0x8D9A +#define GL_BGRA_INTEGER 0x8D9B +#define GL_SAMPLER_1D_ARRAY 0x8DC0 +#define GL_SAMPLER_2D_ARRAY 0x8DC1 +#define GL_SAMPLER_1D_ARRAY_SHADOW 0x8DC3 +#define GL_SAMPLER_2D_ARRAY_SHADOW 0x8DC4 +#define GL_SAMPLER_CUBE_SHADOW 0x8DC5 +#define GL_UNSIGNED_INT_VEC2 0x8DC6 +#define GL_UNSIGNED_INT_VEC3 0x8DC7 +#define GL_UNSIGNED_INT_VEC4 0x8DC8 +#define GL_INT_SAMPLER_1D 0x8DC9 +#define GL_INT_SAMPLER_2D 0x8DCA +#define GL_INT_SAMPLER_3D 0x8DCB +#define GL_INT_SAMPLER_CUBE 0x8DCC +#define GL_INT_SAMPLER_1D_ARRAY 0x8DCE +#define GL_INT_SAMPLER_2D_ARRAY 0x8DCF +#define GL_UNSIGNED_INT_SAMPLER_1D 0x8DD1 +#define GL_UNSIGNED_INT_SAMPLER_2D 0x8DD2 +#define GL_UNSIGNED_INT_SAMPLER_3D 0x8DD3 +#define GL_UNSIGNED_INT_SAMPLER_CUBE 0x8DD4 +#define GL_UNSIGNED_INT_SAMPLER_1D_ARRAY 0x8DD6 +#define GL_UNSIGNED_INT_SAMPLER_2D_ARRAY 0x8DD7 +#define GL_QUERY_WAIT 0x8E13 +#define GL_QUERY_NO_WAIT 0x8E14 +#define GL_QUERY_BY_REGION_WAIT 0x8E15 +#define GL_QUERY_BY_REGION_NO_WAIT 0x8E16 +#define GL_BUFFER_ACCESS_FLAGS 0x911F +#define GL_BUFFER_MAP_LENGTH 0x9120 +#define GL_BUFFER_MAP_OFFSET 0x9121 +#define GL_DEPTH_COMPONENT32F 0x8CAC +#define GL_DEPTH32F_STENCIL8 0x8CAD +#define GL_FLOAT_32_UNSIGNED_INT_24_8_REV 0x8DAD +#define GL_INVALID_FRAMEBUFFER_OPERATION 0x0506 +#define GL_FRAMEBUFFER_ATTACHMENT_COLOR_ENCODING 0x8210 +#define GL_FRAMEBUFFER_ATTACHMENT_COMPONENT_TYPE 0x8211 +#define GL_FRAMEBUFFER_ATTACHMENT_RED_SIZE 0x8212 +#define GL_FRAMEBUFFER_ATTACHMENT_GREEN_SIZE 0x8213 +#define GL_FRAMEBUFFER_ATTACHMENT_BLUE_SIZE 0x8214 +#define GL_FRAMEBUFFER_ATTACHMENT_ALPHA_SIZE 0x8215 +#define GL_FRAMEBUFFER_ATTACHMENT_DEPTH_SIZE 0x8216 +#define GL_FRAMEBUFFER_ATTACHMENT_STENCIL_SIZE 0x8217 +#define GL_FRAMEBUFFER_DEFAULT 0x8218 +#define GL_FRAMEBUFFER_UNDEFINED 0x8219 +#define GL_DEPTH_STENCIL_ATTACHMENT 0x821A +#define GL_MAX_RENDERBUFFER_SIZE 0x84E8 +#define GL_DEPTH_STENCIL 0x84F9 +#define GL_UNSIGNED_INT_24_8 0x84FA +#define GL_DEPTH24_STENCIL8 0x88F0 +#define GL_TEXTURE_STENCIL_SIZE 0x88F1 +#define GL_TEXTURE_RED_TYPE 0x8C10 +#define GL_TEXTURE_GREEN_TYPE 0x8C11 +#define GL_TEXTURE_BLUE_TYPE 0x8C12 +#define GL_TEXTURE_ALPHA_TYPE 0x8C13 +#define GL_TEXTURE_DEPTH_TYPE 0x8C16 +#define GL_UNSIGNED_NORMALIZED 0x8C17 +#define GL_FRAMEBUFFER_BINDING 0x8CA6 +#define GL_DRAW_FRAMEBUFFER_BINDING 0x8CA6 +#define GL_RENDERBUFFER_BINDING 0x8CA7 +#define GL_READ_FRAMEBUFFER 0x8CA8 +#define GL_DRAW_FRAMEBUFFER 0x8CA9 +#define GL_READ_FRAMEBUFFER_BINDING 0x8CAA +#define GL_RENDERBUFFER_SAMPLES 0x8CAB +#define GL_FRAMEBUFFER_ATTACHMENT_OBJECT_TYPE 0x8CD0 +#define GL_FRAMEBUFFER_ATTACHMENT_OBJECT_NAME 0x8CD1 +#define GL_FRAMEBUFFER_ATTACHMENT_TEXTURE_LEVEL 0x8CD2 +#define GL_FRAMEBUFFER_ATTACHMENT_TEXTURE_CUBE_MAP_FACE 0x8CD3 +#define GL_FRAMEBUFFER_ATTACHMENT_TEXTURE_LAYER 0x8CD4 +#define GL_FRAMEBUFFER_COMPLETE 0x8CD5 +#define GL_FRAMEBUFFER_INCOMPLETE_ATTACHMENT 0x8CD6 +#define GL_FRAMEBUFFER_INCOMPLETE_MISSING_ATTACHMENT 0x8CD7 +#define GL_FRAMEBUFFER_INCOMPLETE_DRAW_BUFFER 0x8CDB +#define GL_FRAMEBUFFER_INCOMPLETE_READ_BUFFER 0x8CDC +#define GL_FRAMEBUFFER_UNSUPPORTED 0x8CDD +#define GL_MAX_COLOR_ATTACHMENTS 0x8CDF +#define GL_COLOR_ATTACHMENT0 0x8CE0 +#define GL_COLOR_ATTACHMENT1 0x8CE1 +#define GL_COLOR_ATTACHMENT2 0x8CE2 +#define GL_COLOR_ATTACHMENT3 0x8CE3 +#define GL_COLOR_ATTACHMENT4 0x8CE4 +#define GL_COLOR_ATTACHMENT5 0x8CE5 +#define GL_COLOR_ATTACHMENT6 0x8CE6 +#define GL_COLOR_ATTACHMENT7 0x8CE7 +#define GL_COLOR_ATTACHMENT8 0x8CE8 +#define GL_COLOR_ATTACHMENT9 0x8CE9 +#define GL_COLOR_ATTACHMENT10 0x8CEA +#define GL_COLOR_ATTACHMENT11 0x8CEB +#define GL_COLOR_ATTACHMENT12 0x8CEC +#define GL_COLOR_ATTACHMENT13 0x8CED +#define GL_COLOR_ATTACHMENT14 0x8CEE +#define GL_COLOR_ATTACHMENT15 0x8CEF +#define GL_COLOR_ATTACHMENT16 0x8CF0 +#define GL_COLOR_ATTACHMENT17 0x8CF1 +#define GL_COLOR_ATTACHMENT18 0x8CF2 +#define GL_COLOR_ATTACHMENT19 0x8CF3 +#define GL_COLOR_ATTACHMENT20 0x8CF4 +#define GL_COLOR_ATTACHMENT21 0x8CF5 +#define GL_COLOR_ATTACHMENT22 0x8CF6 +#define GL_COLOR_ATTACHMENT23 0x8CF7 +#define GL_COLOR_ATTACHMENT24 0x8CF8 +#define GL_COLOR_ATTACHMENT25 0x8CF9 +#define GL_COLOR_ATTACHMENT26 0x8CFA +#define GL_COLOR_ATTACHMENT27 0x8CFB +#define GL_COLOR_ATTACHMENT28 0x8CFC +#define GL_COLOR_ATTACHMENT29 0x8CFD +#define GL_COLOR_ATTACHMENT30 0x8CFE +#define GL_COLOR_ATTACHMENT31 0x8CFF +#define GL_DEPTH_ATTACHMENT 0x8D00 +#define GL_STENCIL_ATTACHMENT 0x8D20 +#define GL_FRAMEBUFFER 0x8D40 +#define GL_RENDERBUFFER 0x8D41 +#define GL_RENDERBUFFER_WIDTH 0x8D42 +#define GL_RENDERBUFFER_HEIGHT 0x8D43 +#define GL_RENDERBUFFER_INTERNAL_FORMAT 0x8D44 +#define GL_STENCIL_INDEX1 0x8D46 +#define GL_STENCIL_INDEX4 0x8D47 +#define GL_STENCIL_INDEX8 0x8D48 +#define GL_STENCIL_INDEX16 0x8D49 +#define GL_RENDERBUFFER_RED_SIZE 0x8D50 +#define GL_RENDERBUFFER_GREEN_SIZE 0x8D51 +#define GL_RENDERBUFFER_BLUE_SIZE 0x8D52 +#define GL_RENDERBUFFER_ALPHA_SIZE 0x8D53 +#define GL_RENDERBUFFER_DEPTH_SIZE 0x8D54 +#define GL_RENDERBUFFER_STENCIL_SIZE 0x8D55 +#define GL_FRAMEBUFFER_INCOMPLETE_MULTISAMPLE 0x8D56 +#define GL_MAX_SAMPLES 0x8D57 +#define GL_INDEX 0x8222 +#define GL_TEXTURE_LUMINANCE_TYPE 0x8C14 +#define GL_TEXTURE_INTENSITY_TYPE 0x8C15 +#define GL_FRAMEBUFFER_SRGB 0x8DB9 +#define GL_HALF_FLOAT 0x140B +#define GL_MAP_READ_BIT 0x0001 +#define GL_MAP_WRITE_BIT 0x0002 +#define GL_MAP_INVALIDATE_RANGE_BIT 0x0004 +#define GL_MAP_INVALIDATE_BUFFER_BIT 0x0008 +#define GL_MAP_FLUSH_EXPLICIT_BIT 0x0010 +#define GL_MAP_UNSYNCHRONIZED_BIT 0x0020 +#define GL_COMPRESSED_RED_RGTC1 0x8DBB +#define GL_COMPRESSED_SIGNED_RED_RGTC1 0x8DBC +#define GL_COMPRESSED_RG_RGTC2 0x8DBD +#define GL_COMPRESSED_SIGNED_RG_RGTC2 0x8DBE +#define GL_RG 0x8227 +#define GL_RG_INTEGER 0x8228 +#define GL_R8 0x8229 +#define GL_R16 0x822A +#define GL_RG8 0x822B +#define GL_RG16 0x822C +#define GL_R16F 0x822D +#define GL_R32F 0x822E +#define GL_RG16F 0x822F +#define GL_RG32F 0x8230 +#define GL_R8I 0x8231 +#define GL_R8UI 0x8232 +#define GL_R16I 0x8233 +#define GL_R16UI 0x8234 +#define GL_R32I 0x8235 +#define GL_R32UI 0x8236 +#define GL_RG8I 0x8237 +#define GL_RG8UI 0x8238 +#define GL_RG16I 0x8239 +#define GL_RG16UI 0x823A +#define GL_RG32I 0x823B +#define GL_RG32UI 0x823C +#define GL_VERTEX_ARRAY_BINDING 0x85B5 +#define GL_CLAMP_VERTEX_COLOR 0x891A +#define GL_CLAMP_FRAGMENT_COLOR 0x891B +#define GL_ALPHA_INTEGER 0x8D97 +#define GL_SAMPLER_2D_RECT 0x8B63 +#define GL_SAMPLER_2D_RECT_SHADOW 0x8B64 +#define GL_SAMPLER_BUFFER 0x8DC2 +#define GL_INT_SAMPLER_2D_RECT 0x8DCD +#define GL_INT_SAMPLER_BUFFER 0x8DD0 +#define GL_UNSIGNED_INT_SAMPLER_2D_RECT 0x8DD5 +#define GL_UNSIGNED_INT_SAMPLER_BUFFER 0x8DD8 +#define GL_TEXTURE_BUFFER 0x8C2A +#define GL_MAX_TEXTURE_BUFFER_SIZE 0x8C2B +#define GL_TEXTURE_BINDING_BUFFER 0x8C2C +#define GL_TEXTURE_BUFFER_DATA_STORE_BINDING 0x8C2D +#define GL_TEXTURE_RECTANGLE 0x84F5 +#define GL_TEXTURE_BINDING_RECTANGLE 0x84F6 +#define GL_PROXY_TEXTURE_RECTANGLE 0x84F7 +#define GL_MAX_RECTANGLE_TEXTURE_SIZE 0x84F8 +#define GL_R8_SNORM 0x8F94 +#define GL_RG8_SNORM 0x8F95 +#define GL_RGB8_SNORM 0x8F96 +#define GL_RGBA8_SNORM 0x8F97 +#define GL_R16_SNORM 0x8F98 +#define GL_RG16_SNORM 0x8F99 +#define GL_RGB16_SNORM 0x8F9A +#define GL_RGBA16_SNORM 0x8F9B +#define GL_SIGNED_NORMALIZED 0x8F9C +#define GL_PRIMITIVE_RESTART 0x8F9D +#define GL_PRIMITIVE_RESTART_INDEX 0x8F9E +#define GL_COPY_READ_BUFFER 0x8F36 +#define GL_COPY_WRITE_BUFFER 0x8F37 +#define GL_UNIFORM_BUFFER 0x8A11 +#define GL_UNIFORM_BUFFER_BINDING 0x8A28 +#define GL_UNIFORM_BUFFER_START 0x8A29 +#define GL_UNIFORM_BUFFER_SIZE 0x8A2A +#define GL_MAX_VERTEX_UNIFORM_BLOCKS 0x8A2B +#define GL_MAX_GEOMETRY_UNIFORM_BLOCKS 0x8A2C +#define GL_MAX_FRAGMENT_UNIFORM_BLOCKS 0x8A2D +#define GL_MAX_COMBINED_UNIFORM_BLOCKS 0x8A2E +#define GL_MAX_UNIFORM_BUFFER_BINDINGS 0x8A2F +#define GL_MAX_UNIFORM_BLOCK_SIZE 0x8A30 +#define GL_MAX_COMBINED_VERTEX_UNIFORM_COMPONENTS 0x8A31 +#define GL_MAX_COMBINED_GEOMETRY_UNIFORM_COMPONENTS 0x8A32 +#define GL_MAX_COMBINED_FRAGMENT_UNIFORM_COMPONENTS 0x8A33 +#define GL_UNIFORM_BUFFER_OFFSET_ALIGNMENT 0x8A34 +#define GL_ACTIVE_UNIFORM_BLOCK_MAX_NAME_LENGTH 0x8A35 +#define GL_ACTIVE_UNIFORM_BLOCKS 0x8A36 +#define GL_UNIFORM_TYPE 0x8A37 +#define GL_UNIFORM_SIZE 0x8A38 +#define GL_UNIFORM_NAME_LENGTH 0x8A39 +#define GL_UNIFORM_BLOCK_INDEX 0x8A3A +#define GL_UNIFORM_OFFSET 0x8A3B +#define GL_UNIFORM_ARRAY_STRIDE 0x8A3C +#define GL_UNIFORM_MATRIX_STRIDE 0x8A3D +#define GL_UNIFORM_IS_ROW_MAJOR 0x8A3E +#define GL_UNIFORM_BLOCK_BINDING 0x8A3F +#define GL_UNIFORM_BLOCK_DATA_SIZE 0x8A40 +#define GL_UNIFORM_BLOCK_NAME_LENGTH 0x8A41 +#define GL_UNIFORM_BLOCK_ACTIVE_UNIFORMS 0x8A42 +#define GL_UNIFORM_BLOCK_ACTIVE_UNIFORM_INDICES 0x8A43 +#define GL_UNIFORM_BLOCK_REFERENCED_BY_VERTEX_SHADER 0x8A44 +#define GL_UNIFORM_BLOCK_REFERENCED_BY_GEOMETRY_SHADER 0x8A45 +#define GL_UNIFORM_BLOCK_REFERENCED_BY_FRAGMENT_SHADER 0x8A46 +#define GL_INVALID_INDEX 0xFFFFFFFF +#define GL_CONTEXT_CORE_PROFILE_BIT 0x00000001 +#define GL_CONTEXT_COMPATIBILITY_PROFILE_BIT 0x00000002 +#define GL_LINES_ADJACENCY 0x000A +#define GL_LINE_STRIP_ADJACENCY 0x000B +#define GL_TRIANGLES_ADJACENCY 0x000C +#define GL_TRIANGLE_STRIP_ADJACENCY 0x000D +#define GL_PROGRAM_POINT_SIZE 0x8642 +#define GL_MAX_GEOMETRY_TEXTURE_IMAGE_UNITS 0x8C29 +#define GL_FRAMEBUFFER_ATTACHMENT_LAYERED 0x8DA7 +#define GL_FRAMEBUFFER_INCOMPLETE_LAYER_TARGETS 0x8DA8 +#define GL_GEOMETRY_SHADER 0x8DD9 +#define GL_GEOMETRY_VERTICES_OUT 0x8916 +#define GL_GEOMETRY_INPUT_TYPE 0x8917 +#define GL_GEOMETRY_OUTPUT_TYPE 0x8918 +#define GL_MAX_GEOMETRY_UNIFORM_COMPONENTS 0x8DDF +#define GL_MAX_GEOMETRY_OUTPUT_VERTICES 0x8DE0 +#define GL_MAX_GEOMETRY_TOTAL_OUTPUT_COMPONENTS 0x8DE1 +#define GL_MAX_VERTEX_OUTPUT_COMPONENTS 0x9122 +#define GL_MAX_GEOMETRY_INPUT_COMPONENTS 0x9123 +#define GL_MAX_GEOMETRY_OUTPUT_COMPONENTS 0x9124 +#define GL_MAX_FRAGMENT_INPUT_COMPONENTS 0x9125 +#define GL_CONTEXT_PROFILE_MASK 0x9126 +#define GL_DEPTH_CLAMP 0x864F +#define GL_QUADS_FOLLOW_PROVOKING_VERTEX_CONVENTION 0x8E4C +#define GL_FIRST_VERTEX_CONVENTION 0x8E4D +#define GL_LAST_VERTEX_CONVENTION 0x8E4E +#define GL_PROVOKING_VERTEX 0x8E4F +#define GL_TEXTURE_CUBE_MAP_SEAMLESS 0x884F +#define GL_MAX_SERVER_WAIT_TIMEOUT 0x9111 +#define GL_OBJECT_TYPE 0x9112 +#define GL_SYNC_CONDITION 0x9113 +#define GL_SYNC_STATUS 0x9114 +#define GL_SYNC_FLAGS 0x9115 +#define GL_SYNC_FENCE 0x9116 +#define GL_SYNC_GPU_COMMANDS_COMPLETE 0x9117 +#define GL_UNSIGNALED 0x9118 +#define GL_SIGNALED 0x9119 +#define GL_ALREADY_SIGNALED 0x911A +#define GL_TIMEOUT_EXPIRED 0x911B +#define GL_CONDITION_SATISFIED 0x911C +#define GL_WAIT_FAILED 0x911D +#define GL_TIMEOUT_IGNORED 0xFFFFFFFFFFFFFFFF +#define GL_SYNC_FLUSH_COMMANDS_BIT 0x00000001 +#define GL_SAMPLE_POSITION 0x8E50 +#define GL_SAMPLE_MASK 0x8E51 +#define GL_SAMPLE_MASK_VALUE 0x8E52 +#define GL_MAX_SAMPLE_MASK_WORDS 0x8E59 +#define GL_TEXTURE_2D_MULTISAMPLE 0x9100 +#define GL_PROXY_TEXTURE_2D_MULTISAMPLE 0x9101 +#define GL_TEXTURE_2D_MULTISAMPLE_ARRAY 0x9102 +#define GL_PROXY_TEXTURE_2D_MULTISAMPLE_ARRAY 0x9103 +#define GL_TEXTURE_BINDING_2D_MULTISAMPLE 0x9104 +#define GL_TEXTURE_BINDING_2D_MULTISAMPLE_ARRAY 0x9105 +#define GL_TEXTURE_SAMPLES 0x9106 +#define GL_TEXTURE_FIXED_SAMPLE_LOCATIONS 0x9107 +#define GL_SAMPLER_2D_MULTISAMPLE 0x9108 +#define GL_INT_SAMPLER_2D_MULTISAMPLE 0x9109 +#define GL_UNSIGNED_INT_SAMPLER_2D_MULTISAMPLE 0x910A +#define GL_SAMPLER_2D_MULTISAMPLE_ARRAY 0x910B +#define GL_INT_SAMPLER_2D_MULTISAMPLE_ARRAY 0x910C +#define GL_UNSIGNED_INT_SAMPLER_2D_MULTISAMPLE_ARRAY 0x910D +#define GL_MAX_COLOR_TEXTURE_SAMPLES 0x910E +#define GL_MAX_DEPTH_TEXTURE_SAMPLES 0x910F +#define GL_MAX_INTEGER_SAMPLES 0x9110 +#define GL_VERTEX_ATTRIB_ARRAY_DIVISOR 0x88FE +#define GL_SRC1_COLOR 0x88F9 +#define GL_ONE_MINUS_SRC1_COLOR 0x88FA +#define GL_ONE_MINUS_SRC1_ALPHA 0x88FB +#define GL_MAX_DUAL_SOURCE_DRAW_BUFFERS 0x88FC +#define GL_ANY_SAMPLES_PASSED 0x8C2F +#define GL_SAMPLER_BINDING 0x8919 +#define GL_RGB10_A2UI 0x906F +#define GL_TEXTURE_SWIZZLE_R 0x8E42 +#define GL_TEXTURE_SWIZZLE_G 0x8E43 +#define GL_TEXTURE_SWIZZLE_B 0x8E44 +#define GL_TEXTURE_SWIZZLE_A 0x8E45 +#define GL_TEXTURE_SWIZZLE_RGBA 0x8E46 +#define GL_TIME_ELAPSED 0x88BF +#define GL_TIMESTAMP 0x8E28 +#define GL_INT_2_10_10_10_REV 0x8D9F +#define GL_SAMPLE_SHADING 0x8C36 +#define GL_MIN_SAMPLE_SHADING_VALUE 0x8C37 +#define GL_MIN_PROGRAM_TEXTURE_GATHER_OFFSET 0x8E5E +#define GL_MAX_PROGRAM_TEXTURE_GATHER_OFFSET 0x8E5F +#define GL_TEXTURE_CUBE_MAP_ARRAY 0x9009 +#define GL_TEXTURE_BINDING_CUBE_MAP_ARRAY 0x900A +#define GL_PROXY_TEXTURE_CUBE_MAP_ARRAY 0x900B +#define GL_SAMPLER_CUBE_MAP_ARRAY 0x900C +#define GL_SAMPLER_CUBE_MAP_ARRAY_SHADOW 0x900D +#define GL_INT_SAMPLER_CUBE_MAP_ARRAY 0x900E +#define GL_UNSIGNED_INT_SAMPLER_CUBE_MAP_ARRAY 0x900F +#define GL_DRAW_INDIRECT_BUFFER 0x8F3F +#define GL_DRAW_INDIRECT_BUFFER_BINDING 0x8F43 +#define GL_GEOMETRY_SHADER_INVOCATIONS 0x887F +#define GL_MAX_GEOMETRY_SHADER_INVOCATIONS 0x8E5A +#define GL_MIN_FRAGMENT_INTERPOLATION_OFFSET 0x8E5B +#define GL_MAX_FRAGMENT_INTERPOLATION_OFFSET 0x8E5C +#define GL_FRAGMENT_INTERPOLATION_OFFSET_BITS 0x8E5D +#define GL_MAX_VERTEX_STREAMS 0x8E71 +#define GL_DOUBLE_VEC2 0x8FFC +#define GL_DOUBLE_VEC3 0x8FFD +#define GL_DOUBLE_VEC4 0x8FFE +#define GL_DOUBLE_MAT2 0x8F46 +#define GL_DOUBLE_MAT3 0x8F47 +#define GL_DOUBLE_MAT4 0x8F48 +#define GL_DOUBLE_MAT2x3 0x8F49 +#define GL_DOUBLE_MAT2x4 0x8F4A +#define GL_DOUBLE_MAT3x2 0x8F4B +#define GL_DOUBLE_MAT3x4 0x8F4C +#define GL_DOUBLE_MAT4x2 0x8F4D +#define GL_DOUBLE_MAT4x3 0x8F4E +#define GL_ACTIVE_SUBROUTINES 0x8DE5 +#define GL_ACTIVE_SUBROUTINE_UNIFORMS 0x8DE6 +#define GL_ACTIVE_SUBROUTINE_UNIFORM_LOCATIONS 0x8E47 +#define GL_ACTIVE_SUBROUTINE_MAX_LENGTH 0x8E48 +#define GL_ACTIVE_SUBROUTINE_UNIFORM_MAX_LENGTH 0x8E49 +#define GL_MAX_SUBROUTINES 0x8DE7 +#define GL_MAX_SUBROUTINE_UNIFORM_LOCATIONS 0x8DE8 +#define GL_NUM_COMPATIBLE_SUBROUTINES 0x8E4A +#define GL_COMPATIBLE_SUBROUTINES 0x8E4B +#define GL_PATCHES 0x000E +#define GL_PATCH_VERTICES 0x8E72 +#define GL_PATCH_DEFAULT_INNER_LEVEL 0x8E73 +#define GL_PATCH_DEFAULT_OUTER_LEVEL 0x8E74 +#define GL_TESS_CONTROL_OUTPUT_VERTICES 0x8E75 +#define GL_TESS_GEN_MODE 0x8E76 +#define GL_TESS_GEN_SPACING 0x8E77 +#define GL_TESS_GEN_VERTEX_ORDER 0x8E78 +#define GL_TESS_GEN_POINT_MODE 0x8E79 +#define GL_ISOLINES 0x8E7A +#define GL_FRACTIONAL_ODD 0x8E7B +#define GL_FRACTIONAL_EVEN 0x8E7C +#define GL_MAX_PATCH_VERTICES 0x8E7D +#define GL_MAX_TESS_GEN_LEVEL 0x8E7E +#define GL_MAX_TESS_CONTROL_UNIFORM_COMPONENTS 0x8E7F +#define GL_MAX_TESS_EVALUATION_UNIFORM_COMPONENTS 0x8E80 +#define GL_MAX_TESS_CONTROL_TEXTURE_IMAGE_UNITS 0x8E81 +#define GL_MAX_TESS_EVALUATION_TEXTURE_IMAGE_UNITS 0x8E82 +#define GL_MAX_TESS_CONTROL_OUTPUT_COMPONENTS 0x8E83 +#define GL_MAX_TESS_PATCH_COMPONENTS 0x8E84 +#define GL_MAX_TESS_CONTROL_TOTAL_OUTPUT_COMPONENTS 0x8E85 +#define GL_MAX_TESS_EVALUATION_OUTPUT_COMPONENTS 0x8E86 +#define GL_MAX_TESS_CONTROL_UNIFORM_BLOCKS 0x8E89 +#define GL_MAX_TESS_EVALUATION_UNIFORM_BLOCKS 0x8E8A +#define GL_MAX_TESS_CONTROL_INPUT_COMPONENTS 0x886C +#define GL_MAX_TESS_EVALUATION_INPUT_COMPONENTS 0x886D +#define GL_MAX_COMBINED_TESS_CONTROL_UNIFORM_COMPONENTS 0x8E1E +#define GL_MAX_COMBINED_TESS_EVALUATION_UNIFORM_COMPONENTS 0x8E1F +#define GL_UNIFORM_BLOCK_REFERENCED_BY_TESS_CONTROL_SHADER 0x84F0 +#define GL_UNIFORM_BLOCK_REFERENCED_BY_TESS_EVALUATION_SHADER 0x84F1 +#define GL_TESS_EVALUATION_SHADER 0x8E87 +#define GL_TESS_CONTROL_SHADER 0x8E88 +#define GL_TRANSFORM_FEEDBACK 0x8E22 +#define GL_TRANSFORM_FEEDBACK_BUFFER_PAUSED 0x8E23 +#define GL_TRANSFORM_FEEDBACK_BUFFER_ACTIVE 0x8E24 +#define GL_TRANSFORM_FEEDBACK_BINDING 0x8E25 +#define GL_MAX_TRANSFORM_FEEDBACK_BUFFERS 0x8E70 +#define GL_FIXED 0x140C +#define GL_IMPLEMENTATION_COLOR_READ_TYPE 0x8B9A +#define GL_IMPLEMENTATION_COLOR_READ_FORMAT 0x8B9B +#define GL_LOW_FLOAT 0x8DF0 +#define GL_MEDIUM_FLOAT 0x8DF1 +#define GL_HIGH_FLOAT 0x8DF2 +#define GL_LOW_INT 0x8DF3 +#define GL_MEDIUM_INT 0x8DF4 +#define GL_HIGH_INT 0x8DF5 +#define GL_SHADER_COMPILER 0x8DFA +#define GL_SHADER_BINARY_FORMATS 0x8DF8 +#define GL_NUM_SHADER_BINARY_FORMATS 0x8DF9 +#define GL_MAX_VERTEX_UNIFORM_VECTORS 0x8DFB +#define GL_MAX_VARYING_VECTORS 0x8DFC +#define GL_MAX_FRAGMENT_UNIFORM_VECTORS 0x8DFD +#define GL_RGB565 0x8D62 +#define GL_PROGRAM_BINARY_RETRIEVABLE_HINT 0x8257 +#define GL_PROGRAM_BINARY_LENGTH 0x8741 +#define GL_NUM_PROGRAM_BINARY_FORMATS 0x87FE +#define GL_PROGRAM_BINARY_FORMATS 0x87FF +#define GL_VERTEX_SHADER_BIT 0x00000001 +#define GL_FRAGMENT_SHADER_BIT 0x00000002 +#define GL_GEOMETRY_SHADER_BIT 0x00000004 +#define GL_TESS_CONTROL_SHADER_BIT 0x00000008 +#define GL_TESS_EVALUATION_SHADER_BIT 0x00000010 +#define GL_ALL_SHADER_BITS 0xFFFFFFFF +#define GL_PROGRAM_SEPARABLE 0x8258 +#define GL_ACTIVE_PROGRAM 0x8259 +#define GL_PROGRAM_PIPELINE_BINDING 0x825A +#define GL_MAX_VIEWPORTS 0x825B +#define GL_VIEWPORT_SUBPIXEL_BITS 0x825C +#define GL_VIEWPORT_BOUNDS_RANGE 0x825D +#define GL_LAYER_PROVOKING_VERTEX 0x825E +#define GL_VIEWPORT_INDEX_PROVOKING_VERTEX 0x825F +#define GL_UNDEFINED_VERTEX 0x8260 +#ifndef GL_VERSION_1_0 +#define GL_VERSION_1_0 1 +GLAPI int GLAD_GL_VERSION_1_0; +typedef void (APIENTRYP PFNGLCULLFACEPROC)(GLenum mode); +GLAPI PFNGLCULLFACEPROC glad_glCullFace; +#define glCullFace glad_glCullFace +typedef void (APIENTRYP PFNGLFRONTFACEPROC)(GLenum mode); +GLAPI PFNGLFRONTFACEPROC glad_glFrontFace; +#define glFrontFace glad_glFrontFace +typedef void (APIENTRYP PFNGLHINTPROC)(GLenum target, GLenum mode); +GLAPI PFNGLHINTPROC glad_glHint; +#define glHint glad_glHint +typedef void (APIENTRYP PFNGLLINEWIDTHPROC)(GLfloat width); +GLAPI PFNGLLINEWIDTHPROC glad_glLineWidth; +#define glLineWidth glad_glLineWidth +typedef void (APIENTRYP PFNGLPOINTSIZEPROC)(GLfloat size); +GLAPI PFNGLPOINTSIZEPROC glad_glPointSize; +#define glPointSize glad_glPointSize +typedef void (APIENTRYP PFNGLPOLYGONMODEPROC)(GLenum face, GLenum mode); +GLAPI PFNGLPOLYGONMODEPROC glad_glPolygonMode; +#define glPolygonMode glad_glPolygonMode +typedef void (APIENTRYP PFNGLSCISSORPROC)(GLint x, GLint y, GLsizei width, GLsizei height); +GLAPI PFNGLSCISSORPROC glad_glScissor; +#define glScissor glad_glScissor +typedef void (APIENTRYP PFNGLTEXPARAMETERFPROC)(GLenum target, GLenum pname, GLfloat param); +GLAPI PFNGLTEXPARAMETERFPROC glad_glTexParameterf; +#define glTexParameterf glad_glTexParameterf +typedef void (APIENTRYP PFNGLTEXPARAMETERFVPROC)(GLenum target, GLenum pname, const GLfloat *params); +GLAPI PFNGLTEXPARAMETERFVPROC glad_glTexParameterfv; +#define glTexParameterfv glad_glTexParameterfv +typedef void (APIENTRYP PFNGLTEXPARAMETERIPROC)(GLenum target, GLenum pname, GLint param); +GLAPI PFNGLTEXPARAMETERIPROC glad_glTexParameteri; +#define glTexParameteri glad_glTexParameteri +typedef void (APIENTRYP PFNGLTEXPARAMETERIVPROC)(GLenum target, GLenum pname, const GLint *params); +GLAPI PFNGLTEXPARAMETERIVPROC glad_glTexParameteriv; +#define glTexParameteriv glad_glTexParameteriv +typedef void (APIENTRYP PFNGLTEXIMAGE1DPROC)(GLenum target, GLint level, GLint internalformat, GLsizei width, GLint border, GLenum format, GLenum type, const void *pixels); +GLAPI PFNGLTEXIMAGE1DPROC glad_glTexImage1D; +#define glTexImage1D glad_glTexImage1D +typedef void (APIENTRYP PFNGLTEXIMAGE2DPROC)(GLenum target, GLint level, GLint internalformat, GLsizei width, GLsizei height, GLint border, GLenum format, GLenum type, const void *pixels); +GLAPI PFNGLTEXIMAGE2DPROC glad_glTexImage2D; +#define glTexImage2D glad_glTexImage2D +typedef void (APIENTRYP PFNGLDRAWBUFFERPROC)(GLenum buf); +GLAPI PFNGLDRAWBUFFERPROC glad_glDrawBuffer; +#define glDrawBuffer glad_glDrawBuffer +typedef void (APIENTRYP PFNGLCLEARPROC)(GLbitfield mask); +GLAPI PFNGLCLEARPROC glad_glClear; +#define glClear glad_glClear +typedef void (APIENTRYP PFNGLCLEARCOLORPROC)(GLfloat red, GLfloat green, GLfloat blue, GLfloat alpha); +GLAPI PFNGLCLEARCOLORPROC glad_glClearColor; +#define glClearColor glad_glClearColor +typedef void (APIENTRYP PFNGLCLEARSTENCILPROC)(GLint s); +GLAPI PFNGLCLEARSTENCILPROC glad_glClearStencil; +#define glClearStencil glad_glClearStencil +typedef void (APIENTRYP PFNGLCLEARDEPTHPROC)(GLdouble depth); +GLAPI PFNGLCLEARDEPTHPROC glad_glClearDepth; +#define glClearDepth glad_glClearDepth +typedef void (APIENTRYP PFNGLSTENCILMASKPROC)(GLuint mask); +GLAPI PFNGLSTENCILMASKPROC glad_glStencilMask; +#define glStencilMask glad_glStencilMask +typedef void (APIENTRYP PFNGLCOLORMASKPROC)(GLboolean red, GLboolean green, GLboolean blue, GLboolean alpha); +GLAPI PFNGLCOLORMASKPROC glad_glColorMask; +#define glColorMask glad_glColorMask +typedef void (APIENTRYP PFNGLDEPTHMASKPROC)(GLboolean flag); +GLAPI PFNGLDEPTHMASKPROC glad_glDepthMask; +#define glDepthMask glad_glDepthMask +typedef void (APIENTRYP PFNGLDISABLEPROC)(GLenum cap); +GLAPI PFNGLDISABLEPROC glad_glDisable; +#define glDisable glad_glDisable +typedef void (APIENTRYP PFNGLENABLEPROC)(GLenum cap); +GLAPI PFNGLENABLEPROC glad_glEnable; +#define glEnable glad_glEnable +typedef void (APIENTRYP PFNGLFINISHPROC)(void); +GLAPI PFNGLFINISHPROC glad_glFinish; +#define glFinish glad_glFinish +typedef void (APIENTRYP PFNGLFLUSHPROC)(void); +GLAPI PFNGLFLUSHPROC glad_glFlush; +#define glFlush glad_glFlush +typedef void (APIENTRYP PFNGLBLENDFUNCPROC)(GLenum sfactor, GLenum dfactor); +GLAPI PFNGLBLENDFUNCPROC glad_glBlendFunc; +#define glBlendFunc glad_glBlendFunc +typedef void (APIENTRYP PFNGLLOGICOPPROC)(GLenum opcode); +GLAPI PFNGLLOGICOPPROC glad_glLogicOp; +#define glLogicOp glad_glLogicOp +typedef void (APIENTRYP PFNGLSTENCILFUNCPROC)(GLenum func, GLint ref, GLuint mask); +GLAPI PFNGLSTENCILFUNCPROC glad_glStencilFunc; +#define glStencilFunc glad_glStencilFunc +typedef void (APIENTRYP PFNGLSTENCILOPPROC)(GLenum fail, GLenum zfail, GLenum zpass); +GLAPI PFNGLSTENCILOPPROC glad_glStencilOp; +#define glStencilOp glad_glStencilOp +typedef void (APIENTRYP PFNGLDEPTHFUNCPROC)(GLenum func); +GLAPI PFNGLDEPTHFUNCPROC glad_glDepthFunc; +#define glDepthFunc glad_glDepthFunc +typedef void (APIENTRYP PFNGLPIXELSTOREFPROC)(GLenum pname, GLfloat param); +GLAPI PFNGLPIXELSTOREFPROC glad_glPixelStoref; +#define glPixelStoref glad_glPixelStoref +typedef void (APIENTRYP PFNGLPIXELSTOREIPROC)(GLenum pname, GLint param); +GLAPI PFNGLPIXELSTOREIPROC glad_glPixelStorei; +#define glPixelStorei glad_glPixelStorei +typedef void (APIENTRYP PFNGLREADBUFFERPROC)(GLenum src); +GLAPI PFNGLREADBUFFERPROC glad_glReadBuffer; +#define glReadBuffer glad_glReadBuffer +typedef void (APIENTRYP PFNGLREADPIXELSPROC)(GLint x, GLint y, GLsizei width, GLsizei height, GLenum format, GLenum type, void *pixels); +GLAPI PFNGLREADPIXELSPROC glad_glReadPixels; +#define glReadPixels glad_glReadPixels +typedef void (APIENTRYP PFNGLGETBOOLEANVPROC)(GLenum pname, GLboolean *data); +GLAPI PFNGLGETBOOLEANVPROC glad_glGetBooleanv; +#define glGetBooleanv glad_glGetBooleanv +typedef void (APIENTRYP PFNGLGETDOUBLEVPROC)(GLenum pname, GLdouble *data); +GLAPI PFNGLGETDOUBLEVPROC glad_glGetDoublev; +#define glGetDoublev glad_glGetDoublev +typedef GLenum (APIENTRYP PFNGLGETERRORPROC)(void); +GLAPI PFNGLGETERRORPROC glad_glGetError; +#define glGetError glad_glGetError +typedef void (APIENTRYP PFNGLGETFLOATVPROC)(GLenum pname, GLfloat *data); +GLAPI PFNGLGETFLOATVPROC glad_glGetFloatv; +#define glGetFloatv glad_glGetFloatv +typedef void (APIENTRYP PFNGLGETINTEGERVPROC)(GLenum pname, GLint *data); +GLAPI PFNGLGETINTEGERVPROC glad_glGetIntegerv; +#define glGetIntegerv glad_glGetIntegerv +typedef const GLubyte * (APIENTRYP PFNGLGETSTRINGPROC)(GLenum name); +GLAPI PFNGLGETSTRINGPROC glad_glGetString; +#define glGetString glad_glGetString +typedef void (APIENTRYP PFNGLGETTEXIMAGEPROC)(GLenum target, GLint level, GLenum format, GLenum type, void *pixels); +GLAPI PFNGLGETTEXIMAGEPROC glad_glGetTexImage; +#define glGetTexImage glad_glGetTexImage +typedef void (APIENTRYP PFNGLGETTEXPARAMETERFVPROC)(GLenum target, GLenum pname, GLfloat *params); +GLAPI PFNGLGETTEXPARAMETERFVPROC glad_glGetTexParameterfv; +#define glGetTexParameterfv glad_glGetTexParameterfv +typedef void (APIENTRYP PFNGLGETTEXPARAMETERIVPROC)(GLenum target, GLenum pname, GLint *params); +GLAPI PFNGLGETTEXPARAMETERIVPROC glad_glGetTexParameteriv; +#define glGetTexParameteriv glad_glGetTexParameteriv +typedef void (APIENTRYP PFNGLGETTEXLEVELPARAMETERFVPROC)(GLenum target, GLint level, GLenum pname, GLfloat *params); +GLAPI PFNGLGETTEXLEVELPARAMETERFVPROC glad_glGetTexLevelParameterfv; +#define glGetTexLevelParameterfv glad_glGetTexLevelParameterfv +typedef void (APIENTRYP PFNGLGETTEXLEVELPARAMETERIVPROC)(GLenum target, GLint level, GLenum pname, GLint *params); +GLAPI PFNGLGETTEXLEVELPARAMETERIVPROC glad_glGetTexLevelParameteriv; +#define glGetTexLevelParameteriv glad_glGetTexLevelParameteriv +typedef GLboolean (APIENTRYP PFNGLISENABLEDPROC)(GLenum cap); +GLAPI PFNGLISENABLEDPROC glad_glIsEnabled; +#define glIsEnabled glad_glIsEnabled +typedef void (APIENTRYP PFNGLDEPTHRANGEPROC)(GLdouble n, GLdouble f); +GLAPI PFNGLDEPTHRANGEPROC glad_glDepthRange; +#define glDepthRange glad_glDepthRange +typedef void (APIENTRYP PFNGLVIEWPORTPROC)(GLint x, GLint y, GLsizei width, GLsizei height); +GLAPI PFNGLVIEWPORTPROC glad_glViewport; +#define glViewport glad_glViewport +typedef void (APIENTRYP PFNGLNEWLISTPROC)(GLuint list, GLenum mode); +GLAPI PFNGLNEWLISTPROC glad_glNewList; +#define glNewList glad_glNewList +typedef void (APIENTRYP PFNGLENDLISTPROC)(void); +GLAPI PFNGLENDLISTPROC glad_glEndList; +#define glEndList glad_glEndList +typedef void (APIENTRYP PFNGLCALLLISTPROC)(GLuint list); +GLAPI PFNGLCALLLISTPROC glad_glCallList; +#define glCallList glad_glCallList +typedef void (APIENTRYP PFNGLCALLLISTSPROC)(GLsizei n, GLenum type, const void *lists); +GLAPI PFNGLCALLLISTSPROC glad_glCallLists; +#define glCallLists glad_glCallLists +typedef void (APIENTRYP PFNGLDELETELISTSPROC)(GLuint list, GLsizei range); +GLAPI PFNGLDELETELISTSPROC glad_glDeleteLists; +#define glDeleteLists glad_glDeleteLists +typedef GLuint (APIENTRYP PFNGLGENLISTSPROC)(GLsizei range); +GLAPI PFNGLGENLISTSPROC glad_glGenLists; +#define glGenLists glad_glGenLists +typedef void (APIENTRYP PFNGLLISTBASEPROC)(GLuint base); +GLAPI PFNGLLISTBASEPROC glad_glListBase; +#define glListBase glad_glListBase +typedef void (APIENTRYP PFNGLBEGINPROC)(GLenum mode); +GLAPI PFNGLBEGINPROC glad_glBegin; +#define glBegin glad_glBegin +typedef void (APIENTRYP PFNGLBITMAPPROC)(GLsizei width, GLsizei height, GLfloat xorig, GLfloat yorig, GLfloat xmove, GLfloat ymove, const GLubyte *bitmap); +GLAPI PFNGLBITMAPPROC glad_glBitmap; +#define glBitmap glad_glBitmap +typedef void (APIENTRYP PFNGLCOLOR3BPROC)(GLbyte red, GLbyte green, GLbyte blue); +GLAPI PFNGLCOLOR3BPROC glad_glColor3b; +#define glColor3b glad_glColor3b +typedef void (APIENTRYP PFNGLCOLOR3BVPROC)(const GLbyte *v); +GLAPI PFNGLCOLOR3BVPROC glad_glColor3bv; +#define glColor3bv glad_glColor3bv +typedef void (APIENTRYP PFNGLCOLOR3DPROC)(GLdouble red, GLdouble green, GLdouble blue); +GLAPI PFNGLCOLOR3DPROC glad_glColor3d; +#define glColor3d glad_glColor3d +typedef void (APIENTRYP PFNGLCOLOR3DVPROC)(const GLdouble *v); +GLAPI PFNGLCOLOR3DVPROC glad_glColor3dv; +#define glColor3dv glad_glColor3dv +typedef void (APIENTRYP PFNGLCOLOR3FPROC)(GLfloat red, GLfloat green, GLfloat blue); +GLAPI PFNGLCOLOR3FPROC glad_glColor3f; +#define glColor3f glad_glColor3f +typedef void (APIENTRYP PFNGLCOLOR3FVPROC)(const GLfloat *v); +GLAPI PFNGLCOLOR3FVPROC glad_glColor3fv; +#define glColor3fv glad_glColor3fv +typedef void (APIENTRYP PFNGLCOLOR3IPROC)(GLint red, GLint green, GLint blue); +GLAPI PFNGLCOLOR3IPROC glad_glColor3i; +#define glColor3i glad_glColor3i +typedef void (APIENTRYP PFNGLCOLOR3IVPROC)(const GLint *v); +GLAPI PFNGLCOLOR3IVPROC glad_glColor3iv; +#define glColor3iv glad_glColor3iv +typedef void (APIENTRYP PFNGLCOLOR3SPROC)(GLshort red, GLshort green, GLshort blue); +GLAPI PFNGLCOLOR3SPROC glad_glColor3s; +#define glColor3s glad_glColor3s +typedef void (APIENTRYP PFNGLCOLOR3SVPROC)(const GLshort *v); +GLAPI PFNGLCOLOR3SVPROC glad_glColor3sv; +#define glColor3sv glad_glColor3sv +typedef void (APIENTRYP PFNGLCOLOR3UBPROC)(GLubyte red, GLubyte green, GLubyte blue); +GLAPI PFNGLCOLOR3UBPROC glad_glColor3ub; +#define glColor3ub glad_glColor3ub +typedef void (APIENTRYP PFNGLCOLOR3UBVPROC)(const GLubyte *v); +GLAPI PFNGLCOLOR3UBVPROC glad_glColor3ubv; +#define glColor3ubv glad_glColor3ubv +typedef void (APIENTRYP PFNGLCOLOR3UIPROC)(GLuint red, GLuint green, GLuint blue); +GLAPI PFNGLCOLOR3UIPROC glad_glColor3ui; +#define glColor3ui glad_glColor3ui +typedef void (APIENTRYP PFNGLCOLOR3UIVPROC)(const GLuint *v); +GLAPI PFNGLCOLOR3UIVPROC glad_glColor3uiv; +#define glColor3uiv glad_glColor3uiv +typedef void (APIENTRYP PFNGLCOLOR3USPROC)(GLushort red, GLushort green, GLushort blue); +GLAPI PFNGLCOLOR3USPROC glad_glColor3us; +#define glColor3us glad_glColor3us +typedef void (APIENTRYP PFNGLCOLOR3USVPROC)(const GLushort *v); +GLAPI PFNGLCOLOR3USVPROC glad_glColor3usv; +#define glColor3usv glad_glColor3usv +typedef void (APIENTRYP PFNGLCOLOR4BPROC)(GLbyte red, GLbyte green, GLbyte blue, GLbyte alpha); +GLAPI PFNGLCOLOR4BPROC glad_glColor4b; +#define glColor4b glad_glColor4b +typedef void (APIENTRYP PFNGLCOLOR4BVPROC)(const GLbyte *v); +GLAPI PFNGLCOLOR4BVPROC glad_glColor4bv; +#define glColor4bv glad_glColor4bv +typedef void (APIENTRYP PFNGLCOLOR4DPROC)(GLdouble red, GLdouble green, GLdouble blue, GLdouble alpha); +GLAPI PFNGLCOLOR4DPROC glad_glColor4d; +#define glColor4d glad_glColor4d +typedef void (APIENTRYP PFNGLCOLOR4DVPROC)(const GLdouble *v); +GLAPI PFNGLCOLOR4DVPROC glad_glColor4dv; +#define glColor4dv glad_glColor4dv +typedef void (APIENTRYP PFNGLCOLOR4FPROC)(GLfloat red, GLfloat green, GLfloat blue, GLfloat alpha); +GLAPI PFNGLCOLOR4FPROC glad_glColor4f; +#define glColor4f glad_glColor4f +typedef void (APIENTRYP PFNGLCOLOR4FVPROC)(const GLfloat *v); +GLAPI PFNGLCOLOR4FVPROC glad_glColor4fv; +#define glColor4fv glad_glColor4fv +typedef void (APIENTRYP PFNGLCOLOR4IPROC)(GLint red, GLint green, GLint blue, GLint alpha); +GLAPI PFNGLCOLOR4IPROC glad_glColor4i; +#define glColor4i glad_glColor4i +typedef void (APIENTRYP PFNGLCOLOR4IVPROC)(const GLint *v); +GLAPI PFNGLCOLOR4IVPROC glad_glColor4iv; +#define glColor4iv glad_glColor4iv +typedef void (APIENTRYP PFNGLCOLOR4SPROC)(GLshort red, GLshort green, GLshort blue, GLshort alpha); +GLAPI PFNGLCOLOR4SPROC glad_glColor4s; +#define glColor4s glad_glColor4s +typedef void (APIENTRYP PFNGLCOLOR4SVPROC)(const GLshort *v); +GLAPI PFNGLCOLOR4SVPROC glad_glColor4sv; +#define glColor4sv glad_glColor4sv +typedef void (APIENTRYP PFNGLCOLOR4UBPROC)(GLubyte red, GLubyte green, GLubyte blue, GLubyte alpha); +GLAPI PFNGLCOLOR4UBPROC glad_glColor4ub; +#define glColor4ub glad_glColor4ub +typedef void (APIENTRYP PFNGLCOLOR4UBVPROC)(const GLubyte *v); +GLAPI PFNGLCOLOR4UBVPROC glad_glColor4ubv; +#define glColor4ubv glad_glColor4ubv +typedef void (APIENTRYP PFNGLCOLOR4UIPROC)(GLuint red, GLuint green, GLuint blue, GLuint alpha); +GLAPI PFNGLCOLOR4UIPROC glad_glColor4ui; +#define glColor4ui glad_glColor4ui +typedef void (APIENTRYP PFNGLCOLOR4UIVPROC)(const GLuint *v); +GLAPI PFNGLCOLOR4UIVPROC glad_glColor4uiv; +#define glColor4uiv glad_glColor4uiv +typedef void (APIENTRYP PFNGLCOLOR4USPROC)(GLushort red, GLushort green, GLushort blue, GLushort alpha); +GLAPI PFNGLCOLOR4USPROC glad_glColor4us; +#define glColor4us glad_glColor4us +typedef void (APIENTRYP PFNGLCOLOR4USVPROC)(const GLushort *v); +GLAPI PFNGLCOLOR4USVPROC glad_glColor4usv; +#define glColor4usv glad_glColor4usv +typedef void (APIENTRYP PFNGLEDGEFLAGPROC)(GLboolean flag); +GLAPI PFNGLEDGEFLAGPROC glad_glEdgeFlag; +#define glEdgeFlag glad_glEdgeFlag +typedef void (APIENTRYP PFNGLEDGEFLAGVPROC)(const GLboolean *flag); +GLAPI PFNGLEDGEFLAGVPROC glad_glEdgeFlagv; +#define glEdgeFlagv glad_glEdgeFlagv +typedef void (APIENTRYP PFNGLENDPROC)(void); +GLAPI PFNGLENDPROC glad_glEnd; +#define glEnd glad_glEnd +typedef void (APIENTRYP PFNGLINDEXDPROC)(GLdouble c); +GLAPI PFNGLINDEXDPROC glad_glIndexd; +#define glIndexd glad_glIndexd +typedef void (APIENTRYP PFNGLINDEXDVPROC)(const GLdouble *c); +GLAPI PFNGLINDEXDVPROC glad_glIndexdv; +#define glIndexdv glad_glIndexdv +typedef void (APIENTRYP PFNGLINDEXFPROC)(GLfloat c); +GLAPI PFNGLINDEXFPROC glad_glIndexf; +#define glIndexf glad_glIndexf +typedef void (APIENTRYP PFNGLINDEXFVPROC)(const GLfloat *c); +GLAPI PFNGLINDEXFVPROC glad_glIndexfv; +#define glIndexfv glad_glIndexfv +typedef void (APIENTRYP PFNGLINDEXIPROC)(GLint c); +GLAPI PFNGLINDEXIPROC glad_glIndexi; +#define glIndexi glad_glIndexi +typedef void (APIENTRYP PFNGLINDEXIVPROC)(const GLint *c); +GLAPI PFNGLINDEXIVPROC glad_glIndexiv; +#define glIndexiv glad_glIndexiv +typedef void (APIENTRYP PFNGLINDEXSPROC)(GLshort c); +GLAPI PFNGLINDEXSPROC glad_glIndexs; +#define glIndexs glad_glIndexs +typedef void (APIENTRYP PFNGLINDEXSVPROC)(const GLshort *c); +GLAPI PFNGLINDEXSVPROC glad_glIndexsv; +#define glIndexsv glad_glIndexsv +typedef void (APIENTRYP PFNGLNORMAL3BPROC)(GLbyte nx, GLbyte ny, GLbyte nz); +GLAPI PFNGLNORMAL3BPROC glad_glNormal3b; +#define glNormal3b glad_glNormal3b +typedef void (APIENTRYP PFNGLNORMAL3BVPROC)(const GLbyte *v); +GLAPI PFNGLNORMAL3BVPROC glad_glNormal3bv; +#define glNormal3bv glad_glNormal3bv +typedef void (APIENTRYP PFNGLNORMAL3DPROC)(GLdouble nx, GLdouble ny, GLdouble nz); +GLAPI PFNGLNORMAL3DPROC glad_glNormal3d; +#define glNormal3d glad_glNormal3d +typedef void (APIENTRYP PFNGLNORMAL3DVPROC)(const GLdouble *v); +GLAPI PFNGLNORMAL3DVPROC glad_glNormal3dv; +#define glNormal3dv glad_glNormal3dv +typedef void (APIENTRYP PFNGLNORMAL3FPROC)(GLfloat nx, GLfloat ny, GLfloat nz); +GLAPI PFNGLNORMAL3FPROC glad_glNormal3f; +#define glNormal3f glad_glNormal3f +typedef void (APIENTRYP PFNGLNORMAL3FVPROC)(const GLfloat *v); +GLAPI PFNGLNORMAL3FVPROC glad_glNormal3fv; +#define glNormal3fv glad_glNormal3fv +typedef void (APIENTRYP PFNGLNORMAL3IPROC)(GLint nx, GLint ny, GLint nz); +GLAPI PFNGLNORMAL3IPROC glad_glNormal3i; +#define glNormal3i glad_glNormal3i +typedef void (APIENTRYP PFNGLNORMAL3IVPROC)(const GLint *v); +GLAPI PFNGLNORMAL3IVPROC glad_glNormal3iv; +#define glNormal3iv glad_glNormal3iv +typedef void (APIENTRYP PFNGLNORMAL3SPROC)(GLshort nx, GLshort ny, GLshort nz); +GLAPI PFNGLNORMAL3SPROC glad_glNormal3s; +#define glNormal3s glad_glNormal3s +typedef void (APIENTRYP PFNGLNORMAL3SVPROC)(const GLshort *v); +GLAPI PFNGLNORMAL3SVPROC glad_glNormal3sv; +#define glNormal3sv glad_glNormal3sv +typedef void (APIENTRYP PFNGLRASTERPOS2DPROC)(GLdouble x, GLdouble y); +GLAPI PFNGLRASTERPOS2DPROC glad_glRasterPos2d; +#define glRasterPos2d glad_glRasterPos2d +typedef void (APIENTRYP PFNGLRASTERPOS2DVPROC)(const GLdouble *v); +GLAPI PFNGLRASTERPOS2DVPROC glad_glRasterPos2dv; +#define glRasterPos2dv glad_glRasterPos2dv +typedef void (APIENTRYP PFNGLRASTERPOS2FPROC)(GLfloat x, GLfloat y); +GLAPI PFNGLRASTERPOS2FPROC glad_glRasterPos2f; +#define glRasterPos2f glad_glRasterPos2f +typedef void (APIENTRYP PFNGLRASTERPOS2FVPROC)(const GLfloat *v); +GLAPI PFNGLRASTERPOS2FVPROC glad_glRasterPos2fv; +#define glRasterPos2fv glad_glRasterPos2fv +typedef void (APIENTRYP PFNGLRASTERPOS2IPROC)(GLint x, GLint y); +GLAPI PFNGLRASTERPOS2IPROC glad_glRasterPos2i; +#define glRasterPos2i glad_glRasterPos2i +typedef void (APIENTRYP PFNGLRASTERPOS2IVPROC)(const GLint *v); +GLAPI PFNGLRASTERPOS2IVPROC glad_glRasterPos2iv; +#define glRasterPos2iv glad_glRasterPos2iv +typedef void (APIENTRYP PFNGLRASTERPOS2SPROC)(GLshort x, GLshort y); +GLAPI PFNGLRASTERPOS2SPROC glad_glRasterPos2s; +#define glRasterPos2s glad_glRasterPos2s +typedef void (APIENTRYP PFNGLRASTERPOS2SVPROC)(const GLshort *v); +GLAPI PFNGLRASTERPOS2SVPROC glad_glRasterPos2sv; +#define glRasterPos2sv glad_glRasterPos2sv +typedef void (APIENTRYP PFNGLRASTERPOS3DPROC)(GLdouble x, GLdouble y, GLdouble z); +GLAPI PFNGLRASTERPOS3DPROC glad_glRasterPos3d; +#define glRasterPos3d glad_glRasterPos3d +typedef void (APIENTRYP PFNGLRASTERPOS3DVPROC)(const GLdouble *v); +GLAPI PFNGLRASTERPOS3DVPROC glad_glRasterPos3dv; +#define glRasterPos3dv glad_glRasterPos3dv +typedef void (APIENTRYP PFNGLRASTERPOS3FPROC)(GLfloat x, GLfloat y, GLfloat z); +GLAPI PFNGLRASTERPOS3FPROC glad_glRasterPos3f; +#define glRasterPos3f glad_glRasterPos3f +typedef void (APIENTRYP PFNGLRASTERPOS3FVPROC)(const GLfloat *v); +GLAPI PFNGLRASTERPOS3FVPROC glad_glRasterPos3fv; +#define glRasterPos3fv glad_glRasterPos3fv +typedef void (APIENTRYP PFNGLRASTERPOS3IPROC)(GLint x, GLint y, GLint z); +GLAPI PFNGLRASTERPOS3IPROC glad_glRasterPos3i; +#define glRasterPos3i glad_glRasterPos3i +typedef void (APIENTRYP PFNGLRASTERPOS3IVPROC)(const GLint *v); +GLAPI PFNGLRASTERPOS3IVPROC glad_glRasterPos3iv; +#define glRasterPos3iv glad_glRasterPos3iv +typedef void (APIENTRYP PFNGLRASTERPOS3SPROC)(GLshort x, GLshort y, GLshort z); +GLAPI PFNGLRASTERPOS3SPROC glad_glRasterPos3s; +#define glRasterPos3s glad_glRasterPos3s +typedef void (APIENTRYP PFNGLRASTERPOS3SVPROC)(const GLshort *v); +GLAPI PFNGLRASTERPOS3SVPROC glad_glRasterPos3sv; +#define glRasterPos3sv glad_glRasterPos3sv +typedef void (APIENTRYP PFNGLRASTERPOS4DPROC)(GLdouble x, GLdouble y, GLdouble z, GLdouble w); +GLAPI PFNGLRASTERPOS4DPROC glad_glRasterPos4d; +#define glRasterPos4d glad_glRasterPos4d +typedef void (APIENTRYP PFNGLRASTERPOS4DVPROC)(const GLdouble *v); +GLAPI PFNGLRASTERPOS4DVPROC glad_glRasterPos4dv; +#define glRasterPos4dv glad_glRasterPos4dv +typedef void (APIENTRYP PFNGLRASTERPOS4FPROC)(GLfloat x, GLfloat y, GLfloat z, GLfloat w); +GLAPI PFNGLRASTERPOS4FPROC glad_glRasterPos4f; +#define glRasterPos4f glad_glRasterPos4f +typedef void (APIENTRYP PFNGLRASTERPOS4FVPROC)(const GLfloat *v); +GLAPI PFNGLRASTERPOS4FVPROC glad_glRasterPos4fv; +#define glRasterPos4fv glad_glRasterPos4fv +typedef void (APIENTRYP PFNGLRASTERPOS4IPROC)(GLint x, GLint y, GLint z, GLint w); +GLAPI PFNGLRASTERPOS4IPROC glad_glRasterPos4i; +#define glRasterPos4i glad_glRasterPos4i +typedef void (APIENTRYP PFNGLRASTERPOS4IVPROC)(const GLint *v); +GLAPI PFNGLRASTERPOS4IVPROC glad_glRasterPos4iv; +#define glRasterPos4iv glad_glRasterPos4iv +typedef void (APIENTRYP PFNGLRASTERPOS4SPROC)(GLshort x, GLshort y, GLshort z, GLshort w); +GLAPI PFNGLRASTERPOS4SPROC glad_glRasterPos4s; +#define glRasterPos4s glad_glRasterPos4s +typedef void (APIENTRYP PFNGLRASTERPOS4SVPROC)(const GLshort *v); +GLAPI PFNGLRASTERPOS4SVPROC glad_glRasterPos4sv; +#define glRasterPos4sv glad_glRasterPos4sv +typedef void (APIENTRYP PFNGLRECTDPROC)(GLdouble x1, GLdouble y1, GLdouble x2, GLdouble y2); +GLAPI PFNGLRECTDPROC glad_glRectd; +#define glRectd glad_glRectd +typedef void (APIENTRYP PFNGLRECTDVPROC)(const GLdouble *v1, const GLdouble *v2); +GLAPI PFNGLRECTDVPROC glad_glRectdv; +#define glRectdv glad_glRectdv +typedef void (APIENTRYP PFNGLRECTFPROC)(GLfloat x1, GLfloat y1, GLfloat x2, GLfloat y2); +GLAPI PFNGLRECTFPROC glad_glRectf; +#define glRectf glad_glRectf +typedef void (APIENTRYP PFNGLRECTFVPROC)(const GLfloat *v1, const GLfloat *v2); +GLAPI PFNGLRECTFVPROC glad_glRectfv; +#define glRectfv glad_glRectfv +typedef void (APIENTRYP PFNGLRECTIPROC)(GLint x1, GLint y1, GLint x2, GLint y2); +GLAPI PFNGLRECTIPROC glad_glRecti; +#define glRecti glad_glRecti +typedef void (APIENTRYP PFNGLRECTIVPROC)(const GLint *v1, const GLint *v2); +GLAPI PFNGLRECTIVPROC glad_glRectiv; +#define glRectiv glad_glRectiv +typedef void (APIENTRYP PFNGLRECTSPROC)(GLshort x1, GLshort y1, GLshort x2, GLshort y2); +GLAPI PFNGLRECTSPROC glad_glRects; +#define glRects glad_glRects +typedef void (APIENTRYP PFNGLRECTSVPROC)(const GLshort *v1, const GLshort *v2); +GLAPI PFNGLRECTSVPROC glad_glRectsv; +#define glRectsv glad_glRectsv +typedef void (APIENTRYP PFNGLTEXCOORD1DPROC)(GLdouble s); +GLAPI PFNGLTEXCOORD1DPROC glad_glTexCoord1d; +#define glTexCoord1d glad_glTexCoord1d +typedef void (APIENTRYP PFNGLTEXCOORD1DVPROC)(const GLdouble *v); +GLAPI PFNGLTEXCOORD1DVPROC glad_glTexCoord1dv; +#define glTexCoord1dv glad_glTexCoord1dv +typedef void (APIENTRYP PFNGLTEXCOORD1FPROC)(GLfloat s); +GLAPI PFNGLTEXCOORD1FPROC glad_glTexCoord1f; +#define glTexCoord1f glad_glTexCoord1f +typedef void (APIENTRYP PFNGLTEXCOORD1FVPROC)(const GLfloat *v); +GLAPI PFNGLTEXCOORD1FVPROC glad_glTexCoord1fv; +#define glTexCoord1fv glad_glTexCoord1fv +typedef void (APIENTRYP PFNGLTEXCOORD1IPROC)(GLint s); +GLAPI PFNGLTEXCOORD1IPROC glad_glTexCoord1i; +#define glTexCoord1i glad_glTexCoord1i +typedef void (APIENTRYP PFNGLTEXCOORD1IVPROC)(const GLint *v); +GLAPI PFNGLTEXCOORD1IVPROC glad_glTexCoord1iv; +#define glTexCoord1iv glad_glTexCoord1iv +typedef void (APIENTRYP PFNGLTEXCOORD1SPROC)(GLshort s); +GLAPI PFNGLTEXCOORD1SPROC glad_glTexCoord1s; +#define glTexCoord1s glad_glTexCoord1s +typedef void (APIENTRYP PFNGLTEXCOORD1SVPROC)(const GLshort *v); +GLAPI PFNGLTEXCOORD1SVPROC glad_glTexCoord1sv; +#define glTexCoord1sv glad_glTexCoord1sv +typedef void (APIENTRYP PFNGLTEXCOORD2DPROC)(GLdouble s, GLdouble t); +GLAPI PFNGLTEXCOORD2DPROC glad_glTexCoord2d; +#define glTexCoord2d glad_glTexCoord2d +typedef void (APIENTRYP PFNGLTEXCOORD2DVPROC)(const GLdouble *v); +GLAPI PFNGLTEXCOORD2DVPROC glad_glTexCoord2dv; +#define glTexCoord2dv glad_glTexCoord2dv +typedef void (APIENTRYP PFNGLTEXCOORD2FPROC)(GLfloat s, GLfloat t); +GLAPI PFNGLTEXCOORD2FPROC glad_glTexCoord2f; +#define glTexCoord2f glad_glTexCoord2f +typedef void (APIENTRYP PFNGLTEXCOORD2FVPROC)(const GLfloat *v); +GLAPI PFNGLTEXCOORD2FVPROC glad_glTexCoord2fv; +#define glTexCoord2fv glad_glTexCoord2fv +typedef void (APIENTRYP PFNGLTEXCOORD2IPROC)(GLint s, GLint t); +GLAPI PFNGLTEXCOORD2IPROC glad_glTexCoord2i; +#define glTexCoord2i glad_glTexCoord2i +typedef void (APIENTRYP PFNGLTEXCOORD2IVPROC)(const GLint *v); +GLAPI PFNGLTEXCOORD2IVPROC glad_glTexCoord2iv; +#define glTexCoord2iv glad_glTexCoord2iv +typedef void (APIENTRYP PFNGLTEXCOORD2SPROC)(GLshort s, GLshort t); +GLAPI PFNGLTEXCOORD2SPROC glad_glTexCoord2s; +#define glTexCoord2s glad_glTexCoord2s +typedef void (APIENTRYP PFNGLTEXCOORD2SVPROC)(const GLshort *v); +GLAPI PFNGLTEXCOORD2SVPROC glad_glTexCoord2sv; +#define glTexCoord2sv glad_glTexCoord2sv +typedef void (APIENTRYP PFNGLTEXCOORD3DPROC)(GLdouble s, GLdouble t, GLdouble r); +GLAPI PFNGLTEXCOORD3DPROC glad_glTexCoord3d; +#define glTexCoord3d glad_glTexCoord3d +typedef void (APIENTRYP PFNGLTEXCOORD3DVPROC)(const GLdouble *v); +GLAPI PFNGLTEXCOORD3DVPROC glad_glTexCoord3dv; +#define glTexCoord3dv glad_glTexCoord3dv +typedef void (APIENTRYP PFNGLTEXCOORD3FPROC)(GLfloat s, GLfloat t, GLfloat r); +GLAPI PFNGLTEXCOORD3FPROC glad_glTexCoord3f; +#define glTexCoord3f glad_glTexCoord3f +typedef void (APIENTRYP PFNGLTEXCOORD3FVPROC)(const GLfloat *v); +GLAPI PFNGLTEXCOORD3FVPROC glad_glTexCoord3fv; +#define glTexCoord3fv glad_glTexCoord3fv +typedef void (APIENTRYP PFNGLTEXCOORD3IPROC)(GLint s, GLint t, GLint r); +GLAPI PFNGLTEXCOORD3IPROC glad_glTexCoord3i; +#define glTexCoord3i glad_glTexCoord3i +typedef void (APIENTRYP PFNGLTEXCOORD3IVPROC)(const GLint *v); +GLAPI PFNGLTEXCOORD3IVPROC glad_glTexCoord3iv; +#define glTexCoord3iv glad_glTexCoord3iv +typedef void (APIENTRYP PFNGLTEXCOORD3SPROC)(GLshort s, GLshort t, GLshort r); +GLAPI PFNGLTEXCOORD3SPROC glad_glTexCoord3s; +#define glTexCoord3s glad_glTexCoord3s +typedef void (APIENTRYP PFNGLTEXCOORD3SVPROC)(const GLshort *v); +GLAPI PFNGLTEXCOORD3SVPROC glad_glTexCoord3sv; +#define glTexCoord3sv glad_glTexCoord3sv +typedef void (APIENTRYP PFNGLTEXCOORD4DPROC)(GLdouble s, GLdouble t, GLdouble r, GLdouble q); +GLAPI PFNGLTEXCOORD4DPROC glad_glTexCoord4d; +#define glTexCoord4d glad_glTexCoord4d +typedef void (APIENTRYP PFNGLTEXCOORD4DVPROC)(const GLdouble *v); +GLAPI PFNGLTEXCOORD4DVPROC glad_glTexCoord4dv; +#define glTexCoord4dv glad_glTexCoord4dv +typedef void (APIENTRYP PFNGLTEXCOORD4FPROC)(GLfloat s, GLfloat t, GLfloat r, GLfloat q); +GLAPI PFNGLTEXCOORD4FPROC glad_glTexCoord4f; +#define glTexCoord4f glad_glTexCoord4f +typedef void (APIENTRYP PFNGLTEXCOORD4FVPROC)(const GLfloat *v); +GLAPI PFNGLTEXCOORD4FVPROC glad_glTexCoord4fv; +#define glTexCoord4fv glad_glTexCoord4fv +typedef void (APIENTRYP PFNGLTEXCOORD4IPROC)(GLint s, GLint t, GLint r, GLint q); +GLAPI PFNGLTEXCOORD4IPROC glad_glTexCoord4i; +#define glTexCoord4i glad_glTexCoord4i +typedef void (APIENTRYP PFNGLTEXCOORD4IVPROC)(const GLint *v); +GLAPI PFNGLTEXCOORD4IVPROC glad_glTexCoord4iv; +#define glTexCoord4iv glad_glTexCoord4iv +typedef void (APIENTRYP PFNGLTEXCOORD4SPROC)(GLshort s, GLshort t, GLshort r, GLshort q); +GLAPI PFNGLTEXCOORD4SPROC glad_glTexCoord4s; +#define glTexCoord4s glad_glTexCoord4s +typedef void (APIENTRYP PFNGLTEXCOORD4SVPROC)(const GLshort *v); +GLAPI PFNGLTEXCOORD4SVPROC glad_glTexCoord4sv; +#define glTexCoord4sv glad_glTexCoord4sv +typedef void (APIENTRYP PFNGLVERTEX2DPROC)(GLdouble x, GLdouble y); +GLAPI PFNGLVERTEX2DPROC glad_glVertex2d; +#define glVertex2d glad_glVertex2d +typedef void (APIENTRYP PFNGLVERTEX2DVPROC)(const GLdouble *v); +GLAPI PFNGLVERTEX2DVPROC glad_glVertex2dv; +#define glVertex2dv glad_glVertex2dv +typedef void (APIENTRYP PFNGLVERTEX2FPROC)(GLfloat x, GLfloat y); +GLAPI PFNGLVERTEX2FPROC glad_glVertex2f; +#define glVertex2f glad_glVertex2f +typedef void (APIENTRYP PFNGLVERTEX2FVPROC)(const GLfloat *v); +GLAPI PFNGLVERTEX2FVPROC glad_glVertex2fv; +#define glVertex2fv glad_glVertex2fv +typedef void (APIENTRYP PFNGLVERTEX2IPROC)(GLint x, GLint y); +GLAPI PFNGLVERTEX2IPROC glad_glVertex2i; +#define glVertex2i glad_glVertex2i +typedef void (APIENTRYP PFNGLVERTEX2IVPROC)(const GLint *v); +GLAPI PFNGLVERTEX2IVPROC glad_glVertex2iv; +#define glVertex2iv glad_glVertex2iv +typedef void (APIENTRYP PFNGLVERTEX2SPROC)(GLshort x, GLshort y); +GLAPI PFNGLVERTEX2SPROC glad_glVertex2s; +#define glVertex2s glad_glVertex2s +typedef void (APIENTRYP PFNGLVERTEX2SVPROC)(const GLshort *v); +GLAPI PFNGLVERTEX2SVPROC glad_glVertex2sv; +#define glVertex2sv glad_glVertex2sv +typedef void (APIENTRYP PFNGLVERTEX3DPROC)(GLdouble x, GLdouble y, GLdouble z); +GLAPI PFNGLVERTEX3DPROC glad_glVertex3d; +#define glVertex3d glad_glVertex3d +typedef void (APIENTRYP PFNGLVERTEX3DVPROC)(const GLdouble *v); +GLAPI PFNGLVERTEX3DVPROC glad_glVertex3dv; +#define glVertex3dv glad_glVertex3dv +typedef void (APIENTRYP PFNGLVERTEX3FPROC)(GLfloat x, GLfloat y, GLfloat z); +GLAPI PFNGLVERTEX3FPROC glad_glVertex3f; +#define glVertex3f glad_glVertex3f +typedef void (APIENTRYP PFNGLVERTEX3FVPROC)(const GLfloat *v); +GLAPI PFNGLVERTEX3FVPROC glad_glVertex3fv; +#define glVertex3fv glad_glVertex3fv +typedef void (APIENTRYP PFNGLVERTEX3IPROC)(GLint x, GLint y, GLint z); +GLAPI PFNGLVERTEX3IPROC glad_glVertex3i; +#define glVertex3i glad_glVertex3i +typedef void (APIENTRYP PFNGLVERTEX3IVPROC)(const GLint *v); +GLAPI PFNGLVERTEX3IVPROC glad_glVertex3iv; +#define glVertex3iv glad_glVertex3iv +typedef void (APIENTRYP PFNGLVERTEX3SPROC)(GLshort x, GLshort y, GLshort z); +GLAPI PFNGLVERTEX3SPROC glad_glVertex3s; +#define glVertex3s glad_glVertex3s +typedef void (APIENTRYP PFNGLVERTEX3SVPROC)(const GLshort *v); +GLAPI PFNGLVERTEX3SVPROC glad_glVertex3sv; +#define glVertex3sv glad_glVertex3sv +typedef void (APIENTRYP PFNGLVERTEX4DPROC)(GLdouble x, GLdouble y, GLdouble z, GLdouble w); +GLAPI PFNGLVERTEX4DPROC glad_glVertex4d; +#define glVertex4d glad_glVertex4d +typedef void (APIENTRYP PFNGLVERTEX4DVPROC)(const GLdouble *v); +GLAPI PFNGLVERTEX4DVPROC glad_glVertex4dv; +#define glVertex4dv glad_glVertex4dv +typedef void (APIENTRYP PFNGLVERTEX4FPROC)(GLfloat x, GLfloat y, GLfloat z, GLfloat w); +GLAPI PFNGLVERTEX4FPROC glad_glVertex4f; +#define glVertex4f glad_glVertex4f +typedef void (APIENTRYP PFNGLVERTEX4FVPROC)(const GLfloat *v); +GLAPI PFNGLVERTEX4FVPROC glad_glVertex4fv; +#define glVertex4fv glad_glVertex4fv +typedef void (APIENTRYP PFNGLVERTEX4IPROC)(GLint x, GLint y, GLint z, GLint w); +GLAPI PFNGLVERTEX4IPROC glad_glVertex4i; +#define glVertex4i glad_glVertex4i +typedef void (APIENTRYP PFNGLVERTEX4IVPROC)(const GLint *v); +GLAPI PFNGLVERTEX4IVPROC glad_glVertex4iv; +#define glVertex4iv glad_glVertex4iv +typedef void (APIENTRYP PFNGLVERTEX4SPROC)(GLshort x, GLshort y, GLshort z, GLshort w); +GLAPI PFNGLVERTEX4SPROC glad_glVertex4s; +#define glVertex4s glad_glVertex4s +typedef void (APIENTRYP PFNGLVERTEX4SVPROC)(const GLshort *v); +GLAPI PFNGLVERTEX4SVPROC glad_glVertex4sv; +#define glVertex4sv glad_glVertex4sv +typedef void (APIENTRYP PFNGLCLIPPLANEPROC)(GLenum plane, const GLdouble *equation); +GLAPI PFNGLCLIPPLANEPROC glad_glClipPlane; +#define glClipPlane glad_glClipPlane +typedef void (APIENTRYP PFNGLCOLORMATERIALPROC)(GLenum face, GLenum mode); +GLAPI PFNGLCOLORMATERIALPROC glad_glColorMaterial; +#define glColorMaterial glad_glColorMaterial +typedef void (APIENTRYP PFNGLFOGFPROC)(GLenum pname, GLfloat param); +GLAPI PFNGLFOGFPROC glad_glFogf; +#define glFogf glad_glFogf +typedef void (APIENTRYP PFNGLFOGFVPROC)(GLenum pname, const GLfloat *params); +GLAPI PFNGLFOGFVPROC glad_glFogfv; +#define glFogfv glad_glFogfv +typedef void (APIENTRYP PFNGLFOGIPROC)(GLenum pname, GLint param); +GLAPI PFNGLFOGIPROC glad_glFogi; +#define glFogi glad_glFogi +typedef void (APIENTRYP PFNGLFOGIVPROC)(GLenum pname, const GLint *params); +GLAPI PFNGLFOGIVPROC glad_glFogiv; +#define glFogiv glad_glFogiv +typedef void (APIENTRYP PFNGLLIGHTFPROC)(GLenum light, GLenum pname, GLfloat param); +GLAPI PFNGLLIGHTFPROC glad_glLightf; +#define glLightf glad_glLightf +typedef void (APIENTRYP PFNGLLIGHTFVPROC)(GLenum light, GLenum pname, const GLfloat *params); +GLAPI PFNGLLIGHTFVPROC glad_glLightfv; +#define glLightfv glad_glLightfv +typedef void (APIENTRYP PFNGLLIGHTIPROC)(GLenum light, GLenum pname, GLint param); +GLAPI PFNGLLIGHTIPROC glad_glLighti; +#define glLighti glad_glLighti +typedef void (APIENTRYP PFNGLLIGHTIVPROC)(GLenum light, GLenum pname, const GLint *params); +GLAPI PFNGLLIGHTIVPROC glad_glLightiv; +#define glLightiv glad_glLightiv +typedef void (APIENTRYP PFNGLLIGHTMODELFPROC)(GLenum pname, GLfloat param); +GLAPI PFNGLLIGHTMODELFPROC glad_glLightModelf; +#define glLightModelf glad_glLightModelf +typedef void (APIENTRYP PFNGLLIGHTMODELFVPROC)(GLenum pname, const GLfloat *params); +GLAPI PFNGLLIGHTMODELFVPROC glad_glLightModelfv; +#define glLightModelfv glad_glLightModelfv +typedef void (APIENTRYP PFNGLLIGHTMODELIPROC)(GLenum pname, GLint param); +GLAPI PFNGLLIGHTMODELIPROC glad_glLightModeli; +#define glLightModeli glad_glLightModeli +typedef void (APIENTRYP PFNGLLIGHTMODELIVPROC)(GLenum pname, const GLint *params); +GLAPI PFNGLLIGHTMODELIVPROC glad_glLightModeliv; +#define glLightModeliv glad_glLightModeliv +typedef void (APIENTRYP PFNGLLINESTIPPLEPROC)(GLint factor, GLushort pattern); +GLAPI PFNGLLINESTIPPLEPROC glad_glLineStipple; +#define glLineStipple glad_glLineStipple +typedef void (APIENTRYP PFNGLMATERIALFPROC)(GLenum face, GLenum pname, GLfloat param); +GLAPI PFNGLMATERIALFPROC glad_glMaterialf; +#define glMaterialf glad_glMaterialf +typedef void (APIENTRYP PFNGLMATERIALFVPROC)(GLenum face, GLenum pname, const GLfloat *params); +GLAPI PFNGLMATERIALFVPROC glad_glMaterialfv; +#define glMaterialfv glad_glMaterialfv +typedef void (APIENTRYP PFNGLMATERIALIPROC)(GLenum face, GLenum pname, GLint param); +GLAPI PFNGLMATERIALIPROC glad_glMateriali; +#define glMateriali glad_glMateriali +typedef void (APIENTRYP PFNGLMATERIALIVPROC)(GLenum face, GLenum pname, const GLint *params); +GLAPI PFNGLMATERIALIVPROC glad_glMaterialiv; +#define glMaterialiv glad_glMaterialiv +typedef void (APIENTRYP PFNGLPOLYGONSTIPPLEPROC)(const GLubyte *mask); +GLAPI PFNGLPOLYGONSTIPPLEPROC glad_glPolygonStipple; +#define glPolygonStipple glad_glPolygonStipple +typedef void (APIENTRYP PFNGLSHADEMODELPROC)(GLenum mode); +GLAPI PFNGLSHADEMODELPROC glad_glShadeModel; +#define glShadeModel glad_glShadeModel +typedef void (APIENTRYP PFNGLTEXENVFPROC)(GLenum target, GLenum pname, GLfloat param); +GLAPI PFNGLTEXENVFPROC glad_glTexEnvf; +#define glTexEnvf glad_glTexEnvf +typedef void (APIENTRYP PFNGLTEXENVFVPROC)(GLenum target, GLenum pname, const GLfloat *params); +GLAPI PFNGLTEXENVFVPROC glad_glTexEnvfv; +#define glTexEnvfv glad_glTexEnvfv +typedef void (APIENTRYP PFNGLTEXENVIPROC)(GLenum target, GLenum pname, GLint param); +GLAPI PFNGLTEXENVIPROC glad_glTexEnvi; +#define glTexEnvi glad_glTexEnvi +typedef void (APIENTRYP PFNGLTEXENVIVPROC)(GLenum target, GLenum pname, const GLint *params); +GLAPI PFNGLTEXENVIVPROC glad_glTexEnviv; +#define glTexEnviv glad_glTexEnviv +typedef void (APIENTRYP PFNGLTEXGENDPROC)(GLenum coord, GLenum pname, GLdouble param); +GLAPI PFNGLTEXGENDPROC glad_glTexGend; +#define glTexGend glad_glTexGend +typedef void (APIENTRYP PFNGLTEXGENDVPROC)(GLenum coord, GLenum pname, const GLdouble *params); +GLAPI PFNGLTEXGENDVPROC glad_glTexGendv; +#define glTexGendv glad_glTexGendv +typedef void (APIENTRYP PFNGLTEXGENFPROC)(GLenum coord, GLenum pname, GLfloat param); +GLAPI PFNGLTEXGENFPROC glad_glTexGenf; +#define glTexGenf glad_glTexGenf +typedef void (APIENTRYP PFNGLTEXGENFVPROC)(GLenum coord, GLenum pname, const GLfloat *params); +GLAPI PFNGLTEXGENFVPROC glad_glTexGenfv; +#define glTexGenfv glad_glTexGenfv +typedef void (APIENTRYP PFNGLTEXGENIPROC)(GLenum coord, GLenum pname, GLint param); +GLAPI PFNGLTEXGENIPROC glad_glTexGeni; +#define glTexGeni glad_glTexGeni +typedef void (APIENTRYP PFNGLTEXGENIVPROC)(GLenum coord, GLenum pname, const GLint *params); +GLAPI PFNGLTEXGENIVPROC glad_glTexGeniv; +#define glTexGeniv glad_glTexGeniv +typedef void (APIENTRYP PFNGLFEEDBACKBUFFERPROC)(GLsizei size, GLenum type, GLfloat *buffer); +GLAPI PFNGLFEEDBACKBUFFERPROC glad_glFeedbackBuffer; +#define glFeedbackBuffer glad_glFeedbackBuffer +typedef void (APIENTRYP PFNGLSELECTBUFFERPROC)(GLsizei size, GLuint *buffer); +GLAPI PFNGLSELECTBUFFERPROC glad_glSelectBuffer; +#define glSelectBuffer glad_glSelectBuffer +typedef GLint (APIENTRYP PFNGLRENDERMODEPROC)(GLenum mode); +GLAPI PFNGLRENDERMODEPROC glad_glRenderMode; +#define glRenderMode glad_glRenderMode +typedef void (APIENTRYP PFNGLINITNAMESPROC)(void); +GLAPI PFNGLINITNAMESPROC glad_glInitNames; +#define glInitNames glad_glInitNames +typedef void (APIENTRYP PFNGLLOADNAMEPROC)(GLuint name); +GLAPI PFNGLLOADNAMEPROC glad_glLoadName; +#define glLoadName glad_glLoadName +typedef void (APIENTRYP PFNGLPASSTHROUGHPROC)(GLfloat token); +GLAPI PFNGLPASSTHROUGHPROC glad_glPassThrough; +#define glPassThrough glad_glPassThrough +typedef void (APIENTRYP PFNGLPOPNAMEPROC)(void); +GLAPI PFNGLPOPNAMEPROC glad_glPopName; +#define glPopName glad_glPopName +typedef void (APIENTRYP PFNGLPUSHNAMEPROC)(GLuint name); +GLAPI PFNGLPUSHNAMEPROC glad_glPushName; +#define glPushName glad_glPushName +typedef void (APIENTRYP PFNGLCLEARACCUMPROC)(GLfloat red, GLfloat green, GLfloat blue, GLfloat alpha); +GLAPI PFNGLCLEARACCUMPROC glad_glClearAccum; +#define glClearAccum glad_glClearAccum +typedef void (APIENTRYP PFNGLCLEARINDEXPROC)(GLfloat c); +GLAPI PFNGLCLEARINDEXPROC glad_glClearIndex; +#define glClearIndex glad_glClearIndex +typedef void (APIENTRYP PFNGLINDEXMASKPROC)(GLuint mask); +GLAPI PFNGLINDEXMASKPROC glad_glIndexMask; +#define glIndexMask glad_glIndexMask +typedef void (APIENTRYP PFNGLACCUMPROC)(GLenum op, GLfloat value); +GLAPI PFNGLACCUMPROC glad_glAccum; +#define glAccum glad_glAccum +typedef void (APIENTRYP PFNGLPOPATTRIBPROC)(void); +GLAPI PFNGLPOPATTRIBPROC glad_glPopAttrib; +#define glPopAttrib glad_glPopAttrib +typedef void (APIENTRYP PFNGLPUSHATTRIBPROC)(GLbitfield mask); +GLAPI PFNGLPUSHATTRIBPROC glad_glPushAttrib; +#define glPushAttrib glad_glPushAttrib +typedef void (APIENTRYP PFNGLMAP1DPROC)(GLenum target, GLdouble u1, GLdouble u2, GLint stride, GLint order, const GLdouble *points); +GLAPI PFNGLMAP1DPROC glad_glMap1d; +#define glMap1d glad_glMap1d +typedef void (APIENTRYP PFNGLMAP1FPROC)(GLenum target, GLfloat u1, GLfloat u2, GLint stride, GLint order, const GLfloat *points); +GLAPI PFNGLMAP1FPROC glad_glMap1f; +#define glMap1f glad_glMap1f +typedef void (APIENTRYP PFNGLMAP2DPROC)(GLenum target, GLdouble u1, GLdouble u2, GLint ustride, GLint uorder, GLdouble v1, GLdouble v2, GLint vstride, GLint vorder, const GLdouble *points); +GLAPI PFNGLMAP2DPROC glad_glMap2d; +#define glMap2d glad_glMap2d +typedef void (APIENTRYP PFNGLMAP2FPROC)(GLenum target, GLfloat u1, GLfloat u2, GLint ustride, GLint uorder, GLfloat v1, GLfloat v2, GLint vstride, GLint vorder, const GLfloat *points); +GLAPI PFNGLMAP2FPROC glad_glMap2f; +#define glMap2f glad_glMap2f +typedef void (APIENTRYP PFNGLMAPGRID1DPROC)(GLint un, GLdouble u1, GLdouble u2); +GLAPI PFNGLMAPGRID1DPROC glad_glMapGrid1d; +#define glMapGrid1d glad_glMapGrid1d +typedef void (APIENTRYP PFNGLMAPGRID1FPROC)(GLint un, GLfloat u1, GLfloat u2); +GLAPI PFNGLMAPGRID1FPROC glad_glMapGrid1f; +#define glMapGrid1f glad_glMapGrid1f +typedef void (APIENTRYP PFNGLMAPGRID2DPROC)(GLint un, GLdouble u1, GLdouble u2, GLint vn, GLdouble v1, GLdouble v2); +GLAPI PFNGLMAPGRID2DPROC glad_glMapGrid2d; +#define glMapGrid2d glad_glMapGrid2d +typedef void (APIENTRYP PFNGLMAPGRID2FPROC)(GLint un, GLfloat u1, GLfloat u2, GLint vn, GLfloat v1, GLfloat v2); +GLAPI PFNGLMAPGRID2FPROC glad_glMapGrid2f; +#define glMapGrid2f glad_glMapGrid2f +typedef void (APIENTRYP PFNGLEVALCOORD1DPROC)(GLdouble u); +GLAPI PFNGLEVALCOORD1DPROC glad_glEvalCoord1d; +#define glEvalCoord1d glad_glEvalCoord1d +typedef void (APIENTRYP PFNGLEVALCOORD1DVPROC)(const GLdouble *u); +GLAPI PFNGLEVALCOORD1DVPROC glad_glEvalCoord1dv; +#define glEvalCoord1dv glad_glEvalCoord1dv +typedef void (APIENTRYP PFNGLEVALCOORD1FPROC)(GLfloat u); +GLAPI PFNGLEVALCOORD1FPROC glad_glEvalCoord1f; +#define glEvalCoord1f glad_glEvalCoord1f +typedef void (APIENTRYP PFNGLEVALCOORD1FVPROC)(const GLfloat *u); +GLAPI PFNGLEVALCOORD1FVPROC glad_glEvalCoord1fv; +#define glEvalCoord1fv glad_glEvalCoord1fv +typedef void (APIENTRYP PFNGLEVALCOORD2DPROC)(GLdouble u, GLdouble v); +GLAPI PFNGLEVALCOORD2DPROC glad_glEvalCoord2d; +#define glEvalCoord2d glad_glEvalCoord2d +typedef void (APIENTRYP PFNGLEVALCOORD2DVPROC)(const GLdouble *u); +GLAPI PFNGLEVALCOORD2DVPROC glad_glEvalCoord2dv; +#define glEvalCoord2dv glad_glEvalCoord2dv +typedef void (APIENTRYP PFNGLEVALCOORD2FPROC)(GLfloat u, GLfloat v); +GLAPI PFNGLEVALCOORD2FPROC glad_glEvalCoord2f; +#define glEvalCoord2f glad_glEvalCoord2f +typedef void (APIENTRYP PFNGLEVALCOORD2FVPROC)(const GLfloat *u); +GLAPI PFNGLEVALCOORD2FVPROC glad_glEvalCoord2fv; +#define glEvalCoord2fv glad_glEvalCoord2fv +typedef void (APIENTRYP PFNGLEVALMESH1PROC)(GLenum mode, GLint i1, GLint i2); +GLAPI PFNGLEVALMESH1PROC glad_glEvalMesh1; +#define glEvalMesh1 glad_glEvalMesh1 +typedef void (APIENTRYP PFNGLEVALPOINT1PROC)(GLint i); +GLAPI PFNGLEVALPOINT1PROC glad_glEvalPoint1; +#define glEvalPoint1 glad_glEvalPoint1 +typedef void (APIENTRYP PFNGLEVALMESH2PROC)(GLenum mode, GLint i1, GLint i2, GLint j1, GLint j2); +GLAPI PFNGLEVALMESH2PROC glad_glEvalMesh2; +#define glEvalMesh2 glad_glEvalMesh2 +typedef void (APIENTRYP PFNGLEVALPOINT2PROC)(GLint i, GLint j); +GLAPI PFNGLEVALPOINT2PROC glad_glEvalPoint2; +#define glEvalPoint2 glad_glEvalPoint2 +typedef void (APIENTRYP PFNGLALPHAFUNCPROC)(GLenum func, GLfloat ref); +GLAPI PFNGLALPHAFUNCPROC glad_glAlphaFunc; +#define glAlphaFunc glad_glAlphaFunc +typedef void (APIENTRYP PFNGLPIXELZOOMPROC)(GLfloat xfactor, GLfloat yfactor); +GLAPI PFNGLPIXELZOOMPROC glad_glPixelZoom; +#define glPixelZoom glad_glPixelZoom +typedef void (APIENTRYP PFNGLPIXELTRANSFERFPROC)(GLenum pname, GLfloat param); +GLAPI PFNGLPIXELTRANSFERFPROC glad_glPixelTransferf; +#define glPixelTransferf glad_glPixelTransferf +typedef void (APIENTRYP PFNGLPIXELTRANSFERIPROC)(GLenum pname, GLint param); +GLAPI PFNGLPIXELTRANSFERIPROC glad_glPixelTransferi; +#define glPixelTransferi glad_glPixelTransferi +typedef void (APIENTRYP PFNGLPIXELMAPFVPROC)(GLenum map, GLsizei mapsize, const GLfloat *values); +GLAPI PFNGLPIXELMAPFVPROC glad_glPixelMapfv; +#define glPixelMapfv glad_glPixelMapfv +typedef void (APIENTRYP PFNGLPIXELMAPUIVPROC)(GLenum map, GLsizei mapsize, const GLuint *values); +GLAPI PFNGLPIXELMAPUIVPROC glad_glPixelMapuiv; +#define glPixelMapuiv glad_glPixelMapuiv +typedef void (APIENTRYP PFNGLPIXELMAPUSVPROC)(GLenum map, GLsizei mapsize, const GLushort *values); +GLAPI PFNGLPIXELMAPUSVPROC glad_glPixelMapusv; +#define glPixelMapusv glad_glPixelMapusv +typedef void (APIENTRYP PFNGLCOPYPIXELSPROC)(GLint x, GLint y, GLsizei width, GLsizei height, GLenum type); +GLAPI PFNGLCOPYPIXELSPROC glad_glCopyPixels; +#define glCopyPixels glad_glCopyPixels +typedef void (APIENTRYP PFNGLDRAWPIXELSPROC)(GLsizei width, GLsizei height, GLenum format, GLenum type, const void *pixels); +GLAPI PFNGLDRAWPIXELSPROC glad_glDrawPixels; +#define glDrawPixels glad_glDrawPixels +typedef void (APIENTRYP PFNGLGETCLIPPLANEPROC)(GLenum plane, GLdouble *equation); +GLAPI PFNGLGETCLIPPLANEPROC glad_glGetClipPlane; +#define glGetClipPlane glad_glGetClipPlane +typedef void (APIENTRYP PFNGLGETLIGHTFVPROC)(GLenum light, GLenum pname, GLfloat *params); +GLAPI PFNGLGETLIGHTFVPROC glad_glGetLightfv; +#define glGetLightfv glad_glGetLightfv +typedef void (APIENTRYP PFNGLGETLIGHTIVPROC)(GLenum light, GLenum pname, GLint *params); +GLAPI PFNGLGETLIGHTIVPROC glad_glGetLightiv; +#define glGetLightiv glad_glGetLightiv +typedef void (APIENTRYP PFNGLGETMAPDVPROC)(GLenum target, GLenum query, GLdouble *v); +GLAPI PFNGLGETMAPDVPROC glad_glGetMapdv; +#define glGetMapdv glad_glGetMapdv +typedef void (APIENTRYP PFNGLGETMAPFVPROC)(GLenum target, GLenum query, GLfloat *v); +GLAPI PFNGLGETMAPFVPROC glad_glGetMapfv; +#define glGetMapfv glad_glGetMapfv +typedef void (APIENTRYP PFNGLGETMAPIVPROC)(GLenum target, GLenum query, GLint *v); +GLAPI PFNGLGETMAPIVPROC glad_glGetMapiv; +#define glGetMapiv glad_glGetMapiv +typedef void (APIENTRYP PFNGLGETMATERIALFVPROC)(GLenum face, GLenum pname, GLfloat *params); +GLAPI PFNGLGETMATERIALFVPROC glad_glGetMaterialfv; +#define glGetMaterialfv glad_glGetMaterialfv +typedef void (APIENTRYP PFNGLGETMATERIALIVPROC)(GLenum face, GLenum pname, GLint *params); +GLAPI PFNGLGETMATERIALIVPROC glad_glGetMaterialiv; +#define glGetMaterialiv glad_glGetMaterialiv +typedef void (APIENTRYP PFNGLGETPIXELMAPFVPROC)(GLenum map, GLfloat *values); +GLAPI PFNGLGETPIXELMAPFVPROC glad_glGetPixelMapfv; +#define glGetPixelMapfv glad_glGetPixelMapfv +typedef void (APIENTRYP PFNGLGETPIXELMAPUIVPROC)(GLenum map, GLuint *values); +GLAPI PFNGLGETPIXELMAPUIVPROC glad_glGetPixelMapuiv; +#define glGetPixelMapuiv glad_glGetPixelMapuiv +typedef void (APIENTRYP PFNGLGETPIXELMAPUSVPROC)(GLenum map, GLushort *values); +GLAPI PFNGLGETPIXELMAPUSVPROC glad_glGetPixelMapusv; +#define glGetPixelMapusv glad_glGetPixelMapusv +typedef void (APIENTRYP PFNGLGETPOLYGONSTIPPLEPROC)(GLubyte *mask); +GLAPI PFNGLGETPOLYGONSTIPPLEPROC glad_glGetPolygonStipple; +#define glGetPolygonStipple glad_glGetPolygonStipple +typedef void (APIENTRYP PFNGLGETTEXENVFVPROC)(GLenum target, GLenum pname, GLfloat *params); +GLAPI PFNGLGETTEXENVFVPROC glad_glGetTexEnvfv; +#define glGetTexEnvfv glad_glGetTexEnvfv +typedef void (APIENTRYP PFNGLGETTEXENVIVPROC)(GLenum target, GLenum pname, GLint *params); +GLAPI PFNGLGETTEXENVIVPROC glad_glGetTexEnviv; +#define glGetTexEnviv glad_glGetTexEnviv +typedef void (APIENTRYP PFNGLGETTEXGENDVPROC)(GLenum coord, GLenum pname, GLdouble *params); +GLAPI PFNGLGETTEXGENDVPROC glad_glGetTexGendv; +#define glGetTexGendv glad_glGetTexGendv +typedef void (APIENTRYP PFNGLGETTEXGENFVPROC)(GLenum coord, GLenum pname, GLfloat *params); +GLAPI PFNGLGETTEXGENFVPROC glad_glGetTexGenfv; +#define glGetTexGenfv glad_glGetTexGenfv +typedef void (APIENTRYP PFNGLGETTEXGENIVPROC)(GLenum coord, GLenum pname, GLint *params); +GLAPI PFNGLGETTEXGENIVPROC glad_glGetTexGeniv; +#define glGetTexGeniv glad_glGetTexGeniv +typedef GLboolean (APIENTRYP PFNGLISLISTPROC)(GLuint list); +GLAPI PFNGLISLISTPROC glad_glIsList; +#define glIsList glad_glIsList +typedef void (APIENTRYP PFNGLFRUSTUMPROC)(GLdouble left, GLdouble right, GLdouble bottom, GLdouble top, GLdouble zNear, GLdouble zFar); +GLAPI PFNGLFRUSTUMPROC glad_glFrustum; +#define glFrustum glad_glFrustum +typedef void (APIENTRYP PFNGLLOADIDENTITYPROC)(void); +GLAPI PFNGLLOADIDENTITYPROC glad_glLoadIdentity; +#define glLoadIdentity glad_glLoadIdentity +typedef void (APIENTRYP PFNGLLOADMATRIXFPROC)(const GLfloat *m); +GLAPI PFNGLLOADMATRIXFPROC glad_glLoadMatrixf; +#define glLoadMatrixf glad_glLoadMatrixf +typedef void (APIENTRYP PFNGLLOADMATRIXDPROC)(const GLdouble *m); +GLAPI PFNGLLOADMATRIXDPROC glad_glLoadMatrixd; +#define glLoadMatrixd glad_glLoadMatrixd +typedef void (APIENTRYP PFNGLMATRIXMODEPROC)(GLenum mode); +GLAPI PFNGLMATRIXMODEPROC glad_glMatrixMode; +#define glMatrixMode glad_glMatrixMode +typedef void (APIENTRYP PFNGLMULTMATRIXFPROC)(const GLfloat *m); +GLAPI PFNGLMULTMATRIXFPROC glad_glMultMatrixf; +#define glMultMatrixf glad_glMultMatrixf +typedef void (APIENTRYP PFNGLMULTMATRIXDPROC)(const GLdouble *m); +GLAPI PFNGLMULTMATRIXDPROC glad_glMultMatrixd; +#define glMultMatrixd glad_glMultMatrixd +typedef void (APIENTRYP PFNGLORTHOPROC)(GLdouble left, GLdouble right, GLdouble bottom, GLdouble top, GLdouble zNear, GLdouble zFar); +GLAPI PFNGLORTHOPROC glad_glOrtho; +#define glOrtho glad_glOrtho +typedef void (APIENTRYP PFNGLPOPMATRIXPROC)(void); +GLAPI PFNGLPOPMATRIXPROC glad_glPopMatrix; +#define glPopMatrix glad_glPopMatrix +typedef void (APIENTRYP PFNGLPUSHMATRIXPROC)(void); +GLAPI PFNGLPUSHMATRIXPROC glad_glPushMatrix; +#define glPushMatrix glad_glPushMatrix +typedef void (APIENTRYP PFNGLROTATEDPROC)(GLdouble angle, GLdouble x, GLdouble y, GLdouble z); +GLAPI PFNGLROTATEDPROC glad_glRotated; +#define glRotated glad_glRotated +typedef void (APIENTRYP PFNGLROTATEFPROC)(GLfloat angle, GLfloat x, GLfloat y, GLfloat z); +GLAPI PFNGLROTATEFPROC glad_glRotatef; +#define glRotatef glad_glRotatef +typedef void (APIENTRYP PFNGLSCALEDPROC)(GLdouble x, GLdouble y, GLdouble z); +GLAPI PFNGLSCALEDPROC glad_glScaled; +#define glScaled glad_glScaled +typedef void (APIENTRYP PFNGLSCALEFPROC)(GLfloat x, GLfloat y, GLfloat z); +GLAPI PFNGLSCALEFPROC glad_glScalef; +#define glScalef glad_glScalef +typedef void (APIENTRYP PFNGLTRANSLATEDPROC)(GLdouble x, GLdouble y, GLdouble z); +GLAPI PFNGLTRANSLATEDPROC glad_glTranslated; +#define glTranslated glad_glTranslated +typedef void (APIENTRYP PFNGLTRANSLATEFPROC)(GLfloat x, GLfloat y, GLfloat z); +GLAPI PFNGLTRANSLATEFPROC glad_glTranslatef; +#define glTranslatef glad_glTranslatef +#endif +#ifndef GL_VERSION_1_1 +#define GL_VERSION_1_1 1 +GLAPI int GLAD_GL_VERSION_1_1; +typedef void (APIENTRYP PFNGLDRAWARRAYSPROC)(GLenum mode, GLint first, GLsizei count); +GLAPI PFNGLDRAWARRAYSPROC glad_glDrawArrays; +#define glDrawArrays glad_glDrawArrays +typedef void (APIENTRYP PFNGLDRAWELEMENTSPROC)(GLenum mode, GLsizei count, GLenum type, const void *indices); +GLAPI PFNGLDRAWELEMENTSPROC glad_glDrawElements; +#define glDrawElements glad_glDrawElements +typedef void (APIENTRYP PFNGLGETPOINTERVPROC)(GLenum pname, void **params); +GLAPI PFNGLGETPOINTERVPROC glad_glGetPointerv; +#define glGetPointerv glad_glGetPointerv +typedef void (APIENTRYP PFNGLPOLYGONOFFSETPROC)(GLfloat factor, GLfloat units); +GLAPI PFNGLPOLYGONOFFSETPROC glad_glPolygonOffset; +#define glPolygonOffset glad_glPolygonOffset +typedef void (APIENTRYP PFNGLCOPYTEXIMAGE1DPROC)(GLenum target, GLint level, GLenum internalformat, GLint x, GLint y, GLsizei width, GLint border); +GLAPI PFNGLCOPYTEXIMAGE1DPROC glad_glCopyTexImage1D; +#define glCopyTexImage1D glad_glCopyTexImage1D +typedef void (APIENTRYP PFNGLCOPYTEXIMAGE2DPROC)(GLenum target, GLint level, GLenum internalformat, GLint x, GLint y, GLsizei width, GLsizei height, GLint border); +GLAPI PFNGLCOPYTEXIMAGE2DPROC glad_glCopyTexImage2D; +#define glCopyTexImage2D glad_glCopyTexImage2D +typedef void (APIENTRYP PFNGLCOPYTEXSUBIMAGE1DPROC)(GLenum target, GLint level, GLint xoffset, GLint x, GLint y, GLsizei width); +GLAPI PFNGLCOPYTEXSUBIMAGE1DPROC glad_glCopyTexSubImage1D; +#define glCopyTexSubImage1D glad_glCopyTexSubImage1D +typedef void (APIENTRYP PFNGLCOPYTEXSUBIMAGE2DPROC)(GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint x, GLint y, GLsizei width, GLsizei height); +GLAPI PFNGLCOPYTEXSUBIMAGE2DPROC glad_glCopyTexSubImage2D; +#define glCopyTexSubImage2D glad_glCopyTexSubImage2D +typedef void (APIENTRYP PFNGLTEXSUBIMAGE1DPROC)(GLenum target, GLint level, GLint xoffset, GLsizei width, GLenum format, GLenum type, const void *pixels); +GLAPI PFNGLTEXSUBIMAGE1DPROC glad_glTexSubImage1D; +#define glTexSubImage1D glad_glTexSubImage1D +typedef void (APIENTRYP PFNGLTEXSUBIMAGE2DPROC)(GLenum target, GLint level, GLint xoffset, GLint yoffset, GLsizei width, GLsizei height, GLenum format, GLenum type, const void *pixels); +GLAPI PFNGLTEXSUBIMAGE2DPROC glad_glTexSubImage2D; +#define glTexSubImage2D glad_glTexSubImage2D +typedef void (APIENTRYP PFNGLBINDTEXTUREPROC)(GLenum target, GLuint texture); +GLAPI PFNGLBINDTEXTUREPROC glad_glBindTexture; +#define glBindTexture glad_glBindTexture +typedef void (APIENTRYP PFNGLDELETETEXTURESPROC)(GLsizei n, const GLuint *textures); +GLAPI PFNGLDELETETEXTURESPROC glad_glDeleteTextures; +#define glDeleteTextures glad_glDeleteTextures +typedef void (APIENTRYP PFNGLGENTEXTURESPROC)(GLsizei n, GLuint *textures); +GLAPI PFNGLGENTEXTURESPROC glad_glGenTextures; +#define glGenTextures glad_glGenTextures +typedef GLboolean (APIENTRYP PFNGLISTEXTUREPROC)(GLuint texture); +GLAPI PFNGLISTEXTUREPROC glad_glIsTexture; +#define glIsTexture glad_glIsTexture +typedef void (APIENTRYP PFNGLARRAYELEMENTPROC)(GLint i); +GLAPI PFNGLARRAYELEMENTPROC glad_glArrayElement; +#define glArrayElement glad_glArrayElement +typedef void (APIENTRYP PFNGLCOLORPOINTERPROC)(GLint size, GLenum type, GLsizei stride, const void *pointer); +GLAPI PFNGLCOLORPOINTERPROC glad_glColorPointer; +#define glColorPointer glad_glColorPointer +typedef void (APIENTRYP PFNGLDISABLECLIENTSTATEPROC)(GLenum array); +GLAPI PFNGLDISABLECLIENTSTATEPROC glad_glDisableClientState; +#define glDisableClientState glad_glDisableClientState +typedef void (APIENTRYP PFNGLEDGEFLAGPOINTERPROC)(GLsizei stride, const void *pointer); +GLAPI PFNGLEDGEFLAGPOINTERPROC glad_glEdgeFlagPointer; +#define glEdgeFlagPointer glad_glEdgeFlagPointer +typedef void (APIENTRYP PFNGLENABLECLIENTSTATEPROC)(GLenum array); +GLAPI PFNGLENABLECLIENTSTATEPROC glad_glEnableClientState; +#define glEnableClientState glad_glEnableClientState +typedef void (APIENTRYP PFNGLINDEXPOINTERPROC)(GLenum type, GLsizei stride, const void *pointer); +GLAPI PFNGLINDEXPOINTERPROC glad_glIndexPointer; +#define glIndexPointer glad_glIndexPointer +typedef void (APIENTRYP PFNGLINTERLEAVEDARRAYSPROC)(GLenum format, GLsizei stride, const void *pointer); +GLAPI PFNGLINTERLEAVEDARRAYSPROC glad_glInterleavedArrays; +#define glInterleavedArrays glad_glInterleavedArrays +typedef void (APIENTRYP PFNGLNORMALPOINTERPROC)(GLenum type, GLsizei stride, const void *pointer); +GLAPI PFNGLNORMALPOINTERPROC glad_glNormalPointer; +#define glNormalPointer glad_glNormalPointer +typedef void (APIENTRYP PFNGLTEXCOORDPOINTERPROC)(GLint size, GLenum type, GLsizei stride, const void *pointer); +GLAPI PFNGLTEXCOORDPOINTERPROC glad_glTexCoordPointer; +#define glTexCoordPointer glad_glTexCoordPointer +typedef void (APIENTRYP PFNGLVERTEXPOINTERPROC)(GLint size, GLenum type, GLsizei stride, const void *pointer); +GLAPI PFNGLVERTEXPOINTERPROC glad_glVertexPointer; +#define glVertexPointer glad_glVertexPointer +typedef GLboolean (APIENTRYP PFNGLARETEXTURESRESIDENTPROC)(GLsizei n, const GLuint *textures, GLboolean *residences); +GLAPI PFNGLARETEXTURESRESIDENTPROC glad_glAreTexturesResident; +#define glAreTexturesResident glad_glAreTexturesResident +typedef void (APIENTRYP PFNGLPRIORITIZETEXTURESPROC)(GLsizei n, const GLuint *textures, const GLfloat *priorities); +GLAPI PFNGLPRIORITIZETEXTURESPROC glad_glPrioritizeTextures; +#define glPrioritizeTextures glad_glPrioritizeTextures +typedef void (APIENTRYP PFNGLINDEXUBPROC)(GLubyte c); +GLAPI PFNGLINDEXUBPROC glad_glIndexub; +#define glIndexub glad_glIndexub +typedef void (APIENTRYP PFNGLINDEXUBVPROC)(const GLubyte *c); +GLAPI PFNGLINDEXUBVPROC glad_glIndexubv; +#define glIndexubv glad_glIndexubv +typedef void (APIENTRYP PFNGLPOPCLIENTATTRIBPROC)(void); +GLAPI PFNGLPOPCLIENTATTRIBPROC glad_glPopClientAttrib; +#define glPopClientAttrib glad_glPopClientAttrib +typedef void (APIENTRYP PFNGLPUSHCLIENTATTRIBPROC)(GLbitfield mask); +GLAPI PFNGLPUSHCLIENTATTRIBPROC glad_glPushClientAttrib; +#define glPushClientAttrib glad_glPushClientAttrib +#endif +#ifndef GL_VERSION_1_2 +#define GL_VERSION_1_2 1 +GLAPI int GLAD_GL_VERSION_1_2; +typedef void (APIENTRYP PFNGLDRAWRANGEELEMENTSPROC)(GLenum mode, GLuint start, GLuint end, GLsizei count, GLenum type, const void *indices); +GLAPI PFNGLDRAWRANGEELEMENTSPROC glad_glDrawRangeElements; +#define glDrawRangeElements glad_glDrawRangeElements +typedef void (APIENTRYP PFNGLTEXIMAGE3DPROC)(GLenum target, GLint level, GLint internalformat, GLsizei width, GLsizei height, GLsizei depth, GLint border, GLenum format, GLenum type, const void *pixels); +GLAPI PFNGLTEXIMAGE3DPROC glad_glTexImage3D; +#define glTexImage3D glad_glTexImage3D +typedef void (APIENTRYP PFNGLTEXSUBIMAGE3DPROC)(GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLsizei width, GLsizei height, GLsizei depth, GLenum format, GLenum type, const void *pixels); +GLAPI PFNGLTEXSUBIMAGE3DPROC glad_glTexSubImage3D; +#define glTexSubImage3D glad_glTexSubImage3D +typedef void (APIENTRYP PFNGLCOPYTEXSUBIMAGE3DPROC)(GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLint x, GLint y, GLsizei width, GLsizei height); +GLAPI PFNGLCOPYTEXSUBIMAGE3DPROC glad_glCopyTexSubImage3D; +#define glCopyTexSubImage3D glad_glCopyTexSubImage3D +#endif +#ifndef GL_VERSION_1_3 +#define GL_VERSION_1_3 1 +GLAPI int GLAD_GL_VERSION_1_3; +typedef void (APIENTRYP PFNGLACTIVETEXTUREPROC)(GLenum texture); +GLAPI PFNGLACTIVETEXTUREPROC glad_glActiveTexture; +#define glActiveTexture glad_glActiveTexture +typedef void (APIENTRYP PFNGLSAMPLECOVERAGEPROC)(GLfloat value, GLboolean invert); +GLAPI PFNGLSAMPLECOVERAGEPROC glad_glSampleCoverage; +#define glSampleCoverage glad_glSampleCoverage +typedef void (APIENTRYP PFNGLCOMPRESSEDTEXIMAGE3DPROC)(GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLsizei depth, GLint border, GLsizei imageSize, const void *data); +GLAPI PFNGLCOMPRESSEDTEXIMAGE3DPROC glad_glCompressedTexImage3D; +#define glCompressedTexImage3D glad_glCompressedTexImage3D +typedef void (APIENTRYP PFNGLCOMPRESSEDTEXIMAGE2DPROC)(GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLint border, GLsizei imageSize, const void *data); +GLAPI PFNGLCOMPRESSEDTEXIMAGE2DPROC glad_glCompressedTexImage2D; +#define glCompressedTexImage2D glad_glCompressedTexImage2D +typedef void (APIENTRYP PFNGLCOMPRESSEDTEXIMAGE1DPROC)(GLenum target, GLint level, GLenum internalformat, GLsizei width, GLint border, GLsizei imageSize, const void *data); +GLAPI PFNGLCOMPRESSEDTEXIMAGE1DPROC glad_glCompressedTexImage1D; +#define glCompressedTexImage1D glad_glCompressedTexImage1D +typedef void (APIENTRYP PFNGLCOMPRESSEDTEXSUBIMAGE3DPROC)(GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLsizei width, GLsizei height, GLsizei depth, GLenum format, GLsizei imageSize, const void *data); +GLAPI PFNGLCOMPRESSEDTEXSUBIMAGE3DPROC glad_glCompressedTexSubImage3D; +#define glCompressedTexSubImage3D glad_glCompressedTexSubImage3D +typedef void (APIENTRYP PFNGLCOMPRESSEDTEXSUBIMAGE2DPROC)(GLenum target, GLint level, GLint xoffset, GLint yoffset, GLsizei width, GLsizei height, GLenum format, GLsizei imageSize, const void *data); +GLAPI PFNGLCOMPRESSEDTEXSUBIMAGE2DPROC glad_glCompressedTexSubImage2D; +#define glCompressedTexSubImage2D glad_glCompressedTexSubImage2D +typedef void (APIENTRYP PFNGLCOMPRESSEDTEXSUBIMAGE1DPROC)(GLenum target, GLint level, GLint xoffset, GLsizei width, GLenum format, GLsizei imageSize, const void *data); +GLAPI PFNGLCOMPRESSEDTEXSUBIMAGE1DPROC glad_glCompressedTexSubImage1D; +#define glCompressedTexSubImage1D glad_glCompressedTexSubImage1D +typedef void (APIENTRYP PFNGLGETCOMPRESSEDTEXIMAGEPROC)(GLenum target, GLint level, void *img); +GLAPI PFNGLGETCOMPRESSEDTEXIMAGEPROC glad_glGetCompressedTexImage; +#define glGetCompressedTexImage glad_glGetCompressedTexImage +typedef void (APIENTRYP PFNGLCLIENTACTIVETEXTUREPROC)(GLenum texture); +GLAPI PFNGLCLIENTACTIVETEXTUREPROC glad_glClientActiveTexture; +#define glClientActiveTexture glad_glClientActiveTexture +typedef void (APIENTRYP PFNGLMULTITEXCOORD1DPROC)(GLenum target, GLdouble s); +GLAPI PFNGLMULTITEXCOORD1DPROC glad_glMultiTexCoord1d; +#define glMultiTexCoord1d glad_glMultiTexCoord1d +typedef void (APIENTRYP PFNGLMULTITEXCOORD1DVPROC)(GLenum target, const GLdouble *v); +GLAPI PFNGLMULTITEXCOORD1DVPROC glad_glMultiTexCoord1dv; +#define glMultiTexCoord1dv glad_glMultiTexCoord1dv +typedef void (APIENTRYP PFNGLMULTITEXCOORD1FPROC)(GLenum target, GLfloat s); +GLAPI PFNGLMULTITEXCOORD1FPROC glad_glMultiTexCoord1f; +#define glMultiTexCoord1f glad_glMultiTexCoord1f +typedef void (APIENTRYP PFNGLMULTITEXCOORD1FVPROC)(GLenum target, const GLfloat *v); +GLAPI PFNGLMULTITEXCOORD1FVPROC glad_glMultiTexCoord1fv; +#define glMultiTexCoord1fv glad_glMultiTexCoord1fv +typedef void (APIENTRYP PFNGLMULTITEXCOORD1IPROC)(GLenum target, GLint s); +GLAPI PFNGLMULTITEXCOORD1IPROC glad_glMultiTexCoord1i; +#define glMultiTexCoord1i glad_glMultiTexCoord1i +typedef void (APIENTRYP PFNGLMULTITEXCOORD1IVPROC)(GLenum target, const GLint *v); +GLAPI PFNGLMULTITEXCOORD1IVPROC glad_glMultiTexCoord1iv; +#define glMultiTexCoord1iv glad_glMultiTexCoord1iv +typedef void (APIENTRYP PFNGLMULTITEXCOORD1SPROC)(GLenum target, GLshort s); +GLAPI PFNGLMULTITEXCOORD1SPROC glad_glMultiTexCoord1s; +#define glMultiTexCoord1s glad_glMultiTexCoord1s +typedef void (APIENTRYP PFNGLMULTITEXCOORD1SVPROC)(GLenum target, const GLshort *v); +GLAPI PFNGLMULTITEXCOORD1SVPROC glad_glMultiTexCoord1sv; +#define glMultiTexCoord1sv glad_glMultiTexCoord1sv +typedef void (APIENTRYP PFNGLMULTITEXCOORD2DPROC)(GLenum target, GLdouble s, GLdouble t); +GLAPI PFNGLMULTITEXCOORD2DPROC glad_glMultiTexCoord2d; +#define glMultiTexCoord2d glad_glMultiTexCoord2d +typedef void (APIENTRYP PFNGLMULTITEXCOORD2DVPROC)(GLenum target, const GLdouble *v); +GLAPI PFNGLMULTITEXCOORD2DVPROC glad_glMultiTexCoord2dv; +#define glMultiTexCoord2dv glad_glMultiTexCoord2dv +typedef void (APIENTRYP PFNGLMULTITEXCOORD2FPROC)(GLenum target, GLfloat s, GLfloat t); +GLAPI PFNGLMULTITEXCOORD2FPROC glad_glMultiTexCoord2f; +#define glMultiTexCoord2f glad_glMultiTexCoord2f +typedef void (APIENTRYP PFNGLMULTITEXCOORD2FVPROC)(GLenum target, const GLfloat *v); +GLAPI PFNGLMULTITEXCOORD2FVPROC glad_glMultiTexCoord2fv; +#define glMultiTexCoord2fv glad_glMultiTexCoord2fv +typedef void (APIENTRYP PFNGLMULTITEXCOORD2IPROC)(GLenum target, GLint s, GLint t); +GLAPI PFNGLMULTITEXCOORD2IPROC glad_glMultiTexCoord2i; +#define glMultiTexCoord2i glad_glMultiTexCoord2i +typedef void (APIENTRYP PFNGLMULTITEXCOORD2IVPROC)(GLenum target, const GLint *v); +GLAPI PFNGLMULTITEXCOORD2IVPROC glad_glMultiTexCoord2iv; +#define glMultiTexCoord2iv glad_glMultiTexCoord2iv +typedef void (APIENTRYP PFNGLMULTITEXCOORD2SPROC)(GLenum target, GLshort s, GLshort t); +GLAPI PFNGLMULTITEXCOORD2SPROC glad_glMultiTexCoord2s; +#define glMultiTexCoord2s glad_glMultiTexCoord2s +typedef void (APIENTRYP PFNGLMULTITEXCOORD2SVPROC)(GLenum target, const GLshort *v); +GLAPI PFNGLMULTITEXCOORD2SVPROC glad_glMultiTexCoord2sv; +#define glMultiTexCoord2sv glad_glMultiTexCoord2sv +typedef void (APIENTRYP PFNGLMULTITEXCOORD3DPROC)(GLenum target, GLdouble s, GLdouble t, GLdouble r); +GLAPI PFNGLMULTITEXCOORD3DPROC glad_glMultiTexCoord3d; +#define glMultiTexCoord3d glad_glMultiTexCoord3d +typedef void (APIENTRYP PFNGLMULTITEXCOORD3DVPROC)(GLenum target, const GLdouble *v); +GLAPI PFNGLMULTITEXCOORD3DVPROC glad_glMultiTexCoord3dv; +#define glMultiTexCoord3dv glad_glMultiTexCoord3dv +typedef void (APIENTRYP PFNGLMULTITEXCOORD3FPROC)(GLenum target, GLfloat s, GLfloat t, GLfloat r); +GLAPI PFNGLMULTITEXCOORD3FPROC glad_glMultiTexCoord3f; +#define glMultiTexCoord3f glad_glMultiTexCoord3f +typedef void (APIENTRYP PFNGLMULTITEXCOORD3FVPROC)(GLenum target, const GLfloat *v); +GLAPI PFNGLMULTITEXCOORD3FVPROC glad_glMultiTexCoord3fv; +#define glMultiTexCoord3fv glad_glMultiTexCoord3fv +typedef void (APIENTRYP PFNGLMULTITEXCOORD3IPROC)(GLenum target, GLint s, GLint t, GLint r); +GLAPI PFNGLMULTITEXCOORD3IPROC glad_glMultiTexCoord3i; +#define glMultiTexCoord3i glad_glMultiTexCoord3i +typedef void (APIENTRYP PFNGLMULTITEXCOORD3IVPROC)(GLenum target, const GLint *v); +GLAPI PFNGLMULTITEXCOORD3IVPROC glad_glMultiTexCoord3iv; +#define glMultiTexCoord3iv glad_glMultiTexCoord3iv +typedef void (APIENTRYP PFNGLMULTITEXCOORD3SPROC)(GLenum target, GLshort s, GLshort t, GLshort r); +GLAPI PFNGLMULTITEXCOORD3SPROC glad_glMultiTexCoord3s; +#define glMultiTexCoord3s glad_glMultiTexCoord3s +typedef void (APIENTRYP PFNGLMULTITEXCOORD3SVPROC)(GLenum target, const GLshort *v); +GLAPI PFNGLMULTITEXCOORD3SVPROC glad_glMultiTexCoord3sv; +#define glMultiTexCoord3sv glad_glMultiTexCoord3sv +typedef void (APIENTRYP PFNGLMULTITEXCOORD4DPROC)(GLenum target, GLdouble s, GLdouble t, GLdouble r, GLdouble q); +GLAPI PFNGLMULTITEXCOORD4DPROC glad_glMultiTexCoord4d; +#define glMultiTexCoord4d glad_glMultiTexCoord4d +typedef void (APIENTRYP PFNGLMULTITEXCOORD4DVPROC)(GLenum target, const GLdouble *v); +GLAPI PFNGLMULTITEXCOORD4DVPROC glad_glMultiTexCoord4dv; +#define glMultiTexCoord4dv glad_glMultiTexCoord4dv +typedef void (APIENTRYP PFNGLMULTITEXCOORD4FPROC)(GLenum target, GLfloat s, GLfloat t, GLfloat r, GLfloat q); +GLAPI PFNGLMULTITEXCOORD4FPROC glad_glMultiTexCoord4f; +#define glMultiTexCoord4f glad_glMultiTexCoord4f +typedef void (APIENTRYP PFNGLMULTITEXCOORD4FVPROC)(GLenum target, const GLfloat *v); +GLAPI PFNGLMULTITEXCOORD4FVPROC glad_glMultiTexCoord4fv; +#define glMultiTexCoord4fv glad_glMultiTexCoord4fv +typedef void (APIENTRYP PFNGLMULTITEXCOORD4IPROC)(GLenum target, GLint s, GLint t, GLint r, GLint q); +GLAPI PFNGLMULTITEXCOORD4IPROC glad_glMultiTexCoord4i; +#define glMultiTexCoord4i glad_glMultiTexCoord4i +typedef void (APIENTRYP PFNGLMULTITEXCOORD4IVPROC)(GLenum target, const GLint *v); +GLAPI PFNGLMULTITEXCOORD4IVPROC glad_glMultiTexCoord4iv; +#define glMultiTexCoord4iv glad_glMultiTexCoord4iv +typedef void (APIENTRYP PFNGLMULTITEXCOORD4SPROC)(GLenum target, GLshort s, GLshort t, GLshort r, GLshort q); +GLAPI PFNGLMULTITEXCOORD4SPROC glad_glMultiTexCoord4s; +#define glMultiTexCoord4s glad_glMultiTexCoord4s +typedef void (APIENTRYP PFNGLMULTITEXCOORD4SVPROC)(GLenum target, const GLshort *v); +GLAPI PFNGLMULTITEXCOORD4SVPROC glad_glMultiTexCoord4sv; +#define glMultiTexCoord4sv glad_glMultiTexCoord4sv +typedef void (APIENTRYP PFNGLLOADTRANSPOSEMATRIXFPROC)(const GLfloat *m); +GLAPI PFNGLLOADTRANSPOSEMATRIXFPROC glad_glLoadTransposeMatrixf; +#define glLoadTransposeMatrixf glad_glLoadTransposeMatrixf +typedef void (APIENTRYP PFNGLLOADTRANSPOSEMATRIXDPROC)(const GLdouble *m); +GLAPI PFNGLLOADTRANSPOSEMATRIXDPROC glad_glLoadTransposeMatrixd; +#define glLoadTransposeMatrixd glad_glLoadTransposeMatrixd +typedef void (APIENTRYP PFNGLMULTTRANSPOSEMATRIXFPROC)(const GLfloat *m); +GLAPI PFNGLMULTTRANSPOSEMATRIXFPROC glad_glMultTransposeMatrixf; +#define glMultTransposeMatrixf glad_glMultTransposeMatrixf +typedef void (APIENTRYP PFNGLMULTTRANSPOSEMATRIXDPROC)(const GLdouble *m); +GLAPI PFNGLMULTTRANSPOSEMATRIXDPROC glad_glMultTransposeMatrixd; +#define glMultTransposeMatrixd glad_glMultTransposeMatrixd +#endif +#ifndef GL_VERSION_1_4 +#define GL_VERSION_1_4 1 +GLAPI int GLAD_GL_VERSION_1_4; +typedef void (APIENTRYP PFNGLBLENDFUNCSEPARATEPROC)(GLenum sfactorRGB, GLenum dfactorRGB, GLenum sfactorAlpha, GLenum dfactorAlpha); +GLAPI PFNGLBLENDFUNCSEPARATEPROC glad_glBlendFuncSeparate; +#define glBlendFuncSeparate glad_glBlendFuncSeparate +typedef void (APIENTRYP PFNGLMULTIDRAWARRAYSPROC)(GLenum mode, const GLint *first, const GLsizei *count, GLsizei drawcount); +GLAPI PFNGLMULTIDRAWARRAYSPROC glad_glMultiDrawArrays; +#define glMultiDrawArrays glad_glMultiDrawArrays +typedef void (APIENTRYP PFNGLMULTIDRAWELEMENTSPROC)(GLenum mode, const GLsizei *count, GLenum type, const void *const*indices, GLsizei drawcount); +GLAPI PFNGLMULTIDRAWELEMENTSPROC glad_glMultiDrawElements; +#define glMultiDrawElements glad_glMultiDrawElements +typedef void (APIENTRYP PFNGLPOINTPARAMETERFPROC)(GLenum pname, GLfloat param); +GLAPI PFNGLPOINTPARAMETERFPROC glad_glPointParameterf; +#define glPointParameterf glad_glPointParameterf +typedef void (APIENTRYP PFNGLPOINTPARAMETERFVPROC)(GLenum pname, const GLfloat *params); +GLAPI PFNGLPOINTPARAMETERFVPROC glad_glPointParameterfv; +#define glPointParameterfv glad_glPointParameterfv +typedef void (APIENTRYP PFNGLPOINTPARAMETERIPROC)(GLenum pname, GLint param); +GLAPI PFNGLPOINTPARAMETERIPROC glad_glPointParameteri; +#define glPointParameteri glad_glPointParameteri +typedef void (APIENTRYP PFNGLPOINTPARAMETERIVPROC)(GLenum pname, const GLint *params); +GLAPI PFNGLPOINTPARAMETERIVPROC glad_glPointParameteriv; +#define glPointParameteriv glad_glPointParameteriv +typedef void (APIENTRYP PFNGLFOGCOORDFPROC)(GLfloat coord); +GLAPI PFNGLFOGCOORDFPROC glad_glFogCoordf; +#define glFogCoordf glad_glFogCoordf +typedef void (APIENTRYP PFNGLFOGCOORDFVPROC)(const GLfloat *coord); +GLAPI PFNGLFOGCOORDFVPROC glad_glFogCoordfv; +#define glFogCoordfv glad_glFogCoordfv +typedef void (APIENTRYP PFNGLFOGCOORDDPROC)(GLdouble coord); +GLAPI PFNGLFOGCOORDDPROC glad_glFogCoordd; +#define glFogCoordd glad_glFogCoordd +typedef void (APIENTRYP PFNGLFOGCOORDDVPROC)(const GLdouble *coord); +GLAPI PFNGLFOGCOORDDVPROC glad_glFogCoorddv; +#define glFogCoorddv glad_glFogCoorddv +typedef void (APIENTRYP PFNGLFOGCOORDPOINTERPROC)(GLenum type, GLsizei stride, const void *pointer); +GLAPI PFNGLFOGCOORDPOINTERPROC glad_glFogCoordPointer; +#define glFogCoordPointer glad_glFogCoordPointer +typedef void (APIENTRYP PFNGLSECONDARYCOLOR3BPROC)(GLbyte red, GLbyte green, GLbyte blue); +GLAPI PFNGLSECONDARYCOLOR3BPROC glad_glSecondaryColor3b; +#define glSecondaryColor3b glad_glSecondaryColor3b +typedef void (APIENTRYP PFNGLSECONDARYCOLOR3BVPROC)(const GLbyte *v); +GLAPI PFNGLSECONDARYCOLOR3BVPROC glad_glSecondaryColor3bv; +#define glSecondaryColor3bv glad_glSecondaryColor3bv +typedef void (APIENTRYP PFNGLSECONDARYCOLOR3DPROC)(GLdouble red, GLdouble green, GLdouble blue); +GLAPI PFNGLSECONDARYCOLOR3DPROC glad_glSecondaryColor3d; +#define glSecondaryColor3d glad_glSecondaryColor3d +typedef void (APIENTRYP PFNGLSECONDARYCOLOR3DVPROC)(const GLdouble *v); +GLAPI PFNGLSECONDARYCOLOR3DVPROC glad_glSecondaryColor3dv; +#define glSecondaryColor3dv glad_glSecondaryColor3dv +typedef void (APIENTRYP PFNGLSECONDARYCOLOR3FPROC)(GLfloat red, GLfloat green, GLfloat blue); +GLAPI PFNGLSECONDARYCOLOR3FPROC glad_glSecondaryColor3f; +#define glSecondaryColor3f glad_glSecondaryColor3f +typedef void (APIENTRYP PFNGLSECONDARYCOLOR3FVPROC)(const GLfloat *v); +GLAPI PFNGLSECONDARYCOLOR3FVPROC glad_glSecondaryColor3fv; +#define glSecondaryColor3fv glad_glSecondaryColor3fv +typedef void (APIENTRYP PFNGLSECONDARYCOLOR3IPROC)(GLint red, GLint green, GLint blue); +GLAPI PFNGLSECONDARYCOLOR3IPROC glad_glSecondaryColor3i; +#define glSecondaryColor3i glad_glSecondaryColor3i +typedef void (APIENTRYP PFNGLSECONDARYCOLOR3IVPROC)(const GLint *v); +GLAPI PFNGLSECONDARYCOLOR3IVPROC glad_glSecondaryColor3iv; +#define glSecondaryColor3iv glad_glSecondaryColor3iv +typedef void (APIENTRYP PFNGLSECONDARYCOLOR3SPROC)(GLshort red, GLshort green, GLshort blue); +GLAPI PFNGLSECONDARYCOLOR3SPROC glad_glSecondaryColor3s; +#define glSecondaryColor3s glad_glSecondaryColor3s +typedef void (APIENTRYP PFNGLSECONDARYCOLOR3SVPROC)(const GLshort *v); +GLAPI PFNGLSECONDARYCOLOR3SVPROC glad_glSecondaryColor3sv; +#define glSecondaryColor3sv glad_glSecondaryColor3sv +typedef void (APIENTRYP PFNGLSECONDARYCOLOR3UBPROC)(GLubyte red, GLubyte green, GLubyte blue); +GLAPI PFNGLSECONDARYCOLOR3UBPROC glad_glSecondaryColor3ub; +#define glSecondaryColor3ub glad_glSecondaryColor3ub +typedef void (APIENTRYP PFNGLSECONDARYCOLOR3UBVPROC)(const GLubyte *v); +GLAPI PFNGLSECONDARYCOLOR3UBVPROC glad_glSecondaryColor3ubv; +#define glSecondaryColor3ubv glad_glSecondaryColor3ubv +typedef void (APIENTRYP PFNGLSECONDARYCOLOR3UIPROC)(GLuint red, GLuint green, GLuint blue); +GLAPI PFNGLSECONDARYCOLOR3UIPROC glad_glSecondaryColor3ui; +#define glSecondaryColor3ui glad_glSecondaryColor3ui +typedef void (APIENTRYP PFNGLSECONDARYCOLOR3UIVPROC)(const GLuint *v); +GLAPI PFNGLSECONDARYCOLOR3UIVPROC glad_glSecondaryColor3uiv; +#define glSecondaryColor3uiv glad_glSecondaryColor3uiv +typedef void (APIENTRYP PFNGLSECONDARYCOLOR3USPROC)(GLushort red, GLushort green, GLushort blue); +GLAPI PFNGLSECONDARYCOLOR3USPROC glad_glSecondaryColor3us; +#define glSecondaryColor3us glad_glSecondaryColor3us +typedef void (APIENTRYP PFNGLSECONDARYCOLOR3USVPROC)(const GLushort *v); +GLAPI PFNGLSECONDARYCOLOR3USVPROC glad_glSecondaryColor3usv; +#define glSecondaryColor3usv glad_glSecondaryColor3usv +typedef void (APIENTRYP PFNGLSECONDARYCOLORPOINTERPROC)(GLint size, GLenum type, GLsizei stride, const void *pointer); +GLAPI PFNGLSECONDARYCOLORPOINTERPROC glad_glSecondaryColorPointer; +#define glSecondaryColorPointer glad_glSecondaryColorPointer +typedef void (APIENTRYP PFNGLWINDOWPOS2DPROC)(GLdouble x, GLdouble y); +GLAPI PFNGLWINDOWPOS2DPROC glad_glWindowPos2d; +#define glWindowPos2d glad_glWindowPos2d +typedef void (APIENTRYP PFNGLWINDOWPOS2DVPROC)(const GLdouble *v); +GLAPI PFNGLWINDOWPOS2DVPROC glad_glWindowPos2dv; +#define glWindowPos2dv glad_glWindowPos2dv +typedef void (APIENTRYP PFNGLWINDOWPOS2FPROC)(GLfloat x, GLfloat y); +GLAPI PFNGLWINDOWPOS2FPROC glad_glWindowPos2f; +#define glWindowPos2f glad_glWindowPos2f +typedef void (APIENTRYP PFNGLWINDOWPOS2FVPROC)(const GLfloat *v); +GLAPI PFNGLWINDOWPOS2FVPROC glad_glWindowPos2fv; +#define glWindowPos2fv glad_glWindowPos2fv +typedef void (APIENTRYP PFNGLWINDOWPOS2IPROC)(GLint x, GLint y); +GLAPI PFNGLWINDOWPOS2IPROC glad_glWindowPos2i; +#define glWindowPos2i glad_glWindowPos2i +typedef void (APIENTRYP PFNGLWINDOWPOS2IVPROC)(const GLint *v); +GLAPI PFNGLWINDOWPOS2IVPROC glad_glWindowPos2iv; +#define glWindowPos2iv glad_glWindowPos2iv +typedef void (APIENTRYP PFNGLWINDOWPOS2SPROC)(GLshort x, GLshort y); +GLAPI PFNGLWINDOWPOS2SPROC glad_glWindowPos2s; +#define glWindowPos2s glad_glWindowPos2s +typedef void (APIENTRYP PFNGLWINDOWPOS2SVPROC)(const GLshort *v); +GLAPI PFNGLWINDOWPOS2SVPROC glad_glWindowPos2sv; +#define glWindowPos2sv glad_glWindowPos2sv +typedef void (APIENTRYP PFNGLWINDOWPOS3DPROC)(GLdouble x, GLdouble y, GLdouble z); +GLAPI PFNGLWINDOWPOS3DPROC glad_glWindowPos3d; +#define glWindowPos3d glad_glWindowPos3d +typedef void (APIENTRYP PFNGLWINDOWPOS3DVPROC)(const GLdouble *v); +GLAPI PFNGLWINDOWPOS3DVPROC glad_glWindowPos3dv; +#define glWindowPos3dv glad_glWindowPos3dv +typedef void (APIENTRYP PFNGLWINDOWPOS3FPROC)(GLfloat x, GLfloat y, GLfloat z); +GLAPI PFNGLWINDOWPOS3FPROC glad_glWindowPos3f; +#define glWindowPos3f glad_glWindowPos3f +typedef void (APIENTRYP PFNGLWINDOWPOS3FVPROC)(const GLfloat *v); +GLAPI PFNGLWINDOWPOS3FVPROC glad_glWindowPos3fv; +#define glWindowPos3fv glad_glWindowPos3fv +typedef void (APIENTRYP PFNGLWINDOWPOS3IPROC)(GLint x, GLint y, GLint z); +GLAPI PFNGLWINDOWPOS3IPROC glad_glWindowPos3i; +#define glWindowPos3i glad_glWindowPos3i +typedef void (APIENTRYP PFNGLWINDOWPOS3IVPROC)(const GLint *v); +GLAPI PFNGLWINDOWPOS3IVPROC glad_glWindowPos3iv; +#define glWindowPos3iv glad_glWindowPos3iv +typedef void (APIENTRYP PFNGLWINDOWPOS3SPROC)(GLshort x, GLshort y, GLshort z); +GLAPI PFNGLWINDOWPOS3SPROC glad_glWindowPos3s; +#define glWindowPos3s glad_glWindowPos3s +typedef void (APIENTRYP PFNGLWINDOWPOS3SVPROC)(const GLshort *v); +GLAPI PFNGLWINDOWPOS3SVPROC glad_glWindowPos3sv; +#define glWindowPos3sv glad_glWindowPos3sv +typedef void (APIENTRYP PFNGLBLENDCOLORPROC)(GLfloat red, GLfloat green, GLfloat blue, GLfloat alpha); +GLAPI PFNGLBLENDCOLORPROC glad_glBlendColor; +#define glBlendColor glad_glBlendColor +typedef void (APIENTRYP PFNGLBLENDEQUATIONPROC)(GLenum mode); +GLAPI PFNGLBLENDEQUATIONPROC glad_glBlendEquation; +#define glBlendEquation glad_glBlendEquation +#endif +#ifndef GL_VERSION_1_5 +#define GL_VERSION_1_5 1 +GLAPI int GLAD_GL_VERSION_1_5; +typedef void (APIENTRYP PFNGLGENQUERIESPROC)(GLsizei n, GLuint *ids); +GLAPI PFNGLGENQUERIESPROC glad_glGenQueries; +#define glGenQueries glad_glGenQueries +typedef void (APIENTRYP PFNGLDELETEQUERIESPROC)(GLsizei n, const GLuint *ids); +GLAPI PFNGLDELETEQUERIESPROC glad_glDeleteQueries; +#define glDeleteQueries glad_glDeleteQueries +typedef GLboolean (APIENTRYP PFNGLISQUERYPROC)(GLuint id); +GLAPI PFNGLISQUERYPROC glad_glIsQuery; +#define glIsQuery glad_glIsQuery +typedef void (APIENTRYP PFNGLBEGINQUERYPROC)(GLenum target, GLuint id); +GLAPI PFNGLBEGINQUERYPROC glad_glBeginQuery; +#define glBeginQuery glad_glBeginQuery +typedef void (APIENTRYP PFNGLENDQUERYPROC)(GLenum target); +GLAPI PFNGLENDQUERYPROC glad_glEndQuery; +#define glEndQuery glad_glEndQuery +typedef void (APIENTRYP PFNGLGETQUERYIVPROC)(GLenum target, GLenum pname, GLint *params); +GLAPI PFNGLGETQUERYIVPROC glad_glGetQueryiv; +#define glGetQueryiv glad_glGetQueryiv +typedef void (APIENTRYP PFNGLGETQUERYOBJECTIVPROC)(GLuint id, GLenum pname, GLint *params); +GLAPI PFNGLGETQUERYOBJECTIVPROC glad_glGetQueryObjectiv; +#define glGetQueryObjectiv glad_glGetQueryObjectiv +typedef void (APIENTRYP PFNGLGETQUERYOBJECTUIVPROC)(GLuint id, GLenum pname, GLuint *params); +GLAPI PFNGLGETQUERYOBJECTUIVPROC glad_glGetQueryObjectuiv; +#define glGetQueryObjectuiv glad_glGetQueryObjectuiv +typedef void (APIENTRYP PFNGLBINDBUFFERPROC)(GLenum target, GLuint buffer); +GLAPI PFNGLBINDBUFFERPROC glad_glBindBuffer; +#define glBindBuffer glad_glBindBuffer +typedef void (APIENTRYP PFNGLDELETEBUFFERSPROC)(GLsizei n, const GLuint *buffers); +GLAPI PFNGLDELETEBUFFERSPROC glad_glDeleteBuffers; +#define glDeleteBuffers glad_glDeleteBuffers +typedef void (APIENTRYP PFNGLGENBUFFERSPROC)(GLsizei n, GLuint *buffers); +GLAPI PFNGLGENBUFFERSPROC glad_glGenBuffers; +#define glGenBuffers glad_glGenBuffers +typedef GLboolean (APIENTRYP PFNGLISBUFFERPROC)(GLuint buffer); +GLAPI PFNGLISBUFFERPROC glad_glIsBuffer; +#define glIsBuffer glad_glIsBuffer +typedef void (APIENTRYP PFNGLBUFFERDATAPROC)(GLenum target, GLsizeiptr size, const void *data, GLenum usage); +GLAPI PFNGLBUFFERDATAPROC glad_glBufferData; +#define glBufferData glad_glBufferData +typedef void (APIENTRYP PFNGLBUFFERSUBDATAPROC)(GLenum target, GLintptr offset, GLsizeiptr size, const void *data); +GLAPI PFNGLBUFFERSUBDATAPROC glad_glBufferSubData; +#define glBufferSubData glad_glBufferSubData +typedef void (APIENTRYP PFNGLGETBUFFERSUBDATAPROC)(GLenum target, GLintptr offset, GLsizeiptr size, void *data); +GLAPI PFNGLGETBUFFERSUBDATAPROC glad_glGetBufferSubData; +#define glGetBufferSubData glad_glGetBufferSubData +typedef void * (APIENTRYP PFNGLMAPBUFFERPROC)(GLenum target, GLenum access); +GLAPI PFNGLMAPBUFFERPROC glad_glMapBuffer; +#define glMapBuffer glad_glMapBuffer +typedef GLboolean (APIENTRYP PFNGLUNMAPBUFFERPROC)(GLenum target); +GLAPI PFNGLUNMAPBUFFERPROC glad_glUnmapBuffer; +#define glUnmapBuffer glad_glUnmapBuffer +typedef void (APIENTRYP PFNGLGETBUFFERPARAMETERIVPROC)(GLenum target, GLenum pname, GLint *params); +GLAPI PFNGLGETBUFFERPARAMETERIVPROC glad_glGetBufferParameteriv; +#define glGetBufferParameteriv glad_glGetBufferParameteriv +typedef void (APIENTRYP PFNGLGETBUFFERPOINTERVPROC)(GLenum target, GLenum pname, void **params); +GLAPI PFNGLGETBUFFERPOINTERVPROC glad_glGetBufferPointerv; +#define glGetBufferPointerv glad_glGetBufferPointerv +#endif +#ifndef GL_VERSION_2_0 +#define GL_VERSION_2_0 1 +GLAPI int GLAD_GL_VERSION_2_0; +typedef void (APIENTRYP PFNGLBLENDEQUATIONSEPARATEPROC)(GLenum modeRGB, GLenum modeAlpha); +GLAPI PFNGLBLENDEQUATIONSEPARATEPROC glad_glBlendEquationSeparate; +#define glBlendEquationSeparate glad_glBlendEquationSeparate +typedef void (APIENTRYP PFNGLDRAWBUFFERSPROC)(GLsizei n, const GLenum *bufs); +GLAPI PFNGLDRAWBUFFERSPROC glad_glDrawBuffers; +#define glDrawBuffers glad_glDrawBuffers +typedef void (APIENTRYP PFNGLSTENCILOPSEPARATEPROC)(GLenum face, GLenum sfail, GLenum dpfail, GLenum dppass); +GLAPI PFNGLSTENCILOPSEPARATEPROC glad_glStencilOpSeparate; +#define glStencilOpSeparate glad_glStencilOpSeparate +typedef void (APIENTRYP PFNGLSTENCILFUNCSEPARATEPROC)(GLenum face, GLenum func, GLint ref, GLuint mask); +GLAPI PFNGLSTENCILFUNCSEPARATEPROC glad_glStencilFuncSeparate; +#define glStencilFuncSeparate glad_glStencilFuncSeparate +typedef void (APIENTRYP PFNGLSTENCILMASKSEPARATEPROC)(GLenum face, GLuint mask); +GLAPI PFNGLSTENCILMASKSEPARATEPROC glad_glStencilMaskSeparate; +#define glStencilMaskSeparate glad_glStencilMaskSeparate +typedef void (APIENTRYP PFNGLATTACHSHADERPROC)(GLuint program, GLuint shader); +GLAPI PFNGLATTACHSHADERPROC glad_glAttachShader; +#define glAttachShader glad_glAttachShader +typedef void (APIENTRYP PFNGLBINDATTRIBLOCATIONPROC)(GLuint program, GLuint index, const GLchar *name); +GLAPI PFNGLBINDATTRIBLOCATIONPROC glad_glBindAttribLocation; +#define glBindAttribLocation glad_glBindAttribLocation +typedef void (APIENTRYP PFNGLCOMPILESHADERPROC)(GLuint shader); +GLAPI PFNGLCOMPILESHADERPROC glad_glCompileShader; +#define glCompileShader glad_glCompileShader +typedef GLuint (APIENTRYP PFNGLCREATEPROGRAMPROC)(void); +GLAPI PFNGLCREATEPROGRAMPROC glad_glCreateProgram; +#define glCreateProgram glad_glCreateProgram +typedef GLuint (APIENTRYP PFNGLCREATESHADERPROC)(GLenum type); +GLAPI PFNGLCREATESHADERPROC glad_glCreateShader; +#define glCreateShader glad_glCreateShader +typedef void (APIENTRYP PFNGLDELETEPROGRAMPROC)(GLuint program); +GLAPI PFNGLDELETEPROGRAMPROC glad_glDeleteProgram; +#define glDeleteProgram glad_glDeleteProgram +typedef void (APIENTRYP PFNGLDELETESHADERPROC)(GLuint shader); +GLAPI PFNGLDELETESHADERPROC glad_glDeleteShader; +#define glDeleteShader glad_glDeleteShader +typedef void (APIENTRYP PFNGLDETACHSHADERPROC)(GLuint program, GLuint shader); +GLAPI PFNGLDETACHSHADERPROC glad_glDetachShader; +#define glDetachShader glad_glDetachShader +typedef void (APIENTRYP PFNGLDISABLEVERTEXATTRIBARRAYPROC)(GLuint index); +GLAPI PFNGLDISABLEVERTEXATTRIBARRAYPROC glad_glDisableVertexAttribArray; +#define glDisableVertexAttribArray glad_glDisableVertexAttribArray +typedef void (APIENTRYP PFNGLENABLEVERTEXATTRIBARRAYPROC)(GLuint index); +GLAPI PFNGLENABLEVERTEXATTRIBARRAYPROC glad_glEnableVertexAttribArray; +#define glEnableVertexAttribArray glad_glEnableVertexAttribArray +typedef void (APIENTRYP PFNGLGETACTIVEATTRIBPROC)(GLuint program, GLuint index, GLsizei bufSize, GLsizei *length, GLint *size, GLenum *type, GLchar *name); +GLAPI PFNGLGETACTIVEATTRIBPROC glad_glGetActiveAttrib; +#define glGetActiveAttrib glad_glGetActiveAttrib +typedef void (APIENTRYP PFNGLGETACTIVEUNIFORMPROC)(GLuint program, GLuint index, GLsizei bufSize, GLsizei *length, GLint *size, GLenum *type, GLchar *name); +GLAPI PFNGLGETACTIVEUNIFORMPROC glad_glGetActiveUniform; +#define glGetActiveUniform glad_glGetActiveUniform +typedef void (APIENTRYP PFNGLGETATTACHEDSHADERSPROC)(GLuint program, GLsizei maxCount, GLsizei *count, GLuint *shaders); +GLAPI PFNGLGETATTACHEDSHADERSPROC glad_glGetAttachedShaders; +#define glGetAttachedShaders glad_glGetAttachedShaders +typedef GLint (APIENTRYP PFNGLGETATTRIBLOCATIONPROC)(GLuint program, const GLchar *name); +GLAPI PFNGLGETATTRIBLOCATIONPROC glad_glGetAttribLocation; +#define glGetAttribLocation glad_glGetAttribLocation +typedef void (APIENTRYP PFNGLGETPROGRAMIVPROC)(GLuint program, GLenum pname, GLint *params); +GLAPI PFNGLGETPROGRAMIVPROC glad_glGetProgramiv; +#define glGetProgramiv glad_glGetProgramiv +typedef void (APIENTRYP PFNGLGETPROGRAMINFOLOGPROC)(GLuint program, GLsizei bufSize, GLsizei *length, GLchar *infoLog); +GLAPI PFNGLGETPROGRAMINFOLOGPROC glad_glGetProgramInfoLog; +#define glGetProgramInfoLog glad_glGetProgramInfoLog +typedef void (APIENTRYP PFNGLGETSHADERIVPROC)(GLuint shader, GLenum pname, GLint *params); +GLAPI PFNGLGETSHADERIVPROC glad_glGetShaderiv; +#define glGetShaderiv glad_glGetShaderiv +typedef void (APIENTRYP PFNGLGETSHADERINFOLOGPROC)(GLuint shader, GLsizei bufSize, GLsizei *length, GLchar *infoLog); +GLAPI PFNGLGETSHADERINFOLOGPROC glad_glGetShaderInfoLog; +#define glGetShaderInfoLog glad_glGetShaderInfoLog +typedef void (APIENTRYP PFNGLGETSHADERSOURCEPROC)(GLuint shader, GLsizei bufSize, GLsizei *length, GLchar *source); +GLAPI PFNGLGETSHADERSOURCEPROC glad_glGetShaderSource; +#define glGetShaderSource glad_glGetShaderSource +typedef GLint (APIENTRYP PFNGLGETUNIFORMLOCATIONPROC)(GLuint program, const GLchar *name); +GLAPI PFNGLGETUNIFORMLOCATIONPROC glad_glGetUniformLocation; +#define glGetUniformLocation glad_glGetUniformLocation +typedef void (APIENTRYP PFNGLGETUNIFORMFVPROC)(GLuint program, GLint location, GLfloat *params); +GLAPI PFNGLGETUNIFORMFVPROC glad_glGetUniformfv; +#define glGetUniformfv glad_glGetUniformfv +typedef void (APIENTRYP PFNGLGETUNIFORMIVPROC)(GLuint program, GLint location, GLint *params); +GLAPI PFNGLGETUNIFORMIVPROC glad_glGetUniformiv; +#define glGetUniformiv glad_glGetUniformiv +typedef void (APIENTRYP PFNGLGETVERTEXATTRIBDVPROC)(GLuint index, GLenum pname, GLdouble *params); +GLAPI PFNGLGETVERTEXATTRIBDVPROC glad_glGetVertexAttribdv; +#define glGetVertexAttribdv glad_glGetVertexAttribdv +typedef void (APIENTRYP PFNGLGETVERTEXATTRIBFVPROC)(GLuint index, GLenum pname, GLfloat *params); +GLAPI PFNGLGETVERTEXATTRIBFVPROC glad_glGetVertexAttribfv; +#define glGetVertexAttribfv glad_glGetVertexAttribfv +typedef void (APIENTRYP PFNGLGETVERTEXATTRIBIVPROC)(GLuint index, GLenum pname, GLint *params); +GLAPI PFNGLGETVERTEXATTRIBIVPROC glad_glGetVertexAttribiv; +#define glGetVertexAttribiv glad_glGetVertexAttribiv +typedef void (APIENTRYP PFNGLGETVERTEXATTRIBPOINTERVPROC)(GLuint index, GLenum pname, void **pointer); +GLAPI PFNGLGETVERTEXATTRIBPOINTERVPROC glad_glGetVertexAttribPointerv; +#define glGetVertexAttribPointerv glad_glGetVertexAttribPointerv +typedef GLboolean (APIENTRYP PFNGLISPROGRAMPROC)(GLuint program); +GLAPI PFNGLISPROGRAMPROC glad_glIsProgram; +#define glIsProgram glad_glIsProgram +typedef GLboolean (APIENTRYP PFNGLISSHADERPROC)(GLuint shader); +GLAPI PFNGLISSHADERPROC glad_glIsShader; +#define glIsShader glad_glIsShader +typedef void (APIENTRYP PFNGLLINKPROGRAMPROC)(GLuint program); +GLAPI PFNGLLINKPROGRAMPROC glad_glLinkProgram; +#define glLinkProgram glad_glLinkProgram +typedef void (APIENTRYP PFNGLSHADERSOURCEPROC)(GLuint shader, GLsizei count, const GLchar *const*string, const GLint *length); +GLAPI PFNGLSHADERSOURCEPROC glad_glShaderSource; +#define glShaderSource glad_glShaderSource +typedef void (APIENTRYP PFNGLUSEPROGRAMPROC)(GLuint program); +GLAPI PFNGLUSEPROGRAMPROC glad_glUseProgram; +#define glUseProgram glad_glUseProgram +typedef void (APIENTRYP PFNGLUNIFORM1FPROC)(GLint location, GLfloat v0); +GLAPI PFNGLUNIFORM1FPROC glad_glUniform1f; +#define glUniform1f glad_glUniform1f +typedef void (APIENTRYP PFNGLUNIFORM2FPROC)(GLint location, GLfloat v0, GLfloat v1); +GLAPI PFNGLUNIFORM2FPROC glad_glUniform2f; +#define glUniform2f glad_glUniform2f +typedef void (APIENTRYP PFNGLUNIFORM3FPROC)(GLint location, GLfloat v0, GLfloat v1, GLfloat v2); +GLAPI PFNGLUNIFORM3FPROC glad_glUniform3f; +#define glUniform3f glad_glUniform3f +typedef void (APIENTRYP PFNGLUNIFORM4FPROC)(GLint location, GLfloat v0, GLfloat v1, GLfloat v2, GLfloat v3); +GLAPI PFNGLUNIFORM4FPROC glad_glUniform4f; +#define glUniform4f glad_glUniform4f +typedef void (APIENTRYP PFNGLUNIFORM1IPROC)(GLint location, GLint v0); +GLAPI PFNGLUNIFORM1IPROC glad_glUniform1i; +#define glUniform1i glad_glUniform1i +typedef void (APIENTRYP PFNGLUNIFORM2IPROC)(GLint location, GLint v0, GLint v1); +GLAPI PFNGLUNIFORM2IPROC glad_glUniform2i; +#define glUniform2i glad_glUniform2i +typedef void (APIENTRYP PFNGLUNIFORM3IPROC)(GLint location, GLint v0, GLint v1, GLint v2); +GLAPI PFNGLUNIFORM3IPROC glad_glUniform3i; +#define glUniform3i glad_glUniform3i +typedef void (APIENTRYP PFNGLUNIFORM4IPROC)(GLint location, GLint v0, GLint v1, GLint v2, GLint v3); +GLAPI PFNGLUNIFORM4IPROC glad_glUniform4i; +#define glUniform4i glad_glUniform4i +typedef void (APIENTRYP PFNGLUNIFORM1FVPROC)(GLint location, GLsizei count, const GLfloat *value); +GLAPI PFNGLUNIFORM1FVPROC glad_glUniform1fv; +#define glUniform1fv glad_glUniform1fv +typedef void (APIENTRYP PFNGLUNIFORM2FVPROC)(GLint location, GLsizei count, const GLfloat *value); +GLAPI PFNGLUNIFORM2FVPROC glad_glUniform2fv; +#define glUniform2fv glad_glUniform2fv +typedef void (APIENTRYP PFNGLUNIFORM3FVPROC)(GLint location, GLsizei count, const GLfloat *value); +GLAPI PFNGLUNIFORM3FVPROC glad_glUniform3fv; +#define glUniform3fv glad_glUniform3fv +typedef void (APIENTRYP PFNGLUNIFORM4FVPROC)(GLint location, GLsizei count, const GLfloat *value); +GLAPI PFNGLUNIFORM4FVPROC glad_glUniform4fv; +#define glUniform4fv glad_glUniform4fv +typedef void (APIENTRYP PFNGLUNIFORM1IVPROC)(GLint location, GLsizei count, const GLint *value); +GLAPI PFNGLUNIFORM1IVPROC glad_glUniform1iv; +#define glUniform1iv glad_glUniform1iv +typedef void (APIENTRYP PFNGLUNIFORM2IVPROC)(GLint location, GLsizei count, const GLint *value); +GLAPI PFNGLUNIFORM2IVPROC glad_glUniform2iv; +#define glUniform2iv glad_glUniform2iv +typedef void (APIENTRYP PFNGLUNIFORM3IVPROC)(GLint location, GLsizei count, const GLint *value); +GLAPI PFNGLUNIFORM3IVPROC glad_glUniform3iv; +#define glUniform3iv glad_glUniform3iv +typedef void (APIENTRYP PFNGLUNIFORM4IVPROC)(GLint location, GLsizei count, const GLint *value); +GLAPI PFNGLUNIFORM4IVPROC glad_glUniform4iv; +#define glUniform4iv glad_glUniform4iv +typedef void (APIENTRYP PFNGLUNIFORMMATRIX2FVPROC)(GLint location, GLsizei count, GLboolean transpose, const GLfloat *value); +GLAPI PFNGLUNIFORMMATRIX2FVPROC glad_glUniformMatrix2fv; +#define glUniformMatrix2fv glad_glUniformMatrix2fv +typedef void (APIENTRYP PFNGLUNIFORMMATRIX3FVPROC)(GLint location, GLsizei count, GLboolean transpose, const GLfloat *value); +GLAPI PFNGLUNIFORMMATRIX3FVPROC glad_glUniformMatrix3fv; +#define glUniformMatrix3fv glad_glUniformMatrix3fv +typedef void (APIENTRYP PFNGLUNIFORMMATRIX4FVPROC)(GLint location, GLsizei count, GLboolean transpose, const GLfloat *value); +GLAPI PFNGLUNIFORMMATRIX4FVPROC glad_glUniformMatrix4fv; +#define glUniformMatrix4fv glad_glUniformMatrix4fv +typedef void (APIENTRYP PFNGLVALIDATEPROGRAMPROC)(GLuint program); +GLAPI PFNGLVALIDATEPROGRAMPROC glad_glValidateProgram; +#define glValidateProgram glad_glValidateProgram +typedef void (APIENTRYP PFNGLVERTEXATTRIB1DPROC)(GLuint index, GLdouble x); +GLAPI PFNGLVERTEXATTRIB1DPROC glad_glVertexAttrib1d; +#define glVertexAttrib1d glad_glVertexAttrib1d +typedef void (APIENTRYP PFNGLVERTEXATTRIB1DVPROC)(GLuint index, const GLdouble *v); +GLAPI PFNGLVERTEXATTRIB1DVPROC glad_glVertexAttrib1dv; +#define glVertexAttrib1dv glad_glVertexAttrib1dv +typedef void (APIENTRYP PFNGLVERTEXATTRIB1FPROC)(GLuint index, GLfloat x); +GLAPI PFNGLVERTEXATTRIB1FPROC glad_glVertexAttrib1f; +#define glVertexAttrib1f glad_glVertexAttrib1f +typedef void (APIENTRYP PFNGLVERTEXATTRIB1FVPROC)(GLuint index, const GLfloat *v); +GLAPI PFNGLVERTEXATTRIB1FVPROC glad_glVertexAttrib1fv; +#define glVertexAttrib1fv glad_glVertexAttrib1fv +typedef void (APIENTRYP PFNGLVERTEXATTRIB1SPROC)(GLuint index, GLshort x); +GLAPI PFNGLVERTEXATTRIB1SPROC glad_glVertexAttrib1s; +#define glVertexAttrib1s glad_glVertexAttrib1s +typedef void (APIENTRYP PFNGLVERTEXATTRIB1SVPROC)(GLuint index, const GLshort *v); +GLAPI PFNGLVERTEXATTRIB1SVPROC glad_glVertexAttrib1sv; +#define glVertexAttrib1sv glad_glVertexAttrib1sv +typedef void (APIENTRYP PFNGLVERTEXATTRIB2DPROC)(GLuint index, GLdouble x, GLdouble y); +GLAPI PFNGLVERTEXATTRIB2DPROC glad_glVertexAttrib2d; +#define glVertexAttrib2d glad_glVertexAttrib2d +typedef void (APIENTRYP PFNGLVERTEXATTRIB2DVPROC)(GLuint index, const GLdouble *v); +GLAPI PFNGLVERTEXATTRIB2DVPROC glad_glVertexAttrib2dv; +#define glVertexAttrib2dv glad_glVertexAttrib2dv +typedef void (APIENTRYP PFNGLVERTEXATTRIB2FPROC)(GLuint index, GLfloat x, GLfloat y); +GLAPI PFNGLVERTEXATTRIB2FPROC glad_glVertexAttrib2f; +#define glVertexAttrib2f glad_glVertexAttrib2f +typedef void (APIENTRYP PFNGLVERTEXATTRIB2FVPROC)(GLuint index, const GLfloat *v); +GLAPI PFNGLVERTEXATTRIB2FVPROC glad_glVertexAttrib2fv; +#define glVertexAttrib2fv glad_glVertexAttrib2fv +typedef void (APIENTRYP PFNGLVERTEXATTRIB2SPROC)(GLuint index, GLshort x, GLshort y); +GLAPI PFNGLVERTEXATTRIB2SPROC glad_glVertexAttrib2s; +#define glVertexAttrib2s glad_glVertexAttrib2s +typedef void (APIENTRYP PFNGLVERTEXATTRIB2SVPROC)(GLuint index, const GLshort *v); +GLAPI PFNGLVERTEXATTRIB2SVPROC glad_glVertexAttrib2sv; +#define glVertexAttrib2sv glad_glVertexAttrib2sv +typedef void (APIENTRYP PFNGLVERTEXATTRIB3DPROC)(GLuint index, GLdouble x, GLdouble y, GLdouble z); +GLAPI PFNGLVERTEXATTRIB3DPROC glad_glVertexAttrib3d; +#define glVertexAttrib3d glad_glVertexAttrib3d +typedef void (APIENTRYP PFNGLVERTEXATTRIB3DVPROC)(GLuint index, const GLdouble *v); +GLAPI PFNGLVERTEXATTRIB3DVPROC glad_glVertexAttrib3dv; +#define glVertexAttrib3dv glad_glVertexAttrib3dv +typedef void (APIENTRYP PFNGLVERTEXATTRIB3FPROC)(GLuint index, GLfloat x, GLfloat y, GLfloat z); +GLAPI PFNGLVERTEXATTRIB3FPROC glad_glVertexAttrib3f; +#define glVertexAttrib3f glad_glVertexAttrib3f +typedef void (APIENTRYP PFNGLVERTEXATTRIB3FVPROC)(GLuint index, const GLfloat *v); +GLAPI PFNGLVERTEXATTRIB3FVPROC glad_glVertexAttrib3fv; +#define glVertexAttrib3fv glad_glVertexAttrib3fv +typedef void (APIENTRYP PFNGLVERTEXATTRIB3SPROC)(GLuint index, GLshort x, GLshort y, GLshort z); +GLAPI PFNGLVERTEXATTRIB3SPROC glad_glVertexAttrib3s; +#define glVertexAttrib3s glad_glVertexAttrib3s +typedef void (APIENTRYP PFNGLVERTEXATTRIB3SVPROC)(GLuint index, const GLshort *v); +GLAPI PFNGLVERTEXATTRIB3SVPROC glad_glVertexAttrib3sv; +#define glVertexAttrib3sv glad_glVertexAttrib3sv +typedef void (APIENTRYP PFNGLVERTEXATTRIB4NBVPROC)(GLuint index, const GLbyte *v); +GLAPI PFNGLVERTEXATTRIB4NBVPROC glad_glVertexAttrib4Nbv; +#define glVertexAttrib4Nbv glad_glVertexAttrib4Nbv +typedef void (APIENTRYP PFNGLVERTEXATTRIB4NIVPROC)(GLuint index, const GLint *v); +GLAPI PFNGLVERTEXATTRIB4NIVPROC glad_glVertexAttrib4Niv; +#define glVertexAttrib4Niv glad_glVertexAttrib4Niv +typedef void (APIENTRYP PFNGLVERTEXATTRIB4NSVPROC)(GLuint index, const GLshort *v); +GLAPI PFNGLVERTEXATTRIB4NSVPROC glad_glVertexAttrib4Nsv; +#define glVertexAttrib4Nsv glad_glVertexAttrib4Nsv +typedef void (APIENTRYP PFNGLVERTEXATTRIB4NUBPROC)(GLuint index, GLubyte x, GLubyte y, GLubyte z, GLubyte w); +GLAPI PFNGLVERTEXATTRIB4NUBPROC glad_glVertexAttrib4Nub; +#define glVertexAttrib4Nub glad_glVertexAttrib4Nub +typedef void (APIENTRYP PFNGLVERTEXATTRIB4NUBVPROC)(GLuint index, const GLubyte *v); +GLAPI PFNGLVERTEXATTRIB4NUBVPROC glad_glVertexAttrib4Nubv; +#define glVertexAttrib4Nubv glad_glVertexAttrib4Nubv +typedef void (APIENTRYP PFNGLVERTEXATTRIB4NUIVPROC)(GLuint index, const GLuint *v); +GLAPI PFNGLVERTEXATTRIB4NUIVPROC glad_glVertexAttrib4Nuiv; +#define glVertexAttrib4Nuiv glad_glVertexAttrib4Nuiv +typedef void (APIENTRYP PFNGLVERTEXATTRIB4NUSVPROC)(GLuint index, const GLushort *v); +GLAPI PFNGLVERTEXATTRIB4NUSVPROC glad_glVertexAttrib4Nusv; +#define glVertexAttrib4Nusv glad_glVertexAttrib4Nusv +typedef void (APIENTRYP PFNGLVERTEXATTRIB4BVPROC)(GLuint index, const GLbyte *v); +GLAPI PFNGLVERTEXATTRIB4BVPROC glad_glVertexAttrib4bv; +#define glVertexAttrib4bv glad_glVertexAttrib4bv +typedef void (APIENTRYP PFNGLVERTEXATTRIB4DPROC)(GLuint index, GLdouble x, GLdouble y, GLdouble z, GLdouble w); +GLAPI PFNGLVERTEXATTRIB4DPROC glad_glVertexAttrib4d; +#define glVertexAttrib4d glad_glVertexAttrib4d +typedef void (APIENTRYP PFNGLVERTEXATTRIB4DVPROC)(GLuint index, const GLdouble *v); +GLAPI PFNGLVERTEXATTRIB4DVPROC glad_glVertexAttrib4dv; +#define glVertexAttrib4dv glad_glVertexAttrib4dv +typedef void (APIENTRYP PFNGLVERTEXATTRIB4FPROC)(GLuint index, GLfloat x, GLfloat y, GLfloat z, GLfloat w); +GLAPI PFNGLVERTEXATTRIB4FPROC glad_glVertexAttrib4f; +#define glVertexAttrib4f glad_glVertexAttrib4f +typedef void (APIENTRYP PFNGLVERTEXATTRIB4FVPROC)(GLuint index, const GLfloat *v); +GLAPI PFNGLVERTEXATTRIB4FVPROC glad_glVertexAttrib4fv; +#define glVertexAttrib4fv glad_glVertexAttrib4fv +typedef void (APIENTRYP PFNGLVERTEXATTRIB4IVPROC)(GLuint index, const GLint *v); +GLAPI PFNGLVERTEXATTRIB4IVPROC glad_glVertexAttrib4iv; +#define glVertexAttrib4iv glad_glVertexAttrib4iv +typedef void (APIENTRYP PFNGLVERTEXATTRIB4SPROC)(GLuint index, GLshort x, GLshort y, GLshort z, GLshort w); +GLAPI PFNGLVERTEXATTRIB4SPROC glad_glVertexAttrib4s; +#define glVertexAttrib4s glad_glVertexAttrib4s +typedef void (APIENTRYP PFNGLVERTEXATTRIB4SVPROC)(GLuint index, const GLshort *v); +GLAPI PFNGLVERTEXATTRIB4SVPROC glad_glVertexAttrib4sv; +#define glVertexAttrib4sv glad_glVertexAttrib4sv +typedef void (APIENTRYP PFNGLVERTEXATTRIB4UBVPROC)(GLuint index, const GLubyte *v); +GLAPI PFNGLVERTEXATTRIB4UBVPROC glad_glVertexAttrib4ubv; +#define glVertexAttrib4ubv glad_glVertexAttrib4ubv +typedef void (APIENTRYP PFNGLVERTEXATTRIB4UIVPROC)(GLuint index, const GLuint *v); +GLAPI PFNGLVERTEXATTRIB4UIVPROC glad_glVertexAttrib4uiv; +#define glVertexAttrib4uiv glad_glVertexAttrib4uiv +typedef void (APIENTRYP PFNGLVERTEXATTRIB4USVPROC)(GLuint index, const GLushort *v); +GLAPI PFNGLVERTEXATTRIB4USVPROC glad_glVertexAttrib4usv; +#define glVertexAttrib4usv glad_glVertexAttrib4usv +typedef void (APIENTRYP PFNGLVERTEXATTRIBPOINTERPROC)(GLuint index, GLint size, GLenum type, GLboolean normalized, GLsizei stride, const void *pointer); +GLAPI PFNGLVERTEXATTRIBPOINTERPROC glad_glVertexAttribPointer; +#define glVertexAttribPointer glad_glVertexAttribPointer +#endif +#ifndef GL_VERSION_2_1 +#define GL_VERSION_2_1 1 +GLAPI int GLAD_GL_VERSION_2_1; +typedef void (APIENTRYP PFNGLUNIFORMMATRIX2X3FVPROC)(GLint location, GLsizei count, GLboolean transpose, const GLfloat *value); +GLAPI PFNGLUNIFORMMATRIX2X3FVPROC glad_glUniformMatrix2x3fv; +#define glUniformMatrix2x3fv glad_glUniformMatrix2x3fv +typedef void (APIENTRYP PFNGLUNIFORMMATRIX3X2FVPROC)(GLint location, GLsizei count, GLboolean transpose, const GLfloat *value); +GLAPI PFNGLUNIFORMMATRIX3X2FVPROC glad_glUniformMatrix3x2fv; +#define glUniformMatrix3x2fv glad_glUniformMatrix3x2fv +typedef void (APIENTRYP PFNGLUNIFORMMATRIX2X4FVPROC)(GLint location, GLsizei count, GLboolean transpose, const GLfloat *value); +GLAPI PFNGLUNIFORMMATRIX2X4FVPROC glad_glUniformMatrix2x4fv; +#define glUniformMatrix2x4fv glad_glUniformMatrix2x4fv +typedef void (APIENTRYP PFNGLUNIFORMMATRIX4X2FVPROC)(GLint location, GLsizei count, GLboolean transpose, const GLfloat *value); +GLAPI PFNGLUNIFORMMATRIX4X2FVPROC glad_glUniformMatrix4x2fv; +#define glUniformMatrix4x2fv glad_glUniformMatrix4x2fv +typedef void (APIENTRYP PFNGLUNIFORMMATRIX3X4FVPROC)(GLint location, GLsizei count, GLboolean transpose, const GLfloat *value); +GLAPI PFNGLUNIFORMMATRIX3X4FVPROC glad_glUniformMatrix3x4fv; +#define glUniformMatrix3x4fv glad_glUniformMatrix3x4fv +typedef void (APIENTRYP PFNGLUNIFORMMATRIX4X3FVPROC)(GLint location, GLsizei count, GLboolean transpose, const GLfloat *value); +GLAPI PFNGLUNIFORMMATRIX4X3FVPROC glad_glUniformMatrix4x3fv; +#define glUniformMatrix4x3fv glad_glUniformMatrix4x3fv +#endif +#ifndef GL_VERSION_3_0 +#define GL_VERSION_3_0 1 +GLAPI int GLAD_GL_VERSION_3_0; +typedef void (APIENTRYP PFNGLCOLORMASKIPROC)(GLuint index, GLboolean r, GLboolean g, GLboolean b, GLboolean a); +GLAPI PFNGLCOLORMASKIPROC glad_glColorMaski; +#define glColorMaski glad_glColorMaski +typedef void (APIENTRYP PFNGLGETBOOLEANI_VPROC)(GLenum target, GLuint index, GLboolean *data); +GLAPI PFNGLGETBOOLEANI_VPROC glad_glGetBooleani_v; +#define glGetBooleani_v glad_glGetBooleani_v +typedef void (APIENTRYP PFNGLGETINTEGERI_VPROC)(GLenum target, GLuint index, GLint *data); +GLAPI PFNGLGETINTEGERI_VPROC glad_glGetIntegeri_v; +#define glGetIntegeri_v glad_glGetIntegeri_v +typedef void (APIENTRYP PFNGLENABLEIPROC)(GLenum target, GLuint index); +GLAPI PFNGLENABLEIPROC glad_glEnablei; +#define glEnablei glad_glEnablei +typedef void (APIENTRYP PFNGLDISABLEIPROC)(GLenum target, GLuint index); +GLAPI PFNGLDISABLEIPROC glad_glDisablei; +#define glDisablei glad_glDisablei +typedef GLboolean (APIENTRYP PFNGLISENABLEDIPROC)(GLenum target, GLuint index); +GLAPI PFNGLISENABLEDIPROC glad_glIsEnabledi; +#define glIsEnabledi glad_glIsEnabledi +typedef void (APIENTRYP PFNGLBEGINTRANSFORMFEEDBACKPROC)(GLenum primitiveMode); +GLAPI PFNGLBEGINTRANSFORMFEEDBACKPROC glad_glBeginTransformFeedback; +#define glBeginTransformFeedback glad_glBeginTransformFeedback +typedef void (APIENTRYP PFNGLENDTRANSFORMFEEDBACKPROC)(void); +GLAPI PFNGLENDTRANSFORMFEEDBACKPROC glad_glEndTransformFeedback; +#define glEndTransformFeedback glad_glEndTransformFeedback +typedef void (APIENTRYP PFNGLBINDBUFFERRANGEPROC)(GLenum target, GLuint index, GLuint buffer, GLintptr offset, GLsizeiptr size); +GLAPI PFNGLBINDBUFFERRANGEPROC glad_glBindBufferRange; +#define glBindBufferRange glad_glBindBufferRange +typedef void (APIENTRYP PFNGLBINDBUFFERBASEPROC)(GLenum target, GLuint index, GLuint buffer); +GLAPI PFNGLBINDBUFFERBASEPROC glad_glBindBufferBase; +#define glBindBufferBase glad_glBindBufferBase +typedef void (APIENTRYP PFNGLTRANSFORMFEEDBACKVARYINGSPROC)(GLuint program, GLsizei count, const GLchar *const*varyings, GLenum bufferMode); +GLAPI PFNGLTRANSFORMFEEDBACKVARYINGSPROC glad_glTransformFeedbackVaryings; +#define glTransformFeedbackVaryings glad_glTransformFeedbackVaryings +typedef void (APIENTRYP PFNGLGETTRANSFORMFEEDBACKVARYINGPROC)(GLuint program, GLuint index, GLsizei bufSize, GLsizei *length, GLsizei *size, GLenum *type, GLchar *name); +GLAPI PFNGLGETTRANSFORMFEEDBACKVARYINGPROC glad_glGetTransformFeedbackVarying; +#define glGetTransformFeedbackVarying glad_glGetTransformFeedbackVarying +typedef void (APIENTRYP PFNGLCLAMPCOLORPROC)(GLenum target, GLenum clamp); +GLAPI PFNGLCLAMPCOLORPROC glad_glClampColor; +#define glClampColor glad_glClampColor +typedef void (APIENTRYP PFNGLBEGINCONDITIONALRENDERPROC)(GLuint id, GLenum mode); +GLAPI PFNGLBEGINCONDITIONALRENDERPROC glad_glBeginConditionalRender; +#define glBeginConditionalRender glad_glBeginConditionalRender +typedef void (APIENTRYP PFNGLENDCONDITIONALRENDERPROC)(void); +GLAPI PFNGLENDCONDITIONALRENDERPROC glad_glEndConditionalRender; +#define glEndConditionalRender glad_glEndConditionalRender +typedef void (APIENTRYP PFNGLVERTEXATTRIBIPOINTERPROC)(GLuint index, GLint size, GLenum type, GLsizei stride, const void *pointer); +GLAPI PFNGLVERTEXATTRIBIPOINTERPROC glad_glVertexAttribIPointer; +#define glVertexAttribIPointer glad_glVertexAttribIPointer +typedef void (APIENTRYP PFNGLGETVERTEXATTRIBIIVPROC)(GLuint index, GLenum pname, GLint *params); +GLAPI PFNGLGETVERTEXATTRIBIIVPROC glad_glGetVertexAttribIiv; +#define glGetVertexAttribIiv glad_glGetVertexAttribIiv +typedef void (APIENTRYP PFNGLGETVERTEXATTRIBIUIVPROC)(GLuint index, GLenum pname, GLuint *params); +GLAPI PFNGLGETVERTEXATTRIBIUIVPROC glad_glGetVertexAttribIuiv; +#define glGetVertexAttribIuiv glad_glGetVertexAttribIuiv +typedef void (APIENTRYP PFNGLVERTEXATTRIBI1IPROC)(GLuint index, GLint x); +GLAPI PFNGLVERTEXATTRIBI1IPROC glad_glVertexAttribI1i; +#define glVertexAttribI1i glad_glVertexAttribI1i +typedef void (APIENTRYP PFNGLVERTEXATTRIBI2IPROC)(GLuint index, GLint x, GLint y); +GLAPI PFNGLVERTEXATTRIBI2IPROC glad_glVertexAttribI2i; +#define glVertexAttribI2i glad_glVertexAttribI2i +typedef void (APIENTRYP PFNGLVERTEXATTRIBI3IPROC)(GLuint index, GLint x, GLint y, GLint z); +GLAPI PFNGLVERTEXATTRIBI3IPROC glad_glVertexAttribI3i; +#define glVertexAttribI3i glad_glVertexAttribI3i +typedef void (APIENTRYP PFNGLVERTEXATTRIBI4IPROC)(GLuint index, GLint x, GLint y, GLint z, GLint w); +GLAPI PFNGLVERTEXATTRIBI4IPROC glad_glVertexAttribI4i; +#define glVertexAttribI4i glad_glVertexAttribI4i +typedef void (APIENTRYP PFNGLVERTEXATTRIBI1UIPROC)(GLuint index, GLuint x); +GLAPI PFNGLVERTEXATTRIBI1UIPROC glad_glVertexAttribI1ui; +#define glVertexAttribI1ui glad_glVertexAttribI1ui +typedef void (APIENTRYP PFNGLVERTEXATTRIBI2UIPROC)(GLuint index, GLuint x, GLuint y); +GLAPI PFNGLVERTEXATTRIBI2UIPROC glad_glVertexAttribI2ui; +#define glVertexAttribI2ui glad_glVertexAttribI2ui +typedef void (APIENTRYP PFNGLVERTEXATTRIBI3UIPROC)(GLuint index, GLuint x, GLuint y, GLuint z); +GLAPI PFNGLVERTEXATTRIBI3UIPROC glad_glVertexAttribI3ui; +#define glVertexAttribI3ui glad_glVertexAttribI3ui +typedef void (APIENTRYP PFNGLVERTEXATTRIBI4UIPROC)(GLuint index, GLuint x, GLuint y, GLuint z, GLuint w); +GLAPI PFNGLVERTEXATTRIBI4UIPROC glad_glVertexAttribI4ui; +#define glVertexAttribI4ui glad_glVertexAttribI4ui +typedef void (APIENTRYP PFNGLVERTEXATTRIBI1IVPROC)(GLuint index, const GLint *v); +GLAPI PFNGLVERTEXATTRIBI1IVPROC glad_glVertexAttribI1iv; +#define glVertexAttribI1iv glad_glVertexAttribI1iv +typedef void (APIENTRYP PFNGLVERTEXATTRIBI2IVPROC)(GLuint index, const GLint *v); +GLAPI PFNGLVERTEXATTRIBI2IVPROC glad_glVertexAttribI2iv; +#define glVertexAttribI2iv glad_glVertexAttribI2iv +typedef void (APIENTRYP PFNGLVERTEXATTRIBI3IVPROC)(GLuint index, const GLint *v); +GLAPI PFNGLVERTEXATTRIBI3IVPROC glad_glVertexAttribI3iv; +#define glVertexAttribI3iv glad_glVertexAttribI3iv +typedef void (APIENTRYP PFNGLVERTEXATTRIBI4IVPROC)(GLuint index, const GLint *v); +GLAPI PFNGLVERTEXATTRIBI4IVPROC glad_glVertexAttribI4iv; +#define glVertexAttribI4iv glad_glVertexAttribI4iv +typedef void (APIENTRYP PFNGLVERTEXATTRIBI1UIVPROC)(GLuint index, const GLuint *v); +GLAPI PFNGLVERTEXATTRIBI1UIVPROC glad_glVertexAttribI1uiv; +#define glVertexAttribI1uiv glad_glVertexAttribI1uiv +typedef void (APIENTRYP PFNGLVERTEXATTRIBI2UIVPROC)(GLuint index, const GLuint *v); +GLAPI PFNGLVERTEXATTRIBI2UIVPROC glad_glVertexAttribI2uiv; +#define glVertexAttribI2uiv glad_glVertexAttribI2uiv +typedef void (APIENTRYP PFNGLVERTEXATTRIBI3UIVPROC)(GLuint index, const GLuint *v); +GLAPI PFNGLVERTEXATTRIBI3UIVPROC glad_glVertexAttribI3uiv; +#define glVertexAttribI3uiv glad_glVertexAttribI3uiv +typedef void (APIENTRYP PFNGLVERTEXATTRIBI4UIVPROC)(GLuint index, const GLuint *v); +GLAPI PFNGLVERTEXATTRIBI4UIVPROC glad_glVertexAttribI4uiv; +#define glVertexAttribI4uiv glad_glVertexAttribI4uiv +typedef void (APIENTRYP PFNGLVERTEXATTRIBI4BVPROC)(GLuint index, const GLbyte *v); +GLAPI PFNGLVERTEXATTRIBI4BVPROC glad_glVertexAttribI4bv; +#define glVertexAttribI4bv glad_glVertexAttribI4bv +typedef void (APIENTRYP PFNGLVERTEXATTRIBI4SVPROC)(GLuint index, const GLshort *v); +GLAPI PFNGLVERTEXATTRIBI4SVPROC glad_glVertexAttribI4sv; +#define glVertexAttribI4sv glad_glVertexAttribI4sv +typedef void (APIENTRYP PFNGLVERTEXATTRIBI4UBVPROC)(GLuint index, const GLubyte *v); +GLAPI PFNGLVERTEXATTRIBI4UBVPROC glad_glVertexAttribI4ubv; +#define glVertexAttribI4ubv glad_glVertexAttribI4ubv +typedef void (APIENTRYP PFNGLVERTEXATTRIBI4USVPROC)(GLuint index, const GLushort *v); +GLAPI PFNGLVERTEXATTRIBI4USVPROC glad_glVertexAttribI4usv; +#define glVertexAttribI4usv glad_glVertexAttribI4usv +typedef void (APIENTRYP PFNGLGETUNIFORMUIVPROC)(GLuint program, GLint location, GLuint *params); +GLAPI PFNGLGETUNIFORMUIVPROC glad_glGetUniformuiv; +#define glGetUniformuiv glad_glGetUniformuiv +typedef void (APIENTRYP PFNGLBINDFRAGDATALOCATIONPROC)(GLuint program, GLuint color, const GLchar *name); +GLAPI PFNGLBINDFRAGDATALOCATIONPROC glad_glBindFragDataLocation; +#define glBindFragDataLocation glad_glBindFragDataLocation +typedef GLint (APIENTRYP PFNGLGETFRAGDATALOCATIONPROC)(GLuint program, const GLchar *name); +GLAPI PFNGLGETFRAGDATALOCATIONPROC glad_glGetFragDataLocation; +#define glGetFragDataLocation glad_glGetFragDataLocation +typedef void (APIENTRYP PFNGLUNIFORM1UIPROC)(GLint location, GLuint v0); +GLAPI PFNGLUNIFORM1UIPROC glad_glUniform1ui; +#define glUniform1ui glad_glUniform1ui +typedef void (APIENTRYP PFNGLUNIFORM2UIPROC)(GLint location, GLuint v0, GLuint v1); +GLAPI PFNGLUNIFORM2UIPROC glad_glUniform2ui; +#define glUniform2ui glad_glUniform2ui +typedef void (APIENTRYP PFNGLUNIFORM3UIPROC)(GLint location, GLuint v0, GLuint v1, GLuint v2); +GLAPI PFNGLUNIFORM3UIPROC glad_glUniform3ui; +#define glUniform3ui glad_glUniform3ui +typedef void (APIENTRYP PFNGLUNIFORM4UIPROC)(GLint location, GLuint v0, GLuint v1, GLuint v2, GLuint v3); +GLAPI PFNGLUNIFORM4UIPROC glad_glUniform4ui; +#define glUniform4ui glad_glUniform4ui +typedef void (APIENTRYP PFNGLUNIFORM1UIVPROC)(GLint location, GLsizei count, const GLuint *value); +GLAPI PFNGLUNIFORM1UIVPROC glad_glUniform1uiv; +#define glUniform1uiv glad_glUniform1uiv +typedef void (APIENTRYP PFNGLUNIFORM2UIVPROC)(GLint location, GLsizei count, const GLuint *value); +GLAPI PFNGLUNIFORM2UIVPROC glad_glUniform2uiv; +#define glUniform2uiv glad_glUniform2uiv +typedef void (APIENTRYP PFNGLUNIFORM3UIVPROC)(GLint location, GLsizei count, const GLuint *value); +GLAPI PFNGLUNIFORM3UIVPROC glad_glUniform3uiv; +#define glUniform3uiv glad_glUniform3uiv +typedef void (APIENTRYP PFNGLUNIFORM4UIVPROC)(GLint location, GLsizei count, const GLuint *value); +GLAPI PFNGLUNIFORM4UIVPROC glad_glUniform4uiv; +#define glUniform4uiv glad_glUniform4uiv +typedef void (APIENTRYP PFNGLTEXPARAMETERIIVPROC)(GLenum target, GLenum pname, const GLint *params); +GLAPI PFNGLTEXPARAMETERIIVPROC glad_glTexParameterIiv; +#define glTexParameterIiv glad_glTexParameterIiv +typedef void (APIENTRYP PFNGLTEXPARAMETERIUIVPROC)(GLenum target, GLenum pname, const GLuint *params); +GLAPI PFNGLTEXPARAMETERIUIVPROC glad_glTexParameterIuiv; +#define glTexParameterIuiv glad_glTexParameterIuiv +typedef void (APIENTRYP PFNGLGETTEXPARAMETERIIVPROC)(GLenum target, GLenum pname, GLint *params); +GLAPI PFNGLGETTEXPARAMETERIIVPROC glad_glGetTexParameterIiv; +#define glGetTexParameterIiv glad_glGetTexParameterIiv +typedef void (APIENTRYP PFNGLGETTEXPARAMETERIUIVPROC)(GLenum target, GLenum pname, GLuint *params); +GLAPI PFNGLGETTEXPARAMETERIUIVPROC glad_glGetTexParameterIuiv; +#define glGetTexParameterIuiv glad_glGetTexParameterIuiv +typedef void (APIENTRYP PFNGLCLEARBUFFERIVPROC)(GLenum buffer, GLint drawbuffer, const GLint *value); +GLAPI PFNGLCLEARBUFFERIVPROC glad_glClearBufferiv; +#define glClearBufferiv glad_glClearBufferiv +typedef void (APIENTRYP PFNGLCLEARBUFFERUIVPROC)(GLenum buffer, GLint drawbuffer, const GLuint *value); +GLAPI PFNGLCLEARBUFFERUIVPROC glad_glClearBufferuiv; +#define glClearBufferuiv glad_glClearBufferuiv +typedef void (APIENTRYP PFNGLCLEARBUFFERFVPROC)(GLenum buffer, GLint drawbuffer, const GLfloat *value); +GLAPI PFNGLCLEARBUFFERFVPROC glad_glClearBufferfv; +#define glClearBufferfv glad_glClearBufferfv +typedef void (APIENTRYP PFNGLCLEARBUFFERFIPROC)(GLenum buffer, GLint drawbuffer, GLfloat depth, GLint stencil); +GLAPI PFNGLCLEARBUFFERFIPROC glad_glClearBufferfi; +#define glClearBufferfi glad_glClearBufferfi +typedef const GLubyte * (APIENTRYP PFNGLGETSTRINGIPROC)(GLenum name, GLuint index); +GLAPI PFNGLGETSTRINGIPROC glad_glGetStringi; +#define glGetStringi glad_glGetStringi +typedef GLboolean (APIENTRYP PFNGLISRENDERBUFFERPROC)(GLuint renderbuffer); +GLAPI PFNGLISRENDERBUFFERPROC glad_glIsRenderbuffer; +#define glIsRenderbuffer glad_glIsRenderbuffer +typedef void (APIENTRYP PFNGLBINDRENDERBUFFERPROC)(GLenum target, GLuint renderbuffer); +GLAPI PFNGLBINDRENDERBUFFERPROC glad_glBindRenderbuffer; +#define glBindRenderbuffer glad_glBindRenderbuffer +typedef void (APIENTRYP PFNGLDELETERENDERBUFFERSPROC)(GLsizei n, const GLuint *renderbuffers); +GLAPI PFNGLDELETERENDERBUFFERSPROC glad_glDeleteRenderbuffers; +#define glDeleteRenderbuffers glad_glDeleteRenderbuffers +typedef void (APIENTRYP PFNGLGENRENDERBUFFERSPROC)(GLsizei n, GLuint *renderbuffers); +GLAPI PFNGLGENRENDERBUFFERSPROC glad_glGenRenderbuffers; +#define glGenRenderbuffers glad_glGenRenderbuffers +typedef void (APIENTRYP PFNGLRENDERBUFFERSTORAGEPROC)(GLenum target, GLenum internalformat, GLsizei width, GLsizei height); +GLAPI PFNGLRENDERBUFFERSTORAGEPROC glad_glRenderbufferStorage; +#define glRenderbufferStorage glad_glRenderbufferStorage +typedef void (APIENTRYP PFNGLGETRENDERBUFFERPARAMETERIVPROC)(GLenum target, GLenum pname, GLint *params); +GLAPI PFNGLGETRENDERBUFFERPARAMETERIVPROC glad_glGetRenderbufferParameteriv; +#define glGetRenderbufferParameteriv glad_glGetRenderbufferParameteriv +typedef GLboolean (APIENTRYP PFNGLISFRAMEBUFFERPROC)(GLuint framebuffer); +GLAPI PFNGLISFRAMEBUFFERPROC glad_glIsFramebuffer; +#define glIsFramebuffer glad_glIsFramebuffer +typedef void (APIENTRYP PFNGLBINDFRAMEBUFFERPROC)(GLenum target, GLuint framebuffer); +GLAPI PFNGLBINDFRAMEBUFFERPROC glad_glBindFramebuffer; +#define glBindFramebuffer glad_glBindFramebuffer +typedef void (APIENTRYP PFNGLDELETEFRAMEBUFFERSPROC)(GLsizei n, const GLuint *framebuffers); +GLAPI PFNGLDELETEFRAMEBUFFERSPROC glad_glDeleteFramebuffers; +#define glDeleteFramebuffers glad_glDeleteFramebuffers +typedef void (APIENTRYP PFNGLGENFRAMEBUFFERSPROC)(GLsizei n, GLuint *framebuffers); +GLAPI PFNGLGENFRAMEBUFFERSPROC glad_glGenFramebuffers; +#define glGenFramebuffers glad_glGenFramebuffers +typedef GLenum (APIENTRYP PFNGLCHECKFRAMEBUFFERSTATUSPROC)(GLenum target); +GLAPI PFNGLCHECKFRAMEBUFFERSTATUSPROC glad_glCheckFramebufferStatus; +#define glCheckFramebufferStatus glad_glCheckFramebufferStatus +typedef void (APIENTRYP PFNGLFRAMEBUFFERTEXTURE1DPROC)(GLenum target, GLenum attachment, GLenum textarget, GLuint texture, GLint level); +GLAPI PFNGLFRAMEBUFFERTEXTURE1DPROC glad_glFramebufferTexture1D; +#define glFramebufferTexture1D glad_glFramebufferTexture1D +typedef void (APIENTRYP PFNGLFRAMEBUFFERTEXTURE2DPROC)(GLenum target, GLenum attachment, GLenum textarget, GLuint texture, GLint level); +GLAPI PFNGLFRAMEBUFFERTEXTURE2DPROC glad_glFramebufferTexture2D; +#define glFramebufferTexture2D glad_glFramebufferTexture2D +typedef void (APIENTRYP PFNGLFRAMEBUFFERTEXTURE3DPROC)(GLenum target, GLenum attachment, GLenum textarget, GLuint texture, GLint level, GLint zoffset); +GLAPI PFNGLFRAMEBUFFERTEXTURE3DPROC glad_glFramebufferTexture3D; +#define glFramebufferTexture3D glad_glFramebufferTexture3D +typedef void (APIENTRYP PFNGLFRAMEBUFFERRENDERBUFFERPROC)(GLenum target, GLenum attachment, GLenum renderbuffertarget, GLuint renderbuffer); +GLAPI PFNGLFRAMEBUFFERRENDERBUFFERPROC glad_glFramebufferRenderbuffer; +#define glFramebufferRenderbuffer glad_glFramebufferRenderbuffer +typedef void (APIENTRYP PFNGLGETFRAMEBUFFERATTACHMENTPARAMETERIVPROC)(GLenum target, GLenum attachment, GLenum pname, GLint *params); +GLAPI PFNGLGETFRAMEBUFFERATTACHMENTPARAMETERIVPROC glad_glGetFramebufferAttachmentParameteriv; +#define glGetFramebufferAttachmentParameteriv glad_glGetFramebufferAttachmentParameteriv +typedef void (APIENTRYP PFNGLGENERATEMIPMAPPROC)(GLenum target); +GLAPI PFNGLGENERATEMIPMAPPROC glad_glGenerateMipmap; +#define glGenerateMipmap glad_glGenerateMipmap +typedef void (APIENTRYP PFNGLBLITFRAMEBUFFERPROC)(GLint srcX0, GLint srcY0, GLint srcX1, GLint srcY1, GLint dstX0, GLint dstY0, GLint dstX1, GLint dstY1, GLbitfield mask, GLenum filter); +GLAPI PFNGLBLITFRAMEBUFFERPROC glad_glBlitFramebuffer; +#define glBlitFramebuffer glad_glBlitFramebuffer +typedef void (APIENTRYP PFNGLRENDERBUFFERSTORAGEMULTISAMPLEPROC)(GLenum target, GLsizei samples, GLenum internalformat, GLsizei width, GLsizei height); +GLAPI PFNGLRENDERBUFFERSTORAGEMULTISAMPLEPROC glad_glRenderbufferStorageMultisample; +#define glRenderbufferStorageMultisample glad_glRenderbufferStorageMultisample +typedef void (APIENTRYP PFNGLFRAMEBUFFERTEXTURELAYERPROC)(GLenum target, GLenum attachment, GLuint texture, GLint level, GLint layer); +GLAPI PFNGLFRAMEBUFFERTEXTURELAYERPROC glad_glFramebufferTextureLayer; +#define glFramebufferTextureLayer glad_glFramebufferTextureLayer +typedef void * (APIENTRYP PFNGLMAPBUFFERRANGEPROC)(GLenum target, GLintptr offset, GLsizeiptr length, GLbitfield access); +GLAPI PFNGLMAPBUFFERRANGEPROC glad_glMapBufferRange; +#define glMapBufferRange glad_glMapBufferRange +typedef void (APIENTRYP PFNGLFLUSHMAPPEDBUFFERRANGEPROC)(GLenum target, GLintptr offset, GLsizeiptr length); +GLAPI PFNGLFLUSHMAPPEDBUFFERRANGEPROC glad_glFlushMappedBufferRange; +#define glFlushMappedBufferRange glad_glFlushMappedBufferRange +typedef void (APIENTRYP PFNGLBINDVERTEXARRAYPROC)(GLuint array); +GLAPI PFNGLBINDVERTEXARRAYPROC glad_glBindVertexArray; +#define glBindVertexArray glad_glBindVertexArray +typedef void (APIENTRYP PFNGLDELETEVERTEXARRAYSPROC)(GLsizei n, const GLuint *arrays); +GLAPI PFNGLDELETEVERTEXARRAYSPROC glad_glDeleteVertexArrays; +#define glDeleteVertexArrays glad_glDeleteVertexArrays +typedef void (APIENTRYP PFNGLGENVERTEXARRAYSPROC)(GLsizei n, GLuint *arrays); +GLAPI PFNGLGENVERTEXARRAYSPROC glad_glGenVertexArrays; +#define glGenVertexArrays glad_glGenVertexArrays +typedef GLboolean (APIENTRYP PFNGLISVERTEXARRAYPROC)(GLuint array); +GLAPI PFNGLISVERTEXARRAYPROC glad_glIsVertexArray; +#define glIsVertexArray glad_glIsVertexArray +#endif +#ifndef GL_VERSION_3_1 +#define GL_VERSION_3_1 1 +GLAPI int GLAD_GL_VERSION_3_1; +typedef void (APIENTRYP PFNGLDRAWARRAYSINSTANCEDPROC)(GLenum mode, GLint first, GLsizei count, GLsizei instancecount); +GLAPI PFNGLDRAWARRAYSINSTANCEDPROC glad_glDrawArraysInstanced; +#define glDrawArraysInstanced glad_glDrawArraysInstanced +typedef void (APIENTRYP PFNGLDRAWELEMENTSINSTANCEDPROC)(GLenum mode, GLsizei count, GLenum type, const void *indices, GLsizei instancecount); +GLAPI PFNGLDRAWELEMENTSINSTANCEDPROC glad_glDrawElementsInstanced; +#define glDrawElementsInstanced glad_glDrawElementsInstanced +typedef void (APIENTRYP PFNGLTEXBUFFERPROC)(GLenum target, GLenum internalformat, GLuint buffer); +GLAPI PFNGLTEXBUFFERPROC glad_glTexBuffer; +#define glTexBuffer glad_glTexBuffer +typedef void (APIENTRYP PFNGLPRIMITIVERESTARTINDEXPROC)(GLuint index); +GLAPI PFNGLPRIMITIVERESTARTINDEXPROC glad_glPrimitiveRestartIndex; +#define glPrimitiveRestartIndex glad_glPrimitiveRestartIndex +typedef void (APIENTRYP PFNGLCOPYBUFFERSUBDATAPROC)(GLenum readTarget, GLenum writeTarget, GLintptr readOffset, GLintptr writeOffset, GLsizeiptr size); +GLAPI PFNGLCOPYBUFFERSUBDATAPROC glad_glCopyBufferSubData; +#define glCopyBufferSubData glad_glCopyBufferSubData +typedef void (APIENTRYP PFNGLGETUNIFORMINDICESPROC)(GLuint program, GLsizei uniformCount, const GLchar *const*uniformNames, GLuint *uniformIndices); +GLAPI PFNGLGETUNIFORMINDICESPROC glad_glGetUniformIndices; +#define glGetUniformIndices glad_glGetUniformIndices +typedef void (APIENTRYP PFNGLGETACTIVEUNIFORMSIVPROC)(GLuint program, GLsizei uniformCount, const GLuint *uniformIndices, GLenum pname, GLint *params); +GLAPI PFNGLGETACTIVEUNIFORMSIVPROC glad_glGetActiveUniformsiv; +#define glGetActiveUniformsiv glad_glGetActiveUniformsiv +typedef void (APIENTRYP PFNGLGETACTIVEUNIFORMNAMEPROC)(GLuint program, GLuint uniformIndex, GLsizei bufSize, GLsizei *length, GLchar *uniformName); +GLAPI PFNGLGETACTIVEUNIFORMNAMEPROC glad_glGetActiveUniformName; +#define glGetActiveUniformName glad_glGetActiveUniformName +typedef GLuint (APIENTRYP PFNGLGETUNIFORMBLOCKINDEXPROC)(GLuint program, const GLchar *uniformBlockName); +GLAPI PFNGLGETUNIFORMBLOCKINDEXPROC glad_glGetUniformBlockIndex; +#define glGetUniformBlockIndex glad_glGetUniformBlockIndex +typedef void (APIENTRYP PFNGLGETACTIVEUNIFORMBLOCKIVPROC)(GLuint program, GLuint uniformBlockIndex, GLenum pname, GLint *params); +GLAPI PFNGLGETACTIVEUNIFORMBLOCKIVPROC glad_glGetActiveUniformBlockiv; +#define glGetActiveUniformBlockiv glad_glGetActiveUniformBlockiv +typedef void (APIENTRYP PFNGLGETACTIVEUNIFORMBLOCKNAMEPROC)(GLuint program, GLuint uniformBlockIndex, GLsizei bufSize, GLsizei *length, GLchar *uniformBlockName); +GLAPI PFNGLGETACTIVEUNIFORMBLOCKNAMEPROC glad_glGetActiveUniformBlockName; +#define glGetActiveUniformBlockName glad_glGetActiveUniformBlockName +typedef void (APIENTRYP PFNGLUNIFORMBLOCKBINDINGPROC)(GLuint program, GLuint uniformBlockIndex, GLuint uniformBlockBinding); +GLAPI PFNGLUNIFORMBLOCKBINDINGPROC glad_glUniformBlockBinding; +#define glUniformBlockBinding glad_glUniformBlockBinding +#endif +#ifndef GL_VERSION_3_2 +#define GL_VERSION_3_2 1 +GLAPI int GLAD_GL_VERSION_3_2; +typedef void (APIENTRYP PFNGLDRAWELEMENTSBASEVERTEXPROC)(GLenum mode, GLsizei count, GLenum type, const void *indices, GLint basevertex); +GLAPI PFNGLDRAWELEMENTSBASEVERTEXPROC glad_glDrawElementsBaseVertex; +#define glDrawElementsBaseVertex glad_glDrawElementsBaseVertex +typedef void (APIENTRYP PFNGLDRAWRANGEELEMENTSBASEVERTEXPROC)(GLenum mode, GLuint start, GLuint end, GLsizei count, GLenum type, const void *indices, GLint basevertex); +GLAPI PFNGLDRAWRANGEELEMENTSBASEVERTEXPROC glad_glDrawRangeElementsBaseVertex; +#define glDrawRangeElementsBaseVertex glad_glDrawRangeElementsBaseVertex +typedef void (APIENTRYP PFNGLDRAWELEMENTSINSTANCEDBASEVERTEXPROC)(GLenum mode, GLsizei count, GLenum type, const void *indices, GLsizei instancecount, GLint basevertex); +GLAPI PFNGLDRAWELEMENTSINSTANCEDBASEVERTEXPROC glad_glDrawElementsInstancedBaseVertex; +#define glDrawElementsInstancedBaseVertex glad_glDrawElementsInstancedBaseVertex +typedef void (APIENTRYP PFNGLMULTIDRAWELEMENTSBASEVERTEXPROC)(GLenum mode, const GLsizei *count, GLenum type, const void *const*indices, GLsizei drawcount, const GLint *basevertex); +GLAPI PFNGLMULTIDRAWELEMENTSBASEVERTEXPROC glad_glMultiDrawElementsBaseVertex; +#define glMultiDrawElementsBaseVertex glad_glMultiDrawElementsBaseVertex +typedef void (APIENTRYP PFNGLPROVOKINGVERTEXPROC)(GLenum mode); +GLAPI PFNGLPROVOKINGVERTEXPROC glad_glProvokingVertex; +#define glProvokingVertex glad_glProvokingVertex +typedef GLsync (APIENTRYP PFNGLFENCESYNCPROC)(GLenum condition, GLbitfield flags); +GLAPI PFNGLFENCESYNCPROC glad_glFenceSync; +#define glFenceSync glad_glFenceSync +typedef GLboolean (APIENTRYP PFNGLISSYNCPROC)(GLsync sync); +GLAPI PFNGLISSYNCPROC glad_glIsSync; +#define glIsSync glad_glIsSync +typedef void (APIENTRYP PFNGLDELETESYNCPROC)(GLsync sync); +GLAPI PFNGLDELETESYNCPROC glad_glDeleteSync; +#define glDeleteSync glad_glDeleteSync +typedef GLenum (APIENTRYP PFNGLCLIENTWAITSYNCPROC)(GLsync sync, GLbitfield flags, GLuint64 timeout); +GLAPI PFNGLCLIENTWAITSYNCPROC glad_glClientWaitSync; +#define glClientWaitSync glad_glClientWaitSync +typedef void (APIENTRYP PFNGLWAITSYNCPROC)(GLsync sync, GLbitfield flags, GLuint64 timeout); +GLAPI PFNGLWAITSYNCPROC glad_glWaitSync; +#define glWaitSync glad_glWaitSync +typedef void (APIENTRYP PFNGLGETINTEGER64VPROC)(GLenum pname, GLint64 *data); +GLAPI PFNGLGETINTEGER64VPROC glad_glGetInteger64v; +#define glGetInteger64v glad_glGetInteger64v +typedef void (APIENTRYP PFNGLGETSYNCIVPROC)(GLsync sync, GLenum pname, GLsizei count, GLsizei *length, GLint *values); +GLAPI PFNGLGETSYNCIVPROC glad_glGetSynciv; +#define glGetSynciv glad_glGetSynciv +typedef void (APIENTRYP PFNGLGETINTEGER64I_VPROC)(GLenum target, GLuint index, GLint64 *data); +GLAPI PFNGLGETINTEGER64I_VPROC glad_glGetInteger64i_v; +#define glGetInteger64i_v glad_glGetInteger64i_v +typedef void (APIENTRYP PFNGLGETBUFFERPARAMETERI64VPROC)(GLenum target, GLenum pname, GLint64 *params); +GLAPI PFNGLGETBUFFERPARAMETERI64VPROC glad_glGetBufferParameteri64v; +#define glGetBufferParameteri64v glad_glGetBufferParameteri64v +typedef void (APIENTRYP PFNGLFRAMEBUFFERTEXTUREPROC)(GLenum target, GLenum attachment, GLuint texture, GLint level); +GLAPI PFNGLFRAMEBUFFERTEXTUREPROC glad_glFramebufferTexture; +#define glFramebufferTexture glad_glFramebufferTexture +typedef void (APIENTRYP PFNGLTEXIMAGE2DMULTISAMPLEPROC)(GLenum target, GLsizei samples, GLenum internalformat, GLsizei width, GLsizei height, GLboolean fixedsamplelocations); +GLAPI PFNGLTEXIMAGE2DMULTISAMPLEPROC glad_glTexImage2DMultisample; +#define glTexImage2DMultisample glad_glTexImage2DMultisample +typedef void (APIENTRYP PFNGLTEXIMAGE3DMULTISAMPLEPROC)(GLenum target, GLsizei samples, GLenum internalformat, GLsizei width, GLsizei height, GLsizei depth, GLboolean fixedsamplelocations); +GLAPI PFNGLTEXIMAGE3DMULTISAMPLEPROC glad_glTexImage3DMultisample; +#define glTexImage3DMultisample glad_glTexImage3DMultisample +typedef void (APIENTRYP PFNGLGETMULTISAMPLEFVPROC)(GLenum pname, GLuint index, GLfloat *val); +GLAPI PFNGLGETMULTISAMPLEFVPROC glad_glGetMultisamplefv; +#define glGetMultisamplefv glad_glGetMultisamplefv +typedef void (APIENTRYP PFNGLSAMPLEMASKIPROC)(GLuint maskNumber, GLbitfield mask); +GLAPI PFNGLSAMPLEMASKIPROC glad_glSampleMaski; +#define glSampleMaski glad_glSampleMaski +#endif +#ifndef GL_VERSION_3_3 +#define GL_VERSION_3_3 1 +GLAPI int GLAD_GL_VERSION_3_3; +typedef void (APIENTRYP PFNGLBINDFRAGDATALOCATIONINDEXEDPROC)(GLuint program, GLuint colorNumber, GLuint index, const GLchar *name); +GLAPI PFNGLBINDFRAGDATALOCATIONINDEXEDPROC glad_glBindFragDataLocationIndexed; +#define glBindFragDataLocationIndexed glad_glBindFragDataLocationIndexed +typedef GLint (APIENTRYP PFNGLGETFRAGDATAINDEXPROC)(GLuint program, const GLchar *name); +GLAPI PFNGLGETFRAGDATAINDEXPROC glad_glGetFragDataIndex; +#define glGetFragDataIndex glad_glGetFragDataIndex +typedef void (APIENTRYP PFNGLGENSAMPLERSPROC)(GLsizei count, GLuint *samplers); +GLAPI PFNGLGENSAMPLERSPROC glad_glGenSamplers; +#define glGenSamplers glad_glGenSamplers +typedef void (APIENTRYP PFNGLDELETESAMPLERSPROC)(GLsizei count, const GLuint *samplers); +GLAPI PFNGLDELETESAMPLERSPROC glad_glDeleteSamplers; +#define glDeleteSamplers glad_glDeleteSamplers +typedef GLboolean (APIENTRYP PFNGLISSAMPLERPROC)(GLuint sampler); +GLAPI PFNGLISSAMPLERPROC glad_glIsSampler; +#define glIsSampler glad_glIsSampler +typedef void (APIENTRYP PFNGLBINDSAMPLERPROC)(GLuint unit, GLuint sampler); +GLAPI PFNGLBINDSAMPLERPROC glad_glBindSampler; +#define glBindSampler glad_glBindSampler +typedef void (APIENTRYP PFNGLSAMPLERPARAMETERIPROC)(GLuint sampler, GLenum pname, GLint param); +GLAPI PFNGLSAMPLERPARAMETERIPROC glad_glSamplerParameteri; +#define glSamplerParameteri glad_glSamplerParameteri +typedef void (APIENTRYP PFNGLSAMPLERPARAMETERIVPROC)(GLuint sampler, GLenum pname, const GLint *param); +GLAPI PFNGLSAMPLERPARAMETERIVPROC glad_glSamplerParameteriv; +#define glSamplerParameteriv glad_glSamplerParameteriv +typedef void (APIENTRYP PFNGLSAMPLERPARAMETERFPROC)(GLuint sampler, GLenum pname, GLfloat param); +GLAPI PFNGLSAMPLERPARAMETERFPROC glad_glSamplerParameterf; +#define glSamplerParameterf glad_glSamplerParameterf +typedef void (APIENTRYP PFNGLSAMPLERPARAMETERFVPROC)(GLuint sampler, GLenum pname, const GLfloat *param); +GLAPI PFNGLSAMPLERPARAMETERFVPROC glad_glSamplerParameterfv; +#define glSamplerParameterfv glad_glSamplerParameterfv +typedef void (APIENTRYP PFNGLSAMPLERPARAMETERIIVPROC)(GLuint sampler, GLenum pname, const GLint *param); +GLAPI PFNGLSAMPLERPARAMETERIIVPROC glad_glSamplerParameterIiv; +#define glSamplerParameterIiv glad_glSamplerParameterIiv +typedef void (APIENTRYP PFNGLSAMPLERPARAMETERIUIVPROC)(GLuint sampler, GLenum pname, const GLuint *param); +GLAPI PFNGLSAMPLERPARAMETERIUIVPROC glad_glSamplerParameterIuiv; +#define glSamplerParameterIuiv glad_glSamplerParameterIuiv +typedef void (APIENTRYP PFNGLGETSAMPLERPARAMETERIVPROC)(GLuint sampler, GLenum pname, GLint *params); +GLAPI PFNGLGETSAMPLERPARAMETERIVPROC glad_glGetSamplerParameteriv; +#define glGetSamplerParameteriv glad_glGetSamplerParameteriv +typedef void (APIENTRYP PFNGLGETSAMPLERPARAMETERIIVPROC)(GLuint sampler, GLenum pname, GLint *params); +GLAPI PFNGLGETSAMPLERPARAMETERIIVPROC glad_glGetSamplerParameterIiv; +#define glGetSamplerParameterIiv glad_glGetSamplerParameterIiv +typedef void (APIENTRYP PFNGLGETSAMPLERPARAMETERFVPROC)(GLuint sampler, GLenum pname, GLfloat *params); +GLAPI PFNGLGETSAMPLERPARAMETERFVPROC glad_glGetSamplerParameterfv; +#define glGetSamplerParameterfv glad_glGetSamplerParameterfv +typedef void (APIENTRYP PFNGLGETSAMPLERPARAMETERIUIVPROC)(GLuint sampler, GLenum pname, GLuint *params); +GLAPI PFNGLGETSAMPLERPARAMETERIUIVPROC glad_glGetSamplerParameterIuiv; +#define glGetSamplerParameterIuiv glad_glGetSamplerParameterIuiv +typedef void (APIENTRYP PFNGLQUERYCOUNTERPROC)(GLuint id, GLenum target); +GLAPI PFNGLQUERYCOUNTERPROC glad_glQueryCounter; +#define glQueryCounter glad_glQueryCounter +typedef void (APIENTRYP PFNGLGETQUERYOBJECTI64VPROC)(GLuint id, GLenum pname, GLint64 *params); +GLAPI PFNGLGETQUERYOBJECTI64VPROC glad_glGetQueryObjecti64v; +#define glGetQueryObjecti64v glad_glGetQueryObjecti64v +typedef void (APIENTRYP PFNGLGETQUERYOBJECTUI64VPROC)(GLuint id, GLenum pname, GLuint64 *params); +GLAPI PFNGLGETQUERYOBJECTUI64VPROC glad_glGetQueryObjectui64v; +#define glGetQueryObjectui64v glad_glGetQueryObjectui64v +typedef void (APIENTRYP PFNGLVERTEXATTRIBDIVISORPROC)(GLuint index, GLuint divisor); +GLAPI PFNGLVERTEXATTRIBDIVISORPROC glad_glVertexAttribDivisor; +#define glVertexAttribDivisor glad_glVertexAttribDivisor +typedef void (APIENTRYP PFNGLVERTEXATTRIBP1UIPROC)(GLuint index, GLenum type, GLboolean normalized, GLuint value); +GLAPI PFNGLVERTEXATTRIBP1UIPROC glad_glVertexAttribP1ui; +#define glVertexAttribP1ui glad_glVertexAttribP1ui +typedef void (APIENTRYP PFNGLVERTEXATTRIBP1UIVPROC)(GLuint index, GLenum type, GLboolean normalized, const GLuint *value); +GLAPI PFNGLVERTEXATTRIBP1UIVPROC glad_glVertexAttribP1uiv; +#define glVertexAttribP1uiv glad_glVertexAttribP1uiv +typedef void (APIENTRYP PFNGLVERTEXATTRIBP2UIPROC)(GLuint index, GLenum type, GLboolean normalized, GLuint value); +GLAPI PFNGLVERTEXATTRIBP2UIPROC glad_glVertexAttribP2ui; +#define glVertexAttribP2ui glad_glVertexAttribP2ui +typedef void (APIENTRYP PFNGLVERTEXATTRIBP2UIVPROC)(GLuint index, GLenum type, GLboolean normalized, const GLuint *value); +GLAPI PFNGLVERTEXATTRIBP2UIVPROC glad_glVertexAttribP2uiv; +#define glVertexAttribP2uiv glad_glVertexAttribP2uiv +typedef void (APIENTRYP PFNGLVERTEXATTRIBP3UIPROC)(GLuint index, GLenum type, GLboolean normalized, GLuint value); +GLAPI PFNGLVERTEXATTRIBP3UIPROC glad_glVertexAttribP3ui; +#define glVertexAttribP3ui glad_glVertexAttribP3ui +typedef void (APIENTRYP PFNGLVERTEXATTRIBP3UIVPROC)(GLuint index, GLenum type, GLboolean normalized, const GLuint *value); +GLAPI PFNGLVERTEXATTRIBP3UIVPROC glad_glVertexAttribP3uiv; +#define glVertexAttribP3uiv glad_glVertexAttribP3uiv +typedef void (APIENTRYP PFNGLVERTEXATTRIBP4UIPROC)(GLuint index, GLenum type, GLboolean normalized, GLuint value); +GLAPI PFNGLVERTEXATTRIBP4UIPROC glad_glVertexAttribP4ui; +#define glVertexAttribP4ui glad_glVertexAttribP4ui +typedef void (APIENTRYP PFNGLVERTEXATTRIBP4UIVPROC)(GLuint index, GLenum type, GLboolean normalized, const GLuint *value); +GLAPI PFNGLVERTEXATTRIBP4UIVPROC glad_glVertexAttribP4uiv; +#define glVertexAttribP4uiv glad_glVertexAttribP4uiv +typedef void (APIENTRYP PFNGLVERTEXP2UIPROC)(GLenum type, GLuint value); +GLAPI PFNGLVERTEXP2UIPROC glad_glVertexP2ui; +#define glVertexP2ui glad_glVertexP2ui +typedef void (APIENTRYP PFNGLVERTEXP2UIVPROC)(GLenum type, const GLuint *value); +GLAPI PFNGLVERTEXP2UIVPROC glad_glVertexP2uiv; +#define glVertexP2uiv glad_glVertexP2uiv +typedef void (APIENTRYP PFNGLVERTEXP3UIPROC)(GLenum type, GLuint value); +GLAPI PFNGLVERTEXP3UIPROC glad_glVertexP3ui; +#define glVertexP3ui glad_glVertexP3ui +typedef void (APIENTRYP PFNGLVERTEXP3UIVPROC)(GLenum type, const GLuint *value); +GLAPI PFNGLVERTEXP3UIVPROC glad_glVertexP3uiv; +#define glVertexP3uiv glad_glVertexP3uiv +typedef void (APIENTRYP PFNGLVERTEXP4UIPROC)(GLenum type, GLuint value); +GLAPI PFNGLVERTEXP4UIPROC glad_glVertexP4ui; +#define glVertexP4ui glad_glVertexP4ui +typedef void (APIENTRYP PFNGLVERTEXP4UIVPROC)(GLenum type, const GLuint *value); +GLAPI PFNGLVERTEXP4UIVPROC glad_glVertexP4uiv; +#define glVertexP4uiv glad_glVertexP4uiv +typedef void (APIENTRYP PFNGLTEXCOORDP1UIPROC)(GLenum type, GLuint coords); +GLAPI PFNGLTEXCOORDP1UIPROC glad_glTexCoordP1ui; +#define glTexCoordP1ui glad_glTexCoordP1ui +typedef void (APIENTRYP PFNGLTEXCOORDP1UIVPROC)(GLenum type, const GLuint *coords); +GLAPI PFNGLTEXCOORDP1UIVPROC glad_glTexCoordP1uiv; +#define glTexCoordP1uiv glad_glTexCoordP1uiv +typedef void (APIENTRYP PFNGLTEXCOORDP2UIPROC)(GLenum type, GLuint coords); +GLAPI PFNGLTEXCOORDP2UIPROC glad_glTexCoordP2ui; +#define glTexCoordP2ui glad_glTexCoordP2ui +typedef void (APIENTRYP PFNGLTEXCOORDP2UIVPROC)(GLenum type, const GLuint *coords); +GLAPI PFNGLTEXCOORDP2UIVPROC glad_glTexCoordP2uiv; +#define glTexCoordP2uiv glad_glTexCoordP2uiv +typedef void (APIENTRYP PFNGLTEXCOORDP3UIPROC)(GLenum type, GLuint coords); +GLAPI PFNGLTEXCOORDP3UIPROC glad_glTexCoordP3ui; +#define glTexCoordP3ui glad_glTexCoordP3ui +typedef void (APIENTRYP PFNGLTEXCOORDP3UIVPROC)(GLenum type, const GLuint *coords); +GLAPI PFNGLTEXCOORDP3UIVPROC glad_glTexCoordP3uiv; +#define glTexCoordP3uiv glad_glTexCoordP3uiv +typedef void (APIENTRYP PFNGLTEXCOORDP4UIPROC)(GLenum type, GLuint coords); +GLAPI PFNGLTEXCOORDP4UIPROC glad_glTexCoordP4ui; +#define glTexCoordP4ui glad_glTexCoordP4ui +typedef void (APIENTRYP PFNGLTEXCOORDP4UIVPROC)(GLenum type, const GLuint *coords); +GLAPI PFNGLTEXCOORDP4UIVPROC glad_glTexCoordP4uiv; +#define glTexCoordP4uiv glad_glTexCoordP4uiv +typedef void (APIENTRYP PFNGLMULTITEXCOORDP1UIPROC)(GLenum texture, GLenum type, GLuint coords); +GLAPI PFNGLMULTITEXCOORDP1UIPROC glad_glMultiTexCoordP1ui; +#define glMultiTexCoordP1ui glad_glMultiTexCoordP1ui +typedef void (APIENTRYP PFNGLMULTITEXCOORDP1UIVPROC)(GLenum texture, GLenum type, const GLuint *coords); +GLAPI PFNGLMULTITEXCOORDP1UIVPROC glad_glMultiTexCoordP1uiv; +#define glMultiTexCoordP1uiv glad_glMultiTexCoordP1uiv +typedef void (APIENTRYP PFNGLMULTITEXCOORDP2UIPROC)(GLenum texture, GLenum type, GLuint coords); +GLAPI PFNGLMULTITEXCOORDP2UIPROC glad_glMultiTexCoordP2ui; +#define glMultiTexCoordP2ui glad_glMultiTexCoordP2ui +typedef void (APIENTRYP PFNGLMULTITEXCOORDP2UIVPROC)(GLenum texture, GLenum type, const GLuint *coords); +GLAPI PFNGLMULTITEXCOORDP2UIVPROC glad_glMultiTexCoordP2uiv; +#define glMultiTexCoordP2uiv glad_glMultiTexCoordP2uiv +typedef void (APIENTRYP PFNGLMULTITEXCOORDP3UIPROC)(GLenum texture, GLenum type, GLuint coords); +GLAPI PFNGLMULTITEXCOORDP3UIPROC glad_glMultiTexCoordP3ui; +#define glMultiTexCoordP3ui glad_glMultiTexCoordP3ui +typedef void (APIENTRYP PFNGLMULTITEXCOORDP3UIVPROC)(GLenum texture, GLenum type, const GLuint *coords); +GLAPI PFNGLMULTITEXCOORDP3UIVPROC glad_glMultiTexCoordP3uiv; +#define glMultiTexCoordP3uiv glad_glMultiTexCoordP3uiv +typedef void (APIENTRYP PFNGLMULTITEXCOORDP4UIPROC)(GLenum texture, GLenum type, GLuint coords); +GLAPI PFNGLMULTITEXCOORDP4UIPROC glad_glMultiTexCoordP4ui; +#define glMultiTexCoordP4ui glad_glMultiTexCoordP4ui +typedef void (APIENTRYP PFNGLMULTITEXCOORDP4UIVPROC)(GLenum texture, GLenum type, const GLuint *coords); +GLAPI PFNGLMULTITEXCOORDP4UIVPROC glad_glMultiTexCoordP4uiv; +#define glMultiTexCoordP4uiv glad_glMultiTexCoordP4uiv +typedef void (APIENTRYP PFNGLNORMALP3UIPROC)(GLenum type, GLuint coords); +GLAPI PFNGLNORMALP3UIPROC glad_glNormalP3ui; +#define glNormalP3ui glad_glNormalP3ui +typedef void (APIENTRYP PFNGLNORMALP3UIVPROC)(GLenum type, const GLuint *coords); +GLAPI PFNGLNORMALP3UIVPROC glad_glNormalP3uiv; +#define glNormalP3uiv glad_glNormalP3uiv +typedef void (APIENTRYP PFNGLCOLORP3UIPROC)(GLenum type, GLuint color); +GLAPI PFNGLCOLORP3UIPROC glad_glColorP3ui; +#define glColorP3ui glad_glColorP3ui +typedef void (APIENTRYP PFNGLCOLORP3UIVPROC)(GLenum type, const GLuint *color); +GLAPI PFNGLCOLORP3UIVPROC glad_glColorP3uiv; +#define glColorP3uiv glad_glColorP3uiv +typedef void (APIENTRYP PFNGLCOLORP4UIPROC)(GLenum type, GLuint color); +GLAPI PFNGLCOLORP4UIPROC glad_glColorP4ui; +#define glColorP4ui glad_glColorP4ui +typedef void (APIENTRYP PFNGLCOLORP4UIVPROC)(GLenum type, const GLuint *color); +GLAPI PFNGLCOLORP4UIVPROC glad_glColorP4uiv; +#define glColorP4uiv glad_glColorP4uiv +typedef void (APIENTRYP PFNGLSECONDARYCOLORP3UIPROC)(GLenum type, GLuint color); +GLAPI PFNGLSECONDARYCOLORP3UIPROC glad_glSecondaryColorP3ui; +#define glSecondaryColorP3ui glad_glSecondaryColorP3ui +typedef void (APIENTRYP PFNGLSECONDARYCOLORP3UIVPROC)(GLenum type, const GLuint *color); +GLAPI PFNGLSECONDARYCOLORP3UIVPROC glad_glSecondaryColorP3uiv; +#define glSecondaryColorP3uiv glad_glSecondaryColorP3uiv +#endif +#ifndef GL_VERSION_4_0 +#define GL_VERSION_4_0 1 +GLAPI int GLAD_GL_VERSION_4_0; +typedef void (APIENTRYP PFNGLMINSAMPLESHADINGPROC)(GLfloat value); +GLAPI PFNGLMINSAMPLESHADINGPROC glad_glMinSampleShading; +#define glMinSampleShading glad_glMinSampleShading +typedef void (APIENTRYP PFNGLBLENDEQUATIONIPROC)(GLuint buf, GLenum mode); +GLAPI PFNGLBLENDEQUATIONIPROC glad_glBlendEquationi; +#define glBlendEquationi glad_glBlendEquationi +typedef void (APIENTRYP PFNGLBLENDEQUATIONSEPARATEIPROC)(GLuint buf, GLenum modeRGB, GLenum modeAlpha); +GLAPI PFNGLBLENDEQUATIONSEPARATEIPROC glad_glBlendEquationSeparatei; +#define glBlendEquationSeparatei glad_glBlendEquationSeparatei +typedef void (APIENTRYP PFNGLBLENDFUNCIPROC)(GLuint buf, GLenum src, GLenum dst); +GLAPI PFNGLBLENDFUNCIPROC glad_glBlendFunci; +#define glBlendFunci glad_glBlendFunci +typedef void (APIENTRYP PFNGLBLENDFUNCSEPARATEIPROC)(GLuint buf, GLenum srcRGB, GLenum dstRGB, GLenum srcAlpha, GLenum dstAlpha); +GLAPI PFNGLBLENDFUNCSEPARATEIPROC glad_glBlendFuncSeparatei; +#define glBlendFuncSeparatei glad_glBlendFuncSeparatei +typedef void (APIENTRYP PFNGLDRAWARRAYSINDIRECTPROC)(GLenum mode, const void *indirect); +GLAPI PFNGLDRAWARRAYSINDIRECTPROC glad_glDrawArraysIndirect; +#define glDrawArraysIndirect glad_glDrawArraysIndirect +typedef void (APIENTRYP PFNGLDRAWELEMENTSINDIRECTPROC)(GLenum mode, GLenum type, const void *indirect); +GLAPI PFNGLDRAWELEMENTSINDIRECTPROC glad_glDrawElementsIndirect; +#define glDrawElementsIndirect glad_glDrawElementsIndirect +typedef void (APIENTRYP PFNGLUNIFORM1DPROC)(GLint location, GLdouble x); +GLAPI PFNGLUNIFORM1DPROC glad_glUniform1d; +#define glUniform1d glad_glUniform1d +typedef void (APIENTRYP PFNGLUNIFORM2DPROC)(GLint location, GLdouble x, GLdouble y); +GLAPI PFNGLUNIFORM2DPROC glad_glUniform2d; +#define glUniform2d glad_glUniform2d +typedef void (APIENTRYP PFNGLUNIFORM3DPROC)(GLint location, GLdouble x, GLdouble y, GLdouble z); +GLAPI PFNGLUNIFORM3DPROC glad_glUniform3d; +#define glUniform3d glad_glUniform3d +typedef void (APIENTRYP PFNGLUNIFORM4DPROC)(GLint location, GLdouble x, GLdouble y, GLdouble z, GLdouble w); +GLAPI PFNGLUNIFORM4DPROC glad_glUniform4d; +#define glUniform4d glad_glUniform4d +typedef void (APIENTRYP PFNGLUNIFORM1DVPROC)(GLint location, GLsizei count, const GLdouble *value); +GLAPI PFNGLUNIFORM1DVPROC glad_glUniform1dv; +#define glUniform1dv glad_glUniform1dv +typedef void (APIENTRYP PFNGLUNIFORM2DVPROC)(GLint location, GLsizei count, const GLdouble *value); +GLAPI PFNGLUNIFORM2DVPROC glad_glUniform2dv; +#define glUniform2dv glad_glUniform2dv +typedef void (APIENTRYP PFNGLUNIFORM3DVPROC)(GLint location, GLsizei count, const GLdouble *value); +GLAPI PFNGLUNIFORM3DVPROC glad_glUniform3dv; +#define glUniform3dv glad_glUniform3dv +typedef void (APIENTRYP PFNGLUNIFORM4DVPROC)(GLint location, GLsizei count, const GLdouble *value); +GLAPI PFNGLUNIFORM4DVPROC glad_glUniform4dv; +#define glUniform4dv glad_glUniform4dv +typedef void (APIENTRYP PFNGLUNIFORMMATRIX2DVPROC)(GLint location, GLsizei count, GLboolean transpose, const GLdouble *value); +GLAPI PFNGLUNIFORMMATRIX2DVPROC glad_glUniformMatrix2dv; +#define glUniformMatrix2dv glad_glUniformMatrix2dv +typedef void (APIENTRYP PFNGLUNIFORMMATRIX3DVPROC)(GLint location, GLsizei count, GLboolean transpose, const GLdouble *value); +GLAPI PFNGLUNIFORMMATRIX3DVPROC glad_glUniformMatrix3dv; +#define glUniformMatrix3dv glad_glUniformMatrix3dv +typedef void (APIENTRYP PFNGLUNIFORMMATRIX4DVPROC)(GLint location, GLsizei count, GLboolean transpose, const GLdouble *value); +GLAPI PFNGLUNIFORMMATRIX4DVPROC glad_glUniformMatrix4dv; +#define glUniformMatrix4dv glad_glUniformMatrix4dv +typedef void (APIENTRYP PFNGLUNIFORMMATRIX2X3DVPROC)(GLint location, GLsizei count, GLboolean transpose, const GLdouble *value); +GLAPI PFNGLUNIFORMMATRIX2X3DVPROC glad_glUniformMatrix2x3dv; +#define glUniformMatrix2x3dv glad_glUniformMatrix2x3dv +typedef void (APIENTRYP PFNGLUNIFORMMATRIX2X4DVPROC)(GLint location, GLsizei count, GLboolean transpose, const GLdouble *value); +GLAPI PFNGLUNIFORMMATRIX2X4DVPROC glad_glUniformMatrix2x4dv; +#define glUniformMatrix2x4dv glad_glUniformMatrix2x4dv +typedef void (APIENTRYP PFNGLUNIFORMMATRIX3X2DVPROC)(GLint location, GLsizei count, GLboolean transpose, const GLdouble *value); +GLAPI PFNGLUNIFORMMATRIX3X2DVPROC glad_glUniformMatrix3x2dv; +#define glUniformMatrix3x2dv glad_glUniformMatrix3x2dv +typedef void (APIENTRYP PFNGLUNIFORMMATRIX3X4DVPROC)(GLint location, GLsizei count, GLboolean transpose, const GLdouble *value); +GLAPI PFNGLUNIFORMMATRIX3X4DVPROC glad_glUniformMatrix3x4dv; +#define glUniformMatrix3x4dv glad_glUniformMatrix3x4dv +typedef void (APIENTRYP PFNGLUNIFORMMATRIX4X2DVPROC)(GLint location, GLsizei count, GLboolean transpose, const GLdouble *value); +GLAPI PFNGLUNIFORMMATRIX4X2DVPROC glad_glUniformMatrix4x2dv; +#define glUniformMatrix4x2dv glad_glUniformMatrix4x2dv +typedef void (APIENTRYP PFNGLUNIFORMMATRIX4X3DVPROC)(GLint location, GLsizei count, GLboolean transpose, const GLdouble *value); +GLAPI PFNGLUNIFORMMATRIX4X3DVPROC glad_glUniformMatrix4x3dv; +#define glUniformMatrix4x3dv glad_glUniformMatrix4x3dv +typedef void (APIENTRYP PFNGLGETUNIFORMDVPROC)(GLuint program, GLint location, GLdouble *params); +GLAPI PFNGLGETUNIFORMDVPROC glad_glGetUniformdv; +#define glGetUniformdv glad_glGetUniformdv +typedef GLint (APIENTRYP PFNGLGETSUBROUTINEUNIFORMLOCATIONPROC)(GLuint program, GLenum shadertype, const GLchar *name); +GLAPI PFNGLGETSUBROUTINEUNIFORMLOCATIONPROC glad_glGetSubroutineUniformLocation; +#define glGetSubroutineUniformLocation glad_glGetSubroutineUniformLocation +typedef GLuint (APIENTRYP PFNGLGETSUBROUTINEINDEXPROC)(GLuint program, GLenum shadertype, const GLchar *name); +GLAPI PFNGLGETSUBROUTINEINDEXPROC glad_glGetSubroutineIndex; +#define glGetSubroutineIndex glad_glGetSubroutineIndex +typedef void (APIENTRYP PFNGLGETACTIVESUBROUTINEUNIFORMIVPROC)(GLuint program, GLenum shadertype, GLuint index, GLenum pname, GLint *values); +GLAPI PFNGLGETACTIVESUBROUTINEUNIFORMIVPROC glad_glGetActiveSubroutineUniformiv; +#define glGetActiveSubroutineUniformiv glad_glGetActiveSubroutineUniformiv +typedef void (APIENTRYP PFNGLGETACTIVESUBROUTINEUNIFORMNAMEPROC)(GLuint program, GLenum shadertype, GLuint index, GLsizei bufSize, GLsizei *length, GLchar *name); +GLAPI PFNGLGETACTIVESUBROUTINEUNIFORMNAMEPROC glad_glGetActiveSubroutineUniformName; +#define glGetActiveSubroutineUniformName glad_glGetActiveSubroutineUniformName +typedef void (APIENTRYP PFNGLGETACTIVESUBROUTINENAMEPROC)(GLuint program, GLenum shadertype, GLuint index, GLsizei bufSize, GLsizei *length, GLchar *name); +GLAPI PFNGLGETACTIVESUBROUTINENAMEPROC glad_glGetActiveSubroutineName; +#define glGetActiveSubroutineName glad_glGetActiveSubroutineName +typedef void (APIENTRYP PFNGLUNIFORMSUBROUTINESUIVPROC)(GLenum shadertype, GLsizei count, const GLuint *indices); +GLAPI PFNGLUNIFORMSUBROUTINESUIVPROC glad_glUniformSubroutinesuiv; +#define glUniformSubroutinesuiv glad_glUniformSubroutinesuiv +typedef void (APIENTRYP PFNGLGETUNIFORMSUBROUTINEUIVPROC)(GLenum shadertype, GLint location, GLuint *params); +GLAPI PFNGLGETUNIFORMSUBROUTINEUIVPROC glad_glGetUniformSubroutineuiv; +#define glGetUniformSubroutineuiv glad_glGetUniformSubroutineuiv +typedef void (APIENTRYP PFNGLGETPROGRAMSTAGEIVPROC)(GLuint program, GLenum shadertype, GLenum pname, GLint *values); +GLAPI PFNGLGETPROGRAMSTAGEIVPROC glad_glGetProgramStageiv; +#define glGetProgramStageiv glad_glGetProgramStageiv +typedef void (APIENTRYP PFNGLPATCHPARAMETERIPROC)(GLenum pname, GLint value); +GLAPI PFNGLPATCHPARAMETERIPROC glad_glPatchParameteri; +#define glPatchParameteri glad_glPatchParameteri +typedef void (APIENTRYP PFNGLPATCHPARAMETERFVPROC)(GLenum pname, const GLfloat *values); +GLAPI PFNGLPATCHPARAMETERFVPROC glad_glPatchParameterfv; +#define glPatchParameterfv glad_glPatchParameterfv +typedef void (APIENTRYP PFNGLBINDTRANSFORMFEEDBACKPROC)(GLenum target, GLuint id); +GLAPI PFNGLBINDTRANSFORMFEEDBACKPROC glad_glBindTransformFeedback; +#define glBindTransformFeedback glad_glBindTransformFeedback +typedef void (APIENTRYP PFNGLDELETETRANSFORMFEEDBACKSPROC)(GLsizei n, const GLuint *ids); +GLAPI PFNGLDELETETRANSFORMFEEDBACKSPROC glad_glDeleteTransformFeedbacks; +#define glDeleteTransformFeedbacks glad_glDeleteTransformFeedbacks +typedef void (APIENTRYP PFNGLGENTRANSFORMFEEDBACKSPROC)(GLsizei n, GLuint *ids); +GLAPI PFNGLGENTRANSFORMFEEDBACKSPROC glad_glGenTransformFeedbacks; +#define glGenTransformFeedbacks glad_glGenTransformFeedbacks +typedef GLboolean (APIENTRYP PFNGLISTRANSFORMFEEDBACKPROC)(GLuint id); +GLAPI PFNGLISTRANSFORMFEEDBACKPROC glad_glIsTransformFeedback; +#define glIsTransformFeedback glad_glIsTransformFeedback +typedef void (APIENTRYP PFNGLPAUSETRANSFORMFEEDBACKPROC)(void); +GLAPI PFNGLPAUSETRANSFORMFEEDBACKPROC glad_glPauseTransformFeedback; +#define glPauseTransformFeedback glad_glPauseTransformFeedback +typedef void (APIENTRYP PFNGLRESUMETRANSFORMFEEDBACKPROC)(void); +GLAPI PFNGLRESUMETRANSFORMFEEDBACKPROC glad_glResumeTransformFeedback; +#define glResumeTransformFeedback glad_glResumeTransformFeedback +typedef void (APIENTRYP PFNGLDRAWTRANSFORMFEEDBACKPROC)(GLenum mode, GLuint id); +GLAPI PFNGLDRAWTRANSFORMFEEDBACKPROC glad_glDrawTransformFeedback; +#define glDrawTransformFeedback glad_glDrawTransformFeedback +typedef void (APIENTRYP PFNGLDRAWTRANSFORMFEEDBACKSTREAMPROC)(GLenum mode, GLuint id, GLuint stream); +GLAPI PFNGLDRAWTRANSFORMFEEDBACKSTREAMPROC glad_glDrawTransformFeedbackStream; +#define glDrawTransformFeedbackStream glad_glDrawTransformFeedbackStream +typedef void (APIENTRYP PFNGLBEGINQUERYINDEXEDPROC)(GLenum target, GLuint index, GLuint id); +GLAPI PFNGLBEGINQUERYINDEXEDPROC glad_glBeginQueryIndexed; +#define glBeginQueryIndexed glad_glBeginQueryIndexed +typedef void (APIENTRYP PFNGLENDQUERYINDEXEDPROC)(GLenum target, GLuint index); +GLAPI PFNGLENDQUERYINDEXEDPROC glad_glEndQueryIndexed; +#define glEndQueryIndexed glad_glEndQueryIndexed +typedef void (APIENTRYP PFNGLGETQUERYINDEXEDIVPROC)(GLenum target, GLuint index, GLenum pname, GLint *params); +GLAPI PFNGLGETQUERYINDEXEDIVPROC glad_glGetQueryIndexediv; +#define glGetQueryIndexediv glad_glGetQueryIndexediv +#endif +#ifndef GL_VERSION_4_1 +#define GL_VERSION_4_1 1 +GLAPI int GLAD_GL_VERSION_4_1; +typedef void (APIENTRYP PFNGLRELEASESHADERCOMPILERPROC)(void); +GLAPI PFNGLRELEASESHADERCOMPILERPROC glad_glReleaseShaderCompiler; +#define glReleaseShaderCompiler glad_glReleaseShaderCompiler +typedef void (APIENTRYP PFNGLSHADERBINARYPROC)(GLsizei count, const GLuint *shaders, GLenum binaryFormat, const void *binary, GLsizei length); +GLAPI PFNGLSHADERBINARYPROC glad_glShaderBinary; +#define glShaderBinary glad_glShaderBinary +typedef void (APIENTRYP PFNGLGETSHADERPRECISIONFORMATPROC)(GLenum shadertype, GLenum precisiontype, GLint *range, GLint *precision); +GLAPI PFNGLGETSHADERPRECISIONFORMATPROC glad_glGetShaderPrecisionFormat; +#define glGetShaderPrecisionFormat glad_glGetShaderPrecisionFormat +typedef void (APIENTRYP PFNGLDEPTHRANGEFPROC)(GLfloat n, GLfloat f); +GLAPI PFNGLDEPTHRANGEFPROC glad_glDepthRangef; +#define glDepthRangef glad_glDepthRangef +typedef void (APIENTRYP PFNGLCLEARDEPTHFPROC)(GLfloat d); +GLAPI PFNGLCLEARDEPTHFPROC glad_glClearDepthf; +#define glClearDepthf glad_glClearDepthf +typedef void (APIENTRYP PFNGLGETPROGRAMBINARYPROC)(GLuint program, GLsizei bufSize, GLsizei *length, GLenum *binaryFormat, void *binary); +GLAPI PFNGLGETPROGRAMBINARYPROC glad_glGetProgramBinary; +#define glGetProgramBinary glad_glGetProgramBinary +typedef void (APIENTRYP PFNGLPROGRAMBINARYPROC)(GLuint program, GLenum binaryFormat, const void *binary, GLsizei length); +GLAPI PFNGLPROGRAMBINARYPROC glad_glProgramBinary; +#define glProgramBinary glad_glProgramBinary +typedef void (APIENTRYP PFNGLPROGRAMPARAMETERIPROC)(GLuint program, GLenum pname, GLint value); +GLAPI PFNGLPROGRAMPARAMETERIPROC glad_glProgramParameteri; +#define glProgramParameteri glad_glProgramParameteri +typedef void (APIENTRYP PFNGLUSEPROGRAMSTAGESPROC)(GLuint pipeline, GLbitfield stages, GLuint program); +GLAPI PFNGLUSEPROGRAMSTAGESPROC glad_glUseProgramStages; +#define glUseProgramStages glad_glUseProgramStages +typedef void (APIENTRYP PFNGLACTIVESHADERPROGRAMPROC)(GLuint pipeline, GLuint program); +GLAPI PFNGLACTIVESHADERPROGRAMPROC glad_glActiveShaderProgram; +#define glActiveShaderProgram glad_glActiveShaderProgram +typedef GLuint (APIENTRYP PFNGLCREATESHADERPROGRAMVPROC)(GLenum type, GLsizei count, const GLchar *const*strings); +GLAPI PFNGLCREATESHADERPROGRAMVPROC glad_glCreateShaderProgramv; +#define glCreateShaderProgramv glad_glCreateShaderProgramv +typedef void (APIENTRYP PFNGLBINDPROGRAMPIPELINEPROC)(GLuint pipeline); +GLAPI PFNGLBINDPROGRAMPIPELINEPROC glad_glBindProgramPipeline; +#define glBindProgramPipeline glad_glBindProgramPipeline +typedef void (APIENTRYP PFNGLDELETEPROGRAMPIPELINESPROC)(GLsizei n, const GLuint *pipelines); +GLAPI PFNGLDELETEPROGRAMPIPELINESPROC glad_glDeleteProgramPipelines; +#define glDeleteProgramPipelines glad_glDeleteProgramPipelines +typedef void (APIENTRYP PFNGLGENPROGRAMPIPELINESPROC)(GLsizei n, GLuint *pipelines); +GLAPI PFNGLGENPROGRAMPIPELINESPROC glad_glGenProgramPipelines; +#define glGenProgramPipelines glad_glGenProgramPipelines +typedef GLboolean (APIENTRYP PFNGLISPROGRAMPIPELINEPROC)(GLuint pipeline); +GLAPI PFNGLISPROGRAMPIPELINEPROC glad_glIsProgramPipeline; +#define glIsProgramPipeline glad_glIsProgramPipeline +typedef void (APIENTRYP PFNGLGETPROGRAMPIPELINEIVPROC)(GLuint pipeline, GLenum pname, GLint *params); +GLAPI PFNGLGETPROGRAMPIPELINEIVPROC glad_glGetProgramPipelineiv; +#define glGetProgramPipelineiv glad_glGetProgramPipelineiv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1IPROC)(GLuint program, GLint location, GLint v0); +GLAPI PFNGLPROGRAMUNIFORM1IPROC glad_glProgramUniform1i; +#define glProgramUniform1i glad_glProgramUniform1i +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1IVPROC)(GLuint program, GLint location, GLsizei count, const GLint *value); +GLAPI PFNGLPROGRAMUNIFORM1IVPROC glad_glProgramUniform1iv; +#define glProgramUniform1iv glad_glProgramUniform1iv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1FPROC)(GLuint program, GLint location, GLfloat v0); +GLAPI PFNGLPROGRAMUNIFORM1FPROC glad_glProgramUniform1f; +#define glProgramUniform1f glad_glProgramUniform1f +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1FVPROC)(GLuint program, GLint location, GLsizei count, const GLfloat *value); +GLAPI PFNGLPROGRAMUNIFORM1FVPROC glad_glProgramUniform1fv; +#define glProgramUniform1fv glad_glProgramUniform1fv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1DPROC)(GLuint program, GLint location, GLdouble v0); +GLAPI PFNGLPROGRAMUNIFORM1DPROC glad_glProgramUniform1d; +#define glProgramUniform1d glad_glProgramUniform1d +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1DVPROC)(GLuint program, GLint location, GLsizei count, const GLdouble *value); +GLAPI PFNGLPROGRAMUNIFORM1DVPROC glad_glProgramUniform1dv; +#define glProgramUniform1dv glad_glProgramUniform1dv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1UIPROC)(GLuint program, GLint location, GLuint v0); +GLAPI PFNGLPROGRAMUNIFORM1UIPROC glad_glProgramUniform1ui; +#define glProgramUniform1ui glad_glProgramUniform1ui +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1UIVPROC)(GLuint program, GLint location, GLsizei count, const GLuint *value); +GLAPI PFNGLPROGRAMUNIFORM1UIVPROC glad_glProgramUniform1uiv; +#define glProgramUniform1uiv glad_glProgramUniform1uiv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2IPROC)(GLuint program, GLint location, GLint v0, GLint v1); +GLAPI PFNGLPROGRAMUNIFORM2IPROC glad_glProgramUniform2i; +#define glProgramUniform2i glad_glProgramUniform2i +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2IVPROC)(GLuint program, GLint location, GLsizei count, const GLint *value); +GLAPI PFNGLPROGRAMUNIFORM2IVPROC glad_glProgramUniform2iv; +#define glProgramUniform2iv glad_glProgramUniform2iv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2FPROC)(GLuint program, GLint location, GLfloat v0, GLfloat v1); +GLAPI PFNGLPROGRAMUNIFORM2FPROC glad_glProgramUniform2f; +#define glProgramUniform2f glad_glProgramUniform2f +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2FVPROC)(GLuint program, GLint location, GLsizei count, const GLfloat *value); +GLAPI PFNGLPROGRAMUNIFORM2FVPROC glad_glProgramUniform2fv; +#define glProgramUniform2fv glad_glProgramUniform2fv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2DPROC)(GLuint program, GLint location, GLdouble v0, GLdouble v1); +GLAPI PFNGLPROGRAMUNIFORM2DPROC glad_glProgramUniform2d; +#define glProgramUniform2d glad_glProgramUniform2d +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2DVPROC)(GLuint program, GLint location, GLsizei count, const GLdouble *value); +GLAPI PFNGLPROGRAMUNIFORM2DVPROC glad_glProgramUniform2dv; +#define glProgramUniform2dv glad_glProgramUniform2dv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2UIPROC)(GLuint program, GLint location, GLuint v0, GLuint v1); +GLAPI PFNGLPROGRAMUNIFORM2UIPROC glad_glProgramUniform2ui; +#define glProgramUniform2ui glad_glProgramUniform2ui +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2UIVPROC)(GLuint program, GLint location, GLsizei count, const GLuint *value); +GLAPI PFNGLPROGRAMUNIFORM2UIVPROC glad_glProgramUniform2uiv; +#define glProgramUniform2uiv glad_glProgramUniform2uiv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3IPROC)(GLuint program, GLint location, GLint v0, GLint v1, GLint v2); +GLAPI PFNGLPROGRAMUNIFORM3IPROC glad_glProgramUniform3i; +#define glProgramUniform3i glad_glProgramUniform3i +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3IVPROC)(GLuint program, GLint location, GLsizei count, const GLint *value); +GLAPI PFNGLPROGRAMUNIFORM3IVPROC glad_glProgramUniform3iv; +#define glProgramUniform3iv glad_glProgramUniform3iv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3FPROC)(GLuint program, GLint location, GLfloat v0, GLfloat v1, GLfloat v2); +GLAPI PFNGLPROGRAMUNIFORM3FPROC glad_glProgramUniform3f; +#define glProgramUniform3f glad_glProgramUniform3f +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3FVPROC)(GLuint program, GLint location, GLsizei count, const GLfloat *value); +GLAPI PFNGLPROGRAMUNIFORM3FVPROC glad_glProgramUniform3fv; +#define glProgramUniform3fv glad_glProgramUniform3fv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3DPROC)(GLuint program, GLint location, GLdouble v0, GLdouble v1, GLdouble v2); +GLAPI PFNGLPROGRAMUNIFORM3DPROC glad_glProgramUniform3d; +#define glProgramUniform3d glad_glProgramUniform3d +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3DVPROC)(GLuint program, GLint location, GLsizei count, const GLdouble *value); +GLAPI PFNGLPROGRAMUNIFORM3DVPROC glad_glProgramUniform3dv; +#define glProgramUniform3dv glad_glProgramUniform3dv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3UIPROC)(GLuint program, GLint location, GLuint v0, GLuint v1, GLuint v2); +GLAPI PFNGLPROGRAMUNIFORM3UIPROC glad_glProgramUniform3ui; +#define glProgramUniform3ui glad_glProgramUniform3ui +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3UIVPROC)(GLuint program, GLint location, GLsizei count, const GLuint *value); +GLAPI PFNGLPROGRAMUNIFORM3UIVPROC glad_glProgramUniform3uiv; +#define glProgramUniform3uiv glad_glProgramUniform3uiv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4IPROC)(GLuint program, GLint location, GLint v0, GLint v1, GLint v2, GLint v3); +GLAPI PFNGLPROGRAMUNIFORM4IPROC glad_glProgramUniform4i; +#define glProgramUniform4i glad_glProgramUniform4i +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4IVPROC)(GLuint program, GLint location, GLsizei count, const GLint *value); +GLAPI PFNGLPROGRAMUNIFORM4IVPROC glad_glProgramUniform4iv; +#define glProgramUniform4iv glad_glProgramUniform4iv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4FPROC)(GLuint program, GLint location, GLfloat v0, GLfloat v1, GLfloat v2, GLfloat v3); +GLAPI PFNGLPROGRAMUNIFORM4FPROC glad_glProgramUniform4f; +#define glProgramUniform4f glad_glProgramUniform4f +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4FVPROC)(GLuint program, GLint location, GLsizei count, const GLfloat *value); +GLAPI PFNGLPROGRAMUNIFORM4FVPROC glad_glProgramUniform4fv; +#define glProgramUniform4fv glad_glProgramUniform4fv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4DPROC)(GLuint program, GLint location, GLdouble v0, GLdouble v1, GLdouble v2, GLdouble v3); +GLAPI PFNGLPROGRAMUNIFORM4DPROC glad_glProgramUniform4d; +#define glProgramUniform4d glad_glProgramUniform4d +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4DVPROC)(GLuint program, GLint location, GLsizei count, const GLdouble *value); +GLAPI PFNGLPROGRAMUNIFORM4DVPROC glad_glProgramUniform4dv; +#define glProgramUniform4dv glad_glProgramUniform4dv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4UIPROC)(GLuint program, GLint location, GLuint v0, GLuint v1, GLuint v2, GLuint v3); +GLAPI PFNGLPROGRAMUNIFORM4UIPROC glad_glProgramUniform4ui; +#define glProgramUniform4ui glad_glProgramUniform4ui +typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4UIVPROC)(GLuint program, GLint location, GLsizei count, const GLuint *value); +GLAPI PFNGLPROGRAMUNIFORM4UIVPROC glad_glProgramUniform4uiv; +#define glProgramUniform4uiv glad_glProgramUniform4uiv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX2FVPROC)(GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value); +GLAPI PFNGLPROGRAMUNIFORMMATRIX2FVPROC glad_glProgramUniformMatrix2fv; +#define glProgramUniformMatrix2fv glad_glProgramUniformMatrix2fv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX3FVPROC)(GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value); +GLAPI PFNGLPROGRAMUNIFORMMATRIX3FVPROC glad_glProgramUniformMatrix3fv; +#define glProgramUniformMatrix3fv glad_glProgramUniformMatrix3fv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX4FVPROC)(GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value); +GLAPI PFNGLPROGRAMUNIFORMMATRIX4FVPROC glad_glProgramUniformMatrix4fv; +#define glProgramUniformMatrix4fv glad_glProgramUniformMatrix4fv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX2DVPROC)(GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value); +GLAPI PFNGLPROGRAMUNIFORMMATRIX2DVPROC glad_glProgramUniformMatrix2dv; +#define glProgramUniformMatrix2dv glad_glProgramUniformMatrix2dv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX3DVPROC)(GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value); +GLAPI PFNGLPROGRAMUNIFORMMATRIX3DVPROC glad_glProgramUniformMatrix3dv; +#define glProgramUniformMatrix3dv glad_glProgramUniformMatrix3dv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX4DVPROC)(GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value); +GLAPI PFNGLPROGRAMUNIFORMMATRIX4DVPROC glad_glProgramUniformMatrix4dv; +#define glProgramUniformMatrix4dv glad_glProgramUniformMatrix4dv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX2X3FVPROC)(GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value); +GLAPI PFNGLPROGRAMUNIFORMMATRIX2X3FVPROC glad_glProgramUniformMatrix2x3fv; +#define glProgramUniformMatrix2x3fv glad_glProgramUniformMatrix2x3fv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX3X2FVPROC)(GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value); +GLAPI PFNGLPROGRAMUNIFORMMATRIX3X2FVPROC glad_glProgramUniformMatrix3x2fv; +#define glProgramUniformMatrix3x2fv glad_glProgramUniformMatrix3x2fv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX2X4FVPROC)(GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value); +GLAPI PFNGLPROGRAMUNIFORMMATRIX2X4FVPROC glad_glProgramUniformMatrix2x4fv; +#define glProgramUniformMatrix2x4fv glad_glProgramUniformMatrix2x4fv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX4X2FVPROC)(GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value); +GLAPI PFNGLPROGRAMUNIFORMMATRIX4X2FVPROC glad_glProgramUniformMatrix4x2fv; +#define glProgramUniformMatrix4x2fv glad_glProgramUniformMatrix4x2fv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX3X4FVPROC)(GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value); +GLAPI PFNGLPROGRAMUNIFORMMATRIX3X4FVPROC glad_glProgramUniformMatrix3x4fv; +#define glProgramUniformMatrix3x4fv glad_glProgramUniformMatrix3x4fv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX4X3FVPROC)(GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value); +GLAPI PFNGLPROGRAMUNIFORMMATRIX4X3FVPROC glad_glProgramUniformMatrix4x3fv; +#define glProgramUniformMatrix4x3fv glad_glProgramUniformMatrix4x3fv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX2X3DVPROC)(GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value); +GLAPI PFNGLPROGRAMUNIFORMMATRIX2X3DVPROC glad_glProgramUniformMatrix2x3dv; +#define glProgramUniformMatrix2x3dv glad_glProgramUniformMatrix2x3dv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX3X2DVPROC)(GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value); +GLAPI PFNGLPROGRAMUNIFORMMATRIX3X2DVPROC glad_glProgramUniformMatrix3x2dv; +#define glProgramUniformMatrix3x2dv glad_glProgramUniformMatrix3x2dv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX2X4DVPROC)(GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value); +GLAPI PFNGLPROGRAMUNIFORMMATRIX2X4DVPROC glad_glProgramUniformMatrix2x4dv; +#define glProgramUniformMatrix2x4dv glad_glProgramUniformMatrix2x4dv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX4X2DVPROC)(GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value); +GLAPI PFNGLPROGRAMUNIFORMMATRIX4X2DVPROC glad_glProgramUniformMatrix4x2dv; +#define glProgramUniformMatrix4x2dv glad_glProgramUniformMatrix4x2dv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX3X4DVPROC)(GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value); +GLAPI PFNGLPROGRAMUNIFORMMATRIX3X4DVPROC glad_glProgramUniformMatrix3x4dv; +#define glProgramUniformMatrix3x4dv glad_glProgramUniformMatrix3x4dv +typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX4X3DVPROC)(GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value); +GLAPI PFNGLPROGRAMUNIFORMMATRIX4X3DVPROC glad_glProgramUniformMatrix4x3dv; +#define glProgramUniformMatrix4x3dv glad_glProgramUniformMatrix4x3dv +typedef void (APIENTRYP PFNGLVALIDATEPROGRAMPIPELINEPROC)(GLuint pipeline); +GLAPI PFNGLVALIDATEPROGRAMPIPELINEPROC glad_glValidateProgramPipeline; +#define glValidateProgramPipeline glad_glValidateProgramPipeline +typedef void (APIENTRYP PFNGLGETPROGRAMPIPELINEINFOLOGPROC)(GLuint pipeline, GLsizei bufSize, GLsizei *length, GLchar *infoLog); +GLAPI PFNGLGETPROGRAMPIPELINEINFOLOGPROC glad_glGetProgramPipelineInfoLog; +#define glGetProgramPipelineInfoLog glad_glGetProgramPipelineInfoLog +typedef void (APIENTRYP PFNGLVERTEXATTRIBL1DPROC)(GLuint index, GLdouble x); +GLAPI PFNGLVERTEXATTRIBL1DPROC glad_glVertexAttribL1d; +#define glVertexAttribL1d glad_glVertexAttribL1d +typedef void (APIENTRYP PFNGLVERTEXATTRIBL2DPROC)(GLuint index, GLdouble x, GLdouble y); +GLAPI PFNGLVERTEXATTRIBL2DPROC glad_glVertexAttribL2d; +#define glVertexAttribL2d glad_glVertexAttribL2d +typedef void (APIENTRYP PFNGLVERTEXATTRIBL3DPROC)(GLuint index, GLdouble x, GLdouble y, GLdouble z); +GLAPI PFNGLVERTEXATTRIBL3DPROC glad_glVertexAttribL3d; +#define glVertexAttribL3d glad_glVertexAttribL3d +typedef void (APIENTRYP PFNGLVERTEXATTRIBL4DPROC)(GLuint index, GLdouble x, GLdouble y, GLdouble z, GLdouble w); +GLAPI PFNGLVERTEXATTRIBL4DPROC glad_glVertexAttribL4d; +#define glVertexAttribL4d glad_glVertexAttribL4d +typedef void (APIENTRYP PFNGLVERTEXATTRIBL1DVPROC)(GLuint index, const GLdouble *v); +GLAPI PFNGLVERTEXATTRIBL1DVPROC glad_glVertexAttribL1dv; +#define glVertexAttribL1dv glad_glVertexAttribL1dv +typedef void (APIENTRYP PFNGLVERTEXATTRIBL2DVPROC)(GLuint index, const GLdouble *v); +GLAPI PFNGLVERTEXATTRIBL2DVPROC glad_glVertexAttribL2dv; +#define glVertexAttribL2dv glad_glVertexAttribL2dv +typedef void (APIENTRYP PFNGLVERTEXATTRIBL3DVPROC)(GLuint index, const GLdouble *v); +GLAPI PFNGLVERTEXATTRIBL3DVPROC glad_glVertexAttribL3dv; +#define glVertexAttribL3dv glad_glVertexAttribL3dv +typedef void (APIENTRYP PFNGLVERTEXATTRIBL4DVPROC)(GLuint index, const GLdouble *v); +GLAPI PFNGLVERTEXATTRIBL4DVPROC glad_glVertexAttribL4dv; +#define glVertexAttribL4dv glad_glVertexAttribL4dv +typedef void (APIENTRYP PFNGLVERTEXATTRIBLPOINTERPROC)(GLuint index, GLint size, GLenum type, GLsizei stride, const void *pointer); +GLAPI PFNGLVERTEXATTRIBLPOINTERPROC glad_glVertexAttribLPointer; +#define glVertexAttribLPointer glad_glVertexAttribLPointer +typedef void (APIENTRYP PFNGLGETVERTEXATTRIBLDVPROC)(GLuint index, GLenum pname, GLdouble *params); +GLAPI PFNGLGETVERTEXATTRIBLDVPROC glad_glGetVertexAttribLdv; +#define glGetVertexAttribLdv glad_glGetVertexAttribLdv +typedef void (APIENTRYP PFNGLVIEWPORTARRAYVPROC)(GLuint first, GLsizei count, const GLfloat *v); +GLAPI PFNGLVIEWPORTARRAYVPROC glad_glViewportArrayv; +#define glViewportArrayv glad_glViewportArrayv +typedef void (APIENTRYP PFNGLVIEWPORTINDEXEDFPROC)(GLuint index, GLfloat x, GLfloat y, GLfloat w, GLfloat h); +GLAPI PFNGLVIEWPORTINDEXEDFPROC glad_glViewportIndexedf; +#define glViewportIndexedf glad_glViewportIndexedf +typedef void (APIENTRYP PFNGLVIEWPORTINDEXEDFVPROC)(GLuint index, const GLfloat *v); +GLAPI PFNGLVIEWPORTINDEXEDFVPROC glad_glViewportIndexedfv; +#define glViewportIndexedfv glad_glViewportIndexedfv +typedef void (APIENTRYP PFNGLSCISSORARRAYVPROC)(GLuint first, GLsizei count, const GLint *v); +GLAPI PFNGLSCISSORARRAYVPROC glad_glScissorArrayv; +#define glScissorArrayv glad_glScissorArrayv +typedef void (APIENTRYP PFNGLSCISSORINDEXEDPROC)(GLuint index, GLint left, GLint bottom, GLsizei width, GLsizei height); +GLAPI PFNGLSCISSORINDEXEDPROC glad_glScissorIndexed; +#define glScissorIndexed glad_glScissorIndexed +typedef void (APIENTRYP PFNGLSCISSORINDEXEDVPROC)(GLuint index, const GLint *v); +GLAPI PFNGLSCISSORINDEXEDVPROC glad_glScissorIndexedv; +#define glScissorIndexedv glad_glScissorIndexedv +typedef void (APIENTRYP PFNGLDEPTHRANGEARRAYVPROC)(GLuint first, GLsizei count, const GLdouble *v); +GLAPI PFNGLDEPTHRANGEARRAYVPROC glad_glDepthRangeArrayv; +#define glDepthRangeArrayv glad_glDepthRangeArrayv +typedef void (APIENTRYP PFNGLDEPTHRANGEINDEXEDPROC)(GLuint index, GLdouble n, GLdouble f); +GLAPI PFNGLDEPTHRANGEINDEXEDPROC glad_glDepthRangeIndexed; +#define glDepthRangeIndexed glad_glDepthRangeIndexed +typedef void (APIENTRYP PFNGLGETFLOATI_VPROC)(GLenum target, GLuint index, GLfloat *data); +GLAPI PFNGLGETFLOATI_VPROC glad_glGetFloati_v; +#define glGetFloati_v glad_glGetFloati_v +typedef void (APIENTRYP PFNGLGETDOUBLEI_VPROC)(GLenum target, GLuint index, GLdouble *data); +GLAPI PFNGLGETDOUBLEI_VPROC glad_glGetDoublei_v; +#define glGetDoublei_v glad_glGetDoublei_v +#endif + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/extern/glad/src/glad.c b/extern/glad/src/glad.c new file mode 100644 index 0000000..aeb93ce --- /dev/null +++ b/extern/glad/src/glad.c @@ -0,0 +1,2121 @@ +/* + + OpenGL loader generated by glad 0.1.36 on Sat Apr 12 17:00:07 2025. + + Language/Generator: C/C++ + Specification: gl + APIs: gl=4.1 + Profile: compatibility + Extensions: + + Loader: True + Local files: False + Omit khrplatform: False + Reproducible: False + + Commandline: + --profile="compatibility" --api="gl=4.1" --generator="c" --spec="gl" --extensions="" + Online: + https://glad.dav1d.de/#profile=compatibility&language=c&specification=gl&loader=on&api=gl%3D4.1 +*/ + +#include +#include +#include +#include + +static void* get_proc(const char *namez); + +#if defined(_WIN32) || defined(__CYGWIN__) +#ifndef _WINDOWS_ +#undef APIENTRY +#endif +#include +static HMODULE libGL; + +typedef void* (APIENTRYP PFNWGLGETPROCADDRESSPROC_PRIVATE)(const char*); +static PFNWGLGETPROCADDRESSPROC_PRIVATE gladGetProcAddressPtr; + +#ifdef _MSC_VER +#ifdef __has_include + #if __has_include() + #define HAVE_WINAPIFAMILY 1 + #endif +#elif _MSC_VER >= 1700 && !_USING_V110_SDK71_ + #define HAVE_WINAPIFAMILY 1 +#endif +#endif + +#ifdef HAVE_WINAPIFAMILY + #include + #if !WINAPI_FAMILY_PARTITION(WINAPI_PARTITION_DESKTOP) && WINAPI_FAMILY_PARTITION(WINAPI_PARTITION_APP) + #define IS_UWP 1 + #endif +#endif + +static +int open_gl(void) { +#ifndef IS_UWP + libGL = LoadLibraryW(L"opengl32.dll"); + if(libGL != NULL) { + void (* tmp)(void); + tmp = (void(*)(void)) GetProcAddress(libGL, "wglGetProcAddress"); + gladGetProcAddressPtr = (PFNWGLGETPROCADDRESSPROC_PRIVATE) tmp; + return gladGetProcAddressPtr != NULL; + } +#endif + + return 0; +} + +static +void close_gl(void) { + if(libGL != NULL) { + FreeLibrary((HMODULE) libGL); + libGL = NULL; + } +} +#else +#include +static void* libGL; + +#if !defined(__APPLE__) && !defined(__HAIKU__) +typedef void* (APIENTRYP PFNGLXGETPROCADDRESSPROC_PRIVATE)(const char*); +static PFNGLXGETPROCADDRESSPROC_PRIVATE gladGetProcAddressPtr; +#endif + +static +int open_gl(void) { +#ifdef __APPLE__ + static const char *NAMES[] = { + "../Frameworks/OpenGL.framework/OpenGL", + "/Library/Frameworks/OpenGL.framework/OpenGL", + "/System/Library/Frameworks/OpenGL.framework/OpenGL", + "/System/Library/Frameworks/OpenGL.framework/Versions/Current/OpenGL" + }; +#else + static const char *NAMES[] = {"libGL.so.1", "libGL.so"}; +#endif + + unsigned int index = 0; + for(index = 0; index < (sizeof(NAMES) / sizeof(NAMES[0])); index++) { + libGL = dlopen(NAMES[index], RTLD_NOW | RTLD_GLOBAL); + + if(libGL != NULL) { +#if defined(__APPLE__) || defined(__HAIKU__) + return 1; +#else + gladGetProcAddressPtr = (PFNGLXGETPROCADDRESSPROC_PRIVATE)dlsym(libGL, + "glXGetProcAddressARB"); + return gladGetProcAddressPtr != NULL; +#endif + } + } + + return 0; +} + +static +void close_gl(void) { + if(libGL != NULL) { + dlclose(libGL); + libGL = NULL; + } +} +#endif + +static +void* get_proc(const char *namez) { + void* result = NULL; + if(libGL == NULL) return NULL; + +#if !defined(__APPLE__) && !defined(__HAIKU__) + if(gladGetProcAddressPtr != NULL) { + result = gladGetProcAddressPtr(namez); + } +#endif + if(result == NULL) { +#if defined(_WIN32) || defined(__CYGWIN__) + result = (void*)GetProcAddress((HMODULE) libGL, namez); +#else + result = dlsym(libGL, namez); +#endif + } + + return result; +} + +int gladLoadGL(void) { + int status = 0; + + if(open_gl()) { + status = gladLoadGLLoader(&get_proc); + close_gl(); + } + + return status; +} + +struct gladGLversionStruct GLVersion = { 0, 0 }; + +#if defined(GL_ES_VERSION_3_0) || defined(GL_VERSION_3_0) +#define _GLAD_IS_SOME_NEW_VERSION 1 +#endif + +static int max_loaded_major; +static int max_loaded_minor; + +static const char *exts = NULL; +static int num_exts_i = 0; +static char **exts_i = NULL; + +static int get_exts(void) { +#ifdef _GLAD_IS_SOME_NEW_VERSION + if(max_loaded_major < 3) { +#endif + exts = (const char *)glGetString(GL_EXTENSIONS); +#ifdef _GLAD_IS_SOME_NEW_VERSION + } else { + int index; + + num_exts_i = 0; + glGetIntegerv(GL_NUM_EXTENSIONS, &num_exts_i); + if (num_exts_i > 0) { + exts_i = (char **)malloc((size_t)num_exts_i * (sizeof *exts_i)); + } + + if (exts_i == NULL) { + return 0; + } + + for(index = 0; index < num_exts_i; index++) { + const char *gl_str_tmp = (const char*)glGetStringi(GL_EXTENSIONS, index); + size_t len = strlen(gl_str_tmp); + + char *local_str = (char*)malloc((len+1) * sizeof(char)); + if(local_str != NULL) { + memcpy(local_str, gl_str_tmp, (len+1) * sizeof(char)); + } + exts_i[index] = local_str; + } + } +#endif + return 1; +} + +static void free_exts(void) { + if (exts_i != NULL) { + int index; + for(index = 0; index < num_exts_i; index++) { + free((char *)exts_i[index]); + } + free((void *)exts_i); + exts_i = NULL; + } +} + +static int has_ext(const char *ext) { +#ifdef _GLAD_IS_SOME_NEW_VERSION + if(max_loaded_major < 3) { +#endif + const char *extensions; + const char *loc; + const char *terminator; + extensions = exts; + if(extensions == NULL || ext == NULL) { + return 0; + } + + while(1) { + loc = strstr(extensions, ext); + if(loc == NULL) { + return 0; + } + + terminator = loc + strlen(ext); + if((loc == extensions || *(loc - 1) == ' ') && + (*terminator == ' ' || *terminator == '\0')) { + return 1; + } + extensions = terminator; + } +#ifdef _GLAD_IS_SOME_NEW_VERSION + } else { + int index; + if(exts_i == NULL) return 0; + for(index = 0; index < num_exts_i; index++) { + const char *e = exts_i[index]; + + if(exts_i[index] != NULL && strcmp(e, ext) == 0) { + return 1; + } + } + } +#endif + + return 0; +} +int GLAD_GL_VERSION_1_0 = 0; +int GLAD_GL_VERSION_1_1 = 0; +int GLAD_GL_VERSION_1_2 = 0; +int GLAD_GL_VERSION_1_3 = 0; +int GLAD_GL_VERSION_1_4 = 0; +int GLAD_GL_VERSION_1_5 = 0; +int GLAD_GL_VERSION_2_0 = 0; +int GLAD_GL_VERSION_2_1 = 0; +int GLAD_GL_VERSION_3_0 = 0; +int GLAD_GL_VERSION_3_1 = 0; +int GLAD_GL_VERSION_3_2 = 0; +int GLAD_GL_VERSION_3_3 = 0; +int GLAD_GL_VERSION_4_0 = 0; +int GLAD_GL_VERSION_4_1 = 0; +PFNGLACCUMPROC glad_glAccum = NULL; +PFNGLACTIVESHADERPROGRAMPROC glad_glActiveShaderProgram = NULL; +PFNGLACTIVETEXTUREPROC glad_glActiveTexture = NULL; +PFNGLALPHAFUNCPROC glad_glAlphaFunc = NULL; +PFNGLARETEXTURESRESIDENTPROC glad_glAreTexturesResident = NULL; +PFNGLARRAYELEMENTPROC glad_glArrayElement = NULL; +PFNGLATTACHSHADERPROC glad_glAttachShader = NULL; +PFNGLBEGINPROC glad_glBegin = NULL; +PFNGLBEGINCONDITIONALRENDERPROC glad_glBeginConditionalRender = NULL; +PFNGLBEGINQUERYPROC glad_glBeginQuery = NULL; +PFNGLBEGINQUERYINDEXEDPROC glad_glBeginQueryIndexed = NULL; +PFNGLBEGINTRANSFORMFEEDBACKPROC glad_glBeginTransformFeedback = NULL; +PFNGLBINDATTRIBLOCATIONPROC glad_glBindAttribLocation = NULL; +PFNGLBINDBUFFERPROC glad_glBindBuffer = NULL; +PFNGLBINDBUFFERBASEPROC glad_glBindBufferBase = NULL; +PFNGLBINDBUFFERRANGEPROC glad_glBindBufferRange = NULL; +PFNGLBINDFRAGDATALOCATIONPROC glad_glBindFragDataLocation = NULL; +PFNGLBINDFRAGDATALOCATIONINDEXEDPROC glad_glBindFragDataLocationIndexed = NULL; +PFNGLBINDFRAMEBUFFERPROC glad_glBindFramebuffer = NULL; +PFNGLBINDPROGRAMPIPELINEPROC glad_glBindProgramPipeline = NULL; +PFNGLBINDRENDERBUFFERPROC glad_glBindRenderbuffer = NULL; +PFNGLBINDSAMPLERPROC glad_glBindSampler = NULL; +PFNGLBINDTEXTUREPROC glad_glBindTexture = NULL; +PFNGLBINDTRANSFORMFEEDBACKPROC glad_glBindTransformFeedback = NULL; +PFNGLBINDVERTEXARRAYPROC glad_glBindVertexArray = NULL; +PFNGLBITMAPPROC glad_glBitmap = NULL; +PFNGLBLENDCOLORPROC glad_glBlendColor = NULL; +PFNGLBLENDEQUATIONPROC glad_glBlendEquation = NULL; +PFNGLBLENDEQUATIONSEPARATEPROC glad_glBlendEquationSeparate = NULL; +PFNGLBLENDEQUATIONSEPARATEIPROC glad_glBlendEquationSeparatei = NULL; +PFNGLBLENDEQUATIONIPROC glad_glBlendEquationi = NULL; +PFNGLBLENDFUNCPROC glad_glBlendFunc = NULL; +PFNGLBLENDFUNCSEPARATEPROC glad_glBlendFuncSeparate = NULL; +PFNGLBLENDFUNCSEPARATEIPROC glad_glBlendFuncSeparatei = NULL; +PFNGLBLENDFUNCIPROC glad_glBlendFunci = NULL; +PFNGLBLITFRAMEBUFFERPROC glad_glBlitFramebuffer = NULL; +PFNGLBUFFERDATAPROC glad_glBufferData = NULL; +PFNGLBUFFERSUBDATAPROC glad_glBufferSubData = NULL; +PFNGLCALLLISTPROC glad_glCallList = NULL; +PFNGLCALLLISTSPROC glad_glCallLists = NULL; +PFNGLCHECKFRAMEBUFFERSTATUSPROC glad_glCheckFramebufferStatus = NULL; +PFNGLCLAMPCOLORPROC glad_glClampColor = NULL; +PFNGLCLEARPROC glad_glClear = NULL; +PFNGLCLEARACCUMPROC glad_glClearAccum = NULL; +PFNGLCLEARBUFFERFIPROC glad_glClearBufferfi = NULL; +PFNGLCLEARBUFFERFVPROC glad_glClearBufferfv = NULL; +PFNGLCLEARBUFFERIVPROC glad_glClearBufferiv = NULL; +PFNGLCLEARBUFFERUIVPROC glad_glClearBufferuiv = NULL; +PFNGLCLEARCOLORPROC glad_glClearColor = NULL; +PFNGLCLEARDEPTHPROC glad_glClearDepth = NULL; +PFNGLCLEARDEPTHFPROC glad_glClearDepthf = NULL; +PFNGLCLEARINDEXPROC glad_glClearIndex = NULL; +PFNGLCLEARSTENCILPROC glad_glClearStencil = NULL; +PFNGLCLIENTACTIVETEXTUREPROC glad_glClientActiveTexture = NULL; +PFNGLCLIENTWAITSYNCPROC glad_glClientWaitSync = NULL; +PFNGLCLIPPLANEPROC glad_glClipPlane = NULL; +PFNGLCOLOR3BPROC glad_glColor3b = NULL; +PFNGLCOLOR3BVPROC glad_glColor3bv = NULL; +PFNGLCOLOR3DPROC glad_glColor3d = NULL; +PFNGLCOLOR3DVPROC glad_glColor3dv = NULL; +PFNGLCOLOR3FPROC glad_glColor3f = NULL; +PFNGLCOLOR3FVPROC glad_glColor3fv = NULL; +PFNGLCOLOR3IPROC glad_glColor3i = NULL; +PFNGLCOLOR3IVPROC glad_glColor3iv = NULL; +PFNGLCOLOR3SPROC glad_glColor3s = NULL; +PFNGLCOLOR3SVPROC glad_glColor3sv = NULL; +PFNGLCOLOR3UBPROC glad_glColor3ub = NULL; +PFNGLCOLOR3UBVPROC glad_glColor3ubv = NULL; +PFNGLCOLOR3UIPROC glad_glColor3ui = NULL; +PFNGLCOLOR3UIVPROC glad_glColor3uiv = NULL; +PFNGLCOLOR3USPROC glad_glColor3us = NULL; +PFNGLCOLOR3USVPROC glad_glColor3usv = NULL; +PFNGLCOLOR4BPROC glad_glColor4b = NULL; +PFNGLCOLOR4BVPROC glad_glColor4bv = NULL; +PFNGLCOLOR4DPROC glad_glColor4d = NULL; +PFNGLCOLOR4DVPROC glad_glColor4dv = NULL; +PFNGLCOLOR4FPROC glad_glColor4f = NULL; +PFNGLCOLOR4FVPROC glad_glColor4fv = NULL; +PFNGLCOLOR4IPROC glad_glColor4i = NULL; +PFNGLCOLOR4IVPROC glad_glColor4iv = NULL; +PFNGLCOLOR4SPROC glad_glColor4s = NULL; +PFNGLCOLOR4SVPROC glad_glColor4sv = NULL; +PFNGLCOLOR4UBPROC glad_glColor4ub = NULL; +PFNGLCOLOR4UBVPROC glad_glColor4ubv = NULL; +PFNGLCOLOR4UIPROC glad_glColor4ui = NULL; +PFNGLCOLOR4UIVPROC glad_glColor4uiv = NULL; +PFNGLCOLOR4USPROC glad_glColor4us = NULL; +PFNGLCOLOR4USVPROC glad_glColor4usv = NULL; +PFNGLCOLORMASKPROC glad_glColorMask = NULL; +PFNGLCOLORMASKIPROC glad_glColorMaski = NULL; +PFNGLCOLORMATERIALPROC glad_glColorMaterial = NULL; +PFNGLCOLORP3UIPROC glad_glColorP3ui = NULL; +PFNGLCOLORP3UIVPROC glad_glColorP3uiv = NULL; +PFNGLCOLORP4UIPROC glad_glColorP4ui = NULL; +PFNGLCOLORP4UIVPROC glad_glColorP4uiv = NULL; +PFNGLCOLORPOINTERPROC glad_glColorPointer = NULL; +PFNGLCOMPILESHADERPROC glad_glCompileShader = NULL; +PFNGLCOMPRESSEDTEXIMAGE1DPROC glad_glCompressedTexImage1D = NULL; +PFNGLCOMPRESSEDTEXIMAGE2DPROC glad_glCompressedTexImage2D = NULL; +PFNGLCOMPRESSEDTEXIMAGE3DPROC glad_glCompressedTexImage3D = NULL; +PFNGLCOMPRESSEDTEXSUBIMAGE1DPROC glad_glCompressedTexSubImage1D = NULL; +PFNGLCOMPRESSEDTEXSUBIMAGE2DPROC glad_glCompressedTexSubImage2D = NULL; +PFNGLCOMPRESSEDTEXSUBIMAGE3DPROC glad_glCompressedTexSubImage3D = NULL; +PFNGLCOPYBUFFERSUBDATAPROC glad_glCopyBufferSubData = NULL; +PFNGLCOPYPIXELSPROC glad_glCopyPixels = NULL; +PFNGLCOPYTEXIMAGE1DPROC glad_glCopyTexImage1D = NULL; +PFNGLCOPYTEXIMAGE2DPROC glad_glCopyTexImage2D = NULL; +PFNGLCOPYTEXSUBIMAGE1DPROC glad_glCopyTexSubImage1D = NULL; +PFNGLCOPYTEXSUBIMAGE2DPROC glad_glCopyTexSubImage2D = NULL; +PFNGLCOPYTEXSUBIMAGE3DPROC glad_glCopyTexSubImage3D = NULL; +PFNGLCREATEPROGRAMPROC glad_glCreateProgram = NULL; +PFNGLCREATESHADERPROC glad_glCreateShader = NULL; +PFNGLCREATESHADERPROGRAMVPROC glad_glCreateShaderProgramv = NULL; +PFNGLCULLFACEPROC glad_glCullFace = NULL; +PFNGLDELETEBUFFERSPROC glad_glDeleteBuffers = NULL; +PFNGLDELETEFRAMEBUFFERSPROC glad_glDeleteFramebuffers = NULL; +PFNGLDELETELISTSPROC glad_glDeleteLists = NULL; +PFNGLDELETEPROGRAMPROC glad_glDeleteProgram = NULL; +PFNGLDELETEPROGRAMPIPELINESPROC glad_glDeleteProgramPipelines = NULL; +PFNGLDELETEQUERIESPROC glad_glDeleteQueries = NULL; +PFNGLDELETERENDERBUFFERSPROC glad_glDeleteRenderbuffers = NULL; +PFNGLDELETESAMPLERSPROC glad_glDeleteSamplers = NULL; +PFNGLDELETESHADERPROC glad_glDeleteShader = NULL; +PFNGLDELETESYNCPROC glad_glDeleteSync = NULL; +PFNGLDELETETEXTURESPROC glad_glDeleteTextures = NULL; +PFNGLDELETETRANSFORMFEEDBACKSPROC glad_glDeleteTransformFeedbacks = NULL; +PFNGLDELETEVERTEXARRAYSPROC glad_glDeleteVertexArrays = NULL; +PFNGLDEPTHFUNCPROC glad_glDepthFunc = NULL; +PFNGLDEPTHMASKPROC glad_glDepthMask = NULL; +PFNGLDEPTHRANGEPROC glad_glDepthRange = NULL; +PFNGLDEPTHRANGEARRAYVPROC glad_glDepthRangeArrayv = NULL; +PFNGLDEPTHRANGEINDEXEDPROC glad_glDepthRangeIndexed = NULL; +PFNGLDEPTHRANGEFPROC glad_glDepthRangef = NULL; +PFNGLDETACHSHADERPROC glad_glDetachShader = NULL; +PFNGLDISABLEPROC glad_glDisable = NULL; +PFNGLDISABLECLIENTSTATEPROC glad_glDisableClientState = NULL; +PFNGLDISABLEVERTEXATTRIBARRAYPROC glad_glDisableVertexAttribArray = NULL; +PFNGLDISABLEIPROC glad_glDisablei = NULL; +PFNGLDRAWARRAYSPROC glad_glDrawArrays = NULL; +PFNGLDRAWARRAYSINDIRECTPROC glad_glDrawArraysIndirect = NULL; +PFNGLDRAWARRAYSINSTANCEDPROC glad_glDrawArraysInstanced = NULL; +PFNGLDRAWBUFFERPROC glad_glDrawBuffer = NULL; +PFNGLDRAWBUFFERSPROC glad_glDrawBuffers = NULL; +PFNGLDRAWELEMENTSPROC glad_glDrawElements = NULL; +PFNGLDRAWELEMENTSBASEVERTEXPROC glad_glDrawElementsBaseVertex = NULL; +PFNGLDRAWELEMENTSINDIRECTPROC glad_glDrawElementsIndirect = NULL; +PFNGLDRAWELEMENTSINSTANCEDPROC glad_glDrawElementsInstanced = NULL; +PFNGLDRAWELEMENTSINSTANCEDBASEVERTEXPROC glad_glDrawElementsInstancedBaseVertex = NULL; +PFNGLDRAWPIXELSPROC glad_glDrawPixels = NULL; +PFNGLDRAWRANGEELEMENTSPROC glad_glDrawRangeElements = NULL; +PFNGLDRAWRANGEELEMENTSBASEVERTEXPROC glad_glDrawRangeElementsBaseVertex = NULL; +PFNGLDRAWTRANSFORMFEEDBACKPROC glad_glDrawTransformFeedback = NULL; +PFNGLDRAWTRANSFORMFEEDBACKSTREAMPROC glad_glDrawTransformFeedbackStream = NULL; +PFNGLEDGEFLAGPROC glad_glEdgeFlag = NULL; +PFNGLEDGEFLAGPOINTERPROC glad_glEdgeFlagPointer = NULL; +PFNGLEDGEFLAGVPROC glad_glEdgeFlagv = NULL; +PFNGLENABLEPROC glad_glEnable = NULL; +PFNGLENABLECLIENTSTATEPROC glad_glEnableClientState = NULL; +PFNGLENABLEVERTEXATTRIBARRAYPROC glad_glEnableVertexAttribArray = NULL; +PFNGLENABLEIPROC glad_glEnablei = NULL; +PFNGLENDPROC glad_glEnd = NULL; +PFNGLENDCONDITIONALRENDERPROC glad_glEndConditionalRender = NULL; +PFNGLENDLISTPROC glad_glEndList = NULL; +PFNGLENDQUERYPROC glad_glEndQuery = NULL; +PFNGLENDQUERYINDEXEDPROC glad_glEndQueryIndexed = NULL; +PFNGLENDTRANSFORMFEEDBACKPROC glad_glEndTransformFeedback = NULL; +PFNGLEVALCOORD1DPROC glad_glEvalCoord1d = NULL; +PFNGLEVALCOORD1DVPROC glad_glEvalCoord1dv = NULL; +PFNGLEVALCOORD1FPROC glad_glEvalCoord1f = NULL; +PFNGLEVALCOORD1FVPROC glad_glEvalCoord1fv = NULL; +PFNGLEVALCOORD2DPROC glad_glEvalCoord2d = NULL; +PFNGLEVALCOORD2DVPROC glad_glEvalCoord2dv = NULL; +PFNGLEVALCOORD2FPROC glad_glEvalCoord2f = NULL; +PFNGLEVALCOORD2FVPROC glad_glEvalCoord2fv = NULL; +PFNGLEVALMESH1PROC glad_glEvalMesh1 = NULL; +PFNGLEVALMESH2PROC glad_glEvalMesh2 = NULL; +PFNGLEVALPOINT1PROC glad_glEvalPoint1 = NULL; +PFNGLEVALPOINT2PROC glad_glEvalPoint2 = NULL; +PFNGLFEEDBACKBUFFERPROC glad_glFeedbackBuffer = NULL; +PFNGLFENCESYNCPROC glad_glFenceSync = NULL; +PFNGLFINISHPROC glad_glFinish = NULL; +PFNGLFLUSHPROC glad_glFlush = NULL; +PFNGLFLUSHMAPPEDBUFFERRANGEPROC glad_glFlushMappedBufferRange = NULL; +PFNGLFOGCOORDPOINTERPROC glad_glFogCoordPointer = NULL; +PFNGLFOGCOORDDPROC glad_glFogCoordd = NULL; +PFNGLFOGCOORDDVPROC glad_glFogCoorddv = NULL; +PFNGLFOGCOORDFPROC glad_glFogCoordf = NULL; +PFNGLFOGCOORDFVPROC glad_glFogCoordfv = NULL; +PFNGLFOGFPROC glad_glFogf = NULL; +PFNGLFOGFVPROC glad_glFogfv = NULL; +PFNGLFOGIPROC glad_glFogi = NULL; +PFNGLFOGIVPROC glad_glFogiv = NULL; +PFNGLFRAMEBUFFERRENDERBUFFERPROC glad_glFramebufferRenderbuffer = NULL; +PFNGLFRAMEBUFFERTEXTUREPROC glad_glFramebufferTexture = NULL; +PFNGLFRAMEBUFFERTEXTURE1DPROC glad_glFramebufferTexture1D = NULL; +PFNGLFRAMEBUFFERTEXTURE2DPROC glad_glFramebufferTexture2D = NULL; +PFNGLFRAMEBUFFERTEXTURE3DPROC glad_glFramebufferTexture3D = NULL; +PFNGLFRAMEBUFFERTEXTURELAYERPROC glad_glFramebufferTextureLayer = NULL; +PFNGLFRONTFACEPROC glad_glFrontFace = NULL; +PFNGLFRUSTUMPROC glad_glFrustum = NULL; +PFNGLGENBUFFERSPROC glad_glGenBuffers = NULL; +PFNGLGENFRAMEBUFFERSPROC glad_glGenFramebuffers = NULL; +PFNGLGENLISTSPROC glad_glGenLists = NULL; +PFNGLGENPROGRAMPIPELINESPROC glad_glGenProgramPipelines = NULL; +PFNGLGENQUERIESPROC glad_glGenQueries = NULL; +PFNGLGENRENDERBUFFERSPROC glad_glGenRenderbuffers = NULL; +PFNGLGENSAMPLERSPROC glad_glGenSamplers = NULL; +PFNGLGENTEXTURESPROC glad_glGenTextures = NULL; +PFNGLGENTRANSFORMFEEDBACKSPROC glad_glGenTransformFeedbacks = NULL; +PFNGLGENVERTEXARRAYSPROC glad_glGenVertexArrays = NULL; +PFNGLGENERATEMIPMAPPROC glad_glGenerateMipmap = NULL; +PFNGLGETACTIVEATTRIBPROC glad_glGetActiveAttrib = NULL; +PFNGLGETACTIVESUBROUTINENAMEPROC glad_glGetActiveSubroutineName = NULL; +PFNGLGETACTIVESUBROUTINEUNIFORMNAMEPROC glad_glGetActiveSubroutineUniformName = NULL; +PFNGLGETACTIVESUBROUTINEUNIFORMIVPROC glad_glGetActiveSubroutineUniformiv = NULL; +PFNGLGETACTIVEUNIFORMPROC glad_glGetActiveUniform = NULL; +PFNGLGETACTIVEUNIFORMBLOCKNAMEPROC glad_glGetActiveUniformBlockName = NULL; +PFNGLGETACTIVEUNIFORMBLOCKIVPROC glad_glGetActiveUniformBlockiv = NULL; +PFNGLGETACTIVEUNIFORMNAMEPROC glad_glGetActiveUniformName = NULL; +PFNGLGETACTIVEUNIFORMSIVPROC glad_glGetActiveUniformsiv = NULL; +PFNGLGETATTACHEDSHADERSPROC glad_glGetAttachedShaders = NULL; +PFNGLGETATTRIBLOCATIONPROC glad_glGetAttribLocation = NULL; +PFNGLGETBOOLEANI_VPROC glad_glGetBooleani_v = NULL; +PFNGLGETBOOLEANVPROC glad_glGetBooleanv = NULL; +PFNGLGETBUFFERPARAMETERI64VPROC glad_glGetBufferParameteri64v = NULL; +PFNGLGETBUFFERPARAMETERIVPROC glad_glGetBufferParameteriv = NULL; +PFNGLGETBUFFERPOINTERVPROC glad_glGetBufferPointerv = NULL; +PFNGLGETBUFFERSUBDATAPROC glad_glGetBufferSubData = NULL; +PFNGLGETCLIPPLANEPROC glad_glGetClipPlane = NULL; +PFNGLGETCOMPRESSEDTEXIMAGEPROC glad_glGetCompressedTexImage = NULL; +PFNGLGETDOUBLEI_VPROC glad_glGetDoublei_v = NULL; +PFNGLGETDOUBLEVPROC glad_glGetDoublev = NULL; +PFNGLGETERRORPROC glad_glGetError = NULL; +PFNGLGETFLOATI_VPROC glad_glGetFloati_v = NULL; +PFNGLGETFLOATVPROC glad_glGetFloatv = NULL; +PFNGLGETFRAGDATAINDEXPROC glad_glGetFragDataIndex = NULL; +PFNGLGETFRAGDATALOCATIONPROC glad_glGetFragDataLocation = NULL; +PFNGLGETFRAMEBUFFERATTACHMENTPARAMETERIVPROC glad_glGetFramebufferAttachmentParameteriv = NULL; +PFNGLGETINTEGER64I_VPROC glad_glGetInteger64i_v = NULL; +PFNGLGETINTEGER64VPROC glad_glGetInteger64v = NULL; +PFNGLGETINTEGERI_VPROC glad_glGetIntegeri_v = NULL; +PFNGLGETINTEGERVPROC glad_glGetIntegerv = NULL; +PFNGLGETLIGHTFVPROC glad_glGetLightfv = NULL; +PFNGLGETLIGHTIVPROC glad_glGetLightiv = NULL; +PFNGLGETMAPDVPROC glad_glGetMapdv = NULL; +PFNGLGETMAPFVPROC glad_glGetMapfv = NULL; +PFNGLGETMAPIVPROC glad_glGetMapiv = NULL; +PFNGLGETMATERIALFVPROC glad_glGetMaterialfv = NULL; +PFNGLGETMATERIALIVPROC glad_glGetMaterialiv = NULL; +PFNGLGETMULTISAMPLEFVPROC glad_glGetMultisamplefv = NULL; +PFNGLGETPIXELMAPFVPROC glad_glGetPixelMapfv = NULL; +PFNGLGETPIXELMAPUIVPROC glad_glGetPixelMapuiv = NULL; +PFNGLGETPIXELMAPUSVPROC glad_glGetPixelMapusv = NULL; +PFNGLGETPOINTERVPROC glad_glGetPointerv = NULL; +PFNGLGETPOLYGONSTIPPLEPROC glad_glGetPolygonStipple = NULL; +PFNGLGETPROGRAMBINARYPROC glad_glGetProgramBinary = NULL; +PFNGLGETPROGRAMINFOLOGPROC glad_glGetProgramInfoLog = NULL; +PFNGLGETPROGRAMPIPELINEINFOLOGPROC glad_glGetProgramPipelineInfoLog = NULL; +PFNGLGETPROGRAMPIPELINEIVPROC glad_glGetProgramPipelineiv = NULL; +PFNGLGETPROGRAMSTAGEIVPROC glad_glGetProgramStageiv = NULL; +PFNGLGETPROGRAMIVPROC glad_glGetProgramiv = NULL; +PFNGLGETQUERYINDEXEDIVPROC glad_glGetQueryIndexediv = NULL; +PFNGLGETQUERYOBJECTI64VPROC glad_glGetQueryObjecti64v = NULL; +PFNGLGETQUERYOBJECTIVPROC glad_glGetQueryObjectiv = NULL; +PFNGLGETQUERYOBJECTUI64VPROC glad_glGetQueryObjectui64v = NULL; +PFNGLGETQUERYOBJECTUIVPROC glad_glGetQueryObjectuiv = NULL; +PFNGLGETQUERYIVPROC glad_glGetQueryiv = NULL; +PFNGLGETRENDERBUFFERPARAMETERIVPROC glad_glGetRenderbufferParameteriv = NULL; +PFNGLGETSAMPLERPARAMETERIIVPROC glad_glGetSamplerParameterIiv = NULL; +PFNGLGETSAMPLERPARAMETERIUIVPROC glad_glGetSamplerParameterIuiv = NULL; +PFNGLGETSAMPLERPARAMETERFVPROC glad_glGetSamplerParameterfv = NULL; +PFNGLGETSAMPLERPARAMETERIVPROC glad_glGetSamplerParameteriv = NULL; +PFNGLGETSHADERINFOLOGPROC glad_glGetShaderInfoLog = NULL; +PFNGLGETSHADERPRECISIONFORMATPROC glad_glGetShaderPrecisionFormat = NULL; +PFNGLGETSHADERSOURCEPROC glad_glGetShaderSource = NULL; +PFNGLGETSHADERIVPROC glad_glGetShaderiv = NULL; +PFNGLGETSTRINGPROC glad_glGetString = NULL; +PFNGLGETSTRINGIPROC glad_glGetStringi = NULL; +PFNGLGETSUBROUTINEINDEXPROC glad_glGetSubroutineIndex = NULL; +PFNGLGETSUBROUTINEUNIFORMLOCATIONPROC glad_glGetSubroutineUniformLocation = NULL; +PFNGLGETSYNCIVPROC glad_glGetSynciv = NULL; +PFNGLGETTEXENVFVPROC glad_glGetTexEnvfv = NULL; +PFNGLGETTEXENVIVPROC glad_glGetTexEnviv = NULL; +PFNGLGETTEXGENDVPROC glad_glGetTexGendv = NULL; +PFNGLGETTEXGENFVPROC glad_glGetTexGenfv = NULL; +PFNGLGETTEXGENIVPROC glad_glGetTexGeniv = NULL; +PFNGLGETTEXIMAGEPROC glad_glGetTexImage = NULL; +PFNGLGETTEXLEVELPARAMETERFVPROC glad_glGetTexLevelParameterfv = NULL; +PFNGLGETTEXLEVELPARAMETERIVPROC glad_glGetTexLevelParameteriv = NULL; +PFNGLGETTEXPARAMETERIIVPROC glad_glGetTexParameterIiv = NULL; +PFNGLGETTEXPARAMETERIUIVPROC glad_glGetTexParameterIuiv = NULL; +PFNGLGETTEXPARAMETERFVPROC glad_glGetTexParameterfv = NULL; +PFNGLGETTEXPARAMETERIVPROC glad_glGetTexParameteriv = NULL; +PFNGLGETTRANSFORMFEEDBACKVARYINGPROC glad_glGetTransformFeedbackVarying = NULL; +PFNGLGETUNIFORMBLOCKINDEXPROC glad_glGetUniformBlockIndex = NULL; +PFNGLGETUNIFORMINDICESPROC glad_glGetUniformIndices = NULL; +PFNGLGETUNIFORMLOCATIONPROC glad_glGetUniformLocation = NULL; +PFNGLGETUNIFORMSUBROUTINEUIVPROC glad_glGetUniformSubroutineuiv = NULL; +PFNGLGETUNIFORMDVPROC glad_glGetUniformdv = NULL; +PFNGLGETUNIFORMFVPROC glad_glGetUniformfv = NULL; +PFNGLGETUNIFORMIVPROC glad_glGetUniformiv = NULL; +PFNGLGETUNIFORMUIVPROC glad_glGetUniformuiv = NULL; +PFNGLGETVERTEXATTRIBIIVPROC glad_glGetVertexAttribIiv = NULL; +PFNGLGETVERTEXATTRIBIUIVPROC glad_glGetVertexAttribIuiv = NULL; +PFNGLGETVERTEXATTRIBLDVPROC glad_glGetVertexAttribLdv = NULL; +PFNGLGETVERTEXATTRIBPOINTERVPROC glad_glGetVertexAttribPointerv = NULL; +PFNGLGETVERTEXATTRIBDVPROC glad_glGetVertexAttribdv = NULL; +PFNGLGETVERTEXATTRIBFVPROC glad_glGetVertexAttribfv = NULL; +PFNGLGETVERTEXATTRIBIVPROC glad_glGetVertexAttribiv = NULL; +PFNGLHINTPROC glad_glHint = NULL; +PFNGLINDEXMASKPROC glad_glIndexMask = NULL; +PFNGLINDEXPOINTERPROC glad_glIndexPointer = NULL; +PFNGLINDEXDPROC glad_glIndexd = NULL; +PFNGLINDEXDVPROC glad_glIndexdv = NULL; +PFNGLINDEXFPROC glad_glIndexf = NULL; +PFNGLINDEXFVPROC glad_glIndexfv = NULL; +PFNGLINDEXIPROC glad_glIndexi = NULL; +PFNGLINDEXIVPROC glad_glIndexiv = NULL; +PFNGLINDEXSPROC glad_glIndexs = NULL; +PFNGLINDEXSVPROC glad_glIndexsv = NULL; +PFNGLINDEXUBPROC glad_glIndexub = NULL; +PFNGLINDEXUBVPROC glad_glIndexubv = NULL; +PFNGLINITNAMESPROC glad_glInitNames = NULL; +PFNGLINTERLEAVEDARRAYSPROC glad_glInterleavedArrays = NULL; +PFNGLISBUFFERPROC glad_glIsBuffer = NULL; +PFNGLISENABLEDPROC glad_glIsEnabled = NULL; +PFNGLISENABLEDIPROC glad_glIsEnabledi = NULL; +PFNGLISFRAMEBUFFERPROC glad_glIsFramebuffer = NULL; +PFNGLISLISTPROC glad_glIsList = NULL; +PFNGLISPROGRAMPROC glad_glIsProgram = NULL; +PFNGLISPROGRAMPIPELINEPROC glad_glIsProgramPipeline = NULL; +PFNGLISQUERYPROC glad_glIsQuery = NULL; +PFNGLISRENDERBUFFERPROC glad_glIsRenderbuffer = NULL; +PFNGLISSAMPLERPROC glad_glIsSampler = NULL; +PFNGLISSHADERPROC glad_glIsShader = NULL; +PFNGLISSYNCPROC glad_glIsSync = NULL; +PFNGLISTEXTUREPROC glad_glIsTexture = NULL; +PFNGLISTRANSFORMFEEDBACKPROC glad_glIsTransformFeedback = NULL; +PFNGLISVERTEXARRAYPROC glad_glIsVertexArray = NULL; +PFNGLLIGHTMODELFPROC glad_glLightModelf = NULL; +PFNGLLIGHTMODELFVPROC glad_glLightModelfv = NULL; +PFNGLLIGHTMODELIPROC glad_glLightModeli = NULL; +PFNGLLIGHTMODELIVPROC glad_glLightModeliv = NULL; +PFNGLLIGHTFPROC glad_glLightf = NULL; +PFNGLLIGHTFVPROC glad_glLightfv = NULL; +PFNGLLIGHTIPROC glad_glLighti = NULL; +PFNGLLIGHTIVPROC glad_glLightiv = NULL; +PFNGLLINESTIPPLEPROC glad_glLineStipple = NULL; +PFNGLLINEWIDTHPROC glad_glLineWidth = NULL; +PFNGLLINKPROGRAMPROC glad_glLinkProgram = NULL; +PFNGLLISTBASEPROC glad_glListBase = NULL; +PFNGLLOADIDENTITYPROC glad_glLoadIdentity = NULL; +PFNGLLOADMATRIXDPROC glad_glLoadMatrixd = NULL; +PFNGLLOADMATRIXFPROC glad_glLoadMatrixf = NULL; +PFNGLLOADNAMEPROC glad_glLoadName = NULL; +PFNGLLOADTRANSPOSEMATRIXDPROC glad_glLoadTransposeMatrixd = NULL; +PFNGLLOADTRANSPOSEMATRIXFPROC glad_glLoadTransposeMatrixf = NULL; +PFNGLLOGICOPPROC glad_glLogicOp = NULL; +PFNGLMAP1DPROC glad_glMap1d = NULL; +PFNGLMAP1FPROC glad_glMap1f = NULL; +PFNGLMAP2DPROC glad_glMap2d = NULL; +PFNGLMAP2FPROC glad_glMap2f = NULL; +PFNGLMAPBUFFERPROC glad_glMapBuffer = NULL; +PFNGLMAPBUFFERRANGEPROC glad_glMapBufferRange = NULL; +PFNGLMAPGRID1DPROC glad_glMapGrid1d = NULL; +PFNGLMAPGRID1FPROC glad_glMapGrid1f = NULL; +PFNGLMAPGRID2DPROC glad_glMapGrid2d = NULL; +PFNGLMAPGRID2FPROC glad_glMapGrid2f = NULL; +PFNGLMATERIALFPROC glad_glMaterialf = NULL; +PFNGLMATERIALFVPROC glad_glMaterialfv = NULL; +PFNGLMATERIALIPROC glad_glMateriali = NULL; +PFNGLMATERIALIVPROC glad_glMaterialiv = NULL; +PFNGLMATRIXMODEPROC glad_glMatrixMode = NULL; +PFNGLMINSAMPLESHADINGPROC glad_glMinSampleShading = NULL; +PFNGLMULTMATRIXDPROC glad_glMultMatrixd = NULL; +PFNGLMULTMATRIXFPROC glad_glMultMatrixf = NULL; +PFNGLMULTTRANSPOSEMATRIXDPROC glad_glMultTransposeMatrixd = NULL; +PFNGLMULTTRANSPOSEMATRIXFPROC glad_glMultTransposeMatrixf = NULL; +PFNGLMULTIDRAWARRAYSPROC glad_glMultiDrawArrays = NULL; +PFNGLMULTIDRAWELEMENTSPROC glad_glMultiDrawElements = NULL; +PFNGLMULTIDRAWELEMENTSBASEVERTEXPROC glad_glMultiDrawElementsBaseVertex = NULL; +PFNGLMULTITEXCOORD1DPROC glad_glMultiTexCoord1d = NULL; +PFNGLMULTITEXCOORD1DVPROC glad_glMultiTexCoord1dv = NULL; +PFNGLMULTITEXCOORD1FPROC glad_glMultiTexCoord1f = NULL; +PFNGLMULTITEXCOORD1FVPROC glad_glMultiTexCoord1fv = NULL; +PFNGLMULTITEXCOORD1IPROC glad_glMultiTexCoord1i = NULL; +PFNGLMULTITEXCOORD1IVPROC glad_glMultiTexCoord1iv = NULL; +PFNGLMULTITEXCOORD1SPROC glad_glMultiTexCoord1s = NULL; +PFNGLMULTITEXCOORD1SVPROC glad_glMultiTexCoord1sv = NULL; +PFNGLMULTITEXCOORD2DPROC glad_glMultiTexCoord2d = NULL; +PFNGLMULTITEXCOORD2DVPROC glad_glMultiTexCoord2dv = NULL; +PFNGLMULTITEXCOORD2FPROC glad_glMultiTexCoord2f = NULL; +PFNGLMULTITEXCOORD2FVPROC glad_glMultiTexCoord2fv = NULL; +PFNGLMULTITEXCOORD2IPROC glad_glMultiTexCoord2i = NULL; +PFNGLMULTITEXCOORD2IVPROC glad_glMultiTexCoord2iv = NULL; +PFNGLMULTITEXCOORD2SPROC glad_glMultiTexCoord2s = NULL; +PFNGLMULTITEXCOORD2SVPROC glad_glMultiTexCoord2sv = NULL; +PFNGLMULTITEXCOORD3DPROC glad_glMultiTexCoord3d = NULL; +PFNGLMULTITEXCOORD3DVPROC glad_glMultiTexCoord3dv = NULL; +PFNGLMULTITEXCOORD3FPROC glad_glMultiTexCoord3f = NULL; +PFNGLMULTITEXCOORD3FVPROC glad_glMultiTexCoord3fv = NULL; +PFNGLMULTITEXCOORD3IPROC glad_glMultiTexCoord3i = NULL; +PFNGLMULTITEXCOORD3IVPROC glad_glMultiTexCoord3iv = NULL; +PFNGLMULTITEXCOORD3SPROC glad_glMultiTexCoord3s = NULL; +PFNGLMULTITEXCOORD3SVPROC glad_glMultiTexCoord3sv = NULL; +PFNGLMULTITEXCOORD4DPROC glad_glMultiTexCoord4d = NULL; +PFNGLMULTITEXCOORD4DVPROC glad_glMultiTexCoord4dv = NULL; +PFNGLMULTITEXCOORD4FPROC glad_glMultiTexCoord4f = NULL; +PFNGLMULTITEXCOORD4FVPROC glad_glMultiTexCoord4fv = NULL; +PFNGLMULTITEXCOORD4IPROC glad_glMultiTexCoord4i = NULL; +PFNGLMULTITEXCOORD4IVPROC glad_glMultiTexCoord4iv = NULL; +PFNGLMULTITEXCOORD4SPROC glad_glMultiTexCoord4s = NULL; +PFNGLMULTITEXCOORD4SVPROC glad_glMultiTexCoord4sv = NULL; +PFNGLMULTITEXCOORDP1UIPROC glad_glMultiTexCoordP1ui = NULL; +PFNGLMULTITEXCOORDP1UIVPROC glad_glMultiTexCoordP1uiv = NULL; +PFNGLMULTITEXCOORDP2UIPROC glad_glMultiTexCoordP2ui = NULL; +PFNGLMULTITEXCOORDP2UIVPROC glad_glMultiTexCoordP2uiv = NULL; +PFNGLMULTITEXCOORDP3UIPROC glad_glMultiTexCoordP3ui = NULL; +PFNGLMULTITEXCOORDP3UIVPROC glad_glMultiTexCoordP3uiv = NULL; +PFNGLMULTITEXCOORDP4UIPROC glad_glMultiTexCoordP4ui = NULL; +PFNGLMULTITEXCOORDP4UIVPROC glad_glMultiTexCoordP4uiv = NULL; +PFNGLNEWLISTPROC glad_glNewList = NULL; +PFNGLNORMAL3BPROC glad_glNormal3b = NULL; +PFNGLNORMAL3BVPROC glad_glNormal3bv = NULL; +PFNGLNORMAL3DPROC glad_glNormal3d = NULL; +PFNGLNORMAL3DVPROC glad_glNormal3dv = NULL; +PFNGLNORMAL3FPROC glad_glNormal3f = NULL; +PFNGLNORMAL3FVPROC glad_glNormal3fv = NULL; +PFNGLNORMAL3IPROC glad_glNormal3i = NULL; +PFNGLNORMAL3IVPROC glad_glNormal3iv = NULL; +PFNGLNORMAL3SPROC glad_glNormal3s = NULL; +PFNGLNORMAL3SVPROC glad_glNormal3sv = NULL; +PFNGLNORMALP3UIPROC glad_glNormalP3ui = NULL; +PFNGLNORMALP3UIVPROC glad_glNormalP3uiv = NULL; +PFNGLNORMALPOINTERPROC glad_glNormalPointer = NULL; +PFNGLORTHOPROC glad_glOrtho = NULL; +PFNGLPASSTHROUGHPROC glad_glPassThrough = NULL; +PFNGLPATCHPARAMETERFVPROC glad_glPatchParameterfv = NULL; +PFNGLPATCHPARAMETERIPROC glad_glPatchParameteri = NULL; +PFNGLPAUSETRANSFORMFEEDBACKPROC glad_glPauseTransformFeedback = NULL; +PFNGLPIXELMAPFVPROC glad_glPixelMapfv = NULL; +PFNGLPIXELMAPUIVPROC glad_glPixelMapuiv = NULL; +PFNGLPIXELMAPUSVPROC glad_glPixelMapusv = NULL; +PFNGLPIXELSTOREFPROC glad_glPixelStoref = NULL; +PFNGLPIXELSTOREIPROC glad_glPixelStorei = NULL; +PFNGLPIXELTRANSFERFPROC glad_glPixelTransferf = NULL; +PFNGLPIXELTRANSFERIPROC glad_glPixelTransferi = NULL; +PFNGLPIXELZOOMPROC glad_glPixelZoom = NULL; +PFNGLPOINTPARAMETERFPROC glad_glPointParameterf = NULL; +PFNGLPOINTPARAMETERFVPROC glad_glPointParameterfv = NULL; +PFNGLPOINTPARAMETERIPROC glad_glPointParameteri = NULL; +PFNGLPOINTPARAMETERIVPROC glad_glPointParameteriv = NULL; +PFNGLPOINTSIZEPROC glad_glPointSize = NULL; +PFNGLPOLYGONMODEPROC glad_glPolygonMode = NULL; +PFNGLPOLYGONOFFSETPROC glad_glPolygonOffset = NULL; +PFNGLPOLYGONSTIPPLEPROC glad_glPolygonStipple = NULL; +PFNGLPOPATTRIBPROC glad_glPopAttrib = NULL; +PFNGLPOPCLIENTATTRIBPROC glad_glPopClientAttrib = NULL; +PFNGLPOPMATRIXPROC glad_glPopMatrix = NULL; +PFNGLPOPNAMEPROC glad_glPopName = NULL; +PFNGLPRIMITIVERESTARTINDEXPROC glad_glPrimitiveRestartIndex = NULL; +PFNGLPRIORITIZETEXTURESPROC glad_glPrioritizeTextures = NULL; +PFNGLPROGRAMBINARYPROC glad_glProgramBinary = NULL; +PFNGLPROGRAMPARAMETERIPROC glad_glProgramParameteri = NULL; +PFNGLPROGRAMUNIFORM1DPROC glad_glProgramUniform1d = NULL; +PFNGLPROGRAMUNIFORM1DVPROC glad_glProgramUniform1dv = NULL; +PFNGLPROGRAMUNIFORM1FPROC glad_glProgramUniform1f = NULL; +PFNGLPROGRAMUNIFORM1FVPROC glad_glProgramUniform1fv = NULL; +PFNGLPROGRAMUNIFORM1IPROC glad_glProgramUniform1i = NULL; +PFNGLPROGRAMUNIFORM1IVPROC glad_glProgramUniform1iv = NULL; +PFNGLPROGRAMUNIFORM1UIPROC glad_glProgramUniform1ui = NULL; +PFNGLPROGRAMUNIFORM1UIVPROC glad_glProgramUniform1uiv = NULL; +PFNGLPROGRAMUNIFORM2DPROC glad_glProgramUniform2d = NULL; +PFNGLPROGRAMUNIFORM2DVPROC glad_glProgramUniform2dv = NULL; +PFNGLPROGRAMUNIFORM2FPROC glad_glProgramUniform2f = NULL; +PFNGLPROGRAMUNIFORM2FVPROC glad_glProgramUniform2fv = NULL; +PFNGLPROGRAMUNIFORM2IPROC glad_glProgramUniform2i = NULL; +PFNGLPROGRAMUNIFORM2IVPROC glad_glProgramUniform2iv = NULL; +PFNGLPROGRAMUNIFORM2UIPROC glad_glProgramUniform2ui = NULL; +PFNGLPROGRAMUNIFORM2UIVPROC glad_glProgramUniform2uiv = NULL; +PFNGLPROGRAMUNIFORM3DPROC glad_glProgramUniform3d = NULL; +PFNGLPROGRAMUNIFORM3DVPROC glad_glProgramUniform3dv = NULL; +PFNGLPROGRAMUNIFORM3FPROC glad_glProgramUniform3f = NULL; +PFNGLPROGRAMUNIFORM3FVPROC glad_glProgramUniform3fv = NULL; +PFNGLPROGRAMUNIFORM3IPROC glad_glProgramUniform3i = NULL; +PFNGLPROGRAMUNIFORM3IVPROC glad_glProgramUniform3iv = NULL; +PFNGLPROGRAMUNIFORM3UIPROC glad_glProgramUniform3ui = NULL; +PFNGLPROGRAMUNIFORM3UIVPROC glad_glProgramUniform3uiv = NULL; +PFNGLPROGRAMUNIFORM4DPROC glad_glProgramUniform4d = NULL; +PFNGLPROGRAMUNIFORM4DVPROC glad_glProgramUniform4dv = NULL; +PFNGLPROGRAMUNIFORM4FPROC glad_glProgramUniform4f = NULL; +PFNGLPROGRAMUNIFORM4FVPROC glad_glProgramUniform4fv = NULL; +PFNGLPROGRAMUNIFORM4IPROC glad_glProgramUniform4i = NULL; +PFNGLPROGRAMUNIFORM4IVPROC glad_glProgramUniform4iv = NULL; +PFNGLPROGRAMUNIFORM4UIPROC glad_glProgramUniform4ui = NULL; +PFNGLPROGRAMUNIFORM4UIVPROC glad_glProgramUniform4uiv = NULL; +PFNGLPROGRAMUNIFORMMATRIX2DVPROC glad_glProgramUniformMatrix2dv = NULL; +PFNGLPROGRAMUNIFORMMATRIX2FVPROC glad_glProgramUniformMatrix2fv = NULL; +PFNGLPROGRAMUNIFORMMATRIX2X3DVPROC glad_glProgramUniformMatrix2x3dv = NULL; +PFNGLPROGRAMUNIFORMMATRIX2X3FVPROC glad_glProgramUniformMatrix2x3fv = NULL; +PFNGLPROGRAMUNIFORMMATRIX2X4DVPROC glad_glProgramUniformMatrix2x4dv = NULL; +PFNGLPROGRAMUNIFORMMATRIX2X4FVPROC glad_glProgramUniformMatrix2x4fv = NULL; +PFNGLPROGRAMUNIFORMMATRIX3DVPROC glad_glProgramUniformMatrix3dv = NULL; +PFNGLPROGRAMUNIFORMMATRIX3FVPROC glad_glProgramUniformMatrix3fv = NULL; +PFNGLPROGRAMUNIFORMMATRIX3X2DVPROC glad_glProgramUniformMatrix3x2dv = NULL; +PFNGLPROGRAMUNIFORMMATRIX3X2FVPROC glad_glProgramUniformMatrix3x2fv = NULL; +PFNGLPROGRAMUNIFORMMATRIX3X4DVPROC glad_glProgramUniformMatrix3x4dv = NULL; +PFNGLPROGRAMUNIFORMMATRIX3X4FVPROC glad_glProgramUniformMatrix3x4fv = NULL; +PFNGLPROGRAMUNIFORMMATRIX4DVPROC glad_glProgramUniformMatrix4dv = NULL; +PFNGLPROGRAMUNIFORMMATRIX4FVPROC glad_glProgramUniformMatrix4fv = NULL; +PFNGLPROGRAMUNIFORMMATRIX4X2DVPROC glad_glProgramUniformMatrix4x2dv = NULL; +PFNGLPROGRAMUNIFORMMATRIX4X2FVPROC glad_glProgramUniformMatrix4x2fv = NULL; +PFNGLPROGRAMUNIFORMMATRIX4X3DVPROC glad_glProgramUniformMatrix4x3dv = NULL; +PFNGLPROGRAMUNIFORMMATRIX4X3FVPROC glad_glProgramUniformMatrix4x3fv = NULL; +PFNGLPROVOKINGVERTEXPROC glad_glProvokingVertex = NULL; +PFNGLPUSHATTRIBPROC glad_glPushAttrib = NULL; +PFNGLPUSHCLIENTATTRIBPROC glad_glPushClientAttrib = NULL; +PFNGLPUSHMATRIXPROC glad_glPushMatrix = NULL; +PFNGLPUSHNAMEPROC glad_glPushName = NULL; +PFNGLQUERYCOUNTERPROC glad_glQueryCounter = NULL; +PFNGLRASTERPOS2DPROC glad_glRasterPos2d = NULL; +PFNGLRASTERPOS2DVPROC glad_glRasterPos2dv = NULL; +PFNGLRASTERPOS2FPROC glad_glRasterPos2f = NULL; +PFNGLRASTERPOS2FVPROC glad_glRasterPos2fv = NULL; +PFNGLRASTERPOS2IPROC glad_glRasterPos2i = NULL; +PFNGLRASTERPOS2IVPROC glad_glRasterPos2iv = NULL; +PFNGLRASTERPOS2SPROC glad_glRasterPos2s = NULL; +PFNGLRASTERPOS2SVPROC glad_glRasterPos2sv = NULL; +PFNGLRASTERPOS3DPROC glad_glRasterPos3d = NULL; +PFNGLRASTERPOS3DVPROC glad_glRasterPos3dv = NULL; +PFNGLRASTERPOS3FPROC glad_glRasterPos3f = NULL; +PFNGLRASTERPOS3FVPROC glad_glRasterPos3fv = NULL; +PFNGLRASTERPOS3IPROC glad_glRasterPos3i = NULL; +PFNGLRASTERPOS3IVPROC glad_glRasterPos3iv = NULL; +PFNGLRASTERPOS3SPROC glad_glRasterPos3s = NULL; +PFNGLRASTERPOS3SVPROC glad_glRasterPos3sv = NULL; +PFNGLRASTERPOS4DPROC glad_glRasterPos4d = NULL; +PFNGLRASTERPOS4DVPROC glad_glRasterPos4dv = NULL; +PFNGLRASTERPOS4FPROC glad_glRasterPos4f = NULL; +PFNGLRASTERPOS4FVPROC glad_glRasterPos4fv = NULL; +PFNGLRASTERPOS4IPROC glad_glRasterPos4i = NULL; +PFNGLRASTERPOS4IVPROC glad_glRasterPos4iv = NULL; +PFNGLRASTERPOS4SPROC glad_glRasterPos4s = NULL; +PFNGLRASTERPOS4SVPROC glad_glRasterPos4sv = NULL; +PFNGLREADBUFFERPROC glad_glReadBuffer = NULL; +PFNGLREADPIXELSPROC glad_glReadPixels = NULL; +PFNGLRECTDPROC glad_glRectd = NULL; +PFNGLRECTDVPROC glad_glRectdv = NULL; +PFNGLRECTFPROC glad_glRectf = NULL; +PFNGLRECTFVPROC glad_glRectfv = NULL; +PFNGLRECTIPROC glad_glRecti = NULL; +PFNGLRECTIVPROC glad_glRectiv = NULL; +PFNGLRECTSPROC glad_glRects = NULL; +PFNGLRECTSVPROC glad_glRectsv = NULL; +PFNGLRELEASESHADERCOMPILERPROC glad_glReleaseShaderCompiler = NULL; +PFNGLRENDERMODEPROC glad_glRenderMode = NULL; +PFNGLRENDERBUFFERSTORAGEPROC glad_glRenderbufferStorage = NULL; +PFNGLRENDERBUFFERSTORAGEMULTISAMPLEPROC glad_glRenderbufferStorageMultisample = NULL; +PFNGLRESUMETRANSFORMFEEDBACKPROC glad_glResumeTransformFeedback = NULL; +PFNGLROTATEDPROC glad_glRotated = NULL; +PFNGLROTATEFPROC glad_glRotatef = NULL; +PFNGLSAMPLECOVERAGEPROC glad_glSampleCoverage = NULL; +PFNGLSAMPLEMASKIPROC glad_glSampleMaski = NULL; +PFNGLSAMPLERPARAMETERIIVPROC glad_glSamplerParameterIiv = NULL; +PFNGLSAMPLERPARAMETERIUIVPROC glad_glSamplerParameterIuiv = NULL; +PFNGLSAMPLERPARAMETERFPROC glad_glSamplerParameterf = NULL; +PFNGLSAMPLERPARAMETERFVPROC glad_glSamplerParameterfv = NULL; +PFNGLSAMPLERPARAMETERIPROC glad_glSamplerParameteri = NULL; +PFNGLSAMPLERPARAMETERIVPROC glad_glSamplerParameteriv = NULL; +PFNGLSCALEDPROC glad_glScaled = NULL; +PFNGLSCALEFPROC glad_glScalef = NULL; +PFNGLSCISSORPROC glad_glScissor = NULL; +PFNGLSCISSORARRAYVPROC glad_glScissorArrayv = NULL; +PFNGLSCISSORINDEXEDPROC glad_glScissorIndexed = NULL; +PFNGLSCISSORINDEXEDVPROC glad_glScissorIndexedv = NULL; +PFNGLSECONDARYCOLOR3BPROC glad_glSecondaryColor3b = NULL; +PFNGLSECONDARYCOLOR3BVPROC glad_glSecondaryColor3bv = NULL; +PFNGLSECONDARYCOLOR3DPROC glad_glSecondaryColor3d = NULL; +PFNGLSECONDARYCOLOR3DVPROC glad_glSecondaryColor3dv = NULL; +PFNGLSECONDARYCOLOR3FPROC glad_glSecondaryColor3f = NULL; +PFNGLSECONDARYCOLOR3FVPROC glad_glSecondaryColor3fv = NULL; +PFNGLSECONDARYCOLOR3IPROC glad_glSecondaryColor3i = NULL; +PFNGLSECONDARYCOLOR3IVPROC glad_glSecondaryColor3iv = NULL; +PFNGLSECONDARYCOLOR3SPROC glad_glSecondaryColor3s = NULL; +PFNGLSECONDARYCOLOR3SVPROC glad_glSecondaryColor3sv = NULL; +PFNGLSECONDARYCOLOR3UBPROC glad_glSecondaryColor3ub = NULL; +PFNGLSECONDARYCOLOR3UBVPROC glad_glSecondaryColor3ubv = NULL; +PFNGLSECONDARYCOLOR3UIPROC glad_glSecondaryColor3ui = NULL; +PFNGLSECONDARYCOLOR3UIVPROC glad_glSecondaryColor3uiv = NULL; +PFNGLSECONDARYCOLOR3USPROC glad_glSecondaryColor3us = NULL; +PFNGLSECONDARYCOLOR3USVPROC glad_glSecondaryColor3usv = NULL; +PFNGLSECONDARYCOLORP3UIPROC glad_glSecondaryColorP3ui = NULL; +PFNGLSECONDARYCOLORP3UIVPROC glad_glSecondaryColorP3uiv = NULL; +PFNGLSECONDARYCOLORPOINTERPROC glad_glSecondaryColorPointer = NULL; +PFNGLSELECTBUFFERPROC glad_glSelectBuffer = NULL; +PFNGLSHADEMODELPROC glad_glShadeModel = NULL; +PFNGLSHADERBINARYPROC glad_glShaderBinary = NULL; +PFNGLSHADERSOURCEPROC glad_glShaderSource = NULL; +PFNGLSTENCILFUNCPROC glad_glStencilFunc = NULL; +PFNGLSTENCILFUNCSEPARATEPROC glad_glStencilFuncSeparate = NULL; +PFNGLSTENCILMASKPROC glad_glStencilMask = NULL; +PFNGLSTENCILMASKSEPARATEPROC glad_glStencilMaskSeparate = NULL; +PFNGLSTENCILOPPROC glad_glStencilOp = NULL; +PFNGLSTENCILOPSEPARATEPROC glad_glStencilOpSeparate = NULL; +PFNGLTEXBUFFERPROC glad_glTexBuffer = NULL; +PFNGLTEXCOORD1DPROC glad_glTexCoord1d = NULL; +PFNGLTEXCOORD1DVPROC glad_glTexCoord1dv = NULL; +PFNGLTEXCOORD1FPROC glad_glTexCoord1f = NULL; +PFNGLTEXCOORD1FVPROC glad_glTexCoord1fv = NULL; +PFNGLTEXCOORD1IPROC glad_glTexCoord1i = NULL; +PFNGLTEXCOORD1IVPROC glad_glTexCoord1iv = NULL; +PFNGLTEXCOORD1SPROC glad_glTexCoord1s = NULL; +PFNGLTEXCOORD1SVPROC glad_glTexCoord1sv = NULL; +PFNGLTEXCOORD2DPROC glad_glTexCoord2d = NULL; +PFNGLTEXCOORD2DVPROC glad_glTexCoord2dv = NULL; +PFNGLTEXCOORD2FPROC glad_glTexCoord2f = NULL; +PFNGLTEXCOORD2FVPROC glad_glTexCoord2fv = NULL; +PFNGLTEXCOORD2IPROC glad_glTexCoord2i = NULL; +PFNGLTEXCOORD2IVPROC glad_glTexCoord2iv = NULL; +PFNGLTEXCOORD2SPROC glad_glTexCoord2s = NULL; +PFNGLTEXCOORD2SVPROC glad_glTexCoord2sv = NULL; +PFNGLTEXCOORD3DPROC glad_glTexCoord3d = NULL; +PFNGLTEXCOORD3DVPROC glad_glTexCoord3dv = NULL; +PFNGLTEXCOORD3FPROC glad_glTexCoord3f = NULL; +PFNGLTEXCOORD3FVPROC glad_glTexCoord3fv = NULL; +PFNGLTEXCOORD3IPROC glad_glTexCoord3i = NULL; +PFNGLTEXCOORD3IVPROC glad_glTexCoord3iv = NULL; +PFNGLTEXCOORD3SPROC glad_glTexCoord3s = NULL; +PFNGLTEXCOORD3SVPROC glad_glTexCoord3sv = NULL; +PFNGLTEXCOORD4DPROC glad_glTexCoord4d = NULL; +PFNGLTEXCOORD4DVPROC glad_glTexCoord4dv = NULL; +PFNGLTEXCOORD4FPROC glad_glTexCoord4f = NULL; +PFNGLTEXCOORD4FVPROC glad_glTexCoord4fv = NULL; +PFNGLTEXCOORD4IPROC glad_glTexCoord4i = NULL; +PFNGLTEXCOORD4IVPROC glad_glTexCoord4iv = NULL; +PFNGLTEXCOORD4SPROC glad_glTexCoord4s = NULL; +PFNGLTEXCOORD4SVPROC glad_glTexCoord4sv = NULL; +PFNGLTEXCOORDP1UIPROC glad_glTexCoordP1ui = NULL; +PFNGLTEXCOORDP1UIVPROC glad_glTexCoordP1uiv = NULL; +PFNGLTEXCOORDP2UIPROC glad_glTexCoordP2ui = NULL; +PFNGLTEXCOORDP2UIVPROC glad_glTexCoordP2uiv = NULL; +PFNGLTEXCOORDP3UIPROC glad_glTexCoordP3ui = NULL; +PFNGLTEXCOORDP3UIVPROC glad_glTexCoordP3uiv = NULL; +PFNGLTEXCOORDP4UIPROC glad_glTexCoordP4ui = NULL; +PFNGLTEXCOORDP4UIVPROC glad_glTexCoordP4uiv = NULL; +PFNGLTEXCOORDPOINTERPROC glad_glTexCoordPointer = NULL; +PFNGLTEXENVFPROC glad_glTexEnvf = NULL; +PFNGLTEXENVFVPROC glad_glTexEnvfv = NULL; +PFNGLTEXENVIPROC glad_glTexEnvi = NULL; +PFNGLTEXENVIVPROC glad_glTexEnviv = NULL; +PFNGLTEXGENDPROC glad_glTexGend = NULL; +PFNGLTEXGENDVPROC glad_glTexGendv = NULL; +PFNGLTEXGENFPROC glad_glTexGenf = NULL; +PFNGLTEXGENFVPROC glad_glTexGenfv = NULL; +PFNGLTEXGENIPROC glad_glTexGeni = NULL; +PFNGLTEXGENIVPROC glad_glTexGeniv = NULL; +PFNGLTEXIMAGE1DPROC glad_glTexImage1D = NULL; +PFNGLTEXIMAGE2DPROC glad_glTexImage2D = NULL; +PFNGLTEXIMAGE2DMULTISAMPLEPROC glad_glTexImage2DMultisample = NULL; +PFNGLTEXIMAGE3DPROC glad_glTexImage3D = NULL; +PFNGLTEXIMAGE3DMULTISAMPLEPROC glad_glTexImage3DMultisample = NULL; +PFNGLTEXPARAMETERIIVPROC glad_glTexParameterIiv = NULL; +PFNGLTEXPARAMETERIUIVPROC glad_glTexParameterIuiv = NULL; +PFNGLTEXPARAMETERFPROC glad_glTexParameterf = NULL; +PFNGLTEXPARAMETERFVPROC glad_glTexParameterfv = NULL; +PFNGLTEXPARAMETERIPROC glad_glTexParameteri = NULL; +PFNGLTEXPARAMETERIVPROC glad_glTexParameteriv = NULL; +PFNGLTEXSUBIMAGE1DPROC glad_glTexSubImage1D = NULL; +PFNGLTEXSUBIMAGE2DPROC glad_glTexSubImage2D = NULL; +PFNGLTEXSUBIMAGE3DPROC glad_glTexSubImage3D = NULL; +PFNGLTRANSFORMFEEDBACKVARYINGSPROC glad_glTransformFeedbackVaryings = NULL; +PFNGLTRANSLATEDPROC glad_glTranslated = NULL; +PFNGLTRANSLATEFPROC glad_glTranslatef = NULL; +PFNGLUNIFORM1DPROC glad_glUniform1d = NULL; +PFNGLUNIFORM1DVPROC glad_glUniform1dv = NULL; +PFNGLUNIFORM1FPROC glad_glUniform1f = NULL; +PFNGLUNIFORM1FVPROC glad_glUniform1fv = NULL; +PFNGLUNIFORM1IPROC glad_glUniform1i = NULL; +PFNGLUNIFORM1IVPROC glad_glUniform1iv = NULL; +PFNGLUNIFORM1UIPROC glad_glUniform1ui = NULL; +PFNGLUNIFORM1UIVPROC glad_glUniform1uiv = NULL; +PFNGLUNIFORM2DPROC glad_glUniform2d = NULL; +PFNGLUNIFORM2DVPROC glad_glUniform2dv = NULL; +PFNGLUNIFORM2FPROC glad_glUniform2f = NULL; +PFNGLUNIFORM2FVPROC glad_glUniform2fv = NULL; +PFNGLUNIFORM2IPROC glad_glUniform2i = NULL; +PFNGLUNIFORM2IVPROC glad_glUniform2iv = NULL; +PFNGLUNIFORM2UIPROC glad_glUniform2ui = NULL; +PFNGLUNIFORM2UIVPROC glad_glUniform2uiv = NULL; +PFNGLUNIFORM3DPROC glad_glUniform3d = NULL; +PFNGLUNIFORM3DVPROC glad_glUniform3dv = NULL; +PFNGLUNIFORM3FPROC glad_glUniform3f = NULL; +PFNGLUNIFORM3FVPROC glad_glUniform3fv = NULL; +PFNGLUNIFORM3IPROC glad_glUniform3i = NULL; +PFNGLUNIFORM3IVPROC glad_glUniform3iv = NULL; +PFNGLUNIFORM3UIPROC glad_glUniform3ui = NULL; +PFNGLUNIFORM3UIVPROC glad_glUniform3uiv = NULL; +PFNGLUNIFORM4DPROC glad_glUniform4d = NULL; +PFNGLUNIFORM4DVPROC glad_glUniform4dv = NULL; +PFNGLUNIFORM4FPROC glad_glUniform4f = NULL; +PFNGLUNIFORM4FVPROC glad_glUniform4fv = NULL; +PFNGLUNIFORM4IPROC glad_glUniform4i = NULL; +PFNGLUNIFORM4IVPROC glad_glUniform4iv = NULL; +PFNGLUNIFORM4UIPROC glad_glUniform4ui = NULL; +PFNGLUNIFORM4UIVPROC glad_glUniform4uiv = NULL; +PFNGLUNIFORMBLOCKBINDINGPROC glad_glUniformBlockBinding = NULL; +PFNGLUNIFORMMATRIX2DVPROC glad_glUniformMatrix2dv = NULL; +PFNGLUNIFORMMATRIX2FVPROC glad_glUniformMatrix2fv = NULL; +PFNGLUNIFORMMATRIX2X3DVPROC glad_glUniformMatrix2x3dv = NULL; +PFNGLUNIFORMMATRIX2X3FVPROC glad_glUniformMatrix2x3fv = NULL; +PFNGLUNIFORMMATRIX2X4DVPROC glad_glUniformMatrix2x4dv = NULL; +PFNGLUNIFORMMATRIX2X4FVPROC glad_glUniformMatrix2x4fv = NULL; +PFNGLUNIFORMMATRIX3DVPROC glad_glUniformMatrix3dv = NULL; +PFNGLUNIFORMMATRIX3FVPROC glad_glUniformMatrix3fv = NULL; +PFNGLUNIFORMMATRIX3X2DVPROC glad_glUniformMatrix3x2dv = NULL; +PFNGLUNIFORMMATRIX3X2FVPROC glad_glUniformMatrix3x2fv = NULL; +PFNGLUNIFORMMATRIX3X4DVPROC glad_glUniformMatrix3x4dv = NULL; +PFNGLUNIFORMMATRIX3X4FVPROC glad_glUniformMatrix3x4fv = NULL; +PFNGLUNIFORMMATRIX4DVPROC glad_glUniformMatrix4dv = NULL; +PFNGLUNIFORMMATRIX4FVPROC glad_glUniformMatrix4fv = NULL; +PFNGLUNIFORMMATRIX4X2DVPROC glad_glUniformMatrix4x2dv = NULL; +PFNGLUNIFORMMATRIX4X2FVPROC glad_glUniformMatrix4x2fv = NULL; +PFNGLUNIFORMMATRIX4X3DVPROC glad_glUniformMatrix4x3dv = NULL; +PFNGLUNIFORMMATRIX4X3FVPROC glad_glUniformMatrix4x3fv = NULL; +PFNGLUNIFORMSUBROUTINESUIVPROC glad_glUniformSubroutinesuiv = NULL; +PFNGLUNMAPBUFFERPROC glad_glUnmapBuffer = NULL; +PFNGLUSEPROGRAMPROC glad_glUseProgram = NULL; +PFNGLUSEPROGRAMSTAGESPROC glad_glUseProgramStages = NULL; +PFNGLVALIDATEPROGRAMPROC glad_glValidateProgram = NULL; +PFNGLVALIDATEPROGRAMPIPELINEPROC glad_glValidateProgramPipeline = NULL; +PFNGLVERTEX2DPROC glad_glVertex2d = NULL; +PFNGLVERTEX2DVPROC glad_glVertex2dv = NULL; +PFNGLVERTEX2FPROC glad_glVertex2f = NULL; +PFNGLVERTEX2FVPROC glad_glVertex2fv = NULL; +PFNGLVERTEX2IPROC glad_glVertex2i = NULL; +PFNGLVERTEX2IVPROC glad_glVertex2iv = NULL; +PFNGLVERTEX2SPROC glad_glVertex2s = NULL; +PFNGLVERTEX2SVPROC glad_glVertex2sv = NULL; +PFNGLVERTEX3DPROC glad_glVertex3d = NULL; +PFNGLVERTEX3DVPROC glad_glVertex3dv = NULL; +PFNGLVERTEX3FPROC glad_glVertex3f = NULL; +PFNGLVERTEX3FVPROC glad_glVertex3fv = NULL; +PFNGLVERTEX3IPROC glad_glVertex3i = NULL; +PFNGLVERTEX3IVPROC glad_glVertex3iv = NULL; +PFNGLVERTEX3SPROC glad_glVertex3s = NULL; +PFNGLVERTEX3SVPROC glad_glVertex3sv = NULL; +PFNGLVERTEX4DPROC glad_glVertex4d = NULL; +PFNGLVERTEX4DVPROC glad_glVertex4dv = NULL; +PFNGLVERTEX4FPROC glad_glVertex4f = NULL; +PFNGLVERTEX4FVPROC glad_glVertex4fv = NULL; +PFNGLVERTEX4IPROC glad_glVertex4i = NULL; +PFNGLVERTEX4IVPROC glad_glVertex4iv = NULL; +PFNGLVERTEX4SPROC glad_glVertex4s = NULL; +PFNGLVERTEX4SVPROC glad_glVertex4sv = NULL; +PFNGLVERTEXATTRIB1DPROC glad_glVertexAttrib1d = NULL; +PFNGLVERTEXATTRIB1DVPROC glad_glVertexAttrib1dv = NULL; +PFNGLVERTEXATTRIB1FPROC glad_glVertexAttrib1f = NULL; +PFNGLVERTEXATTRIB1FVPROC glad_glVertexAttrib1fv = NULL; +PFNGLVERTEXATTRIB1SPROC glad_glVertexAttrib1s = NULL; +PFNGLVERTEXATTRIB1SVPROC glad_glVertexAttrib1sv = NULL; +PFNGLVERTEXATTRIB2DPROC glad_glVertexAttrib2d = NULL; +PFNGLVERTEXATTRIB2DVPROC glad_glVertexAttrib2dv = NULL; +PFNGLVERTEXATTRIB2FPROC glad_glVertexAttrib2f = NULL; +PFNGLVERTEXATTRIB2FVPROC glad_glVertexAttrib2fv = NULL; +PFNGLVERTEXATTRIB2SPROC glad_glVertexAttrib2s = NULL; +PFNGLVERTEXATTRIB2SVPROC glad_glVertexAttrib2sv = NULL; +PFNGLVERTEXATTRIB3DPROC glad_glVertexAttrib3d = NULL; +PFNGLVERTEXATTRIB3DVPROC glad_glVertexAttrib3dv = NULL; +PFNGLVERTEXATTRIB3FPROC glad_glVertexAttrib3f = NULL; +PFNGLVERTEXATTRIB3FVPROC glad_glVertexAttrib3fv = NULL; +PFNGLVERTEXATTRIB3SPROC glad_glVertexAttrib3s = NULL; +PFNGLVERTEXATTRIB3SVPROC glad_glVertexAttrib3sv = NULL; +PFNGLVERTEXATTRIB4NBVPROC glad_glVertexAttrib4Nbv = NULL; +PFNGLVERTEXATTRIB4NIVPROC glad_glVertexAttrib4Niv = NULL; +PFNGLVERTEXATTRIB4NSVPROC glad_glVertexAttrib4Nsv = NULL; +PFNGLVERTEXATTRIB4NUBPROC glad_glVertexAttrib4Nub = NULL; +PFNGLVERTEXATTRIB4NUBVPROC glad_glVertexAttrib4Nubv = NULL; +PFNGLVERTEXATTRIB4NUIVPROC glad_glVertexAttrib4Nuiv = NULL; +PFNGLVERTEXATTRIB4NUSVPROC glad_glVertexAttrib4Nusv = NULL; +PFNGLVERTEXATTRIB4BVPROC glad_glVertexAttrib4bv = NULL; +PFNGLVERTEXATTRIB4DPROC glad_glVertexAttrib4d = NULL; +PFNGLVERTEXATTRIB4DVPROC glad_glVertexAttrib4dv = NULL; +PFNGLVERTEXATTRIB4FPROC glad_glVertexAttrib4f = NULL; +PFNGLVERTEXATTRIB4FVPROC glad_glVertexAttrib4fv = NULL; +PFNGLVERTEXATTRIB4IVPROC glad_glVertexAttrib4iv = NULL; +PFNGLVERTEXATTRIB4SPROC glad_glVertexAttrib4s = NULL; +PFNGLVERTEXATTRIB4SVPROC glad_glVertexAttrib4sv = NULL; +PFNGLVERTEXATTRIB4UBVPROC glad_glVertexAttrib4ubv = NULL; +PFNGLVERTEXATTRIB4UIVPROC glad_glVertexAttrib4uiv = NULL; +PFNGLVERTEXATTRIB4USVPROC glad_glVertexAttrib4usv = NULL; +PFNGLVERTEXATTRIBDIVISORPROC glad_glVertexAttribDivisor = NULL; +PFNGLVERTEXATTRIBI1IPROC glad_glVertexAttribI1i = NULL; +PFNGLVERTEXATTRIBI1IVPROC glad_glVertexAttribI1iv = NULL; +PFNGLVERTEXATTRIBI1UIPROC glad_glVertexAttribI1ui = NULL; +PFNGLVERTEXATTRIBI1UIVPROC glad_glVertexAttribI1uiv = NULL; +PFNGLVERTEXATTRIBI2IPROC glad_glVertexAttribI2i = NULL; +PFNGLVERTEXATTRIBI2IVPROC glad_glVertexAttribI2iv = NULL; +PFNGLVERTEXATTRIBI2UIPROC glad_glVertexAttribI2ui = NULL; +PFNGLVERTEXATTRIBI2UIVPROC glad_glVertexAttribI2uiv = NULL; +PFNGLVERTEXATTRIBI3IPROC glad_glVertexAttribI3i = NULL; +PFNGLVERTEXATTRIBI3IVPROC glad_glVertexAttribI3iv = NULL; +PFNGLVERTEXATTRIBI3UIPROC glad_glVertexAttribI3ui = NULL; +PFNGLVERTEXATTRIBI3UIVPROC glad_glVertexAttribI3uiv = NULL; +PFNGLVERTEXATTRIBI4BVPROC glad_glVertexAttribI4bv = NULL; +PFNGLVERTEXATTRIBI4IPROC glad_glVertexAttribI4i = NULL; +PFNGLVERTEXATTRIBI4IVPROC glad_glVertexAttribI4iv = NULL; +PFNGLVERTEXATTRIBI4SVPROC glad_glVertexAttribI4sv = NULL; +PFNGLVERTEXATTRIBI4UBVPROC glad_glVertexAttribI4ubv = NULL; +PFNGLVERTEXATTRIBI4UIPROC glad_glVertexAttribI4ui = NULL; +PFNGLVERTEXATTRIBI4UIVPROC glad_glVertexAttribI4uiv = NULL; +PFNGLVERTEXATTRIBI4USVPROC glad_glVertexAttribI4usv = NULL; +PFNGLVERTEXATTRIBIPOINTERPROC glad_glVertexAttribIPointer = NULL; +PFNGLVERTEXATTRIBL1DPROC glad_glVertexAttribL1d = NULL; +PFNGLVERTEXATTRIBL1DVPROC glad_glVertexAttribL1dv = NULL; +PFNGLVERTEXATTRIBL2DPROC glad_glVertexAttribL2d = NULL; +PFNGLVERTEXATTRIBL2DVPROC glad_glVertexAttribL2dv = NULL; +PFNGLVERTEXATTRIBL3DPROC glad_glVertexAttribL3d = NULL; +PFNGLVERTEXATTRIBL3DVPROC glad_glVertexAttribL3dv = NULL; +PFNGLVERTEXATTRIBL4DPROC glad_glVertexAttribL4d = NULL; +PFNGLVERTEXATTRIBL4DVPROC glad_glVertexAttribL4dv = NULL; +PFNGLVERTEXATTRIBLPOINTERPROC glad_glVertexAttribLPointer = NULL; +PFNGLVERTEXATTRIBP1UIPROC glad_glVertexAttribP1ui = NULL; +PFNGLVERTEXATTRIBP1UIVPROC glad_glVertexAttribP1uiv = NULL; +PFNGLVERTEXATTRIBP2UIPROC glad_glVertexAttribP2ui = NULL; +PFNGLVERTEXATTRIBP2UIVPROC glad_glVertexAttribP2uiv = NULL; +PFNGLVERTEXATTRIBP3UIPROC glad_glVertexAttribP3ui = NULL; +PFNGLVERTEXATTRIBP3UIVPROC glad_glVertexAttribP3uiv = NULL; +PFNGLVERTEXATTRIBP4UIPROC glad_glVertexAttribP4ui = NULL; +PFNGLVERTEXATTRIBP4UIVPROC glad_glVertexAttribP4uiv = NULL; +PFNGLVERTEXATTRIBPOINTERPROC glad_glVertexAttribPointer = NULL; +PFNGLVERTEXP2UIPROC glad_glVertexP2ui = NULL; +PFNGLVERTEXP2UIVPROC glad_glVertexP2uiv = NULL; +PFNGLVERTEXP3UIPROC glad_glVertexP3ui = NULL; +PFNGLVERTEXP3UIVPROC glad_glVertexP3uiv = NULL; +PFNGLVERTEXP4UIPROC glad_glVertexP4ui = NULL; +PFNGLVERTEXP4UIVPROC glad_glVertexP4uiv = NULL; +PFNGLVERTEXPOINTERPROC glad_glVertexPointer = NULL; +PFNGLVIEWPORTPROC glad_glViewport = NULL; +PFNGLVIEWPORTARRAYVPROC glad_glViewportArrayv = NULL; +PFNGLVIEWPORTINDEXEDFPROC glad_glViewportIndexedf = NULL; +PFNGLVIEWPORTINDEXEDFVPROC glad_glViewportIndexedfv = NULL; +PFNGLWAITSYNCPROC glad_glWaitSync = NULL; +PFNGLWINDOWPOS2DPROC glad_glWindowPos2d = NULL; +PFNGLWINDOWPOS2DVPROC glad_glWindowPos2dv = NULL; +PFNGLWINDOWPOS2FPROC glad_glWindowPos2f = NULL; +PFNGLWINDOWPOS2FVPROC glad_glWindowPos2fv = NULL; +PFNGLWINDOWPOS2IPROC glad_glWindowPos2i = NULL; +PFNGLWINDOWPOS2IVPROC glad_glWindowPos2iv = NULL; +PFNGLWINDOWPOS2SPROC glad_glWindowPos2s = NULL; +PFNGLWINDOWPOS2SVPROC glad_glWindowPos2sv = NULL; +PFNGLWINDOWPOS3DPROC glad_glWindowPos3d = NULL; +PFNGLWINDOWPOS3DVPROC glad_glWindowPos3dv = NULL; +PFNGLWINDOWPOS3FPROC glad_glWindowPos3f = NULL; +PFNGLWINDOWPOS3FVPROC glad_glWindowPos3fv = NULL; +PFNGLWINDOWPOS3IPROC glad_glWindowPos3i = NULL; +PFNGLWINDOWPOS3IVPROC glad_glWindowPos3iv = NULL; +PFNGLWINDOWPOS3SPROC glad_glWindowPos3s = NULL; +PFNGLWINDOWPOS3SVPROC glad_glWindowPos3sv = NULL; +static void load_GL_VERSION_1_0(GLADloadproc load) { + if(!GLAD_GL_VERSION_1_0) return; + glad_glCullFace = (PFNGLCULLFACEPROC)load("glCullFace"); + glad_glFrontFace = (PFNGLFRONTFACEPROC)load("glFrontFace"); + glad_glHint = (PFNGLHINTPROC)load("glHint"); + glad_glLineWidth = (PFNGLLINEWIDTHPROC)load("glLineWidth"); + glad_glPointSize = (PFNGLPOINTSIZEPROC)load("glPointSize"); + glad_glPolygonMode = (PFNGLPOLYGONMODEPROC)load("glPolygonMode"); + glad_glScissor = (PFNGLSCISSORPROC)load("glScissor"); + glad_glTexParameterf = (PFNGLTEXPARAMETERFPROC)load("glTexParameterf"); + glad_glTexParameterfv = (PFNGLTEXPARAMETERFVPROC)load("glTexParameterfv"); + glad_glTexParameteri = (PFNGLTEXPARAMETERIPROC)load("glTexParameteri"); + glad_glTexParameteriv = (PFNGLTEXPARAMETERIVPROC)load("glTexParameteriv"); + glad_glTexImage1D = (PFNGLTEXIMAGE1DPROC)load("glTexImage1D"); + glad_glTexImage2D = (PFNGLTEXIMAGE2DPROC)load("glTexImage2D"); + glad_glDrawBuffer = (PFNGLDRAWBUFFERPROC)load("glDrawBuffer"); + glad_glClear = (PFNGLCLEARPROC)load("glClear"); + glad_glClearColor = (PFNGLCLEARCOLORPROC)load("glClearColor"); + glad_glClearStencil = (PFNGLCLEARSTENCILPROC)load("glClearStencil"); + glad_glClearDepth = (PFNGLCLEARDEPTHPROC)load("glClearDepth"); + glad_glStencilMask = (PFNGLSTENCILMASKPROC)load("glStencilMask"); + glad_glColorMask = (PFNGLCOLORMASKPROC)load("glColorMask"); + glad_glDepthMask = (PFNGLDEPTHMASKPROC)load("glDepthMask"); + glad_glDisable = (PFNGLDISABLEPROC)load("glDisable"); + glad_glEnable = (PFNGLENABLEPROC)load("glEnable"); + glad_glFinish = (PFNGLFINISHPROC)load("glFinish"); + glad_glFlush = (PFNGLFLUSHPROC)load("glFlush"); + glad_glBlendFunc = (PFNGLBLENDFUNCPROC)load("glBlendFunc"); + glad_glLogicOp = (PFNGLLOGICOPPROC)load("glLogicOp"); + glad_glStencilFunc = (PFNGLSTENCILFUNCPROC)load("glStencilFunc"); + glad_glStencilOp = (PFNGLSTENCILOPPROC)load("glStencilOp"); + glad_glDepthFunc = (PFNGLDEPTHFUNCPROC)load("glDepthFunc"); + glad_glPixelStoref = (PFNGLPIXELSTOREFPROC)load("glPixelStoref"); + glad_glPixelStorei = (PFNGLPIXELSTOREIPROC)load("glPixelStorei"); + glad_glReadBuffer = (PFNGLREADBUFFERPROC)load("glReadBuffer"); + glad_glReadPixels = (PFNGLREADPIXELSPROC)load("glReadPixels"); + glad_glGetBooleanv = (PFNGLGETBOOLEANVPROC)load("glGetBooleanv"); + glad_glGetDoublev = (PFNGLGETDOUBLEVPROC)load("glGetDoublev"); + glad_glGetError = (PFNGLGETERRORPROC)load("glGetError"); + glad_glGetFloatv = (PFNGLGETFLOATVPROC)load("glGetFloatv"); + glad_glGetIntegerv = (PFNGLGETINTEGERVPROC)load("glGetIntegerv"); + glad_glGetString = (PFNGLGETSTRINGPROC)load("glGetString"); + glad_glGetTexImage = (PFNGLGETTEXIMAGEPROC)load("glGetTexImage"); + glad_glGetTexParameterfv = (PFNGLGETTEXPARAMETERFVPROC)load("glGetTexParameterfv"); + glad_glGetTexParameteriv = (PFNGLGETTEXPARAMETERIVPROC)load("glGetTexParameteriv"); + glad_glGetTexLevelParameterfv = (PFNGLGETTEXLEVELPARAMETERFVPROC)load("glGetTexLevelParameterfv"); + glad_glGetTexLevelParameteriv = (PFNGLGETTEXLEVELPARAMETERIVPROC)load("glGetTexLevelParameteriv"); + glad_glIsEnabled = (PFNGLISENABLEDPROC)load("glIsEnabled"); + glad_glDepthRange = (PFNGLDEPTHRANGEPROC)load("glDepthRange"); + glad_glViewport = (PFNGLVIEWPORTPROC)load("glViewport"); + glad_glNewList = (PFNGLNEWLISTPROC)load("glNewList"); + glad_glEndList = (PFNGLENDLISTPROC)load("glEndList"); + glad_glCallList = (PFNGLCALLLISTPROC)load("glCallList"); + glad_glCallLists = (PFNGLCALLLISTSPROC)load("glCallLists"); + glad_glDeleteLists = (PFNGLDELETELISTSPROC)load("glDeleteLists"); + glad_glGenLists = (PFNGLGENLISTSPROC)load("glGenLists"); + glad_glListBase = (PFNGLLISTBASEPROC)load("glListBase"); + glad_glBegin = (PFNGLBEGINPROC)load("glBegin"); + glad_glBitmap = (PFNGLBITMAPPROC)load("glBitmap"); + glad_glColor3b = (PFNGLCOLOR3BPROC)load("glColor3b"); + glad_glColor3bv = (PFNGLCOLOR3BVPROC)load("glColor3bv"); + glad_glColor3d = (PFNGLCOLOR3DPROC)load("glColor3d"); + glad_glColor3dv = (PFNGLCOLOR3DVPROC)load("glColor3dv"); + glad_glColor3f = (PFNGLCOLOR3FPROC)load("glColor3f"); + glad_glColor3fv = (PFNGLCOLOR3FVPROC)load("glColor3fv"); + glad_glColor3i = (PFNGLCOLOR3IPROC)load("glColor3i"); + glad_glColor3iv = (PFNGLCOLOR3IVPROC)load("glColor3iv"); + glad_glColor3s = (PFNGLCOLOR3SPROC)load("glColor3s"); + glad_glColor3sv = (PFNGLCOLOR3SVPROC)load("glColor3sv"); + glad_glColor3ub = (PFNGLCOLOR3UBPROC)load("glColor3ub"); + glad_glColor3ubv = (PFNGLCOLOR3UBVPROC)load("glColor3ubv"); + glad_glColor3ui = (PFNGLCOLOR3UIPROC)load("glColor3ui"); + glad_glColor3uiv = (PFNGLCOLOR3UIVPROC)load("glColor3uiv"); + glad_glColor3us = (PFNGLCOLOR3USPROC)load("glColor3us"); + glad_glColor3usv = (PFNGLCOLOR3USVPROC)load("glColor3usv"); + glad_glColor4b = (PFNGLCOLOR4BPROC)load("glColor4b"); + glad_glColor4bv = (PFNGLCOLOR4BVPROC)load("glColor4bv"); + glad_glColor4d = (PFNGLCOLOR4DPROC)load("glColor4d"); + glad_glColor4dv = (PFNGLCOLOR4DVPROC)load("glColor4dv"); + glad_glColor4f = (PFNGLCOLOR4FPROC)load("glColor4f"); + glad_glColor4fv = (PFNGLCOLOR4FVPROC)load("glColor4fv"); + glad_glColor4i = (PFNGLCOLOR4IPROC)load("glColor4i"); + glad_glColor4iv = (PFNGLCOLOR4IVPROC)load("glColor4iv"); + glad_glColor4s = (PFNGLCOLOR4SPROC)load("glColor4s"); + glad_glColor4sv = (PFNGLCOLOR4SVPROC)load("glColor4sv"); + glad_glColor4ub = (PFNGLCOLOR4UBPROC)load("glColor4ub"); + glad_glColor4ubv = (PFNGLCOLOR4UBVPROC)load("glColor4ubv"); + glad_glColor4ui = (PFNGLCOLOR4UIPROC)load("glColor4ui"); + glad_glColor4uiv = (PFNGLCOLOR4UIVPROC)load("glColor4uiv"); + glad_glColor4us = (PFNGLCOLOR4USPROC)load("glColor4us"); + glad_glColor4usv = (PFNGLCOLOR4USVPROC)load("glColor4usv"); + glad_glEdgeFlag = (PFNGLEDGEFLAGPROC)load("glEdgeFlag"); + glad_glEdgeFlagv = (PFNGLEDGEFLAGVPROC)load("glEdgeFlagv"); + glad_glEnd = (PFNGLENDPROC)load("glEnd"); + glad_glIndexd = (PFNGLINDEXDPROC)load("glIndexd"); + glad_glIndexdv = (PFNGLINDEXDVPROC)load("glIndexdv"); + glad_glIndexf = (PFNGLINDEXFPROC)load("glIndexf"); + glad_glIndexfv = (PFNGLINDEXFVPROC)load("glIndexfv"); + glad_glIndexi = (PFNGLINDEXIPROC)load("glIndexi"); + glad_glIndexiv = (PFNGLINDEXIVPROC)load("glIndexiv"); + glad_glIndexs = (PFNGLINDEXSPROC)load("glIndexs"); + glad_glIndexsv = (PFNGLINDEXSVPROC)load("glIndexsv"); + glad_glNormal3b = (PFNGLNORMAL3BPROC)load("glNormal3b"); + glad_glNormal3bv = (PFNGLNORMAL3BVPROC)load("glNormal3bv"); + glad_glNormal3d = (PFNGLNORMAL3DPROC)load("glNormal3d"); + glad_glNormal3dv = (PFNGLNORMAL3DVPROC)load("glNormal3dv"); + glad_glNormal3f = (PFNGLNORMAL3FPROC)load("glNormal3f"); + glad_glNormal3fv = (PFNGLNORMAL3FVPROC)load("glNormal3fv"); + glad_glNormal3i = (PFNGLNORMAL3IPROC)load("glNormal3i"); + glad_glNormal3iv = (PFNGLNORMAL3IVPROC)load("glNormal3iv"); + glad_glNormal3s = (PFNGLNORMAL3SPROC)load("glNormal3s"); + glad_glNormal3sv = (PFNGLNORMAL3SVPROC)load("glNormal3sv"); + glad_glRasterPos2d = (PFNGLRASTERPOS2DPROC)load("glRasterPos2d"); + glad_glRasterPos2dv = (PFNGLRASTERPOS2DVPROC)load("glRasterPos2dv"); + glad_glRasterPos2f = (PFNGLRASTERPOS2FPROC)load("glRasterPos2f"); + glad_glRasterPos2fv = (PFNGLRASTERPOS2FVPROC)load("glRasterPos2fv"); + glad_glRasterPos2i = (PFNGLRASTERPOS2IPROC)load("glRasterPos2i"); + glad_glRasterPos2iv = (PFNGLRASTERPOS2IVPROC)load("glRasterPos2iv"); + glad_glRasterPos2s = (PFNGLRASTERPOS2SPROC)load("glRasterPos2s"); + glad_glRasterPos2sv = (PFNGLRASTERPOS2SVPROC)load("glRasterPos2sv"); + glad_glRasterPos3d = (PFNGLRASTERPOS3DPROC)load("glRasterPos3d"); + glad_glRasterPos3dv = (PFNGLRASTERPOS3DVPROC)load("glRasterPos3dv"); + glad_glRasterPos3f = (PFNGLRASTERPOS3FPROC)load("glRasterPos3f"); + glad_glRasterPos3fv = (PFNGLRASTERPOS3FVPROC)load("glRasterPos3fv"); + glad_glRasterPos3i = (PFNGLRASTERPOS3IPROC)load("glRasterPos3i"); + glad_glRasterPos3iv = (PFNGLRASTERPOS3IVPROC)load("glRasterPos3iv"); + glad_glRasterPos3s = (PFNGLRASTERPOS3SPROC)load("glRasterPos3s"); + glad_glRasterPos3sv = (PFNGLRASTERPOS3SVPROC)load("glRasterPos3sv"); + glad_glRasterPos4d = (PFNGLRASTERPOS4DPROC)load("glRasterPos4d"); + glad_glRasterPos4dv = (PFNGLRASTERPOS4DVPROC)load("glRasterPos4dv"); + glad_glRasterPos4f = (PFNGLRASTERPOS4FPROC)load("glRasterPos4f"); + glad_glRasterPos4fv = (PFNGLRASTERPOS4FVPROC)load("glRasterPos4fv"); + glad_glRasterPos4i = (PFNGLRASTERPOS4IPROC)load("glRasterPos4i"); + glad_glRasterPos4iv = (PFNGLRASTERPOS4IVPROC)load("glRasterPos4iv"); + glad_glRasterPos4s = (PFNGLRASTERPOS4SPROC)load("glRasterPos4s"); + glad_glRasterPos4sv = (PFNGLRASTERPOS4SVPROC)load("glRasterPos4sv"); + glad_glRectd = (PFNGLRECTDPROC)load("glRectd"); + glad_glRectdv = (PFNGLRECTDVPROC)load("glRectdv"); + glad_glRectf = (PFNGLRECTFPROC)load("glRectf"); + glad_glRectfv = (PFNGLRECTFVPROC)load("glRectfv"); + glad_glRecti = (PFNGLRECTIPROC)load("glRecti"); + glad_glRectiv = (PFNGLRECTIVPROC)load("glRectiv"); + glad_glRects = (PFNGLRECTSPROC)load("glRects"); + glad_glRectsv = (PFNGLRECTSVPROC)load("glRectsv"); + glad_glTexCoord1d = (PFNGLTEXCOORD1DPROC)load("glTexCoord1d"); + glad_glTexCoord1dv = (PFNGLTEXCOORD1DVPROC)load("glTexCoord1dv"); + glad_glTexCoord1f = (PFNGLTEXCOORD1FPROC)load("glTexCoord1f"); + glad_glTexCoord1fv = (PFNGLTEXCOORD1FVPROC)load("glTexCoord1fv"); + glad_glTexCoord1i = (PFNGLTEXCOORD1IPROC)load("glTexCoord1i"); + glad_glTexCoord1iv = (PFNGLTEXCOORD1IVPROC)load("glTexCoord1iv"); + glad_glTexCoord1s = (PFNGLTEXCOORD1SPROC)load("glTexCoord1s"); + glad_glTexCoord1sv = (PFNGLTEXCOORD1SVPROC)load("glTexCoord1sv"); + glad_glTexCoord2d = (PFNGLTEXCOORD2DPROC)load("glTexCoord2d"); + glad_glTexCoord2dv = (PFNGLTEXCOORD2DVPROC)load("glTexCoord2dv"); + glad_glTexCoord2f = (PFNGLTEXCOORD2FPROC)load("glTexCoord2f"); + glad_glTexCoord2fv = (PFNGLTEXCOORD2FVPROC)load("glTexCoord2fv"); + glad_glTexCoord2i = (PFNGLTEXCOORD2IPROC)load("glTexCoord2i"); + glad_glTexCoord2iv = (PFNGLTEXCOORD2IVPROC)load("glTexCoord2iv"); + glad_glTexCoord2s = (PFNGLTEXCOORD2SPROC)load("glTexCoord2s"); + glad_glTexCoord2sv = (PFNGLTEXCOORD2SVPROC)load("glTexCoord2sv"); + glad_glTexCoord3d = (PFNGLTEXCOORD3DPROC)load("glTexCoord3d"); + glad_glTexCoord3dv = (PFNGLTEXCOORD3DVPROC)load("glTexCoord3dv"); + glad_glTexCoord3f = (PFNGLTEXCOORD3FPROC)load("glTexCoord3f"); + glad_glTexCoord3fv = (PFNGLTEXCOORD3FVPROC)load("glTexCoord3fv"); + glad_glTexCoord3i = (PFNGLTEXCOORD3IPROC)load("glTexCoord3i"); + glad_glTexCoord3iv = (PFNGLTEXCOORD3IVPROC)load("glTexCoord3iv"); + glad_glTexCoord3s = (PFNGLTEXCOORD3SPROC)load("glTexCoord3s"); + glad_glTexCoord3sv = (PFNGLTEXCOORD3SVPROC)load("glTexCoord3sv"); + glad_glTexCoord4d = (PFNGLTEXCOORD4DPROC)load("glTexCoord4d"); + glad_glTexCoord4dv = (PFNGLTEXCOORD4DVPROC)load("glTexCoord4dv"); + glad_glTexCoord4f = (PFNGLTEXCOORD4FPROC)load("glTexCoord4f"); + glad_glTexCoord4fv = (PFNGLTEXCOORD4FVPROC)load("glTexCoord4fv"); + glad_glTexCoord4i = (PFNGLTEXCOORD4IPROC)load("glTexCoord4i"); + glad_glTexCoord4iv = (PFNGLTEXCOORD4IVPROC)load("glTexCoord4iv"); + glad_glTexCoord4s = (PFNGLTEXCOORD4SPROC)load("glTexCoord4s"); + glad_glTexCoord4sv = (PFNGLTEXCOORD4SVPROC)load("glTexCoord4sv"); + glad_glVertex2d = (PFNGLVERTEX2DPROC)load("glVertex2d"); + glad_glVertex2dv = (PFNGLVERTEX2DVPROC)load("glVertex2dv"); + glad_glVertex2f = (PFNGLVERTEX2FPROC)load("glVertex2f"); + glad_glVertex2fv = (PFNGLVERTEX2FVPROC)load("glVertex2fv"); + glad_glVertex2i = (PFNGLVERTEX2IPROC)load("glVertex2i"); + glad_glVertex2iv = (PFNGLVERTEX2IVPROC)load("glVertex2iv"); + glad_glVertex2s = (PFNGLVERTEX2SPROC)load("glVertex2s"); + glad_glVertex2sv = (PFNGLVERTEX2SVPROC)load("glVertex2sv"); + glad_glVertex3d = (PFNGLVERTEX3DPROC)load("glVertex3d"); + glad_glVertex3dv = (PFNGLVERTEX3DVPROC)load("glVertex3dv"); + glad_glVertex3f = (PFNGLVERTEX3FPROC)load("glVertex3f"); + glad_glVertex3fv = (PFNGLVERTEX3FVPROC)load("glVertex3fv"); + glad_glVertex3i = (PFNGLVERTEX3IPROC)load("glVertex3i"); + glad_glVertex3iv = (PFNGLVERTEX3IVPROC)load("glVertex3iv"); + glad_glVertex3s = (PFNGLVERTEX3SPROC)load("glVertex3s"); + glad_glVertex3sv = (PFNGLVERTEX3SVPROC)load("glVertex3sv"); + glad_glVertex4d = (PFNGLVERTEX4DPROC)load("glVertex4d"); + glad_glVertex4dv = (PFNGLVERTEX4DVPROC)load("glVertex4dv"); + glad_glVertex4f = (PFNGLVERTEX4FPROC)load("glVertex4f"); + glad_glVertex4fv = (PFNGLVERTEX4FVPROC)load("glVertex4fv"); + glad_glVertex4i = (PFNGLVERTEX4IPROC)load("glVertex4i"); + glad_glVertex4iv = (PFNGLVERTEX4IVPROC)load("glVertex4iv"); + glad_glVertex4s = (PFNGLVERTEX4SPROC)load("glVertex4s"); + glad_glVertex4sv = (PFNGLVERTEX4SVPROC)load("glVertex4sv"); + glad_glClipPlane = (PFNGLCLIPPLANEPROC)load("glClipPlane"); + glad_glColorMaterial = (PFNGLCOLORMATERIALPROC)load("glColorMaterial"); + glad_glFogf = (PFNGLFOGFPROC)load("glFogf"); + glad_glFogfv = (PFNGLFOGFVPROC)load("glFogfv"); + glad_glFogi = (PFNGLFOGIPROC)load("glFogi"); + glad_glFogiv = (PFNGLFOGIVPROC)load("glFogiv"); + glad_glLightf = (PFNGLLIGHTFPROC)load("glLightf"); + glad_glLightfv = (PFNGLLIGHTFVPROC)load("glLightfv"); + glad_glLighti = (PFNGLLIGHTIPROC)load("glLighti"); + glad_glLightiv = (PFNGLLIGHTIVPROC)load("glLightiv"); + glad_glLightModelf = (PFNGLLIGHTMODELFPROC)load("glLightModelf"); + glad_glLightModelfv = (PFNGLLIGHTMODELFVPROC)load("glLightModelfv"); + glad_glLightModeli = (PFNGLLIGHTMODELIPROC)load("glLightModeli"); + glad_glLightModeliv = (PFNGLLIGHTMODELIVPROC)load("glLightModeliv"); + glad_glLineStipple = (PFNGLLINESTIPPLEPROC)load("glLineStipple"); + glad_glMaterialf = (PFNGLMATERIALFPROC)load("glMaterialf"); + glad_glMaterialfv = (PFNGLMATERIALFVPROC)load("glMaterialfv"); + glad_glMateriali = (PFNGLMATERIALIPROC)load("glMateriali"); + glad_glMaterialiv = (PFNGLMATERIALIVPROC)load("glMaterialiv"); + glad_glPolygonStipple = (PFNGLPOLYGONSTIPPLEPROC)load("glPolygonStipple"); + glad_glShadeModel = (PFNGLSHADEMODELPROC)load("glShadeModel"); + glad_glTexEnvf = (PFNGLTEXENVFPROC)load("glTexEnvf"); + glad_glTexEnvfv = (PFNGLTEXENVFVPROC)load("glTexEnvfv"); + glad_glTexEnvi = (PFNGLTEXENVIPROC)load("glTexEnvi"); + glad_glTexEnviv = (PFNGLTEXENVIVPROC)load("glTexEnviv"); + glad_glTexGend = (PFNGLTEXGENDPROC)load("glTexGend"); + glad_glTexGendv = (PFNGLTEXGENDVPROC)load("glTexGendv"); + glad_glTexGenf = (PFNGLTEXGENFPROC)load("glTexGenf"); + glad_glTexGenfv = (PFNGLTEXGENFVPROC)load("glTexGenfv"); + glad_glTexGeni = (PFNGLTEXGENIPROC)load("glTexGeni"); + glad_glTexGeniv = (PFNGLTEXGENIVPROC)load("glTexGeniv"); + glad_glFeedbackBuffer = (PFNGLFEEDBACKBUFFERPROC)load("glFeedbackBuffer"); + glad_glSelectBuffer = (PFNGLSELECTBUFFERPROC)load("glSelectBuffer"); + glad_glRenderMode = (PFNGLRENDERMODEPROC)load("glRenderMode"); + glad_glInitNames = (PFNGLINITNAMESPROC)load("glInitNames"); + glad_glLoadName = (PFNGLLOADNAMEPROC)load("glLoadName"); + glad_glPassThrough = (PFNGLPASSTHROUGHPROC)load("glPassThrough"); + glad_glPopName = (PFNGLPOPNAMEPROC)load("glPopName"); + glad_glPushName = (PFNGLPUSHNAMEPROC)load("glPushName"); + glad_glClearAccum = (PFNGLCLEARACCUMPROC)load("glClearAccum"); + glad_glClearIndex = (PFNGLCLEARINDEXPROC)load("glClearIndex"); + glad_glIndexMask = (PFNGLINDEXMASKPROC)load("glIndexMask"); + glad_glAccum = (PFNGLACCUMPROC)load("glAccum"); + glad_glPopAttrib = (PFNGLPOPATTRIBPROC)load("glPopAttrib"); + glad_glPushAttrib = (PFNGLPUSHATTRIBPROC)load("glPushAttrib"); + glad_glMap1d = (PFNGLMAP1DPROC)load("glMap1d"); + glad_glMap1f = (PFNGLMAP1FPROC)load("glMap1f"); + glad_glMap2d = (PFNGLMAP2DPROC)load("glMap2d"); + glad_glMap2f = (PFNGLMAP2FPROC)load("glMap2f"); + glad_glMapGrid1d = (PFNGLMAPGRID1DPROC)load("glMapGrid1d"); + glad_glMapGrid1f = (PFNGLMAPGRID1FPROC)load("glMapGrid1f"); + glad_glMapGrid2d = (PFNGLMAPGRID2DPROC)load("glMapGrid2d"); + glad_glMapGrid2f = (PFNGLMAPGRID2FPROC)load("glMapGrid2f"); + glad_glEvalCoord1d = (PFNGLEVALCOORD1DPROC)load("glEvalCoord1d"); + glad_glEvalCoord1dv = (PFNGLEVALCOORD1DVPROC)load("glEvalCoord1dv"); + glad_glEvalCoord1f = (PFNGLEVALCOORD1FPROC)load("glEvalCoord1f"); + glad_glEvalCoord1fv = (PFNGLEVALCOORD1FVPROC)load("glEvalCoord1fv"); + glad_glEvalCoord2d = (PFNGLEVALCOORD2DPROC)load("glEvalCoord2d"); + glad_glEvalCoord2dv = (PFNGLEVALCOORD2DVPROC)load("glEvalCoord2dv"); + glad_glEvalCoord2f = (PFNGLEVALCOORD2FPROC)load("glEvalCoord2f"); + glad_glEvalCoord2fv = (PFNGLEVALCOORD2FVPROC)load("glEvalCoord2fv"); + glad_glEvalMesh1 = (PFNGLEVALMESH1PROC)load("glEvalMesh1"); + glad_glEvalPoint1 = (PFNGLEVALPOINT1PROC)load("glEvalPoint1"); + glad_glEvalMesh2 = (PFNGLEVALMESH2PROC)load("glEvalMesh2"); + glad_glEvalPoint2 = (PFNGLEVALPOINT2PROC)load("glEvalPoint2"); + glad_glAlphaFunc = (PFNGLALPHAFUNCPROC)load("glAlphaFunc"); + glad_glPixelZoom = (PFNGLPIXELZOOMPROC)load("glPixelZoom"); + glad_glPixelTransferf = (PFNGLPIXELTRANSFERFPROC)load("glPixelTransferf"); + glad_glPixelTransferi = (PFNGLPIXELTRANSFERIPROC)load("glPixelTransferi"); + glad_glPixelMapfv = (PFNGLPIXELMAPFVPROC)load("glPixelMapfv"); + glad_glPixelMapuiv = (PFNGLPIXELMAPUIVPROC)load("glPixelMapuiv"); + glad_glPixelMapusv = (PFNGLPIXELMAPUSVPROC)load("glPixelMapusv"); + glad_glCopyPixels = (PFNGLCOPYPIXELSPROC)load("glCopyPixels"); + glad_glDrawPixels = (PFNGLDRAWPIXELSPROC)load("glDrawPixels"); + glad_glGetClipPlane = (PFNGLGETCLIPPLANEPROC)load("glGetClipPlane"); + glad_glGetLightfv = (PFNGLGETLIGHTFVPROC)load("glGetLightfv"); + glad_glGetLightiv = (PFNGLGETLIGHTIVPROC)load("glGetLightiv"); + glad_glGetMapdv = (PFNGLGETMAPDVPROC)load("glGetMapdv"); + glad_glGetMapfv = (PFNGLGETMAPFVPROC)load("glGetMapfv"); + glad_glGetMapiv = (PFNGLGETMAPIVPROC)load("glGetMapiv"); + glad_glGetMaterialfv = (PFNGLGETMATERIALFVPROC)load("glGetMaterialfv"); + glad_glGetMaterialiv = (PFNGLGETMATERIALIVPROC)load("glGetMaterialiv"); + glad_glGetPixelMapfv = (PFNGLGETPIXELMAPFVPROC)load("glGetPixelMapfv"); + glad_glGetPixelMapuiv = (PFNGLGETPIXELMAPUIVPROC)load("glGetPixelMapuiv"); + glad_glGetPixelMapusv = (PFNGLGETPIXELMAPUSVPROC)load("glGetPixelMapusv"); + glad_glGetPolygonStipple = (PFNGLGETPOLYGONSTIPPLEPROC)load("glGetPolygonStipple"); + glad_glGetTexEnvfv = (PFNGLGETTEXENVFVPROC)load("glGetTexEnvfv"); + glad_glGetTexEnviv = (PFNGLGETTEXENVIVPROC)load("glGetTexEnviv"); + glad_glGetTexGendv = (PFNGLGETTEXGENDVPROC)load("glGetTexGendv"); + glad_glGetTexGenfv = (PFNGLGETTEXGENFVPROC)load("glGetTexGenfv"); + glad_glGetTexGeniv = (PFNGLGETTEXGENIVPROC)load("glGetTexGeniv"); + glad_glIsList = (PFNGLISLISTPROC)load("glIsList"); + glad_glFrustum = (PFNGLFRUSTUMPROC)load("glFrustum"); + glad_glLoadIdentity = (PFNGLLOADIDENTITYPROC)load("glLoadIdentity"); + glad_glLoadMatrixf = (PFNGLLOADMATRIXFPROC)load("glLoadMatrixf"); + glad_glLoadMatrixd = (PFNGLLOADMATRIXDPROC)load("glLoadMatrixd"); + glad_glMatrixMode = (PFNGLMATRIXMODEPROC)load("glMatrixMode"); + glad_glMultMatrixf = (PFNGLMULTMATRIXFPROC)load("glMultMatrixf"); + glad_glMultMatrixd = (PFNGLMULTMATRIXDPROC)load("glMultMatrixd"); + glad_glOrtho = (PFNGLORTHOPROC)load("glOrtho"); + glad_glPopMatrix = (PFNGLPOPMATRIXPROC)load("glPopMatrix"); + glad_glPushMatrix = (PFNGLPUSHMATRIXPROC)load("glPushMatrix"); + glad_glRotated = (PFNGLROTATEDPROC)load("glRotated"); + glad_glRotatef = (PFNGLROTATEFPROC)load("glRotatef"); + glad_glScaled = (PFNGLSCALEDPROC)load("glScaled"); + glad_glScalef = (PFNGLSCALEFPROC)load("glScalef"); + glad_glTranslated = (PFNGLTRANSLATEDPROC)load("glTranslated"); + glad_glTranslatef = (PFNGLTRANSLATEFPROC)load("glTranslatef"); +} +static void load_GL_VERSION_1_1(GLADloadproc load) { + if(!GLAD_GL_VERSION_1_1) return; + glad_glDrawArrays = (PFNGLDRAWARRAYSPROC)load("glDrawArrays"); + glad_glDrawElements = (PFNGLDRAWELEMENTSPROC)load("glDrawElements"); + glad_glGetPointerv = (PFNGLGETPOINTERVPROC)load("glGetPointerv"); + glad_glPolygonOffset = (PFNGLPOLYGONOFFSETPROC)load("glPolygonOffset"); + glad_glCopyTexImage1D = (PFNGLCOPYTEXIMAGE1DPROC)load("glCopyTexImage1D"); + glad_glCopyTexImage2D = (PFNGLCOPYTEXIMAGE2DPROC)load("glCopyTexImage2D"); + glad_glCopyTexSubImage1D = (PFNGLCOPYTEXSUBIMAGE1DPROC)load("glCopyTexSubImage1D"); + glad_glCopyTexSubImage2D = (PFNGLCOPYTEXSUBIMAGE2DPROC)load("glCopyTexSubImage2D"); + glad_glTexSubImage1D = (PFNGLTEXSUBIMAGE1DPROC)load("glTexSubImage1D"); + glad_glTexSubImage2D = (PFNGLTEXSUBIMAGE2DPROC)load("glTexSubImage2D"); + glad_glBindTexture = (PFNGLBINDTEXTUREPROC)load("glBindTexture"); + glad_glDeleteTextures = (PFNGLDELETETEXTURESPROC)load("glDeleteTextures"); + glad_glGenTextures = (PFNGLGENTEXTURESPROC)load("glGenTextures"); + glad_glIsTexture = (PFNGLISTEXTUREPROC)load("glIsTexture"); + glad_glArrayElement = (PFNGLARRAYELEMENTPROC)load("glArrayElement"); + glad_glColorPointer = (PFNGLCOLORPOINTERPROC)load("glColorPointer"); + glad_glDisableClientState = (PFNGLDISABLECLIENTSTATEPROC)load("glDisableClientState"); + glad_glEdgeFlagPointer = (PFNGLEDGEFLAGPOINTERPROC)load("glEdgeFlagPointer"); + glad_glEnableClientState = (PFNGLENABLECLIENTSTATEPROC)load("glEnableClientState"); + glad_glIndexPointer = (PFNGLINDEXPOINTERPROC)load("glIndexPointer"); + glad_glInterleavedArrays = (PFNGLINTERLEAVEDARRAYSPROC)load("glInterleavedArrays"); + glad_glNormalPointer = (PFNGLNORMALPOINTERPROC)load("glNormalPointer"); + glad_glTexCoordPointer = (PFNGLTEXCOORDPOINTERPROC)load("glTexCoordPointer"); + glad_glVertexPointer = (PFNGLVERTEXPOINTERPROC)load("glVertexPointer"); + glad_glAreTexturesResident = (PFNGLARETEXTURESRESIDENTPROC)load("glAreTexturesResident"); + glad_glPrioritizeTextures = (PFNGLPRIORITIZETEXTURESPROC)load("glPrioritizeTextures"); + glad_glIndexub = (PFNGLINDEXUBPROC)load("glIndexub"); + glad_glIndexubv = (PFNGLINDEXUBVPROC)load("glIndexubv"); + glad_glPopClientAttrib = (PFNGLPOPCLIENTATTRIBPROC)load("glPopClientAttrib"); + glad_glPushClientAttrib = (PFNGLPUSHCLIENTATTRIBPROC)load("glPushClientAttrib"); +} +static void load_GL_VERSION_1_2(GLADloadproc load) { + if(!GLAD_GL_VERSION_1_2) return; + glad_glDrawRangeElements = (PFNGLDRAWRANGEELEMENTSPROC)load("glDrawRangeElements"); + glad_glTexImage3D = (PFNGLTEXIMAGE3DPROC)load("glTexImage3D"); + glad_glTexSubImage3D = (PFNGLTEXSUBIMAGE3DPROC)load("glTexSubImage3D"); + glad_glCopyTexSubImage3D = (PFNGLCOPYTEXSUBIMAGE3DPROC)load("glCopyTexSubImage3D"); +} +static void load_GL_VERSION_1_3(GLADloadproc load) { + if(!GLAD_GL_VERSION_1_3) return; + glad_glActiveTexture = (PFNGLACTIVETEXTUREPROC)load("glActiveTexture"); + glad_glSampleCoverage = (PFNGLSAMPLECOVERAGEPROC)load("glSampleCoverage"); + glad_glCompressedTexImage3D = (PFNGLCOMPRESSEDTEXIMAGE3DPROC)load("glCompressedTexImage3D"); + glad_glCompressedTexImage2D = (PFNGLCOMPRESSEDTEXIMAGE2DPROC)load("glCompressedTexImage2D"); + glad_glCompressedTexImage1D = (PFNGLCOMPRESSEDTEXIMAGE1DPROC)load("glCompressedTexImage1D"); + glad_glCompressedTexSubImage3D = (PFNGLCOMPRESSEDTEXSUBIMAGE3DPROC)load("glCompressedTexSubImage3D"); + glad_glCompressedTexSubImage2D = (PFNGLCOMPRESSEDTEXSUBIMAGE2DPROC)load("glCompressedTexSubImage2D"); + glad_glCompressedTexSubImage1D = (PFNGLCOMPRESSEDTEXSUBIMAGE1DPROC)load("glCompressedTexSubImage1D"); + glad_glGetCompressedTexImage = (PFNGLGETCOMPRESSEDTEXIMAGEPROC)load("glGetCompressedTexImage"); + glad_glClientActiveTexture = (PFNGLCLIENTACTIVETEXTUREPROC)load("glClientActiveTexture"); + glad_glMultiTexCoord1d = (PFNGLMULTITEXCOORD1DPROC)load("glMultiTexCoord1d"); + glad_glMultiTexCoord1dv = (PFNGLMULTITEXCOORD1DVPROC)load("glMultiTexCoord1dv"); + glad_glMultiTexCoord1f = (PFNGLMULTITEXCOORD1FPROC)load("glMultiTexCoord1f"); + glad_glMultiTexCoord1fv = (PFNGLMULTITEXCOORD1FVPROC)load("glMultiTexCoord1fv"); + glad_glMultiTexCoord1i = (PFNGLMULTITEXCOORD1IPROC)load("glMultiTexCoord1i"); + glad_glMultiTexCoord1iv = (PFNGLMULTITEXCOORD1IVPROC)load("glMultiTexCoord1iv"); + glad_glMultiTexCoord1s = (PFNGLMULTITEXCOORD1SPROC)load("glMultiTexCoord1s"); + glad_glMultiTexCoord1sv = (PFNGLMULTITEXCOORD1SVPROC)load("glMultiTexCoord1sv"); + glad_glMultiTexCoord2d = (PFNGLMULTITEXCOORD2DPROC)load("glMultiTexCoord2d"); + glad_glMultiTexCoord2dv = (PFNGLMULTITEXCOORD2DVPROC)load("glMultiTexCoord2dv"); + glad_glMultiTexCoord2f = (PFNGLMULTITEXCOORD2FPROC)load("glMultiTexCoord2f"); + glad_glMultiTexCoord2fv = (PFNGLMULTITEXCOORD2FVPROC)load("glMultiTexCoord2fv"); + glad_glMultiTexCoord2i = (PFNGLMULTITEXCOORD2IPROC)load("glMultiTexCoord2i"); + glad_glMultiTexCoord2iv = (PFNGLMULTITEXCOORD2IVPROC)load("glMultiTexCoord2iv"); + glad_glMultiTexCoord2s = (PFNGLMULTITEXCOORD2SPROC)load("glMultiTexCoord2s"); + glad_glMultiTexCoord2sv = (PFNGLMULTITEXCOORD2SVPROC)load("glMultiTexCoord2sv"); + glad_glMultiTexCoord3d = (PFNGLMULTITEXCOORD3DPROC)load("glMultiTexCoord3d"); + glad_glMultiTexCoord3dv = (PFNGLMULTITEXCOORD3DVPROC)load("glMultiTexCoord3dv"); + glad_glMultiTexCoord3f = (PFNGLMULTITEXCOORD3FPROC)load("glMultiTexCoord3f"); + glad_glMultiTexCoord3fv = (PFNGLMULTITEXCOORD3FVPROC)load("glMultiTexCoord3fv"); + glad_glMultiTexCoord3i = (PFNGLMULTITEXCOORD3IPROC)load("glMultiTexCoord3i"); + glad_glMultiTexCoord3iv = (PFNGLMULTITEXCOORD3IVPROC)load("glMultiTexCoord3iv"); + glad_glMultiTexCoord3s = (PFNGLMULTITEXCOORD3SPROC)load("glMultiTexCoord3s"); + glad_glMultiTexCoord3sv = (PFNGLMULTITEXCOORD3SVPROC)load("glMultiTexCoord3sv"); + glad_glMultiTexCoord4d = (PFNGLMULTITEXCOORD4DPROC)load("glMultiTexCoord4d"); + glad_glMultiTexCoord4dv = (PFNGLMULTITEXCOORD4DVPROC)load("glMultiTexCoord4dv"); + glad_glMultiTexCoord4f = (PFNGLMULTITEXCOORD4FPROC)load("glMultiTexCoord4f"); + glad_glMultiTexCoord4fv = (PFNGLMULTITEXCOORD4FVPROC)load("glMultiTexCoord4fv"); + glad_glMultiTexCoord4i = (PFNGLMULTITEXCOORD4IPROC)load("glMultiTexCoord4i"); + glad_glMultiTexCoord4iv = (PFNGLMULTITEXCOORD4IVPROC)load("glMultiTexCoord4iv"); + glad_glMultiTexCoord4s = (PFNGLMULTITEXCOORD4SPROC)load("glMultiTexCoord4s"); + glad_glMultiTexCoord4sv = (PFNGLMULTITEXCOORD4SVPROC)load("glMultiTexCoord4sv"); + glad_glLoadTransposeMatrixf = (PFNGLLOADTRANSPOSEMATRIXFPROC)load("glLoadTransposeMatrixf"); + glad_glLoadTransposeMatrixd = (PFNGLLOADTRANSPOSEMATRIXDPROC)load("glLoadTransposeMatrixd"); + glad_glMultTransposeMatrixf = (PFNGLMULTTRANSPOSEMATRIXFPROC)load("glMultTransposeMatrixf"); + glad_glMultTransposeMatrixd = (PFNGLMULTTRANSPOSEMATRIXDPROC)load("glMultTransposeMatrixd"); +} +static void load_GL_VERSION_1_4(GLADloadproc load) { + if(!GLAD_GL_VERSION_1_4) return; + glad_glBlendFuncSeparate = (PFNGLBLENDFUNCSEPARATEPROC)load("glBlendFuncSeparate"); + glad_glMultiDrawArrays = (PFNGLMULTIDRAWARRAYSPROC)load("glMultiDrawArrays"); + glad_glMultiDrawElements = (PFNGLMULTIDRAWELEMENTSPROC)load("glMultiDrawElements"); + glad_glPointParameterf = (PFNGLPOINTPARAMETERFPROC)load("glPointParameterf"); + glad_glPointParameterfv = (PFNGLPOINTPARAMETERFVPROC)load("glPointParameterfv"); + glad_glPointParameteri = (PFNGLPOINTPARAMETERIPROC)load("glPointParameteri"); + glad_glPointParameteriv = (PFNGLPOINTPARAMETERIVPROC)load("glPointParameteriv"); + glad_glFogCoordf = (PFNGLFOGCOORDFPROC)load("glFogCoordf"); + glad_glFogCoordfv = (PFNGLFOGCOORDFVPROC)load("glFogCoordfv"); + glad_glFogCoordd = (PFNGLFOGCOORDDPROC)load("glFogCoordd"); + glad_glFogCoorddv = (PFNGLFOGCOORDDVPROC)load("glFogCoorddv"); + glad_glFogCoordPointer = (PFNGLFOGCOORDPOINTERPROC)load("glFogCoordPointer"); + glad_glSecondaryColor3b = (PFNGLSECONDARYCOLOR3BPROC)load("glSecondaryColor3b"); + glad_glSecondaryColor3bv = (PFNGLSECONDARYCOLOR3BVPROC)load("glSecondaryColor3bv"); + glad_glSecondaryColor3d = (PFNGLSECONDARYCOLOR3DPROC)load("glSecondaryColor3d"); + glad_glSecondaryColor3dv = (PFNGLSECONDARYCOLOR3DVPROC)load("glSecondaryColor3dv"); + glad_glSecondaryColor3f = (PFNGLSECONDARYCOLOR3FPROC)load("glSecondaryColor3f"); + glad_glSecondaryColor3fv = (PFNGLSECONDARYCOLOR3FVPROC)load("glSecondaryColor3fv"); + glad_glSecondaryColor3i = (PFNGLSECONDARYCOLOR3IPROC)load("glSecondaryColor3i"); + glad_glSecondaryColor3iv = (PFNGLSECONDARYCOLOR3IVPROC)load("glSecondaryColor3iv"); + glad_glSecondaryColor3s = (PFNGLSECONDARYCOLOR3SPROC)load("glSecondaryColor3s"); + glad_glSecondaryColor3sv = (PFNGLSECONDARYCOLOR3SVPROC)load("glSecondaryColor3sv"); + glad_glSecondaryColor3ub = (PFNGLSECONDARYCOLOR3UBPROC)load("glSecondaryColor3ub"); + glad_glSecondaryColor3ubv = (PFNGLSECONDARYCOLOR3UBVPROC)load("glSecondaryColor3ubv"); + glad_glSecondaryColor3ui = (PFNGLSECONDARYCOLOR3UIPROC)load("glSecondaryColor3ui"); + glad_glSecondaryColor3uiv = (PFNGLSECONDARYCOLOR3UIVPROC)load("glSecondaryColor3uiv"); + glad_glSecondaryColor3us = (PFNGLSECONDARYCOLOR3USPROC)load("glSecondaryColor3us"); + glad_glSecondaryColor3usv = (PFNGLSECONDARYCOLOR3USVPROC)load("glSecondaryColor3usv"); + glad_glSecondaryColorPointer = (PFNGLSECONDARYCOLORPOINTERPROC)load("glSecondaryColorPointer"); + glad_glWindowPos2d = (PFNGLWINDOWPOS2DPROC)load("glWindowPos2d"); + glad_glWindowPos2dv = (PFNGLWINDOWPOS2DVPROC)load("glWindowPos2dv"); + glad_glWindowPos2f = (PFNGLWINDOWPOS2FPROC)load("glWindowPos2f"); + glad_glWindowPos2fv = (PFNGLWINDOWPOS2FVPROC)load("glWindowPos2fv"); + glad_glWindowPos2i = (PFNGLWINDOWPOS2IPROC)load("glWindowPos2i"); + glad_glWindowPos2iv = (PFNGLWINDOWPOS2IVPROC)load("glWindowPos2iv"); + glad_glWindowPos2s = (PFNGLWINDOWPOS2SPROC)load("glWindowPos2s"); + glad_glWindowPos2sv = (PFNGLWINDOWPOS2SVPROC)load("glWindowPos2sv"); + glad_glWindowPos3d = (PFNGLWINDOWPOS3DPROC)load("glWindowPos3d"); + glad_glWindowPos3dv = (PFNGLWINDOWPOS3DVPROC)load("glWindowPos3dv"); + glad_glWindowPos3f = (PFNGLWINDOWPOS3FPROC)load("glWindowPos3f"); + glad_glWindowPos3fv = (PFNGLWINDOWPOS3FVPROC)load("glWindowPos3fv"); + glad_glWindowPos3i = (PFNGLWINDOWPOS3IPROC)load("glWindowPos3i"); + glad_glWindowPos3iv = (PFNGLWINDOWPOS3IVPROC)load("glWindowPos3iv"); + glad_glWindowPos3s = (PFNGLWINDOWPOS3SPROC)load("glWindowPos3s"); + glad_glWindowPos3sv = (PFNGLWINDOWPOS3SVPROC)load("glWindowPos3sv"); + glad_glBlendColor = (PFNGLBLENDCOLORPROC)load("glBlendColor"); + glad_glBlendEquation = (PFNGLBLENDEQUATIONPROC)load("glBlendEquation"); +} +static void load_GL_VERSION_1_5(GLADloadproc load) { + if(!GLAD_GL_VERSION_1_5) return; + glad_glGenQueries = (PFNGLGENQUERIESPROC)load("glGenQueries"); + glad_glDeleteQueries = (PFNGLDELETEQUERIESPROC)load("glDeleteQueries"); + glad_glIsQuery = (PFNGLISQUERYPROC)load("glIsQuery"); + glad_glBeginQuery = (PFNGLBEGINQUERYPROC)load("glBeginQuery"); + glad_glEndQuery = (PFNGLENDQUERYPROC)load("glEndQuery"); + glad_glGetQueryiv = (PFNGLGETQUERYIVPROC)load("glGetQueryiv"); + glad_glGetQueryObjectiv = (PFNGLGETQUERYOBJECTIVPROC)load("glGetQueryObjectiv"); + glad_glGetQueryObjectuiv = (PFNGLGETQUERYOBJECTUIVPROC)load("glGetQueryObjectuiv"); + glad_glBindBuffer = (PFNGLBINDBUFFERPROC)load("glBindBuffer"); + glad_glDeleteBuffers = (PFNGLDELETEBUFFERSPROC)load("glDeleteBuffers"); + glad_glGenBuffers = (PFNGLGENBUFFERSPROC)load("glGenBuffers"); + glad_glIsBuffer = (PFNGLISBUFFERPROC)load("glIsBuffer"); + glad_glBufferData = (PFNGLBUFFERDATAPROC)load("glBufferData"); + glad_glBufferSubData = (PFNGLBUFFERSUBDATAPROC)load("glBufferSubData"); + glad_glGetBufferSubData = (PFNGLGETBUFFERSUBDATAPROC)load("glGetBufferSubData"); + glad_glMapBuffer = (PFNGLMAPBUFFERPROC)load("glMapBuffer"); + glad_glUnmapBuffer = (PFNGLUNMAPBUFFERPROC)load("glUnmapBuffer"); + glad_glGetBufferParameteriv = (PFNGLGETBUFFERPARAMETERIVPROC)load("glGetBufferParameteriv"); + glad_glGetBufferPointerv = (PFNGLGETBUFFERPOINTERVPROC)load("glGetBufferPointerv"); +} +static void load_GL_VERSION_2_0(GLADloadproc load) { + if(!GLAD_GL_VERSION_2_0) return; + glad_glBlendEquationSeparate = (PFNGLBLENDEQUATIONSEPARATEPROC)load("glBlendEquationSeparate"); + glad_glDrawBuffers = (PFNGLDRAWBUFFERSPROC)load("glDrawBuffers"); + glad_glStencilOpSeparate = (PFNGLSTENCILOPSEPARATEPROC)load("glStencilOpSeparate"); + glad_glStencilFuncSeparate = (PFNGLSTENCILFUNCSEPARATEPROC)load("glStencilFuncSeparate"); + glad_glStencilMaskSeparate = (PFNGLSTENCILMASKSEPARATEPROC)load("glStencilMaskSeparate"); + glad_glAttachShader = (PFNGLATTACHSHADERPROC)load("glAttachShader"); + glad_glBindAttribLocation = (PFNGLBINDATTRIBLOCATIONPROC)load("glBindAttribLocation"); + glad_glCompileShader = (PFNGLCOMPILESHADERPROC)load("glCompileShader"); + glad_glCreateProgram = (PFNGLCREATEPROGRAMPROC)load("glCreateProgram"); + glad_glCreateShader = (PFNGLCREATESHADERPROC)load("glCreateShader"); + glad_glDeleteProgram = (PFNGLDELETEPROGRAMPROC)load("glDeleteProgram"); + glad_glDeleteShader = (PFNGLDELETESHADERPROC)load("glDeleteShader"); + glad_glDetachShader = (PFNGLDETACHSHADERPROC)load("glDetachShader"); + glad_glDisableVertexAttribArray = (PFNGLDISABLEVERTEXATTRIBARRAYPROC)load("glDisableVertexAttribArray"); + glad_glEnableVertexAttribArray = (PFNGLENABLEVERTEXATTRIBARRAYPROC)load("glEnableVertexAttribArray"); + glad_glGetActiveAttrib = (PFNGLGETACTIVEATTRIBPROC)load("glGetActiveAttrib"); + glad_glGetActiveUniform = (PFNGLGETACTIVEUNIFORMPROC)load("glGetActiveUniform"); + glad_glGetAttachedShaders = (PFNGLGETATTACHEDSHADERSPROC)load("glGetAttachedShaders"); + glad_glGetAttribLocation = (PFNGLGETATTRIBLOCATIONPROC)load("glGetAttribLocation"); + glad_glGetProgramiv = (PFNGLGETPROGRAMIVPROC)load("glGetProgramiv"); + glad_glGetProgramInfoLog = (PFNGLGETPROGRAMINFOLOGPROC)load("glGetProgramInfoLog"); + glad_glGetShaderiv = (PFNGLGETSHADERIVPROC)load("glGetShaderiv"); + glad_glGetShaderInfoLog = (PFNGLGETSHADERINFOLOGPROC)load("glGetShaderInfoLog"); + glad_glGetShaderSource = (PFNGLGETSHADERSOURCEPROC)load("glGetShaderSource"); + glad_glGetUniformLocation = (PFNGLGETUNIFORMLOCATIONPROC)load("glGetUniformLocation"); + glad_glGetUniformfv = (PFNGLGETUNIFORMFVPROC)load("glGetUniformfv"); + glad_glGetUniformiv = (PFNGLGETUNIFORMIVPROC)load("glGetUniformiv"); + glad_glGetVertexAttribdv = (PFNGLGETVERTEXATTRIBDVPROC)load("glGetVertexAttribdv"); + glad_glGetVertexAttribfv = (PFNGLGETVERTEXATTRIBFVPROC)load("glGetVertexAttribfv"); + glad_glGetVertexAttribiv = (PFNGLGETVERTEXATTRIBIVPROC)load("glGetVertexAttribiv"); + glad_glGetVertexAttribPointerv = (PFNGLGETVERTEXATTRIBPOINTERVPROC)load("glGetVertexAttribPointerv"); + glad_glIsProgram = (PFNGLISPROGRAMPROC)load("glIsProgram"); + glad_glIsShader = (PFNGLISSHADERPROC)load("glIsShader"); + glad_glLinkProgram = (PFNGLLINKPROGRAMPROC)load("glLinkProgram"); + glad_glShaderSource = (PFNGLSHADERSOURCEPROC)load("glShaderSource"); + glad_glUseProgram = (PFNGLUSEPROGRAMPROC)load("glUseProgram"); + glad_glUniform1f = (PFNGLUNIFORM1FPROC)load("glUniform1f"); + glad_glUniform2f = (PFNGLUNIFORM2FPROC)load("glUniform2f"); + glad_glUniform3f = (PFNGLUNIFORM3FPROC)load("glUniform3f"); + glad_glUniform4f = (PFNGLUNIFORM4FPROC)load("glUniform4f"); + glad_glUniform1i = (PFNGLUNIFORM1IPROC)load("glUniform1i"); + glad_glUniform2i = (PFNGLUNIFORM2IPROC)load("glUniform2i"); + glad_glUniform3i = (PFNGLUNIFORM3IPROC)load("glUniform3i"); + glad_glUniform4i = (PFNGLUNIFORM4IPROC)load("glUniform4i"); + glad_glUniform1fv = (PFNGLUNIFORM1FVPROC)load("glUniform1fv"); + glad_glUniform2fv = (PFNGLUNIFORM2FVPROC)load("glUniform2fv"); + glad_glUniform3fv = (PFNGLUNIFORM3FVPROC)load("glUniform3fv"); + glad_glUniform4fv = (PFNGLUNIFORM4FVPROC)load("glUniform4fv"); + glad_glUniform1iv = (PFNGLUNIFORM1IVPROC)load("glUniform1iv"); + glad_glUniform2iv = (PFNGLUNIFORM2IVPROC)load("glUniform2iv"); + glad_glUniform3iv = (PFNGLUNIFORM3IVPROC)load("glUniform3iv"); + glad_glUniform4iv = (PFNGLUNIFORM4IVPROC)load("glUniform4iv"); + glad_glUniformMatrix2fv = (PFNGLUNIFORMMATRIX2FVPROC)load("glUniformMatrix2fv"); + glad_glUniformMatrix3fv = (PFNGLUNIFORMMATRIX3FVPROC)load("glUniformMatrix3fv"); + glad_glUniformMatrix4fv = (PFNGLUNIFORMMATRIX4FVPROC)load("glUniformMatrix4fv"); + glad_glValidateProgram = (PFNGLVALIDATEPROGRAMPROC)load("glValidateProgram"); + glad_glVertexAttrib1d = (PFNGLVERTEXATTRIB1DPROC)load("glVertexAttrib1d"); + glad_glVertexAttrib1dv = (PFNGLVERTEXATTRIB1DVPROC)load("glVertexAttrib1dv"); + glad_glVertexAttrib1f = (PFNGLVERTEXATTRIB1FPROC)load("glVertexAttrib1f"); + glad_glVertexAttrib1fv = (PFNGLVERTEXATTRIB1FVPROC)load("glVertexAttrib1fv"); + glad_glVertexAttrib1s = (PFNGLVERTEXATTRIB1SPROC)load("glVertexAttrib1s"); + glad_glVertexAttrib1sv = (PFNGLVERTEXATTRIB1SVPROC)load("glVertexAttrib1sv"); + glad_glVertexAttrib2d = (PFNGLVERTEXATTRIB2DPROC)load("glVertexAttrib2d"); + glad_glVertexAttrib2dv = (PFNGLVERTEXATTRIB2DVPROC)load("glVertexAttrib2dv"); + glad_glVertexAttrib2f = (PFNGLVERTEXATTRIB2FPROC)load("glVertexAttrib2f"); + glad_glVertexAttrib2fv = (PFNGLVERTEXATTRIB2FVPROC)load("glVertexAttrib2fv"); + glad_glVertexAttrib2s = (PFNGLVERTEXATTRIB2SPROC)load("glVertexAttrib2s"); + glad_glVertexAttrib2sv = (PFNGLVERTEXATTRIB2SVPROC)load("glVertexAttrib2sv"); + glad_glVertexAttrib3d = (PFNGLVERTEXATTRIB3DPROC)load("glVertexAttrib3d"); + glad_glVertexAttrib3dv = (PFNGLVERTEXATTRIB3DVPROC)load("glVertexAttrib3dv"); + glad_glVertexAttrib3f = (PFNGLVERTEXATTRIB3FPROC)load("glVertexAttrib3f"); + glad_glVertexAttrib3fv = (PFNGLVERTEXATTRIB3FVPROC)load("glVertexAttrib3fv"); + glad_glVertexAttrib3s = (PFNGLVERTEXATTRIB3SPROC)load("glVertexAttrib3s"); + glad_glVertexAttrib3sv = (PFNGLVERTEXATTRIB3SVPROC)load("glVertexAttrib3sv"); + glad_glVertexAttrib4Nbv = (PFNGLVERTEXATTRIB4NBVPROC)load("glVertexAttrib4Nbv"); + glad_glVertexAttrib4Niv = (PFNGLVERTEXATTRIB4NIVPROC)load("glVertexAttrib4Niv"); + glad_glVertexAttrib4Nsv = (PFNGLVERTEXATTRIB4NSVPROC)load("glVertexAttrib4Nsv"); + glad_glVertexAttrib4Nub = (PFNGLVERTEXATTRIB4NUBPROC)load("glVertexAttrib4Nub"); + glad_glVertexAttrib4Nubv = (PFNGLVERTEXATTRIB4NUBVPROC)load("glVertexAttrib4Nubv"); + glad_glVertexAttrib4Nuiv = (PFNGLVERTEXATTRIB4NUIVPROC)load("glVertexAttrib4Nuiv"); + glad_glVertexAttrib4Nusv = (PFNGLVERTEXATTRIB4NUSVPROC)load("glVertexAttrib4Nusv"); + glad_glVertexAttrib4bv = (PFNGLVERTEXATTRIB4BVPROC)load("glVertexAttrib4bv"); + glad_glVertexAttrib4d = (PFNGLVERTEXATTRIB4DPROC)load("glVertexAttrib4d"); + glad_glVertexAttrib4dv = (PFNGLVERTEXATTRIB4DVPROC)load("glVertexAttrib4dv"); + glad_glVertexAttrib4f = (PFNGLVERTEXATTRIB4FPROC)load("glVertexAttrib4f"); + glad_glVertexAttrib4fv = (PFNGLVERTEXATTRIB4FVPROC)load("glVertexAttrib4fv"); + glad_glVertexAttrib4iv = (PFNGLVERTEXATTRIB4IVPROC)load("glVertexAttrib4iv"); + glad_glVertexAttrib4s = (PFNGLVERTEXATTRIB4SPROC)load("glVertexAttrib4s"); + glad_glVertexAttrib4sv = (PFNGLVERTEXATTRIB4SVPROC)load("glVertexAttrib4sv"); + glad_glVertexAttrib4ubv = (PFNGLVERTEXATTRIB4UBVPROC)load("glVertexAttrib4ubv"); + glad_glVertexAttrib4uiv = (PFNGLVERTEXATTRIB4UIVPROC)load("glVertexAttrib4uiv"); + glad_glVertexAttrib4usv = (PFNGLVERTEXATTRIB4USVPROC)load("glVertexAttrib4usv"); + glad_glVertexAttribPointer = (PFNGLVERTEXATTRIBPOINTERPROC)load("glVertexAttribPointer"); +} +static void load_GL_VERSION_2_1(GLADloadproc load) { + if(!GLAD_GL_VERSION_2_1) return; + glad_glUniformMatrix2x3fv = (PFNGLUNIFORMMATRIX2X3FVPROC)load("glUniformMatrix2x3fv"); + glad_glUniformMatrix3x2fv = (PFNGLUNIFORMMATRIX3X2FVPROC)load("glUniformMatrix3x2fv"); + glad_glUniformMatrix2x4fv = (PFNGLUNIFORMMATRIX2X4FVPROC)load("glUniformMatrix2x4fv"); + glad_glUniformMatrix4x2fv = (PFNGLUNIFORMMATRIX4X2FVPROC)load("glUniformMatrix4x2fv"); + glad_glUniformMatrix3x4fv = (PFNGLUNIFORMMATRIX3X4FVPROC)load("glUniformMatrix3x4fv"); + glad_glUniformMatrix4x3fv = (PFNGLUNIFORMMATRIX4X3FVPROC)load("glUniformMatrix4x3fv"); +} +static void load_GL_VERSION_3_0(GLADloadproc load) { + if(!GLAD_GL_VERSION_3_0) return; + glad_glColorMaski = (PFNGLCOLORMASKIPROC)load("glColorMaski"); + glad_glGetBooleani_v = (PFNGLGETBOOLEANI_VPROC)load("glGetBooleani_v"); + glad_glGetIntegeri_v = (PFNGLGETINTEGERI_VPROC)load("glGetIntegeri_v"); + glad_glEnablei = (PFNGLENABLEIPROC)load("glEnablei"); + glad_glDisablei = (PFNGLDISABLEIPROC)load("glDisablei"); + glad_glIsEnabledi = (PFNGLISENABLEDIPROC)load("glIsEnabledi"); + glad_glBeginTransformFeedback = (PFNGLBEGINTRANSFORMFEEDBACKPROC)load("glBeginTransformFeedback"); + glad_glEndTransformFeedback = (PFNGLENDTRANSFORMFEEDBACKPROC)load("glEndTransformFeedback"); + glad_glBindBufferRange = (PFNGLBINDBUFFERRANGEPROC)load("glBindBufferRange"); + glad_glBindBufferBase = (PFNGLBINDBUFFERBASEPROC)load("glBindBufferBase"); + glad_glTransformFeedbackVaryings = (PFNGLTRANSFORMFEEDBACKVARYINGSPROC)load("glTransformFeedbackVaryings"); + glad_glGetTransformFeedbackVarying = (PFNGLGETTRANSFORMFEEDBACKVARYINGPROC)load("glGetTransformFeedbackVarying"); + glad_glClampColor = (PFNGLCLAMPCOLORPROC)load("glClampColor"); + glad_glBeginConditionalRender = (PFNGLBEGINCONDITIONALRENDERPROC)load("glBeginConditionalRender"); + glad_glEndConditionalRender = (PFNGLENDCONDITIONALRENDERPROC)load("glEndConditionalRender"); + glad_glVertexAttribIPointer = (PFNGLVERTEXATTRIBIPOINTERPROC)load("glVertexAttribIPointer"); + glad_glGetVertexAttribIiv = (PFNGLGETVERTEXATTRIBIIVPROC)load("glGetVertexAttribIiv"); + glad_glGetVertexAttribIuiv = (PFNGLGETVERTEXATTRIBIUIVPROC)load("glGetVertexAttribIuiv"); + glad_glVertexAttribI1i = (PFNGLVERTEXATTRIBI1IPROC)load("glVertexAttribI1i"); + glad_glVertexAttribI2i = (PFNGLVERTEXATTRIBI2IPROC)load("glVertexAttribI2i"); + glad_glVertexAttribI3i = (PFNGLVERTEXATTRIBI3IPROC)load("glVertexAttribI3i"); + glad_glVertexAttribI4i = (PFNGLVERTEXATTRIBI4IPROC)load("glVertexAttribI4i"); + glad_glVertexAttribI1ui = (PFNGLVERTEXATTRIBI1UIPROC)load("glVertexAttribI1ui"); + glad_glVertexAttribI2ui = (PFNGLVERTEXATTRIBI2UIPROC)load("glVertexAttribI2ui"); + glad_glVertexAttribI3ui = (PFNGLVERTEXATTRIBI3UIPROC)load("glVertexAttribI3ui"); + glad_glVertexAttribI4ui = (PFNGLVERTEXATTRIBI4UIPROC)load("glVertexAttribI4ui"); + glad_glVertexAttribI1iv = (PFNGLVERTEXATTRIBI1IVPROC)load("glVertexAttribI1iv"); + glad_glVertexAttribI2iv = (PFNGLVERTEXATTRIBI2IVPROC)load("glVertexAttribI2iv"); + glad_glVertexAttribI3iv = (PFNGLVERTEXATTRIBI3IVPROC)load("glVertexAttribI3iv"); + glad_glVertexAttribI4iv = (PFNGLVERTEXATTRIBI4IVPROC)load("glVertexAttribI4iv"); + glad_glVertexAttribI1uiv = (PFNGLVERTEXATTRIBI1UIVPROC)load("glVertexAttribI1uiv"); + glad_glVertexAttribI2uiv = (PFNGLVERTEXATTRIBI2UIVPROC)load("glVertexAttribI2uiv"); + glad_glVertexAttribI3uiv = (PFNGLVERTEXATTRIBI3UIVPROC)load("glVertexAttribI3uiv"); + glad_glVertexAttribI4uiv = (PFNGLVERTEXATTRIBI4UIVPROC)load("glVertexAttribI4uiv"); + glad_glVertexAttribI4bv = (PFNGLVERTEXATTRIBI4BVPROC)load("glVertexAttribI4bv"); + glad_glVertexAttribI4sv = (PFNGLVERTEXATTRIBI4SVPROC)load("glVertexAttribI4sv"); + glad_glVertexAttribI4ubv = (PFNGLVERTEXATTRIBI4UBVPROC)load("glVertexAttribI4ubv"); + glad_glVertexAttribI4usv = (PFNGLVERTEXATTRIBI4USVPROC)load("glVertexAttribI4usv"); + glad_glGetUniformuiv = (PFNGLGETUNIFORMUIVPROC)load("glGetUniformuiv"); + glad_glBindFragDataLocation = (PFNGLBINDFRAGDATALOCATIONPROC)load("glBindFragDataLocation"); + glad_glGetFragDataLocation = (PFNGLGETFRAGDATALOCATIONPROC)load("glGetFragDataLocation"); + glad_glUniform1ui = (PFNGLUNIFORM1UIPROC)load("glUniform1ui"); + glad_glUniform2ui = (PFNGLUNIFORM2UIPROC)load("glUniform2ui"); + glad_glUniform3ui = (PFNGLUNIFORM3UIPROC)load("glUniform3ui"); + glad_glUniform4ui = (PFNGLUNIFORM4UIPROC)load("glUniform4ui"); + glad_glUniform1uiv = (PFNGLUNIFORM1UIVPROC)load("glUniform1uiv"); + glad_glUniform2uiv = (PFNGLUNIFORM2UIVPROC)load("glUniform2uiv"); + glad_glUniform3uiv = (PFNGLUNIFORM3UIVPROC)load("glUniform3uiv"); + glad_glUniform4uiv = (PFNGLUNIFORM4UIVPROC)load("glUniform4uiv"); + glad_glTexParameterIiv = (PFNGLTEXPARAMETERIIVPROC)load("glTexParameterIiv"); + glad_glTexParameterIuiv = (PFNGLTEXPARAMETERIUIVPROC)load("glTexParameterIuiv"); + glad_glGetTexParameterIiv = (PFNGLGETTEXPARAMETERIIVPROC)load("glGetTexParameterIiv"); + glad_glGetTexParameterIuiv = (PFNGLGETTEXPARAMETERIUIVPROC)load("glGetTexParameterIuiv"); + glad_glClearBufferiv = (PFNGLCLEARBUFFERIVPROC)load("glClearBufferiv"); + glad_glClearBufferuiv = (PFNGLCLEARBUFFERUIVPROC)load("glClearBufferuiv"); + glad_glClearBufferfv = (PFNGLCLEARBUFFERFVPROC)load("glClearBufferfv"); + glad_glClearBufferfi = (PFNGLCLEARBUFFERFIPROC)load("glClearBufferfi"); + glad_glGetStringi = (PFNGLGETSTRINGIPROC)load("glGetStringi"); + glad_glIsRenderbuffer = (PFNGLISRENDERBUFFERPROC)load("glIsRenderbuffer"); + glad_glBindRenderbuffer = (PFNGLBINDRENDERBUFFERPROC)load("glBindRenderbuffer"); + glad_glDeleteRenderbuffers = (PFNGLDELETERENDERBUFFERSPROC)load("glDeleteRenderbuffers"); + glad_glGenRenderbuffers = (PFNGLGENRENDERBUFFERSPROC)load("glGenRenderbuffers"); + glad_glRenderbufferStorage = (PFNGLRENDERBUFFERSTORAGEPROC)load("glRenderbufferStorage"); + glad_glGetRenderbufferParameteriv = (PFNGLGETRENDERBUFFERPARAMETERIVPROC)load("glGetRenderbufferParameteriv"); + glad_glIsFramebuffer = (PFNGLISFRAMEBUFFERPROC)load("glIsFramebuffer"); + glad_glBindFramebuffer = (PFNGLBINDFRAMEBUFFERPROC)load("glBindFramebuffer"); + glad_glDeleteFramebuffers = (PFNGLDELETEFRAMEBUFFERSPROC)load("glDeleteFramebuffers"); + glad_glGenFramebuffers = (PFNGLGENFRAMEBUFFERSPROC)load("glGenFramebuffers"); + glad_glCheckFramebufferStatus = (PFNGLCHECKFRAMEBUFFERSTATUSPROC)load("glCheckFramebufferStatus"); + glad_glFramebufferTexture1D = (PFNGLFRAMEBUFFERTEXTURE1DPROC)load("glFramebufferTexture1D"); + glad_glFramebufferTexture2D = (PFNGLFRAMEBUFFERTEXTURE2DPROC)load("glFramebufferTexture2D"); + glad_glFramebufferTexture3D = (PFNGLFRAMEBUFFERTEXTURE3DPROC)load("glFramebufferTexture3D"); + glad_glFramebufferRenderbuffer = (PFNGLFRAMEBUFFERRENDERBUFFERPROC)load("glFramebufferRenderbuffer"); + glad_glGetFramebufferAttachmentParameteriv = (PFNGLGETFRAMEBUFFERATTACHMENTPARAMETERIVPROC)load("glGetFramebufferAttachmentParameteriv"); + glad_glGenerateMipmap = (PFNGLGENERATEMIPMAPPROC)load("glGenerateMipmap"); + glad_glBlitFramebuffer = (PFNGLBLITFRAMEBUFFERPROC)load("glBlitFramebuffer"); + glad_glRenderbufferStorageMultisample = (PFNGLRENDERBUFFERSTORAGEMULTISAMPLEPROC)load("glRenderbufferStorageMultisample"); + glad_glFramebufferTextureLayer = (PFNGLFRAMEBUFFERTEXTURELAYERPROC)load("glFramebufferTextureLayer"); + glad_glMapBufferRange = (PFNGLMAPBUFFERRANGEPROC)load("glMapBufferRange"); + glad_glFlushMappedBufferRange = (PFNGLFLUSHMAPPEDBUFFERRANGEPROC)load("glFlushMappedBufferRange"); + glad_glBindVertexArray = (PFNGLBINDVERTEXARRAYPROC)load("glBindVertexArray"); + glad_glDeleteVertexArrays = (PFNGLDELETEVERTEXARRAYSPROC)load("glDeleteVertexArrays"); + glad_glGenVertexArrays = (PFNGLGENVERTEXARRAYSPROC)load("glGenVertexArrays"); + glad_glIsVertexArray = (PFNGLISVERTEXARRAYPROC)load("glIsVertexArray"); +} +static void load_GL_VERSION_3_1(GLADloadproc load) { + if(!GLAD_GL_VERSION_3_1) return; + glad_glDrawArraysInstanced = (PFNGLDRAWARRAYSINSTANCEDPROC)load("glDrawArraysInstanced"); + glad_glDrawElementsInstanced = (PFNGLDRAWELEMENTSINSTANCEDPROC)load("glDrawElementsInstanced"); + glad_glTexBuffer = (PFNGLTEXBUFFERPROC)load("glTexBuffer"); + glad_glPrimitiveRestartIndex = (PFNGLPRIMITIVERESTARTINDEXPROC)load("glPrimitiveRestartIndex"); + glad_glCopyBufferSubData = (PFNGLCOPYBUFFERSUBDATAPROC)load("glCopyBufferSubData"); + glad_glGetUniformIndices = (PFNGLGETUNIFORMINDICESPROC)load("glGetUniformIndices"); + glad_glGetActiveUniformsiv = (PFNGLGETACTIVEUNIFORMSIVPROC)load("glGetActiveUniformsiv"); + glad_glGetActiveUniformName = (PFNGLGETACTIVEUNIFORMNAMEPROC)load("glGetActiveUniformName"); + glad_glGetUniformBlockIndex = (PFNGLGETUNIFORMBLOCKINDEXPROC)load("glGetUniformBlockIndex"); + glad_glGetActiveUniformBlockiv = (PFNGLGETACTIVEUNIFORMBLOCKIVPROC)load("glGetActiveUniformBlockiv"); + glad_glGetActiveUniformBlockName = (PFNGLGETACTIVEUNIFORMBLOCKNAMEPROC)load("glGetActiveUniformBlockName"); + glad_glUniformBlockBinding = (PFNGLUNIFORMBLOCKBINDINGPROC)load("glUniformBlockBinding"); + glad_glBindBufferRange = (PFNGLBINDBUFFERRANGEPROC)load("glBindBufferRange"); + glad_glBindBufferBase = (PFNGLBINDBUFFERBASEPROC)load("glBindBufferBase"); + glad_glGetIntegeri_v = (PFNGLGETINTEGERI_VPROC)load("glGetIntegeri_v"); +} +static void load_GL_VERSION_3_2(GLADloadproc load) { + if(!GLAD_GL_VERSION_3_2) return; + glad_glDrawElementsBaseVertex = (PFNGLDRAWELEMENTSBASEVERTEXPROC)load("glDrawElementsBaseVertex"); + glad_glDrawRangeElementsBaseVertex = (PFNGLDRAWRANGEELEMENTSBASEVERTEXPROC)load("glDrawRangeElementsBaseVertex"); + glad_glDrawElementsInstancedBaseVertex = (PFNGLDRAWELEMENTSINSTANCEDBASEVERTEXPROC)load("glDrawElementsInstancedBaseVertex"); + glad_glMultiDrawElementsBaseVertex = (PFNGLMULTIDRAWELEMENTSBASEVERTEXPROC)load("glMultiDrawElementsBaseVertex"); + glad_glProvokingVertex = (PFNGLPROVOKINGVERTEXPROC)load("glProvokingVertex"); + glad_glFenceSync = (PFNGLFENCESYNCPROC)load("glFenceSync"); + glad_glIsSync = (PFNGLISSYNCPROC)load("glIsSync"); + glad_glDeleteSync = (PFNGLDELETESYNCPROC)load("glDeleteSync"); + glad_glClientWaitSync = (PFNGLCLIENTWAITSYNCPROC)load("glClientWaitSync"); + glad_glWaitSync = (PFNGLWAITSYNCPROC)load("glWaitSync"); + glad_glGetInteger64v = (PFNGLGETINTEGER64VPROC)load("glGetInteger64v"); + glad_glGetSynciv = (PFNGLGETSYNCIVPROC)load("glGetSynciv"); + glad_glGetInteger64i_v = (PFNGLGETINTEGER64I_VPROC)load("glGetInteger64i_v"); + glad_glGetBufferParameteri64v = (PFNGLGETBUFFERPARAMETERI64VPROC)load("glGetBufferParameteri64v"); + glad_glFramebufferTexture = (PFNGLFRAMEBUFFERTEXTUREPROC)load("glFramebufferTexture"); + glad_glTexImage2DMultisample = (PFNGLTEXIMAGE2DMULTISAMPLEPROC)load("glTexImage2DMultisample"); + glad_glTexImage3DMultisample = (PFNGLTEXIMAGE3DMULTISAMPLEPROC)load("glTexImage3DMultisample"); + glad_glGetMultisamplefv = (PFNGLGETMULTISAMPLEFVPROC)load("glGetMultisamplefv"); + glad_glSampleMaski = (PFNGLSAMPLEMASKIPROC)load("glSampleMaski"); +} +static void load_GL_VERSION_3_3(GLADloadproc load) { + if(!GLAD_GL_VERSION_3_3) return; + glad_glBindFragDataLocationIndexed = (PFNGLBINDFRAGDATALOCATIONINDEXEDPROC)load("glBindFragDataLocationIndexed"); + glad_glGetFragDataIndex = (PFNGLGETFRAGDATAINDEXPROC)load("glGetFragDataIndex"); + glad_glGenSamplers = (PFNGLGENSAMPLERSPROC)load("glGenSamplers"); + glad_glDeleteSamplers = (PFNGLDELETESAMPLERSPROC)load("glDeleteSamplers"); + glad_glIsSampler = (PFNGLISSAMPLERPROC)load("glIsSampler"); + glad_glBindSampler = (PFNGLBINDSAMPLERPROC)load("glBindSampler"); + glad_glSamplerParameteri = (PFNGLSAMPLERPARAMETERIPROC)load("glSamplerParameteri"); + glad_glSamplerParameteriv = (PFNGLSAMPLERPARAMETERIVPROC)load("glSamplerParameteriv"); + glad_glSamplerParameterf = (PFNGLSAMPLERPARAMETERFPROC)load("glSamplerParameterf"); + glad_glSamplerParameterfv = (PFNGLSAMPLERPARAMETERFVPROC)load("glSamplerParameterfv"); + glad_glSamplerParameterIiv = (PFNGLSAMPLERPARAMETERIIVPROC)load("glSamplerParameterIiv"); + glad_glSamplerParameterIuiv = (PFNGLSAMPLERPARAMETERIUIVPROC)load("glSamplerParameterIuiv"); + glad_glGetSamplerParameteriv = (PFNGLGETSAMPLERPARAMETERIVPROC)load("glGetSamplerParameteriv"); + glad_glGetSamplerParameterIiv = (PFNGLGETSAMPLERPARAMETERIIVPROC)load("glGetSamplerParameterIiv"); + glad_glGetSamplerParameterfv = (PFNGLGETSAMPLERPARAMETERFVPROC)load("glGetSamplerParameterfv"); + glad_glGetSamplerParameterIuiv = (PFNGLGETSAMPLERPARAMETERIUIVPROC)load("glGetSamplerParameterIuiv"); + glad_glQueryCounter = (PFNGLQUERYCOUNTERPROC)load("glQueryCounter"); + glad_glGetQueryObjecti64v = (PFNGLGETQUERYOBJECTI64VPROC)load("glGetQueryObjecti64v"); + glad_glGetQueryObjectui64v = (PFNGLGETQUERYOBJECTUI64VPROC)load("glGetQueryObjectui64v"); + glad_glVertexAttribDivisor = (PFNGLVERTEXATTRIBDIVISORPROC)load("glVertexAttribDivisor"); + glad_glVertexAttribP1ui = (PFNGLVERTEXATTRIBP1UIPROC)load("glVertexAttribP1ui"); + glad_glVertexAttribP1uiv = (PFNGLVERTEXATTRIBP1UIVPROC)load("glVertexAttribP1uiv"); + glad_glVertexAttribP2ui = (PFNGLVERTEXATTRIBP2UIPROC)load("glVertexAttribP2ui"); + glad_glVertexAttribP2uiv = (PFNGLVERTEXATTRIBP2UIVPROC)load("glVertexAttribP2uiv"); + glad_glVertexAttribP3ui = (PFNGLVERTEXATTRIBP3UIPROC)load("glVertexAttribP3ui"); + glad_glVertexAttribP3uiv = (PFNGLVERTEXATTRIBP3UIVPROC)load("glVertexAttribP3uiv"); + glad_glVertexAttribP4ui = (PFNGLVERTEXATTRIBP4UIPROC)load("glVertexAttribP4ui"); + glad_glVertexAttribP4uiv = (PFNGLVERTEXATTRIBP4UIVPROC)load("glVertexAttribP4uiv"); + glad_glVertexP2ui = (PFNGLVERTEXP2UIPROC)load("glVertexP2ui"); + glad_glVertexP2uiv = (PFNGLVERTEXP2UIVPROC)load("glVertexP2uiv"); + glad_glVertexP3ui = (PFNGLVERTEXP3UIPROC)load("glVertexP3ui"); + glad_glVertexP3uiv = (PFNGLVERTEXP3UIVPROC)load("glVertexP3uiv"); + glad_glVertexP4ui = (PFNGLVERTEXP4UIPROC)load("glVertexP4ui"); + glad_glVertexP4uiv = (PFNGLVERTEXP4UIVPROC)load("glVertexP4uiv"); + glad_glTexCoordP1ui = (PFNGLTEXCOORDP1UIPROC)load("glTexCoordP1ui"); + glad_glTexCoordP1uiv = (PFNGLTEXCOORDP1UIVPROC)load("glTexCoordP1uiv"); + glad_glTexCoordP2ui = (PFNGLTEXCOORDP2UIPROC)load("glTexCoordP2ui"); + glad_glTexCoordP2uiv = (PFNGLTEXCOORDP2UIVPROC)load("glTexCoordP2uiv"); + glad_glTexCoordP3ui = (PFNGLTEXCOORDP3UIPROC)load("glTexCoordP3ui"); + glad_glTexCoordP3uiv = (PFNGLTEXCOORDP3UIVPROC)load("glTexCoordP3uiv"); + glad_glTexCoordP4ui = (PFNGLTEXCOORDP4UIPROC)load("glTexCoordP4ui"); + glad_glTexCoordP4uiv = (PFNGLTEXCOORDP4UIVPROC)load("glTexCoordP4uiv"); + glad_glMultiTexCoordP1ui = (PFNGLMULTITEXCOORDP1UIPROC)load("glMultiTexCoordP1ui"); + glad_glMultiTexCoordP1uiv = (PFNGLMULTITEXCOORDP1UIVPROC)load("glMultiTexCoordP1uiv"); + glad_glMultiTexCoordP2ui = (PFNGLMULTITEXCOORDP2UIPROC)load("glMultiTexCoordP2ui"); + glad_glMultiTexCoordP2uiv = (PFNGLMULTITEXCOORDP2UIVPROC)load("glMultiTexCoordP2uiv"); + glad_glMultiTexCoordP3ui = (PFNGLMULTITEXCOORDP3UIPROC)load("glMultiTexCoordP3ui"); + glad_glMultiTexCoordP3uiv = (PFNGLMULTITEXCOORDP3UIVPROC)load("glMultiTexCoordP3uiv"); + glad_glMultiTexCoordP4ui = (PFNGLMULTITEXCOORDP4UIPROC)load("glMultiTexCoordP4ui"); + glad_glMultiTexCoordP4uiv = (PFNGLMULTITEXCOORDP4UIVPROC)load("glMultiTexCoordP4uiv"); + glad_glNormalP3ui = (PFNGLNORMALP3UIPROC)load("glNormalP3ui"); + glad_glNormalP3uiv = (PFNGLNORMALP3UIVPROC)load("glNormalP3uiv"); + glad_glColorP3ui = (PFNGLCOLORP3UIPROC)load("glColorP3ui"); + glad_glColorP3uiv = (PFNGLCOLORP3UIVPROC)load("glColorP3uiv"); + glad_glColorP4ui = (PFNGLCOLORP4UIPROC)load("glColorP4ui"); + glad_glColorP4uiv = (PFNGLCOLORP4UIVPROC)load("glColorP4uiv"); + glad_glSecondaryColorP3ui = (PFNGLSECONDARYCOLORP3UIPROC)load("glSecondaryColorP3ui"); + glad_glSecondaryColorP3uiv = (PFNGLSECONDARYCOLORP3UIVPROC)load("glSecondaryColorP3uiv"); +} +static void load_GL_VERSION_4_0(GLADloadproc load) { + if(!GLAD_GL_VERSION_4_0) return; + glad_glMinSampleShading = (PFNGLMINSAMPLESHADINGPROC)load("glMinSampleShading"); + glad_glBlendEquationi = (PFNGLBLENDEQUATIONIPROC)load("glBlendEquationi"); + glad_glBlendEquationSeparatei = (PFNGLBLENDEQUATIONSEPARATEIPROC)load("glBlendEquationSeparatei"); + glad_glBlendFunci = (PFNGLBLENDFUNCIPROC)load("glBlendFunci"); + glad_glBlendFuncSeparatei = (PFNGLBLENDFUNCSEPARATEIPROC)load("glBlendFuncSeparatei"); + glad_glDrawArraysIndirect = (PFNGLDRAWARRAYSINDIRECTPROC)load("glDrawArraysIndirect"); + glad_glDrawElementsIndirect = (PFNGLDRAWELEMENTSINDIRECTPROC)load("glDrawElementsIndirect"); + glad_glUniform1d = (PFNGLUNIFORM1DPROC)load("glUniform1d"); + glad_glUniform2d = (PFNGLUNIFORM2DPROC)load("glUniform2d"); + glad_glUniform3d = (PFNGLUNIFORM3DPROC)load("glUniform3d"); + glad_glUniform4d = (PFNGLUNIFORM4DPROC)load("glUniform4d"); + glad_glUniform1dv = (PFNGLUNIFORM1DVPROC)load("glUniform1dv"); + glad_glUniform2dv = (PFNGLUNIFORM2DVPROC)load("glUniform2dv"); + glad_glUniform3dv = (PFNGLUNIFORM3DVPROC)load("glUniform3dv"); + glad_glUniform4dv = (PFNGLUNIFORM4DVPROC)load("glUniform4dv"); + glad_glUniformMatrix2dv = (PFNGLUNIFORMMATRIX2DVPROC)load("glUniformMatrix2dv"); + glad_glUniformMatrix3dv = (PFNGLUNIFORMMATRIX3DVPROC)load("glUniformMatrix3dv"); + glad_glUniformMatrix4dv = (PFNGLUNIFORMMATRIX4DVPROC)load("glUniformMatrix4dv"); + glad_glUniformMatrix2x3dv = (PFNGLUNIFORMMATRIX2X3DVPROC)load("glUniformMatrix2x3dv"); + glad_glUniformMatrix2x4dv = (PFNGLUNIFORMMATRIX2X4DVPROC)load("glUniformMatrix2x4dv"); + glad_glUniformMatrix3x2dv = (PFNGLUNIFORMMATRIX3X2DVPROC)load("glUniformMatrix3x2dv"); + glad_glUniformMatrix3x4dv = (PFNGLUNIFORMMATRIX3X4DVPROC)load("glUniformMatrix3x4dv"); + glad_glUniformMatrix4x2dv = (PFNGLUNIFORMMATRIX4X2DVPROC)load("glUniformMatrix4x2dv"); + glad_glUniformMatrix4x3dv = (PFNGLUNIFORMMATRIX4X3DVPROC)load("glUniformMatrix4x3dv"); + glad_glGetUniformdv = (PFNGLGETUNIFORMDVPROC)load("glGetUniformdv"); + glad_glGetSubroutineUniformLocation = (PFNGLGETSUBROUTINEUNIFORMLOCATIONPROC)load("glGetSubroutineUniformLocation"); + glad_glGetSubroutineIndex = (PFNGLGETSUBROUTINEINDEXPROC)load("glGetSubroutineIndex"); + glad_glGetActiveSubroutineUniformiv = (PFNGLGETACTIVESUBROUTINEUNIFORMIVPROC)load("glGetActiveSubroutineUniformiv"); + glad_glGetActiveSubroutineUniformName = (PFNGLGETACTIVESUBROUTINEUNIFORMNAMEPROC)load("glGetActiveSubroutineUniformName"); + glad_glGetActiveSubroutineName = (PFNGLGETACTIVESUBROUTINENAMEPROC)load("glGetActiveSubroutineName"); + glad_glUniformSubroutinesuiv = (PFNGLUNIFORMSUBROUTINESUIVPROC)load("glUniformSubroutinesuiv"); + glad_glGetUniformSubroutineuiv = (PFNGLGETUNIFORMSUBROUTINEUIVPROC)load("glGetUniformSubroutineuiv"); + glad_glGetProgramStageiv = (PFNGLGETPROGRAMSTAGEIVPROC)load("glGetProgramStageiv"); + glad_glPatchParameteri = (PFNGLPATCHPARAMETERIPROC)load("glPatchParameteri"); + glad_glPatchParameterfv = (PFNGLPATCHPARAMETERFVPROC)load("glPatchParameterfv"); + glad_glBindTransformFeedback = (PFNGLBINDTRANSFORMFEEDBACKPROC)load("glBindTransformFeedback"); + glad_glDeleteTransformFeedbacks = (PFNGLDELETETRANSFORMFEEDBACKSPROC)load("glDeleteTransformFeedbacks"); + glad_glGenTransformFeedbacks = (PFNGLGENTRANSFORMFEEDBACKSPROC)load("glGenTransformFeedbacks"); + glad_glIsTransformFeedback = (PFNGLISTRANSFORMFEEDBACKPROC)load("glIsTransformFeedback"); + glad_glPauseTransformFeedback = (PFNGLPAUSETRANSFORMFEEDBACKPROC)load("glPauseTransformFeedback"); + glad_glResumeTransformFeedback = (PFNGLRESUMETRANSFORMFEEDBACKPROC)load("glResumeTransformFeedback"); + glad_glDrawTransformFeedback = (PFNGLDRAWTRANSFORMFEEDBACKPROC)load("glDrawTransformFeedback"); + glad_glDrawTransformFeedbackStream = (PFNGLDRAWTRANSFORMFEEDBACKSTREAMPROC)load("glDrawTransformFeedbackStream"); + glad_glBeginQueryIndexed = (PFNGLBEGINQUERYINDEXEDPROC)load("glBeginQueryIndexed"); + glad_glEndQueryIndexed = (PFNGLENDQUERYINDEXEDPROC)load("glEndQueryIndexed"); + glad_glGetQueryIndexediv = (PFNGLGETQUERYINDEXEDIVPROC)load("glGetQueryIndexediv"); +} +static void load_GL_VERSION_4_1(GLADloadproc load) { + if(!GLAD_GL_VERSION_4_1) return; + glad_glReleaseShaderCompiler = (PFNGLRELEASESHADERCOMPILERPROC)load("glReleaseShaderCompiler"); + glad_glShaderBinary = (PFNGLSHADERBINARYPROC)load("glShaderBinary"); + glad_glGetShaderPrecisionFormat = (PFNGLGETSHADERPRECISIONFORMATPROC)load("glGetShaderPrecisionFormat"); + glad_glDepthRangef = (PFNGLDEPTHRANGEFPROC)load("glDepthRangef"); + glad_glClearDepthf = (PFNGLCLEARDEPTHFPROC)load("glClearDepthf"); + glad_glGetProgramBinary = (PFNGLGETPROGRAMBINARYPROC)load("glGetProgramBinary"); + glad_glProgramBinary = (PFNGLPROGRAMBINARYPROC)load("glProgramBinary"); + glad_glProgramParameteri = (PFNGLPROGRAMPARAMETERIPROC)load("glProgramParameteri"); + glad_glUseProgramStages = (PFNGLUSEPROGRAMSTAGESPROC)load("glUseProgramStages"); + glad_glActiveShaderProgram = (PFNGLACTIVESHADERPROGRAMPROC)load("glActiveShaderProgram"); + glad_glCreateShaderProgramv = (PFNGLCREATESHADERPROGRAMVPROC)load("glCreateShaderProgramv"); + glad_glBindProgramPipeline = (PFNGLBINDPROGRAMPIPELINEPROC)load("glBindProgramPipeline"); + glad_glDeleteProgramPipelines = (PFNGLDELETEPROGRAMPIPELINESPROC)load("glDeleteProgramPipelines"); + glad_glGenProgramPipelines = (PFNGLGENPROGRAMPIPELINESPROC)load("glGenProgramPipelines"); + glad_glIsProgramPipeline = (PFNGLISPROGRAMPIPELINEPROC)load("glIsProgramPipeline"); + glad_glGetProgramPipelineiv = (PFNGLGETPROGRAMPIPELINEIVPROC)load("glGetProgramPipelineiv"); + glad_glProgramParameteri = (PFNGLPROGRAMPARAMETERIPROC)load("glProgramParameteri"); + glad_glProgramUniform1i = (PFNGLPROGRAMUNIFORM1IPROC)load("glProgramUniform1i"); + glad_glProgramUniform1iv = (PFNGLPROGRAMUNIFORM1IVPROC)load("glProgramUniform1iv"); + glad_glProgramUniform1f = (PFNGLPROGRAMUNIFORM1FPROC)load("glProgramUniform1f"); + glad_glProgramUniform1fv = (PFNGLPROGRAMUNIFORM1FVPROC)load("glProgramUniform1fv"); + glad_glProgramUniform1d = (PFNGLPROGRAMUNIFORM1DPROC)load("glProgramUniform1d"); + glad_glProgramUniform1dv = (PFNGLPROGRAMUNIFORM1DVPROC)load("glProgramUniform1dv"); + glad_glProgramUniform1ui = (PFNGLPROGRAMUNIFORM1UIPROC)load("glProgramUniform1ui"); + glad_glProgramUniform1uiv = (PFNGLPROGRAMUNIFORM1UIVPROC)load("glProgramUniform1uiv"); + glad_glProgramUniform2i = (PFNGLPROGRAMUNIFORM2IPROC)load("glProgramUniform2i"); + glad_glProgramUniform2iv = (PFNGLPROGRAMUNIFORM2IVPROC)load("glProgramUniform2iv"); + glad_glProgramUniform2f = (PFNGLPROGRAMUNIFORM2FPROC)load("glProgramUniform2f"); + glad_glProgramUniform2fv = (PFNGLPROGRAMUNIFORM2FVPROC)load("glProgramUniform2fv"); + glad_glProgramUniform2d = (PFNGLPROGRAMUNIFORM2DPROC)load("glProgramUniform2d"); + glad_glProgramUniform2dv = (PFNGLPROGRAMUNIFORM2DVPROC)load("glProgramUniform2dv"); + glad_glProgramUniform2ui = (PFNGLPROGRAMUNIFORM2UIPROC)load("glProgramUniform2ui"); + glad_glProgramUniform2uiv = (PFNGLPROGRAMUNIFORM2UIVPROC)load("glProgramUniform2uiv"); + glad_glProgramUniform3i = (PFNGLPROGRAMUNIFORM3IPROC)load("glProgramUniform3i"); + glad_glProgramUniform3iv = (PFNGLPROGRAMUNIFORM3IVPROC)load("glProgramUniform3iv"); + glad_glProgramUniform3f = (PFNGLPROGRAMUNIFORM3FPROC)load("glProgramUniform3f"); + glad_glProgramUniform3fv = (PFNGLPROGRAMUNIFORM3FVPROC)load("glProgramUniform3fv"); + glad_glProgramUniform3d = (PFNGLPROGRAMUNIFORM3DPROC)load("glProgramUniform3d"); + glad_glProgramUniform3dv = (PFNGLPROGRAMUNIFORM3DVPROC)load("glProgramUniform3dv"); + glad_glProgramUniform3ui = (PFNGLPROGRAMUNIFORM3UIPROC)load("glProgramUniform3ui"); + glad_glProgramUniform3uiv = (PFNGLPROGRAMUNIFORM3UIVPROC)load("glProgramUniform3uiv"); + glad_glProgramUniform4i = (PFNGLPROGRAMUNIFORM4IPROC)load("glProgramUniform4i"); + glad_glProgramUniform4iv = (PFNGLPROGRAMUNIFORM4IVPROC)load("glProgramUniform4iv"); + glad_glProgramUniform4f = (PFNGLPROGRAMUNIFORM4FPROC)load("glProgramUniform4f"); + glad_glProgramUniform4fv = (PFNGLPROGRAMUNIFORM4FVPROC)load("glProgramUniform4fv"); + glad_glProgramUniform4d = (PFNGLPROGRAMUNIFORM4DPROC)load("glProgramUniform4d"); + glad_glProgramUniform4dv = (PFNGLPROGRAMUNIFORM4DVPROC)load("glProgramUniform4dv"); + glad_glProgramUniform4ui = (PFNGLPROGRAMUNIFORM4UIPROC)load("glProgramUniform4ui"); + glad_glProgramUniform4uiv = (PFNGLPROGRAMUNIFORM4UIVPROC)load("glProgramUniform4uiv"); + glad_glProgramUniformMatrix2fv = (PFNGLPROGRAMUNIFORMMATRIX2FVPROC)load("glProgramUniformMatrix2fv"); + glad_glProgramUniformMatrix3fv = (PFNGLPROGRAMUNIFORMMATRIX3FVPROC)load("glProgramUniformMatrix3fv"); + glad_glProgramUniformMatrix4fv = (PFNGLPROGRAMUNIFORMMATRIX4FVPROC)load("glProgramUniformMatrix4fv"); + glad_glProgramUniformMatrix2dv = (PFNGLPROGRAMUNIFORMMATRIX2DVPROC)load("glProgramUniformMatrix2dv"); + glad_glProgramUniformMatrix3dv = (PFNGLPROGRAMUNIFORMMATRIX3DVPROC)load("glProgramUniformMatrix3dv"); + glad_glProgramUniformMatrix4dv = (PFNGLPROGRAMUNIFORMMATRIX4DVPROC)load("glProgramUniformMatrix4dv"); + glad_glProgramUniformMatrix2x3fv = (PFNGLPROGRAMUNIFORMMATRIX2X3FVPROC)load("glProgramUniformMatrix2x3fv"); + glad_glProgramUniformMatrix3x2fv = (PFNGLPROGRAMUNIFORMMATRIX3X2FVPROC)load("glProgramUniformMatrix3x2fv"); + glad_glProgramUniformMatrix2x4fv = (PFNGLPROGRAMUNIFORMMATRIX2X4FVPROC)load("glProgramUniformMatrix2x4fv"); + glad_glProgramUniformMatrix4x2fv = (PFNGLPROGRAMUNIFORMMATRIX4X2FVPROC)load("glProgramUniformMatrix4x2fv"); + glad_glProgramUniformMatrix3x4fv = (PFNGLPROGRAMUNIFORMMATRIX3X4FVPROC)load("glProgramUniformMatrix3x4fv"); + glad_glProgramUniformMatrix4x3fv = (PFNGLPROGRAMUNIFORMMATRIX4X3FVPROC)load("glProgramUniformMatrix4x3fv"); + glad_glProgramUniformMatrix2x3dv = (PFNGLPROGRAMUNIFORMMATRIX2X3DVPROC)load("glProgramUniformMatrix2x3dv"); + glad_glProgramUniformMatrix3x2dv = (PFNGLPROGRAMUNIFORMMATRIX3X2DVPROC)load("glProgramUniformMatrix3x2dv"); + glad_glProgramUniformMatrix2x4dv = (PFNGLPROGRAMUNIFORMMATRIX2X4DVPROC)load("glProgramUniformMatrix2x4dv"); + glad_glProgramUniformMatrix4x2dv = (PFNGLPROGRAMUNIFORMMATRIX4X2DVPROC)load("glProgramUniformMatrix4x2dv"); + glad_glProgramUniformMatrix3x4dv = (PFNGLPROGRAMUNIFORMMATRIX3X4DVPROC)load("glProgramUniformMatrix3x4dv"); + glad_glProgramUniformMatrix4x3dv = (PFNGLPROGRAMUNIFORMMATRIX4X3DVPROC)load("glProgramUniformMatrix4x3dv"); + glad_glValidateProgramPipeline = (PFNGLVALIDATEPROGRAMPIPELINEPROC)load("glValidateProgramPipeline"); + glad_glGetProgramPipelineInfoLog = (PFNGLGETPROGRAMPIPELINEINFOLOGPROC)load("glGetProgramPipelineInfoLog"); + glad_glVertexAttribL1d = (PFNGLVERTEXATTRIBL1DPROC)load("glVertexAttribL1d"); + glad_glVertexAttribL2d = (PFNGLVERTEXATTRIBL2DPROC)load("glVertexAttribL2d"); + glad_glVertexAttribL3d = (PFNGLVERTEXATTRIBL3DPROC)load("glVertexAttribL3d"); + glad_glVertexAttribL4d = (PFNGLVERTEXATTRIBL4DPROC)load("glVertexAttribL4d"); + glad_glVertexAttribL1dv = (PFNGLVERTEXATTRIBL1DVPROC)load("glVertexAttribL1dv"); + glad_glVertexAttribL2dv = (PFNGLVERTEXATTRIBL2DVPROC)load("glVertexAttribL2dv"); + glad_glVertexAttribL3dv = (PFNGLVERTEXATTRIBL3DVPROC)load("glVertexAttribL3dv"); + glad_glVertexAttribL4dv = (PFNGLVERTEXATTRIBL4DVPROC)load("glVertexAttribL4dv"); + glad_glVertexAttribLPointer = (PFNGLVERTEXATTRIBLPOINTERPROC)load("glVertexAttribLPointer"); + glad_glGetVertexAttribLdv = (PFNGLGETVERTEXATTRIBLDVPROC)load("glGetVertexAttribLdv"); + glad_glViewportArrayv = (PFNGLVIEWPORTARRAYVPROC)load("glViewportArrayv"); + glad_glViewportIndexedf = (PFNGLVIEWPORTINDEXEDFPROC)load("glViewportIndexedf"); + glad_glViewportIndexedfv = (PFNGLVIEWPORTINDEXEDFVPROC)load("glViewportIndexedfv"); + glad_glScissorArrayv = (PFNGLSCISSORARRAYVPROC)load("glScissorArrayv"); + glad_glScissorIndexed = (PFNGLSCISSORINDEXEDPROC)load("glScissorIndexed"); + glad_glScissorIndexedv = (PFNGLSCISSORINDEXEDVPROC)load("glScissorIndexedv"); + glad_glDepthRangeArrayv = (PFNGLDEPTHRANGEARRAYVPROC)load("glDepthRangeArrayv"); + glad_glDepthRangeIndexed = (PFNGLDEPTHRANGEINDEXEDPROC)load("glDepthRangeIndexed"); + glad_glGetFloati_v = (PFNGLGETFLOATI_VPROC)load("glGetFloati_v"); + glad_glGetDoublei_v = (PFNGLGETDOUBLEI_VPROC)load("glGetDoublei_v"); +} +static int find_extensionsGL(void) { + if (!get_exts()) return 0; + (void)&has_ext; + free_exts(); + return 1; +} + +static void find_coreGL(void) { + + /* Thank you @elmindreda + * https://github.com/elmindreda/greg/blob/master/templates/greg.c.in#L176 + * https://github.com/glfw/glfw/blob/master/src/context.c#L36 + */ + int i, major, minor; + + const char* version; + const char* prefixes[] = { + "OpenGL ES-CM ", + "OpenGL ES-CL ", + "OpenGL ES ", + NULL + }; + + version = (const char*) glGetString(GL_VERSION); + if (!version) return; + + for (i = 0; prefixes[i]; i++) { + const size_t length = strlen(prefixes[i]); + if (strncmp(version, prefixes[i], length) == 0) { + version += length; + break; + } + } + +/* PR #18 */ +#ifdef _MSC_VER + sscanf_s(version, "%d.%d", &major, &minor); +#else + sscanf(version, "%d.%d", &major, &minor); +#endif + + GLVersion.major = major; GLVersion.minor = minor; + max_loaded_major = major; max_loaded_minor = minor; + GLAD_GL_VERSION_1_0 = (major == 1 && minor >= 0) || major > 1; + GLAD_GL_VERSION_1_1 = (major == 1 && minor >= 1) || major > 1; + GLAD_GL_VERSION_1_2 = (major == 1 && minor >= 2) || major > 1; + GLAD_GL_VERSION_1_3 = (major == 1 && minor >= 3) || major > 1; + GLAD_GL_VERSION_1_4 = (major == 1 && minor >= 4) || major > 1; + GLAD_GL_VERSION_1_5 = (major == 1 && minor >= 5) || major > 1; + GLAD_GL_VERSION_2_0 = (major == 2 && minor >= 0) || major > 2; + GLAD_GL_VERSION_2_1 = (major == 2 && minor >= 1) || major > 2; + GLAD_GL_VERSION_3_0 = (major == 3 && minor >= 0) || major > 3; + GLAD_GL_VERSION_3_1 = (major == 3 && minor >= 1) || major > 3; + GLAD_GL_VERSION_3_2 = (major == 3 && minor >= 2) || major > 3; + GLAD_GL_VERSION_3_3 = (major == 3 && minor >= 3) || major > 3; + GLAD_GL_VERSION_4_0 = (major == 4 && minor >= 0) || major > 4; + GLAD_GL_VERSION_4_1 = (major == 4 && minor >= 1) || major > 4; + if (GLVersion.major > 4 || (GLVersion.major >= 4 && GLVersion.minor >= 1)) { + max_loaded_major = 4; + max_loaded_minor = 1; + } +} + +int gladLoadGLLoader(GLADloadproc load) { + GLVersion.major = 0; GLVersion.minor = 0; + glGetString = (PFNGLGETSTRINGPROC)load("glGetString"); + if(glGetString == NULL) return 0; + if(glGetString(GL_VERSION) == NULL) return 0; + find_coreGL(); + load_GL_VERSION_1_0(load); + load_GL_VERSION_1_1(load); + load_GL_VERSION_1_2(load); + load_GL_VERSION_1_3(load); + load_GL_VERSION_1_4(load); + load_GL_VERSION_1_5(load); + load_GL_VERSION_2_0(load); + load_GL_VERSION_2_1(load); + load_GL_VERSION_3_0(load); + load_GL_VERSION_3_1(load); + load_GL_VERSION_3_2(load); + load_GL_VERSION_3_3(load); + load_GL_VERSION_4_0(load); + load_GL_VERSION_4_1(load); + + if (!find_extensionsGL()) return 0; + return GLVersion.major != 0 || GLVersion.minor != 0; +} + diff --git a/extern/stb/stb_image.h b/extern/stb/stb_image.h new file mode 100644 index 0000000..5e807a0 --- /dev/null +++ b/extern/stb/stb_image.h @@ -0,0 +1,7987 @@ +/* stb_image - v2.28 - public domain image loader - http://nothings.org/stb + no warranty implied; use at your own risk + + Do this: + #define STB_IMAGE_IMPLEMENTATION + before you include this file in *one* C or C++ file to create the implementation. + + // i.e. it should look like this: + #include ... + #include ... + #include ... + #define STB_IMAGE_IMPLEMENTATION + #include "stb_image.h" + + You can #define STBI_ASSERT(x) before the #include to avoid using assert.h. + And #define STBI_MALLOC, STBI_REALLOC, and STBI_FREE to avoid using malloc,realloc,free + + + QUICK NOTES: + Primarily of interest to game developers and other people who can + avoid problematic images and only need the trivial interface + + JPEG baseline & progressive (12 bpc/arithmetic not supported, same as stock IJG lib) + PNG 1/2/4/8/16-bit-per-channel + + TGA (not sure what subset, if a subset) + BMP non-1bpp, non-RLE + PSD (composited view only, no extra channels, 8/16 bit-per-channel) + + GIF (*comp always reports as 4-channel) + HDR (radiance rgbE format) + PIC (Softimage PIC) + PNM (PPM and PGM binary only) + + Animated GIF still needs a proper API, but here's one way to do it: + http://gist.github.com/urraka/685d9a6340b26b830d49 + + - decode from memory or through FILE (define STBI_NO_STDIO to remove code) + - decode from arbitrary I/O callbacks + - SIMD acceleration on x86/x64 (SSE2) and ARM (NEON) + + Full documentation under "DOCUMENTATION" below. + + +LICENSE + + See end of file for license information. + +RECENT REVISION HISTORY: + + 2.28 (2023-01-29) many error fixes, security errors, just tons of stuff + 2.27 (2021-07-11) document stbi_info better, 16-bit PNM support, bug fixes + 2.26 (2020-07-13) many minor fixes + 2.25 (2020-02-02) fix warnings + 2.24 (2020-02-02) fix warnings; thread-local failure_reason and flip_vertically + 2.23 (2019-08-11) fix clang static analysis warning + 2.22 (2019-03-04) gif fixes, fix warnings + 2.21 (2019-02-25) fix typo in comment + 2.20 (2019-02-07) support utf8 filenames in Windows; fix warnings and platform ifdefs + 2.19 (2018-02-11) fix warning + 2.18 (2018-01-30) fix warnings + 2.17 (2018-01-29) bugfix, 1-bit BMP, 16-bitness query, fix warnings + 2.16 (2017-07-23) all functions have 16-bit variants; optimizations; bugfixes + 2.15 (2017-03-18) fix png-1,2,4; all Imagenet JPGs; no runtime SSE detection on GCC + 2.14 (2017-03-03) remove deprecated STBI_JPEG_OLD; fixes for Imagenet JPGs + 2.13 (2016-12-04) experimental 16-bit API, only for PNG so far; fixes + 2.12 (2016-04-02) fix typo in 2.11 PSD fix that caused crashes + 2.11 (2016-04-02) 16-bit PNGS; enable SSE2 in non-gcc x64 + RGB-format JPEG; remove white matting in PSD; + allocate large structures on the stack; + correct channel count for PNG & BMP + 2.10 (2016-01-22) avoid warning introduced in 2.09 + 2.09 (2016-01-16) 16-bit TGA; comments in PNM files; STBI_REALLOC_SIZED + + See end of file for full revision history. + + + ============================ Contributors ========================= + + Image formats Extensions, features + Sean Barrett (jpeg, png, bmp) Jetro Lauha (stbi_info) + Nicolas Schulz (hdr, psd) Martin "SpartanJ" Golini (stbi_info) + Jonathan Dummer (tga) James "moose2000" Brown (iPhone PNG) + Jean-Marc Lienher (gif) Ben "Disch" Wenger (io callbacks) + Tom Seddon (pic) Omar Cornut (1/2/4-bit PNG) + Thatcher Ulrich (psd) Nicolas Guillemot (vertical flip) + Ken Miller (pgm, ppm) Richard Mitton (16-bit PSD) + github:urraka (animated gif) Junggon Kim (PNM comments) + Christopher Forseth (animated gif) Daniel Gibson (16-bit TGA) + socks-the-fox (16-bit PNG) + Jeremy Sawicki (handle all ImageNet JPGs) + Optimizations & bugfixes Mikhail Morozov (1-bit BMP) + Fabian "ryg" Giesen Anael Seghezzi (is-16-bit query) + Arseny Kapoulkine Simon Breuss (16-bit PNM) + John-Mark Allen + Carmelo J Fdez-Aguera + + Bug & warning fixes + Marc LeBlanc David Woo Guillaume George Martins Mozeiko + Christpher Lloyd Jerry Jansson Joseph Thomson Blazej Dariusz Roszkowski + Phil Jordan Dave Moore Roy Eltham + Hayaki Saito Nathan Reed Won Chun + Luke Graham Johan Duparc Nick Verigakis the Horde3D community + Thomas Ruf Ronny Chevalier github:rlyeh + Janez Zemva John Bartholomew Michal Cichon github:romigrou + Jonathan Blow Ken Hamada Tero Hanninen github:svdijk + Eugene Golushkov Laurent Gomila Cort Stratton github:snagar + Aruelien Pocheville Sergio Gonzalez Thibault Reuille github:Zelex + Cass Everitt Ryamond Barbiero github:grim210 + Paul Du Bois Engin Manap Aldo Culquicondor github:sammyhw + Philipp Wiesemann Dale Weiler Oriol Ferrer Mesia github:phprus + Josh Tobin Neil Bickford Matthew Gregan github:poppolopoppo + Julian Raschke Gregory Mullen Christian Floisand github:darealshinji + Baldur Karlsson Kevin Schmidt JR Smith github:Michaelangel007 + Brad Weinberger Matvey Cherevko github:mosra + Luca Sas Alexander Veselov Zack Middleton [reserved] + Ryan C. Gordon [reserved] [reserved] + DO NOT ADD YOUR NAME HERE + + Jacko Dirks + + To add your name to the credits, pick a random blank space in the middle and fill it. + 80% of merge conflicts on stb PRs are due to people adding their name at the end + of the credits. +*/ + +#ifndef STBI_INCLUDE_STB_IMAGE_H +#define STBI_INCLUDE_STB_IMAGE_H + +// DOCUMENTATION +// +// Limitations: +// - no 12-bit-per-channel JPEG +// - no JPEGs with arithmetic coding +// - GIF always returns *comp=4 +// +// Basic usage (see HDR discussion below for HDR usage): +// int x,y,n; +// unsigned char *data = stbi_load(filename, &x, &y, &n, 0); +// // ... process data if not NULL ... +// // ... x = width, y = height, n = # 8-bit components per pixel ... +// // ... replace '0' with '1'..'4' to force that many components per pixel +// // ... but 'n' will always be the number that it would have been if you said 0 +// stbi_image_free(data); +// +// Standard parameters: +// int *x -- outputs image width in pixels +// int *y -- outputs image height in pixels +// int *channels_in_file -- outputs # of image components in image file +// int desired_channels -- if non-zero, # of image components requested in result +// +// The return value from an image loader is an 'unsigned char *' which points +// to the pixel data, or NULL on an allocation failure or if the image is +// corrupt or invalid. The pixel data consists of *y scanlines of *x pixels, +// with each pixel consisting of N interleaved 8-bit components; the first +// pixel pointed to is top-left-most in the image. There is no padding between +// image scanlines or between pixels, regardless of format. The number of +// components N is 'desired_channels' if desired_channels is non-zero, or +// *channels_in_file otherwise. If desired_channels is non-zero, +// *channels_in_file has the number of components that _would_ have been +// output otherwise. E.g. if you set desired_channels to 4, you will always +// get RGBA output, but you can check *channels_in_file to see if it's trivially +// opaque because e.g. there were only 3 channels in the source image. +// +// An output image with N components has the following components interleaved +// in this order in each pixel: +// +// N=#comp components +// 1 grey +// 2 grey, alpha +// 3 red, green, blue +// 4 red, green, blue, alpha +// +// If image loading fails for any reason, the return value will be NULL, +// and *x, *y, *channels_in_file will be unchanged. The function +// stbi_failure_reason() can be queried for an extremely brief, end-user +// unfriendly explanation of why the load failed. Define STBI_NO_FAILURE_STRINGS +// to avoid compiling these strings at all, and STBI_FAILURE_USERMSG to get slightly +// more user-friendly ones. +// +// Paletted PNG, BMP, GIF, and PIC images are automatically depalettized. +// +// To query the width, height and component count of an image without having to +// decode the full file, you can use the stbi_info family of functions: +// +// int x,y,n,ok; +// ok = stbi_info(filename, &x, &y, &n); +// // returns ok=1 and sets x, y, n if image is a supported format, +// // 0 otherwise. +// +// Note that stb_image pervasively uses ints in its public API for sizes, +// including sizes of memory buffers. This is now part of the API and thus +// hard to change without causing breakage. As a result, the various image +// loaders all have certain limits on image size; these differ somewhat +// by format but generally boil down to either just under 2GB or just under +// 1GB. When the decoded image would be larger than this, stb_image decoding +// will fail. +// +// Additionally, stb_image will reject image files that have any of their +// dimensions set to a larger value than the configurable STBI_MAX_DIMENSIONS, +// which defaults to 2**24 = 16777216 pixels. Due to the above memory limit, +// the only way to have an image with such dimensions load correctly +// is for it to have a rather extreme aspect ratio. Either way, the +// assumption here is that such larger images are likely to be malformed +// or malicious. If you do need to load an image with individual dimensions +// larger than that, and it still fits in the overall size limit, you can +// #define STBI_MAX_DIMENSIONS on your own to be something larger. +// +// =========================================================================== +// +// UNICODE: +// +// If compiling for Windows and you wish to use Unicode filenames, compile +// with +// #define STBI_WINDOWS_UTF8 +// and pass utf8-encoded filenames. Call stbi_convert_wchar_to_utf8 to convert +// Windows wchar_t filenames to utf8. +// +// =========================================================================== +// +// Philosophy +// +// stb libraries are designed with the following priorities: +// +// 1. easy to use +// 2. easy to maintain +// 3. good performance +// +// Sometimes I let "good performance" creep up in priority over "easy to maintain", +// and for best performance I may provide less-easy-to-use APIs that give higher +// performance, in addition to the easy-to-use ones. Nevertheless, it's important +// to keep in mind that from the standpoint of you, a client of this library, +// all you care about is #1 and #3, and stb libraries DO NOT emphasize #3 above all. +// +// Some secondary priorities arise directly from the first two, some of which +// provide more explicit reasons why performance can't be emphasized. +// +// - Portable ("ease of use") +// - Small source code footprint ("easy to maintain") +// - No dependencies ("ease of use") +// +// =========================================================================== +// +// I/O callbacks +// +// I/O callbacks allow you to read from arbitrary sources, like packaged +// files or some other source. Data read from callbacks are processed +// through a small internal buffer (currently 128 bytes) to try to reduce +// overhead. +// +// The three functions you must define are "read" (reads some bytes of data), +// "skip" (skips some bytes of data), "eof" (reports if the stream is at the end). +// +// =========================================================================== +// +// SIMD support +// +// The JPEG decoder will try to automatically use SIMD kernels on x86 when +// supported by the compiler. For ARM Neon support, you must explicitly +// request it. +// +// (The old do-it-yourself SIMD API is no longer supported in the current +// code.) +// +// On x86, SSE2 will automatically be used when available based on a run-time +// test; if not, the generic C versions are used as a fall-back. On ARM targets, +// the typical path is to have separate builds for NEON and non-NEON devices +// (at least this is true for iOS and Android). Therefore, the NEON support is +// toggled by a build flag: define STBI_NEON to get NEON loops. +// +// If for some reason you do not want to use any of SIMD code, or if +// you have issues compiling it, you can disable it entirely by +// defining STBI_NO_SIMD. +// +// =========================================================================== +// +// HDR image support (disable by defining STBI_NO_HDR) +// +// stb_image supports loading HDR images in general, and currently the Radiance +// .HDR file format specifically. You can still load any file through the existing +// interface; if you attempt to load an HDR file, it will be automatically remapped +// to LDR, assuming gamma 2.2 and an arbitrary scale factor defaulting to 1; +// both of these constants can be reconfigured through this interface: +// +// stbi_hdr_to_ldr_gamma(2.2f); +// stbi_hdr_to_ldr_scale(1.0f); +// +// (note, do not use _inverse_ constants; stbi_image will invert them +// appropriately). +// +// Additionally, there is a new, parallel interface for loading files as +// (linear) floats to preserve the full dynamic range: +// +// float *data = stbi_loadf(filename, &x, &y, &n, 0); +// +// If you load LDR images through this interface, those images will +// be promoted to floating point values, run through the inverse of +// constants corresponding to the above: +// +// stbi_ldr_to_hdr_scale(1.0f); +// stbi_ldr_to_hdr_gamma(2.2f); +// +// Finally, given a filename (or an open file or memory block--see header +// file for details) containing image data, you can query for the "most +// appropriate" interface to use (that is, whether the image is HDR or +// not), using: +// +// stbi_is_hdr(char *filename); +// +// =========================================================================== +// +// iPhone PNG support: +// +// We optionally support converting iPhone-formatted PNGs (which store +// premultiplied BGRA) back to RGB, even though they're internally encoded +// differently. To enable this conversion, call +// stbi_convert_iphone_png_to_rgb(1). +// +// Call stbi_set_unpremultiply_on_load(1) as well to force a divide per +// pixel to remove any premultiplied alpha *only* if the image file explicitly +// says there's premultiplied data (currently only happens in iPhone images, +// and only if iPhone convert-to-rgb processing is on). +// +// =========================================================================== +// +// ADDITIONAL CONFIGURATION +// +// - You can suppress implementation of any of the decoders to reduce +// your code footprint by #defining one or more of the following +// symbols before creating the implementation. +// +// STBI_NO_JPEG +// STBI_NO_PNG +// STBI_NO_BMP +// STBI_NO_PSD +// STBI_NO_TGA +// STBI_NO_GIF +// STBI_NO_HDR +// STBI_NO_PIC +// STBI_NO_PNM (.ppm and .pgm) +// +// - You can request *only* certain decoders and suppress all other ones +// (this will be more forward-compatible, as addition of new decoders +// doesn't require you to disable them explicitly): +// +// STBI_ONLY_JPEG +// STBI_ONLY_PNG +// STBI_ONLY_BMP +// STBI_ONLY_PSD +// STBI_ONLY_TGA +// STBI_ONLY_GIF +// STBI_ONLY_HDR +// STBI_ONLY_PIC +// STBI_ONLY_PNM (.ppm and .pgm) +// +// - If you use STBI_NO_PNG (or _ONLY_ without PNG), and you still +// want the zlib decoder to be available, #define STBI_SUPPORT_ZLIB +// +// - If you define STBI_MAX_DIMENSIONS, stb_image will reject images greater +// than that size (in either width or height) without further processing. +// This is to let programs in the wild set an upper bound to prevent +// denial-of-service attacks on untrusted data, as one could generate a +// valid image of gigantic dimensions and force stb_image to allocate a +// huge block of memory and spend disproportionate time decoding it. By +// default this is set to (1 << 24), which is 16777216, but that's still +// very big. + +#ifndef STBI_NO_STDIO +#include +#endif // STBI_NO_STDIO + +#define STBI_VERSION 1 + +enum +{ + STBI_default = 0, // only used for desired_channels + + STBI_grey = 1, + STBI_grey_alpha = 2, + STBI_rgb = 3, + STBI_rgb_alpha = 4 +}; + +#include +typedef unsigned char stbi_uc; +typedef unsigned short stbi_us; + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef STBIDEF +#ifdef STB_IMAGE_STATIC +#define STBIDEF static +#else +#define STBIDEF extern +#endif +#endif + +////////////////////////////////////////////////////////////////////////////// +// +// PRIMARY API - works on images of any type +// + +// +// load image by filename, open file, or memory buffer +// + +typedef struct +{ + int (*read) (void *user,char *data,int size); // fill 'data' with 'size' bytes. return number of bytes actually read + void (*skip) (void *user,int n); // skip the next 'n' bytes, or 'unget' the last -n bytes if negative + int (*eof) (void *user); // returns nonzero if we are at end of file/data +} stbi_io_callbacks; + +//////////////////////////////////// +// +// 8-bits-per-channel interface +// + +STBIDEF stbi_uc *stbi_load_from_memory (stbi_uc const *buffer, int len , int *x, int *y, int *channels_in_file, int desired_channels); +STBIDEF stbi_uc *stbi_load_from_callbacks(stbi_io_callbacks const *clbk , void *user, int *x, int *y, int *channels_in_file, int desired_channels); + +#ifndef STBI_NO_STDIO +STBIDEF stbi_uc *stbi_load (char const *filename, int *x, int *y, int *channels_in_file, int desired_channels); +STBIDEF stbi_uc *stbi_load_from_file (FILE *f, int *x, int *y, int *channels_in_file, int desired_channels); +// for stbi_load_from_file, file pointer is left pointing immediately after image +#endif + +#ifndef STBI_NO_GIF +STBIDEF stbi_uc *stbi_load_gif_from_memory(stbi_uc const *buffer, int len, int **delays, int *x, int *y, int *z, int *comp, int req_comp); +#endif + +#ifdef STBI_WINDOWS_UTF8 +STBIDEF int stbi_convert_wchar_to_utf8(char *buffer, size_t bufferlen, const wchar_t* input); +#endif + +//////////////////////////////////// +// +// 16-bits-per-channel interface +// + +STBIDEF stbi_us *stbi_load_16_from_memory (stbi_uc const *buffer, int len, int *x, int *y, int *channels_in_file, int desired_channels); +STBIDEF stbi_us *stbi_load_16_from_callbacks(stbi_io_callbacks const *clbk, void *user, int *x, int *y, int *channels_in_file, int desired_channels); + +#ifndef STBI_NO_STDIO +STBIDEF stbi_us *stbi_load_16 (char const *filename, int *x, int *y, int *channels_in_file, int desired_channels); +STBIDEF stbi_us *stbi_load_from_file_16(FILE *f, int *x, int *y, int *channels_in_file, int desired_channels); +#endif + +//////////////////////////////////// +// +// float-per-channel interface +// +#ifndef STBI_NO_LINEAR + STBIDEF float *stbi_loadf_from_memory (stbi_uc const *buffer, int len, int *x, int *y, int *channels_in_file, int desired_channels); + STBIDEF float *stbi_loadf_from_callbacks (stbi_io_callbacks const *clbk, void *user, int *x, int *y, int *channels_in_file, int desired_channels); + + #ifndef STBI_NO_STDIO + STBIDEF float *stbi_loadf (char const *filename, int *x, int *y, int *channels_in_file, int desired_channels); + STBIDEF float *stbi_loadf_from_file (FILE *f, int *x, int *y, int *channels_in_file, int desired_channels); + #endif +#endif + +#ifndef STBI_NO_HDR + STBIDEF void stbi_hdr_to_ldr_gamma(float gamma); + STBIDEF void stbi_hdr_to_ldr_scale(float scale); +#endif // STBI_NO_HDR + +#ifndef STBI_NO_LINEAR + STBIDEF void stbi_ldr_to_hdr_gamma(float gamma); + STBIDEF void stbi_ldr_to_hdr_scale(float scale); +#endif // STBI_NO_LINEAR + +// stbi_is_hdr is always defined, but always returns false if STBI_NO_HDR +STBIDEF int stbi_is_hdr_from_callbacks(stbi_io_callbacks const *clbk, void *user); +STBIDEF int stbi_is_hdr_from_memory(stbi_uc const *buffer, int len); +#ifndef STBI_NO_STDIO +STBIDEF int stbi_is_hdr (char const *filename); +STBIDEF int stbi_is_hdr_from_file(FILE *f); +#endif // STBI_NO_STDIO + + +// get a VERY brief reason for failure +// on most compilers (and ALL modern mainstream compilers) this is threadsafe +STBIDEF const char *stbi_failure_reason (void); + +// free the loaded image -- this is just free() +STBIDEF void stbi_image_free (void *retval_from_stbi_load); + +// get image dimensions & components without fully decoding +STBIDEF int stbi_info_from_memory(stbi_uc const *buffer, int len, int *x, int *y, int *comp); +STBIDEF int stbi_info_from_callbacks(stbi_io_callbacks const *clbk, void *user, int *x, int *y, int *comp); +STBIDEF int stbi_is_16_bit_from_memory(stbi_uc const *buffer, int len); +STBIDEF int stbi_is_16_bit_from_callbacks(stbi_io_callbacks const *clbk, void *user); + +#ifndef STBI_NO_STDIO +STBIDEF int stbi_info (char const *filename, int *x, int *y, int *comp); +STBIDEF int stbi_info_from_file (FILE *f, int *x, int *y, int *comp); +STBIDEF int stbi_is_16_bit (char const *filename); +STBIDEF int stbi_is_16_bit_from_file(FILE *f); +#endif + + + +// for image formats that explicitly notate that they have premultiplied alpha, +// we just return the colors as stored in the file. set this flag to force +// unpremultiplication. results are undefined if the unpremultiply overflow. +STBIDEF void stbi_set_unpremultiply_on_load(int flag_true_if_should_unpremultiply); + +// indicate whether we should process iphone images back to canonical format, +// or just pass them through "as-is" +STBIDEF void stbi_convert_iphone_png_to_rgb(int flag_true_if_should_convert); + +// flip the image vertically, so the first pixel in the output array is the bottom left +STBIDEF void stbi_set_flip_vertically_on_load(int flag_true_if_should_flip); + +// as above, but only applies to images loaded on the thread that calls the function +// this function is only available if your compiler supports thread-local variables; +// calling it will fail to link if your compiler doesn't +STBIDEF void stbi_set_unpremultiply_on_load_thread(int flag_true_if_should_unpremultiply); +STBIDEF void stbi_convert_iphone_png_to_rgb_thread(int flag_true_if_should_convert); +STBIDEF void stbi_set_flip_vertically_on_load_thread(int flag_true_if_should_flip); + +// ZLIB client - used by PNG, available for other purposes + +STBIDEF char *stbi_zlib_decode_malloc_guesssize(const char *buffer, int len, int initial_size, int *outlen); +STBIDEF char *stbi_zlib_decode_malloc_guesssize_headerflag(const char *buffer, int len, int initial_size, int *outlen, int parse_header); +STBIDEF char *stbi_zlib_decode_malloc(const char *buffer, int len, int *outlen); +STBIDEF int stbi_zlib_decode_buffer(char *obuffer, int olen, const char *ibuffer, int ilen); + +STBIDEF char *stbi_zlib_decode_noheader_malloc(const char *buffer, int len, int *outlen); +STBIDEF int stbi_zlib_decode_noheader_buffer(char *obuffer, int olen, const char *ibuffer, int ilen); + + +#ifdef __cplusplus +} +#endif + +// +// +//// end header file ///////////////////////////////////////////////////// +#endif // STBI_INCLUDE_STB_IMAGE_H + +#ifdef STB_IMAGE_IMPLEMENTATION + +#if defined(STBI_ONLY_JPEG) || defined(STBI_ONLY_PNG) || defined(STBI_ONLY_BMP) \ + || defined(STBI_ONLY_TGA) || defined(STBI_ONLY_GIF) || defined(STBI_ONLY_PSD) \ + || defined(STBI_ONLY_HDR) || defined(STBI_ONLY_PIC) || defined(STBI_ONLY_PNM) \ + || defined(STBI_ONLY_ZLIB) + #ifndef STBI_ONLY_JPEG + #define STBI_NO_JPEG + #endif + #ifndef STBI_ONLY_PNG + #define STBI_NO_PNG + #endif + #ifndef STBI_ONLY_BMP + #define STBI_NO_BMP + #endif + #ifndef STBI_ONLY_PSD + #define STBI_NO_PSD + #endif + #ifndef STBI_ONLY_TGA + #define STBI_NO_TGA + #endif + #ifndef STBI_ONLY_GIF + #define STBI_NO_GIF + #endif + #ifndef STBI_ONLY_HDR + #define STBI_NO_HDR + #endif + #ifndef STBI_ONLY_PIC + #define STBI_NO_PIC + #endif + #ifndef STBI_ONLY_PNM + #define STBI_NO_PNM + #endif +#endif + +#if defined(STBI_NO_PNG) && !defined(STBI_SUPPORT_ZLIB) && !defined(STBI_NO_ZLIB) +#define STBI_NO_ZLIB +#endif + + +#include +#include // ptrdiff_t on osx +#include +#include +#include + +#if !defined(STBI_NO_LINEAR) || !defined(STBI_NO_HDR) +#include // ldexp, pow +#endif + +#ifndef STBI_NO_STDIO +#include +#endif + +#ifndef STBI_ASSERT +#include +#define STBI_ASSERT(x) assert(x) +#endif + +#ifdef __cplusplus +#define STBI_EXTERN extern "C" +#else +#define STBI_EXTERN extern +#endif + + +#ifndef _MSC_VER + #ifdef __cplusplus + #define stbi_inline inline + #else + #define stbi_inline + #endif +#else + #define stbi_inline __forceinline +#endif + +#ifndef STBI_NO_THREAD_LOCALS + #if defined(__cplusplus) && __cplusplus >= 201103L + #define STBI_THREAD_LOCAL thread_local + #elif defined(__GNUC__) && __GNUC__ < 5 + #define STBI_THREAD_LOCAL __thread + #elif defined(_MSC_VER) + #define STBI_THREAD_LOCAL __declspec(thread) + #elif defined (__STDC_VERSION__) && __STDC_VERSION__ >= 201112L && !defined(__STDC_NO_THREADS__) + #define STBI_THREAD_LOCAL _Thread_local + #endif + + #ifndef STBI_THREAD_LOCAL + #if defined(__GNUC__) + #define STBI_THREAD_LOCAL __thread + #endif + #endif +#endif + +#if defined(_MSC_VER) || defined(__SYMBIAN32__) +typedef unsigned short stbi__uint16; +typedef signed short stbi__int16; +typedef unsigned int stbi__uint32; +typedef signed int stbi__int32; +#else +#include +typedef uint16_t stbi__uint16; +typedef int16_t stbi__int16; +typedef uint32_t stbi__uint32; +typedef int32_t stbi__int32; +#endif + +// should produce compiler error if size is wrong +typedef unsigned char validate_uint32[sizeof(stbi__uint32)==4 ? 1 : -1]; + +#ifdef _MSC_VER +#define STBI_NOTUSED(v) (void)(v) +#else +#define STBI_NOTUSED(v) (void)sizeof(v) +#endif + +#ifdef _MSC_VER +#define STBI_HAS_LROTL +#endif + +#ifdef STBI_HAS_LROTL + #define stbi_lrot(x,y) _lrotl(x,y) +#else + #define stbi_lrot(x,y) (((x) << (y)) | ((x) >> (-(y) & 31))) +#endif + +#if defined(STBI_MALLOC) && defined(STBI_FREE) && (defined(STBI_REALLOC) || defined(STBI_REALLOC_SIZED)) +// ok +#elif !defined(STBI_MALLOC) && !defined(STBI_FREE) && !defined(STBI_REALLOC) && !defined(STBI_REALLOC_SIZED) +// ok +#else +#error "Must define all or none of STBI_MALLOC, STBI_FREE, and STBI_REALLOC (or STBI_REALLOC_SIZED)." +#endif + +#ifndef STBI_MALLOC +#define STBI_MALLOC(sz) malloc(sz) +#define STBI_REALLOC(p,newsz) realloc(p,newsz) +#define STBI_FREE(p) free(p) +#endif + +#ifndef STBI_REALLOC_SIZED +#define STBI_REALLOC_SIZED(p,oldsz,newsz) STBI_REALLOC(p,newsz) +#endif + +// x86/x64 detection +#if defined(__x86_64__) || defined(_M_X64) +#define STBI__X64_TARGET +#elif defined(__i386) || defined(_M_IX86) +#define STBI__X86_TARGET +#endif + +#if defined(__GNUC__) && defined(STBI__X86_TARGET) && !defined(__SSE2__) && !defined(STBI_NO_SIMD) +// gcc doesn't support sse2 intrinsics unless you compile with -msse2, +// which in turn means it gets to use SSE2 everywhere. This is unfortunate, +// but previous attempts to provide the SSE2 functions with runtime +// detection caused numerous issues. The way architecture extensions are +// exposed in GCC/Clang is, sadly, not really suited for one-file libs. +// New behavior: if compiled with -msse2, we use SSE2 without any +// detection; if not, we don't use it at all. +#define STBI_NO_SIMD +#endif + +#if defined(__MINGW32__) && defined(STBI__X86_TARGET) && !defined(STBI_MINGW_ENABLE_SSE2) && !defined(STBI_NO_SIMD) +// Note that __MINGW32__ doesn't actually mean 32-bit, so we have to avoid STBI__X64_TARGET +// +// 32-bit MinGW wants ESP to be 16-byte aligned, but this is not in the +// Windows ABI and VC++ as well as Windows DLLs don't maintain that invariant. +// As a result, enabling SSE2 on 32-bit MinGW is dangerous when not +// simultaneously enabling "-mstackrealign". +// +// See https://github.com/nothings/stb/issues/81 for more information. +// +// So default to no SSE2 on 32-bit MinGW. If you've read this far and added +// -mstackrealign to your build settings, feel free to #define STBI_MINGW_ENABLE_SSE2. +#define STBI_NO_SIMD +#endif + +#if !defined(STBI_NO_SIMD) && (defined(STBI__X86_TARGET) || defined(STBI__X64_TARGET)) +#define STBI_SSE2 +#include + +#ifdef _MSC_VER + +#if _MSC_VER >= 1400 // not VC6 +#include // __cpuid +static int stbi__cpuid3(void) +{ + int info[4]; + __cpuid(info,1); + return info[3]; +} +#else +static int stbi__cpuid3(void) +{ + int res; + __asm { + mov eax,1 + cpuid + mov res,edx + } + return res; +} +#endif + +#define STBI_SIMD_ALIGN(type, name) __declspec(align(16)) type name + +#if !defined(STBI_NO_JPEG) && defined(STBI_SSE2) +static int stbi__sse2_available(void) +{ + int info3 = stbi__cpuid3(); + return ((info3 >> 26) & 1) != 0; +} +#endif + +#else // assume GCC-style if not VC++ +#define STBI_SIMD_ALIGN(type, name) type name __attribute__((aligned(16))) + +#if !defined(STBI_NO_JPEG) && defined(STBI_SSE2) +static int stbi__sse2_available(void) +{ + // If we're even attempting to compile this on GCC/Clang, that means + // -msse2 is on, which means the compiler is allowed to use SSE2 + // instructions at will, and so are we. + return 1; +} +#endif + +#endif +#endif + +// ARM NEON +#if defined(STBI_NO_SIMD) && defined(STBI_NEON) +#undef STBI_NEON +#endif + +#ifdef STBI_NEON +#include +#ifdef _MSC_VER +#define STBI_SIMD_ALIGN(type, name) __declspec(align(16)) type name +#else +#define STBI_SIMD_ALIGN(type, name) type name __attribute__((aligned(16))) +#endif +#endif + +#ifndef STBI_SIMD_ALIGN +#define STBI_SIMD_ALIGN(type, name) type name +#endif + +#ifndef STBI_MAX_DIMENSIONS +#define STBI_MAX_DIMENSIONS (1 << 24) +#endif + +/////////////////////////////////////////////// +// +// stbi__context struct and start_xxx functions + +// stbi__context structure is our basic context used by all images, so it +// contains all the IO context, plus some basic image information +typedef struct +{ + stbi__uint32 img_x, img_y; + int img_n, img_out_n; + + stbi_io_callbacks io; + void *io_user_data; + + int read_from_callbacks; + int buflen; + stbi_uc buffer_start[128]; + int callback_already_read; + + stbi_uc *img_buffer, *img_buffer_end; + stbi_uc *img_buffer_original, *img_buffer_original_end; +} stbi__context; + + +static void stbi__refill_buffer(stbi__context *s); + +// initialize a memory-decode context +static void stbi__start_mem(stbi__context *s, stbi_uc const *buffer, int len) +{ + s->io.read = NULL; + s->read_from_callbacks = 0; + s->callback_already_read = 0; + s->img_buffer = s->img_buffer_original = (stbi_uc *) buffer; + s->img_buffer_end = s->img_buffer_original_end = (stbi_uc *) buffer+len; +} + +// initialize a callback-based context +static void stbi__start_callbacks(stbi__context *s, stbi_io_callbacks *c, void *user) +{ + s->io = *c; + s->io_user_data = user; + s->buflen = sizeof(s->buffer_start); + s->read_from_callbacks = 1; + s->callback_already_read = 0; + s->img_buffer = s->img_buffer_original = s->buffer_start; + stbi__refill_buffer(s); + s->img_buffer_original_end = s->img_buffer_end; +} + +#ifndef STBI_NO_STDIO + +static int stbi__stdio_read(void *user, char *data, int size) +{ + return (int) fread(data,1,size,(FILE*) user); +} + +static void stbi__stdio_skip(void *user, int n) +{ + int ch; + fseek((FILE*) user, n, SEEK_CUR); + ch = fgetc((FILE*) user); /* have to read a byte to reset feof()'s flag */ + if (ch != EOF) { + ungetc(ch, (FILE *) user); /* push byte back onto stream if valid. */ + } +} + +static int stbi__stdio_eof(void *user) +{ + return feof((FILE*) user) || ferror((FILE *) user); +} + +static stbi_io_callbacks stbi__stdio_callbacks = +{ + stbi__stdio_read, + stbi__stdio_skip, + stbi__stdio_eof, +}; + +static void stbi__start_file(stbi__context *s, FILE *f) +{ + stbi__start_callbacks(s, &stbi__stdio_callbacks, (void *) f); +} + +//static void stop_file(stbi__context *s) { } + +#endif // !STBI_NO_STDIO + +static void stbi__rewind(stbi__context *s) +{ + // conceptually rewind SHOULD rewind to the beginning of the stream, + // but we just rewind to the beginning of the initial buffer, because + // we only use it after doing 'test', which only ever looks at at most 92 bytes + s->img_buffer = s->img_buffer_original; + s->img_buffer_end = s->img_buffer_original_end; +} + +enum +{ + STBI_ORDER_RGB, + STBI_ORDER_BGR +}; + +typedef struct +{ + int bits_per_channel; + int num_channels; + int channel_order; +} stbi__result_info; + +#ifndef STBI_NO_JPEG +static int stbi__jpeg_test(stbi__context *s); +static void *stbi__jpeg_load(stbi__context *s, int *x, int *y, int *comp, int req_comp, stbi__result_info *ri); +static int stbi__jpeg_info(stbi__context *s, int *x, int *y, int *comp); +#endif + +#ifndef STBI_NO_PNG +static int stbi__png_test(stbi__context *s); +static void *stbi__png_load(stbi__context *s, int *x, int *y, int *comp, int req_comp, stbi__result_info *ri); +static int stbi__png_info(stbi__context *s, int *x, int *y, int *comp); +static int stbi__png_is16(stbi__context *s); +#endif + +#ifndef STBI_NO_BMP +static int stbi__bmp_test(stbi__context *s); +static void *stbi__bmp_load(stbi__context *s, int *x, int *y, int *comp, int req_comp, stbi__result_info *ri); +static int stbi__bmp_info(stbi__context *s, int *x, int *y, int *comp); +#endif + +#ifndef STBI_NO_TGA +static int stbi__tga_test(stbi__context *s); +static void *stbi__tga_load(stbi__context *s, int *x, int *y, int *comp, int req_comp, stbi__result_info *ri); +static int stbi__tga_info(stbi__context *s, int *x, int *y, int *comp); +#endif + +#ifndef STBI_NO_PSD +static int stbi__psd_test(stbi__context *s); +static void *stbi__psd_load(stbi__context *s, int *x, int *y, int *comp, int req_comp, stbi__result_info *ri, int bpc); +static int stbi__psd_info(stbi__context *s, int *x, int *y, int *comp); +static int stbi__psd_is16(stbi__context *s); +#endif + +#ifndef STBI_NO_HDR +static int stbi__hdr_test(stbi__context *s); +static float *stbi__hdr_load(stbi__context *s, int *x, int *y, int *comp, int req_comp, stbi__result_info *ri); +static int stbi__hdr_info(stbi__context *s, int *x, int *y, int *comp); +#endif + +#ifndef STBI_NO_PIC +static int stbi__pic_test(stbi__context *s); +static void *stbi__pic_load(stbi__context *s, int *x, int *y, int *comp, int req_comp, stbi__result_info *ri); +static int stbi__pic_info(stbi__context *s, int *x, int *y, int *comp); +#endif + +#ifndef STBI_NO_GIF +static int stbi__gif_test(stbi__context *s); +static void *stbi__gif_load(stbi__context *s, int *x, int *y, int *comp, int req_comp, stbi__result_info *ri); +static void *stbi__load_gif_main(stbi__context *s, int **delays, int *x, int *y, int *z, int *comp, int req_comp); +static int stbi__gif_info(stbi__context *s, int *x, int *y, int *comp); +#endif + +#ifndef STBI_NO_PNM +static int stbi__pnm_test(stbi__context *s); +static void *stbi__pnm_load(stbi__context *s, int *x, int *y, int *comp, int req_comp, stbi__result_info *ri); +static int stbi__pnm_info(stbi__context *s, int *x, int *y, int *comp); +static int stbi__pnm_is16(stbi__context *s); +#endif + +static +#ifdef STBI_THREAD_LOCAL +STBI_THREAD_LOCAL +#endif +const char *stbi__g_failure_reason; + +STBIDEF const char *stbi_failure_reason(void) +{ + return stbi__g_failure_reason; +} + +#ifndef STBI_NO_FAILURE_STRINGS +static int stbi__err(const char *str) +{ + stbi__g_failure_reason = str; + return 0; +} +#endif + +static void *stbi__malloc(size_t size) +{ + return STBI_MALLOC(size); +} + +// stb_image uses ints pervasively, including for offset calculations. +// therefore the largest decoded image size we can support with the +// current code, even on 64-bit targets, is INT_MAX. this is not a +// significant limitation for the intended use case. +// +// we do, however, need to make sure our size calculations don't +// overflow. hence a few helper functions for size calculations that +// multiply integers together, making sure that they're non-negative +// and no overflow occurs. + +// return 1 if the sum is valid, 0 on overflow. +// negative terms are considered invalid. +static int stbi__addsizes_valid(int a, int b) +{ + if (b < 0) return 0; + // now 0 <= b <= INT_MAX, hence also + // 0 <= INT_MAX - b <= INTMAX. + // And "a + b <= INT_MAX" (which might overflow) is the + // same as a <= INT_MAX - b (no overflow) + return a <= INT_MAX - b; +} + +// returns 1 if the product is valid, 0 on overflow. +// negative factors are considered invalid. +static int stbi__mul2sizes_valid(int a, int b) +{ + if (a < 0 || b < 0) return 0; + if (b == 0) return 1; // mul-by-0 is always safe + // portable way to check for no overflows in a*b + return a <= INT_MAX/b; +} + +#if !defined(STBI_NO_JPEG) || !defined(STBI_NO_PNG) || !defined(STBI_NO_TGA) || !defined(STBI_NO_HDR) +// returns 1 if "a*b + add" has no negative terms/factors and doesn't overflow +static int stbi__mad2sizes_valid(int a, int b, int add) +{ + return stbi__mul2sizes_valid(a, b) && stbi__addsizes_valid(a*b, add); +} +#endif + +// returns 1 if "a*b*c + add" has no negative terms/factors and doesn't overflow +static int stbi__mad3sizes_valid(int a, int b, int c, int add) +{ + return stbi__mul2sizes_valid(a, b) && stbi__mul2sizes_valid(a*b, c) && + stbi__addsizes_valid(a*b*c, add); +} + +// returns 1 if "a*b*c*d + add" has no negative terms/factors and doesn't overflow +#if !defined(STBI_NO_LINEAR) || !defined(STBI_NO_HDR) || !defined(STBI_NO_PNM) +static int stbi__mad4sizes_valid(int a, int b, int c, int d, int add) +{ + return stbi__mul2sizes_valid(a, b) && stbi__mul2sizes_valid(a*b, c) && + stbi__mul2sizes_valid(a*b*c, d) && stbi__addsizes_valid(a*b*c*d, add); +} +#endif + +#if !defined(STBI_NO_JPEG) || !defined(STBI_NO_PNG) || !defined(STBI_NO_TGA) || !defined(STBI_NO_HDR) +// mallocs with size overflow checking +static void *stbi__malloc_mad2(int a, int b, int add) +{ + if (!stbi__mad2sizes_valid(a, b, add)) return NULL; + return stbi__malloc(a*b + add); +} +#endif + +static void *stbi__malloc_mad3(int a, int b, int c, int add) +{ + if (!stbi__mad3sizes_valid(a, b, c, add)) return NULL; + return stbi__malloc(a*b*c + add); +} + +#if !defined(STBI_NO_LINEAR) || !defined(STBI_NO_HDR) || !defined(STBI_NO_PNM) +static void *stbi__malloc_mad4(int a, int b, int c, int d, int add) +{ + if (!stbi__mad4sizes_valid(a, b, c, d, add)) return NULL; + return stbi__malloc(a*b*c*d + add); +} +#endif + +// returns 1 if the sum of two signed ints is valid (between -2^31 and 2^31-1 inclusive), 0 on overflow. +static int stbi__addints_valid(int a, int b) +{ + if ((a >= 0) != (b >= 0)) return 1; // a and b have different signs, so no overflow + if (a < 0 && b < 0) return a >= INT_MIN - b; // same as a + b >= INT_MIN; INT_MIN - b cannot overflow since b < 0. + return a <= INT_MAX - b; +} + +// returns 1 if the product of two signed shorts is valid, 0 on overflow. +static int stbi__mul2shorts_valid(short a, short b) +{ + if (b == 0 || b == -1) return 1; // multiplication by 0 is always 0; check for -1 so SHRT_MIN/b doesn't overflow + if ((a >= 0) == (b >= 0)) return a <= SHRT_MAX/b; // product is positive, so similar to mul2sizes_valid + if (b < 0) return a <= SHRT_MIN / b; // same as a * b >= SHRT_MIN + return a >= SHRT_MIN / b; +} + +// stbi__err - error +// stbi__errpf - error returning pointer to float +// stbi__errpuc - error returning pointer to unsigned char + +#ifdef STBI_NO_FAILURE_STRINGS + #define stbi__err(x,y) 0 +#elif defined(STBI_FAILURE_USERMSG) + #define stbi__err(x,y) stbi__err(y) +#else + #define stbi__err(x,y) stbi__err(x) +#endif + +#define stbi__errpf(x,y) ((float *)(size_t) (stbi__err(x,y)?NULL:NULL)) +#define stbi__errpuc(x,y) ((unsigned char *)(size_t) (stbi__err(x,y)?NULL:NULL)) + +STBIDEF void stbi_image_free(void *retval_from_stbi_load) +{ + STBI_FREE(retval_from_stbi_load); +} + +#ifndef STBI_NO_LINEAR +static float *stbi__ldr_to_hdr(stbi_uc *data, int x, int y, int comp); +#endif + +#ifndef STBI_NO_HDR +static stbi_uc *stbi__hdr_to_ldr(float *data, int x, int y, int comp); +#endif + +static int stbi__vertically_flip_on_load_global = 0; + +STBIDEF void stbi_set_flip_vertically_on_load(int flag_true_if_should_flip) +{ + stbi__vertically_flip_on_load_global = flag_true_if_should_flip; +} + +#ifndef STBI_THREAD_LOCAL +#define stbi__vertically_flip_on_load stbi__vertically_flip_on_load_global +#else +static STBI_THREAD_LOCAL int stbi__vertically_flip_on_load_local, stbi__vertically_flip_on_load_set; + +STBIDEF void stbi_set_flip_vertically_on_load_thread(int flag_true_if_should_flip) +{ + stbi__vertically_flip_on_load_local = flag_true_if_should_flip; + stbi__vertically_flip_on_load_set = 1; +} + +#define stbi__vertically_flip_on_load (stbi__vertically_flip_on_load_set \ + ? stbi__vertically_flip_on_load_local \ + : stbi__vertically_flip_on_load_global) +#endif // STBI_THREAD_LOCAL + +static void *stbi__load_main(stbi__context *s, int *x, int *y, int *comp, int req_comp, stbi__result_info *ri, int bpc) +{ + memset(ri, 0, sizeof(*ri)); // make sure it's initialized if we add new fields + ri->bits_per_channel = 8; // default is 8 so most paths don't have to be changed + ri->channel_order = STBI_ORDER_RGB; // all current input & output are this, but this is here so we can add BGR order + ri->num_channels = 0; + + // test the formats with a very explicit header first (at least a FOURCC + // or distinctive magic number first) + #ifndef STBI_NO_PNG + if (stbi__png_test(s)) return stbi__png_load(s,x,y,comp,req_comp, ri); + #endif + #ifndef STBI_NO_BMP + if (stbi__bmp_test(s)) return stbi__bmp_load(s,x,y,comp,req_comp, ri); + #endif + #ifndef STBI_NO_GIF + if (stbi__gif_test(s)) return stbi__gif_load(s,x,y,comp,req_comp, ri); + #endif + #ifndef STBI_NO_PSD + if (stbi__psd_test(s)) return stbi__psd_load(s,x,y,comp,req_comp, ri, bpc); + #else + STBI_NOTUSED(bpc); + #endif + #ifndef STBI_NO_PIC + if (stbi__pic_test(s)) return stbi__pic_load(s,x,y,comp,req_comp, ri); + #endif + + // then the formats that can end up attempting to load with just 1 or 2 + // bytes matching expectations; these are prone to false positives, so + // try them later + #ifndef STBI_NO_JPEG + if (stbi__jpeg_test(s)) return stbi__jpeg_load(s,x,y,comp,req_comp, ri); + #endif + #ifndef STBI_NO_PNM + if (stbi__pnm_test(s)) return stbi__pnm_load(s,x,y,comp,req_comp, ri); + #endif + + #ifndef STBI_NO_HDR + if (stbi__hdr_test(s)) { + float *hdr = stbi__hdr_load(s, x,y,comp,req_comp, ri); + return stbi__hdr_to_ldr(hdr, *x, *y, req_comp ? req_comp : *comp); + } + #endif + + #ifndef STBI_NO_TGA + // test tga last because it's a crappy test! + if (stbi__tga_test(s)) + return stbi__tga_load(s,x,y,comp,req_comp, ri); + #endif + + return stbi__errpuc("unknown image type", "Image not of any known type, or corrupt"); +} + +static stbi_uc *stbi__convert_16_to_8(stbi__uint16 *orig, int w, int h, int channels) +{ + int i; + int img_len = w * h * channels; + stbi_uc *reduced; + + reduced = (stbi_uc *) stbi__malloc(img_len); + if (reduced == NULL) return stbi__errpuc("outofmem", "Out of memory"); + + for (i = 0; i < img_len; ++i) + reduced[i] = (stbi_uc)((orig[i] >> 8) & 0xFF); // top half of each byte is sufficient approx of 16->8 bit scaling + + STBI_FREE(orig); + return reduced; +} + +static stbi__uint16 *stbi__convert_8_to_16(stbi_uc *orig, int w, int h, int channels) +{ + int i; + int img_len = w * h * channels; + stbi__uint16 *enlarged; + + enlarged = (stbi__uint16 *) stbi__malloc(img_len*2); + if (enlarged == NULL) return (stbi__uint16 *) stbi__errpuc("outofmem", "Out of memory"); + + for (i = 0; i < img_len; ++i) + enlarged[i] = (stbi__uint16)((orig[i] << 8) + orig[i]); // replicate to high and low byte, maps 0->0, 255->0xffff + + STBI_FREE(orig); + return enlarged; +} + +static void stbi__vertical_flip(void *image, int w, int h, int bytes_per_pixel) +{ + int row; + size_t bytes_per_row = (size_t)w * bytes_per_pixel; + stbi_uc temp[2048]; + stbi_uc *bytes = (stbi_uc *)image; + + for (row = 0; row < (h>>1); row++) { + stbi_uc *row0 = bytes + row*bytes_per_row; + stbi_uc *row1 = bytes + (h - row - 1)*bytes_per_row; + // swap row0 with row1 + size_t bytes_left = bytes_per_row; + while (bytes_left) { + size_t bytes_copy = (bytes_left < sizeof(temp)) ? bytes_left : sizeof(temp); + memcpy(temp, row0, bytes_copy); + memcpy(row0, row1, bytes_copy); + memcpy(row1, temp, bytes_copy); + row0 += bytes_copy; + row1 += bytes_copy; + bytes_left -= bytes_copy; + } + } +} + +#ifndef STBI_NO_GIF +static void stbi__vertical_flip_slices(void *image, int w, int h, int z, int bytes_per_pixel) +{ + int slice; + int slice_size = w * h * bytes_per_pixel; + + stbi_uc *bytes = (stbi_uc *)image; + for (slice = 0; slice < z; ++slice) { + stbi__vertical_flip(bytes, w, h, bytes_per_pixel); + bytes += slice_size; + } +} +#endif + +static unsigned char *stbi__load_and_postprocess_8bit(stbi__context *s, int *x, int *y, int *comp, int req_comp) +{ + stbi__result_info ri; + void *result = stbi__load_main(s, x, y, comp, req_comp, &ri, 8); + + if (result == NULL) + return NULL; + + // it is the responsibility of the loaders to make sure we get either 8 or 16 bit. + STBI_ASSERT(ri.bits_per_channel == 8 || ri.bits_per_channel == 16); + + if (ri.bits_per_channel != 8) { + result = stbi__convert_16_to_8((stbi__uint16 *) result, *x, *y, req_comp == 0 ? *comp : req_comp); + ri.bits_per_channel = 8; + } + + // @TODO: move stbi__convert_format to here + + if (stbi__vertically_flip_on_load) { + int channels = req_comp ? req_comp : *comp; + stbi__vertical_flip(result, *x, *y, channels * sizeof(stbi_uc)); + } + + return (unsigned char *) result; +} + +static stbi__uint16 *stbi__load_and_postprocess_16bit(stbi__context *s, int *x, int *y, int *comp, int req_comp) +{ + stbi__result_info ri; + void *result = stbi__load_main(s, x, y, comp, req_comp, &ri, 16); + + if (result == NULL) + return NULL; + + // it is the responsibility of the loaders to make sure we get either 8 or 16 bit. + STBI_ASSERT(ri.bits_per_channel == 8 || ri.bits_per_channel == 16); + + if (ri.bits_per_channel != 16) { + result = stbi__convert_8_to_16((stbi_uc *) result, *x, *y, req_comp == 0 ? *comp : req_comp); + ri.bits_per_channel = 16; + } + + // @TODO: move stbi__convert_format16 to here + // @TODO: special case RGB-to-Y (and RGBA-to-YA) for 8-bit-to-16-bit case to keep more precision + + if (stbi__vertically_flip_on_load) { + int channels = req_comp ? req_comp : *comp; + stbi__vertical_flip(result, *x, *y, channels * sizeof(stbi__uint16)); + } + + return (stbi__uint16 *) result; +} + +#if !defined(STBI_NO_HDR) && !defined(STBI_NO_LINEAR) +static void stbi__float_postprocess(float *result, int *x, int *y, int *comp, int req_comp) +{ + if (stbi__vertically_flip_on_load && result != NULL) { + int channels = req_comp ? req_comp : *comp; + stbi__vertical_flip(result, *x, *y, channels * sizeof(float)); + } +} +#endif + +#ifndef STBI_NO_STDIO + +#if defined(_WIN32) && defined(STBI_WINDOWS_UTF8) +STBI_EXTERN __declspec(dllimport) int __stdcall MultiByteToWideChar(unsigned int cp, unsigned long flags, const char *str, int cbmb, wchar_t *widestr, int cchwide); +STBI_EXTERN __declspec(dllimport) int __stdcall WideCharToMultiByte(unsigned int cp, unsigned long flags, const wchar_t *widestr, int cchwide, char *str, int cbmb, const char *defchar, int *used_default); +#endif + +#if defined(_WIN32) && defined(STBI_WINDOWS_UTF8) +STBIDEF int stbi_convert_wchar_to_utf8(char *buffer, size_t bufferlen, const wchar_t* input) +{ + return WideCharToMultiByte(65001 /* UTF8 */, 0, input, -1, buffer, (int) bufferlen, NULL, NULL); +} +#endif + +static FILE *stbi__fopen(char const *filename, char const *mode) +{ + FILE *f; +#if defined(_WIN32) && defined(STBI_WINDOWS_UTF8) + wchar_t wMode[64]; + wchar_t wFilename[1024]; + if (0 == MultiByteToWideChar(65001 /* UTF8 */, 0, filename, -1, wFilename, sizeof(wFilename)/sizeof(*wFilename))) + return 0; + + if (0 == MultiByteToWideChar(65001 /* UTF8 */, 0, mode, -1, wMode, sizeof(wMode)/sizeof(*wMode))) + return 0; + +#if defined(_MSC_VER) && _MSC_VER >= 1400 + if (0 != _wfopen_s(&f, wFilename, wMode)) + f = 0; +#else + f = _wfopen(wFilename, wMode); +#endif + +#elif defined(_MSC_VER) && _MSC_VER >= 1400 + if (0 != fopen_s(&f, filename, mode)) + f=0; +#else + f = fopen(filename, mode); +#endif + return f; +} + + +STBIDEF stbi_uc *stbi_load(char const *filename, int *x, int *y, int *comp, int req_comp) +{ + FILE *f = stbi__fopen(filename, "rb"); + unsigned char *result; + if (!f) return stbi__errpuc("can't fopen", "Unable to open file"); + result = stbi_load_from_file(f,x,y,comp,req_comp); + fclose(f); + return result; +} + +STBIDEF stbi_uc *stbi_load_from_file(FILE *f, int *x, int *y, int *comp, int req_comp) +{ + unsigned char *result; + stbi__context s; + stbi__start_file(&s,f); + result = stbi__load_and_postprocess_8bit(&s,x,y,comp,req_comp); + if (result) { + // need to 'unget' all the characters in the IO buffer + fseek(f, - (int) (s.img_buffer_end - s.img_buffer), SEEK_CUR); + } + return result; +} + +STBIDEF stbi__uint16 *stbi_load_from_file_16(FILE *f, int *x, int *y, int *comp, int req_comp) +{ + stbi__uint16 *result; + stbi__context s; + stbi__start_file(&s,f); + result = stbi__load_and_postprocess_16bit(&s,x,y,comp,req_comp); + if (result) { + // need to 'unget' all the characters in the IO buffer + fseek(f, - (int) (s.img_buffer_end - s.img_buffer), SEEK_CUR); + } + return result; +} + +STBIDEF stbi_us *stbi_load_16(char const *filename, int *x, int *y, int *comp, int req_comp) +{ + FILE *f = stbi__fopen(filename, "rb"); + stbi__uint16 *result; + if (!f) return (stbi_us *) stbi__errpuc("can't fopen", "Unable to open file"); + result = stbi_load_from_file_16(f,x,y,comp,req_comp); + fclose(f); + return result; +} + + +#endif //!STBI_NO_STDIO + +STBIDEF stbi_us *stbi_load_16_from_memory(stbi_uc const *buffer, int len, int *x, int *y, int *channels_in_file, int desired_channels) +{ + stbi__context s; + stbi__start_mem(&s,buffer,len); + return stbi__load_and_postprocess_16bit(&s,x,y,channels_in_file,desired_channels); +} + +STBIDEF stbi_us *stbi_load_16_from_callbacks(stbi_io_callbacks const *clbk, void *user, int *x, int *y, int *channels_in_file, int desired_channels) +{ + stbi__context s; + stbi__start_callbacks(&s, (stbi_io_callbacks *)clbk, user); + return stbi__load_and_postprocess_16bit(&s,x,y,channels_in_file,desired_channels); +} + +STBIDEF stbi_uc *stbi_load_from_memory(stbi_uc const *buffer, int len, int *x, int *y, int *comp, int req_comp) +{ + stbi__context s; + stbi__start_mem(&s,buffer,len); + return stbi__load_and_postprocess_8bit(&s,x,y,comp,req_comp); +} + +STBIDEF stbi_uc *stbi_load_from_callbacks(stbi_io_callbacks const *clbk, void *user, int *x, int *y, int *comp, int req_comp) +{ + stbi__context s; + stbi__start_callbacks(&s, (stbi_io_callbacks *) clbk, user); + return stbi__load_and_postprocess_8bit(&s,x,y,comp,req_comp); +} + +#ifndef STBI_NO_GIF +STBIDEF stbi_uc *stbi_load_gif_from_memory(stbi_uc const *buffer, int len, int **delays, int *x, int *y, int *z, int *comp, int req_comp) +{ + unsigned char *result; + stbi__context s; + stbi__start_mem(&s,buffer,len); + + result = (unsigned char*) stbi__load_gif_main(&s, delays, x, y, z, comp, req_comp); + if (stbi__vertically_flip_on_load) { + stbi__vertical_flip_slices( result, *x, *y, *z, *comp ); + } + + return result; +} +#endif + +#ifndef STBI_NO_LINEAR +static float *stbi__loadf_main(stbi__context *s, int *x, int *y, int *comp, int req_comp) +{ + unsigned char *data; + #ifndef STBI_NO_HDR + if (stbi__hdr_test(s)) { + stbi__result_info ri; + float *hdr_data = stbi__hdr_load(s,x,y,comp,req_comp, &ri); + if (hdr_data) + stbi__float_postprocess(hdr_data,x,y,comp,req_comp); + return hdr_data; + } + #endif + data = stbi__load_and_postprocess_8bit(s, x, y, comp, req_comp); + if (data) + return stbi__ldr_to_hdr(data, *x, *y, req_comp ? req_comp : *comp); + return stbi__errpf("unknown image type", "Image not of any known type, or corrupt"); +} + +STBIDEF float *stbi_loadf_from_memory(stbi_uc const *buffer, int len, int *x, int *y, int *comp, int req_comp) +{ + stbi__context s; + stbi__start_mem(&s,buffer,len); + return stbi__loadf_main(&s,x,y,comp,req_comp); +} + +STBIDEF float *stbi_loadf_from_callbacks(stbi_io_callbacks const *clbk, void *user, int *x, int *y, int *comp, int req_comp) +{ + stbi__context s; + stbi__start_callbacks(&s, (stbi_io_callbacks *) clbk, user); + return stbi__loadf_main(&s,x,y,comp,req_comp); +} + +#ifndef STBI_NO_STDIO +STBIDEF float *stbi_loadf(char const *filename, int *x, int *y, int *comp, int req_comp) +{ + float *result; + FILE *f = stbi__fopen(filename, "rb"); + if (!f) return stbi__errpf("can't fopen", "Unable to open file"); + result = stbi_loadf_from_file(f,x,y,comp,req_comp); + fclose(f); + return result; +} + +STBIDEF float *stbi_loadf_from_file(FILE *f, int *x, int *y, int *comp, int req_comp) +{ + stbi__context s; + stbi__start_file(&s,f); + return stbi__loadf_main(&s,x,y,comp,req_comp); +} +#endif // !STBI_NO_STDIO + +#endif // !STBI_NO_LINEAR + +// these is-hdr-or-not is defined independent of whether STBI_NO_LINEAR is +// defined, for API simplicity; if STBI_NO_LINEAR is defined, it always +// reports false! + +STBIDEF int stbi_is_hdr_from_memory(stbi_uc const *buffer, int len) +{ + #ifndef STBI_NO_HDR + stbi__context s; + stbi__start_mem(&s,buffer,len); + return stbi__hdr_test(&s); + #else + STBI_NOTUSED(buffer); + STBI_NOTUSED(len); + return 0; + #endif +} + +#ifndef STBI_NO_STDIO +STBIDEF int stbi_is_hdr (char const *filename) +{ + FILE *f = stbi__fopen(filename, "rb"); + int result=0; + if (f) { + result = stbi_is_hdr_from_file(f); + fclose(f); + } + return result; +} + +STBIDEF int stbi_is_hdr_from_file(FILE *f) +{ + #ifndef STBI_NO_HDR + long pos = ftell(f); + int res; + stbi__context s; + stbi__start_file(&s,f); + res = stbi__hdr_test(&s); + fseek(f, pos, SEEK_SET); + return res; + #else + STBI_NOTUSED(f); + return 0; + #endif +} +#endif // !STBI_NO_STDIO + +STBIDEF int stbi_is_hdr_from_callbacks(stbi_io_callbacks const *clbk, void *user) +{ + #ifndef STBI_NO_HDR + stbi__context s; + stbi__start_callbacks(&s, (stbi_io_callbacks *) clbk, user); + return stbi__hdr_test(&s); + #else + STBI_NOTUSED(clbk); + STBI_NOTUSED(user); + return 0; + #endif +} + +#ifndef STBI_NO_LINEAR +static float stbi__l2h_gamma=2.2f, stbi__l2h_scale=1.0f; + +STBIDEF void stbi_ldr_to_hdr_gamma(float gamma) { stbi__l2h_gamma = gamma; } +STBIDEF void stbi_ldr_to_hdr_scale(float scale) { stbi__l2h_scale = scale; } +#endif + +static float stbi__h2l_gamma_i=1.0f/2.2f, stbi__h2l_scale_i=1.0f; + +STBIDEF void stbi_hdr_to_ldr_gamma(float gamma) { stbi__h2l_gamma_i = 1/gamma; } +STBIDEF void stbi_hdr_to_ldr_scale(float scale) { stbi__h2l_scale_i = 1/scale; } + + +////////////////////////////////////////////////////////////////////////////// +// +// Common code used by all image loaders +// + +enum +{ + STBI__SCAN_load=0, + STBI__SCAN_type, + STBI__SCAN_header +}; + +static void stbi__refill_buffer(stbi__context *s) +{ + int n = (s->io.read)(s->io_user_data,(char*)s->buffer_start,s->buflen); + s->callback_already_read += (int) (s->img_buffer - s->img_buffer_original); + if (n == 0) { + // at end of file, treat same as if from memory, but need to handle case + // where s->img_buffer isn't pointing to safe memory, e.g. 0-byte file + s->read_from_callbacks = 0; + s->img_buffer = s->buffer_start; + s->img_buffer_end = s->buffer_start+1; + *s->img_buffer = 0; + } else { + s->img_buffer = s->buffer_start; + s->img_buffer_end = s->buffer_start + n; + } +} + +stbi_inline static stbi_uc stbi__get8(stbi__context *s) +{ + if (s->img_buffer < s->img_buffer_end) + return *s->img_buffer++; + if (s->read_from_callbacks) { + stbi__refill_buffer(s); + return *s->img_buffer++; + } + return 0; +} + +#if defined(STBI_NO_JPEG) && defined(STBI_NO_HDR) && defined(STBI_NO_PIC) && defined(STBI_NO_PNM) +// nothing +#else +stbi_inline static int stbi__at_eof(stbi__context *s) +{ + if (s->io.read) { + if (!(s->io.eof)(s->io_user_data)) return 0; + // if feof() is true, check if buffer = end + // special case: we've only got the special 0 character at the end + if (s->read_from_callbacks == 0) return 1; + } + + return s->img_buffer >= s->img_buffer_end; +} +#endif + +#if defined(STBI_NO_JPEG) && defined(STBI_NO_PNG) && defined(STBI_NO_BMP) && defined(STBI_NO_PSD) && defined(STBI_NO_TGA) && defined(STBI_NO_GIF) && defined(STBI_NO_PIC) +// nothing +#else +static void stbi__skip(stbi__context *s, int n) +{ + if (n == 0) return; // already there! + if (n < 0) { + s->img_buffer = s->img_buffer_end; + return; + } + if (s->io.read) { + int blen = (int) (s->img_buffer_end - s->img_buffer); + if (blen < n) { + s->img_buffer = s->img_buffer_end; + (s->io.skip)(s->io_user_data, n - blen); + return; + } + } + s->img_buffer += n; +} +#endif + +#if defined(STBI_NO_PNG) && defined(STBI_NO_TGA) && defined(STBI_NO_HDR) && defined(STBI_NO_PNM) +// nothing +#else +static int stbi__getn(stbi__context *s, stbi_uc *buffer, int n) +{ + if (s->io.read) { + int blen = (int) (s->img_buffer_end - s->img_buffer); + if (blen < n) { + int res, count; + + memcpy(buffer, s->img_buffer, blen); + + count = (s->io.read)(s->io_user_data, (char*) buffer + blen, n - blen); + res = (count == (n-blen)); + s->img_buffer = s->img_buffer_end; + return res; + } + } + + if (s->img_buffer+n <= s->img_buffer_end) { + memcpy(buffer, s->img_buffer, n); + s->img_buffer += n; + return 1; + } else + return 0; +} +#endif + +#if defined(STBI_NO_JPEG) && defined(STBI_NO_PNG) && defined(STBI_NO_PSD) && defined(STBI_NO_PIC) +// nothing +#else +static int stbi__get16be(stbi__context *s) +{ + int z = stbi__get8(s); + return (z << 8) + stbi__get8(s); +} +#endif + +#if defined(STBI_NO_PNG) && defined(STBI_NO_PSD) && defined(STBI_NO_PIC) +// nothing +#else +static stbi__uint32 stbi__get32be(stbi__context *s) +{ + stbi__uint32 z = stbi__get16be(s); + return (z << 16) + stbi__get16be(s); +} +#endif + +#if defined(STBI_NO_BMP) && defined(STBI_NO_TGA) && defined(STBI_NO_GIF) +// nothing +#else +static int stbi__get16le(stbi__context *s) +{ + int z = stbi__get8(s); + return z + (stbi__get8(s) << 8); +} +#endif + +#ifndef STBI_NO_BMP +static stbi__uint32 stbi__get32le(stbi__context *s) +{ + stbi__uint32 z = stbi__get16le(s); + z += (stbi__uint32)stbi__get16le(s) << 16; + return z; +} +#endif + +#define STBI__BYTECAST(x) ((stbi_uc) ((x) & 255)) // truncate int to byte without warnings + +#if defined(STBI_NO_JPEG) && defined(STBI_NO_PNG) && defined(STBI_NO_BMP) && defined(STBI_NO_PSD) && defined(STBI_NO_TGA) && defined(STBI_NO_GIF) && defined(STBI_NO_PIC) && defined(STBI_NO_PNM) +// nothing +#else +////////////////////////////////////////////////////////////////////////////// +// +// generic converter from built-in img_n to req_comp +// individual types do this automatically as much as possible (e.g. jpeg +// does all cases internally since it needs to colorspace convert anyway, +// and it never has alpha, so very few cases ). png can automatically +// interleave an alpha=255 channel, but falls back to this for other cases +// +// assume data buffer is malloced, so malloc a new one and free that one +// only failure mode is malloc failing + +static stbi_uc stbi__compute_y(int r, int g, int b) +{ + return (stbi_uc) (((r*77) + (g*150) + (29*b)) >> 8); +} +#endif + +#if defined(STBI_NO_PNG) && defined(STBI_NO_BMP) && defined(STBI_NO_PSD) && defined(STBI_NO_TGA) && defined(STBI_NO_GIF) && defined(STBI_NO_PIC) && defined(STBI_NO_PNM) +// nothing +#else +static unsigned char *stbi__convert_format(unsigned char *data, int img_n, int req_comp, unsigned int x, unsigned int y) +{ + int i,j; + unsigned char *good; + + if (req_comp == img_n) return data; + STBI_ASSERT(req_comp >= 1 && req_comp <= 4); + + good = (unsigned char *) stbi__malloc_mad3(req_comp, x, y, 0); + if (good == NULL) { + STBI_FREE(data); + return stbi__errpuc("outofmem", "Out of memory"); + } + + for (j=0; j < (int) y; ++j) { + unsigned char *src = data + j * x * img_n ; + unsigned char *dest = good + j * x * req_comp; + + #define STBI__COMBO(a,b) ((a)*8+(b)) + #define STBI__CASE(a,b) case STBI__COMBO(a,b): for(i=x-1; i >= 0; --i, src += a, dest += b) + // convert source image with img_n components to one with req_comp components; + // avoid switch per pixel, so use switch per scanline and massive macros + switch (STBI__COMBO(img_n, req_comp)) { + STBI__CASE(1,2) { dest[0]=src[0]; dest[1]=255; } break; + STBI__CASE(1,3) { dest[0]=dest[1]=dest[2]=src[0]; } break; + STBI__CASE(1,4) { dest[0]=dest[1]=dest[2]=src[0]; dest[3]=255; } break; + STBI__CASE(2,1) { dest[0]=src[0]; } break; + STBI__CASE(2,3) { dest[0]=dest[1]=dest[2]=src[0]; } break; + STBI__CASE(2,4) { dest[0]=dest[1]=dest[2]=src[0]; dest[3]=src[1]; } break; + STBI__CASE(3,4) { dest[0]=src[0];dest[1]=src[1];dest[2]=src[2];dest[3]=255; } break; + STBI__CASE(3,1) { dest[0]=stbi__compute_y(src[0],src[1],src[2]); } break; + STBI__CASE(3,2) { dest[0]=stbi__compute_y(src[0],src[1],src[2]); dest[1] = 255; } break; + STBI__CASE(4,1) { dest[0]=stbi__compute_y(src[0],src[1],src[2]); } break; + STBI__CASE(4,2) { dest[0]=stbi__compute_y(src[0],src[1],src[2]); dest[1] = src[3]; } break; + STBI__CASE(4,3) { dest[0]=src[0];dest[1]=src[1];dest[2]=src[2]; } break; + default: STBI_ASSERT(0); STBI_FREE(data); STBI_FREE(good); return stbi__errpuc("unsupported", "Unsupported format conversion"); + } + #undef STBI__CASE + } + + STBI_FREE(data); + return good; +} +#endif + +#if defined(STBI_NO_PNG) && defined(STBI_NO_PSD) +// nothing +#else +static stbi__uint16 stbi__compute_y_16(int r, int g, int b) +{ + return (stbi__uint16) (((r*77) + (g*150) + (29*b)) >> 8); +} +#endif + +#if defined(STBI_NO_PNG) && defined(STBI_NO_PSD) +// nothing +#else +static stbi__uint16 *stbi__convert_format16(stbi__uint16 *data, int img_n, int req_comp, unsigned int x, unsigned int y) +{ + int i,j; + stbi__uint16 *good; + + if (req_comp == img_n) return data; + STBI_ASSERT(req_comp >= 1 && req_comp <= 4); + + good = (stbi__uint16 *) stbi__malloc(req_comp * x * y * 2); + if (good == NULL) { + STBI_FREE(data); + return (stbi__uint16 *) stbi__errpuc("outofmem", "Out of memory"); + } + + for (j=0; j < (int) y; ++j) { + stbi__uint16 *src = data + j * x * img_n ; + stbi__uint16 *dest = good + j * x * req_comp; + + #define STBI__COMBO(a,b) ((a)*8+(b)) + #define STBI__CASE(a,b) case STBI__COMBO(a,b): for(i=x-1; i >= 0; --i, src += a, dest += b) + // convert source image with img_n components to one with req_comp components; + // avoid switch per pixel, so use switch per scanline and massive macros + switch (STBI__COMBO(img_n, req_comp)) { + STBI__CASE(1,2) { dest[0]=src[0]; dest[1]=0xffff; } break; + STBI__CASE(1,3) { dest[0]=dest[1]=dest[2]=src[0]; } break; + STBI__CASE(1,4) { dest[0]=dest[1]=dest[2]=src[0]; dest[3]=0xffff; } break; + STBI__CASE(2,1) { dest[0]=src[0]; } break; + STBI__CASE(2,3) { dest[0]=dest[1]=dest[2]=src[0]; } break; + STBI__CASE(2,4) { dest[0]=dest[1]=dest[2]=src[0]; dest[3]=src[1]; } break; + STBI__CASE(3,4) { dest[0]=src[0];dest[1]=src[1];dest[2]=src[2];dest[3]=0xffff; } break; + STBI__CASE(3,1) { dest[0]=stbi__compute_y_16(src[0],src[1],src[2]); } break; + STBI__CASE(3,2) { dest[0]=stbi__compute_y_16(src[0],src[1],src[2]); dest[1] = 0xffff; } break; + STBI__CASE(4,1) { dest[0]=stbi__compute_y_16(src[0],src[1],src[2]); } break; + STBI__CASE(4,2) { dest[0]=stbi__compute_y_16(src[0],src[1],src[2]); dest[1] = src[3]; } break; + STBI__CASE(4,3) { dest[0]=src[0];dest[1]=src[1];dest[2]=src[2]; } break; + default: STBI_ASSERT(0); STBI_FREE(data); STBI_FREE(good); return (stbi__uint16*) stbi__errpuc("unsupported", "Unsupported format conversion"); + } + #undef STBI__CASE + } + + STBI_FREE(data); + return good; +} +#endif + +#ifndef STBI_NO_LINEAR +static float *stbi__ldr_to_hdr(stbi_uc *data, int x, int y, int comp) +{ + int i,k,n; + float *output; + if (!data) return NULL; + output = (float *) stbi__malloc_mad4(x, y, comp, sizeof(float), 0); + if (output == NULL) { STBI_FREE(data); return stbi__errpf("outofmem", "Out of memory"); } + // compute number of non-alpha components + if (comp & 1) n = comp; else n = comp-1; + for (i=0; i < x*y; ++i) { + for (k=0; k < n; ++k) { + output[i*comp + k] = (float) (pow(data[i*comp+k]/255.0f, stbi__l2h_gamma) * stbi__l2h_scale); + } + } + if (n < comp) { + for (i=0; i < x*y; ++i) { + output[i*comp + n] = data[i*comp + n]/255.0f; + } + } + STBI_FREE(data); + return output; +} +#endif + +#ifndef STBI_NO_HDR +#define stbi__float2int(x) ((int) (x)) +static stbi_uc *stbi__hdr_to_ldr(float *data, int x, int y, int comp) +{ + int i,k,n; + stbi_uc *output; + if (!data) return NULL; + output = (stbi_uc *) stbi__malloc_mad3(x, y, comp, 0); + if (output == NULL) { STBI_FREE(data); return stbi__errpuc("outofmem", "Out of memory"); } + // compute number of non-alpha components + if (comp & 1) n = comp; else n = comp-1; + for (i=0; i < x*y; ++i) { + for (k=0; k < n; ++k) { + float z = (float) pow(data[i*comp+k]*stbi__h2l_scale_i, stbi__h2l_gamma_i) * 255 + 0.5f; + if (z < 0) z = 0; + if (z > 255) z = 255; + output[i*comp + k] = (stbi_uc) stbi__float2int(z); + } + if (k < comp) { + float z = data[i*comp+k] * 255 + 0.5f; + if (z < 0) z = 0; + if (z > 255) z = 255; + output[i*comp + k] = (stbi_uc) stbi__float2int(z); + } + } + STBI_FREE(data); + return output; +} +#endif + +////////////////////////////////////////////////////////////////////////////// +// +// "baseline" JPEG/JFIF decoder +// +// simple implementation +// - doesn't support delayed output of y-dimension +// - simple interface (only one output format: 8-bit interleaved RGB) +// - doesn't try to recover corrupt jpegs +// - doesn't allow partial loading, loading multiple at once +// - still fast on x86 (copying globals into locals doesn't help x86) +// - allocates lots of intermediate memory (full size of all components) +// - non-interleaved case requires this anyway +// - allows good upsampling (see next) +// high-quality +// - upsampled channels are bilinearly interpolated, even across blocks +// - quality integer IDCT derived from IJG's 'slow' +// performance +// - fast huffman; reasonable integer IDCT +// - some SIMD kernels for common paths on targets with SSE2/NEON +// - uses a lot of intermediate memory, could cache poorly + +#ifndef STBI_NO_JPEG + +// huffman decoding acceleration +#define FAST_BITS 9 // larger handles more cases; smaller stomps less cache + +typedef struct +{ + stbi_uc fast[1 << FAST_BITS]; + // weirdly, repacking this into AoS is a 10% speed loss, instead of a win + stbi__uint16 code[256]; + stbi_uc values[256]; + stbi_uc size[257]; + unsigned int maxcode[18]; + int delta[17]; // old 'firstsymbol' - old 'firstcode' +} stbi__huffman; + +typedef struct +{ + stbi__context *s; + stbi__huffman huff_dc[4]; + stbi__huffman huff_ac[4]; + stbi__uint16 dequant[4][64]; + stbi__int16 fast_ac[4][1 << FAST_BITS]; + +// sizes for components, interleaved MCUs + int img_h_max, img_v_max; + int img_mcu_x, img_mcu_y; + int img_mcu_w, img_mcu_h; + +// definition of jpeg image component + struct + { + int id; + int h,v; + int tq; + int hd,ha; + int dc_pred; + + int x,y,w2,h2; + stbi_uc *data; + void *raw_data, *raw_coeff; + stbi_uc *linebuf; + short *coeff; // progressive only + int coeff_w, coeff_h; // number of 8x8 coefficient blocks + } img_comp[4]; + + stbi__uint32 code_buffer; // jpeg entropy-coded buffer + int code_bits; // number of valid bits + unsigned char marker; // marker seen while filling entropy buffer + int nomore; // flag if we saw a marker so must stop + + int progressive; + int spec_start; + int spec_end; + int succ_high; + int succ_low; + int eob_run; + int jfif; + int app14_color_transform; // Adobe APP14 tag + int rgb; + + int scan_n, order[4]; + int restart_interval, todo; + +// kernels + void (*idct_block_kernel)(stbi_uc *out, int out_stride, short data[64]); + void (*YCbCr_to_RGB_kernel)(stbi_uc *out, const stbi_uc *y, const stbi_uc *pcb, const stbi_uc *pcr, int count, int step); + stbi_uc *(*resample_row_hv_2_kernel)(stbi_uc *out, stbi_uc *in_near, stbi_uc *in_far, int w, int hs); +} stbi__jpeg; + +static int stbi__build_huffman(stbi__huffman *h, int *count) +{ + int i,j,k=0; + unsigned int code; + // build size list for each symbol (from JPEG spec) + for (i=0; i < 16; ++i) { + for (j=0; j < count[i]; ++j) { + h->size[k++] = (stbi_uc) (i+1); + if(k >= 257) return stbi__err("bad size list","Corrupt JPEG"); + } + } + h->size[k] = 0; + + // compute actual symbols (from jpeg spec) + code = 0; + k = 0; + for(j=1; j <= 16; ++j) { + // compute delta to add to code to compute symbol id + h->delta[j] = k - code; + if (h->size[k] == j) { + while (h->size[k] == j) + h->code[k++] = (stbi__uint16) (code++); + if (code-1 >= (1u << j)) return stbi__err("bad code lengths","Corrupt JPEG"); + } + // compute largest code + 1 for this size, preshifted as needed later + h->maxcode[j] = code << (16-j); + code <<= 1; + } + h->maxcode[j] = 0xffffffff; + + // build non-spec acceleration table; 255 is flag for not-accelerated + memset(h->fast, 255, 1 << FAST_BITS); + for (i=0; i < k; ++i) { + int s = h->size[i]; + if (s <= FAST_BITS) { + int c = h->code[i] << (FAST_BITS-s); + int m = 1 << (FAST_BITS-s); + for (j=0; j < m; ++j) { + h->fast[c+j] = (stbi_uc) i; + } + } + } + return 1; +} + +// build a table that decodes both magnitude and value of small ACs in +// one go. +static void stbi__build_fast_ac(stbi__int16 *fast_ac, stbi__huffman *h) +{ + int i; + for (i=0; i < (1 << FAST_BITS); ++i) { + stbi_uc fast = h->fast[i]; + fast_ac[i] = 0; + if (fast < 255) { + int rs = h->values[fast]; + int run = (rs >> 4) & 15; + int magbits = rs & 15; + int len = h->size[fast]; + + if (magbits && len + magbits <= FAST_BITS) { + // magnitude code followed by receive_extend code + int k = ((i << len) & ((1 << FAST_BITS) - 1)) >> (FAST_BITS - magbits); + int m = 1 << (magbits - 1); + if (k < m) k += (~0U << magbits) + 1; + // if the result is small enough, we can fit it in fast_ac table + if (k >= -128 && k <= 127) + fast_ac[i] = (stbi__int16) ((k * 256) + (run * 16) + (len + magbits)); + } + } + } +} + +static void stbi__grow_buffer_unsafe(stbi__jpeg *j) +{ + do { + unsigned int b = j->nomore ? 0 : stbi__get8(j->s); + if (b == 0xff) { + int c = stbi__get8(j->s); + while (c == 0xff) c = stbi__get8(j->s); // consume fill bytes + if (c != 0) { + j->marker = (unsigned char) c; + j->nomore = 1; + return; + } + } + j->code_buffer |= b << (24 - j->code_bits); + j->code_bits += 8; + } while (j->code_bits <= 24); +} + +// (1 << n) - 1 +static const stbi__uint32 stbi__bmask[17]={0,1,3,7,15,31,63,127,255,511,1023,2047,4095,8191,16383,32767,65535}; + +// decode a jpeg huffman value from the bitstream +stbi_inline static int stbi__jpeg_huff_decode(stbi__jpeg *j, stbi__huffman *h) +{ + unsigned int temp; + int c,k; + + if (j->code_bits < 16) stbi__grow_buffer_unsafe(j); + + // look at the top FAST_BITS and determine what symbol ID it is, + // if the code is <= FAST_BITS + c = (j->code_buffer >> (32 - FAST_BITS)) & ((1 << FAST_BITS)-1); + k = h->fast[c]; + if (k < 255) { + int s = h->size[k]; + if (s > j->code_bits) + return -1; + j->code_buffer <<= s; + j->code_bits -= s; + return h->values[k]; + } + + // naive test is to shift the code_buffer down so k bits are + // valid, then test against maxcode. To speed this up, we've + // preshifted maxcode left so that it has (16-k) 0s at the + // end; in other words, regardless of the number of bits, it + // wants to be compared against something shifted to have 16; + // that way we don't need to shift inside the loop. + temp = j->code_buffer >> 16; + for (k=FAST_BITS+1 ; ; ++k) + if (temp < h->maxcode[k]) + break; + if (k == 17) { + // error! code not found + j->code_bits -= 16; + return -1; + } + + if (k > j->code_bits) + return -1; + + // convert the huffman code to the symbol id + c = ((j->code_buffer >> (32 - k)) & stbi__bmask[k]) + h->delta[k]; + if(c < 0 || c >= 256) // symbol id out of bounds! + return -1; + STBI_ASSERT((((j->code_buffer) >> (32 - h->size[c])) & stbi__bmask[h->size[c]]) == h->code[c]); + + // convert the id to a symbol + j->code_bits -= k; + j->code_buffer <<= k; + return h->values[c]; +} + +// bias[n] = (-1<code_bits < n) stbi__grow_buffer_unsafe(j); + if (j->code_bits < n) return 0; // ran out of bits from stream, return 0s intead of continuing + + sgn = j->code_buffer >> 31; // sign bit always in MSB; 0 if MSB clear (positive), 1 if MSB set (negative) + k = stbi_lrot(j->code_buffer, n); + j->code_buffer = k & ~stbi__bmask[n]; + k &= stbi__bmask[n]; + j->code_bits -= n; + return k + (stbi__jbias[n] & (sgn - 1)); +} + +// get some unsigned bits +stbi_inline static int stbi__jpeg_get_bits(stbi__jpeg *j, int n) +{ + unsigned int k; + if (j->code_bits < n) stbi__grow_buffer_unsafe(j); + if (j->code_bits < n) return 0; // ran out of bits from stream, return 0s intead of continuing + k = stbi_lrot(j->code_buffer, n); + j->code_buffer = k & ~stbi__bmask[n]; + k &= stbi__bmask[n]; + j->code_bits -= n; + return k; +} + +stbi_inline static int stbi__jpeg_get_bit(stbi__jpeg *j) +{ + unsigned int k; + if (j->code_bits < 1) stbi__grow_buffer_unsafe(j); + if (j->code_bits < 1) return 0; // ran out of bits from stream, return 0s intead of continuing + k = j->code_buffer; + j->code_buffer <<= 1; + --j->code_bits; + return k & 0x80000000; +} + +// given a value that's at position X in the zigzag stream, +// where does it appear in the 8x8 matrix coded as row-major? +static const stbi_uc stbi__jpeg_dezigzag[64+15] = +{ + 0, 1, 8, 16, 9, 2, 3, 10, + 17, 24, 32, 25, 18, 11, 4, 5, + 12, 19, 26, 33, 40, 48, 41, 34, + 27, 20, 13, 6, 7, 14, 21, 28, + 35, 42, 49, 56, 57, 50, 43, 36, + 29, 22, 15, 23, 30, 37, 44, 51, + 58, 59, 52, 45, 38, 31, 39, 46, + 53, 60, 61, 54, 47, 55, 62, 63, + // let corrupt input sample past end + 63, 63, 63, 63, 63, 63, 63, 63, + 63, 63, 63, 63, 63, 63, 63 +}; + +// decode one 64-entry block-- +static int stbi__jpeg_decode_block(stbi__jpeg *j, short data[64], stbi__huffman *hdc, stbi__huffman *hac, stbi__int16 *fac, int b, stbi__uint16 *dequant) +{ + int diff,dc,k; + int t; + + if (j->code_bits < 16) stbi__grow_buffer_unsafe(j); + t = stbi__jpeg_huff_decode(j, hdc); + if (t < 0 || t > 15) return stbi__err("bad huffman code","Corrupt JPEG"); + + // 0 all the ac values now so we can do it 32-bits at a time + memset(data,0,64*sizeof(data[0])); + + diff = t ? stbi__extend_receive(j, t) : 0; + if (!stbi__addints_valid(j->img_comp[b].dc_pred, diff)) return stbi__err("bad delta","Corrupt JPEG"); + dc = j->img_comp[b].dc_pred + diff; + j->img_comp[b].dc_pred = dc; + if (!stbi__mul2shorts_valid(dc, dequant[0])) return stbi__err("can't merge dc and ac", "Corrupt JPEG"); + data[0] = (short) (dc * dequant[0]); + + // decode AC components, see JPEG spec + k = 1; + do { + unsigned int zig; + int c,r,s; + if (j->code_bits < 16) stbi__grow_buffer_unsafe(j); + c = (j->code_buffer >> (32 - FAST_BITS)) & ((1 << FAST_BITS)-1); + r = fac[c]; + if (r) { // fast-AC path + k += (r >> 4) & 15; // run + s = r & 15; // combined length + if (s > j->code_bits) return stbi__err("bad huffman code", "Combined length longer than code bits available"); + j->code_buffer <<= s; + j->code_bits -= s; + // decode into unzigzag'd location + zig = stbi__jpeg_dezigzag[k++]; + data[zig] = (short) ((r >> 8) * dequant[zig]); + } else { + int rs = stbi__jpeg_huff_decode(j, hac); + if (rs < 0) return stbi__err("bad huffman code","Corrupt JPEG"); + s = rs & 15; + r = rs >> 4; + if (s == 0) { + if (rs != 0xf0) break; // end block + k += 16; + } else { + k += r; + // decode into unzigzag'd location + zig = stbi__jpeg_dezigzag[k++]; + data[zig] = (short) (stbi__extend_receive(j,s) * dequant[zig]); + } + } + } while (k < 64); + return 1; +} + +static int stbi__jpeg_decode_block_prog_dc(stbi__jpeg *j, short data[64], stbi__huffman *hdc, int b) +{ + int diff,dc; + int t; + if (j->spec_end != 0) return stbi__err("can't merge dc and ac", "Corrupt JPEG"); + + if (j->code_bits < 16) stbi__grow_buffer_unsafe(j); + + if (j->succ_high == 0) { + // first scan for DC coefficient, must be first + memset(data,0,64*sizeof(data[0])); // 0 all the ac values now + t = stbi__jpeg_huff_decode(j, hdc); + if (t < 0 || t > 15) return stbi__err("can't merge dc and ac", "Corrupt JPEG"); + diff = t ? stbi__extend_receive(j, t) : 0; + + if (!stbi__addints_valid(j->img_comp[b].dc_pred, diff)) return stbi__err("bad delta", "Corrupt JPEG"); + dc = j->img_comp[b].dc_pred + diff; + j->img_comp[b].dc_pred = dc; + if (!stbi__mul2shorts_valid(dc, 1 << j->succ_low)) return stbi__err("can't merge dc and ac", "Corrupt JPEG"); + data[0] = (short) (dc * (1 << j->succ_low)); + } else { + // refinement scan for DC coefficient + if (stbi__jpeg_get_bit(j)) + data[0] += (short) (1 << j->succ_low); + } + return 1; +} + +// @OPTIMIZE: store non-zigzagged during the decode passes, +// and only de-zigzag when dequantizing +static int stbi__jpeg_decode_block_prog_ac(stbi__jpeg *j, short data[64], stbi__huffman *hac, stbi__int16 *fac) +{ + int k; + if (j->spec_start == 0) return stbi__err("can't merge dc and ac", "Corrupt JPEG"); + + if (j->succ_high == 0) { + int shift = j->succ_low; + + if (j->eob_run) { + --j->eob_run; + return 1; + } + + k = j->spec_start; + do { + unsigned int zig; + int c,r,s; + if (j->code_bits < 16) stbi__grow_buffer_unsafe(j); + c = (j->code_buffer >> (32 - FAST_BITS)) & ((1 << FAST_BITS)-1); + r = fac[c]; + if (r) { // fast-AC path + k += (r >> 4) & 15; // run + s = r & 15; // combined length + if (s > j->code_bits) return stbi__err("bad huffman code", "Combined length longer than code bits available"); + j->code_buffer <<= s; + j->code_bits -= s; + zig = stbi__jpeg_dezigzag[k++]; + data[zig] = (short) ((r >> 8) * (1 << shift)); + } else { + int rs = stbi__jpeg_huff_decode(j, hac); + if (rs < 0) return stbi__err("bad huffman code","Corrupt JPEG"); + s = rs & 15; + r = rs >> 4; + if (s == 0) { + if (r < 15) { + j->eob_run = (1 << r); + if (r) + j->eob_run += stbi__jpeg_get_bits(j, r); + --j->eob_run; + break; + } + k += 16; + } else { + k += r; + zig = stbi__jpeg_dezigzag[k++]; + data[zig] = (short) (stbi__extend_receive(j,s) * (1 << shift)); + } + } + } while (k <= j->spec_end); + } else { + // refinement scan for these AC coefficients + + short bit = (short) (1 << j->succ_low); + + if (j->eob_run) { + --j->eob_run; + for (k = j->spec_start; k <= j->spec_end; ++k) { + short *p = &data[stbi__jpeg_dezigzag[k]]; + if (*p != 0) + if (stbi__jpeg_get_bit(j)) + if ((*p & bit)==0) { + if (*p > 0) + *p += bit; + else + *p -= bit; + } + } + } else { + k = j->spec_start; + do { + int r,s; + int rs = stbi__jpeg_huff_decode(j, hac); // @OPTIMIZE see if we can use the fast path here, advance-by-r is so slow, eh + if (rs < 0) return stbi__err("bad huffman code","Corrupt JPEG"); + s = rs & 15; + r = rs >> 4; + if (s == 0) { + if (r < 15) { + j->eob_run = (1 << r) - 1; + if (r) + j->eob_run += stbi__jpeg_get_bits(j, r); + r = 64; // force end of block + } else { + // r=15 s=0 should write 16 0s, so we just do + // a run of 15 0s and then write s (which is 0), + // so we don't have to do anything special here + } + } else { + if (s != 1) return stbi__err("bad huffman code", "Corrupt JPEG"); + // sign bit + if (stbi__jpeg_get_bit(j)) + s = bit; + else + s = -bit; + } + + // advance by r + while (k <= j->spec_end) { + short *p = &data[stbi__jpeg_dezigzag[k++]]; + if (*p != 0) { + if (stbi__jpeg_get_bit(j)) + if ((*p & bit)==0) { + if (*p > 0) + *p += bit; + else + *p -= bit; + } + } else { + if (r == 0) { + *p = (short) s; + break; + } + --r; + } + } + } while (k <= j->spec_end); + } + } + return 1; +} + +// take a -128..127 value and stbi__clamp it and convert to 0..255 +stbi_inline static stbi_uc stbi__clamp(int x) +{ + // trick to use a single test to catch both cases + if ((unsigned int) x > 255) { + if (x < 0) return 0; + if (x > 255) return 255; + } + return (stbi_uc) x; +} + +#define stbi__f2f(x) ((int) (((x) * 4096 + 0.5))) +#define stbi__fsh(x) ((x) * 4096) + +// derived from jidctint -- DCT_ISLOW +#define STBI__IDCT_1D(s0,s1,s2,s3,s4,s5,s6,s7) \ + int t0,t1,t2,t3,p1,p2,p3,p4,p5,x0,x1,x2,x3; \ + p2 = s2; \ + p3 = s6; \ + p1 = (p2+p3) * stbi__f2f(0.5411961f); \ + t2 = p1 + p3*stbi__f2f(-1.847759065f); \ + t3 = p1 + p2*stbi__f2f( 0.765366865f); \ + p2 = s0; \ + p3 = s4; \ + t0 = stbi__fsh(p2+p3); \ + t1 = stbi__fsh(p2-p3); \ + x0 = t0+t3; \ + x3 = t0-t3; \ + x1 = t1+t2; \ + x2 = t1-t2; \ + t0 = s7; \ + t1 = s5; \ + t2 = s3; \ + t3 = s1; \ + p3 = t0+t2; \ + p4 = t1+t3; \ + p1 = t0+t3; \ + p2 = t1+t2; \ + p5 = (p3+p4)*stbi__f2f( 1.175875602f); \ + t0 = t0*stbi__f2f( 0.298631336f); \ + t1 = t1*stbi__f2f( 2.053119869f); \ + t2 = t2*stbi__f2f( 3.072711026f); \ + t3 = t3*stbi__f2f( 1.501321110f); \ + p1 = p5 + p1*stbi__f2f(-0.899976223f); \ + p2 = p5 + p2*stbi__f2f(-2.562915447f); \ + p3 = p3*stbi__f2f(-1.961570560f); \ + p4 = p4*stbi__f2f(-0.390180644f); \ + t3 += p1+p4; \ + t2 += p2+p3; \ + t1 += p2+p4; \ + t0 += p1+p3; + +static void stbi__idct_block(stbi_uc *out, int out_stride, short data[64]) +{ + int i,val[64],*v=val; + stbi_uc *o; + short *d = data; + + // columns + for (i=0; i < 8; ++i,++d, ++v) { + // if all zeroes, shortcut -- this avoids dequantizing 0s and IDCTing + if (d[ 8]==0 && d[16]==0 && d[24]==0 && d[32]==0 + && d[40]==0 && d[48]==0 && d[56]==0) { + // no shortcut 0 seconds + // (1|2|3|4|5|6|7)==0 0 seconds + // all separate -0.047 seconds + // 1 && 2|3 && 4|5 && 6|7: -0.047 seconds + int dcterm = d[0]*4; + v[0] = v[8] = v[16] = v[24] = v[32] = v[40] = v[48] = v[56] = dcterm; + } else { + STBI__IDCT_1D(d[ 0],d[ 8],d[16],d[24],d[32],d[40],d[48],d[56]) + // constants scaled things up by 1<<12; let's bring them back + // down, but keep 2 extra bits of precision + x0 += 512; x1 += 512; x2 += 512; x3 += 512; + v[ 0] = (x0+t3) >> 10; + v[56] = (x0-t3) >> 10; + v[ 8] = (x1+t2) >> 10; + v[48] = (x1-t2) >> 10; + v[16] = (x2+t1) >> 10; + v[40] = (x2-t1) >> 10; + v[24] = (x3+t0) >> 10; + v[32] = (x3-t0) >> 10; + } + } + + for (i=0, v=val, o=out; i < 8; ++i,v+=8,o+=out_stride) { + // no fast case since the first 1D IDCT spread components out + STBI__IDCT_1D(v[0],v[1],v[2],v[3],v[4],v[5],v[6],v[7]) + // constants scaled things up by 1<<12, plus we had 1<<2 from first + // loop, plus horizontal and vertical each scale by sqrt(8) so together + // we've got an extra 1<<3, so 1<<17 total we need to remove. + // so we want to round that, which means adding 0.5 * 1<<17, + // aka 65536. Also, we'll end up with -128 to 127 that we want + // to encode as 0..255 by adding 128, so we'll add that before the shift + x0 += 65536 + (128<<17); + x1 += 65536 + (128<<17); + x2 += 65536 + (128<<17); + x3 += 65536 + (128<<17); + // tried computing the shifts into temps, or'ing the temps to see + // if any were out of range, but that was slower + o[0] = stbi__clamp((x0+t3) >> 17); + o[7] = stbi__clamp((x0-t3) >> 17); + o[1] = stbi__clamp((x1+t2) >> 17); + o[6] = stbi__clamp((x1-t2) >> 17); + o[2] = stbi__clamp((x2+t1) >> 17); + o[5] = stbi__clamp((x2-t1) >> 17); + o[3] = stbi__clamp((x3+t0) >> 17); + o[4] = stbi__clamp((x3-t0) >> 17); + } +} + +#ifdef STBI_SSE2 +// sse2 integer IDCT. not the fastest possible implementation but it +// produces bit-identical results to the generic C version so it's +// fully "transparent". +static void stbi__idct_simd(stbi_uc *out, int out_stride, short data[64]) +{ + // This is constructed to match our regular (generic) integer IDCT exactly. + __m128i row0, row1, row2, row3, row4, row5, row6, row7; + __m128i tmp; + + // dot product constant: even elems=x, odd elems=y + #define dct_const(x,y) _mm_setr_epi16((x),(y),(x),(y),(x),(y),(x),(y)) + + // out(0) = c0[even]*x + c0[odd]*y (c0, x, y 16-bit, out 32-bit) + // out(1) = c1[even]*x + c1[odd]*y + #define dct_rot(out0,out1, x,y,c0,c1) \ + __m128i c0##lo = _mm_unpacklo_epi16((x),(y)); \ + __m128i c0##hi = _mm_unpackhi_epi16((x),(y)); \ + __m128i out0##_l = _mm_madd_epi16(c0##lo, c0); \ + __m128i out0##_h = _mm_madd_epi16(c0##hi, c0); \ + __m128i out1##_l = _mm_madd_epi16(c0##lo, c1); \ + __m128i out1##_h = _mm_madd_epi16(c0##hi, c1) + + // out = in << 12 (in 16-bit, out 32-bit) + #define dct_widen(out, in) \ + __m128i out##_l = _mm_srai_epi32(_mm_unpacklo_epi16(_mm_setzero_si128(), (in)), 4); \ + __m128i out##_h = _mm_srai_epi32(_mm_unpackhi_epi16(_mm_setzero_si128(), (in)), 4) + + // wide add + #define dct_wadd(out, a, b) \ + __m128i out##_l = _mm_add_epi32(a##_l, b##_l); \ + __m128i out##_h = _mm_add_epi32(a##_h, b##_h) + + // wide sub + #define dct_wsub(out, a, b) \ + __m128i out##_l = _mm_sub_epi32(a##_l, b##_l); \ + __m128i out##_h = _mm_sub_epi32(a##_h, b##_h) + + // butterfly a/b, add bias, then shift by "s" and pack + #define dct_bfly32o(out0, out1, a,b,bias,s) \ + { \ + __m128i abiased_l = _mm_add_epi32(a##_l, bias); \ + __m128i abiased_h = _mm_add_epi32(a##_h, bias); \ + dct_wadd(sum, abiased, b); \ + dct_wsub(dif, abiased, b); \ + out0 = _mm_packs_epi32(_mm_srai_epi32(sum_l, s), _mm_srai_epi32(sum_h, s)); \ + out1 = _mm_packs_epi32(_mm_srai_epi32(dif_l, s), _mm_srai_epi32(dif_h, s)); \ + } + + // 8-bit interleave step (for transposes) + #define dct_interleave8(a, b) \ + tmp = a; \ + a = _mm_unpacklo_epi8(a, b); \ + b = _mm_unpackhi_epi8(tmp, b) + + // 16-bit interleave step (for transposes) + #define dct_interleave16(a, b) \ + tmp = a; \ + a = _mm_unpacklo_epi16(a, b); \ + b = _mm_unpackhi_epi16(tmp, b) + + #define dct_pass(bias,shift) \ + { \ + /* even part */ \ + dct_rot(t2e,t3e, row2,row6, rot0_0,rot0_1); \ + __m128i sum04 = _mm_add_epi16(row0, row4); \ + __m128i dif04 = _mm_sub_epi16(row0, row4); \ + dct_widen(t0e, sum04); \ + dct_widen(t1e, dif04); \ + dct_wadd(x0, t0e, t3e); \ + dct_wsub(x3, t0e, t3e); \ + dct_wadd(x1, t1e, t2e); \ + dct_wsub(x2, t1e, t2e); \ + /* odd part */ \ + dct_rot(y0o,y2o, row7,row3, rot2_0,rot2_1); \ + dct_rot(y1o,y3o, row5,row1, rot3_0,rot3_1); \ + __m128i sum17 = _mm_add_epi16(row1, row7); \ + __m128i sum35 = _mm_add_epi16(row3, row5); \ + dct_rot(y4o,y5o, sum17,sum35, rot1_0,rot1_1); \ + dct_wadd(x4, y0o, y4o); \ + dct_wadd(x5, y1o, y5o); \ + dct_wadd(x6, y2o, y5o); \ + dct_wadd(x7, y3o, y4o); \ + dct_bfly32o(row0,row7, x0,x7,bias,shift); \ + dct_bfly32o(row1,row6, x1,x6,bias,shift); \ + dct_bfly32o(row2,row5, x2,x5,bias,shift); \ + dct_bfly32o(row3,row4, x3,x4,bias,shift); \ + } + + __m128i rot0_0 = dct_const(stbi__f2f(0.5411961f), stbi__f2f(0.5411961f) + stbi__f2f(-1.847759065f)); + __m128i rot0_1 = dct_const(stbi__f2f(0.5411961f) + stbi__f2f( 0.765366865f), stbi__f2f(0.5411961f)); + __m128i rot1_0 = dct_const(stbi__f2f(1.175875602f) + stbi__f2f(-0.899976223f), stbi__f2f(1.175875602f)); + __m128i rot1_1 = dct_const(stbi__f2f(1.175875602f), stbi__f2f(1.175875602f) + stbi__f2f(-2.562915447f)); + __m128i rot2_0 = dct_const(stbi__f2f(-1.961570560f) + stbi__f2f( 0.298631336f), stbi__f2f(-1.961570560f)); + __m128i rot2_1 = dct_const(stbi__f2f(-1.961570560f), stbi__f2f(-1.961570560f) + stbi__f2f( 3.072711026f)); + __m128i rot3_0 = dct_const(stbi__f2f(-0.390180644f) + stbi__f2f( 2.053119869f), stbi__f2f(-0.390180644f)); + __m128i rot3_1 = dct_const(stbi__f2f(-0.390180644f), stbi__f2f(-0.390180644f) + stbi__f2f( 1.501321110f)); + + // rounding biases in column/row passes, see stbi__idct_block for explanation. + __m128i bias_0 = _mm_set1_epi32(512); + __m128i bias_1 = _mm_set1_epi32(65536 + (128<<17)); + + // load + row0 = _mm_load_si128((const __m128i *) (data + 0*8)); + row1 = _mm_load_si128((const __m128i *) (data + 1*8)); + row2 = _mm_load_si128((const __m128i *) (data + 2*8)); + row3 = _mm_load_si128((const __m128i *) (data + 3*8)); + row4 = _mm_load_si128((const __m128i *) (data + 4*8)); + row5 = _mm_load_si128((const __m128i *) (data + 5*8)); + row6 = _mm_load_si128((const __m128i *) (data + 6*8)); + row7 = _mm_load_si128((const __m128i *) (data + 7*8)); + + // column pass + dct_pass(bias_0, 10); + + { + // 16bit 8x8 transpose pass 1 + dct_interleave16(row0, row4); + dct_interleave16(row1, row5); + dct_interleave16(row2, row6); + dct_interleave16(row3, row7); + + // transpose pass 2 + dct_interleave16(row0, row2); + dct_interleave16(row1, row3); + dct_interleave16(row4, row6); + dct_interleave16(row5, row7); + + // transpose pass 3 + dct_interleave16(row0, row1); + dct_interleave16(row2, row3); + dct_interleave16(row4, row5); + dct_interleave16(row6, row7); + } + + // row pass + dct_pass(bias_1, 17); + + { + // pack + __m128i p0 = _mm_packus_epi16(row0, row1); // a0a1a2a3...a7b0b1b2b3...b7 + __m128i p1 = _mm_packus_epi16(row2, row3); + __m128i p2 = _mm_packus_epi16(row4, row5); + __m128i p3 = _mm_packus_epi16(row6, row7); + + // 8bit 8x8 transpose pass 1 + dct_interleave8(p0, p2); // a0e0a1e1... + dct_interleave8(p1, p3); // c0g0c1g1... + + // transpose pass 2 + dct_interleave8(p0, p1); // a0c0e0g0... + dct_interleave8(p2, p3); // b0d0f0h0... + + // transpose pass 3 + dct_interleave8(p0, p2); // a0b0c0d0... + dct_interleave8(p1, p3); // a4b4c4d4... + + // store + _mm_storel_epi64((__m128i *) out, p0); out += out_stride; + _mm_storel_epi64((__m128i *) out, _mm_shuffle_epi32(p0, 0x4e)); out += out_stride; + _mm_storel_epi64((__m128i *) out, p2); out += out_stride; + _mm_storel_epi64((__m128i *) out, _mm_shuffle_epi32(p2, 0x4e)); out += out_stride; + _mm_storel_epi64((__m128i *) out, p1); out += out_stride; + _mm_storel_epi64((__m128i *) out, _mm_shuffle_epi32(p1, 0x4e)); out += out_stride; + _mm_storel_epi64((__m128i *) out, p3); out += out_stride; + _mm_storel_epi64((__m128i *) out, _mm_shuffle_epi32(p3, 0x4e)); + } + +#undef dct_const +#undef dct_rot +#undef dct_widen +#undef dct_wadd +#undef dct_wsub +#undef dct_bfly32o +#undef dct_interleave8 +#undef dct_interleave16 +#undef dct_pass +} + +#endif // STBI_SSE2 + +#ifdef STBI_NEON + +// NEON integer IDCT. should produce bit-identical +// results to the generic C version. +static void stbi__idct_simd(stbi_uc *out, int out_stride, short data[64]) +{ + int16x8_t row0, row1, row2, row3, row4, row5, row6, row7; + + int16x4_t rot0_0 = vdup_n_s16(stbi__f2f(0.5411961f)); + int16x4_t rot0_1 = vdup_n_s16(stbi__f2f(-1.847759065f)); + int16x4_t rot0_2 = vdup_n_s16(stbi__f2f( 0.765366865f)); + int16x4_t rot1_0 = vdup_n_s16(stbi__f2f( 1.175875602f)); + int16x4_t rot1_1 = vdup_n_s16(stbi__f2f(-0.899976223f)); + int16x4_t rot1_2 = vdup_n_s16(stbi__f2f(-2.562915447f)); + int16x4_t rot2_0 = vdup_n_s16(stbi__f2f(-1.961570560f)); + int16x4_t rot2_1 = vdup_n_s16(stbi__f2f(-0.390180644f)); + int16x4_t rot3_0 = vdup_n_s16(stbi__f2f( 0.298631336f)); + int16x4_t rot3_1 = vdup_n_s16(stbi__f2f( 2.053119869f)); + int16x4_t rot3_2 = vdup_n_s16(stbi__f2f( 3.072711026f)); + int16x4_t rot3_3 = vdup_n_s16(stbi__f2f( 1.501321110f)); + +#define dct_long_mul(out, inq, coeff) \ + int32x4_t out##_l = vmull_s16(vget_low_s16(inq), coeff); \ + int32x4_t out##_h = vmull_s16(vget_high_s16(inq), coeff) + +#define dct_long_mac(out, acc, inq, coeff) \ + int32x4_t out##_l = vmlal_s16(acc##_l, vget_low_s16(inq), coeff); \ + int32x4_t out##_h = vmlal_s16(acc##_h, vget_high_s16(inq), coeff) + +#define dct_widen(out, inq) \ + int32x4_t out##_l = vshll_n_s16(vget_low_s16(inq), 12); \ + int32x4_t out##_h = vshll_n_s16(vget_high_s16(inq), 12) + +// wide add +#define dct_wadd(out, a, b) \ + int32x4_t out##_l = vaddq_s32(a##_l, b##_l); \ + int32x4_t out##_h = vaddq_s32(a##_h, b##_h) + +// wide sub +#define dct_wsub(out, a, b) \ + int32x4_t out##_l = vsubq_s32(a##_l, b##_l); \ + int32x4_t out##_h = vsubq_s32(a##_h, b##_h) + +// butterfly a/b, then shift using "shiftop" by "s" and pack +#define dct_bfly32o(out0,out1, a,b,shiftop,s) \ + { \ + dct_wadd(sum, a, b); \ + dct_wsub(dif, a, b); \ + out0 = vcombine_s16(shiftop(sum_l, s), shiftop(sum_h, s)); \ + out1 = vcombine_s16(shiftop(dif_l, s), shiftop(dif_h, s)); \ + } + +#define dct_pass(shiftop, shift) \ + { \ + /* even part */ \ + int16x8_t sum26 = vaddq_s16(row2, row6); \ + dct_long_mul(p1e, sum26, rot0_0); \ + dct_long_mac(t2e, p1e, row6, rot0_1); \ + dct_long_mac(t3e, p1e, row2, rot0_2); \ + int16x8_t sum04 = vaddq_s16(row0, row4); \ + int16x8_t dif04 = vsubq_s16(row0, row4); \ + dct_widen(t0e, sum04); \ + dct_widen(t1e, dif04); \ + dct_wadd(x0, t0e, t3e); \ + dct_wsub(x3, t0e, t3e); \ + dct_wadd(x1, t1e, t2e); \ + dct_wsub(x2, t1e, t2e); \ + /* odd part */ \ + int16x8_t sum15 = vaddq_s16(row1, row5); \ + int16x8_t sum17 = vaddq_s16(row1, row7); \ + int16x8_t sum35 = vaddq_s16(row3, row5); \ + int16x8_t sum37 = vaddq_s16(row3, row7); \ + int16x8_t sumodd = vaddq_s16(sum17, sum35); \ + dct_long_mul(p5o, sumodd, rot1_0); \ + dct_long_mac(p1o, p5o, sum17, rot1_1); \ + dct_long_mac(p2o, p5o, sum35, rot1_2); \ + dct_long_mul(p3o, sum37, rot2_0); \ + dct_long_mul(p4o, sum15, rot2_1); \ + dct_wadd(sump13o, p1o, p3o); \ + dct_wadd(sump24o, p2o, p4o); \ + dct_wadd(sump23o, p2o, p3o); \ + dct_wadd(sump14o, p1o, p4o); \ + dct_long_mac(x4, sump13o, row7, rot3_0); \ + dct_long_mac(x5, sump24o, row5, rot3_1); \ + dct_long_mac(x6, sump23o, row3, rot3_2); \ + dct_long_mac(x7, sump14o, row1, rot3_3); \ + dct_bfly32o(row0,row7, x0,x7,shiftop,shift); \ + dct_bfly32o(row1,row6, x1,x6,shiftop,shift); \ + dct_bfly32o(row2,row5, x2,x5,shiftop,shift); \ + dct_bfly32o(row3,row4, x3,x4,shiftop,shift); \ + } + + // load + row0 = vld1q_s16(data + 0*8); + row1 = vld1q_s16(data + 1*8); + row2 = vld1q_s16(data + 2*8); + row3 = vld1q_s16(data + 3*8); + row4 = vld1q_s16(data + 4*8); + row5 = vld1q_s16(data + 5*8); + row6 = vld1q_s16(data + 6*8); + row7 = vld1q_s16(data + 7*8); + + // add DC bias + row0 = vaddq_s16(row0, vsetq_lane_s16(1024, vdupq_n_s16(0), 0)); + + // column pass + dct_pass(vrshrn_n_s32, 10); + + // 16bit 8x8 transpose + { +// these three map to a single VTRN.16, VTRN.32, and VSWP, respectively. +// whether compilers actually get this is another story, sadly. +#define dct_trn16(x, y) { int16x8x2_t t = vtrnq_s16(x, y); x = t.val[0]; y = t.val[1]; } +#define dct_trn32(x, y) { int32x4x2_t t = vtrnq_s32(vreinterpretq_s32_s16(x), vreinterpretq_s32_s16(y)); x = vreinterpretq_s16_s32(t.val[0]); y = vreinterpretq_s16_s32(t.val[1]); } +#define dct_trn64(x, y) { int16x8_t x0 = x; int16x8_t y0 = y; x = vcombine_s16(vget_low_s16(x0), vget_low_s16(y0)); y = vcombine_s16(vget_high_s16(x0), vget_high_s16(y0)); } + + // pass 1 + dct_trn16(row0, row1); // a0b0a2b2a4b4a6b6 + dct_trn16(row2, row3); + dct_trn16(row4, row5); + dct_trn16(row6, row7); + + // pass 2 + dct_trn32(row0, row2); // a0b0c0d0a4b4c4d4 + dct_trn32(row1, row3); + dct_trn32(row4, row6); + dct_trn32(row5, row7); + + // pass 3 + dct_trn64(row0, row4); // a0b0c0d0e0f0g0h0 + dct_trn64(row1, row5); + dct_trn64(row2, row6); + dct_trn64(row3, row7); + +#undef dct_trn16 +#undef dct_trn32 +#undef dct_trn64 + } + + // row pass + // vrshrn_n_s32 only supports shifts up to 16, we need + // 17. so do a non-rounding shift of 16 first then follow + // up with a rounding shift by 1. + dct_pass(vshrn_n_s32, 16); + + { + // pack and round + uint8x8_t p0 = vqrshrun_n_s16(row0, 1); + uint8x8_t p1 = vqrshrun_n_s16(row1, 1); + uint8x8_t p2 = vqrshrun_n_s16(row2, 1); + uint8x8_t p3 = vqrshrun_n_s16(row3, 1); + uint8x8_t p4 = vqrshrun_n_s16(row4, 1); + uint8x8_t p5 = vqrshrun_n_s16(row5, 1); + uint8x8_t p6 = vqrshrun_n_s16(row6, 1); + uint8x8_t p7 = vqrshrun_n_s16(row7, 1); + + // again, these can translate into one instruction, but often don't. +#define dct_trn8_8(x, y) { uint8x8x2_t t = vtrn_u8(x, y); x = t.val[0]; y = t.val[1]; } +#define dct_trn8_16(x, y) { uint16x4x2_t t = vtrn_u16(vreinterpret_u16_u8(x), vreinterpret_u16_u8(y)); x = vreinterpret_u8_u16(t.val[0]); y = vreinterpret_u8_u16(t.val[1]); } +#define dct_trn8_32(x, y) { uint32x2x2_t t = vtrn_u32(vreinterpret_u32_u8(x), vreinterpret_u32_u8(y)); x = vreinterpret_u8_u32(t.val[0]); y = vreinterpret_u8_u32(t.val[1]); } + + // sadly can't use interleaved stores here since we only write + // 8 bytes to each scan line! + + // 8x8 8-bit transpose pass 1 + dct_trn8_8(p0, p1); + dct_trn8_8(p2, p3); + dct_trn8_8(p4, p5); + dct_trn8_8(p6, p7); + + // pass 2 + dct_trn8_16(p0, p2); + dct_trn8_16(p1, p3); + dct_trn8_16(p4, p6); + dct_trn8_16(p5, p7); + + // pass 3 + dct_trn8_32(p0, p4); + dct_trn8_32(p1, p5); + dct_trn8_32(p2, p6); + dct_trn8_32(p3, p7); + + // store + vst1_u8(out, p0); out += out_stride; + vst1_u8(out, p1); out += out_stride; + vst1_u8(out, p2); out += out_stride; + vst1_u8(out, p3); out += out_stride; + vst1_u8(out, p4); out += out_stride; + vst1_u8(out, p5); out += out_stride; + vst1_u8(out, p6); out += out_stride; + vst1_u8(out, p7); + +#undef dct_trn8_8 +#undef dct_trn8_16 +#undef dct_trn8_32 + } + +#undef dct_long_mul +#undef dct_long_mac +#undef dct_widen +#undef dct_wadd +#undef dct_wsub +#undef dct_bfly32o +#undef dct_pass +} + +#endif // STBI_NEON + +#define STBI__MARKER_none 0xff +// if there's a pending marker from the entropy stream, return that +// otherwise, fetch from the stream and get a marker. if there's no +// marker, return 0xff, which is never a valid marker value +static stbi_uc stbi__get_marker(stbi__jpeg *j) +{ + stbi_uc x; + if (j->marker != STBI__MARKER_none) { x = j->marker; j->marker = STBI__MARKER_none; return x; } + x = stbi__get8(j->s); + if (x != 0xff) return STBI__MARKER_none; + while (x == 0xff) + x = stbi__get8(j->s); // consume repeated 0xff fill bytes + return x; +} + +// in each scan, we'll have scan_n components, and the order +// of the components is specified by order[] +#define STBI__RESTART(x) ((x) >= 0xd0 && (x) <= 0xd7) + +// after a restart interval, stbi__jpeg_reset the entropy decoder and +// the dc prediction +static void stbi__jpeg_reset(stbi__jpeg *j) +{ + j->code_bits = 0; + j->code_buffer = 0; + j->nomore = 0; + j->img_comp[0].dc_pred = j->img_comp[1].dc_pred = j->img_comp[2].dc_pred = j->img_comp[3].dc_pred = 0; + j->marker = STBI__MARKER_none; + j->todo = j->restart_interval ? j->restart_interval : 0x7fffffff; + j->eob_run = 0; + // no more than 1<<31 MCUs if no restart_interal? that's plenty safe, + // since we don't even allow 1<<30 pixels +} + +static int stbi__parse_entropy_coded_data(stbi__jpeg *z) +{ + stbi__jpeg_reset(z); + if (!z->progressive) { + if (z->scan_n == 1) { + int i,j; + STBI_SIMD_ALIGN(short, data[64]); + int n = z->order[0]; + // non-interleaved data, we just need to process one block at a time, + // in trivial scanline order + // number of blocks to do just depends on how many actual "pixels" this + // component has, independent of interleaved MCU blocking and such + int w = (z->img_comp[n].x+7) >> 3; + int h = (z->img_comp[n].y+7) >> 3; + for (j=0; j < h; ++j) { + for (i=0; i < w; ++i) { + int ha = z->img_comp[n].ha; + if (!stbi__jpeg_decode_block(z, data, z->huff_dc+z->img_comp[n].hd, z->huff_ac+ha, z->fast_ac[ha], n, z->dequant[z->img_comp[n].tq])) return 0; + z->idct_block_kernel(z->img_comp[n].data+z->img_comp[n].w2*j*8+i*8, z->img_comp[n].w2, data); + // every data block is an MCU, so countdown the restart interval + if (--z->todo <= 0) { + if (z->code_bits < 24) stbi__grow_buffer_unsafe(z); + // if it's NOT a restart, then just bail, so we get corrupt data + // rather than no data + if (!STBI__RESTART(z->marker)) return 1; + stbi__jpeg_reset(z); + } + } + } + return 1; + } else { // interleaved + int i,j,k,x,y; + STBI_SIMD_ALIGN(short, data[64]); + for (j=0; j < z->img_mcu_y; ++j) { + for (i=0; i < z->img_mcu_x; ++i) { + // scan an interleaved mcu... process scan_n components in order + for (k=0; k < z->scan_n; ++k) { + int n = z->order[k]; + // scan out an mcu's worth of this component; that's just determined + // by the basic H and V specified for the component + for (y=0; y < z->img_comp[n].v; ++y) { + for (x=0; x < z->img_comp[n].h; ++x) { + int x2 = (i*z->img_comp[n].h + x)*8; + int y2 = (j*z->img_comp[n].v + y)*8; + int ha = z->img_comp[n].ha; + if (!stbi__jpeg_decode_block(z, data, z->huff_dc+z->img_comp[n].hd, z->huff_ac+ha, z->fast_ac[ha], n, z->dequant[z->img_comp[n].tq])) return 0; + z->idct_block_kernel(z->img_comp[n].data+z->img_comp[n].w2*y2+x2, z->img_comp[n].w2, data); + } + } + } + // after all interleaved components, that's an interleaved MCU, + // so now count down the restart interval + if (--z->todo <= 0) { + if (z->code_bits < 24) stbi__grow_buffer_unsafe(z); + if (!STBI__RESTART(z->marker)) return 1; + stbi__jpeg_reset(z); + } + } + } + return 1; + } + } else { + if (z->scan_n == 1) { + int i,j; + int n = z->order[0]; + // non-interleaved data, we just need to process one block at a time, + // in trivial scanline order + // number of blocks to do just depends on how many actual "pixels" this + // component has, independent of interleaved MCU blocking and such + int w = (z->img_comp[n].x+7) >> 3; + int h = (z->img_comp[n].y+7) >> 3; + for (j=0; j < h; ++j) { + for (i=0; i < w; ++i) { + short *data = z->img_comp[n].coeff + 64 * (i + j * z->img_comp[n].coeff_w); + if (z->spec_start == 0) { + if (!stbi__jpeg_decode_block_prog_dc(z, data, &z->huff_dc[z->img_comp[n].hd], n)) + return 0; + } else { + int ha = z->img_comp[n].ha; + if (!stbi__jpeg_decode_block_prog_ac(z, data, &z->huff_ac[ha], z->fast_ac[ha])) + return 0; + } + // every data block is an MCU, so countdown the restart interval + if (--z->todo <= 0) { + if (z->code_bits < 24) stbi__grow_buffer_unsafe(z); + if (!STBI__RESTART(z->marker)) return 1; + stbi__jpeg_reset(z); + } + } + } + return 1; + } else { // interleaved + int i,j,k,x,y; + for (j=0; j < z->img_mcu_y; ++j) { + for (i=0; i < z->img_mcu_x; ++i) { + // scan an interleaved mcu... process scan_n components in order + for (k=0; k < z->scan_n; ++k) { + int n = z->order[k]; + // scan out an mcu's worth of this component; that's just determined + // by the basic H and V specified for the component + for (y=0; y < z->img_comp[n].v; ++y) { + for (x=0; x < z->img_comp[n].h; ++x) { + int x2 = (i*z->img_comp[n].h + x); + int y2 = (j*z->img_comp[n].v + y); + short *data = z->img_comp[n].coeff + 64 * (x2 + y2 * z->img_comp[n].coeff_w); + if (!stbi__jpeg_decode_block_prog_dc(z, data, &z->huff_dc[z->img_comp[n].hd], n)) + return 0; + } + } + } + // after all interleaved components, that's an interleaved MCU, + // so now count down the restart interval + if (--z->todo <= 0) { + if (z->code_bits < 24) stbi__grow_buffer_unsafe(z); + if (!STBI__RESTART(z->marker)) return 1; + stbi__jpeg_reset(z); + } + } + } + return 1; + } + } +} + +static void stbi__jpeg_dequantize(short *data, stbi__uint16 *dequant) +{ + int i; + for (i=0; i < 64; ++i) + data[i] *= dequant[i]; +} + +static void stbi__jpeg_finish(stbi__jpeg *z) +{ + if (z->progressive) { + // dequantize and idct the data + int i,j,n; + for (n=0; n < z->s->img_n; ++n) { + int w = (z->img_comp[n].x+7) >> 3; + int h = (z->img_comp[n].y+7) >> 3; + for (j=0; j < h; ++j) { + for (i=0; i < w; ++i) { + short *data = z->img_comp[n].coeff + 64 * (i + j * z->img_comp[n].coeff_w); + stbi__jpeg_dequantize(data, z->dequant[z->img_comp[n].tq]); + z->idct_block_kernel(z->img_comp[n].data+z->img_comp[n].w2*j*8+i*8, z->img_comp[n].w2, data); + } + } + } + } +} + +static int stbi__process_marker(stbi__jpeg *z, int m) +{ + int L; + switch (m) { + case STBI__MARKER_none: // no marker found + return stbi__err("expected marker","Corrupt JPEG"); + + case 0xDD: // DRI - specify restart interval + if (stbi__get16be(z->s) != 4) return stbi__err("bad DRI len","Corrupt JPEG"); + z->restart_interval = stbi__get16be(z->s); + return 1; + + case 0xDB: // DQT - define quantization table + L = stbi__get16be(z->s)-2; + while (L > 0) { + int q = stbi__get8(z->s); + int p = q >> 4, sixteen = (p != 0); + int t = q & 15,i; + if (p != 0 && p != 1) return stbi__err("bad DQT type","Corrupt JPEG"); + if (t > 3) return stbi__err("bad DQT table","Corrupt JPEG"); + + for (i=0; i < 64; ++i) + z->dequant[t][stbi__jpeg_dezigzag[i]] = (stbi__uint16)(sixteen ? stbi__get16be(z->s) : stbi__get8(z->s)); + L -= (sixteen ? 129 : 65); + } + return L==0; + + case 0xC4: // DHT - define huffman table + L = stbi__get16be(z->s)-2; + while (L > 0) { + stbi_uc *v; + int sizes[16],i,n=0; + int q = stbi__get8(z->s); + int tc = q >> 4; + int th = q & 15; + if (tc > 1 || th > 3) return stbi__err("bad DHT header","Corrupt JPEG"); + for (i=0; i < 16; ++i) { + sizes[i] = stbi__get8(z->s); + n += sizes[i]; + } + if(n > 256) return stbi__err("bad DHT header","Corrupt JPEG"); // Loop over i < n would write past end of values! + L -= 17; + if (tc == 0) { + if (!stbi__build_huffman(z->huff_dc+th, sizes)) return 0; + v = z->huff_dc[th].values; + } else { + if (!stbi__build_huffman(z->huff_ac+th, sizes)) return 0; + v = z->huff_ac[th].values; + } + for (i=0; i < n; ++i) + v[i] = stbi__get8(z->s); + if (tc != 0) + stbi__build_fast_ac(z->fast_ac[th], z->huff_ac + th); + L -= n; + } + return L==0; + } + + // check for comment block or APP blocks + if ((m >= 0xE0 && m <= 0xEF) || m == 0xFE) { + L = stbi__get16be(z->s); + if (L < 2) { + if (m == 0xFE) + return stbi__err("bad COM len","Corrupt JPEG"); + else + return stbi__err("bad APP len","Corrupt JPEG"); + } + L -= 2; + + if (m == 0xE0 && L >= 5) { // JFIF APP0 segment + static const unsigned char tag[5] = {'J','F','I','F','\0'}; + int ok = 1; + int i; + for (i=0; i < 5; ++i) + if (stbi__get8(z->s) != tag[i]) + ok = 0; + L -= 5; + if (ok) + z->jfif = 1; + } else if (m == 0xEE && L >= 12) { // Adobe APP14 segment + static const unsigned char tag[6] = {'A','d','o','b','e','\0'}; + int ok = 1; + int i; + for (i=0; i < 6; ++i) + if (stbi__get8(z->s) != tag[i]) + ok = 0; + L -= 6; + if (ok) { + stbi__get8(z->s); // version + stbi__get16be(z->s); // flags0 + stbi__get16be(z->s); // flags1 + z->app14_color_transform = stbi__get8(z->s); // color transform + L -= 6; + } + } + + stbi__skip(z->s, L); + return 1; + } + + return stbi__err("unknown marker","Corrupt JPEG"); +} + +// after we see SOS +static int stbi__process_scan_header(stbi__jpeg *z) +{ + int i; + int Ls = stbi__get16be(z->s); + z->scan_n = stbi__get8(z->s); + if (z->scan_n < 1 || z->scan_n > 4 || z->scan_n > (int) z->s->img_n) return stbi__err("bad SOS component count","Corrupt JPEG"); + if (Ls != 6+2*z->scan_n) return stbi__err("bad SOS len","Corrupt JPEG"); + for (i=0; i < z->scan_n; ++i) { + int id = stbi__get8(z->s), which; + int q = stbi__get8(z->s); + for (which = 0; which < z->s->img_n; ++which) + if (z->img_comp[which].id == id) + break; + if (which == z->s->img_n) return 0; // no match + z->img_comp[which].hd = q >> 4; if (z->img_comp[which].hd > 3) return stbi__err("bad DC huff","Corrupt JPEG"); + z->img_comp[which].ha = q & 15; if (z->img_comp[which].ha > 3) return stbi__err("bad AC huff","Corrupt JPEG"); + z->order[i] = which; + } + + { + int aa; + z->spec_start = stbi__get8(z->s); + z->spec_end = stbi__get8(z->s); // should be 63, but might be 0 + aa = stbi__get8(z->s); + z->succ_high = (aa >> 4); + z->succ_low = (aa & 15); + if (z->progressive) { + if (z->spec_start > 63 || z->spec_end > 63 || z->spec_start > z->spec_end || z->succ_high > 13 || z->succ_low > 13) + return stbi__err("bad SOS", "Corrupt JPEG"); + } else { + if (z->spec_start != 0) return stbi__err("bad SOS","Corrupt JPEG"); + if (z->succ_high != 0 || z->succ_low != 0) return stbi__err("bad SOS","Corrupt JPEG"); + z->spec_end = 63; + } + } + + return 1; +} + +static int stbi__free_jpeg_components(stbi__jpeg *z, int ncomp, int why) +{ + int i; + for (i=0; i < ncomp; ++i) { + if (z->img_comp[i].raw_data) { + STBI_FREE(z->img_comp[i].raw_data); + z->img_comp[i].raw_data = NULL; + z->img_comp[i].data = NULL; + } + if (z->img_comp[i].raw_coeff) { + STBI_FREE(z->img_comp[i].raw_coeff); + z->img_comp[i].raw_coeff = 0; + z->img_comp[i].coeff = 0; + } + if (z->img_comp[i].linebuf) { + STBI_FREE(z->img_comp[i].linebuf); + z->img_comp[i].linebuf = NULL; + } + } + return why; +} + +static int stbi__process_frame_header(stbi__jpeg *z, int scan) +{ + stbi__context *s = z->s; + int Lf,p,i,q, h_max=1,v_max=1,c; + Lf = stbi__get16be(s); if (Lf < 11) return stbi__err("bad SOF len","Corrupt JPEG"); // JPEG + p = stbi__get8(s); if (p != 8) return stbi__err("only 8-bit","JPEG format not supported: 8-bit only"); // JPEG baseline + s->img_y = stbi__get16be(s); if (s->img_y == 0) return stbi__err("no header height", "JPEG format not supported: delayed height"); // Legal, but we don't handle it--but neither does IJG + s->img_x = stbi__get16be(s); if (s->img_x == 0) return stbi__err("0 width","Corrupt JPEG"); // JPEG requires + if (s->img_y > STBI_MAX_DIMENSIONS) return stbi__err("too large","Very large image (corrupt?)"); + if (s->img_x > STBI_MAX_DIMENSIONS) return stbi__err("too large","Very large image (corrupt?)"); + c = stbi__get8(s); + if (c != 3 && c != 1 && c != 4) return stbi__err("bad component count","Corrupt JPEG"); + s->img_n = c; + for (i=0; i < c; ++i) { + z->img_comp[i].data = NULL; + z->img_comp[i].linebuf = NULL; + } + + if (Lf != 8+3*s->img_n) return stbi__err("bad SOF len","Corrupt JPEG"); + + z->rgb = 0; + for (i=0; i < s->img_n; ++i) { + static const unsigned char rgb[3] = { 'R', 'G', 'B' }; + z->img_comp[i].id = stbi__get8(s); + if (s->img_n == 3 && z->img_comp[i].id == rgb[i]) + ++z->rgb; + q = stbi__get8(s); + z->img_comp[i].h = (q >> 4); if (!z->img_comp[i].h || z->img_comp[i].h > 4) return stbi__err("bad H","Corrupt JPEG"); + z->img_comp[i].v = q & 15; if (!z->img_comp[i].v || z->img_comp[i].v > 4) return stbi__err("bad V","Corrupt JPEG"); + z->img_comp[i].tq = stbi__get8(s); if (z->img_comp[i].tq > 3) return stbi__err("bad TQ","Corrupt JPEG"); + } + + if (scan != STBI__SCAN_load) return 1; + + if (!stbi__mad3sizes_valid(s->img_x, s->img_y, s->img_n, 0)) return stbi__err("too large", "Image too large to decode"); + + for (i=0; i < s->img_n; ++i) { + if (z->img_comp[i].h > h_max) h_max = z->img_comp[i].h; + if (z->img_comp[i].v > v_max) v_max = z->img_comp[i].v; + } + + // check that plane subsampling factors are integer ratios; our resamplers can't deal with fractional ratios + // and I've never seen a non-corrupted JPEG file actually use them + for (i=0; i < s->img_n; ++i) { + if (h_max % z->img_comp[i].h != 0) return stbi__err("bad H","Corrupt JPEG"); + if (v_max % z->img_comp[i].v != 0) return stbi__err("bad V","Corrupt JPEG"); + } + + // compute interleaved mcu info + z->img_h_max = h_max; + z->img_v_max = v_max; + z->img_mcu_w = h_max * 8; + z->img_mcu_h = v_max * 8; + // these sizes can't be more than 17 bits + z->img_mcu_x = (s->img_x + z->img_mcu_w-1) / z->img_mcu_w; + z->img_mcu_y = (s->img_y + z->img_mcu_h-1) / z->img_mcu_h; + + for (i=0; i < s->img_n; ++i) { + // number of effective pixels (e.g. for non-interleaved MCU) + z->img_comp[i].x = (s->img_x * z->img_comp[i].h + h_max-1) / h_max; + z->img_comp[i].y = (s->img_y * z->img_comp[i].v + v_max-1) / v_max; + // to simplify generation, we'll allocate enough memory to decode + // the bogus oversized data from using interleaved MCUs and their + // big blocks (e.g. a 16x16 iMCU on an image of width 33); we won't + // discard the extra data until colorspace conversion + // + // img_mcu_x, img_mcu_y: <=17 bits; comp[i].h and .v are <=4 (checked earlier) + // so these muls can't overflow with 32-bit ints (which we require) + z->img_comp[i].w2 = z->img_mcu_x * z->img_comp[i].h * 8; + z->img_comp[i].h2 = z->img_mcu_y * z->img_comp[i].v * 8; + z->img_comp[i].coeff = 0; + z->img_comp[i].raw_coeff = 0; + z->img_comp[i].linebuf = NULL; + z->img_comp[i].raw_data = stbi__malloc_mad2(z->img_comp[i].w2, z->img_comp[i].h2, 15); + if (z->img_comp[i].raw_data == NULL) + return stbi__free_jpeg_components(z, i+1, stbi__err("outofmem", "Out of memory")); + // align blocks for idct using mmx/sse + z->img_comp[i].data = (stbi_uc*) (((size_t) z->img_comp[i].raw_data + 15) & ~15); + if (z->progressive) { + // w2, h2 are multiples of 8 (see above) + z->img_comp[i].coeff_w = z->img_comp[i].w2 / 8; + z->img_comp[i].coeff_h = z->img_comp[i].h2 / 8; + z->img_comp[i].raw_coeff = stbi__malloc_mad3(z->img_comp[i].w2, z->img_comp[i].h2, sizeof(short), 15); + if (z->img_comp[i].raw_coeff == NULL) + return stbi__free_jpeg_components(z, i+1, stbi__err("outofmem", "Out of memory")); + z->img_comp[i].coeff = (short*) (((size_t) z->img_comp[i].raw_coeff + 15) & ~15); + } + } + + return 1; +} + +// use comparisons since in some cases we handle more than one case (e.g. SOF) +#define stbi__DNL(x) ((x) == 0xdc) +#define stbi__SOI(x) ((x) == 0xd8) +#define stbi__EOI(x) ((x) == 0xd9) +#define stbi__SOF(x) ((x) == 0xc0 || (x) == 0xc1 || (x) == 0xc2) +#define stbi__SOS(x) ((x) == 0xda) + +#define stbi__SOF_progressive(x) ((x) == 0xc2) + +static int stbi__decode_jpeg_header(stbi__jpeg *z, int scan) +{ + int m; + z->jfif = 0; + z->app14_color_transform = -1; // valid values are 0,1,2 + z->marker = STBI__MARKER_none; // initialize cached marker to empty + m = stbi__get_marker(z); + if (!stbi__SOI(m)) return stbi__err("no SOI","Corrupt JPEG"); + if (scan == STBI__SCAN_type) return 1; + m = stbi__get_marker(z); + while (!stbi__SOF(m)) { + if (!stbi__process_marker(z,m)) return 0; + m = stbi__get_marker(z); + while (m == STBI__MARKER_none) { + // some files have extra padding after their blocks, so ok, we'll scan + if (stbi__at_eof(z->s)) return stbi__err("no SOF", "Corrupt JPEG"); + m = stbi__get_marker(z); + } + } + z->progressive = stbi__SOF_progressive(m); + if (!stbi__process_frame_header(z, scan)) return 0; + return 1; +} + +static int stbi__skip_jpeg_junk_at_end(stbi__jpeg *j) +{ + // some JPEGs have junk at end, skip over it but if we find what looks + // like a valid marker, resume there + while (!stbi__at_eof(j->s)) { + int x = stbi__get8(j->s); + while (x == 255) { // might be a marker + if (stbi__at_eof(j->s)) return STBI__MARKER_none; + x = stbi__get8(j->s); + if (x != 0x00 && x != 0xff) { + // not a stuffed zero or lead-in to another marker, looks + // like an actual marker, return it + return x; + } + // stuffed zero has x=0 now which ends the loop, meaning we go + // back to regular scan loop. + // repeated 0xff keeps trying to read the next byte of the marker. + } + } + return STBI__MARKER_none; +} + +// decode image to YCbCr format +static int stbi__decode_jpeg_image(stbi__jpeg *j) +{ + int m; + for (m = 0; m < 4; m++) { + j->img_comp[m].raw_data = NULL; + j->img_comp[m].raw_coeff = NULL; + } + j->restart_interval = 0; + if (!stbi__decode_jpeg_header(j, STBI__SCAN_load)) return 0; + m = stbi__get_marker(j); + while (!stbi__EOI(m)) { + if (stbi__SOS(m)) { + if (!stbi__process_scan_header(j)) return 0; + if (!stbi__parse_entropy_coded_data(j)) return 0; + if (j->marker == STBI__MARKER_none ) { + j->marker = stbi__skip_jpeg_junk_at_end(j); + // if we reach eof without hitting a marker, stbi__get_marker() below will fail and we'll eventually return 0 + } + m = stbi__get_marker(j); + if (STBI__RESTART(m)) + m = stbi__get_marker(j); + } else if (stbi__DNL(m)) { + int Ld = stbi__get16be(j->s); + stbi__uint32 NL = stbi__get16be(j->s); + if (Ld != 4) return stbi__err("bad DNL len", "Corrupt JPEG"); + if (NL != j->s->img_y) return stbi__err("bad DNL height", "Corrupt JPEG"); + m = stbi__get_marker(j); + } else { + if (!stbi__process_marker(j, m)) return 1; + m = stbi__get_marker(j); + } + } + if (j->progressive) + stbi__jpeg_finish(j); + return 1; +} + +// static jfif-centered resampling (across block boundaries) + +typedef stbi_uc *(*resample_row_func)(stbi_uc *out, stbi_uc *in0, stbi_uc *in1, + int w, int hs); + +#define stbi__div4(x) ((stbi_uc) ((x) >> 2)) + +static stbi_uc *resample_row_1(stbi_uc *out, stbi_uc *in_near, stbi_uc *in_far, int w, int hs) +{ + STBI_NOTUSED(out); + STBI_NOTUSED(in_far); + STBI_NOTUSED(w); + STBI_NOTUSED(hs); + return in_near; +} + +static stbi_uc* stbi__resample_row_v_2(stbi_uc *out, stbi_uc *in_near, stbi_uc *in_far, int w, int hs) +{ + // need to generate two samples vertically for every one in input + int i; + STBI_NOTUSED(hs); + for (i=0; i < w; ++i) + out[i] = stbi__div4(3*in_near[i] + in_far[i] + 2); + return out; +} + +static stbi_uc* stbi__resample_row_h_2(stbi_uc *out, stbi_uc *in_near, stbi_uc *in_far, int w, int hs) +{ + // need to generate two samples horizontally for every one in input + int i; + stbi_uc *input = in_near; + + if (w == 1) { + // if only one sample, can't do any interpolation + out[0] = out[1] = input[0]; + return out; + } + + out[0] = input[0]; + out[1] = stbi__div4(input[0]*3 + input[1] + 2); + for (i=1; i < w-1; ++i) { + int n = 3*input[i]+2; + out[i*2+0] = stbi__div4(n+input[i-1]); + out[i*2+1] = stbi__div4(n+input[i+1]); + } + out[i*2+0] = stbi__div4(input[w-2]*3 + input[w-1] + 2); + out[i*2+1] = input[w-1]; + + STBI_NOTUSED(in_far); + STBI_NOTUSED(hs); + + return out; +} + +#define stbi__div16(x) ((stbi_uc) ((x) >> 4)) + +static stbi_uc *stbi__resample_row_hv_2(stbi_uc *out, stbi_uc *in_near, stbi_uc *in_far, int w, int hs) +{ + // need to generate 2x2 samples for every one in input + int i,t0,t1; + if (w == 1) { + out[0] = out[1] = stbi__div4(3*in_near[0] + in_far[0] + 2); + return out; + } + + t1 = 3*in_near[0] + in_far[0]; + out[0] = stbi__div4(t1+2); + for (i=1; i < w; ++i) { + t0 = t1; + t1 = 3*in_near[i]+in_far[i]; + out[i*2-1] = stbi__div16(3*t0 + t1 + 8); + out[i*2 ] = stbi__div16(3*t1 + t0 + 8); + } + out[w*2-1] = stbi__div4(t1+2); + + STBI_NOTUSED(hs); + + return out; +} + +#if defined(STBI_SSE2) || defined(STBI_NEON) +static stbi_uc *stbi__resample_row_hv_2_simd(stbi_uc *out, stbi_uc *in_near, stbi_uc *in_far, int w, int hs) +{ + // need to generate 2x2 samples for every one in input + int i=0,t0,t1; + + if (w == 1) { + out[0] = out[1] = stbi__div4(3*in_near[0] + in_far[0] + 2); + return out; + } + + t1 = 3*in_near[0] + in_far[0]; + // process groups of 8 pixels for as long as we can. + // note we can't handle the last pixel in a row in this loop + // because we need to handle the filter boundary conditions. + for (; i < ((w-1) & ~7); i += 8) { +#if defined(STBI_SSE2) + // load and perform the vertical filtering pass + // this uses 3*x + y = 4*x + (y - x) + __m128i zero = _mm_setzero_si128(); + __m128i farb = _mm_loadl_epi64((__m128i *) (in_far + i)); + __m128i nearb = _mm_loadl_epi64((__m128i *) (in_near + i)); + __m128i farw = _mm_unpacklo_epi8(farb, zero); + __m128i nearw = _mm_unpacklo_epi8(nearb, zero); + __m128i diff = _mm_sub_epi16(farw, nearw); + __m128i nears = _mm_slli_epi16(nearw, 2); + __m128i curr = _mm_add_epi16(nears, diff); // current row + + // horizontal filter works the same based on shifted vers of current + // row. "prev" is current row shifted right by 1 pixel; we need to + // insert the previous pixel value (from t1). + // "next" is current row shifted left by 1 pixel, with first pixel + // of next block of 8 pixels added in. + __m128i prv0 = _mm_slli_si128(curr, 2); + __m128i nxt0 = _mm_srli_si128(curr, 2); + __m128i prev = _mm_insert_epi16(prv0, t1, 0); + __m128i next = _mm_insert_epi16(nxt0, 3*in_near[i+8] + in_far[i+8], 7); + + // horizontal filter, polyphase implementation since it's convenient: + // even pixels = 3*cur + prev = cur*4 + (prev - cur) + // odd pixels = 3*cur + next = cur*4 + (next - cur) + // note the shared term. + __m128i bias = _mm_set1_epi16(8); + __m128i curs = _mm_slli_epi16(curr, 2); + __m128i prvd = _mm_sub_epi16(prev, curr); + __m128i nxtd = _mm_sub_epi16(next, curr); + __m128i curb = _mm_add_epi16(curs, bias); + __m128i even = _mm_add_epi16(prvd, curb); + __m128i odd = _mm_add_epi16(nxtd, curb); + + // interleave even and odd pixels, then undo scaling. + __m128i int0 = _mm_unpacklo_epi16(even, odd); + __m128i int1 = _mm_unpackhi_epi16(even, odd); + __m128i de0 = _mm_srli_epi16(int0, 4); + __m128i de1 = _mm_srli_epi16(int1, 4); + + // pack and write output + __m128i outv = _mm_packus_epi16(de0, de1); + _mm_storeu_si128((__m128i *) (out + i*2), outv); +#elif defined(STBI_NEON) + // load and perform the vertical filtering pass + // this uses 3*x + y = 4*x + (y - x) + uint8x8_t farb = vld1_u8(in_far + i); + uint8x8_t nearb = vld1_u8(in_near + i); + int16x8_t diff = vreinterpretq_s16_u16(vsubl_u8(farb, nearb)); + int16x8_t nears = vreinterpretq_s16_u16(vshll_n_u8(nearb, 2)); + int16x8_t curr = vaddq_s16(nears, diff); // current row + + // horizontal filter works the same based on shifted vers of current + // row. "prev" is current row shifted right by 1 pixel; we need to + // insert the previous pixel value (from t1). + // "next" is current row shifted left by 1 pixel, with first pixel + // of next block of 8 pixels added in. + int16x8_t prv0 = vextq_s16(curr, curr, 7); + int16x8_t nxt0 = vextq_s16(curr, curr, 1); + int16x8_t prev = vsetq_lane_s16(t1, prv0, 0); + int16x8_t next = vsetq_lane_s16(3*in_near[i+8] + in_far[i+8], nxt0, 7); + + // horizontal filter, polyphase implementation since it's convenient: + // even pixels = 3*cur + prev = cur*4 + (prev - cur) + // odd pixels = 3*cur + next = cur*4 + (next - cur) + // note the shared term. + int16x8_t curs = vshlq_n_s16(curr, 2); + int16x8_t prvd = vsubq_s16(prev, curr); + int16x8_t nxtd = vsubq_s16(next, curr); + int16x8_t even = vaddq_s16(curs, prvd); + int16x8_t odd = vaddq_s16(curs, nxtd); + + // undo scaling and round, then store with even/odd phases interleaved + uint8x8x2_t o; + o.val[0] = vqrshrun_n_s16(even, 4); + o.val[1] = vqrshrun_n_s16(odd, 4); + vst2_u8(out + i*2, o); +#endif + + // "previous" value for next iter + t1 = 3*in_near[i+7] + in_far[i+7]; + } + + t0 = t1; + t1 = 3*in_near[i] + in_far[i]; + out[i*2] = stbi__div16(3*t1 + t0 + 8); + + for (++i; i < w; ++i) { + t0 = t1; + t1 = 3*in_near[i]+in_far[i]; + out[i*2-1] = stbi__div16(3*t0 + t1 + 8); + out[i*2 ] = stbi__div16(3*t1 + t0 + 8); + } + out[w*2-1] = stbi__div4(t1+2); + + STBI_NOTUSED(hs); + + return out; +} +#endif + +static stbi_uc *stbi__resample_row_generic(stbi_uc *out, stbi_uc *in_near, stbi_uc *in_far, int w, int hs) +{ + // resample with nearest-neighbor + int i,j; + STBI_NOTUSED(in_far); + for (i=0; i < w; ++i) + for (j=0; j < hs; ++j) + out[i*hs+j] = in_near[i]; + return out; +} + +// this is a reduced-precision calculation of YCbCr-to-RGB introduced +// to make sure the code produces the same results in both SIMD and scalar +#define stbi__float2fixed(x) (((int) ((x) * 4096.0f + 0.5f)) << 8) +static void stbi__YCbCr_to_RGB_row(stbi_uc *out, const stbi_uc *y, const stbi_uc *pcb, const stbi_uc *pcr, int count, int step) +{ + int i; + for (i=0; i < count; ++i) { + int y_fixed = (y[i] << 20) + (1<<19); // rounding + int r,g,b; + int cr = pcr[i] - 128; + int cb = pcb[i] - 128; + r = y_fixed + cr* stbi__float2fixed(1.40200f); + g = y_fixed + (cr*-stbi__float2fixed(0.71414f)) + ((cb*-stbi__float2fixed(0.34414f)) & 0xffff0000); + b = y_fixed + cb* stbi__float2fixed(1.77200f); + r >>= 20; + g >>= 20; + b >>= 20; + if ((unsigned) r > 255) { if (r < 0) r = 0; else r = 255; } + if ((unsigned) g > 255) { if (g < 0) g = 0; else g = 255; } + if ((unsigned) b > 255) { if (b < 0) b = 0; else b = 255; } + out[0] = (stbi_uc)r; + out[1] = (stbi_uc)g; + out[2] = (stbi_uc)b; + out[3] = 255; + out += step; + } +} + +#if defined(STBI_SSE2) || defined(STBI_NEON) +static void stbi__YCbCr_to_RGB_simd(stbi_uc *out, stbi_uc const *y, stbi_uc const *pcb, stbi_uc const *pcr, int count, int step) +{ + int i = 0; + +#ifdef STBI_SSE2 + // step == 3 is pretty ugly on the final interleave, and i'm not convinced + // it's useful in practice (you wouldn't use it for textures, for example). + // so just accelerate step == 4 case. + if (step == 4) { + // this is a fairly straightforward implementation and not super-optimized. + __m128i signflip = _mm_set1_epi8(-0x80); + __m128i cr_const0 = _mm_set1_epi16( (short) ( 1.40200f*4096.0f+0.5f)); + __m128i cr_const1 = _mm_set1_epi16( - (short) ( 0.71414f*4096.0f+0.5f)); + __m128i cb_const0 = _mm_set1_epi16( - (short) ( 0.34414f*4096.0f+0.5f)); + __m128i cb_const1 = _mm_set1_epi16( (short) ( 1.77200f*4096.0f+0.5f)); + __m128i y_bias = _mm_set1_epi8((char) (unsigned char) 128); + __m128i xw = _mm_set1_epi16(255); // alpha channel + + for (; i+7 < count; i += 8) { + // load + __m128i y_bytes = _mm_loadl_epi64((__m128i *) (y+i)); + __m128i cr_bytes = _mm_loadl_epi64((__m128i *) (pcr+i)); + __m128i cb_bytes = _mm_loadl_epi64((__m128i *) (pcb+i)); + __m128i cr_biased = _mm_xor_si128(cr_bytes, signflip); // -128 + __m128i cb_biased = _mm_xor_si128(cb_bytes, signflip); // -128 + + // unpack to short (and left-shift cr, cb by 8) + __m128i yw = _mm_unpacklo_epi8(y_bias, y_bytes); + __m128i crw = _mm_unpacklo_epi8(_mm_setzero_si128(), cr_biased); + __m128i cbw = _mm_unpacklo_epi8(_mm_setzero_si128(), cb_biased); + + // color transform + __m128i yws = _mm_srli_epi16(yw, 4); + __m128i cr0 = _mm_mulhi_epi16(cr_const0, crw); + __m128i cb0 = _mm_mulhi_epi16(cb_const0, cbw); + __m128i cb1 = _mm_mulhi_epi16(cbw, cb_const1); + __m128i cr1 = _mm_mulhi_epi16(crw, cr_const1); + __m128i rws = _mm_add_epi16(cr0, yws); + __m128i gwt = _mm_add_epi16(cb0, yws); + __m128i bws = _mm_add_epi16(yws, cb1); + __m128i gws = _mm_add_epi16(gwt, cr1); + + // descale + __m128i rw = _mm_srai_epi16(rws, 4); + __m128i bw = _mm_srai_epi16(bws, 4); + __m128i gw = _mm_srai_epi16(gws, 4); + + // back to byte, set up for transpose + __m128i brb = _mm_packus_epi16(rw, bw); + __m128i gxb = _mm_packus_epi16(gw, xw); + + // transpose to interleave channels + __m128i t0 = _mm_unpacklo_epi8(brb, gxb); + __m128i t1 = _mm_unpackhi_epi8(brb, gxb); + __m128i o0 = _mm_unpacklo_epi16(t0, t1); + __m128i o1 = _mm_unpackhi_epi16(t0, t1); + + // store + _mm_storeu_si128((__m128i *) (out + 0), o0); + _mm_storeu_si128((__m128i *) (out + 16), o1); + out += 32; + } + } +#endif + +#ifdef STBI_NEON + // in this version, step=3 support would be easy to add. but is there demand? + if (step == 4) { + // this is a fairly straightforward implementation and not super-optimized. + uint8x8_t signflip = vdup_n_u8(0x80); + int16x8_t cr_const0 = vdupq_n_s16( (short) ( 1.40200f*4096.0f+0.5f)); + int16x8_t cr_const1 = vdupq_n_s16( - (short) ( 0.71414f*4096.0f+0.5f)); + int16x8_t cb_const0 = vdupq_n_s16( - (short) ( 0.34414f*4096.0f+0.5f)); + int16x8_t cb_const1 = vdupq_n_s16( (short) ( 1.77200f*4096.0f+0.5f)); + + for (; i+7 < count; i += 8) { + // load + uint8x8_t y_bytes = vld1_u8(y + i); + uint8x8_t cr_bytes = vld1_u8(pcr + i); + uint8x8_t cb_bytes = vld1_u8(pcb + i); + int8x8_t cr_biased = vreinterpret_s8_u8(vsub_u8(cr_bytes, signflip)); + int8x8_t cb_biased = vreinterpret_s8_u8(vsub_u8(cb_bytes, signflip)); + + // expand to s16 + int16x8_t yws = vreinterpretq_s16_u16(vshll_n_u8(y_bytes, 4)); + int16x8_t crw = vshll_n_s8(cr_biased, 7); + int16x8_t cbw = vshll_n_s8(cb_biased, 7); + + // color transform + int16x8_t cr0 = vqdmulhq_s16(crw, cr_const0); + int16x8_t cb0 = vqdmulhq_s16(cbw, cb_const0); + int16x8_t cr1 = vqdmulhq_s16(crw, cr_const1); + int16x8_t cb1 = vqdmulhq_s16(cbw, cb_const1); + int16x8_t rws = vaddq_s16(yws, cr0); + int16x8_t gws = vaddq_s16(vaddq_s16(yws, cb0), cr1); + int16x8_t bws = vaddq_s16(yws, cb1); + + // undo scaling, round, convert to byte + uint8x8x4_t o; + o.val[0] = vqrshrun_n_s16(rws, 4); + o.val[1] = vqrshrun_n_s16(gws, 4); + o.val[2] = vqrshrun_n_s16(bws, 4); + o.val[3] = vdup_n_u8(255); + + // store, interleaving r/g/b/a + vst4_u8(out, o); + out += 8*4; + } + } +#endif + + for (; i < count; ++i) { + int y_fixed = (y[i] << 20) + (1<<19); // rounding + int r,g,b; + int cr = pcr[i] - 128; + int cb = pcb[i] - 128; + r = y_fixed + cr* stbi__float2fixed(1.40200f); + g = y_fixed + cr*-stbi__float2fixed(0.71414f) + ((cb*-stbi__float2fixed(0.34414f)) & 0xffff0000); + b = y_fixed + cb* stbi__float2fixed(1.77200f); + r >>= 20; + g >>= 20; + b >>= 20; + if ((unsigned) r > 255) { if (r < 0) r = 0; else r = 255; } + if ((unsigned) g > 255) { if (g < 0) g = 0; else g = 255; } + if ((unsigned) b > 255) { if (b < 0) b = 0; else b = 255; } + out[0] = (stbi_uc)r; + out[1] = (stbi_uc)g; + out[2] = (stbi_uc)b; + out[3] = 255; + out += step; + } +} +#endif + +// set up the kernels +static void stbi__setup_jpeg(stbi__jpeg *j) +{ + j->idct_block_kernel = stbi__idct_block; + j->YCbCr_to_RGB_kernel = stbi__YCbCr_to_RGB_row; + j->resample_row_hv_2_kernel = stbi__resample_row_hv_2; + +#ifdef STBI_SSE2 + if (stbi__sse2_available()) { + j->idct_block_kernel = stbi__idct_simd; + j->YCbCr_to_RGB_kernel = stbi__YCbCr_to_RGB_simd; + j->resample_row_hv_2_kernel = stbi__resample_row_hv_2_simd; + } +#endif + +#ifdef STBI_NEON + j->idct_block_kernel = stbi__idct_simd; + j->YCbCr_to_RGB_kernel = stbi__YCbCr_to_RGB_simd; + j->resample_row_hv_2_kernel = stbi__resample_row_hv_2_simd; +#endif +} + +// clean up the temporary component buffers +static void stbi__cleanup_jpeg(stbi__jpeg *j) +{ + stbi__free_jpeg_components(j, j->s->img_n, 0); +} + +typedef struct +{ + resample_row_func resample; + stbi_uc *line0,*line1; + int hs,vs; // expansion factor in each axis + int w_lores; // horizontal pixels pre-expansion + int ystep; // how far through vertical expansion we are + int ypos; // which pre-expansion row we're on +} stbi__resample; + +// fast 0..255 * 0..255 => 0..255 rounded multiplication +static stbi_uc stbi__blinn_8x8(stbi_uc x, stbi_uc y) +{ + unsigned int t = x*y + 128; + return (stbi_uc) ((t + (t >>8)) >> 8); +} + +static stbi_uc *load_jpeg_image(stbi__jpeg *z, int *out_x, int *out_y, int *comp, int req_comp) +{ + int n, decode_n, is_rgb; + z->s->img_n = 0; // make stbi__cleanup_jpeg safe + + // validate req_comp + if (req_comp < 0 || req_comp > 4) return stbi__errpuc("bad req_comp", "Internal error"); + + // load a jpeg image from whichever source, but leave in YCbCr format + if (!stbi__decode_jpeg_image(z)) { stbi__cleanup_jpeg(z); return NULL; } + + // determine actual number of components to generate + n = req_comp ? req_comp : z->s->img_n >= 3 ? 3 : 1; + + is_rgb = z->s->img_n == 3 && (z->rgb == 3 || (z->app14_color_transform == 0 && !z->jfif)); + + if (z->s->img_n == 3 && n < 3 && !is_rgb) + decode_n = 1; + else + decode_n = z->s->img_n; + + // nothing to do if no components requested; check this now to avoid + // accessing uninitialized coutput[0] later + if (decode_n <= 0) { stbi__cleanup_jpeg(z); return NULL; } + + // resample and color-convert + { + int k; + unsigned int i,j; + stbi_uc *output; + stbi_uc *coutput[4] = { NULL, NULL, NULL, NULL }; + + stbi__resample res_comp[4]; + + for (k=0; k < decode_n; ++k) { + stbi__resample *r = &res_comp[k]; + + // allocate line buffer big enough for upsampling off the edges + // with upsample factor of 4 + z->img_comp[k].linebuf = (stbi_uc *) stbi__malloc(z->s->img_x + 3); + if (!z->img_comp[k].linebuf) { stbi__cleanup_jpeg(z); return stbi__errpuc("outofmem", "Out of memory"); } + + r->hs = z->img_h_max / z->img_comp[k].h; + r->vs = z->img_v_max / z->img_comp[k].v; + r->ystep = r->vs >> 1; + r->w_lores = (z->s->img_x + r->hs-1) / r->hs; + r->ypos = 0; + r->line0 = r->line1 = z->img_comp[k].data; + + if (r->hs == 1 && r->vs == 1) r->resample = resample_row_1; + else if (r->hs == 1 && r->vs == 2) r->resample = stbi__resample_row_v_2; + else if (r->hs == 2 && r->vs == 1) r->resample = stbi__resample_row_h_2; + else if (r->hs == 2 && r->vs == 2) r->resample = z->resample_row_hv_2_kernel; + else r->resample = stbi__resample_row_generic; + } + + // can't error after this so, this is safe + output = (stbi_uc *) stbi__malloc_mad3(n, z->s->img_x, z->s->img_y, 1); + if (!output) { stbi__cleanup_jpeg(z); return stbi__errpuc("outofmem", "Out of memory"); } + + // now go ahead and resample + for (j=0; j < z->s->img_y; ++j) { + stbi_uc *out = output + n * z->s->img_x * j; + for (k=0; k < decode_n; ++k) { + stbi__resample *r = &res_comp[k]; + int y_bot = r->ystep >= (r->vs >> 1); + coutput[k] = r->resample(z->img_comp[k].linebuf, + y_bot ? r->line1 : r->line0, + y_bot ? r->line0 : r->line1, + r->w_lores, r->hs); + if (++r->ystep >= r->vs) { + r->ystep = 0; + r->line0 = r->line1; + if (++r->ypos < z->img_comp[k].y) + r->line1 += z->img_comp[k].w2; + } + } + if (n >= 3) { + stbi_uc *y = coutput[0]; + if (z->s->img_n == 3) { + if (is_rgb) { + for (i=0; i < z->s->img_x; ++i) { + out[0] = y[i]; + out[1] = coutput[1][i]; + out[2] = coutput[2][i]; + out[3] = 255; + out += n; + } + } else { + z->YCbCr_to_RGB_kernel(out, y, coutput[1], coutput[2], z->s->img_x, n); + } + } else if (z->s->img_n == 4) { + if (z->app14_color_transform == 0) { // CMYK + for (i=0; i < z->s->img_x; ++i) { + stbi_uc m = coutput[3][i]; + out[0] = stbi__blinn_8x8(coutput[0][i], m); + out[1] = stbi__blinn_8x8(coutput[1][i], m); + out[2] = stbi__blinn_8x8(coutput[2][i], m); + out[3] = 255; + out += n; + } + } else if (z->app14_color_transform == 2) { // YCCK + z->YCbCr_to_RGB_kernel(out, y, coutput[1], coutput[2], z->s->img_x, n); + for (i=0; i < z->s->img_x; ++i) { + stbi_uc m = coutput[3][i]; + out[0] = stbi__blinn_8x8(255 - out[0], m); + out[1] = stbi__blinn_8x8(255 - out[1], m); + out[2] = stbi__blinn_8x8(255 - out[2], m); + out += n; + } + } else { // YCbCr + alpha? Ignore the fourth channel for now + z->YCbCr_to_RGB_kernel(out, y, coutput[1], coutput[2], z->s->img_x, n); + } + } else + for (i=0; i < z->s->img_x; ++i) { + out[0] = out[1] = out[2] = y[i]; + out[3] = 255; // not used if n==3 + out += n; + } + } else { + if (is_rgb) { + if (n == 1) + for (i=0; i < z->s->img_x; ++i) + *out++ = stbi__compute_y(coutput[0][i], coutput[1][i], coutput[2][i]); + else { + for (i=0; i < z->s->img_x; ++i, out += 2) { + out[0] = stbi__compute_y(coutput[0][i], coutput[1][i], coutput[2][i]); + out[1] = 255; + } + } + } else if (z->s->img_n == 4 && z->app14_color_transform == 0) { + for (i=0; i < z->s->img_x; ++i) { + stbi_uc m = coutput[3][i]; + stbi_uc r = stbi__blinn_8x8(coutput[0][i], m); + stbi_uc g = stbi__blinn_8x8(coutput[1][i], m); + stbi_uc b = stbi__blinn_8x8(coutput[2][i], m); + out[0] = stbi__compute_y(r, g, b); + out[1] = 255; + out += n; + } + } else if (z->s->img_n == 4 && z->app14_color_transform == 2) { + for (i=0; i < z->s->img_x; ++i) { + out[0] = stbi__blinn_8x8(255 - coutput[0][i], coutput[3][i]); + out[1] = 255; + out += n; + } + } else { + stbi_uc *y = coutput[0]; + if (n == 1) + for (i=0; i < z->s->img_x; ++i) out[i] = y[i]; + else + for (i=0; i < z->s->img_x; ++i) { *out++ = y[i]; *out++ = 255; } + } + } + } + stbi__cleanup_jpeg(z); + *out_x = z->s->img_x; + *out_y = z->s->img_y; + if (comp) *comp = z->s->img_n >= 3 ? 3 : 1; // report original components, not output + return output; + } +} + +static void *stbi__jpeg_load(stbi__context *s, int *x, int *y, int *comp, int req_comp, stbi__result_info *ri) +{ + unsigned char* result; + stbi__jpeg* j = (stbi__jpeg*) stbi__malloc(sizeof(stbi__jpeg)); + if (!j) return stbi__errpuc("outofmem", "Out of memory"); + memset(j, 0, sizeof(stbi__jpeg)); + STBI_NOTUSED(ri); + j->s = s; + stbi__setup_jpeg(j); + result = load_jpeg_image(j, x,y,comp,req_comp); + STBI_FREE(j); + return result; +} + +static int stbi__jpeg_test(stbi__context *s) +{ + int r; + stbi__jpeg* j = (stbi__jpeg*)stbi__malloc(sizeof(stbi__jpeg)); + if (!j) return stbi__err("outofmem", "Out of memory"); + memset(j, 0, sizeof(stbi__jpeg)); + j->s = s; + stbi__setup_jpeg(j); + r = stbi__decode_jpeg_header(j, STBI__SCAN_type); + stbi__rewind(s); + STBI_FREE(j); + return r; +} + +static int stbi__jpeg_info_raw(stbi__jpeg *j, int *x, int *y, int *comp) +{ + if (!stbi__decode_jpeg_header(j, STBI__SCAN_header)) { + stbi__rewind( j->s ); + return 0; + } + if (x) *x = j->s->img_x; + if (y) *y = j->s->img_y; + if (comp) *comp = j->s->img_n >= 3 ? 3 : 1; + return 1; +} + +static int stbi__jpeg_info(stbi__context *s, int *x, int *y, int *comp) +{ + int result; + stbi__jpeg* j = (stbi__jpeg*) (stbi__malloc(sizeof(stbi__jpeg))); + if (!j) return stbi__err("outofmem", "Out of memory"); + memset(j, 0, sizeof(stbi__jpeg)); + j->s = s; + result = stbi__jpeg_info_raw(j, x, y, comp); + STBI_FREE(j); + return result; +} +#endif + +// public domain zlib decode v0.2 Sean Barrett 2006-11-18 +// simple implementation +// - all input must be provided in an upfront buffer +// - all output is written to a single output buffer (can malloc/realloc) +// performance +// - fast huffman + +#ifndef STBI_NO_ZLIB + +// fast-way is faster to check than jpeg huffman, but slow way is slower +#define STBI__ZFAST_BITS 9 // accelerate all cases in default tables +#define STBI__ZFAST_MASK ((1 << STBI__ZFAST_BITS) - 1) +#define STBI__ZNSYMS 288 // number of symbols in literal/length alphabet + +// zlib-style huffman encoding +// (jpegs packs from left, zlib from right, so can't share code) +typedef struct +{ + stbi__uint16 fast[1 << STBI__ZFAST_BITS]; + stbi__uint16 firstcode[16]; + int maxcode[17]; + stbi__uint16 firstsymbol[16]; + stbi_uc size[STBI__ZNSYMS]; + stbi__uint16 value[STBI__ZNSYMS]; +} stbi__zhuffman; + +stbi_inline static int stbi__bitreverse16(int n) +{ + n = ((n & 0xAAAA) >> 1) | ((n & 0x5555) << 1); + n = ((n & 0xCCCC) >> 2) | ((n & 0x3333) << 2); + n = ((n & 0xF0F0) >> 4) | ((n & 0x0F0F) << 4); + n = ((n & 0xFF00) >> 8) | ((n & 0x00FF) << 8); + return n; +} + +stbi_inline static int stbi__bit_reverse(int v, int bits) +{ + STBI_ASSERT(bits <= 16); + // to bit reverse n bits, reverse 16 and shift + // e.g. 11 bits, bit reverse and shift away 5 + return stbi__bitreverse16(v) >> (16-bits); +} + +static int stbi__zbuild_huffman(stbi__zhuffman *z, const stbi_uc *sizelist, int num) +{ + int i,k=0; + int code, next_code[16], sizes[17]; + + // DEFLATE spec for generating codes + memset(sizes, 0, sizeof(sizes)); + memset(z->fast, 0, sizeof(z->fast)); + for (i=0; i < num; ++i) + ++sizes[sizelist[i]]; + sizes[0] = 0; + for (i=1; i < 16; ++i) + if (sizes[i] > (1 << i)) + return stbi__err("bad sizes", "Corrupt PNG"); + code = 0; + for (i=1; i < 16; ++i) { + next_code[i] = code; + z->firstcode[i] = (stbi__uint16) code; + z->firstsymbol[i] = (stbi__uint16) k; + code = (code + sizes[i]); + if (sizes[i]) + if (code-1 >= (1 << i)) return stbi__err("bad codelengths","Corrupt PNG"); + z->maxcode[i] = code << (16-i); // preshift for inner loop + code <<= 1; + k += sizes[i]; + } + z->maxcode[16] = 0x10000; // sentinel + for (i=0; i < num; ++i) { + int s = sizelist[i]; + if (s) { + int c = next_code[s] - z->firstcode[s] + z->firstsymbol[s]; + stbi__uint16 fastv = (stbi__uint16) ((s << 9) | i); + z->size [c] = (stbi_uc ) s; + z->value[c] = (stbi__uint16) i; + if (s <= STBI__ZFAST_BITS) { + int j = stbi__bit_reverse(next_code[s],s); + while (j < (1 << STBI__ZFAST_BITS)) { + z->fast[j] = fastv; + j += (1 << s); + } + } + ++next_code[s]; + } + } + return 1; +} + +// zlib-from-memory implementation for PNG reading +// because PNG allows splitting the zlib stream arbitrarily, +// and it's annoying structurally to have PNG call ZLIB call PNG, +// we require PNG read all the IDATs and combine them into a single +// memory buffer + +typedef struct +{ + stbi_uc *zbuffer, *zbuffer_end; + int num_bits; + stbi__uint32 code_buffer; + + char *zout; + char *zout_start; + char *zout_end; + int z_expandable; + + stbi__zhuffman z_length, z_distance; +} stbi__zbuf; + +stbi_inline static int stbi__zeof(stbi__zbuf *z) +{ + return (z->zbuffer >= z->zbuffer_end); +} + +stbi_inline static stbi_uc stbi__zget8(stbi__zbuf *z) +{ + return stbi__zeof(z) ? 0 : *z->zbuffer++; +} + +static void stbi__fill_bits(stbi__zbuf *z) +{ + do { + if (z->code_buffer >= (1U << z->num_bits)) { + z->zbuffer = z->zbuffer_end; /* treat this as EOF so we fail. */ + return; + } + z->code_buffer |= (unsigned int) stbi__zget8(z) << z->num_bits; + z->num_bits += 8; + } while (z->num_bits <= 24); +} + +stbi_inline static unsigned int stbi__zreceive(stbi__zbuf *z, int n) +{ + unsigned int k; + if (z->num_bits < n) stbi__fill_bits(z); + k = z->code_buffer & ((1 << n) - 1); + z->code_buffer >>= n; + z->num_bits -= n; + return k; +} + +static int stbi__zhuffman_decode_slowpath(stbi__zbuf *a, stbi__zhuffman *z) +{ + int b,s,k; + // not resolved by fast table, so compute it the slow way + // use jpeg approach, which requires MSbits at top + k = stbi__bit_reverse(a->code_buffer, 16); + for (s=STBI__ZFAST_BITS+1; ; ++s) + if (k < z->maxcode[s]) + break; + if (s >= 16) return -1; // invalid code! + // code size is s, so: + b = (k >> (16-s)) - z->firstcode[s] + z->firstsymbol[s]; + if (b >= STBI__ZNSYMS) return -1; // some data was corrupt somewhere! + if (z->size[b] != s) return -1; // was originally an assert, but report failure instead. + a->code_buffer >>= s; + a->num_bits -= s; + return z->value[b]; +} + +stbi_inline static int stbi__zhuffman_decode(stbi__zbuf *a, stbi__zhuffman *z) +{ + int b,s; + if (a->num_bits < 16) { + if (stbi__zeof(a)) { + return -1; /* report error for unexpected end of data. */ + } + stbi__fill_bits(a); + } + b = z->fast[a->code_buffer & STBI__ZFAST_MASK]; + if (b) { + s = b >> 9; + a->code_buffer >>= s; + a->num_bits -= s; + return b & 511; + } + return stbi__zhuffman_decode_slowpath(a, z); +} + +static int stbi__zexpand(stbi__zbuf *z, char *zout, int n) // need to make room for n bytes +{ + char *q; + unsigned int cur, limit, old_limit; + z->zout = zout; + if (!z->z_expandable) return stbi__err("output buffer limit","Corrupt PNG"); + cur = (unsigned int) (z->zout - z->zout_start); + limit = old_limit = (unsigned) (z->zout_end - z->zout_start); + if (UINT_MAX - cur < (unsigned) n) return stbi__err("outofmem", "Out of memory"); + while (cur + n > limit) { + if(limit > UINT_MAX / 2) return stbi__err("outofmem", "Out of memory"); + limit *= 2; + } + q = (char *) STBI_REALLOC_SIZED(z->zout_start, old_limit, limit); + STBI_NOTUSED(old_limit); + if (q == NULL) return stbi__err("outofmem", "Out of memory"); + z->zout_start = q; + z->zout = q + cur; + z->zout_end = q + limit; + return 1; +} + +static const int stbi__zlength_base[31] = { + 3,4,5,6,7,8,9,10,11,13, + 15,17,19,23,27,31,35,43,51,59, + 67,83,99,115,131,163,195,227,258,0,0 }; + +static const int stbi__zlength_extra[31]= +{ 0,0,0,0,0,0,0,0,1,1,1,1,2,2,2,2,3,3,3,3,4,4,4,4,5,5,5,5,0,0,0 }; + +static const int stbi__zdist_base[32] = { 1,2,3,4,5,7,9,13,17,25,33,49,65,97,129,193, +257,385,513,769,1025,1537,2049,3073,4097,6145,8193,12289,16385,24577,0,0}; + +static const int stbi__zdist_extra[32] = +{ 0,0,0,0,1,1,2,2,3,3,4,4,5,5,6,6,7,7,8,8,9,9,10,10,11,11,12,12,13,13}; + +static int stbi__parse_huffman_block(stbi__zbuf *a) +{ + char *zout = a->zout; + for(;;) { + int z = stbi__zhuffman_decode(a, &a->z_length); + if (z < 256) { + if (z < 0) return stbi__err("bad huffman code","Corrupt PNG"); // error in huffman codes + if (zout >= a->zout_end) { + if (!stbi__zexpand(a, zout, 1)) return 0; + zout = a->zout; + } + *zout++ = (char) z; + } else { + stbi_uc *p; + int len,dist; + if (z == 256) { + a->zout = zout; + return 1; + } + if (z >= 286) return stbi__err("bad huffman code","Corrupt PNG"); // per DEFLATE, length codes 286 and 287 must not appear in compressed data + z -= 257; + len = stbi__zlength_base[z]; + if (stbi__zlength_extra[z]) len += stbi__zreceive(a, stbi__zlength_extra[z]); + z = stbi__zhuffman_decode(a, &a->z_distance); + if (z < 0 || z >= 30) return stbi__err("bad huffman code","Corrupt PNG"); // per DEFLATE, distance codes 30 and 31 must not appear in compressed data + dist = stbi__zdist_base[z]; + if (stbi__zdist_extra[z]) dist += stbi__zreceive(a, stbi__zdist_extra[z]); + if (zout - a->zout_start < dist) return stbi__err("bad dist","Corrupt PNG"); + if (zout + len > a->zout_end) { + if (!stbi__zexpand(a, zout, len)) return 0; + zout = a->zout; + } + p = (stbi_uc *) (zout - dist); + if (dist == 1) { // run of one byte; common in images. + stbi_uc v = *p; + if (len) { do *zout++ = v; while (--len); } + } else { + if (len) { do *zout++ = *p++; while (--len); } + } + } + } +} + +static int stbi__compute_huffman_codes(stbi__zbuf *a) +{ + static const stbi_uc length_dezigzag[19] = { 16,17,18,0,8,7,9,6,10,5,11,4,12,3,13,2,14,1,15 }; + stbi__zhuffman z_codelength; + stbi_uc lencodes[286+32+137];//padding for maximum single op + stbi_uc codelength_sizes[19]; + int i,n; + + int hlit = stbi__zreceive(a,5) + 257; + int hdist = stbi__zreceive(a,5) + 1; + int hclen = stbi__zreceive(a,4) + 4; + int ntot = hlit + hdist; + + memset(codelength_sizes, 0, sizeof(codelength_sizes)); + for (i=0; i < hclen; ++i) { + int s = stbi__zreceive(a,3); + codelength_sizes[length_dezigzag[i]] = (stbi_uc) s; + } + if (!stbi__zbuild_huffman(&z_codelength, codelength_sizes, 19)) return 0; + + n = 0; + while (n < ntot) { + int c = stbi__zhuffman_decode(a, &z_codelength); + if (c < 0 || c >= 19) return stbi__err("bad codelengths", "Corrupt PNG"); + if (c < 16) + lencodes[n++] = (stbi_uc) c; + else { + stbi_uc fill = 0; + if (c == 16) { + c = stbi__zreceive(a,2)+3; + if (n == 0) return stbi__err("bad codelengths", "Corrupt PNG"); + fill = lencodes[n-1]; + } else if (c == 17) { + c = stbi__zreceive(a,3)+3; + } else if (c == 18) { + c = stbi__zreceive(a,7)+11; + } else { + return stbi__err("bad codelengths", "Corrupt PNG"); + } + if (ntot - n < c) return stbi__err("bad codelengths", "Corrupt PNG"); + memset(lencodes+n, fill, c); + n += c; + } + } + if (n != ntot) return stbi__err("bad codelengths","Corrupt PNG"); + if (!stbi__zbuild_huffman(&a->z_length, lencodes, hlit)) return 0; + if (!stbi__zbuild_huffman(&a->z_distance, lencodes+hlit, hdist)) return 0; + return 1; +} + +static int stbi__parse_uncompressed_block(stbi__zbuf *a) +{ + stbi_uc header[4]; + int len,nlen,k; + if (a->num_bits & 7) + stbi__zreceive(a, a->num_bits & 7); // discard + // drain the bit-packed data into header + k = 0; + while (a->num_bits > 0) { + header[k++] = (stbi_uc) (a->code_buffer & 255); // suppress MSVC run-time check + a->code_buffer >>= 8; + a->num_bits -= 8; + } + if (a->num_bits < 0) return stbi__err("zlib corrupt","Corrupt PNG"); + // now fill header the normal way + while (k < 4) + header[k++] = stbi__zget8(a); + len = header[1] * 256 + header[0]; + nlen = header[3] * 256 + header[2]; + if (nlen != (len ^ 0xffff)) return stbi__err("zlib corrupt","Corrupt PNG"); + if (a->zbuffer + len > a->zbuffer_end) return stbi__err("read past buffer","Corrupt PNG"); + if (a->zout + len > a->zout_end) + if (!stbi__zexpand(a, a->zout, len)) return 0; + memcpy(a->zout, a->zbuffer, len); + a->zbuffer += len; + a->zout += len; + return 1; +} + +static int stbi__parse_zlib_header(stbi__zbuf *a) +{ + int cmf = stbi__zget8(a); + int cm = cmf & 15; + /* int cinfo = cmf >> 4; */ + int flg = stbi__zget8(a); + if (stbi__zeof(a)) return stbi__err("bad zlib header","Corrupt PNG"); // zlib spec + if ((cmf*256+flg) % 31 != 0) return stbi__err("bad zlib header","Corrupt PNG"); // zlib spec + if (flg & 32) return stbi__err("no preset dict","Corrupt PNG"); // preset dictionary not allowed in png + if (cm != 8) return stbi__err("bad compression","Corrupt PNG"); // DEFLATE required for png + // window = 1 << (8 + cinfo)... but who cares, we fully buffer output + return 1; +} + +static const stbi_uc stbi__zdefault_length[STBI__ZNSYMS] = +{ + 8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8, 8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8, + 8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8, 8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8, + 8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8, 8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8, + 8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8, 8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8, + 8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8, 9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9, + 9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9, 9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9, + 9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9, 9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9, + 9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9, 9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9, + 7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7, 7,7,7,7,7,7,7,7,8,8,8,8,8,8,8,8 +}; +static const stbi_uc stbi__zdefault_distance[32] = +{ + 5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5 +}; +/* +Init algorithm: +{ + int i; // use <= to match clearly with spec + for (i=0; i <= 143; ++i) stbi__zdefault_length[i] = 8; + for ( ; i <= 255; ++i) stbi__zdefault_length[i] = 9; + for ( ; i <= 279; ++i) stbi__zdefault_length[i] = 7; + for ( ; i <= 287; ++i) stbi__zdefault_length[i] = 8; + + for (i=0; i <= 31; ++i) stbi__zdefault_distance[i] = 5; +} +*/ + +static int stbi__parse_zlib(stbi__zbuf *a, int parse_header) +{ + int final, type; + if (parse_header) + if (!stbi__parse_zlib_header(a)) return 0; + a->num_bits = 0; + a->code_buffer = 0; + do { + final = stbi__zreceive(a,1); + type = stbi__zreceive(a,2); + if (type == 0) { + if (!stbi__parse_uncompressed_block(a)) return 0; + } else if (type == 3) { + return 0; + } else { + if (type == 1) { + // use fixed code lengths + if (!stbi__zbuild_huffman(&a->z_length , stbi__zdefault_length , STBI__ZNSYMS)) return 0; + if (!stbi__zbuild_huffman(&a->z_distance, stbi__zdefault_distance, 32)) return 0; + } else { + if (!stbi__compute_huffman_codes(a)) return 0; + } + if (!stbi__parse_huffman_block(a)) return 0; + } + } while (!final); + return 1; +} + +static int stbi__do_zlib(stbi__zbuf *a, char *obuf, int olen, int exp, int parse_header) +{ + a->zout_start = obuf; + a->zout = obuf; + a->zout_end = obuf + olen; + a->z_expandable = exp; + + return stbi__parse_zlib(a, parse_header); +} + +STBIDEF char *stbi_zlib_decode_malloc_guesssize(const char *buffer, int len, int initial_size, int *outlen) +{ + stbi__zbuf a; + char *p = (char *) stbi__malloc(initial_size); + if (p == NULL) return NULL; + a.zbuffer = (stbi_uc *) buffer; + a.zbuffer_end = (stbi_uc *) buffer + len; + if (stbi__do_zlib(&a, p, initial_size, 1, 1)) { + if (outlen) *outlen = (int) (a.zout - a.zout_start); + return a.zout_start; + } else { + STBI_FREE(a.zout_start); + return NULL; + } +} + +STBIDEF char *stbi_zlib_decode_malloc(char const *buffer, int len, int *outlen) +{ + return stbi_zlib_decode_malloc_guesssize(buffer, len, 16384, outlen); +} + +STBIDEF char *stbi_zlib_decode_malloc_guesssize_headerflag(const char *buffer, int len, int initial_size, int *outlen, int parse_header) +{ + stbi__zbuf a; + char *p = (char *) stbi__malloc(initial_size); + if (p == NULL) return NULL; + a.zbuffer = (stbi_uc *) buffer; + a.zbuffer_end = (stbi_uc *) buffer + len; + if (stbi__do_zlib(&a, p, initial_size, 1, parse_header)) { + if (outlen) *outlen = (int) (a.zout - a.zout_start); + return a.zout_start; + } else { + STBI_FREE(a.zout_start); + return NULL; + } +} + +STBIDEF int stbi_zlib_decode_buffer(char *obuffer, int olen, char const *ibuffer, int ilen) +{ + stbi__zbuf a; + a.zbuffer = (stbi_uc *) ibuffer; + a.zbuffer_end = (stbi_uc *) ibuffer + ilen; + if (stbi__do_zlib(&a, obuffer, olen, 0, 1)) + return (int) (a.zout - a.zout_start); + else + return -1; +} + +STBIDEF char *stbi_zlib_decode_noheader_malloc(char const *buffer, int len, int *outlen) +{ + stbi__zbuf a; + char *p = (char *) stbi__malloc(16384); + if (p == NULL) return NULL; + a.zbuffer = (stbi_uc *) buffer; + a.zbuffer_end = (stbi_uc *) buffer+len; + if (stbi__do_zlib(&a, p, 16384, 1, 0)) { + if (outlen) *outlen = (int) (a.zout - a.zout_start); + return a.zout_start; + } else { + STBI_FREE(a.zout_start); + return NULL; + } +} + +STBIDEF int stbi_zlib_decode_noheader_buffer(char *obuffer, int olen, const char *ibuffer, int ilen) +{ + stbi__zbuf a; + a.zbuffer = (stbi_uc *) ibuffer; + a.zbuffer_end = (stbi_uc *) ibuffer + ilen; + if (stbi__do_zlib(&a, obuffer, olen, 0, 0)) + return (int) (a.zout - a.zout_start); + else + return -1; +} +#endif + +// public domain "baseline" PNG decoder v0.10 Sean Barrett 2006-11-18 +// simple implementation +// - only 8-bit samples +// - no CRC checking +// - allocates lots of intermediate memory +// - avoids problem of streaming data between subsystems +// - avoids explicit window management +// performance +// - uses stb_zlib, a PD zlib implementation with fast huffman decoding + +#ifndef STBI_NO_PNG +typedef struct +{ + stbi__uint32 length; + stbi__uint32 type; +} stbi__pngchunk; + +static stbi__pngchunk stbi__get_chunk_header(stbi__context *s) +{ + stbi__pngchunk c; + c.length = stbi__get32be(s); + c.type = stbi__get32be(s); + return c; +} + +static int stbi__check_png_header(stbi__context *s) +{ + static const stbi_uc png_sig[8] = { 137,80,78,71,13,10,26,10 }; + int i; + for (i=0; i < 8; ++i) + if (stbi__get8(s) != png_sig[i]) return stbi__err("bad png sig","Not a PNG"); + return 1; +} + +typedef struct +{ + stbi__context *s; + stbi_uc *idata, *expanded, *out; + int depth; +} stbi__png; + + +enum { + STBI__F_none=0, + STBI__F_sub=1, + STBI__F_up=2, + STBI__F_avg=3, + STBI__F_paeth=4, + // synthetic filters used for first scanline to avoid needing a dummy row of 0s + STBI__F_avg_first, + STBI__F_paeth_first +}; + +static stbi_uc first_row_filter[5] = +{ + STBI__F_none, + STBI__F_sub, + STBI__F_none, + STBI__F_avg_first, + STBI__F_paeth_first +}; + +static int stbi__paeth(int a, int b, int c) +{ + int p = a + b - c; + int pa = abs(p-a); + int pb = abs(p-b); + int pc = abs(p-c); + if (pa <= pb && pa <= pc) return a; + if (pb <= pc) return b; + return c; +} + +static const stbi_uc stbi__depth_scale_table[9] = { 0, 0xff, 0x55, 0, 0x11, 0,0,0, 0x01 }; + +// create the png data from post-deflated data +static int stbi__create_png_image_raw(stbi__png *a, stbi_uc *raw, stbi__uint32 raw_len, int out_n, stbi__uint32 x, stbi__uint32 y, int depth, int color) +{ + int bytes = (depth == 16? 2 : 1); + stbi__context *s = a->s; + stbi__uint32 i,j,stride = x*out_n*bytes; + stbi__uint32 img_len, img_width_bytes; + int k; + int img_n = s->img_n; // copy it into a local for later + + int output_bytes = out_n*bytes; + int filter_bytes = img_n*bytes; + int width = x; + + STBI_ASSERT(out_n == s->img_n || out_n == s->img_n+1); + a->out = (stbi_uc *) stbi__malloc_mad3(x, y, output_bytes, 0); // extra bytes to write off the end into + if (!a->out) return stbi__err("outofmem", "Out of memory"); + + if (!stbi__mad3sizes_valid(img_n, x, depth, 7)) return stbi__err("too large", "Corrupt PNG"); + img_width_bytes = (((img_n * x * depth) + 7) >> 3); + img_len = (img_width_bytes + 1) * y; + + // we used to check for exact match between raw_len and img_len on non-interlaced PNGs, + // but issue #276 reported a PNG in the wild that had extra data at the end (all zeros), + // so just check for raw_len < img_len always. + if (raw_len < img_len) return stbi__err("not enough pixels","Corrupt PNG"); + + for (j=0; j < y; ++j) { + stbi_uc *cur = a->out + stride*j; + stbi_uc *prior; + int filter = *raw++; + + if (filter > 4) + return stbi__err("invalid filter","Corrupt PNG"); + + if (depth < 8) { + if (img_width_bytes > x) return stbi__err("invalid width","Corrupt PNG"); + cur += x*out_n - img_width_bytes; // store output to the rightmost img_len bytes, so we can decode in place + filter_bytes = 1; + width = img_width_bytes; + } + prior = cur - stride; // bugfix: need to compute this after 'cur +=' computation above + + // if first row, use special filter that doesn't sample previous row + if (j == 0) filter = first_row_filter[filter]; + + // handle first byte explicitly + for (k=0; k < filter_bytes; ++k) { + switch (filter) { + case STBI__F_none : cur[k] = raw[k]; break; + case STBI__F_sub : cur[k] = raw[k]; break; + case STBI__F_up : cur[k] = STBI__BYTECAST(raw[k] + prior[k]); break; + case STBI__F_avg : cur[k] = STBI__BYTECAST(raw[k] + (prior[k]>>1)); break; + case STBI__F_paeth : cur[k] = STBI__BYTECAST(raw[k] + stbi__paeth(0,prior[k],0)); break; + case STBI__F_avg_first : cur[k] = raw[k]; break; + case STBI__F_paeth_first: cur[k] = raw[k]; break; + } + } + + if (depth == 8) { + if (img_n != out_n) + cur[img_n] = 255; // first pixel + raw += img_n; + cur += out_n; + prior += out_n; + } else if (depth == 16) { + if (img_n != out_n) { + cur[filter_bytes] = 255; // first pixel top byte + cur[filter_bytes+1] = 255; // first pixel bottom byte + } + raw += filter_bytes; + cur += output_bytes; + prior += output_bytes; + } else { + raw += 1; + cur += 1; + prior += 1; + } + + // this is a little gross, so that we don't switch per-pixel or per-component + if (depth < 8 || img_n == out_n) { + int nk = (width - 1)*filter_bytes; + #define STBI__CASE(f) \ + case f: \ + for (k=0; k < nk; ++k) + switch (filter) { + // "none" filter turns into a memcpy here; make that explicit. + case STBI__F_none: memcpy(cur, raw, nk); break; + STBI__CASE(STBI__F_sub) { cur[k] = STBI__BYTECAST(raw[k] + cur[k-filter_bytes]); } break; + STBI__CASE(STBI__F_up) { cur[k] = STBI__BYTECAST(raw[k] + prior[k]); } break; + STBI__CASE(STBI__F_avg) { cur[k] = STBI__BYTECAST(raw[k] + ((prior[k] + cur[k-filter_bytes])>>1)); } break; + STBI__CASE(STBI__F_paeth) { cur[k] = STBI__BYTECAST(raw[k] + stbi__paeth(cur[k-filter_bytes],prior[k],prior[k-filter_bytes])); } break; + STBI__CASE(STBI__F_avg_first) { cur[k] = STBI__BYTECAST(raw[k] + (cur[k-filter_bytes] >> 1)); } break; + STBI__CASE(STBI__F_paeth_first) { cur[k] = STBI__BYTECAST(raw[k] + stbi__paeth(cur[k-filter_bytes],0,0)); } break; + } + #undef STBI__CASE + raw += nk; + } else { + STBI_ASSERT(img_n+1 == out_n); + #define STBI__CASE(f) \ + case f: \ + for (i=x-1; i >= 1; --i, cur[filter_bytes]=255,raw+=filter_bytes,cur+=output_bytes,prior+=output_bytes) \ + for (k=0; k < filter_bytes; ++k) + switch (filter) { + STBI__CASE(STBI__F_none) { cur[k] = raw[k]; } break; + STBI__CASE(STBI__F_sub) { cur[k] = STBI__BYTECAST(raw[k] + cur[k- output_bytes]); } break; + STBI__CASE(STBI__F_up) { cur[k] = STBI__BYTECAST(raw[k] + prior[k]); } break; + STBI__CASE(STBI__F_avg) { cur[k] = STBI__BYTECAST(raw[k] + ((prior[k] + cur[k- output_bytes])>>1)); } break; + STBI__CASE(STBI__F_paeth) { cur[k] = STBI__BYTECAST(raw[k] + stbi__paeth(cur[k- output_bytes],prior[k],prior[k- output_bytes])); } break; + STBI__CASE(STBI__F_avg_first) { cur[k] = STBI__BYTECAST(raw[k] + (cur[k- output_bytes] >> 1)); } break; + STBI__CASE(STBI__F_paeth_first) { cur[k] = STBI__BYTECAST(raw[k] + stbi__paeth(cur[k- output_bytes],0,0)); } break; + } + #undef STBI__CASE + + // the loop above sets the high byte of the pixels' alpha, but for + // 16 bit png files we also need the low byte set. we'll do that here. + if (depth == 16) { + cur = a->out + stride*j; // start at the beginning of the row again + for (i=0; i < x; ++i,cur+=output_bytes) { + cur[filter_bytes+1] = 255; + } + } + } + } + + // we make a separate pass to expand bits to pixels; for performance, + // this could run two scanlines behind the above code, so it won't + // intefere with filtering but will still be in the cache. + if (depth < 8) { + for (j=0; j < y; ++j) { + stbi_uc *cur = a->out + stride*j; + stbi_uc *in = a->out + stride*j + x*out_n - img_width_bytes; + // unpack 1/2/4-bit into a 8-bit buffer. allows us to keep the common 8-bit path optimal at minimal cost for 1/2/4-bit + // png guarante byte alignment, if width is not multiple of 8/4/2 we'll decode dummy trailing data that will be skipped in the later loop + stbi_uc scale = (color == 0) ? stbi__depth_scale_table[depth] : 1; // scale grayscale values to 0..255 range + + // note that the final byte might overshoot and write more data than desired. + // we can allocate enough data that this never writes out of memory, but it + // could also overwrite the next scanline. can it overwrite non-empty data + // on the next scanline? yes, consider 1-pixel-wide scanlines with 1-bit-per-pixel. + // so we need to explicitly clamp the final ones + + if (depth == 4) { + for (k=x*img_n; k >= 2; k-=2, ++in) { + *cur++ = scale * ((*in >> 4) ); + *cur++ = scale * ((*in ) & 0x0f); + } + if (k > 0) *cur++ = scale * ((*in >> 4) ); + } else if (depth == 2) { + for (k=x*img_n; k >= 4; k-=4, ++in) { + *cur++ = scale * ((*in >> 6) ); + *cur++ = scale * ((*in >> 4) & 0x03); + *cur++ = scale * ((*in >> 2) & 0x03); + *cur++ = scale * ((*in ) & 0x03); + } + if (k > 0) *cur++ = scale * ((*in >> 6) ); + if (k > 1) *cur++ = scale * ((*in >> 4) & 0x03); + if (k > 2) *cur++ = scale * ((*in >> 2) & 0x03); + } else if (depth == 1) { + for (k=x*img_n; k >= 8; k-=8, ++in) { + *cur++ = scale * ((*in >> 7) ); + *cur++ = scale * ((*in >> 6) & 0x01); + *cur++ = scale * ((*in >> 5) & 0x01); + *cur++ = scale * ((*in >> 4) & 0x01); + *cur++ = scale * ((*in >> 3) & 0x01); + *cur++ = scale * ((*in >> 2) & 0x01); + *cur++ = scale * ((*in >> 1) & 0x01); + *cur++ = scale * ((*in ) & 0x01); + } + if (k > 0) *cur++ = scale * ((*in >> 7) ); + if (k > 1) *cur++ = scale * ((*in >> 6) & 0x01); + if (k > 2) *cur++ = scale * ((*in >> 5) & 0x01); + if (k > 3) *cur++ = scale * ((*in >> 4) & 0x01); + if (k > 4) *cur++ = scale * ((*in >> 3) & 0x01); + if (k > 5) *cur++ = scale * ((*in >> 2) & 0x01); + if (k > 6) *cur++ = scale * ((*in >> 1) & 0x01); + } + if (img_n != out_n) { + int q; + // insert alpha = 255 + cur = a->out + stride*j; + if (img_n == 1) { + for (q=x-1; q >= 0; --q) { + cur[q*2+1] = 255; + cur[q*2+0] = cur[q]; + } + } else { + STBI_ASSERT(img_n == 3); + for (q=x-1; q >= 0; --q) { + cur[q*4+3] = 255; + cur[q*4+2] = cur[q*3+2]; + cur[q*4+1] = cur[q*3+1]; + cur[q*4+0] = cur[q*3+0]; + } + } + } + } + } else if (depth == 16) { + // force the image data from big-endian to platform-native. + // this is done in a separate pass due to the decoding relying + // on the data being untouched, but could probably be done + // per-line during decode if care is taken. + stbi_uc *cur = a->out; + stbi__uint16 *cur16 = (stbi__uint16*)cur; + + for(i=0; i < x*y*out_n; ++i,cur16++,cur+=2) { + *cur16 = (cur[0] << 8) | cur[1]; + } + } + + return 1; +} + +static int stbi__create_png_image(stbi__png *a, stbi_uc *image_data, stbi__uint32 image_data_len, int out_n, int depth, int color, int interlaced) +{ + int bytes = (depth == 16 ? 2 : 1); + int out_bytes = out_n * bytes; + stbi_uc *final; + int p; + if (!interlaced) + return stbi__create_png_image_raw(a, image_data, image_data_len, out_n, a->s->img_x, a->s->img_y, depth, color); + + // de-interlacing + final = (stbi_uc *) stbi__malloc_mad3(a->s->img_x, a->s->img_y, out_bytes, 0); + if (!final) return stbi__err("outofmem", "Out of memory"); + for (p=0; p < 7; ++p) { + int xorig[] = { 0,4,0,2,0,1,0 }; + int yorig[] = { 0,0,4,0,2,0,1 }; + int xspc[] = { 8,8,4,4,2,2,1 }; + int yspc[] = { 8,8,8,4,4,2,2 }; + int i,j,x,y; + // pass1_x[4] = 0, pass1_x[5] = 1, pass1_x[12] = 1 + x = (a->s->img_x - xorig[p] + xspc[p]-1) / xspc[p]; + y = (a->s->img_y - yorig[p] + yspc[p]-1) / yspc[p]; + if (x && y) { + stbi__uint32 img_len = ((((a->s->img_n * x * depth) + 7) >> 3) + 1) * y; + if (!stbi__create_png_image_raw(a, image_data, image_data_len, out_n, x, y, depth, color)) { + STBI_FREE(final); + return 0; + } + for (j=0; j < y; ++j) { + for (i=0; i < x; ++i) { + int out_y = j*yspc[p]+yorig[p]; + int out_x = i*xspc[p]+xorig[p]; + memcpy(final + out_y*a->s->img_x*out_bytes + out_x*out_bytes, + a->out + (j*x+i)*out_bytes, out_bytes); + } + } + STBI_FREE(a->out); + image_data += img_len; + image_data_len -= img_len; + } + } + a->out = final; + + return 1; +} + +static int stbi__compute_transparency(stbi__png *z, stbi_uc tc[3], int out_n) +{ + stbi__context *s = z->s; + stbi__uint32 i, pixel_count = s->img_x * s->img_y; + stbi_uc *p = z->out; + + // compute color-based transparency, assuming we've + // already got 255 as the alpha value in the output + STBI_ASSERT(out_n == 2 || out_n == 4); + + if (out_n == 2) { + for (i=0; i < pixel_count; ++i) { + p[1] = (p[0] == tc[0] ? 0 : 255); + p += 2; + } + } else { + for (i=0; i < pixel_count; ++i) { + if (p[0] == tc[0] && p[1] == tc[1] && p[2] == tc[2]) + p[3] = 0; + p += 4; + } + } + return 1; +} + +static int stbi__compute_transparency16(stbi__png *z, stbi__uint16 tc[3], int out_n) +{ + stbi__context *s = z->s; + stbi__uint32 i, pixel_count = s->img_x * s->img_y; + stbi__uint16 *p = (stbi__uint16*) z->out; + + // compute color-based transparency, assuming we've + // already got 65535 as the alpha value in the output + STBI_ASSERT(out_n == 2 || out_n == 4); + + if (out_n == 2) { + for (i = 0; i < pixel_count; ++i) { + p[1] = (p[0] == tc[0] ? 0 : 65535); + p += 2; + } + } else { + for (i = 0; i < pixel_count; ++i) { + if (p[0] == tc[0] && p[1] == tc[1] && p[2] == tc[2]) + p[3] = 0; + p += 4; + } + } + return 1; +} + +static int stbi__expand_png_palette(stbi__png *a, stbi_uc *palette, int len, int pal_img_n) +{ + stbi__uint32 i, pixel_count = a->s->img_x * a->s->img_y; + stbi_uc *p, *temp_out, *orig = a->out; + + p = (stbi_uc *) stbi__malloc_mad2(pixel_count, pal_img_n, 0); + if (p == NULL) return stbi__err("outofmem", "Out of memory"); + + // between here and free(out) below, exitting would leak + temp_out = p; + + if (pal_img_n == 3) { + for (i=0; i < pixel_count; ++i) { + int n = orig[i]*4; + p[0] = palette[n ]; + p[1] = palette[n+1]; + p[2] = palette[n+2]; + p += 3; + } + } else { + for (i=0; i < pixel_count; ++i) { + int n = orig[i]*4; + p[0] = palette[n ]; + p[1] = palette[n+1]; + p[2] = palette[n+2]; + p[3] = palette[n+3]; + p += 4; + } + } + STBI_FREE(a->out); + a->out = temp_out; + + STBI_NOTUSED(len); + + return 1; +} + +static int stbi__unpremultiply_on_load_global = 0; +static int stbi__de_iphone_flag_global = 0; + +STBIDEF void stbi_set_unpremultiply_on_load(int flag_true_if_should_unpremultiply) +{ + stbi__unpremultiply_on_load_global = flag_true_if_should_unpremultiply; +} + +STBIDEF void stbi_convert_iphone_png_to_rgb(int flag_true_if_should_convert) +{ + stbi__de_iphone_flag_global = flag_true_if_should_convert; +} + +#ifndef STBI_THREAD_LOCAL +#define stbi__unpremultiply_on_load stbi__unpremultiply_on_load_global +#define stbi__de_iphone_flag stbi__de_iphone_flag_global +#else +static STBI_THREAD_LOCAL int stbi__unpremultiply_on_load_local, stbi__unpremultiply_on_load_set; +static STBI_THREAD_LOCAL int stbi__de_iphone_flag_local, stbi__de_iphone_flag_set; + +STBIDEF void stbi_set_unpremultiply_on_load_thread(int flag_true_if_should_unpremultiply) +{ + stbi__unpremultiply_on_load_local = flag_true_if_should_unpremultiply; + stbi__unpremultiply_on_load_set = 1; +} + +STBIDEF void stbi_convert_iphone_png_to_rgb_thread(int flag_true_if_should_convert) +{ + stbi__de_iphone_flag_local = flag_true_if_should_convert; + stbi__de_iphone_flag_set = 1; +} + +#define stbi__unpremultiply_on_load (stbi__unpremultiply_on_load_set \ + ? stbi__unpremultiply_on_load_local \ + : stbi__unpremultiply_on_load_global) +#define stbi__de_iphone_flag (stbi__de_iphone_flag_set \ + ? stbi__de_iphone_flag_local \ + : stbi__de_iphone_flag_global) +#endif // STBI_THREAD_LOCAL + +static void stbi__de_iphone(stbi__png *z) +{ + stbi__context *s = z->s; + stbi__uint32 i, pixel_count = s->img_x * s->img_y; + stbi_uc *p = z->out; + + if (s->img_out_n == 3) { // convert bgr to rgb + for (i=0; i < pixel_count; ++i) { + stbi_uc t = p[0]; + p[0] = p[2]; + p[2] = t; + p += 3; + } + } else { + STBI_ASSERT(s->img_out_n == 4); + if (stbi__unpremultiply_on_load) { + // convert bgr to rgb and unpremultiply + for (i=0; i < pixel_count; ++i) { + stbi_uc a = p[3]; + stbi_uc t = p[0]; + if (a) { + stbi_uc half = a / 2; + p[0] = (p[2] * 255 + half) / a; + p[1] = (p[1] * 255 + half) / a; + p[2] = ( t * 255 + half) / a; + } else { + p[0] = p[2]; + p[2] = t; + } + p += 4; + } + } else { + // convert bgr to rgb + for (i=0; i < pixel_count; ++i) { + stbi_uc t = p[0]; + p[0] = p[2]; + p[2] = t; + p += 4; + } + } + } +} + +#define STBI__PNG_TYPE(a,b,c,d) (((unsigned) (a) << 24) + ((unsigned) (b) << 16) + ((unsigned) (c) << 8) + (unsigned) (d)) + +static int stbi__parse_png_file(stbi__png *z, int scan, int req_comp) +{ + stbi_uc palette[1024], pal_img_n=0; + stbi_uc has_trans=0, tc[3]={0}; + stbi__uint16 tc16[3]; + stbi__uint32 ioff=0, idata_limit=0, i, pal_len=0; + int first=1,k,interlace=0, color=0, is_iphone=0; + stbi__context *s = z->s; + + z->expanded = NULL; + z->idata = NULL; + z->out = NULL; + + if (!stbi__check_png_header(s)) return 0; + + if (scan == STBI__SCAN_type) return 1; + + for (;;) { + stbi__pngchunk c = stbi__get_chunk_header(s); + switch (c.type) { + case STBI__PNG_TYPE('C','g','B','I'): + is_iphone = 1; + stbi__skip(s, c.length); + break; + case STBI__PNG_TYPE('I','H','D','R'): { + int comp,filter; + if (!first) return stbi__err("multiple IHDR","Corrupt PNG"); + first = 0; + if (c.length != 13) return stbi__err("bad IHDR len","Corrupt PNG"); + s->img_x = stbi__get32be(s); + s->img_y = stbi__get32be(s); + if (s->img_y > STBI_MAX_DIMENSIONS) return stbi__err("too large","Very large image (corrupt?)"); + if (s->img_x > STBI_MAX_DIMENSIONS) return stbi__err("too large","Very large image (corrupt?)"); + z->depth = stbi__get8(s); if (z->depth != 1 && z->depth != 2 && z->depth != 4 && z->depth != 8 && z->depth != 16) return stbi__err("1/2/4/8/16-bit only","PNG not supported: 1/2/4/8/16-bit only"); + color = stbi__get8(s); if (color > 6) return stbi__err("bad ctype","Corrupt PNG"); + if (color == 3 && z->depth == 16) return stbi__err("bad ctype","Corrupt PNG"); + if (color == 3) pal_img_n = 3; else if (color & 1) return stbi__err("bad ctype","Corrupt PNG"); + comp = stbi__get8(s); if (comp) return stbi__err("bad comp method","Corrupt PNG"); + filter= stbi__get8(s); if (filter) return stbi__err("bad filter method","Corrupt PNG"); + interlace = stbi__get8(s); if (interlace>1) return stbi__err("bad interlace method","Corrupt PNG"); + if (!s->img_x || !s->img_y) return stbi__err("0-pixel image","Corrupt PNG"); + if (!pal_img_n) { + s->img_n = (color & 2 ? 3 : 1) + (color & 4 ? 1 : 0); + if ((1 << 30) / s->img_x / s->img_n < s->img_y) return stbi__err("too large", "Image too large to decode"); + } else { + // if paletted, then pal_n is our final components, and + // img_n is # components to decompress/filter. + s->img_n = 1; + if ((1 << 30) / s->img_x / 4 < s->img_y) return stbi__err("too large","Corrupt PNG"); + } + // even with SCAN_header, have to scan to see if we have a tRNS + break; + } + + case STBI__PNG_TYPE('P','L','T','E'): { + if (first) return stbi__err("first not IHDR", "Corrupt PNG"); + if (c.length > 256*3) return stbi__err("invalid PLTE","Corrupt PNG"); + pal_len = c.length / 3; + if (pal_len * 3 != c.length) return stbi__err("invalid PLTE","Corrupt PNG"); + for (i=0; i < pal_len; ++i) { + palette[i*4+0] = stbi__get8(s); + palette[i*4+1] = stbi__get8(s); + palette[i*4+2] = stbi__get8(s); + palette[i*4+3] = 255; + } + break; + } + + case STBI__PNG_TYPE('t','R','N','S'): { + if (first) return stbi__err("first not IHDR", "Corrupt PNG"); + if (z->idata) return stbi__err("tRNS after IDAT","Corrupt PNG"); + if (pal_img_n) { + if (scan == STBI__SCAN_header) { s->img_n = 4; return 1; } + if (pal_len == 0) return stbi__err("tRNS before PLTE","Corrupt PNG"); + if (c.length > pal_len) return stbi__err("bad tRNS len","Corrupt PNG"); + pal_img_n = 4; + for (i=0; i < c.length; ++i) + palette[i*4+3] = stbi__get8(s); + } else { + if (!(s->img_n & 1)) return stbi__err("tRNS with alpha","Corrupt PNG"); + if (c.length != (stbi__uint32) s->img_n*2) return stbi__err("bad tRNS len","Corrupt PNG"); + has_trans = 1; + // non-paletted with tRNS = constant alpha. if header-scanning, we can stop now. + if (scan == STBI__SCAN_header) { ++s->img_n; return 1; } + if (z->depth == 16) { + for (k = 0; k < s->img_n; ++k) tc16[k] = (stbi__uint16)stbi__get16be(s); // copy the values as-is + } else { + for (k = 0; k < s->img_n; ++k) tc[k] = (stbi_uc)(stbi__get16be(s) & 255) * stbi__depth_scale_table[z->depth]; // non 8-bit images will be larger + } + } + break; + } + + case STBI__PNG_TYPE('I','D','A','T'): { + if (first) return stbi__err("first not IHDR", "Corrupt PNG"); + if (pal_img_n && !pal_len) return stbi__err("no PLTE","Corrupt PNG"); + if (scan == STBI__SCAN_header) { + // header scan definitely stops at first IDAT + if (pal_img_n) + s->img_n = pal_img_n; + return 1; + } + if (c.length > (1u << 30)) return stbi__err("IDAT size limit", "IDAT section larger than 2^30 bytes"); + if ((int)(ioff + c.length) < (int)ioff) return 0; + if (ioff + c.length > idata_limit) { + stbi__uint32 idata_limit_old = idata_limit; + stbi_uc *p; + if (idata_limit == 0) idata_limit = c.length > 4096 ? c.length : 4096; + while (ioff + c.length > idata_limit) + idata_limit *= 2; + STBI_NOTUSED(idata_limit_old); + p = (stbi_uc *) STBI_REALLOC_SIZED(z->idata, idata_limit_old, idata_limit); if (p == NULL) return stbi__err("outofmem", "Out of memory"); + z->idata = p; + } + if (!stbi__getn(s, z->idata+ioff,c.length)) return stbi__err("outofdata","Corrupt PNG"); + ioff += c.length; + break; + } + + case STBI__PNG_TYPE('I','E','N','D'): { + stbi__uint32 raw_len, bpl; + if (first) return stbi__err("first not IHDR", "Corrupt PNG"); + if (scan != STBI__SCAN_load) return 1; + if (z->idata == NULL) return stbi__err("no IDAT","Corrupt PNG"); + // initial guess for decoded data size to avoid unnecessary reallocs + bpl = (s->img_x * z->depth + 7) / 8; // bytes per line, per component + raw_len = bpl * s->img_y * s->img_n /* pixels */ + s->img_y /* filter mode per row */; + z->expanded = (stbi_uc *) stbi_zlib_decode_malloc_guesssize_headerflag((char *) z->idata, ioff, raw_len, (int *) &raw_len, !is_iphone); + if (z->expanded == NULL) return 0; // zlib should set error + STBI_FREE(z->idata); z->idata = NULL; + if ((req_comp == s->img_n+1 && req_comp != 3 && !pal_img_n) || has_trans) + s->img_out_n = s->img_n+1; + else + s->img_out_n = s->img_n; + if (!stbi__create_png_image(z, z->expanded, raw_len, s->img_out_n, z->depth, color, interlace)) return 0; + if (has_trans) { + if (z->depth == 16) { + if (!stbi__compute_transparency16(z, tc16, s->img_out_n)) return 0; + } else { + if (!stbi__compute_transparency(z, tc, s->img_out_n)) return 0; + } + } + if (is_iphone && stbi__de_iphone_flag && s->img_out_n > 2) + stbi__de_iphone(z); + if (pal_img_n) { + // pal_img_n == 3 or 4 + s->img_n = pal_img_n; // record the actual colors we had + s->img_out_n = pal_img_n; + if (req_comp >= 3) s->img_out_n = req_comp; + if (!stbi__expand_png_palette(z, palette, pal_len, s->img_out_n)) + return 0; + } else if (has_trans) { + // non-paletted image with tRNS -> source image has (constant) alpha + ++s->img_n; + } + STBI_FREE(z->expanded); z->expanded = NULL; + // end of PNG chunk, read and skip CRC + stbi__get32be(s); + return 1; + } + + default: + // if critical, fail + if (first) return stbi__err("first not IHDR", "Corrupt PNG"); + if ((c.type & (1 << 29)) == 0) { + #ifndef STBI_NO_FAILURE_STRINGS + // not threadsafe + static char invalid_chunk[] = "XXXX PNG chunk not known"; + invalid_chunk[0] = STBI__BYTECAST(c.type >> 24); + invalid_chunk[1] = STBI__BYTECAST(c.type >> 16); + invalid_chunk[2] = STBI__BYTECAST(c.type >> 8); + invalid_chunk[3] = STBI__BYTECAST(c.type >> 0); + #endif + return stbi__err(invalid_chunk, "PNG not supported: unknown PNG chunk type"); + } + stbi__skip(s, c.length); + break; + } + // end of PNG chunk, read and skip CRC + stbi__get32be(s); + } +} + +static void *stbi__do_png(stbi__png *p, int *x, int *y, int *n, int req_comp, stbi__result_info *ri) +{ + void *result=NULL; + if (req_comp < 0 || req_comp > 4) return stbi__errpuc("bad req_comp", "Internal error"); + if (stbi__parse_png_file(p, STBI__SCAN_load, req_comp)) { + if (p->depth <= 8) + ri->bits_per_channel = 8; + else if (p->depth == 16) + ri->bits_per_channel = 16; + else + return stbi__errpuc("bad bits_per_channel", "PNG not supported: unsupported color depth"); + result = p->out; + p->out = NULL; + if (req_comp && req_comp != p->s->img_out_n) { + if (ri->bits_per_channel == 8) + result = stbi__convert_format((unsigned char *) result, p->s->img_out_n, req_comp, p->s->img_x, p->s->img_y); + else + result = stbi__convert_format16((stbi__uint16 *) result, p->s->img_out_n, req_comp, p->s->img_x, p->s->img_y); + p->s->img_out_n = req_comp; + if (result == NULL) return result; + } + *x = p->s->img_x; + *y = p->s->img_y; + if (n) *n = p->s->img_n; + } + STBI_FREE(p->out); p->out = NULL; + STBI_FREE(p->expanded); p->expanded = NULL; + STBI_FREE(p->idata); p->idata = NULL; + + return result; +} + +static void *stbi__png_load(stbi__context *s, int *x, int *y, int *comp, int req_comp, stbi__result_info *ri) +{ + stbi__png p; + p.s = s; + return stbi__do_png(&p, x,y,comp,req_comp, ri); +} + +static int stbi__png_test(stbi__context *s) +{ + int r; + r = stbi__check_png_header(s); + stbi__rewind(s); + return r; +} + +static int stbi__png_info_raw(stbi__png *p, int *x, int *y, int *comp) +{ + if (!stbi__parse_png_file(p, STBI__SCAN_header, 0)) { + stbi__rewind( p->s ); + return 0; + } + if (x) *x = p->s->img_x; + if (y) *y = p->s->img_y; + if (comp) *comp = p->s->img_n; + return 1; +} + +static int stbi__png_info(stbi__context *s, int *x, int *y, int *comp) +{ + stbi__png p; + p.s = s; + return stbi__png_info_raw(&p, x, y, comp); +} + +static int stbi__png_is16(stbi__context *s) +{ + stbi__png p; + p.s = s; + if (!stbi__png_info_raw(&p, NULL, NULL, NULL)) + return 0; + if (p.depth != 16) { + stbi__rewind(p.s); + return 0; + } + return 1; +} +#endif + +// Microsoft/Windows BMP image + +#ifndef STBI_NO_BMP +static int stbi__bmp_test_raw(stbi__context *s) +{ + int r; + int sz; + if (stbi__get8(s) != 'B') return 0; + if (stbi__get8(s) != 'M') return 0; + stbi__get32le(s); // discard filesize + stbi__get16le(s); // discard reserved + stbi__get16le(s); // discard reserved + stbi__get32le(s); // discard data offset + sz = stbi__get32le(s); + r = (sz == 12 || sz == 40 || sz == 56 || sz == 108 || sz == 124); + return r; +} + +static int stbi__bmp_test(stbi__context *s) +{ + int r = stbi__bmp_test_raw(s); + stbi__rewind(s); + return r; +} + + +// returns 0..31 for the highest set bit +static int stbi__high_bit(unsigned int z) +{ + int n=0; + if (z == 0) return -1; + if (z >= 0x10000) { n += 16; z >>= 16; } + if (z >= 0x00100) { n += 8; z >>= 8; } + if (z >= 0x00010) { n += 4; z >>= 4; } + if (z >= 0x00004) { n += 2; z >>= 2; } + if (z >= 0x00002) { n += 1;/* >>= 1;*/ } + return n; +} + +static int stbi__bitcount(unsigned int a) +{ + a = (a & 0x55555555) + ((a >> 1) & 0x55555555); // max 2 + a = (a & 0x33333333) + ((a >> 2) & 0x33333333); // max 4 + a = (a + (a >> 4)) & 0x0f0f0f0f; // max 8 per 4, now 8 bits + a = (a + (a >> 8)); // max 16 per 8 bits + a = (a + (a >> 16)); // max 32 per 8 bits + return a & 0xff; +} + +// extract an arbitrarily-aligned N-bit value (N=bits) +// from v, and then make it 8-bits long and fractionally +// extend it to full full range. +static int stbi__shiftsigned(unsigned int v, int shift, int bits) +{ + static unsigned int mul_table[9] = { + 0, + 0xff/*0b11111111*/, 0x55/*0b01010101*/, 0x49/*0b01001001*/, 0x11/*0b00010001*/, + 0x21/*0b00100001*/, 0x41/*0b01000001*/, 0x81/*0b10000001*/, 0x01/*0b00000001*/, + }; + static unsigned int shift_table[9] = { + 0, 0,0,1,0,2,4,6,0, + }; + if (shift < 0) + v <<= -shift; + else + v >>= shift; + STBI_ASSERT(v < 256); + v >>= (8-bits); + STBI_ASSERT(bits >= 0 && bits <= 8); + return (int) ((unsigned) v * mul_table[bits]) >> shift_table[bits]; +} + +typedef struct +{ + int bpp, offset, hsz; + unsigned int mr,mg,mb,ma, all_a; + int extra_read; +} stbi__bmp_data; + +static int stbi__bmp_set_mask_defaults(stbi__bmp_data *info, int compress) +{ + // BI_BITFIELDS specifies masks explicitly, don't override + if (compress == 3) + return 1; + + if (compress == 0) { + if (info->bpp == 16) { + info->mr = 31u << 10; + info->mg = 31u << 5; + info->mb = 31u << 0; + } else if (info->bpp == 32) { + info->mr = 0xffu << 16; + info->mg = 0xffu << 8; + info->mb = 0xffu << 0; + info->ma = 0xffu << 24; + info->all_a = 0; // if all_a is 0 at end, then we loaded alpha channel but it was all 0 + } else { + // otherwise, use defaults, which is all-0 + info->mr = info->mg = info->mb = info->ma = 0; + } + return 1; + } + return 0; // error +} + +static void *stbi__bmp_parse_header(stbi__context *s, stbi__bmp_data *info) +{ + int hsz; + if (stbi__get8(s) != 'B' || stbi__get8(s) != 'M') return stbi__errpuc("not BMP", "Corrupt BMP"); + stbi__get32le(s); // discard filesize + stbi__get16le(s); // discard reserved + stbi__get16le(s); // discard reserved + info->offset = stbi__get32le(s); + info->hsz = hsz = stbi__get32le(s); + info->mr = info->mg = info->mb = info->ma = 0; + info->extra_read = 14; + + if (info->offset < 0) return stbi__errpuc("bad BMP", "bad BMP"); + + if (hsz != 12 && hsz != 40 && hsz != 56 && hsz != 108 && hsz != 124) return stbi__errpuc("unknown BMP", "BMP type not supported: unknown"); + if (hsz == 12) { + s->img_x = stbi__get16le(s); + s->img_y = stbi__get16le(s); + } else { + s->img_x = stbi__get32le(s); + s->img_y = stbi__get32le(s); + } + if (stbi__get16le(s) != 1) return stbi__errpuc("bad BMP", "bad BMP"); + info->bpp = stbi__get16le(s); + if (hsz != 12) { + int compress = stbi__get32le(s); + if (compress == 1 || compress == 2) return stbi__errpuc("BMP RLE", "BMP type not supported: RLE"); + if (compress >= 4) return stbi__errpuc("BMP JPEG/PNG", "BMP type not supported: unsupported compression"); // this includes PNG/JPEG modes + if (compress == 3 && info->bpp != 16 && info->bpp != 32) return stbi__errpuc("bad BMP", "bad BMP"); // bitfields requires 16 or 32 bits/pixel + stbi__get32le(s); // discard sizeof + stbi__get32le(s); // discard hres + stbi__get32le(s); // discard vres + stbi__get32le(s); // discard colorsused + stbi__get32le(s); // discard max important + if (hsz == 40 || hsz == 56) { + if (hsz == 56) { + stbi__get32le(s); + stbi__get32le(s); + stbi__get32le(s); + stbi__get32le(s); + } + if (info->bpp == 16 || info->bpp == 32) { + if (compress == 0) { + stbi__bmp_set_mask_defaults(info, compress); + } else if (compress == 3) { + info->mr = stbi__get32le(s); + info->mg = stbi__get32le(s); + info->mb = stbi__get32le(s); + info->extra_read += 12; + // not documented, but generated by photoshop and handled by mspaint + if (info->mr == info->mg && info->mg == info->mb) { + // ?!?!? + return stbi__errpuc("bad BMP", "bad BMP"); + } + } else + return stbi__errpuc("bad BMP", "bad BMP"); + } + } else { + // V4/V5 header + int i; + if (hsz != 108 && hsz != 124) + return stbi__errpuc("bad BMP", "bad BMP"); + info->mr = stbi__get32le(s); + info->mg = stbi__get32le(s); + info->mb = stbi__get32le(s); + info->ma = stbi__get32le(s); + if (compress != 3) // override mr/mg/mb unless in BI_BITFIELDS mode, as per docs + stbi__bmp_set_mask_defaults(info, compress); + stbi__get32le(s); // discard color space + for (i=0; i < 12; ++i) + stbi__get32le(s); // discard color space parameters + if (hsz == 124) { + stbi__get32le(s); // discard rendering intent + stbi__get32le(s); // discard offset of profile data + stbi__get32le(s); // discard size of profile data + stbi__get32le(s); // discard reserved + } + } + } + return (void *) 1; +} + + +static void *stbi__bmp_load(stbi__context *s, int *x, int *y, int *comp, int req_comp, stbi__result_info *ri) +{ + stbi_uc *out; + unsigned int mr=0,mg=0,mb=0,ma=0, all_a; + stbi_uc pal[256][4]; + int psize=0,i,j,width; + int flip_vertically, pad, target; + stbi__bmp_data info; + STBI_NOTUSED(ri); + + info.all_a = 255; + if (stbi__bmp_parse_header(s, &info) == NULL) + return NULL; // error code already set + + flip_vertically = ((int) s->img_y) > 0; + s->img_y = abs((int) s->img_y); + + if (s->img_y > STBI_MAX_DIMENSIONS) return stbi__errpuc("too large","Very large image (corrupt?)"); + if (s->img_x > STBI_MAX_DIMENSIONS) return stbi__errpuc("too large","Very large image (corrupt?)"); + + mr = info.mr; + mg = info.mg; + mb = info.mb; + ma = info.ma; + all_a = info.all_a; + + if (info.hsz == 12) { + if (info.bpp < 24) + psize = (info.offset - info.extra_read - 24) / 3; + } else { + if (info.bpp < 16) + psize = (info.offset - info.extra_read - info.hsz) >> 2; + } + if (psize == 0) { + // accept some number of extra bytes after the header, but if the offset points either to before + // the header ends or implies a large amount of extra data, reject the file as malformed + int bytes_read_so_far = s->callback_already_read + (int)(s->img_buffer - s->img_buffer_original); + int header_limit = 1024; // max we actually read is below 256 bytes currently. + int extra_data_limit = 256*4; // what ordinarily goes here is a palette; 256 entries*4 bytes is its max size. + if (bytes_read_so_far <= 0 || bytes_read_so_far > header_limit) { + return stbi__errpuc("bad header", "Corrupt BMP"); + } + // we established that bytes_read_so_far is positive and sensible. + // the first half of this test rejects offsets that are either too small positives, or + // negative, and guarantees that info.offset >= bytes_read_so_far > 0. this in turn + // ensures the number computed in the second half of the test can't overflow. + if (info.offset < bytes_read_so_far || info.offset - bytes_read_so_far > extra_data_limit) { + return stbi__errpuc("bad offset", "Corrupt BMP"); + } else { + stbi__skip(s, info.offset - bytes_read_so_far); + } + } + + if (info.bpp == 24 && ma == 0xff000000) + s->img_n = 3; + else + s->img_n = ma ? 4 : 3; + if (req_comp && req_comp >= 3) // we can directly decode 3 or 4 + target = req_comp; + else + target = s->img_n; // if they want monochrome, we'll post-convert + + // sanity-check size + if (!stbi__mad3sizes_valid(target, s->img_x, s->img_y, 0)) + return stbi__errpuc("too large", "Corrupt BMP"); + + out = (stbi_uc *) stbi__malloc_mad3(target, s->img_x, s->img_y, 0); + if (!out) return stbi__errpuc("outofmem", "Out of memory"); + if (info.bpp < 16) { + int z=0; + if (psize == 0 || psize > 256) { STBI_FREE(out); return stbi__errpuc("invalid", "Corrupt BMP"); } + for (i=0; i < psize; ++i) { + pal[i][2] = stbi__get8(s); + pal[i][1] = stbi__get8(s); + pal[i][0] = stbi__get8(s); + if (info.hsz != 12) stbi__get8(s); + pal[i][3] = 255; + } + stbi__skip(s, info.offset - info.extra_read - info.hsz - psize * (info.hsz == 12 ? 3 : 4)); + if (info.bpp == 1) width = (s->img_x + 7) >> 3; + else if (info.bpp == 4) width = (s->img_x + 1) >> 1; + else if (info.bpp == 8) width = s->img_x; + else { STBI_FREE(out); return stbi__errpuc("bad bpp", "Corrupt BMP"); } + pad = (-width)&3; + if (info.bpp == 1) { + for (j=0; j < (int) s->img_y; ++j) { + int bit_offset = 7, v = stbi__get8(s); + for (i=0; i < (int) s->img_x; ++i) { + int color = (v>>bit_offset)&0x1; + out[z++] = pal[color][0]; + out[z++] = pal[color][1]; + out[z++] = pal[color][2]; + if (target == 4) out[z++] = 255; + if (i+1 == (int) s->img_x) break; + if((--bit_offset) < 0) { + bit_offset = 7; + v = stbi__get8(s); + } + } + stbi__skip(s, pad); + } + } else { + for (j=0; j < (int) s->img_y; ++j) { + for (i=0; i < (int) s->img_x; i += 2) { + int v=stbi__get8(s),v2=0; + if (info.bpp == 4) { + v2 = v & 15; + v >>= 4; + } + out[z++] = pal[v][0]; + out[z++] = pal[v][1]; + out[z++] = pal[v][2]; + if (target == 4) out[z++] = 255; + if (i+1 == (int) s->img_x) break; + v = (info.bpp == 8) ? stbi__get8(s) : v2; + out[z++] = pal[v][0]; + out[z++] = pal[v][1]; + out[z++] = pal[v][2]; + if (target == 4) out[z++] = 255; + } + stbi__skip(s, pad); + } + } + } else { + int rshift=0,gshift=0,bshift=0,ashift=0,rcount=0,gcount=0,bcount=0,acount=0; + int z = 0; + int easy=0; + stbi__skip(s, info.offset - info.extra_read - info.hsz); + if (info.bpp == 24) width = 3 * s->img_x; + else if (info.bpp == 16) width = 2*s->img_x; + else /* bpp = 32 and pad = 0 */ width=0; + pad = (-width) & 3; + if (info.bpp == 24) { + easy = 1; + } else if (info.bpp == 32) { + if (mb == 0xff && mg == 0xff00 && mr == 0x00ff0000 && ma == 0xff000000) + easy = 2; + } + if (!easy) { + if (!mr || !mg || !mb) { STBI_FREE(out); return stbi__errpuc("bad masks", "Corrupt BMP"); } + // right shift amt to put high bit in position #7 + rshift = stbi__high_bit(mr)-7; rcount = stbi__bitcount(mr); + gshift = stbi__high_bit(mg)-7; gcount = stbi__bitcount(mg); + bshift = stbi__high_bit(mb)-7; bcount = stbi__bitcount(mb); + ashift = stbi__high_bit(ma)-7; acount = stbi__bitcount(ma); + if (rcount > 8 || gcount > 8 || bcount > 8 || acount > 8) { STBI_FREE(out); return stbi__errpuc("bad masks", "Corrupt BMP"); } + } + for (j=0; j < (int) s->img_y; ++j) { + if (easy) { + for (i=0; i < (int) s->img_x; ++i) { + unsigned char a; + out[z+2] = stbi__get8(s); + out[z+1] = stbi__get8(s); + out[z+0] = stbi__get8(s); + z += 3; + a = (easy == 2 ? stbi__get8(s) : 255); + all_a |= a; + if (target == 4) out[z++] = a; + } + } else { + int bpp = info.bpp; + for (i=0; i < (int) s->img_x; ++i) { + stbi__uint32 v = (bpp == 16 ? (stbi__uint32) stbi__get16le(s) : stbi__get32le(s)); + unsigned int a; + out[z++] = STBI__BYTECAST(stbi__shiftsigned(v & mr, rshift, rcount)); + out[z++] = STBI__BYTECAST(stbi__shiftsigned(v & mg, gshift, gcount)); + out[z++] = STBI__BYTECAST(stbi__shiftsigned(v & mb, bshift, bcount)); + a = (ma ? stbi__shiftsigned(v & ma, ashift, acount) : 255); + all_a |= a; + if (target == 4) out[z++] = STBI__BYTECAST(a); + } + } + stbi__skip(s, pad); + } + } + + // if alpha channel is all 0s, replace with all 255s + if (target == 4 && all_a == 0) + for (i=4*s->img_x*s->img_y-1; i >= 0; i -= 4) + out[i] = 255; + + if (flip_vertically) { + stbi_uc t; + for (j=0; j < (int) s->img_y>>1; ++j) { + stbi_uc *p1 = out + j *s->img_x*target; + stbi_uc *p2 = out + (s->img_y-1-j)*s->img_x*target; + for (i=0; i < (int) s->img_x*target; ++i) { + t = p1[i]; p1[i] = p2[i]; p2[i] = t; + } + } + } + + if (req_comp && req_comp != target) { + out = stbi__convert_format(out, target, req_comp, s->img_x, s->img_y); + if (out == NULL) return out; // stbi__convert_format frees input on failure + } + + *x = s->img_x; + *y = s->img_y; + if (comp) *comp = s->img_n; + return out; +} +#endif + +// Targa Truevision - TGA +// by Jonathan Dummer +#ifndef STBI_NO_TGA +// returns STBI_rgb or whatever, 0 on error +static int stbi__tga_get_comp(int bits_per_pixel, int is_grey, int* is_rgb16) +{ + // only RGB or RGBA (incl. 16bit) or grey allowed + if (is_rgb16) *is_rgb16 = 0; + switch(bits_per_pixel) { + case 8: return STBI_grey; + case 16: if(is_grey) return STBI_grey_alpha; + // fallthrough + case 15: if(is_rgb16) *is_rgb16 = 1; + return STBI_rgb; + case 24: // fallthrough + case 32: return bits_per_pixel/8; + default: return 0; + } +} + +static int stbi__tga_info(stbi__context *s, int *x, int *y, int *comp) +{ + int tga_w, tga_h, tga_comp, tga_image_type, tga_bits_per_pixel, tga_colormap_bpp; + int sz, tga_colormap_type; + stbi__get8(s); // discard Offset + tga_colormap_type = stbi__get8(s); // colormap type + if( tga_colormap_type > 1 ) { + stbi__rewind(s); + return 0; // only RGB or indexed allowed + } + tga_image_type = stbi__get8(s); // image type + if ( tga_colormap_type == 1 ) { // colormapped (paletted) image + if (tga_image_type != 1 && tga_image_type != 9) { + stbi__rewind(s); + return 0; + } + stbi__skip(s,4); // skip index of first colormap entry and number of entries + sz = stbi__get8(s); // check bits per palette color entry + if ( (sz != 8) && (sz != 15) && (sz != 16) && (sz != 24) && (sz != 32) ) { + stbi__rewind(s); + return 0; + } + stbi__skip(s,4); // skip image x and y origin + tga_colormap_bpp = sz; + } else { // "normal" image w/o colormap - only RGB or grey allowed, +/- RLE + if ( (tga_image_type != 2) && (tga_image_type != 3) && (tga_image_type != 10) && (tga_image_type != 11) ) { + stbi__rewind(s); + return 0; // only RGB or grey allowed, +/- RLE + } + stbi__skip(s,9); // skip colormap specification and image x/y origin + tga_colormap_bpp = 0; + } + tga_w = stbi__get16le(s); + if( tga_w < 1 ) { + stbi__rewind(s); + return 0; // test width + } + tga_h = stbi__get16le(s); + if( tga_h < 1 ) { + stbi__rewind(s); + return 0; // test height + } + tga_bits_per_pixel = stbi__get8(s); // bits per pixel + stbi__get8(s); // ignore alpha bits + if (tga_colormap_bpp != 0) { + if((tga_bits_per_pixel != 8) && (tga_bits_per_pixel != 16)) { + // when using a colormap, tga_bits_per_pixel is the size of the indexes + // I don't think anything but 8 or 16bit indexes makes sense + stbi__rewind(s); + return 0; + } + tga_comp = stbi__tga_get_comp(tga_colormap_bpp, 0, NULL); + } else { + tga_comp = stbi__tga_get_comp(tga_bits_per_pixel, (tga_image_type == 3) || (tga_image_type == 11), NULL); + } + if(!tga_comp) { + stbi__rewind(s); + return 0; + } + if (x) *x = tga_w; + if (y) *y = tga_h; + if (comp) *comp = tga_comp; + return 1; // seems to have passed everything +} + +static int stbi__tga_test(stbi__context *s) +{ + int res = 0; + int sz, tga_color_type; + stbi__get8(s); // discard Offset + tga_color_type = stbi__get8(s); // color type + if ( tga_color_type > 1 ) goto errorEnd; // only RGB or indexed allowed + sz = stbi__get8(s); // image type + if ( tga_color_type == 1 ) { // colormapped (paletted) image + if (sz != 1 && sz != 9) goto errorEnd; // colortype 1 demands image type 1 or 9 + stbi__skip(s,4); // skip index of first colormap entry and number of entries + sz = stbi__get8(s); // check bits per palette color entry + if ( (sz != 8) && (sz != 15) && (sz != 16) && (sz != 24) && (sz != 32) ) goto errorEnd; + stbi__skip(s,4); // skip image x and y origin + } else { // "normal" image w/o colormap + if ( (sz != 2) && (sz != 3) && (sz != 10) && (sz != 11) ) goto errorEnd; // only RGB or grey allowed, +/- RLE + stbi__skip(s,9); // skip colormap specification and image x/y origin + } + if ( stbi__get16le(s) < 1 ) goto errorEnd; // test width + if ( stbi__get16le(s) < 1 ) goto errorEnd; // test height + sz = stbi__get8(s); // bits per pixel + if ( (tga_color_type == 1) && (sz != 8) && (sz != 16) ) goto errorEnd; // for colormapped images, bpp is size of an index + if ( (sz != 8) && (sz != 15) && (sz != 16) && (sz != 24) && (sz != 32) ) goto errorEnd; + + res = 1; // if we got this far, everything's good and we can return 1 instead of 0 + +errorEnd: + stbi__rewind(s); + return res; +} + +// read 16bit value and convert to 24bit RGB +static void stbi__tga_read_rgb16(stbi__context *s, stbi_uc* out) +{ + stbi__uint16 px = (stbi__uint16)stbi__get16le(s); + stbi__uint16 fiveBitMask = 31; + // we have 3 channels with 5bits each + int r = (px >> 10) & fiveBitMask; + int g = (px >> 5) & fiveBitMask; + int b = px & fiveBitMask; + // Note that this saves the data in RGB(A) order, so it doesn't need to be swapped later + out[0] = (stbi_uc)((r * 255)/31); + out[1] = (stbi_uc)((g * 255)/31); + out[2] = (stbi_uc)((b * 255)/31); + + // some people claim that the most significant bit might be used for alpha + // (possibly if an alpha-bit is set in the "image descriptor byte") + // but that only made 16bit test images completely translucent.. + // so let's treat all 15 and 16bit TGAs as RGB with no alpha. +} + +static void *stbi__tga_load(stbi__context *s, int *x, int *y, int *comp, int req_comp, stbi__result_info *ri) +{ + // read in the TGA header stuff + int tga_offset = stbi__get8(s); + int tga_indexed = stbi__get8(s); + int tga_image_type = stbi__get8(s); + int tga_is_RLE = 0; + int tga_palette_start = stbi__get16le(s); + int tga_palette_len = stbi__get16le(s); + int tga_palette_bits = stbi__get8(s); + int tga_x_origin = stbi__get16le(s); + int tga_y_origin = stbi__get16le(s); + int tga_width = stbi__get16le(s); + int tga_height = stbi__get16le(s); + int tga_bits_per_pixel = stbi__get8(s); + int tga_comp, tga_rgb16=0; + int tga_inverted = stbi__get8(s); + // int tga_alpha_bits = tga_inverted & 15; // the 4 lowest bits - unused (useless?) + // image data + unsigned char *tga_data; + unsigned char *tga_palette = NULL; + int i, j; + unsigned char raw_data[4] = {0}; + int RLE_count = 0; + int RLE_repeating = 0; + int read_next_pixel = 1; + STBI_NOTUSED(ri); + STBI_NOTUSED(tga_x_origin); // @TODO + STBI_NOTUSED(tga_y_origin); // @TODO + + if (tga_height > STBI_MAX_DIMENSIONS) return stbi__errpuc("too large","Very large image (corrupt?)"); + if (tga_width > STBI_MAX_DIMENSIONS) return stbi__errpuc("too large","Very large image (corrupt?)"); + + // do a tiny bit of precessing + if ( tga_image_type >= 8 ) + { + tga_image_type -= 8; + tga_is_RLE = 1; + } + tga_inverted = 1 - ((tga_inverted >> 5) & 1); + + // If I'm paletted, then I'll use the number of bits from the palette + if ( tga_indexed ) tga_comp = stbi__tga_get_comp(tga_palette_bits, 0, &tga_rgb16); + else tga_comp = stbi__tga_get_comp(tga_bits_per_pixel, (tga_image_type == 3), &tga_rgb16); + + if(!tga_comp) // shouldn't really happen, stbi__tga_test() should have ensured basic consistency + return stbi__errpuc("bad format", "Can't find out TGA pixelformat"); + + // tga info + *x = tga_width; + *y = tga_height; + if (comp) *comp = tga_comp; + + if (!stbi__mad3sizes_valid(tga_width, tga_height, tga_comp, 0)) + return stbi__errpuc("too large", "Corrupt TGA"); + + tga_data = (unsigned char*)stbi__malloc_mad3(tga_width, tga_height, tga_comp, 0); + if (!tga_data) return stbi__errpuc("outofmem", "Out of memory"); + + // skip to the data's starting position (offset usually = 0) + stbi__skip(s, tga_offset ); + + if ( !tga_indexed && !tga_is_RLE && !tga_rgb16 ) { + for (i=0; i < tga_height; ++i) { + int row = tga_inverted ? tga_height -i - 1 : i; + stbi_uc *tga_row = tga_data + row*tga_width*tga_comp; + stbi__getn(s, tga_row, tga_width * tga_comp); + } + } else { + // do I need to load a palette? + if ( tga_indexed) + { + if (tga_palette_len == 0) { /* you have to have at least one entry! */ + STBI_FREE(tga_data); + return stbi__errpuc("bad palette", "Corrupt TGA"); + } + + // any data to skip? (offset usually = 0) + stbi__skip(s, tga_palette_start ); + // load the palette + tga_palette = (unsigned char*)stbi__malloc_mad2(tga_palette_len, tga_comp, 0); + if (!tga_palette) { + STBI_FREE(tga_data); + return stbi__errpuc("outofmem", "Out of memory"); + } + if (tga_rgb16) { + stbi_uc *pal_entry = tga_palette; + STBI_ASSERT(tga_comp == STBI_rgb); + for (i=0; i < tga_palette_len; ++i) { + stbi__tga_read_rgb16(s, pal_entry); + pal_entry += tga_comp; + } + } else if (!stbi__getn(s, tga_palette, tga_palette_len * tga_comp)) { + STBI_FREE(tga_data); + STBI_FREE(tga_palette); + return stbi__errpuc("bad palette", "Corrupt TGA"); + } + } + // load the data + for (i=0; i < tga_width * tga_height; ++i) + { + // if I'm in RLE mode, do I need to get a RLE stbi__pngchunk? + if ( tga_is_RLE ) + { + if ( RLE_count == 0 ) + { + // yep, get the next byte as a RLE command + int RLE_cmd = stbi__get8(s); + RLE_count = 1 + (RLE_cmd & 127); + RLE_repeating = RLE_cmd >> 7; + read_next_pixel = 1; + } else if ( !RLE_repeating ) + { + read_next_pixel = 1; + } + } else + { + read_next_pixel = 1; + } + // OK, if I need to read a pixel, do it now + if ( read_next_pixel ) + { + // load however much data we did have + if ( tga_indexed ) + { + // read in index, then perform the lookup + int pal_idx = (tga_bits_per_pixel == 8) ? stbi__get8(s) : stbi__get16le(s); + if ( pal_idx >= tga_palette_len ) { + // invalid index + pal_idx = 0; + } + pal_idx *= tga_comp; + for (j = 0; j < tga_comp; ++j) { + raw_data[j] = tga_palette[pal_idx+j]; + } + } else if(tga_rgb16) { + STBI_ASSERT(tga_comp == STBI_rgb); + stbi__tga_read_rgb16(s, raw_data); + } else { + // read in the data raw + for (j = 0; j < tga_comp; ++j) { + raw_data[j] = stbi__get8(s); + } + } + // clear the reading flag for the next pixel + read_next_pixel = 0; + } // end of reading a pixel + + // copy data + for (j = 0; j < tga_comp; ++j) + tga_data[i*tga_comp+j] = raw_data[j]; + + // in case we're in RLE mode, keep counting down + --RLE_count; + } + // do I need to invert the image? + if ( tga_inverted ) + { + for (j = 0; j*2 < tga_height; ++j) + { + int index1 = j * tga_width * tga_comp; + int index2 = (tga_height - 1 - j) * tga_width * tga_comp; + for (i = tga_width * tga_comp; i > 0; --i) + { + unsigned char temp = tga_data[index1]; + tga_data[index1] = tga_data[index2]; + tga_data[index2] = temp; + ++index1; + ++index2; + } + } + } + // clear my palette, if I had one + if ( tga_palette != NULL ) + { + STBI_FREE( tga_palette ); + } + } + + // swap RGB - if the source data was RGB16, it already is in the right order + if (tga_comp >= 3 && !tga_rgb16) + { + unsigned char* tga_pixel = tga_data; + for (i=0; i < tga_width * tga_height; ++i) + { + unsigned char temp = tga_pixel[0]; + tga_pixel[0] = tga_pixel[2]; + tga_pixel[2] = temp; + tga_pixel += tga_comp; + } + } + + // convert to target component count + if (req_comp && req_comp != tga_comp) + tga_data = stbi__convert_format(tga_data, tga_comp, req_comp, tga_width, tga_height); + + // the things I do to get rid of an error message, and yet keep + // Microsoft's C compilers happy... [8^( + tga_palette_start = tga_palette_len = tga_palette_bits = + tga_x_origin = tga_y_origin = 0; + STBI_NOTUSED(tga_palette_start); + // OK, done + return tga_data; +} +#endif + +// ************************************************************************************************* +// Photoshop PSD loader -- PD by Thatcher Ulrich, integration by Nicolas Schulz, tweaked by STB + +#ifndef STBI_NO_PSD +static int stbi__psd_test(stbi__context *s) +{ + int r = (stbi__get32be(s) == 0x38425053); + stbi__rewind(s); + return r; +} + +static int stbi__psd_decode_rle(stbi__context *s, stbi_uc *p, int pixelCount) +{ + int count, nleft, len; + + count = 0; + while ((nleft = pixelCount - count) > 0) { + len = stbi__get8(s); + if (len == 128) { + // No-op. + } else if (len < 128) { + // Copy next len+1 bytes literally. + len++; + if (len > nleft) return 0; // corrupt data + count += len; + while (len) { + *p = stbi__get8(s); + p += 4; + len--; + } + } else if (len > 128) { + stbi_uc val; + // Next -len+1 bytes in the dest are replicated from next source byte. + // (Interpret len as a negative 8-bit int.) + len = 257 - len; + if (len > nleft) return 0; // corrupt data + val = stbi__get8(s); + count += len; + while (len) { + *p = val; + p += 4; + len--; + } + } + } + + return 1; +} + +static void *stbi__psd_load(stbi__context *s, int *x, int *y, int *comp, int req_comp, stbi__result_info *ri, int bpc) +{ + int pixelCount; + int channelCount, compression; + int channel, i; + int bitdepth; + int w,h; + stbi_uc *out; + STBI_NOTUSED(ri); + + // Check identifier + if (stbi__get32be(s) != 0x38425053) // "8BPS" + return stbi__errpuc("not PSD", "Corrupt PSD image"); + + // Check file type version. + if (stbi__get16be(s) != 1) + return stbi__errpuc("wrong version", "Unsupported version of PSD image"); + + // Skip 6 reserved bytes. + stbi__skip(s, 6 ); + + // Read the number of channels (R, G, B, A, etc). + channelCount = stbi__get16be(s); + if (channelCount < 0 || channelCount > 16) + return stbi__errpuc("wrong channel count", "Unsupported number of channels in PSD image"); + + // Read the rows and columns of the image. + h = stbi__get32be(s); + w = stbi__get32be(s); + + if (h > STBI_MAX_DIMENSIONS) return stbi__errpuc("too large","Very large image (corrupt?)"); + if (w > STBI_MAX_DIMENSIONS) return stbi__errpuc("too large","Very large image (corrupt?)"); + + // Make sure the depth is 8 bits. + bitdepth = stbi__get16be(s); + if (bitdepth != 8 && bitdepth != 16) + return stbi__errpuc("unsupported bit depth", "PSD bit depth is not 8 or 16 bit"); + + // Make sure the color mode is RGB. + // Valid options are: + // 0: Bitmap + // 1: Grayscale + // 2: Indexed color + // 3: RGB color + // 4: CMYK color + // 7: Multichannel + // 8: Duotone + // 9: Lab color + if (stbi__get16be(s) != 3) + return stbi__errpuc("wrong color format", "PSD is not in RGB color format"); + + // Skip the Mode Data. (It's the palette for indexed color; other info for other modes.) + stbi__skip(s,stbi__get32be(s) ); + + // Skip the image resources. (resolution, pen tool paths, etc) + stbi__skip(s, stbi__get32be(s) ); + + // Skip the reserved data. + stbi__skip(s, stbi__get32be(s) ); + + // Find out if the data is compressed. + // Known values: + // 0: no compression + // 1: RLE compressed + compression = stbi__get16be(s); + if (compression > 1) + return stbi__errpuc("bad compression", "PSD has an unknown compression format"); + + // Check size + if (!stbi__mad3sizes_valid(4, w, h, 0)) + return stbi__errpuc("too large", "Corrupt PSD"); + + // Create the destination image. + + if (!compression && bitdepth == 16 && bpc == 16) { + out = (stbi_uc *) stbi__malloc_mad3(8, w, h, 0); + ri->bits_per_channel = 16; + } else + out = (stbi_uc *) stbi__malloc(4 * w*h); + + if (!out) return stbi__errpuc("outofmem", "Out of memory"); + pixelCount = w*h; + + // Initialize the data to zero. + //memset( out, 0, pixelCount * 4 ); + + // Finally, the image data. + if (compression) { + // RLE as used by .PSD and .TIFF + // Loop until you get the number of unpacked bytes you are expecting: + // Read the next source byte into n. + // If n is between 0 and 127 inclusive, copy the next n+1 bytes literally. + // Else if n is between -127 and -1 inclusive, copy the next byte -n+1 times. + // Else if n is 128, noop. + // Endloop + + // The RLE-compressed data is preceded by a 2-byte data count for each row in the data, + // which we're going to just skip. + stbi__skip(s, h * channelCount * 2 ); + + // Read the RLE data by channel. + for (channel = 0; channel < 4; channel++) { + stbi_uc *p; + + p = out+channel; + if (channel >= channelCount) { + // Fill this channel with default data. + for (i = 0; i < pixelCount; i++, p += 4) + *p = (channel == 3 ? 255 : 0); + } else { + // Read the RLE data. + if (!stbi__psd_decode_rle(s, p, pixelCount)) { + STBI_FREE(out); + return stbi__errpuc("corrupt", "bad RLE data"); + } + } + } + + } else { + // We're at the raw image data. It's each channel in order (Red, Green, Blue, Alpha, ...) + // where each channel consists of an 8-bit (or 16-bit) value for each pixel in the image. + + // Read the data by channel. + for (channel = 0; channel < 4; channel++) { + if (channel >= channelCount) { + // Fill this channel with default data. + if (bitdepth == 16 && bpc == 16) { + stbi__uint16 *q = ((stbi__uint16 *) out) + channel; + stbi__uint16 val = channel == 3 ? 65535 : 0; + for (i = 0; i < pixelCount; i++, q += 4) + *q = val; + } else { + stbi_uc *p = out+channel; + stbi_uc val = channel == 3 ? 255 : 0; + for (i = 0; i < pixelCount; i++, p += 4) + *p = val; + } + } else { + if (ri->bits_per_channel == 16) { // output bpc + stbi__uint16 *q = ((stbi__uint16 *) out) + channel; + for (i = 0; i < pixelCount; i++, q += 4) + *q = (stbi__uint16) stbi__get16be(s); + } else { + stbi_uc *p = out+channel; + if (bitdepth == 16) { // input bpc + for (i = 0; i < pixelCount; i++, p += 4) + *p = (stbi_uc) (stbi__get16be(s) >> 8); + } else { + for (i = 0; i < pixelCount; i++, p += 4) + *p = stbi__get8(s); + } + } + } + } + } + + // remove weird white matte from PSD + if (channelCount >= 4) { + if (ri->bits_per_channel == 16) { + for (i=0; i < w*h; ++i) { + stbi__uint16 *pixel = (stbi__uint16 *) out + 4*i; + if (pixel[3] != 0 && pixel[3] != 65535) { + float a = pixel[3] / 65535.0f; + float ra = 1.0f / a; + float inv_a = 65535.0f * (1 - ra); + pixel[0] = (stbi__uint16) (pixel[0]*ra + inv_a); + pixel[1] = (stbi__uint16) (pixel[1]*ra + inv_a); + pixel[2] = (stbi__uint16) (pixel[2]*ra + inv_a); + } + } + } else { + for (i=0; i < w*h; ++i) { + unsigned char *pixel = out + 4*i; + if (pixel[3] != 0 && pixel[3] != 255) { + float a = pixel[3] / 255.0f; + float ra = 1.0f / a; + float inv_a = 255.0f * (1 - ra); + pixel[0] = (unsigned char) (pixel[0]*ra + inv_a); + pixel[1] = (unsigned char) (pixel[1]*ra + inv_a); + pixel[2] = (unsigned char) (pixel[2]*ra + inv_a); + } + } + } + } + + // convert to desired output format + if (req_comp && req_comp != 4) { + if (ri->bits_per_channel == 16) + out = (stbi_uc *) stbi__convert_format16((stbi__uint16 *) out, 4, req_comp, w, h); + else + out = stbi__convert_format(out, 4, req_comp, w, h); + if (out == NULL) return out; // stbi__convert_format frees input on failure + } + + if (comp) *comp = 4; + *y = h; + *x = w; + + return out; +} +#endif + +// ************************************************************************************************* +// Softimage PIC loader +// by Tom Seddon +// +// See http://softimage.wiki.softimage.com/index.php/INFO:_PIC_file_format +// See http://ozviz.wasp.uwa.edu.au/~pbourke/dataformats/softimagepic/ + +#ifndef STBI_NO_PIC +static int stbi__pic_is4(stbi__context *s,const char *str) +{ + int i; + for (i=0; i<4; ++i) + if (stbi__get8(s) != (stbi_uc)str[i]) + return 0; + + return 1; +} + +static int stbi__pic_test_core(stbi__context *s) +{ + int i; + + if (!stbi__pic_is4(s,"\x53\x80\xF6\x34")) + return 0; + + for(i=0;i<84;++i) + stbi__get8(s); + + if (!stbi__pic_is4(s,"PICT")) + return 0; + + return 1; +} + +typedef struct +{ + stbi_uc size,type,channel; +} stbi__pic_packet; + +static stbi_uc *stbi__readval(stbi__context *s, int channel, stbi_uc *dest) +{ + int mask=0x80, i; + + for (i=0; i<4; ++i, mask>>=1) { + if (channel & mask) { + if (stbi__at_eof(s)) return stbi__errpuc("bad file","PIC file too short"); + dest[i]=stbi__get8(s); + } + } + + return dest; +} + +static void stbi__copyval(int channel,stbi_uc *dest,const stbi_uc *src) +{ + int mask=0x80,i; + + for (i=0;i<4; ++i, mask>>=1) + if (channel&mask) + dest[i]=src[i]; +} + +static stbi_uc *stbi__pic_load_core(stbi__context *s,int width,int height,int *comp, stbi_uc *result) +{ + int act_comp=0,num_packets=0,y,chained; + stbi__pic_packet packets[10]; + + // this will (should...) cater for even some bizarre stuff like having data + // for the same channel in multiple packets. + do { + stbi__pic_packet *packet; + + if (num_packets==sizeof(packets)/sizeof(packets[0])) + return stbi__errpuc("bad format","too many packets"); + + packet = &packets[num_packets++]; + + chained = stbi__get8(s); + packet->size = stbi__get8(s); + packet->type = stbi__get8(s); + packet->channel = stbi__get8(s); + + act_comp |= packet->channel; + + if (stbi__at_eof(s)) return stbi__errpuc("bad file","file too short (reading packets)"); + if (packet->size != 8) return stbi__errpuc("bad format","packet isn't 8bpp"); + } while (chained); + + *comp = (act_comp & 0x10 ? 4 : 3); // has alpha channel? + + for(y=0; ytype) { + default: + return stbi__errpuc("bad format","packet has bad compression type"); + + case 0: {//uncompressed + int x; + + for(x=0;xchannel,dest)) + return 0; + break; + } + + case 1://Pure RLE + { + int left=width, i; + + while (left>0) { + stbi_uc count,value[4]; + + count=stbi__get8(s); + if (stbi__at_eof(s)) return stbi__errpuc("bad file","file too short (pure read count)"); + + if (count > left) + count = (stbi_uc) left; + + if (!stbi__readval(s,packet->channel,value)) return 0; + + for(i=0; ichannel,dest,value); + left -= count; + } + } + break; + + case 2: {//Mixed RLE + int left=width; + while (left>0) { + int count = stbi__get8(s), i; + if (stbi__at_eof(s)) return stbi__errpuc("bad file","file too short (mixed read count)"); + + if (count >= 128) { // Repeated + stbi_uc value[4]; + + if (count==128) + count = stbi__get16be(s); + else + count -= 127; + if (count > left) + return stbi__errpuc("bad file","scanline overrun"); + + if (!stbi__readval(s,packet->channel,value)) + return 0; + + for(i=0;ichannel,dest,value); + } else { // Raw + ++count; + if (count>left) return stbi__errpuc("bad file","scanline overrun"); + + for(i=0;ichannel,dest)) + return 0; + } + left-=count; + } + break; + } + } + } + } + + return result; +} + +static void *stbi__pic_load(stbi__context *s,int *px,int *py,int *comp,int req_comp, stbi__result_info *ri) +{ + stbi_uc *result; + int i, x,y, internal_comp; + STBI_NOTUSED(ri); + + if (!comp) comp = &internal_comp; + + for (i=0; i<92; ++i) + stbi__get8(s); + + x = stbi__get16be(s); + y = stbi__get16be(s); + + if (y > STBI_MAX_DIMENSIONS) return stbi__errpuc("too large","Very large image (corrupt?)"); + if (x > STBI_MAX_DIMENSIONS) return stbi__errpuc("too large","Very large image (corrupt?)"); + + if (stbi__at_eof(s)) return stbi__errpuc("bad file","file too short (pic header)"); + if (!stbi__mad3sizes_valid(x, y, 4, 0)) return stbi__errpuc("too large", "PIC image too large to decode"); + + stbi__get32be(s); //skip `ratio' + stbi__get16be(s); //skip `fields' + stbi__get16be(s); //skip `pad' + + // intermediate buffer is RGBA + result = (stbi_uc *) stbi__malloc_mad3(x, y, 4, 0); + if (!result) return stbi__errpuc("outofmem", "Out of memory"); + memset(result, 0xff, x*y*4); + + if (!stbi__pic_load_core(s,x,y,comp, result)) { + STBI_FREE(result); + result=0; + } + *px = x; + *py = y; + if (req_comp == 0) req_comp = *comp; + result=stbi__convert_format(result,4,req_comp,x,y); + + return result; +} + +static int stbi__pic_test(stbi__context *s) +{ + int r = stbi__pic_test_core(s); + stbi__rewind(s); + return r; +} +#endif + +// ************************************************************************************************* +// GIF loader -- public domain by Jean-Marc Lienher -- simplified/shrunk by stb + +#ifndef STBI_NO_GIF +typedef struct +{ + stbi__int16 prefix; + stbi_uc first; + stbi_uc suffix; +} stbi__gif_lzw; + +typedef struct +{ + int w,h; + stbi_uc *out; // output buffer (always 4 components) + stbi_uc *background; // The current "background" as far as a gif is concerned + stbi_uc *history; + int flags, bgindex, ratio, transparent, eflags; + stbi_uc pal[256][4]; + stbi_uc lpal[256][4]; + stbi__gif_lzw codes[8192]; + stbi_uc *color_table; + int parse, step; + int lflags; + int start_x, start_y; + int max_x, max_y; + int cur_x, cur_y; + int line_size; + int delay; +} stbi__gif; + +static int stbi__gif_test_raw(stbi__context *s) +{ + int sz; + if (stbi__get8(s) != 'G' || stbi__get8(s) != 'I' || stbi__get8(s) != 'F' || stbi__get8(s) != '8') return 0; + sz = stbi__get8(s); + if (sz != '9' && sz != '7') return 0; + if (stbi__get8(s) != 'a') return 0; + return 1; +} + +static int stbi__gif_test(stbi__context *s) +{ + int r = stbi__gif_test_raw(s); + stbi__rewind(s); + return r; +} + +static void stbi__gif_parse_colortable(stbi__context *s, stbi_uc pal[256][4], int num_entries, int transp) +{ + int i; + for (i=0; i < num_entries; ++i) { + pal[i][2] = stbi__get8(s); + pal[i][1] = stbi__get8(s); + pal[i][0] = stbi__get8(s); + pal[i][3] = transp == i ? 0 : 255; + } +} + +static int stbi__gif_header(stbi__context *s, stbi__gif *g, int *comp, int is_info) +{ + stbi_uc version; + if (stbi__get8(s) != 'G' || stbi__get8(s) != 'I' || stbi__get8(s) != 'F' || stbi__get8(s) != '8') + return stbi__err("not GIF", "Corrupt GIF"); + + version = stbi__get8(s); + if (version != '7' && version != '9') return stbi__err("not GIF", "Corrupt GIF"); + if (stbi__get8(s) != 'a') return stbi__err("not GIF", "Corrupt GIF"); + + stbi__g_failure_reason = ""; + g->w = stbi__get16le(s); + g->h = stbi__get16le(s); + g->flags = stbi__get8(s); + g->bgindex = stbi__get8(s); + g->ratio = stbi__get8(s); + g->transparent = -1; + + if (g->w > STBI_MAX_DIMENSIONS) return stbi__err("too large","Very large image (corrupt?)"); + if (g->h > STBI_MAX_DIMENSIONS) return stbi__err("too large","Very large image (corrupt?)"); + + if (comp != 0) *comp = 4; // can't actually tell whether it's 3 or 4 until we parse the comments + + if (is_info) return 1; + + if (g->flags & 0x80) + stbi__gif_parse_colortable(s,g->pal, 2 << (g->flags & 7), -1); + + return 1; +} + +static int stbi__gif_info_raw(stbi__context *s, int *x, int *y, int *comp) +{ + stbi__gif* g = (stbi__gif*) stbi__malloc(sizeof(stbi__gif)); + if (!g) return stbi__err("outofmem", "Out of memory"); + if (!stbi__gif_header(s, g, comp, 1)) { + STBI_FREE(g); + stbi__rewind( s ); + return 0; + } + if (x) *x = g->w; + if (y) *y = g->h; + STBI_FREE(g); + return 1; +} + +static void stbi__out_gif_code(stbi__gif *g, stbi__uint16 code) +{ + stbi_uc *p, *c; + int idx; + + // recurse to decode the prefixes, since the linked-list is backwards, + // and working backwards through an interleaved image would be nasty + if (g->codes[code].prefix >= 0) + stbi__out_gif_code(g, g->codes[code].prefix); + + if (g->cur_y >= g->max_y) return; + + idx = g->cur_x + g->cur_y; + p = &g->out[idx]; + g->history[idx / 4] = 1; + + c = &g->color_table[g->codes[code].suffix * 4]; + if (c[3] > 128) { // don't render transparent pixels; + p[0] = c[2]; + p[1] = c[1]; + p[2] = c[0]; + p[3] = c[3]; + } + g->cur_x += 4; + + if (g->cur_x >= g->max_x) { + g->cur_x = g->start_x; + g->cur_y += g->step; + + while (g->cur_y >= g->max_y && g->parse > 0) { + g->step = (1 << g->parse) * g->line_size; + g->cur_y = g->start_y + (g->step >> 1); + --g->parse; + } + } +} + +static stbi_uc *stbi__process_gif_raster(stbi__context *s, stbi__gif *g) +{ + stbi_uc lzw_cs; + stbi__int32 len, init_code; + stbi__uint32 first; + stbi__int32 codesize, codemask, avail, oldcode, bits, valid_bits, clear; + stbi__gif_lzw *p; + + lzw_cs = stbi__get8(s); + if (lzw_cs > 12) return NULL; + clear = 1 << lzw_cs; + first = 1; + codesize = lzw_cs + 1; + codemask = (1 << codesize) - 1; + bits = 0; + valid_bits = 0; + for (init_code = 0; init_code < clear; init_code++) { + g->codes[init_code].prefix = -1; + g->codes[init_code].first = (stbi_uc) init_code; + g->codes[init_code].suffix = (stbi_uc) init_code; + } + + // support no starting clear code + avail = clear+2; + oldcode = -1; + + len = 0; + for(;;) { + if (valid_bits < codesize) { + if (len == 0) { + len = stbi__get8(s); // start new block + if (len == 0) + return g->out; + } + --len; + bits |= (stbi__int32) stbi__get8(s) << valid_bits; + valid_bits += 8; + } else { + stbi__int32 code = bits & codemask; + bits >>= codesize; + valid_bits -= codesize; + // @OPTIMIZE: is there some way we can accelerate the non-clear path? + if (code == clear) { // clear code + codesize = lzw_cs + 1; + codemask = (1 << codesize) - 1; + avail = clear + 2; + oldcode = -1; + first = 0; + } else if (code == clear + 1) { // end of stream code + stbi__skip(s, len); + while ((len = stbi__get8(s)) > 0) + stbi__skip(s,len); + return g->out; + } else if (code <= avail) { + if (first) { + return stbi__errpuc("no clear code", "Corrupt GIF"); + } + + if (oldcode >= 0) { + p = &g->codes[avail++]; + if (avail > 8192) { + return stbi__errpuc("too many codes", "Corrupt GIF"); + } + + p->prefix = (stbi__int16) oldcode; + p->first = g->codes[oldcode].first; + p->suffix = (code == avail) ? p->first : g->codes[code].first; + } else if (code == avail) + return stbi__errpuc("illegal code in raster", "Corrupt GIF"); + + stbi__out_gif_code(g, (stbi__uint16) code); + + if ((avail & codemask) == 0 && avail <= 0x0FFF) { + codesize++; + codemask = (1 << codesize) - 1; + } + + oldcode = code; + } else { + return stbi__errpuc("illegal code in raster", "Corrupt GIF"); + } + } + } +} + +// this function is designed to support animated gifs, although stb_image doesn't support it +// two back is the image from two frames ago, used for a very specific disposal format +static stbi_uc *stbi__gif_load_next(stbi__context *s, stbi__gif *g, int *comp, int req_comp, stbi_uc *two_back) +{ + int dispose; + int first_frame; + int pi; + int pcount; + STBI_NOTUSED(req_comp); + + // on first frame, any non-written pixels get the background colour (non-transparent) + first_frame = 0; + if (g->out == 0) { + if (!stbi__gif_header(s, g, comp,0)) return 0; // stbi__g_failure_reason set by stbi__gif_header + if (!stbi__mad3sizes_valid(4, g->w, g->h, 0)) + return stbi__errpuc("too large", "GIF image is too large"); + pcount = g->w * g->h; + g->out = (stbi_uc *) stbi__malloc(4 * pcount); + g->background = (stbi_uc *) stbi__malloc(4 * pcount); + g->history = (stbi_uc *) stbi__malloc(pcount); + if (!g->out || !g->background || !g->history) + return stbi__errpuc("outofmem", "Out of memory"); + + // image is treated as "transparent" at the start - ie, nothing overwrites the current background; + // background colour is only used for pixels that are not rendered first frame, after that "background" + // color refers to the color that was there the previous frame. + memset(g->out, 0x00, 4 * pcount); + memset(g->background, 0x00, 4 * pcount); // state of the background (starts transparent) + memset(g->history, 0x00, pcount); // pixels that were affected previous frame + first_frame = 1; + } else { + // second frame - how do we dispose of the previous one? + dispose = (g->eflags & 0x1C) >> 2; + pcount = g->w * g->h; + + if ((dispose == 3) && (two_back == 0)) { + dispose = 2; // if I don't have an image to revert back to, default to the old background + } + + if (dispose == 3) { // use previous graphic + for (pi = 0; pi < pcount; ++pi) { + if (g->history[pi]) { + memcpy( &g->out[pi * 4], &two_back[pi * 4], 4 ); + } + } + } else if (dispose == 2) { + // restore what was changed last frame to background before that frame; + for (pi = 0; pi < pcount; ++pi) { + if (g->history[pi]) { + memcpy( &g->out[pi * 4], &g->background[pi * 4], 4 ); + } + } + } else { + // This is a non-disposal case eithe way, so just + // leave the pixels as is, and they will become the new background + // 1: do not dispose + // 0: not specified. + } + + // background is what out is after the undoing of the previou frame; + memcpy( g->background, g->out, 4 * g->w * g->h ); + } + + // clear my history; + memset( g->history, 0x00, g->w * g->h ); // pixels that were affected previous frame + + for (;;) { + int tag = stbi__get8(s); + switch (tag) { + case 0x2C: /* Image Descriptor */ + { + stbi__int32 x, y, w, h; + stbi_uc *o; + + x = stbi__get16le(s); + y = stbi__get16le(s); + w = stbi__get16le(s); + h = stbi__get16le(s); + if (((x + w) > (g->w)) || ((y + h) > (g->h))) + return stbi__errpuc("bad Image Descriptor", "Corrupt GIF"); + + g->line_size = g->w * 4; + g->start_x = x * 4; + g->start_y = y * g->line_size; + g->max_x = g->start_x + w * 4; + g->max_y = g->start_y + h * g->line_size; + g->cur_x = g->start_x; + g->cur_y = g->start_y; + + // if the width of the specified rectangle is 0, that means + // we may not see *any* pixels or the image is malformed; + // to make sure this is caught, move the current y down to + // max_y (which is what out_gif_code checks). + if (w == 0) + g->cur_y = g->max_y; + + g->lflags = stbi__get8(s); + + if (g->lflags & 0x40) { + g->step = 8 * g->line_size; // first interlaced spacing + g->parse = 3; + } else { + g->step = g->line_size; + g->parse = 0; + } + + if (g->lflags & 0x80) { + stbi__gif_parse_colortable(s,g->lpal, 2 << (g->lflags & 7), g->eflags & 0x01 ? g->transparent : -1); + g->color_table = (stbi_uc *) g->lpal; + } else if (g->flags & 0x80) { + g->color_table = (stbi_uc *) g->pal; + } else + return stbi__errpuc("missing color table", "Corrupt GIF"); + + o = stbi__process_gif_raster(s, g); + if (!o) return NULL; + + // if this was the first frame, + pcount = g->w * g->h; + if (first_frame && (g->bgindex > 0)) { + // if first frame, any pixel not drawn to gets the background color + for (pi = 0; pi < pcount; ++pi) { + if (g->history[pi] == 0) { + g->pal[g->bgindex][3] = 255; // just in case it was made transparent, undo that; It will be reset next frame if need be; + memcpy( &g->out[pi * 4], &g->pal[g->bgindex], 4 ); + } + } + } + + return o; + } + + case 0x21: // Comment Extension. + { + int len; + int ext = stbi__get8(s); + if (ext == 0xF9) { // Graphic Control Extension. + len = stbi__get8(s); + if (len == 4) { + g->eflags = stbi__get8(s); + g->delay = 10 * stbi__get16le(s); // delay - 1/100th of a second, saving as 1/1000ths. + + // unset old transparent + if (g->transparent >= 0) { + g->pal[g->transparent][3] = 255; + } + if (g->eflags & 0x01) { + g->transparent = stbi__get8(s); + if (g->transparent >= 0) { + g->pal[g->transparent][3] = 0; + } + } else { + // don't need transparent + stbi__skip(s, 1); + g->transparent = -1; + } + } else { + stbi__skip(s, len); + break; + } + } + while ((len = stbi__get8(s)) != 0) { + stbi__skip(s, len); + } + break; + } + + case 0x3B: // gif stream termination code + return (stbi_uc *) s; // using '1' causes warning on some compilers + + default: + return stbi__errpuc("unknown code", "Corrupt GIF"); + } + } +} + +static void *stbi__load_gif_main_outofmem(stbi__gif *g, stbi_uc *out, int **delays) +{ + STBI_FREE(g->out); + STBI_FREE(g->history); + STBI_FREE(g->background); + + if (out) STBI_FREE(out); + if (delays && *delays) STBI_FREE(*delays); + return stbi__errpuc("outofmem", "Out of memory"); +} + +static void *stbi__load_gif_main(stbi__context *s, int **delays, int *x, int *y, int *z, int *comp, int req_comp) +{ + if (stbi__gif_test(s)) { + int layers = 0; + stbi_uc *u = 0; + stbi_uc *out = 0; + stbi_uc *two_back = 0; + stbi__gif g; + int stride; + int out_size = 0; + int delays_size = 0; + + STBI_NOTUSED(out_size); + STBI_NOTUSED(delays_size); + + memset(&g, 0, sizeof(g)); + if (delays) { + *delays = 0; + } + + do { + u = stbi__gif_load_next(s, &g, comp, req_comp, two_back); + if (u == (stbi_uc *) s) u = 0; // end of animated gif marker + + if (u) { + *x = g.w; + *y = g.h; + ++layers; + stride = g.w * g.h * 4; + + if (out) { + void *tmp = (stbi_uc*) STBI_REALLOC_SIZED( out, out_size, layers * stride ); + if (!tmp) + return stbi__load_gif_main_outofmem(&g, out, delays); + else { + out = (stbi_uc*) tmp; + out_size = layers * stride; + } + + if (delays) { + int *new_delays = (int*) STBI_REALLOC_SIZED( *delays, delays_size, sizeof(int) * layers ); + if (!new_delays) + return stbi__load_gif_main_outofmem(&g, out, delays); + *delays = new_delays; + delays_size = layers * sizeof(int); + } + } else { + out = (stbi_uc*)stbi__malloc( layers * stride ); + if (!out) + return stbi__load_gif_main_outofmem(&g, out, delays); + out_size = layers * stride; + if (delays) { + *delays = (int*) stbi__malloc( layers * sizeof(int) ); + if (!*delays) + return stbi__load_gif_main_outofmem(&g, out, delays); + delays_size = layers * sizeof(int); + } + } + memcpy( out + ((layers - 1) * stride), u, stride ); + if (layers >= 2) { + two_back = out - 2 * stride; + } + + if (delays) { + (*delays)[layers - 1U] = g.delay; + } + } + } while (u != 0); + + // free temp buffer; + STBI_FREE(g.out); + STBI_FREE(g.history); + STBI_FREE(g.background); + + // do the final conversion after loading everything; + if (req_comp && req_comp != 4) + out = stbi__convert_format(out, 4, req_comp, layers * g.w, g.h); + + *z = layers; + return out; + } else { + return stbi__errpuc("not GIF", "Image was not as a gif type."); + } +} + +static void *stbi__gif_load(stbi__context *s, int *x, int *y, int *comp, int req_comp, stbi__result_info *ri) +{ + stbi_uc *u = 0; + stbi__gif g; + memset(&g, 0, sizeof(g)); + STBI_NOTUSED(ri); + + u = stbi__gif_load_next(s, &g, comp, req_comp, 0); + if (u == (stbi_uc *) s) u = 0; // end of animated gif marker + if (u) { + *x = g.w; + *y = g.h; + + // moved conversion to after successful load so that the same + // can be done for multiple frames. + if (req_comp && req_comp != 4) + u = stbi__convert_format(u, 4, req_comp, g.w, g.h); + } else if (g.out) { + // if there was an error and we allocated an image buffer, free it! + STBI_FREE(g.out); + } + + // free buffers needed for multiple frame loading; + STBI_FREE(g.history); + STBI_FREE(g.background); + + return u; +} + +static int stbi__gif_info(stbi__context *s, int *x, int *y, int *comp) +{ + return stbi__gif_info_raw(s,x,y,comp); +} +#endif + +// ************************************************************************************************* +// Radiance RGBE HDR loader +// originally by Nicolas Schulz +#ifndef STBI_NO_HDR +static int stbi__hdr_test_core(stbi__context *s, const char *signature) +{ + int i; + for (i=0; signature[i]; ++i) + if (stbi__get8(s) != signature[i]) + return 0; + stbi__rewind(s); + return 1; +} + +static int stbi__hdr_test(stbi__context* s) +{ + int r = stbi__hdr_test_core(s, "#?RADIANCE\n"); + stbi__rewind(s); + if(!r) { + r = stbi__hdr_test_core(s, "#?RGBE\n"); + stbi__rewind(s); + } + return r; +} + +#define STBI__HDR_BUFLEN 1024 +static char *stbi__hdr_gettoken(stbi__context *z, char *buffer) +{ + int len=0; + char c = '\0'; + + c = (char) stbi__get8(z); + + while (!stbi__at_eof(z) && c != '\n') { + buffer[len++] = c; + if (len == STBI__HDR_BUFLEN-1) { + // flush to end of line + while (!stbi__at_eof(z) && stbi__get8(z) != '\n') + ; + break; + } + c = (char) stbi__get8(z); + } + + buffer[len] = 0; + return buffer; +} + +static void stbi__hdr_convert(float *output, stbi_uc *input, int req_comp) +{ + if ( input[3] != 0 ) { + float f1; + // Exponent + f1 = (float) ldexp(1.0f, input[3] - (int)(128 + 8)); + if (req_comp <= 2) + output[0] = (input[0] + input[1] + input[2]) * f1 / 3; + else { + output[0] = input[0] * f1; + output[1] = input[1] * f1; + output[2] = input[2] * f1; + } + if (req_comp == 2) output[1] = 1; + if (req_comp == 4) output[3] = 1; + } else { + switch (req_comp) { + case 4: output[3] = 1; /* fallthrough */ + case 3: output[0] = output[1] = output[2] = 0; + break; + case 2: output[1] = 1; /* fallthrough */ + case 1: output[0] = 0; + break; + } + } +} + +static float *stbi__hdr_load(stbi__context *s, int *x, int *y, int *comp, int req_comp, stbi__result_info *ri) +{ + char buffer[STBI__HDR_BUFLEN]; + char *token; + int valid = 0; + int width, height; + stbi_uc *scanline; + float *hdr_data; + int len; + unsigned char count, value; + int i, j, k, c1,c2, z; + const char *headerToken; + STBI_NOTUSED(ri); + + // Check identifier + headerToken = stbi__hdr_gettoken(s,buffer); + if (strcmp(headerToken, "#?RADIANCE") != 0 && strcmp(headerToken, "#?RGBE") != 0) + return stbi__errpf("not HDR", "Corrupt HDR image"); + + // Parse header + for(;;) { + token = stbi__hdr_gettoken(s,buffer); + if (token[0] == 0) break; + if (strcmp(token, "FORMAT=32-bit_rle_rgbe") == 0) valid = 1; + } + + if (!valid) return stbi__errpf("unsupported format", "Unsupported HDR format"); + + // Parse width and height + // can't use sscanf() if we're not using stdio! + token = stbi__hdr_gettoken(s,buffer); + if (strncmp(token, "-Y ", 3)) return stbi__errpf("unsupported data layout", "Unsupported HDR format"); + token += 3; + height = (int) strtol(token, &token, 10); + while (*token == ' ') ++token; + if (strncmp(token, "+X ", 3)) return stbi__errpf("unsupported data layout", "Unsupported HDR format"); + token += 3; + width = (int) strtol(token, NULL, 10); + + if (height > STBI_MAX_DIMENSIONS) return stbi__errpf("too large","Very large image (corrupt?)"); + if (width > STBI_MAX_DIMENSIONS) return stbi__errpf("too large","Very large image (corrupt?)"); + + *x = width; + *y = height; + + if (comp) *comp = 3; + if (req_comp == 0) req_comp = 3; + + if (!stbi__mad4sizes_valid(width, height, req_comp, sizeof(float), 0)) + return stbi__errpf("too large", "HDR image is too large"); + + // Read data + hdr_data = (float *) stbi__malloc_mad4(width, height, req_comp, sizeof(float), 0); + if (!hdr_data) + return stbi__errpf("outofmem", "Out of memory"); + + // Load image data + // image data is stored as some number of sca + if ( width < 8 || width >= 32768) { + // Read flat data + for (j=0; j < height; ++j) { + for (i=0; i < width; ++i) { + stbi_uc rgbe[4]; + main_decode_loop: + stbi__getn(s, rgbe, 4); + stbi__hdr_convert(hdr_data + j * width * req_comp + i * req_comp, rgbe, req_comp); + } + } + } else { + // Read RLE-encoded data + scanline = NULL; + + for (j = 0; j < height; ++j) { + c1 = stbi__get8(s); + c2 = stbi__get8(s); + len = stbi__get8(s); + if (c1 != 2 || c2 != 2 || (len & 0x80)) { + // not run-length encoded, so we have to actually use THIS data as a decoded + // pixel (note this can't be a valid pixel--one of RGB must be >= 128) + stbi_uc rgbe[4]; + rgbe[0] = (stbi_uc) c1; + rgbe[1] = (stbi_uc) c2; + rgbe[2] = (stbi_uc) len; + rgbe[3] = (stbi_uc) stbi__get8(s); + stbi__hdr_convert(hdr_data, rgbe, req_comp); + i = 1; + j = 0; + STBI_FREE(scanline); + goto main_decode_loop; // yes, this makes no sense + } + len <<= 8; + len |= stbi__get8(s); + if (len != width) { STBI_FREE(hdr_data); STBI_FREE(scanline); return stbi__errpf("invalid decoded scanline length", "corrupt HDR"); } + if (scanline == NULL) { + scanline = (stbi_uc *) stbi__malloc_mad2(width, 4, 0); + if (!scanline) { + STBI_FREE(hdr_data); + return stbi__errpf("outofmem", "Out of memory"); + } + } + + for (k = 0; k < 4; ++k) { + int nleft; + i = 0; + while ((nleft = width - i) > 0) { + count = stbi__get8(s); + if (count > 128) { + // Run + value = stbi__get8(s); + count -= 128; + if ((count == 0) || (count > nleft)) { STBI_FREE(hdr_data); STBI_FREE(scanline); return stbi__errpf("corrupt", "bad RLE data in HDR"); } + for (z = 0; z < count; ++z) + scanline[i++ * 4 + k] = value; + } else { + // Dump + if ((count == 0) || (count > nleft)) { STBI_FREE(hdr_data); STBI_FREE(scanline); return stbi__errpf("corrupt", "bad RLE data in HDR"); } + for (z = 0; z < count; ++z) + scanline[i++ * 4 + k] = stbi__get8(s); + } + } + } + for (i=0; i < width; ++i) + stbi__hdr_convert(hdr_data+(j*width + i)*req_comp, scanline + i*4, req_comp); + } + if (scanline) + STBI_FREE(scanline); + } + + return hdr_data; +} + +static int stbi__hdr_info(stbi__context *s, int *x, int *y, int *comp) +{ + char buffer[STBI__HDR_BUFLEN]; + char *token; + int valid = 0; + int dummy; + + if (!x) x = &dummy; + if (!y) y = &dummy; + if (!comp) comp = &dummy; + + if (stbi__hdr_test(s) == 0) { + stbi__rewind( s ); + return 0; + } + + for(;;) { + token = stbi__hdr_gettoken(s,buffer); + if (token[0] == 0) break; + if (strcmp(token, "FORMAT=32-bit_rle_rgbe") == 0) valid = 1; + } + + if (!valid) { + stbi__rewind( s ); + return 0; + } + token = stbi__hdr_gettoken(s,buffer); + if (strncmp(token, "-Y ", 3)) { + stbi__rewind( s ); + return 0; + } + token += 3; + *y = (int) strtol(token, &token, 10); + while (*token == ' ') ++token; + if (strncmp(token, "+X ", 3)) { + stbi__rewind( s ); + return 0; + } + token += 3; + *x = (int) strtol(token, NULL, 10); + *comp = 3; + return 1; +} +#endif // STBI_NO_HDR + +#ifndef STBI_NO_BMP +static int stbi__bmp_info(stbi__context *s, int *x, int *y, int *comp) +{ + void *p; + stbi__bmp_data info; + + info.all_a = 255; + p = stbi__bmp_parse_header(s, &info); + if (p == NULL) { + stbi__rewind( s ); + return 0; + } + if (x) *x = s->img_x; + if (y) *y = s->img_y; + if (comp) { + if (info.bpp == 24 && info.ma == 0xff000000) + *comp = 3; + else + *comp = info.ma ? 4 : 3; + } + return 1; +} +#endif + +#ifndef STBI_NO_PSD +static int stbi__psd_info(stbi__context *s, int *x, int *y, int *comp) +{ + int channelCount, dummy, depth; + if (!x) x = &dummy; + if (!y) y = &dummy; + if (!comp) comp = &dummy; + if (stbi__get32be(s) != 0x38425053) { + stbi__rewind( s ); + return 0; + } + if (stbi__get16be(s) != 1) { + stbi__rewind( s ); + return 0; + } + stbi__skip(s, 6); + channelCount = stbi__get16be(s); + if (channelCount < 0 || channelCount > 16) { + stbi__rewind( s ); + return 0; + } + *y = stbi__get32be(s); + *x = stbi__get32be(s); + depth = stbi__get16be(s); + if (depth != 8 && depth != 16) { + stbi__rewind( s ); + return 0; + } + if (stbi__get16be(s) != 3) { + stbi__rewind( s ); + return 0; + } + *comp = 4; + return 1; +} + +static int stbi__psd_is16(stbi__context *s) +{ + int channelCount, depth; + if (stbi__get32be(s) != 0x38425053) { + stbi__rewind( s ); + return 0; + } + if (stbi__get16be(s) != 1) { + stbi__rewind( s ); + return 0; + } + stbi__skip(s, 6); + channelCount = stbi__get16be(s); + if (channelCount < 0 || channelCount > 16) { + stbi__rewind( s ); + return 0; + } + STBI_NOTUSED(stbi__get32be(s)); + STBI_NOTUSED(stbi__get32be(s)); + depth = stbi__get16be(s); + if (depth != 16) { + stbi__rewind( s ); + return 0; + } + return 1; +} +#endif + +#ifndef STBI_NO_PIC +static int stbi__pic_info(stbi__context *s, int *x, int *y, int *comp) +{ + int act_comp=0,num_packets=0,chained,dummy; + stbi__pic_packet packets[10]; + + if (!x) x = &dummy; + if (!y) y = &dummy; + if (!comp) comp = &dummy; + + if (!stbi__pic_is4(s,"\x53\x80\xF6\x34")) { + stbi__rewind(s); + return 0; + } + + stbi__skip(s, 88); + + *x = stbi__get16be(s); + *y = stbi__get16be(s); + if (stbi__at_eof(s)) { + stbi__rewind( s); + return 0; + } + if ( (*x) != 0 && (1 << 28) / (*x) < (*y)) { + stbi__rewind( s ); + return 0; + } + + stbi__skip(s, 8); + + do { + stbi__pic_packet *packet; + + if (num_packets==sizeof(packets)/sizeof(packets[0])) + return 0; + + packet = &packets[num_packets++]; + chained = stbi__get8(s); + packet->size = stbi__get8(s); + packet->type = stbi__get8(s); + packet->channel = stbi__get8(s); + act_comp |= packet->channel; + + if (stbi__at_eof(s)) { + stbi__rewind( s ); + return 0; + } + if (packet->size != 8) { + stbi__rewind( s ); + return 0; + } + } while (chained); + + *comp = (act_comp & 0x10 ? 4 : 3); + + return 1; +} +#endif + +// ************************************************************************************************* +// Portable Gray Map and Portable Pixel Map loader +// by Ken Miller +// +// PGM: http://netpbm.sourceforge.net/doc/pgm.html +// PPM: http://netpbm.sourceforge.net/doc/ppm.html +// +// Known limitations: +// Does not support comments in the header section +// Does not support ASCII image data (formats P2 and P3) + +#ifndef STBI_NO_PNM + +static int stbi__pnm_test(stbi__context *s) +{ + char p, t; + p = (char) stbi__get8(s); + t = (char) stbi__get8(s); + if (p != 'P' || (t != '5' && t != '6')) { + stbi__rewind( s ); + return 0; + } + return 1; +} + +static void *stbi__pnm_load(stbi__context *s, int *x, int *y, int *comp, int req_comp, stbi__result_info *ri) +{ + stbi_uc *out; + STBI_NOTUSED(ri); + + ri->bits_per_channel = stbi__pnm_info(s, (int *)&s->img_x, (int *)&s->img_y, (int *)&s->img_n); + if (ri->bits_per_channel == 0) + return 0; + + if (s->img_y > STBI_MAX_DIMENSIONS) return stbi__errpuc("too large","Very large image (corrupt?)"); + if (s->img_x > STBI_MAX_DIMENSIONS) return stbi__errpuc("too large","Very large image (corrupt?)"); + + *x = s->img_x; + *y = s->img_y; + if (comp) *comp = s->img_n; + + if (!stbi__mad4sizes_valid(s->img_n, s->img_x, s->img_y, ri->bits_per_channel / 8, 0)) + return stbi__errpuc("too large", "PNM too large"); + + out = (stbi_uc *) stbi__malloc_mad4(s->img_n, s->img_x, s->img_y, ri->bits_per_channel / 8, 0); + if (!out) return stbi__errpuc("outofmem", "Out of memory"); + if (!stbi__getn(s, out, s->img_n * s->img_x * s->img_y * (ri->bits_per_channel / 8))) { + STBI_FREE(out); + return stbi__errpuc("bad PNM", "PNM file truncated"); + } + + if (req_comp && req_comp != s->img_n) { + if (ri->bits_per_channel == 16) { + out = (stbi_uc *) stbi__convert_format16((stbi__uint16 *) out, s->img_n, req_comp, s->img_x, s->img_y); + } else { + out = stbi__convert_format(out, s->img_n, req_comp, s->img_x, s->img_y); + } + if (out == NULL) return out; // stbi__convert_format frees input on failure + } + return out; +} + +static int stbi__pnm_isspace(char c) +{ + return c == ' ' || c == '\t' || c == '\n' || c == '\v' || c == '\f' || c == '\r'; +} + +static void stbi__pnm_skip_whitespace(stbi__context *s, char *c) +{ + for (;;) { + while (!stbi__at_eof(s) && stbi__pnm_isspace(*c)) + *c = (char) stbi__get8(s); + + if (stbi__at_eof(s) || *c != '#') + break; + + while (!stbi__at_eof(s) && *c != '\n' && *c != '\r' ) + *c = (char) stbi__get8(s); + } +} + +static int stbi__pnm_isdigit(char c) +{ + return c >= '0' && c <= '9'; +} + +static int stbi__pnm_getinteger(stbi__context *s, char *c) +{ + int value = 0; + + while (!stbi__at_eof(s) && stbi__pnm_isdigit(*c)) { + value = value*10 + (*c - '0'); + *c = (char) stbi__get8(s); + if((value > 214748364) || (value == 214748364 && *c > '7')) + return stbi__err("integer parse overflow", "Parsing an integer in the PPM header overflowed a 32-bit int"); + } + + return value; +} + +static int stbi__pnm_info(stbi__context *s, int *x, int *y, int *comp) +{ + int maxv, dummy; + char c, p, t; + + if (!x) x = &dummy; + if (!y) y = &dummy; + if (!comp) comp = &dummy; + + stbi__rewind(s); + + // Get identifier + p = (char) stbi__get8(s); + t = (char) stbi__get8(s); + if (p != 'P' || (t != '5' && t != '6')) { + stbi__rewind(s); + return 0; + } + + *comp = (t == '6') ? 3 : 1; // '5' is 1-component .pgm; '6' is 3-component .ppm + + c = (char) stbi__get8(s); + stbi__pnm_skip_whitespace(s, &c); + + *x = stbi__pnm_getinteger(s, &c); // read width + if(*x == 0) + return stbi__err("invalid width", "PPM image header had zero or overflowing width"); + stbi__pnm_skip_whitespace(s, &c); + + *y = stbi__pnm_getinteger(s, &c); // read height + if (*y == 0) + return stbi__err("invalid width", "PPM image header had zero or overflowing width"); + stbi__pnm_skip_whitespace(s, &c); + + maxv = stbi__pnm_getinteger(s, &c); // read max value + if (maxv > 65535) + return stbi__err("max value > 65535", "PPM image supports only 8-bit and 16-bit images"); + else if (maxv > 255) + return 16; + else + return 8; +} + +static int stbi__pnm_is16(stbi__context *s) +{ + if (stbi__pnm_info(s, NULL, NULL, NULL) == 16) + return 1; + return 0; +} +#endif + +static int stbi__info_main(stbi__context *s, int *x, int *y, int *comp) +{ + #ifndef STBI_NO_JPEG + if (stbi__jpeg_info(s, x, y, comp)) return 1; + #endif + + #ifndef STBI_NO_PNG + if (stbi__png_info(s, x, y, comp)) return 1; + #endif + + #ifndef STBI_NO_GIF + if (stbi__gif_info(s, x, y, comp)) return 1; + #endif + + #ifndef STBI_NO_BMP + if (stbi__bmp_info(s, x, y, comp)) return 1; + #endif + + #ifndef STBI_NO_PSD + if (stbi__psd_info(s, x, y, comp)) return 1; + #endif + + #ifndef STBI_NO_PIC + if (stbi__pic_info(s, x, y, comp)) return 1; + #endif + + #ifndef STBI_NO_PNM + if (stbi__pnm_info(s, x, y, comp)) return 1; + #endif + + #ifndef STBI_NO_HDR + if (stbi__hdr_info(s, x, y, comp)) return 1; + #endif + + // test tga last because it's a crappy test! + #ifndef STBI_NO_TGA + if (stbi__tga_info(s, x, y, comp)) + return 1; + #endif + return stbi__err("unknown image type", "Image not of any known type, or corrupt"); +} + +static int stbi__is_16_main(stbi__context *s) +{ + #ifndef STBI_NO_PNG + if (stbi__png_is16(s)) return 1; + #endif + + #ifndef STBI_NO_PSD + if (stbi__psd_is16(s)) return 1; + #endif + + #ifndef STBI_NO_PNM + if (stbi__pnm_is16(s)) return 1; + #endif + return 0; +} + +#ifndef STBI_NO_STDIO +STBIDEF int stbi_info(char const *filename, int *x, int *y, int *comp) +{ + FILE *f = stbi__fopen(filename, "rb"); + int result; + if (!f) return stbi__err("can't fopen", "Unable to open file"); + result = stbi_info_from_file(f, x, y, comp); + fclose(f); + return result; +} + +STBIDEF int stbi_info_from_file(FILE *f, int *x, int *y, int *comp) +{ + int r; + stbi__context s; + long pos = ftell(f); + stbi__start_file(&s, f); + r = stbi__info_main(&s,x,y,comp); + fseek(f,pos,SEEK_SET); + return r; +} + +STBIDEF int stbi_is_16_bit(char const *filename) +{ + FILE *f = stbi__fopen(filename, "rb"); + int result; + if (!f) return stbi__err("can't fopen", "Unable to open file"); + result = stbi_is_16_bit_from_file(f); + fclose(f); + return result; +} + +STBIDEF int stbi_is_16_bit_from_file(FILE *f) +{ + int r; + stbi__context s; + long pos = ftell(f); + stbi__start_file(&s, f); + r = stbi__is_16_main(&s); + fseek(f,pos,SEEK_SET); + return r; +} +#endif // !STBI_NO_STDIO + +STBIDEF int stbi_info_from_memory(stbi_uc const *buffer, int len, int *x, int *y, int *comp) +{ + stbi__context s; + stbi__start_mem(&s,buffer,len); + return stbi__info_main(&s,x,y,comp); +} + +STBIDEF int stbi_info_from_callbacks(stbi_io_callbacks const *c, void *user, int *x, int *y, int *comp) +{ + stbi__context s; + stbi__start_callbacks(&s, (stbi_io_callbacks *) c, user); + return stbi__info_main(&s,x,y,comp); +} + +STBIDEF int stbi_is_16_bit_from_memory(stbi_uc const *buffer, int len) +{ + stbi__context s; + stbi__start_mem(&s,buffer,len); + return stbi__is_16_main(&s); +} + +STBIDEF int stbi_is_16_bit_from_callbacks(stbi_io_callbacks const *c, void *user) +{ + stbi__context s; + stbi__start_callbacks(&s, (stbi_io_callbacks *) c, user); + return stbi__is_16_main(&s); +} + +#endif // STB_IMAGE_IMPLEMENTATION + +/* + revision history: + 2.20 (2019-02-07) support utf8 filenames in Windows; fix warnings and platform ifdefs + 2.19 (2018-02-11) fix warning + 2.18 (2018-01-30) fix warnings + 2.17 (2018-01-29) change sbti__shiftsigned to avoid clang -O2 bug + 1-bit BMP + *_is_16_bit api + avoid warnings + 2.16 (2017-07-23) all functions have 16-bit variants; + STBI_NO_STDIO works again; + compilation fixes; + fix rounding in unpremultiply; + optimize vertical flip; + disable raw_len validation; + documentation fixes + 2.15 (2017-03-18) fix png-1,2,4 bug; now all Imagenet JPGs decode; + warning fixes; disable run-time SSE detection on gcc; + uniform handling of optional "return" values; + thread-safe initialization of zlib tables + 2.14 (2017-03-03) remove deprecated STBI_JPEG_OLD; fixes for Imagenet JPGs + 2.13 (2016-11-29) add 16-bit API, only supported for PNG right now + 2.12 (2016-04-02) fix typo in 2.11 PSD fix that caused crashes + 2.11 (2016-04-02) allocate large structures on the stack + remove white matting for transparent PSD + fix reported channel count for PNG & BMP + re-enable SSE2 in non-gcc 64-bit + support RGB-formatted JPEG + read 16-bit PNGs (only as 8-bit) + 2.10 (2016-01-22) avoid warning introduced in 2.09 by STBI_REALLOC_SIZED + 2.09 (2016-01-16) allow comments in PNM files + 16-bit-per-pixel TGA (not bit-per-component) + info() for TGA could break due to .hdr handling + info() for BMP to shares code instead of sloppy parse + can use STBI_REALLOC_SIZED if allocator doesn't support realloc + code cleanup + 2.08 (2015-09-13) fix to 2.07 cleanup, reading RGB PSD as RGBA + 2.07 (2015-09-13) fix compiler warnings + partial animated GIF support + limited 16-bpc PSD support + #ifdef unused functions + bug with < 92 byte PIC,PNM,HDR,TGA + 2.06 (2015-04-19) fix bug where PSD returns wrong '*comp' value + 2.05 (2015-04-19) fix bug in progressive JPEG handling, fix warning + 2.04 (2015-04-15) try to re-enable SIMD on MinGW 64-bit + 2.03 (2015-04-12) extra corruption checking (mmozeiko) + stbi_set_flip_vertically_on_load (nguillemot) + fix NEON support; fix mingw support + 2.02 (2015-01-19) fix incorrect assert, fix warning + 2.01 (2015-01-17) fix various warnings; suppress SIMD on gcc 32-bit without -msse2 + 2.00b (2014-12-25) fix STBI_MALLOC in progressive JPEG + 2.00 (2014-12-25) optimize JPG, including x86 SSE2 & NEON SIMD (ryg) + progressive JPEG (stb) + PGM/PPM support (Ken Miller) + STBI_MALLOC,STBI_REALLOC,STBI_FREE + GIF bugfix -- seemingly never worked + STBI_NO_*, STBI_ONLY_* + 1.48 (2014-12-14) fix incorrectly-named assert() + 1.47 (2014-12-14) 1/2/4-bit PNG support, both direct and paletted (Omar Cornut & stb) + optimize PNG (ryg) + fix bug in interlaced PNG with user-specified channel count (stb) + 1.46 (2014-08-26) + fix broken tRNS chunk (colorkey-style transparency) in non-paletted PNG + 1.45 (2014-08-16) + fix MSVC-ARM internal compiler error by wrapping malloc + 1.44 (2014-08-07) + various warning fixes from Ronny Chevalier + 1.43 (2014-07-15) + fix MSVC-only compiler problem in code changed in 1.42 + 1.42 (2014-07-09) + don't define _CRT_SECURE_NO_WARNINGS (affects user code) + fixes to stbi__cleanup_jpeg path + added STBI_ASSERT to avoid requiring assert.h + 1.41 (2014-06-25) + fix search&replace from 1.36 that messed up comments/error messages + 1.40 (2014-06-22) + fix gcc struct-initialization warning + 1.39 (2014-06-15) + fix to TGA optimization when req_comp != number of components in TGA; + fix to GIF loading because BMP wasn't rewinding (whoops, no GIFs in my test suite) + add support for BMP version 5 (more ignored fields) + 1.38 (2014-06-06) + suppress MSVC warnings on integer casts truncating values + fix accidental rename of 'skip' field of I/O + 1.37 (2014-06-04) + remove duplicate typedef + 1.36 (2014-06-03) + convert to header file single-file library + if de-iphone isn't set, load iphone images color-swapped instead of returning NULL + 1.35 (2014-05-27) + various warnings + fix broken STBI_SIMD path + fix bug where stbi_load_from_file no longer left file pointer in correct place + fix broken non-easy path for 32-bit BMP (possibly never used) + TGA optimization by Arseny Kapoulkine + 1.34 (unknown) + use STBI_NOTUSED in stbi__resample_row_generic(), fix one more leak in tga failure case + 1.33 (2011-07-14) + make stbi_is_hdr work in STBI_NO_HDR (as specified), minor compiler-friendly improvements + 1.32 (2011-07-13) + support for "info" function for all supported filetypes (SpartanJ) + 1.31 (2011-06-20) + a few more leak fixes, bug in PNG handling (SpartanJ) + 1.30 (2011-06-11) + added ability to load files via callbacks to accomidate custom input streams (Ben Wenger) + removed deprecated format-specific test/load functions + removed support for installable file formats (stbi_loader) -- would have been broken for IO callbacks anyway + error cases in bmp and tga give messages and don't leak (Raymond Barbiero, grisha) + fix inefficiency in decoding 32-bit BMP (David Woo) + 1.29 (2010-08-16) + various warning fixes from Aurelien Pocheville + 1.28 (2010-08-01) + fix bug in GIF palette transparency (SpartanJ) + 1.27 (2010-08-01) + cast-to-stbi_uc to fix warnings + 1.26 (2010-07-24) + fix bug in file buffering for PNG reported by SpartanJ + 1.25 (2010-07-17) + refix trans_data warning (Won Chun) + 1.24 (2010-07-12) + perf improvements reading from files on platforms with lock-heavy fgetc() + minor perf improvements for jpeg + deprecated type-specific functions so we'll get feedback if they're needed + attempt to fix trans_data warning (Won Chun) + 1.23 fixed bug in iPhone support + 1.22 (2010-07-10) + removed image *writing* support + stbi_info support from Jetro Lauha + GIF support from Jean-Marc Lienher + iPhone PNG-extensions from James Brown + warning-fixes from Nicolas Schulz and Janez Zemva (i.stbi__err. Janez (U+017D)emva) + 1.21 fix use of 'stbi_uc' in header (reported by jon blow) + 1.20 added support for Softimage PIC, by Tom Seddon + 1.19 bug in interlaced PNG corruption check (found by ryg) + 1.18 (2008-08-02) + fix a threading bug (local mutable static) + 1.17 support interlaced PNG + 1.16 major bugfix - stbi__convert_format converted one too many pixels + 1.15 initialize some fields for thread safety + 1.14 fix threadsafe conversion bug + header-file-only version (#define STBI_HEADER_FILE_ONLY before including) + 1.13 threadsafe + 1.12 const qualifiers in the API + 1.11 Support installable IDCT, colorspace conversion routines + 1.10 Fixes for 64-bit (don't use "unsigned long") + optimized upsampling by Fabian "ryg" Giesen + 1.09 Fix format-conversion for PSD code (bad global variables!) + 1.08 Thatcher Ulrich's PSD code integrated by Nicolas Schulz + 1.07 attempt to fix C++ warning/errors again + 1.06 attempt to fix C++ warning/errors again + 1.05 fix TGA loading to return correct *comp and use good luminance calc + 1.04 default float alpha is 1, not 255; use 'void *' for stbi_image_free + 1.03 bugfixes to STBI_NO_STDIO, STBI_NO_HDR + 1.02 support for (subset of) HDR files, float interface for preferred access to them + 1.01 fix bug: possible bug in handling right-side up bmps... not sure + fix bug: the stbi__bmp_load() and stbi__tga_load() functions didn't work at all + 1.00 interface to zlib that skips zlib header + 0.99 correct handling of alpha in palette + 0.98 TGA loader by lonesock; dynamically add loaders (untested) + 0.97 jpeg errors on too large a file; also catch another malloc failure + 0.96 fix detection of invalid v value - particleman@mollyrocket forum + 0.95 during header scan, seek to markers in case of padding + 0.94 STBI_NO_STDIO to disable stdio usage; rename all #defines the same + 0.93 handle jpegtran output; verbose errors + 0.92 read 4,8,16,24,32-bit BMP files of several formats + 0.91 output 24-bit Windows 3.0 BMP files + 0.90 fix a few more warnings; bump version number to approach 1.0 + 0.61 bugfixes due to Marc LeBlanc, Christopher Lloyd + 0.60 fix compiling as c++ + 0.59 fix warnings: merge Dave Moore's -Wall fixes + 0.58 fix bug: zlib uncompressed mode len/nlen was wrong endian + 0.57 fix bug: jpg last huffman symbol before marker was >9 bits but less than 16 available + 0.56 fix bug: zlib uncompressed mode len vs. nlen + 0.55 fix bug: restart_interval not initialized to 0 + 0.54 allow NULL for 'int *comp' + 0.53 fix bug in png 3->4; speedup png decoding + 0.52 png handles req_comp=3,4 directly; minor cleanup; jpeg comments + 0.51 obey req_comp requests, 1-component jpegs return as 1-component, + on 'test' only check type, not whether we support this variant + 0.50 (2006-11-19) + first released version +*/ + + +/* +------------------------------------------------------------------------------ +This software is available under 2 licenses -- choose whichever you prefer. +------------------------------------------------------------------------------ +ALTERNATIVE A - MIT License +Copyright (c) 2017 Sean Barrett +Permission is hereby granted, free of charge, to any person obtaining a copy of +this software and associated documentation files (the "Software"), to deal in +the Software without restriction, including without limitation the rights to +use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies +of the Software, and to permit persons to whom the Software is furnished to do +so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +------------------------------------------------------------------------------ +ALTERNATIVE B - Public Domain (www.unlicense.org) +This is free and unencumbered software released into the public domain. +Anyone is free to copy, modify, publish, use, compile, sell, or distribute this +software, either in source code form or as a compiled binary, for any purpose, +commercial or non-commercial, and by any means. +In jurisdictions that recognize copyright laws, the author or authors of this +software dedicate any and all copyright interest in the software to the public +domain. We make this dedication for the benefit of the public at large and to +the detriment of our heirs and successors. We intend this dedication to be an +overt act of relinquishment in perpetuity of all present and future rights to +this software under copyright law. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN +ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION +WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +------------------------------------------------------------------------------ +*/ diff --git a/include/box3d/base.h b/include/box3d/base.h new file mode 100644 index 0000000..4f16490 --- /dev/null +++ b/include/box3d/base.h @@ -0,0 +1,169 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +// clang-format off +// +// Shared library macros +#if defined(_WIN32) && defined(box3d_EXPORTS) + // build the Windows DLL + #define BOX3D_EXPORT __declspec(dllexport) +#elif defined(_WIN32) && defined(BOX3D_DLL) + // using the Windows DLL + #define BOX3D_EXPORT __declspec(dllimport) +#elif defined(box3d_EXPORTS) + // building or using the shared library + #define BOX3D_EXPORT __attribute__((visibility("default"))) +#else + // static library + #define BOX3D_EXPORT +#endif + +// C++ macros +#ifdef __cplusplus + #define B3_API extern "C" BOX3D_EXPORT + #define B3_INLINE inline + +#if defined( _MSC_VER ) + #define B3_FORCE_INLINE __forceinline +#elif defined( __GNUC__ ) || defined( __clang__ ) + #define B3_FORCE_INLINE inline __attribute__((always_inline)) +#else + #define B3_FORCE_INLINE inline +#endif + + #define B3_LITERAL(T) T + #define B3_ZERO_INIT {} +#else + #define B3_API BOX3D_EXPORT + #define B3_INLINE static inline + +#if defined( _MSC_VER ) + #define B3_FORCE_INLINE static __forceinline +#elif defined( __GNUC__ ) || defined( __clang__ ) + #define B3_FORCE_INLINE static inline __attribute__((always_inline)) +#else + #define B3_FORCE_INLINE static inline +#endif + +/// Used for C literals like (b3Vec3){1.0f, 2.0f, 3.0f} where C++ requires b3Vec3{1.0f, 2.0f, 3.0f} + #define B3_LITERAL(T) (T) + #define B3_ZERO_INIT {0} +#endif +// clang-format on + +#if defined( BOX3D_VALIDATE ) && !defined( NDEBUG ) +#define B3_ENABLE_VALIDATION 1 +#else +#define B3_ENABLE_VALIDATION 0 +#endif + +/** + * @defgroup base Base + * Base functionality + * @{ + */ + +/// This is used to indicate null for interfaces that work with indices instead of pointers +#define B3_NULL_INDEX -1 + +/// Prototype for user allocation function. +/// @param size the allocation size in bytes +/// @param alignment the required alignment, guaranteed to be a power of 2 +typedef void* b3AllocFcn( int32_t size, int32_t alignment ); + +/// Prototype for user free function. +/// @param mem the memory previously allocated through `b3AllocFcn` +typedef void b3FreeFcn( void* mem ); + +/// Prototype for the user assert callback. Return 0 to skip the debugger break. +typedef int b3AssertFcn( const char* condition, const char* fileName, int lineNumber ); + +/// Prototype for user log callback. Used to log warnings. +typedef void b3LogFcn( const char* message ); + +/// This allows the user to override the allocation functions. These should be +/// set during application startup. +B3_API void b3SetAllocator( b3AllocFcn* allocFcn, b3FreeFcn* freeFcn ); + +/// Total bytes allocated by Box3D +B3_API int32_t b3GetByteCount( void ); + +/// Override the default assert callback. +/// @param assertFcn a non-null assert callback +B3_API void b3SetAssertFcn( b3AssertFcn* assertFcn ); + +// see https://github.com/scottt/debugbreak +#if defined( _MSC_VER ) +#define B3_BREAKPOINT __debugbreak() +#elif defined( __GNUC__ ) || defined( __clang__ ) +#define B3_BREAKPOINT __builtin_trap() +#else +// Unknown compiler +#include +#define B3_BREAKPOINT assert( 0 ) +#endif + +#if !defined( NDEBUG ) || defined( B3_ENABLE_ASSERT ) +B3_API int b3InternalAssert( const char* condition, const char* fileName, int lineNumber ); +#define B3_ASSERT( condition ) \ + ( (void)( ( !!( condition ) ) || ( b3InternalAssert( #condition, __FILE__, (int)( __LINE__ ) ), 0 ) ) ) +#else +#define B3_ASSERT( ... ) ( (void)0 ) +#endif + +#if B3_ENABLE_VALIDATION +// Floating point tolerance checks should use this instead of the regular assertion +#define B3_VALIDATE( condition ) B3_ASSERT( condition ) +#else +#define B3_VALIDATE( ... ) ( (void)0 ) +#endif + +/// Override the default logging callback. +B3_API void b3SetLogFcn( b3LogFcn* logFcn ); + +/// Version numbering scheme. +/// See https://semver.org/ +typedef struct b3Version +{ + /// Significant changes + int major; + + /// Incremental changes + int minor; + + /// Bug fixes + int revision; +} b3Version; + +/// Get the current version of Box2D +B3_API b3Version b3GetVersion( void ); + +/**@}*/ + +//! @cond + +/// Get the absolute number of system ticks. The value is platform specific. +B3_API uint64_t b3GetTicks( void ); + +/// Get the milliseconds passed from an initial tick value. +B3_API float b3GetMilliseconds( uint64_t ticks ); + +/// Get the milliseconds passed from an initial tick value. +B3_API float b3GetMillisecondsAndReset( uint64_t* ticks ); + +/// Yield to be used in a busy loop. +B3_API void b3Yield( void ); + +// Simple djb2 hash function for determinism testing +#define B3_HASH_INIT 5381 +B3_API uint32_t b3Hash( uint32_t hash, const uint8_t* data, int count ); + +// Dump file support functions +B3_API void b3WriteBinaryFile( void* data, int size, const char* fileName ); +B3_API void* b3ReadBinaryFile( const char* prefix, const char* fileName, int* memSize ); + +//! @endcond diff --git a/include/box3d/box3d.h b/include/box3d/box3d.h new file mode 100644 index 0000000..8f369f3 --- /dev/null +++ b/include/box3d/box3d.h @@ -0,0 +1,1492 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "base.h" +#include "collision.h" +#include "id.h" +#include "math_functions.h" +#include "types.h" + +#include + +/** + * @defgroup world World + * These functions allow you to create a simulation world. + * + * You can add rigid bodies and joint constraints to the world and run the simulation. You can get contact + * information to get contact points and normals as well as events. You can query to world, checking for overlaps and casting rays + * or shapes. There is also debugging information such as debug draw, timing information, and counters. You can find documentation + * here: https://box2d.org/ + * @{ + */ + +/// Create a world for rigid body simulation. A world contains bodies, shapes, and constraints. You make create +/// up to 128 worlds. Each world is completely independent and may be simulated in parallel. +/// @return the world id. +B3_API b3WorldId b3CreateWorld( const b3WorldDef* def ); + +/// Destroy a world +B3_API void b3DestroyWorld( b3WorldId worldId ); + +/// Get the current number of worlds +B3_API int b3GetWorldCount( void ); + +/// Get the maximum number of simultaneous worlds that have been created +B3_API int b3GetMaxWorldCount( void ); + +/// World id validation. Provides validation for up to 64K allocations. +B3_API bool b3World_IsValid( b3WorldId id ); + +/// Simulate a world for one time step. This performs collision detection, integration, and constraint solution. +/// @param worldId The world to simulate +/// @param timeStep The amount of time to simulate, this should be a fixed number. Usually 1/60. +/// @param subStepCount The number of sub-steps, increasing the sub-step count can increase accuracy. Usually 4. +B3_API void b3World_Step( b3WorldId worldId, float timeStep, int subStepCount ); + +/// Call this to draw shapes and other debug draw data +B3_API void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits ); + +/// Get the body events for the current time step. The event data is transient. Do not store a reference to this data. +B3_API b3BodyEvents b3World_GetBodyEvents( b3WorldId worldId ); + +/// Get sensor events for the current time step. The event data is transient. Do not store a reference to this data. +B3_API b3SensorEvents b3World_GetSensorEvents( b3WorldId worldId ); + +/// Get contact events for this current time step. The event data is transient. Do not store a reference to this data. +B3_API b3ContactEvents b3World_GetContactEvents( b3WorldId worldId ); + +/// Get the joint events for the current time step. The event data is transient. Do not store a reference to this data. +B3_API b3JointEvents b3World_GetJointEvents( b3WorldId worldId ); + +/// Overlap test for all shapes that *potentially* overlap the provided AABB +B3_API b3TreeStats b3World_OverlapAABB( b3WorldId worldId, b3AABB aabb, b3QueryFilter filter, b3OverlapResultFcn* fcn, + void* context ); + +/// Overlap test for all shapes that overlap the provided shape proxy. +B3_API b3TreeStats b3World_OverlapShape( b3WorldId worldId, const b3ShapeProxy* proxy, b3QueryFilter filter, + b3OverlapResultFcn* fcn, void* context ); + +/// Cast a ray into the world to collect shapes in the path of the ray. +/// Your callback function controls whether you get the closest point, any point, or n-points. +/// @note The callback function may receive shapes in any order +/// @param worldId The world to cast the ray against +/// @param origin The start point of the ray +/// @param translation The translation of the ray from the start point to the end point +/// @param filter Contains bit flags to filter unwanted shapes from the results +/// @param fcn A user implemented callback function +/// @param context A user context that is passed along to the callback function +/// @return traversal performance counters +B3_API b3TreeStats b3World_CastRay( b3WorldId worldId, b3Vec3 origin, b3Vec3 translation, b3QueryFilter filter, + b3CastResultFcn* fcn, void* context ); + +/// Cast a ray into the world to collect the closest hit. This is a convenience function. Ignores initial overlap. +/// This is less general than b3World_CastRay() and does not allow for custom filtering. +B3_API b3RayResult b3World_CastRayClosest( b3WorldId worldId, b3Vec3 origin, b3Vec3 translation, b3QueryFilter filter ); + +/// Cast a shape through the world. Similar to a cast ray except that a shape is cast instead of a point. +/// @see b3World_CastRay +B3_API b3TreeStats b3World_CastShape( b3WorldId worldId, const b3ShapeProxy* proxy, b3Vec3 translation, b3QueryFilter filter, + b3CastResultFcn* fcn, void* context ); + +/// Cast a capsule mover through the world. This is a special shape cast that handles sliding along other shapes while reducing +/// clipping. This is not a good source of information about what the mover is touching. Instead use the planes returned by +/// b3World_CollideMover. +/// @param worldId World to cast the mover against +/// @param mover Capsule mover in world space +/// @param translation Desired mover translation +/// @param filter Contains bit flags to filter unwanted shapes from the results +/// @param fcn Optional callback for custom shape filtering +/// @param context A user context that is passed along to the callback function +/// @return the translation fraction +B3_API float b3World_CastMover( b3WorldId worldId, const b3Capsule* mover, b3Vec3 translation, b3QueryFilter filter, + b3MoverFilterFcn* fcn, void* context ); + +/// Collide a capsule mover with the world, gathering collision planes that can be fed to b3SolvePlanes. Useful for +/// kinematic character movement. +B3_API void b3World_CollideMover( b3WorldId worldId, const b3Capsule* mover, b3QueryFilter filter, b3PlaneResultFcn* fcn, + void* context ); + +/// Enable/disable sleep. If your application does not need sleeping, you can gain some performance +/// by disabling sleep completely at the world level. +/// @see b3WorldDef +B3_API void b3World_EnableSleeping( b3WorldId worldId, bool flag ); + +/// Is body sleeping enabled? +B3_API bool b3World_IsSleepingEnabled( b3WorldId worldId ); + +/// Enable/disable continuous collision between dynamic and static bodies. Generally you should keep continuous +/// collision enabled to prevent fast moving objects from going through static objects. The performance gain from +/// disabling continuous collision is minor. +/// @see b3WorldDef +B3_API void b3World_EnableContinuous( b3WorldId worldId, bool flag ); + +/// Is continuous collision enabled? +B3_API bool b3World_IsContinuousEnabled( b3WorldId worldId ); + +/// Adjust the restitution threshold. It is recommended not to make this value very small +/// because it will prevent bodies from sleeping. Usually in meters per second. +/// @see b3WorldDef +B3_API void b3World_SetRestitutionThreshold( b3WorldId worldId, float value ); + +/// Get the the restitution speed threshold. Usually in meters per second. +B3_API float b3World_GetRestitutionThreshold( b3WorldId worldId ); + +/// Adjust the hit event threshold. This controls the collision speed needed to generate a b3ContactHitEvent. +/// Usually in meters per second. +/// @see b3WorldDef::hitEventThreshold +B3_API void b3World_SetHitEventThreshold( b3WorldId worldId, float value ); + +/// Get the the hit event speed threshold. Usually in meters per second. +B3_API float b3World_GetHitEventThreshold( b3WorldId worldId ); + +/// Register the custom filter callback. This is optional. +B3_API void b3World_SetCustomFilterCallback( b3WorldId worldId, b3CustomFilterFcn* fcn, void* context ); + +/// Register the pre-solve callback. This is optional. +B3_API void b3World_SetPreSolveCallback( b3WorldId worldId, b3PreSolveFcn* fcn, 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. +/// @see b3WorldDef +B3_API void b3World_SetGravity( b3WorldId worldId, b3Vec3 gravity ); + +/// Get the gravity vector +B3_API b3Vec3 b3World_GetGravity( b3WorldId worldId ); + +/// Apply a radial explosion +/// @param worldId The world id +/// @param explosionDef The explosion definition +B3_API void b3World_Explode( b3WorldId worldId, const b3ExplosionDef* explosionDef ); + +/// Adjust contact tuning parameters +/// @param worldId The world id +/// @param hertz The contact stiffness (cycles per second) +/// @param dampingRatio The contact bounciness with 1 being critical damping (non-dimensional) +/// @param contactSpeed The maximum contact constraint push out speed (meters per second) +/// @note Advanced feature +B3_API void b3World_SetContactTuning( b3WorldId worldId, float hertz, float dampingRatio, float contactSpeed ); + +/// Set the contact point recycling distance. Setting this to zero disables contact point recycling. +/// Usually in meters. +B3_API void b3World_SetContactRecycleDistance( b3WorldId worldId, float recycleDistance ); + +/// Get the contact point recycling distance. Usually in meters. +B3_API float b3World_GetContactRecycleDistance( b3WorldId worldId ); + +/// Set the maximum linear speed. Usually in m/s. +B3_API void b3World_SetMaximumLinearSpeed( b3WorldId worldId, float maximumLinearSpeed ); + +/// Get the maximum linear speed. Usually in m/s. +B3_API float b3World_GetMaximumLinearSpeed( b3WorldId worldId ); + +/// Enable/disable constraint warm starting. Advanced feature for testing. Disabling +/// sleeping greatly reduces stability and provides no performance gain. +B3_API void b3World_EnableWarmStarting( b3WorldId worldId, bool flag ); + +/// Is constraint warm starting enabled? +B3_API bool b3World_IsWarmStartingEnabled( b3WorldId worldId ); + +/// Get the number of awake bodies +B3_API int b3World_GetAwakeBodyCount( b3WorldId worldId ); + +/// Get the current world performance profile +B3_API b3Profile b3World_GetProfile( b3WorldId worldId ); + +/// Get world counters and sizes +B3_API b3Counters b3World_GetCounters( b3WorldId worldId ); + +/// Get max capacity. This can be used with b3WorldDef to avoid run-time allocations and copies +B3_API b3Capacity b3World_GetMaxCapacity( b3WorldId worldId ); + +/// Set the user data pointer. +B3_API void b3World_SetUserData( b3WorldId worldId, void* userData ); + +/// Get the user data pointer. +B3_API void* b3World_GetUserData( b3WorldId worldId ); + +/// Set the friction callback. Passing NULL resets to default. +B3_API void b3World_SetFrictionCallback( b3WorldId worldId, b3FrictionCallback* callback ); + +/// Set the restitution callback. Passing NULL resets to default. +B3_API void b3World_SetRestitutionCallback( b3WorldId worldId, b3RestitutionCallback* callback ); + +/// Set the worker count. Must be between in the range [1, B3_MAX_WORKERS] +B3_API void b3World_SetWorkerCount( b3WorldId worldId, int count ); + +/// Get the worker count. +B3_API int b3World_GetWorkerCount( b3WorldId worldId ); + +/// Dump memory stats to box3d_memory.txt +B3_API void b3World_DumpMemoryStats( b3WorldId worldId ); + +/// Dump shape bounds to box3d_bounds.txt +B3_API void b3World_DumpShapeBounds( b3WorldId worldId, b3BodyType type ); + +/// This is for internal testing +B3_API void b3World_RebuildStaticTree( b3WorldId worldId ); + +/// This is for internal testing +B3_API void b3World_EnableSpeculative( b3WorldId worldId, bool flag ); + +/// Dump world to a text file. Saves only awake bodies and associated static bodies. +/// Meshes are saved to binary b3m files. +B3_API void b3World_DumpAwake( b3WorldId worldId ); + +/// Dump world to a text file. Meshes are saved to binary b3m files. +B3_API void b3World_Dump( b3WorldId worldId ); + +/** @} */ + +/** + * @defgroup body Body + * This is the body API. + * @{ + */ + +/// Create a rigid body given a definition. No reference to the definition is retained. So you can create the definition +/// on the stack and pass it as a pointer. +/// @code{.c} +/// b3BodyDef bodyDef = b3DefaultBodyDef(); +/// b3BodyId myBodyId = b3CreateBody(myWorldId, &bodyDef); +/// @endcode +/// @warning This function is locked during callbacks. +B3_API b3BodyId b3CreateBody( b3WorldId worldId, const b3BodyDef* def ); + +/// Destroy a rigid body given an id. This destroys all shapes and joints attached to the body. +/// Do not keep references to the associated shapes and joints. +B3_API void b3DestroyBody( b3BodyId bodyId ); + +/// Body identifier validation. A valid body exists in a world and is non-null. +/// This can be used to detect orphaned ids. Provides validation for up to 64K allocations. +B3_API bool b3Body_IsValid( b3BodyId id ); + +/// Get the body type: static, kinematic, or dynamic +B3_API b3BodyType b3Body_GetType( b3BodyId bodyId ); + +/// Change the body type. This is an expensive operation. This automatically updates the mass +/// properties regardless of the automatic mass setting. +B3_API void b3Body_SetType( b3BodyId bodyId, b3BodyType type ); + +/// Set the body name. Up to 31 characters excluding 0 termination. +B3_API void b3Body_SetName( b3BodyId bodyId, const char* name ); + +/// Get the body name. +B3_API const char* b3Body_GetName( b3BodyId bodyId ); + +/// Set the user data for a body +B3_API void b3Body_SetUserData( b3BodyId bodyId, void* userData ); + +/// Get the user data stored in a body +B3_API void* b3Body_GetUserData( b3BodyId bodyId ); + +/// Get the world position of a body. This is the location of the body origin. +B3_API b3Vec3 b3Body_GetPosition( b3BodyId bodyId ); + +/// Get the world rotation of a body as a cosine/sine pair (complex number) +B3_API b3Quat b3Body_GetRotation( b3BodyId bodyId ); + +/// Get the world transform of a body. +B3_API b3Transform b3Body_GetTransform( b3BodyId bodyId ); + +/// Set the world transform of a body. This acts as a teleport and is fairly expensive. +/// @note Generally you should create a body with then intended transform. +/// @see b3BodyDef::position and b3BodyDef::rotation +B3_API void b3Body_SetTransform( b3BodyId bodyId, b3Vec3 position, b3Quat rotation ); + +/// Get a local point on a body given a world point +B3_API b3Vec3 b3Body_GetLocalPoint( b3BodyId bodyId, b3Vec3 worldPoint ); + +/// Get a world point on a body given a local point +B3_API b3Vec3 b3Body_GetWorldPoint( b3BodyId bodyId, b3Vec3 localPoint ); + +/// Get a local vector on a body given a world vector +B3_API b3Vec3 b3Body_GetLocalVector( b3BodyId bodyId, b3Vec3 worldVector ); + +/// Get a world vector on a body given a local vector +B3_API b3Vec3 b3Body_GetWorldVector( b3BodyId bodyId, b3Vec3 localVector ); + +/// Get the linear velocity of a body's center of mass. Usually in meters per second. +B3_API b3Vec3 b3Body_GetLinearVelocity( b3BodyId bodyId ); + +/// Get the angular velocity of a body in radians per second +B3_API b3Vec3 b3Body_GetAngularVelocity( b3BodyId bodyId ); + +/// Set the linear velocity of a body. Usually in meters per second. +B3_API void b3Body_SetLinearVelocity( b3BodyId bodyId, b3Vec3 linearVelocity ); + +/// Set the angular velocity of a body in radians per second +B3_API void b3Body_SetAngularVelocity( b3BodyId bodyId, b3Vec3 angularVelocity ); + +/// Set the velocity to reach the given transform after a given time step. +/// The result will be close but maybe not exact. This is meant for kinematic bodies. +/// The target is not applied if the velocity would be below the sleep threshold. +/// This will optionally wake the body if asleep, but only if the movement is significant. +B3_API void b3Body_SetTargetTransform( b3BodyId bodyId, b3Transform target, float timeStep, bool wake ); + +/// Get the linear velocity of a local point attached to a body. Usually in meters per second. +B3_API b3Vec3 b3Body_GetLocalPointVelocity( b3BodyId bodyId, b3Vec3 localPoint ); + +/// Get the linear velocity of a world point attached to a body. Usually in meters per second. +B3_API b3Vec3 b3Body_GetWorldPointVelocity( b3BodyId bodyId, b3Vec3 worldPoint ); + +/// Apply a force at a world point. If the force is not applied at the center of mass, +/// it will generate a torque and affect the angular velocity. This optionally wakes up the body. +/// The force is ignored if the body is not awake. +/// @param bodyId The body id +/// @param force The world force vector, usually in newtons (N) +/// @param point The world position of the point of application +/// @param wake Option to wake up the body +B3_API void b3Body_ApplyForce( b3BodyId bodyId, b3Vec3 force, b3Vec3 point, bool wake ); + +/// Apply a force to the center of mass. This optionally wakes up the body. +/// The force is ignored if the body is not awake. +/// @param bodyId The body id +/// @param force the world force vector, usually in newtons (N). +/// @param wake also wake up the body +B3_API void b3Body_ApplyForceToCenter( b3BodyId bodyId, b3Vec3 force, bool wake ); + +/// Apply a torque. This affects the angular velocity without affecting the linear velocity. +/// This optionally wakes the body. The torque is ignored if the body is not awake. +/// @param bodyId The body id +/// @param torque the world torque vector, usually in N*m. +/// @param wake also wake up the body +B3_API void b3Body_ApplyTorque( b3BodyId bodyId, b3Vec3 torque, bool wake ); + +/// Apply an impulse at a point. This immediately modifies the velocity. +/// It also modifies the angular velocity if the point of application +/// is not at the center of mass. This optionally wakes the body. +/// The impulse is ignored if the body is not awake. +/// @param bodyId The body id +/// @param impulse the world impulse vector, usually in N*s or kg*m/s. +/// @param point the world position of the point of application. +/// @param wake also wake up the body +/// @warning This should be used for one-shot impulses. If you need a steady force, +/// use a force instead, which will work better with the sub-stepping solver. +B3_API void b3Body_ApplyLinearImpulse( b3BodyId bodyId, b3Vec3 impulse, b3Vec3 point, bool wake ); + +/// Apply an impulse to the center of mass. This immediately modifies the velocity. +/// The impulse is ignored if the body is not awake. This optionally wakes the body. +/// @param bodyId The body id +/// @param impulse the world impulse vector, usually in N*s or kg*m/s. +/// @param wake also wake up the body +/// @warning This should be used for one-shot impulses. If you need a steady force, +/// use a force instead, which will work better with the sub-stepping solver. +B3_API void b3Body_ApplyLinearImpulseToCenter( b3BodyId bodyId, b3Vec3 impulse, bool wake ); + +/// Apply an angular impulse in world space. The impulse is ignored if the body is not awake. +/// This optionally wakes the body. +/// @param bodyId The body id +/// @param impulse the world angular impulse vector, usually in units of kg*m*m/s +/// @param wake also wake up the body +/// @warning This should be used for one-shot impulses. If you need a steady torque, +/// use a torque instead, which will work better with the sub-stepping solver. +B3_API void b3Body_ApplyAngularImpulse( b3BodyId bodyId, b3Vec3 impulse, bool wake ); + +/// Get the mass of the body, usually in kilograms +B3_API float b3Body_GetMass( b3BodyId bodyId ); + +/// Get the rotational inertia of the body in local space, usually in kg*m^2 +B3_API b3Matrix3 b3Body_GetLocalRotationalInertia( b3BodyId bodyId ); + +/// Get the inverse mass of the body, usually in 1/kilograms +B3_API float b3Body_GetInverseMass( b3BodyId bodyId ); + +/// Get the inverse rotational inertia of the body in world space, usually in 1/kg*m^2 +B3_API b3Matrix3 b3Body_GetWorldInverseRotationalInertia( b3BodyId bodyId ); + +/// Get the center of mass position of the body in local space +B3_API b3Vec3 b3Body_GetLocalCenterOfMass( b3BodyId bodyId ); + +/// Get the center of mass position of the body in world space +B3_API b3Vec3 b3Body_GetWorldCenterOfMass( b3BodyId 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. +B3_API void b3Body_SetMassData( b3BodyId bodyId, b3MassData massData ); + +/// Get the mass data for a body +B3_API b3MassData b3Body_GetMassData( b3BodyId bodyId ); + +/// This update the mass properties to the sum of the mass properties of the shapes. +/// This normally does not need to be called unless you called SetMassData to override +/// the mass and you later want to reset the mass. +/// You may also use this when automatic mass computation has been disabled. +/// You should call this regardless of body type. +B3_API void b3Body_ApplyMassFromShapes( b3BodyId bodyId ); + +/// Adjust the linear damping. Normally this is set in b3BodyDef before creation. +B3_API void b3Body_SetLinearDamping( b3BodyId bodyId, float linearDamping ); + +/// Get the current linear damping. +B3_API float b3Body_GetLinearDamping( b3BodyId bodyId ); + +/// Adjust the angular damping. Normally this is set in b3BodyDef before creation. +B3_API void b3Body_SetAngularDamping( b3BodyId bodyId, float angularDamping ); + +/// Get the current angular damping. +B3_API float b3Body_GetAngularDamping( b3BodyId bodyId ); + +/// Adjust the gravity scale. Normally this is set in b3BodyDef before creation. +/// @see b3BodyDef::gravityScale +B3_API void b3Body_SetGravityScale( b3BodyId bodyId, float gravityScale ); + +/// Get the current gravity scale +B3_API float b3Body_GetGravityScale( b3BodyId bodyId ); + +/// @return true if this body is awake +B3_API bool b3Body_IsAwake( b3BodyId bodyId ); + +/// Wake a body from sleep. This wakes the entire island the body is touching. +/// @warning Putting a body to sleep will put the entire island of bodies touching this body to sleep, +/// which can be expensive and possibly unintuitive. +B3_API void b3Body_SetAwake( b3BodyId bodyId, bool awake ); + +/// Enable or disable sleeping for this body. If sleeping is disabled the body will wake. +B3_API void b3Body_EnableSleep( b3BodyId bodyId, bool enableSleep ); + +/// Returns true if sleeping is enabled for this body +B3_API bool b3Body_IsSleepEnabled( b3BodyId bodyId ); + +/// Set the sleep threshold, usually in meters per second +B3_API void b3Body_SetSleepThreshold( b3BodyId bodyId, float sleepThreshold ); + +/// Get the sleep threshold, usually in meters per second. +B3_API float b3Body_GetSleepThreshold( b3BodyId bodyId ); + +/// Returns true if this body is enabled +B3_API bool b3Body_IsEnabled( b3BodyId bodyId ); + +/// Disable a body by removing it completely from the simulation. This is expensive. +B3_API void b3Body_Disable( b3BodyId bodyId ); + +/// Enable a body by adding it to the simulation. This is expensive. +B3_API void b3Body_Enable( b3BodyId bodyId ); + +/// Set the motion locks on this body. +B3_API void b3Body_SetMotionLocks( b3BodyId bodyId, b3MotionLocks locks ); + +/// Get the motion locks for this body. +B3_API b3MotionLocks b3Body_GetMotionLocks( b3BodyId bodyId ); + +/// Set this body to be a bullet. A bullet does continuous collision detection +/// against dynamic bodies (but not other bullets). +B3_API void b3Body_SetBullet( b3BodyId bodyId, bool flag ); + +/// Is this body a bullet? +B3_API bool b3Body_IsBullet( b3BodyId bodyId ); + +/// Enable/disable hit events on all shapes +/// @see b3ShapeDef::enableHitEvents +B3_API void b3Body_EnableHitEvents( b3BodyId bodyId, bool enableHitEvents ); + +/// Get the world that owns this body +B3_API b3WorldId b3Body_GetWorld( b3BodyId bodyId ); + +/// Get the number of shapes on this body +B3_API int b3Body_GetShapeCount( b3BodyId bodyId ); + +/// Get the shape ids for all shapes on this body, up to the provided capacity. +/// @returns the number of shape ids stored in the user array +B3_API int b3Body_GetShapes( b3BodyId bodyId, b3ShapeId* shapeArray, int capacity ); + +/// Get the number of joints on this body +B3_API int b3Body_GetJointCount( b3BodyId bodyId ); + +/// Get the joint ids for all joints on this body, up to the provided capacity +/// @returns the number of joint ids stored in the user array +B3_API int b3Body_GetJoints( b3BodyId bodyId, b3JointId* jointArray, int capacity ); + +/// Get the maximum capacity required for retrieving all the touching contacts on a body +B3_API int b3Body_GetContactCapacity( b3BodyId bodyId ); + +/// Get the touching contact data for a body +B3_API int b3Body_GetContactData( b3BodyId bodyId, b3ContactData* contactData, int capacity ); + +/// Get the current world AABB that contains all the attached shapes. Note that this may not encompass the body origin. +/// If there are no shapes attached then the returned AABB is empty and centered on the body origin. +B3_API b3AABB b3Body_ComputeAABB( b3BodyId bodyId ); + +/// Get the closest point on a body to a world target. +B3_API float b3Body_GetClosestPoint( b3BodyId bodyId, b3Vec3* result, b3Vec3 target ); + +/// Cast a ray at a specific body using a specified body transform. +B3_API b3BodyCastResult b3Body_CastRay( b3BodyId bodyId, const b3BodyRayCastInput* input, b3Transform bodyTransform ); + +/// Cast a shape at a specific body using a specified body transform. +B3_API b3BodyCastResult b3Body_CastShape( b3BodyId bodyId, const b3BodyShapeCastInput* input, b3Transform bodyTransform ); + +/// Overlap a shape with a specific body using a specified body transform. +B3_API bool b3Body_OverlapShape( b3BodyId bodyId, const b3ShapeProxy* proxy, b3QueryFilter filter, b3Transform bodyTransform ); + +/// Collide a character mover with a specific body using a specified body transform. +B3_API int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int planeCapacity, const b3Capsule* mover, + b3QueryFilter filter, b3Transform bodyTransform ); + +/** @} */ + +/** + * @defgroup shape Shape + * Functions to create, destroy, and access. + * Shapes bind raw geometry to bodies and hold material properties including friction and restitution. + * @{ + */ + +/// Create a circle shape and attach it to a body. The shape definition and geometry are fully cloned. +/// Contacts are not created until the next time step. +/// @return the shape id for accessing the shape +B3_API b3ShapeId b3CreateSphereShape( b3BodyId bodyId, const b3ShapeDef* def, const b3Sphere* sphere ); + +/// Create a capsule shape and attach it to a body. The shape definition and geometry are fully cloned. +/// Contacts are not created until the next time step. +/// @return the shape id for accessing the shape +B3_API b3ShapeId b3CreateCapsuleShape( b3BodyId bodyId, const b3ShapeDef* def, const b3Capsule* capsule ); + +/// Create a convex hull shape and attach it to a body. The hull is cloned. +/// The shape definition and geometry are fully cloned. Contacts are not created until the next time step. +/// @return the shape id for accessing the shape +B3_API b3ShapeId b3CreateHullShape( b3BodyId bodyId, const b3ShapeDef* def, const b3Hull* hull ); + +/// Create a convex hull shape and attach it to a body. The hull is cloned then transformed with scale applied first. +/// The shape definition and geometry are fully cloned. Contacts are not created until the next time step. +/// @return the shape id for accessing the shape +B3_API b3ShapeId b3CreateTransformedHullShape( b3BodyId bodyId, const b3ShapeDef* def, const b3Hull* hull, b3Transform transform, + b3Vec3 scale ); + +/// Create a mesh hull shape and attach it to a body. The shape definition is fully cloned but the mesh is not. +/// Contacts are not created until the next time step. +/// Mesh collision only creates contacts on static bodies. +/// @warning this holds reference to the input mesh data which must remain valid for the lifetime of this shape +/// @return the shape id for accessing the shape +B3_API b3ShapeId b3CreateMeshShape( b3BodyId bodyId, const b3ShapeDef* def, const b3MeshData* mesh, b3Vec3 scale ); + +/// Create a height-field shape and attach it to a body. The shape definition is fully cloned but the height field is not. +/// Contacts are not created until the next time step. +/// Height field is only allowed on static bodies. +/// @warning this holds reference to the input height field which must remain valid for the lifetime of this shape +/// @return the shape id for accessing the shape +B3_API b3ShapeId b3CreateHeightFieldShape( b3BodyId bodyId, const b3ShapeDef* def, const b3HeightField* heightField ); + +/// Compound shapes are only allowed on static bodies. +B3_API b3ShapeId b3CreateCompoundShape( b3BodyId bodyId, b3ShapeDef* def, const b3Compound* compound ); + +/// Destroy a shape. You may defer the body mass update which can improve performance if several shapes on a +/// body are destroyed at once. +/// @see b3Body_ApplyMassFromShapes +B3_API void b3DestroyShape( b3ShapeId shapeId, bool updateBodyMass ); + +/// Shape identifier validation. Provides validation for up to 64K allocations. +B3_API bool b3Shape_IsValid( b3ShapeId id ); + +/// Get the type of a shape +B3_API b3ShapeType b3Shape_GetType( b3ShapeId shapeId ); + +/// Get the id of the body that a shape is attached to +B3_API b3BodyId b3Shape_GetBody( b3ShapeId shapeId ); + +/// Get the world that owns this shape +B3_API b3WorldId b3Shape_GetWorld( b3ShapeId shapeId ); + +/// Returns true If the shape is a sensor +B3_API bool b3Shape_IsSensor( b3ShapeId shapeId ); + +/// Set the shape name. Up to 31 characters excluding 0 termination. +B3_API void b3Shape_SetName( b3ShapeId shapeId, const char* name ); + +/// Get the shape name. +B3_API const char* b3Shape_GetName( b3ShapeId shapeId ); + +/// Set the user data for a shape +B3_API void b3Shape_SetUserData( b3ShapeId shapeId, void* userData ); + +/// Get the user data for a shape. This is useful when you get a shape id +/// from an event or query. +B3_API void* b3Shape_GetUserData( b3ShapeId shapeId ); + +/// Set the mass density of a shape, usually in kg/m^2. +/// This will optionally update the mass properties on the parent body. +/// @see b3ShapeDef::density, b3Body_ApplyMassFromShapes +B3_API void b3Shape_SetDensity( b3ShapeId shapeId, float density, bool updateBodyMass ); + +/// Get the density of a shape, usually in kg/m^2 +B3_API float b3Shape_GetDensity( b3ShapeId shapeId ); + +/// Set the friction on a shape +B3_API void b3Shape_SetFriction( b3ShapeId shapeId, float friction ); + +/// Get the friction of a shape +B3_API float b3Shape_GetFriction( b3ShapeId shapeId ); + +/// Set the shape restitution (bounciness) +B3_API void b3Shape_SetRestitution( b3ShapeId shapeId, float restitution ); + +/// Get the shape restitution +B3_API float b3Shape_GetRestitution( b3ShapeId shapeId ); + +/// Set the shape base surface material. Does not change per triangle materials. +B3_API void b3Shape_SetSurfaceMaterial( b3ShapeId shapeId, b3SurfaceMaterial surfaceMaterial ); + +/// Get the base shape surface material. +B3_API b3SurfaceMaterial b3Shape_GetSurfaceMaterial( b3ShapeId shapeId ); + +/// Get the number of mesh surface materials. +B3_API int b3Shape_GetMeshMaterialCount( b3ShapeId shapeId ); + +/// Set a surface material for a mesh shape. +B3_API void b3Shape_SetMeshMaterial( b3ShapeId shapeId, b3SurfaceMaterial surfaceMaterial, int index ); + +/// Get a surface material for a mesh shape +B3_API b3SurfaceMaterial b3Shape_GetMeshSurfaceMaterial( b3ShapeId shapeId, int index ); + +/// Get the shape filter +B3_API b3Filter b3Shape_GetFilter( b3ShapeId shapeId ); + +/// Set the current filter. This is almost as expensive as recreating the shape. +/// @see b3ShapeDef::filter +/// @param shapeId the shape +/// @param filter the new filter +/// @param invokeContacts if true then the shape will have all contacts recomputed the next time step (expensive) +B3_API void b3Shape_SetFilter( b3ShapeId shapeId, b3Filter filter, bool invokeContacts ); + +/// Enable sensor events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. +/// @see b3ShapeDef::isSensor +B3_API void b3Shape_EnableSensorEvents( b3ShapeId shapeId, bool flag ); + +/// Returns true if sensor events are enabled +B3_API bool b3Shape_AreSensorEventsEnabled( b3ShapeId shapeId ); + +/// Enable contact events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. +/// @see b3ShapeDef::enableContactEvents +B3_API void b3Shape_EnableContactEvents( b3ShapeId shapeId, bool flag ); + +/// Returns true if contact events are enabled +B3_API bool b3Shape_AreContactEventsEnabled( b3ShapeId shapeId ); + +/// Enable pre-solve contact events for this shape. Only applies to dynamic bodies. These are expensive +/// and must be carefully handled due to multithreading. Ignored for sensors. +/// @see b3PreSolveFcn +B3_API void b3Shape_EnablePreSolveEvents( b3ShapeId shapeId, bool flag ); + +/// Returns true if pre-solve events are enabled +B3_API bool b3Shape_ArePreSolveEventsEnabled( b3ShapeId shapeId ); + +/// Enable contact hit events for this shape. Ignored for sensors. +/// @see b3WorldDef.hitEventThreshold +B3_API void b3Shape_EnableHitEvents( b3ShapeId shapeId, bool flag ); + +/// Returns true if hit events are enabled +B3_API bool b3Shape_AreHitEventsEnabled( b3ShapeId shapeId ); + +/// Ray cast a shape directly +B3_API b3CastOutput b3Shape_RayCast( b3ShapeId shapeId, const b3RayCastInput* input ); + +/// Get a copy of the shape's sphere. Asserts the type is correct. +B3_API b3Sphere b3Shape_GetSphere( b3ShapeId shapeId ); + +/// Get a copy of the shape's capsule. Asserts the type is correct. +B3_API b3Capsule b3Shape_GetCapsule( b3ShapeId shapeId ); + +/// Get the shape's convex hull. Asserts the type is correct. +B3_API const b3Hull* b3Shape_GetHull( b3ShapeId shapeId ); + +/// Get the shape's mesh. Asserts the type is correct. +B3_API b3Mesh b3Shape_GetMesh( b3ShapeId shapeId ); + +/// Get the shape's height field. Asserts the type is correct. +B3_API const b3HeightField* b3Shape_GetHeightField( b3ShapeId shapeId ); + +/// Allows you to change a shape to be a sphere or update the current sphere. +/// This does not modify the mass properties. +/// @see b3Body_ApplyMassFromShapes +B3_API void b3Shape_SetSphere( b3ShapeId shapeId, const b3Sphere* sphere ); + +/// Allows you to change a shape to be a capsule or update the current capsule. +/// This does not modify the mass properties. +/// @see b3Body_ApplyMassFromShapes +B3_API void b3Shape_SetCapsule( b3ShapeId shapeId, const b3Capsule* capsule ); + +/// Allows you to change a shape to be a hull or update the current hull. +/// This does not modify the mass properties. +/// @see b3Body_ApplyMassFromShapes +B3_API void b3Shape_SetHull( b3ShapeId shapeId, const b3Hull* hull ); + +/// Allows you to change a shape to be a mesh or update the current mesh. +/// This does not modify the mass properties. +/// @see b3Body_ApplyMassFromShapes +B3_API void b3Shape_SetMesh( b3ShapeId shapeId, const b3MeshData* meshData, b3Vec3 scale ); + +/// Get the maximum capacity required for retrieving all the touching contacts on a shape +B3_API int b3Shape_GetContactCapacity( b3ShapeId shapeId ); + +/// Get the touching contact data for a shape. The provided shapeId will be either shapeIdA or shapeIdB on the contact data. +/// @note Box3D uses speculative collision so some contact points may be separated. +/// @returns the number of elements filled in the provided array +/// @warning do not ignore the return value, it specifies the valid number of elements +B3_API int b3Shape_GetContactData( b3ShapeId shapeId, b3ContactData* contactData, int capacity ); + +/// Get the maximum capacity required for retrieving all the overlapped shapes on a sensor shape. +/// This returns 0 if the provided shape is not a sensor. +/// @param shapeId the id of a sensor shape +/// @returns the required capacity to get all the overlaps in b3Shape_GetSensorOverlaps +B3_API int b3Shape_GetSensorCapacity( b3ShapeId shapeId ); + +/// Get the overlap data for a sensor shape. +/// @param shapeId the id of a sensor shape +/// @param visitorIds a user allocated array that is filled with the overlapping shapes (visitors) +/// @param capacity the capacity of overlappedShapes +/// @returns the number of elements filled in the provided array +/// @warning do not ignore the return value, it specifies the valid number of elements +/// @warning overlaps may contain destroyed shapes so use b3Shape_IsValid to confirm each overlap +B3_API int b3Shape_GetSensorData( b3ShapeId shapeId, b3ShapeId* visitorIds, int capacity ); + +/// Get the current world AABB +B3_API b3AABB b3Shape_GetAABB( b3ShapeId shapeId ); + +/// Compute the mass data for a shape +B3_API b3MassData b3Shape_ComputeMassData( b3ShapeId shapeId ); + +/// Get the closest point on a shape to a target point. Target and result are in world space. +B3_API b3Vec3 b3Shape_GetClosestPoint( b3ShapeId shapeId, b3Vec3 target ); + +/// Apply a wind force to the body for this shape using the density of air. This considers +/// the projected area of the shape in the wind direction. This also considers +/// the relative velocity of the shape. +/// @param shapeId the shape id +/// @param wind the wind velocity in world space +/// @param drag the drag coefficient, the force that opposes the relative velocity +/// @param lift the lift coefficient, the force that is perpendicular to the relative velocity +/// @param maxSpeed the maximum relative speed. Speed cap is necessary for stability. Typically 10m/s or less. +/// @param wake should this wake the body +B3_API void b3Shape_ApplyWind( b3ShapeId shapeId, b3Vec3 wind, float drag, float lift, float maxSpeed, bool wake ); + +/** @} */ + +/** + * @defgroup joint Joint + * @brief Joints allow you to connect rigid bodies together while allowing various forms of relative motions. + * @{ + */ + +/// Destroy a joint +B3_API void b3DestroyJoint( b3JointId jointId, bool wakeAttached ); + +/// Joint identifier validation. Provides validation for up to 64K allocations. +B3_API bool b3Joint_IsValid( b3JointId id ); + +/// Get the joint type +B3_API b3JointType b3Joint_GetType( b3JointId jointId ); + +/// Get body A id on a joint +B3_API b3BodyId b3Joint_GetBodyA( b3JointId jointId ); + +/// Get body B id on a joint +B3_API b3BodyId b3Joint_GetBodyB( b3JointId jointId ); + +/// Get the world that owns this joint +B3_API b3WorldId b3Joint_GetWorld( b3JointId jointId ); + +/// Set the local frame on bodyA +B3_API void b3Joint_SetLocalFrameA( b3JointId jointId, b3Transform localFrame ); + +/// Get the local frame on bodyA +B3_API b3Transform b3Joint_GetLocalFrameA( b3JointId jointId ); + +/// Set the local frame on bodyB +B3_API void b3Joint_SetLocalFrameB( b3JointId jointId, b3Transform localFrame ); + +/// Get the local frame on bodyB +B3_API b3Transform b3Joint_GetLocalFrameB( b3JointId jointId ); + +/// Toggle collision between connected bodies +B3_API void b3Joint_SetCollideConnected( b3JointId jointId, bool shouldCollide ); + +/// Is collision allowed between connected bodies? +B3_API bool b3Joint_GetCollideConnected( b3JointId jointId ); + +/// Set the user data on a joint +B3_API void b3Joint_SetUserData( b3JointId jointId, void* userData ); + +/// Get the user data on a joint +B3_API void* b3Joint_GetUserData( b3JointId jointId ); + +/// Wake the bodies connect to this joint +B3_API void b3Joint_WakeBodies( b3JointId jointId ); + +/// Get the current constraint force for this joint +B3_API b3Vec3 b3Joint_GetConstraintForce( b3JointId jointId ); + +/// Get the current constraint torque for this joint +B3_API b3Vec3 b3Joint_GetConstraintTorque( b3JointId jointId ); + +/// Get the current linear separation error for this joint. Does not consider admissible movement. Usually in meters. +B3_API float b3Joint_GetLinearSeparation( b3JointId jointId ); + +/// Get the current angular separation error for this joint. Does not consider admissible movement. Usually in meters. +B3_API float b3Joint_GetAngularSeparation( b3JointId jointId ); + +/// Set the joint constraint tuning. Advanced feature. +/// @param jointId the joint +/// @param hertz the stiffness in Hertz (cycles per second) +/// @param dampingRatio the non-dimensional damping ratio (one for critical damping) +B3_API void b3Joint_SetConstraintTuning( b3JointId jointId, float hertz, float dampingRatio ); + +/// Get the joint constraint tuning. Advanced feature. +B3_API void b3Joint_GetConstraintTuning( b3JointId jointId, float* hertz, float* dampingRatio ); + +/// Set the force threshold for joint events (Newtons) +B3_API void b3Joint_SetForceThreshold( b3JointId jointId, float threshold ); + +/// Get the force threshold for joint events (Newtons) +B3_API float b3Joint_GetForceThreshold( b3JointId jointId ); + +/// Set the torque threshold for joint events (N-m) +B3_API void b3Joint_SetTorqueThreshold( b3JointId jointId, float threshold ); + +/// Get the torque threshold for joint events (N-m) +B3_API float b3Joint_GetTorqueThreshold( b3JointId jointId ); + +/** + * @defgroup cone_joint Parallel Joint + * @brief Functions for the parallel joint. + * @{ + */ + +/// Create a parallel joint +/// @see b3ParallelJointDef for details +B3_API b3JointId b3CreateParallelJoint( b3WorldId worldId, const b3ParallelJointDef* def ); + +/// Set the spring stiffness in Hertz +B3_API void b3ParallelJoint_SetSpringHertz( b3JointId jointId, float hertz ); + +/// Set the spring damping ratio, non-dimensional +B3_API void b3ParallelJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio ); + +/// Get the spring Hertz +B3_API float b3ParallelJoint_GetSpringHertz( b3JointId jointId ); + +/// Get the spring damping ratio +B3_API float b3ParallelJoint_GetSpringDampingRatio( b3JointId jointId ); + +/// Set the maximum spring torque, usually in newtons-meters +B3_API void b3ParallelJoint_SetMaxTorque( b3JointId jointId, float force ); + +/// Get the maximum spring torque, usually in newtons-meters +B3_API float b3ParallelJoint_GetMaxTorque( b3JointId jointId ); +/** @} */ + +/** + * @defgroup distance_joint Distance Joint + * @brief Functions for the distance joint. + * @{ + */ + +/// Create a distance joint +/// @see b3DistanceJointDef for details +B3_API b3JointId b3CreateDistanceJoint( b3WorldId worldId, const b3DistanceJointDef* def ); + +/// Set the rest length of a distance joint +/// @param jointId The id for a distance joint +/// @param length The new distance joint length +B3_API void b3DistanceJoint_SetLength( b3JointId jointId, float length ); + +/// Get the rest length of a distance joint +B3_API float b3DistanceJoint_GetLength( b3JointId jointId ); + +/// Enable/disable the distance joint spring. When disabled the distance joint is rigid. +B3_API void b3DistanceJoint_EnableSpring( b3JointId jointId, bool enableSpring ); + +/// Is the distance joint spring enabled? +B3_API bool b3DistanceJoint_IsSpringEnabled( b3JointId jointId ); + +/// Set the force range for the spring. +B3_API void b3DistanceJoint_SetSpringForceRange( b3JointId jointId, float lowerForce, float upperForce ); + +/// Get the force range for the spring. +B3_API void b3DistanceJoint_GetSpringForceRange( b3JointId jointId, float* lowerForce, float* upperForce ); + +/// Set the spring stiffness in Hertz +B3_API void b3DistanceJoint_SetSpringHertz( b3JointId jointId, float hertz ); + +/// Set the spring damping ratio, non-dimensional +B3_API void b3DistanceJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio ); + +/// Get the spring Hertz +B3_API float b3DistanceJoint_GetSpringHertz( b3JointId jointId ); + +/// Get the spring damping ratio +B3_API float b3DistanceJoint_GetSpringDampingRatio( b3JointId jointId ); + +/// Enable joint limit. The limit only works if the joint spring is enabled. Otherwise the joint is rigid +/// and the limit has no effect. +B3_API void b3DistanceJoint_EnableLimit( b3JointId jointId, bool enableLimit ); + +/// Is the distance joint limit enabled? +B3_API bool b3DistanceJoint_IsLimitEnabled( b3JointId jointId ); + +/// Set the minimum and maximum length parameters of a distance joint +B3_API void b3DistanceJoint_SetLengthRange( b3JointId jointId, float minLength, float maxLength ); + +/// Get the distance joint minimum length +B3_API float b3DistanceJoint_GetMinLength( b3JointId jointId ); + +/// Get the distance joint maximum length +B3_API float b3DistanceJoint_GetMaxLength( b3JointId jointId ); + +/// Get the current length of a distance joint +B3_API float b3DistanceJoint_GetCurrentLength( b3JointId jointId ); + +/// Enable/disable the distance joint motor +B3_API void b3DistanceJoint_EnableMotor( b3JointId jointId, bool enableMotor ); + +/// Is the distance joint motor enabled? +B3_API bool b3DistanceJoint_IsMotorEnabled( b3JointId jointId ); + +/// Set the distance joint motor speed, usually in meters per second +B3_API void b3DistanceJoint_SetMotorSpeed( b3JointId jointId, float motorSpeed ); + +/// Get the distance joint motor speed, usually in meters per second +B3_API float b3DistanceJoint_GetMotorSpeed( b3JointId jointId ); + +/// Set the distance joint maximum motor force, usually in newtons +B3_API void b3DistanceJoint_SetMaxMotorForce( b3JointId jointId, float force ); + +/// Get the distance joint maximum motor force, usually in newtons +B3_API float b3DistanceJoint_GetMaxMotorForce( b3JointId jointId ); + +/// Get the distance joint current motor force, usually in newtons +B3_API float b3DistanceJoint_GetMotorForce( b3JointId jointId ); +/** @} */ + +/** + * @defgroup motor_joint Motor Joint + * @brief Functions for the motor joint. + * + * The motor joint is designed to control the movement of a body while still being + * responsive to collisions. A spring controls the position and rotation. A velocity motor + * can be used to control velocity and allows for friction in top-down games. Both types + * of control can be combined. For example, you can have a spring with friction. + * Position and velocity control have force and torque limits. + * @{ + */ + +/// Create a motor joint +/// @see b3MotorJointDef for details +B3_API b3JointId b3CreateMotorJoint( b3WorldId worldId, const b3MotorJointDef* def ); + +/// Set the desired relative linear velocity in meters per second +B3_API void b3MotorJoint_SetLinearVelocity( b3JointId jointId, b3Vec3 velocity ); + +/// Get the desired relative linear velocity in meters per second +B3_API b3Vec3 b3MotorJoint_GetLinearVelocity( b3JointId jointId ); + +/// Set the desired relative angular velocity in radians per second +B3_API void b3MotorJoint_SetAngularVelocity( b3JointId jointId, b3Vec3 velocity ); + +/// Get the desired relative angular velocity in radians per second +B3_API b3Vec3 b3MotorJoint_GetAngularVelocity( b3JointId jointId ); + +/// Set the motor joint maximum force, usually in newtons +B3_API void b3MotorJoint_SetMaxVelocityForce( b3JointId jointId, float maxForce ); + +/// Get the motor joint maximum force, usually in newtons +B3_API float b3MotorJoint_GetMaxVelocityForce( b3JointId jointId ); + +/// Set the motor joint maximum torque, usually in newton-meters +B3_API void b3MotorJoint_SetMaxVelocityTorque( b3JointId jointId, float maxTorque ); + +/// Get the motor joint maximum torque, usually in newton-meters +B3_API float b3MotorJoint_GetMaxVelocityTorque( b3JointId jointId ); + +/// Set the spring linear hertz stiffness +B3_API void b3MotorJoint_SetLinearHertz( b3JointId jointId, float hertz ); + +/// Get the spring linear hertz stiffness +B3_API float b3MotorJoint_GetLinearHertz( b3JointId jointId ); + +/// Set the spring linear damping ratio. Use 1.0 for critical damping. +B3_API void b3MotorJoint_SetLinearDampingRatio( b3JointId jointId, float damping ); + +/// Get the spring linear damping ratio. +B3_API float b3MotorJoint_GetLinearDampingRatio( b3JointId jointId ); + +/// Set the spring angular hertz stiffness +B3_API void b3MotorJoint_SetAngularHertz( b3JointId jointId, float hertz ); + +/// Get the spring angular hertz stiffness +B3_API float b3MotorJoint_GetAngularHertz( b3JointId jointId ); + +/// Set the spring angular damping ratio. Use 1.0 for critical damping. +B3_API void b3MotorJoint_SetAngularDampingRatio( b3JointId jointId, float damping ); + +/// Get the spring angular damping ratio. +B3_API float b3MotorJoint_GetAngularDampingRatio( b3JointId jointId ); + +/// Set the maximum spring force in newtons. +B3_API void b3MotorJoint_SetMaxSpringForce( b3JointId jointId, float maxForce ); + +/// Get the maximum spring force in newtons. +B3_API float b3MotorJoint_GetMaxSpringForce( b3JointId jointId ); + +/// Set the maximum spring torque in newtons * meters +B3_API void b3MotorJoint_SetMaxSpringTorque( b3JointId jointId, float maxTorque ); + +/// Get the maximum spring torque in newtons * meters +B3_API float b3MotorJoint_GetMaxSpringTorque( b3JointId jointId ); + +/**@}*/ + +/**@}*/ +/** + * @defgroup null_joint Null Joint + * @brief Functions for the null joint. + * + * The null 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 b3FilterJointDef for details +B3_API b3JointId b3CreateFilterJoint( b3WorldId worldId, const b3FilterJointDef* def ); + +/**@}*/ + +/** + * @defgroup prismatic_joint Prismatic Joint + * @brief A prismatic joint allows for translation along a single axis with no rotation. + * + * The prismatic joint is useful for things like pistons and moving platforms, where you want a body to translate + * along an axis and have no rotation. Also called a *slider* joint. + * @{ + */ + +/// Create a prismatic (slider) joint. +/// @see b3PrismaticJointDef for details +B3_API b3JointId b3CreatePrismaticJoint( b3WorldId worldId, const b3PrismaticJointDef* def ); + +/// Enable/disable the joint spring. +B3_API void b3PrismaticJoint_EnableSpring( b3JointId jointId, bool enableSpring ); + +/// Is the prismatic joint spring enabled or not? +B3_API bool b3PrismaticJoint_IsSpringEnabled( b3JointId jointId ); + +/// Set the prismatic joint stiffness in Hertz. +/// This should usually be less than a quarter of the simulation rate. For example, if the simulation +/// runs at 60Hz then the joint stiffness should be 15Hz or less. +B3_API void b3PrismaticJoint_SetSpringHertz( b3JointId jointId, float hertz ); + +/// Get the prismatic joint stiffness in Hertz +B3_API float b3PrismaticJoint_GetSpringHertz( b3JointId jointId ); + +/// Set the prismatic joint damping ratio (non-dimensional) +B3_API void b3PrismaticJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio ); + +/// Get the prismatic spring damping ratio (non-dimensional) +B3_API float b3PrismaticJoint_GetSpringDampingRatio( b3JointId jointId ); + +/// Set the prismatic joint target translation. Usually in meters. +B3_API void b3PrismaticJoint_SetTargetTranslation( b3JointId jointId, float targetTranslation ); + +/// Get the prismatic joint target translation. Usually in meters. +B3_API float b3PrismaticJoint_GetTargetTranslation( b3JointId jointId ); + +/// Enable/disable a prismatic joint limit +B3_API void b3PrismaticJoint_EnableLimit( b3JointId jointId, bool enableLimit ); + +/// Is the prismatic joint limit enabled? +B3_API bool b3PrismaticJoint_IsLimitEnabled( b3JointId jointId ); + +/// Get the prismatic joint lower limit +B3_API float b3PrismaticJoint_GetLowerLimit( b3JointId jointId ); + +/// Get the prismatic joint upper limit +B3_API float b3PrismaticJoint_GetUpperLimit( b3JointId jointId ); + +/// Set the prismatic joint limits +B3_API void b3PrismaticJoint_SetLimits( b3JointId jointId, float lower, float upper ); + +/// Enable/disable a prismatic joint motor +B3_API void b3PrismaticJoint_EnableMotor( b3JointId jointId, bool enableMotor ); + +/// Is the prismatic joint motor enabled? +B3_API bool b3PrismaticJoint_IsMotorEnabled( b3JointId jointId ); + +/// Set the prismatic joint motor speed, usually in meters per second +B3_API void b3PrismaticJoint_SetMotorSpeed( b3JointId jointId, float motorSpeed ); + +/// Get the prismatic joint motor speed, usually in meters per second +B3_API float b3PrismaticJoint_GetMotorSpeed( b3JointId jointId ); + +/// Set the prismatic joint maximum motor force, usually in newtons +B3_API void b3PrismaticJoint_SetMaxMotorForce( b3JointId jointId, float force ); + +/// Get the prismatic joint maximum motor force, usually in newtons +B3_API float b3PrismaticJoint_GetMaxMotorForce( b3JointId jointId ); + +/// Get the prismatic joint current motor force, usually in newtons +B3_API float b3PrismaticJoint_GetMotorForce( b3JointId jointId ); + +/// Get the current joint translation, usually in meters. +B3_API float b3PrismaticJoint_GetTranslation( b3JointId jointId ); + +/// Get the current joint translation speed, usually in meters per second. +B3_API float b3PrismaticJoint_GetSpeed( b3JointId 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. + * @{ + */ + +/// Create a revolute joint +/// @see b3RevoluteJointDef for details +B3_API b3JointId b3CreateRevoluteJoint( b3WorldId worldId, const b3RevoluteJointDef* def ); + +/// Enable/disable the revolute joint spring +B3_API void b3RevoluteJoint_EnableSpring( b3JointId jointId, bool enableSpring ); + +/// It the revolute angular spring enabled? +B3_API bool b3RevoluteJoint_IsSpringEnabled( b3JointId jointId ); + +/// Set the revolute joint spring stiffness in Hertz +B3_API void b3RevoluteJoint_SetSpringHertz( b3JointId jointId, float hertz ); + +/// Get the revolute joint spring stiffness in Hertz +B3_API float b3RevoluteJoint_GetSpringHertz( b3JointId jointId ); + +/// Set the revolute joint spring damping ratio, non-dimensional +B3_API void b3RevoluteJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio ); + +/// Get the revolute joint spring damping ratio, non-dimensional +B3_API float b3RevoluteJoint_GetSpringDampingRatio( b3JointId jointId ); + +/// Set the revolute joint target angle in radians +B3_API void b3RevoluteJoint_SetTargetAngle( b3JointId jointId, float targetRadians ); + +/// Get the revolute joint target angle in radians +B3_API float b3RevoluteJoint_GetTargetAngle( b3JointId jointId ); + +/// Get the revolute joint current angle in radians relative to the reference angle +/// @see b3RevoluteJointDef::referenceAngle +B3_API float b3RevoluteJoint_GetAngle( b3JointId jointId ); + +/// Enable/disable the revolute joint limit +B3_API void b3RevoluteJoint_EnableLimit( b3JointId jointId, bool enableLimit ); + +/// Is the revolute joint limit enabled? +B3_API bool b3RevoluteJoint_IsLimitEnabled( b3JointId jointId ); + +/// Get the revolute joint lower limit in radians +B3_API float b3RevoluteJoint_GetLowerLimit( b3JointId jointId ); + +/// Get the revolute joint upper limit in radians +B3_API float b3RevoluteJoint_GetUpperLimit( b3JointId jointId ); + +/// Set the revolute joint limits in radians +B3_API void b3RevoluteJoint_SetLimits( b3JointId jointId, float lowerLimitRadians, float upperLimitRadians ); + +/// Enable/disable a revolute joint motor +B3_API void b3RevoluteJoint_EnableMotor( b3JointId jointId, bool enableMotor ); + +/// Is the revolute joint motor enabled? +B3_API bool b3RevoluteJoint_IsMotorEnabled( b3JointId jointId ); + +/// Set the revolute joint motor speed in radians per second +B3_API void b3RevoluteJoint_SetMotorSpeed( b3JointId jointId, float motorSpeed ); + +/// Get the revolute joint motor speed in radians per second +B3_API float b3RevoluteJoint_GetMotorSpeed( b3JointId jointId ); + +/// Get the revolute joint current motor torque, usually in newton-meters +B3_API float b3RevoluteJoint_GetMotorTorque( b3JointId jointId ); + +/// Set the revolute joint maximum motor torque, usually in newton-meters +B3_API void b3RevoluteJoint_SetMaxMotorTorque( b3JointId jointId, float torque ); + +/// Get the revolute joint maximum motor torque, usually in newton-meters +B3_API float b3RevoluteJoint_GetMaxMotorTorque( b3JointId jointId ); + +/**@}*/ + +/** + * @defgroup spherical_joint Spherical Joint + * @brief A spherical joint allows for relative rotation in the 3D space with no relative translation. + * + * Also called a *ball-in-socket* or *point-to-point* joint. + * @{ + */ + +/// Create a revolute joint +/// @see b3RevoluteJointDef for details +B3_API b3JointId b3CreateSphericalJoint( b3WorldId worldId, const b3SphericalJointDef* def ); + +/// Enable/disable the spherical joint cone limit +B3_API void b3SphericalJoint_EnableConeLimit( b3JointId jointId, bool enableLimit ); + +/// Is the spherical joint cone limit enabled? +B3_API bool b3SphericalJoint_IsConeLimitEnabled( b3JointId jointId ); + +/// Get the spherical joint cone limit in radians +B3_API float b3SphericalJoint_GetConeLimit( b3JointId jointId ); + +/// Set the spherical joint limits in radians +B3_API void b3SphericalJoint_SetConeLimit( b3JointId jointId, float angleRadians ); + +/// Get the spherical joint current cone angle in radians. +B3_API float b3SphericalJoint_GetConeAngle( b3JointId jointId ); + +/// Enable/disable the spherical joint limit +B3_API void b3SphericalJoint_EnableTwistLimit( b3JointId jointId, bool enableLimit ); + +/// Is the spherical joint limit enabled? +B3_API bool b3SphericalJoint_IsTwistLimitEnabled( b3JointId jointId ); + +/// Get the spherical joint lower limit in radians +B3_API float b3SphericalJoint_GetLowerTwistLimit( b3JointId jointId ); + +/// Get the spherical joint upper limit in radians +B3_API float b3SphericalJoint_GetUpperTwistLimit( b3JointId jointId ); + +/// Set the spherical joint limits in radians +B3_API void b3SphericalJoint_SetTwistLimits( b3JointId jointId, float lowerLimitRadians, float upperLimitRadians ); + +/// Get the spherical joint current twist angle in radians. +B3_API float b3SphericalJoint_GetTwistAngle( b3JointId jointId ); + +/// Enable/disable the spherical joint spring +B3_API void b3SphericalJoint_EnableSpring( b3JointId jointId, bool enableSpring ); + +/// It the spherical angular spring enabled? +B3_API bool b3SphericalJoint_IsSpringEnabled( b3JointId jointId ); + +/// Set the spherical joint spring stiffness in Hertz +B3_API void b3SphericalJoint_SetSpringHertz( b3JointId jointId, float hertz ); + +/// Get the spherical joint spring stiffness in Hertz +B3_API float b3SphericalJoint_GetSpringHertz( b3JointId jointId ); + +/// Set the spherical joint spring damping ratio, non-dimensional +B3_API void b3SphericalJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio ); + +/// Get the spherical joint spring damping ratio, non-dimensional +B3_API float b3SphericalJoint_GetSpringDampingRatio( b3JointId jointId ); + +/// Set the spherical joint spring target rotation +B3_API void b3SphericalJoint_SetTargetRotation( b3JointId jointId, b3Quat targetRotation ); + +/// Get the spherical joint spring target rotation +B3_API b3Quat b3SphericalJoint_GetTargetRotation( b3JointId jointId ); + +/// Enable/disable a spherical joint motor +B3_API void b3SphericalJoint_EnableMotor( b3JointId jointId, bool enableMotor ); + +/// Is the spherical joint motor enabled? +B3_API bool b3SphericalJoint_IsMotorEnabled( b3JointId jointId ); + +/// Set the spherical joint motor velocity in radians per second +B3_API void b3SphericalJoint_SetMotorVelocity( b3JointId jointId, b3Vec3 motorVelocity ); + +/// Get the spherical joint motor velocity in radians per second +B3_API b3Vec3 b3SphericalJoint_GetMotorVelocity( b3JointId jointId ); + +/// Get the spherical joint current motor torque, usually in newton-meters +B3_API b3Vec3 b3SphericalJoint_GetMotorTorque( b3JointId jointId ); + +/// Set the spherical joint maximum motor torque, usually in newton-meters +B3_API void b3SphericalJoint_SetMaxMotorTorque( b3JointId jointId, float torque ); + +/// Get the spherical joint maximum motor torque, usually in newton-meters +B3_API float b3SphericalJoint_GetMaxMotorTorque( b3JointId jointId ); + +/**@}*/ + +/** + * @defgroup weld_joint Weld Joint + * @brief A weld joint fully constrains the relative transform between two bodies while allowing for springiness + * + * A weld joint constrains the relative rotation and translation between two bodies. Both rotation and translation + * can have damped springs. + * + * @note The accuracy of weld joint is limited by the accuracy of the solver. Long chains of weld joints may flex. + * @{ + */ + +/// Create a weld joint +/// @see b3WeldJointDef for details +B3_API b3JointId b3CreateWeldJoint( b3WorldId worldId, const b3WeldJointDef* def ); + +/// Set the weld joint linear stiffness in Hertz. 0 is rigid. +B3_API void b3WeldJoint_SetLinearHertz( b3JointId jointId, float hertz ); + +/// Get the weld joint linear stiffness in Hertz +B3_API float b3WeldJoint_GetLinearHertz( b3JointId jointId ); + +/// Set the weld joint linear damping ratio (non-dimensional) +B3_API void b3WeldJoint_SetLinearDampingRatio( b3JointId jointId, float dampingRatio ); + +/// Get the weld joint linear damping ratio (non-dimensional) +B3_API float b3WeldJoint_GetLinearDampingRatio( b3JointId jointId ); + +/// Set the weld joint angular stiffness in Hertz. 0 is rigid. +B3_API void b3WeldJoint_SetAngularHertz( b3JointId jointId, float hertz ); + +/// Get the weld joint angular stiffness in Hertz +B3_API float b3WeldJoint_GetAngularHertz( b3JointId jointId ); + +/// Set weld joint angular damping ratio, non-dimensional +B3_API void b3WeldJoint_SetAngularDampingRatio( b3JointId jointId, float dampingRatio ); + +/// Get the weld joint angular damping ratio, non-dimensional +B3_API float b3WeldJoint_GetAngularDampingRatio( b3JointId jointId ); + +/** @} */ + +/** + * @defgroup wheel_joint Wheel Joint + * The wheel joint can be used to simulate wheels on vehicles. + * + * The wheel joint restricts body B to move along a local axis in body A. Body B is free to + * rotate. Supports a linear spring, linear limits, and a rotational motor. + * + * @{ + */ + +/// Create a wheel joint +/// @see b3WheelJointDef for details +B3_API b3JointId b3CreateWheelJoint( b3WorldId worldId, const b3WheelJointDef* def ); + +/// Enable/disable the wheel joint spring +B3_API void b3WheelJoint_EnableSuspension( b3JointId jointId, bool flag ); + +/// Is the wheel joint spring enabled? +B3_API bool b3WheelJoint_IsSuspensionEnabled( b3JointId jointId ); + +/// Set the wheel joint stiffness in Hertz +B3_API void b3WheelJoint_SetSuspensionHertz( b3JointId jointId, float hertz ); + +/// Get the wheel joint stiffness in Hertz +B3_API float b3WheelJoint_GetSuspensionHertz( b3JointId jointId ); + +/// Set the wheel joint damping ratio, non-dimensional +B3_API void b3WheelJoint_SetSuspensionDampingRatio( b3JointId jointId, float dampingRatio ); + +/// Get the wheel joint damping ratio, non-dimensional +B3_API float b3WheelJoint_GetSuspensionDampingRatio( b3JointId jointId ); + +/// Enable/disable the wheel joint limit +B3_API void b3WheelJoint_EnableSuspensionLimit( b3JointId jointId, bool flag ); + +/// Is the wheel joint limit enabled? +B3_API bool b3WheelJoint_IsSuspensionLimitEnabled( b3JointId jointId ); + +/// Get the wheel joint lower limit +B3_API float b3WheelJoint_GetLowerSuspensionLimit( b3JointId jointId ); + +/// Get the wheel joint upper limit +B3_API float b3WheelJoint_GetUpperSuspensionLimit( b3JointId jointId ); + +/// Set the wheel joint limits +B3_API void b3WheelJoint_SetSuspensionLimits( b3JointId jointId, float lower, float upper ); + +/// Enable/disable the wheel joint motor +B3_API void b3WheelJoint_EnableSpinMotor( b3JointId jointId, bool flag ); + +/// Is the wheel joint motor enabled? +B3_API bool b3WheelJoint_IsSpinMotorEnabled( b3JointId jointId ); + +/// Set the wheel joint motor speed in radians per second +B3_API void b3WheelJoint_SetSpinMotorSpeed( b3JointId jointId, float speed ); + +/// Get the wheel joint motor speed in radians per second +B3_API float b3WheelJoint_GetSpinMotorSpeed( b3JointId jointId ); + +/// Set the wheel joint maximum motor torque, usually in newton-meters +B3_API void b3WheelJoint_SetMaxSpinTorque( b3JointId jointId, float torque ); + +/// Get the wheel joint maximum motor torque, usually in newton-meters +B3_API float b3WheelJoint_GetMaxSpinTorque( b3JointId jointId ); + +/// Get the wheel joint current motor torque, usually in newton-meters +B3_API float b3WheelJoint_GetSpinTorque( b3JointId jointId ); + +/// Enable/disable wheel steering. Steering allows the wheel to rotate about the suspension axis. +B3_API void b3WheelJoint_EnableSteering( b3JointId jointId, bool flag ); + +/// Can the wheel steer? +B3_API bool b3WheelJoint_IsSteeringEnabled( b3JointId jointId ); + +/// Set the wheel joint steering stiffness in Hertz +B3_API void b3WheelJoint_SetSteeringHertz( b3JointId jointId, float hertz ); + +/// Get the wheel joint steering stiffness in Hertz +B3_API float b3WheelJoint_GetSteeringHertz( b3JointId jointId ); + +/// Set the wheel joint steering damping ratio, non-dimensional +B3_API void b3WheelJoint_SetSteeringDampingRatio( b3JointId jointId, float dampingRatio ); + +/// Get the wheel joint steering damping ratio, non-dimensional +B3_API float b3WheelJoint_GetSteeringDampingRatio( b3JointId jointId ); + +/// Set the wheel joint maximum steering torque in N*m. +B3_API void b3WheelJoint_SetMaxSteeringTorque( b3JointId jointId, float torque ); + +/// Get the wheel joint maximum steering torque in N*m. +B3_API float b3WheelJoint_GetMaxSteeringTorque( b3JointId jointId ); + +/// Enable/disable the wheel joint steering limit +B3_API void b3WheelJoint_EnableSteeringLimit( b3JointId jointId, bool flag ); + +/// Is the wheel joint steering limit enabled? +B3_API bool b3WheelJoint_IsSteeringLimitEnabled( b3JointId jointId ); + +/// Get the wheel joint lower steering limit in radians +B3_API float b3WheelJoint_GetLowerSteeringLimit( b3JointId jointId ); + +/// Get the wheel joint upper steering limit in radians +B3_API float b3WheelJoint_GetUpperSteeringLimit( b3JointId jointId ); + +/// Set the wheel joint steering limits in radians +B3_API void b3WheelJoint_SetSteeringLimits( b3JointId jointId, float lowerRadians, float upperRadians ); + +/// Set the wheel joint target steering angle in radians +B3_API void b3WheelJoint_SetTargetSteeringAngle( b3JointId jointId, float radians ); + +/// Get the wheel joint target steering angle in radians +B3_API float b3WheelJoint_GetTargetSteeringAngle( b3JointId jointId ); + +/// Get the current spin speed in radians per second +B3_API float b3WheelJoint_GetSpinSpeed( b3JointId jointId ); + +/// Get the current spin torque in N*m +B3_API float b3WheelJoint_GetSpinTorque( b3JointId jointId ); + +/// Get the current steering angle in radians +B3_API float b3WheelJoint_GetSteeringAngle( b3JointId jointId ); + +/// Get the current steering torque in N*m +B3_API float b3WheelJoint_GetSteeringTorque( b3JointId jointId ); + +/**@}*/ + +/**@}*/ + +/** + * @defgroup contact Contact + * Access to contacts + * @{ + */ + +/// Contact identifier validation. Provides validation for up to 2^32 allocations. +B3_API bool b3Contact_IsValid( b3ContactId id ); + +/// Get the manifolds for a contact. The manifold may have no points if the contact is not touching. +B3_API b3ContactData b3Contact_GetData( b3ContactId contactId ); + +/**@}*/ diff --git a/include/box3d/collision.h b/include/box3d/collision.h new file mode 100644 index 0000000..98dec64 --- /dev/null +++ b/include/box3d/collision.h @@ -0,0 +1,1543 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "base.h" +#include "math_functions.h" + +#include +#include + +/// Material properties supported per triangle on meshes and height fields +/// @ingroup shape +typedef struct b3SurfaceMaterial +{ + /// The Coulomb (dry) friction coefficient, usually in the range [0,1]. + float friction; + + /// The coefficient of restitution (bounce) usually in the range [0,1]. + /// https://en.wikipedia.org/wiki/Coefficient_of_restitution + float restitution; + + /// The rolling resistance usually in the range [0,1]. This is only used for spheres and capsules. + float rollingResistance; + + /// The tangent velocity for conveyor belts. This is local to the shape and will be projected + /// onto the contact surface. + b3Vec3 tangentVelocity; + + /// User material identifier. This is passed with query results and to friction and restitution + /// combining functions. It is not used internally. + uint64_t userMaterialId; + + /// Custom debug draw color. Ignored if 0. + /// @see b3HexColor + uint32_t customColor; +} b3SurfaceMaterial; + +/// Use this to initialize your surface material +/// @ingroup shape +B3_API b3SurfaceMaterial b3DefaultSurfaceMaterial( void ); + +/** + * @defgroup geometry Geometry + * @brief Geometry types and algorithms + * + * Definitions of spheres, capsules, hulls, meshes, and height fields. + * Functions should take the shape as the first argument to assist editor auto-complete. + * @{ + */ + +#define B3_MAX_SHAPE_CAST_POINTS 64 + +/// Low level ray cast input data +typedef struct b3RayCastInput +{ + /// Start point of the ray cast + b3Vec3 origin; + + /// Translation of the ray cast + b3Vec3 translation; + + /// The maximum fraction of the translation to consider, typically 1 + float maxFraction; +} b3RayCastInput; + +/// A shape proxy is used by the GJK algorithm. It can represent a convex shape. +typedef struct b3ShapeProxy +{ + /// The point cloud + const b3Vec3* points; + + /// The number of points. Do not exceed B3_MAX_SHAPE_CAST_POINTS. + int count; + + /// The external radius of the point cloud + float radius; +} b3ShapeProxy; + +/// Low level shape cast input in generic form. This allows casting an arbitrary point +/// cloud wrap with a radius. For example, a sphere is a single point with a non-zero radius. +/// A capsule is two points with a non-zero radius. A box is four points with a zero radius. +typedef struct b3ShapeCastInput +{ + /// A generic query shape + b3ShapeProxy proxy; + + /// The translation of the shape cast + b3Vec3 translation; + + /// The maximum fraction of the translation to consider, typically 1 + float maxFraction; + + /// Allow shape cast to encroach when initially touching. This only works if the radius is greater than zero. + bool canEncroach; +} b3ShapeCastInput; + +/// Low level ray cast or shape-cast output data +typedef struct b3CastOutput +{ + /// The surface normal at the hit point + b3Vec3 normal; + + /// The surface hit point + b3Vec3 point; + + /// The fraction of the input translation at collision + float fraction; + + /// The number of iterations used + int iterations; + + /// The index of the mesh or height field triangle hit + int triangleIndex; + + /// The index of the compound child shape + int childIndex; + + /// The material index. May be -1 for null. + int materialIndex; + + /// Did the cast hit? + bool hit; +} b3CastOutput; + +/** + * @defgroup tree Dynamic Tree + * The dynamic tree is a binary AABB tree to organize and query large numbers of geometric objects + * + * Box2D uses the dynamic tree internally to sort collision shapes into a binary bounding volume hierarchy. + * This data structure may have uses in games for organizing other geometry data and may be used independently + * of Box2D rigid body simulation. + * + * A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt. + * A dynamic tree arranges data in a binary tree to accelerate + * queries such as AABB queries and ray casts. Leaf nodes are proxies + * with an AABB. These are used to hold a user collision object. + * Nodes are pooled and relocatable, so I use node indices rather than pointers. + * The dynamic tree is made available for advanced users that would like to use it to organize + * spatial game data besides rigid bodies. + * @{ + */ + +typedef enum b3TreeNodeFlags +{ + b3_allocatedNode = 0x0001, + b3_enlargedNode = 0x0002, + b3_leafNode = 0x0004, +} b3TreeNodeFlags; + +typedef struct b3TreeNodeChildren +{ + int child1; + int child2; +} b3TreeNodeChildren; + +/// A node in the dynamic tree. This is private data placed here for performance reasons. +/// todo test padding to 64 bytes to avoid straddling cache lines +typedef struct b3TreeNode +{ + /// The node bounding box + b3AABB aabb; // 24 + + /// Category bits for collision filtering + uint64_t categoryBits; // 8 + + union + { + /// Children (internal node) + b3TreeNodeChildren children; + + /// User data (leaf node) + uint64_t userData; + }; // 8 + + union + { + /// The node parent index (allocated node) + int parent; + + /// The node freelist next index (free node) + int next; + }; // 4 + + /// Height of the node. Leaves have a height of 0. + uint16_t height; // 2 + uint16_t flags; // 2 +} b3TreeNode; + +#define B3_DYNAMIC_TREE_VERSION 0x8E867C390754064Bull + +/// The dynamic tree structure. This should be considered private data. +/// It is placed here for performance reasons. +typedef struct b3DynamicTree +{ + uint64_t version; + + /// The tree nodes + b3TreeNode* nodes; + + /// The root index + int root; + + /// The number of nodes + int nodeCount; + + /// The allocated node space + int nodeCapacity; + + /// Number of proxies created + int proxyCount; + + /// Node free list + int freeList; + + /// Leaf indices for rebuild + int* leafIndices; + + /// Leaf bounding boxes for rebuild + b3AABB* leafBoxes; + + /// Leaf bounding box centers for rebuild + b3Vec3* leafCenters; + + /// Bins for sorting during rebuild + int* binIndices; + + /// Allocated space for rebuilding + int rebuildCapacity; +} b3DynamicTree; + +/// These are performance results returned by dynamic tree queries. +typedef struct b3TreeStats +{ + /// Number of internal nodes visited during the query + int nodeVisits; + + /// Number of leaf nodes visited during the query + int leafVisits; +} b3TreeStats; + +/// Constructing the tree initializes the node pool. +B3_API b3DynamicTree b3DynamicTree_Create( int proxyCapacity ); + +/// Destroy the tree, freeing the node pool. +B3_API void b3DynamicTree_Destroy( b3DynamicTree* tree ); + +/// Create a proxy. Provide an AABB and a userData value. +B3_API int b3DynamicTree_CreateProxy( b3DynamicTree* tree, b3AABB aabb, uint64_t categoryBits, uint64_t userData ); + +/// Destroy a proxy. This asserts if the id is invalid. +B3_API void b3DynamicTree_DestroyProxy( b3DynamicTree* tree, int proxyId ); + +/// Move a proxy to a new AABB by removing and reinserting into the tree. +B3_API void b3DynamicTree_MoveProxy( b3DynamicTree* tree, int proxyId, b3AABB aabb ); + +/// Enlarge a proxy and enlarge ancestors as necessary. +B3_API void b3DynamicTree_EnlargeProxy( b3DynamicTree* tree, int proxyId, b3AABB aabb ); + +/// Modify the category bits on a proxy. This is an expensive operation. +B3_API void b3DynamicTree_SetCategoryBits( b3DynamicTree* tree, int proxyId, uint64_t categoryBits ); + +/// Get the category bits on a proxy. +B3_API uint64_t b3DynamicTree_GetCategoryBits( b3DynamicTree* tree, int proxyId ); + +/// This function receives proxies found in the AABB query. +/// @return true if the query should continue +typedef bool b3TreeQueryCallbackFcn( int proxyId, uint64_t userData, void* context ); + +/// Query an AABB for overlapping proxies. The callback class is called for each proxy that overlaps the supplied AABB. +/// @return performance data +B3_API b3TreeStats b3DynamicTree_Query( const b3DynamicTree* tree, b3AABB aabb, uint64_t maskBits, bool requireAllBits, + b3TreeQueryCallbackFcn* callback, void* context ); + +/// This function receives the minimum distance squared so far and proxy to check in the closest query. +/// @return minimum distance squared to user objects in the proxy +typedef float b3TreeQueryClosestCallbackFcn( float distanceSqrMin, int proxyId, uint64_t userData, void* context ); + +/// Query an AABB for the closest object. The callback class is called for each proxy that might be closest to the supplied point. +/// @param tree the dynamic tree to query +/// @param point the query point +/// @param maskBits nodes are skipped if the bit-wise AND with the node category bits is zero +/// @param requireAllBits nodes are skipped if the bit-wise AND with the node category bits does not equal the maskBits +/// @param callback a user provided instance of b3TreeQueryClosestCallbackFcn +/// @param context a user context object that is provide to the callback +/// @param minDistanceSqr the initial and final minimum squared distance. Provide a small initial to restrict the search and +/// improve performance. If the value is large this query has performance that scales linearly with the number of proxies and +/// would be slower than a brute force search. +/// @return performance data +B3_API b3TreeStats b3DynamicTree_QueryClosest( const b3DynamicTree* tree, b3Vec3 point, uint64_t maskBits, bool requireAllBits, + b3TreeQueryClosestCallbackFcn* callback, void* context, float* minDistanceSqr ); + +/// This function receives clipped ray cast input for a proxy. The function +/// returns the new ray fraction. +/// - return a value of 0 to terminate the ray cast +/// - return a value less than input->maxFraction to clip the ray +/// - return a value of input->maxFraction to continue the ray cast without clipping +typedef float b3TreeRayCastCallbackFcn( const b3RayCastInput* input, int proxyId, uint64_t userData, void* context ); + +/// Ray cast against the proxies in the tree. This relies on the callback +/// to perform a exact ray cast in the case were the proxy contains a shape. +/// The callback also performs the any collision filtering. This has performance +/// roughly equal to k * log(n), where k is the number of collisions and n is the +/// number of proxies in the tree. +/// Bit-wise filtering using mask bits can greatly improve performance in some scenarios. +/// However, this filtering may be approximate, so the user should still apply filtering to results. +/// @param tree the dynamic tree to ray cast +/// @param input the ray cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1) +/// @param maskBits bit mask test: `bool accept = (maskBits & node->categoryBits) != 0;` +/// @param requireAllBits modifies bit mask test: `bool accept = (maskBits & node->categoryBits) == maskBits;` +/// @param callback a callback class that is called for each proxy that is hit by the ray +/// @param context user context that is passed to the callback +/// @return performance data +B3_API b3TreeStats b3DynamicTree_RayCast( const b3DynamicTree* tree, const b3RayCastInput* input, uint64_t maskBits, + bool requireAllBits, b3TreeRayCastCallbackFcn* callback, void* context ); + +/// This function receives clipped ray cast input for a proxy. The function +/// returns the new ray fraction. +/// - return a value of 0 to terminate the ray cast +/// - return a value less than input->maxFraction to clip the ray +/// - return a value of input->maxFraction to continue the ray cast without clipping +typedef float b3TreeShapeCastCallbackFcn( const b3ShapeCastInput* input, int proxyId, uint64_t userData, void* context ); + +/// Ray cast against the proxies in the tree. This relies on the callback +/// to perform a exact ray cast in the case were the proxy contains a shape. +/// The callback also performs the any collision filtering. This has performance +/// roughly equal to k * log(n), where k is the number of collisions and n is the +/// number of proxies in the tree. +/// @param tree the dynamic tree to ray cast +/// @param input the ray cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). +/// @param maskBits bit mask test: `bool accept = (maskBits & node->categoryBits) != 0;` +/// @param requireAllBits modifies bit mask test: `bool accept = (maskBits & node->categoryBits) == maskBits;` +/// @param callback a callback class that is called for each proxy that is hit by the shape +/// @param context user context that is passed to the callback +/// @return performance data +B3_API b3TreeStats b3DynamicTree_ShapeCast( const b3DynamicTree* tree, const b3ShapeCastInput* input, uint64_t maskBits, + bool requireAllBits, b3TreeShapeCastCallbackFcn* callback, void* context ); + +/// Validate this tree. For testing. +B3_API void b3DynamicTree_Validate( const b3DynamicTree* tree ); + +/// Get the height of the binary tree. +B3_API int b3DynamicTree_GetHeight( const b3DynamicTree* tree ); + +/// Get the ratio of the sum of the node areas to the root area. +B3_API float b3DynamicTree_GetAreaRatio( const b3DynamicTree* tree ); + +/// Get the bounding box that contains the entire tree +B3_API b3AABB b3DynamicTree_GetRootBounds( const b3DynamicTree* tree ); + +/// Get the number of proxies created +B3_API int b3DynamicTree_GetProxyCount( const b3DynamicTree* tree ); + +/// Rebuild the tree while retaining subtrees that haven't changed. Returns the number of boxes sorted. +B3_API int b3DynamicTree_Rebuild( b3DynamicTree* tree, bool fullBuild ); + +/// Get the number of bytes used by this tree +B3_API int b3DynamicTree_GetByteCount( const b3DynamicTree* tree ); + +/// Validate this tree. For testing. +B3_API void b3DynamicTree_Validate( const b3DynamicTree* tree ); + +/// Validate this tree has no enlarged AABBs. For testing. +B3_API void b3DynamicTree_ValidateNoEnlarged( const b3DynamicTree* tree ); + +/// Save this tree to a file for debugging +B3_API void b3DynamicTree_Save( const b3DynamicTree* tree, const char* fileName ); + +/// Load a file for debugging +B3_API b3DynamicTree b3DynamicTree_Load( const char* fileName, float scale ); + +/// Get proxy user data +B3_INLINE uint64_t b3DynamicTree_GetUserData( const b3DynamicTree* tree, int proxyId ) +{ + return tree->nodes[proxyId].userData; +} + +/// Get the AABB of a proxy +B3_INLINE b3AABB b3DynamicTree_GetAABB( const b3DynamicTree* tree, int proxyId ) +{ + return tree->nodes[proxyId].aabb; +} + +/**@}*/ + +/// This holds the mass data computed for a shape. +typedef struct b3MassData +{ + float mass; + b3Vec3 center; + + // todo_erin this should be central inertia + b3Matrix3 inertia; +} b3MassData; + +/// A solid sphere +typedef struct b3Sphere +{ + /// The local center + b3Vec3 center; + + /// The radius + float radius; +} b3Sphere; + +/// A solid capsule can be viewed as two hemispheres connected +/// by a rectangle. +typedef struct b3Capsule +{ + /// Local center of the first hemisphere + b3Vec3 center1; + + /// Local center of the second hemisphere + b3Vec3 center2; + + /// The radius of the hemispheres + float radius; +} b3Capsule; + +typedef struct b3HullVertex +{ + // A half-edge that has this vertex as the origin + // Can be used along with edge twins and winding order + // to traverse all the edges connected to this vertex. + uint8_t edge; +} b3HullVertex; + +/// Half-edge for hull data structure +typedef struct b3HullHalfEdge +{ + // Next edge index CCW + uint8_t next; + + // Twin edge index + uint8_t twin; + + // index of origin vertex and point + uint8_t origin; + + // Face to the left of this edge + uint8_t face; +} b3HullHalfEdge; + +typedef struct b3HullFace +{ + // An arbitrary half-edge on this face + uint8_t edge; +} b3HullFace; + +#define B3_HULL_VERSION 0x8F5034CF987D4FD9ull + +/// A convex hull. +/// @note This data structure has data hanging off the end and cannot be directly copied. +typedef struct b3Hull +{ + // Version must be first and match B3_HULL_VERSION + uint64_t version; + int byteCount; + + // Hash of this hull (this field is zero when the hash is computed) + uint32_t hash; + + // Bulk properties + b3AABB aabb; + float surfaceArea; + float volume; + float innerRadius; + b3Vec3 center; + b3Matrix3 centralInertia; + + // Geometry + int vertexCount; + int vertexOffset; + int pointOffset; + + // This is the half-edge count (double the edge count) + int edgeCount; + int edgeOffset; + + int faceCount; + int faceOffset; + int planeOffset; + +} b3Hull; + +B3_INLINE const b3HullVertex* b3GetHullVertices( const b3Hull* hull ) +{ + if ( hull->vertexOffset == 0 ) + { + return NULL; + } + + return (const b3HullVertex*)( (intptr_t)hull + hull->vertexOffset ); +} + +B3_INLINE const b3Vec3* b3GetHullPoints( const b3Hull* hull ) +{ + if ( hull->pointOffset == 0 ) + { + return NULL; + } + + return (const b3Vec3*)( (intptr_t)hull + hull->pointOffset ); +} + +B3_INLINE const b3HullHalfEdge* b3GetHullEdges( const b3Hull* hull ) +{ + if ( hull->edgeOffset == 0 ) + { + return NULL; + } + + return (const b3HullHalfEdge*)( (intptr_t)hull + hull->edgeOffset ); +} + +B3_INLINE const b3HullFace* b3GetHullFaces( const b3Hull* hull ) +{ + if ( hull->faceOffset == 0 ) + { + return NULL; + } + + return (const b3HullFace*)( (intptr_t)hull + hull->faceOffset ); +} + +B3_INLINE const b3Plane* b3GetHullPlanes( const b3Hull* hull ) +{ + if ( hull->planeOffset == 0 ) + { + return NULL; + } + + return (const b3Plane*)( (intptr_t)hull + hull->planeOffset ); +} + +B3_API b3Hull* b3CreateCylinder( float height, float radius, float yOffset, int sides ); +B3_API b3Hull* b3CreateCone( float height, float radius1, float radius2, int slices ); +B3_API b3Hull* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCount ); +B3_API b3Hull* b3CloneHull( const b3Hull* hull ); +B3_API b3Hull* b3CloneAndTransformHull( const b3Hull* original, b3Transform transform, b3Vec3 scale ); +B3_API void b3DestroyHull( b3Hull* hull ); + +/// Efficient box hull +typedef struct b3BoxHull +{ + b3Hull base; + b3HullVertex boxVertices[8]; + b3Vec3 boxPoints[8]; + b3HullHalfEdge boxEdges[24]; + b3HullFace boxFaces[6]; + b3Plane boxPlanes[6]; +} b3BoxHull; + +/// This makes a box hull cube. Do not call b3DestroyHull on this. +B3_API b3BoxHull b3MakeCubeHull( float halfWidth ); + +/// This makes a box hull. Do not call b3DestroyHull on this. +B3_API b3BoxHull b3MakeBoxHull( float hx, float hy, float hz ); + +B3_API b3BoxHull b3MakeOffsetBoxHull( float hx, float hy, float hz, b3Vec3 offset ); + +/// This makes an transformed box hull. Do not call b3DestroyHull on this. +/// @param hx, hy, hz positive half widths +/// @param transform local transform of box +B3_API b3BoxHull b3MakeTransformedBoxHull( float hx, float hy, float hz, b3Transform transform ); + +/// This makes an transformed box hull with post scaling. This is useful for boxes that are scaled in +/// a level editor. Such scaling can have reflection and sheer. In the case of sheer the result +/// may be approximate. If you need to support sheer consider using b3CreateHull. +/// Do not call b3DestroyHull on this. +/// @param halfWidths positive half widths +/// @param transform local transform of box +/// @param postScale scale applied after the transform, may be negative +B3_API b3BoxHull b3MakeScaledBoxHull( b3Vec3 halfWidths, b3Transform transform, b3Vec3 postScale ); + +/// This takes a box with a transform and post scale and converts it into a box with the post scale +/// resolved with new half-widths and transform. This accepts non-uniform and negative scale. +/// This is approximate if there is shear. +/// @param halfWidths [in/out] the box half widths +/// @param transform [in/out] the box transform with rotation and translation +/// @param postScale the post scale being applied to the box after the transform +/// @param minHalfWidth the minimum half width after scale is applied +B3_API void b3ScaleBox( b3Vec3* halfWidths, b3Transform* transform, b3Vec3 postScale, float minHalfWidth ); + +// 64-bit guid that is changed every time the format of the mesh changes +#define B3_MESH_VERSION 0x4A1E7B2C8D5F3091ull + +typedef enum b3MeshEdgeFlags +{ + b3_concaveEdge1 = 0x01, + b3_concaveEdge2 = 0x02, + b3_concaveEdge3 = 0x04, + + b3_inverseConcaveEdge1 = 0x10, + b3_inverseConcaveEdge2 = 0x20, + b3_inverseConcaveEdge3 = 0x40, + + b3_allConcaveEdges = b3_concaveEdge1 | b3_concaveEdge2 | b3_concaveEdge3, + + b3_flatEdge1 = b3_concaveEdge1 | b3_inverseConcaveEdge1, + b3_flatEdge2 = b3_concaveEdge2 | b3_inverseConcaveEdge2, + b3_flatEdge3 = b3_concaveEdge3 | b3_inverseConcaveEdge3, + + b3_allFlatEdges = b3_flatEdge1 | b3_flatEdge2 | b3_flatEdge3, + +} b3MeshEdgeFlags; + +typedef struct b3MeshTriangle +{ + int32_t index1; + int32_t index2; + int32_t index3; +} b3MeshTriangle; + +typedef struct b3MeshNode +{ + b3Vec3 lowerBound; + union + { + // Internal node + struct + { + uint32_t axis : 2; + uint32_t childOffset : 30; + } asNode; + + // Leaf node + struct + { + uint32_t type : 2; + uint32_t triangleCount : 30; + } asLeaf; + } data; + + b3Vec3 upperBound; + uint32_t triangleOffset; +} b3MeshNode; + +/// This is a sorted triangle collision tree. +/// @note This struct has data hanging off the end and cannot be directly copied. +typedef struct b3MeshData +{ + // Version must be first + uint64_t version; + int byteCount; + + // Hash of this mesh (this field is zero when the hash is computed) + uint32_t hash; + + b3AABB bounds; + + /// Combined surface area of all triangles. Single-sided. + float surfaceArea; + + int treeHeight; + int degenerateCount; + + // Offset of the node array in bytes from the struct address + int nodeOffset; + int nodeCount; + + // Offset of the vertex array in bytes from the struct address + int vertexOffset; + int vertexCount; + + // Offset of the triangle array in bytes from the struct address + int triangleOffset; + int triangleCount; + + // Offset of the material array in bytes from the struct address + int materialOffset; + int materialCount; + + // Offset of the triangle flag array in bytes from the struct address + int flagsOffset; +} b3MeshData; + +/// This small struct allows a mesh data to be re-used with different scale. +typedef struct b3Mesh +{ + const b3MeshData* data; + + /// This scale may be non-uniform and have negative components. However, + /// no component may be zero or very small in magnitude. + b3Vec3 scale; + +} b3Mesh; + +// todo these don't need to be inline +B3_INLINE const b3MeshNode* b3GetMeshNodes( const b3MeshData* mesh ) +{ + if ( mesh->nodeOffset == 0 ) + { + return NULL; + } + + return (const b3MeshNode*)( (intptr_t)mesh + mesh->nodeOffset ); +} + +B3_INLINE const b3Vec3* b3GetMeshVertices( const b3MeshData* mesh ) +{ + if ( mesh->vertexOffset == 0 ) + { + return NULL; + } + + return (const b3Vec3*)( (intptr_t)mesh + mesh->vertexOffset ); +} + +B3_INLINE const b3MeshTriangle* b3GetMeshTriangles( const b3MeshData* mesh ) +{ + if ( mesh->triangleOffset == 0 ) + { + return NULL; + } + + return (const b3MeshTriangle*)( (intptr_t)mesh + mesh->triangleOffset ); +} + +// @note count is equal to the triangle count +B3_INLINE const uint8_t* b3GetMeshMaterialIndices( const b3MeshData* mesh ) +{ + if ( mesh->materialOffset == 0 ) + { + return NULL; + } + + return (const uint8_t*)( (intptr_t)mesh + mesh->materialOffset ); +} + +// @note count is equal to the triangle count +B3_INLINE const uint8_t* b3GetMeshFlags( const b3MeshData* mesh ) +{ + if ( mesh->flagsOffset == 0 ) + { + return NULL; + } + + return (const uint8_t*)( (intptr_t)mesh + mesh->flagsOffset ); +} + +/// This is used to create a re-usable collision mesh +typedef struct b3MeshDef +{ + /// Triangle vertices + b3Vec3* vertices; + + /// Triangle vertex indices. 3 for each triangle. + int32_t* indices; + + /// Triangle material index. 1 per triangle. Indexes into b3ShapeDef::materials. + /// This allows different run-time material data to be associated with different + /// instances of this mesh. + uint8_t* materialIndices; + + /// Tolerance for vertex welding in length units. + float weldTolerance; + + int vertexCount; + int triangleCount; + + /// Optionally weld nearby vertices. + bool weldVertices; + + /// Use the median split instead of SAH to speed up mesh creation. Good + /// for meshes that are structured like a grid. + bool useMedianSplit; + + /// Compute triangle adjacency information using shared edges + bool identifyEdges; + +} b3MeshDef; + +/// Creates a grid mesh along the x and z axes. +/// @param xCount the number of rows in the x direction +/// @param zCount the number of rows in the z direction +/// @param cellWidth the width of each cell +/// @param materialCount the number of materials to generate +/// @param identifyEdges compute adjacency information +B3_API b3MeshData* b3CreateGridMesh( int xCount, int zCount, float cellWidth, int materialCount, bool identifyEdges ); + +B3_API b3MeshData* b3CreateWaveMesh( int xCount, int zCount, float cellWidth, float amplitude, float rowFrequency, + float columnFrequency ); +B3_API b3MeshData* b3CreateTorusMesh( int radialResolution, int tubularResolution, float radius, float thickness ); +B3_API b3MeshData* b3CreateBoxMesh( b3Vec3 center, b3Vec3 extent, bool identifyEdges ); +B3_API b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, int degenerateCapacity ); +B3_API void b3DestroyMesh( b3MeshData* mesh ); +B3_API int b3GetHeight( const b3MeshData* mesh ); + +/// Data used to create a height field +typedef struct b3HeightFieldDef +{ + // Grid point heights + // count = countX * countZ + float* heights; + + // Grid cell material + // A value of 0xFF is reserved for holes + // count = (countX - 1) * (countZ - 1) + uint8_t* materialIndices; + + b3Vec3 scale; + int countX; + int countZ; + + // Global minimum and maximum heights used for quantization. This is important + // if you want height fields to be placed next to each other and line up exactly. + // In that case, both height fields should use the same minimum and maximum heights. + // All height values are clamped to this range. + // These values are in unscaled space. + float globalMinimumHeight; + float globalMaximumHeight; + + bool clockwiseWinding; +} b3HeightFieldDef; + +// This material index is used to designate holes in a height field. +#define B3_HEIGHT_FIELD_HOLE 0xFF + +// 64-bit guid that is changed every time the format of the height field changes +#define B3_HEIGHT_FIELD_VERSION 0xB6E83F1D947A28C5ull + +// A height field with compressed storage. +typedef struct b3HeightField +{ + // Version must be first + uint64_t version; + + // todo_erin use offset storage + uint16_t* compressedHeights; + uint8_t* materialIndices; + uint8_t* flags; + + // Hash of this height field (this field is zero when the hash is computed). + // Pointer values above are skipped; the bulk arrays they reference and the + // scalar block below are folded in at construction time. + uint32_t hash; + + b3AABB aabb; + float minHeight; + float maxHeight; + float heightScale; + b3Vec3 scale; + int columnCount; + int rowCount; + bool clockwise; +} b3HeightField; + +// Create a height field +B3_API b3HeightField* b3CreateHeightField( const b3HeightFieldDef* data ); +B3_API b3HeightField* b3CreateGrid( int rowCount, int columnCount, b3Vec3 scale, bool makeHoles ); +B3_API b3HeightField* b3CreateWave( int rowCount, int columnCount, b3Vec3 scale, float rowFrequency, float columnFrequency, + bool makeHoles ); +B3_API void b3DestroyHeightField( b3HeightField* heightField ); + +// Save input height data to a file +B3_API void b3DumpHeightData( const b3HeightFieldDef* data, const char* fileName ); + +// Create a height field by loading a previously saved height data +B3_API b3HeightField* b3LoadHeightField( const char* fileName ); + +typedef struct b3CompoundCapsuleDef +{ + b3Capsule capsule; + b3SurfaceMaterial material; +} b3CompoundCapsuleDef; + +typedef struct b3CompoundHullDef +{ + const b3Hull* hull; + b3Transform transform; + b3SurfaceMaterial material; +} b3CompoundHullDef; + +typedef struct b3CompoundMeshDef +{ + const b3MeshData* meshData; + b3Transform transform; + b3Vec3 scale; + + // This array must line up with the material indices on the triangles. + const b3SurfaceMaterial* materials; + int materialCount; + +} b3CompoundMeshDef; + +typedef struct b3CompoundSphereDef +{ + b3Sphere sphere; + b3SurfaceMaterial material; +} b3CompoundSphereDef; + +/// Definition to creating a compound shape. No data in this structure +/// is referenced in the compound. +typedef struct b3CompoundDef +{ + b3CompoundCapsuleDef* capsules; + int capsuleCount; + + /// Hulls instances. + b3CompoundHullDef* hulls; + int hullCount; + + /// Mesh instances + b3CompoundMeshDef* meshes; + int meshCount; + + /// Spheres are fully cloned + b3CompoundSphereDef* spheres; + int sphereCount; +} b3CompoundDef; + +#define B3_COMPOUND_VERSION ( 0x902AC5D34D9BD452ull ^ B3_DYNAMIC_TREE_VERSION ^ B3_MESH_VERSION ^ B3_HULL_VERSION ) + +/// Meshes used in compounds have limited space for materials. If you have +/// a mesh with many materials, you can use it outside of the the compound. +#define B3_MAX_COMPOUND_MESH_MATERIALS 4 + +typedef struct b3Compound +{ + uint64_t version; + int byteCount; + + // immutable tree + // The tree node pointer must be fixed up using the node offset + // todo use a custom tree like b3MeshData to save space and improve performance + int nodeOffset; + b3DynamicTree tree; + + int materialOffset; + int materialCount; + + int capsuleOffset; + int capsuleCount; + + int hullOffset; + int hullCount; + int sharedHullCount; + + int meshOffset; + int meshCount; + int sharedMeshCount; + + int sphereOffset; + int sphereCount; +} b3Compound; + +typedef struct b3CompoundCapsule +{ + b3Capsule capsule; + int materialIndex; +} b3CompoundCapsule; + +typedef struct b3CompoundHull +{ + const b3Hull* hull; + b3Transform transform; + int materialIndex; +} b3CompoundHull; + +typedef struct b3CompoundMesh +{ + const b3MeshData* meshData; + b3Transform transform; + b3Vec3 scale; + + // This is used to access the surface material from b3GetCompoundMaterials + // materialIndex = materialIndices[triangle->materialIndex] + int materialIndices[B3_MAX_COMPOUND_MESH_MATERIALS]; +} b3CompoundMesh; + +typedef struct b3CompoundSphere +{ + b3Sphere sphere; + int materialIndex; +} b3CompoundSphere; + +B3_API b3CompoundCapsule b3GetCompoundCapsule( const b3Compound* compound, int index ); +B3_API b3CompoundHull b3GetCompoundHull( const b3Compound* compound, int index ); +B3_API b3CompoundMesh b3GetCompoundMesh( const b3Compound* compound, int index ); +B3_API b3CompoundSphere b3GetCompoundSphere( const b3Compound* compound, int index ); +B3_API const b3SurfaceMaterial* b3GetCompoundMaterials( const b3Compound* compound ); + +// All input data in the definition is cloned into the resulting compound +B3_API b3Compound* b3CreateCompound( const b3CompoundDef* def ); +B3_API void b3DestroyCompound( b3Compound* compound ); + +/// If bytes is null then this returns the number of required bytes. This clones all the +/// data into the bytes buffer. This is expected to run offline or asynchronously. +/// This mutates the compound to nullify pointers, leaving the compound in an unusable state. +B3_API uint8_t* b3ConvertCompoundToBytes( b3Compound* compound ); + +/// Convert bytes to compound. This does not clone. The bytes must remain in scope while the +/// compound is used. This is done to improve run-time performance and allow for instancing. +/// The bytes are mutated to fixup pointers. +B3_API b3Compound* b3ConvertBytesToCompound( uint8_t* bytes, int byteCount ); + +/// Compute mass properties of a sphere +B3_API b3MassData b3ComputeSphereMass( const b3Sphere* shape, float density ); + +/// Compute mass properties of a capsule +B3_API b3MassData b3ComputeCapsuleMass( const b3Capsule* shape, float density ); + +/// Compute mass properties of a hull +B3_API b3MassData b3ComputeHullMass( const b3Hull* shape, float density ); + +/// Compute the bounding box of a transformed sphere +B3_API b3AABB b3ComputeSphereAABB( const b3Sphere* shape, b3Transform transform ); + +/// Compute the bounding box of a transformed capsule +B3_API b3AABB b3ComputeCapsuleAABB( const b3Capsule* shape, b3Transform transform ); + +/// Compute the bounding box of a transformed hull +B3_API b3AABB b3ComputeHullAABB( const b3Hull* shape, b3Transform transform ); + +/// Compute the bounding box of a transformed mesh. Scale may be non-uniform and have negative components. +B3_API b3AABB b3ComputeMeshAABB( const b3MeshData* shape, b3Transform transform, b3Vec3 scale ); + +/// Compute the bounding box of a transformed height-field +B3_API b3AABB b3ComputeHeightFieldAABB( const b3HeightField* shape, b3Transform transform ); + +/// Compute the bounding box of a compound +B3_API b3AABB b3ComputeCompoundAABB( const b3Compound* shape, b3Transform transform ); + +/// Use this to ensure your ray cast input is valid and avoid internal assertions. +B3_API bool b3IsValidRay( const b3RayCastInput* input ); + +/// Overlap shape versus capsule +B3_API bool b3OverlapCapsule( const b3Capsule* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ); + +/// Overlap shape versus compound +B3_API bool b3OverlapCompound( const b3Compound* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ); + +/// Overlap shape versus height field +B3_API bool b3OverlapHeightField( const b3HeightField* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ); + +/// Overlap shape versus hull +B3_API bool b3OverlapHull( const b3Hull* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ); + +/// Overlap shape versus mesh +B3_API bool b3OverlapMesh( const b3Mesh* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ); + +/// Overlap shape versus sphere +B3_API bool b3OverlapSphere( const b3Sphere* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ); + +/// Ray cast versus sphere in local space. Initial overlap is treated as a miss. +B3_API b3CastOutput b3RayCastSphere( const b3Sphere* shape, const b3RayCastInput* input ); + +/// Ray cast versus hollow sphere in local space. +B3_API b3CastOutput b3RayCastHollowSphere( const b3Sphere* shape, const b3RayCastInput* input ); + +/// Ray cast versus capsule in local space. Initial overlap is treated as a miss. +B3_API b3CastOutput b3RayCastCapsule( const b3Capsule* shape, const b3RayCastInput* input ); + +/// Ray cast versus compound in local space. Initial overlap is treated as a miss. +B3_API b3CastOutput b3RayCastCompound( const b3Compound* shape, const b3RayCastInput* input ); + +/// Ray cast versus hull shape in local space. Initial overlap is treated as a miss. +B3_API b3CastOutput b3RayCastHull( const b3Hull* shape, const b3RayCastInput* input ); + +/// Ray cast versus mesh in local space. +B3_API b3CastOutput b3RayCastMesh( const b3Mesh* shape, const b3RayCastInput* input ); + +/// Ray cast versus height field in local space. +B3_API b3CastOutput b3RayCastHeightField( const b3HeightField* shape, const b3RayCastInput* input ); + +/// Shape cast versus a sphere. Initial overlap is treated as a miss. +B3_API b3CastOutput b3ShapeCastSphere( const b3Sphere* shape, const b3ShapeCastInput* input ); + +/// Shape cast versus a capsule. Initial overlap is treated as a miss. +B3_API b3CastOutput b3ShapeCastCapsule( const b3Capsule* shape, const b3ShapeCastInput* input ); + +/// Shape cast versus compound. Initial overlap is treated as a miss. +B3_API b3CastOutput b3ShapeCastCompound( const b3Compound* shape, const b3ShapeCastInput* input ); + +/// Shape cast versus a hull. Initial overlap is treated as a miss. +B3_API b3CastOutput b3ShapeCastHull( const b3Hull* shape, const b3ShapeCastInput* input ); + +/// Shape cast versus a mesh. Initial overlap is treated as a miss. +B3_API b3CastOutput b3ShapeCastMesh( const b3Mesh* shape, const b3ShapeCastInput* input ); + +/// Shape cast versus a height field. Initial overlap is treated as a miss. +B3_API b3CastOutput b3ShapeCastHeightField( const b3HeightField* shape, const b3ShapeCastInput* input ); + +/// Query callback. +typedef bool b3MeshQueryFcn( b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleIndex, void* context ); + +/// Query a mesh for triangles overlapping a bounding box in local space. May have false positives. Useful for debug draw. +/// @param mesh the mesh to query, includes scale +/// @param bounds the bounding box in local space +/// @param fcn a user function to collect triangles +/// @param context the context sent to the user function. +B3_API void b3QueryMesh( const b3Mesh* mesh, const b3AABB bounds, b3MeshQueryFcn* fcn, void* context ); + +/// Query a height field for triangles overlapping a bounding box in local space. May have false positives. Useful for debug draw. +/// @param heightField the height field to query +/// @param bounds the bounding box in local space +/// @param fcn a user function to collect triangles +/// @param context the context sent to the user function. +B3_API void b3QueryHeightField( const b3HeightField* heightField, b3AABB bounds, b3MeshQueryFcn* fcn, void* context ); + +/** + * @defgroup distance Distance + * Functions for computing the distance between shapes. + * + * These are advanced functions you can use to perform distance calculations. There + * are functions for computing the closest points between shapes, doing linear shape casts, + * and doing rotational shape casts. The latter is called time of impact (TOI). + * @{ + */ + +/// Result of computing the distance between two line segments +typedef struct b3SegmentDistanceResult +{ + /// The closest point on the first segment + b3Vec3 closest1; + + /// The closest point on the second segment + b3Vec3 closest2; + + /// The barycentric coordinate on the first segment + float fraction1; + + /// The barycentric coordinate on the second segment + float fraction2; + + /// The squared distance between the closest points + float distanceSquared; +} b3SegmentDistanceResult; + +/// Compute the distance between two line segments, clamping at the end points if needed. +B3_API b3SegmentDistanceResult b3SegmentDistance( b3Vec3 p1, b3Vec3 q1, b3Vec3 p2, b3Vec3 q2 ); + +/// Used to warm start the GJK simplex. If you call this function multiple times with nearby +/// transforms this might improve performance. Otherwise you can zero initialize this. +/// The distance cache must be initialized to zero on the first call. +/// Users should generally just zero initialize this structure for each call. +typedef struct b3SimplexCache +{ + /// Value use to compare length, area, volume of two simplexes. + float metric; + + // todo use an index of 0xFF as a sentinel and remove the count + /// The number of stored simplex points + uint16_t count; + + /// The cached simplex indices on shape A + uint8_t indexA[4]; + + /// The cached simplex indices on shape B + uint8_t indexB[4]; + +} b3SimplexCache; + +static const b3SimplexCache b3_emptyDistanceCache = B3_ZERO_INIT; + +/// Input for b3ShapeDistance +typedef struct b3DistanceInput +{ + /// The proxy for shape A + b3ShapeProxy proxyA; + + /// The proxy for shape B + b3ShapeProxy proxyB; + + /// The world transform for shape A + b3Transform transformA; + + /// The world transform for shape B + b3Transform transformB; + + /// Should the proxy radius be considered? + bool useRadii; +} b3DistanceInput; + +/// Output for b3ShapeDistance +typedef struct b3DistanceOutput +{ + b3Vec3 pointA; ///< Closest point on shapeA + b3Vec3 pointB; ///< Closest point on shapeB + b3Vec3 normal; ///< Normal vector pointing from A to B + float distance; ///< The final distance, zero if overlapped + int iterations; ///< Number of GJK iterations used + int simplexCount; ///< The number of simplexes stored in the simplex array +} b3DistanceOutput; + +/// Simplex vertex for debugging the GJK algorithm +typedef struct b3SimplexVertex +{ + b3Vec3 wA; ///< support point in proxyA + b3Vec3 wB; ///< support point in proxyB + b3Vec3 w; ///< wB - wA + float a; ///< barycentric coordinates + int indexA; ///< wA index + int indexB; ///< wB index +} b3SimplexVertex; + +/// Simplex from the GJK algorithm +typedef struct b3Simplex +{ + b3SimplexVertex vertices[4]; ///< vertices + int count; ///< number of valid vertices +} b3Simplex; + +/// Compute the closest points between two shapes represented as point clouds. +/// b3SimplexCache cache is input/output. On the first call set b3SimplexCache.count to zero. +/// The underlying GJK algorithm may be debugged by passing in debug simplexes and capacity. You may pass in NULL and 0 for these. +/// todo make cache optional (can be null) +B3_API b3DistanceOutput b3ShapeDistance( const b3DistanceInput* input, b3SimplexCache* cache, b3Simplex* simplexes, + int simplexCapacity ); + +/// Input parameters for b3ShapeCast +typedef struct b3ShapeCastPairInput +{ + b3ShapeProxy proxyA; ///< The proxy for shape A + b3ShapeProxy proxyB; ///< The proxy for shape B + b3Transform transformA; ///< The world transform for shape A + b3Transform transformB; ///< The world transform for shape B + b3Vec3 translationB; ///< The translation of shape B + float maxFraction; ///< The fraction of the translation to consider, typically 1 + bool canEncroach; ///< Allows shapes with a radius to move slightly closer if already touching +} b3ShapeCastPairInput; + +/// Perform a linear shape cast of shape B moving and shape A fixed. Determines the hit point, normal, and translation fraction. +B3_API b3CastOutput b3ShapeCast( const b3ShapeCastPairInput* input ); + +/// This describes the motion of a body/shape for TOI computation. Shapes are defined with respect to the body origin, +/// which may not coincide with the center of mass. However, to support dynamics we must interpolate the center of mass +/// position. +typedef struct b3Sweep +{ + b3Vec3 localCenter; ///< Local center of mass position + b3Vec3 c1; ///< Starting center of mass world position + b3Vec3 c2; ///< Ending center of mass world position + b3Quat q1; ///< Starting world rotation + b3Quat q2; ///< Ending world rotation +} b3Sweep; + +/// Evaluate the transform sweep at a specific time. +B3_API b3Transform b3GetSweepTransform( const b3Sweep* sweep, float time ); + +/// Time of impact input +typedef struct b3TOIInput +{ + b3ShapeProxy proxyA; ///< The proxy for shape A + b3ShapeProxy proxyB; ///< The proxy for shape B + b3Sweep sweepA; ///< The movement of shape A + b3Sweep sweepB; ///< The movement of shape B + float maxFraction; ///< Defines the sweep interval [0, tMax] +} b3TOIInput; + +/// Describes the TOI output +typedef enum b3TOIState +{ + b3_toiStateUnknown, + b3_toiStateFailed, + b3_toiStateOverlapped, + b3_toiStateHit, + b3_toiStateSeparated +} b3TOIState; + +/// Time of impact output +typedef struct b3TOIOutput +{ + /// The type of result + b3TOIState state; + + /// The hit point + b3Vec3 point; + + /// The hit normal + b3Vec3 normal; + + /// The sweep time of the collision + float fraction; + + /// The final distance + float distance; + + /// Number of outer iterations + int distanceIterations; + + /// Total number of push back iterations + int pushBackIterations; + + /// Total number of root iterations + int rootIterations; + + bool usedFallback; +} b3TOIOutput; + +/// Compute the upper bound on time before two shapes penetrate. Time is represented as +/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate, +/// non-tunneling collisions. If you change the time interval, you should call this function +/// again. +B3_API b3TOIOutput b3TimeOfImpact( const b3TOIInput* input ); + +/**@}*/ + +/// A manifold point is a contact point belonging to a contact manifold. +/// It holds details related to the geometry and dynamics of the contact points. +/// Box3D uses speculative collision so some contact points may be separated. +/// You may use the maxNormalImpulse to determine if there was an interaction during +/// the time step. +typedef struct b3ManifoldPoint +{ + /// Location of the contact point relative to the bodyA center of mass in world space. + b3Vec3 anchorA; + + /// Location of the contact point relative to the bodyB center of mass in world space. + b3Vec3 anchorB; + + /// The separation of the contact point, negative if penetrating + float separation; + + /// Cached separation used for contact recycling + float baseSeparation; + + /// The impulse along the manifold normal vector. Since Box3D uses sub-stepping, this is + /// result from the final sub-step. + float normalImpulse; + + /// The total normal impulse applied during sub-stepping. This is important + /// to identify speculative contact points that had an interaction in the time step. + float totalNormalImpulse; + + /// Relative normal velocity pre-solve. Used for hit events. If the normal impulse is + /// zero then there was no hit. Negative means shapes are approaching. + float normalVelocity; + + /// Local point for matching + /// Uniquely identifies a contact point between two shapes + uint32_t featureId; + + /// Triangle index if one of the shapes is a mesh or height field + int triangleIndex; + + /// Did this contact point exist the previous step? + bool persisted; +} b3ManifoldPoint; + +/// The maximum number of contact points between two touching shapes. +#define B3_MAX_MANIFOLD_POINTS 4 + +/// A contact manifold describes the contact points between colliding shapes. +/// @note Box3D uses speculative collision so some contact points may be separated. +typedef struct b3Manifold +{ + /// The manifold points. There may be 1 to 4 valid points. + b3ManifoldPoint points[B3_MAX_MANIFOLD_POINTS]; + + /// The unit normal vector in world space, points from shape A to bodyB + b3Vec3 normal; + + // Central friction angular impulse (applied about the normal) + float twistImpulse; + + // Central friction linear impulse + b3Vec3 frictionImpulse; + + // Rolling resistance angular impulse + b3Vec3 rollingImpulse; + + /// The number of contacts points, will be 0, 1, or 2 + int pointCount; + +} b3Manifold; + +// todo adopt this for SAT +typedef enum +{ + b3_invalidAxis = 0, + b3_backsideAxis, + b3_faceAxisA, + b3_faceAxisB, + b3_edgePairAxis, + b3_closestPointsAxis, + + // These are for testing + b3_manualFaceAxisA, + b3_manualFaceAxisB, + b3_manualEdgePairAxis, +} b3SeparatingFeature; + +typedef enum +{ + b3_featureNone = 0, + b3_featureTriangleFace, + b3_featureHullFace, + // v1-v2 + b3_featureEdge1, + // v2-v3 + b3_featureEdge2, + // v3-v1 + b3_featureEdge3, + b3_featureVertex1, + b3_featureVertex2, + b3_featureVertex3 +} b3TriangleFeature; + +typedef struct +{ + float separation; + // b3SeparatingFeature + uint8_t type; + uint8_t indexA; + uint8_t indexB; + uint8_t hit; +} b3SATCache; + +/// Contact points are always the result of two edges intersecting. +/// It can be two edges of the same shape, which is just a shape vertex. +/// Or a contact point can be the result of two edges crossing from different shapes. +/// This is designed to support hull versus hull, but it is adapted to work +/// with all shape types. The feature pair is used to identify contact points +/// for temporal coherence and warm starting. +typedef struct b3FeaturePair +{ + // Incoming type (either edge on shape A or shape B) + uint8_t owner1; + // Incoming edge index (into associated shape array) + uint8_t index1; + // Outgoing type (either edge on shape A or shape B) + uint8_t owner2; + // Outgoing edge index (into associated shape array) + uint8_t index2; +} b3FeaturePair; + +/// A local manifold point and normal in frame A +typedef struct b3LocalManifoldPoint +{ + b3Vec3 point; + b3Vec3 normal; + float separation; + b3FeaturePair pair; + int triangleIndex; +} b3LocalManifoldPoint; + +/// A local manifold with no dynamic information. Used by b3Collide functions. +typedef struct b3LocalManifold +{ + b3Vec3 normal; + b3Plane trianglePlane; + b3LocalManifoldPoint* points; + int pointCount; + int triangleIndex; + int i1, i2, i3; + float squaredDistance; + b3TriangleFeature feature; + int triangleFlags; +} b3LocalManifold; + +B3_API void b3CollideSpheres( b3LocalManifold* manifold, int capacity, const b3Sphere* sphereA, const b3Sphere* sphereB, + b3Transform transformBtoA ); + +B3_API void b3CollideCapsuleAndSphere( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA, const b3Sphere* sphereB, + b3Transform transformBtoA ); + +B3_API void b3CollideHullAndSphere( b3LocalManifold* manifold, int capacity, const b3Hull* hullA, const b3Sphere* sphereB, + b3Transform transformBtoA, b3SimplexCache* cache ); + +B3_API void b3CollideCapsules( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA, const b3Capsule* capsuleB, + b3Transform transformBtoA ); + +B3_API void b3CollideHullAndCapsule( b3LocalManifold* manifold, int capacity, const b3Hull* hullA, const b3Capsule* capsuleB, + b3Transform transformBtoA, b3SimplexCache* cache ); + +B3_API void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3Hull* hullA, const b3Hull* hullB, + b3Transform transformBtoA, b3SATCache* cache ); + +B3_API void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA, + const b3Vec3* triangleB, b3SimplexCache* cache ); + +// b3MeshEdgeFlags +B3_API void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3Hull* hullA, const b3Vec3* triangleB, + int triangleFlags, b3SATCache* cache ); + +B3_API void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const b3Sphere* sphereA, const b3Vec3* triangleB ); + +B3_API bool b3IntersectRayAndAABB( b3Vec3 lowerBound, b3Vec3 upperBound, b3Vec3 p1, b3Vec3 p2, float* minFraction, + float* maxFraction ); + +/**@}*/ + +/** + * @defgroup character + * Experimental character movement solver + * @{ + */ + +typedef struct b3PlaneResult +{ + b3Plane plane; + b3Vec3 point; +} b3PlaneResult; + +typedef struct b3CollisionPlane +{ + b3Plane plane; + float pushLimit; + float push; + bool clipVelocity; +} b3CollisionPlane; + +typedef struct b3PlaneSolverResult +{ + b3Vec3 delta; + int iterationCount; +} b3PlaneSolverResult; + +B3_API b3PlaneSolverResult b3SolvePlanes( b3Vec3 targetDelta, b3CollisionPlane* planes, int count ); +B3_API b3Vec3 b3ClipVector( b3Vec3 vector, const b3CollisionPlane* planes, int count ); + +/**@}*/ + +typedef struct b3ClosestApproachResult +{ + b3Vec3 point1; + float lambda1; + b3Vec3 point2; + float lambda2; +} b3ClosestApproachResult; + +/// Compute the closest point on the segment a-b to the target q +B3_API b3Vec3 b3ClosestPointOnSegment( b3Vec3 a, b3Vec3 b, b3Vec3 q ); + +/// Compute the closest points on two infinite lines +B3_API b3ClosestApproachResult b3ClosestApproachLines( b3Vec3 p1, b3Vec3 d1, b3Vec3 p2, b3Vec3 d2 ); + +/// Compute the closest points on two line segments +B3_API b3ClosestApproachResult b3ClosestApproachSegments( b3Vec3 p1, b3Vec3 q1, b3Vec3 p2, b3Vec3 q2 ); + +typedef struct b3Point2D +{ + b3Vec2 p; + float separation; + int16_t originalIndex; + bool persisted; +} b3Point2D; + +B3_API int b3Hull2D( b3Point2D* pts, int count, b3Point2D* hull ); +B3_API int b3SimplifyHull2D( b3Point2D* hull, int count, int target ); +B3_API int b3CullPoints( b3Point2D* points, int count, int target ); diff --git a/include/box3d/constants.h b/include/box3d/constants.h new file mode 100644 index 0000000..847fff4 --- /dev/null +++ b/include/box3d/constants.h @@ -0,0 +1,82 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "base.h" + +/// Box2D bases all length units on meters, but you may need different units for your game. +/// You can set this value to use different units. This should be done at application startup +/// and only modified once. Default value is 1. +/// @warning This must be modified before any calls to Box2D +B3_API void b3SetLengthUnitsPerMeter( float lengthUnits ); + +/// Get the current length units per meter. +B3_API float b3GetLengthUnitsPerMeter( void ); + +// Used to detect bad values. Positions greater than about 16km will have precision +// problems, so 100km as a limit should be fine in all cases. +#define B3_HUGE ( 100000.0f * b3GetLengthUnitsPerMeter() ) + +/// Maximum parallel workers. Used for some fixed size arrays. +#define B3_MAX_WORKERS 32 + +/// Maximum number of tasks queued per world step. b2EnqueueTaskCallback will never be called +/// more than this per world step. This is related to B3_MAX_WORKERS. With 32 workers, +/// the maximum observed task count is 130. This allows an external task system to use a fixed +/// size array for Box3D task, which may help with creating stable user task pointers. +#define B3_MAX_TASKS 256 + +// Maximum number of colors in the constraint graph. Constraints that cannot +// find a color are added to the overflow set which are solved single-threaded. +// The compound barrel benchmark has minor overflow with 24 colors +#define B3_GRAPH_COLOR_COUNT 24 + +// Number of contact point buckets for counting the number of contact points per +// shape contact pair. This is just for reporting and doesn't affect simulation. +#define B3_CONTACT_MANIFOLD_COUNT_BUCKETS 8 + +// A small length used as a collision and constraint tolerance. Usually it is +// chosen to be numerically significant, but visually insignificant. In meters. +// @warning modifying this can have a significant impact on stability +#define B3_LINEAR_SLOP ( 0.005f * b3GetLengthUnitsPerMeter() ) + +#define B3_MIN_CAPSULE_LENGTH ( B3_LINEAR_SLOP ) + +// The distance between shapes where they are considered overlapped. This is needed +// because GJK may return small positive values for overlapped shapes in degenerate +// configurations. +#define B3_OVERLAP_SLOP ( 0.1f * B3_LINEAR_SLOP ) + +// Maximum number of simultaneous worlds that can be allocated +#ifndef B3_MAX_WORLDS +#define B3_MAX_WORLDS 128 +#endif + +// The maximum rotation of a body per time step. This limit is very large and is used +// to prevent numerical problems. You shouldn't need to adjust this. +// @warning increasing this to 0.5f * b3_pi or greater will break continuous collision. +#define B3_MAX_ROTATION ( 0.25f * B3_PI ) + +// @warning modifying this can have a significant impact on performance and stability +#define B3_SPECULATIVE_DISTANCE ( 4.0f * B3_LINEAR_SLOP ) + +/// The default contact recycling distance. +#define B3_CONTACT_RECYCLE_DISTANCE ( 10.0f * B3_LINEAR_SLOP ) + +// This is used to fatten AABBs in the dynamic tree. This allows proxies +// to move by a small amount without triggering a tree adjustment. This is in meters. +// @warning modifying this can have a significant impact on performance +#define B3_AABB_MARGIN ( 0.05f * b3GetLengthUnitsPerMeter() ) + +// The time that a body must be still before it will go to sleep. In seconds. +#define B3_TIME_TO_SLEEP 0.5f + +// Body and shape name lengths +#define B3_NAME_LENGTH 64 + +// These generous limits allow for easy hashing. See b3ShapePairKey. +#define B3_SHAPE_POWER 22 +#define B3_CHILD_POWER ( 64 - 2 * B3_SHAPE_POWER ) +#define B3_MAX_SHAPES ( 1 << B3_SHAPE_POWER ) +#define B3_MAX_CHILD_SHAPES ( 1 << B3_CHILD_POWER ) diff --git a/include/box3d/id.h b/include/box3d/id.h new file mode 100644 index 0000000..7d59c94 --- /dev/null +++ b/include/box3d/id.h @@ -0,0 +1,172 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +// Note: this file should be stand-alone + +/** + * @defgroup id Ids + * These ids serve as handles to internal Box2D objects. + * These should be considered opaque data and passed by value. + * Include this header if you need the id types and not the whole Box2D API. + * All ids are considered null if initialized to zero. + * + * For example in C++: + * + * @code{.cxx} + * b3WorldId worldId = {}; + * @endcode + * + * Or in C: + * + * @code{.c} + * b3WorldId worldId = {0}; + * @endcode + * + * These are both considered null. + * + * @warning Do not use the internals of these ids. They are subject to change. Ids should be treated as opaque objects. + * @warning You should use ids to access objects in Box2D. Do not access files within the src folder. Such usage is unsupported. + * @{ + */ + +/// World id references a world instance. This should be treated as an opaque handle. +typedef struct b3WorldId +{ + uint16_t index1; + uint16_t generation; +} b3WorldId; + +/// Body id references a body instance. This should be treated as an opaque handle. +typedef struct b3BodyId +{ + int32_t index1; + uint16_t world0; + uint16_t generation; +} b3BodyId; + +/// Shape id references a shape instance. This should be treated as an opaque handle. +typedef struct b3ShapeId +{ + int32_t index1; + uint16_t world0; + uint16_t generation; +} b3ShapeId; + +/// Joint id references a joint instance. This should be treated as an opaque handle. +typedef struct b3JointId +{ + int32_t index1; + uint16_t world0; + uint16_t generation; +} b3JointId; + +/// Contact id references a contact instance. This should be treated as an opaque handled. +typedef struct b3ContactId +{ + int32_t index1; + uint16_t world0; + int16_t padding; + uint32_t generation; +} b3ContactId; + +#ifdef __cplusplus + #define B3_NULL_ID {} + #define B3_ID_INLINE inline +#else + #define B3_NULL_ID { 0 } + #define B3_ID_INLINE static inline +#endif + +/// Use these to make your identifiers null. +/// You may also use zero initialization to get null. +static const b3WorldId b3_nullWorldId = B3_NULL_ID; +static const b3BodyId b3_nullBodyId = B3_NULL_ID; +static const b3ShapeId b3_nullShapeId = B3_NULL_ID; +static const b3JointId b3_nullJointId = B3_NULL_ID; +static const b3ContactId b3_nullContactId = B3_NULL_ID; + +/// Macro to determine if any id is null. +#define B3_IS_NULL( id ) ( id.index1 == 0 ) + +/// Macro to determine if any id is non-null. +#define B3_IS_NON_NULL( id ) ( id.index1 != 0 ) + +/// Compare two ids for equality. Doesn't work for b3WorldId. Don't mix types. +#define B3_ID_EQUALS( id1, id2 ) ( id1.index1 == id2.index1 && id1.world0 == id2.world0 && id1.generation == id2.generation ) + +/// Store a world id into a uint32_t. +B3_ID_INLINE uint32_t b3StoreWorldId( b3WorldId id ) +{ + return ( (uint32_t)id.index1 << 16 ) | (uint32_t)id.generation; +} + +/// Load a uint32_t into a world id. +B3_ID_INLINE b3WorldId b3LoadWorldId( uint32_t x ) +{ + b3WorldId id = { (uint16_t)( x >> 16 ), (uint16_t)( x ) }; + return id; +} + +/// Store a body id into a uint64_t. +B3_ID_INLINE uint64_t b3StoreBodyId( b3BodyId id ) +{ + return ( (uint64_t)id.index1 << 32 ) | ( (uint64_t)id.world0 ) << 16 | (uint64_t)id.generation; +} + +/// Load a uint64_t into a body id. +B3_ID_INLINE b3BodyId b3LoadBodyId( uint64_t x ) +{ + b3BodyId id = { (int32_t)( x >> 32 ), (uint16_t)( x >> 16 ), (uint16_t)( x ) }; + return id; +} + +/// Store a shape id into a uint64_t. +B3_ID_INLINE uint64_t b3StoreShapeId( b3ShapeId id ) +{ + return ( (uint64_t)id.index1 << 32 ) | ( (uint64_t)id.world0 ) << 16 | (uint64_t)id.generation; +} + +/// Load a uint64_t into a shape id. +B3_ID_INLINE b3ShapeId b3LoadShapeId( uint64_t x ) +{ + b3ShapeId id = { (int32_t)( x >> 32 ), (uint16_t)( x >> 16 ), (uint16_t)( x ) }; + return id; +} + +/// Store a joint id into a uint64_t. +B3_ID_INLINE uint64_t b3StoreJointId( b3JointId id ) +{ + return ( (uint64_t)id.index1 << 32 ) | ( (uint64_t)id.world0 ) << 16 | (uint64_t)id.generation; +} + +/// Load a uint64_t into a joint id. +B3_ID_INLINE b3JointId b3LoadJointId( uint64_t x ) +{ + b3JointId id = { (int32_t)( x >> 32 ), (uint16_t)( x >> 16 ), (uint16_t)( x ) }; + return id; +} + +/// Store a contact id into 16 bytes +B3_ID_INLINE void b3StoreContactId( b3ContactId id, uint32_t values[3] ) +{ + values[0] = (uint32_t)id.index1; + values[1] = (uint32_t)id.world0; + values[2] = (uint32_t)id.generation; +} + +/// Load a two uint64_t into a contact id. +B3_ID_INLINE b3ContactId b3LoadContactId( uint32_t values[3] ) +{ + b3ContactId id; + id.index1 = (int32_t)values[0]; + id.world0 = (uint16_t)values[1]; + id.padding = 0; + id.generation = (uint32_t)values[2]; + return id; +} + +/**@}*/ diff --git a/include/box3d/math_functions.h b/include/box3d/math_functions.h new file mode 100644 index 0000000..13a1092 --- /dev/null +++ b/include/box3d/math_functions.h @@ -0,0 +1,1106 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "base.h" + +#include + +// for sqrtf and remainderf +#include +#include + +/** + * @defgroup math Math + * @brief Vector math types and functions + * @{ + */ + +typedef struct b3Vec2 +{ + float x; + float y; +} b3Vec2; + +typedef struct b3Vec3 +{ + float x; + float y; + float z; +} b3Vec3; + +/// Cosine and sine pair +/// This uses a custom implementation designed for cross-platform determinism +typedef struct b3CosSin +{ + /// cosine and sine + float cosine; + float sine; +} b3CosSin; + +typedef struct b3Quat +{ + b3Vec3 v; + float s; +} b3Quat; + +typedef struct b3Transform +{ + b3Vec3 p; + b3Quat q; +} b3Transform; + +typedef struct b3Matrix3 +{ + b3Vec3 cx, cy, cz; +} b3Matrix3; + +typedef struct b3AABB +{ + b3Vec3 lowerBound; + b3Vec3 upperBound; +} b3AABB; + +/// separation = dot(normal, point) - offset +typedef struct b3Plane +{ + b3Vec3 normal; + float offset; +} b3Plane; + +/**@}*/ + +/** + * @addtogroup math + * @{ + */ + +#define B3_PI 3.141592654f +#define B3_DEG_TO_RAD 0.01745329251f +#define B3_RAD_TO_DEG 57.2957795131f + +/// Minimum scale used for scaling collision meshes, etc. +#define B3_MIN_SCALE 0.01f + +static const b3Vec3 b3Vec3_zero = { 0.0f, 0.0f, 0.0f }; +static const b3Vec3 b3Vec3_one = { 1.0f, 1.0f, 1.0f }; +static const b3Vec3 b3Vec3_axisX = { 1.0f, 0.0f, 0.0f }; +static const b3Vec3 b3Vec3_axisY = { 0.0f, 1.0f, 0.0f }; +static const b3Vec3 b3Vec3_axisZ = { 0.0f, 0.0f, 1.0f }; + +static const b3Quat b3Quat_identity = { { 0.0f, 0.0f, 0.0f }, 1.0f }; +static const b3Transform b3Transform_identity = { { 0.0f, 0.0f, 0.0f }, { { 0.0f, 0.0f, 0.0f }, 1.0f } }; + +static const b3Matrix3 b3Mat3_zero = { + { 0.0f, 0.0f, 0.0f }, + { 0.0f, 0.0f, 0.0f }, + { 0.0f, 0.0f, 0.0f }, +}; + +static const b3Matrix3 b3Mat3_identity = { + { 1.0f, 0.0f, 0.0f }, + { 0.0f, 1.0f, 0.0f }, + { 0.0f, 0.0f, 1.0f }, +}; + +/// @return the minimum of two integers +B3_INLINE int b3MinInt( int a, int b ) +{ + return a < b ? a : b; +} + +/// @return the maximum of two integers +B3_INLINE int b3MaxInt( int a, int b ) +{ + return a > b ? a : b; +} + +/// @return an integer clamped between a lower and upper bound +B3_INLINE int b3ClampInt( int a, int lower, int upper ) +{ + return a < lower ? lower : ( upper < a ? upper : a ); +} + +/// Is this float valid (finite and not NaN) +B3_API bool b3IsValidFloat( float a ); + +/// @return the absolute value of a float +B3_INLINE float b3AbsFloat( float a ) +{ + return a < 0 ? -a : a; +} + +/// @return the minimum of two floats +B3_INLINE float b3MinFloat( float a, float b ) +{ + return a < b ? a : b; +} + +/// @return the maximum of two floats +B3_INLINE float b3MaxFloat( float a, float b ) +{ + return a > b ? a : b; +} + +/// @return a float clamped between a lower and upper bound +B3_INLINE float b3ClampFloat( float a, float lower, float upper ) +{ + return a < lower ? lower : ( upper < a ? upper : a ); +} + +/// Interpolate a scalar +B3_INLINE float b3LerpFloat( float a, float b, float alpha ) +{ + return ( 1.0f - alpha ) * a + alpha * b; +} + +/// Compute an approximate arctangent in the range [-pi, pi] +/// This is hand coded for cross-platform determinism. The atan2f +/// function in the standard library is not cross-platform deterministic. +/// Accurate to around 0.0023 degrees +B3_API float b3Atan2( float y, float x ); + +/// Compute the cosine and sine of an angle in radians. Implemented +/// for cross-platform determinism. +B3_API b3CosSin b3ComputeCosSin( float radians ); + +B3_INLINE float b3Sin( float radians ) +{ + b3CosSin cs = b3ComputeCosSin( radians ); + return cs.sine; +} + +B3_INLINE float b3Cos( float radians ) +{ + b3CosSin cs = b3ComputeCosSin( radians ); + return cs.cosine; +} + +/// Convert any angle into the range [-pi, pi] +B3_INLINE float b3UnwindAngle( float radians ) +{ + // Assuming this is deterministic + return remainderf( radians, 2.0f * B3_PI ); +} + +/// Vector addition +B3_INLINE b3Vec3 b3Add( b3Vec3 a, b3Vec3 b ) +{ + return B3_LITERAL( b3Vec3 ){ a.x + b.x, a.y + b.y, a.z + b.z }; +} + +/// Vector subtraction +B3_INLINE b3Vec3 b3Sub( b3Vec3 a, b3Vec3 b ) +{ + return B3_LITERAL( b3Vec3 ){ a.x - b.x, a.y - b.y, a.z - b.z }; +} + +/// Vector component-wise multiplication +B3_INLINE b3Vec3 b3Mul( b3Vec3 a, b3Vec3 b ) +{ + return B3_LITERAL( b3Vec3 ){ a.x * b.x, a.y * b.y, a.z * b.z }; +} + +/// Vector negation +B3_INLINE b3Vec3 b3Neg( b3Vec3 a ) +{ + return B3_LITERAL( b3Vec3 ){ -a.x, -a.y, -a.z }; +} + +/// Vector dot product +B3_INLINE float b3Dot( b3Vec3 a, b3Vec3 b ) +{ + return a.x * b.x + a.y * b.y + a.z * b.z; +} + +/// Vector length +B3_INLINE float b3Length( b3Vec3 v ) +{ + return sqrtf( b3Dot( v, v ) ); +} + +/// Vector length squared +B3_INLINE float b3LengthSquared( b3Vec3 a ) +{ + return a.x * a.x + a.y * a.y + a.z * a.z; +} + +B3_INLINE float b3Distance( b3Vec3 v1, b3Vec3 v2 ) +{ + b3Vec3 dv = { v2.x - v1.x, v2.y - v1.y, v2.z - v1.z }; + return b3Length( dv ); +} + +B3_INLINE float b3DistanceSquared( b3Vec3 v1, b3Vec3 v2 ) +{ + b3Vec3 dv = { v2.x - v1.x, v2.y - v1.y, v2.z - v1.z }; + return dv.x * dv.x + dv.y * dv.y + dv.z * dv.z; +} + +B3_INLINE b3Vec3 b3Normalize( b3Vec3 a ) +{ + float lengthSquared = a.x * a.x + a.y * a.y + a.z * a.z; + + if ( lengthSquared > 1000.0f * FLT_MIN ) + { + float s = 1.0f / sqrtf( lengthSquared ); + b3Vec3 u = { s * a.x, s * a.y, s * a.z }; + return u; + } + + return B3_LITERAL( b3Vec3 ){ 0.0f, 0.0f, 0.0f }; +} + +B3_INLINE b3Vec3 b3GetLengthAndNormalize( float* length, b3Vec3 v ) +{ + *length = b3Length( v ); + if ( *length < FLT_EPSILON ) + { + return b3Vec3_zero; + } + + float invLength = 1.0f / *length; + b3Vec3 n = { invLength * v.x, invLength * v.y, invLength * v.z }; + return n; +} + +B3_INLINE b3Vec3 b3Perp( b3Vec3 v ) +{ + // Suppose vector a has all equal components and is a unit vector: a = (s, s, s) + // Then 3*s*s = 1, s = sqrt(1/3) = 0.57735. This means that at least one component + // of a unit vector must be greater or equal to 0.57735. + b3Vec3 p; + if ( v.x < -0.5f || 0.5f < v.x ) + { + p = B3_LITERAL( b3Vec3 ){ v.y, -v.x, 0.0f }; + } + else + { + p = B3_LITERAL( b3Vec3 ){ 0.0f, v.z, -v.y }; + } + + return b3Normalize( p ); +} + +B3_INLINE bool b3IsNormalized( b3Vec3 a ) +{ + float aa = b3Dot( a, a ); + return b3AbsFloat( 1.0f - aa ) < 100.0f * FLT_EPSILON; +} + +/// a + s * b +B3_INLINE b3Vec3 b3MulAdd( b3Vec3 a, float s, b3Vec3 b ) +{ + return B3_LITERAL( b3Vec3 ){ a.x + s * b.x, a.y + s * b.y, a.z + s * b.z }; +} + +/// a - s * b +B3_INLINE b3Vec3 b3MulSub( b3Vec3 a, float s, b3Vec3 b ) +{ + return B3_LITERAL( b3Vec3 ){ a.x - s * b.x, a.y - s * b.y, a.z - s * b.z }; +} + +/// a + s * b +B3_INLINE b3Vec3 b3MulSV( float s, b3Vec3 a ) +{ + return B3_LITERAL( b3Vec3 ){ s * a.x, s * a.y, s * a.z }; +} + +B3_INLINE b3Vec3 b3Cross( b3Vec3 a, b3Vec3 b ) +{ + b3Vec3 c; + c.x = a.y * b.z - a.z * b.y; + c.y = a.z * b.x - a.x * b.z; + c.z = a.x * b.y - a.y * b.x; + return c; +} + +B3_INLINE b3Vec3 b3Lerp( b3Vec3 a, b3Vec3 b, float alpha ) +{ + B3_ASSERT( 0.0f <= alpha && alpha <= 1.0f ); + + b3Vec3 c = { + ( 1.0f - alpha ) * a.x + alpha * b.x, + ( 1.0f - alpha ) * a.y + alpha * b.y, + ( 1.0f - alpha ) * a.z + alpha * b.z, + }; + return c; +} + +B3_INLINE b3Vec3 b3Blend2( float s, b3Vec3 a, float t, b3Vec3 b ) +{ + b3Vec3 d = { + s * a.x + t * b.x, + s * a.y + t * b.y, + s * a.z + t * b.z, + }; + return d; +} + +B3_INLINE b3Vec3 b3Blend3( float s, b3Vec3 a, float t, b3Vec3 b, float u, b3Vec3 c ) +{ + b3Vec3 d = { + s * a.x + t * b.x + u * c.x, + s * a.y + t * b.y + u * c.y, + s * a.z + t * b.z + u * c.z, + }; + return d; +} + +B3_INLINE b3Vec3 b3Abs( b3Vec3 a ) +{ + return B3_LITERAL( b3Vec3 ){ + b3AbsFloat( a.x ), + b3AbsFloat( a.y ), + b3AbsFloat( a.z ), + }; +} + +// -1 or 1 (1 if zero) +B3_INLINE b3Vec3 b3Sign( b3Vec3 a ) +{ + return B3_LITERAL( b3Vec3 ){ + a.x >= 0.0f ? 1.0f : -1.0f, + a.y >= 0.0f ? 1.0f : -1.0f, + a.z >= 0.0f ? 1.0f : -1.0f, + }; +} + +B3_INLINE b3Vec3 b3Min( b3Vec3 a, b3Vec3 b ) +{ + return B3_LITERAL( b3Vec3 ){ + b3MinFloat( a.x, b.x ), + b3MinFloat( a.y, b.y ), + b3MinFloat( a.z, b.z ), + }; +} + +B3_INLINE b3Vec3 b3Max( b3Vec3 a, b3Vec3 b ) +{ + return B3_LITERAL( b3Vec3 ){ + b3MaxFloat( a.x, b.x ), + b3MaxFloat( a.y, b.y ), + b3MaxFloat( a.z, b.z ), + }; +} + +B3_INLINE b3Vec3 b3Clamp( b3Vec3 a, b3Vec3 lower, b3Vec3 upper ) +{ + b3Vec3 b; + b.x = b3ClampFloat( a.x, lower.x, upper.x ); + b.y = b3ClampFloat( a.y, lower.y, upper.y ); + b.z = b3ClampFloat( a.z, lower.z, upper.z ); + return b; +} + +// Create a safe scaling value for scaling collision. This allows +// negative scale, but keeps scale sufficiently far from zero. +B3_INLINE b3Vec3 b3SafeScale( b3Vec3 a ) +{ + b3Vec3 absScale = b3Abs( a ); + b3Vec3 minScale = { B3_MIN_SCALE, B3_MIN_SCALE, B3_MIN_SCALE }; + b3Vec3 safeScale = b3Mul( b3Sign( a ), b3Max( absScale, minScale ) ); + return safeScale; +} + +B3_INLINE bool b3IsNormalizedQuat( b3Quat q ) +{ + float qq = q.v.x * q.v.x + q.v.y * q.v.y + q.v.z * q.v.z + q.s * q.s; + return 1.0f - 20.0f * FLT_EPSILON < qq && qq < 1.0f + 20.0f * FLT_EPSILON; +} + +/// Rotate a vector +B3_INLINE b3Vec3 b3RotateVector( b3Quat q, b3Vec3 v ) +{ + // v + 2 * cross(q.v, cross(q.v, v) + q.s * v) + // B3_ASSERT( b3IsNormalizedQuat( q ) ); + b3Vec3 t1 = b3Cross( q.v, v ); + b3Vec3 t2 = b3MulAdd( t1, q.s, v ); + b3Vec3 t3 = b3Cross( q.v, t2 ); + return b3MulAdd( v, 2.0f, t3 ); +} + +/// Inverse rotate a vector +B3_INLINE b3Vec3 b3InvRotateVector( b3Quat q, b3Vec3 v ) +{ + // v + 2 * cross(q.v, cross(q.v, v) - q.s * v) + // B3_ASSERT( b3IsNormalizedQuat( q ) ); + b3Vec3 t1 = b3Cross( q.v, v ); + b3Vec3 t2 = b3MulSub( t1, q.s, v ); + b3Vec3 t3 = b3Cross( q.v, t2 ); + return b3MulAdd( v, 2.0f, t3 ); +} + +B3_INLINE float b3DotQuat( b3Quat a, b3Quat b ) +{ + return a.v.x * b.v.x + a.v.y * b.v.y + a.v.z * b.v.z + a.s * b.s; +} + +B3_INLINE b3Quat b3MulQuat( b3Quat q1, b3Quat q2 ) +{ + b3Vec3 t1 = b3Cross( q1.v, q2.v ); + b3Vec3 t2 = b3MulAdd( t1, q1.s, q2.v ); + b3Vec3 t3 = b3MulAdd( t2, q2.s, q1.v ); + b3Quat q = { t3, q1.s * q2.s - b3Dot( q1.v, q2.v ) }; + return q; +} + +// inv(q1) * q2 +B3_INLINE b3Quat b3InvMulQuat( b3Quat q1, b3Quat q2 ) +{ + b3Vec3 t1 = b3Cross( q2.v, q1.v ); + b3Vec3 t2 = b3MulAdd( t1, q1.s, q2.v ); + b3Vec3 t3 = b3MulSub( t2, q2.s, q1.v ); + b3Quat q = { t3, q1.s * q2.s + b3Dot( q1.v, q2.v ) }; + return q; +} + +B3_INLINE b3Quat b3Conjugate( b3Quat q ) +{ + return B3_LITERAL( b3Quat ){ { -q.v.x, -q.v.y, -q.v.z }, q.s }; +} + +B3_INLINE b3Quat b3NegateQuat( b3Quat q ) +{ + return B3_LITERAL( b3Quat ){ { -q.v.x, -q.v.y, -q.v.z }, -q.s }; +} + +B3_INLINE b3Quat b3NormalizeQuat( b3Quat q ) +{ + float lengthSq = b3DotQuat( q, q ); + if ( lengthSq > 1000.0f * FLT_MIN ) + { + float s = 1.0f / sqrtf( lengthSq ); + b3Quat qn = { { s * q.v.x, s * q.v.y, s * q.v.z }, s * q.s }; + return qn; + } + + return b3Quat_identity; +} + +B3_INLINE b3Quat b3MakeQuatFromAxisAngle( b3Vec3 axis, float radians ) +{ + B3_ASSERT( b3IsNormalized( axis ) ); + b3CosSin cs = b3ComputeCosSin( 0.5f * radians ); + b3Quat q = { { cs.sine * axis.x, cs.sine * axis.y, cs.sine * axis.z }, cs.cosine }; + return q; +} + +B3_INLINE b3Vec3 b3GetAxisAngle( float* radians, b3Quat q ) +{ + float length = sqrtf( q.v.x * q.v.x + q.v.y * q.v.y + q.v.z * q.v.z ); + *radians = 2.0f * b3Atan2( length, q.s ); + if ( length > 0.0f ) + { + float invLength = 1.0f / length; + b3Vec3 axis = { invLength * q.v.x, invLength * q.v.y, invLength * q.v.z }; + return axis; + } + + return b3Vec3_zero; +} + +/// Get the angle for a quaternion in radians +B3_INLINE float b3GetQuatAngle( b3Quat q ) +{ + float length = sqrtf( q.v.x * q.v.x + q.v.y * q.v.y + q.v.z * q.v.z ); + return 2.0f * b3Atan2( length, q.s ); +} + +// Twist angle around the z-axis, used for twist limit and revolute angle limit +B3_INLINE float b3GetTwistAngle( b3Quat q ) +{ + // Account for polarity to keep the twist angle in range. + // This is simpler than asking the user to check polarity or unwinding. + float twist = q.s < 0.0f ? b3Atan2( -q.v.z, -q.s ) : b3Atan2( q.v.z, q.s ); + twist *= 2.0f; + B3_ASSERT( -B3_PI <= twist && twist <= B3_PI ); + return twist; +} + +// Swing angle used for cone limit +B3_INLINE float b3GetSwingAngle( b3Quat q ) +{ + // Polarity should not matter because all terms are squared. + float x = sqrtf( q.v.z * q.v.z + q.s * q.s ); + float y = sqrtf( q.v.x * q.v.x + q.v.y * q.v.y ); + float swing = 2.0f * b3Atan2( y, x ); + B3_ASSERT( 0.0f <= swing && swing <= B3_PI ); + return swing; +} + +/// Extract a quaternion from a rotation matrix +B3_API b3Quat b3MakeQuatFromMatrix( const b3Matrix3* m ); + +/// Find a quaternion that rotations one vector to another +B3_API b3Quat b3ComputeQuatBetweenUnitVectors( b3Vec3 v1, b3Vec3 v2 ); + +/// Linearly interpolate and normalize between two quaternions +B3_INLINE b3Quat b3NLerp( b3Quat q1, b3Quat q2, float alpha ) +{ + B3_ASSERT( 0.0f <= alpha && alpha <= 1.0f ); + if ( b3DotQuat( q1, q2 ) < 0.0f ) + { + q1 = B3_LITERAL( b3Quat ){ { -q1.v.x, -q1.v.y, -q1.v.z }, -q1.s }; + } + + b3Quat q; + q.v = b3Lerp( q1.v, q2.v, alpha ); + q.s = ( 1.0f - alpha ) * q1.s + alpha * q2.s; + + return b3NormalizeQuat( q ); +} + +/// Multiply two transforms. If the result is applied to a point p local to frame B, +/// the transform would first convert p to a point local to frame A, then into a point +/// in the world frame. This is useful if frame B is a child of frame A. +B3_INLINE b3Transform b3MulTransforms( b3Transform a, b3Transform b ) +{ + b3Transform out; + out.p = b3Add( b3RotateVector( a.q, b.p ), a.p ); + out.q = b3MulQuat( a.q, b.q ); + return out; +} + +/// Creates a transform that converts a local point in frame B to a local point in frame A. +/// This is useful for transforming points between the local spaces of two frames that are +/// in world space. +B3_FORCE_INLINE b3Transform b3InvMulTransforms( b3Transform a, b3Transform b ) +{ + b3Transform out; + out.p = b3InvRotateVector( a.q, b3Sub( b.p, a.p ) ); + out.q = b3InvMulQuat( a.q, b.q ); + return out; +} + +B3_INLINE b3Transform b3InvertTransform( b3Transform t ) +{ + b3Transform out; + out.p = b3InvRotateVector( t.q, b3Neg( t.p ) ); + out.q = b3Conjugate( t.q ); + return out; +} + +B3_INLINE b3Transform b3NormalizeTransform( b3Transform t ) +{ + b3Transform out; + out.p = t.p; + out.q = b3NormalizeQuat( t.q ); + return out; +} + +B3_INLINE b3Vec3 b3TransformPoint( b3Transform t, b3Vec3 v ) +{ + b3Vec3 rv = b3RotateVector( t.q, v ); + return b3Add( rv, t.p ); +} + +B3_INLINE b3Vec3 b3InvTransformPoint( b3Transform t, b3Vec3 v ) +{ + return b3InvRotateVector( t.q, b3Sub( v, t.p ) ); +} + +B3_INLINE b3Matrix3 b3MakeDiagonalMatrix( float a, float b, float c ) +{ + return B3_LITERAL( b3Matrix3 ){ { a, 0.0f, 0.0f }, { 0.0f, b, 0.0f }, { 0.0f, 0.0f, c } }; +} + +B3_INLINE float b3Det( b3Matrix3 m ) +{ + return b3Dot( m.cx, b3Cross( m.cy, m.cz ) ); +} + +B3_INLINE b3Vec3 b3MulMV( b3Matrix3 m, b3Vec3 a ) +{ + b3Vec3 b = { + m.cx.x * a.x + m.cy.x * a.y + m.cz.x * a.z, + m.cx.y * a.x + m.cy.y * a.y + m.cz.y * a.z, + m.cx.z * a.x + m.cy.z * a.y + m.cz.z * a.z, + }; + return b; +} + +B3_INLINE b3Matrix3 b3NegateMat3( b3Matrix3 a ) +{ + return B3_LITERAL( b3Matrix3 ){ + { -a.cx.x, -a.cx.y, -a.cx.z }, + { -a.cy.x, -a.cy.y, -a.cy.z }, + { -a.cz.x, -a.cz.y, -a.cz.z }, + }; +} + +B3_INLINE b3Matrix3 b3AddMM( b3Matrix3 a, b3Matrix3 b ) +{ + return B3_LITERAL( b3Matrix3 ){ + { a.cx.x + b.cx.x, a.cx.y + b.cx.y, a.cx.z + b.cx.z }, + { a.cy.x + b.cy.x, a.cy.y + b.cy.y, a.cy.z + b.cy.z }, + { a.cz.x + b.cz.x, a.cz.y + b.cz.y, a.cz.z + b.cz.z }, + }; +} + +B3_INLINE b3Matrix3 b3SubMM( b3Matrix3 a, b3Matrix3 b ) +{ + return B3_LITERAL( b3Matrix3 ){ + { a.cx.x - b.cx.x, a.cx.y - b.cx.y, a.cx.z - b.cx.z }, + { a.cy.x - b.cy.x, a.cy.y - b.cy.y, a.cy.z - b.cy.z }, + { a.cz.x - b.cz.x, a.cz.y - b.cz.y, a.cz.z - b.cz.z }, + }; +} + +B3_INLINE b3Matrix3 b3MulSM( float s, b3Matrix3 a ) +{ + return B3_LITERAL( b3Matrix3 ){ + { s * a.cx.x, s * a.cx.y, s * a.cx.z }, + { s * a.cy.x, s * a.cy.y, s * a.cy.z }, + { s * a.cz.x, s * a.cz.y, s * a.cz.z }, + }; +} + +B3_INLINE b3Matrix3 b3MulMM( b3Matrix3 a, b3Matrix3 b ) +{ + b3Matrix3 out; + out.cx = b3MulMV( a, b.cx ); + out.cy = b3MulMV( a, b.cy ); + out.cz = b3MulMV( a, b.cz ); + return out; +} + +B3_INLINE b3Matrix3 b3Transpose( b3Matrix3 m ) +{ + b3Matrix3 out; + out.cx = B3_LITERAL( b3Vec3 ){ m.cx.x, m.cy.x, m.cz.x }; + out.cy = B3_LITERAL( b3Vec3 ){ m.cx.y, m.cy.y, m.cz.y }; + out.cz = B3_LITERAL( b3Vec3 ){ m.cx.z, m.cy.z, m.cz.z }; + + return out; +} + +B3_INLINE b3Matrix3 b3InvertMatrix( b3Matrix3 m ) +{ + float det = b3Det( m ); + if ( b3AbsFloat( det ) > 1000.0f * FLT_MIN ) + { + float invDet = 1.0f / det; + b3Matrix3 out; + out.cx = b3MulSV( invDet, b3Cross( m.cy, m.cz ) ); + out.cy = b3MulSV( invDet, b3Cross( m.cz, m.cx ) ); + out.cz = b3MulSV( invDet, b3Cross( m.cx, m.cy ) ); + + return b3Transpose( out ); + } + + return b3Mat3_zero; +} + +B3_INLINE b3Vec3 b3Solve3( b3Matrix3 m, b3Vec3 x ) +{ + float det = b3Det( m ); + if ( b3AbsFloat( det ) > 1000.0f * FLT_MIN ) + { + float invDet = 1.0f / det; + b3Matrix3 a; + a.cx = b3Cross( m.cy, m.cz ); + a.cy = b3Cross( m.cz, m.cx ); + a.cz = b3Cross( m.cx, m.cy ); + + b3Vec3 b = { + invDet * b3Dot( a.cx, x ), + invDet * b3Dot( a.cy, x ), + invDet * b3Dot( a.cz, x ), + }; + + return b; + } + + return b3Vec3_zero; +} + +B3_INLINE b3Matrix3 b3InvertT( b3Matrix3 m ) +{ + float det = b3Det( m ); + if ( b3AbsFloat( det ) > 1000.0f * FLT_MIN ) + { + float invDet = 1.0f / det; + b3Matrix3 out; + out.cx = b3MulSV( invDet, b3Cross( m.cy, m.cz ) ); + out.cy = b3MulSV( invDet, b3Cross( m.cz, m.cx ) ); + out.cz = b3MulSV( invDet, b3Cross( m.cx, m.cy ) ); + return out; + } + + return b3Mat3_zero; +} + +B3_INLINE b3Matrix3 b3InvertSymmetric( b3Matrix3 m ) +{ + float det = b3Det( m ); + if ( b3AbsFloat( det ) > 1000.0f * FLT_MIN ) + { + float invDet = 1.0f / det; + b3Matrix3 out; + out.cx = b3MulSV( invDet, b3Cross( m.cy, m.cz ) ); + out.cy = b3MulSV( invDet, b3Cross( m.cz, m.cx ) ); + out.cz = b3MulSV( invDet, b3Cross( m.cx, m.cy ) ); + return out; + } + + return b3Mat3_zero; +} + +B3_INLINE b3Matrix3 b3Skew( b3Vec3 v ) +{ + b3Matrix3 out; + out.cx = B3_LITERAL( b3Vec3 ){ 0, v.z, -v.y }; + out.cy = B3_LITERAL( b3Vec3 ){ -v.z, 0, v.x }; + out.cz = B3_LITERAL( b3Vec3 ){ v.y, -v.x, 0 }; + + return out; +} + +B3_INLINE b3Matrix3 b3AbsMatrix3( b3Matrix3 m ) +{ + b3Matrix3 out; + out.cx = b3Abs( m.cx ); + out.cy = b3Abs( m.cy ); + out.cz = b3Abs( m.cz ); + + return out; +} + +B3_INLINE float b3Trace( b3Matrix3 m ) +{ + return m.cx.x + m.cy.y + m.cz.z; +} + +B3_INLINE b3Matrix3 b3ScaleMatrix( float s, b3Matrix3 m ) +{ + return B3_LITERAL( b3Matrix3 ){ + b3MulSV( s, m.cx ), + b3MulSV( s, m.cy ), + b3MulSV( s, m.cz ), + }; +} + +// This force inline improves the performance of b3ShapeDistance +B3_FORCE_INLINE b3Matrix3 b3MakeMatrixFromQuat( b3Quat q ) +{ + float xx = q.v.x * q.v.x; + float yy = q.v.y * q.v.y; + float zz = q.v.z * q.v.z; + float xy = q.v.x * q.v.y; + float xz = q.v.x * q.v.z; + float xw = q.v.x * q.s; + float yz = q.v.y * q.v.z; + float yw = q.v.y * q.s; + float zw = q.v.z * q.s; + + return B3_LITERAL( b3Matrix3 ){ + { 1.0f - 2.0f * ( yy + zz ), 2.0f * ( xy + zw ), 2.0f * ( xz - yw ) }, + { 2.0f * ( xy - zw ), 1.0f - 2.0f * ( xx + zz ), 2.0f * ( yz + xw ) }, + { 2.0f * ( xz + yw ), 2.0f * ( yz - xw ), 1.0f - 2.0f * ( xx + yy ) }, + }; +} + +// https://en.wikipedia.org/wiki/Parallel_axis_theorem +B3_API b3Matrix3 b3Steiner( float mass, b3Vec3 origin ); + +B3_INLINE b3Plane b3MakePlaneFromNormalAndPoint( b3Vec3 normal, b3Vec3 point ) +{ + return B3_LITERAL( b3Plane ){ normal, b3Dot( normal, point ) }; +} + +B3_INLINE b3Plane b3MakePlaneFromPoints( b3Vec3 point1, b3Vec3 point2, b3Vec3 point3 ) +{ + b3Plane plane; + plane.normal = b3Cross( b3Sub( point2, point1 ), b3Sub( point3, point1 ) ); + plane.normal = b3Normalize( plane.normal ); + plane.offset = b3Dot( plane.normal, point1 ); + return plane; +} + +B3_INLINE b3Plane b3NegatePlane( b3Plane plane ) +{ + return B3_LITERAL( b3Plane ){ { -plane.normal.x, -plane.normal.y, -plane.normal.z }, -plane.offset }; +} + +// normal2 = q * normal1 +// offset2 = dot(normal2, p) + offset1 +B3_INLINE b3Plane b3TransformPlane( b3Transform transform, b3Plane plane ) +{ + b3Vec3 normal = b3RotateVector( transform.q, plane.normal ); + return B3_LITERAL( b3Plane ){ normal, plane.offset + b3Dot( normal, transform.p ) }; +} + +// offset1 = offset2 - dot(normal2, p) +// normal1 = qc * normal2 +B3_INLINE b3Plane b3InvTransformPlane( b3Transform transform, b3Plane plane ) +{ + float offset = plane.offset - b3Dot( plane.normal, transform.p ); + b3Vec3 normal = b3InvRotateVector( transform.q, plane.normal ); + return B3_LITERAL( b3Plane ){ normal, offset }; +} + +/// Signed separation of a point from a plane +B3_INLINE float b3PlaneSeparation( b3Plane plane, b3Vec3 point ) +{ + return b3Dot( plane.normal, point ) - plane.offset; +} + +B3_INLINE b3Plane b3ScalePlane( float scale, b3Plane plane ) +{ + return B3_LITERAL( b3Plane ){ plane.normal, scale * plane.offset }; +} + +B3_INLINE b3AABB b3MakeAABB( const b3Vec3* points, int count, float radius ) +{ + B3_ASSERT( count > 0 ); + b3AABB a = { points[0], points[0] }; + for ( int i = 1; i < count; ++i ) + { + a.lowerBound = b3Min( a.lowerBound, points[i] ); + a.upperBound = b3Max( a.upperBound, points[i] ); + } + + b3Vec3 r = { radius, radius, radius }; + a.lowerBound = b3Sub( a.lowerBound, r ); + a.upperBound = b3Add( a.upperBound, r ); + + return a; +} + +B3_INLINE b3AABB b3AABB_AddPoint( b3AABB a, b3Vec3 point ) +{ + return B3_LITERAL( b3AABB ){ b3Min( a.lowerBound, point ), b3Max( a.upperBound, point ) }; +} + +B3_INLINE bool b3AABB_ContainsPoint( b3AABB a, b3Vec3 point ) +{ + if ( a.lowerBound.x > point.x || point.x > a.upperBound.x ) + return false; + if ( a.lowerBound.y > point.y || point.y > a.upperBound.y ) + return false; + if ( a.lowerBound.z > point.z || point.z > a.upperBound.z ) + return false; + + return true; +} + +B3_INLINE bool b3AABB_Contains( b3AABB a, b3AABB b ) +{ + if ( a.lowerBound.x > b.lowerBound.x || b.upperBound.x > a.upperBound.x ) + return false; + if ( a.lowerBound.y > b.lowerBound.y || b.upperBound.y > a.upperBound.y ) + return false; + if ( a.lowerBound.z > b.lowerBound.z || b.upperBound.z > a.upperBound.z ) + return false; + + return true; +} + +B3_INLINE float b3AABB_Area( b3AABB bounds ) +{ + b3Vec3 delta = b3Sub( bounds.upperBound, bounds.lowerBound ); + return 2.0f * ( delta.x * delta.y + delta.y * delta.z + delta.z * delta.x ); +} + +B3_INLINE b3Vec3 b3AABB_Center( b3AABB bounds ) +{ + return b3MulSV( 0.5f, b3Add( bounds.upperBound, bounds.lowerBound ) ); +} + +B3_INLINE b3Vec3 b3AABB_Extents( b3AABB bounds ) +{ + return b3MulSV( 0.5f, b3Sub( bounds.upperBound, bounds.lowerBound ) ); +} + +B3_INLINE b3AABB b3AABB_Union( b3AABB bounds1, b3AABB bounds2 ) +{ + b3AABB out; + out.lowerBound = b3Min( bounds1.lowerBound, bounds2.lowerBound ); + out.upperBound = b3Max( bounds1.upperBound, bounds2.upperBound ); + return out; +} + +B3_INLINE b3AABB b3AABB_Inflate( b3AABB bounds, float extension ) +{ + b3Vec3 radius = { extension, extension, extension }; + + b3AABB out; + out.lowerBound = b3Sub( bounds.lowerBound, radius ); + out.upperBound = b3Add( bounds.upperBound, radius ); + return out; +} + +B3_INLINE bool b3AABB_Overlaps( b3AABB a, b3AABB b ) +{ + // No intersection if separated along one axis + if ( a.upperBound.x < b.lowerBound.x || a.lowerBound.x > b.upperBound.x ) + return false; + if ( a.upperBound.y < b.lowerBound.y || a.lowerBound.y > b.upperBound.y ) + return false; + if ( a.upperBound.z < b.lowerBound.z || a.lowerBound.z > b.upperBound.z ) + return false; + + // Overlapping on all axis means bounds are intersecting + return true; +} + +B3_INLINE b3AABB b3AABB_Transform( b3Transform transform, b3AABB a ) +{ + b3Vec3 center = b3TransformPoint( transform, b3AABB_Center( a ) ); + b3Matrix3 m = b3MakeMatrixFromQuat( transform.q ); + b3Vec3 extent = b3MulMV( b3AbsMatrix3( m ), b3AABB_Extents( a ) ); + b3AABB out = { b3Sub( center, extent ), b3Add( center, extent ) }; + return out; +} + +B3_INLINE b3Vec3 b3ClosestPointToAABB( b3Vec3 point, b3AABB a ) +{ + return b3Clamp( point, a.lowerBound, a.upperBound ); +} + +/// Is this a valid number? Not NaN or infinity. +B3_API bool b3IsValidFloat( float a ); + +/// Is this a valid vector? Not NaN or infinity. +B3_API bool b3IsValidVec3( b3Vec3 a ); + +/// Is this a valid quaternion? Not NaN or infinity. Is normalized. +B3_API bool b3IsValidQuat( b3Quat q ); + +/// Is this a valid transform? Not NaN or infinity. Is normalized. +B3_API bool b3IsValidTransform( b3Transform a ); + +/// Is this a valid matrix? Not NaN or infinity. +B3_API bool b3IsValidMatrix3( b3Matrix3 a ); + +/// Is this a valid bounding box? Not Nan or infinity. Upper bound greater than or equal to lower bound. +B3_API bool b3IsValidAABB( b3AABB a ); + +/// Is this AABB valid and not huge or very far from the origin? +B3_API bool b3IsSaneAABB( b3AABB a ); + +/// Is this a valid plane? Normal is a unit vector. Not Nan or infinity. +B3_API bool b3IsValidPlane( b3Plane a ); + +/**@}*/ + +/** + * @defgroup math_cpp C++ Math + * @brief Math operator overloads for C++ + * Some of the simpler ones are expanded to improve debug performance. + * See math_functions.h for details. + * @{ + */ + +#ifdef __cplusplus + +B3_FORCE_INLINE b3Vec3& operator+=( b3Vec3& a, b3Vec3 b ) +{ + a.x += b.x; + a.y += b.y; + a.z += b.z; + return a; +} + +B3_FORCE_INLINE b3Vec3& operator-=( b3Vec3& a, b3Vec3 b ) +{ + a.x -= b.x; + a.y -= b.y; + a.z -= b.z; + return a; +} + +B3_FORCE_INLINE b3Vec3& operator*=( b3Vec3& a, float s ) +{ + a.x *= s; + a.y *= s; + a.z *= s; + return a; +} + +B3_FORCE_INLINE b3Vec3 operator-( b3Vec3 a ) +{ + return { -a.x, -a.y, -a.z }; +} + +B3_FORCE_INLINE b3Vec3 operator*( float s, b3Vec3 a ) +{ + return { s * a.x, s * a.y, s * a.z }; +} + +B3_FORCE_INLINE b3Vec3 operator*( b3Vec3 a, float s ) +{ + return { s * a.x, s * a.y, s * a.z }; +} + +B3_FORCE_INLINE b3Vec3 operator*( b3Vec3 a, b3Vec3 b ) +{ + return { a.x * b.x, a.y * b.y, a.z * b.z }; +} + +B3_FORCE_INLINE b3Vec3 operator+( b3Vec3 a, b3Vec3 b ) +{ + return { a.x + b.x, a.y + b.y, a.z + b.z }; +} + +B3_FORCE_INLINE b3Vec3 operator-( b3Vec3 a, b3Vec3 b ) +{ + return { a.x - b.x, a.y - b.y, a.z - b.z }; +} + +B3_INLINE b3Vec3 operator*( b3Matrix3 m, b3Vec3 v ) +{ + return b3MulMV( m, v ); +} + +B3_INLINE b3Matrix3& operator+=( b3Matrix3& a, b3Matrix3 b ) +{ + a = b3AddMM( a, b ); + return a; +} + +B3_INLINE b3Matrix3 operator*( b3Matrix3 a, b3Matrix3 b ) +{ + return b3MulMM( a, b ); +} + +B3_INLINE b3Matrix3 operator+( b3Matrix3 a, b3Matrix3 b ) +{ + return b3AddMM( a, b ); +} + +B3_INLINE b3Matrix3 operator-( b3Matrix3 a, b3Matrix3 b ) +{ + return b3SubMM( a, b ); +} + +B3_INLINE b3Matrix3 operator*( float s, b3Matrix3 a ) +{ + return b3MulSM( s, a ); +} + +B3_INLINE b3Quat operator+( b3Quat q1, b3Quat q2 ) +{ + return { q1.v + q2.v, q1.s + q2.s }; +} + +B3_INLINE b3Quat operator-( b3Quat q1, b3Quat q2 ) +{ + return { q1.v - q2.v, q1.s - q2.s }; +} + +B3_INLINE b3Quat operator*( float f, b3Quat q ) +{ + return { f * q.v, f * q.s }; +} + +B3_INLINE b3Quat operator*( b3Quat q, float f ) +{ + return { q.v * f, q.s * f }; +} + +#endif + +/**@}*/ diff --git a/include/box3d/types.h b/include/box3d/types.h new file mode 100644 index 0000000..58bc7fb --- /dev/null +++ b/include/box3d/types.h @@ -0,0 +1,1523 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "base.h" +#include "collision.h" +#include "constants.h" +#include "id.h" +#include "math_functions.h" + +#include + +#define B3_DEFAULT_CATEGORY_BITS 1 +#define B3_DEFAULT_MASK_BITS UINT64_MAX + +/// Task interface +/// This is the prototype for a Box3D task. Your task system is expected to run this callback on a worker thread, +/// exactly once per enqueue, passing back the same taskContext pointer supplied to b3EnqueueTaskCallback. +/// @ingroup world +typedef void b3TaskCallback( void* taskContext ); + +/// These functions can be provided to Box3D to invoke a task system. +/// Returns a pointer to the user's task object. May be nullptr. A nullptr indicates to Box3D that the work was executed +/// serially within the callback and there is no need to call b3FinishTaskCallback. Otherwise the returned +/// value must be non-null will be passed to b3FinishTaskCallback as the userTask. +/// @param task the Box3D task to be called by the scheduler +/// @param taskContext the Box3D context object that the scheduler must pass to the task +/// @param userContext the scheduler context object that is opaque to Box3D +/// @param taskName the Box3D task name that the scheduler can use for diagnostics +/// @ingroup world +typedef void* b3EnqueueTaskCallback( b3TaskCallback* task, void* taskContext, void* userContext, const char* taskName ); + +/// Finishes a user task object that wraps a Box3D task. +/// @ingroup world +typedef void b3FinishTaskCallback( void* userTask, void* userContext ); + +// The user needs to be able to create debug draw shapes for multi-pass rendering to work efficiently. +// These user shapes are created and destroyed via callback so they can be bound to shape lifetime and scaling updates. +typedef struct b3DebugShape b3DebugShape; +typedef void* b3CreateDebugShapeCallback( const b3DebugShape* debugShape, void* userContext ); +typedef void b3DestroyDebugShapeCallback( void* userShape, void* userContext ); + +/// Optional friction mixing callback. This intentionally provides no context objects because this is called +/// from a worker thread. +/// @warning This function should not attempt to modify Box3D state or user application state. +typedef float b3FrictionCallback( float frictionA, uint64_t userMaterialIdA, float frictionB, uint64_t userMaterialIdB ); + +/// Optional restitution mixing callback. This intentionally provides no context objects because this is called +/// from a worker thread. +/// @warning This function should not attempt to modify Box3D state or user application state. +typedef float b3RestitutionCallback( float restitutionA, uint64_t userMaterialIdA, float restitutionB, uint64_t userMaterialIdB ); + +/// Result from b3World_RayCastClosest +/// @ingroup world +typedef struct b3RayResult +{ + b3ShapeId shapeId; + b3Vec3 point; + b3Vec3 normal; + uint64_t userMaterialId; + float fraction; + int triangleIndex; + int childIndex; + int nodeVisits; + int leafVisits; + bool hit; +} b3RayResult; + +/// Optional world capacities that can be use to avoid run-time allocations +typedef struct b3Capacity +{ + int staticShapeCount; + int dynamicShapeCount; + int staticBodyCount; + int dynamicBodyCount; + int contactCount; +} b3Capacity; + +/// World definition used to create a simulation world. Must be initialized using b3DefaultWorldDef. +typedef struct b3WorldDef +{ + /// Gravity vector. Box3D has no up-vector defined. + b3Vec3 gravity; + + /// Restitution speed threshold, usually in m/s. Collisions above this + /// speed have restitution applied (will bounce). + float restitutionThreshold; + + /// Hit event speed threshold, usually in m/s. Collisions above this + /// speed can generate hit events if the shape also enables hit events. + float hitEventThreshold; + + /// Contact stiffness. Cycles per second. Increasing this increases the speed of overlap recovery, but can introduce jitter. + float contactHertz; + + /// Contact bounciness. Non-dimensional. You can speed up overlap recovery by decreasing this with + /// the trade-off that overlap resolution becomes more energetic. + float contactDampingRatio; + + /// This parameter controls how fast overlap is resolved and usually has units of meters per second. This only + /// puts a cap on the resolution speed. The resolution speed is increased by increasing the hertz and/or + /// decreasing the damping ratio. + float contactSpeed; + + /// Maximum linear speed. Usually meters per second. + float maximumLinearSpeed; + + /// Optional mixing callback for friction. The default uses sqrt(frictionA * frictionB). + b3FrictionCallback* frictionCallback; + + /// Optional mixing callback for restitution. The default uses max(restitutionA, restitutionB). + b3RestitutionCallback* restitutionCallback; + + /// Can bodies go to sleep to improve performance + bool enableSleep; + + /// Enable continuous collision + bool enableContinuous; + + /// Number of workers to use with the provided task system. Box3D performs best when using only + /// performance cores and accessing a single L2 cache. Efficiency cores and hyper-threading provide + /// little benefit and may even harm performance. + /// @note Box3D does not create threads. This is the number of threads your applications has created + /// that you are allocating to b3World_Step. + /// @warning Do not modify the default value unless you are also providing a task system and providing + /// task callbacks (enqueueTask and finishTask). + uint32_t workerCount; + + /// function to spawn task + b3EnqueueTaskCallback* enqueueTask; + + /// function to finish a task + b3FinishTaskCallback* finishTask; + + /// User context that is provided to enqueueTask and finishTask + void* userTaskContext; + + /// User data associated with a world + void* userData; + + /// Used to create debug draw shapes. This is called when a shape is + /// first drawn using b3DebugDraw. + b3CreateDebugShapeCallback* createDebugShape; + + /// Used to destroy debug draw shapes. This is called when a shape is modified or destroyed. + b3DestroyDebugShapeCallback* destroyDebugShape; + + /// This is passed to the debug shape callbacks to provide a user context. + void* userDebugShapeContext; + + /// Optional initial capacities + b3Capacity capacity; + + /// Used internally to detect a valid definition. DO NOT SET. + int internalValue; +} b3WorldDef; + +/// Use this to initialize your world definition +/// @ingroup world +B3_API b3WorldDef b3DefaultWorldDef( void ); + +/// The body simulation type. +/// Each body is one of these three types. The type determines how the body behaves in the simulation. +/// @ingroup body +typedef enum b3BodyType +{ + /// zero mass, zero velocity, may be manually moved + b3_staticBody = 0, + + /// zero mass, velocity set by user, moved by solver + b3_kinematicBody = 1, + + /// positive mass, velocity determined by forces, moved by solver + b3_dynamicBody = 2, + + /// number of body types + b3_bodyTypeCount, +} b3BodyType; + +/// Motion locks to restrict the body movement +typedef struct b3MotionLocks +{ + /// Prevent translation along the x-axis + bool linearX; + + /// Prevent translation along the y-axis + bool linearY; + + /// Prevent translation along the z-axis + bool linearZ; + + /// Prevent rotation around the x-axis + bool angularX; + + /// Prevent rotation around the y-axis + bool angularY; + + /// Prevent rotation around the z-axis + bool angularZ; +} b3MotionLocks; + +/// A body definition holds all the data needed to construct a rigid body. +/// You can safely re-use body definitions. Shapes are added to a body after construction. +/// Body definitions are temporary objects used to bundle creation parameters. +/// Must be initialized using b3DefaultBodyDef(). +/// @ingroup body +typedef struct b3BodyDef +{ + /// The body type: static, kinematic, or dynamic. + b3BodyType type; + + /// The initial world position of the body. Bodies should be created with the desired position. + /// @note Creating bodies at the origin and then moving them nearly doubles the cost of body creation, especially + /// if the body is moved after shapes have been added. + b3Vec3 position; + + /// The initial world rotation of the body. + b3Quat rotation; + + /// The initial linear velocity of the body's origin. Usually in meters per second. + b3Vec3 linearVelocity; + + /// The initial angular velocity of the body. Radians per second. + b3Vec3 angularVelocity; + + /// Linear damping is used to reduce the linear velocity. The damping parameter + /// can be larger than 1 but the damping effect becomes sensitive to the + /// time step when the damping parameter is large. + /// Generally linear damping is undesirable because it makes objects move slowly + /// as if they are floating. + float linearDamping; + + /// Angular damping is used to reduce the angular velocity. The damping parameter + /// can be larger than 1.0f but the damping effect becomes sensitive to the + /// time step when the damping parameter is large. + /// Angular damping can be use slow down rotating bodies. + float angularDamping; + + /// Scale the gravity applied to this body. Non-dimensional. + float gravityScale; + + /// Sleep speed threshold, default is 0.05 meters per second + float sleepThreshold; + + /// Optional body name for debugging. Up to 31 characters (excluding null termination) + const char* name; + + /// Use this to store application specific body data. + void* userData; + + /// Motions locks to restrict linear and angular movement + b3MotionLocks motionLocks; + + /// Set this flag to false if this body should never fall asleep. + bool enableSleep; + + /// Is this body initially awake or sleeping? + bool isAwake; + + /// Treat this body as high speed object that performs continuous collision detection + /// against dynamic and kinematic bodies, but not other bullet bodies. + /// @warning Bullets should be used sparingly. They are not a solution for general dynamic-versus-dynamic + /// continuous collision. + bool isBullet; + + /// Used to disable a body. A disabled body does not move or collide. + bool isEnabled; + + /// This allows this body to bypass rotational speed limits. Should only be used + /// for circular objects, like wheels. + bool allowFastRotation; + + /// Enable continuous collision. This is true by default and should only be disabled for application reasons, such + /// as wheels on a vehicle. + /// todo not hooked up yet + bool enableContinuousCollision; + + /// Used internally to detect a valid definition. DO NOT SET. + int internalValue; +} b3BodyDef; + +/// Use this to initialize your body definition +/// @ingroup body +B3_API b3BodyDef b3DefaultBodyDef( void ); + +/// This is used to filter collision on shapes. It affects shape-vs-shape collision +/// and shape-versus-query collision (such as b3World_CastRay). +/// @ingroup shape +typedef struct b3Filter +{ + /// The collision category bits. Normally you would just set one bit. The category bits should + /// represent your application object types. For example: + /// @code{.cpp} + /// enum MyCategories + /// { + /// Static = 0x00000001, + /// Dynamic = 0x00000002, + /// Debris = 0x00000004, + /// Player = 0x00000008, + /// // etc + /// }; + /// @endcode + uint64_t categoryBits; + + /// The collision mask bits. This states the categories that this + /// shape would accept for collision. + /// For example, you may want your player to only collide with static objects + /// and other players. + /// @code{.c} + /// maskBits = Static | Player; + /// @endcode + uint64_t maskBits; + + /// Collision groups allow a certain group of objects to never collide (negative) + /// or always collide (positive). A group index of zero has no effect. Non-zero group filtering + /// always wins against the mask bits. + /// For example, you may want ragdolls to collide with other ragdolls but you don't want + /// ragdoll self-collision. In this case you would give each ragdoll a unique negative group index + /// and apply that group index to all shapes on the ragdoll. + int groupIndex; +} b3Filter; + +/// Use this to initialize your filter +/// @ingroup shape +B3_API b3Filter b3DefaultFilter( void ); + +/// The query filter is used to filter collisions between queries and shapes. For example, +/// you may want a ray-cast representing a projectile to hit players and the static environment +/// but not debris. +/// @ingroup shape +typedef struct b3QueryFilter +{ + /// The collision category bits of this query. Normally you would just set one bit. + uint64_t categoryBits; + + /// The collision mask bits. This states the shape categories that this + /// query would accept for collision. + uint64_t maskBits; +} b3QueryFilter; + +/// Use this to initialize your query filter +/// @ingroup shape +B3_API b3QueryFilter b3DefaultQueryFilter( void ); + +/// Shape type +/// @ingroup shape +typedef enum b3ShapeType +{ + /// A capsule is an extruded sphere + b3_capsuleShape, + + /// A compound shape composed of up to 64K spheres, capsules, hulls, and meshes + b3_compoundShape, + + /// A height field useful for terrain + b3_heightShape, + + /// A convex hull + b3_hullShape, + + /// A triangle soup + b3_meshShape, + + /// A sphere with an offset + b3_sphereShape, + + /// The number of shape types + b3_shapeTypeCount +} b3ShapeType; + +/// Used to create a shape +typedef struct b3ShapeDef +{ + /// Optional body name for debugging. Up to 31 characters (excluding null termination) + const char* name; + + /// Use this to store application specific shape data. + void* userData; + + /// Surface material used on mesh shapes per triangle. Ignored for convex shapes. Ignored for compound shapes. + b3SurfaceMaterial* materials; + + /// Surface material count. + int materialCount; + + /// The base surface material. Ignored for compound shapes. + b3SurfaceMaterial baseMaterial; + + /// The density, usually in kg/m^2. + float density; + + /// Explosion scale for b3World_Explode. non-dimensional + float explosionScale; + + /// Contact filtering data. + b3Filter filter; + + /// Enable custom filtering. Only one of the two shapes needs to enable custom filtering. See b2WorldDef. + bool enableCustomFiltering; + + /// A sensor shape generates overlap events but never generates a collision response. + /// Sensors do not have continuous collision. Instead, use a ray or shape cast for those scenarios. + /// Sensors still contribute to the body mass if they have non-zero density. + /// @note Sensor events are disabled by default. + /// @see enableSensorEvents + bool isSensor; + + /// Enable sensor events for this shape. This applies to sensors and non-sensors. False by default, even for sensors. + bool enableSensorEvents; + + /// Enable contact events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. False by default. + bool enableContactEvents; + + /// Enable hit events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. False by default. + bool enableHitEvents; + + /// Enable pre-solve contact events for this shape. Only applies to dynamic bodies. These are expensive + /// and must be carefully handled due to multithreading. Ignored for sensors. + bool enablePreSolveEvents; + + /// When shapes are created they will scan the environment for collision the next time step. This can significantly slow down + /// static body creation when there are many static shapes. + /// This is flag is ignored for dynamic and kinematic shapes which always invoke contact creation. + bool invokeContactCreation; + + /// Should the body update the mass properties when this shape is created. Default is true. + bool updateBodyMass; + + /// Used internally to detect a valid definition. DO NOT SET. + int internalValue; + +} b3ShapeDef; + +/// Use this to initialize your shape definition +/// @ingroup shape +B3_API b3ShapeDef b3DefaultShapeDef( void ); + +//! @cond +/// Profiling data. Times are in milliseconds. +typedef struct b3Profile +{ + float step; + float pairs; + float collide; + float solve; + float solverSetup; + float constraints; + float prepareConstraints; + float integrateVelocities; + float warmStart; + float solveImpulses; + float integratePositions; + float relaxImpulses; + float applyRestitution; + float storeImpulses; + float splitIslands; + float transforms; + float sensorHits; + float jointEvents; + float hitEvents; + float refit; + float bullets; + float sleepIslands; + float sensors; +} b3Profile; + +/// Counters that give details of the simulation size. +typedef struct b3Counters +{ + int bodyCount; + int shapeCount; + int contactCount; + int jointCount; + int islandCount; + int stackUsed; + int staticTreeHeight; + int treeHeight; + int satCallCount; + int satCacheHitCount; + int byteCount; + int taskCount; + int colorCounts[24]; + int manifoldCounts[B3_CONTACT_MANIFOLD_COUNT_BUCKETS]; + + // Number of contacts touched by the collide pass (graph contacts + awake-set non-touching). + int awakeContactCount; + + // Number of contacts recycled in the most recent step. + int recycledContactCount; +} b3Counters; +//! @endcond + +/// Joint type enumeration +/// +/// This is useful because all joint types use b3JointId and sometimes you +/// want to get the type of a joint. +/// @ingroup joint +typedef enum b3JointType +{ + b3_parallelJoint, + b3_distanceJoint, + b3_filterJoint, + b3_motorJoint, + b3_prismaticJoint, + b3_revoluteJoint, + b3_sphericalJoint, + b3_weldJoint, + b3_wheelJoint, +} b3JointType; + +/// Base joint definition used by all joint types. +/// The local frames are measured from the body's origin +/// rather than the center of mass because: +/// 1. you might not know where the center of mass will be +/// 2. if you add/remove shapes from a body and recompute the mass, the joints will be broken +typedef struct b3JointDef +{ + /// User data pointer + void* userData; + + /// The first attached body + b3BodyId bodyIdA; + + /// The second attached body + b3BodyId bodyIdB; + + /// The first local joint frame + b3Transform localFrameA; + + /// The second local joint frame + b3Transform localFrameB; + + /// Force threshold for joint events + float forceThreshold; + + /// Torque threshold for joint events + float torqueThreshold; + + /// Constraint hertz (advanced feature) + float constraintHertz; + + /// Constraint damping ratio (advanced feature) + float constraintDampingRatio; + + /// Debug draw scale + float drawScale; + + /// Set this flag to true if the attached bodies should collide + bool collideConnected; + + /// Used internally to detect a valid definition. DO NOT SET. + int internalValue; +} b3JointDef; + +/// Distance joint definition +/// Connects a point on body A with a point on body B by a segment. +/// Useful for ropes and springs. +/// @ingroup distance_joint +typedef struct b3DistanceJointDef +{ + /// Base joint definition + b3JointDef base; + + /// The rest length of this joint. Clamped to a stable minimum value. + float length; + + /// Enable the distance constraint to behave like a spring. If false + /// then the distance joint will be rigid, overriding the limit and motor. + bool enableSpring; + + /// The lower spring force controls how much tension it can sustain + float lowerSpringForce; + + /// The upper spring force controls how much compression it an sustain + float upperSpringForce; + + /// The spring linear stiffness Hertz, cycles per second + float hertz; + + /// The spring linear damping ratio, non-dimensional + float dampingRatio; + + /// Enable/disable the joint limit + bool enableLimit; + + /// Minimum length. Clamped to a stable minimum value. + float minLength; + + /// Maximum length. Must be greater than or equal to the minimum length. + float maxLength; + + /// Enable/disable the joint motor + bool enableMotor; + + /// The maximum motor force, usually in newtons + float maxMotorForce; + + /// The desired motor speed, usually in meters per second + float motorSpeed; +} b3DistanceJointDef; + +/// Use this to initialize your joint definition +/// @ingroup distance_joint +B3_API b3DistanceJointDef b3DefaultDistanceJointDef( void ); + +/// A motor joint is used to control the relative position and velocity between two bodies. +/// @ingroup motor_joint +typedef struct b3MotorJointDef +{ + /// Base joint definition + b3JointDef base; + + /// The desired linear velocity + b3Vec3 linearVelocity; + + /// The maximum motor force in newtons + float maxVelocityForce; + + /// The desired angular velocity + b3Vec3 angularVelocity; + + /// The maximum motor torque in newton-meters + float maxVelocityTorque; + + /// Linear spring hertz for position control + float linearHertz; + + /// Linear spring damping ratio + float linearDampingRatio; + + /// Maximum spring force in newtons + float maxSpringForce; + + /// Angular spring hertz for position control + float angularHertz; + + /// Angular spring damping ratio + float angularDampingRatio; + + /// Maximum spring torque in newton-meters + float maxSpringTorque; +} b3MotorJointDef; + +/// Use this to initialize your joint definition +/// @ingroup motor_joint +B3_API b3MotorJointDef b3DefaultMotorJointDef( void ); + +/// A filter joint is used to disable collision between two specific bodies. +/// +/// @ingroup filter_joint +typedef struct b3FilterJointDef +{ + /// Base joint definition + b3JointDef base; +} b3FilterJointDef; + +/// Use this to initialize your joint definition +/// @ingroup filter_joint +B3_API b3FilterJointDef b3DefaultFilterJointDef( void ); + +/// Parallel joint definition +/// Constrains the angle between axis z in body A and axis z in body B +/// using a spring. +/// Useful to keep a body upright. +/// @ingroup cone_joint +typedef struct b3ParallelJointDef +{ + /// Base joint definition + b3JointDef base; + + float hertz; + float dampingRatio; + float maxTorque; + +} b3ParallelJointDef; + +/// Use this to initialize your joint definition +/// @ingroup parallel_joint +B3_API b3ParallelJointDef b3DefaultParallelJointDef( void ); + +/// Prismatic joint definition +/// Body B may slide along the x-axis in local frame A. Body B cannot rotate relative to body A. +/// The joint translation is zero when the local frame origins coincide in world space. +/// @ingroup prismatic_joint +typedef struct b3PrismaticJointDef +{ + /// Base joint definition + b3JointDef base; + + /// Enable a linear spring along the prismatic joint axis + bool enableSpring; + + /// The spring stiffness Hertz, cycles per second + float hertz; + + /// The spring damping ratio, non-dimensional + float dampingRatio; + + /// The target translation for the joint in meters. The spring-damper will drive + /// to this translation. + float targetTranslation; + + /// Enable/disable the joint limit + bool enableLimit; + + /// The lower translation limit + float lowerTranslation; + + /// The upper translation limit + float upperTranslation; + + /// Enable/disable the joint motor + bool enableMotor; + + /// The maximum motor force, typically in newtons + float maxMotorForce; + + /// The desired motor speed, typically in meters per second + float motorSpeed; +} b3PrismaticJointDef; + +/// Use this to initialize your joint definition +/// @ingroupd prismatic_joint +B3_API b3PrismaticJointDef b3DefaultPrismaticJointDef( void ); + +/// Revolute joint definition +/// A point on body B is fixed to a point on body A. Allows relative rotation about the z-axis. +/// @ingroup revolute_joint +typedef struct b3RevoluteJointDef +{ + /// Base joint definition + b3JointDef base; + + /// The bodyB angle minus bodyA angle in the reference state (radians). + /// This defines the zero angle for the joint limit. + float targetAngle; + + /// Enable a rotational spring on the revolute hinge axis + bool enableSpring; + + /// The spring stiffness Hertz, cycles per second + float hertz; + + /// The spring damping ratio, non-dimensional + float dampingRatio; + + /// A flag to enable joint limits + bool enableLimit; + + /// The lower angle for the joint limit in radians + float lowerAngle; + + /// The upper angle for the joint limit in radians + float upperAngle; + + /// A flag to enable the joint motor + bool enableMotor; + + /// The maximum motor torque, typically in newton-meters + float maxMotorTorque; + + /// The desired motor speed in radians per second + float motorSpeed; +} b3RevoluteJointDef; + +/// Use this to initialize your joint definition. +/// @ingroup revolute_joint +B3_API b3RevoluteJointDef b3DefaultRevoluteJointDef( void ); + +/// Spherical joint definition +/// A point on body B is fixed to a point on body A. Allows rotation about the shared point. +/// @ingroup spherical_joint +typedef struct b3SphericalJointDef +{ + /// Base joint definition + b3JointDef base; + + /// Enable a rotational spring that attempts to align the two joint frames. + bool enableSpring; + + /// The spring stiffness Hertz, cycles per second. This may be clamped internally + /// according to the time step to maintain stability. Non-negative number. + float hertz; + + /// The spring damping ratio, non-dimensional. Non-negative number. + float dampingRatio; + + /// Target spring rotation, joint frame B relative to joint frame A. + b3Quat targetRotation; + + /// A flag to enable the cone limit. The cone is centered on the frameA z-axis. + bool enableConeLimit; + + /// The angle for the cone limit in radians. Valid range is [0, pi] + float coneAngle; + + /// A flag to enable the twist limit. The twist is centered on the frameB z-axis. + bool enableTwistLimit; + + /// The angle for the lower twist limit in radians. Valid range is [-pi, pi]. + float lowerTwistAngle; + + /// The angle for the upper twist limit in radians. Valid range is [-pi, pi]. + float upperTwistAngle; + + /// A flag to enable the joint motor + bool enableMotor; + + /// The maximum motor torque, typically in newton-meters. Non-negative number. + float maxMotorTorque; + + /// The desired motor angular velocity in radians per second. + b3Vec3 motorVelocity; +} b3SphericalJointDef; + +/// Use this to initialize your joint definition. +/// @ingroup spherical_joint +B3_API b3SphericalJointDef b3DefaultSphericalJointDef( void ); + +/// Weld joint definition +/// Connects two bodies together rigidly. This constraint provides springs to mimic +/// soft-body simulation. +/// @note The approximate solver in Box3D cannot hold many bodies together rigidly +/// @ingroup weld_joint +typedef struct b3WeldJointDef +{ + /// Base joint definition + b3JointDef base; + + /// Linear stiffness expressed as Hertz (cycles per second). Use zero for maximum stiffness. + float linearHertz; + + /// Angular stiffness as Hertz (cycles per second). Use zero for maximum stiffness. + float angularHertz; + + /// Linear damping ratio, non-dimensional. Use 1 for critical damping. + float linearDampingRatio; + + /// Linear damping ratio, non-dimensional. Use 1 for critical damping. + float angularDampingRatio; +} b3WeldJointDef; + +/// Use this to initialize your joint definition +/// @ingroup weld_joint +B3_API b3WeldJointDef b3DefaultWeldJointDef( void ); + +/// Wheel joint definition +/// Body A is the chassis and body B is the wheel. +/// The wheel rotates around the local z-axis in frame B. +/// The wheel translations along the local x-axis in frame A. +/// The wheel can optionally steer along the x-axis in frame A. +/// @ingroup wheel_joint +typedef struct b3WheelJointDef +{ + /// Base joint definition + b3JointDef base; + + /// Enable a linear spring along the local axis + bool enableSuspensionSpring; + + /// Spring stiffness in Hertz + float suspensionHertz; + + /// Spring damping ratio, non-dimensional + float suspensionDampingRatio; + + /// Enable/disable the joint linear limit + bool enableSuspensionLimit; + + /// The lower suspension translation limit + float lowerSuspensionLimit; + + /// The upper translation limit + float upperSuspensionLimit; + + /// Enable/disable the joint rotational motor + bool enableSpinMotor; + + /// The maximum motor torque, typically in newton-meters + float maxSpinTorque; + + /// The desired motor speed in radians per second + float spinSpeed; + + /// Enable steering, otherwise the steering is fixed forward + bool enableSteering; + + /// Steering stiffness in Hertz + float steeringHertz; + + /// Spring damping ratio, non-dimensional + float steeringDampingRatio; + + /// The target steering angle in radians + float targetSteeringAngle; + + /// The maximum steering torque in N*m + float maxSteeringTorque; + + /// Enable/disable the steering angular limit + bool enableSteeringLimit; + + /// The lower steering angle in radians + float lowerSteeringLimit; + + /// The upper steering angle in radians + float upperSteeringLimit; +} b3WheelJointDef; + +/// Use this to initialize your joint definition +/// @ingroup wheel_joint +B3_API b3WheelJointDef b3DefaultWheelJointDef( void ); + +/// The explosion definition is used to configure options for explosions. Explosions +/// consider shape geometry when computing the impulse. +/// @ingroup world +typedef struct b3ExplosionDef +{ + /// Mask bits to filter shapes + uint64_t maskBits; + + /// The center of the explosion in world space + b3Vec3 position; + + /// The radius of the explosion + float radius; + + /// The falloff distance beyond the radius. Impulse is reduced to zero at this distance. + float falloff; + + /// Impulse per unit area. This applies an impulse according to the shape area that + /// is facing the explosion. Explosions only apply to spheres, capsules, and hulls. This + /// may be negative for implosions. + float impulsePerArea; +} b3ExplosionDef; + +/// Use this to initialize your explosion definition +/// @ingroup world +B3_API b3ExplosionDef b3DefaultExplosionDef( void ); + +/** + * @defgroup events Events + * World event types. + * + * Events are used to collect events that occur during the world time step. These events + * are then available to query after the time step is complete. This is preferable to callbacks + * because Box3D uses multithreaded simulation. + * + * Also when events occur in the simulation step it may be problematic to modify the world, which is + * often what applications want to do when events occur. + * + * With event arrays, you can scan the events in a loop and modify the world. However, you need to be careful + * that some event data may become invalid. There are several samples that show how to do this safely. + * + * @{ + */ + +/// A begin-touch event is generated when a shape starts to overlap a sensor shape. +typedef struct b3SensorBeginTouchEvent +{ + /// The id of the sensor shape + b3ShapeId sensorShapeId; + + /// The id of the shape that began touching the sensor shape + b3ShapeId visitorShapeId; +} b3SensorBeginTouchEvent; + +/// An end touch event is generated when a shape stops overlapping a sensor shape. +/// These include things like setting the transform, destroying a body or shape, or changing +/// a filter. You will also get an end event if the sensor or visitor are destroyed. +/// Therefore you should always confirm the shape id is valid using b3Shape_IsValid. +typedef struct b3SensorEndTouchEvent +{ + /// The id of the sensor shape + /// @warning this shape may have been destroyed + /// @see b2Shape_IsValid + b3ShapeId sensorShapeId; + + /// The id of the shape that stopped touching the sensor shape + /// @warning this shape may have been destroyed + /// @see b2Shape_IsValid + b3ShapeId visitorShapeId; +} b3SensorEndTouchEvent; + +/// Sensor events are buffered in the world and are available +/// as begin/end overlap event arrays after the time step is complete. +/// Note: these may become invalid if bodies and/or shapes are destroyed +typedef struct b3SensorEvents +{ + /// Array of sensor begin touch events + b3SensorBeginTouchEvent* beginEvents; + + /// Array of sensor end touch events + b3SensorEndTouchEvent* endEvents; + + /// The number of begin touch events + int beginCount; + + /// The number of end touch events + int endCount; +} b3SensorEvents; + +/// A begin-touch event is generated when two shapes begin touching. +typedef struct b3ContactBeginTouchEvent +{ + /// Id of the first shape + b3ShapeId shapeIdA; + + /// Id of the second shape + b3ShapeId shapeIdB; + + /// The transient contact id. This contact maybe destroyed automatically when the world is modified or simulated. + /// Used b3Contact_IsValid before using this id. + b3ContactId contactId; +} b3ContactBeginTouchEvent; + +/// An end touch event is generated when two shapes stop touching. +/// You will get an end event if you do anything that destroys contacts previous to the last +/// world step. These include things like setting the transform, destroying a body +/// or shape, or changing a filter or body type. +typedef struct b3ContactEndTouchEvent +{ + /// Id of the first shape + /// @warning this shape may have been destroyed + /// @see b3Shape_IsValid + b3ShapeId shapeIdA; + + /// Id of the first shape + /// @warning this shape may have been destroyed + /// @see b3Shape_IsValid + b3ShapeId shapeIdB; + + /// Id of the contact. + /// @warning this contact may have been destroyed + /// @see b3Contact_IsValid + b3ContactId contactId; +} b3ContactEndTouchEvent; + +/// A hit touch event is generated when two shapes collide with a speed faster than the hit speed threshold. +/// This may be reported for speculative contacts that have a confirmed impulse. +typedef struct b3ContactHitEvent +{ + /// Id of the first shape + b3ShapeId shapeIdA; + + /// Id of the second shape + b3ShapeId shapeIdB; + + /// Id of the contact. + /// @warning this contact may have been destroyed + /// @see b3Contact_IsValid + b3ContactId contactId; + + /// Point where the shapes hit at the beginning of the time step. + /// This is a mid-point between the two surfaces. It could be at speculative + /// point where the two shapes were not touching at the beginning of the time step. + b3Vec3 point; + + /// Normal vector pointing from shape A to shape B + b3Vec3 normal; + + /// The speed the shapes are approaching. Always positive. Typically in meters per second. + float approachSpeed; + + /// User material on shape A + uint64_t userMaterialIdA; + + /// User material on shape B + uint64_t userMaterialIdB; + +} b3ContactHitEvent; + +/// Contact events are buffered in the world and are available +/// as event arrays after the time step is complete. +/// Note: these may become invalid if bodies and/or shapes are destroyed +typedef struct b3ContactEvents +{ + /// Array of begin touch events + b3ContactBeginTouchEvent* beginEvents; + + /// Array of end touch events + b3ContactEndTouchEvent* endEvents; + + /// Array of hit events + b3ContactHitEvent* hitEvents; + + /// Number of begin touch events + int beginCount; + + /// Number of end touch events + int endCount; + + /// Number of hit events + int hitCount; +} b3ContactEvents; + +/// Body move events triggered when a body moves. +/// Triggered when a body moves due to simulation. Not reported for bodies moved by the user. +/// This also has a flag to indicate that the body went to sleep so the application can also +/// sleep that actor/entity/object associated with the body. +/// On the other hand if the flag does not indicate the body went to sleep then the application +/// can treat the actor/entity/object associated with the body as awake. +/// This is an efficient way for an application to update game object transforms rather than +/// calling functions such as b3Body_GetTransform() because this data is delivered as a contiguous array +/// and it is only populated with bodies that have moved. +/// @note If sleeping is disabled all dynamic and kinematic bodies will trigger move events. +typedef struct b3BodyMoveEvent +{ + void* userData; + b3Transform transform; + b3BodyId bodyId; + bool fellAsleep; +} b3BodyMoveEvent; + +/// Body events are buffered in the world and are available +/// as event arrays after the time step is complete. +/// Note: this date becomes invalid if bodies are destroyed +typedef struct b3BodyEvents +{ + /// Array of move events + b3BodyMoveEvent* moveEvents; + + /// Number of move events + int moveCount; +} b3BodyEvents; + +/// Joint events report joints that are awake and have a force and/or torque exceeding the threshold +/// The observed forces and torques are not returned for efficiency reasons. +typedef struct b3JointEvent +{ + /// The joint id + b3JointId jointId; + + /// The user data from the joint for convenience + void* userData; +} b3JointEvent; + +/// Joint events are buffered in the world and are available +/// as event arrays after the time step is complete. +/// Note: this data becomes invalid if joints are destroyed +typedef struct b3JointEvents +{ + /// Array of events + b3JointEvent* jointEvents; + + /// Number of events + int count; +} b3JointEvents; + +/// The contact data for two shapes. By convention the manifold normal points +/// from shape A to shape B. +/// @see b3Shape_GetContactData() and b3Body_GetContactData() +typedef struct b3ContactData +{ + b3ContactId contactId; + b3ShapeId shapeIdA; + b3ShapeId shapeIdB; + const struct b3Manifold* manifolds; + int manifoldCount; +} b3ContactData; + +/**@}*/ + +/// Prototype for a contact filter callback. +/// This is called when a contact pair is considered for collision. This allows you to +/// perform custom logic to prevent collision between shapes. This is only called if +/// one of the two shapes has custom filtering enabled. @see b3ShapeDef. +/// Notes: +/// - this function must be thread-safe +/// - this is only called if one of the two shapes has enabled custom filtering +/// - this is called only for awake dynamic bodies +/// Return false if you want to disable the collision +/// @warning Do not attempt to modify the world inside this callback +/// @ingroup world +typedef bool b3CustomFilterFcn( b3ShapeId shapeIdA, b3ShapeId shapeIdB, void* context ); + +/// Prototype for a pre-solve callback. +/// This is called after a contact is updated. This allows you to inspect a +/// collision before it goes to the solver. +/// Notes: +/// - this function must be thread-safe +/// - this is only called if the shape has enabled pre-solve events +/// - this may be called for awake dynamic bodies and sensors +/// - this is not called for sensors +/// Return false if you want to disable the contact this step +/// This has limited information because it is used during CCD which does not have the +/// full contact manifold. +/// @warning Do not attempt to modify the world inside this callback +/// @ingroup world +typedef bool b3PreSolveFcn( b3ShapeId shapeIdA, b3ShapeId shapeIdB, b3Vec3 point, b3Vec3 normal, void* context ); + +/// Prototype callback for overlap queries. +/// Called for each shape found in the query. +/// @see b3World_QueryAABB +/// @return false to terminate the query. +/// @ingroup world +typedef bool b3OverlapResultFcn( b3ShapeId shapeId, void* context ); + +/// Prototype callback for ray casts. +/// Called for each shape found in the query. You control how the ray cast +/// proceeds by returning a float: +/// return -1: ignore this shape and continue +/// return 0: terminate the ray cast +/// return fraction: clip the ray to this point +/// return 1: don't clip the ray and continue +/// @param shapeId the shape hit by the ray +/// @param point the point of initial intersection +/// @param normal the normal vector at the point of intersection +/// @param fraction the fraction along the ray at the point of intersection +/// @param userMaterialId the shape or triangle surface type +/// @param triangleIndex the triangle index for mesh or height field shapes or -1 for other shape types +/// @param childIndex the child shape index for compound shapes +/// @param context the user context +/// @return -1 to filter, 0 to terminate, fraction to clip the ray for closest hit, 1 to continue +/// @see b3World_CastRay +/// @ingroup world +typedef float b3CastResultFcn( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t userMaterialId, + int triangleIndex, int childIndex, void* context ); + +// Used to collect collision planes for character movers. +// Return true to continue gathering planes. +typedef bool b3PlaneResultFcn( b3ShapeId shapeId, const b3PlaneResult* plane, int planeCount, void* context ); + +// Used to filter shapes for shape casting character movers. +// Return true to accept the collision +typedef bool b3MoverFilterFcn( b3ShapeId shapeId, void* context ); + +/// Body ray cast for ray casting a specific body with a specified transform. +typedef struct b3BodyRayCastInput +{ + b3Vec3 origin; + b3Vec3 translation; + b3QueryFilter filter; + float maxFraction; +} b3BodyRayCastInput; + +/// Body shape cast for shape casting a specific body with a specified transform. +typedef struct b3BodyShapeCastInput +{ + b3ShapeProxy proxy; + b3Vec3 translation; + b3QueryFilter filter; + float maxFraction; + bool canEncroach; +} b3BodyShapeCastInput; + +/// Body cast result for ray and shape casts. +typedef struct b3BodyCastResult +{ + b3ShapeId shapeId; + b3Vec3 point; + b3Vec3 normal; + float fraction; + int triangleIndex; + uint64_t userMaterialId; + int iterations; + bool hit; +} b3BodyCastResult; + +/// Body plane result for movers. +typedef struct b3BodyPlaneResult +{ + b3ShapeId shapeId; + b3PlaneResult result; +} b3BodyPlaneResult; + +/// These colors are used for debug draw and mostly match the named SVG colors. +/// See https://www.rapidtables.com/web/color/index.html +/// https://johndecember.com/html/spec/colorsvg.html +/// https://upload.wikimedia.org/wikipedia/commons/2/2b/SVG_Recognized_color_keyword_names.svg +typedef enum b3HexColor +{ + b3_colorAliceBlue = 0xF0F8FF, + b3_colorAntiqueWhite = 0xFAEBD7, + b3_colorAqua = 0x00FFFF, + b3_colorAquamarine = 0x7FFFD4, + b3_colorAzure = 0xF0FFFF, + b3_colorBeige = 0xF5F5DC, + b3_colorBisque = 0xFFE4C4, + b3_colorBlack = 0x000000, + b3_colorBlanchedAlmond = 0xFFEBCD, + b3_colorBlue = 0x0000FF, + b3_colorBlueViolet = 0x8A2BE2, + b3_colorBrown = 0xA52A2A, + b3_colorBurlywood = 0xDEB887, + b3_colorCadetBlue = 0x5F9EA0, + b3_colorChartreuse = 0x7FFF00, + b3_colorChocolate = 0xD2691E, + b3_colorCoral = 0xFF7F50, + b3_colorCornflowerBlue = 0x6495ED, + b3_colorCornsilk = 0xFFF8DC, + b3_colorCrimson = 0xDC143C, + b3_colorCyan = 0x00FFFF, + b3_colorDarkBlue = 0x00008B, + b3_colorDarkCyan = 0x008B8B, + b3_colorDarkGoldenRod = 0xB8860B, + b3_colorDarkGray = 0xA9A9A9, + b3_colorDarkGreen = 0x006400, + b3_colorDarkKhaki = 0xBDB76B, + b3_colorDarkMagenta = 0x8B008B, + b3_colorDarkOliveGreen = 0x556B2F, + b3_colorDarkOrange = 0xFF8C00, + b3_colorDarkOrchid = 0x9932CC, + b3_colorDarkRed = 0x8B0000, + b3_colorDarkSalmon = 0xE9967A, + b3_colorDarkSeaGreen = 0x8FBC8F, + b3_colorDarkSlateBlue = 0x483D8B, + b3_colorDarkSlateGray = 0x2F4F4F, + b3_colorDarkTurquoise = 0x00CED1, + b3_colorDarkViolet = 0x9400D3, + b3_colorDeepPink = 0xFF1493, + b3_colorDeepSkyBlue = 0x00BFFF, + b3_colorDimGray = 0x696969, + b3_colorDodgerBlue = 0x1E90FF, + b3_colorFireBrick = 0xB22222, + b3_colorFloralWhite = 0xFFFAF0, + b3_colorForestGreen = 0x228B22, + b3_colorFuchsia = 0xFF00FF, + b3_colorGainsboro = 0xDCDCDC, + b3_colorGhostWhite = 0xF8F8FF, + b3_colorGold = 0xFFD700, + b3_colorGoldenRod = 0xDAA520, + b3_colorGray = 0x808080, + b3_colorGreen = 0x008000, + b3_colorGreenYellow = 0xADFF2F, + b3_colorHoneyDew = 0xF0FFF0, + b3_colorHotPink = 0xFF69B4, + b3_colorIndianRed = 0xCD5C5C, + b3_colorIndigo = 0x4B0082, + b3_colorIvory = 0xFFFFF0, + b3_colorKhaki = 0xF0E68C, + b3_colorLavender = 0xE6E6FA, + b3_colorLavenderBlush = 0xFFF0F5, + b3_colorLawnGreen = 0x7CFC00, + b3_colorLemonChiffon = 0xFFFACD, + b3_colorLightBlue = 0xADD8E6, + b3_colorLightCoral = 0xF08080, + b3_colorLightCyan = 0xE0FFFF, + b3_colorLightGoldenRodYellow = 0xFAFAD2, + b3_colorLightGray = 0xD3D3D3, + b3_colorLightGreen = 0x90EE90, + b3_colorLightPink = 0xFFB6C1, + b3_colorLightSalmon = 0xFFA07A, + b3_colorLightSeaGreen = 0x20B2AA, + b3_colorLightSkyBlue = 0x87CEFA, + b3_colorLightSlateGray = 0x778899, + b3_colorLightSteelBlue = 0xB0C4DE, + b3_colorLightYellow = 0xFFFFE0, + b3_colorLime = 0x00FF00, + b3_colorLimeGreen = 0x32CD32, + b3_colorLinen = 0xFAF0E6, + b3_colorMagenta = 0xFF00FF, + b3_colorMaroon = 0x800000, + b3_colorMediumAquaMarine = 0x66CDAA, + b3_colorMediumBlue = 0x0000CD, + b3_colorMediumOrchid = 0xBA55D3, + b3_colorMediumPurple = 0x9370DB, + b3_colorMediumSeaGreen = 0x3CB371, + b3_colorMediumSlateBlue = 0x7B68EE, + b3_colorMediumSpringGreen = 0x00FA9A, + b3_colorMediumTurquoise = 0x48D1CC, + b3_colorMediumVioletRed = 0xC71585, + b3_colorMidnightBlue = 0x191970, + b3_colorMintCream = 0xF5FFFA, + b3_colorMistyRose = 0xFFE4E1, + b3_colorMoccasin = 0xFFE4B5, + b3_colorNavajoWhite = 0xFFDEAD, + b3_colorNavy = 0x000080, + b3_colorOldLace = 0xFDF5E6, + b3_colorOlive = 0x808000, + b3_colorOliveDrab = 0x6B8E23, + b3_colorOrange = 0xFFA500, + b3_colorOrangeRed = 0xFF4500, + b3_colorOrchid = 0xDA70D6, + b3_colorPaleGoldenRod = 0xEEE8AA, + b3_colorPaleGreen = 0x98FB98, + b3_colorPaleTurquoise = 0xAFEEEE, + b3_colorPaleVioletRed = 0xDB7093, + b3_colorPapayaWhip = 0xFFEFD5, + b3_colorPeachPuff = 0xFFDAB9, + b3_colorPeru = 0xCD853F, + b3_colorPink = 0xFFC0CB, + b3_colorPlum = 0xDDA0DD, + b3_colorPowderBlue = 0xB0E0E6, + b3_colorPurple = 0x800080, + b3_colorRebeccaPurple = 0x663399, + b3_colorRed = 0xFF0000, + b3_colorRosyBrown = 0xBC8F8F, + b3_colorRoyalBlue = 0x4169E1, + b3_colorSaddleBrown = 0x8B4513, + b3_colorSalmon = 0xFA8072, + b3_colorSandyBrown = 0xF4A460, + b3_colorSeaGreen = 0x2E8B57, + b3_colorSeaShell = 0xFFF5EE, + b3_colorSienna = 0xA0522D, + b3_colorSilver = 0xC0C0C0, + b3_colorSkyBlue = 0x87CEEB, + b3_colorSlateBlue = 0x6A5ACD, + b3_colorSlateGray = 0x708090, + b3_colorSnow = 0xFFFAFA, + b3_colorSpringGreen = 0x00FF7F, + b3_colorSteelBlue = 0x4682B4, + b3_colorTan = 0xD2B48C, + b3_colorTeal = 0x008080, + b3_colorThistle = 0xD8BFD8, + b3_colorTomato = 0xFF6347, + b3_colorTurquoise = 0x40E0D0, + b3_colorViolet = 0xEE82EE, + b3_colorWheat = 0xF5DEB3, + b3_colorWhite = 0xFFFFFF, + b3_colorWhiteSmoke = 0xF5F5F5, + b3_colorYellow = 0xFFFF00, + b3_colorYellowGreen = 0x9ACD32, + + b3_colorBox2DRed = 0xDC3132, + b3_colorBox2DBlue = 0x30AEBF, + b3_colorBox2DGreen = 0x8CC924, + b3_colorBox2DYellow = 0xFFEE8C +} b3HexColor; + +/// Get the visualization color assigned to a constraint graph color slot. The last index +/// (B3_GRAPH_COLOR_COUNT - 1) is the overflow color. +B3_API b3HexColor b3GetGraphColor( int index ); + +// This is sent to the user for debug shape creation. The user should know the type in case they have +// custom sphere or capsule rendering. +typedef struct b3DebugShape +{ + b3ShapeId shapeId; + b3ShapeType type; + union + { + const b3Capsule* capsule; + const b3Compound* compound; + const b3HeightField* heightField; + const b3Hull* hull; + const b3Mesh* mesh; + const b3Sphere* sphere; + }; +} b3DebugShape; + +typedef struct b3DebugDraw +{ + // Draws a shape and returns true if drawing should continue + bool ( *DrawShapeFcn )( void* userShape, b3Transform transform, b3HexColor color, void* context ); + + /// Draw a line segment. + void ( *DrawSegmentFcn )( b3Vec3 p1, b3Vec3 p2, b3HexColor color, void* context ); + + /// Draw a transform. Choose your own length scale. + void ( *DrawTransformFcn )( b3Transform transform, void* context ); + + /// Draw a point. + void ( *DrawPointFcn )( b3Vec3 p, float size, b3HexColor color, void* context ); + + /// Draw a bounding box. + void ( *DrawBoundsFcn )( b3AABB aabb, b3HexColor color, void* context ); + + /// Draw a bounding box. + void ( *DrawBoxFcn )( b3Vec3 extents, b3Transform transform, b3HexColor color, void* context ); + + /// Draw a string in world space + void ( *DrawStringFcn )( b3Vec3 p, const char* s, b3HexColor color, void* context ); + + /// World bounds to use for debug draw + b3AABB drawingBounds; + + /// Scale to use when drawing forces + float forceScale; + + /// Global scaling for joint drawing + float jointScale; + + /// Option to draw shapes + bool drawShapes; + + /// Option to draw joints + bool drawJoints; + + /// Option to draw additional information for joints + bool drawJointExtras; + + /// Option to draw the bounding boxes for shapes + bool drawBounds; + + /// Option to draw the mass and center of mass of dynamic bodies + bool drawMass; + + /// Option to draw body names + bool drawBodyNames; + + /// Option to draw contact points + bool drawContacts; + + /// Draw contact anchor A or B + int drawAnchorA; + + /// Option to visualize the graph coloring used for contacts and joints + bool drawGraphColors; + + /// Option to draw contact features + bool drawContactFeatures; + + /// Option to draw contact normals + bool drawContactNormals; + + /// Option to draw contact normal forces + bool drawContactForces; + + /// Option to draw contact friction forces + bool drawFrictionForces; + + /// Option to draw islands as bounding boxes + bool drawIslands; + + /// User context that is passed as an argument to drawing callback functions + void* context; +} b3DebugDraw; + +B3_API b3DebugDraw b3DefaultDebugDraw( void ); + +/**@}*/ diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt new file mode 100644 index 0000000..05f97d2 --- /dev/null +++ b/samples/CMakeLists.txt @@ -0,0 +1,149 @@ +# Samples app + +option(BUILD_SHARED_LIBS "Build using shared libraries" OFF) +# set(FETCHCONTENT_QUIET FALSE) + +# glad for OpenGL API +set(GLAD_DIR ${CMAKE_SOURCE_DIR}/extern/glad) + +add_library( + glad STATIC + ${GLAD_DIR}/src/glad.c + ${GLAD_DIR}/include/glad/glad.h + ${GLAD_DIR}/include/KHR/khrplatform.h +) +target_include_directories(glad PUBLIC ${GLAD_DIR}/include) + +# glfw for windowing and input +SET(GLFW_BUILD_DOCS OFF CACHE BOOL "GLFW Docs") +SET(GLFW_BUILD_EXAMPLES OFF CACHE BOOL "GLFW Examples") +SET(GLFW_BUILD_TESTS OFF CACHE BOOL "GLFW Tests") +SET(GLFW_INSTALL OFF CACHE BOOL "GLFW Install") + +FetchContent_Declare( + glfw + GIT_REPOSITORY https://github.com/glfw/glfw.git + GIT_TAG master + GIT_SHALLOW TRUE + GIT_PROGRESS TRUE +) +FetchContent_MakeAvailable(glfw) + +# imgui and glfw backend for GUI +# https://gist.github.com/jeffamstutz/992723dfabac4e3ffff265eb71a24cd9 +# Modified to pin to a specific imgui release +FetchContent_Populate(imgui + URL https://github.com/ocornut/imgui/archive/refs/tags/v1.92.5.zip + SOURCE_DIR ${CMAKE_SOURCE_DIR}/build/imgui +) + +set(IMGUI_DIR ${CMAKE_SOURCE_DIR}/build/imgui) + +add_library(imgui STATIC + ${IMGUI_DIR}/imconfig.h + ${IMGUI_DIR}/imgui.h + ${IMGUI_DIR}/imstb_truetype.h + + ${IMGUI_DIR}/imgui.cpp + ${IMGUI_DIR}/imgui_draw.cpp + ${IMGUI_DIR}/imgui_demo.cpp + ${IMGUI_DIR}/imgui_tables.cpp + ${IMGUI_DIR}/imgui_widgets.cpp + + ${IMGUI_DIR}/backends/imgui_impl_glfw.cpp + ${IMGUI_DIR}/backends/imgui_impl_opengl3.cpp +) + +target_link_libraries(imgui PUBLIC glfw glad) +target_include_directories(imgui PUBLIC ${IMGUI_DIR} ${IMGUI_DIR}/backends) +# The sample app also uses stb_truetype and this keeps the symbols separate +target_compile_definitions(imgui PRIVATE IMGUI_STB_NAMESPACE=imgui_stb) +set_target_properties(imgui PROPERTIES + CXX_STANDARD 20 + CXX_STANDARD_REQUIRED YES + CXX_EXTENSIONS NO +) + +# implot for drawing real time plots +FetchContent_Declare( + implot + GIT_REPOSITORY https://github.com/epezent/implot.git + GIT_TAG v0.17 + GIT_SHALLOW TRUE + GIT_PROGRESS TRUE +) +FetchContent_MakeAvailable(implot) + +add_library(implot STATIC + ${implot_SOURCE_DIR}/implot.cpp + ${implot_SOURCE_DIR}/implot_items.cpp + ${implot_SOURCE_DIR}/implot_demo.cpp + ${implot_SOURCE_DIR}/implot.h + ${implot_SOURCE_DIR}/implot_internal.h +) +target_link_libraries(implot PUBLIC imgui) +target_include_directories(implot PUBLIC ${implot_SOURCE_DIR}) + +set (BOX3D_SAMPLE_FILES + camera.cpp + camera.h + earcut.h + font.cpp + font.h + geo_buffer.cpp + geo_buffer.h + jsmn.h + main.cpp + mesh_loader.cpp + mesh_loader.h + renderer.cpp + renderer.h + sample.cpp + sample.h + scene.cpp + scene.h + shader.cpp + shader.h + ssao_buffer.cpp + ssao_buffer.h + stb_image_write.h + stb_truetype.h + tiny_obj_loader.h + utility.h + + sample_benchmark.cpp + sample_bodies.cpp + sample_character.cpp + sample_collision.cpp + sample_compound.cpp + sample_continuous.cpp + sample_determinism.cpp + sample_events.cpp + sample_geometry.cpp + sample_issues.cpp + sample_joint.cpp + sample_manifold.cpp + sample_mesh.cpp + sample_ragdoll.cpp + sample_robustness.cpp + sample_shapes.cpp + sample_stacking.cpp + sample_tree.cpp +) + +add_executable(samples ${BOX3D_SAMPLE_FILES}) +target_include_directories(samples PRIVATE ${CMAKE_SOURCE_DIR}/data) +target_include_directories(samples PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(samples PRIVATE box3d shared glfw imgui implot glad) + +set_target_properties(samples PROPERTIES + CMAKE_COMPILE_WARNING_AS_ERROR ON + CXX_STANDARD 20 + CXX_STANDARD_REQUIRED YES + CXX_EXTENSIONS NO +) + +source_group(TREE ${CMAKE_CURRENT_SOURCE_DIR} FILES ${BOX3D_SAMPLE_FILES}) + +message(STATUS "runtime = ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}") +message(STATUS "binary = ${CMAKE_CURRENT_BINARY_DIR}") diff --git a/samples/camera.cpp b/samples/camera.cpp new file mode 100644 index 0000000..b0f4f19 --- /dev/null +++ b/samples/camera.cpp @@ -0,0 +1,330 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "camera.h" + +#include +// prevent clang format sorting glad.h with glfw3.h +#include + +// ImGui +#include +#include + +Matrix4 MakeOrthographicMatrix( float left, float right, float bottom, float top, float near, float far) +{ + Matrix4 m; + m.cx = Vector4{ 2.0f / (right - left), 0.0f, 0.0f, 0.0f }; + m.cy = Vector4{ 0.0f, 2.0f / (top - bottom), 0.0f, 0.0f }; + m.cz = Vector4{ 0.0f, 0.0f, -2.0f / (far - near), 0.0f }; + m.cw.x = -(right + left) / (right - left); + m.cw.y = -(top + bottom) / (top - bottom); + m.cw.z = -(far + near) / (far - near); + m.cw.w = 1.0f; + return m; +} + +Camera::Camera() +{ + m_yaw = 0.0f; + m_pitch = 0.0f; + m_radius = 1.0f; + m_speed = 20.0f; + m_pivot = b3Vec3_zero; + m_nearPlane = 0.02f; + m_farPlane = 100000.0f; + m_fovY = 50.0f; + m_tanY = tanf( 0.5f * B3_DEG_TO_RAD * m_fovY ); + m_thirdPerson = false; + + m_bufferWidth = 1920; + m_bufferHeight = 1080; + + Resize(1920, 1080); + UpdateTransform(); +} + +void Camera::Resize( int width, int height ) +{ + m_width = b3MaxInt(width, 1 ); + m_height = b3MaxInt(height, 1 ); + m_ratio = float( m_width ) / float( m_height ); + + m_nearHeight = 2.0f * m_nearPlane * m_tanY; + m_nearWidth = m_ratio * m_nearHeight; + + m_farHeight = m_farPlane * ( m_nearHeight / m_nearPlane ); + m_farWidth = m_farPlane * ( m_nearWidth / m_nearPlane ); +} + +b3Vec3 Camera::GetRight() const +{ + return m_right; +} + +b3Vec3 Camera::GetUp() const +{ + return m_up; +} + +b3Vec3 Camera::GetForward() const +{ + return m_forward; +} + +b3Vec3 Camera::GetPosition() const +{ + return m_position; +} + +void Camera::Frame( b3AABB bounds) +{ + b3Vec3 center = b3AABB_Center(bounds); + b3Vec3 extent = b3AABB_Extents(bounds); + + float height = 2.0f * b3MaxFloat(extent.x, b3MaxFloat(extent.y, extent.z ) ); + float radius = 0.5f * height / tanf( 0.5f * B3_DEG_TO_RAD * m_fovY ); + + m_pivot = center; + m_radius = 1.25f * radius; +} + +void Camera::SetView( float yaw, float pitch, float radius, b3Vec3 pivot) +{ + m_yaw = yaw; + m_pitch = pitch; + m_radius = radius; + m_pivot = pivot; + UpdateTransform(); +} + +Matrix4 Camera::GetWorldMatrix() const +{ + Matrix4 m; + m.cx = { m_right.x, m_right.y, m_right.z, 0.0f }; + m.cy = { m_up.x, m_up.y, m_up.z, 0.0f }; + m.cz = { m_forward.x, m_forward.y, m_forward.z, 0.0f }; + m.cw = { m_position.x, m_position.y, m_position.z, 1.0f }; + return m; +} + +static Matrix4 InvertOrtho( const Matrix4& m) +{ + // [ R | t ] + // M = [ --+-- ] + // [ 0 | 1 ] + // + // y = M * x -> y = R * x + t -> x = R^-1 * (y - t) -> x = R^-1 * y - R^-1 * t + // + // [ R | t ]-1 [ R^-1 | -R^-1 * t ] + // M^-1 = [ --+-- ] = [ -----+---------- ] + // [ 0 | 1 ] [ 0 | 1 ] + b3Matrix3 r; + r.cx = {m.cx.x, m.cx.y, m.cx.z }; + r.cy = {m.cy.x, m.cy.y, m.cy.z }; + r.cz = {m.cz.x, m.cz.y, m.cz.z }; + b3Matrix3 invR = b3InvertMatrix(r); + + b3Vec3 t{m.cw.x, m.cw.y, m.cw.z }; + b3Vec3 invRXT = invR * t; + + Matrix4 out; + out.cx = {invR.cx.x, invR.cx.y, invR.cx.z, 0.0f }; + out.cy = {invR.cy.x, invR.cy.y, invR.cy.z, 0.0f }; + out.cz = {invR.cz.x, invR.cz.y, invR.cz.z, 0.0f }; + out.cw = { -invRXT.x, -invRXT.y, -invRXT.z, 1.0f }; + + return out; +} + +Matrix4 Camera::GetViewMatrix() const +{ + Matrix4 v = GetWorldMatrix(); + v = InvertOrtho(v); + return v; +} + +// http://www.songho.ca/opengl/gl_projectionmatrix.html +// https://unspecified.wordpress.com/2012/06/21/calculating-the-gluperspective-matrix-and-other-opengl-matrix-maths/ +// https://software.intel.com/en-us/articles/alternatives-to-using-z-bias-to-fix-z-fighting-issues +static Matrix4 Perspective( float fieldOfViewY, float aspectRatio, float nearPlane, float farPlane) +{ + float cot = 1.0f / tanf( 0.5f * fieldOfViewY); + float depth = farPlane - nearPlane; + + Matrix4 out; + out.cx = {cot / aspectRatio, 0.0f, 0.0f, 0.0f }; + out.cy = { 0.0f, cot, 0.0f, 0.0f }; + out.cz = { 0.0f, 0.0f, -(nearPlane + farPlane) / depth, -1.0f }; + out.cw = { 0.0f, 0.0f, -2.0f * nearPlane * farPlane / depth, 0.0f }; + return out; +} + +Matrix4 Camera::GetProjectionMatrix() const +{ + return Perspective( B3_DEG_TO_RAD * m_fovY, m_ratio, m_nearPlane, m_farPlane ); +} + +PickRay Camera::BuildPickRay( float x, float y) const +{ + // Use similar triangles to build pick ray + + // Scale, flip, shift into frustum coordinates + float xRatio = x / m_width - 0.5f; + float yRatio = 0.5f - y / m_height; + + // Near plane coordinates using ratio + float xNear = xRatio * m_nearWidth; + float yNear = yRatio * m_nearHeight; + + // Far plane coordinates using ratio + float xFar = xRatio * m_farWidth; + float yFar = yRatio * m_farHeight; + + // Coordinates of near and far plane points in camera space + Vector4 nearPoint{xNear, yNear, -m_nearPlane, 1.0f }; + Vector4 farPoint{xFar, yFar, -m_farPlane, 1.0f }; + + // Convert to world space + Matrix4 m = GetWorldMatrix(); + nearPoint = MulMV(m, nearPoint); + farPoint = MulMV(m, farPoint); + + PickRay ray; + ray.origin = {nearPoint.x, nearPoint.y, nearPoint.z }; + ray.translation = b3Vec3{farPoint.x, farPoint.y, farPoint.z } - ray.origin; + return ray; +} + +bool Camera::WorldToScreen( float& xScreen, float& yScreen, b3Vec3 worldPoint) const +{ + Matrix4 view = GetViewMatrix(); + Vector4 cameraPoint = MulMV(view, Vector4{ worldPoint.x, worldPoint.y, worldPoint.z, 1.0f }); + + float x = cameraPoint.x; + float y = cameraPoint.y; + float z = cameraPoint.z; + + // Frustum comes out the back of the camera + if (z > -m_nearPlane || z < -m_farPlane ) + { + return false; + } + + // Find the frustum size at depth Z + float heightAtZ = -2.0f * z * m_tanY; + float widthAtZ = m_ratio * heightAtZ; + + // Compute ratios of the point within the frustum rectangle at Z + float xRatio = x / widthAtZ; + float yRatio = y / heightAtZ; + + // Shift, flip, and scale + xScreen = (xRatio + 0.5f ) * m_width; + yScreen = ( 0.5f - yRatio) * m_height; + + return true; +} + +void Camera::Update( GLFWwindow* window, float elapsedTime) +{ + ImGuiIO& io = ImGui::GetIO(); + + if ( m_thirdPerson ) + { + const float sensitivity = 1.0f; + m_radius -= sensitivity * io.MouseWheel; + m_radius = b3MaxFloat( 0.25f, m_radius ); + return; + } + + if (io.KeyAlt ) + { + // Orbit + if ( ImGui::IsMouseDragging( ImGuiMouseButton_Left ) ) + { + const float sensitivity = 0.2f; + m_yaw -= sensitivity * io.MouseDelta.x; + m_pitch += sensitivity * io.MouseDelta.y; + m_pitch = b3ClampFloat( m_pitch, -85.0f, 85.0f ); + } + + if ( ImGui::IsMouseDragging( ImGuiMouseButton_Middle ) ) + { + const float sensitivity = 0.01f; + m_pivot -= sensitivity * io.MouseDelta.x * m_right; + m_pivot += sensitivity * io.MouseDelta.y * m_up; + } + + if ( ImGui::IsMouseDragging( ImGuiMouseButton_Right ) ) + { + const float sensitivity = 0.02f; + m_radius -= sensitivity * io.MouseDelta.y; + m_radius = b3MaxFloat( 0.001f, m_radius ); + } + + const float sensitivity = 1.0f; + m_radius -= sensitivity * io.MouseWheel; + m_radius = b3MaxFloat( 0.001f, m_radius ); + + UpdateTransform(); + } + else if ( ImGui::IsMouseDown( ImGuiMouseButton_Right ) ) + { + m_speed += io.MouseWheel; + m_speed = b3ClampFloat( m_speed, 0.001f * 60.0f, 500.0f * 60.0f ); + + if ( ImGui::IsMouseDragging( ImGuiMouseButton_Right ) ) + { + const float sensitivity = 0.2f; + m_yaw -= sensitivity * io.MouseDelta.x; + m_pitch += sensitivity * io.MouseDelta.y; + m_pitch = b3ClampFloat( m_pitch, -85.0f, 85.0f ); + + float sinYaw = sinf( B3_DEG_TO_RAD * m_yaw ), cosYaw = cosf( B3_DEG_TO_RAD * m_yaw ); + float sinPitch = sinf( B3_DEG_TO_RAD * m_pitch ), cosPitch = cosf( B3_DEG_TO_RAD * m_pitch ); + + m_forward = {sinYaw * cosPitch, sinPitch, cosYaw * cosPitch}; + m_up = { 0.0f, 1.0f, 0.0f }; + m_up = b3Normalize( m_up - b3Dot( m_forward, m_up ) * m_forward ); + m_right = b3Normalize( b3Cross( m_up, m_forward ) ); + } + + // Smoother with GLFW + if ( glfwGetKey(window, GLFW_KEY_W ) ) + { + m_position -= m_speed * elapsedTime * m_forward; + } + + if ( glfwGetKey(window, GLFW_KEY_S ) ) + { + m_position += m_speed * elapsedTime * m_forward; + } + + if ( glfwGetKey(window, GLFW_KEY_A ) ) + { + m_position -= m_speed * elapsedTime * m_right; + } + + if ( glfwGetKey(window, GLFW_KEY_D ) ) + { + m_position += m_speed * elapsedTime * m_right; + } + + m_pivot = m_position - m_radius * m_forward; + } +} + +void Camera::UpdateTransform() +{ + m_yaw = B3_RAD_TO_DEG * b3UnwindAngle( B3_DEG_TO_RAD * m_yaw ); + float sinYaw = sinf( B3_DEG_TO_RAD * m_yaw ), cosYaw = cosf( B3_DEG_TO_RAD * m_yaw ); + float sinPitch = sinf( B3_DEG_TO_RAD * m_pitch ), cosPitch = cosf( B3_DEG_TO_RAD * m_pitch ); + + m_forward = {sinYaw * cosPitch, sinPitch, cosYaw * cosPitch}; + m_up = { 0.0f, 1.0f, 0.0f }; + m_up = b3Normalize( m_up - b3Dot( m_forward, m_up ) * m_forward ); + m_right = b3Normalize( b3Cross( m_up, m_forward ) ); + + m_position = m_pivot + m_radius * m_forward; +} diff --git a/samples/camera.h b/samples/camera.h new file mode 100644 index 0000000..21a3d5b --- /dev/null +++ b/samples/camera.h @@ -0,0 +1,85 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "utility.h" + +#include "box3d/types.h" + +struct GLFWwindow; + +struct PickRay +{ + b3Vec3 origin; + b3Vec3 translation; +}; + +struct Camera +{ + Camera(); + + // Window + void Resize( int width, int height ); + + // View + b3Vec3 GetRight() const; + b3Vec3 GetUp() const; + b3Vec3 GetForward() const; + b3Vec3 GetPosition() const; + + void Frame( b3AABB bounds); + void SetView( float yaw, float pitch, float radius, b3Vec3 pivot = b3Vec3_zero ); + + Matrix4 GetWorldMatrix() const; + Matrix4 GetViewMatrix() const; + + // Projection + Matrix4 GetProjectionMatrix() const; + PickRay BuildPickRay( float x, float y) const; + bool WorldToScreen( float& xScreen, float& yScreen, b3Vec3 worldPoint) const; + + // Control + void Update( GLFWwindow* window, float elapsedTime); + + void UpdateTransform(); + + // Window + int m_width; + int m_height; + + // Frame buffer + int m_bufferWidth; + int m_bufferHeight; + + // View yaw in degrees + float m_yaw; + + // View pitch in degrees + float m_pitch; + + float m_radius; + b3Vec3 m_pivot; + float m_speed; + + // Projection + float m_fovY; + float m_tanY; + float m_ratio; + float m_nearPlane; + float m_farPlane; + + float m_nearWidth; + float m_nearHeight; + + float m_farWidth; + float m_farHeight; + + // World coordinates + b3Vec3 m_right; + b3Vec3 m_up; + b3Vec3 m_forward; + b3Vec3 m_position; + + bool m_thirdPerson; +}; diff --git a/samples/earcut.h b/samples/earcut.h new file mode 100644 index 0000000..01bd7e9 --- /dev/null +++ b/samples/earcut.h @@ -0,0 +1,820 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace mapbox { + +namespace util { + +template struct nth { + inline static typename std::tuple_element::type + get(const T& t) { return std::get(t); }; +}; + +} + +namespace detail { + +template +class Earcut { +public: + std::vector indices; + std::size_t vertices = 0; + + template + void operator()(const Polygon& points); + +private: + struct Node { + Node(N index, double x_, double y_) : i(index), x(x_), y(y_) {} + Node(const Node&) = delete; + Node& operator=(const Node&) = delete; + Node(Node&&) = delete; + Node& operator=(Node&&) = delete; + + const N i; + const double x; + const double y; + + // previous and next vertice nodes in a polygon ring + Node* prev = nullptr; + Node* next = nullptr; + + // z-order curve value + int32_t z = 0; + + // previous and next nodes in z-order + Node* prevZ = nullptr; + Node* nextZ = nullptr; + + // indicates whether this is a steiner point + bool steiner = false; + }; + + template Node* linkedList(const Ring& points, const bool clockwise); + Node* filterPoints(Node* start, Node* end = nullptr); + void earcutLinked(Node* ear, int pass = 0); + bool isEar(Node* ear); + bool isEarHashed(Node* ear); + Node* cureLocalIntersections(Node* start); + void splitEarcut(Node* start); + template Node* eliminateHoles(const Polygon& points, Node* outerNode); + Node* eliminateHole(Node* hole, Node* outerNode); + Node* findHoleBridge(Node* hole, Node* outerNode); + bool sectorContainsSector(const Node* m, const Node* p); + void indexCurve(Node* start); + Node* sortLinked(Node* list); + int32_t zOrder(const double x_, const double y_); + Node* getLeftmost(Node* start); + bool pointInTriangle(double ax, double ay, double bx, double by, double cx, double cy, double px, double py) const; + bool isValidDiagonal(Node* a, Node* b); + double area(const Node* p, const Node* q, const Node* r) const; + bool equals(const Node* p1, const Node* p2); + bool intersects(const Node* p1, const Node* q1, const Node* p2, const Node* q2); + bool onSegment(const Node* p, const Node* q, const Node* r); + int sign(double val); + bool intersectsPolygon(const Node* a, const Node* b); + bool locallyInside(const Node* a, const Node* b); + bool middleInside(const Node* a, const Node* b); + Node* splitPolygon(Node* a, Node* b); + template Node* insertNode(std::size_t i, const Point& p, Node* last); + void removeNode(Node* p); + + bool hashing; + double minX, maxX; + double minY, maxY; + double inv_size = 0; + + template > + class ObjectPool { + public: + ObjectPool() { } + ObjectPool(std::size_t blockSize_) { + reset(blockSize_); + } + ~ObjectPool() { + clear(); + } + template + T* construct(Args&&... args) { + if (currentIndex >= blockSize) { + currentBlock = alloc_traits::allocate(alloc, blockSize); + allocations.emplace_back(currentBlock); + currentIndex = 0; + } + T* object = ¤tBlock[currentIndex++]; + alloc_traits::construct(alloc, object, std::forward(args)...); + return object; + } + void reset(std::size_t newBlockSize) { + for (auto allocation : allocations) { + alloc_traits::deallocate(alloc, allocation, blockSize); + } + allocations.clear(); + blockSize = std::max(1, newBlockSize); + currentBlock = nullptr; + currentIndex = blockSize; + } + void clear() { reset(blockSize); } + private: + T* currentBlock = nullptr; + std::size_t currentIndex = 1; + std::size_t blockSize = 1; + std::vector allocations; + Alloc alloc; + typedef typename std::allocator_traits alloc_traits; + }; + ObjectPool nodes; +}; + +template template +void Earcut::operator()(const Polygon& points) { + // reset + indices.clear(); + vertices = 0; + + if (points.empty()) return; + + double x; + double y; + int threshold = 80; + std::size_t len = 0; + + for (size_t i = 0; threshold >= 0 && i < points.size(); i++) { + threshold -= static_cast(points[i].size()); + len += points[i].size(); + } + + //estimate size of nodes and indices + nodes.reset(len * 3 / 2); + indices.reserve(len + points[0].size()); + + Node* outerNode = linkedList(points[0], true); + if (!outerNode || outerNode->prev == outerNode->next) return; + + if (points.size() > 1) outerNode = eliminateHoles(points, outerNode); + + // if the shape is not too simple, we'll use z-order curve hash later; calculate polygon bbox + hashing = threshold < 0; + if (hashing) { + Node* p = outerNode->next; + minX = maxX = outerNode->x; + minY = maxY = outerNode->y; + do { + x = p->x; + y = p->y; + minX = std::min(minX, x); + minY = std::min(minY, y); + maxX = std::max(maxX, x); + maxY = std::max(maxY, y); + p = p->next; + } while (p != outerNode); + + // minX, minY and size are later used to transform coords into integers for z-order calculation + inv_size = std::max(maxX - minX, maxY - minY); + inv_size = inv_size != .0 ? (1. / inv_size) : .0; + } + + earcutLinked(outerNode); + + nodes.clear(); +} + +// create a circular doubly linked list from polygon points in the specified winding order +template template +typename Earcut::Node* +Earcut::linkedList(const Ring& points, const bool clockwise) { + using Point = typename Ring::value_type; + double sum = 0; + const std::size_t len = points.size(); + std::size_t i, j; + Node* last = nullptr; + + // calculate original winding order of a polygon ring + for (i = 0, j = len > 0 ? len - 1 : 0; i < len; j = i++) { + const auto& p1 = points[i]; + const auto& p2 = points[j]; + const double p20 = util::nth<0, Point>::get(p2); + const double p10 = util::nth<0, Point>::get(p1); + const double p11 = util::nth<1, Point>::get(p1); + const double p21 = util::nth<1, Point>::get(p2); + sum += (p20 - p10) * (p11 + p21); + } + + // link points into circular doubly-linked list in the specified winding order + if (clockwise == (sum > 0)) { + for (i = 0; i < len; i++) last = insertNode(vertices + i, points[i], last); + } else { + for (i = len; i-- > 0;) last = insertNode(vertices + i, points[i], last); + } + + if (last && equals(last, last->next)) { + removeNode(last); + last = last->next; + } + + vertices += len; + + return last; +} + +// eliminate colinear or duplicate points +template +typename Earcut::Node* +Earcut::filterPoints(Node* start, Node* end) { + if (!end) end = start; + + Node* p = start; + bool again; + do { + again = false; + + if (!p->steiner && (equals(p, p->next) || area(p->prev, p, p->next) == 0)) { + removeNode(p); + p = end = p->prev; + + if (p == p->next) break; + again = true; + + } else { + p = p->next; + } + } while (again || p != end); + + return end; +} + +// main ear slicing loop which triangulates a polygon (given as a linked list) +template +void Earcut::earcutLinked(Node* ear, int pass) { + if (!ear) return; + + // interlink polygon nodes in z-order + if (!pass && hashing) indexCurve(ear); + + Node* stop = ear; + Node* prev; + Node* next; + + int iterations = 0; + + // iterate through ears, slicing them one by one + while (ear->prev != ear->next) { + iterations++; + prev = ear->prev; + next = ear->next; + + if (hashing ? isEarHashed(ear) : isEar(ear)) { + // cut off the triangle + indices.emplace_back(prev->i); + indices.emplace_back(ear->i); + indices.emplace_back(next->i); + + removeNode(ear); + + // skipping the next vertice leads to less sliver triangles + ear = next->next; + stop = next->next; + + continue; + } + + ear = next; + + // if we looped through the whole remaining polygon and can't find any more ears + if (ear == stop) { + // try filtering points and slicing again + if (!pass) earcutLinked(filterPoints(ear), 1); + + // if this didn't work, try curing all small self-intersections locally + else if (pass == 1) { + ear = cureLocalIntersections(filterPoints(ear)); + earcutLinked(ear, 2); + + // as a last resort, try splitting the remaining polygon into two + } else if (pass == 2) splitEarcut(ear); + + break; + } + } +} + +// check whether a polygon node forms a valid ear with adjacent nodes +template +bool Earcut::isEar(Node* ear) { + const Node* a = ear->prev; + const Node* b = ear; + const Node* c = ear->next; + + if (area(a, b, c) >= 0) return false; // reflex, can't be an ear + + // now make sure we don't have other points inside the potential ear + Node* p = ear->next->next; + + while (p != ear->prev) { + if (pointInTriangle(a->x, a->y, b->x, b->y, c->x, c->y, p->x, p->y) && + area(p->prev, p, p->next) >= 0) return false; + p = p->next; + } + + return true; +} + +template +bool Earcut::isEarHashed(Node* ear) { + const Node* a = ear->prev; + const Node* b = ear; + const Node* c = ear->next; + + if (area(a, b, c) >= 0) return false; // reflex, can't be an ear + + // triangle bbox; min & max are calculated like this for speed + const double minTX = std::min(a->x, std::min(b->x, c->x)); + const double minTY = std::min(a->y, std::min(b->y, c->y)); + const double maxTX = std::max(a->x, std::max(b->x, c->x)); + const double maxTY = std::max(a->y, std::max(b->y, c->y)); + + // z-order range for the current triangle bbox; + const int32_t minZ = zOrder(minTX, minTY); + const int32_t maxZ = zOrder(maxTX, maxTY); + + // first look for points inside the triangle in increasing z-order + Node* p = ear->nextZ; + + while (p && p->z <= maxZ) { + if (p != ear->prev && p != ear->next && + pointInTriangle(a->x, a->y, b->x, b->y, c->x, c->y, p->x, p->y) && + area(p->prev, p, p->next) >= 0) return false; + p = p->nextZ; + } + + // then look for points in decreasing z-order + p = ear->prevZ; + + while (p && p->z >= minZ) { + if (p != ear->prev && p != ear->next && + pointInTriangle(a->x, a->y, b->x, b->y, c->x, c->y, p->x, p->y) && + area(p->prev, p, p->next) >= 0) return false; + p = p->prevZ; + } + + return true; +} + +// go through all polygon nodes and cure small local self-intersections +template +typename Earcut::Node* +Earcut::cureLocalIntersections(Node* start) { + Node* p = start; + do { + Node* a = p->prev; + Node* b = p->next->next; + + // a self-intersection where edge (v[i-1],v[i]) intersects (v[i+1],v[i+2]) + if (!equals(a, b) && intersects(a, p, p->next, b) && locallyInside(a, b) && locallyInside(b, a)) { + indices.emplace_back(a->i); + indices.emplace_back(p->i); + indices.emplace_back(b->i); + + // remove two nodes involved + removeNode(p); + removeNode(p->next); + + p = start = b; + } + p = p->next; + } while (p != start); + + return filterPoints(p); +} + +// try splitting polygon into two and triangulate them independently +template +void Earcut::splitEarcut(Node* start) { + // look for a valid diagonal that divides the polygon into two + Node* a = start; + do { + Node* b = a->next->next; + while (b != a->prev) { + if (a->i != b->i && isValidDiagonal(a, b)) { + // split the polygon in two by the diagonal + Node* c = splitPolygon(a, b); + + // filter colinear points around the cuts + a = filterPoints(a, a->next); + c = filterPoints(c, c->next); + + // run earcut on each half + earcutLinked(a); + earcutLinked(c); + return; + } + b = b->next; + } + a = a->next; + } while (a != start); +} + +// link every hole into the outer loop, producing a single-ring polygon without holes +template template +typename Earcut::Node* +Earcut::eliminateHoles(const Polygon& points, Node* outerNode) { + const size_t len = points.size(); + + std::vector queue; + for (size_t i = 1; i < len; i++) { + Node* list = linkedList(points[i], false); + if (list) { + if (list == list->next) list->steiner = true; + queue.push_back(getLeftmost(list)); + } + } + std::sort(queue.begin(), queue.end(), [](const Node* a, const Node* b) { + return a->x < b->x; + }); + + // process holes from left to right + for (size_t i = 0; i < queue.size(); i++) { + outerNode = eliminateHole(queue[i], outerNode); + outerNode = filterPoints(outerNode, outerNode->next); + } + + return outerNode; +} + +// find a bridge between vertices that connects hole with an outer ring and and link it +template +typename Earcut::Node* +Earcut::eliminateHole(Node* hole, Node* outerNode) { + Node* bridge = findHoleBridge(hole, outerNode); + if (!bridge) { + return outerNode; + } + + Node* bridgeReverse = splitPolygon(bridge, hole); + + // filter collinear points around the cuts + Node* filteredBridge = filterPoints(bridge, bridge->next); + filterPoints(bridgeReverse, bridgeReverse->next); + + // Check if input node was removed by the filtering + return outerNode == bridge ? filteredBridge : outerNode; +} + +// David Eberly's algorithm for finding a bridge between hole and outer polygon +template +typename Earcut::Node* +Earcut::findHoleBridge(Node* hole, Node* outerNode) { + Node* p = outerNode; + double hx = hole->x; + double hy = hole->y; + double qx = -std::numeric_limits::infinity(); + Node* m = nullptr; + + // find a segment intersected by a ray from the hole's leftmost Vertex to the left; + // segment's endpoint with lesser x will be potential connection Vertex + do { + if (hy <= p->y && hy >= p->next->y && p->next->y != p->y) { + double x = p->x + (hy - p->y) * (p->next->x - p->x) / (p->next->y - p->y); + if (x <= hx && x > qx) { + qx = x; + if (x == hx) { + if (hy == p->y) return p; + if (hy == p->next->y) return p->next; + } + m = p->x < p->next->x ? p : p->next; + } + } + p = p->next; + } while (p != outerNode); + + if (!m) return 0; + + if (hx == qx) return m; // hole touches outer segment; pick leftmost endpoint + + // look for points inside the triangle of hole Vertex, segment intersection and endpoint; + // if there are no points found, we have a valid connection; + // otherwise choose the Vertex of the minimum angle with the ray as connection Vertex + + const Node* stop = m; + double tanMin = std::numeric_limits::infinity(); + double tanCur = 0; + + p = m; + double mx = m->x; + double my = m->y; + + do { + if (hx >= p->x && p->x >= mx && hx != p->x && + pointInTriangle(hy < my ? hx : qx, hy, mx, my, hy < my ? qx : hx, hy, p->x, p->y)) { + + tanCur = std::abs(hy - p->y) / (hx - p->x); // tangential + + if (locallyInside(p, hole) && + (tanCur < tanMin || (tanCur == tanMin && (p->x > m->x || sectorContainsSector(m, p))))) { + m = p; + tanMin = tanCur; + } + } + + p = p->next; + } while (p != stop); + + return m; +} + +// whether sector in vertex m contains sector in vertex p in the same coordinates +template +bool Earcut::sectorContainsSector(const Node* m, const Node* p) { + return area(m->prev, m, p->prev) < 0 && area(p->next, m, m->next) < 0; +} + +// interlink polygon nodes in z-order +template +void Earcut::indexCurve(Node* start) { + assert(start); + Node* p = start; + + do { + p->z = p->z ? p->z : zOrder(p->x, p->y); + p->prevZ = p->prev; + p->nextZ = p->next; + p = p->next; + } while (p != start); + + p->prevZ->nextZ = nullptr; + p->prevZ = nullptr; + + sortLinked(p); +} + +// Simon Tatham's linked list merge sort algorithm +// http://www.chiark.greenend.org.uk/~sgtatham/algorithms/listsort.html +template +typename Earcut::Node* +Earcut::sortLinked(Node* list) { + assert(list); + Node* p; + Node* q; + Node* e; + Node* tail; + int i, numMerges, pSize, qSize; + int inSize = 1; + + for (;;) { + p = list; + list = nullptr; + tail = nullptr; + numMerges = 0; + + while (p) { + numMerges++; + q = p; + pSize = 0; + for (i = 0; i < inSize; i++) { + pSize++; + q = q->nextZ; + if (!q) break; + } + + qSize = inSize; + + while (pSize > 0 || (qSize > 0 && q)) { + + if (pSize == 0) { + e = q; + q = q->nextZ; + qSize--; + } else if (qSize == 0 || !q) { + e = p; + p = p->nextZ; + pSize--; + } else if (p->z <= q->z) { + e = p; + p = p->nextZ; + pSize--; + } else { + e = q; + q = q->nextZ; + qSize--; + } + + if (tail) tail->nextZ = e; + else list = e; + + e->prevZ = tail; + tail = e; + } + + p = q; + } + + tail->nextZ = nullptr; + + if (numMerges <= 1) return list; + + inSize *= 2; + } +} + +// z-order of a Vertex given coords and size of the data bounding box +template +int32_t Earcut::zOrder(const double x_, const double y_) { + // coords are transformed into non-negative 15-bit integer range + int32_t x = static_cast(32767.0 * (x_ - minX) * inv_size); + int32_t y = static_cast(32767.0 * (y_ - minY) * inv_size); + + x = (x | (x << 8)) & 0x00FF00FF; + x = (x | (x << 4)) & 0x0F0F0F0F; + x = (x | (x << 2)) & 0x33333333; + x = (x | (x << 1)) & 0x55555555; + + y = (y | (y << 8)) & 0x00FF00FF; + y = (y | (y << 4)) & 0x0F0F0F0F; + y = (y | (y << 2)) & 0x33333333; + y = (y | (y << 1)) & 0x55555555; + + return x | (y << 1); +} + +// find the leftmost node of a polygon ring +template +typename Earcut::Node* +Earcut::getLeftmost(Node* start) { + Node* p = start; + Node* leftmost = start; + do { + if (p->x < leftmost->x || (p->x == leftmost->x && p->y < leftmost->y)) + leftmost = p; + p = p->next; + } while (p != start); + + return leftmost; +} + +// check if a point lies within a convex triangle +template +bool Earcut::pointInTriangle(double ax, double ay, double bx, double by, double cx, double cy, double px, double py) const { + return (cx - px) * (ay - py) - (ax - px) * (cy - py) >= 0 && + (ax - px) * (by - py) - (bx - px) * (ay - py) >= 0 && + (bx - px) * (cy - py) - (cx - px) * (by - py) >= 0; +} + +// check if a diagonal between two polygon nodes is valid (lies in polygon interior) +template +bool Earcut::isValidDiagonal(Node* a, Node* b) { + return a->next->i != b->i && a->prev->i != b->i && !intersectsPolygon(a, b) && // dones't intersect other edges + ((locallyInside(a, b) && locallyInside(b, a) && middleInside(a, b) && // locally visible + (area(a->prev, a, b->prev) != 0.0 || area(a, b->prev, b) != 0.0)) || // does not create opposite-facing sectors + (equals(a, b) && area(a->prev, a, a->next) > 0 && area(b->prev, b, b->next) > 0)); // special zero-length case +} + +// signed area of a triangle +template +double Earcut::area(const Node* p, const Node* q, const Node* r) const { + return (q->y - p->y) * (r->x - q->x) - (q->x - p->x) * (r->y - q->y); +} + +// check if two points are equal +template +bool Earcut::equals(const Node* p1, const Node* p2) { + return p1->x == p2->x && p1->y == p2->y; +} + +// check if two segments intersect +template +bool Earcut::intersects(const Node* p1, const Node* q1, const Node* p2, const Node* q2) { + int o1 = sign(area(p1, q1, p2)); + int o2 = sign(area(p1, q1, q2)); + int o3 = sign(area(p2, q2, p1)); + int o4 = sign(area(p2, q2, q1)); + + if (o1 != o2 && o3 != o4) return true; // general case + + if (o1 == 0 && onSegment(p1, p2, q1)) return true; // p1, q1 and p2 are collinear and p2 lies on p1q1 + if (o2 == 0 && onSegment(p1, q2, q1)) return true; // p1, q1 and q2 are collinear and q2 lies on p1q1 + if (o3 == 0 && onSegment(p2, p1, q2)) return true; // p2, q2 and p1 are collinear and p1 lies on p2q2 + if (o4 == 0 && onSegment(p2, q1, q2)) return true; // p2, q2 and q1 are collinear and q1 lies on p2q2 + + return false; +} + +// for collinear points p, q, r, check if point q lies on segment pr +template +bool Earcut::onSegment(const Node* p, const Node* q, const Node* r) { + return q->x <= std::max(p->x, r->x) && + q->x >= std::min(p->x, r->x) && + q->y <= std::max(p->y, r->y) && + q->y >= std::min(p->y, r->y); +} + +template +int Earcut::sign(double val) { + return (0.0 < val) - (val < 0.0); +} + +// check if a polygon diagonal intersects any polygon segments +template +bool Earcut::intersectsPolygon(const Node* a, const Node* b) { + const Node* p = a; + do { + if (p->i != a->i && p->next->i != a->i && p->i != b->i && p->next->i != b->i && + intersects(p, p->next, a, b)) return true; + p = p->next; + } while (p != a); + + return false; +} + +// check if a polygon diagonal is locally inside the polygon +template +bool Earcut::locallyInside(const Node* a, const Node* b) { + return area(a->prev, a, a->next) < 0 ? + area(a, b, a->next) >= 0 && area(a, a->prev, b) >= 0 : + area(a, b, a->prev) < 0 || area(a, a->next, b) < 0; +} + +// check if the middle Vertex of a polygon diagonal is inside the polygon +template +bool Earcut::middleInside(const Node* a, const Node* b) { + const Node* p = a; + bool inside = false; + double px = (a->x + b->x) / 2; + double py = (a->y + b->y) / 2; + do { + if (((p->y > py) != (p->next->y > py)) && p->next->y != p->y && + (px < (p->next->x - p->x) * (py - p->y) / (p->next->y - p->y) + p->x)) + inside = !inside; + p = p->next; + } while (p != a); + + return inside; +} + +// link two polygon vertices with a bridge; if the vertices belong to the same ring, it splits +// polygon into two; if one belongs to the outer ring and another to a hole, it merges it into a +// single ring +template +typename Earcut::Node* +Earcut::splitPolygon(Node* a, Node* b) { + Node* a2 = nodes.construct(a->i, a->x, a->y); + Node* b2 = nodes.construct(b->i, b->x, b->y); + Node* an = a->next; + Node* bp = b->prev; + + a->next = b; + b->prev = a; + + a2->next = an; + an->prev = a2; + + b2->next = a2; + a2->prev = b2; + + bp->next = b2; + b2->prev = bp; + + return b2; +} + +// create a node and util::optionally link it with previous one (in a circular doubly linked list) +template template +typename Earcut::Node* +Earcut::insertNode(std::size_t i, const Point& pt, Node* last) { + Node* p = nodes.construct(static_cast(i), util::nth<0, Point>::get(pt), util::nth<1, Point>::get(pt)); + + if (!last) { + p->prev = p; + p->next = p; + + } else { + assert(last); + p->next = last->next; + p->prev = last; + last->next->prev = p; + last->next = p; + } + return p; +} + +template +void Earcut::removeNode(Node* p) { + p->next->prev = p->prev; + p->prev->next = p->next; + + if (p->prevZ) p->prevZ->nextZ = p->nextZ; + if (p->nextZ) p->nextZ->prevZ = p->prevZ; +} +} + +template +std::vector earcut(const Polygon& poly) { + mapbox::detail::Earcut earcut; + earcut(poly); + return std::move(earcut.indices); +} +} diff --git a/samples/font.cpp b/samples/font.cpp new file mode 100644 index 0000000..f60d82c --- /dev/null +++ b/samples/font.cpp @@ -0,0 +1,276 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include "font.h" + +#include "camera.h" +#include "shader.h" +#include "utility.h" + +#define STBTT_STATIC +#define STB_TRUETYPE_IMPLEMENTATION +#include "stb_truetype.h" + +//#define STB_IMAGE_WRITE_IMPLEMENTATION +//#include "stb_image_write.h" + +#include "glad/glad.h" + +#include +#include + +static const char* vertexShaderSrc = R"( +#version 330 core + +layout (location = 0) in vec2 aPosition; +layout (location = 1) in vec2 aUV; +layout (location = 2) in vec4 aColor; + +out vec4 color; +out vec2 uv; + +uniform mat4 ProjectionMatrix; + +void main() +{ + gl_Position = ProjectionMatrix * vec4(aPosition, 0.0, 1.0); + + color = aColor; + uv = aUV; +} +)"; + +static const char* fragmentShaderSrc = R"( +#version 330 core + +in vec4 color; +in vec2 uv; + +uniform sampler2D FontAtlas; + +out vec4 fragColor; + +void main() +{ + fragColor = vec4(color.rgb, color.a * texture(FontAtlas, uv).r); +} +)"; + +Font::Font( const Camera* camera, float fontSize ) +{ + m_camera = camera; + m_fontSize = fontSize; + m_characters = (stbtt_bakedchar*)malloc( m_characterCount * sizeof( stbtt_bakedchar ) ); + m_textureId = 0; + m_vaoId = 0; + m_vboId = 0; + m_programId = 0; +} + +Font::~Font() +{ + if ( m_programId != 0 ) + { + DestroyShader( m_programId ); + } + + glDeleteBuffers( 1, &m_vboId ); + glDeleteVertexArrays( 1, &m_vaoId ); + + if ( m_textureId != 0 ) + { + glDeleteTextures( 1, &m_textureId ); + } + + free( m_characters ); +} + +void Font::AddText( float x, float y, b3HexColor color, const char* text ) +{ + if (text == nullptr) + { + return; + } + + b3Vec3 position = { x, y, 0.0f }; + RGBA8 c = MakeRGBA8( color ); + int pw = m_atlasWidth; + int ph = m_atlasHeight; + + int i = 0; + while ( text[i] != 0 ) + { + int index = (int)text[i] - m_firstCharacter; + + if ( 0 <= index && index < m_characterCount ) + { + // 1=opengl + stbtt_aligned_quad q; + stbtt_GetBakedQuad( m_characters, pw, ph, index, &position.x, &position.y, &q, 1 ); + + Vertex v1 = { { q.x0, q.y0 }, { q.s0, q.t0 }, c }; + Vertex v2 = { { q.x1, q.y0 }, { q.s1, q.t0 }, c }; + Vertex v3 = { { q.x1, q.y1 }, { q.s1, q.t1 }, c }; + Vertex v4 = { { q.x0, q.y1 }, { q.s0, q.t1 }, c }; + + m_vertices.push_back( v1 ); + m_vertices.push_back( v3 ); + m_vertices.push_back( v2 ); + m_vertices.push_back( v1 ); + m_vertices.push_back( v4 ); + m_vertices.push_back( v3 ); + } + + i += 1; + } +} + +void Font::Flush() +{ + Matrix4 projectionMatrix = MakeOrthographicMatrix( 0.0f, m_camera->m_width, m_camera->m_height, 0.0f, -1.0f, 1.0f ); + + glUseProgram( m_programId ); + + glEnable( GL_BLEND ); + glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA ); + glDisable( GL_DEPTH_TEST ); + + int slot = 0; + glActiveTexture( GL_TEXTURE0 + slot ); + glBindTexture( GL_TEXTURE_2D, m_textureId ); + + glBindVertexArray( m_vaoId ); + glBindBuffer( GL_ARRAY_BUFFER, m_vboId ); + + int textureUniform = glGetUniformLocation( m_programId, "FontAtlas" ); + glUniform1i( textureUniform, slot ); + + int matrixUniform = glGetUniformLocation( m_programId, "ProjectionMatrix" ); + glUniformMatrix4fv( matrixUniform, 1, GL_FALSE, &projectionMatrix.cx.x ); + + int totalVertexCount = int( m_vertices.size() ); + int drawCallCount = ( totalVertexCount / m_vboCapacity ) + 1; + + for ( int i = 0; i < drawCallCount; i++ ) + { + const Vertex* data = m_vertices.data() + i * m_vboCapacity; + + int vertexCount; + if ( i == drawCallCount - 1 ) + { + vertexCount = totalVertexCount % m_vboCapacity; + } + else + { + vertexCount = m_vboCapacity; + } + + // todo macOS optimization? + // glBufferData( GL_ARRAY_BUFFER, m_vboCapacity * sizeof( Vertex ), nullptr, GL_DYNAMIC_DRAW ); + glBufferSubData( GL_ARRAY_BUFFER, 0, vertexCount * sizeof( Vertex ), data ); + glDrawArrays( GL_TRIANGLES, 0, vertexCount ); + } + + glBindBuffer( GL_ARRAY_BUFFER, 0 ); + glBindVertexArray( 0 ); + glBindTexture( GL_TEXTURE_2D, 0 ); + + glDisable( GL_BLEND ); + glEnable( GL_DEPTH_TEST ); + + CheckOpenGL(); + + m_vertices.clear(); +} + +bool Font::Initialize( const char* path ) +{ + // Create atlas + FILE* file = fopen( path, "rb" ); + if ( file == nullptr ) + { + assert( false ); + return false; + } + + int fileBufferCapacity = 1 << 20; + unsigned char* fileBuffer = (unsigned char*)malloc( fileBufferCapacity * sizeof( unsigned char ) ); + fread( fileBuffer, 1, fileBufferCapacity, file ); + + int pw = m_atlasWidth; + int ph = m_atlasHeight; + unsigned char* tempBitmap = (unsigned char*)malloc( pw * ph * sizeof( unsigned char ) ); + stbtt_BakeFontBitmap( fileBuffer, 0, m_fontSize, tempBitmap, pw, ph, m_firstCharacter, m_characterCount, m_characters ); + + glGenTextures( 1, &m_textureId ); + glBindTexture( GL_TEXTURE_2D, m_textureId ); + glTexImage2D( GL_TEXTURE_2D, 0, GL_R8, pw, ph, 0, GL_RED, GL_UNSIGNED_BYTE, tempBitmap ); + glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR ); + + // stbi_write_png( "build/fontAtlas.png", pw, ph, 1, tempBitmap, pw ); + + fclose( file ); + free( fileBuffer ); + free( tempBitmap ); + fileBuffer = nullptr; + tempBitmap = nullptr; + + m_programId = CreateProgramFromStrings( vertexShaderSrc, fragmentShaderSrc ); + if ( m_programId == 0 ) + { + return false; + } + + // Setting up the VAO and VBO + glGenBuffers( 1, &m_vboId ); + glBindBuffer( GL_ARRAY_BUFFER, m_vboId ); + glBufferData( GL_ARRAY_BUFFER, m_vboCapacity * sizeof( Vertex ), nullptr, GL_DYNAMIC_DRAW ); + + glGenVertexArrays( 1, &m_vaoId ); + glBindVertexArray( m_vaoId ); + + // position attribute + glVertexAttribPointer( 0, 2, GL_FLOAT, GL_FALSE, sizeof( Vertex ), (void*)offsetof( Vertex, position ) ); + glEnableVertexAttribArray( 0 ); + + // uv attribute + glVertexAttribPointer( 1, 2, GL_FLOAT, GL_FALSE, sizeof( Vertex ), (void*)offsetof( Vertex, uv ) ); + glEnableVertexAttribArray( 1 ); + + // color attribute will be expanded to floats using normalization + glVertexAttribPointer( 2, 4, GL_UNSIGNED_BYTE, GL_TRUE, sizeof( Vertex ), (void*)offsetof( Vertex, color ) ); + glEnableVertexAttribArray( 2 ); + + glBindVertexArray( 0 ); + + CheckOpenGL(); + + return true; +} + +// For testing +// Matrix4 MakeProjectionMatrix( float width, float height ) +//{ +// Matrix4 m; +// m.cx = { 2.0f / width, 0.0f, 0.0f, 0.0f }; +// m.cy = { 0.0f, 2.0f / height, 0.0f, 0.0f }; +// m.cz = { 0.0f, 0.0f, -1.0f, 0.0f }; +// m.cw = { -1.0f, -1.0f, 0.0f, 1.0f }; +// return m; +//} + +Font* Font::Create( const Camera* camera, const char* path, int fontSize ) +{ + Font* font = new Font( camera, fontSize ); + if ( font->Initialize( path ) == false ) + { + delete font; + font = nullptr; + } + + return font; +} diff --git a/samples/font.h b/samples/font.h new file mode 100644 index 0000000..dd37ab9 --- /dev/null +++ b/samples/font.h @@ -0,0 +1,51 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box3d/types.h" +#include "utility.h" +#include "stb_truetype.h" + +#include + +struct Camera; + +struct Vertex +{ + b3Vec2 position; + b3Vec2 uv; + RGBA8 color; +}; + +class Font +{ +public: + static Font* Create( const Camera* camera, const char* path, int fontSize ); + ~Font(); + + void AddText( float x, float y, b3HexColor color, const char* text); + + void Flush( ); + +private: + Font( const Camera* camera, float fontSize ); + bool Initialize( const char* path ); + + static constexpr int m_firstCharacter = 32; + static constexpr int m_characterCount = 96; + static constexpr int m_atlasWidth = 512; + static constexpr int m_atlasHeight = 512; + + // The number of vertices the vbo can hold + static constexpr int m_vboCapacity = 6 * 10000; + + const Camera* m_camera; + float m_fontSize; + stbtt_bakedchar* m_characters; + std::vector m_vertices; + unsigned int m_textureId; + uint32_t m_vaoId; + uint32_t m_vboId; + uint32_t m_programId; +}; diff --git a/samples/geo_buffer.cpp b/samples/geo_buffer.cpp new file mode 100644 index 0000000..1e23dc2 --- /dev/null +++ b/samples/geo_buffer.cpp @@ -0,0 +1,150 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "geo_buffer.h" + +#include +#include +#include +#include + +struct GeoBuffer +{ + int32_t width, height; + uint32_t frameBufferId; + uint32_t positionId; + uint32_t normalId; + uint32_t albedoId; + uint32_t depthTextureId; +}; + +GeoBuffer* CreateGeoBuffer(int32_t width, int32_t height) +{ + GeoBuffer* buffer = new GeoBuffer; + + buffer->width = width; + buffer->height = height; + + glGenFramebuffers(1, &buffer->frameBufferId); + glBindFramebuffer(GL_FRAMEBUFFER, buffer->frameBufferId); + + // position color buffer + glGenTextures(1, &buffer->positionId); + glBindTexture(GL_TEXTURE_2D, buffer->positionId); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA16F, width, height, 0, GL_RGBA, GL_FLOAT, nullptr); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); + glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, buffer->positionId, 0); + + // normal color buffer + glGenTextures(1, &buffer->normalId); + glBindTexture(GL_TEXTURE_2D, buffer->normalId); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA16F, width, height, 0, GL_RGBA, GL_FLOAT, nullptr); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT1, GL_TEXTURE_2D, buffer->normalId, 0); + + // color + specular color buffer + glGenTextures(1, &buffer->albedoId); + glBindTexture(GL_TEXTURE_2D, buffer->albedoId); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, 0, GL_RGBA, GL_UNSIGNED_BYTE, nullptr); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT2, GL_TEXTURE_2D, buffer->albedoId, 0); + + // tell OpenGL which color attachments we'll use (of this framebuffer) for rendering + const unsigned int attachments[3] = {GL_COLOR_ATTACHMENT0, GL_COLOR_ATTACHMENT1, GL_COLOR_ATTACHMENT2}; + glDrawBuffers(3, attachments); + + // create and attach depth buffer as a texture + // Using a texture instead of a renderbuffer avoids a slow blit path on Apple Silicon + glGenTextures(1, &buffer->depthTextureId); + glBindTexture(GL_TEXTURE_2D, buffer->depthTextureId); + glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT24, width, height, 0, GL_DEPTH_COMPONENT, GL_FLOAT, nullptr); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + glFramebufferTexture2D(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, buffer->depthTextureId, 0); + + // finally check if framebuffer is complete + if (glCheckFramebufferStatus(GL_FRAMEBUFFER) != GL_FRAMEBUFFER_COMPLETE) + { + fprintf(stderr, "Failed to initialize glad\n"); + free(buffer); + return nullptr; + } + + glBindFramebuffer(GL_FRAMEBUFFER, 0); + return buffer; +} + +void BindGeoBuffer(GeoBuffer* buffer) +{ + glBindFramebuffer(GL_FRAMEBUFFER, buffer->frameBufferId); + + // Clear each attachment to zero independently of the global glClearColor, so the + // deferred lighting pass can identify sky pixels by gPosition.xyz == 0. + static const GLfloat zero[4] = { 0.0f, 0.0f, 0.0f, 0.0f }; + glClearBufferfv(GL_COLOR, 0, zero); + glClearBufferfv(GL_COLOR, 1, zero); + glClearBufferfv(GL_COLOR, 2, zero); + glClear(GL_DEPTH_BUFFER_BIT); +} + +#if 0 +void tbBindGeoPositionNormal(GeoBuffer* buffer) +{ + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D, buffer->positionId); + glActiveTexture(GL_TEXTURE1); + glBindTexture(GL_TEXTURE_2D, buffer->normalId); +} +#endif + +void BindGeoTextures(GeoBuffer* buffer) +{ + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D, buffer->positionId); + glActiveTexture(GL_TEXTURE1); + glBindTexture(GL_TEXTURE_2D, buffer->normalId); + glActiveTexture(GL_TEXTURE2); + glBindTexture(GL_TEXTURE_2D, buffer->albedoId); +} + +void BindGeoPosition(GeoBuffer* buffer, int textureUnit) +{ + glActiveTexture(GL_TEXTURE0 + textureUnit); + glBindTexture(GL_TEXTURE_2D, buffer->positionId); +} + +void BindGeoDepthTexture(GeoBuffer* buffer, int textureUnit) +{ + glActiveTexture(GL_TEXTURE0 + textureUnit); + glBindTexture(GL_TEXTURE_2D, buffer->depthTextureId); +} + +#if 0 +void tbResize(GeoBuffer* buffer, int32_t width, int32_t height) +{ + glBindTexture(GL_TEXTURE_2D, buffer->positionId); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA16F, width, height, 0, GL_RGBA, GL_FLOAT, nullptr); + + glBindTexture(GL_TEXTURE_2D, buffer->normalId); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA16F, width, height, 0, GL_RGBA, GL_FLOAT, nullptr); + + glBindTexture(GL_TEXTURE_2D, buffer->albedoId); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, 0, GL_RGBA, GL_UNSIGNED_BYTE, nullptr); +} +#endif + +void DestroyGeoBuffer(GeoBuffer* buffer) +{ + glDeleteFramebuffers(1, &buffer->frameBufferId); + glDeleteTextures(1, &buffer->positionId); + glDeleteTextures(1, &buffer->normalId); + glDeleteTextures(1, &buffer->albedoId); + glDeleteTextures(1, &buffer->depthTextureId); + + delete buffer; +} diff --git a/samples/geo_buffer.h b/samples/geo_buffer.h new file mode 100644 index 0000000..4bee59a --- /dev/null +++ b/samples/geo_buffer.h @@ -0,0 +1,17 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +struct GeoBuffer; + +GeoBuffer* CreateGeoBuffer(int32_t width, int32_t height); +void DestroyGeoBuffer(GeoBuffer* buffer); + +void BindGeoBuffer(GeoBuffer* buffer); +//void tbBindGeoPositionNormal(GeoBuffer* buffer); +void BindGeoTextures(GeoBuffer* buffer); +void BindGeoPosition(GeoBuffer* buffer, int textureUnit); +void BindGeoDepthTexture(GeoBuffer* buffer, int textureUnit); diff --git a/samples/imconfig.h b/samples/imconfig.h new file mode 100644 index 0000000..5841c82 --- /dev/null +++ b/samples/imconfig.h @@ -0,0 +1,156 @@ +//----------------------------------------------------------------------------- +// DEAR IMGUI COMPILE-TIME OPTIONS +// Runtime options (clipboard callbacks, enabling various features, etc.) can generally be set via the ImGuiIO structure. +// You can use ImGui::SetAllocatorFunctions() before calling ImGui::CreateContext() to rewire memory allocation functions. +//----------------------------------------------------------------------------- +// A) You may edit imconfig.h (and not overwrite it when updating Dear ImGui, or maintain a patch/rebased branch with your +// modifications to it) B) or '#define IMGUI_USER_CONFIG "my_imgui_config.h"' in your project and then add directives in your own +// file without touching this template. +//----------------------------------------------------------------------------- +// You need to make sure that configuration settings are defined consistently _everywhere_ Dear ImGui is used, which include the +// imgui*.cpp files but also _any_ of your code that uses Dear ImGui. This is because some compile-time options have an affect on +// data structures. Defining those options in imconfig.h will ensure every compilation unit gets to see the same data structure +// layouts. Call IMGUI_CHECKVERSION() from your .cpp file to verify that the data structures your files are using are matching the +// ones imgui.cpp is using. +//----------------------------------------------------------------------------- + +#pragma once + +//---- Define assertion handler. Defaults to calling assert(). +// If your macro uses multiple statements, make sure is enclosed in a 'do { .. } while (0)' block so it can be used as a single +// statement. +// #define IM_ASSERT(_EXPR) MyAssert(_EXPR) +// #define IM_ASSERT(_EXPR) ((void)(_EXPR)) // Disable asserts + +//---- Define attributes of all API symbols declarations, e.g. for DLL under Windows +// Using Dear ImGui via a shared library is not recommended, because of function call overhead and because we don't guarantee +// backward nor forward ABI compatibility. +// - Windows DLL users: heaps and globals are not shared across DLL boundaries! You will need to call SetCurrentContext() + +// SetAllocatorFunctions() +// for each static/DLL boundary you are calling from. Read "Context and Memory Allocators" section of imgui.cpp for more +// details. +// #define IMGUI_API __declspec(dllexport) // MSVC Windows: DLL export +// #define IMGUI_API __declspec(dllimport) // MSVC Windows: DLL import +// #define IMGUI_API __attribute__((visibility("default"))) // GCC/Clang: override visibility when set is hidden + +//---- Don't define obsolete functions/enums/behaviors. Consider enabling from time to time after updating to clean your code of +//obsolete function/names. #define IMGUI_DISABLE_OBSOLETE_FUNCTIONS #define IMGUI_DISABLE_OBSOLETE_KEYIO // 1.87+ disable legacy +// io.KeyMap[]+io.KeysDown[] in favor io.AddKeyEvent(). This is automatically done by IMGUI_DISABLE_OBSOLETE_FUNCTIONS. + +//---- Disable all of Dear ImGui or don't implement standard windows/tools. +// It is very strongly recommended to NOT disable the demo windows and debug tool during development. They are extremely useful in +// day to day work. Please read comments in imgui_demo.cpp. +// #define IMGUI_DISABLE // Disable everything: all headers and source files will be empty. +// #define IMGUI_DISABLE_DEMO_WINDOWS // Disable demo windows: ShowDemoWindow()/ShowStyleEditor() will be +// empty. #define IMGUI_DISABLE_DEBUG_TOOLS // Disable metrics/debugger and other debug tools: +// ShowMetricsWindow(), ShowDebugLogWindow() and ShowIDStackToolWindow() will be empty. + +//---- Don't implement some functions to reduce linkage requirements. +// #define IMGUI_DISABLE_WIN32_DEFAULT_CLIPBOARD_FUNCTIONS // [Win32] Don't implement default clipboard handler. Won't use and +// link with OpenClipboard/GetClipboardData/CloseClipboard etc. (user32.lib/.a, kernel32.lib/.a) #define +// IMGUI_ENABLE_WIN32_DEFAULT_IME_FUNCTIONS // [Win32] [Default with Visual Studio] Implement default IME handler +// (require imm32.lib/.a, auto-link for Visual Studio, -limm32 on command-line for MinGW) #define +// IMGUI_DISABLE_WIN32_DEFAULT_IME_FUNCTIONS // [Win32] [Default with non-Visual Studio compilers] Don't implement default +// IME handler (won't require imm32.lib/.a) #define IMGUI_DISABLE_WIN32_FUNCTIONS // [Win32] Won't use and +// link with any Win32 function (clipboard, IME). #define IMGUI_ENABLE_OSX_DEFAULT_CLIPBOARD_FUNCTIONS // [OSX] Implement +// default OSX clipboard handler (need to link with '-framework ApplicationServices', this is why this is not the default). +// #define IMGUI_DISABLE_DEFAULT_SHELL_FUNCTIONS // Don't implement default platform_io.Platform_OpenInShellFn() +// handler (Win32: ShellExecute(), require shell32.lib/.a, Mac/Linux: use system("")). #define +// IMGUI_DISABLE_DEFAULT_FORMAT_FUNCTIONS // Don't implement ImFormatString/ImFormatStringV so you can implement them +// yourself (e.g. if you don't want to link with vsnprintf) #define IMGUI_DISABLE_DEFAULT_MATH_FUNCTIONS // Don't +// implement ImFabs/ImSqrt/ImPow/ImFmod/ImCos/ImSin/ImAcos/ImAtan2 so you can implement them yourself. #define +// IMGUI_DISABLE_FILE_FUNCTIONS // Don't implement ImFileOpen/ImFileClose/ImFileRead/ImFileWrite and +// ImFileHandle at all (replace them with dummies) #define IMGUI_DISABLE_DEFAULT_FILE_FUNCTIONS // Don't implement +// ImFileOpen/ImFileClose/ImFileRead/ImFileWrite and ImFileHandle so you can implement them yourself if you don't want to link +// with fopen/fclose/fread/fwrite. This will also disable the LogToTTY() function. #define IMGUI_DISABLE_DEFAULT_ALLOCATORS // +// Don't implement default allocators calling malloc()/free() to avoid linking with them. You will need to call +// ImGui::SetAllocatorFunctions(). #define IMGUI_DISABLE_SSE // Disable use of SSE intrinsics even +// if available + +//---- Enable Test Engine / Automation features. +// #define IMGUI_ENABLE_TEST_ENGINE // Enable imgui_test_engine hooks. Generally set automatically by +// include "imgui_te_config.h", see Test Engine for details. + +//---- Include imgui_user.h at the end of imgui.h as a convenience +// May be convenient for some users to only explicitly include vanilla imgui.h and have extra stuff included. +// #define IMGUI_INCLUDE_IMGUI_USER_H +// #define IMGUI_USER_H_FILENAME "my_folder/my_imgui_user.h" + +//---- Pack colors to BGRA8 instead of RGBA8 (to avoid converting from one to another) +// #define IMGUI_USE_BGRA_PACKED_COLOR + +//---- Use 32-bit for ImWchar (default is 16-bit) to support Unicode planes 1-16. (e.g. point beyond 0xFFFF like emoticons, +//dingbats, symbols, shapes, ancient languages, etc...) #define IMGUI_USE_WCHAR32 + +//---- Avoid multiple STB libraries implementations, or redefine path/filenames to prioritize another version +// By default the embedded implementations are declared static and not available outside of Dear ImGui sources files. +// #define IMGUI_STB_TRUETYPE_FILENAME "my_folder/stb_truetype.h" +// #define IMGUI_STB_RECT_PACK_FILENAME "my_folder/stb_rect_pack.h" +// #define IMGUI_STB_SPRINTF_FILENAME "my_folder/stb_sprintf.h" // only used if IMGUI_USE_STB_SPRINTF is defined. +// #define IMGUI_DISABLE_STB_TRUETYPE_IMPLEMENTATION +// #define IMGUI_DISABLE_STB_RECT_PACK_IMPLEMENTATION +// #define IMGUI_DISABLE_STB_SPRINTF_IMPLEMENTATION // only disabled if IMGUI_USE_STB_SPRINTF is defined. + +//---- Use stb_sprintf.h for a faster implementation of vsnprintf instead of the one from libc (unless +//IMGUI_DISABLE_DEFAULT_FORMAT_FUNCTIONS is defined) +// Compatibility checks of arguments and formats done by clang and GCC will be disabled in order to support the extra formats +// provided by stb_sprintf.h. +// #define IMGUI_USE_STB_SPRINTF + +//---- Use FreeType to build and rasterize the font atlas (instead of stb_truetype which is embedded by default in Dear ImGui) +// Requires FreeType headers to be available in the include path. Requires program to be compiled with +// 'misc/freetype/imgui_freetype.cpp' (in this repository) + the FreeType library (not provided). On Windows you may use vcpkg +// with 'vcpkg install freetype --triplet=x64-windows' + 'vcpkg integrate install'. +#define IMGUI_ENABLE_FREETYPE + +//---- Use FreeType+lunasvg library to render OpenType SVG fonts (SVGinOT) +// Requires lunasvg headers to be available in the include path + program to be linked with the lunasvg library (not provided). +// Only works in combination with IMGUI_ENABLE_FREETYPE. +// (implementation is based on Freetype's rsvg-port.c which is licensed under CeCILL-C Free Software License Agreement) +// #define IMGUI_ENABLE_FREETYPE_LUNASVG + +//---- Use stb_truetype to build and rasterize the font atlas (default) +// The only purpose of this define is if you want force compilation of the stb_truetype backend ALONG with the FreeType backend. +// #define IMGUI_ENABLE_STB_TRUETYPE + +//---- Define constructor and implicit cast operators to convert back<>forth between your math types and ImVec2/ImVec4. +// This will be inlined as part of ImVec2 and ImVec4 class declarations. +/* +#define IM_VEC2_CLASS_EXTRA \ + constexpr ImVec2(const MyVec2& f) : x(f.x), y(f.y) {} \ + operator MyVec2() const { return MyVec2(x,y); } + +#define IM_VEC4_CLASS_EXTRA \ + constexpr ImVec4(const MyVec4& f) : x(f.x), y(f.y), z(f.z), w(f.w) {} \ + operator MyVec4() const { return MyVec4(x,y,z,w); } +*/ +//---- ...Or use Dear ImGui's own very basic math operators. +// #define IMGUI_DEFINE_MATH_OPERATORS + +//---- Use 32-bit vertex indices (default is 16-bit) is one way to allow large meshes with more than 64K vertices. +// Your renderer backend will need to support it (most example renderer backends support both 16/32-bit indices). +// Another way to allow large meshes while keeping 16-bit indices is to handle ImDrawCmd::VtxOffset in your renderer. +// Read about ImGuiBackendFlags_RendererHasVtxOffset for details. +// #define ImDrawIdx unsigned int + +//---- Override ImDrawCallback signature (will need to modify renderer backends accordingly) +// struct ImDrawList; +// struct ImDrawCmd; +// typedef void (*MyImDrawCallback)(const ImDrawList* draw_list, const ImDrawCmd* cmd, void* my_renderer_user_data); +// #define ImDrawCallback MyImDrawCallback + +//---- Debug Tools: Macro to break in Debugger (we provide a default implementation of this in the codebase) +// (use 'Metrics->Tools->Item Picker' to pick widgets with the mouse and break into them for easy debugging.) +// #define IM_DEBUG_BREAK IM_ASSERT(0) +// #define IM_DEBUG_BREAK __debugbreak() + +//---- Debug Tools: Enable slower asserts +// #define IMGUI_DEBUG_PARANOID + +//---- Tip: You can add extra functions within the ImGui:: namespace from anywhere (e.g. your own sources/header files) +/* +namespace ImGui +{ + void MyFunction(const char* name, MyMatrix44* mtx); +} +*/ diff --git a/samples/jsmn.h b/samples/jsmn.h new file mode 100644 index 0000000..8ac14c1 --- /dev/null +++ b/samples/jsmn.h @@ -0,0 +1,471 @@ +/* + * MIT License + * + * Copyright (c) 2010 Serge Zaitsev + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ +#ifndef JSMN_H +#define JSMN_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#ifdef JSMN_STATIC +#define JSMN_API static +#else +#define JSMN_API extern +#endif + +/** + * JSON type identifier. Basic types are: + * o Object + * o Array + * o String + * o Other primitive: number, boolean (true/false) or null + */ +typedef enum { + JSMN_UNDEFINED = 0, + JSMN_OBJECT = 1 << 0, + JSMN_ARRAY = 1 << 1, + JSMN_STRING = 1 << 2, + JSMN_PRIMITIVE = 1 << 3 +} jsmntype_t; + +enum jsmnerr { + /* Not enough tokens were provided */ + JSMN_ERROR_NOMEM = -1, + /* Invalid character inside JSON string */ + JSMN_ERROR_INVAL = -2, + /* The string is not a full JSON packet, more bytes expected */ + JSMN_ERROR_PART = -3 +}; + +/** + * JSON token description. + * type type (object, array, string etc.) + * start start position in JSON data string + * end end position in JSON data string + */ +typedef struct jsmntok { + jsmntype_t type; + int start; + int end; + int size; +#ifdef JSMN_PARENT_LINKS + int parent; +#endif +} jsmntok_t; + +/** + * JSON parser. Contains an array of token blocks available. Also stores + * the string being parsed now and current position in that string. + */ +typedef struct jsmn_parser { + unsigned int pos; /* offset in the JSON string */ + unsigned int toknext; /* next token to allocate */ + int toksuper; /* superior token node, e.g. parent object or array */ +} jsmn_parser; + +/** + * Create JSON parser over an array of tokens + */ +JSMN_API void jsmn_init(jsmn_parser *parser); + +/** + * Run JSON parser. It parses a JSON data string into and array of tokens, each + * describing + * a single JSON object. + */ +JSMN_API int jsmn_parse(jsmn_parser *parser, const char *js, const size_t len, + jsmntok_t *tokens, const unsigned int num_tokens); + +#ifndef JSMN_HEADER +/** + * Allocates a fresh unused token from the token pool. + */ +static jsmntok_t *jsmn_alloc_token(jsmn_parser *parser, jsmntok_t *tokens, + const size_t num_tokens) { + jsmntok_t *tok; + if (parser->toknext >= num_tokens) { + return NULL; + } + tok = &tokens[parser->toknext++]; + tok->start = tok->end = -1; + tok->size = 0; +#ifdef JSMN_PARENT_LINKS + tok->parent = -1; +#endif + return tok; +} + +/** + * Fills token type and boundaries. + */ +static void jsmn_fill_token(jsmntok_t *token, const jsmntype_t type, + const int start, const int end) { + token->type = type; + token->start = start; + token->end = end; + token->size = 0; +} + +/** + * Fills next available token with JSON primitive. + */ +static int jsmn_parse_primitive(jsmn_parser *parser, const char *js, + const size_t len, jsmntok_t *tokens, + const size_t num_tokens) { + jsmntok_t *token; + int start; + + start = parser->pos; + + for (; parser->pos < len && js[parser->pos] != '\0'; parser->pos++) { + switch (js[parser->pos]) { +#ifndef JSMN_STRICT + /* In strict mode primitive must be followed by "," or "}" or "]" */ + case ':': +#endif + case '\t': + case '\r': + case '\n': + case ' ': + case ',': + case ']': + case '}': + goto found; + default: + /* to quiet a warning from gcc*/ + break; + } + if (js[parser->pos] < 32 || js[parser->pos] >= 127) { + parser->pos = start; + return JSMN_ERROR_INVAL; + } + } +#ifdef JSMN_STRICT + /* In strict mode primitive must be followed by a comma/object/array */ + parser->pos = start; + return JSMN_ERROR_PART; +#endif + +found: + if (tokens == NULL) { + parser->pos--; + return 0; + } + token = jsmn_alloc_token(parser, tokens, num_tokens); + if (token == NULL) { + parser->pos = start; + return JSMN_ERROR_NOMEM; + } + jsmn_fill_token(token, JSMN_PRIMITIVE, start, parser->pos); +#ifdef JSMN_PARENT_LINKS + token->parent = parser->toksuper; +#endif + parser->pos--; + return 0; +} + +/** + * Fills next token with JSON string. + */ +static int jsmn_parse_string(jsmn_parser *parser, const char *js, + const size_t len, jsmntok_t *tokens, + const size_t num_tokens) { + jsmntok_t *token; + + int start = parser->pos; + + /* Skip starting quote */ + parser->pos++; + + for (; parser->pos < len && js[parser->pos] != '\0'; parser->pos++) { + char c = js[parser->pos]; + + /* Quote: end of string */ + if (c == '\"') { + if (tokens == NULL) { + return 0; + } + token = jsmn_alloc_token(parser, tokens, num_tokens); + if (token == NULL) { + parser->pos = start; + return JSMN_ERROR_NOMEM; + } + jsmn_fill_token(token, JSMN_STRING, start + 1, parser->pos); +#ifdef JSMN_PARENT_LINKS + token->parent = parser->toksuper; +#endif + return 0; + } + + /* Backslash: Quoted symbol expected */ + if (c == '\\' && parser->pos + 1 < len) { + int i; + parser->pos++; + switch (js[parser->pos]) { + /* Allowed escaped symbols */ + case '\"': + case '/': + case '\\': + case 'b': + case 'f': + case 'r': + case 'n': + case 't': + break; + /* Allows escaped symbol \uXXXX */ + case 'u': + parser->pos++; + for (i = 0; i < 4 && parser->pos < len && js[parser->pos] != '\0'; + i++) { + /* If it isn't a hex character we have an error */ + if (!((js[parser->pos] >= 48 && js[parser->pos] <= 57) || /* 0-9 */ + (js[parser->pos] >= 65 && js[parser->pos] <= 70) || /* A-F */ + (js[parser->pos] >= 97 && js[parser->pos] <= 102))) { /* a-f */ + parser->pos = start; + return JSMN_ERROR_INVAL; + } + parser->pos++; + } + parser->pos--; + break; + /* Unexpected symbol */ + default: + parser->pos = start; + return JSMN_ERROR_INVAL; + } + } + } + parser->pos = start; + return JSMN_ERROR_PART; +} + +/** + * Parse JSON string and fill tokens. + */ +JSMN_API int jsmn_parse(jsmn_parser *parser, const char *js, const size_t len, + jsmntok_t *tokens, const unsigned int num_tokens) { + int r; + int i; + jsmntok_t *token; + int count = parser->toknext; + + for (; parser->pos < len && js[parser->pos] != '\0'; parser->pos++) { + char c; + jsmntype_t type; + + c = js[parser->pos]; + switch (c) { + case '{': + case '[': + count++; + if (tokens == NULL) { + break; + } + token = jsmn_alloc_token(parser, tokens, num_tokens); + if (token == NULL) { + return JSMN_ERROR_NOMEM; + } + if (parser->toksuper != -1) { + jsmntok_t *t = &tokens[parser->toksuper]; +#ifdef JSMN_STRICT + /* In strict mode an object or array can't become a key */ + if (t->type == JSMN_OBJECT) { + return JSMN_ERROR_INVAL; + } +#endif + t->size++; +#ifdef JSMN_PARENT_LINKS + token->parent = parser->toksuper; +#endif + } + token->type = (c == '{' ? JSMN_OBJECT : JSMN_ARRAY); + token->start = parser->pos; + parser->toksuper = parser->toknext - 1; + break; + case '}': + case ']': + if (tokens == NULL) { + break; + } + type = (c == '}' ? JSMN_OBJECT : JSMN_ARRAY); +#ifdef JSMN_PARENT_LINKS + if (parser->toknext < 1) { + return JSMN_ERROR_INVAL; + } + token = &tokens[parser->toknext - 1]; + for (;;) { + if (token->start != -1 && token->end == -1) { + if (token->type != type) { + return JSMN_ERROR_INVAL; + } + token->end = parser->pos + 1; + parser->toksuper = token->parent; + break; + } + if (token->parent == -1) { + if (token->type != type || parser->toksuper == -1) { + return JSMN_ERROR_INVAL; + } + break; + } + token = &tokens[token->parent]; + } +#else + for (i = parser->toknext - 1; i >= 0; i--) { + token = &tokens[i]; + if (token->start != -1 && token->end == -1) { + if (token->type != type) { + return JSMN_ERROR_INVAL; + } + parser->toksuper = -1; + token->end = parser->pos + 1; + break; + } + } + /* Error if unmatched closing bracket */ + if (i == -1) { + return JSMN_ERROR_INVAL; + } + for (; i >= 0; i--) { + token = &tokens[i]; + if (token->start != -1 && token->end == -1) { + parser->toksuper = i; + break; + } + } +#endif + break; + case '\"': + r = jsmn_parse_string(parser, js, len, tokens, num_tokens); + if (r < 0) { + return r; + } + count++; + if (parser->toksuper != -1 && tokens != NULL) { + tokens[parser->toksuper].size++; + } + break; + case '\t': + case '\r': + case '\n': + case ' ': + break; + case ':': + parser->toksuper = parser->toknext - 1; + break; + case ',': + if (tokens != NULL && parser->toksuper != -1 && + tokens[parser->toksuper].type != JSMN_ARRAY && + tokens[parser->toksuper].type != JSMN_OBJECT) { +#ifdef JSMN_PARENT_LINKS + parser->toksuper = tokens[parser->toksuper].parent; +#else + for (i = parser->toknext - 1; i >= 0; i--) { + if (tokens[i].type == JSMN_ARRAY || tokens[i].type == JSMN_OBJECT) { + if (tokens[i].start != -1 && tokens[i].end == -1) { + parser->toksuper = i; + break; + } + } + } +#endif + } + break; +#ifdef JSMN_STRICT + /* In strict mode primitives are: numbers and booleans */ + case '-': + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + case 't': + case 'f': + case 'n': + /* And they must not be keys of the object */ + if (tokens != NULL && parser->toksuper != -1) { + const jsmntok_t *t = &tokens[parser->toksuper]; + if (t->type == JSMN_OBJECT || + (t->type == JSMN_STRING && t->size != 0)) { + return JSMN_ERROR_INVAL; + } + } +#else + /* In non-strict mode every unquoted value is a primitive */ + default: +#endif + r = jsmn_parse_primitive(parser, js, len, tokens, num_tokens); + if (r < 0) { + return r; + } + count++; + if (parser->toksuper != -1 && tokens != NULL) { + tokens[parser->toksuper].size++; + } + break; + +#ifdef JSMN_STRICT + /* Unexpected char in strict mode */ + default: + return JSMN_ERROR_INVAL; +#endif + } + } + + if (tokens != NULL) { + for (i = parser->toknext - 1; i >= 0; i--) { + /* Unmatched opened object or array */ + if (tokens[i].start != -1 && tokens[i].end == -1) { + return JSMN_ERROR_PART; + } + } + } + + return count; +} + +/** + * Creates a new parser based over a given buffer with an array of tokens + * available. + */ +JSMN_API void jsmn_init(jsmn_parser *parser) { + parser->pos = 0; + parser->toknext = 0; + parser->toksuper = -1; +} + +#endif /* JSMN_HEADER */ + +#ifdef __cplusplus +} +#endif + +#endif /* JSMN_H */ diff --git a/samples/main.cpp b/samples/main.cpp new file mode 100644 index 0000000..f8fa767 --- /dev/null +++ b/samples/main.cpp @@ -0,0 +1,356 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +// CRT's memory leak detection +#if defined( _WIN64 ) +#ifndef _CRTDBG_MAP_ALLOC +#define _CRTDBG_MAP_ALLOC +#endif +#include +#endif + +// clang-format off +#include "glad/glad.h" +#include "GLFW/glfw3.h" +// clang-format on + +#include "font.h" +#include "imgui.h" +#include "imgui_impl_glfw.h" +#include "imgui_impl_opengl3.h" +#include "implot.h" +#include "renderer.h" +#include "sample.h" +#include "shader.h" + +#include + +#ifdef TRACY_ENABLE +#include +#endif + +// imgui notes +// high-dpi support: https://github.com/ocornut/imgui/issues/5081 +// linear blending: +// http://arkanis.de/weblog/2023-08-14-simple-good-quality-subpixel-text-rendering-in-opengl-with-stb-truetype-and-dual-source-blending + +inline bool IsPowerOfTwo( int32_t x ) +{ + return ( x != 0 ) && ( ( x & ( x - 1 ) ) == 0 ); +} + +void* AllocFcn( int32_t size, int32_t alignment ) +{ + // Allocation must be a multiple of alignment or risk a seg fault + // https://en.cppreference.com/w/c/memory/aligned_alloc + assert( IsPowerOfTwo( alignment ) ); + size_t sizeAligned = ( ( size - 1 ) | ( alignment - 1 ) ) + 1; + assert( ( sizeAligned & ( alignment - 1 ) ) == 0 ); + +#if defined( _MSC_VER ) + void* ptr = _aligned_malloc( sizeAligned, alignment ); +#else + void* ptr = aligned_alloc( alignment, sizeAligned ); +#endif + assert( ptr != nullptr ); + return ptr; +} + +void FreeFcn( void* mem ) +{ +#if defined( _MSC_VER ) + _aligned_free( mem ); +#else + free( mem ); +#endif +} + +static int AssertHandler( const char* cond, const char* file, int line ) +{ + printf( "Box3D Assert: %s, %s, line %d\n", cond, file, line ); + return 1; +} + +#define WINDOW_TITLE "Box3D v0.1.0" + +static int s_windowWidth = 1920; +static int s_windowHeight = 1080; +static int s_bufferWidth = s_windowWidth; +static int s_bufferHeight = s_windowHeight; + +Font* s_font = nullptr; +SampleManager s_manager; + +static void WindowSizeCallback( GLFWwindow* window, int width, int height ) +{ + s_windowWidth = width; + s_windowHeight = height; + glfwGetFramebufferSize( window, &s_bufferWidth, &s_bufferHeight ); + s_manager.Resize( s_windowWidth, s_windowHeight, s_bufferWidth, s_bufferHeight ); +} + +static void ErrorCallback( int error, const char* description ) +{ + // Report GLFW error + printf( "GLFW Error %d: %s", error, description ); +} + +static void KeyCallback( GLFWwindow* window, int key, int scancode, int action, int mods ) +{ + ImGui_ImplGlfw_KeyCallback( window, key, scancode, action, mods ); + if ( ImGui::GetIO().WantCaptureKeyboard ) + { + return; + } + + s_manager.Keyboard( key, action, mods ); +} + +void CharCallback( GLFWwindow* window, unsigned int character ) +{ + ImGui_ImplGlfw_CharCallback( window, character ); +} + +static void MouseButtonCallback( GLFWwindow* window, int button, int action, int modifiers ) +{ + ImGui_ImplGlfw_MouseButtonCallback( window, button, action, modifiers ); + if ( ImGui::GetIO().WantCaptureMouse ) + { + return; + } + + double xd, yd; + glfwGetCursorPos( window, &xd, &yd ); + + b3Vec2 ps = { float( xd ), float( yd ) }; + + // Use the mouse to move things around. + if ( button == GLFW_MOUSE_BUTTON_1 ) + { + if ( action == GLFW_PRESS ) + { + s_manager.m_sample->MouseDown( ps, button, modifiers ); + } + + if ( action == GLFW_RELEASE ) + { + s_manager.m_sample->MouseUp( ps, button ); + } + } +} + +static void MouseMotionCallback( GLFWwindow* window, double x, double y ) +{ + b3Vec2 ps = { float( x ), float( y ) }; + + ImGui_ImplGlfw_CursorPosCallback( window, ps.x, ps.y ); + + s_manager.m_sample->MouseMove( ps ); +} + +static void ScrollCallback( GLFWwindow* window, double deltaX, double deltaY ) +{ + ImGui_ImplGlfw_ScrollCallback( window, deltaX, deltaY ); +} + +static void Startup( GLFWwindow* window ) +{ + // Initialize font + s_font = Font::Create( &s_manager.m_context.camera, "data/fonts/Roboto-Medium.ttf", 18.0f ); + + if ( s_font == nullptr ) + { + exit( EXIT_FAILURE ); + } + + IMGUI_CHECKVERSION(); + ImGui::CreateContext(); + ImPlot::CreateContext(); + + // Setup Platform/Renderer bindings + ImGui_ImplGlfw_InitForOpenGL( window, false ); + ImGui_ImplOpenGL3_Init(); + + ImGuiIO& io = ImGui::GetIO(); + io.Fonts->AddFontFromFileTTF( "data/fonts/Roboto-Medium.ttf", 16.0f ); + + // Initialize test manager + s_manager.Startup( window, s_windowWidth, s_windowHeight, s_bufferWidth, s_bufferHeight ); +} + +static void Render( GLFWwindow* window, float elapsedTime ) +{ + // Start the Dear ImGui frame + ImGui_ImplOpenGL3_NewFrame(); + ImGui_ImplGlfw_NewFrame(); + + ImGui::NewFrame(); + + glViewport( 0, 0, s_bufferWidth, s_bufferHeight ); + + glClearColor( 0.3f, 0.3f, 0.3f, 1.0f ); + glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); + + s_manager.Render( window, elapsedTime ); + + int y = s_windowHeight - 10; + + int triangleIndex = s_manager.m_sample->m_triangleIndex; + uint64_t materialId = s_manager.m_sample->m_userMaterialId; + + const Camera* camera = &s_manager.m_context.camera; + static char buffer[128]; + snprintf( buffer, 128, "%.1f ms - step %d - camera (%g, %g, %g, %g) - tri %d - mat %d", 1000.0f * elapsedTime, s_manager.m_sample->m_stepCount, + camera->m_yaw, camera->m_pitch, camera->m_radius, camera->m_speed, triangleIndex, (int)materialId ); + DrawText( 10, y, b3_colorLightGray, buffer ); + + if ( s_font != nullptr ) + { + s_font->Flush(); + } + + //ImGui::ShowDemoWindow(); + //ImPlot::ShowDemoWindow(); + + ImGui::Render(); + ImGui_ImplOpenGL3_RenderDrawData( ImGui::GetDrawData() ); + + // Platform functions may have changed the current OpenGL context + glfwMakeContextCurrent( window ); + glfwSwapBuffers( window ); +} + +static void Shutdown( GLFWwindow* window ) +{ + // Shutdown test manager + s_manager.Shutdown( window ); + + ImGui_ImplOpenGL3_Shutdown(); + ImGui_ImplGlfw_Shutdown(); + ImPlot::DestroyContext(); + ImGui::DestroyContext(); + + delete s_font; + s_font = nullptr; +} + +int main() +{ +#if defined( _WIN64 ) + // Enable run-time memory check for debug builds + // How to break at the leaking allocation, in the watch window enter this variable + // and set it to the allocation number in {}. Do this at the first line in main. + // https://learn.microsoft.com/en-us/troubleshoot/developer/visualstudio/cpp/libraries/use-rtbreakalloc-debug-memory-allocation + //_CrtSetBreakAlloc(1392); + _CrtSetReportMode( _CRT_WARN, _CRTDBG_MODE_DEBUG | _CRTDBG_MODE_FILE ); + _CrtSetReportFile( _CRT_WARN, _CRTDBG_FILE_STDOUT ); + //_CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF); +#endif + +#ifdef TRACY_ENABLE + tracy::StartupProfiler(); +#endif + + b3SetAllocator( AllocFcn, FreeFcn ); + b3SetAssertFcn( AssertHandler ); + + // Initialize GLFW + glfwSetErrorCallback( ErrorCallback ); + if ( !glfwInit() ) + { + fprintf( stderr, "Failed to initialize GLFW\n" ); + return -1; + } + + // Create OpenGL window + glfwWindowHint( GLFW_CONTEXT_VERSION_MAJOR, 4 ); + glfwWindowHint( GLFW_CONTEXT_VERSION_MINOR, 1 ); + glfwWindowHint( GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE ); + + // MSAA + glfwWindowHint( GLFW_SAMPLES, 4 ); + + GLFWwindow* window = glfwCreateWindow( s_windowWidth, s_windowHeight, WINDOW_TITLE, nullptr, nullptr ); + if ( window == nullptr ) + { + glfwTerminate(); + return -1; + } + + glfwGetWindowSize( window, &s_windowWidth, &s_windowHeight ); + glfwGetFramebufferSize( window, &s_bufferWidth, &s_bufferHeight ); + + // Set window callbacks + glfwSetKeyCallback( window, KeyCallback ); + glfwSetCharCallback( window, CharCallback ); + glfwSetCursorPosCallback( window, MouseMotionCallback ); + glfwSetMouseButtonCallback( window, MouseButtonCallback ); + glfwSetScrollCallback( window, ScrollCallback ); + glfwSetWindowSizeCallback( window, WindowSizeCallback ); + + // Make OpenGL context current + glfwMakeContextCurrent( window ); + + // This enables v-sync + // glfwSwapInterval( 1 ); + + // Setup GLAD bindings + if ( !gladLoadGL() ) + { + fprintf( stderr, "Failed to initialize glad\n" ); + glfwTerminate(); + return -1; + } + + { + const char* glVersionString = (const char*)glGetString( GL_VERSION ); + const char* glslVersionString = (const char*)glGetString( GL_SHADING_LANGUAGE_VERSION ); + printf( "OpenGL %s, GLSL %s\n", glVersionString, glslVersionString ); + } + + Startup( window ); + + glEnable( GL_DEPTH_TEST ); + + float frameTime = 0.0f; + while ( !glfwWindowShouldClose( window ) ) + { + glfwPollEvents(); + + if (s_manager.m_context.minimized) + { + continue; + } + + double time1 = glfwGetTime(); + + s_manager.Step(); + Render( window, frameTime ); + + // Limit frame rate to 60Hz + double time2 = glfwGetTime(); + double targetTime = time1 + 1.0 / 60.0; + while ( time2 < targetTime ) + { + b3Yield(); + time2 = glfwGetTime(); + } + + frameTime = float( time2 - time1 ); + } + + Shutdown( window ); + + glfwDestroyWindow( window ); + glfwTerminate(); + +#ifdef TRACY_ENABLE + tracy::ShutdownProfiler(); +#endif + +#if defined( _WIN64 ) + _CrtDumpMemoryLeaks(); +#endif + + return 0; +} diff --git a/samples/mesh_loader.cpp b/samples/mesh_loader.cpp new file mode 100644 index 0000000..d31d307 --- /dev/null +++ b/samples/mesh_loader.cpp @@ -0,0 +1,172 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "mesh_loader.h" + +#include "box3d/collision.h" +#include "box3d/math_functions.h" + +#define TINYOBJLOADER_IMPLEMENTATION +#define TINYOBJLOADER_USE_MAPBOX_EARCUT +#include "tiny_obj_loader.h" + +#include + +// Load a wavefront obj file using tiny obj loader and earcut for triangulation. +void LoadTempMesh( const char* path, TempMesh* tempMesh, float scale, bool zUp ) +{ + std::string inputFile = path; + tinyobj::ObjReaderConfig readerConfig; + readerConfig.triangulate = true; + readerConfig.mtl_search_path = "./"; // Path to material files + + tinyobj::ObjReader reader; + + if ( !reader.ParseFromFile( inputFile, readerConfig ) ) + { + if ( !reader.Error().empty() ) + { + std::cerr << "TinyObjReader: " << reader.Error(); + } + exit( 1 ); + } + + if ( !reader.Warning().empty() ) + { + std::cout << "TinyObjReader: " << reader.Warning(); + } + + const tinyobj::attrib_t& attrib = reader.GetAttrib(); + const std::vector& shapes = reader.GetShapes(); + // const std::vector& objMaterials = reader.GetMaterials(); + + tempMesh->vertices.clear(); + tempMesh->indices.clear(); + tempMesh->materialIndices.clear(); + + int vertexCount = int( attrib.vertices.size() / 3 ); + for ( int i = 0; i < vertexCount; i++ ) + { + float x = scale * attrib.vertices[3 * i + 0]; + float y = scale * attrib.vertices[3 * i + 1]; + float z = scale * attrib.vertices[3 * i + 2]; + b3Vec3 vertex = zUp ? b3Vec3{ y, z, x } : b3Vec3{ x, y, z }; + tempMesh->vertices.push_back( vertex ); + } + + // Loop over shapes + int shapeCount = (int)shapes.size(); + int materialIndex = 0; + for ( int shapeIndex = 0; shapeIndex < shapeCount; ++shapeIndex ) + { + int faceCount = (int)shapes[shapeIndex].mesh.num_face_vertices.size(); + int baseIndex = 0; + for ( int faceIndex = 0; faceIndex < faceCount; ++faceIndex ) + { + int faceVertexCount = (int)shapes[shapeIndex].mesh.num_face_vertices[faceIndex]; + + // todo only triangles for now + if ( faceVertexCount != 3 ) + { + baseIndex += faceVertexCount; + continue; + } + + for ( int vertexIndex = 0; vertexIndex < faceVertexCount; ++vertexIndex ) + { + tinyobj::index_t idx = shapes[shapeIndex].mesh.indices[baseIndex + vertexIndex]; + tempMesh->indices.push_back( idx.vertex_index ); + } + + tempMesh->materialIndices.push_back( materialIndex ); + materialIndex = ( materialIndex + 1 ) % 3; + + baseIndex += faceVertexCount; + } + } +} + +b3MeshData* CreateMeshData( const char* path, float scale, bool zUp, bool useMedianSplit, bool identifyConvexEdges, bool weldVertices ) +{ + std::string inputFile = path; + tinyobj::ObjReaderConfig readerConfig; + readerConfig.triangulate = true; + readerConfig.mtl_search_path = "./"; // Path to material files + + tinyobj::ObjReader reader; + + if ( !reader.ParseFromFile( inputFile, readerConfig ) ) + { + if ( !reader.Error().empty() ) + { + std::cerr << "TinyObjReader: " << reader.Error(); + } + exit( 1 ); + } + + if ( !reader.Warning().empty() ) + { + std::cout << "TinyObjReader: " << reader.Warning(); + } + + const tinyobj::attrib_t& attrib = reader.GetAttrib(); + const std::vector& shapes = reader.GetShapes(); + // const std::vector& objMaterials = reader.GetMaterials(); + + TempMesh mesh; + + int vertexCount = int( attrib.vertices.size() / 3 ); + for ( int i = 0; i < vertexCount; i++ ) + { + float x = scale * attrib.vertices[3 * i + 0]; + float y = scale * attrib.vertices[3 * i + 1]; + float z = scale * attrib.vertices[3 * i + 2]; + b3Vec3 vertex = zUp ? b3Vec3{ y, z, x } : b3Vec3{ x, y, z }; + mesh.vertices.push_back( vertex ); + } + + // Loop over shapes + int shapeCount = (int)shapes.size(); + int materialIndex = 0; + for ( int shapeIndex = 0; shapeIndex < shapeCount; ++shapeIndex ) + { + int faceCount = (int)shapes[shapeIndex].mesh.num_face_vertices.size(); + int baseIndex = 0; + for ( int faceIndex = 0; faceIndex < faceCount; ++faceIndex ) + { + int faceVertexCount = (int)shapes[shapeIndex].mesh.num_face_vertices[faceIndex]; + + // todo only triangles for now + if ( faceVertexCount != 3 ) + { + baseIndex += faceVertexCount; + continue; + } + + for ( int vertexIndex = 0; vertexIndex < faceVertexCount; ++vertexIndex ) + { + tinyobj::index_t idx = shapes[shapeIndex].mesh.indices[baseIndex + vertexIndex]; + mesh.indices.push_back( idx.vertex_index ); + } + + mesh.materialIndices.push_back( materialIndex ); + materialIndex = ( materialIndex + 1 ) % 3; + + baseIndex += faceVertexCount; + } + } + + b3MeshDef def = {}; + def.vertices = mesh.vertices.data(); + def.vertexCount = (int)mesh.vertices.size(); + def.indices = mesh.indices.data(); + def.triangleCount = (int)mesh.indices.size() / 3; + def.materialIndices = mesh.materialIndices.data(); + def.useMedianSplit = useMedianSplit; + def.identifyEdges = identifyConvexEdges; + def.weldVertices = weldVertices; + def.weldTolerance = 0.002f; + + b3MeshData* meshData = b3CreateMesh( &def, nullptr, 0 ); + return meshData; +} diff --git a/samples/mesh_loader.h b/samples/mesh_loader.h new file mode 100644 index 0000000..d91dad6 --- /dev/null +++ b/samples/mesh_loader.h @@ -0,0 +1,21 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box3d/math_functions.h" + +#include + +struct b3MeshData; +b3MeshData* CreateMeshData( const char* path, float scale, bool zUp, bool useMedianSplit, bool identifyConvexEdges, + bool weldVertices ); + +struct TempMesh +{ + std::vector vertices; + std::vector indices; + std::vector materialIndices; +}; + +void LoadTempMesh( const char* path, TempMesh* tempMesh, float scale, bool zUp ); diff --git a/samples/renderer.cpp b/samples/renderer.cpp new file mode 100644 index 0000000..5373313 --- /dev/null +++ b/samples/renderer.cpp @@ -0,0 +1,783 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "renderer.h" + +#include "camera.h" +#include "font.h" +#include "sample.h" +#include "scene.h" +#include "shader.h" + +#include "box3d/collision.h" +#include "box3d/math_functions.h" + +// clang-format off +#include +#include +// clang-format on + +#include "box3d/box3d.h" + +#include +#include +#include +#include +#include + +struct PointData +{ + b3Vec3 position; + float size; + RGBA8 rgba; +}; + +struct PointDraw +{ + enum + { + e_batchSize = 2048 + }; + + std::vector Points; + + uint32_t programId; + + uint32_t vaoId; + uint32_t vboId; + + int ProjectionUniform; + int ViewUniform; + int SizeUniform; +}; + +PointDraw* CreateDebugPoints() +{ + const char* vs = R"( + #version 330 core + uniform mat4 projection; + uniform mat4 view; + layout(location = 0) in vec3 point; + layout(location = 1) in vec4 color; + layout(location = 2) in float size; + out vec4 f_color; + void main(void) + { + gl_Position = projection * view * vec4(point, 1.0f); + f_color = color; + gl_PointSize = size; + } + )"; + + const char* fs = R"( + #version 330 core + in vec4 f_color; + out vec4 color; + void main(void) + { + color = f_color; + } + )"; + + PointDraw* draw = new PointDraw; + + draw->programId = CreateProgramFromStrings( vs, fs ); + draw->ProjectionUniform = GetUniformLocation( draw->programId, "projection" ); + draw->ViewUniform = GetUniformLocation( draw->programId, "view" ); + + constexpr int vertexAttribute = 0; + constexpr int sizeAttribute = 1; + constexpr int colorAttribute = 2; + + // Generate + glGenVertexArrays( 1, &draw->vaoId ); + glGenBuffers( 1, &draw->vboId ); + + glBindVertexArray( draw->vaoId ); + glEnableVertexAttribArray( vertexAttribute ); + glEnableVertexAttribArray( sizeAttribute ); + glEnableVertexAttribArray( colorAttribute ); + + // Vertex buffer + glBindBuffer( GL_ARRAY_BUFFER, draw->vboId ); + + glBufferData( GL_ARRAY_BUFFER, PointDraw::e_batchSize * sizeof( PointData ), nullptr, GL_DYNAMIC_DRAW ); + + glVertexAttribPointer( vertexAttribute, 3, GL_FLOAT, GL_FALSE, sizeof( PointData ), (void*)offsetof( PointData, position ) ); + glVertexAttribPointer( colorAttribute, 4, GL_FLOAT, GL_FALSE, sizeof( PointData ), (void*)offsetof( PointData, size ) ); + // save bandwidth by expanding color to floats in the shader + glVertexAttribPointer( sizeAttribute, 4, GL_UNSIGNED_BYTE, GL_TRUE, sizeof( PointData ), (void*)offsetof( PointData, rgba ) ); + + CheckOpenGL(); + + // Cleanup + glBindBuffer( GL_ARRAY_BUFFER, 0 ); + glBindVertexArray( 0 ); + + return draw; +} + +void DestroyDebugPoints( PointDraw* draw ) +{ + if ( draw->vaoId ) + { + glDeleteVertexArrays( 1, &draw->vaoId ); + glDeleteBuffers( 1, &draw->vboId ); + draw->vaoId = 0; + } + + DestroyShader( draw->programId ); + draw->programId = 0; + + delete draw; +} + +void AddPoint( PointDraw* draw, b3Vec3 point, float size, b3HexColor color ) +{ + draw->Points.push_back( { point, size, MakeRGBA8( color ) } ); +} + +void FlushPoints( PointDraw* draw, Camera* camera ) +{ + if ( draw->Points.size() == 0 ) + { + return; + } + + glDepthFunc( GL_LEQUAL ); + + glUseProgram( draw->programId ); + + Matrix4 projection = camera->GetProjectionMatrix(); + Matrix4 view = camera->GetViewMatrix(); + + SetUniformMatrix4( draw->ProjectionUniform, projection ); + SetUniformMatrix4( draw->ViewUniform, view ); + + glBindVertexArray( draw->vaoId ); + + int totalCount = (int)draw->Points.size(); + PointData* points = draw->Points.data(); + + while ( totalCount > 0 ) + { + int count = b3MinInt( totalCount, PointDraw::e_batchSize ); + + glBindBuffer( GL_ARRAY_BUFFER, draw->vboId ); + // todo macOS optimization? + // glBufferData( GL_ARRAY_BUFFER, PointDraw::e_batchSize * sizeof( PointData ), nullptr, GL_DYNAMIC_DRAW ); + glBufferSubData( GL_ARRAY_BUFFER, 0, count * sizeof( PointData ), points ); + + glEnable( GL_PROGRAM_POINT_SIZE ); + glDrawArrays( GL_POINTS, 0, count ); + glDisable( GL_PROGRAM_POINT_SIZE ); + + points += count; + totalCount -= count; + } + + CheckOpenGL(); + + glBindBuffer( GL_ARRAY_BUFFER, 0 ); + glBindVertexArray( 0 ); + glUseProgram( 0 ); + + draw->Points.clear(); +} + +struct LinePoint +{ + b3Vec3 position; + RGBA8 rgba; +}; + +struct LineDraw +{ + enum + { + e_batchSize = 2048 + }; + + std::vector points; + + uint32_t programId; + + uint32_t VaoId; + uint32_t VboId; + + int ProjectionUniform; + int ViewUniform; +}; + +LineDraw* CreateDebugLines() +{ + const char* vs = R"( + #version 330 core + uniform mat4 projection; + uniform mat4 view; + layout(location = 0) in vec3 point; + layout(location = 1) in vec4 color; + out vec4 f_color; + void main(void) + { + gl_Position = projection * view * vec4(point, 1.0f); + f_color = color; + } + )"; + + const char* fs = R"( + #version 330 core + in vec4 f_color; + out vec4 color; + void main(void) + { + color = f_color; + } + )"; + + LineDraw* draw = new LineDraw; + + draw->programId = CreateProgramFromStrings( vs, fs ); + draw->ProjectionUniform = GetUniformLocation( draw->programId, "projection" ); + draw->ViewUniform = GetUniformLocation( draw->programId, "view" ); + + constexpr int vertexAttribute = 0; + constexpr int colorAttribute = 1; + + // Generate + glGenVertexArrays( 1, &draw->VaoId ); + glGenBuffers( 1, &draw->VboId ); + + glBindVertexArray( draw->VaoId ); + glEnableVertexAttribArray( vertexAttribute ); + glEnableVertexAttribArray( colorAttribute ); + + // Vertex buffer + glBindBuffer( GL_ARRAY_BUFFER, draw->VboId ); + glBufferData( GL_ARRAY_BUFFER, LineDraw::e_batchSize * sizeof( LinePoint ), nullptr, GL_DYNAMIC_DRAW ); + + glVertexAttribPointer( vertexAttribute, 3, GL_FLOAT, GL_FALSE, sizeof( LinePoint ), + (void*)offsetof( LinePoint, position ) ); + // save bandwidth by expanding color to floats in the shader + glVertexAttribPointer( colorAttribute, 4, GL_UNSIGNED_BYTE, GL_TRUE, sizeof( LinePoint ), + (void*)offsetof( LinePoint, rgba ) ); + + CheckOpenGL(); + + // Cleanup + glBindBuffer( GL_ARRAY_BUFFER, 0 ); + glBindVertexArray( 0 ); + + return draw; +} + +void DestroyDebugLines( LineDraw* draw ) +{ + if ( draw->VaoId ) + { + glDeleteVertexArrays( 1, &draw->VaoId ); + glDeleteBuffers( 1, &draw->VboId ); + draw->VaoId = 0; + } + + DestroyShader( draw->programId ); + draw->programId = 0; + + delete draw; +} + +void AddLine( LineDraw* draw, b3Vec3 point1, b3Vec3 point2, b3HexColor color ) +{ + draw->points.push_back( { point1, MakeRGBA8( color ) } ); + draw->points.push_back( { point2, MakeRGBA8( color ) } ); +} + +void FlushLines( LineDraw* draw, Camera* camera ) +{ + if ( draw->points.size() == 0 ) + { + return; + } + + // WARNING: very slow + //glEnable( GL_LINE_SMOOTH ); + + glEnable( GL_BLEND ); + glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA ); + + glUseProgram( draw->programId ); + + Matrix4 projection = camera->GetProjectionMatrix(); + Matrix4 view = camera->GetViewMatrix(); + + SetUniformMatrix4( draw->ProjectionUniform, projection ); + SetUniformMatrix4( draw->ViewUniform, view ); + + glBindVertexArray( draw->VaoId ); + glBindBuffer( GL_ARRAY_BUFFER, draw->VboId ); + + int count = (int)draw->points.size(); + LinePoint* points = draw->points.data(); + + while ( count > 0 ) + { + int batchCount = b3MinInt( count, LineDraw::e_batchSize ); + + // todo buffer orphaning for macOS? + // glBufferData( GL_ARRAY_BUFFER, LineDraw::e_batchSize * sizeof( LinePoint ), nullptr, GL_DYNAMIC_DRAW ); + glBufferSubData( GL_ARRAY_BUFFER, 0, batchCount * sizeof( LinePoint ), points ); + glDrawArrays( GL_LINES, 0, batchCount ); + + points += batchCount; + count -= batchCount; + } + + CheckOpenGL(); + + glBindBuffer( GL_ARRAY_BUFFER, 0 ); + glBindVertexArray( 0 ); + glUseProgram( 0 ); + glDisable( GL_BLEND ); + + draw->points.clear(); +} + +static DebugShape* CreateDebugSphere( const b3Sphere* sphere ) +{ + DebugShape* s = new DebugShape; + s->type = DebugShapeType::sphere; + s->sphere.center = sphere->center; + s->sphere.radius = sphere->radius; + return s; +} + +static DebugShape* CreateDebugCapsule( const b3Capsule* capsule, Arena arena ) +{ + DebugShape* s = new DebugShape; + s->type = DebugShapeType::capsule; + s->capsule.center1 = capsule->center1; + s->capsule.center2 = capsule->center2; + s->capsule.radius = capsule->radius; + return s; +} + +static DebugShape* CreateDebugCompound(Scene* scene, const b3Compound* compound, Arena arena ) +{ + DebugShape* s = new DebugShape; + memset( s, 0, sizeof( DebugShape ) ); + s->type = DebugShapeType::compound; + + // Capsules + s->compound.capsuleCount = compound->capsuleCount; + DebugCapsule* debugCapsules = new DebugCapsule[s->compound.capsuleCount]; + for (int i = 0; i < s->compound.capsuleCount; ++i) + { + b3CompoundCapsule element = b3GetCompoundCapsule( compound, i ); + + debugCapsules[i].center1 = element.capsule.center1; + debugCapsules[i].center2 = element.capsule.center2; + debugCapsules[i].radius = element.capsule.radius; + } + s->compound.capsules = debugCapsules; + + // Hulls + s->compound.hullCount = compound->hullCount; + DebugHull* debugHulls = new DebugHull[s->compound.hullCount]; + for (int i = 0; i < s->compound.hullCount; ++i) + { + b3CompoundHull hull = b3GetCompoundHull( compound, i ); + + uint32_t id = CreateInstanceHull( scene, hull.hull, arena ); + debugHulls[i].transform = hull.transform; + debugHulls[i].id = id; + } + s->compound.hulls = debugHulls; + + // Meshes + s->compound.meshCount = compound->meshCount; + DebugMesh* debugMeshes = new DebugMesh[s->compound.meshCount]; + for ( int i = 0; i < s->compound.meshCount; ++i ) + { + b3CompoundMesh mesh = b3GetCompoundMesh( compound, i ); + + uint32_t id = CreateInstanceMesh( scene, mesh.meshData, arena ); + debugMeshes[i].transform = mesh.transform; + debugMeshes[i].scale = mesh.scale; + debugMeshes[i].id = id; + } + s->compound.meshes = debugMeshes; + + // Spheres + s->compound.sphereCount = compound->sphereCount; + DebugSphere* debugSpheres = new DebugSphere[s->compound.sphereCount]; + for (int i = 0; i < s->compound.sphereCount; ++i) + { + b3CompoundSphere sphere = b3GetCompoundSphere( compound, i ); + debugSpheres[i].center = sphere.sphere.center; + debugSpheres[i].radius = sphere.sphere.radius; + } + s->compound.spheres = debugSpheres; + + return s; +} + +static void DestroyDebugCompound( DebugCompound* compound ) +{ + delete[] compound->capsules; + delete[] compound->hulls; + delete[] compound->meshes; + delete[] compound->spheres; +} + +static DebugShape* CreateDebugHull( Scene* scene, const b3Hull* hull, Arena arena ) +{ + uint32_t id = CreateInstanceHull( scene, hull, arena ); + + DebugShape* s = new DebugShape; + s->type = DebugShapeType::hull; + s->hull.id = id; + s->hull.transform = b3Transform_identity; + return s; +} + +static DebugShape* CreateDebugMesh( Scene* scene, const b3Mesh* mesh, Arena arena ) +{ + uint32_t id = CreateInstanceMesh( scene, mesh->data, arena ); + + DebugShape* s = new DebugShape; + s->type = DebugShapeType::mesh; + s->mesh.id = id; + s->mesh.transform = b3Transform_identity; + s->mesh.scale = mesh->scale; + return s; +} + +static DebugShape* CreateDebugHeightField( Scene* scene, const b3HeightField* heightField, Arena arena ) +{ + uint32_t id = CreateInstanceHeightField( scene, heightField, arena ); + + DebugShape* s = new DebugShape; + s->type = DebugShapeType::heightField; + s->heightField.id = id; + return s; +} + +void* CreateDebugShape( const b3DebugShape* debugShape, void* context ) +{ + SampleContext* sampleContext = static_cast( context ); + DebugShape* s = nullptr; + switch ( debugShape->type ) + { + case b3_capsuleShape: + s = CreateDebugCapsule( debugShape->capsule, sampleContext->arena ); + break; + + case b3_compoundShape: + s = CreateDebugCompound( sampleContext->scene, debugShape->compound, sampleContext->arena ); + break; + + case b3_hullShape: + s = CreateDebugHull( sampleContext->scene, debugShape->hull, sampleContext->arena ); + break; + + case b3_meshShape: + s = CreateDebugMesh( sampleContext->scene, debugShape->mesh, sampleContext->arena ); + break; + + case b3_heightShape: + s = CreateDebugHeightField( sampleContext->scene, debugShape->heightField, sampleContext->arena ); + break; + + case b3_sphereShape: + s = CreateDebugSphere( debugShape->sphere ); + break; + + default: + break; + } + + if (s != nullptr) + { + b3BodyId bodyId = b3Shape_GetBody( debugShape->shapeId ); + s->bodyType = b3Body_GetType( bodyId ); + } + else + { + s->bodyType = b3_staticBody; + } + + return s; +} + +void DestroyDebugShape( void* userShape, void* context ) +{ + DebugShape* s = static_cast( userShape ); + switch ( s->type ) + { + case DebugShapeType::compound: + DestroyDebugCompound( &s->compound ); + break; + + default: + break; + } + + delete s; +} + +extern Font* s_font; +void DrawText( int x, int y, b3HexColor color, const char* text ) +{ + s_font->AddText( x, y, color, text ); +} + +void DrawFormat( int x, int y, b3HexColor color, const char* format, ... ) +{ + va_list args; + va_start( args, format ); + char text[512]; + vsnprintf( text, sizeof( text ), format, args ); + DrawText( x, y, color, text ); + va_end( args ); +} + +void DrawWorldString( Camera* camera, b3Vec3 point, b3HexColor color, const char* format, ... ) +{ + float x, y; + bool valid = camera->WorldToScreen( x, y, point ); + if ( valid ) + { + va_list args; + va_start( args, format ); + char text[512]; + vsnprintf( text, sizeof( text ), format, args ); + DrawText( (int)x, (int)y, color, text ); + va_end( args ); + } +} + +static void DrawDisc( Scene* scene, b3Vec3 center, b3Vec3 normal, float radius, b3HexColor color ) +{ + b3Vec3 tangent1 = b3Perp( normal ); + b3Vec3 tangent2 = b3Cross( normal, tangent1 ); + + int n = 9; + float delta = B3_PI / (float)n; + b3CosSin cs = b3ComputeCosSin( delta ); + float x1 = radius, y1 = 0.0f; + b3Vec3 vertex1 = center + x1 * tangent1 + y1 * tangent2; + + for ( int i = 0; i < 2 * n; ++i ) + { + float x2 = cs.cosine * x1 - cs.sine * y1; + float y2 = cs.sine * x1 + cs.cosine * y1; + b3Vec3 vertex2 = center + x2 * tangent1 + y2 * tangent2; + + DrawLine( scene, vertex1, vertex2, color ); + + x1 = x2; + y1 = y2; + vertex1 = vertex2; + } +} + +static void DrawArc( Scene* scene, b3Vec3 center, b3Vec3 normal, float radius, b3Vec3 start, float maxAngle, b3HexColor color ) +{ + b3Vec3 tangent1 = b3Normalize( start ); + b3Vec3 tangent2 = b3Cross( normal, tangent1 ); + + for ( float angle = 0.0f; angle < maxAngle; angle += 5.0f ) + { + float alpha1 = B3_DEG_TO_RAD * ( angle + 0.0f ); + b3Vec3 vertex1 = center + b3Cos( alpha1 ) * radius * tangent1 + b3Sin( alpha1 ) * radius * tangent2; + float alpha2 = B3_DEG_TO_RAD * ( angle + 5.0f ); + b3Vec3 vertex2 = center + b3Cos( alpha2 ) * radius * tangent1 + b3Sin( alpha2 ) * radius * tangent2; + + DrawLine( scene, vertex1, vertex2, color ); + } +} + +void DrawGrid( Scene* scene, int size ) +{ + float grid = float( size ); + + for ( int x = -size; x <= size; ++x ) + { + if ( x == 0 ) + { + DrawLine( scene, { 0.0f, 0.0f, -grid }, { 0.0f, 0.0f, 0.0f }, b3_colorLightGray ); + DrawLine( scene, { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, grid }, b3_colorBlue ); + } + else if ( ( x % 10 ) == 0 ) + { + DrawLine( scene, { float( x ), 0.0f, -grid }, { float( x ), 0.0f, grid }, b3_colorDarkGray ); + } + else + { + DrawLine( scene, { float( x ), 0.0f, -grid }, { float( x ), 0.0f, grid }, b3_colorGray ); + } + } + + for ( int z = -size; z <= size; ++z ) + { + if ( z == 0 ) + { + DrawLine( scene, { -grid, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f }, b3_colorLightGray ); + DrawLine( scene, { 0.0f, 0.0f, 0.0f }, { grid, 0.0f, 0.0f }, b3_colorRed ); + } + else if ( ( z % 10 ) == 0 ) + { + DrawLine( scene, { -grid, 0.0f, float( z ) }, { grid, 0.0f, float( z ) }, b3_colorDarkGray ); + } + else + { + DrawLine( scene, { -grid, 0.0f, float( z ) }, { grid, 0.0f, float( z ) }, b3_colorGray ); + } + } +} + +void DrawCross( Scene* scene, b3Vec3 center, float radius, b3HexColor color ) +{ + DrawLine( scene, center - b3Vec3{ radius, 0.0, 0.0f }, center + b3Vec3{ radius, 0.0f, 0.0f }, color ); + DrawLine( scene, center - b3Vec3{ 0.0f, radius, 0.0f }, center + b3Vec3{ 0.0f, radius, 0.0f }, color ); + DrawLine( scene, center - b3Vec3{ 0.0f, 0.0f, radius }, center + b3Vec3{ 0.0f, 0.0f, radius }, color ); +} + +void DrawSphere( Scene* scene, const b3Transform& transform, const b3Sphere& sphere, b3HexColor color ) +{ + b3Vec3 center = b3TransformPoint( transform, sphere.center ); + float radius = sphere.radius; + + b3Vec3 axisX = b3RotateVector( transform.q, { 1.0f, 0.0f, 0.0f } ); + b3Vec3 axisY = b3RotateVector( transform.q, { 0.0f, 1.0f, 0.0f } ); + b3Vec3 axisZ = b3RotateVector( transform.q, { 0.0f, 0.0f, 1.0f } ); + + DrawDisc( scene, center, axisX, radius, color ); + DrawDisc( scene, center, axisY, radius, color ); + DrawDisc( scene, center, axisZ, radius, color ); +} + +void DrawCapsule( Scene* scene, const b3Transform& transform, const b3Capsule& capsule, b3HexColor color ) +{ + b3Vec3 center1 = b3TransformPoint( transform, capsule.center1 ); + b3Vec3 center2 = b3TransformPoint( transform, capsule.center2 ); + float radius = capsule.radius; + + b3Vec3 normal = b3Normalize( center2 - center1 ); + b3Vec3 tangent1 = b3Perp( normal ); + b3Vec3 tangent2 = b3Cross( normal, tangent1 ); + + DrawLine( scene, center1 + radius * tangent1, center2 + radius * tangent1, color ); + DrawLine( scene, center1 + radius * tangent2, center2 + radius * tangent2, color ); + DrawLine( scene, center1 - radius * tangent1, center2 - radius * tangent1, color ); + DrawLine( scene, center1 - radius * tangent2, center2 - radius * tangent2, color ); + + DrawArc( scene, center1, -tangent1, radius, tangent2, 180.0f, color ); + DrawArc( scene, center1, tangent2, radius, tangent1, 180.0f, color ); + DrawArc( scene, center2, tangent1, radius, tangent2, 180.0f, color ); + DrawArc( scene, center2, -tangent2, radius, tangent1, 180.0f, color ); + + DrawDisc( scene, center1, normal, radius, color ); + DrawDisc( scene, center2, normal, radius, color ); +} + +void DrawHull( Scene* scene, const b3Transform& transform, const b3Hull* hull, b3HexColor color, bool drawNormals ) +{ + const b3HullHalfEdge* edges = b3GetHullEdges( hull ); + const b3Vec3* points = b3GetHullPoints( hull ); + + for ( int edgeIndex = 0; edgeIndex < hull->edgeCount; edgeIndex += 2 ) + { + const b3HullHalfEdge* edge = edges + edgeIndex + 0; + const b3HullHalfEdge* twin = edges + edgeIndex + 1; + + b3Vec3 vertex1 = b3TransformPoint( transform, points[edge->origin] ); + b3Vec3 vertex2 = b3TransformPoint( transform, points[twin->origin] ); + + DrawLine( scene, vertex1, vertex2, color ); + } + + if (drawNormals) + { + const b3HullFace* faces = b3GetHullFaces( hull ); + const b3Plane* planes = b3GetHullPlanes( hull ); + int faceCount = hull->faceCount; + + for (int i = 0; i < faceCount; ++i) + { + uint8_t startEdgeIndex = faces[i].edge; + uint8_t edgeIndex = startEdgeIndex; + + int count = 0; + b3Vec3 p1 = b3Vec3_zero; + do + { + const b3HullHalfEdge* edge = edges + edgeIndex; + p1 += points[edge->origin]; + count += 1; + edgeIndex = edge->next; + } + while ( edgeIndex != startEdgeIndex ); + + assert( count > 0 ); + p1 = b3MulSV( 1.0f / count, p1 ); + b3Vec3 p2 = p1 + 0.2f * planes[i].normal; + + p1 = b3TransformPoint( transform, p1 ); + p2 = b3TransformPoint( transform, p2 ); + + DrawLine( scene, p1, p2, b3_colorWhite ); + DrawPoint( scene, p1, 6.0f, b3_colorLime ); + } + } +} + +void DrawEllipsoid( Scene* scene, const b3Transform& transform, b3Vec3 radius, b3HexColor color ) +{ + b3Vec3 ex = { 1.0f, 0.0, 0.0f }; + b3Vec3 ey = { 0.0f, 1.0, 0.0f }; + b3Vec3 ez = { 0.0f, 0.0, 1.0f }; + + for ( float angle = 0.0f; angle < 360.0f; angle += 5.0f ) + { + float alpha1 = B3_DEG_TO_RAD * ( angle + 0.0f ); + float alpha2 = B3_DEG_TO_RAD * ( angle + 5.0f ); + + // XY - Plane + { + b3Vec3 vertex1 = radius * ( b3Cos( alpha1 ) * ex + b3Sin( alpha1 ) * ey ); + b3Vec3 vertex2 = radius * ( b3Cos( alpha2 ) * ex + b3Sin( alpha2 ) * ey ); + vertex1 = b3TransformPoint( transform, vertex1 ); + vertex2 = b3TransformPoint( transform, vertex2 ); + DrawLine( scene, vertex1, vertex2, color ); + } + // XZ - Plane + { + b3Vec3 vertex1 = radius * ( b3Cos( alpha1 ) * ex + b3Sin( alpha1 ) * ez ); + b3Vec3 vertex2 = radius * ( b3Cos( alpha2 ) * ex + b3Sin( alpha2 ) * ez ); + vertex1 = b3TransformPoint( transform, vertex1 ); + vertex2 = b3TransformPoint( transform, vertex2 ); + DrawLine( scene, vertex1, vertex2, color ); + } + // YZ - Plane + { + b3Vec3 vertex1 = radius * ( b3Cos( alpha1 ) * ex + b3Sin( alpha1 ) * ez ); + b3Vec3 vertex2 = radius * ( b3Cos( alpha2 ) * ey + b3Sin( alpha2 ) * ez ); + vertex1 = b3TransformPoint( transform, vertex1 ); + vertex2 = b3TransformPoint( transform, vertex2 ); + DrawLine( scene, vertex1, vertex2, color ); + } + } +} + +void DrawTransform( Scene* scene, const b3Transform& transform, float length ) +{ + b3Vec3 p = transform.p; + b3Vec3 ax = b3RotateVector( transform.q, { 1.0f, 0.0f, 0.0f } ); + b3Vec3 ay = b3RotateVector( transform.q, { 0.0f, 1.0f, 0.0f } ); + b3Vec3 az = b3RotateVector( transform.q, { 0.0f, 0.0f, 1.0f } ); + + DrawLine( scene, p, p + length * ax, b3_colorRed ); + DrawLine( scene, p, p + length * ay, b3_colorGreen ); + DrawLine( scene, p, p + length * az, b3_colorBlue ); +} diff --git a/samples/renderer.h b/samples/renderer.h new file mode 100644 index 0000000..0185677 --- /dev/null +++ b/samples/renderer.h @@ -0,0 +1,44 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "utility.h" + +struct b3Capsule; +struct b3DebugShape; +struct b3Hull; +struct b3MeshData; +struct b3Sphere; + +struct Camera; +struct LineDraw; +struct PointDraw; +struct Scene; + +void* CreateDebugShape( const b3DebugShape* debugShape, void* context ); +void DestroyDebugShape( void* userShape, void* userContext ); + +void DrawText( int x, int y, b3HexColor color, const char* text ); +void DrawFormat( int x, int y, b3HexColor color, const char* format, ... ); +void DrawWorldString( Camera* camera, b3Vec3 point, b3HexColor color, const char* format, ... ); + +void DrawGrid( Scene* scene, int size ); +void DrawCross( Scene* scene, b3Vec3 center, float radius, b3HexColor color ); +void DrawSphere( Scene* scene, const b3Transform& transform, const b3Sphere& sphere, b3HexColor color ); +void DrawCapsule( Scene* scene, const b3Transform& transform, const b3Capsule& capsule, b3HexColor color ); +void DrawHull( Scene* scene, const b3Transform& transform, const b3Hull* hull, b3HexColor color, bool drawNormals ); +void DrawEllipsoid( Scene* scene, const b3Transform& transform, b3Vec3 radius, b3HexColor color ); +void DrawTransform( Scene* scene, const b3Transform& transform, float length ); + +// Debug points +PointDraw* CreateDebugPoints(); +void DestroyDebugPoints( PointDraw* draw ); +void AddPoint( PointDraw* draw, b3Vec3 point, float size, b3HexColor color ); +void FlushPoints( PointDraw* draw, Camera* camera ); + +// Debug lines +LineDraw* CreateDebugLines(); +void DestroyDebugLines( LineDraw* draw ); +void AddLine( LineDraw* draw, b3Vec3 point1, b3Vec3 point2, b3HexColor color ); +void FlushLines( LineDraw* draw, Camera* camera ); diff --git a/samples/sample.cpp b/samples/sample.cpp new file mode 100644 index 0000000..d07b691 --- /dev/null +++ b/samples/sample.cpp @@ -0,0 +1,1757 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include "sample.h" + +#include "camera.h" +#include "imgui.h" +#include "implot.h" +#include "jsmn.h" +#include "renderer.h" +#include "scene.h" +#include "shader.h" +#include "utils.h" + +// clang-format off +#include "glad/glad.h" +#include "GLFW/glfw3.h" +// clang-format on + +#include "human.h" + +#include "box3d/box3d.h" + +#include +#include + +// Load a file. You must free the character array. +static char* ReadFile( int& size, const char* filename ) +{ + FILE* file = fopen( filename, "rb" ); + if ( file == nullptr ) + { + return nullptr; + } + + fseek( file, 0, SEEK_END ); + size = static_cast( ftell( file ) ); + fseek( file, 0, SEEK_SET ); + + if ( size == 0 ) + { + fclose( file ); + return nullptr; + } + + char* data = (char*)malloc( size + 1 ); + fread( data, size, 1, file ); + fclose( file ); + data[size] = 0; + + return data; +} + +static const char* settingsFileName = "settings.ini"; + +void Arena::Create( int byteCount ) +{ + data = (char*)malloc( byteCount ); + capacity = byteCount; + index = 0; +} + +void Arena::Destroy() +{ + free( data ); + data = nullptr; + capacity = 0; + index = 0; +} + +void* Arena::Allocate( int byteCount ) +{ + if ( index + byteCount > capacity ) + { + assert( false ); + return nullptr; + } + + void* mem = data + index; + index += byteCount; + return mem; +} + +void Arena::Clear() +{ + index = 0; +} + +static bool DrawShapeCallback( void* userShape, b3Transform transform, b3HexColor color, void* context ) +{ + DebugShape* shape = (DebugShape*)userShape; + SampleContext* sampleContext = (SampleContext*)context; + float alpha = sampleContext->transparentDynamic && shape->bodyType != b3_staticBody ? 0.6f : 1.0f; + DrawDebugShape( sampleContext->scene, shape, transform, color, alpha ); + return true; +} + +static void DrawPointCallback( b3Vec3 point, float size, b3HexColor color, void* context ) +{ + SampleContext* sampleContext = (SampleContext*)context; + DrawPoint( sampleContext->scene, point, size, color ); +} + +static void DrawLineCallback( b3Vec3 vertex1, b3Vec3 vertex2, b3HexColor color, void* context ) +{ + SampleContext* sampleContext = (SampleContext*)context; + DrawLine( sampleContext->scene, vertex1, vertex2, color ); +} + +static void DrawBoundsCallback( b3AABB bounds, b3HexColor color, void* context ) +{ + SampleContext* sampleContext = (SampleContext*)context; + float extension = 0.0f; + DrawBounds( sampleContext->scene, bounds, extension, color ); +} + +static void DrawBoxCallback( b3Vec3 extents, b3Transform transform, b3HexColor color, void* context ) +{ + SampleContext* sampleContext = (SampleContext*)context; + DrawBox( sampleContext->scene, extents, transform, color ); +} + +static void DrawTransformCallback( b3Transform transform, void* context ) +{ + SampleContext* sampleContext = (SampleContext*)context; + DrawTransform( sampleContext->scene, transform, 0.25f ); +} + +static void DrawSceneTextCallback( b3Vec3 position, const char* text, b3HexColor color, void* context ) +{ + SampleContext* sampleContext = (SampleContext*)context; + DrawWorldString( &sampleContext->camera, position, color, text ); +} + +void SampleContext::Save() +{ + FILE* file = fopen( settingsFileName, "w" ); + fprintf( file, "{\n" ); + fprintf( file, " \"sampleIndex\": %d,\n", sampleIndex ); + fprintf( file, " \"drawShapes\": %s,\n", debugDraw.drawShapes ? "true" : "false" ); + fprintf( file, " \"drawJoints\": %s,\n", debugDraw.drawJoints ? "true" : "false" ); + fprintf( file, " \"drawContacts\": %s,\n", debugDraw.drawContacts ? "true" : "false" ); + fprintf( file, " \"drawImpulses\": %s,\n", debugDraw.drawContactForces ? "true" : "false" ); + fprintf( file, " \"drawBounds\": %s,\n", debugDraw.drawBounds ? "true" : "false" ); + fprintf( file, " \"drawCounters\": %s,\n", drawCounters ? "true" : "false" ); + fprintf( file, " \"drawProfile\": %s,\n", drawProfile ? "true" : "false" ); + fprintf( file, " \"transparent\": %s,\n", transparent ? "true" : "false" ); + fprintf( file, " \"enableSleep\": %s\n", enableSleep ? "true" : "false" ); + fprintf( file, " \"enableContinuous\": %s,\n", enableContinuous ? "true" : "false" ); + fprintf( file, "}\n" ); + fclose( file ); +} + +#define MAX_TOKENS 32 + +static int jsoneq( const char* json, jsmntok_t* tok, const char* s ) +{ + if ( tok->type == JSMN_STRING && (int)strlen( s ) == tok->end - tok->start && + strncmp( json + tok->start, s, tok->end - tok->start ) == 0 ) + { + return 0; + } + return -1; +} + +void SampleContext::Load() +{ + debugDraw = b3DefaultDebugDraw(); + debugDraw.DrawShapeFcn = DrawShapeCallback; + debugDraw.DrawSegmentFcn = DrawLineCallback; + debugDraw.DrawPointFcn = DrawPointCallback; + debugDraw.DrawBoundsFcn = DrawBoundsCallback; + debugDraw.DrawBoxFcn = DrawBoxCallback; + debugDraw.DrawTransformFcn = DrawTransformCallback; + debugDraw.DrawStringFcn = DrawSceneTextCallback; + debugDraw.context = this; + + recycleDistance = B3_CONTACT_RECYCLE_DISTANCE; + + int size = 0; + char* data = ReadFile( size, settingsFileName ); + if ( data == nullptr ) + { + return; + } + + jsmn_parser parser; + jsmntok_t tokens[MAX_TOKENS]; + + jsmn_init( &parser ); + + int tokenCount = jsmn_parse( &parser, data, size, tokens, MAX_TOKENS ); + char buffer[32]; + + for ( int i = 0; i < tokenCount; ++i ) + { + if ( jsoneq( data, &tokens[i], "sampleIndex" ) == 0 ) + { + int count = tokens[i + 1].end - tokens[i + 1].start; + assert( count < 32 ); + const char* s = data + tokens[i + 1].start; + strncpy( buffer, s, count ); + buffer[count] = 0; + char* dummy; + sampleIndex = (int)strtol( buffer, &dummy, 10 ); + + if ( sampleIndex < 0 ) + { + sampleIndex = 0; + } + else if ( sampleIndex >= SampleManager::sEntryCount ) + { + sampleIndex = SampleManager::sEntryCount - 1; + } + } + else if ( jsoneq( data, &tokens[i], "drawShapes" ) == 0 ) + { + const char* s = data + tokens[i + 1].start; + if ( strncmp( s, "true", 4 ) == 0 ) + { + debugDraw.drawShapes = true; + } + else if ( strncmp( s, "false", 5 ) == 0 ) + { + debugDraw.drawShapes = false; + } + } + } + + free( data ); +} + +Sample::Sample( SampleContext* context ) +{ + m_context = context; + m_window = context->window; + m_scene = m_context->scene; + m_camera = &m_context->camera; + + m_worldId = b3_nullWorldId; + + m_mouseBodyId = {}; + m_mouseJointId = {}; + m_mouseFraction = 0.0f; + m_mouseForceScale = 100.0f; + + m_textLine = 0; + m_textIncrement = 22; + m_stepCount = 0; + m_userMaterialId = 0; + + memset( m_profiles, 0, sizeof( m_profiles ) ); + m_currentProfileIndex = 0; + m_profileReadIndex = 0; + m_profileWriteIndex = 0; + + m_stepWhilePaused = true; + m_didStep = false; + + m_haveMouseLast = false; + m_mouseLast = { 0.0f, 0.0f }; + m_mouseDelta = { 0.0f, 0.0f }; + m_launchSpeedScale = 5.0f; + + g_randomSeed = RAND_SEED; + + b3Capacity capacity = {}; + CreateWorld( &capacity ); +} + +Sample::~Sample() +{ + b3DestroyWorld( m_worldId ); + + DestroySharedMeshes( m_context->scene ); +} + +void Sample::CreateWorld( b3Capacity* capacity ) +{ + if ( B3_IS_NON_NULL( m_worldId ) ) + { + b3DestroyWorld( m_worldId ); + } + + m_mouseBodyId = {}; + m_mouseJointId = {}; + m_mousePoint = {}; + + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.workerCount = m_context->workerCount; + worldDef.userTaskContext = this; + worldDef.enableSleep = m_context->enableSleep; + worldDef.createDebugShape = CreateDebugShape; + worldDef.destroyDebugShape = DestroyDebugShape; + worldDef.userDebugShapeContext = m_context; + worldDef.capacity = *capacity; + m_worldId = b3CreateWorld( &worldDef ); + + b3World_SetContactRecycleDistance( m_worldId, m_context->recycleDistance ); +} + +void Sample::Step() +{ + m_didStep = false; + + float timeStep = 0.0f; + if ( m_context->pause == false || m_context->singleStep > 0 ) + { + timeStep = m_context->hertz > 0.0f ? 1.0f / m_context->hertz : 0.0f; + m_context->singleStep = b3MaxInt( 0, m_context->singleStep - 1 ); + } + + if ( B3_IS_NON_NULL( m_mouseJointId ) && b3Joint_IsValid( m_mouseJointId ) == false ) + { + // The world or attached body was destroyed. + m_mouseJointId = {}; + + if ( B3_IS_NON_NULL( m_mouseBodyId ) ) + { + b3DestroyBody( m_mouseBodyId ); + m_mouseBodyId = {}; + } + } + + if ( B3_IS_NON_NULL( m_mouseBodyId ) && timeStep > 0.0f ) + { + b3Body_SetTargetTransform( m_mouseBodyId, { m_mousePoint, b3Quat_identity }, timeStep, true ); + } + + b3World_EnableSleeping( m_worldId, m_context->enableSleep ); + b3World_EnableWarmStarting( m_worldId, m_context->enableWarmStarting ); + b3World_EnableContinuous( m_worldId, m_context->enableContinuous ); + + if ( timeStep > 0.0f || m_stepWhilePaused ) + { + b3World_Step( m_worldId, timeStep, m_context->subStepCount ); + } + + // if (glfwGetKey( m_window, 'F' )) + //{ + // b3World_DumpShapeBounds( m_worldId, b3_staticBody ); + // } + + if ( timeStep > 0.0f ) + { + m_stepCount += 1; + m_didStep = true; + + if ( m_profileWriteIndex - m_profileReadIndex == m_profileCapacity ) + { + m_profileReadIndex += 1; + } + + m_currentProfileIndex = m_profileWriteIndex & ( m_profileCapacity - 1 ); + m_profiles[m_currentProfileIndex] = b3World_GetProfile( m_worldId ); + + m_profileWriteIndex += 1; + } + + m_triangleIndex = -1; + m_userMaterialId = 0; + + { + double screenX, screenY; + glfwGetCursorPos( m_window, &screenX, &screenY ); + PickRay pickRay = m_camera->BuildPickRay( (float)screenX, (float)screenY ); + + b3RayResult result = b3World_CastRayClosest( m_worldId, pickRay.origin, pickRay.translation, b3DefaultQueryFilter() ); + + if ( result.hit ) + { + b3ShapeType type = b3Shape_GetType( result.shapeId ); + if ( type == b3_meshShape ) + { + m_triangleIndex = result.triangleIndex; + } + + m_userMaterialId = result.userMaterialId; + } + } +} + +void Sample::Render() +{ + // Draw world + b3Vec3 position = m_context->camera.GetPosition(); + b3Vec3 r = { 1000.0f, 1000.0f, 1000.0f }; + b3AABB bounds = { position - r, position + r }; + m_context->debugDraw.drawingBounds = bounds; + + b3World_Draw( m_worldId, &m_context->debugDraw, B3_DEFAULT_MASK_BITS ); + + DrawTextLine( "camera radius = %g", m_camera->m_radius ); +} + +void Sample::ResetProfile() +{ + m_stepCount = 0; +} + +void Sample::UpdateUI() +{ + float fontSize = ImGui::GetFontSize(); + + if ( m_context->drawProfile ) + { + ImGui::SetNextWindowPos( { fontSize, 8.0f * fontSize }, ImGuiCond_FirstUseEver ); + ImGui::Begin( "Profile (ms)", nullptr, ImGuiWindowFlags_NoCollapse | ImGuiWindowFlags_AlwaysAutoResize ); + + const int count = static_cast( m_profileWriteIndex - m_profileReadIndex ); + + // Unroll ring buffer into per-field histories. + constexpr int kRowCount = 22; + float histories[kRowCount][m_profileCapacity]; + float totals[kRowCount] = {}; + for ( int i = 0; i < count; ++i ) + { + int idx = static_cast( ( m_profileReadIndex + i ) & ( m_profileCapacity - 1 ) ); + const b3Profile& p = m_profiles[idx]; + histories[0][i] = p.step; + histories[1][i] = p.pairs; + histories[2][i] = p.collide; + histories[3][i] = p.solve; + histories[4][i] = p.solverSetup; + histories[5][i] = p.constraints; + histories[6][i] = p.prepareConstraints; + histories[7][i] = p.integrateVelocities; + histories[8][i] = p.warmStart; + histories[9][i] = p.solveImpulses; + histories[10][i] = p.integratePositions; + histories[11][i] = p.relaxImpulses; + histories[12][i] = p.applyRestitution; + histories[13][i] = p.storeImpulses; + histories[14][i] = p.splitIslands; + histories[15][i] = p.transforms; + histories[16][i] = p.jointEvents; + histories[17][i] = p.hitEvents; + histories[18][i] = p.refit; + histories[19][i] = p.sleepIslands; + histories[20][i] = p.bullets; + histories[21][i] = p.sensors; + + totals[0] += p.step; + totals[1] += p.pairs; + totals[2] += p.collide; + totals[3] += p.solve; + totals[4] += p.solverSetup; + totals[5] += p.constraints; + totals[6] += p.prepareConstraints; + totals[7] += p.integrateVelocities; + totals[8] += p.warmStart; + totals[9] += p.solveImpulses; + totals[10] += p.integratePositions; + totals[11] += p.relaxImpulses; + totals[12] += p.applyRestitution; + totals[13] += p.storeImpulses; + totals[14] += p.splitIslands; + totals[15] += p.transforms; + totals[16] += p.jointEvents; + totals[17] += p.hitEvents; + totals[18] += p.refit; + totals[19] += p.sleepIslands; + totals[20] += p.bullets; + totals[21] += p.sensors; + } + + const b3Profile& cur = m_profiles[m_currentProfileIndex]; + const float now[kRowCount] = { + cur.step, + cur.pairs, + cur.collide, + cur.solve, + cur.solverSetup, + cur.constraints, + cur.prepareConstraints, + cur.integrateVelocities, + cur.warmStart, + cur.solveImpulses, + cur.integratePositions, + cur.relaxImpulses, + cur.applyRestitution, + cur.storeImpulses, + cur.splitIslands, + cur.transforms, + cur.jointEvents, + cur.hitEvents, + cur.refit, + cur.sleepIslands, + cur.bullets, + cur.sensors, + }; + + // Rolling average + float avg[kRowCount] = {}; + if ( count > 0 ) + { + float scale = 1.0f / count; + for ( int i = 0; i < kRowCount; ++i ) + { + avg[i] = scale * totals[i]; + } + } + + // Match Frame Time chart's first three colors so rows read with the line plot. + const ImU32 colorStep = IM_COL32( 102, 153, 255, 255 ); + const ImU32 colorCollide = IM_COL32( 255, 140, 51, 255 ); + const ImU32 colorSolve = IM_COL32( 102, 204, 102, 255 ); + const ImU32 colorDefault = IM_COL32( 220, 220, 220, 255 ); + + struct RowDef + { + const char* name; + int indent; + ImU32 color; + }; + const RowDef rows[kRowCount] = { + { "step", 0, colorStep }, { "pairs", 0, colorDefault }, { "collide", 0, colorCollide }, + { "solve", 0, colorSolve }, { "setup", 1, colorDefault }, { "constraints", 1, colorDefault }, + { "prepare", 2, colorDefault }, { "velocities", 2, colorDefault }, { "warm start", 2, colorDefault }, + { "bias", 2, colorDefault }, { "positions", 2, colorDefault }, { "relax", 2, colorDefault }, + { "restitution", 2, colorDefault }, { "store", 2, colorDefault }, { "split islands", 2, colorDefault }, + { "transforms", 1, colorDefault }, { "joint events", 1, colorDefault }, { "hit events", 1, colorDefault }, + { "refit BVH", 1, colorDefault }, { "sleep", 1, colorDefault }, { "bullets", 1, colorDefault }, + { "sensors", 0, colorDefault }, + }; + + // Derive parent/child links from the indent levels so we can collapse subtrees. + int parents[kRowCount]; + bool hasChildren[kRowCount] = {}; + { + int stack[8]; + int stackSize = 0; + for ( int i = 0; i < kRowCount; ++i ) + { + while ( stackSize > 0 && rows[stack[stackSize - 1]].indent >= rows[i].indent ) + { + --stackSize; + } + parents[i] = stackSize > 0 ? stack[stackSize - 1] : -1; + stack[stackSize++] = i; + if ( parents[i] >= 0 ) + { + hasChildren[parents[i]] = true; + } + } + } + + static bool s_rowOpen[kRowCount]; + static bool s_showPlots = false; + + // Bars are drawn relative to the step row so the proportions are visually consistent. + const float stepNow = b3MaxFloat( cur.step, 0.001f ); + + if ( ImGui::Button( "Reset" ) ) + { + ResetProfile(); + } + ImGui::SameLine(); + ImGui::Checkbox( "Show plots", &s_showPlots ); + + const ImGuiTableFlags tableFlags = ImGuiTableFlags_BordersInnerV | ImGuiTableFlags_RowBg | ImGuiTableFlags_SizingFixedFit; + + const int colCount = s_showPlots ? 6 : 5; + if ( ImGui::BeginTable( "profile", colCount, tableFlags ) ) + { + ImGui::TableSetupColumn( "section", ImGuiTableColumnFlags_WidthFixed, 8.0f * fontSize ); + ImGui::TableSetupColumn( "now", ImGuiTableColumnFlags_WidthFixed, 3.0f * fontSize ); + ImGui::TableSetupColumn( "avg", ImGuiTableColumnFlags_WidthFixed, 3.0f * fontSize ); + ImGui::TableSetupColumn( "max", ImGuiTableColumnFlags_WidthFixed, 3.0f * fontSize ); + ImGui::TableSetupColumn( "% step", ImGuiTableColumnFlags_WidthFixed, 8.0f * fontSize ); + if ( s_showPlots ) + { + ImGui::TableSetupColumn( "history", ImGuiTableColumnFlags_WidthFixed, 16.0f * fontSize ); + } + ImGui::TableHeadersRow(); + + const float rowHeight = 1.5f * fontSize; + + for ( int r = 0; r < kRowCount; ++r ) + { + bool visible = true; + for ( int p = parents[r]; p >= 0; p = parents[p] ) + { + if ( !s_rowOpen[p] ) + { + visible = false; + break; + } + } + if ( !visible ) + { + continue; + } + + const RowDef& d = rows[r]; + const float* hist = histories[r]; + + float rollingMax = 0.0f; + for ( int i = 0; i < count; ++i ) + { + rollingMax = b3MaxFloat( rollingMax, hist[i] ); + } + + ImGui::TableNextRow(); + + ImGui::TableNextColumn(); + if ( d.indent > 0 ) + { + ImGui::Indent( d.indent * fontSize ); + } + if ( hasChildren[r] ) + { + ImGuiTreeNodeFlags flags = ImGuiTreeNodeFlags_OpenOnArrow | ImGuiTreeNodeFlags_OpenOnDoubleClick | + ImGuiTreeNodeFlags_NoTreePushOnOpen; + ImGui::PushStyleColor( ImGuiCol_Text, d.color ); + s_rowOpen[r] = ImGui::TreeNodeEx( d.name, flags ); + ImGui::PopStyleColor(); + } + else + { + float leafIndent = ImGui::GetTreeNodeToLabelSpacing(); + ImGui::Indent( leafIndent ); + ImGui::PushStyleColor( ImGuiCol_Text, d.color ); + ImGui::TextUnformatted( d.name ); + ImGui::PopStyleColor(); + ImGui::Unindent( leafIndent ); + } + if ( d.indent > 0 ) + { + ImGui::Unindent( d.indent * fontSize ); + } + + ImGui::TableNextColumn(); + ImGui::Text( "%6.2f", now[r] ); + ImGui::TableNextColumn(); + ImGui::Text( "%6.2f", avg[r] ); + ImGui::TableNextColumn(); + ImGui::Text( "%6.2f", rollingMax ); + + ImGui::TableNextColumn(); + float frac = b3ClampFloat( now[r] / stepNow, 0.0f, 1.0f ); + ImGui::PushStyleColor( ImGuiCol_PlotHistogram, d.color ); + ImGui::ProgressBar( frac, ImVec2( -FLT_MIN, 0.0f ), "" ); + ImGui::PopStyleColor(); + + if ( s_showPlots ) + { + ImGui::TableNextColumn(); + if ( count > 1 ) + { + char id[16]; + snprintf( id, sizeof( id ), "##h%d", r ); + ImGui::PushStyleColor( ImGuiCol_PlotLines, d.color ); + ImGui::PlotLines( id, hist, count, 0, nullptr, 0.0f, rollingMax * 1.05f + 0.001f, + ImVec2( -FLT_MIN, rowHeight ) ); + ImGui::PopStyleColor(); + } + } + } + ImGui::EndTable(); + } + + ImGui::End(); + } + + if ( m_context->drawCounters ) + { + b3Counters s = b3World_GetCounters( m_worldId ); + constexpr int colorCount = sizeof( s.colorCounts ) / sizeof( s.colorCounts[0] ); + const int overflowIndex = colorCount - 1; + constexpr int manifoldBucketCount = sizeof( s.manifoldCounts ) / sizeof( s.manifoldCounts[0] ); + + // Bars are scaled to the largest non-overflow color so the distribution shape reads clearly; + // overflow gets its own bar against the same scale, with a red tint to flag coupling problems. + int totalCount = 0; + int maxCount = 1; + for ( int i = 0; i < colorCount; ++i ) + { + totalCount += s.colorCounts[i]; + if ( i != overflowIndex && s.colorCounts[i] > maxCount ) + { + maxCount = s.colorCounts[i]; + } + } + + int totalManifolds = 0; + int maxManifolds = 1; + for ( int i = 0; i < manifoldBucketCount; ++i ) + { + totalManifolds += s.manifoldCounts[i]; + if ( s.manifoldCounts[i] > maxManifolds ) + { + maxManifolds = s.manifoldCounts[i]; + } + } + + ImGui::SetNextWindowPos( { fontSize, 8.0f * fontSize }, ImGuiCond_FirstUseEver ); + ImGui::Begin( "Counters", nullptr, ImGuiWindowFlags_NoCollapse | ImGuiWindowFlags_AlwaysAutoResize ); + + ImGui::Text( "bodies/shapes/contacts/joints = %d/%d/%d/%d", s.bodyCount, s.shapeCount, s.contactCount, s.jointCount ); + { + float frac = s.awakeContactCount > 0 + ? b3ClampFloat( (float)s.recycledContactCount / (float)s.awakeContactCount, 0.0f, 1.0f ) + : 0.0f; + + char overlay[32]; + snprintf( overlay, sizeof( overlay ), "%d / %d", s.recycledContactCount, s.awakeContactCount ); + + ImGui::TextUnformatted( "recycled contacts" ); + ImGui::SameLine(); + ImGui::ProgressBar( frac, ImVec2( -FLT_MIN, 0.0f ), overlay ); + } + ImGui::Text( "islands/tasks = %d/%d", s.islandCount, s.taskCount ); + ImGui::Text( "tree height static/movable = %d/%d", s.staticTreeHeight, s.treeHeight ); + ImGui::Text( "sat call/hit = %d/%d", s.satCallCount, s.satCacheHitCount ); + ImGui::Text( "stack allocator size = %d K", s.stackUsed / 1024 ); + ImGui::Text( "total allocation = %d K", s.byteCount / 1024 ); + + ImGui::Separator(); + b3Capacity c = b3World_GetMaxCapacity( m_worldId ); + ImGui::Text( "max capacities" ); + ImGui::BulletText( "static shapes/bodies = %d/%d", c.staticShapeCount, c.staticBodyCount ); + ImGui::BulletText( "dynamic shapes/bodies = %d/%d", c.dynamicShapeCount, c.dynamicBodyCount ); + ImGui::BulletText( "contacts = %d", c.contactCount ); + + ImGui::Separator(); + ImGui::Text( "%d constraints across %d colors", totalCount, colorCount ); + + const ImGuiTableFlags tableFlags = ImGuiTableFlags_BordersInnerV | ImGuiTableFlags_RowBg | ImGuiTableFlags_SizingFixedFit; + if ( ImGui::BeginTable( "graphColors", 3, tableFlags ) ) + { + ImGui::TableSetupColumn( "color", ImGuiTableColumnFlags_WidthFixed, 3.5f * fontSize ); + ImGui::TableSetupColumn( "count", ImGuiTableColumnFlags_WidthFixed, 5.0f * fontSize ); + ImGui::TableSetupColumn( "share", ImGuiTableColumnFlags_WidthFixed, 16.0f * fontSize ); + ImGui::TableHeadersRow(); + + const float invMax = 1.0f / static_cast( maxCount ); + + for ( int i = 0; i < colorCount; ++i ) + { + int count = s.colorCounts[i]; + bool isOverflow = ( i == overflowIndex ); + + // Skip empty slots, but always show overflow — a non-zero overflow row is the signal we care about. + if ( count == 0 && !isOverflow ) + { + continue; + } + + uint32_t hex = static_cast( b3GetGraphColor( i ) ); + ImU32 swatch = IM_COL32( ( hex >> 16 ) & 0xFF, ( hex >> 8 ) & 0xFF, hex & 0xFF, 255 ); + ImU32 barColor = isOverflow ? IM_COL32( 220, 60, 60, 255 ) : swatch; + + ImGui::TableNextRow(); + + ImGui::TableNextColumn(); + if ( isOverflow ) + { + ImGui::PushStyleColor( ImGuiCol_Text, IM_COL32( 220, 60, 60, 255 ) ); + ImGui::TextUnformatted( "over" ); + ImGui::PopStyleColor(); + } + else + { + ImGui::PushStyleColor( ImGuiCol_Text, swatch ); + ImGui::Text( "%d", i ); + ImGui::PopStyleColor(); + } + + ImGui::TableNextColumn(); + ImGui::Text( "%d", count ); + + ImGui::TableNextColumn(); + float frac = b3ClampFloat( count * invMax, 0.0f, 1.0f ); + ImGui::PushStyleColor( ImGuiCol_PlotHistogram, barColor ); + ImGui::ProgressBar( frac, ImVec2( -FLT_MIN, 0.0f ), "" ); + ImGui::PopStyleColor(); + } + ImGui::EndTable(); + } + + ImGui::Separator(); + ImGui::Text( "%d manifolds across %d buckets", totalManifolds, manifoldBucketCount ); + if ( ImGui::BeginTable( "manifolds", 3, tableFlags ) ) + { + ImGui::TableSetupColumn( "points", ImGuiTableColumnFlags_WidthFixed, 3.5f * fontSize ); + ImGui::TableSetupColumn( "count", ImGuiTableColumnFlags_WidthFixed, 5.0f * fontSize ); + ImGui::TableSetupColumn( "share", ImGuiTableColumnFlags_WidthFixed, 16.0f * fontSize ); + ImGui::TableHeadersRow(); + + const float invMax = 1.0f / static_cast( maxManifolds ); + + for ( int i = 0; i < manifoldBucketCount; ++i ) + { + int count = s.manifoldCounts[i]; + if ( count == 0 ) + { + continue; + } + + ImGui::TableNextRow(); + + ImGui::TableNextColumn(); + ImGui::Text( "%d", i + 1 ); + + ImGui::TableNextColumn(); + ImGui::Text( "%d", count ); + + ImGui::TableNextColumn(); + float frac = b3ClampFloat( count * invMax, 0.0f, 1.0f ); + ImGui::ProgressBar( frac, ImVec2( -FLT_MIN, 0.0f ), "" ); + } + ImGui::EndTable(); + } + + ImGui::End(); + } + + if ( m_context->frameTime ) + { + float frameTimeHeight = 400.0f; + float frameTimeWidth = 800.0f; + + ImGui::SetNextWindowPos( { 30.0f, 30.0f }, ImGuiCond_FirstUseEver ); + ImGui::SetNextWindowSize( { frameTimeWidth, frameTimeHeight }, ImGuiCond_FirstUseEver ); + + ImGui::Begin( "Frame Time", nullptr, ImGuiWindowFlags_NoCollapse ); + + ImGui::PushItemWidth( ImGui::GetWindowWidth() - 20.0f ); + + float maxValue = 0.0f; + float times[m_profileCapacity]; + float stepTimes[m_profileCapacity]; + float collideTimes[m_profileCapacity]; + float solveTimes[m_profileCapacity]; + int count = m_profileWriteIndex - m_profileReadIndex; + for ( int i = 0; i < count; ++i ) + { + int index = ( m_profileReadIndex + i ) & ( m_profileCapacity - 1 ); + times[i] = i / 60.0f; + stepTimes[i] = m_profiles[index].step; + collideTimes[i] = m_profiles[index].collide; + solveTimes[i] = m_profiles[index].solve; + maxValue = b3MaxFloat( stepTimes[i], maxValue ); + } + + // This is the pixel size, not the range. + ImVec2 plotSize = { -1, 22.0f * ImGui::GetTextLineHeight() }; + if ( ImPlot::BeginPlot( "Profile", plotSize, ImPlotFlags_NoTitle ) ) + { + ImPlot::SetupAxes( "t", "ms" ); + ImPlot::SetupAxes( "t", "ms", 0, ImPlotAxisFlags_AutoFit | ImPlotAxisFlags_RangeFit ); + ImPlot::SetupAxesLimits( 0, m_profileCapacity / 60.0, 0.0, maxValue ); + ImPlot::PlotLine( "step", times, stepTimes, count ); + ImPlot::PlotLine( "collide", times, collideTimes, count ); + ImPlot::PlotLine( "solve", times, solveTimes, count ); + ImPlot::EndPlot(); + } + + ImGui::PopItemWidth(); + ImGui::End(); + } +} + +void Sample::MouseDown( b3Vec2 p, int button, int modifiers ) +{ + if ( modifiers == 0 ) + { + PickRay pickRay = m_camera->BuildPickRay( p.x, p.y ); + + b3RayResult result = b3World_CastRayClosest( m_worldId, pickRay.origin, pickRay.translation, b3DefaultQueryFilter() ); + + if ( result.hit ) + { + m_mousePoint = result.point; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_kinematicBody; + bodyDef.position = m_mousePoint; + bodyDef.enableSleep = false; + m_mouseBodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3BodyId bodyId = b3Shape_GetBody( result.shapeId ); + + b3MotorJointDef jointDef = b3DefaultMotorJointDef(); + jointDef.base.bodyIdA = m_mouseBodyId; + jointDef.base.bodyIdB = bodyId; + jointDef.base.localFrameB.p = b3Body_GetLocalPoint( bodyId, result.point ); + jointDef.linearHertz = 7.5f; + jointDef.linearDampingRatio = 1.0f; + + b3MassData massData = b3Body_GetMassData( bodyId ); + float g = b3Length( b3World_GetGravity( m_worldId ) ); + float mg = massData.mass * g; + jointDef.maxSpringForce = m_mouseForceScale * mg; + + if ( massData.mass > 0.0f ) + { + // This acts like angular friction + float trace = massData.inertia.cx.x + massData.inertia.cy.y + massData.inertia.cz.z; + float lever = sqrtf( trace / ( 3.0f * massData.mass ) ); + jointDef.maxVelocityTorque = 0.5f * lever * mg; + } + + m_mouseJointId = b3CreateMotorJoint( m_worldId, &jointDef ); + + b3Body_SetAwake( bodyId, true ); + + m_mouseFraction = result.fraction; + } + } + else if ( modifiers & GLFW_MOD_SHIFT ) + { + PickRay pickRay = m_camera->BuildPickRay( p.x, p.y ); + b3Vec3 direction = b3Normalize( pickRay.translation ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + if ( modifiers & GLFW_MOD_CONTROL ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = pickRay.origin + 2.0f * direction; + bodyDef.linearVelocity = ( 10.0f * m_launchSpeedScale ) * direction; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3Hull* hull = b3CreateCylinder( 2.0f, 0.15f, 0.0f, 6 ); + b3CreateHullShape( bodyId, &shapeDef, hull ); + b3DestroyHull( hull ); + } + else if ( modifiers & GLFW_MOD_ALT ) + { + b3Vec3 position = pickRay.origin + 2.0f * direction; + Human human = {}; + CreateHuman( &human, m_worldId, position, 1.0f, 1.0f, 1.0f, 0, nullptr, true ); + Human_SetVelocity( &human, ( 5.0f * m_launchSpeedScale ) * direction ); + } + else + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = pickRay.origin + 2.0f * direction; + bodyDef.linearVelocity = ( 20.0f * m_launchSpeedScale ) * direction; + bodyDef.isBullet = true; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3Sphere sphere = { b3Vec3_zero, 0.25f }; + shapeDef.density *= 4.0f; + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + } + } +} + +void Sample::MouseUp( b3Vec2 p, int button ) +{ + if ( b3Joint_IsValid( m_mouseJointId ) ) + { + b3DestroyJoint( m_mouseJointId, true ); + } + + if ( b3Body_IsValid( m_mouseBodyId ) ) + { + b3DestroyBody( m_mouseBodyId ); + } + + m_mouseJointId = b3_nullJointId; + m_mouseBodyId = b3_nullBodyId; + m_mouseFraction = 0.0f; +} + +void Sample::MouseMove( b3Vec2 p ) +{ + if ( m_camera->m_thirdPerson ) + { + if ( m_haveMouseLast == false ) + { + m_mouseLast = p; + m_haveMouseLast = true; + } + else + { + m_mouseDelta = { p.x - m_mouseLast.x, p.y - m_mouseLast.y }; + // printf( "mdx = %g, mdy = %g\n", m_mouseDelta.x, m_mouseDelta.y ); + m_mouseLast = p; + } + + const float sensitivity = 0.1f; + m_camera->m_yaw -= 2.0f * sensitivity * m_mouseDelta.x; + m_camera->m_pitch += sensitivity * m_mouseDelta.y; + m_camera->m_pitch = b3ClampFloat( m_camera->m_pitch, -85.0f, 85.0f ); + } + + PickRay pickRay = m_camera->BuildPickRay( p.x, p.y ); + if ( B3_IS_NON_NULL( m_mouseJointId ) ) + { + m_mousePoint = pickRay.origin + m_mouseFraction * pickRay.translation; + // b3Transform localFrameA = { target, b3Quat_identity }; + // b3Joint_SetLocalFrameA( m_mouseJointId, localFrameA ); + } +} + +void Sample::DrawTextLine( const char* text, ... ) +{ + va_list args; + va_start( args, text ); + char buffer[512]; + vsnprintf( buffer, sizeof( buffer ), text, args ); + DrawText( 5, m_textLine, b3_colorWhite, buffer ); + va_end( args ); + m_textLine += m_textIncrement; +} + +SampleEntry SampleManager::sEntries[MAX_SAMPLES]; +int SampleManager::sEntryCount = 0; + +int SampleManager::Register( const char* category, const char* name, SampleCreateFcn* fcn ) +{ + int index = sEntryCount; + if ( index < MAX_SAMPLES ) + { + sEntries[index] = { category, name, fcn }; + sEntryCount += 1; + return index; + } + + return -1; +} + +static int CompareSamples( const void* a, const void* b ) +{ + SampleEntry* entryA = (SampleEntry*)a; + SampleEntry* entryB = (SampleEntry*)b; + + int result = strcmp( entryA->Category, entryB->Category ); + if ( result == 0 ) + { + result = strcmp( entryA->Name, entryB->Name ); + } + + return result; +} + +void SampleManager::Startup( GLFWwindow* window, int width, int height, int bufferWidth, int bufferHeight ) +{ + m_context.Load(); + + m_context.workerCount = b3MinInt( 8, GetNumberOfCores() / 2 ); + + // todo_erin testing + // m_context.settings.workerCount = 1; + + m_context.arena.Create( 100 * 1024 * 1024 ); + m_context.window = window; + m_context.camera.Resize( width, height ); + + assert( bufferWidth > 0 && bufferHeight > 0 ); + m_context.camera.m_bufferWidth = bufferWidth; + m_context.camera.m_bufferHeight = bufferHeight; + m_context.scene = CreateScene( &m_context.camera, m_context.arena ); + + if ( m_context.scene == nullptr ) + { + fprintf( stderr, "Failed to create scene\n" ); + glfwTerminate(); + } + + qsort( sEntries, sEntryCount, sizeof( SampleEntry ), CompareSamples ); + + m_sample = sEntries[m_context.sampleIndex].CreateFcn( &m_context ); +} + +void SampleManager::Step() +{ + const SampleEntry* entry = sEntries + m_context.sampleIndex; + DrawFormat( 5, m_sample->m_textIncrement, b3_colorCyan, "%s : %s", entry->Category, entry->Name ); + m_sample->m_textLine = 2 * m_sample->m_textIncrement; + + if ( m_context.pause ) + { + m_sample->DrawTextLine( "****PAUSED****" ); + } + + m_sample->Step(); +} + +void SampleManager::Resize( int width, int height, int bufferWidth, int bufferHeight ) +{ + m_context.minimized = false; + if ( width == 0 || height == 0 || bufferWidth == 0 || bufferHeight == 0 ) + { + m_context.minimized = true; + return; + } + + m_context.camera.Resize( width, height ); + m_context.camera.m_bufferWidth = bufferWidth; + m_context.camera.m_bufferHeight = bufferHeight; + ResizeScene( m_context.scene, &m_context.camera ); +} + +void SampleManager::Render( GLFWwindow* window, float elapsedTime ) +{ + RenderScene( window, elapsedTime ); + UpdateUI( window ); +} + +void SampleManager::Shutdown( GLFWwindow* window ) +{ + // Must destroy sample first because it will destroy debug shapes. + delete m_sample; + m_sample = nullptr; + + m_context.Save(); + + DestroyScene( m_context.scene ); + m_context.scene = nullptr; + + m_context.arena.Destroy(); +} + +void SampleManager::Keyboard( int key, int action, int modifiers ) +{ + if ( action == GLFW_PRESS ) + { + switch ( key ) + { + case GLFW_KEY_ESCAPE: + glfwSetWindowShouldClose( m_context.window, GL_TRUE ); + break; + + case GLFW_KEY_TAB: + m_showMenu = !m_showMenu; + break; + + case GLFW_KEY_O: + if ( modifiers & GLFW_MOD_SHIFT ) + { + m_context.singleStep += 5; + } + else + { + m_context.singleStep += 1; + } + break; + + case GLFW_KEY_P: + m_context.pause = !m_context.pause; + break; + + case GLFW_KEY_R: + m_context.restart = true; + CreateSample(); + break; + + case GLFW_KEY_LEFT_BRACKET: + m_context.sampleIndex = b3MaxInt( 0, m_context.sampleIndex - 1 ); + m_context.restart = false; + CreateSample(); + break; + + case GLFW_KEY_RIGHT_BRACKET: + m_context.sampleIndex = b3MinInt( sEntryCount - 1, m_context.sampleIndex + 1 ); + m_context.restart = false; + CreateSample(); + break; + + default: + m_sample->Keyboard( key, action, modifiers ); + break; + } + } +} + +void SampleManager::CreateSample() +{ + B3_ASSERT( m_sample ); + + if (m_context.restart == false) + { + EnableGrid( m_context.scene, false ); + } + + delete m_sample; + m_sample = sEntries[m_context.sampleIndex].CreateFcn( &m_context ); +} + +void SampleManager::RenderScene( GLFWwindow* window, float elapsedTime ) +{ + // Update camera + m_context.camera.Update( window, elapsedTime ); + + m_sample->Render(); + // CheckOpenGL(); + + // Actually render + DrawScene( m_context.scene, &m_context.camera ); + // CheckOpenGL(); +} + +void SampleManager::UpdateUI( GLFWwindow* window ) +{ + if ( m_showMenu == false ) + { + return; + } + + int maxWorkers = B3_MAX_WORKERS; + + int savedTestIndex = m_context.sampleIndex; + float menuWidth = 220.0f; + b3WorldId worldId = m_sample->m_worldId; + + ImGui::SetNextWindowPos( { m_context.camera.m_width - menuWidth - 10.0f, 10.0f } ); + ImGui::SetNextWindowSize( { menuWidth, m_context.camera.m_height - 20.0f } ); + + ImGui::Begin( "Tools", &m_showMenu, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoCollapse ); + + if ( ImGui::BeginTabBar( "ControlTabs", ImGuiTabBarFlags_None ) ) + { + if ( ImGui::BeginTabItem( "Controls" ) ) + { + ImGui::SliderInt( "Sub-steps", &m_context.subStepCount, 1, 50 ); + ImGui::SliderFloat( "Hertz", &m_context.hertz, 5.0f, 2000.0f, "%.0f hz" ); + + if ( ImGui::SliderInt( "Workers", &m_context.workerCount, 1, maxWorkers ) ) + { + m_context.workerCount = b3ClampInt( m_context.workerCount, 1, maxWorkers ); + m_context.restart = true; + CreateSample(); + } + + ImGui::Separator(); + + ImGui::Checkbox( "Sleep", &m_context.enableSleep ); + ImGui::Checkbox( "Warm Starting", &m_context.enableWarmStarting ); + ImGui::Checkbox( "Continuous", &m_context.enableContinuous ); + + ImGui::PushItemWidth( 100.0f ); + float recyclingCentimeters = 100.0f * m_context.recycleDistance; + if ( ImGui::SliderFloat( "Recycle", &recyclingCentimeters, 0.0f, 10.0f, "%.1f cm" ) ) + { + m_context.recycleDistance = 0.01f * recyclingCentimeters; + b3World_SetContactRecycleDistance( worldId, m_context.recycleDistance ); + } + ImGui::PopItemWidth(); + + ImGui::Separator(); + + ImGui::Checkbox( "Shapes", &m_context.debugDraw.drawShapes ); + ImGui::Checkbox( "Transparent", &m_context.transparentDynamic ); + ImGui::Checkbox( "Joints", &m_context.debugDraw.drawJoints ); + ImGui::Checkbox( "Joint Extras", &m_context.debugDraw.drawJointExtras ); + ImGui::Checkbox( "Bounds", &m_context.debugDraw.drawBounds ); + + ImGui::Separator(); + + ImGui::Checkbox( "Contact Points", &m_context.debugDraw.drawContacts ); + ImGui::RadioButton( "Anchor A", &m_context.debugDraw.drawAnchorA, 1 ); + ImGui::SameLine(); + ImGui::RadioButton( "Anchor B", &m_context.debugDraw.drawAnchorA, 0 ); + ImGui::Checkbox( "Contact Normals", &m_context.debugDraw.drawContactNormals ); + ImGui::Checkbox( "Contact Forces", &m_context.debugDraw.drawContactForces ); + ImGui::Checkbox( "Contact Features", &m_context.debugDraw.drawContactFeatures ); + ImGui::Checkbox( "Friction Forces", &m_context.debugDraw.drawFrictionForces ); + + ImGui::Separator(); + + ImGui::Checkbox( "Mass", &m_context.debugDraw.drawMass ); + ImGui::Checkbox( "Body Names", &m_context.debugDraw.drawBodyNames ); + ImGui::Checkbox( "Graph Colors", &m_context.debugDraw.drawGraphColors ); + ImGui::Checkbox( "Draw Islands", &m_context.debugDraw.drawIslands ); + ImGui::Checkbox( "Counters", &m_context.drawCounters ); + ImGui::Checkbox( "Profile", &m_context.drawProfile ); + ImGui::Checkbox( "Frame Time", &m_context.frameTime ); + + ImGui::PushItemWidth( 80.0f ); + ImGui::InputFloat( "Joint Scale", &m_context.debugDraw.jointScale ); + ImGui::InputFloat( "Force Scale", &m_context.debugDraw.forceScale ); + ImGui::PopItemWidth(); + + ImGui::Separator(); + + bool useSSAO = IsSSAOEnabled( m_context.scene ); + if ( ImGui::Checkbox( "SSAO", &useSSAO ) ) + { + EnableSSAO( m_context.scene, useSSAO ); + } + + bool useShadows = AreShadowsEnabled( m_context.scene ); + if ( ImGui::Checkbox( "Shadows", &useShadows ) ) + { + EnableShadows( m_context.scene, useShadows ); + } + + SceneAOSettings* ao = GetAOSettings( m_context.scene ); + ImGui::SliderFloat( "AO Radius", &ao->radius, 0.0f, 5.0f ); + ImGui::SliderFloat( "AO Bias", &ao->bias, 0.0f, 0.5f ); + ImGui::SliderFloat( "AO Min", &ao->minScale, 0.0f, 1.0f ); + ImGui::SliderFloat( "AO Power", &ao->power, 0.5f, 8.0f ); + ImGui::SliderFloat( "AO Direct", &ao->direct, 0.0f, 1.0f ); + ImGui::Checkbox( "AO Only", &ao->aoOnly ); + + ImGui::Separator(); + + if ( ImGui::Button( "Pause", ImVec2( -1, 0 ) ) ) + { + m_context.pause = !m_context.pause; + } + + if ( ImGui::Button( "Restart", ImVec2( -1, 0 ) ) ) + { + m_context.restart = true; + CreateSample(); + } + + if ( ImGui::Button( "Dump", ImVec2( -1, 0 ) ) ) + { + b3World_Dump( m_sample->m_worldId ); + } + + if ( ImGui::Button( "Dump Awake", ImVec2( -1, 0 ) ) ) + { + b3World_DumpAwake( m_sample->m_worldId ); + } + + if ( ImGui::Button( "Quit", ImVec2( -1, 0 ) ) ) + { + glfwSetWindowShouldClose( window, GL_TRUE ); + } + + ImGui::EndTabItem(); + } + + ImGuiTreeNodeFlags leafNodeFlags = ImGuiTreeNodeFlags_OpenOnArrow | ImGuiTreeNodeFlags_OpenOnDoubleClick; + leafNodeFlags |= ImGuiTreeNodeFlags_Leaf | ImGuiTreeNodeFlags_NoTreePushOnOpen; + + ImGuiTreeNodeFlags nodeFlags = ImGuiTreeNodeFlags_OpenOnArrow | ImGuiTreeNodeFlags_OpenOnDoubleClick; + + if ( ImGui::BeginTabItem( "Tests" ) ) + { + const char* category = sEntries[0].Category; + int i = 0; + while ( i < sEntryCount ) + { + bool categorySelected = strcmp( category, sEntries[m_context.sampleIndex].Category ) == 0; + ImGuiTreeNodeFlags nodeSelectionFlags = categorySelected ? ImGuiTreeNodeFlags_Selected : 0; + bool nodeOpen = ImGui::TreeNodeEx( category, nodeFlags | nodeSelectionFlags ); + + if ( nodeOpen ) + { + while ( i < sEntryCount && strcmp( category, sEntries[i].Category ) == 0 ) + { + ImGuiTreeNodeFlags selectionFlags = 0; + if ( m_context.sampleIndex == i ) + { + selectionFlags = ImGuiTreeNodeFlags_Selected; + } + ImGui::TreeNodeEx( (void*)(intptr_t)i, leafNodeFlags | selectionFlags, "%s", sEntries[i].Name ); + if ( ImGui::IsItemClicked() ) + { + m_context.sampleIndex = i; + } + ++i; + } + ImGui::TreePop(); + } + else + { + while ( i < sEntryCount && strcmp( category, sEntries[i].Category ) == 0 ) + { + ++i; + } + } + + if ( i < sEntryCount ) + { + category = sEntries[i].Category; + } + } + ImGui::EndTabItem(); + } + ImGui::EndTabBar(); + } + + ImGui::End(); + + m_sample->UpdateUI(); + + // todo wrong place for this + if ( m_context.sampleIndex != savedTestIndex ) + { + m_context.restart = false; + CreateSample(); + } +} + +void Sample::ToggleThirdPerson() +{ + if ( m_camera->m_thirdPerson ) + { + glfwSetInputMode( m_window, GLFW_CURSOR, GLFW_CURSOR_NORMAL ); + m_camera->m_thirdPerson = false; + } + else + { + glfwSetInputMode( m_window, GLFW_CURSOR, GLFW_CURSOR_DISABLED ); + m_camera->m_thirdPerson = true; + } +} + +float CastClosestCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t materialId, int triangleIndex, + int childIndex, void* context ) +{ + CastClosestContext* rayContext = (CastClosestContext*)context; + rayContext->shapeId = shapeId; + rayContext->point = point; + rayContext->normal = normal; + rayContext->fraction = fraction; + rayContext->materialId = materialId; + rayContext->childIndex = childIndex; + rayContext->triangleIndex = triangleIndex; + rayContext->hit = true; + return fraction; +} + +static bool MoverFilterCallback( b3ShapeId shapeId, void* context ) +{ + CharacterMover* self = (CharacterMover*)context; + for ( int i = 0; i < self->m_ignoreCount; ++i ) + { + if ( B3_ID_EQUALS( shapeId, self->m_ignoreShapeIds[i] ) ) + { + return false; + } + } + + return true; +} + +void CharacterMover::Initialize( Sample* sample, b3Vec3 position ) +{ + m_sample = sample; + m_transform.p = position; + m_transform.q = b3Quat_identity; + m_velocity = { 0.0f, 0.0f, 0.0f }; + m_capsule = { { 0.0f, -0.5f, 0.0f }, { 0.0f, 0.5f, 0.0f }, 0.3f }; + + m_planeCount = 0; + m_totalIterations = 0; + m_pogoVelocity = 0.0f; + m_onGround = false; + m_sprint = false; + + m_ignoreShapeIds = nullptr; + m_ignoreCount = 0; +} + +static bool PlaneResultFcn( b3ShapeId shapeId, const b3PlaneResult* planeResults, int planeCount, void* context ) +{ + if ( MoverFilterCallback( shapeId, context ) == false ) + { + // ignore these planes but continue looking for more + return true; + } + + CharacterMover* self = static_cast( context ); + float maxPush = FLT_MAX; + bool clipVelocity = true; + MoverShapeUserData* userData = static_cast( (void*)b3Shape_GetUserData( shapeId ) ); + if ( userData != nullptr ) + { + maxPush = userData->maxPush; + clipVelocity = userData->clipVelocity; + } + + for ( int i = 0; i < planeCount && self->m_planeCount < CharacterMover::m_planeCapacity; ++i ) + { + assert( b3IsValidPlane( planeResults[i].plane ) ); + self->m_planes[self->m_planeCount] = { + .plane = planeResults[i].plane, + .pushLimit = maxPush, + .push = 0.0f, + .clipVelocity = clipVelocity, + }; + self->m_planeExtras[self->m_planeCount] = { + .point = planeResults[i].point, + .shapeId = shapeId, + }; + self->m_planeCount += 1; + } + + return true; +} + +void CharacterMover::SolveMove( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 throttle, bool clipVelocity ) +{ + // Friction + float speed = b3Length( m_velocity ); + if ( speed < m_minSpeed ) + { + m_velocity.x = 0.0f; + m_velocity.y = 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 = b3MaxFloat( 0.0f, speed - drop ); + m_velocity *= newSpeed / speed; + } + + float maxSpeed = m_sprint ? 1.5f * m_maxSpeed : m_maxSpeed; + + b3Vec3 desiredVelocity = maxSpeed * throttle.x * forward + maxSpeed * throttle.y * right; + float desiredSpeed; + b3Vec3 desiredDirection = b3GetLengthAndNormalize( &desiredSpeed, desiredVelocity ); + + if ( desiredSpeed > maxSpeed ) + { + desiredVelocity *= maxSpeed / desiredSpeed; + desiredSpeed = maxSpeed; + } + + if ( m_onGround ) + { + m_velocity.y = 0.0f; + } + + // Accelerate + float currentSpeed = b3Dot( m_velocity, desiredDirection ); + float addSpeed = desiredSpeed - currentSpeed; + if ( addSpeed > 0.0f ) + { + float accelSpeed = m_accelerate * maxSpeed * timeStep; + if ( accelSpeed > addSpeed ) + { + accelSpeed = addSpeed; + } + + m_velocity += accelSpeed * desiredDirection; + } + + m_velocity.y -= m_gravity * timeStep; + + b3WorldId worldId = m_sample->m_worldId; + Scene* scene = m_sample->m_scene; + + float pogoRestLength = 3.0f * m_capsule.radius; + float rayLength = pogoRestLength + m_capsule.radius; + b3Vec3 rayOrigin = b3TransformPoint( m_transform, m_capsule.center1 ); + b3Vec3 rayTranslation = -rayLength * b3Vec3_axisY; + b3QueryFilter skipTeamFilter = { 1, ~2u }; + b3RayResult rayResult = b3World_CastRayClosest( worldId, rayOrigin, rayTranslation, skipTeamFilter ); + + if ( rayResult.hit == false ) + { + m_onGround = false; + m_pogoVelocity = 0.0f; + + DrawLine( scene, rayOrigin, rayOrigin + rayTranslation, b3_colorGray ); + } + else + { + m_onGround = true; + float pogoCurrentLength = rayResult.fraction * rayLength; + + float zeta = 0.7f; + float hertz = 4.0f; + float omega = 2.0f * B3_PI * hertz; + float omegaH = omega * timeStep; + + m_pogoVelocity = ( m_pogoVelocity - omega * omegaH * ( pogoCurrentLength - pogoRestLength ) ) / + ( 1.0f + 2.0f * zeta * omegaH + omegaH * omegaH ); + DrawLine( scene, rayOrigin, rayResult.point, b3_colorGreen ); + } + + b3Vec3 startPosition = m_transform.p; + b3Vec3 target = m_transform.p + timeStep * m_velocity + timeStep * m_pogoVelocity * b3Vec3_axisY; + + // Want the mover to collide with allies + b3QueryFilter moverFilter = { .categoryBits = 1, .maskBits = ~0u }; + + // The cast should ignore allies + b3QueryFilter castFilter = { .categoryBits = 1, .maskBits = ~2u }; + + m_totalIterations = 0; + float tolerance = 0.01f; + + for ( int iteration = 0; iteration < 5; ++iteration ) + { + m_planeCount = 0; + + b3Capsule mover; + mover.center1 = b3TransformPoint( m_transform, m_capsule.center1 ); + mover.center2 = b3TransformPoint( m_transform, m_capsule.center2 ); + mover.radius = m_capsule.radius; + + b3World_CollideMover( worldId, &mover, moverFilter, PlaneResultFcn, this ); + + b3Vec3 targetDelta = target - m_transform.p; + b3PlaneSolverResult result = b3SolvePlanes( targetDelta, m_planes, m_planeCount ); + + m_totalIterations += result.iterationCount; + + b3Vec3 delta = result.delta; + + float fraction = b3World_CastMover( worldId, &mover, delta, castFilter, MoverFilterCallback, this ); + + delta *= fraction; + m_transform.p += delta; + + if ( b3LengthSquared( delta ) < tolerance * tolerance ) + { + break; + } + } + + for ( int i = 0; i < m_planeCount; ++i ) + { + b3BodyId bodyId = b3Shape_GetBody( m_planeExtras[i].shapeId ); + b3BodyType bodyType = b3Body_GetType( bodyId ); + if ( bodyType != b3_dynamicBody ) + { + continue; + } + + b3Vec3 point = m_planeExtras[i].point; + b3Vec3 normal = b3Neg( m_planes[i].plane.normal ); + + float invMassA = 0.0f; + float invMassB = b3Body_GetInverseMass( bodyId ); + b3Matrix3 invIB = b3Body_GetWorldInverseRotationalInertia( bodyId ); + + b3Vec3 pB = b3Body_GetWorldCenterOfMass( bodyId ); + b3Vec3 rB = point - pB; + + b3Vec3 rnB = b3Cross( rB, normal ); + float kNormal = invMassA + invMassB + b3Dot( rnB, b3MulMV( invIB, rnB ) ); + float normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; + + b3Vec3 vB = b3Body_GetLinearVelocity( bodyId ); + b3Vec3 omegaB = b3Body_GetAngularVelocity( bodyId ); + b3Vec3 vrB = b3Add( vB, b3Cross( omegaB, rB ) ); + float vn = b3Dot( b3Sub( vrB, m_velocity ), normal ); + float impulse = b3MaxFloat( -normalMass * vn, 0.0f ); + + b3Vec3 P = b3MulSV( impulse, normal ); + m_velocity = b3MulSub( m_velocity, invMassA, P ); + + b3Body_ApplyLinearImpulse( bodyId, P, point, true ); + } + + if ( clipVelocity ) + { + // Using the velocity clipper can avoid picking up velocity from depenetration. + // This allows the mover to avoid velocity from soft collision depenetration. + m_velocity = b3ClipVector( m_velocity, m_planes, m_planeCount ); + } + else if ( timeStep > 0.0f ) + { + // Using the position delta is more holistic and intuitive in some cases. + m_velocity = ( 1.0f / timeStep ) * ( m_transform.p - startPosition ); + } +} + +void CharacterMover::Step( b3ShapeId* ignoreShapes, int ignoreCount, bool clipVelocity ) +{ + m_ignoreShapeIds = ignoreShapes; + m_ignoreCount = ignoreCount; + + b3Vec2 throttle = { 0.0f, 0.0f }; + b3Vec3 forward = -m_sample->m_camera->GetForward(); + b3Vec3 right = m_sample->m_camera->GetRight(); + forward.y = 0.0f; + + if ( m_sample->m_camera->m_thirdPerson ) + { + if ( glfwGetKey( m_sample->m_window, GLFW_KEY_W ) ) + { + throttle.x += 1.0f; + } + + if ( glfwGetKey( m_sample->m_window, GLFW_KEY_S ) ) + { + throttle.x -= 1.0f; + } + + if ( glfwGetKey( m_sample->m_window, GLFW_KEY_A ) ) + { + throttle.y -= 1.0f; + } + + if ( glfwGetKey( m_sample->m_window, GLFW_KEY_D ) ) + { + throttle.y += 1.0f; + } + + if ( glfwGetKey( m_sample->m_window, GLFW_KEY_SPACE ) && m_onGround == true ) + { + m_velocity.y = m_jumpSpeed; + m_onGround = false; + } + + if ( m_onGround == true ) + { + m_sprint = glfwGetKey( m_sample->m_window, GLFW_KEY_LEFT_SHIFT ) != 0; + } + else + { + m_sprint = false; + } + } + + float hertz = m_sample->m_context->hertz; + float timeStep = hertz > 0.0f ? 1.0f / hertz : 0.0f; + + // throttle = { 0.0f, 0.0f, -1.0f }; + + SolveMove( timeStep, forward, right, throttle, clipVelocity ); + + Scene* scene = m_sample->m_scene; + + int count = m_planeCount; + for ( int i = 0; i < count; ++i ) + { + b3Plane plane = m_planes[i].plane; + b3Vec3 p1 = m_transform.p + ( plane.offset - m_capsule.radius ) * plane.normal; + b3Vec3 p2 = p1 + 0.1f * plane.normal; + DrawPoint( scene, p1, 5.0f, b3_colorYellow ); + DrawLine( scene, p1, p2, b3_colorYellow ); + } + + DrawCapsule( scene, m_transform, m_capsule, b3_colorBlue ); + DrawLine( scene, m_transform.p, m_transform.p + m_velocity, b3_colorPurple ); + + b3Vec3 p = m_transform.p; + // m_sample->DrawTextLine( "position %.2f %.2f %.2f", p.x, p.y, p.z ); + // m_sample->DrawTextLine( "forward %.2f %.2f %.2f", forward.x, forward.y, forward.z ); + // m_sample->DrawTextLine( "right %.2f %.2f %.2f", right.x, right.y, right.z ); + // m_sample->DrawTextLine( "velocity %.2f %.2f %.2f", m_velocity.x, m_velocity.y, m_velocity.z ); + // m_sample->DrawTextLine( "iterations %d", m_totalIterations ); + + if ( m_sample->m_camera->m_thirdPerson ) + { + m_sample->m_camera->m_pivot = p; + m_sample->m_camera->UpdateTransform(); + } + + m_ignoreShapeIds = nullptr; + m_ignoreCount = 0; +} diff --git a/samples/sample.h b/samples/sample.h new file mode 100644 index 0000000..e490bb8 --- /dev/null +++ b/samples/sample.h @@ -0,0 +1,215 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "camera.h" + +class Font; +struct MouseEvent; +struct MouseMoveEvent; +struct MouseWheelEvent; +struct Scene; +struct GLFWwindow; + +struct SampleContext +{ + void Save(); + void Load(); + + GLFWwindow* window = nullptr; + Camera camera; + Arena arena; + Scene* scene = nullptr; + bool minimized = false; + class Sample* sample = nullptr; + b3Capacity capacity; + b3DebugDraw debugDraw; + + int windowWidth = 1920; + int windowHeight = 1080; + + // Used to shift the origin and test round-off problems + b3Vec3 origin = b3Vec3_zero; + + float hertz = 60.0f; + float recycleDistance = 0.05f; + int subStepCount = 4; + int workerCount = 1; + bool transparentDynamic = false; + bool drawCounters = false; + bool drawProfile = false; + bool frameTime = false; + bool transparent = false; + bool enableWarmStarting = true; + bool enableContinuous = true; + bool enableSleep = true; + bool pause = false; + int singleStep = 0; + bool restart = false; + + int sampleIndex = 0; +}; + +class Sample +{ +public: + explicit Sample( SampleContext* context ); + virtual ~Sample(); + + void CreateWorld( b3Capacity* capacity ); + + // Update and render are split to support pausing the simulation + virtual void Step(); + + virtual void Render(); + + virtual void UpdateUI(); + + virtual void Keyboard( int key, int action, int modifiers ) + { + } + + virtual void MouseDown( b3Vec2 p, int button, int modifiers ); + virtual void MouseUp( b3Vec2 p, int button ); + virtual void MouseMove( b3Vec2 p ); + + void ToggleThirdPerson(); + void DrawTextLine( const char* text, ... ); + void ResetProfile(); + +#if defined( NDEBUG ) + static constexpr bool m_isDebug = false; +#else + static constexpr bool m_isDebug = true; +#endif + + static constexpr int m_maxTasks = 64; + static constexpr int m_maxThreads = 64; + static constexpr int m_profileCapacity = 512; + + SampleContext* m_context; + GLFWwindow* m_window; + Scene* m_scene; + Camera* m_camera; + + b3WorldId m_worldId; + b3Vec3 m_mousePoint; + b3BodyId m_mouseBodyId; + b3JointId m_mouseJointId; + float m_mouseFraction; + float m_mouseForceScale; + float m_launchSpeedScale; + int m_stepCount; + int m_textLine; + int m_textIncrement; + int m_triangleIndex; + uint64_t m_userMaterialId; + + b3Profile m_profiles[m_profileCapacity]; + int m_currentProfileIndex; + int m_profileReadIndex; + int m_profileWriteIndex; + + b3Vec2 m_mouseLast; + b3Vec2 m_mouseDelta; + bool m_didStep; + bool m_stepWhilePaused; + bool m_haveMouseLast; +}; + +#define MAX_SAMPLES 256 + +typedef Sample* SampleCreateFcn( SampleContext* context ); + +struct SampleEntry +{ + const char* Category; + const char* Name; + SampleCreateFcn* CreateFcn; +}; + +class SampleManager +{ +public: + static int Register( const char* category, const char* name, SampleCreateFcn* fcn ); + + void Startup( GLFWwindow* window, int width, int height, int bufferWidth, int bufferHeight ); + void Step(); + void Resize( int width, int height, int bufferWidth, int bufferHeight ); + void Render( GLFWwindow* window, float elapsedTime ); + void Shutdown( GLFWwindow* window ); + + void Keyboard( int key, int action, int modifiers ); + + void CreateSample(); + + void RenderScene( GLFWwindow* window, float elapsedTime ); + void UpdateUI( GLFWwindow* window ); + + static SampleEntry sEntries[MAX_SAMPLES]; + static int sEntryCount; + + SampleContext m_context; + Sample* m_sample = nullptr; + bool m_showMenu = true; +}; + +struct CastClosestContext +{ + b3ShapeId shapeId; + b3Vec3 point; + b3Vec3 normal; + float fraction; + uint64_t materialId; + int childIndex; + int triangleIndex; + bool hit; +}; + +float CastClosestCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t materialId, + int triangleIndex, int childIndex, void* context ); + +struct MoverShapeUserData +{ + float maxPush; + bool clipVelocity; +}; + +struct PlaneExtra +{ + b3Vec3 point; + b3ShapeId shapeId; +}; + +struct CharacterMover +{ + static constexpr int m_planeCapacity = 8; + static constexpr float m_jumpSpeed = 5.0f; + static constexpr float m_maxSpeed = 6.0f; + static constexpr float m_minSpeed = 0.01f; + static constexpr float m_stopSpeed = 1.0f; + static constexpr float m_accelerate = 30.0f; + static constexpr float m_friction = 4.0f; + static constexpr float m_gravity = 15.0f; + + void Initialize( Sample* sample, b3Vec3 position ); + void SolveMove( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 throttle, bool clipVelocity ); + void Step( b3ShapeId* ignoreShapes, int ignoreCount, bool clipVelocity ); + + Sample* m_sample; + b3Transform m_transform; + b3Vec3 m_velocity; + b3Capsule m_capsule; + b3CollisionPlane m_planes[m_planeCapacity] = {}; + PlaneExtra m_planeExtras[m_planeCapacity] = {}; + int m_planeCount; + int m_totalIterations; + float m_pogoVelocity; + bool m_onGround; + bool m_sprint; + + // Transient + b3ShapeId* m_ignoreShapeIds; + int m_ignoreCount; +}; diff --git a/samples/sample_benchmark.cpp b/samples/sample_benchmark.cpp new file mode 100644 index 0000000..37d9ff4 --- /dev/null +++ b/samples/sample_benchmark.cpp @@ -0,0 +1,1468 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "benchmarks.h" +#include "camera.h" +#include "imgui.h" +#include "utils.h" +#include "renderer.h" +#include "sample.h" +#include "scene.h" + +#include "box3d/box3d.h" + +#include + +inline bool operator<( b3BodyId a, b3BodyId b ) +{ + uint64_t ua = b3StoreBodyId( a ); + uint64_t ub = b3StoreBodyId( b ); + return ua < ub; +} + +class BenchmarkLargePyramid : public Sample +{ +public: + explicit BenchmarkLargePyramid( SampleContext* context ) + : Sample( context ) + { + if (context->restart == false) + { + m_camera->SetView( 0.0f, 5.0f, 80.0f, { 0.0f, 18.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + CreateLargePyramid( m_worldId ); + } + + static Sample* Create( SampleContext* context ) + { + return new BenchmarkLargePyramid( context ); + } +}; + +static int sampleLargePyramid = SampleManager::Register( "Benchmark", "Large Pyramid", BenchmarkLargePyramid::Create ); + +class BenchmarkWidePyramid : public Sample +{ +public: + explicit BenchmarkWidePyramid( SampleContext* context ) + : Sample( context ) + { + if (context->restart == false) + { + m_camera->SetView( 0.0f, 5.0f, 80.0f, { 0.0f, 18.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + CreateWidePyramid( m_worldId ); + } + + static Sample* Create( SampleContext* context ) + { + return new BenchmarkWidePyramid( context ); + } +}; + +static int sampleWidePyramid = SampleManager::Register( "Benchmark", "Wide Pyramid", BenchmarkWidePyramid::Create ); + +class BenchmarkManyPyramids : public Sample +{ +public: + explicit BenchmarkManyPyramids( SampleContext* context ) + : Sample( context ) + { + if (context->restart == false) + { + m_camera->SetView( 0.0f, 5.0f, 80.0f, { 0.0f, 18.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + CreateManyPyramids( m_worldId ); + + // float frictionTorque = 5.0f; + // float hertz = 1.0f; + // float dampingRatio = 0.7f; + // bool colorize = true; + + // Human human = {}; + // b3Vec3 position = m_isDebug ? b3Vec3{ 0.0f, 20.0f, 0.0f } : b3Vec3{ 5.0f, 20.0f, 53.0f }; + // CreateHuman( &human, m_worldId, position, frictionTorque, hertz, dampingRatio, 0, nullptr, colorize ); + } + + void Render() override + { + Sample::Render(); + b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 4.0f ); + } + + static Sample* Create( SampleContext* context ) + { + return new BenchmarkManyPyramids( context ); + } +}; + +static int sampleManyPyramids = SampleManager::Register( "Benchmark", "Many Pyramids", BenchmarkManyPyramids::Create ); + +class BenchmarkRain : public Sample +{ +public: + explicit BenchmarkRain( SampleContext* context ) + : Sample( context ) + { + if (context->restart == false) + { + m_camera->SetView( 45.0f, 30.0f, 40.0f, b3Vec3_zero ); + context->debugDraw.drawJoints = false; + } + + b3Capacity capacity = {}; + GetRainCapacity( &capacity ); + CreateWorld( &capacity ); + + CreateRain( m_worldId ); + } + + ~BenchmarkRain() override + { + DestroyRain(); + } + + void Step() override + { + StepRain( m_worldId, m_stepCount ); + + // This is for testing adjustable worker count + // if (m_stepCount == 200) + //{ + // m_scheduler->Initialize( 3 ); + // b3World_SetWorkerCount( m_worldId, 3 ); + //} + + Sample::Step(); + } + + void Render() override + { + Sample::Render(); + b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 2.0f ); + } + + static Sample* Create( SampleContext* context ) + { + return new BenchmarkRain( context ); + } +}; + +static int sampleRain = SampleManager::Register( "Benchmark", "Rain", BenchmarkRain::Create ); + +#if 0 +class BenchmarkLargeWorld : public Sample +{ +public: + explicit BenchmarkLargeWorld( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 40.0f, b3Vec3_zero ); + } + + CreateRain( m_worldId ); + } + + ~BenchmarkLargeWorld() override + { + DestroyRain(); + } + + void Step() override + { + StepRain( m_worldId, m_stepCount ); + + // This is for testing adjustable worker count + // if (m_stepCount == 200) + //{ + // m_scheduler->Initialize( 3 ); + // b3World_SetWorkerCount( m_worldId, 3 ); + //} + + Sample::Step(); + } + + void Render() override + { + Sample::Render(); + b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 2.0f ); + } + + static Sample* Create( SampleContext* context ) + { + return new BenchmarkLargeWorld( context ); + } +}; + +static int sampleLargeWorld = SampleManager::Register( "Benchmark", "Large World", BenchmarkLargeWorld::Create ); +#endif + +class BenchmarkJointGrid : public Sample +{ +public: + explicit BenchmarkJointGrid( SampleContext* context ) + : Sample( context ) + { + if (context->restart == false) + { + m_camera->SetView( -25.0f, 25.0f, 94.0f, { 30.0f, -30.0f, 30.0f } ); + } + + CreateJointGrid( m_worldId ); + } + + void Render() override + { + Sample::Render(); + b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 4.0f ); + + // transform.p.y = 0.0f; + // transform.p.z = -5.0f; + + // for (int i = 0; i < 100; ++i) + //{ + // transform.p.x = -50.0f; + // for (int j = 0; j < 100; ++j) + // { + // AddSphere( m_scene, transform, 0.5f, b3_colorYellowGreen ); + // transform.p.x += 1.0f; + // } + + // transform.p.y -= 1.0f; + //} + } + + static Sample* Create( SampleContext* context ) + { + return new BenchmarkJointGrid( context ); + } +}; + +static int sampleJointGrid = SampleManager::Register( "Benchmark", "Joint Grid", BenchmarkJointGrid::Create ); + +class FallingBoxes : public Sample +{ +public: + explicit FallingBoxes( SampleContext* context ) + : Sample( context ) + { + if (context->restart == false) + { + m_camera->SetView( 45.0f, 45.0f, 250.0f, b3Vec3_zero ); + EnableGrid( m_scene, true ); + } + + { + b3BoxHull groundBox = b3MakeBoxHull( 250.0f, 1.0f, 250.0f ); + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( groundId, &shapeDef, &groundBox.base ); + } + + { + constexpr int n = m_isDebug ? 4 : 50; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeBoxHull( 1.0f, 1.0f, 1.0f ); + + for (int i = 0; i < n; ++i) + { + for (int j = 0; j < 8; ++j) + { + for (int k = 0; k < 8; ++k) + { + bodyDef.position = { -16.0f + 4.0f * j, 4.0f * i + 5.0f, -16.0f + 4.0f * k }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + } + } + } + } + + static Sample* Create( SampleContext* context ) + { + return new FallingBoxes( context ); + } +}; + +static int sampleFallingBoxes = SampleManager::Register( "Benchmark", "Falling Boxes", FallingBoxes::Create ); + +class CandyCups : public Sample +{ +public: + explicit CandyCups( SampleContext* context ) + : Sample( context ) + { + if (context->restart == false) + { + m_camera->SetView( 45.0f, 35.0f, 40.0f, b3Vec3_zero ); + EnableGrid( m_scene, true ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull groundBox = b3MakeBoxHull( 250.0f, 1.0f, 250.0f ); + b3CreateHullShape( groundId, &shapeDef, &groundBox.base ); + } + + { + constexpr int n = m_isDebug ? 4 : 16; + constexpr int m = m_isDebug ? 4 : 16; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + m_convex = CreateConvex( 0.6f, 0.0f, 0.95f, 1.0f, context->arena ); + for (int i = 0; i < n; ++i) + { + for (int j = 0; j < m; ++j) + { + for (int k = 0; k < m; ++k) + { + bodyDef.position = { -10.0f + 2.5f * j, 1.0f * i, -10.0f + 2.5f * k }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, m_convex ); + } + } + } + } + } + + ~CandyCups() override + { + b3DestroyHull( m_convex ); + } + + b3Hull* CreateConvex( float radius1, float height1, float radius2, float height2, Arena arena ) const + { + const int sideCount = 8; + const float deltaAlpha = 2.0f * B3_PI / sideCount; + + int vertexCount = 2 * sideCount; + b3Vec3* vertexBase = static_cast( arena.Allocate( vertexCount * sizeof( b3Vec3 ) ) ); + + float alpha = 0.0f; + for (int sideIndex = 0; sideIndex < sideCount; ++sideIndex) + { + b3CosSin cs = b3ComputeCosSin( alpha ); + + float x1 = radius1 * cs.cosine; + float z1 = radius1 * cs.sine; + float x2 = radius2 * cs.cosine; + float z2 = radius2 * cs.sine; + + vertexBase[2 * sideIndex + 0] = { x1, height1, z1 }; + vertexBase[2 * sideIndex + 1] = { x2, height2, z2 }; + alpha += deltaAlpha; + } + + return b3CreateHull( vertexBase, vertexCount, vertexCount ); + } + + static Sample* Create( SampleContext* context ) + { + return new CandyCups( context ); + } + + b3Hull* m_convex; +}; + +static int sampleSmallConvexes = SampleManager::Register( "Benchmark", "Candy Cups", CandyCups::Create ); + +class BenchmarkExplosion : public Sample +{ +public: + explicit BenchmarkExplosion( SampleContext* context ) + : Sample( context ) + { + if (m_context->restart == false) + { + m_camera->SetView( 0.0f, 40.0f, 10.0f, { 0.0f, 0.0f, 0.0f } ); + } + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.name = "grid"; + m_gridMesh = b3CreateGridMesh( 40, 40, 1.0f, 0, true ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateMeshShape( groundId, &shapeDef, m_gridMesh, b3Vec3_one ); + + { + b3Transform transform; + transform.p = { 0.0f, 5.0f, -20.0f }; + transform.q = b3Quat_identity; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 20.0f, 5.0f, 0.1f, transform ); + shapeDef.name = "wall1"; + b3CreateHullShape( groundId, &shapeDef, &wallBox.base ); + } + + { + b3Transform transform; + transform.p = { 0.0f, 5.0f, 20.0f }; + transform.q = b3Quat_identity; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 20.0f, 5.0f, 0.1f, transform ); + shapeDef.name = "wall2"; + b3CreateHullShape( groundId, &shapeDef, &wallBox.base ); + } + + { + b3Transform transform; + transform.p = { -20.0f, 5.0f, 0.0f }; + transform.q = b3Quat_identity; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 0.1f, 5.0f, 20.0f, transform ); + shapeDef.name = "wall3"; + b3CreateHullShape( groundId, &shapeDef, &wallBox.base ); + } + + { + b3Transform transform; + transform.p = { 20.0f, 5.0f, 0.0f }; + transform.q = b3Quat_identity; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 0.1f, 5.0f, 20.0f, transform ); + shapeDef.name = "wall4"; + b3CreateHullShape( groundId, &shapeDef, &wallBox.base ); + } + + // Using 15 sides rather than 16 to avoid manifold degeneracies + m_cylinder = b3CreateCylinder( 0.5f, 0.2f, 0.0f, 15 ); + + constexpr int n = m_isDebug ? 3 : 16; + + bodyDef.type = b3_dynamicBody; + shapeDef.explosionScale = 2.0f; + + for (int i = -n; i <= n; ++i) + { + for (int k = -n; k <= n; ++k) + { + bodyDef.position = { 1.0f * i, 0.0f, 1.0f * k }; + + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, m_cylinder ); + } + } + + m_impulse = 1000.0f; + + m_stepWhilePaused = false; + } + + ~BenchmarkExplosion() override + { + b3DestroyHull( m_cylinder ); + b3DestroyMesh( m_gridMesh ); + } + + void Explode() + { + b3ExplosionDef def = b3DefaultExplosionDef(); + def.radius = 16.0f; + def.position = { 0.0f, -4.0f, 0.0f }; + def.impulsePerArea = m_impulse; + + b3World_Explode( m_worldId, &def ); + } + + void UpdateUI() override + { + Sample::UpdateUI(); + + float fontSize = ImGui::GetFontSize(); + float height = 10.0f * fontSize; + ImGui::SetNextWindowPos( { 1.0f * fontSize, m_camera->m_height - height - 3.0f * fontSize }, ImGuiCond_Once ); + ImGui::SetNextWindowSize( { 20.0f * fontSize, height } ); + + ImGui::Begin( "Explosion", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + ImGui::SliderFloat( "impulse", &m_impulse, 0.0f, 2000.0f, "%.0f" ); + + if (ImGui::Button( "explode" )) + { + Explode(); + } + + ImGui::End(); + } + + void Render() override + { + Sample::Render(); + b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 4.0f ); + } + + static Sample* Create( SampleContext* context ) + { + return new BenchmarkExplosion( context ); + } + + b3MeshData* m_gridMesh; + b3Hull* m_cylinder; + float m_impulse; +}; + +static int sampleExplosion = SampleManager::Register( "Benchmark", "Explosion", BenchmarkExplosion::Create ); + +class BenchmarkHeightField : public Sample +{ +public: + struct Context + { + b3Vec3 point; + b3Vec3 normal; + float fraction; + bool hit; + }; + + static float CastCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t userMaterialId, + int triangleIndex, int childIndex, void* context ) + { + (void)shapeId; + (void)userMaterialId; + (void)triangleIndex; + (void)childIndex; + + Context* rayContext = (Context*)context; + rayContext->point = point; + rayContext->normal = normal; + rayContext->fraction = fraction; + rayContext->hit = true; + return fraction; + } + + explicit BenchmarkHeightField( SampleContext* context ) + : Sample( context ) + { + if (context->restart == false) + { + m_camera->SetView( 45.0f, 30.0f, 20.0f ); + } + + m_columnCount = 50; + m_rowCount = 50; + m_radius = 0.1f; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { -0.5f * m_columnCount, 0.0f, -0.5f * m_rowCount }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + + m_heightField = b3CreateWave( 50, 50, b3Vec3_one, 0.02f, 0.04f, true ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHeightFieldShape( body, &shapeDef, m_heightField ); + } + + ~BenchmarkHeightField() override + { + b3DestroyHeightField( m_heightField ); + } + + void UpdateUI() override + { + float fontSize = ImGui::GetFontSize(); + float height = 4.0f * fontSize; + ImGui::SetNextWindowPos( { 1.0f * fontSize, m_camera->m_height - height - 3.0f * fontSize }, ImGuiCond_Once ); + ImGui::SetNextWindowSize( { 20.0f * fontSize, height } ); + + ImGui::Begin( "Height Field", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + ImGui::SliderFloat( "radius", &m_radius, 0.0f, 1.0f, "%.1f" ); + ImGui::End(); + } + + void Render() override + { + Sample::Render(); + + DrawLine( m_scene, b3Vec3_zero, 0.4f * b3Vec3_axisX, b3_colorRed ); + DrawLine( m_scene, b3Vec3_zero, 0.4f * b3Vec3_axisY, b3_colorGreen ); + DrawLine( m_scene, b3Vec3_zero, 0.4f * b3Vec3_axisZ, b3_colorBlue ); + + int hitCount = 0; + int iterationCount = 0; + int innerIterationCount = 0; + + float delta = m_isDebug ? 2.0f * 0.95f : 0.4f; + + float spanX = 0.94f * 0.5f * m_columnCount; + float spanZ = 0.96f * 0.5f * m_rowCount; + + uint64_t startTick = b3GetTicks(); + + b3Vec3 rayTranslation = { 80000.0f, -80000.0f, 8.0f }; + // b3Vec3 rayTranslation = { 0.0f, -16.0f, 0.0f }; + + b3Sphere sphere = { b3Vec3_zero, m_radius }; + int castCount = 0; + + for (float x = -spanX; x <= spanX; x += delta) + { + for (float z = -spanZ; z <= spanZ; z += delta) + { + b3Vec3 rayOrigin = { x, 2.0f, z }; + + b3RayResult result = {}; + if (m_radius == 0.0f) + { + result = b3World_CastRayClosest( m_worldId, rayOrigin, rayTranslation, b3DefaultQueryFilter() ); + } + else + { + Context context = {}; + b3ShapeProxy proxy = { &rayOrigin, 1, m_radius }; + b3World_CastShape( m_worldId, &proxy, rayTranslation, b3DefaultQueryFilter(), CastCallback, &context ); + + if (context.hit) + { + result.point = context.point; + result.normal = context.normal; + result.fraction = context.fraction; + result.hit = true; + } + } + + castCount += 1; + + if (result.hit) + { + hitCount += 1; + if (m_isDebug) + { + b3Vec3 point = result.point; + DrawLine( m_scene, point, point + 0.5f * result.normal, b3_colorGreen ); + DrawPoint( m_scene, point, 10.0f, b3_colorGreen ); + + if (m_radius > 0.0f) + { + b3Transform transform = b3Transform_identity; + transform.p = rayOrigin + result.fraction * rayTranslation; + DrawSphere( m_scene, transform, sphere, b3_colorPurple ); + } + + b3Vec3 rayEnd = rayOrigin + result.fraction * rayTranslation; + DrawLine( m_scene, rayOrigin, rayEnd, b3_colorYellow ); + DrawPoint( m_scene, rayOrigin, 2.0f, b3_colorRed ); + DrawPoint( m_scene, rayEnd, 2.0f, b3_colorRed ); + } + } + else if (m_isDebug) + { + b3Vec3 rayEnd = rayOrigin + rayTranslation; + DrawLine( m_scene, rayOrigin, rayEnd, b3_colorYellow ); + DrawPoint( m_scene, rayOrigin, 2.0f, b3_colorRed ); + DrawPoint( m_scene, rayEnd, 2.0f, b3_colorRed ); + } + } + } + + float milliseconds = b3GetMilliseconds( startTick ); + uint64_t tickCount = b3GetTicks() - startTick; + float aveIterations = float( iterationCount ) / float( castCount ); + float aveInner = float( innerIterationCount ) / float( castCount ); + + float aveCastTime = 1000.0f * milliseconds / float( castCount ); + + DrawTextLine( "count = %d, hit count = %d, iterations = %d, inner = %d, ticks = %ld", castCount, hitCount, iterationCount, + innerIterationCount, tickCount ); + + DrawTextLine( "ave iterations = %.1f, ave inner = %.1f, ave cast us %.3f", aveIterations, aveInner, aveCastTime ); + } + + static Sample* Create( SampleContext* context ) + { + return new BenchmarkHeightField( context ); + } + + b3HeightField* m_heightField; + int m_columnCount; + int m_rowCount; + float m_radius; +}; + +static int sampleHeightFieldBenchmark = SampleManager::Register( "Benchmark", "Height Field", BenchmarkHeightField::Create ); + +class BenchmarkCompoundCapsules : public Sample +{ +public: + explicit BenchmarkCompoundCapsules( SampleContext* context ) + : Sample( context ) + { + if (context->restart == false) + { + float distance = 220.0f; + float height = 20.0f; + m_camera->SetView( 25.0f, 10.0f, distance, { 0.0f, height, 0.0f } ); + } + + CreateCompoundCapsules( m_worldId ); + } + + ~BenchmarkCompoundCapsules() override + { + DestroyCompoundCapsules(); + } + + void Render() override + { + Sample::Render(); + b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 4.0f ); + } + + static Sample* Create( SampleContext* context ) + { + return new BenchmarkCompoundCapsules( context ); + } +}; + +static int sampleCompoundCapsules = + SampleManager::Register( "Benchmark", "Compounds Capsules", BenchmarkCompoundCapsules::Create ); + +class BenchmarkCompoundHulls : public Sample +{ +public: + explicit BenchmarkCompoundHulls( SampleContext* context ) + : Sample( context ) + { + if (context->restart == false) + { + float distance = 220.0f; + float height = 20.0f; + m_camera->SetView( 25.0f, 10.0f, distance, { 0.0f, height, 0.0f } ); + } + + CreateCompoundHulls( m_worldId ); + } + + ~BenchmarkCompoundHulls() override + { + DestroyCompoundHulls(); + } + + static Sample* Create( SampleContext* context ) + { + return new BenchmarkCompoundHulls( context ); + } +}; + +static int sampleCompoundHulls = SampleManager::Register( "Benchmark", "Compounds Hulls", BenchmarkCompoundHulls::Create ); + +class WeldedHulls : public Sample +{ +public: + explicit WeldedHulls( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + float distance = 220.0f; + float height = 20.0f; + m_camera->SetView( 25.0f, 10.0f, distance, { 0.0f, height, 0.0f } ); + } + + CreateWeldedHulls( m_worldId ); + } + + ~WeldedHulls() override + { + DestroyWeldedHulls(); + } + + static Sample* Create( SampleContext* context ) + { + return new WeldedHulls( context ); + } +}; + +static int sampleWeldedHulls = SampleManager::Register( "Benchmark", "Welded Hulls", WeldedHulls::Create ); + +struct ShapeUserData +{ + int row; + bool active; +}; + +class BenchmarkSensor : public Sample +{ +public: + explicit BenchmarkSensor( SampleContext* context ) + : Sample( context ) + { + if (m_context->restart == false) + { + m_camera->SetView( 5.0f, 18.0f, 180.0f, { 0.0f, 5.0f, 0.0f } ); + } + + b3World_SetCustomFilterCallback( m_worldId, FilterFcn, this ); + + m_activeSensor.row = 0; + m_activeSensor.active = true; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + { + float gridSize = 3.0f; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.isSensor = true; + shapeDef.enableSensorEvents = true; + shapeDef.userData = &m_activeSensor; + + float y = 0.0f; + float x = -40.0f * gridSize; + for (int i = 0; i < 81; ++i) + { + b3Vec3 extents = { 0.5f * gridSize, 0.5f * gridSize, 0.5f * gridSize }; + b3Transform transform = { { x, y, 0.0f }, b3Quat_identity }; + b3BoxHull box = b3MakeTransformedBoxHull( extents.x, extents.y, extents.z, transform ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + x += gridSize; + } + } + + g_randomSeed = 42; + + float shift = 5.0f; + float xCenter = 0.5f * shift * m_columnCount; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.isSensor = true; + shapeDef.enableSensorEvents = true; + + float yStart = 10.0f; + + for (int j = 0; j < m_rowCount; ++j) + { + m_passiveSensors[j].row = j; + m_passiveSensors[j].active = false; + shapeDef.userData = m_passiveSensors + j; + + float y = j * shift + yStart; + for (int i = 0; i < m_columnCount; ++i) + { + float x = i * shift - xCenter; + b3Vec3 extents = { 0.5f, 0.5f, 0.5f }; + b3Transform transform = { { x, y, 0.0f }, b3Quat_identity }; + b3BoxHull box = b3MakeTransformedBoxHull( extents.x, extents.y, extents.z, transform ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + } + + m_maxBeginCount = 0; + m_maxEndCount = 0; + m_lastStepCount = 0; + m_filterRow = m_rowCount >> 1; + } + + void CreateRow( float y ) + { + float shift = 5.0f; + float xCenter = 0.5f * shift * m_columnCount; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.gravityScale = 0.0f; + bodyDef.linearVelocity = { 0.0f, -5.0f }; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.enableSensorEvents = true; + + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f }; + for (int i = 0; i < m_columnCount; ++i) + { + // stagger bodies to avoid bunching up events into a single update + float yOffset = RandomFloatRange( -1.0f, 1.0f ); + bodyDef.position = { shift * i - xCenter, y + yOffset, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + } + } + + void Step() override + { + Sample::Step(); + + if (m_stepCount == m_lastStepCount) + { + return; + } + + std::set zombies; + + b3SensorEvents events = b3World_GetSensorEvents( m_worldId ); + for (int i = 0; i < events.beginCount; ++i) + { + b3SensorBeginTouchEvent* event = events.beginEvents + i; + + // shapes on begin touch are always valid + + ShapeUserData* userData = static_cast( b3Shape_GetUserData( event->sensorShapeId ) ); + if (userData->active) + { + zombies.emplace( b3Shape_GetBody( event->visitorShapeId ) ); + } + else + { + // Check custom filter correctness + assert( userData->row != m_filterRow ); + + // Modify color while overlapped with a sensor + b3SurfaceMaterial surfaceMaterial = b3Shape_GetSurfaceMaterial( event->visitorShapeId ); + surfaceMaterial.customColor = b3_colorLime; + b3Shape_SetSurfaceMaterial( event->visitorShapeId, surfaceMaterial ); + } + } + + for (int i = 0; i < events.endCount; ++i) + { + b3SensorEndTouchEvent* event = events.endEvents + i; + + if (b3Shape_IsValid( event->visitorShapeId ) == false) + { + continue; + } + + // Restore color to default + b3SurfaceMaterial surfaceMaterial = b3Shape_GetSurfaceMaterial( event->visitorShapeId ); + surfaceMaterial.customColor = 0; + b3Shape_SetSurfaceMaterial( event->visitorShapeId, surfaceMaterial ); + } + + for (b3BodyId bodyId : zombies) + { + b3DestroyBody( bodyId ); + } + + int delay = 0x1F; + + if (( m_stepCount & delay ) == 0) + { + CreateRow( 10.0f + m_rowCount * 5.0f ); + } + + m_lastStepCount = m_stepCount; + + m_maxBeginCount = b3MaxInt( events.beginCount, m_maxBeginCount ); + m_maxEndCount = b3MaxInt( events.endCount, m_maxEndCount ); + DrawTextLine( "max begin touch events = %d", m_maxBeginCount ); + DrawTextLine( "max end touch events = %d", m_maxEndCount ); + } + + bool Filter( b3ShapeId idA, b3ShapeId idB ) + { + ShapeUserData* userData = nullptr; + if (b3Shape_IsSensor( idA )) + { + userData = (ShapeUserData*)b3Shape_GetUserData( idA ); + } + else if (b3Shape_IsSensor( idB )) + { + userData = (ShapeUserData*)b3Shape_GetUserData( idB ); + } + + if (userData != nullptr) + { + return userData->active == true || userData->row != m_filterRow; + } + + return true; + } + + static bool FilterFcn( b3ShapeId idA, b3ShapeId idB, void* context ) + { + BenchmarkSensor* self = (BenchmarkSensor*)context; + return self->Filter( idA, idB ); + } + + static Sample* Create( SampleContext* context ) + { + return new BenchmarkSensor( context ); + } + + static constexpr int m_columnCount = 40; + static constexpr int m_rowCount = 40; + int m_maxBeginCount; + int m_maxEndCount; + ShapeUserData m_passiveSensors[m_rowCount]; + ShapeUserData m_activeSensor; + int m_lastStepCount; + int m_filterRow; +}; + +static int benchmarkSensor = SampleManager::Register( "Benchmark", "Sensor", BenchmarkSensor::Create ); + +class BenchmarkWasher : public Sample +{ +public: + explicit BenchmarkWasher( SampleContext* context ) + : Sample( context ) + { + if (m_context->restart == false) + { + m_camera->SetView( 0.0f, 20.0f, 60.0, { 0.0f, 15.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + b3Capacity capacity = {}; + GetWasherCapacity( &capacity ); + CreateWorld( &capacity ); + + CreateWasher( m_worldId ); + } + + static Sample* Create( SampleContext* context ) + { + return new BenchmarkWasher( context ); + } +}; + +static int benchmarkWasher = SampleManager::Register( "Benchmark", "Washer", BenchmarkWasher::Create ); + +class BenchmarkHull : public Sample +{ +public: + explicit BenchmarkHull( SampleContext* context ) + : Sample( context ) + { + if (m_context->restart == false) + { + m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Vec3_zero ); + } + + g_randomSeed = 42; + + m_count = 64; + for (int i = 0; i < m_count; ++i) + { + m_points[i] = RandomVec3( { -1.0f, -1.0f, -1.0f }, { 1.0f, 1.0f, 1.0f } ); + } + + m_hull = b3CreateHull( m_points, m_count, m_count ); + m_scale = { -1.0f, 1.0f, 1.0f }; + m_transformedHull = b3CloneAndTransformHull( m_hull, b3Transform_identity, m_scale ); + } + + ~BenchmarkHull() override + { + b3DestroyHull( m_hull ); + b3DestroyHull( m_transformedHull ); + } + + void Render() override + { + b3Transform transform1 = { { -2.0f, 0.0f, 0.0f }, b3Quat_identity }; + b3Transform transform2 = { { 2.0f, 0.0f, 0.0f }, b3Quat_identity }; + + DrawHull( m_scene, transform1, m_hull, b3_colorGreen, true ); + DrawHull( m_scene, transform2, m_transformedHull, b3_colorYellow, true ); + + Sample::Render(); + } + + void Step() override + { + uint64_t startTick = b3GetTicks(); + float area = 0.0f; + int trials = m_isDebug ? 200 : 2000; + + for (int i = 0; i < trials; ++i) + { + b3Hull* hull = b3CreateHull( m_points, m_count, m_count ); + area += hull->surfaceArea; + b3DestroyHull( hull ); + } + + float createTime = b3GetMillisecondsAndReset( &startTick ); + + float scaledArea = 0.0f; + for (int i = 0; i < trials; ++i) + { + b3Hull* hull = b3CloneAndTransformHull( m_hull, b3Transform_identity, m_scale ); + scaledArea += hull->surfaceArea; + b3DestroyHull( hull ); + } + + float cloneTime = b3GetMilliseconds( startTick ); + + DrawTextLine( "trials = %d", trials ); + DrawTextLine( "createTime (us) = %.2f, area = %.2f", 1000.0f * createTime / trials, area / trials ); + DrawTextLine( "cloneTime (us) = %.2f, area = %.2f", 1000.0f * cloneTime / trials, scaledArea / trials ); + DrawTextLine( "createTime / cloneTime = %.2f", createTime / cloneTime ); + Sample::Step(); + } + + static Sample* Create( SampleContext* sampleContext ) + { + return new BenchmarkHull( sampleContext ); + } + + static constexpr int m_capacity = 64; + b3Hull* m_hull; + b3Hull* m_transformedHull; + b3Vec3 m_scale; + b3Vec3 m_points[m_capacity]; + int m_count; +}; + +static int sampleBenchmarkHull = SampleManager::Register( "Benchmark", "Hull", BenchmarkHull::Create ); + +class BenchmarkChains : public Sample +{ +public: + explicit BenchmarkChains( SampleContext* context ) + : Sample( context ) + { + if (context->restart == false) + { + m_camera->SetView( 0.0f, 15.0f, 50.0f, { 0.0f, 5.0f, 0.0f } ); + EnableGrid( context->scene, false ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + m_meshData = b3CreateWaveMesh( 80, 80, 1.0f, 0.5f, 0.05f, 0.01f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateMeshShape( groundId, &shapeDef, m_meshData, b3Vec3_one ); + + float linkRadius = 0.125f; + float linkExtent = 0.25f; + b3Capsule capsule = { { 0.0f, -linkExtent, 0.0f }, { 0.0f, linkExtent, 0.0f }, linkRadius }; + + bodyDef.enableSleep = false; + + int linkCount = 4; + + b3SphericalJointDef jointDef = b3DefaultSphericalJointDef(); + jointDef.base.localFrameA = { { 0.0f, -linkExtent, 0.0f }, b3Quat_identity }; + jointDef.base.localFrameB = { { 0.0f, linkExtent, 0.0f }, b3Quat_identity }; + jointDef.enableSpring = true; + jointDef.hertz = 1.0f; + jointDef.dampingRatio = 0.7f; + jointDef.enableMotor = true; + jointDef.maxMotorTorque = 1.0f; + + int shapeIndex = 0; + + float x = -1.0f * m_gridCount; + for (int rowIndex = 0; rowIndex < m_gridCount; ++rowIndex) + { + float z = -1.0f * m_gridCount; + for (int columnIndex = 0; columnIndex < m_gridCount; ++columnIndex) + { + for (int i = 0; i < linkCount; ++i) + { + bodyDef.position = { x, ( 1.0f - 2.0f * i ) * linkExtent + 3.0f, z }; + + bodyDef.type = i == 0 ? b3_staticBody : b3_dynamicBody; + + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeId shapeId = b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + if (i == linkCount - 1) + { + m_shapeIds[shapeIndex] = shapeId; + shapeIndex += 1; + } + + if (i > 0) + { + jointDef.base.bodyIdB = bodyId; + b3CreateSphericalJoint( m_worldId, &jointDef ); + } + + jointDef.base.bodyIdA = bodyId; + } + + z += 2.0f; + } + + x += 2.0f; + } + + m_noise = {}; + } + + ~BenchmarkChains() override + { + b3DestroyMesh( m_meshData ); + } + + void Step() override + { + b3Vec3 baseWind = { 20.0f, 0.0f, 0.0f }; + float speed; + b3Vec3 direction = b3GetLengthAndNormalize( &speed, baseWind ); + b3Vec3 wind = b3MulSV( speed, b3Add( direction, m_noise ) ); + + for (int i = 0; i < m_gridCount * m_gridCount; ++i) + { + b3Shape_ApplyWind( m_shapeIds[i], wind, 1.0f, 1.0f, 20.0f, false ); + } + + b3Vec3 rand = RandomVec3( { -0.3f, -0.3f, -0.3f }, { 0.3f, 0.3f, 0.3f } ); + m_noise = b3Lerp( m_noise, rand, 0.05f ); + + Sample::Step(); + } + + static Sample* Create( SampleContext* context ) + { + return new BenchmarkChains( context ); + } + + static constexpr int m_gridCount = m_isDebug ? 10 : 25; + b3ShapeId m_shapeIds[m_gridCount * m_gridCount]; + b3MeshData* m_meshData; + b3Vec3 m_noise; +}; + +static int sampleBenchmarkChains = SampleManager::Register( "Benchmark", "Chains", BenchmarkChains::Create ); + +class BenchmarkDestruction : public Sample +{ +public: + explicit BenchmarkDestruction( SampleContext* context ) + : Sample( context ) + { + if (m_context->restart == false) + { + if (m_small) + { + m_camera->SetView( 0.0f, 40.0f, 10.0f, { 0.0f, 0.0f, 0.0f } ); + } + else + { + m_camera->SetView( 0.0f, 40.0f, 30.0f, { 0.0f, 0.0f, 0.0f } ); + } + EnableGrid( m_context->scene, false ); + } + + int bodyCapacity = m_gridCount * m_gridCount * m_gridCount; + + b3Capacity capacity = {}; + capacity.dynamicShapeCount = bodyCapacity; + capacity.dynamicBodyCount = bodyCapacity; + capacity.contactCount = m_small ? 10000 : 50000; + CreateWorld( &capacity ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.name = "grid"; + m_gridMesh = b3CreateGridMesh( 40, 40, 1.0f, 0, true ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateMeshShape( groundId, &shapeDef, m_gridMesh, b3Vec3_one ); + + { + b3Transform transform; + transform.p = { 0.0f, 5.0f, -20.0f }; + transform.q = b3Quat_identity; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 20.0f, 5.0f, 0.1f, transform ); + shapeDef.name = "wall1"; + b3CreateHullShape( groundId, &shapeDef, &wallBox.base ); + } + + { + b3Transform transform; + transform.p = { 0.0f, 5.0f, 20.0f }; + transform.q = b3Quat_identity; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 20.0f, 5.0f, 0.1f, transform ); + shapeDef.name = "wall2"; + b3CreateHullShape( groundId, &shapeDef, &wallBox.base ); + } + + { + b3Transform transform; + transform.p = { -20.0f, 5.0f, 0.0f }; + transform.q = b3Quat_identity; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 0.1f, 5.0f, 20.0f, transform ); + shapeDef.name = "wall3"; + b3CreateHullShape( groundId, &shapeDef, &wallBox.base ); + } + + { + b3Transform transform; + transform.p = { 20.0f, 5.0f, 0.0f }; + transform.q = b3Quat_identity; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 0.1f, 5.0f, 20.0f, transform ); + shapeDef.name = "wall4"; + b3CreateHullShape( groundId, &shapeDef, &wallBox.base ); + } + + m_bodyIds = (b3BodyId*)malloc( bodyCapacity * sizeof( b3BodyId ) ); + memset( m_bodyIds, 0, bodyCapacity * sizeof( b3BodyId ) ); + m_bodyCount = 0; + + m_explosionDef = b3DefaultExplosionDef(); + m_explosionDef.radius = m_extent; + m_explosionDef.falloff = 0.5f * m_extent; + m_explosionDef.position = { 0.0f, 2.0f * m_extent, 0.0f }; + m_explosionDef.impulsePerArea = m_small ? 200.0f : 1000.0f; + + m_spawnMilliseconds = 0.0f; + m_destroyMilliseconds = 0.0f; + + Spawn(); + } + + ~BenchmarkDestruction() override + { + b3DestroyMesh( m_gridMesh ); + free( m_bodyIds ); + } + + void DestroyBodies() + { + uint64_t ticks = b3GetTicks(); + + for (int i = 0; i < m_bodyCount; ++i) + { + b3DestroyBody( m_bodyIds[i] ); + } + + m_destroyMilliseconds = b3GetMilliseconds( ticks ); + } + + void Spawn() + { + uint64_t ticks = b3GetTicks(); + + float a = m_extent / m_gridCount; + b3BoxHull box = b3MakeBoxHull( 0.8f * a, 0.8f * a, 0.8f * a ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + int randomRange = m_small ? 3 : 2; + m_bodyCount = 0; + for (int i = 0; i < m_gridCount; ++i) + { + for (int j = 0; j < m_gridCount; ++j) + { + for (int k = 0; k < m_gridCount; ++k) + { + if (RandomIntRange( 1, randomRange ) == 1) + { + continue; + } + + bodyDef.position = { ( 2.0f * i - m_gridCount + 1.0f ) * a, ( 2.0f * j + 1.0f ) * a, + ( 2.0f * k - m_gridCount + 1.0f ) * a }; + + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + m_bodyIds[m_bodyCount] = bodyId; + m_bodyCount += 1; + } + } + } + + b3World_Explode( m_worldId, &m_explosionDef ); + + m_spawnMilliseconds = b3GetMilliseconds( ticks ); + } + + void Render() override + { + Sample::Render(); + + b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 4.0f ); + + DrawTextLine( "spawn = %.2f ms", m_spawnMilliseconds ); + DrawTextLine( "destroy = %.2f ms", m_destroyMilliseconds ); + + float r = m_explosionDef.radius; + DrawSphere( m_scene, b3Transform_identity, { m_explosionDef.position, r }, b3_colorAqua ); + + float rf = r + m_explosionDef.falloff; + DrawSphere( m_scene, b3Transform_identity, { m_explosionDef.position, rf }, b3_colorCornsilk ); + } + + void Step() override + { + Sample::Step(); + + int spawnStep = m_small ? 80 : 140; + if (m_stepCount % spawnStep == 0) + { + DestroyBodies(); + Spawn(); + } + } + + static Sample* Create( SampleContext* context ) + { + return new BenchmarkDestruction( context ); + } + + static constexpr bool m_small = m_isDebug; + static constexpr int m_gridCount = m_small ? 6 : 20; + static constexpr float m_extent = m_small ? 0.75f : 2.5f; + + b3BodyId* m_bodyIds; + int m_bodyCount; + b3ExplosionDef m_explosionDef; + b3MeshData* m_gridMesh; + float m_spawnMilliseconds; + float m_destroyMilliseconds; +}; + +static int sampleDestruction = SampleManager::Register( "Benchmark", "Destruction", BenchmarkDestruction::Create ); + +class BenchmarkJunkyard : public Sample +{ +public: + explicit BenchmarkJunkyard( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 100.0f, b3Vec3_zero ); + context->debugDraw.drawJoints = false; + EnableGrid( m_scene, true ); + } + + CreateJunkyard( m_worldId ); + } + + void Step() override + { + StepJunkyard( m_worldId, m_stepCount ); + Sample::Step(); + } + + static Sample* Create( SampleContext* context ) + { + return new BenchmarkJunkyard( context ); + } +}; + +static int sampleJunkyard = SampleManager::Register( "Benchmark", "Junkyard", BenchmarkJunkyard::Create ); diff --git a/samples/sample_bodies.cpp b/samples/sample_bodies.cpp new file mode 100644 index 0000000..40d7ada --- /dev/null +++ b/samples/sample_bodies.cpp @@ -0,0 +1,965 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "GLFW/glfw3.h" +#include "camera.h" +#include "imgui.h" +#include "renderer.h" +#include "sample.h" +#include "scene.h" + +#include "box3d/box3d.h" + +class BodyType : public Sample +{ +public: + explicit BodyType( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 30.0f, 30.0f, { 0.0f, 1.5f, 0.0f } ); + } + + m_type = b3_dynamicBody; + m_isEnabled = true; + + b3BodyId groundId = b3_nullBodyId; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position.y = -1.0f; + bodyDef.name = "ground"; + groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 20.0f, 1.0f, 20.0f ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + + // Define attachment + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { -2.0f, 3.0f, 0.0f }; + bodyDef.name = "attach1"; + m_attachmentId = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 0.5f, 2.0f, 0.5f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + b3CreateHullShape( m_attachmentId, &shapeDef, &box.base ); + } + + // Define second attachment + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = m_type; + bodyDef.isEnabled = m_isEnabled; + bodyDef.position = { 3.0f, 3.0f }; + bodyDef.name = "attach2"; + m_secondAttachmentId = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 0.5f, 2.0f, 0.5f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + b3CreateHullShape( m_secondAttachmentId, &shapeDef, &box.base ); + } + + // Define platform + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = m_type; + bodyDef.isEnabled = m_isEnabled; + bodyDef.position = { -4.0f, 5.0f }; + bodyDef.name = "platform"; + m_platformId = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull box = b3MakeTransformedBoxHull( + 0.5f, 4.0f, 0.5f, { { 4.0f, 0.0f, 0.0f }, b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.5f * B3_PI ) } ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 2.0f; + b3CreateHullShape( m_platformId, &shapeDef, &box.base ); + + b3RevoluteJointDef revoluteDef = b3DefaultRevoluteJointDef(); + b3Vec3 pivot = { -2.0f, 5.0f, 0.0f }; + revoluteDef.base.bodyIdA = m_attachmentId; + revoluteDef.base.bodyIdB = m_platformId; + revoluteDef.base.localFrameA.p = b3Body_GetLocalPoint( m_attachmentId, pivot ); + revoluteDef.base.localFrameB.p = b3Body_GetLocalPoint( m_platformId, pivot ); + revoluteDef.maxMotorTorque = 50.0f; + revoluteDef.enableMotor = true; + b3CreateRevoluteJoint( m_worldId, &revoluteDef ); + + pivot = { 3.0f, 5.0f }; + revoluteDef.base.bodyIdA = m_secondAttachmentId; + revoluteDef.base.bodyIdB = m_platformId; + revoluteDef.base.localFrameA.p = b3Body_GetLocalPoint( m_secondAttachmentId, pivot ); + revoluteDef.base.localFrameB.p = b3Body_GetLocalPoint( m_platformId, pivot ); + revoluteDef.maxMotorTorque = 50.0f; + revoluteDef.enableMotor = true; + b3CreateRevoluteJoint( m_worldId, &revoluteDef ); + + b3PrismaticJointDef prismaticDef = b3DefaultPrismaticJointDef(); + b3Vec3 anchor = { 0.0f, 5.0f, 0.0f }; + prismaticDef.base.bodyIdA = groundId; + prismaticDef.base.bodyIdB = m_platformId; + prismaticDef.base.localFrameA.p = b3Body_GetLocalPoint( groundId, anchor ); + prismaticDef.base.localFrameB.p = b3Body_GetLocalPoint( m_platformId, anchor ); + prismaticDef.maxMotorForce = 1000.0f; + prismaticDef.motorSpeed = 0.0f; + prismaticDef.enableMotor = true; + prismaticDef.lowerTranslation = -10.0f; + prismaticDef.upperTranslation = 10.0f; + prismaticDef.enableLimit = true; + + b3CreatePrismaticJoint( m_worldId, &prismaticDef ); + + m_speed = 3.0f; + } + + // Create a payload + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { -3.0f, 8.0f }; + bodyDef.name = "crate1"; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 0.75f, 0.75f, 0.75f ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 2.0f; + + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + + // Create a second payload + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = m_type; + bodyDef.isEnabled = m_isEnabled; + bodyDef.position = { 2.0f, 8.0f }; + bodyDef.name = "crate2"; + m_secondPayloadId = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 0.75f, 0.75f, 0.75f ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 2.0f; + + b3CreateHullShape( m_secondPayloadId, &shapeDef, &box.base ); + } + + // Create a separate body on the ground + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = m_type; + bodyDef.isEnabled = m_isEnabled; + bodyDef.position = { 8.0f, 0.2f }; + bodyDef.name = "debris"; + m_touchingBodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3Capsule capsule = { { 0.0f, 0.0f }, { 1.0f, 0.0f }, 0.25f }; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 2.0f; + + b3CreateCapsuleShape( m_touchingBodyId, &shapeDef, &capsule ); + } + + // Create a separate floating body + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = m_type; + bodyDef.isEnabled = m_isEnabled; + bodyDef.position = { -8.0f, 12.0f }; + bodyDef.gravityScale = 0.0f; + bodyDef.name = "floater"; + m_floatingBodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3Sphere sphere = { { 0.0f, 0.5f, 0.0f }, 0.25f }; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 2.0f; + + b3CreateSphereShape( m_floatingBodyId, &shapeDef, &sphere ); + } + } + + void UpdateUI() override + { + float fontSize = ImGui::GetFontSize(); + float height = 11.0f * fontSize; + ImGui::SetNextWindowPos( ImVec2( 0.5f * fontSize, m_camera->m_height - height - 2.0f * fontSize ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 9.0f * fontSize, height ) ); + ImGui::Begin( "Body Type", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + if ( ImGui::RadioButton( "Static", m_type == b3_staticBody ) ) + { + m_type = b3_staticBody; + b3Body_SetType( m_platformId, b3_staticBody ); + b3Body_SetType( m_secondAttachmentId, b3_staticBody ); + b3Body_SetType( m_secondPayloadId, b3_staticBody ); + b3Body_SetType( m_touchingBodyId, b3_staticBody ); + b3Body_SetType( m_floatingBodyId, b3_staticBody ); + } + + if ( ImGui::RadioButton( "Kinematic", m_type == b3_kinematicBody ) ) + { + m_type = b3_kinematicBody; + b3Body_SetType( m_platformId, b3_kinematicBody ); + b3Body_SetLinearVelocity( m_platformId, { -m_speed, 0.0f } ); + b3Body_SetAngularVelocity( m_platformId, b3Vec3_zero ); + + b3Body_SetType( m_secondAttachmentId, b3_kinematicBody ); + b3Body_SetLinearVelocity( m_secondAttachmentId, b3Vec3_zero ); + b3Body_SetAngularVelocity( m_secondAttachmentId, b3Vec3_zero ); + + b3Body_SetType( m_secondPayloadId, b3_kinematicBody ); + b3Body_SetType( m_touchingBodyId, b3_kinematicBody ); + b3Body_SetType( m_floatingBodyId, b3_kinematicBody ); + } + + if ( ImGui::RadioButton( "Dynamic", m_type == b3_dynamicBody ) ) + { + m_type = b3_dynamicBody; + b3Body_SetType( m_platformId, b3_dynamicBody ); + b3Body_SetType( m_secondAttachmentId, b3_dynamicBody ); + b3Body_SetType( m_secondPayloadId, b3_dynamicBody ); + b3Body_SetType( m_touchingBodyId, b3_dynamicBody ); + b3Body_SetType( m_floatingBodyId, b3_dynamicBody ); + } + + if ( ImGui::Checkbox( "Enable", &m_isEnabled ) ) + { + if ( m_isEnabled ) + { + b3Body_Enable( m_attachmentId ); + b3Body_Enable( m_secondPayloadId ); + b3Body_Enable( m_floatingBodyId ); + } + else + { + b3Body_Disable( m_attachmentId ); + b3Body_Disable( m_secondPayloadId ); + b3Body_Disable( m_floatingBodyId ); + } + } + + ImGui::End(); + } + + void Step() override + { + // Drive the kinematic body. + if ( m_type == b3_kinematicBody ) + { + b3Vec3 p = b3Body_GetPosition( m_platformId ); + b3Vec3 v = b3Body_GetLinearVelocity( m_platformId ); + + if ( ( p.x < -14.0f && v.x < 0.0f ) || ( p.x > 6.0f && v.x > 0.0f ) ) + { + v.x = -v.x; + b3Body_SetLinearVelocity( m_platformId, v ); + } + } + + Sample::Step(); + } + + static Sample* Create( SampleContext* context ) + { + return new BodyType( context ); + } + + b3BodyId m_attachmentId; + b3BodyId m_secondAttachmentId; + b3BodyId m_platformId; + b3BodyId m_secondPayloadId; + b3BodyId m_touchingBodyId; + b3BodyId m_floatingBodyId; + b3BodyType m_type; + float m_speed; + bool m_isEnabled; +}; + +static int sampleBodyType = SampleManager::Register( "Bodies", "Body Type", BodyType::Create ); + +class SpinningBooks : public Sample +{ +public: + explicit SpinningBooks( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 30.0f, 10.0f, { 0.0f, 1.0f, 0.0f } ); + } + + b3BoxHull box = b3MakeBoxHull( 0.35f, 0.08f, 0.5f ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.gravityScale = 0.0f; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + bodyDef.position = { -2.0f, 2.0f, 0.0f }; + bodyDef.angularVelocity = { 5.0f, 0.01f, 0.01f }; + + b3BodyId body1 = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( body1, &shapeDef, &box.base ); + + bodyDef.position = { 0.0f, 2.0f, 0.0f }; + bodyDef.angularVelocity = { 0.01f, 5.0f, 0.01f }; + + b3BodyId body2 = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( body2, &shapeDef, &box.base ); + + bodyDef.position = { 2.0f, 2.0f, 0.0f }; + bodyDef.angularVelocity = { 0.01f, 0.01f, -5.0f }; + + b3BodyId body3 = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( body3, &shapeDef, &box.base ); + } + + void Render() override + { + DrawGrid( m_scene, 10 ); + Sample::Render(); + } + + static Sample* Create( SampleContext* context ) + { + return new SpinningBooks( context ); + } +}; + +static int sampleBook = SampleManager::Register( "Bodies", "Spinning Book", SpinningBooks::Create ); + +// Dzhanibekov effect +class GyroscopicTorque : public Sample +{ +public: + explicit GyroscopicTorque( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 20.0f, 4.0f, { 0.0f, 2.0f, 0.0f } ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 2.0f, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisX, -0.5f * B3_PI ); + bodyDef.angularVelocity = { 0.01f, 0.01f, 10.0f }; + bodyDef.gravityScale = 0.0f; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3Hull* cylinder = b3CreateCylinder( 0.6f, 0.15f, 0.0f, 32 ); + b3BoxHull box = b3MakeBoxHull( 1.0f, 0.05f, 0.1f ); + b3CreateHullShape( bodyId, &shapeDef, cylinder ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + b3DestroyHull( cylinder ); + } + + void Render() override + { + Sample::Render(); + DrawGrid( m_scene, 10 ); + } + + static Sample* Create( SampleContext* sampleContext ) + { + return new GyroscopicTorque( sampleContext ); + } +}; + +static int sampleGyroscopicTorque = SampleManager::Register( "Bodies", "Gyroscopic Torque", GyroscopicTorque::Create ); + +class Weeble : public Sample +{ +public: + static Sample* Create( SampleContext* context ) + { + return new Weeble( context ); + } + + explicit Weeble( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 45.0f, 25.0f, 25.0f, b3Vec3_zero ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundBody = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 32.0f, 1.0f, 32.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( groundBody, &shapeDef, &box.base ); + + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 3.0f, 0.0f }; + m_weebleId = b3CreateBody( m_worldId, &bodyDef ); + + b3Capsule capsule = { { 0.0f, -1.0f }, { 0.0f, 1.0f }, 1.0f }; + shapeDef.baseMaterial.rollingResistance = 0.1f; + b3CreateCapsuleShape( m_weebleId, &shapeDef, &capsule ); + + float mass = b3Body_GetMass( m_weebleId ); + b3Matrix3 inertiaTensor = b3Body_GetLocalRotationalInertia( m_weebleId ); + + b3Vec3 offset = { 0.0f, -1.5f, 0.0f }; + + // See: https://en.wikipedia.org/wiki/Parallel_axis_theorem + inertiaTensor += b3Steiner( mass, offset ); + + b3MassData massData = { + .mass = mass, + .center = offset, + .inertia = inertiaTensor, + }; + + b3Body_SetMassData( m_weebleId, massData ); + + m_explosionPosition = { 0.0f, -0.1f, 0.0f }; + m_explosionRadius = 8.0f; + m_explosionMagnitude = 10000.0f; + } + + void UpdateUI() override + { + float height = 120.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 200.0f, height ) ); + + ImGui::Begin( "Weeble", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + if ( ImGui::Button( "Teleport" ) ) + { + b3Body_SetTransform( m_weebleId, { 0.0f, 5.0f, 0.0f }, b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.95f * B3_PI ) ); + b3Body_SetAwake( m_weebleId, true ); + } + + if ( ImGui::Button( "Explode" ) ) + { + b3ExplosionDef def = b3DefaultExplosionDef(); + def.position = m_explosionPosition; + def.radius = m_explosionRadius; + def.falloff = 0.1f; + def.impulsePerArea = m_explosionMagnitude; + b3World_Explode( m_worldId, &def ); + } + ImGui::PushItemWidth( 100.0f ); + + ImGui::SliderFloat( "Magnitude", &m_explosionMagnitude, -100000.0f, 100000.0f, "%.0f" ); + + ImGui::PopItemWidth(); + ImGui::End(); + } + + void Render() override + { + Sample::Render(); + b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 4.0f ); + } + + void Step() override + { + Sample::Step(); + + b3Sphere sphere = { b3Vec3_zero, m_explosionRadius }; + DrawSphere( m_scene, { m_explosionPosition, b3Quat_identity }, sphere, b3_colorAzure ); + + // This shows how to get the velocity of a point on a body + b3Vec3 localPoint = { 0.0f, 2.0f, 0.0f }; + b3Vec3 worldPoint = b3Body_GetWorldPoint( m_weebleId, localPoint ); + + b3Vec3 v1 = b3Body_GetLocalPointVelocity( m_weebleId, localPoint ); + b3Vec3 v2 = b3Body_GetWorldPointVelocity( m_weebleId, worldPoint ); + + b3Vec3 offset = { 0.05f, 0.0f }; + DrawLine( m_scene, worldPoint, worldPoint + v1, b3_colorRed ); + DrawLine( m_scene, worldPoint + offset, worldPoint + v2 + offset, b3_colorGreen ); + } + + b3BodyId m_weebleId; + b3Vec3 m_explosionPosition; + float m_explosionRadius; + float m_explosionMagnitude; +}; + +static int sampleWeeble = SampleManager::Register( "Bodies", "Weeble", Weeble::Create ); + +class DisableBody : public Sample +{ +public: + enum + { + e_count = 4 + }; + + explicit DisableBody( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 45.0f, 25.0f, 10.0f, b3Vec3_zero ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + + b3BodyId groundBody = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeBoxHull( 25.0f, 1.0f, 25.0f ); + b3CreateHullShape( groundBody, &shapeDef, &box.base ); + + float linkRadius = 0.1f; + float linkLength = 5.0f * linkRadius; + b3Capsule capsule = { { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, linkLength }, linkRadius }; + + b3BodyId parentId = {}; + for ( int link = 0; link < e_count; ++link ) + { + bodyDef.position = { 0.0f, 0.8f * float( e_count ) * linkLength, link * linkLength }; + bodyDef.type = B3_IS_NULL( parentId ) ? b3_kinematicBody : b3_dynamicBody; + b3BodyId childId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateCapsuleShape( childId, &shapeDef, &capsule ); + m_bodyIds[link] = childId; + + if ( B3_IS_NON_NULL( parentId ) ) + { + b3WeldJointDef jointDef = b3DefaultWeldJointDef(); + jointDef.base.bodyIdA = parentId; + jointDef.base.bodyIdB = childId; + jointDef.base.localFrameA.p = { 0.0f, 0.0f, linkLength }; + jointDef.angularHertz = 10.0f; + jointDef.angularDampingRatio = 1.0f; + b3CreateWeldJoint( m_worldId, &jointDef ); + } + + parentId = childId; + } + + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 3.0f, 3.0f, 0.0f }; + m_ballId = b3CreateBody( m_worldId, &bodyDef ); + + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f }; + b3CreateSphereShape( m_ballId, &shapeDef, &sphere ); + } + + void UpdateUI() override + { + float height = 100.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 200.0f, height ) ); + + ImGui::Begin( "Distance", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + { + bool enabled = b3Body_IsEnabled( m_bodyIds[2] ); + if ( ImGui::Checkbox( "Enable Link", &enabled ) ) + { + if ( enabled ) + { + b3Body_Enable( m_bodyIds[2] ); + } + else + { + b3Body_Disable( m_bodyIds[2] ); + } + } + } + + { + bool enabled = b3Body_IsEnabled( m_ballId ); + if ( ImGui::Checkbox( "Enable Ball", &enabled ) ) + { + if ( enabled ) + { + b3Body_Enable( m_ballId ); + } + else + { + b3Body_Disable( m_ballId ); + } + } + } + + ImGui::End(); + } + + void Step() override + { + b3Body_ApplyLinearImpulseToCenter( m_bodyIds[2], { 0.0f, 0.1f, 0.0f }, true ); + + Sample::Step(); + } + + static Sample* Create( SampleContext* context ) + { + return new DisableBody( context ); + } + + b3BodyId m_bodyIds[e_count]; + b3BodyId m_ballId; +}; + +static int sampleDisable = SampleManager::Register( "Bodies", "Disable", DisableBody::Create ); + +class BodyCast : public Sample +{ +public: + explicit BodyCast( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 120.0f, 30.0f, 20.0f, { 0.0f, 1.5f, 0.0f } ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_kinematicBody; + bodyDef.position = { 5.0f, 5.0f, 0.0f }; + bodyDef.angularVelocity = { 0.1f, -0.1f, 0.1f }; + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + + m_cylinder = b3CreateCylinder( 2.0f, 0.5f, 0.0f, 16 ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( m_bodyId, &shapeDef, m_cylinder ); + + m_transform.p = { -10.0f, 2.0f, 0.0f }; + m_transform.q = b3MakeQuatFromAxisAngle( b3Normalize( { 1.0f, -2.0f, 3.0f } ), 0.75f * B3_PI ); + + m_baseTranslation = b3Vec3_zero; + m_baseX = 0; + m_baseY = 0; + m_origin = b3Vec3_zero; + m_tracking = false; + } + + ~BodyCast() override + { + b3DestroyHull( m_cylinder ); + } + + void MouseDown( b3Vec2 p, int button, int modifiers ) override + { + if ( button == 0 && modifiers == GLFW_MOD_ALT ) + { + PickRay pickRay = m_camera->BuildPickRay( p.x, p.y ); + m_origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation ); + m_baseTranslation = m_transform.p; + m_tracking = true; + } + else + { + m_tracking = false; + } + } + + void MouseMove( b3Vec2 p ) override + { + if ( m_tracking ) + { + PickRay pickRay = m_camera->BuildPickRay( p.x, p.y ); + b3Vec3 origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation ); + m_transform.p = m_baseTranslation + origin - m_origin; + } + } + + void Render() override + { + Sample::Render(); + + DrawGrid( m_scene, 10 ); + b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 4.0f ); + + DrawHull( m_scene, m_transform, m_cylinder, b3_colorBlue, false ); + } + + void Step() override + { + // Cast ray + { + b3BodyRayCastInput input = {}; + input.origin = { -9.75f, 3.0f, -4.0f }; + input.translation = { 0.0f, 0.0f, 8.0f }; + input.filter = b3DefaultQueryFilter(); + input.maxFraction = 1.0f; + b3BodyCastResult result = b3Body_CastRay( m_bodyId, &input, m_transform ); + + DrawLine( m_scene, input.origin, input.origin + input.maxFraction * input.translation, b3_colorCyan ); + + if ( result.hit ) + { + DrawLine( m_scene, result.point, result.point + 0.2f * result.normal, b3_colorYellow ); + DrawPoint( m_scene, result.point, 10.0f, b3_colorYellow ); + } + + DrawPoint( m_scene, input.origin, 10.0f, b3_colorGreen ); + DrawPoint( m_scene, input.origin + input.translation, 10.0f, b3_colorRed ); + } + + // Cast sphere + { + b3Sphere sphere = { { -14.5f, 2.5f, 0.5f }, 0.2f }; + + b3BodyShapeCastInput input = {}; + input.proxy = { &sphere.center, 1, sphere.radius }; + input.translation = { 8.0f, 0.0f, 0.0f }; + input.filter = b3DefaultQueryFilter(); + input.maxFraction = 1.0f; + input.canEncroach = true; + b3BodyCastResult result = b3Body_CastShape( m_bodyId, &input, m_transform ); + + if ( result.hit ) + { + b3Transform transform = { result.fraction * input.translation, b3Quat_identity }; + DrawSphere( m_scene, transform, sphere, b3_colorGreen ); + DrawLine( m_scene, result.point, result.point + 0.2f * result.normal, b3_colorYellow ); + } + else + { + b3Transform transform = { input.maxFraction * input.translation, b3Quat_identity }; + DrawSphere( m_scene, transform, sphere, b3_colorWhite ); + } + + DrawLine( m_scene, sphere.center, sphere.center + input.maxFraction * input.translation, b3_colorWhite ); + DrawPoint( m_scene, sphere.center, 10.0f, b3_colorGreen ); + DrawPoint( m_scene, sphere.center + input.maxFraction * input.translation, 10.0f, b3_colorRed ); + } + + // Overlap capsule + { + b3Capsule capsule = { { -10.5f, 2.0f, 0.5f }, { -9.5f, 1.0f, 0.5f }, 0.5f }; + b3ShapeProxy proxy = { &capsule.center1, 2, capsule.radius }; + bool overlaps = b3Body_OverlapShape( m_bodyId, &proxy, b3DefaultQueryFilter(), m_transform ); + + if ( overlaps ) + { + DrawCapsule( m_scene, b3Transform_identity, capsule, b3_colorGreen ); + } + else + { + DrawCapsule( m_scene, b3Transform_identity, capsule, b3_colorGray ); + } + } + + // Collide capsule + { + b3Capsule capsule = { { -10.25f, 2.0f, -0.75f }, { -10.25f, 3.0f, -0.75f }, 0.3f }; + b3BodyPlaneResult bodyPlanes[4]; + int count = b3Body_CollideMover( m_bodyId, bodyPlanes, 4, &capsule, b3DefaultQueryFilter(), m_transform ); + DrawCapsule( m_scene, b3Transform_identity, capsule, b3_colorPurple ); + + for ( int i = 0; i < count; ++i ) + { + b3PlaneResult result = bodyPlanes[i].result; + DrawPlane( m_scene, result.plane.normal, result.point, b3_colorOrange ); + } + } + } + + static Sample* Create( SampleContext* context ) + { + return new BodyCast( context ); + } + + b3Hull* m_cylinder; + b3BodyId m_bodyId; + b3Transform m_transform; + + b3Vec3 m_baseTranslation; + b3Vec3 m_origin; + int m_baseX; + int m_baseY; + bool m_tracking; +}; + +static int sampleBodyCast = SampleManager::Register( "Bodies", "Cast", BodyCast::Create ); + +// This shows how to drive a kinematic body to reach a target +class Kinematic : public Sample +{ +public: + explicit Kinematic( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 30.0f, 10.0f, { 0.0f, 1.5f, 0.0f } ); + } + + m_amplitude = 2.0f; + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_kinematicBody; + bodyDef.position.x = 2.0f * m_amplitude; + bodyDef.position.y = m_amplitude; + + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 0.1f, 1.0f, 0.2f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( m_bodyId, &shapeDef, &box.base ); + } + + m_time = 0.0f; + } + + void Render() override + { + Sample::Render(); + DrawGrid( m_scene, 10 ); + DrawTransform( m_scene, b3Transform_identity, 4.0f ); + } + + void Step() override + { + float timeStep = m_context->hertz > 0.0f ? 1.0f / m_context->hertz : 0.0f; + if ( m_context->pause && m_context->singleStep == 0 ) + { + timeStep = 0.0f; + } + + float delay = 2.0f; + + if ( timeStep > 0.0f && m_time > delay ) + { + float t = m_time - delay; + + b3Vec3 point; + point.x = 2.0f * m_amplitude * cosf( t ); + point.y = m_amplitude * ( sinf( 2.0f * t ) + 1.0f ); + point.z = 0.0f; + b3Quat rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 2.0f * t ); + + b3Vec3 axis = b3RotateVector( rotation, { 0.0f, 1.0f, 0.0f } ); + DrawLine( m_scene, point - 0.5f * axis, point + 0.5f * axis, b3_colorPlum ); + DrawPoint( m_scene, point, 10.0f, b3_colorPlum ); + + b3Body_SetTargetTransform( m_bodyId, { point, rotation }, timeStep, true ); + } + + Sample::Step(); + + m_time += timeStep; + } + + static Sample* Create( SampleContext* context ) + { + return new Kinematic( context ); + } + + b3BodyId m_bodyId; + float m_amplitude; + float m_time; +}; + +static int sampleKinematic = SampleManager::Register( "Bodies", "Kinematic", Kinematic::Create ); + +class LockMixing : public Sample +{ +public: + explicit LockMixing( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 40.0f, b3Vec3_zero ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull hull = b3MakeBoxHull( 15.0f, 1.0f, 15.0f ); + b3CreateHullShape( groundId, &shapeDef, &hull.base ); + } + + b3BoxHull cube = b3MakeBoxHull( 1.0f, 1.0f, 1.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.name = "free"; + bodyDef.position = { 0.0f, 2.0f, 0.0f }; + + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &cube.base ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.name = "angular xz"; + bodyDef.position = { 2.0f, 2.0f, 0.0f }; + bodyDef.motionLocks.angularX = true; + //bodyDef.motionLocks.angularY = true; + bodyDef.motionLocks.angularZ = true; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &cube.base ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.name = "linear xyz"; + bodyDef.position = { -2.0f, 2.0f, 0.0f }; + bodyDef.motionLocks.linearX = true; + bodyDef.motionLocks.linearY = true; + bodyDef.motionLocks.linearZ = true; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &cube.base ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.name = "full"; + bodyDef.position = { 0.0f, 1.0f, 2.0f }; + bodyDef.motionLocks.linearX = true; + bodyDef.motionLocks.linearY = true; + bodyDef.motionLocks.linearZ = true; + bodyDef.motionLocks.angularX = true; + bodyDef.motionLocks.angularY = true; + bodyDef.motionLocks.angularZ = true; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &cube.base ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "static"; + bodyDef.position = { 0.0f, 1.0f, -3.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &cube.base ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new LockMixing( context ); + } +}; + +static int sampleLockMixing = SampleManager::Register( "Bodies", "Lock Mixing", LockMixing::Create ); diff --git a/samples/sample_character.cpp b/samples/sample_character.cpp new file mode 100644 index 0000000..d05b07f --- /dev/null +++ b/samples/sample_character.cpp @@ -0,0 +1,438 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "camera.h" +#include "mesh_loader.h" +#include "renderer.h" +#include "sample.h" +#include "scene.h" + +#include "box3d/box3d.h" + +#include +// prevent clang format sorting glad.h with glfw3.h +#include +#include + +class CapsulePlane : public Sample +{ +public: + static Sample* Create( SampleContext* context ) + { + return new CapsulePlane( context ); + } + + explicit CapsulePlane( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 120.0f, 30.0f, 20.0f, { 0.0f, 1.5f, 0.0f } ); + } + + m_transform = { { 0.0f, 1.0f, 0.4f }, b3Quat_identity }; + m_capsule = { { 0.0f, -0.5f, 0.0f }, { 0.0f, 0.5f, 0.0f }, 0.25f }; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, 1.0f, 1.0f }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3CreateHullShape( body, &shapeDef, &box.base ); + + m_baseTranslation = b3Vec3_zero; + m_baseX = 0; + m_baseY = 0; + m_origin = b3Vec3_zero; + m_tracking = false; + m_planeCount = 0; + } + + void Solve() + { + b3PlaneSolverResult result = b3SolvePlanes( b3Vec3_zero, m_planes, m_planeCount ); + m_transform.p += result.delta; + } + + void Render() override + { + Sample::Render(); + + DrawGrid( m_scene, 10 ); + DrawLine( m_scene, b3Vec3_zero, 2.0f * b3Vec3_axisX, b3_colorRed ); + DrawLine( m_scene, b3Vec3_zero, 2.0f * b3Vec3_axisY, b3_colorGreen ); + DrawLine( m_scene, b3Vec3_zero, 2.0f * b3Vec3_axisZ, b3_colorBlue ); + } + + void UpdateUI() override + { + ImGui::SetNextWindowPos( ImVec2( 10.0f, 600.0f ) ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, 80.0f ) ); + ImGui::Begin( "Capsule Plane", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + if ( ImGui::Button( "Solve" ) ) + { + Solve(); + } + + ImGui::End(); + } + + static bool PlaneResultFcn( b3ShapeId shape, const b3PlaneResult* results, int planeCount, void* context ) + { + CapsulePlane* self = static_cast( context ); + for ( int i = 0; i < planeCount && self->m_planeCount < 3; ++i ) + { + self->m_planes[self->m_planeCount] = { results[i].plane, FLT_MAX, 0.0f, true }; + self->m_planeCount += 1; + } + return true; + } + + void MouseDown( b3Vec2 p, int button, int modifiers ) override + { + if ( button == 0 && ( modifiers & GLFW_MOD_ALT ) == 0 ) + { + PickRay pickRay = m_camera->BuildPickRay( p.x, p.y ); + m_origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation ); + m_baseTranslation = m_transform.p; + m_tracking = true; + } + } + + void MouseUp( b3Vec2 p, int button ) override + { + m_tracking = false; + } + + void MouseMove( b3Vec2 p ) override + { + if ( m_tracking ) + { + PickRay pickRay = m_camera->BuildPickRay( p.x, p.y ); + b3Vec3 origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation ); + m_transform.p = m_baseTranslation + origin - m_origin; + } + } + + void Step() override + { + m_planeCount = 0; + + DrawCapsule( m_scene, m_transform, m_capsule, b3_colorGreen ); + b3QueryFilter filter = b3DefaultQueryFilter(); + + b3Capsule capsule = { m_capsule.center1 + m_transform.p, m_capsule.center2 + m_transform.p, m_capsule.radius }; + b3World_CollideMover( m_worldId, &capsule, filter, PlaneResultFcn, this ); + + for ( int i = 0; i < m_planeCount; ++i ) + { + b3Plane plane = m_planes[i].plane; + b3Vec3 p1 = m_transform.p + ( plane.offset - m_capsule.radius ) * plane.normal; + b3Vec3 p2 = p1 + 0.1f * plane.normal; + DrawPoint( m_scene, p1, 5.0f, b3_colorYellow ); + DrawLine( m_scene, p1, p2, b3_colorYellow ); + } + } + + static constexpr int m_planeCapacity = 3; + + b3Transform m_transform; + b3Capsule m_capsule; + b3CollisionPlane m_planes[m_planeCapacity] = {}; + int m_planeCount; + + b3Vec3 m_baseTranslation; + b3Vec3 m_origin; + int m_baseX; + int m_baseY; + bool m_tracking; +}; + +static int sampleCapsulePlane = SampleManager::Register( "Character", "CapsulePlane", CapsulePlane::Create ); + +class BasicMover : public Sample +{ +public: + explicit BasicMover( SampleContext* context ) + : Sample( context ) + { + b3Vec3 moverPosition = { 7.5f, 0.75f, 9.0f }; + + if ( m_context->restart == false ) + { + m_camera->SetView( 120.0f, 30.0f, 5.0f, moverPosition ); + } + + m_mover.Initialize( this, moverPosition ); + + { + m_levelMesh = CreateMeshData( "data/meshes/test_map01.obj", 1.0f, false, false, true, true ); + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3SurfaceMaterial materials[3]; + materials[0] = { 0.6f, 0.0f, 0 }; + materials[1] = { 0.6f, 1.0f, 1 }; + materials[2] = { 0.1f, 0.0f, 2 }; + shapeDef.materials = materials; + shapeDef.materialCount = 3; + + b3CreateMeshShape( body, &shapeDef, m_levelMesh, b3Vec3_one ); + + // b3Transform transform = { { 0.0f, 1.0f, 14.0f }, b3MakeQuatFromAxisAngle( b3Vec3_axisY, 0.75f * B3_PI ) }; + // b3BoxHull box = b3MakeTransformedBoxHull( 5.0f, 1.0f, 0.5f, transform ); + // b3CreateHullShape( body, &shapeDef, &box.base ); + + { + b3Transform transform = { { 4.0f, 1.0f, 14.0f }, b3Quat_identity }; + b3BoxHull box = b3MakeTransformedBoxHull( 1.0f, 1.0f, 1.0f, transform ); + b3CreateHullShape( body, &shapeDef, &box.base ); + } + { + b3Transform transform = { { 4.0f, 1.0f, 13.95f }, b3Quat_identity }; + b3BoxHull box = b3MakeTransformedBoxHull( 1.0f, 1.0f, 1.0f, transform ); + b3CreateHullShape( body, &shapeDef, &box.base ); + } + { + b3Transform transform = { { 5.8f, 1.0f, 13.7f }, b3MakeQuatFromAxisAngle( b3Vec3_axisY, 0.1f * B3_PI ) }; + b3BoxHull box = b3MakeTransformedBoxHull( 1.0f, 1.0f, 1.0f, transform ); + b3CreateHullShape( body, &shapeDef, &box.base ); + } + } + + { + m_stairs = CreateMeshData( "data/meshes/stairs.obj", 1.0f, false, false, true, true ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { -10.0f, 0.0f, 0.0f }; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + b3CreateMeshShape( body, &shapeDef, m_stairs, { 0.75f, 0.75f, -1.5f } ); + } + + { + m_torus = b3CreateTorusMesh( 10, 12, 2.0f, 1.0f ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { -10.0f, 1.0f, -8.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisY, 0.5f * B3_PI ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + b3CreateMeshShape( body, &shapeDef, m_torus, { -0.75f, 1.5f, 0.5f } ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 20.0f, 0.0f, 0.0f }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + + m_heightField = b3CreateWave( 50.0f, 50.0f, b3Vec3_one, 0.02f, 0.04f, true ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3SurfaceMaterial materials[3]; + materials[0] = { 0.6f, 0.0f, 0 }; + materials[1] = { 0.6f, 1.0f, 1 }; + materials[2] = { 0.1f, 0.0f, 2 }; + shapeDef.materials = materials; + shapeDef.materialCount = 3; + + b3CreateHeightFieldShape( body, &shapeDef, m_heightField ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, 1.4f, 6.0f }; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + m_enemyShape.maxPush = 1.0f; + m_enemyShape.clipVelocity = true; + + b3Capsule capsule = { + { 0.0f, -0.5f, 0.0f }, + { 0.0f, 0.5f, 0.0f }, + 0.3f, + }; + + //shapeDef.filter = { 2u, ~0u, 0 }; + shapeDef.userData = &m_enemyShape; + shapeDef.baseMaterial.customColor = b3_colorMediumVioletRed; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + b3CreateCapsuleShape( body, &shapeDef, &capsule ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, 1.4f, 5.0f }; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + m_friendlyShape.maxPush = 0.01f; + m_friendlyShape.clipVelocity = false; + + b3Capsule capsule = { + { 0.0f, -0.5f, 0.0f }, + { 0.0f, 0.5f, 0.0f }, + 0.3f, + }; + + shapeDef.filter = { 2u, ~0u, 0 }; + shapeDef.userData = &m_friendlyShape; + shapeDef.baseMaterial.customColor = b3_colorLimeGreen; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + b3CreateCapsuleShape( body, &shapeDef, &capsule ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 7.0f, 5.0f, 0.0f }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3Sphere sphere = { b3Vec3_zero, 0.5f }; + b3CreateSphereShape( body, &shapeDef, &sphere ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 7.0f, 2.0f, -3.0f }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.customColor = b3_colorFloralWhite; + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.25f, 0.5f ); + m_ignoreShapeId = b3CreateHullShape( body, &shapeDef, &box.base ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + bodyDef.type = b3_dynamicBody; + bodyDef.position = { -2.0f, 1.6f, 0.0f }; + bodyDef.gravityScale = 2.0f; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1000.0f; + + b3BoxHull box = b3MakeBoxHull( 0.75f, 1.5f, 0.1f ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + b3Quat axisQuat = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisZ, b3Vec3_axisY ); + b3Vec3 offset = { -0.75f, 0.0f, 0.0f }; + + b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = bodyId; + jointDef.base.localFrameA.p = b3Add( bodyDef.position, offset ); + jointDef.base.localFrameA.q = axisQuat; + jointDef.base.localFrameB.p = offset; + jointDef.base.localFrameB.q = axisQuat; + + jointDef.enableLimit = true; + jointDef.lowerAngle = B3_DEG_TO_RAD * -90.0f; + jointDef.upperAngle = B3_DEG_TO_RAD * 90.0f; + jointDef.enableSpring = true; + jointDef.hertz = 1.0f; + jointDef.dampingRatio = 0.5f; + jointDef.enableMotor = false; + jointDef.maxMotorTorque = 100.0f; + jointDef.base.drawScale = 2.0f; + + b3CreateRevoluteJoint( m_worldId, &jointDef ); + } + + //{ + // b3BodyDef BodyDef = b3DefaultBodyDef(); + // BodyDef.type = b3_dynamicBody; + // BodyDef.position = { 25.0f, 5.0f, 10.0f }; + // b3BodyId Body = b3CreateBody(m_worldId, &bodyDef ); + + // b3ShapeDef ShapeDef = b3DefaultShapeDef(); + // b3Sphere Sphere = { b3Vec3_zero, 0.5f }; + // Body->AddSphere( &ShapeDef, Sphere ); + //} + + m_camera->m_thirdPerson = true; + m_clipVelocity = true; + glfwSetInputMode( m_window, GLFW_CURSOR, GLFW_CURSOR_DISABLED ); + + //m_haveMouseLast = false; + //m_mouseLast = { 0.0f, 0.0f }; + //m_mouseDelta = { 0.0f, 0.0f }; + } + + ~BasicMover() override + { + m_camera->m_thirdPerson = false; + glfwSetInputMode( m_window, GLFW_CURSOR, GLFW_CURSOR_NORMAL ); + b3DestroyMesh( m_levelMesh ); + b3DestroyMesh( m_stairs ); + b3DestroyMesh( m_torus ); + b3DestroyHeightField( m_heightField ); + } + + void Render() override + { + Sample::Render(); + DrawTransform( m_scene, { { 0.0f, 0.0f, 0.02f }, b3Quat_identity }, 2.0f ); + } + + void UpdateUI() override + { + float fontSize = ImGui::GetFontSize(); + float height = 9.0f * fontSize; + ImGui::SetNextWindowPos( ImVec2( 0.5f * fontSize, m_camera->m_height - height - 2.0f * fontSize ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 19.0f * fontSize, height ) ); + + ImGui::Begin( "Mover", nullptr, ImGuiWindowFlags_NoResize ); + + bool thirdPerson = m_camera->m_thirdPerson; + if ( ImGui::Checkbox( "Third Person (Key: T)", &thirdPerson ) ) + { + ToggleThirdPerson(); + } + + ImGui::Checkbox( "Clip Velocity", &m_clipVelocity ); + + ImGui::End(); + } + + void Keyboard( int key, int action, int mods ) override + { + if ( key == GLFW_KEY_T && action == GLFW_PRESS ) + { + ToggleThirdPerson(); + } + } + + void Step() override + { + m_mover.Step(&m_ignoreShapeId, 1, m_clipVelocity); + DrawTextLine( "third person (T) = %d", m_camera->m_thirdPerson ); + DrawTextLine( "deltaX = %g, deltaY = %g", m_mouseDelta.x, m_mouseDelta.y ); + + Sample::Step(); + } + + static Sample* Create( SampleContext* context ) + { + return new BasicMover( context ); + } + + CharacterMover m_mover; + MoverShapeUserData m_enemyShape; + MoverShapeUserData m_friendlyShape; + b3MeshData* m_levelMesh; + b3MeshData* m_stairs; + b3MeshData* m_torus; + b3HeightField* m_heightField; + b3ShapeId m_ignoreShapeId; + bool m_clipVelocity; +}; + +static int sampleMover = SampleManager::Register( "Character", "Mover", BasicMover::Create ); diff --git a/samples/sample_collision.cpp b/samples/sample_collision.cpp new file mode 100644 index 0000000..3622e4f --- /dev/null +++ b/samples/sample_collision.cpp @@ -0,0 +1,2821 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "GLFW/glfw3.h" +#include "camera.h" +#include "utils.h" +#include "renderer.h" +#include "sample.h" +#include "scene.h" + +#include "box3d/box3d.h" + +#include + +class RayCurtain : public Sample +{ +public: + static Sample* Create( SampleContext* context ) + { + return new RayCurtain( context ); + } + + explicit RayCurtain( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 20.0f ); + } + + b3BoxHull box = b3MakeBoxHull( 0.6f, 0.6f, 0.6f ); + + m_mesh = b3CreateTorusMesh( 10, 12, 0.65f, 0.35f ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_kinematicBody; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + bodyDef.position = { -6.0f, 3.0f, 0.0f }; + bodyDef.angularVelocity = { 0.8f, 0.4f, 0.8f }; + b3BodyId sphereBody = b3CreateBody( m_worldId, &bodyDef ); + b3Sphere sphere = { b3Vec3_zero, 0.9f }; + b3CreateSphereShape( sphereBody, &shapeDef, &sphere ); + + bodyDef.position = { -2.0f, 3.0f, 0.0f }; + bodyDef.angularVelocity = { 0.8f, 0.4f, 0.8f }; + b3BodyId capsuleBody = b3CreateBody( m_worldId, &bodyDef ); + b3Capsule capsule = { { -0.5f, 0.0f, 0.0f }, { 0.5f, 0.0f, 0.0f }, 0.8f }; + b3CreateCapsuleShape( capsuleBody, &shapeDef, &capsule ); + + bodyDef.position = { 2.0f, 3.0f, 0.0f }; + bodyDef.angularVelocity = { 0.8f, 0.4f, 0.8f }; + b3BodyId hullBody = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( hullBody, &shapeDef, &box.base ); + + bodyDef.position = { 6.0f, 3.0f, 0.0f }; + bodyDef.angularVelocity = { 0.8f, 0.4f, 0.8f }; + b3BodyId meshBody = b3CreateBody( m_worldId, &bodyDef ); + b3CreateMeshShape( meshBody, &shapeDef, m_mesh, b3Vec3_one ); + + m_absSpeed = 0.015f; + m_offset = 2.0f; + m_speed = -m_absSpeed; + } + + ~RayCurtain() override + { + b3DestroyMesh( m_mesh ); + } + + void Render() override + { + Sample::Render(); + + DrawGrid( m_scene, 10 ); + DrawLine( m_scene, b3Vec3_zero, 0.4f * b3Vec3_axisX, b3_colorRed ); + DrawLine( m_scene, b3Vec3_zero, 0.4f * b3Vec3_axisY, b3_colorGreen ); + DrawLine( m_scene, b3Vec3_zero, 0.4f * b3Vec3_axisZ, b3_colorBlue ); + + for ( float x = -8.0f; x <= 8.0f; x += 0.1f ) + { + b3Vec3 rayOrigin = { x, 8.0f, m_offset }; + b3Vec3 rayTranslation = b3Vec3{ x, 0.0f, m_offset } - rayOrigin; + + b3RayResult result = b3World_CastRayClosest( m_worldId, rayOrigin, rayTranslation, b3DefaultQueryFilter() ); + if ( result.hit ) + { + b3Vec3 point = result.point; + + DrawLine( m_scene, point, point + 0.5f * result.normal, b3_colorGreen ); + } + + DrawPoint( m_scene, rayOrigin, 4.0f, b3_colorGreen ); + DrawPoint( m_scene, rayOrigin + rayTranslation, 4.0f, b3_colorRed ); + DrawLine( m_scene, rayOrigin, rayOrigin + rayTranslation, b3_colorYellow ); + } + + if ( m_offset > 2.0f ) + { + m_speed = -m_absSpeed; + } + else if ( m_offset < -2.0f ) + { + m_speed = m_absSpeed; + } + + if ( m_context->pause == false ) + { + m_offset += m_speed; + } + } + + float m_offset; + float m_absSpeed; + float m_speed; + b3MeshData* m_mesh; +}; + +static int sampleRayCurtain = SampleManager::Register( "Collision", "Ray Curtain", RayCurtain::Create ); + +// Context for ray cast callbacks. Do what you want with this. +struct CastContext +{ + b3Vec3 points[3]; + b3Vec3 normals[3]; + float fractions[3]; + uint64_t materialIds[3]; + int triangleIndices[3]; + int childIndices[3]; + int count; + bool initialOverlap; +}; + +// This callback finds the closest hit. This is the most common callback used in games. +static float RayCastClosestCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t materialId, + int triangleIndex, int childIndex, void* context ) +{ + CastContext* rayContext = (CastContext*)context; + + // Check for initial overlap + if ( rayContext->initialOverlap == false && fraction == 0.0f ) + { + // By returning -1, we instruct the calling code to ignore this shape and + // continue the ray-cast to the next shape. + return -1.0f; + } + + // Ignore a specific shape. Also ignore initial overlap. + intptr_t ignoreFlag = intptr_t( b3Shape_GetUserData( shapeId ) ); + if ( ignoreFlag == 1 ) + { + // By returning -1, we instruct the calling code to ignore this shape and + // continue the ray-cast to the next shape. + return -1.0f; + } + + rayContext->points[0] = point; + rayContext->normals[0] = normal; + rayContext->fractions[0] = fraction; + rayContext->materialIds[0] = materialId; + rayContext->triangleIndices[0] = triangleIndex; + rayContext->childIndices[0] = childIndex; + rayContext->count = 1; + + // By returning the current fraction, we instruct the calling code to clip the ray and + // continue the ray-cast to the next shape. WARNING: do not assume that shapes + // are reported in order. However, by clipping, we can always get the closest shape. + return fraction; +} + +// This callback finds any hit. For this type of query we are usually just checking for obstruction, +// so the hit data is not relevant. +// NOTE: shape hits are not ordered, so this may not return the closest hit +static float RayCastAnyCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t materialId, + int triangleIndex, int childIndex, void* context ) +{ + CastContext* rayContext = (CastContext*)context; + + // Check for initial overlap + if ( rayContext->initialOverlap == false && fraction == 0.0f ) + { + // By returning -1, we instruct the calling code to ignore this shape and + // continue the ray-cast to the next shape. + return -1.0f; + } + + intptr_t ignoreFlag = intptr_t( b3Shape_GetUserData( shapeId ) ); + if ( ignoreFlag == 1 ) + { + // By returning -1, we instruct the calling code to ignore this shape and + // continue the ray-cast to the next shape. + return -1.0f; + } + + rayContext->points[0] = point; + rayContext->normals[0] = normal; + rayContext->fractions[0] = fraction; + rayContext->materialIds[0] = materialId; + rayContext->triangleIndices[0] = triangleIndex; + rayContext->childIndices[0] = childIndex; + rayContext->count = 1; + + // At this point we have a hit, so we know the ray is obstructed. + // By returning 0, we instruct the calling code to terminate the ray-cast. + return 0.0f; +} + +// This ray cast collects multiple hits along the ray. +// The shapes are not necessary reported in order, so we might not capture +// the closest shape. +// NOTE: shape hits are not ordered, so this may return hits in any order. This means that +// if you limit the number of results, you may discard the closest hit. You can see this +// behavior in the sample. +static float RayCastMultipleCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t materialId, + int triangleIndex, int childIndex, void* context ) +{ + CastContext* rayContext = (CastContext*)context; + + // Check for initial overlap + if ( rayContext->initialOverlap == false && fraction == 0.0f ) + { + // By returning -1, we instruct the calling code to ignore this shape and + // continue the ray-cast to the next shape. + return -1.0f; + } + + intptr_t ignoreFlag = intptr_t( b3Shape_GetUserData( shapeId ) ); + if ( ignoreFlag == 1 ) + { + // By returning -1, we instruct the calling code to ignore this shape and + // continue the ray-cast to the next shape. + return -1.0f; + } + + int count = rayContext->count; + assert( count < 3 ); + + rayContext->points[count] = point; + rayContext->normals[count] = normal; + rayContext->fractions[count] = fraction; + rayContext->materialIds[count] = materialId; + rayContext->triangleIndices[count] = triangleIndex; + rayContext->childIndices[count] = childIndex; + rayContext->count = count + 1; + + if ( rayContext->count == 3 ) + { + // At this point the buffer is full. + // By returning 0, we instruct the calling code to terminate the ray-cast. + return 0.0f; + } + + // By returning 1, we instruct the caller to continue without clipping the ray. + return 1.0f; +} + +// This ray cast collects multiple hits along the ray and sorts them. +static float RayCastSortedCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t materialId, + int triangleIndex, int childIndex, void* context ) +{ + CastContext* rayContext = (CastContext*)context; + + // Check for initial overlap + if ( rayContext->initialOverlap == false && fraction == 0.0f ) + { + // By returning -1, we instruct the calling code to ignore this shape and + // continue the ray-cast to the next shape. + return -1.0f; + } + + intptr_t ignoreFlag = intptr_t( b3Shape_GetUserData( shapeId ) ); + if ( ignoreFlag == 1 ) + { + // By returning -1, we instruct the calling code to ignore this shape and + // continue the ray-cast to the next shape. + return -1.0f; + } + + int count = rayContext->count; + assert( count <= 3 ); + + int index = 3; + while ( fraction < rayContext->fractions[index - 1] ) + { + index -= 1; + + if ( index == 0 ) + { + break; + } + } + + if ( index == 3 ) + { + // not closer, continue but tell the caller not to consider fractions further than the largest fraction acquired + // this only happens once the buffer is full + assert( rayContext->count == 3 ); + assert( rayContext->fractions[2] <= 1.0f ); + return rayContext->fractions[2]; + } + + for ( int j = 2; j > index; --j ) + { + rayContext->points[j] = rayContext->points[j - 1]; + rayContext->normals[j] = rayContext->normals[j - 1]; + rayContext->fractions[j] = rayContext->fractions[j - 1]; + rayContext->materialIds[j] = rayContext->materialIds[j - 1]; + rayContext->triangleIndices[j] = rayContext->triangleIndices[j - 1]; + rayContext->childIndices[j] = rayContext->childIndices[j - 1]; + } + + rayContext->points[index] = point; + rayContext->normals[index] = normal; + rayContext->fractions[index] = fraction; + rayContext->materialIds[index] = materialId; + rayContext->triangleIndices[index] = triangleIndex; + rayContext->childIndices[index] = childIndex; + rayContext->count = count < 3 ? count + 1 : 3; + + if ( rayContext->count == 3 ) + { + return rayContext->fractions[2]; + } + + // By returning 1, we instruct the caller to continue without clipping the ray. + return 1.0f; +} + +class CastWorld : public Sample +{ +public: + enum + { + e_any = 0, + e_closest = 1, + e_multiple = 2, + e_sorted = 3 + }; + + enum + { + e_rayCast = 0, + e_sphereCast = 1, + e_capsuleCast = 2, + e_boxCast = 3 + }; + + enum + { + e_maxCount = 64 + }; + + static Sample* Create( SampleContext* context ) + { + return new CastWorld( context ); + } + + explicit CastWorld( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 20.0f ); + } + + m_sphere = { b3Vec3_zero, 0.9f }; + m_capsule = { { -0.5f, 0.0f, 0.0f }, { 0.5f, 0.0f, 0.0f }, 0.8f }; + m_box = b3MakeBoxHull( 0.6f, 0.6f, 0.6f ); + m_mesh = b3CreateTorusMesh( 10, 12, 0.65f, 0.35f ); + + m_heightField = b3CreateWave( 10, 10, 0.5f * b3Vec3_one, 0.03f, 0.09f, false ); + + m_bodyIndex = 0; + + for ( int i = 0; i < e_maxCount; ++i ) + { + m_bodies[i] = {}; + } + + m_mode = e_closest; + + m_castType = e_rayCast; + m_castRadius = 0.5f; + + m_origin = { -20.0f, 10.0f, 0.0f }; + m_translation = { 20.0f, 10.0f, 0.0f }; + + m_castContext = {}; + m_initialOverlap = false; + } + + ~CastWorld() override + { + b3DestroyMesh( m_mesh ); + b3DestroyHeightField( m_heightField ); + } + + void CreateShapes( b3ShapeType shapeType, int count ) + { + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.gravityScale = 0.0f; + + for ( int i = 0; i < count; ++i ) + { + if ( B3_IS_NULL( m_bodies[m_bodyIndex] ) == false ) + { + b3DestroyBody( m_bodies[m_bodyIndex] ); + m_bodies[m_bodyIndex] = b3_nullBodyId; + } + + if ( m_bodyIndex % 3 == 0 ) + { + bodyDef.type = b3_kinematicBody; + } + else if ( m_bodyIndex % 2 == 0 ) + { + bodyDef.type = b3_dynamicBody; + } + else + { + bodyDef.type = b3_staticBody; + } + + // Heightfield must be a static body + if (shapeType == b3_heightShape) + { + bodyDef.type = b3_staticBody; + } + + bodyDef.position = RandomVec3Uniform( -20.0f, 20.0f ); + + b3Vec3 axis = RandomVec3Uniform( -1.0f, 1.0f ); + axis = b3Normalize( axis ); + float angle = RandomFloatRange( -B3_PI, B3_PI ); + bodyDef.rotation = b3MakeQuatFromAxisAngle( axis, angle ); + + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + int flag = ( m_bodyIndex & m_ignoreBase ) == m_ignoreBase ? 1 : 0; + shapeDef.userData = (void*)(intptr_t)flag; + + switch ( shapeType ) + { + case b3_sphereShape: + shapeDef.baseMaterial.userMaterialId = 11; + b3CreateSphereShape( bodyId, &shapeDef, &m_sphere ); + break; + + case b3_capsuleShape: + shapeDef.baseMaterial.userMaterialId = 22; + b3CreateCapsuleShape( bodyId, &shapeDef, &m_capsule ); + break; + + case b3_hullShape: + shapeDef.baseMaterial.userMaterialId = 33; + b3CreateHullShape( bodyId, &shapeDef, &m_box.base ); + break; + + case b3_meshShape: + shapeDef.baseMaterial.userMaterialId = 44; + b3CreateMeshShape( bodyId, &shapeDef, m_mesh, { 4.0f, 3.0f, -2.0f } ); + break; + + case b3_heightShape: + { + shapeDef.baseMaterial.userMaterialId = 55; + b3SurfaceMaterial materials[3] = { + { + .userMaterialId = 111, + }, + { + .userMaterialId = 222, + }, + { + .userMaterialId = 333, + }, + }; + shapeDef.materials = materials; + shapeDef.materialCount = 3; + + b3CreateHeightFieldShape( bodyId, &shapeDef, m_heightField ); + } + break; + + default: + assert( false ); + break; + } + + m_bodies[m_bodyIndex] = bodyId; + + m_bodyIndex = ( m_bodyIndex + 1 ) % e_maxCount; + } + } + + void DestroyBody() + { + for ( int i = 0; i < e_maxCount; ++i ) + { + if ( B3_IS_NULL( m_bodies[i] ) == false ) + { + b3DestroyBody( m_bodies[i] ); + m_bodies[i] = b3_nullBodyId; + return; + } + } + } + + void MouseDown( b3Vec2 p, int button, int modifiers ) override + { + if ( button == 0 && modifiers == GLFW_MOD_CONTROL ) + { + PickRay pickRay = m_camera->BuildPickRay( p.x, p.y ); + m_origin = pickRay.origin; + m_translation = 100.0f * b3Normalize( pickRay.translation ); + } + } + + void UpdateUI() override + { + float height = 360.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 210.0f, height ) ); + + ImGui::Begin( "Options", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + const char* castTypes[] = { "Ray", "Sphere", "Capsule", "Box" }; + if ( ImGui::Combo( "Cast Type", &m_castType, castTypes, IM_ARRAYSIZE( castTypes ) ) ) + { + } + + if ( m_castType != e_rayCast && m_castType != e_boxCast ) + { + ImGui::SliderFloat( "Radius", &m_castRadius, 0.1f, 2.0f, "%.1f" ); + } + + const char* modes[] = { "Any", "Closest", "Multiple", "Sorted" }; + if ( ImGui::Combo( "Mode", &m_mode, modes, IM_ARRAYSIZE( modes ) ) ) + { + } + + ImGui::Checkbox( "Initial Overlap", &m_initialOverlap ); + + if ( ImGui::Button( "Spheres" ) ) + { + CreateShapes( b3_sphereShape, 10 ); + } + + if ( ImGui::Button( "Capsules" ) ) + { + CreateShapes( b3_capsuleShape, 10 ); + } + + if ( ImGui::Button( "Hulls" ) ) + { + CreateShapes( b3_hullShape, 10 ); + } + + if ( ImGui::Button( "Meshes" ) ) + { + CreateShapes( b3_meshShape, 1 ); + } + + if ( ImGui::Button( "Height Field" ) ) + { + CreateShapes( b3_heightShape, 1 ); + } + + if ( ImGui::Button( "Destroy Shape" ) ) + { + DestroyBody(); + } + + ImGui::End(); + + for ( int i = 0; i < m_maxCount; ++i ) + { + if ( ( i & m_ignoreBase ) == m_ignoreBase && B3_IS_NULL( m_bodies[i] ) == false ) + { + b3AABB bounds = b3Body_ComputeAABB( m_bodies[i] ); + DrawBounds( m_scene, bounds, 0.0f, b3_colorYellow ); + } + } + } + + void Render() override + { + DrawTextLine( "Ctrl + left mouse to cast through cursor" ); + DrawTextLine( "Shapes drawn in yellow boxes are ignored by the ray" ); + + switch ( m_mode ) + { + case e_any: + DrawTextLine( "Cast mode: any - check for obstruction - unsorted" ); + break; + + case e_closest: + DrawTextLine( "Cast mode: closest - find closest shape along the cast" ); + break; + + case e_multiple: + DrawTextLine( "Cast mode: multiple - gather multiple shapes - unsorted" ); + break; + + case e_sorted: + DrawTextLine( "Cast mode: sorted - gather multiple shapes sorted by closeness" ); + break; + + default: + assert( false ); + break; + } + + for ( int i = 0; i < m_castContext.count; ++i ) + { + DrawTextLine( "material = %d, triangle = %d", m_castContext.materialIds[i], m_castContext.triangleIndices[i] ); + } + + DrawGrid( m_scene, 10 ); + DrawTransform( m_scene, b3Transform_identity, 1.0f ); + + Sample::Render(); + } + + void Step() override + { + Sample::Step(); + + b3HexColor color1 = b3_colorOrange; + b3HexColor color2 = b3_colorAqua; + b3HexColor green = b3_colorGreen; + b3HexColor gray = b3_colorGray; + + b3CastResultFcn* functions[] = { RayCastAnyCallback, RayCastClosestCallback, RayCastMultipleCallback, + RayCastSortedCallback }; + b3CastResultFcn* modeFcn = functions[m_mode]; + + m_castContext = {}; + m_castContext.initialOverlap = m_initialOverlap; + + // Must initialize fractions for sorting + m_castContext.fractions[0] = FLT_MAX; + m_castContext.fractions[1] = FLT_MAX; + m_castContext.fractions[2] = FLT_MAX; + + b3Sphere sphere = { m_origin, m_castRadius }; + b3Capsule capsule = { m_origin, m_origin + b3Vec3{ 0.0f, 1.0f, 0.0f }, m_castRadius }; + b3BoxHull box = {}; + + b3Vec3 pointBuffer[2]; + + b3ShapeProxy proxy = {}; + + switch ( m_castType ) + { + case e_rayCast: + proxy.count = 0; + break; + + case e_sphereCast: + proxy.count = 1; + proxy.radius = m_castRadius; + proxy.points = &sphere.center; + break; + + case e_capsuleCast: + proxy.count = 2; + proxy.radius = m_castRadius; + pointBuffer[0] = capsule.center1; + pointBuffer[1] = capsule.center2; + proxy.points = pointBuffer; + break; + + case e_boxCast: + { + b3Vec3 extent = { m_castRadius, 0.5f * m_castRadius, 0.25f * m_castRadius }; + box = b3MakeTransformedBoxHull( extent.x, extent.y, extent.z, { m_origin, b3Quat_identity } ); + proxy.points = box.boxPoints; + proxy.count = box.base.vertexCount; + } + break; + + default: + assert( false ); + break; + } + + if ( m_castType == e_rayCast ) + { + b3World_CastRay( m_worldId, m_origin, m_translation, b3DefaultQueryFilter(), modeFcn, &m_castContext ); + } + else + { + b3World_CastShape( m_worldId, &proxy, m_translation, b3DefaultQueryFilter(), modeFcn, &m_castContext ); + } + + if ( m_castContext.count > 0 ) + { + assert( m_castContext.count <= 3 ); + b3HexColor colors[3] = { b3_colorRed, b3_colorGreen, b3_colorBlue }; + DrawLine( m_scene, m_origin, m_origin + m_translation, color2 ); + + for ( int i = 0; i < m_castContext.count; ++i ) + { + b3Vec3 point = m_castContext.points[i]; + b3Vec3 normal = m_castContext.normals[i]; + DrawPoint( m_scene, point, 10.0f, colors[i] ); + b3Vec3 head = point + 0.5f * normal; + + b3Transform transform = { m_castContext.fractions[i] * m_translation, b3Quat_identity }; + + if ( m_castType == e_rayCast ) + { + DrawLine( m_scene, point, head, colors[i] ); + } + else if ( m_castType == e_sphereCast ) + { + DrawLine( m_scene, point, head, color1 ); + DrawSphere( m_scene, transform, sphere, colors[i] ); + } + else if ( m_castType == e_capsuleCast ) + { + DrawLine( m_scene, point, head, color1 ); + DrawCapsule( m_scene, transform, capsule, colors[i] ); + } + else if ( m_castType == e_boxCast ) + { + DrawLine( m_scene, point, head, color1 ); + DrawHull( m_scene, transform, &box.base, colors[i], false ); + } + } + } + else + { + DrawLine( m_scene, m_origin, m_origin + m_translation, color2 ); + + b3Transform transform = { m_translation, b3Quat_identity }; + + if ( m_castType == e_sphereCast ) + { + DrawSphere( m_scene, transform, sphere, gray ); + } + else if ( m_castType == e_capsuleCast ) + { + DrawCapsule( m_scene, transform, capsule, gray ); + } + else if ( m_castType == e_boxCast ) + { + DrawHull( m_scene, transform, &box.base, gray, false ); + } + } + + DrawPoint( m_scene, m_origin, 2.0f, green ); + } + + static constexpr int m_maxCount = 64; + static constexpr int m_ignoreBase = 0x3; + + b3Capsule m_capsule; + b3Sphere m_sphere; + b3BoxHull m_box; + b3MeshData* m_mesh; + b3HeightField* m_heightField; + + b3BodyId m_bodies[m_maxCount]; + int m_bodyIndex; + int m_mode; + int m_castType; + float m_castRadius; + bool m_initialOverlap; + + b3Vec3 m_origin; + b3Vec3 m_translation; + + CastContext m_castContext; +}; + +static int sampleCastWorld = SampleManager::Register( "Collision", "Cast World", CastWorld::Create ); + +class MeshScale : public Sample +{ +public: + static Sample* Create( SampleContext* context ) + { + return new MeshScale( context ); + } + + explicit MeshScale( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 20.0f ); + } + + m_mesh = b3CreateBoxMesh( { 0.0f, 0.0f, 0.0f }, { 0.5f, 0.5f, 0.5f }, true ); + m_scale = { 1.0f, 1.0f, 1.0f }; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + m_meshBodyId = b3CreateBody( m_worldId, &bodyDef ); + m_meshShapeId = b3CreateMeshShape( m_meshBodyId, &shapeDef, m_mesh, m_scale ); + m_start = { -2.0f, 0.0f, 0.0f }; + m_sphereCast = true; + } + + ~MeshScale() override + { + b3DestroyMesh( m_mesh ); + } + + void UpdateUI() override + { + float height = 200.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 200.0f, height ) ); + + ImGui::Begin( "Mesh Scale", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + b3Vec3 scale = m_scale; + bool changed = false; + changed = changed || ImGui::SliderFloat( "Scale X", &scale.x, -2.0f, 2.0f, "%.1f" ); + changed = changed || ImGui::SliderFloat( "Scale Y", &scale.y, -2.0f, 2.0f, "%.1f" ); + changed = changed || ImGui::SliderFloat( "Scale Z", &scale.z, -2.0f, 2.0f, "%.1f" ); + + if ( changed ) + { + m_scale = scale; + b3Shape_SetMesh( m_meshShapeId, m_mesh, m_scale ); + } + + ImGui::SliderFloat( "Start Y", &m_start.y, -2.0f, 2.0f, "%.1f" ); + ImGui::SliderFloat( "Start Z", &m_start.z, -2.0f, 2.0f, "%.1f" ); + + ImGui::Checkbox( "sphere Cast", &m_sphereCast ); + + ImGui::End(); + } + + void Step() override + { + b3Vec3 rayOrigin = m_start; + b3Vec3 rayTranslation = { 4.0f, 0.0f, 0.0f }; + + DrawPoint( m_scene, rayOrigin, 8.0f, b3_colorGreen ); + DrawPoint( m_scene, rayOrigin + rayTranslation, 8.0f, b3_colorRed ); + DrawLine( m_scene, rayOrigin, rayOrigin + rayTranslation, b3_colorWhite ); + + if ( m_sphereCast ) + { + b3Sphere sphere = { rayOrigin, 0.25f }; + + CastContext context = {}; + b3ShapeProxy proxy = { &sphere.center, 1, sphere.radius }; + b3World_CastShape( m_worldId, &proxy, rayTranslation, b3DefaultQueryFilter(), RayCastClosestCallback, &context ); + + if ( context.count > 0 ) + { + b3Transform transform = { context.fractions[0] * rayTranslation, b3Quat_identity }; + DrawSphere( m_scene, transform, sphere, b3_colorYellow ); + + b3Vec3 point = context.points[0]; + DrawLine( m_scene, point, point + 0.5f * context.normals[0], b3_colorGreen ); + DrawPoint( m_scene, point, 5.0f, b3_colorYellow ); + } + else + { + b3Transform transform = { rayTranslation, b3Quat_identity }; + DrawSphere( m_scene, transform, sphere, b3_colorGray ); + } + } + else + { + b3RayResult result = b3World_CastRayClosest( m_worldId, rayOrigin, rayTranslation, b3DefaultQueryFilter() ); + if ( result.hit ) + { + b3Vec3 point = result.point; + DrawLine( m_scene, point, point + 0.5f * result.normal, b3_colorGreen ); + DrawPoint( m_scene, point, 5.0f, b3_colorYellow ); + } + } + } + + b3Vec3 m_scale; + b3MeshData* m_mesh; + b3BodyId m_meshBodyId; + b3ShapeId m_meshShapeId; + b3Vec3 m_start; + bool m_sphereCast; +}; + +static int sampleMeshScale = SampleManager::Register( "Collision", "Mesh Scale", MeshScale::Create ); + +class ShapeCast : public Sample +{ +public: + explicit ShapeCast( SampleContext* context ) + : Sample( context ) + { + m_camera->SetView( 120.0f, 30.0f, 20.0f, { 0.0f, 1.5f, 0.0f } ); + + b3BoxHull box = b3MakeBoxHull( 0.6f, 0.6f, 0.6f ); + m_mesh = b3CreateTorusMesh( 10, 12, 0.65f, 0.35f ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + for ( int index = 0; index < 3; ++index ) + { + bodyDef.position = { -6.0f, 3.0f + 2.0f * index, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisX, 0.5f * B3_PI ); + b3BodyId sphereBody = b3CreateBody( m_worldId, &bodyDef ); + b3Sphere sphere = { b3Vec3_zero, 0.9f }; + b3CreateSphereShape( sphereBody, &shapeDef, &sphere ); + + bodyDef.position = { -2.0f, 3.0f + 2.0f * index, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.25f * B3_PI ); + b3BodyId capsuleBody = b3CreateBody( m_worldId, &bodyDef ); + b3Capsule capsule = { { -0.5f, 0.0f, 0.0f }, { 0.5f, 0.0f, 0.0f }, 0.7f }; + b3CreateCapsuleShape( capsuleBody, &shapeDef, &capsule ); + + bodyDef.position = { 2.0f, 3.0f + 2.0f * index, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.25f * B3_PI ); + b3BodyId hullBody = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( hullBody, &shapeDef, &box.base ); + + bodyDef.position = { 6.0f, 3.0f + 2.0f * index, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisX, 0.5f * B3_PI ); + b3BodyId meshBody = b3CreateBody( m_worldId, &bodyDef ); + b3CreateMeshShape( meshBody, &shapeDef, m_mesh, b3Vec3_one ); + + // todo add height field + } + + m_baseX = 0; + m_baseY = 0; + m_castOffset = {}; + m_trackingX = false; + m_trackingY = false; + m_initialOverlap = false; + } + + ~ShapeCast() override + { + b3DestroyMesh( m_mesh ); + } + + void CastSpheres() const + { + for ( int castIndex = 0; castIndex < 4; ++castIndex ) + { + // World space sweep + b3Sphere sphere = { b3Vec3_zero, 0.3f }; + b3Vec3 offset = b3Vec3{ -6.0f + 4.0f * castIndex, 3.0f, -5.0f } + m_castOffset; + sphere.center += offset; + b3Vec3 translation = 10.0f * b3Vec3_axisZ; + b3ShapeProxy proxy = { &sphere.center, 1, sphere.radius }; + + CastContext context = {}; + context.initialOverlap = m_initialOverlap; + + b3World_CastShape( m_worldId, &proxy, translation, b3DefaultQueryFilter(), RayCastClosestCallback, &context ); + + DrawSphere( m_scene, b3Transform_identity, sphere, b3_colorGreen ); + + if ( context.count > 0 ) + { + b3Vec3 point = context.points[0]; + b3Vec3 normal = context.normals[0]; + float fraction = context.fractions[0]; + + // final position with overlap resolution + DrawSphere( m_scene, { fraction * translation, b3Quat_identity }, sphere, b3_colorRed ); + + DrawPoint( m_scene, point, 2.0f, b3_colorRed ); + DrawLine( m_scene, point, point + 0.2f * normal, b3_colorYellow ); + } + else + { + DrawSphere( m_scene, { translation, b3Quat_identity }, sphere, b3_colorGray ); + } + } + } + + void CastCapsules() const + { + for ( int castIndex = 0; castIndex < 4; ++castIndex ) + { + b3Capsule capsule = { { -0.2f, -0.2f, -0.2f }, { 0.2f, 0.2f, 0.2f }, 0.2f }; + b3Vec3 offset = b3Vec3{ -6.0f + 4.0f * castIndex, 5.0f, -5.0f } + m_castOffset; + capsule.center1 += offset; + capsule.center2 += offset; + b3Vec3 translation = 10.0f * b3Vec3_axisZ; + b3ShapeProxy proxy = { &capsule.center1, 2, capsule.radius }; + + CastContext context = {}; + context.initialOverlap = m_initialOverlap; + + b3World_CastShape( m_worldId, &proxy, translation, b3DefaultQueryFilter(), RayCastClosestCallback, &context ); + + DrawCapsule( m_scene, b3Transform_identity, capsule, b3_colorGreen ); + + if ( context.count > 0 ) + { + b3Vec3 point = context.points[0]; + b3Vec3 normal = context.normals[0]; + float fraction = context.fractions[0]; + + DrawCapsule( m_scene, { fraction * translation, b3Quat_identity }, capsule, b3_colorRed ); + + DrawPoint( m_scene, point, 2.0f, b3_colorRed ); + DrawLine( m_scene, point, point + 0.2f * normal, b3_colorYellow ); + } + else + { + DrawCapsule( m_scene, { translation, b3Quat_identity }, capsule, b3_colorGray ); + } + } + } + + void CastHulls() const + { + for ( int castIndex = 0; castIndex < 4; ++castIndex ) + { + b3Vec3 offset = b3Vec3{ -6.0f + 4.0f * castIndex, 7.0f, -5.0f } + m_castOffset; + b3Quat qx = b3MakeQuatFromAxisAngle( b3Vec3_axisX, 0.25f * B3_PI ); + b3Quat qy = b3MakeQuatFromAxisAngle( b3Vec3_axisY, 0.25f * B3_PI ); + b3Quat qz = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.25f * B3_PI ); + + b3Quat q = b3MulQuat( qx, b3MulQuat( qy, qz ) ); + + b3Transform transform = { offset, q }; + + b3BoxHull box = b3MakeTransformedBoxHull( 0.3f, 0.3f, 0.3f, transform ); + + b3Vec3 translation = 10.0f * b3Vec3_axisZ; + b3ShapeProxy proxy = { box.boxPoints, box.base.vertexCount, 0.0f }; + + CastContext context = {}; + context.initialOverlap = m_initialOverlap; + + b3World_CastShape( m_worldId, &proxy, translation, b3DefaultQueryFilter(), RayCastClosestCallback, &context ); + + DrawHull( m_scene, b3Transform_identity, &box.base, b3_colorGreen, false ); + + if ( context.count > 0 ) + { + b3Vec3 point = context.points[0]; + b3Vec3 normal = context.normals[0]; + float fraction = context.fractions[0]; + + // final position with overlap resolution + DrawHull( m_scene, { fraction * translation, b3Quat_identity }, &box.base, b3_colorRed, false ); + + DrawPoint( m_scene, point, 2.0f, b3_colorRed ); + DrawLine( m_scene, point, point + 0.2f * normal, b3_colorYellow ); + } + else + { + DrawHull( m_scene, { translation, b3Quat_identity }, &box.base, b3_colorGray, false ); + } + } + } + + void UpdateUI() override + { + float height = 80.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 210.0f, height ) ); + ImGui::Begin( "Shape Cast", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + ImGui::Checkbox( "Initial Overlap", &m_initialOverlap ); + ImGui::End(); + } + + void Render() override + { + Sample::Render(); + + DrawTextLine( "Shift + LMB and drag to shift start position" ); + + DrawGrid( m_scene, 10 ); + DrawTransform( m_scene, b3Transform_identity, 1.0f ); + } + + void MouseDown( b3Vec2 p, int button, int modifiers ) override + { + if ( button == 0 ) + { + if ( modifiers == GLFW_MOD_SHIFT ) + { + m_trackingX = true; + m_baseX = p.x; + } + else if ( modifiers == GLFW_MOD_CONTROL ) + { + m_trackingY = true; + m_baseY = p.y; + } + } + } + + void MouseUp( b3Vec2 p, int button ) override + { + if ( button == 0 ) + { + m_trackingX = false; + m_trackingY = false; + } + } + + void MouseMove( b3Vec2 p ) override + { + if ( m_trackingX ) + { + m_castOffset.z = 0.05f * ( m_baseX - p.x ); + } + + if ( m_trackingY ) + { + m_castOffset.y = 0.05f * ( m_baseY - p.y ); + } + } + + void Step() override + { + CastSpheres(); + CastCapsules(); + CastHulls(); + } + + static Sample* Create( SampleContext* context ) + { + return new ShapeCast( context ); + } + + b3MeshData* m_mesh; + int m_baseX; + int m_baseY; + b3Vec3 m_castOffset; + bool m_trackingX; + bool m_trackingY; + bool m_initialOverlap; +}; + +static int sampleShapeCast = SampleManager::Register( "Collision", "Shape Cast", ShapeCast::Create ); + +// Tests shape overlap versus world +class OverlapWorld : public Sample +{ +public: + enum + { + BODY_COUNT = 12 + }; + + static Sample* Create( SampleContext* context ) + { + return new OverlapWorld( context ); + } + + explicit OverlapWorld( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + + m_camera->SetView( 120.0f, 30.0f, 20.0f, { 0.0f, 1.5f, 0.0f } ); + } + + b3World_SetGravity( m_worldId, b3Vec3_zero ); + + m_box = b3MakeBoxHull( 0.6f, 0.6f, 0.6f ); + m_mesh = b3CreateTorusMesh( 10, 12, 0.65f, 0.35f ); + + m_heightField = b3CreateWave( 10, 10, 0.2f * b3Vec3_one, 0.03f, 0.09f, false ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + for ( int index = 0; index < 3; ++index ) + { + b3BodyType type = b3BodyType( index ); + + bodyDef.type = type; + bodyDef.position = { -6.0f, 3.0f + 2.0f * index, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisX, 0.5f * B3_PI ); + b3BodyId sphereBody = b3CreateBody( m_worldId, &bodyDef ); + b3Sphere sphere = { b3Vec3_zero, 0.8f }; + b3CreateSphereShape( sphereBody, &shapeDef, &sphere ); + + bodyDef.type = type; + bodyDef.position = { -3.0f, 3.0f + 2.0f * index, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.25f * B3_PI ); + b3BodyId capsuleBody = b3CreateBody( m_worldId, &bodyDef ); + b3Capsule capsule = { { -0.5f, 0.0f, 0.0f }, { 0.5f, 0.0f, 0.0f }, 0.5f }; + b3CreateCapsuleShape( capsuleBody, &shapeDef, &capsule ); + + bodyDef.type = type; + bodyDef.position = { 0.0f, 3.0f + 2.0f * index, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.25f * B3_PI ); + b3BodyId hullBody = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( hullBody, &shapeDef, &m_box.base ); + + bodyDef.type = type; + bodyDef.position = { 3.0f, 3.0f + 2.0f * index, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisX, 0.5f * B3_PI ); + b3BodyId meshBody = b3CreateBody( m_worldId, &bodyDef ); + b3CreateMeshShape( meshBody, &shapeDef, m_mesh, { -0.5f, 1.5f, -1.0f } ); + + // Height fields only on static bodies + bodyDef.type = b3_staticBody; + bodyDef.position = { 5.0f, 2.0f + 2.0f * index, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisX, -0.5f * B3_PI ); + b3BodyId heightBody = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHeightFieldShape( heightBody, &shapeDef, m_heightField ); + } + + m_baseX = 0; + m_castOffset = 0.0f; + m_tracking = false; + } + + ~OverlapWorld() override + { + b3DestroyHeightField( m_heightField ); + b3DestroyMesh( m_mesh ); + } + + static bool OverlapResultFcn( b3ShapeId shapeId, void* context ) + { + b3HexColor* color = static_cast( context ); + *color = b3_colorRed; + + // terminate the query + return false; + } + + void OverlapSpheres() const + { + for ( int i = 0; i < 5; ++i ) + { + b3Sphere sphere = { { -6.0f + 3.0f * i, 3.0f, -5.0f + m_castOffset }, 0.3f }; + b3ShapeProxy proxy = { &sphere.center, 1, sphere.radius }; + b3HexColor color = b3_colorGreen; + b3World_OverlapShape( m_worldId, &proxy, b3DefaultQueryFilter(), OverlapResultFcn, &color ); + DrawSphere( m_scene, b3Transform_identity, sphere, color ); + } + } + + void OverlapCapsules() const + { + + for ( int i = 0; i < 5; ++i ) + { + b3Vec3 offset = { -6.0f + 3.0f * i, 5.0f, -5.0f + m_castOffset }; + b3Capsule capsule = { { -0.2f, -0.2f, -0.2f }, { 0.2f, 0.2f, 0.2f }, 0.2f }; + capsule.center1 += offset; + capsule.center2 += offset; + b3ShapeProxy proxy = { &capsule.center1, 2, capsule.radius }; + b3HexColor color = b3_colorGreen; + b3World_OverlapShape( m_worldId, &proxy, b3DefaultQueryFilter(), OverlapResultFcn, &color ); + DrawCapsule( m_scene, b3Transform_identity, capsule, color ); + } + } + + void OverlapHulls() const + { + for ( int i = 0; i < 5; ++i ) + { + b3Transform transform{ { -6.0f + 3.0f * i, 7.0f, -5.0f + m_castOffset }, b3Quat_identity }; + b3BoxHull box = b3MakeTransformedBoxHull( 0.3f, 0.3f, 0.3f, transform ); + b3ShapeProxy proxy = { box.boxPoints, box.base.vertexCount, 0.0f }; + b3HexColor color = b3_colorGreen; + b3World_OverlapShape( m_worldId, &proxy, b3DefaultQueryFilter(), OverlapResultFcn, &color ); + DrawHull( m_scene, b3Transform_identity, &box.base, color, false ); + } + } + + void Render() override + { + Sample::Render(); + + DrawTextLine( "Shift + LMB and drag to move shapes" ); + + DrawGrid( m_scene, 10 ); + DrawLine( m_scene, b3Vec3_zero, 0.4f * b3Vec3_axisX, b3_colorRed ); + DrawLine( m_scene, b3Vec3_zero, 0.4f * b3Vec3_axisY, b3_colorGreen ); + DrawLine( m_scene, b3Vec3_zero, 0.4f * b3Vec3_axisZ, b3_colorBlue ); + } + + void MouseDown( b3Vec2 p, int button, int modifiers ) override + { + if ( button == 0 ) + { + if ( modifiers == GLFW_MOD_SHIFT ) + { + m_tracking = true; + m_baseX = p.x; + } + } + } + + void MouseUp( b3Vec2 p, int button ) override + { + if ( button == 0 ) + { + m_tracking = false; + } + } + + void MouseMove( b3Vec2 p ) override + { + if ( m_tracking ) + { + m_castOffset = 0.05f * ( m_baseX - p.x ); + } + } + + void Step() override + { + OverlapSpheres(); + OverlapCapsules(); + OverlapHulls(); + } + + b3BoxHull m_box; + b3MeshData* m_mesh; + b3HeightField* m_heightField; + int m_baseX; + float m_castOffset; + bool m_tracking; +}; + +static int sampleOverlapWorld = SampleManager::Register( "Collision", "Overlap World", OverlapWorld::Create ); + +class InitialOverlap : public Sample +{ +public: + explicit InitialOverlap( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( -140.0f, 10.0f, 10.0f, { 0.0f, 0.0f, 0.0f } ); + } + + int indices[] = { 0, 1, 2, 2, 3, 0 }; + + b3Vec3 vertices[] = { { -0.5f, 0.5f, 0.5f }, { -0.5f, 0.5f, -0.5f }, { -0.5f, -0.5f, -0.5f }, { -0.5f, -0.5f, 0.5f } }; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, 0.0f, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 10.0f * B3_DEG_TO_RAD ); + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + // m_mesh = b3CreateBoxMesh( { 0.0f, 0.0f, 0.0f }, { 0.5f, 0.5f, 0.5f } ); + b3MeshDef def = {}; + def.triangleCount = 2; + def.indices = indices; + def.vertexCount = 4; + def.vertices = vertices; + def.materialIndices = nullptr; + def.useMedianSplit = false; + + m_mesh = b3CreateMesh( &def, nullptr, 0 ); + b3CreateMeshShape( body, &shapeDef, m_mesh, 4.0f * b3Vec3_one ); + + m_initialOverlap = true; + } + + ~InitialOverlap() override + { + b3DestroyMesh( m_mesh ); + } + + void Render() override + { + Sample::Render(); + + DrawTransform( m_scene, b3Transform_identity, 1.0f ); + } + + void UpdateUI() override + { + float height = 80.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 200.0f, height ) ); + ImGui::Begin( "Initial Overlap", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + ImGui::Checkbox( "initial overlap", &m_initialOverlap ); + ImGui::End(); + } + + void Step() override + { + Sample::Step(); + + b3Vec3 offset = { -2.1f, -0.8f, 0.95f }; + + CastContext context = {}; + context.initialOverlap = m_initialOverlap; + + b3Capsule capsule = { + .center1 = offset, + .center2 = offset + b3Vec3{ 0.0f, 1.0f, 0.0f }, + .radius = 0.25f, + }; + + // zero length cast + b3Vec3 translation = { 0.0f, 0.0f, 0.0f }; + + b3ShapeProxy proxy = { &capsule.center1, 2, capsule.radius }; + + DrawCapsule( m_scene, b3Transform_identity, capsule, b3_colorGreen ); + + b3World_CastShape( m_worldId, &proxy, translation, b3DefaultQueryFilter(), RayCastClosestCallback, &context ); + + float fraction = context.count > 0 ? context.fractions[0] : 1.0f; + b3Transform shapeEnd = { fraction * translation, b3Quat_identity }; + DrawCapsule( m_scene, shapeEnd, capsule, context.count > 0 ? b3_colorRed : b3_colorGreen ); + + if ( context.count > 0 ) + { + b3Vec3 p = context.points[0]; + b3Vec3 n = context.normals[0]; + DrawLine( m_scene, p, p + 0.5f * n, b3_colorAliceBlue ); + DrawPoint( m_scene, p, 8.0f, b3_colorAliceBlue ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new InitialOverlap( context ); + } + + b3MeshData* m_mesh; + bool m_initialOverlap; +}; + +static int sampleInitialOverlap = SampleManager::Register( "Collision", "Initial Overlap", InitialOverlap::Create ); + +// pointA[0] = {0.000000000, 0.000000000, 0.000000000} +// pointA[1] = {0.000000000, -6400.000000000, 0.000000000} +// pointA[2] = {6400.000000000, 0.000000000, 22.609375000} +// pointB[i] = {43616.210937500, -100213.000000000, 132631.812500000} +// pointB[i] = {342231.968750000, 359711.687500000, 132631.812500000} +// radiusB = 40.0f +// xfA = {{0.000000000, 0.000000000, 0.000000000}, {{0.000000000, 0.000000000, 0.000000000}, 1.000000000} +// xfB = {{-115200.000000000, -19200.000000000, -202755.000000000}, {{0.000000000, 0.000000000, 0.000000000}, 1.000000000} +// t = {0.008614914, 0.000000000, 72267.117187500} +// maxFraction = 0.970617533, canEncroach = 0 +class ShapeCastDebug : public Sample +{ +public: + explicit ShapeCastDebug( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 120.0f, 30.0f, 20.0f, { 0.0f, 1.5f, 0.0f } ); + } + + // this triggers an assert with scale of 1 and b3_lengthUnitsPerMeter of 100 + float scale = 0.01f; + m_triangle[0] = scale * b3Vec3{ 0.000000000, 0.000000000, 0.000000000 }; + m_triangle[1] = scale * b3Vec3{ 0.000000000, -6400.000000000, 0.000000000 }; + m_triangle[2] = scale * b3Vec3{ 6400.000000000, 0.000000000, 22.609375000 }; + + b3Vec3 origin = m_triangle[0]; + m_triangle[0] = b3Vec3_zero; + m_triangle[1] -= origin; + m_triangle[2] -= origin; + + m_points[0] = scale * b3Vec3{ 200.305283, 200.460999, 9.53760529 }; + m_points[1] = scale * b3Vec3{ -200.305283, 200.460999, 9.53760529 }; + m_points[2] = scale * b3Vec3{ -200.305283, -200.460999, 9.53760529 }; + m_points[3] = scale * b3Vec3{ 200.305283, -200.460999, 9.53760529 }; + m_points[4] = scale * b3Vec3{ 200.305283, 200.460999, -9.53760529 }; + m_points[5] = scale * b3Vec3{ -200.305283, 200.460999, -9.53760529 }; + m_points[6] = scale * b3Vec3{ -200.305283, -200.460999, -9.53760529 }; + m_points[7] = scale * b3Vec3{ 200.305283, -200.460999, -9.53760529 }; + + m_capsule.center1 = scale * b3Vec3{ 43616.210937500, -100213.000000000, 132631.812500000 }; + m_capsule.center2 = scale * b3Vec3{ 342231.968750000, 359711.687500000, 132631.812500000 }; + m_capsule.radius = scale * 1.0f; + + float length = b3Distance( m_capsule.center1, m_capsule.center2 ); + (void)length; + + m_transform = { scale * b3Vec3{ -115200.000000000, -19200.000000000, -202755.000000000 } - origin, + { { 0.00000000, 0.00000000, 0.0f }, 1.0f } }; + + m_translation = scale * b3Vec3{ 0.008614914, 0.000000000, 72267.117187500 }; + + m_box = b3CreateHull( m_points, 8, 8 ); + } + + ~ShapeCastDebug() override + { + b3DestroyHull( m_box ); + } + + void Render() override + { + Sample::Render(); + + DrawGrid( m_scene, 10 ); + DrawTransform( m_scene, b3Transform_identity, 1.0f ); + + b3ShapeCastPairInput input; + input.proxyA = { m_triangle, 3, 0.0f }; + // input.proxyB = { m_points, 8, 0.0f }; + input.proxyB = { &m_capsule.center1, 2, m_capsule.radius }; + input.transformA = b3Transform_identity; + input.transformB = m_transform; + input.translationB = m_translation; + input.maxFraction = 0.970617533; + input.canEncroach = false; + + b3CastOutput output = b3ShapeCast( &input ); + + DrawFace( m_scene, m_triangle[0], m_triangle[1], m_triangle[2], b3_colorCyan ); + // DrawHull( m_scene, m_transform, m_box, b3_colorGreen, false ); + DrawCapsule( m_scene, m_transform, m_capsule, b3_colorGreen ); + + if ( output.hit ) + { + // final position with overlap resolution + b3Vec3 shapeEnd = m_transform.p + output.fraction * m_translation; + // DrawHull( m_scene, { shapeEnd, m_transform.q }, m_box, b3_colorRed, false ); + DrawCapsule( m_scene, { shapeEnd, m_transform.q }, m_capsule, b3_colorRed ); + } + + b3Vec3 shapeEnd = m_transform.p + m_translation; + // DrawHull( m_scene, { shapeEnd, m_transform.q }, m_box, b3_colorGray, false ); + DrawCapsule( m_scene, { shapeEnd, m_transform.q }, m_capsule, b3_colorGray ); + } + + static Sample* Create( SampleContext* context ) + { + return new ShapeCastDebug( context ); + } + + b3Transform m_transform; + b3Vec3 m_translation; + b3Capsule m_capsule; + b3Hull* m_box; + b3Vec3 m_points[8]; + b3Vec3 m_triangle[3]; +}; + +static int sampleShapeCastDebug = SampleManager::Register( "Collision", "Shape Cast Debug", ShapeCastDebug::Create ); + +class DistanceDebug : public Sample +{ +public: + explicit DistanceDebug( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 120.0f, 30.0f, 20.0f, { 0.0f, 1.5f, 0.0f } ); + } + + // this triggers an assert with scale of 1 and b3_lengthUnitsPerMeter of 100 + float scale = 0.01f; + m_triangle[0] = scale * b3Vec3{ 1400.00000, 1600.00000, 70.1534424 }; + m_triangle[1] = scale * b3Vec3{ 1400.00000, 1500.00000, 66.1250000 }; + m_triangle[2] = scale * b3Vec3{ 1500.00000, 1600.00000, 72.3507080 }; + + // shifting to the origin give more accuracy + b3Vec3 origin = m_triangle[0]; + m_triangle[0] = b3Vec3_zero; + m_triangle[1] -= origin; + m_triangle[2] -= origin; + + m_points[0] = scale * b3Vec3{ 200.305283, 200.460999, 9.53760529 }; + m_points[1] = scale * b3Vec3{ -200.305283, 200.460999, 9.53760529 }; + m_points[2] = scale * b3Vec3{ -200.305283, -200.460999, 9.53760529 }; + m_points[3] = scale * b3Vec3{ 200.305283, -200.460999, 9.53760529 }; + m_points[4] = scale * b3Vec3{ 200.305283, 200.460999, -9.53760529 }; + m_points[5] = scale * b3Vec3{ -200.305283, 200.460999, -9.53760529 }; + m_points[6] = scale * b3Vec3{ -200.305283, -200.460999, -9.53760529 }; + m_points[7] = scale * b3Vec3{ 200.305283, -200.460999, -9.53760529 }; + + m_boxA = b3MakeBoxHull( 40.0f, 1.0f, 40.0f ); + m_boxB = b3MakeTransformedBoxHull( 0.5f, 10.0f, 0.5f, { { 0.0f, 10.0f, 0.0f }, b3Quat_identity } ); + + // m_transformA = { + // .p = { 0.0f, -1.0f, 0.0f }, + // .q = b3Quat_identity, + // }; + + m_transformB = { + .p = { -1.64657831e-06, 1.00989532471, 0.0f }, + .q = { { 0.00000000, 0.00000000, 0.00494779600 }, 0.999987781 }, + }; + + m_box = b3CreateHull( m_points, 8, 8 ); + + m_simplexCount = 0; + m_simplexIndex = 0; + + memset( m_simplexes, 0, sizeof( m_simplexes ) ); + } + + ~DistanceDebug() override + { + b3DestroyHull( m_box ); + } + + void Render() override + { + Sample::Render(); + + DrawGrid( m_scene, 10 ); + DrawTransform( m_scene, b3Transform_identity, 1.0f ); + } + + void UpdateUI() override + { + float height = 80.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Distance Debug", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + ImGui::SliderInt( "simplex index", &m_simplexIndex, 0, m_simplexCount - 1 ); + + ImGui::End(); + } + + static void BuildWitnessPoints( const b3Simplex* simplex, b3Vec3& vertexA, b3Vec3& vertexB ) + { + const b3SimplexVertex* vs = simplex->vertices; + int count = simplex->count; + B3_ASSERT( 1 <= count && count <= 4 ); + + switch ( count ) + { + case 1: + vertexA = vs[0].wA; + vertexB = vs[0].wB; + break; + + case 2: + vertexA = vs[0].a * vs[0].wA + vs[1].a * vs[1].wA; + vertexB = vs[0].a * vs[0].wB + vs[1].a * vs[1].wB; + break; + + case 3: + vertexA = vs[0].a * vs[0].wA + vs[1].a * vs[1].wA + vs[2].a * vs[2].wA; + vertexB = vs[0].a * vs[0].wB + vs[1].a * vs[1].wB + vs[2].a * vs[2].wB; + break; + + case 4: + // Force identical points and *zero* distance + vertexA = vertexB = vs[0].a * vs[0].wA + vs[1].a * vs[1].wA + vs[2].a * vs[2].wA + vs[3].a * vs[3].wA; + break; + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + } + + static b3Vec3 GetClosestPoint( const b3Simplex* simplex ) + { + int count = simplex->count; + B3_ASSERT( 1 <= count && count <= 4 ); + + const b3SimplexVertex* vs = simplex->vertices; + + switch ( count ) + { + case 1: + return simplex->vertices[0].w; + + case 2: + return vs[0].a * vs[0].w + vs[1].a * vs[1].w; + + case 3: + return vs[0].a * vs[0].w + vs[1].a * vs[1].w + vs[2].a * vs[2].w; + + case 4: + return vs[0].a * vs[0].w + vs[1].a * vs[1].w + vs[2].a * vs[2].w + vs[3].a * vs[3].w; + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + return b3Vec3_zero; + } + + void Step() override + { + b3DistanceInput input; + input.proxyA = { m_boxA.boxPoints, 8, 0.0f }; + input.proxyB = { m_boxB.boxPoints, 8, 0.0f }; + input.transformA = b3Transform_identity; + input.transformB = m_transformB; + input.useRadii = false; + + b3SimplexCache cache = {}; + b3DistanceOutput output = b3ShapeDistance( &input, &cache, m_simplexes, 32 ); + m_simplexCount = output.simplexCount; + + // DrawFace( m_scene, m_triangle[0], m_triangle[1], m_triangle[2], b3_colorCyan ); + // DrawHull( m_scene, m_transformB, m_box, b3_colorGreen, false ); + + DrawHull( m_scene, b3Transform_identity, &m_boxA.base, b3_colorGreen, false ); + DrawHull( m_scene, m_transformB, &m_boxB.base, b3_colorCyan, false ); + + for ( int i = 0; i < m_boxB.base.vertexCount; ++i ) + { + b3Vec3 p = b3TransformPoint( m_transformB, m_boxB.boxPoints[i] ); + DrawWorldString( m_camera, p, b3_colorAliceBlue, " %d", i ); + } + + DrawPoint( m_scene, output.pointA, 5.0f, b3_colorWhite ); + DrawPoint( m_scene, output.pointB, 5.0f, b3_colorWhite ); + DrawLine( m_scene, output.pointA, output.pointA + 1.0f * output.normal, b3_colorWhite ); + + b3Vec3 n = output.normal; + DrawTextLine( "distance = %g, normal = %g, %g, %g", output.distance, n.x, n.y, n.z ); + + B3_ASSERT( output.distance > 0.0f && b3IsNormalized( output.normal ) ); + + if ( m_simplexCount > 0 ) + { + b3Simplex* simplex = m_simplexes + m_simplexIndex; + + b3Vec3 v1, v2; + BuildWitnessPoints( simplex, v1, v2 ); + + DrawPoint( m_scene, v1, 10.0f, b3_colorGreen ); + DrawPoint( m_scene, v2, 10.0f, b3_colorGreen ); + + for ( int i = 0; i < simplex->count; ++i ) + { + v1 = simplex->vertices[i].wA; + v2 = simplex->vertices[i].wB; + DrawPoint( m_scene, v1, 5.0f, b3_colorRed ); + DrawPoint( m_scene, v2, 5.0f, b3_colorRed ); + } + + b3Vec3 p = GetClosestPoint( simplex ); + float distance = b3Length( p ); + + DrawTextLine( "current distance = %.5f", distance ); + } + + for ( int i = 0; i < m_simplexCount; ++i ) + { + b3Simplex* simplex = m_simplexes + i; + + if ( simplex->count == 1 ) + { + b3SimplexVertex* v1 = simplex->vertices + 0; + DrawTextLine( "v1s : %d, v2s : %d", v1->indexA, v1->indexB ); + } + else if ( simplex->count == 2 ) + { + b3SimplexVertex* v1 = simplex->vertices + 0; + b3SimplexVertex* v2 = simplex->vertices + 1; + DrawTextLine( "v1s : %d/%d, v2s : %d/%d", v1->indexA, v2->indexA, v1->indexB, v2->indexB ); + } + else if ( simplex->count == 3 ) + { + b3SimplexVertex* v1 = simplex->vertices + 0; + b3SimplexVertex* v2 = simplex->vertices + 1; + b3SimplexVertex* v3 = simplex->vertices + 2; + DrawTextLine( "v1s : %d/%d/%d, v2s : %d/%d/%d", v1->indexA, v2->indexA, v3->indexA, v1->indexB, v2->indexB, + v3->indexB ); + } + else if ( simplex->count == 3 ) + { + b3SimplexVertex* v1 = simplex->vertices + 0; + b3SimplexVertex* v2 = simplex->vertices + 1; + b3SimplexVertex* v3 = simplex->vertices + 2; + b3SimplexVertex* v4 = simplex->vertices + 3; + DrawTextLine( "v1s : %d/%d/%d/%d, v2s : %d/%d/%d/%d", v1->indexA, v2->indexA, v3->indexA, v4->indexA, v1->indexB, + v2->indexB, v3->indexB, v4->indexB ); + } + } + } + + static Sample* Create( SampleContext* context ) + { + return new DistanceDebug( context ); + } + + b3Transform m_transformB; + b3Hull* m_box; + b3BoxHull m_boxA; + b3BoxHull m_boxB; + b3Vec3 m_points[8]; + b3Vec3 m_triangle[3]; + b3Simplex m_simplexes[32]; + int m_simplexIndex; + int m_simplexCount; +}; + +static int sampleDistanceDebug = SampleManager::Register( "Collision", "Distance Debug", DistanceDebug::Create ); + +class ShapeDistance : public Sample +{ +public: + enum ShapeType + { + e_point, + e_segment, + e_triangle, + e_box + }; + + explicit ShapeDistance( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( -90.0f, 0.0f, 10.0f, { 0.0f, 0.0f, 0.0f } ); + } + + m_point = b3Vec3_zero; + m_segment[0] = { -0.5f, 0.0f }; + m_segment[1] = { 0.5f, 0.0f }; + + m_triangle[0] = { -0.5f, 0.0f }; + m_triangle[1] = { 0.5f, 0.0f }; + m_triangle[2] = { 0.0f, 1.0f }; + m_box = b3MakeBoxHull( 0.25f, 0.5f, 1.0f ); + + m_triangle[0] = { 0.0f, 0.0f, -4.0f }; + m_triangle[1] = { 0.0f, 0.0f, -8.0f }; + m_triangle[2] = { -4.0f, 0.0f, -8.0f }; + m_box = b3MakeBoxHull( 0.02f, 0.2f, 0.04f ); + + m_transform = { { 0.0f, 0.0f, 0.0f }, b3Quat_identity }; + m_transform = { + .p = { 0.00000000, 0.796873450, -4.50000000 }, + .q = { { -0.964603841, -0.0856034234, 0.248849452 }, 0.0168983955 }, + }; + + m_cache = {}; + m_simplexCount = 0; + m_simplexIndex = 0; + m_dragStart = { 0.0f, 0.0f }; + m_basePosition = { 0.0f, 0.0f, 0.0f }; + m_baseQuat = { .v = { .z = 1.0f } }; + + m_dragging = false; + m_rotating = false; + m_showIndices = false; + m_useCache = false; + m_drawSimplex = false; + + m_typeA = e_triangle; + m_typeB = e_box; + m_radiusA = 0.0f; + m_radiusB = 0.0f; + + m_proxyA = MakeProxy( m_typeA, m_radiusA ); + m_proxyB = MakeProxy( m_typeB, m_radiusB ); + } + + b3ShapeProxy MakeProxy( ShapeType type, float radius ) + { + b3ShapeProxy proxy = {}; + proxy.radius = radius; + + switch ( type ) + { + case e_point: + proxy.points = &m_point; + proxy.count = 1; + break; + + case e_segment: + proxy.points = m_segment; + proxy.count = 2; + break; + + case e_triangle: + proxy.points = m_triangle; + proxy.count = 3; + break; + + case e_box: + proxy.points = b3GetHullPoints( &m_box.base ); + proxy.count = m_box.base.vertexCount; + break; + + default: + assert( false ); + } + + return proxy; + } + + void DrawShape( ShapeType type, b3Transform transform, float radius, b3HexColor color ) + { + switch ( type ) + { + case e_point: + { + if ( radius > 0.0f ) + { + b3Sphere sphere = { + .center = m_point, + .radius = radius, + }; + DrawSphere( m_scene, transform, sphere, color ); + } + else + { + b3Vec3 p = b3TransformPoint( transform, m_point ); + DrawPoint( m_scene, p, 5.0f, color ); + } + } + break; + + case e_segment: + { + if ( radius > 0.0f ) + { + b3Capsule capsule = { + .center1 = m_segment[0], + .center2 = m_segment[1], + .radius = radius, + }; + + DrawCapsule( m_scene, transform, capsule, color ); + } + else + { + b3Vec3 p1 = b3TransformPoint( transform, m_segment[0] ); + b3Vec3 p2 = b3TransformPoint( transform, m_segment[1] ); + DrawLine( m_scene, p1, p2, color ); + } + } + break; + + case e_triangle: + { + b3Vec3 p1 = b3TransformPoint( transform, m_triangle[0] ); + b3Vec3 p2 = b3TransformPoint( transform, m_triangle[1] ); + b3Vec3 p3 = b3TransformPoint( transform, m_triangle[2] ); + DrawLine( m_scene, p1, p2, color ); + DrawLine( m_scene, p2, p3, color ); + DrawLine( m_scene, p3, p1, color ); + } + break; + + case e_box: + DrawHull( m_scene, transform, &m_box.base, color, false ); + break; + + default: + assert( false ); + } + } + + void UpdateUI() override + { + float fontSize = ImGui::GetFontSize(); + float height = 16.0f * fontSize; + ImGui::SetNextWindowPos( ImVec2( 0.5f * fontSize, m_camera->m_height - height - 2.0f * fontSize ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 19.0f * fontSize, height ) ); + + ImGui::Begin( "Shape Distance", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + const char* shapeTypes[] = { "point", "segment", "triangle", "box" }; + int shapeType = int( m_typeA ); + if ( ImGui::Combo( "shape A", &shapeType, shapeTypes, IM_ARRAYSIZE( shapeTypes ) ) ) + { + m_typeA = ShapeType( shapeType ); + m_proxyA = MakeProxy( m_typeA, m_radiusA ); + } + + if ( ( m_typeA == e_point || m_typeA == e_segment ) && ImGui::SliderFloat( "radius A", &m_radiusA, 0.0f, 0.5f, "%.2f" ) ) + { + m_proxyA.radius = m_radiusA; + } + + shapeType = int( m_typeB ); + if ( ImGui::Combo( "shape B", &shapeType, shapeTypes, IM_ARRAYSIZE( shapeTypes ) ) ) + { + m_typeB = ShapeType( shapeType ); + m_proxyB = MakeProxy( m_typeB, m_radiusB ); + } + + if ( ( m_typeB == e_point || m_typeB == e_segment ) && ImGui::SliderFloat( "radius B", &m_radiusB, 0.0f, 0.5f, "%.2f" ) ) + { + m_proxyB.radius = m_radiusB; + } + + ImGui::Separator(); + + ImGui::Checkbox( "show indices", &m_showIndices ); + ImGui::Checkbox( "use cache", &m_useCache ); + + ImGui::Separator(); + + if ( ImGui::Checkbox( "draw simplex", &m_drawSimplex ) ) + { + m_simplexIndex = 0; + } + + if ( m_drawSimplex && m_simplexCount > 0 ) + { + ImGui::SliderInt( "index", &m_simplexIndex, 0, m_simplexCount - 1 ); + m_simplexIndex = b3ClampInt( m_simplexIndex, 0, m_simplexCount - 1 ); + } + + ImGui::End(); + } + + void MouseDown( b3Vec2 ps, int button, int mods ) override + { + if ( button == GLFW_MOUSE_BUTTON_1 ) + { + if ( mods == 0 && m_rotating == false ) + { + m_dragging = true; + PickRay ray = m_camera->BuildPickRay( ps.x, ps.y ); + b3Vec3 d = b3Normalize( ray.translation ); + + // Intersect with plane going through origin + // p = c + alpha * f + // dot(p, f) = 0 + // dot(c, f) + alpha = 0 + // p = c - dot(c, f) * f + m_dragStart = b3MulSub( ray.origin, b3Dot( ray.origin, d ), d ); + m_basePosition = m_transform.p; + } + else if ( mods == GLFW_MOD_SHIFT && m_dragging == false ) + { + m_rotating = true; + m_rotateStart = ps.x; + m_baseQuat = m_transform.q; + } + } + } + + void MouseUp( b3Vec2, int button ) override + { + if ( button == GLFW_MOUSE_BUTTON_1 ) + { + m_dragging = false; + m_rotating = false; + } + } + + void MouseMove( b3Vec2 ps ) override + { + if ( m_dragging ) + { + PickRay ray = m_camera->BuildPickRay( ps.x, ps.y ); + b3Vec3 d = b3Normalize( ray.translation ); + b3Vec3 p = b3MulSub( ray.origin, b3Dot( ray.origin, d ), d ); + m_transform.p = m_basePosition + ( p - m_dragStart ); + } + else if ( m_rotating && m_camera->m_width > 0.0f ) + { + float dx = ( ps.x - m_rotateStart ) / m_camera->m_width; + float angle = b3ClampFloat( 2.0f * dx, -B3_PI, B3_PI ); + b3Vec3 axis = m_camera->GetForward(); + b3Quat deltaQuat = b3MakeQuatFromAxisAngle( axis, angle ); + m_transform.q = b3MulQuat( deltaQuat, m_baseQuat ); + } + } + + static b3Vec3 Weight2( float a1, b3Vec3 w1, float a2, b3Vec3 w2 ) + { + return { a1 * w1.x + a2 * w2.x, a1 * w1.y + a2 * w2.y }; + } + + static b3Vec3 Weight3( float a1, b3Vec3 w1, float a2, b3Vec3 w2, float a3, b3Vec3 w3 ) + { + return { a1 * w1.x + a2 * w2.x + a3 * w3.x, a1 * w1.y + a2 * w2.y + a3 * w3.y }; + } + + static void ComputeWitnessPoints( b3Vec3* a, b3Vec3* b, const b3Simplex* s ) + { + const b3SimplexVertex* vs = s->vertices; + int count = s->count; + B3_ASSERT( 1 <= count && count <= 4 ); + + switch ( count ) + { + case 1: + *a = vs[0].wA; + *b = vs[0].wB; + break; + + case 2: + *a = vs[0].a * vs[0].wA + vs[1].a * vs[1].wA; + *b = vs[0].a * vs[0].wB + vs[1].a * vs[1].wB; + break; + + case 3: + *a = vs[0].a * vs[0].wA + vs[1].a * vs[1].wA + vs[2].a * vs[2].wA; + *b = vs[0].a * vs[0].wB + vs[1].a * vs[1].wB + vs[2].a * vs[2].wB; + break; + + case 4: + // Force identical points and *zero* distance + *a = *b = vs[0].a * vs[0].wA + vs[1].a * vs[1].wA + vs[2].a * vs[2].wA + vs[3].a * vs[3].wA; + break; + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + } + + void Render() override + { + DrawTransform( m_scene, b3Transform_identity, 0.5f ); + Sample::Render(); + } + + void Step() override + { + b3DistanceInput input; + input.proxyA = m_proxyA; + input.proxyB = m_proxyB; + input.transformA = b3Transform_identity; + input.transformB = m_transform; + input.useRadii = m_radiusA > 0.0f || m_radiusB > 0.0f; + + if ( m_useCache == false ) + { + m_cache.count = 0; + } + + b3DistanceOutput output = b3ShapeDistance( &input, &m_cache, m_simplexes, m_simplexCapacity ); + + if ( output.distance > 0.0f ) + { + assert( b3IsNormalized( output.normal ) ); + b3Vec3 d = b3Sub( output.pointB, output.pointA ); + float length = b3Length( d ); + assert( b3AbsFloat( length - output.distance ) < 10.0f * FLT_EPSILON ); + b3Vec3 n = b3Normalize( d ); + assert( b3AbsFloat( n.x - output.normal.x ) < 100.0f * FLT_EPSILON ); + assert( b3AbsFloat( n.y - output.normal.y ) < 100.0f * FLT_EPSILON ); + assert( b3AbsFloat( n.z - output.normal.z ) < 100.0f * FLT_EPSILON ); + } + + m_simplexCount = output.simplexCount; + + DrawShape( m_typeA, b3Transform_identity, m_radiusA, b3_colorCyan ); + DrawShape( m_typeB, m_transform, m_radiusB, b3_colorBisque ); + + if ( m_drawSimplex ) + { + b3Simplex* simplex = m_simplexes + m_simplexIndex; + b3SimplexVertex* vertices = simplex->vertices; + + if ( m_simplexIndex > 0 ) + { + // The first recorded simplex does not have valid barycentric coordinates + b3Vec3 pointA, pointB; + ComputeWitnessPoints( &pointA, &pointB, simplex ); + + DrawLine( m_scene, pointA, pointB, b3_colorWhite ); + DrawPoint( m_scene, pointA, 10.0f, b3_colorLightGreen ); + DrawPoint( m_scene, pointB, 10.0f, b3_colorLightBlue ); + } + + b3HexColor colors[3] = { b3_colorRed, b3_colorGreen, b3_colorBlue }; + + for ( int i = 0; i < simplex->count; ++i ) + { + b3SimplexVertex* vertex = vertices + i; + DrawPoint( m_scene, vertex->wA, 10.0f, colors[i] ); + DrawPoint( m_scene, vertex->wB, 10.0f, colors[i] ); + } + } + else + { + DrawLine( m_scene, output.pointA, output.pointB, b3_colorDimGray ); + DrawPoint( m_scene, output.pointA, 10.0f, b3_colorLightGreen ); + DrawPoint( m_scene, output.pointB, 10.0f, b3_colorLightBlue ); + DrawLine( m_scene, output.pointA, output.pointA + 0.5f * output.normal, b3_colorYellow ); + } + + if ( m_showIndices ) + { + for ( int i = 0; i < m_proxyA.count; ++i ) + { + b3Vec3 p = m_proxyA.points[i]; + DrawWorldString( m_camera, p, b3_colorWhite, " %d", i ); + } + + for ( int i = 0; i < m_proxyB.count; ++i ) + { + b3Vec3 p = b3TransformPoint( m_transform, m_proxyB.points[i] ); + DrawWorldString( m_camera, p, b3_colorWhite, " %d", i ); + } + } + + DrawTextLine( "mouse button 1: drag" ); + DrawTextLine( "mouse button 1 + shift: rotate" ); + DrawTextLine( "distance = %.4f, iterations = %d", output.distance, output.iterations ); + + if ( m_cache.count == 1 ) + { + DrawTextLine( "cache = {%d}, {%d}", m_cache.indexA[0], m_cache.indexB[0] ); + } + else if ( m_cache.count == 2 ) + { + DrawTextLine( "cache = {%d, %d}, {%d, %d}", m_cache.indexA[0], m_cache.indexA[1], m_cache.indexB[0], + m_cache.indexB[1] ); + } + else if ( m_cache.count == 3 ) + { + DrawTextLine( "cache = {%d, %d, %d}, {%d, %d, %d}", m_cache.indexA[0], m_cache.indexA[1], m_cache.indexA[2], + m_cache.indexB[0], m_cache.indexB[1], m_cache.indexB[2] ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new ShapeDistance( context ); + } + + static constexpr int m_simplexCapacity = 20; + + b3BoxHull m_box; + b3Vec3 m_triangle[3]; + b3Vec3 m_point; + b3Vec3 m_segment[2]; + + ShapeType m_typeA; + ShapeType m_typeB; + float m_radiusA; + float m_radiusB; + b3ShapeProxy m_proxyA; + b3ShapeProxy m_proxyB; + + b3SimplexCache m_cache; + b3Simplex m_simplexes[m_simplexCapacity]; + int m_simplexCount; + int m_simplexIndex; + + b3Transform m_transform; + + b3Vec3 m_basePosition; + b3Vec3 m_dragStart; + + b3Quat m_baseQuat; + float m_rotateStart; + + bool m_dragging; + bool m_rotating; + bool m_showIndices; + bool m_useCache; + bool m_drawSimplex; +}; + +static int sampleShapeDistance = SampleManager::Register( "Collision", "Shape Distance", ShapeDistance::Create ); + +class TimeOfImpact : public Sample +{ +public: + enum ShapeType + { + e_box, + e_capsule, + e_triangle, + }; + + explicit TimeOfImpact( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( -90.0f, 0.0f, 10.0f, { 0.0f, 0.0f, 0.0f } ); + } + + m_box = b3MakeBoxHull( 0.02f, 0.2f, 0.04f ); + m_capsule = { { 0.0f, -0.2f, 0.0f }, { 0.0f, 0.2f, 0.0f }, 0.02f }; + m_triangle[0] = { -4.0f, 0.0f, -4.0f }; + m_triangle[1] = { -4.0f, 0.0f, -8.0f }; + m_triangle[2] = { -8.0f, 0.0f, -8.0f }; + + m_typeA = e_triangle; + m_typeB = e_capsule; + + m_proxyA = MakeProxy( m_typeA ); + m_proxyB = MakeProxy( m_typeB ); + + m_sweepA = {}; + m_sweepA.q1.s = 1.0f; + m_sweepA.q2.s = 1.0f; + + m_sweepB = {}; + m_sweepB.c1 = { -4.06512070, 0.101333618, -7.87591267 }; + m_sweepB.c2 = { -4.15895557, 0.0356027633, -7.69682646 }; + m_sweepB.q1 = { { -0.860495985, -0.272824734, 0.0724888667 }, 0.424097389 }; + m_sweepB.q2 = { { -0.604184389, -0.424355596, 0.0457959622 }, 0.672894001 }; + } + + b3ShapeProxy MakeProxy( ShapeType type ) + { + b3ShapeProxy proxy = {}; + + switch ( type ) + { + case e_capsule: + proxy.points = &m_capsule.center1; + proxy.radius = m_capsule.radius; + proxy.count = 2; + break; + + case e_triangle: + proxy.points = m_triangle; + proxy.count = 3; + break; + + case e_box: + proxy.points = b3GetHullPoints( &m_box.base ); + proxy.count = m_box.base.vertexCount; + break; + + default: + assert( false ); + } + + return proxy; + } + + void DrawShape( ShapeType type, b3Transform transform, b3HexColor color ) + { + switch ( type ) + { + case e_capsule: + { + DrawCapsule( m_scene, transform, m_capsule, color ); + b3Vec3 center = b3Lerp( m_capsule.center1, m_capsule.center2, 0.5f ); + center = b3TransformPoint( transform, center ); + b3Transform xf = { + .p = center, + .q = transform.q, + }; + DrawTransform( m_scene, xf, 0.025f ); + } + break; + + case e_triangle: + { + b3Vec3 p1 = b3TransformPoint( transform, m_triangle[0] ); + b3Vec3 p2 = b3TransformPoint( transform, m_triangle[1] ); + b3Vec3 p3 = b3TransformPoint( transform, m_triangle[2] ); + DrawLine( m_scene, p1, p2, color ); + DrawLine( m_scene, p2, p3, color ); + DrawLine( m_scene, p3, p1, color ); + DrawWorldString( m_camera, p1, b3_colorWhite, "0" ); + DrawWorldString( m_camera, p2, b3_colorWhite, "1" ); + DrawWorldString( m_camera, p3, b3_colorWhite, "2" ); + } + break; + + case e_box: + DrawHull( m_scene, transform, &m_box.base, color, false ); + break; + + default: + assert( false ); + } + } + + void UpdateUI() override + { + float fontSize = ImGui::GetFontSize(); + float height = 16.0f * fontSize; + ImGui::SetNextWindowPos( ImVec2( 0.5f * fontSize, m_camera->m_height - height - 2.0f * fontSize ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 19.0f * fontSize, height ) ); + + ImGui::Begin( "Time of Impact", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + const char* shapeTypes[] = { "point", "segment", "triangle", "box" }; + int shapeType = int( m_typeA ); + if ( ImGui::Combo( "shape A", &shapeType, shapeTypes, IM_ARRAYSIZE( shapeTypes ) ) ) + { + m_typeA = ShapeType( shapeType ); + m_proxyA = MakeProxy( m_typeA ); + } + + shapeType = int( m_typeB ); + if ( ImGui::Combo( "shape B", &shapeType, shapeTypes, IM_ARRAYSIZE( shapeTypes ) ) ) + { + m_typeB = ShapeType( shapeType ); + m_proxyB = MakeProxy( m_typeB ); + } + + ImGui::Separator(); + + ImGui::End(); + } + + void Render() override + { + DrawTransform( m_scene, b3Transform_identity, 0.5f ); + Sample::Render(); + } + + void Step() override + { + b3TOIInput input = {}; + input.proxyA = m_proxyA; + input.proxyB = m_proxyB; + input.sweepA = m_sweepA; + input.sweepB = m_sweepB; + input.maxFraction = 1.0f; + + b3TOIOutput output = b3TimeOfImpact( &input ); + + DrawShape( m_typeA, b3Transform_identity, b3_colorCyan ); + + b3Transform transform1 = b3GetSweepTransform( &m_sweepB, 0.0f ); + b3Transform transform2 = b3GetSweepTransform( &m_sweepB, 1.0f ); + + // qr = inv(q1) * q2 + b3Quat qr = b3InvMulQuat( transform1.q, transform2.q ); + float angle; + (void)b3GetAxisAngle( &angle, qr ); + DrawTextLine( "angle = %g", 180.0f * angle / B3_PI ); + + DrawShape( m_typeB, transform1, b3_colorLightGreen ); + DrawShape( m_typeB, transform2, b3_colorLightCoral ); + + if ( output.fraction < 1.0f ) + { + b3Transform transformHit = b3GetSweepTransform( &m_sweepB, output.fraction ); + DrawShape( m_typeB, transformHit, b3_colorLightCyan ); + } + + if ( output.state == b3_toiStateHit || output.state == b3_toiStateFailed ) + { + DrawLine( m_scene, output.point, output.point + 0.5f * output.normal, b3_colorDimGray ); + DrawPoint( m_scene, output.point, 10.0f, b3_colorLightGreen ); + } + + switch ( output.state ) + { + case b3_toiStateUnknown: + DrawTextLine( "unknown" ); + break; + + case b3_toiStateFailed: + DrawTextLine( "failed" ); + break; + + case b3_toiStateOverlapped: + DrawTextLine( "overlapped" ); + break; + + case b3_toiStateHit: + DrawTextLine( "hit %g", output.fraction ); + break; + + case b3_toiStateSeparated: + DrawTextLine( "separated" ); + break; + + default: + assert( false ); + break; + } + + DrawTextLine( "iterations / push / root = %d / %d / %d", output.distanceIterations, output.pushBackIterations, + output.rootIterations ); + } + + static Sample* Create( SampleContext* context ) + { + return new TimeOfImpact( context ); + } + + b3BoxHull m_box; + b3Vec3 m_triangle[3]; + b3Capsule m_capsule; + + ShapeType m_typeA; + ShapeType m_typeB; + b3ShapeProxy m_proxyA; + b3ShapeProxy m_proxyB; + b3Sweep m_sweepA; + b3Sweep m_sweepB; +}; + +static int sampleTimeOfImpact = SampleManager::Register( "Collision", "Time of Impact", TimeOfImpact::Create ); + +class CapsuleCastRay : public Sample +{ +public: + static Sample* Create( SampleContext* context ) + { + return new CapsuleCastRay( context ); + } + + explicit CapsuleCastRay( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 120.0f, 30.0f, 20.0f, { 0.0f, 1.5f, 0.0f } ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_kinematicBody; + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + m_scale = 1.0f; + + // b3Capsule Capsule = { + // { 1.88094640f, 1.13457823f, 30.2966614f }, { -1.37514091f, -2.26233172f, 848.671997f }, 51.7184296f + // }; + + b3Capsule capsule = { + .center1 = { 0.0f, 0.0f, 0.0f }, + .center2 = { 0.0f, 1.0f, 0.0f }, + .radius = 0.5f, + }; + + m_offset = capsule.center1; + + b3CreateCapsuleShape( m_bodyId, &shapeDef, &capsule ); + } + + void Render() override + { + Sample::Render(); + + DrawGrid( m_scene, 10 ); + DrawLine( m_scene, b3Vec3_zero, 0.4f * b3Vec3_axisX, b3_colorRed ); + DrawLine( m_scene, b3Vec3_zero, 0.4f * b3Vec3_axisY, b3_colorGreen ); + DrawLine( m_scene, b3Vec3_zero, 0.4f * b3Vec3_axisZ, b3_colorBlue ); + + b3BodyRayCastInput input; + input.origin = { -1.0f, 0.5f, 0.0f }; + input.translation = { 2.0f, 0.0f, 0.0f }; + input.filter = b3DefaultQueryFilter(); + input.maxFraction = 1.0f; + + b3BodyCastResult result = b3Body_CastRay( m_bodyId, &input, b3Transform_identity ); + + DrawLine( m_scene, input.origin, input.origin + input.translation, b3_colorGray ); + DrawPoint( m_scene, input.origin, 4.0f, b3_colorGreen ); + DrawPoint( m_scene, input.origin + input.translation, 4.0f, b3_colorRed ); + + if ( result.hit ) + { + DrawPoint( m_scene, result.point, 4.0f, b3_colorOrange ); + } + } + + b3BodyId m_bodyId; + b3Vec3 m_offset; + float m_scale; +}; + +static int sampleCapsuleCastRay = SampleManager::Register( "Collision", "Capsule Cast Ray", CapsuleCastRay::Create ); + +class Hull2D : public Sample +{ +public: + enum SourceMode + { + e_random = 0, + e_preset = 1, + }; + + static Sample* Create( SampleContext* context ) + { + return new Hull2D( context ); + } + + explicit Hull2D( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 0.0f, 20.0f, { 0.0f, 0.0f, 0.0f } ); + } + + m_sourceMode = e_preset; + m_inputCount = 0; + m_hullCount = 0; + m_simplifiedCount = 0; + m_targetCount = 6; + m_randomCount = 20; + m_simpleCull = true; + m_drawLabels = true; + + Generate(); + } + + void Generate() + { + if ( m_sourceMode == e_random ) + { + m_inputCount = m_randomCount; + for ( int i = 0; i < m_inputCount; ++i ) + { + m_inputPoints[i].p = { RandomFloatRange( -5.0f, 5.0f ), RandomFloatRange( -5.0f, 5.0f ) }; + m_inputPoints[i].separation = 0.0f; + m_inputPoints[i].originalIndex = (int16_t)i; + m_inputPoints[i].persisted = false; + } + } + else + { +#if 0 + b3Vec2 points[m_pointCapacity]; + int n = b3MinInt(15, m_pointCapacity); + float delta = 2.0f * B3_PI / (float)n; + // This makes a difference when comparing areas + //b3CosSin cs = b3ComputeCosSin( delta ); + b3CosSin cs = { + cosf( delta ), + sinf( delta ), + }; + ; + float x1 = 0.5f, y1 = 0.0f; + + for ( int i = 0; i < n; ++i ) + { + points[i] = {x1, y1}; + float x2 = cs.cosine * x1 - cs.sine * y1; + float y2 = cs.sine * x1 + cs.cosine * y1; + x1 = x2; + y1 = y2; + } + m_drawScale = 1.0f; +#else + b3Vec2 points[] = { + { 0.00000000, 0.00000000 }, { -0.00305632968, 5.54900616e-05 }, { -0.00305632968, 0.0246563386 }, + { -0.00305632968, 0.0246562902 }, { -0.00305632968, 0.0800221488 }, + }; + int n = sizeof( points ) / sizeof( b3Vec2 ); + m_drawScale = 50.0f; +#endif + + m_targetCount = b3MinInt( 6, n ); + m_inputCount = n; + + for ( int i = 0; i < m_inputCount; ++i ) + { + m_inputPoints[i].p = { points[i].x, points[i].y }; + m_inputPoints[i].separation = 0.0f; + m_inputPoints[i].originalIndex = (int16_t)i; + m_inputPoints[i].persisted = false; + } + } + } + + void ComputeHull() + { + // Copy input for hull computation because b3Hull2D may modify the array + + if ( m_simpleCull ) + { + for ( int i = 0; i < m_inputCount; ++i ) + { + m_hullPoints[i] = m_inputPoints[i]; + } + + m_hullCount = b3CullPoints( m_hullPoints, m_inputCount, m_targetCount ); + } + else + { + // Copy input for hull computation because b3Hull2D may modify the array + b3Point2D points[m_pointCapacity]; + for ( int i = 0; i < m_inputCount; ++i ) + { + points[i] = m_inputPoints[i]; + } + + m_hullCount = b3Hull2D( points, m_inputCount, m_hullPoints ); + + // Copy hull for simplification + for ( int i = 0; i < m_hullCount; ++i ) + { + m_simplifiedPoints[i] = m_hullPoints[i]; + } + + int target = m_targetCount < 3 ? 3 : m_targetCount; + m_simplifiedCount = b3SimplifyHull2D( m_simplifiedPoints, m_hullCount, target ); + } + } + + void UpdateUI() override + { + float fontSize = ImGui::GetFontSize(); + float height = 16.0f * fontSize; + ImGui::SetNextWindowPos( ImVec2( 0.5f * fontSize, m_camera->m_height - height - 2.0f * fontSize ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 19.0f * fontSize, height ) ); + + ImGui::Begin( "Hull 2D", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + const char* modes[] = { "Random", "Preset" }; + if ( ImGui::Combo( "Source", (int*)&m_sourceMode, modes, IM_ARRAYSIZE( modes ) ) ) + { + Generate(); + } + + if ( m_sourceMode == e_random ) + { + if ( ImGui::SliderInt( "Point Count", &m_randomCount, 4, m_pointCapacity ) ) + { + Generate(); + } + + if ( ImGui::Button( "Regenerate" ) ) + { + Generate(); + } + } + + ImGui::Checkbox( "Simple Cull", &m_simpleCull ); + ImGui::SliderInt( "Target Count", &m_targetCount, 1, m_inputCount ); + + ImGui::Checkbox( "Draw Labels", &m_drawLabels ); + + ImGui::End(); + } + + void Render() override + { + Sample::Render(); + + DrawTextLine( "Hull 2D: input=%d, hull=%d, simplified=%d", m_inputCount, m_hullCount, m_simplifiedCount ); + + DrawTransform( m_scene, b3Transform_identity, 0.4f ); + + bool onHull[m_pointCapacity] = {}; + for ( int i = 0; i < m_hullCount; ++i ) + { + onHull[m_hullPoints[i].originalIndex] = true; + } + + float s = m_drawScale; + + // Draw input points (small gray) + for ( int i = 0; i < m_inputCount; ++i ) + { + if ( onHull[i] ) + { + continue; + } + + b3Vec3 p = { s * m_inputPoints[i].p.x, s * m_inputPoints[i].p.y, 0.0f }; + DrawPoint( m_scene, p, 4.0f, b3_colorGray ); + + if ( m_drawLabels ) + { + DrawWorldString( m_camera, p, b3_colorGray, " %d", i ); + } + } + + // Draw convex hull edges and vertices (cyan) + for ( int i = 0; i < m_hullCount; ++i ) + { + int next = ( i + 1 ) % m_hullCount; + b3Vec3 p1 = { s * m_hullPoints[i].p.x, s * m_hullPoints[i].p.y, 0.0f }; + b3Vec3 p2 = { s * m_hullPoints[next].p.x, s * m_hullPoints[next].p.y, 0.0f }; + DrawLine( m_scene, p1, p2, b3_colorCyan ); + DrawPoint( m_scene, p1, 6.0f, b3_colorCyan ); + + if ( m_drawLabels ) + { + DrawWorldString( m_camera, p1, b3_colorCyan, " %d : %d", m_hullPoints[i].originalIndex, i ); + } + } + + if ( m_simpleCull == false ) + { + // Draw simplified hull edges and vertices (yellow, slightly offset in z so visible) + for ( int i = 0; i < m_simplifiedCount; ++i ) + { + int next = ( i + 1 ) % m_simplifiedCount; + b3Vec3 p1 = { s * m_simplifiedPoints[i].p.x, s * m_simplifiedPoints[i].p.y, 0.01f }; + b3Vec3 p2 = { s * m_simplifiedPoints[next].p.x, s * m_simplifiedPoints[next].p.y, 0.01f }; + DrawLine( m_scene, p1, p2, b3_colorYellow ); + DrawPoint( m_scene, p1, 10.0f, b3_colorYellow ); + } + } + } + + void Step() override + { + ComputeHull(); + } + + static constexpr int m_pointCapacity = 128; + SourceMode m_sourceMode; + b3Point2D m_inputPoints[m_pointCapacity]; + b3Point2D m_hullPoints[2 * m_pointCapacity]; + b3Point2D m_simplifiedPoints[2 * m_pointCapacity]; + float m_drawScale; + int m_inputCount; + int m_hullCount; + int m_simplifiedCount; + int m_targetCount; + int m_randomCount; + bool m_simpleCull; + bool m_drawLabels; +}; + +static int sampleHull2D = SampleManager::Register( "Collision", "Hull 2D", Hull2D::Create ); diff --git a/samples/sample_compound.cpp b/samples/sample_compound.cpp new file mode 100644 index 0000000..67a263c --- /dev/null +++ b/samples/sample_compound.cpp @@ -0,0 +1,800 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "camera.h" +#include "human.h" +#include "mesh_loader.h" +#include "renderer.h" +#include "sample.h" +#include "scene.h" +#include "utils.h" + +#include "box3d/box3d.h" + +#include + +class SimpleCompound : public Sample +{ +public: + explicit SimpleCompound( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 45.0f, b3Vec3_zero ); + } + + { + float a = 4.0f; + b3Vec3 extents = { a, 0.125f * a, a }; + b3SurfaceMaterial material = b3DefaultSurfaceMaterial(); + b3BoxHull box = b3MakeBoxHull( extents.x, extents.y, extents.z ); + + b3Transform hullTransform; + hullTransform.p = { 1.0f, -0.125f * a, 0.0f }; + hullTransform.q = b3MakeQuatFromAxisAngle( b3Normalize({ 1.0f, 0.0f, 1.0f }), 0.0f * B3_PI ); + + b3CompoundHullDef hullDef = { + .hull = &box.base, + .transform = hullTransform, + .material = material, + }; + + b3CompoundDef def = {}; + def.hulls = &hullDef; + def.hullCount = 1; + + m_compound = b3CreateCompound( &def ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 2.0f, -1.0f, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( { 0.0f, 1.0f, 0.0f }, 0.25f * B3_PI ); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + (void)b3CreateCompoundShape( groundId, &shapeDef, m_compound ); + } + + b3World_SetContactRecycleDistance( m_worldId, 0.0f ); + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 2.0f, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3Sphere sphere = { b3Vec3_zero, 0.25f }; + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + } + + //{ + // b3BodyDef bodyDef = b3DefaultBodyDef(); + // bodyDef.type = b3_dynamicBody; + // bodyDef.position = { 0.0f, 10.0f, 2.0f }; + // b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + // b3ShapeDef shapeDef = b3DefaultShapeDef(); + // b3BoxHull box = b3MakeBoxHull( 0.2f, 0.3f, 0.4f ); + // b3CreateHullShape( bodyId, &shapeDef, &box.base ); + //} + + m_launchSpeedScale = 1.0f; + } + + ~SimpleCompound() override + { + b3DestroyCompound( m_compound ); + } + + void Render() override + { + Sample::Render(); + b3Transform transform = { { 0.0f, 0.01f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 1.0f ); + + int height = b3DynamicTree_GetHeight( &m_compound->tree ); + DrawTextLine( "compound tree height = %d", height ); + } + + static Sample* Create( SampleContext* context ) + { + return new SimpleCompound( context ); + } + + b3Compound* m_compound; +}; + +static int sampleSimple = SampleManager::Register( "Compound", "Simple", SimpleCompound::Create ); + +class CompoundSpheres : public Sample +{ +public: + explicit CompoundSpheres( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 45.0f, b3Vec3_zero ); + } + + float h = 10.0f; + b3Vec3 lower = { -h, -h, -h }; + b3Vec3 upper = { h, h, h }; + b3CompoundSphereDef spheres[m_count]; + for ( int i = 0; i < m_count; ++i ) + { + spheres[i].sphere.center = RandomVec3( lower, upper ); + spheres[i].sphere.radius = RandomFloatRange( 0.01f * h, 0.05f * h ); + spheres[i].material = b3DefaultSurfaceMaterial(); + } + + b3CompoundDef def = {}; + def.spheres = spheres; + def.sphereCount = m_count; + + m_compound = b3CreateCompound( &def ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + (void)b3CreateCompoundShape( groundId, &shapeDef, m_compound ); + } + + ~CompoundSpheres() override + { + b3DestroyCompound( m_compound ); + } + + void Render() override + { + Sample::Render(); + b3Transform transform = { { 0.0f, 0.01f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 1.0f ); + + int height = b3DynamicTree_GetHeight( &m_compound->tree ); + DrawTextLine( "compound tree height = %d", height ); + } + + static Sample* Create( SampleContext* context ) + { + return new CompoundSpheres( context ); + } + + static constexpr int m_count = 20; + b3Compound* m_compound; +}; + +static int sampleCompoundSpheres = SampleManager::Register( "Compound", "Spheres", CompoundSpheres::Create ); + +class CompoundHulls : public Sample +{ +public: + explicit CompoundHulls( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 45.0f, b3Vec3_zero ); + } + + float h = 10.0f; + b3Vec3 lower = { -h, -h, -h }; + b3Vec3 upper = { h, h, h }; + b3SurfaceMaterial material = b3DefaultSurfaceMaterial(); + b3BoxHull boxHulls[m_count]; + b3CompoundHullDef hulls[m_count]; + for ( int i = 0; i < m_count; ++i ) + { + b3Vec3 extents = { + .x = RandomFloatRange( 0.01f * h, 0.05f * h ), + .y = RandomFloatRange( 0.01f * h, 0.05f * h ), + .z = RandomFloatRange( 0.01f * h, 0.05f * h ), + }; + + b3Transform transform; + transform.p = RandomVec3( lower, upper ); + transform.q = RandomQuat(); + + boxHulls[i] = b3MakeBoxHull( extents.x, extents.y, extents.z ); + hulls[i].hull = &boxHulls[i].base; + hulls[i].transform = transform; + hulls[i].material = material; + } + + b3CompoundDef def = {}; + def.hulls = hulls; + def.hullCount = m_count; + + m_compound = b3CreateCompound( &def ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + (void)b3CreateCompoundShape( groundId, &shapeDef, m_compound ); + } + + ~CompoundHulls() override + { + b3DestroyCompound( m_compound ); + } + + void Render() override + { + Sample::Render(); + b3Transform transform = { { 0.0f, 0.01f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 1.0f ); + + int height = b3DynamicTree_GetHeight( &m_compound->tree ); + DrawTextLine( "compound tree height = %d", height ); + } + + static Sample* Create( SampleContext* context ) + { + return new CompoundHulls( context ); + } + + static constexpr int m_count = 20; + b3Compound* m_compound; +}; + +static int sampleCompoundHulls = SampleManager::Register( "Compound", "Hulls", CompoundHulls::Create ); + +class TileFloor : public Sample +{ +public: + explicit TileFloor( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 45.0f, b3Vec3_zero ); + } + + { + constexpr int gridCount = 50; + constexpr int boxCount = gridCount * gridCount; + + float a = 4.0f; + b3Vec3 extents = { a, 0.5f * a, a }; + b3SurfaceMaterial material = b3DefaultSurfaceMaterial(); + b3BoxHull box = b3MakeBoxHull( extents.x, extents.y, extents.z ); + + // Must allocate to avoid stack overflow + b3CompoundHullDef* hulls = new b3CompoundHullDef[boxCount]; + b3Transform transform = b3Transform_identity; + + int index = 0; + for ( int i = 0; i < gridCount; ++i ) + { + transform.p.x = ( 2.0f * i - gridCount ) * a; + + for ( int j = 0; j < gridCount; ++j ) + { + transform.p.z = ( 2.0f * j - gridCount ) * a; + transform.p.y = RandomFloatRange( -0.5f, 0.25f ) * a; + + assert( index < boxCount ); + + hulls[index].hull = &box.base; + hulls[index].transform = transform; + hulls[index].material = material; + + index += 1; + } + } + + b3CompoundDef def = {}; + def.hulls = hulls; + def.hullCount = boxCount; + + m_compound = b3CreateCompound( &def ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { -2.0f, 1.0f, -3.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Normalize( { 1.0f, -1.0f, 0.5f } ), 0.0f ); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + (void)b3CreateCompoundShape( groundId, &shapeDef, m_compound ); + + delete[] hulls; + hulls = nullptr; + } + + //b3World_SetContactRecycleDistance( m_worldId, 0.0f ); + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 3.0f, 12.0f, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3Sphere sphere = { b3Vec3_zero, 0.25f }; + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + } + +#if 0 + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 10.0f, 2.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeBoxHull( 0.2f, 0.3f, 0.4f ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } +#endif + } + + ~TileFloor() override + { + b3DestroyCompound( m_compound ); + } + + void Render() override + { + Sample::Render(); + b3Transform transform = { { 0.0f, 0.01f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 1.0f ); + + DrawTextLine( "compound hull count = %d, mesh count = %d", m_compound->hullCount, m_compound->meshCount ); + DrawTextLine( "compound byte count = %d", m_compound->byteCount ); + + int treeBytes = b3DynamicTree_GetByteCount( &m_compound->tree ); + int height = b3DynamicTree_GetHeight( &m_compound->tree ); + DrawTextLine( "compound tree byte count = %d, height = %d", treeBytes, height ); + } + + static Sample* Create( SampleContext* context ) + { + return new TileFloor( context ); + } + + b3Compound* m_compound; +}; + +static int sampleTileFloor = SampleManager::Register( "Compound", "Tile Floor", TileFloor::Create ); + +class MeshTile : public Sample +{ +public: + explicit MeshTile( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 45.0f, b3Vec3_zero ); + } + + { + constexpr int gridCount = 2; + constexpr int boxCount = gridCount * gridCount; + + float a = 4.0f; + b3Vec3 extents = { a, 0.5f * a, a }; + b3SurfaceMaterial material = b3DefaultSurfaceMaterial(); + + b3MeshData* box = b3CreateBoxMesh( { 0.0f, 0.0f, 0.0f }, extents, true ); + b3CompoundMeshDef* meshes = new b3CompoundMeshDef[boxCount]; + b3Transform transform = b3Transform_identity; + transform.p.y = -0.5f * a; + + int index = 0; + for ( int i = 0; i < gridCount; ++i ) + { + transform.p.x = ( 2.0f * i - gridCount ) * a; + + for ( int j = 0; j < gridCount; ++j ) + { + transform.p.z = ( 2.0f * j - gridCount ) * a; + + transform.p.y = RandomFloatRange( -0.5f, 0.25f ) * a; + + assert( index < boxCount ); + + meshes[index].meshData = box; + meshes[index].transform = transform; + meshes[index].scale = { 1.0f, 1.0f, 1.0f }; + meshes[index].materials = &material; + meshes[index].materialCount = 1; + + index += 1; + } + } + + b3CompoundDef def = {}; + def.meshes = meshes; + def.meshCount = boxCount; + + m_compound = b3CreateCompound( &def ); + delete[] meshes; + b3DestroyMesh( box ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + (void)b3CreateCompoundShape( groundId, &shapeDef, m_compound ); + } + +#if 0 + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 3.0f, 12.0f, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3Sphere sphere = { b3Vec3_zero, 0.25f }; + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + } +#endif + //{ + // b3BodyDef bodyDef = b3DefaultBodyDef(); + // bodyDef.type = b3_dynamicBody; + // bodyDef.position = { 0.0f, 10.0f, 2.0f }; + // b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + // b3ShapeDef shapeDef = b3DefaultShapeDef(); + // b3BoxHull box = b3MakeBoxHull( 0.2f, 0.3f, 0.4f ); + // b3CreateHullShape( bodyId, &shapeDef, &box.base ); + //} + } + + ~MeshTile() override + { + b3DestroyCompound( m_compound ); + } + + void Render() override + { + Sample::Render(); + b3Transform transform = { { 0.0f, 0.01f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 1.0f ); + + DrawTextLine( "compound instance count = %d, byte count = %d", m_compound->meshCount, m_compound->byteCount ); + + int treeBytes = b3DynamicTree_GetByteCount( &m_compound->tree ); + int height = b3DynamicTree_GetHeight( &m_compound->tree ); + DrawTextLine( "compound tree byte count = %d, height = %d", treeBytes, height ); + } + + static Sample* Create( SampleContext* context ) + { + return new MeshTile( context ); + } + + b3Compound* m_compound; +}; + +static int sampleCompoundMesh = SampleManager::Register( "Compound", "Mesh Tile", MeshTile::Create ); + +static bool OverlapResultFcn( b3ShapeId shapeId, void* context ) +{ + (void)shapeId; + bool* overlap = (bool*)context; + *overlap = true; + return false; +} + +class Village : public Sample +{ +public: + explicit Village( SampleContext* context ) + : Sample( context ) + { + constexpr int gridCount = m_isDebug ? 8 : 200; + constexpr float a = 4.0f; + m_worldWidth = 2.0f * gridCount * a; + + b3Vec3 position = { 0.0f, 10.0f, 0.0f }; + if ( m_context->restart == false ) + { + m_camera->SetView( 45.0f, 10.0f, 5.0f, position ); + } + + m_launchSpeedScale = 1.0f; + m_mover.Initialize( this, position ); + + { + constexpr int capsuleCapacity = gridCount * gridCount / 8 + 1; + constexpr int hullCount = gridCount * gridCount; + constexpr int sphereCapacity = gridCount * gridCount / 8 + 1; + + b3Vec3 extents = { a, 0.5f * a, a }; + b3SurfaceMaterial material = b3DefaultSurfaceMaterial(); + b3BoxHull box = b3MakeBoxHull( extents.x, extents.y, extents.z ); + + // Must allocate to avoid stack overflow + b3CompoundCapsuleDef* capsules = new b3CompoundCapsuleDef[capsuleCapacity]; + b3CompoundHullDef* hulls = new b3CompoundHullDef[hullCount]; + b3CompoundSphereDef* spheres = new b3CompoundSphereDef[sphereCapacity]; + + b3Transform transform = b3Transform_identity; + + int capsuleIndex = 0; + int hullIndex = 0; + int sphereIndex = 0; + + for ( int i = 0; i < gridCount; ++i ) + { + transform.p.x = ( 2.0f * i - gridCount ) * a; + + for ( int j = 0; j < gridCount; ++j ) + { + transform.p.z = ( 2.0f * j - gridCount ) * a; + transform.p.y = RandomFloatRange( -0.25f, 0.125f ) * a; + + assert( hullIndex < hullCount ); + + if ( ( i & 1 ) && ( j & 1 ) ) + { + b3Vec3 p1 = transform.p + RandomVec3( { -a, a, -a }, { a, 2.0f * a, a } ); + b3Vec3 p2 = transform.p + RandomVec3( { -a, a, -a }, { a, 2.0f * a, a } ); + float radius = RandomFloatRange( 0.1f, 0.5f ); + + if ( capsuleIndex < sphereIndex ) + { + assert( capsuleIndex < capsuleCapacity ); + capsules[capsuleIndex].capsule = { p1, p2, radius }; + capsules[capsuleIndex].material = material; + capsuleIndex += 1; + } + else + { + assert( sphereIndex < capsuleCapacity ); + spheres[sphereIndex].sphere = { p1, radius }; + spheres[sphereIndex].material = material; + sphereIndex += 1; + } + } + + hulls[hullIndex].hull = &box.base; + hulls[hullIndex].transform = transform; + hulls[hullIndex].material = material; + + hullIndex += 1; + } + } + + assert( capsuleIndex <= capsuleCapacity ); + assert( hullIndex == hullCount ); + assert( sphereIndex <= sphereCapacity ); + + constexpr int meshGridCount = gridCount / 4; + constexpr int meshCount = meshGridCount * meshGridCount; + constexpr float b = 4.0f * a; + constexpr int materialCapacity = 5; + + b3SurfaceMaterial meshMaterials[materialCapacity]; + b3MeshData* buildingMesh = CreateMeshData( "data/meshes/building.obj", 1.0f, false, false, true, true ); + + int materialCount = buildingMesh->materialCount; + assert( materialCount <= materialCapacity ); + + for ( int i = 0; i < materialCount; ++i ) + { + meshMaterials[i] = b3DefaultSurfaceMaterial(); + if ( i == 0 ) + { + meshMaterials[i].friction = 0.0f; + } + else if ( i == 1 ) + { + meshMaterials[i].restitution = 0.5f; + } + meshMaterials[i].userMaterialId = i + 42; + } + + b3CompoundMeshDef* meshes = new b3CompoundMeshDef[meshCount]; + transform = b3Transform_identity; + + int meshIndex = 0; + for ( int i = 0; i < meshGridCount; ++i ) + { + transform.p.x = ( 2.0f * i - meshGridCount ) * b + 0.5f * b; + + for ( int j = 0; j < meshGridCount; ++j ) + { + transform.p.y = 0.5f * a; + transform.p.z = ( 2.0f * j - meshGridCount ) * b + 0.5f * b; + transform.q = b3MakeQuatFromAxisAngle( { 0.0f, 1.0f, 0.0 }, RandomFloatRange( -B3_PI, B3_PI ) ); + + assert( meshIndex < meshCount ); + + meshes[meshIndex].meshData = buildingMesh; + meshes[meshIndex].transform = transform; + meshes[meshIndex].scale = RandomVec3( { 0.5f, 0.5f, 0.5f }, { 2.0f, 2.0f, 2.0f } ); + + if ( meshIndex & 1 ) + { + meshes[meshIndex].scale.x = -meshes[meshIndex].scale.x; + } + + if ( meshIndex & 3 ) + { + meshes[meshIndex].scale.z = -meshes[meshIndex].scale.z; + } + + meshes[meshIndex].materials = meshMaterials; + meshes[meshIndex].materialCount = materialCount; + + meshIndex += 1; + } + } + + assert( meshIndex == meshCount ); + + b3CompoundDef def = {}; + def.capsules = capsules; + def.capsuleCount = capsuleIndex; + def.hullCount = hullCount; + def.hulls = hulls; + def.hullCount = hullCount; + def.meshes = meshes; + def.meshCount = meshCount; + def.spheres = spheres; + def.sphereCount = sphereIndex; + + m_compound = b3CreateCompound( &def ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { -1.0f, -0.5f, 2.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( {0.0f, 1.0f, 0.0f}, -1.15f * B3_PI ); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + (void)b3CreateCompoundShape( groundId, &shapeDef, m_compound ); + + delete[] capsules; + capsules = nullptr; + + delete[] hulls; + hulls = nullptr; + + delete[] meshes; + meshes = nullptr; + + delete[] spheres; + spheres = nullptr; + + b3DestroyMesh( buildingMesh ); + } + + m_rayOrigin = { -0.45f * m_worldWidth, 20.0f, -0.45f * m_worldWidth }; + } + + ~Village() override + { + b3DestroyCompound( m_compound ); + } + + void Keyboard( int key, int action, int mods ) override + { + if ( key == 'T' && action == 1 ) + { + ToggleThirdPerson(); + } + } + + void Render() override + { + Sample::Render(); + b3Transform transform = { { 0.0f, 0.01f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 4.0f ); + + DrawTextLine( "surface type = %d", m_userMaterialId ); + DrawTextLine( "compound capsules/hulls/meshes/sphere = %d / %d / %d / %d", m_compound->capsuleCount, + m_compound->hullCount, m_compound->meshCount, m_compound->sphereCount ); + DrawTextLine( "compound byte count = %d", m_compound->byteCount ); + + int treeBytes = b3DynamicTree_GetByteCount( &m_compound->tree ); + int height = b3DynamicTree_GetHeight( &m_compound->tree ); + DrawTextLine( "compound tree byte count = %d, height = %d", treeBytes, height ); + } + + void Step() override + { + m_mover.Step( nullptr, 0, true ); + DrawTextLine( "third person (T) = %d", m_camera->m_thirdPerson ); + + b3Vec3 translation = { 10.0f, -40.0f, -5.0f }; + b3QueryFilter filter = b3DefaultQueryFilter(); + + { + CastClosestContext context = {}; + (void)b3World_CastRay( m_worldId, m_rayOrigin, translation, filter, CastClosestCallback, &context ); + + DrawLine( m_scene, m_rayOrigin, m_rayOrigin + translation, b3_colorAliceBlue ); + if ( context.hit ) + { + b3Vec3 p1 = context.point; + b3Vec3 p2 = b3MulAdd( p1, 0.5f, context.normal ); + DrawLine( m_scene, p1, p2, b3_colorYellow ); + DrawPoint( m_scene, p1, 8.0f, b3_colorLightCoral ); + DrawTextLine( "ray hit triangle/child/material = %d / %d / %d", context.triangleIndex, context.childIndex, + context.materialId ); + } + else + { + DrawTextLine( "ray miss" ); + } + } + + { + CastClosestContext context = {}; + b3Vec3 origin = b3Sub( m_rayOrigin, { 1.0f, 0.0f, 1.0f } ); + b3ShapeProxy proxy = { &origin, 1, 0.25f }; + b3World_CastShape( m_worldId, &proxy, translation, filter, CastClosestCallback, &context ); + + DrawLine( m_scene, origin, origin + translation, b3_colorAliceBlue ); + if ( context.hit ) + { + b3Vec3 position = b3MulAdd( origin, context.fraction, translation ); + b3Vec3 p1 = context.point; + b3Vec3 p2 = b3MulAdd( p1, 0.5f, context.normal ); + DrawLine( m_scene, p1, p2, b3_colorYellow ); + DrawPoint( m_scene, p1, 8.0f, b3_colorLightCoral ); + b3Sphere sphere = { position, 0.25f }; + DrawSphere( m_scene, b3Transform_identity, sphere, b3_colorOrchid ); + DrawTextLine( "shape hit triangle/child/material = %d / %d / %d", context.triangleIndex, context.childIndex, + context.materialId ); + } + else + { + DrawTextLine( "shape miss" ); + } + } + + { + bool overlap = false; + b3Vec3 origin = { m_rayOrigin.x - 1.0f, 2.0f, m_rayOrigin.z - 1.0f }; + b3ShapeProxy proxy = { &origin, 1, 0.3f }; + b3World_OverlapShape( m_worldId, &proxy, filter, OverlapResultFcn, &overlap ); + + b3HexColor color = overlap ? b3_colorDarkMagenta : b3_colorDarkSeaGreen; + b3Sphere sphere = { origin, 0.3f }; + DrawSphere( m_scene, b3Transform_identity, sphere, color ); + } + + if ( m_rayOrigin.x > 0.45f * m_worldWidth ) + { + m_rayOrigin.x = -0.45f * m_worldWidth; + m_rayOrigin.z += 8.0f; + } + + if ( m_rayOrigin.z > 0.45f * m_worldWidth ) + { + m_rayOrigin.z = -0.45f * m_worldWidth; + } + + float timeStep = 0.0f; + if ( m_context->pause == false || m_context->singleStep > 0 ) + { + timeStep = m_context->hertz > 0.0f ? 1.0f / m_context->hertz : 0.0f; + } + + m_rayOrigin.x += 2.0f * timeStep; + + Sample::Step(); + } + + static Sample* Create( SampleContext* context ) + { + return new Village( context ); + } + + b3Compound* m_compound; + CharacterMover m_mover; + float m_worldWidth; + b3Vec3 m_rayOrigin; +}; + +static int sampleVillage = SampleManager::Register( "Compound", "Village", Village::Create ); diff --git a/samples/sample_continuous.cpp b/samples/sample_continuous.cpp new file mode 100644 index 0000000..81a11e2 --- /dev/null +++ b/samples/sample_continuous.cpp @@ -0,0 +1,1040 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "GLFW/glfw3.h" +#include "camera.h" +#include "imgui.h" +#include "renderer.h" +#include "sample.h" +#include "scene.h" +#include "stability.h" +#include "utils.h" + +#include "box3d/box3d.h" + +#include +#include + +class ThinWall : public Sample +{ +public: + explicit ThinWall( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 30.0f, b3Vec3_zero ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + b3BoxHull groundBox = b3MakeBoxHull( 50.0f, 1.0f, 50.0f ); + b3CreateHullShape( groundId, &shapeDef, &groundBox.base ); + + bodyDef.position = { 0.0f, 10.0f, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisX, 90.0f * B3_DEG_TO_RAD ); + b3BodyId wallId = b3CreateBody( m_worldId, &bodyDef ); + b3BoxHull wallBox = b3MakeBoxHull( 10.0f, 0.1f, 10.0f ); + b3CreateHullShape( wallId, &shapeDef, &wallBox.base ); + + bodyDef.type = b3_dynamicBody; + bodyDef.rotation = b3Quat_identity; + shapeDef.baseMaterial.rollingResistance = 0.1f; + + bodyDef.position = { -5.0, 10.0f, 20.0f }; + bodyDef.linearVelocity = { 0.0f, 0.0, -180.0f }; + bodyDef.angularVelocity = { 20.0f, 0.0f, 0.0f }; + b3BodyId sphereBodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3Sphere sphere = { b3Vec3_zero, 0.1f }; + b3CreateSphereShape( sphereBodyId, &shapeDef, &sphere ); + + bodyDef.position = { 0.0, 10.0f, 20.0f }; + bodyDef.linearVelocity = { 0.0f, 0.0, -180.0f }; + bodyDef.angularVelocity = { 20.0f, -5.0f, 0.0f }; + b3BodyId capsuleBodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3Capsule capsule = { { -0.3f, 0.0f, 0.0f }, { 0.3f, 0.0f, 0.0f }, 0.1f }; + b3CreateCapsuleShape( capsuleBodyId, &shapeDef, &capsule ); + + bodyDef.position = { 5.0, 10.0f, 20.0f }; + bodyDef.linearVelocity = { 0.0f, 0.0, -180.0f }; + bodyDef.angularVelocity = { 20.0f, 5.0f, 0.0f }; + b3BodyId boxBodyId = b3CreateBody( m_worldId, &bodyDef ); + b3BoxHull box = b3MakeBoxHull( 0.4f, 0.1f, 0.1f ); + b3CreateHullShape( boxBodyId, &shapeDef, &box.base ); + } + + static Sample* Create( SampleContext* context ) + { + return new ThinWall( context ); + } +}; + +static int sampleThinWall = SampleManager::Register( "Continuous", "Thin Wall", ThinWall::Create ); + +class BounceHouse : public Sample +{ +public: + explicit BounceHouse( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 45.0f, 50.0f, b3Vec3_zero ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundBodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull groundBox = b3MakeBoxHull( 10.0f, 1.0f, 10.0f ); + b3CreateHullShape( groundBodyId, &shapeDef, &groundBox.base ); + + { + b3Transform transform = { { 10.0f, 5.0f, 0.0f }, b3Quat_identity }; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 0.1f, 5.0f, 10.0f, transform ); + b3CreateHullShape( groundBodyId, &shapeDef, &wallBox.base ); + } + + { + b3Transform transform = { { -10.0f, 5.0f, 0.0f }, b3Quat_identity }; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 0.1f, 5.0f, 10.0f, transform ); + b3CreateHullShape( groundBodyId, &shapeDef, &wallBox.base ); + } + + { + b3Transform transform = { { 0.0f, 5.0f, -10.0f }, b3Quat_identity }; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 10.0f, 5.0f, 0.1f, transform ); + b3CreateHullShape( groundBodyId, &shapeDef, &wallBox.base ); + } + + { + b3Transform transform = { { 0.0f, 5.0f, 10.0f }, b3Quat_identity }; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 10.0f, 5.0f, 0.1f, transform ); + b3CreateHullShape( groundBodyId, &shapeDef, &wallBox.base ); + } + + bodyDef.type = b3_dynamicBody; + bodyDef.gravityScale = 0.0f; + bodyDef.position = { -8.0, 4.0f, 0.0f }; + bodyDef.linearVelocity = { 120.0f, 0.0, 120.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3Sphere sphere = { b3Vec3_zero, 0.5f }; + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = 1.0f; + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + + // This mesh tries to collide with the hulls. Not a good setup. + m_dummyMesh = b3CreateBoxMesh( b3Vec3_zero, { 0.8f, 0.4f, 0.4f }, true ); + // b3CreateMeshShape( bodyId, &shapeDef, m_dummyMesh, b3Vec3_one ); + } + + ~BounceHouse() override + { + b3DestroyMesh( m_dummyMesh ); + } + + static Sample* Create( SampleContext* context ) + { + return new BounceHouse( context ); + } + + b3MeshData* m_dummyMesh; +}; + +static int sampleBounceHouse = SampleManager::Register( "Continuous", "Bounce House", BounceHouse::Create ); + +class SpinningStick : public Sample +{ +public: + explicit SpinningStick( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 25.0f, 50.0f, { 0.0f, 2.0f, 0.0f } ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + bodyDef.position = { 0.0f, -0.1f, 0.0f }; + b3BodyId groundBodyId = b3CreateBody( m_worldId, &bodyDef ); + b3BoxHull groundBox = b3MakeBoxHull( 10.0f, 0.1f, 10.0f ); + b3CreateHullShape( groundBodyId, &shapeDef, &groundBox.base ); + + bodyDef.position = { 0.0f, 0.5f, 0.0f }; + b3BodyId wallBodyId = b3CreateBody( m_worldId, &bodyDef ); + b3BoxHull wallBox = b3MakeBoxHull( 0.125f, 0.5f, 10.0f ); + b3CreateHullShape( wallBodyId, &shapeDef, &wallBox.base ); + + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 20.0f, 0.5f }; + bodyDef.linearVelocity = { 0.0f, -100.0f, 0.0f }; + b3Vec3 range = { 50.0f, 50.0f, 50.0f }; + b3Vec3 omega = RandomVec3( -range, range ); + bodyDef.angularVelocity = omega; + printf( "%f, %f, %f\n", omega.x, omega.y, omega.z ); + b3BodyId stickBodyId = b3CreateBody( m_worldId, &bodyDef ); + b3BoxHull stickBox = b3MakeBoxHull( 2.0f, 0.1f, 0.1f ); + shapeDef.baseMaterial.rollingResistance = 0.1f; + b3CreateHullShape( stickBodyId, &shapeDef, &stickBox.base ); + } + + static Sample* Create( SampleContext* context ) + { + return new SpinningStick( context ); + } +}; + +static int sampleSpinningStick = SampleManager::Register( "Continuous", "Spinning Stick", SpinningStick::Create ); + +// This tests bullets and chain reactions +class BulletVersusStack : public Sample +{ +public: + static Sample* Create( SampleContext* context ) + { + return new BulletVersusStack( context ); + } + + explicit BulletVersusStack( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( -7.5f, 20.0f, 30.0f, { 0.0f, 2.0f, 0.0f } ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundBodyId = b3CreateBody( m_worldId, &bodyDef ); + b3BoxHull groundBox = b3MakeBoxHull( 50.0f, 1.0f, 50.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( groundBodyId, &shapeDef, &groundBox.base ); + + b3Transform transform = { { 1.0f, 5.0f, 0.0f }, b3Quat_identity }; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 0.1f, 5.0f, 10.0f, transform ); + b3CreateHullShape( groundBodyId, &shapeDef, &wallBox.base ); + } + + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + for ( int row = 0; row < 10; ++row ) + { + bodyDef.position = { 0.0f, 0.5f + 1.1f * row, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + + m_bulletId = b3_nullBodyId; + } + + void Render() override + { + Sample::Render(); + DrawTransform( m_scene, { { 0.0f, 0.1f, 0.0f }, b3Quat_identity }, 1.0f ); + } + + void Launch() + { + if ( B3_IS_NON_NULL( m_bulletId ) ) + { + b3DestroyBody( m_bulletId ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.isBullet = true; + bodyDef.position = { -20.5f, 5.5f, 0.0f }; + bodyDef.linearVelocity = { 500.0f, 0.0f, 0.0f }; + m_bulletId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density *= 10.0f; + b3Sphere sphere = { b3Vec3_zero, 0.25f }; + b3CreateSphereShape( m_bulletId, &shapeDef, &sphere ); + } + + void Keyboard( int key, int action, int mods ) override + { + if ( key == 'L' ) + { + Launch(); + } + } + + void UpdateUI() override + { + float height = 80.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 150.0f, height ) ); + + ImGui::Begin( "Bullet vs Stack", nullptr, ImGuiWindowFlags_NoResize ); + + ImGui::PushItemWidth( 200.0f ); + + if ( ImGui::Button( "Launch" ) ) + { + Launch(); + } + + ImGui::PopItemWidth(); + + ImGui::End(); + } + + b3BodyId m_bulletId; +}; + +static int sampleContinuous6 = SampleManager::Register( "Continuous", "Bullet vs Stack", BulletVersusStack::Create ); + +class NeedleMesh : public Sample +{ +public: + explicit NeedleMesh( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 25.0f, 4.0f, { 0.0f, 1.2f, 0.0f } ); + } + + int slices = 8; + m_needle1 = CreateNeedle( 0.99f, 0.1f, { 0.2f, 0.0f, 0.2f }, slices ); + m_needle2 = CreateNeedle( 1.01f, 0.1f, { 0.2f, 0.0f, -0.2f }, slices ); + m_needle3 = CreateNeedle( 0.98f, 0.1f, { -0.2f, 0.0f, -0.2f }, slices ); + m_needle4 = CreateNeedle( 1.02f, 0.1f, { -0.2f, 0.0f, 0.2f }, slices ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundBodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateMeshShape( groundBodyId, &shapeDef, m_needle1, b3Vec3_one ); + b3CreateMeshShape( groundBodyId, &shapeDef, m_needle2, b3Vec3_one ); + b3CreateMeshShape( groundBodyId, &shapeDef, m_needle3, b3Vec3_one ); + b3CreateMeshShape( groundBodyId, &shapeDef, m_needle4, b3Vec3_one ); + + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 5.0f, 0.0f }; + bodyDef.linearVelocity = { 0.0f, -10.0f, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 0.3f, 0.01f, 0.3f ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + + ~NeedleMesh() override + { + b3DestroyMesh( m_needle4 ); + b3DestroyMesh( m_needle3 ); + b3DestroyMesh( m_needle2 ); + b3DestroyMesh( m_needle1 ); + } + + void Render() override + { + DrawGrid( m_scene, 10 ); + Sample::Render(); + } + + b3MeshData* CreateNeedle( float height, float radius, b3Vec3 center, int slices ) const + { + // Vertices + int vertexCount = slices + 1; + b3Vec3* vertices = (b3Vec3*)malloc( vertexCount * sizeof( b3Vec3 ) ); + B3_ASSERT( vertices ); + + float alpha = 0.0f; + float deltaAlpha = 2.0f * B3_PI / slices; + + vertices[0] = b3Vec3{ 0.0f, height, 0.0f } + center; + for ( int index = 1; index < vertexCount; ++index ) + { + b3CosSin cs = b3ComputeCosSin( alpha ); + vertices[index] = b3Vec3{ radius * cs.cosine, 0.0f, radius * cs.sine } + center; + alpha += deltaAlpha; + } + + // Indices + int triangleCount = slices; + int* indexBase = (int*)malloc( 3 * triangleCount * sizeof( int ) ); + B3_ASSERT( indexBase ); + + int index1 = vertexCount - 1; + for ( int index = 0; index < triangleCount; ++index ) + { + int index2 = index + 1; + indexBase[3 * index + 0] = 0; + indexBase[3 * index + 1] = index2; + indexBase[3 * index + 2] = index1; + index1 = index2; + } + + b3MeshDef def = {}; + def.vertexCount = vertexCount; + def.vertices = vertices; + def.triangleCount = triangleCount; + def.indices = indexBase; + def.useMedianSplit = true; + + b3MeshData* data = b3CreateMesh( &def, nullptr, 0 ); + + free( vertices ); + free( indexBase ); + + return data; + } + + static Sample* Create( SampleContext* context ) + { + return new NeedleMesh( context ); + } + + b3MeshData* m_needle1; + b3MeshData* m_needle2; + b3MeshData* m_needle3; + b3MeshData* m_needle4; + b3Hull* mConvex; +}; + +static int sampleNeedleMesh = SampleManager::Register( "Continuous", "Needle Mesh", NeedleMesh::Create ); + +struct IndexPair +{ + int index1; + int index2; +}; + +static inline IndexPair ConvertToPair( void* userData ) +{ + static_assert( sizeof( intptr_t ) >= 8 && sizeof( int ) == 4 ); + intptr_t value = (intptr_t)userData; + int index1 = (int)value; + int index2 = (int)( value >> 32 ); + return { index1, index2 }; +} + +static inline void* ConvertToUserData( IndexPair pair ) +{ + intptr_t value = (intptr_t)pair.index2 << 32 | (intptr_t)pair.index1; + return (void*)value; +} + +class MeshDrop : public Sample +{ +public: + enum ShapeType + { + e_box, + e_capsule, + e_cylinder, + e_sphere, + }; + + explicit MeshDrop( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 30.0f, 20.0f, b3Vec3_zero ); + m_context->debugDraw.forceScale = 0.1f; + } + + m_groundId = b3_nullBodyId; + m_groundMesh = nullptr; + m_groundAmplitude = 0.5f; + CreateGround(); + + m_bodies = new b3BodyId[m_bodyCount]; + memset( m_bodies, 0, m_bodyCount * sizeof( b3BodyId ) ); + + m_cylinder = b3CreateCylinder( 0.4f, 0.05f, 0.0f, 6 ); + m_shapeType = e_box; + m_runCount = 0; + m_failure = false; + m_autoGenerate = false; + + m_stepWhilePaused = true; + + Generate(); + } + + ~MeshDrop() override + { + delete[] m_bodies; + b3DestroyHull( m_cylinder ); + b3DestroyMesh( m_groundMesh ); + } + + void CreateGround() + { + if ( B3_IS_NON_NULL( m_groundId ) ) + { + b3DestroyBody( m_groundId ); + m_groundId = b3_nullBodyId; + } + + if ( m_groundMesh != nullptr ) + { + b3DestroyMesh( m_groundMesh ); + m_groundMesh = nullptr; + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + m_groundId = b3CreateBody( m_worldId, &bodyDef ); + + int gridCount = 40; + float cellWidth = 1.0f; + float rowHz = 0.1f; + float columnHz = 0.2f; + + m_groundMesh = b3CreateWaveMesh( gridCount, gridCount, cellWidth, m_groundAmplitude, rowHz, columnHz ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.filter.categoryBits = 1; + b3CreateMeshShape( m_groundId, &shapeDef, m_groundMesh, b3Vec3_one ); + + float extent = 0.5f * gridCount * cellWidth; + float halfHeight = 1.0f; + + { + b3Transform transform; + transform.p = { 0.0f, halfHeight, -extent }; + transform.q = b3Quat_identity; + b3BoxHull wallBox = b3MakeTransformedBoxHull( extent, halfHeight, 0.1f, transform ); + shapeDef.name = "wall1"; + b3CreateHullShape( m_groundId, &shapeDef, &wallBox.base ); + } + + { + b3Transform transform; + transform.p = { 0.0f, halfHeight, extent }; + transform.q = b3Quat_identity; + b3BoxHull wallBox = b3MakeTransformedBoxHull( extent, halfHeight, 0.1f, transform ); + shapeDef.name = "wall2"; + b3CreateHullShape( m_groundId, &shapeDef, &wallBox.base ); + } + + { + b3Transform transform; + transform.p = { -extent, halfHeight, 0.0f }; + transform.q = b3Quat_identity; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 0.1f, halfHeight, extent, transform ); + shapeDef.name = "wall3"; + b3CreateHullShape( m_groundId, &shapeDef, &wallBox.base ); + } + + { + b3Transform transform; + transform.p = { extent, halfHeight, 0.0f }; + transform.q = b3Quat_identity; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 0.1f, halfHeight, extent, transform ); + shapeDef.name = "wall4"; + b3CreateHullShape( m_groundId, &shapeDef, &wallBox.base ); + } + } + + void Generate() + { + for ( int i = 0; i < m_bodyCount; ++i ) + { + if ( B3_IS_NULL( m_bodies[i] ) ) + { + continue; + } + + b3DestroyBody( m_bodies[i] ); + m_bodies[i] = b3_nullBodyId; + } + + // The soft contact solver cannot always resolve this small shape at 4 sub-steps. + // Also this small shape has trouble moving advancing in the time of impact code + // because the size is very close to the tolerances used. + // b3BoxHull box = b3MakeBoxHull( 0.01f, 0.1f, 0.02f ); + b3BoxHull box = b3MakeBoxHull( 0.02f, 0.2f, 0.04f ); + b3Capsule capsule = { { 0.0f, -0.2f, 0.0f }, { 0.0f, 0.2f, 0.0f }, 0.05f }; + b3Sphere sphere = { b3Vec3_zero, 0.05f }; + + int bodyIndex = 0; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.rollingResistance = 0.1f; + + // Don't allow shapes to collide with each other. This + // makes isolating failures easier. + shapeDef.filter.categoryBits = 2; + shapeDef.filter.maskBits = 1; + + g_randomSeed = (uint32_t)b3GetTicks(); + + bool simulateAll = true; + // g_randomSeed = 3963634789; + + m_runCount += 1; + m_stepCount = 0; + printf( "run %d seed %u\n", m_runCount, g_randomSeed ); + + for ( int i = 0; i < m_gridCount; ++i ) + { + for ( int j = 0; j < m_gridCount; ++j ) + { + b3Vec3 linearVelocity = RandomVec3Uniform( -1.0f, 1.0f ); + b3Vec3 angularVelocity = RandomVec3Uniform( -5.0f, 5.0f ); + + if ( simulateAll || ( i == 5 && j == 4 ) ) + { + bodyDef.position = { 0.5f * ( i - 0.5f * m_gridCount ), 5.0f, 0.5f * ( j - 0.5f * m_gridCount ) }; + bodyDef.linearVelocity = linearVelocity; + bodyDef.angularVelocity = angularVelocity; + // bodyDef.angularDamping = 0.1f; + bodyDef.userData = ConvertToUserData( { i, j } ); + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + if ( m_shapeType == e_box ) + { + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + else if ( m_shapeType == e_capsule ) + { + b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + } + else if ( m_shapeType == e_cylinder ) + { + b3CreateHullShape( bodyId, &shapeDef, m_cylinder ); + } + else + { + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + } + + m_bodies[bodyIndex] = bodyId; + } + else + { + m_bodies[bodyIndex] = b3_nullBodyId; + } + bodyIndex += 1; + } + } + + assert( bodyIndex == m_bodyCount ); + } + + void UpdateUI() override + { + Sample::UpdateUI(); + + float fontSize = ImGui::GetFontSize(); + float height = 9.0f * fontSize; + ImGui::SetNextWindowPos( ImVec2( 0.5f * fontSize, m_camera->m_height - height - 2.0f * fontSize ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 19.0f * fontSize, height ) ); + + ImGui::Begin( "Mesh Drop", nullptr, ImGuiWindowFlags_NoResize ); + + const char* shapeTypes[] = { "box", "capsule", "cylinder", "sphere" }; + int shapeType = (int)m_shapeType; + if ( ImGui::Combo( "Type", &shapeType, shapeTypes, IM_ARRAYSIZE( shapeTypes ) ) ) + { + m_stepCount = 0; + m_shapeType = ShapeType( shapeType ); + Generate(); + } + + if ( ImGui::SliderFloat( "Amplitude", &m_groundAmplitude, 0.0f, 1.0f ) ) + { + CreateGround(); + Generate(); + } + + if ( ImGui::Button( "Generate" ) ) + { + Generate(); + } + + if ( ImGui::Button( "Auto Generate" ) ) + { + m_autoGenerate = !m_autoGenerate; + m_stepCount = 0; + } + + ImGui::End(); + } + + void Step() override + { + Sample::Step(); + + { + double screenX, screenY; + glfwGetCursorPos( m_window, &screenX, &screenY ); + PickRay pickRay = m_camera->BuildPickRay( (float)screenX, (float)screenY ); + + b3RayResult result = b3World_CastRayClosest( m_worldId, pickRay.origin, pickRay.translation, b3DefaultQueryFilter() ); + + if ( result.hit ) + { + b3BodyId bodyId = b3Shape_GetBody( result.shapeId ); + void* userData = b3Body_GetUserData( bodyId ); + IndexPair pair = ConvertToPair( userData ); + DrawTextLine( "indices: (%d, %d)\n", pair.index1, pair.index2 ); + } + } + + for ( int i = 0; i < m_bodyCount && m_failure == false; i += 1 ) + { + b3BodyId bodyId = m_bodies[i]; + if ( B3_IS_NULL( bodyId ) ) + { + continue; + } + + b3Vec3 massCenter = b3Body_GetWorldCenterOfMass( bodyId ); + if ( massCenter.y < -2.0f ) + { + IndexPair pair = ConvertToPair( b3Body_GetUserData( bodyId ) ); + printf( "index1: %d - index2: %d\n", pair.index1, pair.index2 ); + printf( "(%6.2f, %6.2f, %6.2f)\n", massCenter.x, massCenter.y, massCenter.z ); + m_context->pause = true; + m_failure = true; + m_autoGenerate = false; + } + + // This is for debugging the fallback inner sphere + // b3Sphere sphere = {massCenter, 0.015f}; + // DrawSphere( m_scene, b3Transform_identity, sphere, b3_colorLightGray ); + } + + if ( m_autoGenerate ) + { + b3BodyEvents bodyEvents = b3World_GetBodyEvents( m_worldId ); + if ( bodyEvents.moveCount == 0 ) + { + Generate(); + } + + float timeStep = 0.0f; + if ( m_context->pause == false || m_context->singleStep > 0 ) + { + timeStep = m_context->hertz > 0.0f ? 1.0f / m_context->hertz : 0.0f; + m_context->singleStep = b3MaxInt( 0, m_context->singleStep - 1 ); + } + + // Many steps + if ( timeStep > 0.0f ) + { + int n = 20; + for ( int i = 0; i < n; ++i ) + { + b3World_Step( m_worldId, timeStep, m_context->subStepCount ); + } + + m_stepCount += n; + } + + if ( m_stepCount > 1000 ) + { + m_context->pause = true; + m_failure = true; + m_autoGenerate = false; + } + } + } + + static Sample* Create( SampleContext* context ) + { + return new MeshDrop( context ); + } + + static constexpr int m_gridCount = 32; + // static constexpr int m_gridCount = 3; + static constexpr int m_bodyCount = m_gridCount * m_gridCount; + + b3MeshData* m_groundMesh; + float m_groundAmplitude; + b3BodyId m_groundId; + b3Hull* m_cylinder; + b3BodyId* m_bodies; + ShapeType m_shapeType; + int m_runCount; + bool m_failure; + bool m_autoGenerate; +}; + +static int sampleMeshDrop = SampleManager::Register( "Continuous", "Mesh Drop", MeshDrop::Create ); + +class MeshDropUnitTest : public Sample +{ +public: + explicit MeshDropUnitTest( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 30.0f, 20.0f, b3Vec3_zero ); + m_context->debugDraw.forceScale = 0.1f; + } + + m_data = CreateMeshDrop( m_worldId ); + m_failed = false; + } + + ~MeshDropUnitTest() override + { + DestroyMeshDrop( &m_data ); + } + + void Step() override + { + Sample::Step(); + + if ( m_failed == false ) + { + if ( m_stepCount >= 300 ) + { + b3BodyEvents bodyEvents = b3World_GetBodyEvents( m_worldId ); + if ( bodyEvents.moveCount > 0 ) + { + m_failed = true; + } + } + } + + if ( m_failed == true ) + { + DrawTextLine( "failed!" ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new MeshDropUnitTest( context ); + } + + MeshDropData m_data; + bool m_failed; +}; + +static int sampleMeshDropUnitTest = SampleManager::Register( "Continuous", "Mesh Drop Unit Test", MeshDropUnitTest::Create ); + +// This sample shows that clustering based on the manifold normal can lead to clipping and/or tunneling. +class HumpMesh : public Sample +{ +public: + explicit HumpMesh( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 25.0f, 4.0f, { 0.0f, 1.2f, 0.0f } ); + } + + m_hump = CreateHump( 8.0f ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundBodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateMeshShape( groundBodyId, &shapeDef, m_hump, b3Vec3_one ); + + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 5.0f, 0.0f }; + bodyDef.linearVelocity = { 0.0f, -50.0f, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.05f, 1.0f ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + + ~HumpMesh() override + { + b3DestroyMesh( m_hump ); + } + + void Render() override + { + DrawGrid( m_scene, 10 ); + Sample::Render(); + } + + b3MeshData* CreateHump( float cellWidth ) const + { + // Vertices + b3Vec3 vertices[6]; + + int index = 0; + float x = -0.5f * cellWidth; + for ( int ix = 0; ix <= 1; ++ix ) + { + float z = -cellWidth; + for ( int iz = 0; iz <= 2; ++iz ) + { + vertices[index] = { x, 0.0f, z }; + if ( iz == 1 ) + { + vertices[index].y = 0.05f * cellWidth; + } + z += cellWidth; + index += 1; + } + x += cellWidth; + } + B3_ASSERT( index == 6 ); + + int triangleCount = 4; + int indices[3 * 4]; + + index = 0; + for ( int ix = 0; ix < 1; ++ix ) + { + for ( int iz = 0; iz < 2; ++iz ) + { + int index1 = iz + 3 * ix; + int index2 = index1 + 1; + int index3 = index2 + 3; + int index4 = index3 - 1; + + B3_ASSERT( index1 < 6 ); + B3_ASSERT( index2 < 6 ); + B3_ASSERT( index3 < 6 ); + B3_ASSERT( index4 < 6 ); + + indices[index + 0] = index1; + indices[index + 1] = index2; + indices[index + 2] = index3; + + indices[index + 3] = index3; + indices[index + 4] = index4; + indices[index + 5] = index1; + + index += 6; + } + } + B3_ASSERT( index == 3 * triangleCount ); + + b3MeshDef def = {}; + def.vertexCount = 6; + def.vertices = vertices; + def.triangleCount = 4; + def.indices = indices; + def.useMedianSplit = true; + def.identifyEdges = true; + + b3MeshData* meshData = b3CreateMesh( &def, nullptr, 0 ); + return meshData; + } + + static Sample* Create( SampleContext* context ) + { + return new HumpMesh( context ); + } + + b3MeshData* m_hump; +}; + +static int sampleHumpMesh = SampleManager::Register( "Continuous", "Hump Mesh", HumpMesh::Create ); + +class IsFast : public Sample +{ +public: + explicit IsFast( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 15.0f, 50.0f, { 0.0f, 15.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_staticBody; + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundBody = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull groundBox = b3MakeBoxHull( 40.0f, 1.0f, 40.0f ); + b3CreateHullShape( groundBody, &shapeDef, &groundBox.base ); + } + + b3BoxHull box = b3MakeBoxHull( 0.5f, 10.0f, 0.5f ); + b3Capsule capsule = {{0.0f, -9.5f, 0.0f}, {0.0f, 9.5f, 0.0f}, 0.5f}; + + bool useCapsule = false; + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { -12.0f, 20.0f, 0.0f }; + bodyDef.gravityScale = 0.0f; + bodyDef.angularVelocity = { 0.0f, 0.0f, 4.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + if (useCapsule) + { + b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + } + else + { + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 20.0f, 0.0f }; + bodyDef.gravityScale = 0.0f; + bodyDef.angularVelocity = { 0.0f, 4.0f, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + if ( useCapsule ) + { + b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + } + else + { + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 12.0f, 20.0f, 0.0f }; + bodyDef.gravityScale = 0.0f; + bodyDef.angularVelocity = { 4.0f, 0.0f, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + if ( useCapsule ) + { + b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + } + else + { + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + } + } + + static Sample* Create( SampleContext* context ) + { + return new IsFast( context ); + } +}; + +static int sampleIsFast = SampleManager::Register( "Continuous", "Is Fast", IsFast::Create ); diff --git a/samples/sample_determinism.cpp b/samples/sample_determinism.cpp new file mode 100644 index 0000000..8e8a273 --- /dev/null +++ b/samples/sample_determinism.cpp @@ -0,0 +1,59 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "camera.h" +#include "determinism.h" +#include "sample.h" +#include "scene.h" + +#include + +class FallingRagdolls : public Sample +{ +public: + explicit FallingRagdolls( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 40.0f, b3Vec3_zero ); + } + + m_data = CreateFallingRagdolls( m_worldId ); + m_done = false; + } + + ~FallingRagdolls() override + { + DestroyFallingRagdolls( &m_data ); + } + + void Step( ) override + { + Sample::Step( ); + + if ( m_done == false ) + { + m_done = UpdateFallingRagdolls( m_worldId, &m_data ); + if (m_done) + { + printf( "sleep step = %d, hash = 0x%08X\n", m_data.sleepStep, m_data.hash ); + } + } + else + { + DrawTextLine( "sleep step = %d, hash = 0x%08X", m_data.sleepStep, m_data.hash ); + + } + } + + static Sample* Create( SampleContext* context ) + { + return new FallingRagdolls( context ); + } + + FallingRagdollData m_data; + bool m_done; +}; + +static int sampleFallingRagdolls = SampleManager::Register( "Determinism", "Falling Ragdolls", FallingRagdolls::Create ); diff --git a/samples/sample_events.cpp b/samples/sample_events.cpp new file mode 100644 index 0000000..2222a6e --- /dev/null +++ b/samples/sample_events.cpp @@ -0,0 +1,918 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "GLFW/glfw3.h" +#include "camera.h" +#include "imgui.h" +#include "renderer.h" +#include "sample.h" +#include "scene.h" +#include "utils.h" + +#include "box3d/box3d.h" + +#include + +class SensorVisit : public Sample +{ +public: + static Sample* Create( SampleContext* context ) + { + return new SensorVisit( context ); + } + + explicit SensorVisit( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 30.0f, 20.0f, { 0.0f, 5.0f, 0.0f } ); + } + + // Visitor + b3BoxHull dynamicBox = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 12.5f, 0.0f }; + b3BodyId dynamicBody = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.enableSensorEvents = true; + b3CreateHullShape( dynamicBody, &shapeDef, &dynamicBox.base ); + + // Sensor + b3BoxHull sensorBox = b3MakeBoxHull( 2.0f, 2.0f, 2.0f ); + + bodyDef.type = b3_kinematicBody; + bodyDef.position = { 0.0f, 2.0f, 0.0f }; + b3BodyId sensorBody = b3CreateBody( m_worldId, &bodyDef ); + shapeDef.isSensor = true; + shapeDef.enableSensorEvents = true; + m_sensorShapeId = b3CreateHullShape( sensorBody, &shapeDef, &sensorBox.base ); + } + + void Render() override + { + DrawGrid( m_scene, 10 ); + Sample::Render(); + } + + void Step() override + { + Sample::Step(); + + b3SensorEvents events = b3World_GetSensorEvents( m_worldId ); + + for ( int i = 0; i < events.beginCount; ++i ) + { + b3SensorBeginTouchEvent* event = events.beginEvents + i; + if ( B3_ID_EQUALS( event->sensorShapeId, m_sensorShapeId ) ) + { + b3BodyId bodyId = b3Shape_GetBody( event->visitorShapeId ); + b3DestroyBody( bodyId ); + break; + } + } + } + + b3ShapeId m_sensorShapeId; +}; + +static int sampleSensorVisit = SampleManager::Register( "Events", "Sensor Visit", SensorVisit::Create ); + +class HitEvent : public Sample +{ +public: + enum + { + e_maxEvents = 32, + }; + + static Sample* Create( SampleContext* context ) + { + return new HitEvent( context ); + } + + explicit HitEvent( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 30.0f, 100.0f, { 0.0f, 5.0f, 0.0f } ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + constexpr int materialCount = 6; + m_gridMesh = b3CreateGridMesh( 20, 20, 8.0f, materialCount, true ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + b3SurfaceMaterial materials[materialCount]; + for ( int i = 0; i < materialCount; ++i ) + { + materials[i] = b3DefaultSurfaceMaterial(); + materials[i].userMaterialId = i + 1; + } + + shapeDef.materials = materials; + shapeDef.materialCount = materialCount; + + b3CreateMeshShape( groundId, &shapeDef, m_gridMesh, b3Vec3_one ); + // b3BoxHull groundBox = b3MakeTransformedBoxHull( 80.0f, 1.0f, 80.0f, { { 0.0f, -1.0f, 0.0f }, b3Quat_identity } ); + // b3CreateHullShape( groundId, &shapeDef, &groundBox.base ); + } + + b3WeldJointDef jointDef = b3DefaultWeldJointDef(); + jointDef.angularHertz = 10.0f; + jointDef.angularDampingRatio = 2.0f; + + float r = 0.75f; + float y = r; + float l = 1.5f; + float offset = 0.05f; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.enableHitEvents = true; + shapeDef.baseMaterial.rollingResistance = 0.2f; + shapeDef.baseMaterial.userMaterialId = 42; + shapeDef.updateBodyMass = false; + + b3Vec3 origin = { 0.0f, 0.0f, 0.0f }; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = origin; + + b3BodyId prevBodyId = {}; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + int shapeCount = 22; + float velocityScale = 0.5f; + int shapesPerBody = 3; + + for ( int i = 0; i < shapeCount; ++i ) + { + b3Capsule capsule = { { offset, y, 0.0f }, { 0.0f, y + l, -offset }, r }; + b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + + if ( ( i + 1 ) % shapesPerBody == 0 || i == shapeCount - 1 ) + { + b3Body_ApplyMassFromShapes( bodyId ); + + b3Vec3 center = b3Body_GetWorldCenterOfMass( bodyId ); + b3Vec3 omega = { 0.0f, 0.0f, -1.0f * velocityScale }; + b3Vec3 v = b3Cross( omega, center - origin ); + b3Body_SetAngularVelocity( bodyId, omega ); + b3Body_SetLinearVelocity( bodyId, v ); + + if ( i < shapeCount - 1 ) + { + prevBodyId = bodyId; + + if ( i < shapeCount - 1 ) + { + bodyId = b3CreateBody( m_worldId, &bodyDef ); + + if ( B3_IS_NON_NULL( prevBodyId ) ) + { + jointDef.base.bodyIdA = prevBodyId; + jointDef.base.bodyIdB = bodyId; + jointDef.base.localFrameA.p = { 0.0f, y + l + r, 0.0f }; + jointDef.base.localFrameB.p = { 0.0f, y + l + r, 0.0f }; + + b3CreateWeldJoint( m_worldId, &jointDef ); + } + + velocityScale *= 0.75f; + } + } + } + + y += l + 2.0f * r; + r = 0.95f * r; + offset = -offset; + } + + m_eventCount = 0; + } + + ~HitEvent() override + { + b3DestroyMesh( m_gridMesh ); + } + + void Render() override + { + Sample::Render(); + b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 4.0f ); + + for ( int i = 0; i < m_eventCount; ++i ) + { + // void DrawLine(Scene * Scene, b3Vector3 Vertex1, b3Vector3 Vertex2, b3Color Color); + // void DrawPoint(Scene * Scene, b3Vector3 Point, b3Color Color, float Size); + + b3Vec3 p1 = m_events[i].point; + DrawPoint( m_scene, m_events[i].point, 10.0f, b3_colorYellow ); + + b3Vec3 p2 = p1 - m_events[i].approachSpeed * m_events[i].normal; + DrawLine( m_scene, p1, p2, b3_colorYellow ); + + DrawWorldString( m_camera, p1, b3_colorWhite, "%.1f, %d", m_events[i].approachSpeed, m_events[i].userMaterialIdA ); + } + + DrawTextLine( "event count = %d", m_eventCount ); + } + + void Step() override + { + Sample::Step(); + + b3ContactEvents events = b3World_GetContactEvents( m_worldId ); + for ( int i = 0; i < events.hitCount && m_eventCount < e_maxEvents; ++i ) + { + m_events[m_eventCount] = events.hitEvents[i]; + m_eventCount += 1; + } + } + + static constexpr int m_maxEvents = 32; + b3MeshData* m_gridMesh; + b3ContactHitEvent m_events[m_maxEvents] = {}; + int m_eventCount; +}; + +static int sampleHitEvent = SampleManager::Register( "Events", "Hit", HitEvent::Create ); + +class MoveEvent : public Sample +{ +public: + explicit MoveEvent( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 30.0f, 40.0f, { 0.0f, 5.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + bodyDef.type = b3_staticBody; + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundBody = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull groundBox = b3MakeBoxHull( 40.0f, 1.0f, 40.0f ); + b3CreateHullShape( groundBody, &shapeDef, &groundBox.base ); + + b3Vec3 pivot = { 0.0f, 1.0f, 0.0f }; + bodyDef.type = b3_dynamicBody; + bodyDef.position = pivot; + bodyDef.name = "big box"; + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + + m_localPivot = b3Body_GetLocalPoint( m_bodyId, pivot ); + + shapeDef.enableHitEvents = true; + b3BoxHull dynamicBox = b3MakeTransformedBoxHull( 0.5f, 10.0f, 0.5f, { { 0.0f, 10.0f, 0.0f }, b3Quat_identity } ); + b3CreateHullShape( m_bodyId, &shapeDef, &dynamicBox.base ); + + b3Vec3 center = b3Body_GetWorldCenterOfMass( m_bodyId ); + + b3Vec3 r = pivot - center; + float rr = b3LengthSquared( r ); + if ( rr > 0.0f ) + { + b3Vec3 v = { -10.0f, 0.0f, 0.0f }; + b3Vec3 omega = ( 1.0f / rr ) * b3Cross( v, r ); + b3Body_SetAngularVelocity( m_bodyId, omega ); + b3Body_SetLinearVelocity( m_bodyId, v ); + } + } + + void Render() override + { + Sample::Render(); + b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 4.0f ); + + b3Vec3 vp = b3Body_GetLocalPointVelocity( m_bodyId, m_localPivot ); + b3Vec3 v = b3Body_GetLinearVelocity( m_bodyId ); + b3Vec3 omega = b3Body_GetAngularVelocity( m_bodyId ); + + DrawTextLine( "vp = [%.2f, %.2f, %.2f], v = [%.2f, %.2f, %.2f], w = [%.2f, %.2f, %.2f]", vp.x, vp.y, vp.z, v.x, v.y, v.z, + omega.x, omega.y, omega.z ); + } + + void Step() override + { + Sample::Step(); + + b3BodyEvents moveEvents = b3World_GetBodyEvents( m_worldId ); + for ( int i = 0; i < moveEvents.moveCount; ++i ) + { + b3BodyId body = moveEvents.moveEvents[i].bodyId; + if ( moveEvents.moveEvents[i].fellAsleep ) + { + DrawTextLine( "%s fell asleep", b3Body_GetName( body ) ); + } + else + { + DrawTextLine( "%s moved", b3Body_GetName( body ) ); + } + } + } + + static Sample* Create( SampleContext* context ) + { + return new MoveEvent( context ); + } + + b3BodyId m_bodyId; + b3Vec3 m_localPivot; +}; + +static int sampleMoveEvent = SampleManager::Register( "Events", "Move", MoveEvent::Create ); + +// This sample shows how to break joints when the internal reaction force becomes large. Instead of polling, this uses events. +class JointEvent : public Sample +{ +public: + enum + { + e_count = 6 + }; + + explicit JointEvent( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 30.0f, 40.0f, { 0.0f, 5.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + b3BodyId groundId; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + groundId = b3CreateBody( m_worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeTransformedBoxHull( 20.0f, 1.0f, 20.0f, { { 0.0f, -1.0f, 0.0f }, b3Quat_identity } ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + + for ( int i = 0; i < e_count; ++i ) + { + m_jointIds[i] = b3_nullJointId; + } + + b3Vec3 position = { -12.5f, 10.0, 0.0f }; + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.enableSleep = false; + + b3BoxHull box = b3MakeBoxHull( 1.0f, 1.0f, 0.5f ); + + int index = 0; + + float forceThreshold = 3000.0f; + float torqueThreshold = 10000.0f; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + + // distance joint + { + assert( index < e_count ); + + bodyDef.position = position; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + float length = 2.0f; + b3Vec3 pivot1 = { position.x, position.y + 1.0f + length, 0.0f }; + b3Vec3 pivot2 = { position.x, position.y + 1.0f, 0.0f }; + b3DistanceJointDef jointDef = b3DefaultDistanceJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = bodyId; + jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot1 ); + jointDef.base.localFrameB.p = b3Body_GetLocalPoint( jointDef.base.bodyIdB, pivot2 ); + jointDef.length = length; + jointDef.base.forceThreshold = forceThreshold; + jointDef.base.torqueThreshold = torqueThreshold; + jointDef.base.collideConnected = true; + jointDef.base.userData = (void*)(intptr_t)index; + m_jointIds[index] = b3CreateDistanceJoint( m_worldId, &jointDef ); + } + + position.x += 5.0f; + ++index; + + // motor joint +#if 0 + { + assert( index < e_count ); + + bodyDef.position = position; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + b3MotorJointDef jointDef = b3DefaultMotorJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = bodyId; + jointDef.base.localFrameA.p = position; + jointDef.maxForce = 1000.0f; + jointDef.maxTorque = 20.0f; + jointDef.base.forceThreshold = forceThreshold; + jointDef.base.torqueThreshold = torqueThreshold; + jointDef.base.collideConnected = true; + jointDef.base.userData = (void*)(intptr_t)index; + m_jointIds[index] = b3CreateMotorJoint( m_worldId, &jointDef ); + } +#else + m_jointIds[index] = {}; +#endif + + position.x += 5.0f; + ++index; + + // prismatic joint + { + assert( index < e_count ); + + bodyDef.position = position; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + b3Vec3 pivot = { position.x - 1.0f, position.y, 0.0f }; + b3PrismaticJointDef jointDef = b3DefaultPrismaticJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = bodyId; + jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot ); + jointDef.base.localFrameB.p = b3Body_GetLocalPoint( jointDef.base.bodyIdB, pivot ); + jointDef.base.forceThreshold = forceThreshold; + jointDef.base.torqueThreshold = torqueThreshold; + jointDef.base.collideConnected = true; + jointDef.base.userData = (void*)(intptr_t)index; + m_jointIds[index] = b3CreatePrismaticJoint( m_worldId, &jointDef ); + } + + position.x += 5.0f; + ++index; + + // revolute joint + { + assert( index < e_count ); + + bodyDef.position = position; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + b3Vec3 pivot = { position.x - 1.0f, position.y, 0.0f }; + b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = bodyId; + jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot ); + jointDef.base.localFrameB.p = b3Body_GetLocalPoint( jointDef.base.bodyIdB, pivot ); + jointDef.base.forceThreshold = forceThreshold; + jointDef.base.torqueThreshold = torqueThreshold; + jointDef.base.collideConnected = true; + jointDef.base.userData = (void*)(intptr_t)index; + m_jointIds[index] = b3CreateRevoluteJoint( m_worldId, &jointDef ); + } + + position.x += 5.0f; + ++index; + + // weld joint + { + assert( index < e_count ); + + bodyDef.position = position; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + b3Vec3 pivot = { position.x - 1.0f, position.y, 0.0f }; + b3WeldJointDef jointDef = b3DefaultWeldJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = bodyId; + jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot ); + jointDef.base.localFrameB.p = b3Body_GetLocalPoint( jointDef.base.bodyIdB, pivot ); + jointDef.angularHertz = 2.0f; + jointDef.angularDampingRatio = 0.5f; + jointDef.base.forceThreshold = forceThreshold; + jointDef.base.torqueThreshold = torqueThreshold; + jointDef.base.collideConnected = true; + jointDef.base.userData = (void*)(intptr_t)index; + m_jointIds[index] = b3CreateWeldJoint( m_worldId, &jointDef ); + } + + position.x += 5.0f; + ++index; + + // wheel joint +#if 0 + { + assert( index < e_count ); + + bodyDef.position = position; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + b3Vec3 pivot = { position.x - 1.0f, position.y, 0.0f }; + b3WheelJointDef jointDef = b3DefaultWheelJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = bodyId; + jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot ); + jointDef.base.localFrameB.p = b3Body_GetLocalPoint( jointDef.base.bodyIdB, pivot ); + jointDef.hertz = 1.0f; + jointDef.dampingRatio = 0.7f; + jointDef.lowerTranslation = -1.0f; + jointDef.upperTranslation = 1.0f; + jointDef.enableLimit = true; + jointDef.enableMotor = true; + jointDef.maxMotorTorque = 10.0f; + jointDef.motorSpeed = 1.0f; + jointDef.base.forceThreshold = forceThreshold; + jointDef.base.torqueThreshold = torqueThreshold; + jointDef.base.collideConnected = true; + jointDef.base.userData = (void*)(intptr_t)index; + m_jointIds[index] = b3CreateWheelJoint( m_worldId, &jointDef ); + } +#else + m_jointIds[index] = {}; +#endif + + position.x += 5.0f; + ++index; + } + + void Step() override + { + Sample::Step(); + + // Process joint events + b3JointEvents events = b3World_GetJointEvents( m_worldId ); + for ( int i = 0; i < events.count; ++i ) + { + // Destroy the joint if it is still valid + const b3JointEvent* event = events.jointEvents + i; + + if ( b3Joint_IsValid( event->jointId ) ) + { + int index = (int)(intptr_t)event->userData; + assert( 0 <= index && index < e_count ); + b3DestroyJoint( event->jointId, true ); + m_jointIds[index] = b3_nullJointId; + } + } + } + + static Sample* Create( SampleContext* context ) + { + return new JointEvent( context ); + } + + b3JointId m_jointIds[e_count]; +}; + +static int sampleJointEvent = SampleManager::Register( "Events", "Joint", JointEvent::Create ); + +class PersistentContact : public Sample +{ +public: + explicit PersistentContact( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 30.0f, 40.0f, { 0.0f, 5.0f, 0.0f } ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + m_meshData = b3CreateGridMesh( 20, 20, 2.0f, 2, true ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateMeshShape( groundId, &shapeDef, m_meshData, b3Vec3_one ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { -18.0f, 1.0f, 0.5f }; + bodyDef.linearVelocity = { 4.0f, 0.0f, 0.0f }; + + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 20.0f; + shapeDef.enableContactEvents = true; + shapeDef.baseMaterial.rollingResistance = 0.01f; + b3Sphere sphere = { b3Vec3_zero, 0.5f }; + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + } + + m_contactId = b3_nullContactId; + } + + ~PersistentContact() override + { + b3DestroyMesh( m_meshData ); + } + + void Step() override + { + Sample::Step(); + + b3ContactEvents events = b3World_GetContactEvents( m_worldId ); + for ( int i = 0; i < events.beginCount && i < 1; ++i ) + { + m_contactId = events.beginEvents[i].contactId; + } + + for ( int i = 0; i < events.endCount; ++i ) + { + if ( B3_ID_EQUALS( m_contactId, events.endEvents[i].contactId ) ) + { + m_contactId = b3_nullContactId; + break; + } + } + + if ( B3_IS_NON_NULL( m_contactId ) && b3Contact_IsValid( m_contactId ) ) + { + b3ContactData data = b3Contact_GetData( m_contactId ); + b3Vec3 centerOfMass = b3Body_GetWorldCenterOfMass( b3Shape_GetBody( data.shapeIdA ) ); + + for ( int i = 0; i < data.manifoldCount; ++i ) + { + const b3Manifold* manifold = data.manifolds + i; + b3Vec3 normal = manifold->normal; + for ( int j = 0; j < manifold->pointCount; ++j ) + { + const b3ManifoldPoint* manifoldPoint = manifold->points + i; + b3Vec3 p1 = manifoldPoint->anchorA + centerOfMass; + b3Vec3 p2 = p1 + manifoldPoint->totalNormalImpulse * normal; + DrawLine( m_scene, p1, p2, b3_colorCrimson ); + DrawPoint( m_scene, p1, 6.0f, b3_colorCrimson ); + DrawWorldString( m_camera, p1, b3_colorGray, "%.2f", manifoldPoint->totalNormalImpulse ); + } + } + } + else + { + m_contactId = b3_nullContactId; + } + } + + static Sample* Create( SampleContext* context ) + { + return new PersistentContact( context ); + } + + b3ContactId m_contactId; + b3MeshData* m_meshData; +}; + +static int samplePersistentContact = SampleManager::Register( "Events", "Persistent Contact", PersistentContact::Create ); + +class SensorHits : public Sample +{ +public: + explicit SensorHits( SampleContext* context ) + : Sample( context ) + , m_transforms{} + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 30.0f, 40.0f, { 0.0f, 5.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + b3BodyId groundId; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "ground"; + + groundId = b3CreateBody( m_worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + b3BoxHull groundBox = b3MakeOffsetBoxHull( 10.0f, 1.0f, 10.0f, {0.0f, -1.0f, 0.0f} ); + b3CreateHullShape( groundId, &shapeDef, &groundBox.base ); + + b3BoxHull wallBox = b3MakeTransformedBoxHull( 0.1f, 5.0f, 5.0f, { { 10.0f, 5.0f, 0.0f }, b3Quat_identity } ); + b3CreateHullShape( groundId, &shapeDef, &wallBox.base ); + } + + m_gridMesh = b3CreateGridMesh( 2, 2, 5.0f, 0, true ); + + // Static sensor + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "static sensor"; + bodyDef.position = { -4.0f, 6.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.5f * B3_PI ); + + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.isSensor = true; + shapeDef.enableSensorEvents = true; + + m_staticSensorId = b3CreateMeshShape( bodyId, &shapeDef, m_gridMesh, b3Vec3_one ); + } + + // Kinematic sensor + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "kinematic sensor"; + bodyDef.type = b3_kinematicBody; + bodyDef.position = { 0.0f, 6.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.5f * B3_PI ); + bodyDef.linearVelocity = { 0.5f, 0.0f }; + + m_kinematicBodyId = b3CreateBody( m_worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.isSensor = true; + shapeDef.enableSensorEvents = true; + + m_kinematicSensorId = b3CreateMeshShape( m_kinematicBodyId, &shapeDef, m_gridMesh, b3Vec3_one ); + } + + // Dynamic sensor + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "dynamic sensor"; + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 4.0f, 1.0f }; + + m_dynamicBodyId = b3CreateBody( m_worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.isSensor = true; + shapeDef.enableSensorEvents = true; + + b3Capsule capsule = { { 0.0f, 1.0f }, { 0.0f, 9.0f }, 0.1f }; + m_dynamicSensorId = b3CreateCapsuleShape( m_dynamicBodyId, &shapeDef, &capsule ); + + b3Vec3 pivot = bodyDef.position + b3Vec3{ 0.0f, 6.0f, 0.0f }; + b3PrismaticJointDef jointDef = b3DefaultPrismaticJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = m_dynamicBodyId; + jointDef.base.localFrameA.p = b3Body_GetLocalPoint( groundId, pivot ); + jointDef.base.localFrameB.p = b3Body_GetLocalPoint( m_dynamicBodyId, pivot ); + jointDef.enableMotor = true; + jointDef.maxMotorForce = 1000.0f; + jointDef.motorSpeed = 0.5f; + + m_jointId = b3CreatePrismaticJoint( m_worldId, &jointDef ); + } + + m_beginCount = 0; + m_endCount = 0; + m_bodyId = {}; + m_shapeId = {}; + m_transformCount = 0; + m_isBullet = true; + + Launch(); + } + + ~SensorHits() override + { + b3DestroyMesh( m_gridMesh ); + } + + void Launch() + { + if ( B3_IS_NON_NULL( m_bodyId ) ) + { + b3DestroyBody( m_bodyId ); + } + + m_transformCount = 0; + m_beginCount = 0; + m_endCount = 0; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { -26.7f, 6.0f }; + float speed = RandomFloatRange( 200.0f, 300.0f ); + bodyDef.linearVelocity = { speed, 0.0f }; + bodyDef.isBullet = m_isBullet; + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.enableSensorEvents = true; + shapeDef.baseMaterial.friction = 0.8f; + shapeDef.baseMaterial.rollingResistance = 0.01f; + b3Sphere sphere = { { 0.0f, 0.0f }, 0.25f }; + m_shapeId = b3CreateSphereShape( m_bodyId, &shapeDef, &sphere ); + } + + void UpdateUI() override + { + float height = 120.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_context->camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 120.0f, height ) ); + + ImGui::Begin( "Sensor Hit", nullptr, ImGuiWindowFlags_NoResize ); + + ImGui::Checkbox( "Bullet", &m_isBullet ); + + if ( ImGui::Button( "Launch" ) || glfwGetKey( m_context->window, GLFW_KEY_B ) == GLFW_PRESS ) + { + Launch(); + } + + ImGui::End(); + } + + void CollectTransforms( b3ShapeId sensorShapeId ) + { + constexpr int capacity = 5; + b3ShapeId visitorIds[capacity]; + int count = b3Shape_GetSensorData( sensorShapeId, visitorIds, capacity ); + + for ( int i = 0; i < count && m_transformCount < m_transformCapacity; ++i ) + { + b3BodyId sensorBodyId = b3Shape_GetBody( sensorShapeId ); + m_transforms[m_transformCount] = b3Body_GetTransform( sensorBodyId ); + m_transforms[m_transformCount].p = b3Body_GetWorldCenterOfMass( sensorBodyId ); + m_transformCount += 1; + } + } + + void Step() override + { + b3Vec3 p = b3Body_GetPosition( m_kinematicBodyId ); + if ( p.x > 1.0f ) + { + b3Body_SetLinearVelocity( m_kinematicBodyId, { -0.5f, 0.0f } ); + } + else if ( p.x < -1.0f ) + { + b3Body_SetLinearVelocity( m_kinematicBodyId, { 0.5f, 0.0f } ); + } + + float x = b3PrismaticJoint_GetTranslation( m_jointId ); + if ( x > 1.0f ) + { + b3PrismaticJoint_SetMotorSpeed( m_jointId, -0.5f ); + } + else if ( x < -1.0f ) + { + b3PrismaticJoint_SetMotorSpeed( m_jointId, 0.5f ); + } + + Sample::Step(); + + for ( int i = 0; i < m_transformCount; ++i ) + { + DrawTransform( m_scene, m_transforms[i], 0.1f ); + } + + b3SensorEvents sensorEvents = b3World_GetSensorEvents( m_worldId ); + m_beginCount += sensorEvents.beginCount; + m_endCount += sensorEvents.endCount; + + for ( int i = 0; i < sensorEvents.beginCount; ++i ) + { + const b3SensorBeginTouchEvent* event = sensorEvents.beginEvents + i; + CollectTransforms( event->sensorShapeId ); + } + + DrawTextLine( "begin touch count = %d", m_beginCount ); + DrawTextLine( "end touch count = %d", m_endCount ); + } + + static Sample* Create( SampleContext* context ) + { + return new SensorHits( context ); + } + + b3MeshData* m_gridMesh; + + b3ShapeId m_staticSensorId; + b3ShapeId m_kinematicSensorId; + b3ShapeId m_dynamicSensorId; + + b3BodyId m_kinematicBodyId; + b3BodyId m_dynamicBodyId; + b3JointId m_jointId; + + b3BodyId m_bodyId; + b3ShapeId m_shapeId; + + static constexpr int m_transformCapacity = 20; + int m_transformCount; + b3Transform m_transforms[m_transformCapacity]; + + bool m_isBullet; + int m_beginCount; + int m_endCount; +}; + +static int sampleSensorHits = SampleManager::Register( "Events", "Sensor Hits", SensorHits::Create ); diff --git a/samples/sample_geometry.cpp b/samples/sample_geometry.cpp new file mode 100644 index 0000000..eda7496 --- /dev/null +++ b/samples/sample_geometry.cpp @@ -0,0 +1,651 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "camera.h" +#include "renderer.h" +#include "sample.h" +#include "scene.h" +#include "utils.h" + +#include + +class BoxHull : public Sample +{ +public: + explicit BoxHull( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Vec3_zero ); + } + + m_hull = nullptr; + + m_halfWidths = { 1.0f, 0.5f, 0.25f }; + m_postScale = { 1.0f, 1.0f, 1.0f }; + m_rotation = { 0.0f, 0.0f, 0.0f }; + m_transform = b3Transform_identity; + + UpdateRotation(); + CreateHulls( m_halfWidths, m_transform, m_postScale ); + } + + ~BoxHull() override + { + b3DestroyHull( m_hull ); + } + + void UpdateRotation() + { + b3Quat qx = b3MakeQuatFromAxisAngle( b3Vec3_axisX, B3_DEG_TO_RAD * m_rotation.x ); + b3Quat qy = b3MakeQuatFromAxisAngle( b3Vec3_axisY, B3_DEG_TO_RAD * m_rotation.y ); + b3Quat qz = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, B3_DEG_TO_RAD * m_rotation.z ); + + m_transform.q = b3MulQuat( qz, b3MulQuat( qy, qx ) ); + } + + void CreateHulls( b3Vec3 h, b3Transform transform, b3Vec3 postScale ) + { + if ( m_hull != nullptr ) + { + b3DestroyHull( m_hull ); + m_hull = nullptr; + } + + b3Vec3 scale = b3SafeScale( postScale ); + b3Vec3 points[8]; + + points[0] = scale * b3TransformPoint( transform, { h.x, h.y, h.z } ); + points[1] = scale * b3TransformPoint( transform, { h.x, h.y, -h.z } ); + points[2] = scale * b3TransformPoint( transform, { h.x, -h.y, h.z } ); + points[3] = scale * b3TransformPoint( transform, { h.x, -h.y, -h.z } ); + points[4] = scale * b3TransformPoint( transform, { -h.x, h.y, h.z } ); + points[5] = scale * b3TransformPoint( transform, { -h.x, h.y, -h.z } ); + points[6] = scale * b3TransformPoint( transform, { -h.x, -h.y, h.z } ); + points[7] = scale * b3TransformPoint( transform, { -h.x, -h.y, -h.z } ); + + m_hull = b3CreateHull( points, 8, 8 ); + m_box = b3MakeScaledBoxHull( h, transform, postScale ); + } + + void UpdateUI() override + { + float height = 180.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 350.0f, height ) ); + ImGui::Begin( "Box Hull", nullptr, 0 ); + + ImGui::PushItemWidth( 320.0f ); + + if ( ImGui::SliderFloat3( "h", &m_halfWidths.x, 0.1f, 2.0f, "%.1f" ) ) + { + CreateHulls( m_halfWidths, m_transform, m_postScale ); + } + + if ( ImGui::SliderFloat3( "c", &m_transform.p.x, -2.0f, 2.0f, "%.1f" ) ) + { + CreateHulls( m_halfWidths, m_transform, m_postScale ); + } + + if ( ImGui::SliderFloat3( "r", &m_rotation.x, -180.0f, 180.0f, "%.0f" ) ) + { + UpdateRotation(); + CreateHulls( m_halfWidths, m_transform, m_postScale ); + } + + if ( ImGui::SliderFloat3( "s", &m_postScale.x, -2.0f, 2.0f, "%.1f" ) ) + { + CreateHulls( m_halfWidths, m_transform, m_postScale ); + } + + ImGui::PopItemWidth(); + + if ( ImGui::Button( "refresh" ) ) + { + CreateHulls( m_halfWidths, m_transform, m_postScale ); + } + + ImGui::End(); + } + + void Render() override + { + DrawHull( m_scene, b3Transform_identity, m_hull, b3_colorYellow, true ); + DrawHull( m_scene, b3Transform_identity, &m_box.base, b3_colorCyan, true ); + + DrawTransform( m_scene, b3Transform_identity, 1.0f ); + + Sample::Render(); + } + + static Sample* Create( SampleContext* sampleContext ) + { + return new BoxHull( sampleContext ); + } + + b3Vec3 m_halfWidths; + b3Vec3 m_rotation; + b3Transform m_transform; + b3Vec3 m_postScale; + b3BoxHull m_box; + b3Hull* m_hull; +}; + +static int sampleBoxHull = SampleManager::Register( "Geometry", "Box Hull", BoxHull::Create ); + +class Hull : public Sample +{ +public: + explicit Hull( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Vec3_zero ); + } + + m_hull = nullptr; + + // this fails because it generates too many edges + b3Vec3 points[] = { + { -3.9866004, 75.4595108, 28.3783073 }, { -13.1079493, 73.080368, 28.296587 }, + { -18.6611958, 72.0040894, 16.9292431 }, { 4.82537603, 79.2908554, 22.2369995 }, + { -12.7315464, 79.2187576, 2.94275379 }, { -21.806488, 78.7758865, 0.985544085 }, + { -27.7619209, 73.3481522, 11.9647141 }, { -22.3994541, 72.2203826, 21.4116211 }, + { -25.3797474, 76.7417755, 27.9124985 }, { -22.7552319, 77.0559006, 29.4733639 }, + { -6.81736374, 78.3484726, 36.8649979 }, { 3.62397718, 85.5270843, 29.2077713 }, + { 7.90363788, 84.121231, 18.2612896 }, { -12.3809223, 84.5280533, -0.43230924 }, + { 5.83599472, 95.2908325, 4.4423275 }, { -22.5541401, 89.9094467, -4.87791252 }, + { -43.9060402, 78.5287094, 1.32877088 }, { -42.6015129, 76.7829742, 7.67437983 }, + { -25.735527, 78.1218796, 27.908411 }, { -23.5183544, 77.6326675, 29.1178799 }, + { 2.0977366, 100.430191, 34.3929482 }, { 1.09743047, 103.952553, 35.5656395 }, + { 8.50175952, 96.0529861, 8.73674774 }, { 2.52570295, 103.303696, 32.2314339 }, + { -20.099781, 89.4923248, -4.15468454 }, { 2.8092947, 123.516098, -1.12693477 }, + { -43.9318161, 79.1106186, 1.39006138 }, { -23.358511, 90.9599686, -4.25683546 }, + { 2.10804915, 123.603645, -1.38435471 }, { -44.1329117, 78.7192383, 1.54941654 }, + { -42.4365158, 77.725357, 8.14835929 }, { -43.204792, 77.5811691, 7.14319515 }, + { -44.17416, 78.7810363, 2.50146222 }, { -32.8975143, 99.1221771, 7.55588436 }, + { -0.624746263, 110.070351, 32.7381058 }, { 0.00431228895, 109.14341, 33.6411133 }, + { -0.58865279, 122.980537, 16.6554794 }, { 2.18539238, 124.324593, -0.620266676 }, + { -1.02177501, 123.881721, 16.8230057 }, { 1.9842999, 124.571777, -0.321986318 }, + { 1.86570692, 124.365791, -0.599836588 }, { -43.591507, 78.1373291, 6.1135149 }, + { -43.8235397, 79.2239074, 3.48619604 }, { -43.591507, 78.50811, 5.54555655 }, + { 1.21086729, 124.49453, 1.07543683 }, { -1.86223853, 124.195847, 15.6257992 }, + { -1.46520972, 124.355492, 16.9864483 }, { 1.654302, 124.612976, 0.621887207 }, + }; + + static_assert( sizeof( points ) / sizeof( points[0] ) < m_capacity, "bad" ); + + m_count = sizeof( points ) / sizeof( points[0] ); + for ( int i = 0; i < m_count; ++i ) + { + m_points[i] = 0.01f * points[i]; + } + + // This shift shouldn't be necessary but I'm doing it so the hull + // appears on the screen. + // for ( int i = 0; i < m_count; ++i ) + //{ + // m_points[i] -= m_points[0]; + // m_points[i] *= 0.01f; + //} + + m_hull = b3CreateHull( m_points, m_count, 16 ); + + (void)m_hull; + } + + ~Hull() override + { + if ( m_hull != nullptr ) + { + b3DestroyHull( m_hull ); + } + } + + void Render() override + { + if ( m_hull != nullptr ) + { + DrawHull( m_scene, b3Transform_identity, m_hull, b3_colorYellow, true ); + } + + DrawTransform( m_scene, b3Transform_identity, 1.0f ); + + Sample::Render(); + } + + static Sample* Create( SampleContext* sampleContext ) + { + return new Hull( sampleContext ); + } + + static constexpr int m_capacity = 64; + b3Hull* m_hull; + b3Vec3 m_points[m_capacity]; + int m_count; +}; + +static int sampleHull = SampleManager::Register( "Geometry", "Hull", Hull::Create ); + +class HullReduction : public Sample +{ +public: + enum Type + { + e_box, + e_sphere, + }; + + explicit HullReduction( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Vec3_zero ); + } + + m_hull = nullptr; + + m_type = e_sphere; + m_count = 16; + GeneratePoints(); + GenerateHull(); + } + + void GeneratePoints() + { + g_randomSeed = 42; + + if ( m_type == e_box ) + { + b3Vec3 lower = { -2.0f, -2.0f, -2.0f }; + b3Vec3 upper = { 2.0f, 2.0f, 2.0f }; + b3AABB box = { { -1.0f, -1.0f, -1.0f }, { 1.0f, 1.0f, 1.0f } }; + float a = 0.001f; + b3AABB noise = { { -a, -a, -a }, { a, a, a } }; + + for ( int i = 0; i < m_capacity; ++i ) + { + b3Vec3 p = RandomVec3( lower, upper ); + p = b3Clamp( p, box.lowerBound, box.upperBound ); + b3Vec3 f = RandomVec3( noise.lowerBound, noise.upperBound ); + m_points[i] = b3Add(p, f); + } + } + else + { + for ( int i = 0; i < m_capacity; ++i ) + { + m_points[i] = RandomUnitVector(); + } + } + } + + void GenerateHull() + { + if ( m_hull != nullptr ) + { + b3DestroyHull( m_hull ); + } + + m_hull = b3CreateHull( m_points, m_capacity, m_count ); + } + + ~HullReduction() override + { + if ( m_hull != nullptr ) + { + b3DestroyHull( m_hull ); + } + } + + void UpdateUI() override + { + float fontSize = ImGui::GetFontSize(); + float height = 12.0f * fontSize; + ImGui::SetNextWindowPos( { 1.0f * fontSize, m_camera->m_height - height - 3.0f * fontSize }, ImGuiCond_Once ); + ImGui::SetNextWindowSize( { 16.0f * fontSize, height } ); + + ImGui::Begin( "Hull Reduction", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::RadioButton( "Box", m_type == e_box ) ) + { + m_type = e_box; + GeneratePoints(); + GenerateHull(); + } + + if ( ImGui::RadioButton( "Sphere", m_type == e_sphere ) ) + { + m_type = e_sphere; + GeneratePoints(); + GenerateHull(); + } + + if ( ImGui::SliderInt( "count", &m_count, 4, m_capacity ) ) + { + GenerateHull(); + } + + ImGui::End(); + } + + void Render() override + { + if ( m_hull != nullptr ) + { + DrawHull( m_scene, b3Transform_identity, m_hull, b3_colorYellow, true ); + + DrawTextLine( "v/f/e = %d/%d/%d", m_hull->vertexCount, m_hull->faceCount, m_hull->edgeCount / 2 ); + } + + DrawTransform( m_scene, b3Transform_identity, 1.0f ); + + Sample::Render(); + } + + static Sample* Create( SampleContext* sampleContext ) + { + return new HullReduction( sampleContext ); + } + + static constexpr int m_capacity = 128; + Type m_type; + b3Hull* m_hull; + b3Vec3 m_points[m_capacity]; + int m_count; +}; + +static int sampleHullReduction = SampleManager::Register( "Geometry", "Hull Reduction", HullReduction::Create ); + +class HullScale : public Sample +{ +public: + explicit HullScale( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Vec3_zero ); + } + + m_original = b3CreateCylinder( 1.0f, 0.5f, 0.0f, 9 ); + m_box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + // m_original = &m_box.base; + m_scale = { 1.0f, 1.0f, 1.0f }; + m_angles = b3Vec3_zero; + m_offset = b3Vec3_zero; + m_hull = b3CloneAndTransformHull( m_original, b3Transform_identity, m_scale ); + } + + ~HullScale() override + { + if ( m_original != &m_box.base ) + { + b3DestroyHull( m_original ); + } + + b3DestroyHull( m_hull ); + } + + void UpdateHull() + { + b3DestroyHull( m_hull ); + + b3Quat qx = b3MakeQuatFromAxisAngle( b3Vec3_axisX, m_angles.x * B3_PI / 180.0f ); + b3Quat qy = b3MakeQuatFromAxisAngle( b3Vec3_axisY, m_angles.y * B3_PI / 180.0f ); + b3Quat qz = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, m_angles.z * B3_PI / 180.0f ); + b3Quat q = b3MulQuat( qz, b3MulQuat( qy, qx ) ); + + m_hull = b3CloneAndTransformHull( m_original, { m_offset, q }, m_scale ); + } + + void Render() override + { + b3Transform transform1 = { { -2.0f, 0.0f, 0.0f }, b3Quat_identity }; + b3Transform transform2 = { { 2.0f, 0.0f, 0.0f }, b3Quat_identity }; + + DrawHull( m_scene, transform1, m_original, b3_colorGreen, true ); + DrawHull( m_scene, transform2, m_hull, b3_colorYellow, true ); + DrawTransform( m_scene, b3Transform_identity, 1.0f ); + + DrawTextLine( "hull 1: area = %g, volume = %g, radius = %g", m_original->surfaceArea, m_original->volume, + m_original->innerRadius ); + DrawTextLine( "hull 2: area = %g, volume = %g, radius = %g", m_hull->surfaceArea, m_hull->volume, m_hull->innerRadius ); + + Sample::Render(); + } + + void UpdateUI() override + { + float fontSize = ImGui::GetFontSize(); + float height = 18.0f * fontSize; + ImGui::SetNextWindowPos( { 1.0f * fontSize, m_camera->m_height - height - 3.0f * fontSize }, ImGuiCond_Once ); + ImGui::SetNextWindowSize( { 18.0f * fontSize, height } ); + + ImGui::Begin( "Hull Scale", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::SliderFloat( "sx", &m_scale.x, -2.0f, 2.0f, "%.1f" ) ) + { + UpdateHull(); + } + + if ( ImGui::SliderFloat( "sy", &m_scale.y, -2.0f, 2.0f, "%.1f" ) ) + { + UpdateHull(); + } + + if ( ImGui::SliderFloat( "sz", &m_scale.z, -2.0f, 2.0f, "%.1f" ) ) + { + UpdateHull(); + } + + if ( ImGui::SliderFloat( "rx", &m_angles.x, -180.0f, 180.0f, "%.0f" ) ) + { + UpdateHull(); + } + + if ( ImGui::SliderFloat( "ry", &m_angles.y, -180.0f, 180.0f, "%.0f" ) ) + { + UpdateHull(); + } + + if ( ImGui::SliderFloat( "rz", &m_angles.z, -180.0f, 180.0f, "%.0f" ) ) + { + UpdateHull(); + } + + if ( ImGui::SliderFloat( "px", &m_offset.x, -1.0f, 1.0f, "%.1f" ) ) + { + UpdateHull(); + } + + if ( ImGui::SliderFloat( "py", &m_offset.y, -1.0f, 1.0f, "%.1f" ) ) + { + UpdateHull(); + } + + if ( ImGui::SliderFloat( "pz", &m_offset.z, -1.0f, 1.0f, "%.1f" ) ) + { + UpdateHull(); + } + + ImGui::End(); + } + + static Sample* Create( SampleContext* sampleContext ) + { + return new HullScale( sampleContext ); + } + + b3BoxHull m_box; + b3Hull* m_original; + b3Hull* m_hull; + b3Vec3 m_angles; + b3Vec3 m_scale; + b3Vec3 m_offset; +}; + +static int sampleHullScale = SampleManager::Register( "Geometry", "Hull Scale", HullScale::Create ); + +class CapsuleMass : public Sample +{ +public: + explicit CapsuleMass( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Vec3_zero ); + } + + m_radius = 1.0f; + m_length = 2.0f; + m_sides = 6; + m_capsule = { { -0.5f * m_length, 0.0f, 0.0f }, { 0.5f * m_length, 0.0f, 0.0f }, m_radius }; + m_box = b3MakeBoxHull( m_radius + 0.5f * m_length, m_radius, m_radius ); + + m_hull = nullptr; + CreateCapsuleHull( m_sides ); + } + + ~CapsuleMass() override + { + if ( m_hull != nullptr ) + { + b3DestroyHull( m_hull ); + } + } + + void CreateCapsuleHull( int sides ) + { + if ( m_hull != nullptr ) + { + b3DestroyHull( m_hull ); + m_hull = nullptr; + } + + constexpr int N = m_maxSides; + constexpr int M = 2 * N * N; + b3Vec3 points[M]; + + if ( sides > N ) + { + return; + } + + int count = 2 * sides * sides; + float d = B3_PI / ( sides - 1.0f ); + float angle1 = -0.5f * B3_PI; + int index = 0; + for ( int i = 0; i < sides; ++i ) + { + float s1 = sinf( angle1 ); + float c1 = cosf( angle1 ); + float angle2 = -0.5f * B3_PI; + for ( int j = 0; j < sides; ++j ) + { + points[index].x = 1.0f + m_radius * c1; + points[index].y = m_radius * s1 * cosf( angle2 ); + points[index].z = m_radius * s1 * sinf( angle2 ); + angle2 += d; + index += 1; + } + + angle1 += d; + } + + angle1 = 0.5f * B3_PI; + for ( int i = 0; i < sides; ++i ) + { + float s1 = sinf( angle1 ); + float c1 = cosf( angle1 ); + float angle2 = -0.5f * B3_PI; + for ( int j = 0; j < sides; ++j ) + { + points[index].x = -1.0f + m_radius * c1; + points[index].y = m_radius * s1 * cosf( angle2 ); + points[index].z = m_radius * s1 * sinf( angle2 ); + angle2 += d; + index += 1; + } + + angle1 += d; + } + + assert( index == count ); + + m_hull = b3CreateHull( points, count, count ); + } + + void Render() override + { + DrawCapsule( m_scene, b3Transform_identity, m_capsule, b3_colorAqua ); + DrawHull( m_scene, b3Transform_identity, &m_box.base, b3_colorBlueViolet, true ); + + if ( m_hull != nullptr ) + { + DrawHull( m_scene, b3Transform_identity, m_hull, b3_colorYellow, true ); + } + + DrawTransform( m_scene, b3Transform_identity, 1.0f ); + + if ( m_hull != nullptr ) + { + b3MassData lowerMassData = b3ComputeHullMass( m_hull, 1.0f ); + b3MassData massData = b3ComputeCapsuleMass( &m_capsule, 1.0f ); + b3MassData upperMassData = b3ComputeHullMass( &m_box.base, 1.0f ); + + DrawTextLine( "mass hull: %g", lowerMassData.mass ); + DrawTextLine( "mass capsule: %g", massData.mass ); + DrawTextLine( "mass box: %g", upperMassData.mass ); + m_textLine += m_textIncrement; + DrawTextLine( "Ixx hull: %g", lowerMassData.inertia.cx.x ); + DrawTextLine( "Ixx capsule: %g", massData.inertia.cx.x ); + DrawTextLine( "Ixx box: %g", upperMassData.inertia.cx.x ); + m_textLine += m_textIncrement; + DrawTextLine( "Iyy hull: %g", lowerMassData.inertia.cy.y ); + DrawTextLine( "Iyy capsule: %g", massData.inertia.cy.y ); + DrawTextLine( "Iyy box: %g", upperMassData.inertia.cy.y ); + m_textLine += m_textIncrement; + DrawTextLine( "Izz hull: %g", lowerMassData.inertia.cz.z ); + DrawTextLine( "Izz capsule: %g", massData.inertia.cz.z ); + DrawTextLine( "Izz box: %g", upperMassData.inertia.cz.z ); + } + + Sample::Render(); + } + + void UpdateUI() override + { + float height = 180.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 350.0f, height ) ); + ImGui::Begin( "Mass Data", nullptr, 0 ); + + if ( ImGui::SliderInt( "sides", &m_sides, 3, m_maxSides ) ) + { + CreateCapsuleHull( m_sides ); + } + + ImGui::End(); + } + + static Sample* Create( SampleContext* sampleContext ) + { + return new CapsuleMass( sampleContext ); + } + + // Limited by edge count + static constexpr int m_maxSides = 6; + b3Capsule m_capsule; + b3BoxHull m_box; + b3Hull* m_hull; + float m_radius; + float m_length; + int m_sides; +}; + +static int sampleCapsuleMass = SampleManager::Register( "Geometry", "Capsule Mass", CapsuleMass::Create ); diff --git a/samples/sample_issues.cpp b/samples/sample_issues.cpp new file mode 100644 index 0000000..3324205 --- /dev/null +++ b/samples/sample_issues.cpp @@ -0,0 +1,401 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "camera.h" +#include "imgui.h" +#include "renderer.h" +#include "sample.h" +#include "scene.h" + +#include "box3d/box3d.h" + +#include + +class DumpLoader : public Sample +{ +public: + explicit DumpLoader( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 15.0f, { 0.0f, 2.0f, 0.0f } ); + //m_camera->SetView( 45.0f, 30.0f, 300.0f, { 3910.62109f, 9862.50293f, 875.395081f } ); + } + + b3SetLengthUnitsPerMeter( 1.0f ); + + const char* dumpPrefix = "data/dumps/single_box/"; + + #include "dumps/single_box/box3d_dump.inl" + } + + ~DumpLoader() override + { + for (b3MeshData* md : m_meshes) + { + b3DestroyMesh( md ); + } + + b3SetLengthUnitsPerMeter( 1.0f ); + } + + static Sample* Create( SampleContext* context ) + { + return new DumpLoader( context ); + } + + std::vector m_meshes; +}; + +static int sampleDumpLoader = SampleManager::Register( "Issues", "Dump Loader", DumpLoader::Create ); + +class Crash : public Sample +{ +public: + explicit Crash( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 15.0f, { 0.0f, 2.0f, 0.0f } ); + } + + b3BodyId groundId; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + m_gridMesh = b3CreateGridMesh( 20, 20, 2, 0, true ); + b3CreateMeshShape( groundId, &shapeDef, m_gridMesh, b3Vec3_one ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 2.0f, 4.0f, 0.0f }; + m_bodyId1 = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3CreateHullShape( m_bodyId1, &shapeDef, &box.base ); + + bodyDef.position = { -2.0f, 4.0f, 0.0f }; + m_bodyId2 = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( m_bodyId2, &shapeDef, &box.base ); + } + + ~Crash() override + { + b3DestroyMesh( m_gridMesh ); + } + + void UpdateUI() override + { + float height = 320.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 300.0f, height ) ); + + ImGui::Begin( "Crash", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::Button( "Add Joint" ) ) + { + b3WeldJointDef jointDef = b3DefaultWeldJointDef(); + jointDef.base.bodyIdA = m_bodyId1; + jointDef.base.bodyIdB = m_bodyId2; + b3CreateWeldJoint( m_worldId, &jointDef ); + } + + ImGui::End(); + } + + static Sample* Create( SampleContext* context ) + { + return new Crash( context ); + } + + b3BodyId m_bodyId1; + b3BodyId m_bodyId2; + b3MeshData* m_gridMesh; +}; + +static int sampleCrash = SampleManager::Register( "Issues", "Crash", Crash::Create ); + +class MultiplePrismatic : public Sample +{ +public: + explicit MultiplePrismatic( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 0.0f, 65.0f, { 0.0f, 0.0f, 0.0f } ); + } + + b3BodyId groundId; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + groundId = b3CreateBody( m_worldId, &bodyDef ); + } + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3PrismaticJointDef jointDef = b3DefaultPrismaticJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.localFrameA.p = { 0.0f, 0.0f, 0.0f }; + jointDef.base.localFrameB.p = { 0.0f, -0.6f, 0.0f }; + jointDef.base.drawScale = 2.0f; + jointDef.base.constraintHertz = 240.0f; + jointDef.lowerTranslation = -6.0f; + jointDef.upperTranslation = 6.0f; + jointDef.enableLimit = true; + + for ( int i = 0; i < 6; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, 0.6f + 1.2f * i, 0.0f }; + bodyDef.type = b3_dynamicBody; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + jointDef.base.bodyIdB = bodyId; + b3CreatePrismaticJoint( m_worldId, &jointDef ); + + jointDef.base.bodyIdA = bodyId; + jointDef.base.localFrameA.p = { 0.0f, 0.6f, 0.0f }; + } + + // huge mouse force + m_mouseForceScale = 1000000.0f; + } + + static Sample* Create( SampleContext* context ) + { + return new MultiplePrismatic( context ); + } +}; + +static int sampleMultiplePrismatic = SampleManager::Register( "Issues", "Multiple Prismatic", MultiplePrismatic::Create ); + +class HullCrash : public Sample +{ +public: + explicit HullCrash( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Vec3_zero ); + } + + + m_hull = nullptr; + +#if 0 + // bad hull SM_Waterfall_MED_Wide_01 + b3Vec3 points[] = { + { 0.100183107, -498.925385, -1275.39966 }, { 0.100183107, -498.925415, 0.125000000 }, + { 0.100183107, 486.343750, 0.125000000 }, { 0.100183107, 486.343719, -1275.39966 }, + { -395.117462, 486.343781, -1462.43750 }, { -395.117462, 486.343750, -96.7426758 }, + { -395.117462, -498.925415, -96.7424469 }, { -395.117462, -498.925446, -1462.52612 }, + { -186.979691, 486.294891, -1462.47949 }, { -298.250000, 486.294891, 0.125000000 }, + { -395.121216, 486.294891, -1462.52612 }, { -186.984360, -498.913361, -1462.48413 }, + { -298.250000, -498.913361, 0.125000000 }, + }; + +#elif 1 + b3Vec3 points[] = { + { 100.000000, -142.292389, 130.826111 }, { 99.5354385, -71.3011093, 130.826111 }, + { 99.5930862, -80.1112213, -100.000000 }, { 100.000000, -142.292389, -100.000000 }, + { 99.5930862, -80.1112213, 130.826111 }, + }; +#else + b3Vec3 points[] = { + { -11.3861933, -24.2451687, -12.0037909 }, { -11.3889809, -24.2466526, -11.9013014 }, + { -11.3804407, -24.3151531, -12.0046492 }, { -11.3832273, -24.3166409, -11.9021587 }, + { -14.4396200, -24.3636723, -12.1324549 }, { -14.4432650, -24.3655701, -12.0299988 }, + { -14.4356947, -24.4337788, -12.1336164 }, { -14.4393377, -24.4356804, -12.0311594 }, + }; +#endif + + static_assert( sizeof( points ) / sizeof( points[0] ) < m_capacity, "bad" ); + + m_count = sizeof( points ) / sizeof( points[0] ); + for ( int i = 0; i < m_count; ++i ) + { + m_points[i] = 0.01f * points[i]; + } + + // This shift shouldn't be necessary but I'm doing it so the hull + // appears on the screen. + // for ( int i = 0; i < m_count; ++i ) + //{ + // m_points[i] -= m_points[0]; + // m_points[i] *= 0.01f; + //} + + m_hull = b3CreateHull( m_points, m_count, m_count ); + + (void)m_hull; + } + + ~HullCrash() override + { + if ( m_hull != nullptr ) + { + b3DestroyHull( m_hull ); + } + } + + void Render() override + { + if ( m_hull != nullptr ) + { + DrawHull( m_scene, b3Transform_identity, m_hull, b3_colorYellow, true ); + } + else + { + for (int i = 0; i < m_count; ++i) + { + DrawPoint( m_scene, m_points[i], 5.0f, b3_colorWhite ); + } + } + + DrawTransform( m_scene, b3Transform_identity, 1.0f ); + + Sample::Render(); + } + + static Sample* Create( SampleContext* sampleContext ) + { + return new HullCrash( sampleContext ); + } + + static constexpr int m_capacity = 64; + b3Hull* m_hull; + b3Vec3 m_points[m_capacity]; + int m_count; +}; + +static int sampleHullCrash = SampleManager::Register( "Issues", "Hull Crash", HullCrash::Create ); + +class ConvexJitter : public Sample +{ +public: + explicit ConvexJitter( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 15.0f, 10.0f, { 0.0f, 2.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + +#if 1 + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeBoxHull( 10.0f, 1.0f, 10.0f ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } +#endif + + float s = 0.01f; + + { + b3Vec3 b = { -459.292877f, 217.398331f, 1.00115335f }; + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { s * b.x, s * b.z + 2.0f, s * b.y }; + bodyDef.rotation = { { 0.0f, -0.707106769f, 0.0f }, 0.707106769f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + constexpr int count = 16; + b3Vec3 points[count]; + points[0] = { -44.8770714, -91.6598053, -1.92012548 }; + points[1] = { -92.5001831, 51.0151291, 15.8006573 }; + points[2] = { -91.0282211, -9.44371605, 15.6148796 }; + points[3] = { 90.2375641, 77.3870087, 15.9356089 }; + points[4] = { -85.5353241, 91.3750992, -1.36629653 }; + points[5] = { 88.9092178, -87.2975464, -1.86754704 }; + points[6] = { 83.7932816, -89.8572235, 15.4168339 }; + points[7] = { 87.0243988, 88.9776535, -1.32423306 }; + points[8] = { -91.6564941, -85.4949493, 15.3782759 }; + points[9] = { -90.2922516, -87.2074127, -1.92012548 }; + points[10] = { -87.2944870, 89.9510498, 15.9215889 }; + points[11] = { 79.2338104, 89.9690781, 15.9724140 }; + points[12] = { -91.6744461, 81.0823212, -1.39959598 }; + points[13] = { 90.3452759, -76.4459610, 15.4588966 }; + points[14] = { -87.4021912, -89.2263107, 15.3677588 }; + points[15] = { 76.3258057, 92.0059967, 1.82873762 }; + + for ( int i = 0; i < count; ++i ) + { + b3Vec3 p = points[i]; + points[i] = { s * p.x, s * p.z, s * p.y }; + } + + b3Hull* hull = b3CreateHull( points, count, count ); + + b3CreateHullShape( bodyId, &shapeDef, hull ); + b3DestroyHull( hull ); + } + + { + b3Vec3 b = { -402.321838f, 157.310364f, 16.8169250f }; + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { s * b.x, s * b.z + 2.0f, s * b.y }; + bodyDef.rotation = { { 0.0f, -0.00152086187f, 0.0f }, 0.999998868f }; + bodyDef.type = b3_dynamicBody; + + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.rollingResistance = 0.1f; + + constexpr int count = 18; + b3Vec3 points[count]; + points[0] = { 29.5000000, 17.1488495, 0.175081104 }; + points[1] = { 29.5000000, -17.2990532, 0.125000000 }; + points[2] = { 29.4840164, -17.3057766, 24.0200863 }; + points[3] = { 29.4840164, 17.1648350, 24.1781254 }; + points[4] = { -29.1345520, 17.5529804, 0.125000000 }; + points[5] = { -29.1345520, 17.5529804, 23.7899799 }; + points[6] = { -29.1441040, 16.9679585, 24.3750000 }; + points[7] = { -29.1345520, -17.2990532, 24.3750000 }; + points[8] = { -29.1345520, -17.2990532, 0.175081253 }; + points[9] = { 29.0720215, 17.5529785, 0.125000000 }; + points[10] = { 29.0859070, 17.5629406, 23.8120594 }; + points[11] = { 29.1401348, -17.2990532, 24.3750000 }; + points[12] = { 29.1123581, 16.9722290, 24.4027710 }; + points[13] = { 29.3944912, 17.2543602, 24.1206398 }; + points[14] = { -29.1345520, -17.2990532, 24.0759430 }; + points[15] = { -29.1345520, -16.9722252, 24.4027710 }; + points[16] = { 29.1123619, -16.9722271, 24.4027729 }; + points[17] = { 29.5000000, 17.3429642, 24.0000000 }; + + for ( int i = 0; i < count; ++i ) + { + b3Vec3 p = points[i]; + points[i] = { s * p.x, s * p.z, s * p.y }; + } + + b3Hull* hull = b3CreateHull( points, count, count ); + + b3CreateHullShape( bodyId, &shapeDef, hull ); + b3DestroyHull( hull ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new ConvexJitter( context ); + } +}; + +static int sampleConvexJitter = SampleManager::Register( "Issues", "Convex Jitter", ConvexJitter::Create ); diff --git a/samples/sample_joint.cpp b/samples/sample_joint.cpp new file mode 100644 index 0000000..e0b87c6 --- /dev/null +++ b/samples/sample_joint.cpp @@ -0,0 +1,2784 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "GLFW/glfw3.h" +#include "camera.h" +#include "imgui.h" +#include "renderer.h" +#include "sample.h" +#include "scene.h" + +#include "box3d/box3d.h" + +// Test the distance joint and all options +class DistanceJoint : public Sample +{ +public: + enum + { + e_maxCount = 10 + }; + + explicit DistanceJoint( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 0.0f, 40.0f, { 0.0f, 10.0f, 0.0f } ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + m_groundId = b3CreateBody( m_worldId, &bodyDef ); + } + + m_count = 0; + m_hertz = 5.0f; + m_dampingRatio = 0.5f; + m_length = 1.0f; + m_minLength = m_length; + m_maxLength = m_length; + m_tensionForce = 2000.0f; + m_compressionForce = 100.0f; + m_enableSpring = false; + m_enableLimit = false; + + for ( int i = 0; i < e_maxCount; ++i ) + { + m_bodyIds[i] = b3_nullBodyId; + m_jointIds[i] = b3_nullJointId; + } + + CreateScene( 1 ); + } + + void CreateScene( int newCount ) + { + for ( int i = 0; i < m_count; ++i ) + { + b3DestroyJoint( m_jointIds[i], false ); + m_jointIds[i] = b3_nullJointId; + } + + for ( int i = 0; i < m_count; ++i ) + { + b3DestroyBody( m_bodyIds[i] ); + m_bodyIds[i] = b3_nullBodyId; + } + + m_count = newCount; + + float radius = 0.25f; + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, radius }; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 20.0f; + + float yOffset = 20.0f; + + b3DistanceJointDef jointDef = b3DefaultDistanceJointDef(); + jointDef.hertz = m_hertz; + jointDef.dampingRatio = m_dampingRatio; + jointDef.length = m_length; + jointDef.lowerSpringForce = -m_tensionForce; + jointDef.upperSpringForce = m_compressionForce; + jointDef.minLength = m_minLength; + jointDef.maxLength = m_maxLength; + jointDef.enableSpring = m_enableSpring; + jointDef.enableLimit = m_enableLimit; + + b3BodyId prevBodyId = m_groundId; + for ( int i = 0; i < m_count; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.angularDamping = 1.0f; + bodyDef.position = { m_length * ( i + 1.0f ), yOffset }; + m_bodyIds[i] = b3CreateBody( m_worldId, &bodyDef ); + b3CreateSphereShape( m_bodyIds[i], &shapeDef, &sphere ); + + b3Vec3 pivotA = { m_length * i, yOffset, 0.0f }; + b3Vec3 pivotB = { m_length * ( i + 1.0f ), yOffset, 0.0f }; + jointDef.base.bodyIdA = prevBodyId; + jointDef.base.bodyIdB = m_bodyIds[i]; + jointDef.base.localFrameA.p = b3Body_GetLocalPoint( prevBodyId, pivotA ); + jointDef.base.localFrameB.p = b3Body_GetLocalPoint( m_bodyIds[i], pivotB ); + m_jointIds[i] = b3CreateDistanceJoint( m_worldId, &jointDef ); + + prevBodyId = m_bodyIds[i]; + } + } + + void UpdateUI() override + { + float fontSize = ImGui::GetFontSize(); + float height = 20.0f * fontSize; + ImGui::SetNextWindowPos( { 0.5f * fontSize, m_camera->m_height - height - 2.0f * fontSize }, ImGuiCond_Once ); + ImGui::SetNextWindowSize( { 18.0f * fontSize, height } ); + + ImGui::Begin( "Distance Joint", nullptr, ImGuiWindowFlags_NoResize ); + ImGui::PushItemWidth( 10.0f * fontSize ); + + if ( ImGui::SliderFloat( "Length", &m_length, 0.1f, 4.0f, "%3.1f" ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b3DistanceJoint_SetLength( m_jointIds[i], m_length ); + b3Joint_WakeBodies( m_jointIds[i] ); + } + } + + if ( ImGui::Checkbox( "Spring", &m_enableSpring ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b3DistanceJoint_EnableSpring( m_jointIds[i], m_enableSpring ); + b3Joint_WakeBodies( m_jointIds[i] ); + } + } + + if ( m_enableSpring ) + { + if ( ImGui::SliderFloat( "Tension", &m_tensionForce, 0.0f, 4000.0f ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b3DistanceJoint_SetSpringForceRange( m_jointIds[i], -m_tensionForce, m_compressionForce ); + b3Joint_WakeBodies( m_jointIds[i] ); + } + } + + if ( ImGui::SliderFloat( "Compression", &m_compressionForce, 0.0f, 200.0f ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b3DistanceJoint_SetSpringForceRange( m_jointIds[i], -m_tensionForce, m_compressionForce ); + b3Joint_WakeBodies( m_jointIds[i] ); + } + } + + if ( ImGui::SliderFloat( "Hertz", &m_hertz, 0.0f, 15.0f, "%3.1f" ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b3DistanceJoint_SetSpringHertz( m_jointIds[i], m_hertz ); + b3Joint_WakeBodies( m_jointIds[i] ); + } + } + + if ( ImGui::SliderFloat( "Damping", &m_dampingRatio, 0.0f, 4.0f, "%3.1f" ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b3DistanceJoint_SetSpringDampingRatio( m_jointIds[i], m_dampingRatio ); + b3Joint_WakeBodies( m_jointIds[i] ); + } + } + } + + if ( ImGui::Checkbox( "Limit", &m_enableLimit ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b3DistanceJoint_EnableLimit( m_jointIds[i], m_enableLimit ); + b3Joint_WakeBodies( m_jointIds[i] ); + } + } + + if ( m_enableLimit ) + { + if ( ImGui::SliderFloat( "Min Length", &m_minLength, 0.1f, 4.0f, "%3.1f" ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b3DistanceJoint_SetLengthRange( m_jointIds[i], m_minLength, m_maxLength ); + b3Joint_WakeBodies( m_jointIds[i] ); + } + } + + if ( ImGui::SliderFloat( "Max Length", &m_maxLength, 0.1f, 4.0f, "%3.1f" ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b3DistanceJoint_SetLengthRange( m_jointIds[i], m_minLength, m_maxLength ); + b3Joint_WakeBodies( m_jointIds[i] ); + } + } + } + + int count = m_count; + if ( ImGui::SliderInt( "Count", &count, 1, e_maxCount ) ) + { + CreateScene( count ); + } + + ImGui::PopItemWidth(); + ImGui::End(); + } + + void Render() override + { + Sample::Render(); + + DrawGrid( m_scene, 20 ); + } + + static Sample* Create( SampleContext* context ) + { + return new DistanceJoint( context ); + } + + b3BodyId m_groundId; + b3BodyId m_bodyIds[e_maxCount]; + b3JointId m_jointIds[e_maxCount]; + int m_count; + float m_hertz; + float m_dampingRatio; + float m_length; + float m_tensionForce; + float m_compressionForce; + float m_minLength; + float m_maxLength; + bool m_enableSpring; + bool m_enableLimit; +}; + +static int sampleDistanceJoint = SampleManager::Register( "Joints", "Distance Joint", DistanceJoint::Create ); + +class FilterJoint : public Sample +{ +public: + explicit FilterJoint( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 15.0f, { 0.0f, 2.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + b3BodyId groundId; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull groundBox = b3MakeBoxHull( 20.0f, 1.0f, 20.0f ); + b3CreateHullShape( groundId, &shapeDef, &groundBox.base ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 2.0f, 4.0f, 0.0f }; + b3BodyId bodyId1 = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3CreateHullShape( bodyId1, &shapeDef, &box.base ); + + bodyDef.position = { -2.0f, 4.0f, 0.0f }; + b3BodyId bodyId2 = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId2, &shapeDef, &box.base ); + + b3FilterJointDef jointDef = b3DefaultFilterJointDef(); + jointDef.base.bodyIdA = bodyId1; + jointDef.base.bodyIdB = bodyId2; + b3CreateFilterJoint( m_worldId, &jointDef ); + } + + void Render() override + { + Sample::Render(); + + b3Transform transform = b3Transform_identity; + transform.p.y += 0.05f; + DrawTransform( m_scene, transform, 2.0f ); + } + + static Sample* Create( SampleContext* context ) + { + return new FilterJoint( context ); + } +}; + +static int sampleFilterJoint = SampleManager::Register( "Joints", "Filter", FilterJoint::Create ); + +/// This test shows how to use a motor joint. A motor joint +/// can be used to animate a dynamic body. With finite motor forces +/// the body can be blocked by collision with other bodies. +class MotorJoint : public Sample +{ +public: + explicit MotorJoint( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 0.0f, 40.0f, { 0.0f, 10.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + b3BodyId groundId; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position.y = -1.0f; + groundId = b3CreateBody( m_worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeBoxHull( 20.0f, 1.0f, 20.0f ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + + m_transform = { .p = { 0.0f, 10.0f, 0.0f }, .q = b3Quat_identity }; + + // Define a target body + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_kinematicBody; + bodyDef.position = m_transform.p; + m_targetId = b3CreateBody( m_worldId, &bodyDef ); + } + + // Define motorized body + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = m_transform.p; + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 1.0f, 0.25f, 0.25f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( m_bodyId, &shapeDef, &box.base ); + + m_maxForce = 400000.0f; + m_maxTorque = 500000.0f; + + b3MotorJointDef jointDef = b3DefaultMotorJointDef(); + jointDef.base.bodyIdA = m_targetId; + jointDef.base.bodyIdB = m_bodyId; + jointDef.linearHertz = 4.0f; + jointDef.linearDampingRatio = 0.7f; + jointDef.angularHertz = 4.0f; + jointDef.angularDampingRatio = 0.7f; + jointDef.maxSpringForce = m_maxForce; + jointDef.maxSpringTorque = m_maxTorque; + + m_jointId = b3CreateMotorJoint( m_worldId, &jointDef ); + } + + // Define spring body + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { -2.0f, 2.0f, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + b3MotorJointDef jointDef = b3DefaultMotorJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = bodyId; + jointDef.base.localFrameA.p = b3Add( bodyDef.position, { 0.25f, 1.25f, 0.0f } ); + jointDef.base.localFrameB.p = { 0.25f, 0.25f }; + jointDef.base.collideConnected = true; + jointDef.linearHertz = 7.5f; + jointDef.linearDampingRatio = 0.7f; + jointDef.angularHertz = 7.5f; + jointDef.angularDampingRatio = 0.7f; + jointDef.maxSpringForce = 200000.0f; + jointDef.maxSpringTorque = 10000.0f; + + b3CreateMotorJoint( m_worldId, &jointDef ); + } + + m_speed = 0.0f; + m_time = 0.0f; + } + + void UpdateUI() override + { + float fontSize = ImGui::GetFontSize(); + float height = 180.0f; + ImGui::SetNextWindowPos( ImVec2( 0.5f * fontSize, m_camera->m_height - height - 2.0f * fontSize ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Motor Joint", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::SliderFloat( "Speed", &m_speed, -5.0f, 5.0f, "%.0f" ) ) + { + } + + if ( ImGui::SliderFloat( "Max Force", &m_maxForce, 0.0f, 1000000.0f, "%.0f" ) ) + { + b3MotorJoint_SetMaxSpringForce( m_jointId, m_maxForce ); + } + + if ( ImGui::SliderFloat( "Max Torque", &m_maxTorque, 0.0f, 1000000.0f, "%.0f" ) ) + { + b3MotorJoint_SetMaxSpringTorque( m_jointId, m_maxTorque ); + } + + if ( ImGui::Button( "Apply Impulse" ) ) + { + b3Body_ApplyLinearImpulseToCenter( m_bodyId, { 100000.0f, 0.0f }, true ); + } + + ImGui::End(); + } + + void Step() override + { + float timeStep = m_context->hertz > 0.0f ? 1.0f / m_context->hertz : 0.0f; + + if ( m_context->pause ) + { + if ( m_context->singleStep == 0 ) + { + timeStep = 0.0f; + } + } + + if ( timeStep > 0.0f ) + { + m_time += m_speed * timeStep; + + b3Vec3 linearOffset; + linearOffset.x = 6.0f * sinf( 2.0f * m_time ); + linearOffset.y = 10.0f + 4.0f * sinf( 1.0f * m_time ); + linearOffset.z = 0.0f; + + float angularOffset = 2.0f * m_time; + m_transform = { linearOffset, b3MakeQuatFromAxisAngle( b3Vec3_axisZ, angularOffset ) }; + + b3Body_SetTargetTransform( m_targetId, m_transform, timeStep, true ); + } + + DrawTransform( m_scene, m_transform, 1.0f ); + + Sample::Step(); + + b3Vec3 force = b3Joint_GetConstraintForce( m_jointId ); + b3Vec3 torque = b3Joint_GetConstraintTorque( m_jointId ); + + DrawTextLine( "force = %3.f, torque = %3.f", b3Length( force ), b3Length( torque ) ); + } + + static Sample* Create( SampleContext* context ) + { + return new MotorJoint( context ); + } + + b3BodyId m_targetId; + b3BodyId m_bodyId; + b3JointId m_jointId; + b3Transform m_transform; + float m_time; + float m_speed; + float m_maxForce; + float m_maxTorque; +}; + +static int sampleMotorJoint = SampleManager::Register( "Joints", "Motor Joint", MotorJoint::Create ); + +class TopDownFriction : public Sample +{ +public: + explicit TopDownFriction( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 0.0f, 25.0f, { 0.0f, 10.0f, 0.0f } ); + } + + b3BodyId groundId; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + groundId = b3CreateBody( m_worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeTransformedBoxHull( 10.0f, 0.5f, 50.0f, { b3Vec3_zero, b3Quat_identity } ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + + box = b3MakeTransformedBoxHull( 0.5f, 10.0f, 50.0f, { { -10.0f, 10.0f, 0.0f }, b3Quat_identity } ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + + box = b3MakeTransformedBoxHull( 0.5f, 10.0f, 50.0f, { { 10.0f, 10.0f, 0.0f }, b3Quat_identity } ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + + box = b3MakeTransformedBoxHull( 10.0f, 0.5f, 50.0f, { { 00.0f, 20.0f, 0.0f }, b3Quat_identity } ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + + b3MotorJointDef jointDef = b3DefaultMotorJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.collideConnected = true; + jointDef.maxVelocityForce = 1000.0f; + jointDef.maxVelocityTorque = 1000.0f; + + b3Capsule capsule = { { -0.25f, 0.0f }, { 0.25f, 0.0f }, 0.25f }; + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.35f }; + b3BoxHull cube = b3MakeBoxHull( 0.35f, 0.35f, 0.35f ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.gravityScale = 0.0f; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.restitution = 0.8f; + + int n = 10; + float x = -5.0f, y = 15.0f; + for ( int i = 0; i < n; ++i ) + { + for ( int j = 0; j < n; ++j ) + { + bodyDef.position = { x, y }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + int remainder = ( n * i + j ) % 4; + if ( remainder == 0 ) + { + b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + } + else if ( remainder == 1 ) + { + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + } + else + { + b3CreateHullShape( bodyId, &shapeDef, &cube.base ); + } + + jointDef.base.bodyIdB = bodyId; + b3CreateMotorJoint( m_worldId, &jointDef ); + + x += 1.0f; + } + + x = -5.0f; + y -= 1.0f; + } + } + + void UpdateUI() override + { + float fontSize = ImGui::GetFontSize(); + float height = 180.0f; + ImGui::SetNextWindowPos( ImVec2( 0.5f * fontSize, m_camera->m_height - height - 2.0f * fontSize ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Top Down Friction", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::Button( "Explode" ) ) + { + b3Sphere sphere = { { 0.0f, 10.0f, 0.0 }, 10.0f }; + b3ExplosionDef def = b3DefaultExplosionDef(); + def.position = sphere.center; + def.radius = sphere.radius; + def.falloff = 5.0f; + def.impulsePerArea = 10000.0f; + b3World_Explode( m_worldId, &def ); + + DrawSphere( m_scene, b3Transform_identity, sphere, b3_colorWhite ); + } + + ImGui::End(); + } + + static Sample* Create( SampleContext* context ) + { + return new TopDownFriction( context ); + } +}; + +static int sampleTopDownFriction = SampleManager::Register( "Joints", "Top Down Friction", TopDownFriction::Create ); + +class PrismaticJoint : public Sample +{ +public: + explicit PrismaticJoint( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 15.0f, { 0.0f, 2.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + m_targetTranslation = 0.0f; + m_motorSpeed = 0.0f; + m_motorForce = 20.0f; + m_hertz = 2.0f; + m_dampingRatio = 0.7f; + m_lowerTranslation = -1.0f; + m_upperTranslation = 1.0f; + m_enableSpring = true; + m_enableMotor = false; + m_enableLimit = false; + + b3BodyId groundId; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull groundBox = b3MakeBoxHull( 20.0f, 1.0f, 20.0f ); + b3CreateHullShape( groundId, &shapeDef, &groundBox.base ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 4.0f, 0.0f }; + bodyDef.gravityScale = 0.0f; + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + b3BoxHull box = b3MakeBoxHull( 0.5f, 1.5f, 0.25f ); + b3CreateHullShape( m_bodyId, &shapeDef, &box.base ); + + b3PrismaticJointDef jointDef = b3DefaultPrismaticJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = m_bodyId; + jointDef.base.localFrameA.p = { 0.0f, 6.5f, 0.0f }; + jointDef.base.localFrameB.p = { 0.0f, 1.5f, 0.0f }; + jointDef.base.constraintHertz = 120.0f; + jointDef.enableLimit = m_enableLimit; + jointDef.lowerTranslation = m_lowerTranslation; + jointDef.upperTranslation = m_upperTranslation; + jointDef.enableSpring = m_enableSpring; + jointDef.hertz = m_hertz; + jointDef.dampingRatio = m_dampingRatio; + jointDef.targetTranslation = m_targetTranslation; + jointDef.enableMotor = m_enableMotor; + jointDef.maxMotorForce = m_motorForce; + jointDef.motorSpeed = m_motorSpeed; + + m_jointId = b3CreatePrismaticJoint( m_worldId, &jointDef ); + } + + void UpdateUI() override + { + float height = 320.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 300.0f, height ) ); + + ImGui::Begin( "Prismatic Joint", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::Checkbox( "Limit", &m_enableLimit ) ) + { + b3PrismaticJoint_EnableLimit( m_jointId, m_enableLimit ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( m_enableLimit ) + { + if ( ImGui::SliderFloat( "Lower Translation", &m_lowerTranslation, -10.0f, 10.0f, "%.1f" ) ) + { + m_lowerTranslation = b3MinFloat( m_lowerTranslation, m_upperTranslation ); + b3PrismaticJoint_SetLimits( m_jointId, m_lowerTranslation, m_upperTranslation ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat( "Upper Translation", &m_upperTranslation, -10.0f, 10.0f, "%.1f" ) ) + { + m_upperTranslation = b3MaxFloat( m_upperTranslation, m_lowerTranslation ); + b3PrismaticJoint_SetLimits( m_jointId, m_lowerTranslation, m_upperTranslation ); + b3Joint_WakeBodies( m_jointId ); + } + } + + if ( ImGui::Checkbox( "Motor", &m_enableMotor ) ) + { + b3PrismaticJoint_EnableMotor( m_jointId, m_enableMotor ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( m_enableMotor ) + { + if ( ImGui::SliderFloat( "Max Force", &m_motorForce, 0.0f, 100000.0f, "%.0f" ) ) + { + b3PrismaticJoint_SetMaxMotorForce( m_jointId, m_motorForce ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat( "Speed", &m_motorSpeed, -10.0f, 10.0f, "%.0f" ) ) + { + b3PrismaticJoint_SetMotorSpeed( m_jointId, m_motorSpeed ); + b3Joint_WakeBodies( m_jointId ); + } + } + + if ( ImGui::Checkbox( "Spring", &m_enableSpring ) ) + { + b3PrismaticJoint_EnableSpring( m_jointId, m_enableSpring ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( m_enableSpring ) + { + if ( ImGui::SliderFloat( "Hertz", &m_hertz, 0.0f, 10.0f, "%.1f" ) ) + { + b3PrismaticJoint_SetSpringHertz( m_jointId, m_hertz ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat( "Damping", &m_dampingRatio, 0.0f, 2.0f, "%.1f" ) ) + { + b3PrismaticJoint_SetSpringDampingRatio( m_jointId, m_dampingRatio ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat( "Translation", &m_targetTranslation, -20.0f, 20.0f, "%.1f" ) ) + { + b3PrismaticJoint_SetTargetTranslation( m_jointId, m_targetTranslation ); + b3Joint_WakeBodies( m_jointId ); + } + } + + ImGui::End(); + } + + void Render() override + { + Sample::Render(); + } + + static Sample* Create( SampleContext* context ) + { + return new PrismaticJoint( context ); + } + + b3BodyId m_bodyId; + b3JointId m_jointId; + float m_targetTranslation; + float m_motorSpeed; + float m_motorForce; + float m_hertz; + float m_dampingRatio; + float m_lowerTranslation; + float m_upperTranslation; + bool m_enableSpring; + bool m_enableMotor; + bool m_enableLimit; +}; + +static int samplePrismaticJoint = SampleManager::Register( "Joints", "Prismatic", PrismaticJoint::Create ); + +class SphericalJoint : public Sample +{ +public: + explicit SphericalJoint( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 15.0f, { 0.0f, 2.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + b3BodyId groundId; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull groundBox = b3MakeBoxHull( 20.0f, 1.0f, 20.0f ); + b3CreateHullShape( groundId, &shapeDef, &groundBox.base ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 4.0f, 1.5f }; + bodyDef.gravityScale = 0.0f; + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.25f, 1.5f ); + b3CreateHullShape( m_bodyId, &shapeDef, &box.base ); + + b3SphericalJointDef jointDef = b3DefaultSphericalJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = m_bodyId; + jointDef.base.drawScale = 2.0f; + jointDef.base.localFrameA.p = { 0.0f, 5.5f, 0.0f }; + jointDef.base.localFrameB.p = { 0.0f, 0.0f, -1.5f }; + jointDef.enableConeLimit = m_enableConeLimit; + jointDef.coneAngle = B3_DEG_TO_RAD * m_coneAngleDegrees; + jointDef.enableTwistLimit = m_enableTwistLimit; + jointDef.lowerTwistAngle = B3_DEG_TO_RAD * m_lowerTwistDegrees; + jointDef.upperTwistAngle = B3_DEG_TO_RAD * m_upperTwistDegrees; + jointDef.enableSpring = m_enableSpring; + jointDef.hertz = m_hertz; + jointDef.dampingRatio = m_dampingRatio; + jointDef.enableMotor = m_enableMotor; + jointDef.maxMotorTorque = m_motorTorque; + jointDef.motorVelocity = m_motorVelocity; + + m_jointId = b3CreateSphericalJoint( m_worldId, &jointDef ); + } + + void UpdateUI() override + { + float height = 360.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 300.0f, height ) ); + + ImGui::Begin( "Spherical Joint", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::Checkbox( "Cone Limit", &m_enableConeLimit ) ) + { + b3SphericalJoint_EnableConeLimit( m_jointId, m_enableConeLimit ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( m_enableConeLimit ) + { + if ( ImGui::SliderFloat( "Cone Angle", &m_coneAngleDegrees, 0.0f, 90.0f, "%.0f" ) ) + { + b3SphericalJoint_SetConeLimit( m_jointId, B3_DEG_TO_RAD * m_coneAngleDegrees ); + b3Joint_WakeBodies( m_jointId ); + } + } + + if ( ImGui::Checkbox( "Twist Limit", &m_enableTwistLimit ) ) + { + b3SphericalJoint_EnableTwistLimit( m_jointId, m_enableTwistLimit ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( m_enableTwistLimit ) + { + if ( ImGui::SliderFloat( "Lower Twist", &m_lowerTwistDegrees, -180.0f, 180.0f, "%.0f" ) ) + { + m_lowerTwistDegrees = b3MinFloat( m_lowerTwistDegrees, m_upperTwistDegrees ); + b3SphericalJoint_SetTwistLimits( m_jointId, B3_DEG_TO_RAD * m_lowerTwistDegrees, + B3_DEG_TO_RAD * m_upperTwistDegrees ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat( "Upper Twist", &m_upperTwistDegrees, -180.0f, 180.0f, "%.0f" ) ) + { + m_upperTwistDegrees = b3MaxFloat( m_upperTwistDegrees, m_lowerTwistDegrees ); + b3SphericalJoint_SetTwistLimits( m_jointId, B3_DEG_TO_RAD * m_lowerTwistDegrees, + B3_DEG_TO_RAD * m_upperTwistDegrees ); + b3Joint_WakeBodies( m_jointId ); + } + } + + if ( ImGui::Checkbox( "Motor", &m_enableMotor ) ) + { + b3SphericalJoint_EnableMotor( m_jointId, m_enableMotor ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( m_enableMotor ) + { + if ( ImGui::SliderFloat( "Max Torque", &m_motorTorque, 0.0f, 50.0f, "%.0f" ) ) + { + b3SphericalJoint_SetMaxMotorTorque( m_jointId, m_motorTorque ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat3( "Velocity", &m_motorVelocity.x, -10.0f, 10.0f, "%.0f" ) ) + { + b3SphericalJoint_SetMotorVelocity( m_jointId, m_motorVelocity ); + b3Joint_WakeBodies( m_jointId ); + } + } + + if ( ImGui::Checkbox( "Spring", &m_enableSpring ) ) + { + b3SphericalJoint_EnableSpring( m_jointId, m_enableSpring ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( m_enableSpring ) + { + if ( ImGui::SliderFloat( "Hertz", &m_hertz, 0.0f, 10.0f, "%.1f" ) ) + { + b3SphericalJoint_SetSpringHertz( m_jointId, m_hertz ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat( "Damping", &m_dampingRatio, 0.0f, 2.0f, "%.1f" ) ) + { + b3SphericalJoint_SetSpringDampingRatio( m_jointId, m_dampingRatio ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat3( "Rotation", &m_targetRotation.x, -180.0f, 180.0f, "%.0f" ) ) + { + b3Quat qx = b3MakeQuatFromAxisAngle( b3Vec3_axisX, B3_DEG_TO_RAD * m_targetRotation.x ); + b3Quat qy = b3MakeQuatFromAxisAngle( b3Vec3_axisY, B3_DEG_TO_RAD * m_targetRotation.y ); + b3Quat qz = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, B3_DEG_TO_RAD * m_targetRotation.z ); + b3Quat q = b3MulQuat( qz, b3MulQuat( qy, qx ) ); + b3SphericalJoint_SetTargetRotation( m_jointId, q ); + b3Joint_WakeBodies( m_jointId ); + } + } + + ImGui::End(); + } + + void Render() override + { + Sample::Render(); + + b3Transform transform = b3Transform_identity; + transform.p.y += 0.05f; + DrawTransform( m_scene, transform, 2.0f ); + + transform = b3Body_GetTransform( m_bodyId ); + DrawTransform( m_scene, transform, 2.0f ); + } + + static Sample* Create( SampleContext* context ) + { + return new SphericalJoint( context ); + } + + b3BodyId m_bodyId; + b3JointId m_jointId; + b3Vec3 m_targetRotation = b3Vec3_zero; + b3Vec3 m_motorVelocity = b3Vec3_zero; + float m_motorTorque = 20.0f; + float m_hertz = 2.0f; + float m_dampingRatio = 0.7f; + float m_coneAngleDegrees = 30.0f; + float m_lowerTwistDegrees = -35.0f; + float m_upperTwistDegrees = 35.0f; + bool m_enableSpring = true; + bool m_enableMotor = false; + bool m_enableTwistLimit = false; + bool m_enableConeLimit = false; +}; + +static int sampleSphericalJoint = SampleManager::Register( "Joints", "Spherical", SphericalJoint::Create ); + +class ParallelJoint : public Sample +{ +public: + explicit ParallelJoint( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 15.0f, { 0.0f, 2.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + m_hertz = 10.0f; + m_dampingRatio = 0.7f; + m_maxTorque = 5000.0f; + + b3BodyId groundId; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull groundBox = b3MakeBoxHull( 20.0f, 1.0f, 20.0f ); + b3CreateHullShape( groundId, &shapeDef, &groundBox.base ); + } + + { + b3Transform transform; + transform.p = { 0.0f, 5.0f, -20.0f }; + transform.q = b3Quat_identity; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 20.0f, 5.0f, 0.1f, transform ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( groundId, &shapeDef, &wallBox.base ); + } + + { + b3Transform transform; + transform.p = { 0.0f, 5.0f, 20.0f }; + transform.q = b3Quat_identity; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 20.0f, 5.0f, 0.1f, transform ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( groundId, &shapeDef, &wallBox.base ); + } + + { + b3Transform transform; + transform.p = { -20.0f, 5.0f, 0.0f }; + transform.q = b3Quat_identity; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 0.1f, 5.0f, 20.0f, transform ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( groundId, &shapeDef, &wallBox.base ); + } + + { + b3Transform transform; + transform.p = { 20.0f, 5.0f, 0.0f }; + transform.q = b3Quat_identity; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 0.1f, 5.0f, 20.0f, transform ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( groundId, &shapeDef, &wallBox.base ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 4.0f, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisX, 0.25f * B3_PI ); + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + b3BoxHull box = b3MakeBoxHull( 0.5f, 1.5f, 0.25f ); + b3CreateHullShape( m_bodyId, &shapeDef, &box.base ); + + b3ParallelJointDef jointDef = b3DefaultParallelJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = m_bodyId; + jointDef.base.localFrameA.q = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisZ, b3Vec3_axisY ); + jointDef.base.localFrameB.q = b3InvMulQuat( bodyDef.rotation, jointDef.base.localFrameA.q ); + + jointDef.base.drawScale = 2.0f; + jointDef.base.collideConnected = true; + jointDef.hertz = m_hertz; + jointDef.dampingRatio = m_dampingRatio; + + m_jointId = b3CreateParallelJoint( m_worldId, &jointDef ); + } + + void UpdateUI() override + { + float fontSize = ImGui::GetFontSize(); + float height = 20.0f * fontSize; + ImGui::SetNextWindowPos( { 1.0f * fontSize, m_camera->m_height - height - 3.0f * fontSize }, ImGuiCond_Once ); + ImGui::SetNextWindowSize( { 20.0f * fontSize, height } ); + + ImGui::Begin( "Parallel Joint", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::SliderFloat( "Hertz", &m_hertz, 0.0f, 5.0f, "%.1f" ) ) + { + b3ParallelJoint_SetSpringHertz( m_jointId, m_hertz ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat( "Damping", &m_dampingRatio, 0.0f, 2.0f, "%.1f" ) ) + { + b3ParallelJoint_SetSpringDampingRatio( m_jointId, m_dampingRatio ); + b3Joint_WakeBodies( m_jointId ); + } + + ImGui::End(); + } + + static Sample* Create( SampleContext* context ) + { + return new ParallelJoint( context ); + } + + b3BodyId m_bodyId; + b3JointId m_jointId; + float m_maxTorque; + float m_hertz; + float m_dampingRatio; +}; + +static int sampleParallelJoint = SampleManager::Register( "Joints", "Parallel Spring", ParallelJoint::Create ); + +class RevoluteJoint : public Sample +{ +public: + explicit RevoluteJoint( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 15.0f, { 0.0f, 2.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + m_targetAngle = 0.0f; + m_motorSpeed = 0.0f; + m_motorTorque = 5000.0f; + m_hertz = 2.0f; + m_dampingRatio = 0.7f; + m_lowerDegrees = -35.0f; + m_upperDegrees = 35.0f; + m_enableSpring = false; + m_enableMotor = false; + m_enableLimit = false; + + b3BodyId groundId; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull groundBox = b3MakeBoxHull( 20.0f, 1.0f, 20.0f ); + b3CreateHullShape( groundId, &shapeDef, &groundBox.base ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 4.0f, 0.0f }; + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + b3BoxHull box = b3MakeBoxHull( 0.5f, 1.5f, 0.25f ); + b3CreateHullShape( m_bodyId, &shapeDef, &box.base ); + + b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = m_bodyId; + jointDef.base.localFrameA.p = { 0.0f, 6.5f, 0.0f }; + jointDef.base.localFrameB.p = { 0.0f, 1.5f, 0.0f }; + jointDef.base.drawScale = 2.0f; + jointDef.enableLimit = m_enableLimit; + jointDef.lowerAngle = B3_DEG_TO_RAD * m_lowerDegrees; + jointDef.upperAngle = B3_DEG_TO_RAD * m_upperDegrees; + jointDef.enableSpring = m_enableSpring; + jointDef.hertz = m_hertz; + jointDef.dampingRatio = m_dampingRatio; + jointDef.enableMotor = m_enableMotor; + jointDef.maxMotorTorque = m_motorTorque; + jointDef.motorSpeed = m_motorSpeed; + + m_jointId = b3CreateRevoluteJoint( m_worldId, &jointDef ); + } + + void UpdateUI() override + { + float height = 320.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 300.0f, height ) ); + + ImGui::Begin( "Revolute Joint", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::Checkbox( "Limit", &m_enableLimit ) ) + { + b3RevoluteJoint_EnableLimit( m_jointId, m_enableLimit ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( m_enableLimit ) + { + if ( ImGui::SliderFloat( "Lower Angle", &m_lowerDegrees, -180.0f, 180.0f, "%.0f" ) ) + { + m_lowerDegrees = b3MinFloat( m_lowerDegrees, m_upperDegrees ); + b3RevoluteJoint_SetLimits( m_jointId, B3_DEG_TO_RAD * m_lowerDegrees, B3_DEG_TO_RAD * m_upperDegrees ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat( "Upper Angle", &m_upperDegrees, -180.0f, 180.0f, "%.0f" ) ) + { + m_upperDegrees = b3MaxFloat( m_upperDegrees, m_lowerDegrees ); + b3RevoluteJoint_SetLimits( m_jointId, B3_DEG_TO_RAD * m_lowerDegrees, B3_DEG_TO_RAD * m_upperDegrees ); + b3Joint_WakeBodies( m_jointId ); + } + } + + if ( ImGui::Checkbox( "Motor", &m_enableMotor ) ) + { + b3RevoluteJoint_EnableMotor( m_jointId, m_enableMotor ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( m_enableMotor ) + { + if ( ImGui::SliderFloat( "Max Torque", &m_motorTorque, 0.0f, 50000.0f, "%.0f" ) ) + { + b3RevoluteJoint_SetMaxMotorTorque( m_jointId, m_motorTorque ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat( "Speed", &m_motorSpeed, -10.0f, 10.0f, "%.0f" ) ) + { + b3RevoluteJoint_SetMotorSpeed( m_jointId, m_motorSpeed ); + b3Joint_WakeBodies( m_jointId ); + } + } + + if ( ImGui::Checkbox( "Spring", &m_enableSpring ) ) + { + b3RevoluteJoint_EnableSpring( m_jointId, m_enableSpring ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( m_enableSpring ) + { + if ( ImGui::SliderFloat( "Hertz", &m_hertz, 0.0f, 10.0f, "%.1f" ) ) + { + b3RevoluteJoint_SetSpringHertz( m_jointId, m_hertz ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat( "Damping", &m_dampingRatio, 0.0f, 2.0f, "%.1f" ) ) + { + b3RevoluteJoint_SetSpringDampingRatio( m_jointId, m_dampingRatio ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat( "Rotation", &m_targetAngle, -180.0f, 180.0f, "%.0f" ) ) + { + b3RevoluteJoint_SetTargetAngle( m_jointId, B3_DEG_TO_RAD * m_targetAngle ); + b3Joint_WakeBodies( m_jointId ); + } + } + + ImGui::End(); + } + + void Render() override + { + Sample::Render(); + + b3MassData massData = b3Body_GetMassData( m_bodyId ); + b3Vec3 angularVelocity = b3Body_GetAngularVelocity( m_bodyId ); + b3Vec3 linearVelocity = b3Body_GetLinearVelocity( m_bodyId ); + float kineticEnergy = 0.5f * b3Dot( angularVelocity, b3MulMV( massData.inertia, angularVelocity ) ); + kineticEnergy += 0.5f * massData.mass * b3Dot( linearVelocity, linearVelocity ); + b3Vec3 center = b3Body_GetWorldCenterOfMass( m_bodyId ); + b3Vec3 gravity = b3World_GetGravity( m_worldId ); + float potentialEnergy = -massData.mass * center.y * gravity.y; + DrawTextLine( "kinetic energy = %g", kineticEnergy ); + DrawTextLine( "potential energy = %g", potentialEnergy ); + DrawTextLine( "total energy = %g", kineticEnergy + potentialEnergy ); + } + + static Sample* Create( SampleContext* context ) + { + return new RevoluteJoint( context ); + } + + b3BodyId m_bodyId; + b3JointId m_jointId; + float m_targetAngle; + float m_motorSpeed; + float m_motorTorque; + float m_hertz; + float m_dampingRatio; + float m_lowerDegrees; + float m_upperDegrees; + bool m_enableSpring; + bool m_enableMotor; + bool m_enableLimit; +}; + +static int sampleRevoluteJoint = SampleManager::Register( "Joints", "Revolute", RevoluteJoint::Create ); + +class WeldJoint : public Sample +{ +public: + explicit WeldJoint( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 15.0f, { 0.0f, 2.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + b3BodyId groundId; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull groundBox = b3MakeBoxHull( 20.0f, 1.0f, 20.0f ); + b3CreateHullShape( groundId, &shapeDef, &groundBox.base ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 4.0f, 0.0f }; + bodyDef.gravityScale = 0.0f; + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + b3BoxHull box = b3MakeBoxHull( 0.5f, 1.5f, 0.25f ); + b3CreateHullShape( m_bodyId, &shapeDef, &box.base ); + + b3WeldJointDef jointDef = b3DefaultWeldJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = m_bodyId; + jointDef.base.localFrameA.p = { 0.0f, 6.5f, 0.0f }; + jointDef.base.localFrameB.p = { 0.0f, 1.5f, 0.0f }; + jointDef.base.constraintHertz = 240.0f; + jointDef.linearHertz = m_linearHertz; + jointDef.linearDampingRatio = m_linearDampingRatio; + jointDef.angularHertz = m_angularHertz; + jointDef.angularDampingRatio = m_angularDampingRatio; + jointDef.base.drawScale = 2.0f; + + m_jointId = b3CreateWeldJoint( m_worldId, &jointDef ); + } + + void UpdateUI() override + { + float height = 150.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 350.0f, height ) ); + + ImGui::Begin( "Weld Joint", nullptr, ImGuiWindowFlags_NoResize ); + + ImGui::PushItemWidth( 200.0f ); + + if ( ImGui::SliderFloat( "Linear Hertz", &m_linearHertz, 0.0f, 10.0f, "%.1f" ) ) + { + b3WeldJoint_SetLinearHertz( m_jointId, m_linearHertz ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat( "Linear Damping", &m_linearDampingRatio, 0.0f, 2.0f, "%.1f" ) ) + { + b3WeldJoint_SetLinearDampingRatio( m_jointId, m_linearDampingRatio ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat( "Angular Hertz", &m_angularHertz, 0.0f, 10.0f, "%.1f" ) ) + { + b3WeldJoint_SetAngularHertz( m_jointId, m_angularHertz ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat( "Angular Damping", &m_angularDampingRatio, 0.0f, 2.0f, "%.1f" ) ) + { + b3WeldJoint_SetAngularDampingRatio( m_jointId, m_angularDampingRatio ); + b3Joint_WakeBodies( m_jointId ); + } + + ImGui::PopItemWidth(); + + ImGui::End(); + } + + void Render() override + { + Sample::Render(); + } + + static Sample* Create( SampleContext* context ) + { + return new WeldJoint( context ); + } + + b3BodyId m_bodyId; + b3JointId m_jointId; + float m_linearHertz = 0.0f; + float m_linearDampingRatio = 0.0f; + float m_angularHertz = 2.0f; + float m_angularDampingRatio = 0.7f; +}; + +static int sampleWeldJoint = SampleManager::Register( "Joints", "Weld", WeldJoint::Create ); + +class WheelJoint : public Sample +{ +public: + explicit WheelJoint( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 25.0f, 20.0f, 7.0f, { 0.0f, 2.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + m_spinSpeed = 0.0f; + m_maxSpinTorque = 20.0f; + m_suspensionHertz = 2.0f; + m_suspensionDampingRatio = 0.7f; + m_lowerTranslation = -1.0f; + m_upperTranslation = 1.0f; + m_enableSuspension = false; + m_enableSpinMotor = false; + m_enableSuspensionLimit = false; + + m_enableSteering = false; + m_steeringHertz = 1.0f; + m_steeringDampingRatio = 0.7f; + m_enableSteeringLimit = false; + m_lowerSteeringDegrees = -45.0f; + m_upperSteeringDegrees = 45.0f; + m_maxSteeringTorque = 20.0f; + m_targetSteeringDegrees = 0.0f; + + b3BodyId groundId; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull groundBox = b3MakeBoxHull( 20.0f, 1.0f, 20.0f ); + b3CreateHullShape( groundId, &shapeDef, &groundBox.base ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 2.0f, 0.0f }; + bodyDef.rotation = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisY, b3Vec3_axisZ ); + // bodyDef.gravityScale = 0.0f; + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + b3Hull* hull = b3CreateCylinder( 0.25f, 0.4f, 0.0f, 12 ); + // b3BoxHull box = b3MakeBoxHull( 0.5f, 1.5f, 0.25f ); + b3CreateHullShape( m_bodyId, &shapeDef, hull ); + b3DestroyHull( hull ); + + b3WheelJointDef jointDef = b3DefaultWheelJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = m_bodyId; + jointDef.base.localFrameA.p = { 0.0f, 3.0f, 0.0f }; + jointDef.base.localFrameA.q = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisX, b3Vec3_axisY ); + jointDef.base.localFrameB.p = { 0.0f, 0.0f, 0.0f }; + jointDef.base.localFrameB.q = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisZ, b3Vec3_axisY ); + jointDef.base.collideConnected = true; + jointDef.enableSuspensionLimit = m_enableSuspensionLimit; + jointDef.lowerSuspensionLimit = m_lowerTranslation; + jointDef.upperSuspensionLimit = m_upperTranslation; + jointDef.enableSuspensionSpring = m_enableSuspension; + jointDef.suspensionHertz = m_suspensionHertz; + jointDef.suspensionDampingRatio = m_suspensionDampingRatio; + jointDef.enableSpinMotor = m_enableSpinMotor; + jointDef.maxSpinTorque = m_maxSpinTorque; + jointDef.spinSpeed = m_spinSpeed; + jointDef.enableSteering = m_enableSteering; + jointDef.steeringHertz = m_steeringHertz; + jointDef.steeringDampingRatio = m_steeringDampingRatio; + jointDef.targetSteeringAngle = B3_PI / 180.0f * m_targetSteeringDegrees; + jointDef.maxSteeringTorque = m_maxSteeringTorque; + jointDef.enableSteeringLimit = m_enableSteeringLimit; + jointDef.lowerSteeringLimit = B3_PI / 180.0f * m_lowerSteeringDegrees; + jointDef.upperSteeringLimit = B3_PI / 180.0f * m_upperSteeringDegrees; + + m_jointId = b3CreateWheelJoint( m_worldId, &jointDef ); + } + + void UpdateUI() override + { + float fontSize = ImGui::GetFontSize(); + float height = 30.0f * fontSize; + ImGui::SetNextWindowPos( { 1.0f * fontSize, m_camera->m_height - height - 3.0f * fontSize }, ImGuiCond_Once ); + ImGui::SetNextWindowSize( { 25.0f * fontSize, height } ); + + ImGui::Begin( "Wheel Joint", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::Checkbox( "Suspension Limit", &m_enableSuspensionLimit ) ) + { + b3WheelJoint_EnableSuspensionLimit( m_jointId, m_enableSuspensionLimit ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( m_enableSuspensionLimit ) + { + if ( ImGui::SliderFloat( "Lower Translation", &m_lowerTranslation, -10.0f, 10.0f, "%.1f" ) ) + { + m_lowerTranslation = b3MinFloat( m_lowerTranslation, m_upperTranslation ); + b3WheelJoint_SetSuspensionLimits( m_jointId, m_lowerTranslation, m_upperTranslation ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat( "Upper Translation", &m_upperTranslation, -10.0f, 10.0f, "%.1f" ) ) + { + m_upperTranslation = b3MaxFloat( m_upperTranslation, m_lowerTranslation ); + b3WheelJoint_SetSuspensionLimits( m_jointId, m_lowerTranslation, m_upperTranslation ); + b3Joint_WakeBodies( m_jointId ); + } + } + + if ( ImGui::Checkbox( "Motor", &m_enableSpinMotor ) ) + { + b3WheelJoint_EnableSpinMotor( m_jointId, m_enableSpinMotor ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( m_enableSpinMotor ) + { + if ( ImGui::SliderFloat( "Max Torque", &m_maxSpinTorque, 0.0f, 100.0f, "%.0f" ) ) + { + b3WheelJoint_SetMaxSpinTorque( m_jointId, m_maxSpinTorque ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat( "Speed", &m_spinSpeed, -10.0f, 10.0f, "%.0f" ) ) + { + b3WheelJoint_SetSpinMotorSpeed( m_jointId, m_spinSpeed ); + b3Joint_WakeBodies( m_jointId ); + } + } + + if ( ImGui::Checkbox( "Suspension", &m_enableSuspension ) ) + { + b3WheelJoint_EnableSuspension( m_jointId, m_enableSuspension ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( m_enableSuspension ) + { + if ( ImGui::SliderFloat( "Suspension Hertz", &m_suspensionHertz, 0.0f, 10.0f, "%.1f" ) ) + { + b3WheelJoint_SetSuspensionHertz( m_jointId, m_suspensionHertz ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat( "Suspension Damping", &m_suspensionDampingRatio, 0.0f, 2.0f, "%.1f" ) ) + { + b3WheelJoint_SetSuspensionDampingRatio( m_jointId, m_suspensionDampingRatio ); + b3Joint_WakeBodies( m_jointId ); + } + } + + if ( ImGui::Checkbox( "Steering", &m_enableSteering ) ) + { + b3WheelJoint_EnableSteering( m_jointId, m_enableSteering ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( m_enableSteering ) + { + if ( ImGui::SliderFloat( "Steering Hertz", &m_steeringHertz, 0.0f, 10.0f, "%.1f" ) ) + { + b3WheelJoint_SetSteeringHertz( m_jointId, m_steeringHertz ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat( "Steering Damping", &m_steeringDampingRatio, 0.0f, 2.0f, "%.1f" ) ) + { + b3WheelJoint_SetSuspensionDampingRatio( m_jointId, m_suspensionDampingRatio ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat( "Steering Degrees", &m_targetSteeringDegrees, -90.0f, 90.0f, "%.0f" ) ) + { + b3WheelJoint_SetTargetSteeringAngle( m_jointId, m_targetSteeringDegrees * B3_PI / 180.0f ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::Checkbox( "Steering Limit", &m_enableSteeringLimit ) ) + { + b3WheelJoint_EnableSteeringLimit( m_jointId, m_enableSteeringLimit ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( m_enableSteeringLimit ) + { + if ( ImGui::SliderFloat( "Lower Degrees", &m_lowerSteeringDegrees, -90.0f, 0.0f, "%.0f" ) ) + { + b3WheelJoint_SetSteeringLimits( m_jointId, B3_PI / 180.0f * m_lowerSteeringDegrees, + B3_PI / 180.0f * m_upperSteeringDegrees ); + b3Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat( "Upper Degrees", &m_upperSteeringDegrees, 0.0f, 90.0f, "%.0f" ) ) + { + b3WheelJoint_SetSteeringLimits( m_jointId, B3_PI / 180.0f * m_lowerSteeringDegrees, + B3_PI / 180.0f * m_upperSteeringDegrees ); + b3Joint_WakeBodies( m_jointId ); + } + } + } + + ImGui::End(); + } + + void Render() override + { + Sample::Render(); + + float angle = b3WheelJoint_GetSteeringAngle( m_jointId ); + DrawTextLine( "steering degrees = %.1f", 180.0f / B3_PI * angle ); + + b3Transform transform = b3Transform_identity; + transform.p.y += 0.05f; + DrawTransform( m_scene, transform, 2.0f ); + } + + static Sample* Create( SampleContext* context ) + { + return new WheelJoint( context ); + } + + b3BodyId m_bodyId; + b3JointId m_jointId; + float m_spinSpeed; + float m_maxSpinTorque; + float m_suspensionHertz; + float m_suspensionDampingRatio; + float m_lowerTranslation; + float m_upperTranslation; + bool m_enableSuspension; + bool m_enableSpinMotor; + bool m_enableSuspensionLimit; + bool m_enableSteering; + float m_steeringHertz; + float m_steeringDampingRatio; + bool m_enableSteeringLimit; + float m_lowerSteeringDegrees; + float m_upperSteeringDegrees; + float m_maxSteeringTorque; + float m_targetSteeringDegrees; +}; + +static int sampleWheelJoint = SampleManager::Register( "Joints", "Wheel", WheelJoint::Create ); + +class BallAndChain : public Sample +{ +public: + explicit BallAndChain( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 180.0f, 15.0f, 50.0f, { 0.0f, -20.0f, 0.0f } ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundBody = b3CreateBody( m_worldId, &bodyDef ); + + float linkRadius = 0.125f; + float linkExtent = 0.5f; + b3Capsule capsule = { { -linkExtent, 0.0f, 0.0f }, { linkExtent, 0.0f, 0.0f }, linkRadius }; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + int linkCount = 32; + + bodyDef.type = b3_dynamicBody; + b3BodyId parent = groundBody; + b3SphericalJointDef jointDef = b3DefaultSphericalJointDef(); + jointDef.base.localFrameA = b3Transform_identity; + jointDef.base.localFrameB = { { -linkExtent, 0.0f, 0.0f }, b3Quat_identity }; + jointDef.enableMotor = true; + jointDef.maxMotorTorque = 10.0f; + + for ( int i = 0; i < linkCount; ++i ) + { + bodyDef.position = { ( 1.0f + 2.0f * i ) * linkExtent, 0.0f, 0.0f }; + b3BodyId childId = b3CreateBody( m_worldId, &bodyDef ); + + b3CreateCapsuleShape( childId, &shapeDef, &capsule ); + + jointDef.base.bodyIdA = parent; + jointDef.base.bodyIdB = childId; + b3CreateSphericalJoint( m_worldId, &jointDef ); + + jointDef.base.localFrameA = { { linkExtent, 0.0f, 0.0f }, b3Quat_identity }; + parent = childId; + } + + float sphereRadius = 2.0f; + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, sphereRadius }; + + bodyDef.position = { ( 1.0f + 2.0f * linkCount ) * linkExtent + sphereRadius - linkExtent, 0.0f, 0.0f }; + + b3BodyId childId = b3CreateBody( m_worldId, &bodyDef ); + + b3CreateSphereShape( childId, &shapeDef, &sphere ); + jointDef.base.bodyIdA = parent; + jointDef.base.bodyIdB = childId; + jointDef.base.localFrameB = { { -sphereRadius, 0.0f, 0.0f }, b3Quat_identity }; + b3CreateSphericalJoint( m_worldId, &jointDef ); + } + + static Sample* Create( SampleContext* context ) + { + return new BallAndChain( context ); + } +}; + +static int sampleBallAndChain = SampleManager::Register( "Joints", "Ball and Chain", BallAndChain::Create ); + +class Door : public Sample +{ +public: + explicit Door( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 15.0f, { 0.0f, 2.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + m_groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull groundBox = b3MakeBoxHull( 20.0f, 1.0f, 20.0f ); + b3CreateHullShape( m_groundId, &shapeDef, &groundBox.base ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 1.5f, 0.0f }; + bodyDef.gravityScale = 2.0f; + m_doorId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1000.0f; + + b3BoxHull box = b3MakeBoxHull( 0.75f, 1.5f, 0.1f ); + b3CreateHullShape( m_doorId, &shapeDef, &box.base ); + } + + //{ + // b3BoxHull cube = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + // b3BodyDef bodyDef = b3DefaultBodyDef(); + // bodyDef.name = "cube"; + // bodyDef.type = b3_dynamicBody; + // bodyDef.position = { 0.0f, 2.0f, 0.0f }; + // bodyDef.gravityScale = 2.0f; + // b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + // b3ShapeDef shapeDef = b3DefaultShapeDef(); + // b3CreateHullShape( bodyId, &shapeDef, &cube.base ); + //} + + m_magnitude = 50000.0f; + m_twoJoints = true; + m_constraintHertz = 120.0f; + m_constraintDampingRatio = 0.0f; + + CreateJoints(); + } + + void CreateJoints() + { + if ( b3Joint_IsValid( m_jointId1 ) ) + { + b3DestroyJoint( m_jointId1, false ); + m_jointId1 = b3_nullJointId; + } + + if ( b3Joint_IsValid( m_jointId2 ) ) + { + b3DestroyJoint( m_jointId2, false ); + m_jointId2 = b3_nullJointId; + } + + b3Quat axisQuat = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisZ, b3Vec3_axisY ); + + { + b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef(); + jointDef.base.bodyIdA = m_groundId; + jointDef.base.bodyIdB = m_doorId; + jointDef.base.localFrameA.p = { -0.75f, 1.0f, 0.0f }; + jointDef.base.localFrameA.q = axisQuat; + jointDef.base.localFrameB.p = { -0.75f, -1.5f, 0.0f }; + jointDef.base.localFrameB.q = axisQuat; + jointDef.base.constraintHertz = m_constraintHertz; + jointDef.base.constraintDampingRatio = m_constraintDampingRatio; + jointDef.enableLimit = true; + jointDef.lowerAngle = B3_DEG_TO_RAD * -90.0f; + jointDef.upperAngle = B3_DEG_TO_RAD * 90.0f; + jointDef.enableSpring = true; + jointDef.hertz = 1.0f; + jointDef.dampingRatio = 0.5f; + jointDef.enableMotor = false; + jointDef.maxMotorTorque = 100.0f; + jointDef.motorSpeed = 0.0f; + jointDef.base.drawScale = 2.0f; + + m_jointId1 = b3CreateRevoluteJoint( m_worldId, &jointDef ); + } + + if ( m_twoJoints ) + { + b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef(); + jointDef.base.bodyIdA = m_groundId; + jointDef.base.bodyIdB = m_doorId; + jointDef.base.localFrameA.p = { -0.75f, 4.0f, 0.0f }; + jointDef.base.localFrameA.q = axisQuat; + jointDef.base.localFrameB.p = { -0.75f, 1.5f, 0.0f }; + jointDef.base.localFrameB.q = axisQuat; + jointDef.base.constraintHertz = m_constraintHertz; + jointDef.base.constraintDampingRatio = m_constraintDampingRatio; + jointDef.enableLimit = true; + jointDef.lowerAngle = B3_DEG_TO_RAD * -90.0f; + jointDef.upperAngle = B3_DEG_TO_RAD * 90.0f; + jointDef.enableSpring = true; + jointDef.hertz = 1.0f; + jointDef.dampingRatio = 0.5f; + jointDef.enableMotor = false; + jointDef.maxMotorTorque = 100.0f; + jointDef.motorSpeed = 0.0f; + jointDef.base.drawScale = 2.0f; + + m_jointId2 = b3CreateRevoluteJoint( m_worldId, &jointDef ); + } + } + + void MouseDown( b3Vec2 p, int button, int modifiers ) override + { + if ( button == 0 && modifiers == 2 ) + { + PickRay pickRay = m_camera->BuildPickRay( p.x, p.y ); + + b3RayResult result = b3World_CastRayClosest( m_worldId, pickRay.origin, pickRay.translation, b3DefaultQueryFilter() ); + + if ( result.hit ) + { + b3BodyId bodyId = b3Shape_GetBody( result.shapeId ); + + b3Vec3 impulse = m_magnitude * b3Normalize( pickRay.translation ); + b3Body_ApplyLinearImpulse( bodyId, impulse, result.point, true ); + } + } + else + { + Sample::MouseDown( p, button, modifiers ); + } + } + + void UpdateUI() override + { + float height = 220.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 350.0f, height ) ); + + ImGui::Begin( "Door", nullptr, ImGuiWindowFlags_NoResize ); + + ImGui::PushItemWidth( 200.0f ); + + if ( ImGui::Button( "impulse" ) ) + { + b3Vec3 p = b3Body_GetWorldPoint( m_doorId, { 0.75f, 0.0f, 0.0f } ); + b3Body_ApplyLinearImpulse( m_doorId, { 0.0f, 0.0f, -m_magnitude }, p, true ); + m_translationError1 = 0.0f; + m_translationError2 = 0.0f; + } + + ImGui::SliderFloat( "magnitude", &m_magnitude, 1000.0f, 100000.0f, "%.0f" ); + + if ( ImGui::Checkbox( "limit", &m_enableLimit ) ) + { + b3RevoluteJoint_EnableLimit( m_jointId1, m_enableLimit ); + + if ( b3Joint_IsValid( m_jointId2 ) ) + { + b3RevoluteJoint_EnableLimit( m_jointId2, m_enableLimit ); + } + } + + if ( ImGui::Checkbox( "two joints", &m_twoJoints ) ) + { + CreateJoints(); + } + + if ( ImGui::SliderFloat( "hertz", &m_constraintHertz, 15.0f, 240.0f, "%.0f" ) ) + { + b3Joint_SetConstraintTuning( m_jointId1, m_constraintHertz, m_constraintDampingRatio ); + + if ( B3_IS_NON_NULL( m_jointId2 ) ) + { + b3Joint_SetConstraintTuning( m_jointId2, m_constraintHertz, m_constraintDampingRatio ); + } + } + + if ( ImGui::SliderFloat( "damping", &m_constraintDampingRatio, 0.0f, 10.0f, "%.1f" ) ) + { + b3Joint_SetConstraintTuning( m_jointId1, m_constraintHertz, m_constraintDampingRatio ); + + if ( B3_IS_NON_NULL( m_jointId2 ) ) + { + b3Joint_SetConstraintTuning( m_jointId2, m_constraintHertz, m_constraintDampingRatio ); + } + } + + ImGui::PopItemWidth(); + + ImGui::End(); + } + + void Step() override + { + Sample::Step(); + + b3Vec3 p = b3Body_GetWorldPoint( m_doorId, { 0.75f, 0.0f, 0.0f } ); + DrawPoint( m_scene, p, 10.0f, b3_colorDarkKhaki ); + + DrawTransform( m_scene, b3Transform_identity, 1.0f ); + + float translationError1 = b3Joint_GetLinearSeparation( m_jointId1 ); + m_translationError1 = b3MaxFloat( m_translationError1, translationError1 ); + DrawTextLine( "translation error 1 = %g", m_translationError1 ); + + if ( B3_IS_NON_NULL( m_jointId2 ) ) + { + float translationError2 = b3Joint_GetLinearSeparation( m_jointId2 ); + m_translationError2 = b3MaxFloat( m_translationError2, translationError2 ); + DrawTextLine( "translation error 2 = %g", m_translationError2 ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new Door( context ); + } + + b3BodyId m_groundId; + b3BodyId m_doorId; + b3JointId m_jointId1; + b3JointId m_jointId2; + float m_magnitude; + float m_translationError1; + float m_translationError2; + float m_constraintHertz; + float m_constraintDampingRatio; + bool m_enableLimit; + bool m_twoJoints; +}; + +static int sampleDoor = SampleManager::Register( "Joints", "Door", Door::Create ); + +// A suspension bridge +class Bridge : public Sample +{ +public: + explicit Bridge( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 20.0f, 35.0, { 0.0f, 10.0f, 0.0f } ); + } + + b3BodyId groundId = b3_nullBodyId; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + groundId = b3CreateBody( m_worldId, &bodyDef ); + } + + { + float a = 0.125f; + + b3BoxHull box = b3MakeBoxHull( a, 0.125f, 0.5f ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 20.0f; + + b3SphericalJointDef jointDef = b3DefaultSphericalJointDef(); + // b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef(); + jointDef.base.constraintHertz = 1000.0f; + jointDef.enableSpring = true; + jointDef.hertz = 2.0f; + jointDef.dampingRatio = 1.0f; + + m_gravityScale = 1.0f; + + float xbase = -160.0f * a; + + b3BodyId prevBodyId = groundId; + for ( int i = 0; i < m_count; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { xbase + a * ( 1.0f + 2.0f * i ), 20.0f }; + bodyDef.linearDamping = 0.1f; + bodyDef.angularDamping = 0.1f; + m_bodyIds[i] = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( m_bodyIds[i], &shapeDef, &box.base ); + + { + b3Vec3 pivot = { xbase + 2.0f * a * i, 20.0f, -0.5f }; + jointDef.base.bodyIdA = prevBodyId; + jointDef.base.bodyIdB = m_bodyIds[i]; + jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot ); + jointDef.base.localFrameB.p = b3Body_GetLocalPoint( jointDef.base.bodyIdB, pivot ); + b3CreateSphericalJoint( m_worldId, &jointDef ); + // b3CreateRevoluteJoint( m_worldId, &jointDef ); + } + + { + b3Vec3 pivot = { xbase + 2.0f * a * i, 20.0f, 0.5f }; + jointDef.base.bodyIdA = prevBodyId; + jointDef.base.bodyIdB = m_bodyIds[i]; + jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot ); + jointDef.base.localFrameB.p = b3Body_GetLocalPoint( jointDef.base.bodyIdB, pivot ); + b3CreateSphericalJoint( m_worldId, &jointDef ); + // b3CreateRevoluteJoint( m_worldId, &jointDef ); + } + + prevBodyId = m_bodyIds[i]; + } + + { + b3Vec3 pivot = { xbase + 2.0f * a * m_count, 20.0f, -0.5f }; + jointDef.base.bodyIdA = prevBodyId; + jointDef.base.bodyIdB = groundId; + jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot ); + jointDef.base.localFrameB.p = b3Body_GetLocalPoint( jointDef.base.bodyIdB, pivot ); + b3CreateSphericalJoint( m_worldId, &jointDef ); + // b3CreateRevoluteJoint( m_worldId, &jointDef ); + } + + { + b3Vec3 pivot = { xbase + 2.0f * a * m_count, 20.0f, 0.5f }; + jointDef.base.bodyIdA = prevBodyId; + jointDef.base.bodyIdB = groundId; + jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot ); + jointDef.base.localFrameB.p = b3Body_GetLocalPoint( jointDef.base.bodyIdB, pivot ); + b3CreateSphericalJoint( m_worldId, &jointDef ); + // b3CreateRevoluteJoint( m_worldId, &jointDef ); + } + } + } + + void UpdateUI() override + { + float height = 80.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_context->camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Bridge", nullptr, ImGuiWindowFlags_NoResize ); + + // Slider takes half the window + ImGui::PushItemWidth( ImGui::GetWindowWidth() * 0.5f ); + + if ( ImGui::SliderFloat( "Gravity scale", &m_gravityScale, -1.0f, 1.0f, "%.1f" ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b3Body_SetGravityScale( m_bodyIds[i], m_gravityScale ); + } + } + + ImGui::End(); + } + + void Render() override + { + Sample::Render(); + + DrawGrid( m_scene, 20 ); + } + + static Sample* Create( SampleContext* context ) + { + return new Bridge( context ); + } + + static constexpr int m_count = 150; + b3BodyId m_bodyIds[m_count]; + float m_gravityScale; +}; + +static int sampleBridgeIndex = SampleManager::Register( "Joints", "Bridge", Bridge::Create ); + +// This test ensures joints work correctly with bodies that have motion locks +class MotionLocks : public Sample +{ +public: + explicit MotionLocks( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 30.0f, 40.0f, { 0.0f, 5.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + b3BodyId groundId; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + groundId = b3CreateBody( m_worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeTransformedBoxHull( 20.0f, 1.0f, 20.0f, { { 0.0f, -1.0f, 0.0f }, b3Quat_identity } ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + + m_motionLocks = {}; + + for ( int i = 0; i < m_capacity; ++i ) + { + m_bodyIds[i] = b3_nullBodyId; + } + + b3Vec3 position = { -12.5f, 10.0, 0.0f }; + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.enableSleep = false; + + b3BoxHull box = b3MakeBoxHull( 1.0f, 1.0f, 0.5f ); + + m_count = 0; + + float forceThreshold = 20000.0f; + float torqueThreshold = 10000.0f; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + + // distance joint + { + assert( m_count < m_capacity ); + + bodyDef.position = position; + m_bodyIds[m_count] = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( m_bodyIds[m_count], &shapeDef, &box.base ); + + float length = 2.0f; + b3Vec3 pivot1 = { position.x, position.y + 1.0f + length, 0.0f }; + b3Vec3 pivot2 = { position.x, position.y + 1.0f, 0.0f }; + b3DistanceJointDef jointDef = b3DefaultDistanceJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = m_bodyIds[m_count]; + jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot1 ); + jointDef.base.localFrameB.p = b3Body_GetLocalPoint( jointDef.base.bodyIdB, pivot2 ); + jointDef.length = length; + jointDef.base.forceThreshold = forceThreshold; + jointDef.base.torqueThreshold = torqueThreshold; + jointDef.base.collideConnected = true; + jointDef.base.userData = (void*)(intptr_t)m_count; + b3CreateDistanceJoint( m_worldId, &jointDef ); + } + + position.x += 5.0f; + ++m_count; + + // motor joint +#if 0 + { + assert( m_bodyCount < e_count ); + + bodyDef.position = position; + m_bodyIds[m_bodyCount] = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( m_bodyIds[m_bodyCount], &shapeDef, &box.base ); + + b3MotorJointDef jointDef = b3DefaultMotorJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = m_bodyIds[m_bodyCount]; + jointDef.base.localFrameA.p = position; + jointDef.maxForce = 1000.0f; + jointDef.maxTorque = 20.0f; + jointDef.base.forceThreshold = forceThreshold; + jointDef.base.torqueThreshold = torqueThreshold; + jointDef.base.collideConnected = true; + jointDef.base.userData = (void*)(intptr_t)m_bodyCount; + b3CreateMotorJoint( m_worldId, &jointDef ); + } + + position.x += 5.0f; + ++m_count; +#endif + + // prismatic joint + { + assert( m_count < m_capacity ); + + bodyDef.position = position; + m_bodyIds[m_count] = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( m_bodyIds[m_count], &shapeDef, &box.base ); + + b3Vec3 pivot = { position.x - 1.0f, position.y, 0.0f }; + b3PrismaticJointDef jointDef = b3DefaultPrismaticJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = m_bodyIds[m_count]; + jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot ); + jointDef.base.localFrameB.p = b3Body_GetLocalPoint( jointDef.base.bodyIdB, pivot ); + jointDef.base.forceThreshold = forceThreshold; + jointDef.base.torqueThreshold = torqueThreshold; + jointDef.base.collideConnected = true; + jointDef.base.userData = (void*)(intptr_t)m_count; + b3CreatePrismaticJoint( m_worldId, &jointDef ); + } + + position.x += 5.0f; + ++m_count; + + // revolute joint + { + assert( m_count < m_capacity ); + + bodyDef.position = position; + m_bodyIds[m_count] = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( m_bodyIds[m_count], &shapeDef, &box.base ); + + b3Vec3 pivot = { position.x - 1.0f, position.y, 0.0f }; + b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = m_bodyIds[m_count]; + jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot ); + jointDef.base.localFrameB.p = b3Body_GetLocalPoint( jointDef.base.bodyIdB, pivot ); + jointDef.base.forceThreshold = forceThreshold; + jointDef.base.torqueThreshold = torqueThreshold; + jointDef.base.collideConnected = true; + jointDef.base.userData = (void*)(intptr_t)m_count; + b3CreateRevoluteJoint( m_worldId, &jointDef ); + } + + position.x += 5.0f; + ++m_count; + + // weld joint + { + assert( m_count < m_capacity ); + + bodyDef.position = position; + m_bodyIds[m_count] = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( m_bodyIds[m_count], &shapeDef, &box.base ); + + b3Vec3 pivot = { position.x - 1.0f, position.y, 0.0f }; + b3WeldJointDef jointDef = b3DefaultWeldJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = m_bodyIds[m_count]; + jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot ); + jointDef.base.localFrameB.p = b3Body_GetLocalPoint( jointDef.base.bodyIdB, pivot ); + jointDef.angularHertz = 2.0f; + jointDef.angularDampingRatio = 0.5f; + jointDef.base.forceThreshold = forceThreshold; + jointDef.base.torqueThreshold = torqueThreshold; + jointDef.base.collideConnected = true; + jointDef.base.userData = (void*)(intptr_t)m_count; + b3CreateWeldJoint( m_worldId, &jointDef ); + } + + position.x += 5.0f; + ++m_count; + + // wheel joint +#if 0 + { + assert( index < e_count ); + + bodyDef.position = position; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + b3Vec3 pivot = { position.x - 1.0f, position.y, 0.0f }; + b3WheelJointDef jointDef = b3DefaultWheelJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = bodyId; + jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot ); + jointDef.base.localFrameB.p = b3Body_GetLocalPoint( jointDef.base.bodyIdB, pivot ); + jointDef.hertz = 1.0f; + jointDef.dampingRatio = 0.7f; + jointDef.lowerTranslation = -1.0f; + jointDef.upperTranslation = 1.0f; + jointDef.enableLimit = true; + jointDef.enableMotor = true; + jointDef.maxMotorTorque = 10.0f; + jointDef.motorSpeed = 1.0f; + jointDef.base.forceThreshold = forceThreshold; + jointDef.base.torqueThreshold = torqueThreshold; + jointDef.base.collideConnected = true; + jointDef.base.userData = (void*)(intptr_t)index; + b3CreateWheelJoint( m_worldId, &jointDef ); + } + + position.x += 5.0f; + ++m_count; +#endif + } + + void UpdateUI() override + { + float fontSize = ImGui::GetFontSize(); + float height = 13.0f * fontSize; + ImGui::SetNextWindowPos( ImVec2( 0.5f * fontSize, m_camera->m_height - height - 2.0f * fontSize ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 10.0f * fontSize, height ) ); + + ImGui::Begin( "Motion Locks", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::Checkbox( "Lock Linear X", &m_motionLocks.linearX ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b3Body_SetMotionLocks( m_bodyIds[i], m_motionLocks ); + b3Body_SetAwake( m_bodyIds[i], true ); + } + } + + if ( ImGui::Checkbox( "Lock Linear Y", &m_motionLocks.linearY ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b3Body_SetMotionLocks( m_bodyIds[i], m_motionLocks ); + b3Body_SetAwake( m_bodyIds[i], true ); + } + } + + if ( ImGui::Checkbox( "Lock Linear Z", &m_motionLocks.linearZ ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b3Body_SetMotionLocks( m_bodyIds[i], m_motionLocks ); + b3Body_SetAwake( m_bodyIds[i], true ); + } + } + + if ( ImGui::Checkbox( "Lock Angular X", &m_motionLocks.angularX ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b3Body_SetMotionLocks( m_bodyIds[i], m_motionLocks ); + b3Body_SetAwake( m_bodyIds[i], true ); + } + } + + if ( ImGui::Checkbox( "Lock Angular Y", &m_motionLocks.angularY ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b3Body_SetMotionLocks( m_bodyIds[i], m_motionLocks ); + b3Body_SetAwake( m_bodyIds[i], true ); + } + } + + if ( ImGui::Checkbox( "Lock Angular Z", &m_motionLocks.angularZ ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b3Body_SetMotionLocks( m_bodyIds[i], m_motionLocks ); + b3Body_SetAwake( m_bodyIds[i], true ); + } + } + + ImGui::End(); + + if ( glfwGetKey( m_context->window, GLFW_KEY_L ) == GLFW_PRESS ) + { + b3Body_ApplyLinearImpulseToCenter( m_bodyIds[0], { 100.0f, 0.0f }, true ); + } + } + + void Render() override + { + Sample::Render(); + + b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 1.0f ); + } + + static Sample* Create( SampleContext* context ) + { + return new MotionLocks( context ); + } + + static constexpr int m_capacity = 6; + b3BodyId m_bodyIds[m_capacity]; + int m_count; + b3MotionLocks m_motionLocks; +}; + +static int sampleMotionLocks = SampleManager::Register( "Joints", "Motion Locks", MotionLocks::Create ); + +class Driving : public Sample +{ +public: + explicit Driving( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 25.0f, 20.0f, 7.0f, { 0.0f, 2.0f, 0.0f } ); + } + + m_spinSpeed = 30.0f; + m_maxSpinTorque = 5.0f; + m_suspensionHertz = 4.0f; + m_suspensionDampingRatio = 0.7f; + m_lowerTranslation = -0.2f; + m_upperTranslation = 0.2f; + m_steeringHertz = 10.0f; + m_steeringDampingRatio = 0.7f; + m_lowerSteeringDegrees = -45.0f; + m_upperSteeringDegrees = 45.0f; + m_maxSteeringTorque = 5.0f; + m_targetSteeringDegrees = 0.0f; + + b3BodyId groundId; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + // bodyDef.position = { 0.0f, -1.0f, 0.0f }; + bodyDef.position = { -20.0f, 0.0f, -20.0f }; + groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + // b3BoxHull groundBox = b3MakeBoxHull( 200.0f, 1.0f, 200.0f ); + // b3CreateHullShape( groundId, &shapeDef, &groundBox.base ); + + m_heightField = b3CreateWave( 50.0f, 50.0f, { 4.0f, 2.0f, 4.0f }, 0.02f, 0.04f, false ); + // b3ShapeDef shapeDef = b3DefaultShapeDef(); + // b3SurfaceMaterial materials[3] = b3DefaultSurfaceMaterial(); + // materials[0] = { .friction = 0.6f, .restitution = 0.0f, 0 }; + // materials[1] = { .friction = 0.6f, .restitution = 1.0f, 1 }; + // materials[2] = { .friction = 0.6f, .restitution = 0.0f, 2 }; + // shapeDef.materials = materials; + // shapeDef.materialCount = 3; + + b3CreateHeightFieldShape( groundId, &shapeDef, m_heightField ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + { + bodyDef.position = { 0.0f, 2.5f, 0.0f }; + bodyDef.type = b3_dynamicBody; + m_chassisId = b3CreateBody( m_worldId, &bodyDef ); + + shapeDef.density = 0.5f; + b3BoxHull box = b3MakeBoxHull( 2.0f, 0.5f, 1.0f ); + b3CreateHullShape( m_chassisId, &shapeDef, &box.base ); + } + + // Keep vehicle upright + { + b3ParallelJointDef parallelJointDef = b3DefaultParallelJointDef(); + parallelJointDef.base.bodyIdA = groundId; + parallelJointDef.base.bodyIdB = m_chassisId; + parallelJointDef.base.localFrameA.q = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisZ, b3Vec3_axisY ); + parallelJointDef.base.localFrameB.q = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisZ, b3Vec3_axisY ); + parallelJointDef.base.drawScale = 2.0f; + parallelJointDef.base.collideConnected = true; + parallelJointDef.hertz = 0.5f; + parallelJointDef.dampingRatio = 1.0f; + b3CreateParallelJoint( m_worldId, ¶llelJointDef ); + } + + shapeDef.density = 2.0f; + shapeDef.baseMaterial.friction = 3.0f; + + bodyDef.type = b3_dynamicBody; + bodyDef.allowFastRotation = true; + bodyDef.enableContinuousCollision = false; + bodyDef.rotation = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisY, b3Vec3_axisZ ); + + // b3Hull* hull = b3CreateCylinder( 0.25f, 0.4f, 0.0f, 16 ); + + b3WheelJointDef jointDef = b3DefaultWheelJointDef(); + jointDef.base.bodyIdA = m_chassisId; + jointDef.base.localFrameA.q = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisX, b3Vec3_axisY ); + jointDef.base.localFrameB.q = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisZ, b3Vec3_axisY ); + jointDef.enableSuspensionLimit = true; + jointDef.lowerSuspensionLimit = m_lowerTranslation; + jointDef.upperSuspensionLimit = m_upperTranslation; + jointDef.enableSuspensionSpring = true; + jointDef.suspensionHertz = m_suspensionHertz; + jointDef.suspensionDampingRatio = m_suspensionDampingRatio; + jointDef.enableSpinMotor = true; + jointDef.maxSpinTorque = m_maxSpinTorque; + jointDef.enableSteering = true; + jointDef.steeringHertz = m_steeringHertz; + jointDef.steeringDampingRatio = m_steeringDampingRatio; + jointDef.targetSteeringAngle = 0.0f; + jointDef.maxSteeringTorque = m_maxSteeringTorque; + jointDef.enableSteeringLimit = true; + jointDef.lowerSteeringLimit = B3_PI / 180.0f * m_lowerSteeringDegrees; + jointDef.upperSteeringLimit = B3_PI / 180.0f * m_upperSteeringDegrees; + + b3Sphere sphere = { b3Vec3_zero, 0.4f }; + + { + bodyDef.position = { 1.5f, 2.0f, 0.8f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + // b3CreateHullShape( bodyId, &shapeDef, hull ); + + jointDef.base.bodyIdB = bodyId; + jointDef.base.localFrameA.p = { 1.5f, -0.5f, 0.8f }; + jointDef.enableSteering = true; + jointDef.enableSpinMotor = false; + m_frontLeftId = b3CreateWheelJoint( m_worldId, &jointDef ); + } + + { + bodyDef.position = { 1.5f, 2.0f, -0.8f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + // b3CreateHullShape( bodyId, &shapeDef, hull ); + + jointDef.base.bodyIdB = bodyId; + jointDef.base.localFrameA.p = { 1.5f, -0.5f, -0.8f }; + jointDef.enableSteering = true; + jointDef.enableSpinMotor = false; + m_frontRightId = b3CreateWheelJoint( m_worldId, &jointDef ); + } + + { + bodyDef.position = { -1.5f, 2.0f, 0.8f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + // b3CreateHullShape( bodyId, &shapeDef, hull ); + + jointDef.base.bodyIdB = bodyId; + jointDef.base.localFrameA.p = { -1.5f, -0.5f, 0.8f }; + jointDef.enableSteering = false; + jointDef.enableSpinMotor = true; + m_rearLeftId = b3CreateWheelJoint( m_worldId, &jointDef ); + } + + { + bodyDef.position = { -1.5f, 2.0f, -0.8f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + // b3CreateHullShape( bodyId, &shapeDef, hull ); + + jointDef.base.bodyIdB = bodyId; + jointDef.base.localFrameA.p = { -1.5f, -0.5f, -0.8f }; + jointDef.enableSteering = false; + jointDef.enableSpinMotor = true; + m_rearRightId = b3CreateWheelJoint( m_worldId, &jointDef ); + } + + // b3DestroyHull( hull ); + + m_camera->m_thirdPerson = true; + glfwSetInputMode( m_window, GLFW_CURSOR, GLFW_CURSOR_DISABLED ); + + m_haveMouseLast = false; + m_mouseLast = { 0.0f, 0.0f }; + m_mouseDelta = { 0.0f, 0.0f }; + } + + ~Driving() override + { + m_camera->m_thirdPerson = false; + glfwSetInputMode( m_window, GLFW_CURSOR, GLFW_CURSOR_NORMAL ); + b3DestroyHeightField( m_heightField ); + } + +#if 0 + void MouseMove( b3Vec2 p ) override + { + if ( m_camera->m_thirdPerson ) + { + if ( m_haveMouseLast == false ) + { + m_mouseLast = p; + m_haveMouseLast = true; + } + else + { + m_mouseDelta = { p.x - m_mouseLast.x, p.y - m_mouseLast.y }; + m_mouseLast = p; + } + + const float sensitivity = 0.05f; + m_camera->m_yaw -= 2.0f * sensitivity * m_mouseDelta.x; + m_camera->m_pitch += sensitivity * m_mouseDelta.y; + m_camera->m_pitch = b3ClampFloat( m_camera->m_pitch, -85.0f, 85.0f ); + } + + Sample::MouseMove( p ); + } +#endif + + void Keyboard( int key, int action, int mods ) override + { + if ( key == GLFW_KEY_T && action == GLFW_PRESS ) + { + ToggleThirdPerson(); + } + } + + void UpdateUI() override + { + float fontSize = ImGui::GetFontSize(); + float height = 22.0f * fontSize; + ImGui::SetNextWindowPos( { 1.0f * fontSize, m_camera->m_height - height - 3.0f * fontSize }, ImGuiCond_Once ); + ImGui::SetNextWindowSize( { 25.0f * fontSize, height } ); + + ImGui::Begin( "Driving", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::SliderFloat( "Lower Translation", &m_lowerTranslation, -10.0f, 10.0f, "%.1f" ) ) + { + m_lowerTranslation = b3MinFloat( m_lowerTranslation, m_upperTranslation ); + b3WheelJoint_SetSuspensionLimits( m_frontLeftId, m_lowerTranslation, m_upperTranslation ); + b3WheelJoint_SetSuspensionLimits( m_frontRightId, m_lowerTranslation, m_upperTranslation ); + b3WheelJoint_SetSuspensionLimits( m_rearLeftId, m_lowerTranslation, m_upperTranslation ); + b3WheelJoint_SetSuspensionLimits( m_rearRightId, m_lowerTranslation, m_upperTranslation ); + } + + if ( ImGui::SliderFloat( "Upper Translation", &m_upperTranslation, -10.0f, 10.0f, "%.1f" ) ) + { + m_upperTranslation = b3MaxFloat( m_upperTranslation, m_lowerTranslation ); + b3WheelJoint_SetSuspensionLimits( m_frontLeftId, m_lowerTranslation, m_upperTranslation ); + b3WheelJoint_SetSuspensionLimits( m_frontRightId, m_lowerTranslation, m_upperTranslation ); + b3WheelJoint_SetSuspensionLimits( m_rearLeftId, m_lowerTranslation, m_upperTranslation ); + b3WheelJoint_SetSuspensionLimits( m_rearRightId, m_lowerTranslation, m_upperTranslation ); + } + + ImGui::SliderFloat( "Max Torque", &m_maxSpinTorque, 0.0f, 100.0f, "%.0f" ); + ImGui::SliderFloat( "Speed", &m_spinSpeed, 0.0f, 100.0f, "%.0f" ); + + if ( ImGui::SliderFloat( "Suspension Hertz", &m_suspensionHertz, 0.0f, 10.0f, "%.1f" ) ) + { + b3WheelJoint_SetSuspensionHertz( m_frontLeftId, m_suspensionHertz ); + b3WheelJoint_SetSuspensionHertz( m_frontRightId, m_suspensionHertz ); + b3WheelJoint_SetSuspensionHertz( m_rearLeftId, m_suspensionHertz ); + b3WheelJoint_SetSuspensionHertz( m_rearRightId, m_suspensionHertz ); + } + + if ( ImGui::SliderFloat( "Suspension Damping", &m_suspensionDampingRatio, 0.0f, 2.0f, "%.1f" ) ) + { + b3WheelJoint_SetSuspensionDampingRatio( m_frontLeftId, m_suspensionDampingRatio ); + b3WheelJoint_SetSuspensionDampingRatio( m_frontRightId, m_suspensionDampingRatio ); + b3WheelJoint_SetSuspensionDampingRatio( m_rearLeftId, m_suspensionDampingRatio ); + b3WheelJoint_SetSuspensionDampingRatio( m_rearRightId, m_suspensionDampingRatio ); + } + + if ( ImGui::SliderFloat( "Steering Hertz", &m_steeringHertz, 0.0f, 10.0f, "%.1f" ) ) + { + b3WheelJoint_SetSteeringHertz( m_frontLeftId, m_steeringHertz ); + b3WheelJoint_SetSteeringHertz( m_frontRightId, m_steeringHertz ); + } + + if ( ImGui::SliderFloat( "Steering Damping", &m_steeringDampingRatio, 0.0f, 2.0f, "%.1f" ) ) + { + b3WheelJoint_SetSteeringDampingRatio( m_frontLeftId, m_steeringDampingRatio ); + b3WheelJoint_SetSteeringDampingRatio( m_frontRightId, m_steeringDampingRatio ); + } + + if ( ImGui::SliderFloat( "Steering Torque", &m_maxSteeringTorque, 0.0f, 20.0f, "%.1f" ) ) + { + b3WheelJoint_SetMaxSteeringTorque( m_frontLeftId, m_maxSteeringTorque ); + b3WheelJoint_SetMaxSteeringTorque( m_frontRightId, m_maxSteeringTorque ); + } + + if ( ImGui::SliderFloat( "Lower Degrees", &m_lowerSteeringDegrees, -90.0f, 0.0f, "%.0f" ) ) + { + b3WheelJoint_SetSteeringLimits( m_frontLeftId, B3_PI / 180.0f * m_lowerSteeringDegrees, + B3_PI / 180.0f * m_upperSteeringDegrees ); + b3WheelJoint_SetSteeringLimits( m_frontRightId, B3_PI / 180.0f * m_lowerSteeringDegrees, + B3_PI / 180.0f * m_upperSteeringDegrees ); + } + + if ( ImGui::SliderFloat( "Upper Degrees", &m_upperSteeringDegrees, 0.0f, 90.0f, "%.0f" ) ) + { + b3WheelJoint_SetSteeringLimits( m_frontLeftId, B3_PI / 180.0f * m_lowerSteeringDegrees, + B3_PI / 180.0f * m_upperSteeringDegrees ); + b3WheelJoint_SetSteeringLimits( m_frontRightId, B3_PI / 180.0f * m_lowerSteeringDegrees, + B3_PI / 180.0f * m_upperSteeringDegrees ); + } + + bool thirdPerson = m_camera->m_thirdPerson; + if ( ImGui::Checkbox( "Third Person (Key: T)", &thirdPerson ) ) + { + ToggleThirdPerson(); + } + + ImGui::End(); + } + + void Render() override + { + Sample::Render(); + + b3Vec3 velocity = b3Body_GetLinearVelocity( m_chassisId ); + b3Quat quat = b3Body_GetRotation( m_chassisId ); + b3Vec3 forward = b3RotateVector( quat, { -1.0f, 0.0f, 0.0f } ); + float speed = b3Dot( velocity, forward ); + DrawTextLine( "speed = %.1f", speed ); + + float leftSpeed = b3WheelJoint_GetSpinSpeed( m_rearLeftId ); + float rightSpeed = b3WheelJoint_GetSpinSpeed( m_rearRightId ); + DrawTextLine( "spin speed = %.1f/%.1f", leftSpeed, rightSpeed ); + + float leftSpinTorque = b3WheelJoint_GetSpinTorque( m_rearLeftId ); + float rightSpinTorque = b3WheelJoint_GetSpinTorque( m_rearRightId ); + DrawTextLine( "spin torque = %.1f/%.1f", leftSpinTorque, rightSpinTorque ); + + float angleLeft = b3WheelJoint_GetSteeringAngle( m_frontLeftId ); + float angleRight = b3WheelJoint_GetSteeringAngle( m_frontRightId ); + DrawTextLine( "steering degrees = %.1f/%.1f", 180.0f / B3_PI * angleLeft, 180.0f / B3_PI * angleRight ); + + float leftSteerTorque = b3WheelJoint_GetSteeringTorque( m_frontLeftId ); + float rightSteerTorque = b3WheelJoint_GetSteeringTorque( m_frontRightId ); + DrawTextLine( "steering torque = %.1f/%.1f", leftSteerTorque, rightSteerTorque ); + + b3Transform transform = b3Transform_identity; + transform.p.y += 0.05f; + DrawTransform( m_scene, transform, 2.0f ); + } + + void Step() override + { + b3Vec2 throttle = { 0.0f, 0.0f }; + + if ( m_camera->m_thirdPerson ) + { + if ( glfwGetKey( m_window, GLFW_KEY_W ) ) + { + throttle.x += 1.0f; + b3Body_SetAwake( m_chassisId, true ); + } + + if ( glfwGetKey( m_window, GLFW_KEY_S ) ) + { + throttle.x -= 1.0f; + b3Body_SetAwake( m_chassisId, true ); + } + + if ( glfwGetKey( m_window, GLFW_KEY_A ) ) + { + throttle.y += 1.0f; + b3Body_SetAwake( m_chassisId, true ); + } + + if ( glfwGetKey( m_window, GLFW_KEY_D ) ) + { + throttle.y -= 1.0f; + b3Body_SetAwake( m_chassisId, true ); + } + } + + float maxSteeringAngle = 0.25f * B3_PI; + b3WheelJoint_SetTargetSteeringAngle( m_frontLeftId, maxSteeringAngle * throttle.y ); + b3WheelJoint_SetTargetSteeringAngle( m_frontRightId, maxSteeringAngle * throttle.y ); + + b3WheelJoint_SetSpinMotorSpeed( m_rearLeftId, -m_spinSpeed * throttle.x ); + b3WheelJoint_SetSpinMotorSpeed( m_rearRightId, -m_spinSpeed * throttle.x ); + + if ( m_camera->m_thirdPerson ) + { + b3Transform transform = b3Body_GetTransform( m_chassisId ); + m_camera->m_pivot = transform.p; + m_camera->UpdateTransform(); + } + + Sample::Step(); + } + + static Sample* Create( SampleContext* context ) + { + return new Driving( context ); + } + + b3HeightField* m_heightField; + b3BodyId m_chassisId; + b3JointId m_frontLeftId; + b3JointId m_frontRightId; + b3JointId m_rearLeftId; + b3JointId m_rearRightId; + + float m_spinSpeed; + float m_maxSpinTorque; + float m_suspensionHertz; + float m_suspensionDampingRatio; + float m_lowerTranslation; + float m_upperTranslation; + float m_steeringHertz; + float m_steeringDampingRatio; + float m_lowerSteeringDegrees; + float m_upperSteeringDegrees; + float m_maxSteeringTorque; + float m_targetSteeringDegrees; + + b3Vec2 m_mouseLast; + b3Vec2 m_mouseDelta; + bool m_haveMouseLast; +}; + +static int sampleDriving = SampleManager::Register( "Joints", "Driving", Driving::Create ); diff --git a/samples/sample_manifold.cpp b/samples/sample_manifold.cpp new file mode 100644 index 0000000..94b934f --- /dev/null +++ b/samples/sample_manifold.cpp @@ -0,0 +1,940 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "GLFW/glfw3.h" +#include "camera.h" +#include "renderer.h" +#include "sample.h" +#include "scene.h" + +#include "box3d/constants.h" + +#include + +class Manifold : public Sample +{ +public: + explicit Manifold( SampleContext* sampleContext ) + : Sample( sampleContext ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 35.0f, 30.0f, 50.0f, { 0.0f, 5.0f, 0.0f } ); + } + + memset( m_points, 0, sizeof( m_points ) ); + m_manifold = {}; + m_manifold.points = m_points; + + { + m_transformA.p = { 3.5f, 0.5f, 0.0f }; + m_transformA.q = b3MakeQuatFromAxisAngle( { 0.0f, 1.0f, 0.0f }, 0.5f * B3_PI ); + } + + { + m_transformB.p = { 0.0f, 1.5f, 3.5f }; + m_transformB.q = b3Quat_identity; + } + + m_simplexCache = {}; + m_satCache = {}; + m_manualFeature = 0; + m_baseTranslation = b3Vec3_zero; + m_baseQuaternion = b3Quat_identity; + m_baseX = 0; + m_baseY = 0; + m_origin = b3Vec3_zero; + m_useCache = false; + m_tracking = false; + m_rotating = false; + } + + void Render() override + { + DrawTextLine( "origin: %g %g %g", m_origin.x, m_origin.y, m_origin.z ); + DrawTextLine( "count = %d", m_manifold.pointCount ); + + DrawTransform( m_scene, b3Transform_identity, 1.0f ); + + if ( m_manifold.pointCount == 0 ) + { + return; + } + + float length = 0.5f * b3GetLengthUnitsPerMeter(); + b3Vec3 normal = b3RotateVector( m_transformA.q, m_manifold.normal ); + + for ( int pointIndex = 0; pointIndex < m_manifold.pointCount; ++pointIndex ) + { + const b3LocalManifoldPoint& manifoldPoint = m_manifold.points[pointIndex]; + + b3Vec3 point = b3TransformPoint( m_transformA, manifoldPoint.point ); + + DrawLine( m_scene, point, point + length * normal, b3_colorWhite ); + + if ( manifoldPoint.separation > 0.0f ) + { + DrawPoint( m_scene, point, 10.0f, b3_colorWhite ); + } + else + { + DrawPoint( m_scene, point, 10.0f, b3_colorYellow ); + } + + DrawWorldString( m_camera, point, b3_colorWhite, " %.3f", manifoldPoint.separation ); + + b3Vec3 perp = b3Perp( normal ); + b3FeaturePair pair = manifoldPoint.pair; + DrawWorldString( m_camera, point + 0.025f * normal + 0.05f * perp, b3_colorPapayaWhip, " %X:%X %X:%X", pair.owner1, + pair.index1, pair.owner2, pair.index2 ); + } + + Sample::Render(); + } + + void UpdateUI() override + { + float fontSize = ImGui::GetFontSize(); + float height = 12.0f * fontSize; + ImGui::SetNextWindowPos( ImVec2( 0.5f * fontSize, m_camera->m_height - height - 2.0f * fontSize ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 10.0f * fontSize, height ) ); + ImGui::Begin( "Manifold", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + ImGui::Checkbox( "Use cache", &m_useCache ); + if ( m_useCache ) + { + ImGui::RadioButton( "auto", &m_manualFeature, 0 ); + ImGui::RadioButton( "faceA", &m_manualFeature, 1 ); + ImGui::RadioButton( "faceB", &m_manualFeature, 2 ); + ImGui::RadioButton( "edgePair", &m_manualFeature, 3 ); + } + + ImGui::End(); + } + + void MouseDown( b3Vec2 p, int button, int modifiers ) override + { + if ( button == 0 && ( modifiers & GLFW_MOD_ALT ) == 0 ) + { + if ( modifiers & GLFW_MOD_SHIFT ) + { + m_baseX = p.x; + m_baseY = p.y; + m_baseQuaternion = m_transformB.q; + m_rotating = true; + } + else + { + PickRay pickRay = m_camera->BuildPickRay( p.x, p.y ); + m_origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation ); + m_baseTranslation = m_transformB.p; + m_tracking = true; + } + } + } + + void MouseUp( b3Vec2 p, int button ) override + { + m_tracking = false; + m_rotating = false; + } + + void MouseMove( b3Vec2 p ) override + { + if ( m_tracking ) + { + PickRay pickRay = m_camera->BuildPickRay( p.x, p.y ); + b3Vec3 origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation ); + m_transformB.p = m_baseTranslation + origin - m_origin; + } + + if ( m_rotating ) + { + int x = p.x; + int y = p.y; + + b3Quat qx = b3MakeQuatFromAxisAngle( b3Vec3_axisY, 0.01f * ( x - m_baseX ) ); + b3Quat qz = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.01f * ( y - m_baseY ) ); + m_transformB.q = b3NormalizeQuat( b3MulQuat( m_baseQuaternion, b3MulQuat( qx, qz ) ) ); + } + } + + static constexpr int m_pointCapacity = 64; + b3LocalManifold m_manifold; + b3LocalManifoldPoint m_points[m_pointCapacity]; + b3Transform m_transformA; + b3Transform m_transformB; + b3Vec3 m_baseTranslation; + b3Quat m_baseQuaternion; + b3Vec3 m_origin; + b3SimplexCache m_simplexCache; + b3SATCache m_satCache; + int m_manualFeature; + int m_baseX; + int m_baseY; + bool m_useCache; + bool m_tracking; + bool m_rotating; +}; + +class TriangleManifold : public Sample +{ +public: + explicit TriangleManifold( SampleContext* sampleContext ) + : Sample( sampleContext ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 35.0f, 30.0f, 50.0f, { 0.0f, 5.0f, 0.0f } ); + } + + m_manifold = {}; + memset( m_points, 0, sizeof( m_points ) ); + m_manifold.points = m_points; + + { + m_transformA.p = { 3.5f, 0.5f, 0.0f }; + m_transformA.q = b3MakeQuatFromAxisAngle( { 0.0f, 1.0f, 0.0f }, 0.5f * B3_PI ); + } + + { + m_transformB.p = { 0.0f, 1.5f, 3.5f }; + m_transformB.q = b3Quat_identity; + } + + m_simplexCache = {}; + m_satCache = {}; + m_baseTranslation = b3Vec3_zero; + m_baseQuaternion = b3Quat_identity; + m_baseX = 0; + m_baseY = 0; + m_origin = b3Vec3_zero; + m_useCache = false; + m_tracking = false; + m_rotating = false; + + m_manualFeature = 0; + } + + void Render() override + { + DrawTextLine( "origin: %g %g %g", m_origin.x, m_origin.y, m_origin.z ); + DrawTextLine( "count = %d", m_manifold.pointCount ); + DrawTextLine( "feature = %d", m_manifold.feature ); + DrawTextLine( "cache hit = %d", m_satCache.hit ); + + DrawTransform( m_scene, b3Transform_identity, 1.0f ); + + if ( m_manifold.pointCount > 0 ) + { + float length = 0.5f * b3GetLengthUnitsPerMeter(); + + for ( int pointIndex = 0; pointIndex < m_manifold.pointCount; ++pointIndex ) + { + const b3LocalManifoldPoint& manifoldPoint = m_manifold.points[pointIndex]; + + b3Vec3 point = manifoldPoint.point; + + DrawLine( m_scene, point, point + length * m_manifold.normal, b3_colorWhite ); + + if ( manifoldPoint.separation > 0.0f ) + { + DrawPoint( m_scene, point, 10.0f, b3_colorWhite ); + } + else + { + DrawPoint( m_scene, point, 10.0f, b3_colorYellow ); + } + + DrawWorldString( m_camera, point, b3_colorWhite, " %.2f", 100.0f * manifoldPoint.separation ); + + b3FeaturePair pair = manifoldPoint.pair; + DrawWorldString( m_camera, point + 0.025f * m_manifold.normal, b3_colorPapayaWhip, " %X:%X %X:%X", pair.owner1, + pair.index1, pair.owner2, pair.index2 ); + } + } + + b3Vec3 p1 = b3TransformPoint( m_transformA, m_triangle[0] ); + b3Vec3 p2 = b3TransformPoint( m_transformA, m_triangle[1] ); + b3Vec3 p3 = b3TransformPoint( m_transformA, m_triangle[2] ); + DrawFace( m_scene, p1, p2, p3, b3_colorCyan ); + + DrawWorldString( m_camera, p1, b3_colorWhite, "0" ); + DrawWorldString( m_camera, p2, b3_colorWhite, "1" ); + DrawWorldString( m_camera, p3, b3_colorWhite, "2" ); + + b3Vec3 center = 1.0f / 3.0f * ( p1 + p2 + p3 ); + b3Vec3 normal = b3Normalize( b3Cross( p2 - p1, p3 - p1 ) ); + DrawArrow( m_scene, center, center + 0.5f * normal, 0.05f, b3_colorMediumPurple ); + + Sample::Render(); + } + + void UpdateUI() override + { + float fontSize = ImGui::GetFontSize(); + float height = 12.0f * fontSize; + ImGui::SetNextWindowPos( ImVec2( 0.5f * fontSize, m_camera->m_height - height - 2.0f * fontSize ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 10.0f * fontSize, height ) ); + + ImGui::Begin( "Triangle Manifold", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + ImGui::Checkbox( "Use cache", &m_useCache ); + if ( m_useCache ) + { + ImGui::RadioButton( "auto", &m_manualFeature, 0 ); + ImGui::RadioButton( "faceA", &m_manualFeature, 1 ); + ImGui::RadioButton( "faceB", &m_manualFeature, 2 ); + ImGui::RadioButton( "edgePair", &m_manualFeature, 3 ); + } + + ImGui::End(); + } + + void MouseDown( b3Vec2 p, int button, int modifiers ) override + { + if ( button == 0 && ( modifiers & GLFW_MOD_ALT ) == 0 ) + { + if ( modifiers & GLFW_MOD_SHIFT ) + { + m_baseX = p.x; + m_baseY = p.y; + m_baseQuaternion = m_transformB.q; + m_rotating = true; + } + else + { + PickRay pickRay = m_camera->BuildPickRay( p.x, p.y ); + m_origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation ); + m_baseTranslation = m_transformB.p; + m_tracking = true; + } + } + } + + void MouseUp( b3Vec2 p, int button ) override + { + m_tracking = false; + m_rotating = false; + } + + void MouseMove( b3Vec2 p ) override + { + if ( m_tracking ) + { + PickRay pickRay = m_camera->BuildPickRay( p.x, p.y ); + b3Vec3 origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation ); + m_transformB.p = m_baseTranslation + origin - m_origin; + } + + if ( m_rotating ) + { + int x = p.x; + int y = p.y; + + b3Quat qx = b3MakeQuatFromAxisAngle( b3Vec3_axisY, 0.01f * ( x - m_baseX ) ); + b3Quat qz = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.01f * ( y - m_baseY ) ); + m_transformB.q = b3NormalizeQuat( b3MulQuat( m_baseQuaternion, b3MulQuat( qx, qz ) ) ); + } + } + + static constexpr int m_pointCapacity = 8; + b3LocalManifold m_manifold; + b3LocalManifoldPoint m_points[m_pointCapacity]; + b3Transform m_transformA; + b3Transform m_transformB; + b3Vec3 m_triangle[3] = {}; + b3Vec3 m_baseTranslation; + b3Quat m_baseQuaternion; + b3Vec3 m_origin; + b3SimplexCache m_simplexCache; + b3SATCache m_satCache; + int m_manualFeature; + int m_baseX; + int m_baseY; + bool m_useCache; + bool m_tracking; + bool m_rotating; +}; + +class SphereAndSphere : public Manifold +{ +public: + explicit SphereAndSphere( SampleContext* sampleContext ) + : Manifold( sampleContext ) + { + m_sphere = { { 0.5f, 0.0f, -0.25f }, 2.0f }; + } + + void Render() override + { + DrawSphere( m_scene, m_transformA, m_sphere, b3_colorGreen ); + DrawSphere( m_scene, m_transformB, m_sphere, b3_colorCyan ); + + Manifold::Render(); + } + + void Step() override + { + b3Transform transformBtoA = b3InvMulTransforms( m_transformA, m_transformB ); + b3CollideSpheres( &m_manifold, m_pointCapacity, &m_sphere, &m_sphere, transformBtoA ); + } + + static Sample* Create( SampleContext* sampleContext ) + { + return new SphereAndSphere( sampleContext ); + } + + b3Sphere m_sphere; +}; + +static int sampleCollideSpheres = SampleManager::Register( "Manifold", "Sphere vs Sphere", SphereAndSphere::Create ); + +class CapsuleAndSphere : public Manifold +{ +public: + static Sample* Create( SampleContext* sampleContext ) + { + return new CapsuleAndSphere( sampleContext ); + } + + explicit CapsuleAndSphere( SampleContext* sampleContext ) + : Manifold( sampleContext ) + { + m_capsule = { { -2.0f, 0.0f, 0.0f }, { 2.0f, 0.0f, 0.0f }, 1.0f }; + m_sphere = { { 0.0f, 0.0f, 0.0f }, 2.0f }; + + m_transformA = { { 0.0f, 0.0f, 0.0f }, b3Quat_identity }; + m_transformB = { { -4.0f, 0.0f, 0.0f }, b3Quat_identity }; + } + + void Render() override + { + DrawCapsule( m_scene, m_transformA, m_capsule, b3_colorCyan ); + DrawSphere( m_scene, m_transformB, m_sphere, b3_colorGreen ); + + Manifold::Render(); + } + + void Step() override + { + b3Transform transformBtoA = b3InvMulTransforms( m_transformA, m_transformB ); + b3CollideCapsuleAndSphere( &m_manifold, m_pointCapacity, &m_capsule, &m_sphere, transformBtoA ); + } + + b3Sphere m_sphere; + b3Capsule m_capsule; +}; + +static int sampleSphereAndCapsule = SampleManager::Register( "Manifold", "Capsule vs Sphere", CapsuleAndSphere::Create ); + +class HullAndSphere : public Manifold +{ +public: + explicit HullAndSphere( SampleContext* sampleContext ) + : Manifold( sampleContext ) + { + m_sphere = { { 0.0f, 0.0f, 0.0f }, 1.0f }; + m_hull = b3MakeBoxHull( 2.0f, 0.5f, 0.5f ); + + m_transformA = { { 0.0f, 0.0f, 0.0f }, b3Quat_identity }; + m_transformB = { { 1.5f, 0.0f, 0.0f }, b3Quat_identity }; + } + + void Render() override + { + DrawHull( m_scene, m_transformA, &m_hull.base, b3_colorCyan, false ); + DrawSphere( m_scene, m_transformB, m_sphere, b3_colorGreen ); + + Manifold::Render(); + } + + void Step() override + { + if ( m_useCache == false ) + { + m_simplexCache = {}; + } + + b3Transform transformBtoA = b3InvMulTransforms( m_transformA, m_transformB ); + b3CollideHullAndSphere( &m_manifold, m_pointCapacity, &m_hull.base, &m_sphere, transformBtoA, &m_simplexCache ); + } + + static Sample* Create( SampleContext* sampleContext ) + { + return new HullAndSphere( sampleContext ); + } + + b3Sphere m_sphere; + b3BoxHull m_hull; +}; + +static int sampleSphereAndHull = SampleManager::Register( "Manifold", "Hull vs Sphere", HullAndSphere::Create ); + +class TriangleAndSphere : public TriangleManifold +{ +public: + static Sample* Create( SampleContext* sampleContext ) + { + return new TriangleAndSphere( sampleContext ); + } + + explicit TriangleAndSphere( SampleContext* sampleContext ) + : TriangleManifold( sampleContext ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 30.0f, 10.0f, b3Vec3_zero ); + } + + m_sphere = { { 0.0f, 0.0f, 0.0f }, 1.0f }; + m_triangle[0] = { 0.0f, 0.0f, 0.0f }; + m_triangle[1] = { 4.0f, 0.0f, 4.0f }; + m_triangle[2] = { 4.0f, 0.0f, 0.0f }; + + // b3Quat qA = b3MakeQuatFromAxisAngle( { 0.0f, 1.0f, 0.0f }, 2.0f ); + // m_transformA = { { 1.0f, 1.0f, 0.0f }, qA }; + + m_transformA = b3Transform_identity; + m_transformB = { { 2.0f, 0.5f, 1.0f }, b3Quat_identity }; + } + + void Render() override + { + DrawSphere( m_scene, m_transformB, m_sphere, b3_colorGreen ); + + TriangleManifold::Render(); + } + + void Step() override + { + // Convert triangle to frame B + b3Transform xf = b3InvMulTransforms( m_transformB, m_transformA ); + b3Vec3 localTriangle[3] = { + b3TransformPoint( xf, m_triangle[0] ), + b3TransformPoint( xf, m_triangle[1] ), + b3TransformPoint( xf, m_triangle[2] ), + }; + + b3CollideSphereAndTriangle( &m_manifold, m_pointCapacity, &m_sphere, localTriangle ); + + // Convert manifold from frame B to world space + m_manifold.normal = b3RotateVector( m_transformB.q, m_manifold.normal ); + for ( int i = 0; i < m_manifold.pointCount; ++i ) + { + m_manifold.points[i].point = b3TransformPoint( m_transformB, m_manifold.points[i].point ); + } + } + + b3Sphere m_sphere; +}; + +static int sampleSphereAndTriangle = SampleManager::Register( "Manifold", "Triangle vs Sphere", TriangleAndSphere::Create ); + +class CapsuleAndCapsule : public Manifold +{ +public: + explicit CapsuleAndCapsule( SampleContext* sampleContext ) + : Manifold( sampleContext ) + { + m_capsule = { { -2.0f, 0.0f, 0.0f }, { 2.0f, 0.0f, 0.0f }, 1.0f }; + + m_transformA = { { 1.0f, 1.0f, 0.0f }, b3Quat_identity }; + m_transformB = { { -4.0f, 1.0f, 0.0f }, b3Quat_identity }; + } + + void Render() override + { + DrawCapsule( m_scene, m_transformA, m_capsule, b3_colorGreen ); + DrawCapsule( m_scene, m_transformB, m_capsule, b3_colorCyan ); + + Manifold::Render(); + } + + void Step() override + { + b3Transform transformBtoA = b3InvMulTransforms( m_transformA, m_transformB ); + b3CollideCapsules( &m_manifold, m_pointCapacity, &m_capsule, &m_capsule, transformBtoA ); + } + + static Sample* Create( SampleContext* sampleContext ) + { + return new CapsuleAndCapsule( sampleContext ); + } + + b3Capsule m_capsule; +}; + +static int sampleCapsuleAndCapsule = SampleManager::Register( "Manifold", "Capsule vs Capsule", CapsuleAndCapsule::Create ); + +class CapsuleAndHull : public Manifold +{ +public: + explicit CapsuleAndHull( SampleContext* sampleContext ) + : Manifold( sampleContext ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 30.0f, 5.0f, b3Vec3_zero ); + } + + m_capsule = { { -1.0f, 0.0f, 0.0f }, { 1.0f, 0.0f, 0.0f }, 0.5f }; + m_hull = b3MakeBoxHull( 1.0f, 0.5f, 0.5f ); + + m_transformA = { { 0.0f, 0.0f, 0.0f }, b3Quat_identity }; + m_transformB = { { 0.0f, 1.0f, 0.0f }, b3Quat_identity }; + + /* + p [ 1.58523774, 0.729615569, 0.451690674 ] b3Vec3 + q [ [ -0.00256555085, -0.0201825816, 0.126076236 ], 0.991811991 ] b3Quat + */ + // m_capsule = { + // { 0.0799999982, -0.0151330000, -0.0918010026 }, { -0.0799999982, -0.0151330000, -0.0918010026 }, 0.100000001 }; + // m_hull = b3CreateBox( { 0.5f, 1.0f, 1.5f } ); + + m_transformB = { { 1.58523774, 0.729615569, 0.451690674 }, + { { -0.00256555085, -0.0201825816, 0.126076236 }, 0.991811991 } }; + } + + void Render() override + { + DrawHull( m_scene, m_transformA, &m_hull.base, b3_colorCyan, false ); + DrawCapsule( m_scene, m_transformB, m_capsule, b3_colorGreen ); + + Manifold::Render(); + } + + void Step() override + { + if ( m_useCache == false ) + { + m_simplexCache = {}; + } + + b3Transform transformBtoA = b3InvMulTransforms( m_transformA, m_transformB ); + b3CollideHullAndCapsule( &m_manifold, m_pointCapacity, &m_hull.base, &m_capsule, transformBtoA, &m_simplexCache ); + } + + static Sample* Create( SampleContext* sampleContext ) + { + return new CapsuleAndHull( sampleContext ); + } + + b3BoxHull m_hull; + b3Capsule m_capsule; +}; + +static int sampleCapsuleAndHull = SampleManager::Register( "Manifold", "Capsule vs Hull", CapsuleAndHull::Create ); + +class TriangleAndCapsule : public TriangleManifold +{ +public: + static Sample* Create( SampleContext* sampleContext ) + { + return new TriangleAndCapsule( sampleContext ); + } + + explicit TriangleAndCapsule( SampleContext* sampleContext ) + : TriangleManifold( sampleContext ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 30.0f, 10.0f, b3Vec3_zero ); + } + + m_capsule = { { 0.0f, -0.2f, 0.0f }, { 0.0f, 0.2f, 0.0f }, 0.05f }; + m_triangle[0] = { -4.0f, 0.0f, -4.0f }; + m_triangle[1] = { -4.0f, 0.0f, 0.0f }; + m_triangle[2] = { 0.0f, 0.0f, 0.0f }; + + m_transformA = b3Transform_identity; + m_transformB = { { 0.0f, 0.5f, 0.0f }, b3Quat_identity }; + m_transformB.q = b3MakeQuatFromAxisAngle( { 0.0f, 0.0f, 1.0f }, 0.5f * B3_PI ); + + m_transformB = { { -0.500000000, 0.123778239, -0.500000000 }, + { { -0.157559350, 0.294042289, 0.821513653 }, -0.462417006 } }; + } + + void Render() override + { + DrawCapsule( m_scene, m_transformB, m_capsule, b3_colorGreen ); + DrawTransform( m_scene, m_transformB, 0.1f ); + + TriangleManifold::Render(); + } + + void Step() override + { + if ( m_useCache == false ) + { + m_simplexCache = {}; + } + + // Put triangle into capsule coordinates + b3Transform xf = b3InvMulTransforms( m_transformB, m_transformA ); + b3Vec3 localTriangle[3] = { + b3TransformPoint( xf, m_triangle[0] ), + b3TransformPoint( xf, m_triangle[1] ), + b3TransformPoint( xf, m_triangle[2] ), + }; + + b3CollideCapsuleAndTriangle( &m_manifold, m_pointCapacity, &m_capsule, localTriangle, &m_simplexCache ); + + // Convert manifold from frame B to world space + m_manifold.normal = b3RotateVector( m_transformB.q, m_manifold.normal ); + for ( int i = 0; i < m_manifold.pointCount; ++i ) + { + m_manifold.points[i].point = b3TransformPoint( m_transformB, m_manifold.points[i].point ); + } + } + + b3Capsule m_capsule; +}; + +static int sampleCapsuleAndTriangle = SampleManager::Register( "Manifold", "Triangle vs Capsule", TriangleAndCapsule::Create ); + +class HullAndHull : public Manifold +{ +public: + explicit HullAndHull( SampleContext* context ) + : Manifold( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 15.0f, 4.0f, b3Vec3_zero ); + } + + // m_boxA = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, { { 0.0f, -0.5f, 0.0f }, b3Quat_identity } ); + + b3Transform transform = { { 1.0f, 0.5f, 0.0f }, b3Quat_identity }; + m_boxA = b3MakeTransformedBoxHull( 0.5f, 1.0f, 1.0f, transform ); + + // m_hull = CreateConvex( 0.6f, 0.0f, 0.95f, 1.0f, context->arena ); + m_hullA = &m_boxA.base; + + m_boxB = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + // m_hull = CreateConvex( 0.6f, 0.0f, 0.95f, 1.0f, context->arena ); + m_hullB = &m_boxB.base; + + /* +- xfA {p=[ -10.0000000, 0.00000000, -10.0000000 ] q=[ [ 0.00000000, 0.00000000, 0.00000000 ], 1.00000000 ] } b3Transform ++ p [ -10.0000000, 0.00000000, -10.0000000 ] b3Vec3 ++ q [ [ 0.00000000, 0.00000000, 0.00000000 ], 1.00000000 ] b3Quat +- xfB {p=[ -10.0000000, 1.00000000, -10.0000000 ] q=[ [ 0.00000000, 0.00000000, 0.00000000 ], 1.00000000 ] } b3Transform ++ p [ -10.0000000, 1.00000000, -10.0000000 ] b3Vec3 ++ q [ [ 0.00000000, 0.00000000, 0.00000000 ], 1.00000000 ] b3Quat + + */ + m_transformA = { { 0.0f, 0.0f, 0.0f }, b3Quat_identity }; + m_transformB = { { 0.0f, 0.0f, 0.0f }, b3Quat_identity }; + + /* + p [ 0.691772044, 0.951781273, 0.0741987228 ] b3Vec3 + q [ [ -0.00162522844, -0.0456664972, 0.0355294086 ], 0.998323441 ] b3Quat + */ + // m_transformB = { { 0.691772044, 0.951781273, 0.0741987228 }, + // { { -0.00162522844, -0.0456664972, 0.0355294086 }, 0.998323441 } }; + + m_satCache = {}; + } + + ~HullAndHull() override + { + if ( m_hullA != &m_boxA.base ) + { + b3DestroyHull( m_hullA ); + } + + if ( m_hullB != &m_boxB.base ) + { + b3DestroyHull( m_hullB ); + } + } + + b3Hull* CreateConvex( float radius1, float height1, float radius2, float height2, Arena arena ) const + { + const int sideCount = 32; + const float deltaAlpha = 2.0f * B3_PI / sideCount; + + int vertexCount = 2 * sideCount; + b3Vec3* vertexBase = static_cast( arena.Allocate( vertexCount * sizeof( b3Vec3 ) ) ); + + float alpha = 0.0f; + for ( int sideIndex = 0; sideIndex < sideCount; ++sideIndex ) + { + b3CosSin cs = b3ComputeCosSin( alpha ); + + float x1 = radius1 * cs.cosine; + float z1 = radius1 * cs.sine; + float x2 = radius2 * cs.cosine; + float z2 = radius2 * cs.sine; + + vertexBase[2 * sideIndex + 0] = { x1, height1, z1 }; + vertexBase[2 * sideIndex + 1] = { x2, height2, z2 }; + alpha += deltaAlpha; + } + + return b3CreateHull( vertexBase, vertexCount, vertexCount ); + } + + void Render() override + { + DrawHull( m_scene, m_transformA, m_hullA, b3_colorGreen, false ); + DrawHull( m_scene, m_transformB, m_hullB, b3_colorCyan, false ); + + Manifold::Render(); + } + + void Step() override + { + if ( m_useCache == true ) + { + if ( m_manualFeature == 1 ) + { + m_satCache.type = b3_manualFaceAxisA; + } + else if ( m_manualFeature == 2 ) + { + m_satCache.type = b3_manualFaceAxisB; + } + else if ( m_manualFeature == 3 ) + { + m_satCache.type = b3_manualEdgePairAxis; + } + } + else + { + m_satCache = {}; + } + + b3Transform transformBtoA = b3InvMulTransforms( m_transformA, m_transformB ); + b3CollideHulls( &m_manifold, m_pointCapacity, m_hullA, m_hullB, transformBtoA, &m_satCache ); + + DrawTextLine( "SAT type: %d", m_satCache.type ); + } + + static Sample* Create( SampleContext* sampleContext ) + { + return new HullAndHull( sampleContext ); + } + + b3BoxHull m_boxA; + b3BoxHull m_boxB; + b3Hull* m_hullA; + b3Hull* m_hullB; +}; + +static int sampleCollideHulls = SampleManager::Register( "Manifold", "Hull vs Hull", HullAndHull::Create ); + +class TriangleAndHull : public TriangleManifold +{ +public: + static Sample* Create( SampleContext* sampleContext ) + { + return new TriangleAndHull( sampleContext ); + } + + explicit TriangleAndHull( SampleContext* sampleContext ) + : TriangleManifold( sampleContext ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 30.0f, 3.0f, b3Vec3_zero ); + } + + m_triangle[0] = { 1.00000000, 0, 1.00000000 }; + m_triangle[1] = { 1.00000000, 0, 0.00000000 }; + m_triangle[2] = { 0.00000000, 0, 0.00000000 }; + + m_boxHull = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + + m_transformA = b3Transform_identity; + m_transformB = b3Transform_identity; + m_transformB.p = { 0.0f, 0.45f, 0.1f }; + + // b3MeshEdgeFlags + m_flags = 0; + + /* + * separation -0.0415397286 float + type 2 '\x2' unsigned char + indexA 0 '\0' unsigned char + indexB 1 '\x1' unsigned char + hit 1 '\x1' unsigned char + + */ + + m_satCache = { + .separation = -0.0107582733f, + .type = 1, + .indexA = 0, + .indexB = 4, + }; + + m_satCache = {}; + m_useCache = false; + m_cylinder = b3CreateCylinder( 0.4f, 0.05f, 0.0f, 6 ); + m_hull = &m_boxHull.base; + } + + ~TriangleAndHull() override + { + b3DestroyHull( m_cylinder ); + } + + void Render() override + { + DrawHull( m_scene, m_transformB, m_hull, b3_colorGreen, false ); + + b3Transform xf; + xf.p = b3TransformPoint( m_transformB, m_hull->center ); + xf.q = m_transformB.q; + DrawTransform( m_scene, xf, 0.1f ); + + TriangleManifold::Render(); + } + + void Step() override + { + if ( m_useCache == true ) + { + if ( m_manualFeature == 1 ) + { + m_satCache.type = b3_manualFaceAxisA; + } + else if ( m_manualFeature == 2 ) + { + m_satCache.type = b3_manualFaceAxisB; + } + else if ( m_manualFeature == 3 ) + { + m_satCache.type = b3_manualEdgePairAxis; + } + } + else + { + m_satCache = {}; + } + + b3Transform xf = b3InvMulTransforms( m_transformB, m_transformA ); + b3Vec3 localTriangle[3] = { + b3TransformPoint( xf, m_triangle[0] ), + b3TransformPoint( xf, m_triangle[1] ), + b3TransformPoint( xf, m_triangle[2] ), + }; + + b3CollideHullAndTriangle( &m_manifold, m_pointCapacity, m_hull, localTriangle, m_flags, &m_satCache ); + + // Convert manifold from frame B to world space + m_manifold.normal = b3RotateVector( m_transformB.q, m_manifold.normal ); + for ( int i = 0; i < m_manifold.pointCount; ++i ) + { + m_manifold.points[i].point = b3TransformPoint( m_transformB, m_manifold.points[i].point ); + } + } + + int m_flags; + b3BoxHull m_boxHull; + b3Hull* m_cylinder; + b3Hull* m_hull; +}; + +static int sampleHullAndTriangle = SampleManager::Register( "Manifold", "Triangle vs Hull", TriangleAndHull::Create ); diff --git a/samples/sample_mesh.cpp b/samples/sample_mesh.cpp new file mode 100644 index 0000000..6568a24 --- /dev/null +++ b/samples/sample_mesh.cpp @@ -0,0 +1,1523 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "camera.h" +#include "human.h" +#include "mesh_loader.h" +#include "renderer.h" +#include "sample.h" +#include "scene.h" + +#include "box3d/box3d.h" + +#include +#include +#include + +enum class ShapeType +{ + sphere, + capsule, + box, + cylinder +}; + +class GridMesh : public Sample +{ +public: + explicit GridMesh( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 6.0f, b3Vec3_zero ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + m_gridMesh = b3CreateGridMesh( 20, 20, 1.0f, 0, true ); + + m_scale = b3Vec3_one; + m_scale = {2.0f, 2.0f, 2.0f}; + //m_scale = { -1.2f, 2.0f, 4.0f }; + //m_scale = { 8.0f, 8.0f, 8.0f }; + //m_scale = { 0.5f, 0.5f, 0.5f }; + //m_scale = { 0.25f, 0.25f, 0.25f }; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + m_gridShapeId = b3CreateMeshShape( groundId, &shapeDef, m_gridMesh, m_scale ); + + m_shapeType = ShapeType::cylinder; + m_bodyId = b3_nullBodyId; + m_cylinderHull = b3CreateCylinder( 1.0f, 0.25f, 0.0f, 15 ); + //m_cylinderHull = b3CreateCylinder( 0.5f, 0.2f, 0.0f, 15 ); + + Spawn(); + + m_context->debugDraw.forceScale = 0.01f; + } + + ~GridMesh() override + { + b3DestroyHull( m_cylinderHull ); + b3DestroyMesh( m_gridMesh ); + } + + void Spawn() + { + if ( B3_IS_NON_NULL( m_bodyId ) ) + { + b3DestroyBody( m_bodyId ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + //bodyDef.position = { 0.5f, 2.0f, -0.5f }; + bodyDef.position = { 0.1f, 1.0f, -0.1f }; + // bodyDef.linearVelocity = { 2.0f, 0.0f, 0.0f }; + // bodyDef.position = { 0.208701894, 0.0791450217, -0.298710823 }; + // bodyDef.rotation = { { 0.0808477178, -0.00305216131, 0.000387475739 }, 0.996721804 }; + // bodyDef.linearVelocity = { 7.28029263e-05, 0.000761006493, 0.0104003791 }; + // bodyDef.angularVelocity = { 0.116714634, -0.00774557842, -0.000242586044 }; + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + switch ( m_shapeType ) + { + case ShapeType::sphere: + { + shapeDef.baseMaterial.rollingResistance = 0.05f; + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f }; + b3CreateSphereShape( m_bodyId, &shapeDef, &sphere ); + } + break; + + case ShapeType::capsule: + { + shapeDef.baseMaterial.rollingResistance = 0.05f; + b3Capsule capsule = { { 0.0f, 0.0f, 1.276f }, { 0.0f, 0.0f, 0.476f }, 0.15f }; + b3CreateCapsuleShape( m_bodyId, &shapeDef, &capsule ); + } + break; + + case ShapeType::box: + { + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + //shapeDef.baseMaterial.friction = 0.01f; + b3CreateHullShape( m_bodyId, &shapeDef, &box.base ); + } + break; + + case ShapeType::cylinder: + { + shapeDef.baseMaterial.rollingResistance = 0.01f; + //shapeDef.baseMaterial.friction = 0.1f; + b3CreateHullShape( m_bodyId, &shapeDef, m_cylinderHull ); + } + break; + + default: + assert( false ); + } + } + + void UpdateUI() override + { + float height = 220.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 280.0f, height ) ); + ImGui::Begin( "Mesh Grid", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + if ( ImGui::RadioButton( "Sphere", m_shapeType == ShapeType::sphere ) ) + { + m_shapeType = ShapeType::sphere; + Spawn(); + } + + if ( ImGui::RadioButton( "Capsule", m_shapeType == ShapeType::capsule ) ) + { + m_shapeType = ShapeType::capsule; + Spawn(); + } + + if ( ImGui::RadioButton( "Box", m_shapeType == ShapeType::box ) ) + { + m_shapeType = ShapeType::box; + Spawn(); + } + + if ( ImGui::RadioButton( "Cylinder", m_shapeType == ShapeType::cylinder ) ) + { + m_shapeType = ShapeType::cylinder; + Spawn(); + } + + b3Vec3 scale = m_scale; + bool changed = false; + changed = changed || ImGui::SliderFloat( "Scale X", &scale.x, -2.0f, 2.0f, "%.1f" ); + changed = changed || ImGui::SliderFloat( "Scale Z", &scale.z, -2.0f, 2.0f, "%.1f" ); + + if ( changed ) + { + m_scale = scale; + b3Shape_SetMesh( m_gridShapeId, m_gridMesh, m_scale ); + } + + ImGui::End(); + } + + void Render() override + { + Sample::Render(); + + DrawTextLine( "triangle count = %d, bytes = %d", m_gridMesh->triangleCount, m_gridMesh->byteCount ); + b3Transform transform = { { 0.0f, 0.01f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 1.0f ); + } + + void Step() override + { + if ( m_stepCount == 83 ) + { + // m_context->settings.pause = true; + } + + if ( m_stepCount < 100 ) + { + // b3Vec3 p = b3Body_GetPosition( m_bodyId ); + // printf( "%d : %g %g %g\n", m_stepCount, p.x, p.y, p.z ); + // b3Vec3 v = b3Body_GetLinearVelocity( m_bodyId ); + // printf( "%d : %g %g %g\n", m_stepCount, v.x, v.y, v.z ); + } + + Sample::Step(); + } + + static Sample* Create( SampleContext* context ) + { + return new GridMesh( context ); + } + + b3Hull* m_cylinderHull; + ShapeType m_shapeType; + b3BodyId m_bodyId; + b3MeshData* m_gridMesh; + b3ShapeId m_gridShapeId; + b3Vec3 m_scale; +}; + +static int sampleGridMesh = SampleManager::Register( "Mesh", "Grid", GridMesh::Create ); + +class BigBoxMesh : public Sample +{ +public: + explicit BigBoxMesh( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 6.0f, b3Vec3_zero ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + m_boxMesh = b3CreateBoxMesh( b3Vec3_zero, { 50.0f, 1.0f, 50.0f }, true ); + m_scale = b3Vec3_one; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.5f; + //shapeDef.baseMaterial.restitution = 0.5f; + //shapeDef.baseMaterial.rollingResistance = 0.5f; + m_gridShapeId = b3CreateMeshShape( groundId, &shapeDef, m_boxMesh, m_scale ); + + m_shapeType = ShapeType::cylinder; + m_bodyId = b3_nullBodyId; + m_cylinderHull = b3CreateCylinder( 0.5f, 0.2f, 0.0f, 15 ); + + m_stepWhilePaused = false; + + Spawn(); + } + + ~BigBoxMesh() override + { + b3DestroyHull( m_cylinderHull ); + b3DestroyMesh( m_boxMesh ); + } + + void Spawn() + { + if ( B3_IS_NON_NULL( m_bodyId ) ) + { + b3DestroyBody( m_bodyId ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 2.0f, 0.0f }; + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.rollingResistance = 0.05f; + + switch ( m_shapeType ) + { + case ShapeType::sphere: + { + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f }; + b3CreateSphereShape( m_bodyId, &shapeDef, &sphere ); + } + break; + + case ShapeType::capsule: + { + b3SurfaceMaterial material = b3DefaultSurfaceMaterial(); + //material.friction = 0.0f; + //material.restitution = 1.0f; + material.rollingResistance = 0.1f; + shapeDef.materials = &material; + shapeDef.materialCount = 1; + b3Capsule capsule = { { 0.0f, 0.0f, 1.276f }, { 0.0f, 0.0f, 0.476f }, 0.15f }; + b3CreateCapsuleShape( m_bodyId, &shapeDef, &capsule ); + } + break; + + case ShapeType::box: + { + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + //shapeDef.baseMaterial.friction = 0.01f; + b3CreateHullShape( m_bodyId, &shapeDef, &box.base ); + } + break; + + case ShapeType::cylinder: + { + b3CreateHullShape( m_bodyId, &shapeDef, m_cylinderHull ); + } + break; + + default: + assert( false ); + } + } + + void UpdateUI() override + { + float height = 220.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 280.0f, height ) ); + ImGui::Begin( "Mesh Grid", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + if ( ImGui::RadioButton( "Sphere", m_shapeType == ShapeType::sphere ) ) + { + m_shapeType = ShapeType::sphere; + Spawn(); + } + + if ( ImGui::RadioButton( "Capsule", m_shapeType == ShapeType::capsule ) ) + { + m_shapeType = ShapeType::capsule; + Spawn(); + } + + if ( ImGui::RadioButton( "Box", m_shapeType == ShapeType::box ) ) + { + m_shapeType = ShapeType::box; + Spawn(); + } + + if ( ImGui::RadioButton( "Cylinder", m_shapeType == ShapeType::cylinder ) ) + { + m_shapeType = ShapeType::cylinder; + Spawn(); + } + + b3Vec3 scale = m_scale; + bool changed = false; + changed = changed || ImGui::SliderFloat( "Scale X", &scale.x, -2.0f, 2.0f, "%.1f" ); + changed = changed || ImGui::SliderFloat( "Scale Z", &scale.z, -2.0f, 2.0f, "%.1f" ); + + if ( changed ) + { + m_scale = scale; + b3Shape_SetMesh( m_gridShapeId, m_boxMesh, m_scale ); + } + + ImGui::End(); + } + + void Render() override + { + Sample::Render(); + b3Transform transform = { { 0.0f, 0.01f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 1.0f ); + } + + void Step() override + { + if ( m_stepCount == 83 ) + { + // m_context->settings.pause = true; + } + + if ( m_stepCount < 100 ) + { + // b3Vec3 p = b3Body_GetPosition( m_bodyId ); + // printf( "%d : %g %g %g\n", m_stepCount, p.x, p.y, p.z ); + // b3Vec3 v = b3Body_GetLinearVelocity( m_bodyId ); + // printf( "%d : %g %g %g\n", m_stepCount, v.x, v.y, v.z ); + } + + Sample::Step(); + } + + static Sample* Create( SampleContext* context ) + { + return new BigBoxMesh( context ); + } + + b3Hull* m_cylinderHull; + ShapeType m_shapeType; + b3BodyId m_bodyId; + b3MeshData* m_boxMesh; + b3ShapeId m_gridShapeId; + b3Vec3 m_scale; +}; + +static int sampleBigBoxMesh = SampleManager::Register( "Mesh", "Big Box", BigBoxMesh::Create ); + +class BoxMesh : public Sample +{ +public: + explicit BoxMesh( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 6.0f, b3Vec3_zero ); + EnableGrid( m_scene, true ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisY, 0.25f * B3_PI ); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeBoxHull( 20.0f, 1.0f, 20.0f ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + + m_boxMesh = b3CreateBoxMesh( { 0.0f, 1.0f, 0.0f }, { 1.0f, 1.0f, 1.0f }, true ); + // m_scale = { -0.6f, 1.0f, 2.0f }; + m_scale = b3Vec3_one; + m_boxShapeId = b3CreateMeshShape( groundId, &shapeDef, m_boxMesh, m_scale ); + + m_cylinderHull = b3CreateCylinder( 1.0f, 0.75f, 0.0f, 8 ); + + m_shapeType = ShapeType::box; + m_bodyId = b3_nullBodyId; + + Spawn(); + } + + ~BoxMesh() override + { + b3DestroyMesh( m_boxMesh ); + b3DestroyHull( m_cylinderHull ); + } + + void Spawn() + { + if ( B3_IS_NON_NULL( m_bodyId ) ) + { + b3DestroyBody( m_bodyId ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 1.5f, 0.0f }; + //bodyDef.position = { -0.2f, 1.5f, -1.25f }; + + if ( m_shapeType == ShapeType::cylinder ) + { + bodyDef.position.y -= 0.5f; + } + + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + switch ( m_shapeType ) + { + case ShapeType::sphere: + { + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f }; + b3CreateSphereShape( m_bodyId, &shapeDef, &sphere ); + } + break; + + case ShapeType::capsule: + { + b3Capsule capsule = { { -0.5f, 0.0f, 0.0f }, { 0.5f, 0.0f, 0.0f }, 0.1f }; + b3CreateCapsuleShape( m_bodyId, &shapeDef, &capsule ); + } + break; + + case ShapeType::box: + { + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + // b3BoxHull box = b3MakeBoxHull( 0.1f, 1.0f, 0.1f ); + b3CreateHullShape( m_bodyId, &shapeDef, &box.base ); + } + break; + + case ShapeType::cylinder: + { + b3CreateHullShape( m_bodyId, &shapeDef, m_cylinderHull ); + } + break; + + default: + assert( false ); + } + } + + void UpdateUI() override + { + float height = 220.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 280.0f, height ) ); + ImGui::Begin( "Box Mesh", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + if ( ImGui::RadioButton( "Sphere", m_shapeType == ShapeType::sphere ) ) + { + m_shapeType = ShapeType::sphere; + Spawn(); + } + + if ( ImGui::RadioButton( "Capsule", m_shapeType == ShapeType::capsule ) ) + { + m_shapeType = ShapeType::capsule; + Spawn(); + } + + if ( ImGui::RadioButton( "Box", m_shapeType == ShapeType::box ) ) + { + m_shapeType = ShapeType::box; + Spawn(); + } + + if ( ImGui::RadioButton( "Cylinder", m_shapeType == ShapeType::cylinder ) ) + { + m_shapeType = ShapeType::cylinder; + Spawn(); + } + + b3Vec3 scale = m_scale; + bool changed = false; + changed = changed || ImGui::SliderFloat( "Scale X", &scale.x, -2.0f, 2.0f, "%.1f" ); + changed = changed || ImGui::SliderFloat( "Scale Z", &scale.z, -2.0f, 2.0f, "%.1f" ); + + if ( changed ) + { + m_scale = scale; + b3Shape_SetMesh( m_boxShapeId, m_boxMesh, m_scale ); + } + + ImGui::End(); + } + + void Render() override + { + Sample::Render(); + b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 2.0f ); + } + + void Step() override + { + if ( m_stepCount == 83 ) + { + // m_context->settings.pause = true; + } + + if ( m_stepCount < 100 ) + { + // b3Vec3 p = b3Body_GetPosition( m_bodyId ); + // printf( "%d : %g %g %g\n", m_stepCount, p.x, p.y, p.z ); + // b3Vec3 v = b3Body_GetLinearVelocity( m_bodyId ); + // printf( "%d : %g %g %g\n", m_stepCount, v.x, v.y, v.z ); + } + + Sample::Step(); + } + + static Sample* Create( SampleContext* context ) + { + return new BoxMesh( context ); + } + + ShapeType m_shapeType; + b3BodyId m_bodyId; + b3MeshData* m_boxMesh; + b3Hull* m_cylinderHull; + b3ShapeId m_boxShapeId; + b3Vec3 m_scale; +}; + +static int sampleBoxMesh = SampleManager::Register( "Mesh", "Box", BoxMesh::Create ); + +class MeshReflection : public Sample +{ +public: + enum + { + e_humanCount = 20 + }; + + explicit MeshReflection( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 40.0f, b3Vec3_zero ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + { + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + m_gridMesh = b3CreateGridMesh( 20, 20, 2.0f, 2, true ); + b3CreateMeshShape( body, &shapeDef, m_gridMesh, b3Vec3_one ); + } + + { + m_buildingMesh = CreateMeshData( "data/meshes/building.obj", 1.0f, false, false, true, true ); + + b3ShapeDef meshShapeDef = b3DefaultShapeDef(); + b3SurfaceMaterial materials[3] = { + { + .friction = 0.6f, + }, + { + .friction = 0.0f, + .restitution = 0.95f, + .userMaterialId = 1, + }, + { + .friction = 0.2f, + .restitution = 0.2f, + .userMaterialId = 2, + }, + }; + meshShapeDef.materials = materials; + meshShapeDef.materialCount = 3; + + bodyDef.position = { -10.0f, 0.0f, 0.0f }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + b3CreateMeshShape( body, &meshShapeDef, m_buildingMesh, b3Vec3_one ); + + m_scale = { -1.0f, 1.0f, 1.0f }; + bodyDef.position = { 10.0f, 0.0f, 0.0f }; + m_meshBodyId = b3CreateBody( m_worldId, &bodyDef ); + m_meshShapeId = b3CreateMeshShape( m_meshBodyId, &meshShapeDef, m_buildingMesh, m_scale ); + } + + bodyDef.type = b3_dynamicBody; + { + bodyDef.position = { 6.0f, 15.0f, 0.0f }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + b3Sphere sphere = { b3Vec3_zero, 0.5f }; + shapeDef.baseMaterial.rollingResistance = 0.2f; + shapeDef.baseMaterial.userMaterialId = 42; + b3CreateSphereShape( body, &shapeDef, &sphere ); + } + + { + bodyDef.position = { 9.0f, 15.0f, 0.0f }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + b3Capsule capsule = { { -0.5f, 0.5f, 0.0f }, { 0.5f, 0.0f, 0.0f }, 0.25f }; + shapeDef.baseMaterial.rollingResistance = 0.2f; + shapeDef.baseMaterial.userMaterialId = 11; + b3CreateCapsuleShape( body, &shapeDef, &capsule ); + } + + { + bodyDef.position = { 12.0f, 15.0f, 0.0f }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + b3BoxHull hull = b3MakeBoxHull( 0.25f, 0.5f, 0.75f ); + shapeDef.baseMaterial.userMaterialId = 555; + b3CreateHullShape( body, &shapeDef, &hull.base ); + } + + float frictionTorque = 5.0f; + float hertz = 1.0f; + float dampingRatio = 0.7f; + bool colorize = false; + for ( int humanIndex = 0; humanIndex < e_humanCount; humanIndex += 1 ) + { + m_humans[humanIndex] = {}; + + int groupIndex = humanIndex; + b3Vec3 position = { -14.0f + 1.5f * humanIndex, 8.0f, 0.0f }; + CreateHuman( &m_humans[humanIndex], m_worldId, position, frictionTorque, hertz, dampingRatio, groupIndex, nullptr, + colorize ); + } + } + + ~MeshReflection() override + { + b3DestroyMesh( m_gridMesh ); + b3DestroyMesh( m_buildingMesh ); + + for ( int i = 0; i < e_humanCount; ++i ) + { + DestroyHuman( m_humans + i ); + } + } + + void UpdateUI() override + { + ImGui::SetNextWindowPos( ImVec2( 10.0f, 600.0f ) ); + ImGui::SetNextWindowSize( ImVec2( 200.0f, 150.0f ) ); + ImGui::Begin( "Mesh Reflection", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + bool changed = false; + b3Vec3 scale = m_scale; + if ( ImGui::RadioButton( "Neg X", scale.x < 0.0f ) ) + { + scale.x = -1.0f; + changed = true; + } + + ImGui::SameLine(); + + if ( ImGui::RadioButton( "Pos X", scale.x > 0.0f ) ) + { + scale.x = 1.0f; + changed = true; + } + + if ( ImGui::RadioButton( "Neg Y", scale.y < 0.0f ) ) + { + scale.y = -1.0f; + changed = true; + } + + ImGui::SameLine(); + + if ( ImGui::RadioButton( "Pos Y", scale.y > 0.0f ) ) + { + scale.y = 1.0f; + changed = true; + } + + if ( ImGui::RadioButton( "Neg Z", scale.z < 0.0f ) ) + { + scale.z = -1.0f; + changed = true; + } + + ImGui::SameLine(); + + if ( ImGui::RadioButton( "Pos Z", scale.z > 0.0f ) ) + { + scale.z = 1.0f; + changed = true; + } + + if ( changed ) + { + m_scale = scale; + + b3Shape_SetMesh( m_meshShapeId, m_buildingMesh, m_scale ); + } + + ImGui::End(); + } + + void Render() override + { + DrawTextLine( "scale = (%.2g, %.2g, %.2g)", m_scale.x, m_scale.y, m_scale.z ); + DrawTextLine( "surface type = %d", m_userMaterialId ); + + Sample::Render(); + } + + static Sample* Create( SampleContext* context ) + { + return new MeshReflection( context ); + } + + b3Vec3 m_scale; + b3MeshData* m_gridMesh; + b3MeshData* m_buildingMesh; + b3BodyId m_meshBodyId; + b3ShapeId m_meshShapeId; + Human m_humans[e_humanCount]; +}; + +static int sampleMeshReflection = SampleManager::Register( "Mesh", "Reflection", MeshReflection::Create ); + +struct CastContext +{ + b3Vec3 point; + b3Vec3 normal; + float fraction; + bool hit; +}; + +class HeightField : public Sample +{ +public: + static Sample* Create( SampleContext* context ) + { + return new HeightField( context ); + } + + explicit HeightField( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 40.0f, b3Vec3_zero ); + } + +#if defined(NDEBUG) + m_rowCount = 400; + m_columnCount = 400; +#else + m_rowCount = 100; + m_columnCount = 100; +#endif + + m_groundId = {}; + m_bodyId1 = {}; + m_bodyId2 = {}; + m_amplitude = 0.75f; + m_holes = false; + + // m_rayOrigin = { 1.4f, 2.0f, 1.5f }; + // m_rayTranslation = { -3.0f, -4.0f, 3.0f }; + // m_rayOrigin = { 30.0f, 2.0f, 0.0f }; + // m_rayTranslation = { -60.0f, -4.0f, 0.0f }; + + m_rayOrigin = { 0.0f, 2.0f, 15.0f }; + m_rayTranslation = { 0.0f, -4.0f, 0.0f }; + m_radius = 0.571f; + + m_heightField = nullptr; + + CreateScene(); + } + + ~HeightField() override + { + b3DestroyHeightField( m_heightField ); + } + + void CreateScene() + { + if ( B3_IS_NULL( m_groundId ) == false ) + { + b3DestroyBody( m_groundId ); + m_groundId = {}; + } + + if ( B3_IS_NULL( m_bodyId1 ) == false ) + { + b3DestroyBody( m_bodyId1 ); + m_bodyId1 = {}; + } + + if ( B3_IS_NULL( m_bodyId2 ) == false ) + { + b3DestroyBody( m_bodyId2 ); + m_bodyId2 = {}; + } + + if ( m_heightField != nullptr ) + { + b3DestroyHeightField( m_heightField ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + m_scale = { 2.0f, 2.0f * m_amplitude, 2.0f }; + + if ( m_amplitude == 0.0f ) + { + m_heightField = b3CreateGrid( m_rowCount, m_columnCount, m_scale, m_holes ); + } + else + { + m_heightField = b3CreateWave( m_rowCount, m_columnCount, m_scale, 0.1f, 0.03333f, m_holes ); + } + + bodyDef.position = { -0.5f * m_heightField->scale.x * ( m_columnCount - 1 ), 0.0f, + -0.5f * m_heightField->scale.z * ( m_rowCount - 1 ) }; + // bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisY, 0.25f * B3_PI ); + m_groundId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHeightFieldShape( m_groundId, &shapeDef, m_heightField ); + +#if 0 + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 4.0f, 1.0f }; + m_bodyId1 = b3CreateBody( m_worldId, &bodyDef ); + b3Sphere sphere = { b3Vec3_zero, 0.5f }; + b3CreateSphereShape( m_bodyId1, &shapeDef, &sphere ); + + bodyDef.position = { 0.0f, 0.5f, -0.5f }; + bodyDef.linearVelocity = { 5.0f, 0.0f, 0.0f }; + m_bodyId2 = b3CreateBody( m_worldId, &bodyDef ); + b3BoxHull box = b3MakeBoxHull( 0.3f, 0.3f, 0.3f ); + shapeDef.baseMaterial.friction = 0.05f; + b3CreateHullShape( m_bodyId2, &shapeDef, &box.base ); +#endif + } + + void UpdateUI() override + { + float fontSize = ImGui::GetFontSize(); + float height = 18.0f * fontSize; + ImGui::SetNextWindowPos( { 1.0f * fontSize, m_camera->m_height - height - 3.0f * fontSize }, ImGuiCond_Once ); + ImGui::SetNextWindowSize( { 20.0f * fontSize, height } ); + + ImGui::Begin( "Height Field", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + ImGui::PushItemWidth( 200.0f ); + + if ( ImGui::SliderInt( "columns", &m_columnCount, 1, 500 ) ) + { + CreateScene(); + } + + if ( ImGui::SliderInt( "rows", &m_rowCount, 1, 500 ) ) + { + CreateScene(); + } + + if ( ImGui::SliderFloat( "amplitude", &m_amplitude, 0.0f, 2.0f ) ) + { + CreateScene(); + } + + if ( ImGui::Checkbox( "holes", &m_holes ) ) + { + CreateScene(); + } + + ImGui::SliderFloat( "ray x", &m_rayOrigin.x, -2.0f * m_radius - 1.0f * m_scale.x * ( m_columnCount - 1 ), + 2.0f * m_radius + 1.0f * m_scale.x * ( m_columnCount - 1 ) ); + ImGui::SliderFloat( "ray z", &m_rayOrigin.z, -2.0f * m_radius - 1.0f * m_scale.z * ( m_rowCount - 1 ), + 2.0f * m_radius + 1.0f * m_scale.z * ( m_rowCount - 1 ) ); + ImGui::SliderFloat( "delta x", &m_rayTranslation.x, -2.0f * m_scale.x * ( m_columnCount - 1 ), + 2.0f * m_scale.x * ( m_columnCount - 1 ) ); + ImGui::SliderFloat( "delta z", &m_rayTranslation.z, -2.0f * m_scale.z * ( m_rowCount - 1 ), + 2.0f * m_scale.z * ( m_rowCount - 1 ) ); + ImGui::SliderFloat( "radius", &m_radius, 0.0f, 1.0f ); + + ImGui::PopItemWidth(); + ImGui::End(); + } + + void Render() override + { + Sample::Render(); + b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 0.5f ); + } + + // This callback finds the closest hit. + static float CastCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t surfaceType, + int triangleIndex, int childIndex, void* context ) + { + (void)shapeId; + (void)surfaceType; + (void)triangleIndex; + (void)childIndex; + + CastContext* castContext = (CastContext*)context; + + castContext->point = point; + castContext->normal = normal; + castContext->fraction = fraction; + castContext->hit = true; + + // By returning the current fraction, we instruct the calling code to clip the ray and + // continue the ray-cast to the next shape. WARNING: do not assume that shapes + // are reported in order. However, by clipping, we can always get the closest shape. + return fraction; + } + + void Step() override + { + Sample::Step(); + + if ( m_radius == 0.0f ) + { + // m_rayOrigin = { 0.0f, -FLT_EPSILON, 0.0f }; + // m_rayTranslation = { -1000.0f, 0.0f, 0.0 }; + + b3RayResult result = b3World_CastRayClosest( m_worldId, m_rayOrigin, m_rayTranslation, b3DefaultQueryFilter() ); + + DrawPoint( m_scene, m_rayOrigin, 6.0f, b3_colorGreenYellow ); + DrawPoint( m_scene, m_rayOrigin + m_rayTranslation, 6.0f, b3_colorRed ); + DrawLine( m_scene, m_rayOrigin, m_rayOrigin + m_rayTranslation, b3_colorGray ); + + if ( result.hit ) + { + b3Vec3 point = result.point; + DrawLine( m_scene, point, point + 0.5f * result.normal, b3_colorGray ); + DrawPoint( m_scene, point, 10.0f, b3_colorOrange ); + } + } + else + { + b3ShapeProxy proxy = { + .points = &m_rayOrigin, + .count = 1, + .radius = m_radius, + }; + + CastContext result = {}; + result.fraction = 1.0f; + + b3World_CastShape( m_worldId, &proxy, m_rayTranslation, b3DefaultQueryFilter(), CastCallback, &result ); + + DrawPoint( m_scene, m_rayOrigin, 2.0f, b3_colorGreen ); + DrawPoint( m_scene, m_rayOrigin + m_rayTranslation, 2.0f, b3_colorRed ); + DrawLine( m_scene, m_rayOrigin, m_rayOrigin + m_rayTranslation, b3_colorYellow ); + + b3Sphere sphere = { b3Vec3_zero, m_radius }; + DrawSphere( m_scene, { m_rayOrigin + result.fraction * m_rayTranslation, b3Quat_identity }, sphere, b3_colorOrange ); + + if ( result.hit ) + { + b3Vec3 point = result.point; + DrawLine( m_scene, point, point + 0.5f * result.normal, b3_colorGreen ); + DrawPoint( m_scene, point, 6.0f, b3_colorPurple ); + } + } + } + + b3BodyId m_groundId; + b3BodyId m_bodyId1; + b3BodyId m_bodyId2; + float m_amplitude; + b3Vec3 m_scale; + b3Vec3 m_rayOrigin; + b3Vec3 m_rayTranslation; + b3HeightField* m_heightField; + float m_radius; + int m_rowCount; + int m_columnCount; + bool m_holes; +}; + +static int sampleHeightField = SampleManager::Register( "Mesh", "Height Field", HeightField::Create ); + +static float ComputeInternalSurfaceArea( const b3MeshData* data ) +{ + const b3MeshNode* nodes = b3GetMeshNodes( data ); + int nodeCount = data->nodeCount; + float area = 0.0f; + for ( int i = 1; i < nodeCount; ++i ) + { + const b3MeshNode* node = nodes + i; + if ( node->data.asLeaf.type == 3 ) + { + continue; + } + + b3AABB box = { + node->lowerBound, + node->upperBound, + }; + + area += b3AABB_Area( box ); + } + + return area; +} + +class MeshViewer : public Sample +{ +public: + explicit MeshViewer( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 50.0f, b3Vec3_zero ); + } + + m_meshIndex = 0; + m_mesh = nullptr; + m_bodyId = b3_nullBodyId; + m_medianSplit = true; + m_concaveEdges = true; + m_weldVertices = true; + m_weldToleranceMillimeters = 1.5f; + m_area = 0.0f; + m_drawLevel = -1; + m_height = 0; + m_buildTime = 0.0f; + m_launchSpeedScale = 1.0f; + + LoadMesh(); + } + + ~MeshViewer() override + { + b3DestroyMesh( m_mesh ); + } + + void LoadMesh() + { + if ( B3_IS_NON_NULL( m_bodyId ) ) + { + b3DestroyBody( m_bodyId ); + m_bodyId = b3_nullBodyId; + } + + if ( m_mesh != nullptr ) + { + b3DestroyMesh( m_mesh ); + m_mesh = nullptr; + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + float scale = 0.01f; + + const char* filesNames[m_meshCount] = { + "voxel_mesh_01.obj", "voxel_mesh_02.obj", "voxel_mesh_03.obj", "voxel_mesh_04.obj", + }; + + char buffer[64] = {}; + snprintf( buffer, 64, "data/meshes/%s", filesNames[m_meshIndex] ); + + LoadTempMesh( buffer, &m_tempMesh, scale, true ); + + b3MeshDef def = {}; + def.vertices = m_tempMesh.vertices.data(); + def.vertexCount = (int)m_tempMesh.vertices.size(); + def.indices = m_tempMesh.indices.data(); + def.triangleCount = (int)m_tempMesh.indices.size() / 3; + def.materialIndices = m_tempMesh.materialIndices.data(); + def.useMedianSplit = m_medianSplit; + def.identifyEdges = m_concaveEdges; + def.weldVertices = m_weldVertices; + def.weldTolerance = 0.001f * m_weldToleranceMillimeters; + + uint64_t startTicks = b3GetTicks(); + m_mesh = b3CreateMesh( &def, m_degenerateTriangles, m_degenerateCapacity ); + m_buildTime = b3GetMilliseconds( startTicks ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateMeshShape( m_bodyId, &shapeDef, m_mesh, b3Vec3_one ); + + m_area = ComputeInternalSurfaceArea( m_mesh ); + m_height = b3GetHeight( m_mesh ); + m_drawLevel = b3ClampInt( m_drawLevel, -1, m_height ); + } + + void DrawNodes() + { + if ( m_drawLevel < 0 || m_mesh == nullptr ) + { + return; + } + + int count = m_mesh->nodeCount; + if ( count == 0 ) + { + return; + } + + constexpr int colorCount = 20; + b3HexColor colors[colorCount] = { + b3_colorAliceBlue, b3_colorAntiqueWhite, b3_colorAqua, b3_colorAquamarine, b3_colorAzure, + b3_colorBeige, b3_colorBisque, b3_colorBlanchedAlmond, b3_colorBlue, b3_colorBlueViolet, + b3_colorBrown, b3_colorBurlywood, b3_colorCadetBlue, b3_colorChartreuse, b3_colorChocolate, + b3_colorCoral, b3_colorCornflowerBlue, b3_colorCornsilk, b3_colorCrimson, b3_colorCyan, + }; + + const b3MeshNode* nodes = b3GetMeshNodes( m_mesh ); + const b3MeshNode* root = nodes + 0; + + // Queue to store nodes along with their levels + std::queue> q; + + // Start with the root at level 0 + q.push( { root, 0 } ); + + while ( !q.empty() ) + { + std::pair pair = q.front(); + q.pop(); + + const b3MeshNode* node = pair.first; + int level = pair.second; + + // If the current level matches the target level, add the node's data to the result + if ( level == m_drawLevel ) + { + b3AABB box = { + node->lowerBound, + node->upperBound, + }; + DrawBounds( m_scene, box, 0.0f, colors[level % colorCount] ); + + int axis = node->data.asNode.axis; + b3Vec3 center = b3AABB_Center( box ); + if ( axis == 0 ) + { + DrawArrow( m_scene, center, center + 0.1f * b3Vec3_axisX, 0.02f, b3_colorRed ); + } + else if ( axis == 1 ) + { + DrawArrow( m_scene, center, center + 0.1f * b3Vec3_axisY, 0.02f, b3_colorGreen ); + } + else if ( axis == 2 ) + { + DrawArrow( m_scene, center, center + 0.1f * b3Vec3_axisZ, 0.02f, b3_colorBlue ); + } + else + { + DrawSphere( m_scene, b3Transform_identity, { center, 0.03f }, b3_colorOrange ); + } + } + + // If the current level exceeds the target level, stop processing + if ( level > m_drawLevel ) + { + break; + } + + bool isLeaf = node->data.asNode.axis == 3; + if ( isLeaf == false ) + { + // Enqueue the left and right children with their respective levels + // I'm using some internal knowledge of how the node data is organized. + q.push( { node + 1, level + 1 } ); + q.push( { node + node->data.asNode.childOffset, level + 1 } ); + } + } + } + + void UpdateUI() override + { + float fontSize = ImGui::GetFontSize(); + float height = 14.0f * fontSize; + ImGui::SetNextWindowPos( { 1.0f * fontSize, m_camera->m_height - height - 3.0f * fontSize }, ImGuiCond_Once ); + ImGui::SetNextWindowSize( { 20.0f * fontSize, height } ); + + ImGui::Begin( "Mesh Viewer", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + if ( ImGui::SliderInt( "index", &m_meshIndex, 0, m_meshCount - 1 ) ) + { + LoadMesh(); + } + + if ( ImGui::RadioButton( "median split", m_medianSplit ) ) + { + m_medianSplit = true; + LoadMesh(); + } + + if ( ImGui::RadioButton( "sah binning", m_medianSplit == false ) ) + { + m_medianSplit = false; + LoadMesh(); + } + + if ( ImGui::Checkbox( "concave edges", &m_concaveEdges ) ) + { + LoadMesh(); + } + + if ( ImGui::Checkbox( "weld vertices", &m_weldVertices ) ) + { + LoadMesh(); + } + + if ( ImGui::SliderFloat( "tolerance", &m_weldToleranceMillimeters, 0.0f, 10.0f, "%.1f" ) ) + { + LoadMesh(); + } + + ImGui::SliderInt( "draw level", &m_drawLevel, -1, m_height ); + ImGui::End(); + } + + void Render() override + { + Sample::Render(); + DrawTransform( m_scene, b3Transform_identity, 1.0f ); + + DrawTextLine( "triangle count = %d", m_mesh->triangleCount ); + DrawTextLine( "vertex count = %d", m_mesh->vertexCount ); + DrawTextLine( "degenerate count = %d", m_mesh->degenerateCount ); + DrawTextLine( "node area = %g", m_area ); + DrawTextLine( "height = %d", m_height ); + DrawTextLine( "build time (ms) = %g", m_buildTime ); + + DrawNodes(); + + b3Vec3 offset = { 0.01f, 0.01f, 0.01f }; + for ( int i = 0; i < m_mesh->degenerateCount; ++i ) + { + int triangleIndex = m_degenerateTriangles[i]; + int i1 = m_tempMesh.indices[3 * triangleIndex + 0]; + int i2 = m_tempMesh.indices[3 * triangleIndex + 1]; + int i3 = m_tempMesh.indices[3 * triangleIndex + 2]; + b3Vec3 v1 = m_tempMesh.vertices[i1]; + b3Vec3 v2 = m_tempMesh.vertices[i2]; + b3Vec3 v3 = m_tempMesh.vertices[i3]; + + float area = b3Length( b3Cross( v2 - v1, v3 - v1 ) ); + (void)area; + + b3Vec3 p = ( 1.0f / 3.0f ) * ( v1 + v2 + v3 ); + DrawPoint( m_scene, p, 10.0f, b3_colorCyan ); + + DrawWorldString( m_camera, p + offset, b3_colorOrange, "%d", triangleIndex ); + + { + DrawPoint( m_scene, v1, 10.0f, b3_colorRed ); + DrawPoint( m_scene, v2, 10.0f, b3_colorGreen ); + DrawPoint( m_scene, v3, 10.0f, b3_colorBlue ); + + DrawWorldString( m_camera, v1 + offset, b3_colorRed, "%d", i1 ); + DrawWorldString( m_camera, v2 + offset, b3_colorGreen, "%d", i2 ); + DrawWorldString( m_camera, v3 + offset, b3_colorBlue, "%d", i3 ); + } + } + } + + static Sample* Create( SampleContext* context ) + { + return new MeshViewer( context ); + } + + static constexpr int m_meshCount = 4; + static constexpr int m_degenerateCapacity = 64; + TempMesh m_tempMesh; + b3MeshData* m_mesh; + b3BodyId m_bodyId; + float m_area; + float m_weldToleranceMillimeters; + float m_buildTime; + int m_meshIndex; + int m_degenerateTriangles[m_degenerateCapacity]; + int m_drawLevel; + int m_height; + bool m_medianSplit; + bool m_concaveEdges; + bool m_weldVertices; +}; + +static int sampleMeshViewer = SampleManager::Register( "Mesh", "Viewer", MeshViewer::Create ); + +// Results 9/29/25 +// base median split: 0.222 ms per mesh +// weld: 0.526 +// weld + edge: 0.755 +class MeshCreationBenchmark : public Sample +{ +public: + explicit MeshCreationBenchmark( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 40.0f, b3Vec3_zero ); + } + + LoadTempMesh( "data/meshes/voxel_mesh_01.obj", m_tempMeshes + 0, 0.01f, true ); + LoadTempMesh( "data/meshes/voxel_mesh_02.obj", m_tempMeshes + 1, 0.01f, true ); + LoadTempMesh( "data/meshes/voxel_mesh_03.obj", m_tempMeshes + 2, 0.01f, true ); + LoadTempMesh( "data/meshes/voxel_mesh_04.obj", m_tempMeshes + 3, 0.01f, true ); + + m_time = FLT_MAX; + } + + void Step() override + { + bool computeArea = false; + int iterations = m_isDebug ? 1 : 10; + int meshCount = m_meshCount; + int triangleCount = 0; + float area = 0.0f; + + for ( int i = 0; i < iterations; ++i ) + { + uint64_t startTicks = b3GetTicks(); + + for ( int j = 0; j < meshCount; ++j ) + { + TempMesh& mesh = m_tempMeshes[j]; + b3MeshDef def = {}; + def.vertices = mesh.vertices.data(); + def.vertexCount = (int)mesh.vertices.size(); + def.indices = mesh.indices.data(); + def.triangleCount = (int)mesh.indices.size() / 3; + def.materialIndices = mesh.materialIndices.data(); + def.useMedianSplit = true; + def.identifyEdges = false; + def.weldVertices = true; + def.weldTolerance = 0.0015f; + + b3MeshData* meshData = b3CreateMesh( &def, nullptr, 0 ); + triangleCount += i == 0 ? meshData->triangleCount : 0; + + if ( computeArea && i == 0 ) + { + area += ComputeInternalSurfaceArea( meshData ); + } + + b3DestroyMesh( meshData ); + } + + float ms = b3GetMilliseconds( startTicks ); + m_time = b3MinFloat( m_time, ms ); + } + + DrawTextLine( "triangle count = %d, area = %g", triangleCount, area ); + DrawTextLine( "total time = %.4f ms", m_time ); + DrawTextLine( "time per mesh = %.4f ms", m_time / meshCount ); + } + + static Sample* Create( SampleContext* context ) + { + return new MeshCreationBenchmark( context ); + } + + static constexpr int m_meshCount = 4; + TempMesh m_tempMeshes[m_meshCount]; + float m_time; +}; + +static int sampleMeshCreationBenchmark = SampleManager::Register( "Mesh", "Creation Benchmark", MeshCreationBenchmark::Create ); + +class VoxelMesh : public Sample +{ +public: + explicit VoxelMesh( SampleContext* context ) + : Sample( context ) + { + b3Vec3 origin = { 5000.0f, 3500.0f, -7000.0f }; + + if ( context->restart == false ) + { + m_camera->SetView( -115.0f, 5.0f, 5.0f, b3Vec3{ 0.0f, 10.0f, 0.0f } + origin ); + } + + m_launchSpeedScale = 1.0f; + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "ground"; + bodyDef.position = origin; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + float scale = 0.01f; + TempMesh tempMesh; + const char* fileName = "data/meshes/collision_mesh_01.obj"; + LoadTempMesh( fileName, &tempMesh, scale, true ); + + b3MeshDef def = {}; + def.vertices = tempMesh.vertices.data(); + def.vertexCount = (int)tempMesh.vertices.size(); + def.indices = tempMesh.indices.data(); + def.triangleCount = (int)tempMesh.indices.size() / 3; + def.materialIndices = tempMesh.materialIndices.data(); + def.useMedianSplit = true; + + // this has a big impact on stability because the faces are nearly coplanar + def.identifyEdges = true; + + def.weldVertices = true; + def.weldTolerance = 0.002f; + + m_meshData = b3CreateMesh( &def, nullptr, 0 ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateMeshShape( groundId, &shapeDef, m_meshData, b3Vec3_one ); + } + + { + b3Vec3 points[16]; + points[0] = { -3.13548756, 3.81141949, 237.289047 }; + points[1] = { -16.2333279, -23.4977913, 235.486603 }; + points[2] = { -13.8834839, 6.20244455, 23.7760544 }; + points[3] = { 14.0794125, 4.63170528, 24.9530792 }; + points[4] = { 3.98322797, -16.4192238, 236.704071 }; + points[5] = { -23.3520412, -3.26714420, 236.071594 }; + points[6] = { 13.4517860, -6.94963741, 24.4085312 }; + points[7] = { -5.24953651, 13.9316301, 24.5058060 }; + points[8] = { -4.65071201, -24.1484108, 235.974121 }; + points[9] = { -14.5111103, -5.37889385, 23.2315063 }; + points[10] = { 6.33307076, 13.2810068, 24.9935150 }; + points[11] = { 4.81784487, -14.6788225, 23.6787796 }; + points[12] = { -14.7180958, 4.46204281, 236.801331 }; + points[13] = { -23.9796677, -14.8484812, 235.527039 }; + points[14] = { 4.61085415, -4.83788204, 237.248611 }; + points[15] = { -6.76476669, -14.0281992, 23.1910706 }; + + for ( int i = 0; i < 16; ++i ) + { + b3Vec3 p = points[i]; + points[i] = b3MulSV( 0.01f, b3Vec3{ p.y, p.z, p.x } ); + } + + b3Hull* hull = b3CreateHull( points, 16, 16 ); + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "cylinder"; + bodyDef.type = b3_dynamicBody; + bodyDef.position = b3Vec3{ 5020.27734, 3506.22559, -6986.48584 }; + bodyDef.rotation = { { 0.664546967, 0.669287264, 0.135021493 }, 0.303646326 }; + + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.rollingResistance = 0.1f; + b3CreateHullShape( bodyId, &shapeDef, hull ); + b3DestroyHull( hull ); + } + + // float jointFrictionTorque = 5.0f; + // float jointHertz = 1.0f; + // float jointDampingRatio = 0.7f; + + m_human = {}; + // CreateHuman( &m_human, m_worldId, b3Vec3{ 15.0f, 10.0f, 15.0f } + origin, jointFrictionTorque, jointHertz, + // jointDampingRatio, 1, nullptr, false ); + } + + ~VoxelMesh() override + { + b3DestroyMesh( m_meshData ); + } + + void Step() override + { + Sample::Step(); + } + + static Sample* Create( SampleContext* context ) + { + return new VoxelMesh( context ); + } + + b3MeshData* m_meshData; + Human m_human; +}; + +static int sampleVoxelMesh = SampleManager::Register( "Mesh", "Voxel", VoxelMesh::Create ); diff --git a/samples/sample_ragdoll.cpp b/samples/sample_ragdoll.cpp new file mode 100644 index 0000000..86b31d2 --- /dev/null +++ b/samples/sample_ragdoll.cpp @@ -0,0 +1,484 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "camera.h" +#include "human.h" +#include "imgui.h" +#include "sample.h" +#include "scene.h" + +#include "box3d/box3d.h" + +class RagdollOnBox : public Sample +{ +public: + explicit RagdollOnBox( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 6.0f, b3Vec3_zero ); + EnableGrid( m_scene, true ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeBoxHull( 10.0f, 1.0f, 10.0f ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + + m_jointFrictionTorque = 5.0f; + m_jointHertz = 1.0f; + m_jointDampingRatio = 0.7f; + + m_human = {}; + + Spawn(); + } + + void Spawn() + { + CreateHuman( &m_human, m_worldId, { 0.0f, 2.0f, 0.0f }, m_jointFrictionTorque, m_jointHertz, m_jointDampingRatio, 1, + nullptr, false ); + // Human_ApplyRandomAngularImpulse( &m_human, 10.0f ); + } + + void UpdateUI() override + { + float height = 140.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 350.0f, height ) ); + + ImGui::Begin( "Ragdoll on Box", nullptr, ImGuiWindowFlags_NoResize ); + ImGui::PushItemWidth( 200.0f ); + + if ( ImGui::SliderFloat( "Joint Friction", &m_jointFrictionTorque, 0.0f, 20.0f, "%3.0f" ) ) + { + Human_SetJointFrictionTorque( &m_human, m_jointFrictionTorque ); + } + + if ( ImGui::SliderFloat( "Hertz", &m_jointHertz, 0.0f, 20.0f, "%3.1f" ) ) + { + Human_SetJointSpringHertz( &m_human, m_jointHertz ); + } + + if ( ImGui::SliderFloat( "Damping", &m_jointDampingRatio, 0.0f, 4.0f, "%3.1f" ) ) + { + Human_SetJointDampingRatio( &m_human, m_jointDampingRatio ); + } + + if ( ImGui::Button( "Respawn" ) ) + { + DestroyHuman( &m_human ); + Spawn(); + } + ImGui::PopItemWidth(); + ImGui::End(); + } + + static Sample* Create( SampleContext* context ) + { + return new RagdollOnBox( context ); + } + + Human m_human; + float m_jointFrictionTorque; + float m_jointHertz; + float m_jointDampingRatio; +}; + +static int sampleRagdollOnBox = SampleManager::Register( "Ragdoll", "Box", RagdollOnBox::Create ); + +class RagdollOnMesh : public Sample +{ +public: + explicit RagdollOnMesh( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 6.0f, b3Vec3_zero ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + m_groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + m_groundMesh = b3CreateGridMesh( 20, 20, 2.0f, 2, true ); + b3CreateMeshShape( m_groundId, &shapeDef, m_groundMesh, b3Vec3_one ); + } + + { + b3Transform transform; + transform.p = { 0.0f, 5.0f, -20.0f }; + transform.q = b3Quat_identity; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 20.0f, 5.0f, 0.1f, transform ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( m_groundId, &shapeDef, &wallBox.base ); + } + + { + b3Transform transform; + transform.p = { 0.0f, 5.0f, 20.0f }; + transform.q = b3Quat_identity; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 20.0f, 5.0f, 0.1f, transform ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( m_groundId, &shapeDef, &wallBox.base ); + } + + { + b3Transform transform; + transform.p = { -20.0f, 5.0f, 0.0f }; + transform.q = b3Quat_identity; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 0.1f, 5.0f, 20.0f, transform ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( m_groundId, &shapeDef, &wallBox.base ); + } + + { + b3Transform transform; + transform.p = { 20.0f, 5.0f, 0.0f }; + transform.q = b3Quat_identity; + b3BoxHull wallBox = b3MakeTransformedBoxHull( 0.1f, 5.0f, 20.0f, transform ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( m_groundId, &shapeDef, &wallBox.base ); + } + + m_jointFrictionTorque = 5.0f; + m_jointHertz = 2.0f; + m_jointDampingRatio = 0.7f; + + m_human = {}; + + Spawn(); + } + + ~RagdollOnMesh() override + { + b3DestroyMesh( m_groundMesh ); + } + + void Spawn() + { + CreateHuman( &m_human, m_worldId, { 0.0f, 1.0f, 0.0f }, m_jointFrictionTorque, m_jointHertz, m_jointDampingRatio, 1, + nullptr, false ); + // Human_AlignSpring( &m_human, m_worldId, m_groundId, 25.0f, 1.0f ); + // Human_ApplyRandomAngularImpulse( &m_human, 10.0f ); + // b3Body_SetType( m_human.bones[bone_thigh_l].bodyId, b3_kinematicBody ); + // b3Body_SetType( m_human.bones[bone_thigh_r].bodyId, b3_kinematicBody ); + // b3Body_SetType( m_human.bones[bone_pelvis].bodyId, b3_kinematicBody ); + // Human_CreateMotorAnchors( &m_human, m_worldId ); + Human_CreateParallelAnchors( &m_human, m_worldId ); + } + + void UpdateUI() override + { + float height = 140.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 350.0f, height ) ); + + ImGui::Begin( "Ragdoll", nullptr, ImGuiWindowFlags_NoResize ); + ImGui::PushItemWidth( 200.0f ); + + if ( ImGui::SliderFloat( "Joint Friction", &m_jointFrictionTorque, 0.0f, 20.0f, "%3.0f" ) ) + { + Human_SetJointFrictionTorque( &m_human, m_jointFrictionTorque ); + } + + if ( ImGui::SliderFloat( "Hertz", &m_jointHertz, 0.0f, 20.0f, "%3.1f" ) ) + { + Human_SetJointSpringHertz( &m_human, m_jointHertz ); + } + + if ( ImGui::SliderFloat( "Damping", &m_jointDampingRatio, 0.0f, 4.0f, "%3.1f" ) ) + { + Human_SetJointDampingRatio( &m_human, m_jointDampingRatio ); + } + + if ( ImGui::Button( "Respawn" ) ) + { + DestroyHuman( &m_human ); + Spawn(); + } + ImGui::PopItemWidth(); + ImGui::End(); + } + + static Sample* Create( SampleContext* context ) + { + return new RagdollOnMesh( context ); + } + + b3MeshData* m_groundMesh; + b3BodyId m_groundId; + Human m_human; + float m_jointFrictionTorque; + float m_jointHertz; + float m_jointDampingRatio; +}; + +static int sampleRagdollMesh = SampleManager::Register( "Ragdoll", "Mesh", RagdollOnMesh::Create ); + +class RagdollPile : public Sample +{ +public: + enum + { +#ifdef NDEBUG + e_count = 20 +#else + e_count = 8 +#endif + }; + + explicit RagdollPile( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 180.0f, 30.0f, 20.0f, b3Vec3_zero ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + m_groundMesh = b3CreateGridMesh( 20, 20, 1.0f, 1, true ); + b3CreateMeshShape( groundId, &shapeDef, m_groundMesh, b3Vec3_one ); + + for ( int i = 0; i < e_count; ++i ) + { + b3Vec3 position = { 0.1f * i, 2.0f + 0.5f * i, -0.1f * i }; + float torque = 10.0f; + float hertz = 0.5f; + float damping = 0.7f; + int groupIndex = i; + void* userData = nullptr; + bool colorize = false; + CreateHuman( m_humans + i, m_worldId, position, torque, hertz, damping, groupIndex, userData, colorize ); + } + } + + ~RagdollPile() override + { + b3DestroyMesh( m_groundMesh ); + } + + static Sample* Create( SampleContext* context ) + { + return new RagdollPile( context ); + } + + b3MeshData* m_groundMesh; + Human m_humans[e_count] = {}; +}; + +static int sampleRagdollPile = SampleManager::Register( "Ragdoll", "Pile", RagdollPile::Create ); + +class RagdollIncline : public Sample +{ +public: + explicit RagdollIncline( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( -20.0f, 30.0f, 25.0f, b3Vec3_zero ); + } + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + m_groundMesh = b3CreateGridMesh( 4, 4, 2.0f, 1, true ); + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { -10.0f, 2.0f, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, -0.2f * B3_PI ); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateMeshShape( groundId, &shapeDef, m_groundMesh, b3Vec3_one ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, 0.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + b3Vec3 scale = { 4.0f, 4.0f, 4.0f }; + b3CreateMeshShape( groundId, &shapeDef, m_groundMesh, scale ); + } + + m_human = {}; + b3Vec3 position = { -12.0f, 6.0f, 0.0f }; + float torque = 10.0f; + float hertz = 2.0f; + float damping = 0.7f; + int groupIndex = 1; + void* userData = nullptr; + bool colorize = false; + CreateHuman( &m_human, m_worldId, position, torque, hertz, damping, groupIndex, userData, colorize ); + m_time = 0.0f; + m_motorized = true; + } + + ~RagdollIncline() override + { + b3DestroyMesh( m_groundMesh ); + } + + void Step() override + { + if ( m_time > 2.0f && m_motorized == true ) + { + Human_SetJointFrictionTorque( &m_human, 0.5f ); + Human_SetJointSpringHertz( &m_human, 0.5f ); + m_motorized = false; + } + + m_time += m_context->hertz > 0.0f ? 1.0f / m_context->hertz : 0.0f; + + Sample::Step(); + } + + static Sample* Create( SampleContext* context ) + { + return new RagdollIncline( context ); + } + + b3MeshData* m_groundMesh; + Human m_human; + float m_time; + bool m_motorized; +}; + +static int sampleRagdollIncline = SampleManager::Register( "Ragdoll", "Incline", RagdollIncline::Create ); + +#if 0 +class RagdollPose : public Sample +{ +public: + static Sample* Create( SampleContext* context ) + { + return new RagdollPose( context ); + } + + explicit RagdollPose( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 6.0f, b3Vec3_zero ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, 0.0f, 0.0f }; + // bodyDef.rotation = b3Rotation(B3_VEC3_AXIS_Y, 0.25f * B3_PI); + b3BodyId groundId = m_worldId->CreateBody( &bodyDef ); + m_ground = b3CreateGrid( 4, 2.0f, 0.0f ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + groundId->AddMesh( &shapeDef, m_ground ); + + b3Hull* Hull = b3CreateOffsetBox( { { -3.0f, 0.5f, 0.0f }, b3Quat_identity }, { 0.25f, 0.5f, 3.0f } ); + groundId->AddHull( &shapeDef, Hull ); + b3DestroyHull( Hull ); + + m_motorized = false; + m_motorHertz = 1.0f; + m_human.Spawn( m_worldId, { 0.0f, 0.1f, 0.0f }, m_motorHertz, m_motorized ); + + m_poseControl = true; + m_poseHertz = 2.0f; + m_human.EnablePoseControl( m_worldId, m_poseHertz, m_poseControl ); + + m_time = 0.0f; + + { + b3Hull* Cylinder = b3CreateCylinder( 0.5f, 0.5f, 0.0f, 16 ); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 3.0f, 0.5f, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( B3_VEC3_AXIS_Z, 0.5f * B3_PI ); + b3BodyId Body = m_worldId->CreateBody( &bodyDef ); + Body->AddHull( &shapeDef, Cylinder ); + b3DestroyHull( Cylinder ); + } + + m_angle = 0.0f; + m_angularVelocity = 0.0f; + } + + ~RagdollPose() override + { + b3DestroyMesh( m_ground ); + } + + void OnRenderUI( GLFWwindow* ) override + { + ImGui::SetNextWindowPos( ImVec2( 10.0f, 600.0f ) ); + ImGui::SetNextWindowSize( ImVec2( 260.0f, 160.0f ) ); + ImGui::Begin( "Pose", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + if ( ImGui::Checkbox( "motors", &m_motorized ) ) + { + m_human.EnableMotors( m_motorized ); + m_human.AdjustMotors( m_motorHertz ); + } + + if ( ImGui::SliderFloat( "motor hertz", &m_motorHertz, 0.01f, 20.0f, "%.2f" ) ) + { + m_human.AdjustMotors( m_motorHertz ); + } + + if ( ImGui::Checkbox( "pose", &m_poseControl ) ) + { + m_human.EnablePoseControl( m_worldId, m_poseHertz, m_poseControl ); + } + + if ( ImGui::SliderFloat( "pose hertz", &m_poseHertz, 0.01f, 8.0f, "%.2f" ) ) + { + m_human.AdjustPoseControl( m_poseHertz ); + } + + ImGui::SliderFloat( "omega", &m_angularVelocity, 0.0f, 8.0f, "%.1f" ); + + ImGui::End(); + } + + void OnUpdate() override + { + float timeStep = 1.0f / m_context->Settings.Frequency; + m_time += timeStep; + + if ( m_poseControl ) + { + b3Transform transform = m_human.m_rootBody->GetTransform(); + transform.p.x = 4.0f * sinf( 0.5f * m_time ); + transform.p.y = 0.5f * ( cosf( 1.0f * m_time + B3_PI ) + 1.0f ); + m_angle += m_angularVelocity * timeStep; + transform.q = b3MakeQuatFromAxisAngle( B3_VEC3_AXIS_Y, m_angle ); + m_human.DriveBase( transform, timeStep ); + } + + // transform = mHuman.m_bones[0].poseJoint->GetRelativeTransform(); + // transform.Translation.Y = 0.5f * ( cosf( 1.0f * mTime + B3_PI ) + 1.0f ); + // mHuman.m_bones[0].poseJoint->SetRelativeTransform( transform ); + + Sample::OnUpdate(); + } + + b3MeshData* m_ground; + Human m_human; + float m_time; + float m_motorHertz; + float m_poseHertz; + float m_angle; + float m_angularVelocity; + bool m_motorized; + bool m_poseControl; +}; + +static int sampleRagodllPose = SampleManager::Register( "Ragdoll", "Pose", RagdollPose::Create ); +#endif diff --git a/samples/sample_robustness.cpp b/samples/sample_robustness.cpp new file mode 100644 index 0000000..f32befb --- /dev/null +++ b/samples/sample_robustness.cpp @@ -0,0 +1,404 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "camera.h" +#include "sample.h" +#include "scene.h" + +#include "box3d/box3d.h" + +#include +#include + +// Pyramid with heavy box on top +class HighMassRatio1 : public Sample +{ +public: + explicit HighMassRatio1( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 45.0f, 20.0f, 50.0f, b3Vec3_zero ); + EnableGrid( m_scene, true ); + } + + float extent = 1.0f; + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeTransformedBoxHull( 50.0f, 1.0f, 50.0f, { { 0.0f, -1.0f, 0.0f }, b3Quat_identity } ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + b3BoxHull box = b3MakeBoxHull( extent, extent, extent ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + for ( int j = 0; j < 3; ++j ) + { + int count = 10; + float offset = -20.0f * extent + 2.0f * ( count + 1.0f ) * extent * j; + float y = extent; + while ( count > 0 ) + { + for ( int i = 0; i < count; ++i ) + { + float coeff = i - 0.5f * count; + + float yy = count == 1 ? y + 2.0f : y; + bodyDef.position = { 2.0f * coeff * extent + offset, yy, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + shapeDef.density = count == 1 ? ( j + 1.0f ) * 100.0f : 1.0f; + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + + --count; + y += 2.0f * extent; + } + } + } + } + + static Sample* Create( SampleContext* context ) + { + return new HighMassRatio1( context ); + } +}; + +static int sampleHighMassRatio1 = SampleManager::Register( "Robustness", "HighMassRatio1", HighMassRatio1::Create ); + +class OverlapRecovery : public Sample +{ +public: + explicit OverlapRecovery( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 45.0f, 20.0f, 50.0f, b3Vec3_zero ); + EnableGrid( m_scene, true ); + } + + m_bodyIds = nullptr; + m_bodyCount = 0; + m_baseCount = 4; + m_overlap = 0.25f; + m_extent = 0.5f; + m_speed = 3.0f; + m_hertz = 30.0f; + m_dampingRatio = 10.0f; + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeTransformedBoxHull( 20.0f, 1.0f, 20.0f, { { 0.0f, -1.0f, 0.0f }, b3Quat_identity } ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + + CreateScene(); + } + + ~OverlapRecovery() override + { + free( m_bodyIds ); + } + + void CreateScene() + { + for ( int i = 0; i < m_bodyCount; ++i ) + { + b3DestroyBody( m_bodyIds[i] ); + } + + b3World_SetContactTuning( m_worldId, m_hertz, m_dampingRatio, m_speed ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + b3BoxHull box = b3MakeBoxHull( m_extent, m_extent, m_extent ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + + m_bodyCount = m_baseCount * ( m_baseCount + 1 ) / 2; + m_bodyIds = (b3BodyId*)realloc( m_bodyIds, m_bodyCount * sizeof( b3BodyId ) ); + + int bodyIndex = 0; + float fraction = 1.0f - m_overlap; + float y = m_extent; + for ( int i = 0; i < m_baseCount; ++i ) + { + float x = fraction * m_extent * ( i - m_baseCount ); + for ( int j = i; j < m_baseCount; ++j ) + { + bodyDef.position = { x, y }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + m_bodyIds[bodyIndex++] = bodyId; + + x += 2.0f * fraction * m_extent; + } + + y += 2.0f * fraction * m_extent; + } + + assert( bodyIndex == m_bodyCount ); + } + + void UpdateUI() override + { + float height = 220.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_context->camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 220.0f, height ) ); + + ImGui::Begin( "Overlap Recovery", nullptr, ImGuiWindowFlags_NoResize ); + ImGui::PushItemWidth( 100.0f ); + + bool changed = false; + changed = changed || ImGui::SliderFloat( "Extent", &m_extent, 0.1f, 1.0f, "%.1f" ); + changed = changed || ImGui::SliderInt( "Base Count", &m_baseCount, 1, 10 ); + changed = changed || ImGui::SliderFloat( "Overlap", &m_overlap, 0.0f, 1.0f, "%.2f" ); + changed = changed || ImGui::SliderFloat( "Speed", &m_speed, 0.0f, 10.0f, "%.1f" ); + changed = changed || ImGui::SliderFloat( "Hertz", &m_hertz, 0.0f, 240.0f, "%.f" ); + changed = changed || ImGui::SliderFloat( "Damping Ratio", &m_dampingRatio, 0.0f, 20.0f, "%.1f" ); + changed = changed || ImGui::Button( "Reset Scene" ); + + if ( changed ) + { + CreateScene(); + } + + ImGui::PopItemWidth(); + ImGui::End(); + } + + static Sample* Create( SampleContext* context ) + { + return new OverlapRecovery( context ); + } + + b3BodyId* m_bodyIds; + int m_bodyCount; + int m_baseCount; + float m_overlap; + float m_extent; + float m_speed; + float m_hertz; + float m_dampingRatio; +}; + +static int sampleOverlapRecovery = SampleManager::Register( "Robustness", "Overlap Recovery", OverlapRecovery::Create ); + +class Cart : public Sample +{ +public: + explicit Cart( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 15.0f, { 0.0f, 2.0f, 0.0f } ); + //context->settings.subStepCount = 12; + EnableGrid( m_scene, true ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull groundBox = b3MakeBoxHull( 20.0f, 1.0f, 20.0f ); + b3CreateHullShape( groundId, &shapeDef, &groundBox.base ); + } + + b3World_SetGravity( m_worldId, { 0, -22, 0 } ); + + m_contactHertz = 240.0f; + m_contactDampingRatio = 10.0f; + m_contactSpeed = 0.5f; + b3World_SetContactTuning( m_worldId, m_contactHertz, m_contactDampingRatio, m_contactSpeed ); + + m_constraintHertz = 240.0f; + m_constraintDampingRatio = 0.0f; + + m_chassisId = {}; + m_wheelId1 = {}; + m_wheelId2 = {}; + m_wheelId3 = {}; + m_wheelId4 = {}; + m_jointId1 = {}; + m_jointId2 = {}; + m_jointId3 = {}; + m_jointId4 = {}; + + CreateScene(); + } + + void CreateScene() + { + if ( B3_IS_NON_NULL( m_chassisId ) ) + { + b3DestroyBody( m_chassisId ); + } + + if ( B3_IS_NON_NULL( m_wheelId1 ) ) + { + b3DestroyBody( m_wheelId1 ); + } + + if ( B3_IS_NON_NULL( m_wheelId2 ) ) + { + b3DestroyBody( m_wheelId2 ); + } + + if ( B3_IS_NON_NULL( m_wheelId3 ) ) + { + b3DestroyBody( m_wheelId3 ); + } + + if ( B3_IS_NON_NULL( m_wheelId4 ) ) + { + b3DestroyBody( m_wheelId4 ); + } + + float yBase = 1.0f; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, yBase, 0.0f }; + m_chassisId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1000.0f; + + b3BoxHull box = b3MakeTransformedBoxHull( 1.0f, 0.25f, 0.5f, { { 0.0f, 0.25f, 0.0f }, b3Quat_identity } ); + b3CreateHullShape( m_chassisId, &shapeDef, &box.base ); + + shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.rollingResistance = 0.02f; + shapeDef.density = 200.0f; + + b3Sphere sphere = { b3Vec3_zero, 0.1f }; + bodyDef.position = { -0.9f, yBase - 0.15f, 0.4f }; + m_wheelId1 = b3CreateBody( m_worldId, &bodyDef ); + b3CreateSphereShape( m_wheelId1, &shapeDef, &sphere ); + + bodyDef.position = { 0.9f, yBase - 0.15f, 0.4f }; + m_wheelId2 = b3CreateBody( m_worldId, &bodyDef ); + b3CreateSphereShape( m_wheelId2, &shapeDef, &sphere ); + + bodyDef.position = { 0.9f, yBase - 0.15f, -0.4f }; + m_wheelId3 = b3CreateBody( m_worldId, &bodyDef ); + b3CreateSphereShape( m_wheelId3, &shapeDef, &sphere ); + + bodyDef.position = { -0.9f, yBase - 0.15f, -0.4f }; + m_wheelId4 = b3CreateBody( m_worldId, &bodyDef ); + b3CreateSphereShape( m_wheelId4, &shapeDef, &sphere ); + + b3SphericalJointDef jointDef = b3DefaultSphericalJointDef(); + jointDef.base.constraintHertz = m_constraintHertz; + jointDef.base.constraintDampingRatio = m_constraintDampingRatio; + jointDef.base.drawScale = 0.5f; + + jointDef.base.bodyIdA = m_chassisId; + jointDef.base.bodyIdB = m_wheelId1; + jointDef.base.localFrameA.p = { -0.9f, -0.15f, 0.4f }; + jointDef.base.localFrameB.p = { 0.0f, 0.0f, 0.0f }; + m_jointId1 = b3CreateSphericalJoint( m_worldId, &jointDef ); + + jointDef.base.bodyIdA = m_chassisId; + jointDef.base.bodyIdB = m_wheelId2; + jointDef.base.localFrameA.p = { 0.9f, -0.15f, 0.4f }; + jointDef.base.localFrameB.p = { 0.0f, 0.0f, 0.0f }; + m_jointId2 = b3CreateSphericalJoint( m_worldId, &jointDef ); + + jointDef.base.bodyIdA = m_chassisId; + jointDef.base.bodyIdB = m_wheelId3; + jointDef.base.localFrameA.p = { 0.9f, -0.15f, -0.4f }; + jointDef.base.localFrameB.p = { 0.0f, 0.0f, 0.0f }; + m_jointId3 = b3CreateSphericalJoint( m_worldId, &jointDef ); + + jointDef.base.bodyIdA = m_chassisId; + jointDef.base.bodyIdB = m_wheelId4; + jointDef.base.localFrameA.p = { -0.9f, -0.15f, -0.4f }; + jointDef.base.localFrameB.p = { 0.0f, 0.0f, 0.0f }; + m_jointId4 = b3CreateSphericalJoint( m_worldId, &jointDef ); + } + + void UpdateUI() override + { + float height = 240.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_context->camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 320.0f, height ) ); + + ImGui::Begin( "Cart", nullptr, ImGuiWindowFlags_NoResize ); + ImGui::PushItemWidth( 200.0f ); + + bool changed = false; + ImGui::Text( "Contact" ); + changed = changed || ImGui::SliderFloat( "Hertz##contact", &m_contactHertz, 0.0f, 240.0f, "%.f" ); + changed = changed || ImGui::SliderFloat( "Damping Ratio##contact", &m_contactDampingRatio, 0.0f, 100.0f, "%.f" ); + changed = changed || ImGui::SliderFloat( "Speed", &m_contactSpeed, 0.0f, 5.0f, "%.1f" ); + + if ( changed ) + { + b3World_SetContactTuning( m_worldId, m_contactHertz, m_contactDampingRatio, m_contactSpeed ); + CreateScene(); + } + + ImGui::Separator(); + + changed = false; + ImGui::Text( "Joint" ); + changed = changed || ImGui::SliderFloat( "Hertz##joint", &m_constraintHertz, 0.0f, 240.0f, "%.f" ); + changed = changed || ImGui::SliderFloat( "Damping Ratio##joint", &m_constraintDampingRatio, 0.0f, 20.0f, "%.f" ); + + ImGui::Separator(); + + changed = changed || ImGui::Button( "Reset Scene" ); + + if ( changed ) + { + b3Joint_SetConstraintTuning( m_jointId1, m_constraintHertz, m_constraintDampingRatio ); + b3Joint_SetConstraintTuning( m_jointId2, m_constraintHertz, m_constraintDampingRatio ); + b3Joint_SetConstraintTuning( m_jointId3, m_constraintHertz, m_constraintDampingRatio ); + b3Joint_SetConstraintTuning( m_jointId4, m_constraintHertz, m_constraintDampingRatio ); + CreateScene(); + } + + ImGui::PopItemWidth(); + ImGui::End(); + } + + static Sample* Create( SampleContext* context ) + { + return new Cart( context ); + } + + b3BodyId m_chassisId; + b3BodyId m_wheelId1; + b3BodyId m_wheelId2; + b3BodyId m_wheelId3; + b3BodyId m_wheelId4; + b3JointId m_jointId1; + b3JointId m_jointId2; + b3JointId m_jointId3; + b3JointId m_jointId4; + + float m_contactHertz; + float m_contactDampingRatio; + float m_contactSpeed; + float m_constraintHertz; + float m_constraintDampingRatio; +}; + +static int sampleCart = SampleManager::Register( "Robustness", "Cart", Cart::Create ); diff --git a/samples/sample_shapes.cpp b/samples/sample_shapes.cpp new file mode 100644 index 0000000..3ec2f61 --- /dev/null +++ b/samples/sample_shapes.cpp @@ -0,0 +1,1145 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "camera.h" +#include "human.h" +#include "imgui.h" +#include "mesh_loader.h" +#include "utils.h" +#include "renderer.h" +#include "sample.h" +#include "scene.h" + +#include "box3d/box3d.h" + +class InclinedPlane : public Sample +{ +public: + explicit InclinedPlane( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( -55.0f, 30.0f, 60.0f, { 0.0f, 7.5f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundBody = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 1.0f; + b3BoxHull groundBox = b3MakeBoxHull( 50.0f, 1.0f, 50.0f ); + b3CreateHullShape( groundBody, &shapeDef, &groundBox.base ); + + bodyDef.position = { 0.0f, 7.5f, -5.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( { 1.0f, 0.0f, 0.0f }, 40.0f * B3_DEG_TO_RAD ); + b3BodyId planeBody = b3CreateBody( m_worldId, &bodyDef ); + b3BoxHull planeBox = b3MakeBoxHull( 16.0f, 0.5f, 10.0f ); + shapeDef.baseMaterial.friction = 1.0f; + b3CreateHullShape( planeBody, &shapeDef, &planeBox.base ); + + b3BoxHull box = b3MakeBoxHull( 1.0f, 1.0f, 1.0f ); + bodyDef.type = b3_dynamicBody; + for ( int index = 0; index < m_boxCount; ++index ) + { + bodyDef.position = { -10.0f + 5.0f * index, 15.75f, -10.6f }; + b3BodyId boxBody = b3CreateBody( m_worldId, &bodyDef ); + shapeDef.baseMaterial.friction = ( index + 1 ) * ( index + 1 ) * 0.04f; + b3CreateHullShape( boxBody, &shapeDef, &box.base ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new InclinedPlane( context ); + } + + static constexpr int m_boxCount = 5; +}; + +static int sampleInclinedPlane = SampleManager::Register( "Shapes", "Inclined Plane", InclinedPlane::Create ); + +class RollingResistance : public Sample +{ +public: + explicit RollingResistance( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( -140.0f, 17.0f, 60.0f, { 0.0f, 7.5f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundBody = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull mGround = b3MakeBoxHull( 50.0f, 1.0f, 50.0f ); + b3CreateHullShape( groundBody, &shapeDef, &mGround.base ); + + bodyDef.position = { 0.0f, 2.0f, -20.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( { 1.0f, 0.0f, 0.0f }, 10.0f * B3_DEG_TO_RAD ); + b3BodyId planeBody = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull mPlane = b3MakeBoxHull( 32.0f, 0.5f, 15.0f ); + b3CreateHullShape( planeBody, &shapeDef, &mPlane.base ); + + b3Sphere sphere = { b3Vec3_zero, 1.0f }; + bodyDef.type = b3_dynamicBody; + for ( int index = 0; index < m_count; ++index ) + { + bodyDef.position = { -25.0f + 5.0f * index, 8.0f, -24.0f }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + + shapeDef.baseMaterial.rollingResistance = 0.05f * index; + b3CreateSphereShape( body, &shapeDef, &sphere ); + } + + b3Capsule capsule = { { -1.0f, 0.0f, 0.0f }, { 1.0f, 0.0f, 0.0f }, 0.5f }; + bodyDef.type = b3_dynamicBody; + for ( int index = 0; index < m_count; ++index ) + { + bodyDef.position = { 2.0f + 5.0f * index, 8.0f, -24.0f }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + + shapeDef.baseMaterial.rollingResistance = 0.05f * index; + b3CreateCapsuleShape( body, &shapeDef, &capsule ); + } + } + + static constexpr int m_count = 5; + + static Sample* Create( SampleContext* context ) + { + return new RollingResistance( context ); + } +}; + +static int sampleRollingResistance = SampleManager::Register( "Shapes", "Rolling Resistance", RollingResistance::Create ); + +class HighResistance : public Sample +{ +public: + explicit HighResistance( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 5.0f, 40.0f, { 0.0f, 7.5f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundBody = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull mGround = b3MakeBoxHull( 50.0f, 1.0f, 50.0f ); + b3CreateHullShape( groundBody, &shapeDef, &mGround.base ); + + b3Capsule capsule = { { 0.0f, -1.0f, 0.0f }, { 0.0f, 1.0f, 0.0f }, 0.5f }; + bodyDef.type = b3_dynamicBody; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, B3_DEG_TO_RAD * 30.0f ); + for ( int index = 0; index < m_count; ++index ) + { + bodyDef.position = { -22.0f + 5.0f * index, 1.5f, 0.0f }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + + shapeDef.baseMaterial.rollingResistance = 0.2f * index; + b3CreateCapsuleShape( body, &shapeDef, &capsule ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new HighResistance( context ); + } + + static constexpr int m_count = 10; +}; + +static int sampleHighResistance = SampleManager::Register( "Shapes", "High Resistance", HighResistance::Create ); + +class IsotropicFriction : public Sample +{ +public: + explicit IsotropicFriction( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 150.0f, b3Vec3_zero ); + EnableGrid( m_scene, true ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundBody = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull mGround = b3MakeBoxHull( 75.0f, 1.0f, 75.0f ); + shapeDef.baseMaterial.friction = 1.0f; + b3CreateHullShape( groundBody, &shapeDef, &mGround.base ); + + b3BoxHull box = b3MakeBoxHull( 1.0f, 1.0f, 1.0f ); + bodyDef.type = b3_dynamicBody; + shapeDef.baseMaterial.friction = 0.6f; + for ( int index = 0; index < m_boxCount; ++index ) + { + float alpha = B3_PI / 16.0f * index; + b3CosSin cs = b3ComputeCosSin( alpha ); + b3Vec3 position = { 15.0f * cs.cosine, 1.0f, 15.0f * cs.sine }; + b3Quat orientation = b3MakeQuatFromAxisAngle( { 0.0f, 1.0f, 0.0f }, -alpha ); + b3Vec3 velocity = { 25.0f * cs.cosine, 0.0f, 25.0f * cs.sine }; + + bodyDef.position = position; + bodyDef.rotation = orientation; + bodyDef.linearVelocity = velocity; + b3BodyId boxBody = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( boxBody, &shapeDef, &box.base ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new IsotropicFriction( context ); + } + + static constexpr int m_boxCount = 32; +}; + +static int sampleIsotropicFriction = SampleManager::Register( "Shapes", "Isotropic Friction", IsotropicFriction::Create ); + +// todo what is the point of this? +class SlideTwist : public Sample +{ +public: + explicit SlideTwist( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( -30.0f, 17.0f, 22.0f, { 0.0f, 5.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + b3Quat orientation = b3MakeQuatFromAxisAngle( b3Vec3_axisX, 20.0f * B3_DEG_TO_RAD ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundBody = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 1.0f; + b3BoxHull mGround = b3MakeBoxHull( 50.0f, 1.0f, 50.0f ); + b3CreateHullShape( groundBody, &shapeDef, &mGround.base ); + + bodyDef.position = { 0.0f, 4.0f, 0.0f }; + bodyDef.rotation = orientation; + b3BodyId planeBody = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull mPlane = b3MakeBoxHull( 10.0f, 0.5f, 10.0f ); + shapeDef.baseMaterial.friction = 0.6f; + b3CreateHullShape( planeBody, &shapeDef, &mPlane.base ); + + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 5.0f, 0.0f }; + bodyDef.rotation = orientation; + bodyDef.angularVelocity = 25.0f * b3RotateVector( orientation, b3Vec3_axisY ); + b3BodyId boxBody = b3CreateBody( m_worldId, &bodyDef ); + b3BoxHull mBox = b3MakeBoxHull( 1.0f, 0.5f, 1.0f ); + shapeDef.baseMaterial.friction = 0.3f; + b3CreateHullShape( boxBody, &shapeDef, &mBox.base ); + } + + static Sample* Create( SampleContext* context ) + { + return new SlideTwist( context ); + } +}; + +static int sampleSlideTwist = SampleManager::Register( "Shapes", "Slide Twist", SlideTwist::Create ); + +class Restitution : public Sample +{ +public: + enum ShapeType + { + e_sphereShape = 0, + e_boxShape + }; + + explicit Restitution( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 25.0f, 85.0f, { 0.0f, 20.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundBody = b3CreateBody( m_worldId, &bodyDef ); + b3BoxHull groundBox = b3MakeBoxHull( 50.0f, 1.0f, 20.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( groundBody, &shapeDef, &groundBox.base ); + + m_shapeType = e_sphereShape; + + CreateBodies(); + } + + void CreateBodies() + { + for ( int i = 0; i < m_count; ++i ) + { + if ( B3_IS_NON_NULL( m_bodyIds[i] ) ) + { + b3DestroyBody( m_bodyIds[i] ); + m_bodyIds[i] = b3_nullBodyId; + } + } + + b3Sphere sphere = { b3Vec3_zero, 0.5f }; + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + float dr = 1.0f / ( m_count > 1 ? m_count - 1 : 1 ); + float x = -1.0f * ( m_count - 1 ); + float dx = 2.0f; + + for ( int i = 0; i < m_count; ++i ) + { + bodyDef.position = { x, 40.0f, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + m_bodyIds[i] = bodyId; + + if ( m_shapeType == e_sphereShape ) + { + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + } + else + { + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + + shapeDef.baseMaterial.restitution += dr; + x += dx; + } + } + + void UpdateUI() override + { + float height = 100.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Restitution", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::RadioButton( "Sphere", m_shapeType == e_sphereShape ) ) + { + m_shapeType = e_sphereShape; + CreateBodies(); + } + + if ( ImGui::RadioButton( "Box", m_shapeType == e_boxShape ) ) + { + m_shapeType = e_boxShape; + CreateBodies(); + } + + ImGui::End(); + } + + static Sample* Create( SampleContext* context ) + { + return new Restitution( context ); + } + + static constexpr int m_count = 40; + + b3BodyId m_bodyIds[m_count] = {}; + ShapeType m_shapeType; +}; + +static int sampleRestitution = SampleManager::Register( "Shapes", "Restitution", Restitution::Create ); + +// This shows an optimization when creating many static shapes you can skip having them invoke collision, assuming +// dynamic bodies are added after the static bodies. +class StaticInvoke : public Sample +{ +public: + explicit StaticInvoke( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 25.0f, 10.0f, { 0.0f, 1.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundBody = b3CreateBody( m_worldId, &bodyDef ); + b3BoxHull groundBox = b3MakeBoxHull( 20.0f, 1.0f, 20.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( groundBody, &shapeDef, &groundBox.base ); + + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.25f, 1.0f, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + shapeDef.baseMaterial.rollingResistance = 0.2f; + b3Sphere sphere = { b3Vec3_zero, 0.5f }; + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + + m_invoke = false; + m_bodyId = {}; + } + + void CreateStatic() + { + if ( B3_IS_NON_NULL( m_bodyId ) ) + { + b3DestroyBody( m_bodyId ); + m_bodyId = b3_nullBodyId; + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, 0.5f, 0.0f }; + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3Sphere sphere = { b3Vec3_zero, 0.5f }; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.invokeContactCreation = m_invoke; + + b3CreateSphereShape( m_bodyId, &shapeDef, &sphere ); + } + + void UpdateUI() override + { + float height = 150.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, m_camera->m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Restitution", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::RadioButton( "Invoke", m_invoke == true ) ) + { + m_invoke = true; + } + + if ( ImGui::RadioButton( "Passive", m_invoke == false ) ) + { + m_invoke = false; + } + + if ( B3_IS_NULL( m_bodyId ) ) + { + if ( ImGui::Button( "Create" ) ) + { + CreateStatic(); + } + } + else + { + if ( ImGui::Button( "Destroy" ) ) + { + b3DestroyBody( m_bodyId ); + m_bodyId = {}; + } + } + + ImGui::End(); + } + + void Render() override + { + Sample::Render(); + b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 4.0f ); + } + + void Step() override + { + Sample::Step(); + + if ( m_stepCount == 20 ) + { + CreateStatic(); + } + } + + static Sample* Create( SampleContext* context ) + { + return new StaticInvoke( context ); + } + + b3BodyId m_bodyId; + bool m_invoke; +}; + +static int sampleStaticInvoke = SampleManager::Register( "Shapes", "Static Invoke", StaticInvoke::Create ); + +class ConveyorBelt : public Sample +{ +public: + explicit ConveyorBelt( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 25.0f, 40.0f, { 0.0f, 1.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + // Ground + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull groundBox = b3MakeBoxHull( 20.0f, 1.0f, 20.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( groundId, &shapeDef, &groundBox.base ); + } + + // Platform + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { -5.0f, 5.0f, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisY, 0.2f ); + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 10.0f, 0.25f, 2.0f ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.8f; + shapeDef.baseMaterial.tangentVelocity = { 2.0f, 0.0f, 0.0f }; + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + + // Boxes + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull cube = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + for ( int i = 0; i < 5; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { -10.0f + 2.0f * i, 7.0f, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3CreateHullShape( bodyId, &shapeDef, &cube.base ); + } + } + + void Render() override + { + Sample::Render(); + b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 4.0f ); + } + + static Sample* Create( SampleContext* context ) + { + return new ConveyorBelt( context ); + } +}; + +static int sampleConveyorBelt = SampleManager::Register( "Shapes", "Conveyor Belt", ConveyorBelt::Create ); + +class ConveyorMesh : public Sample +{ +public: + explicit ConveyorMesh( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 65.0f, 25.0f, 28.0f, { 0.0f, 1.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + // Ground + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull groundBox = b3MakeBoxHull( 20.0f, 1.0f, 20.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( groundId, &shapeDef, &groundBox.base ); + } + + // Mesh + { + m_meshData = CreateMeshData( "data/meshes/conveyor.obj", 1.0f, false, true, true, true ); + + int triangleCount = m_meshData->triangleCount; + + uint8_t* materialIndices = (uint8_t*)( (intptr_t)m_meshData + m_meshData->materialOffset ); + + memset( materialIndices, 0, triangleCount * sizeof( uint8_t ) ); + m_velocities[0] = { 0.0f, 0.0f, 0.0f }; + + // +x -z + materialIndices[0] = 1; + materialIndices[4] = 1; + m_velocities[1] = { 0.7f, 0.0f, -0.2f }; + + // +x +z + materialIndices[9] = 2; + materialIndices[12] = 2; + m_velocities[2] = { 0.6f, 0.0f, 0.4f }; + + // +z + materialIndices[21] = 3; + materialIndices[38] = 3; + m_velocities[3] = { 0.0f, 0.0f, 1.3f }; + + // -x +z + materialIndices[43] = 4; + materialIndices[46] = 4; + m_velocities[4] = { -0.6f, 0.0f, 0.4f }; + + // -x -z + materialIndices[30] = 5; + materialIndices[33] = 5; + m_velocities[5] = { -0.75f, 0.0f, -0.4f }; + + // -z + materialIndices[18] = 6; + materialIndices[24] = 6; + m_velocities[6] = { 0.0f, 0.0f, -1.3f }; + + b3SurfaceMaterial materials[7]; + for ( int i = 0; i < 7; ++i ) + { + materials[i] = b3DefaultSurfaceMaterial(); + materials[i].friction = 0.8f; + materials[i].tangentVelocity = 2.0f * m_velocities[i]; + } + + m_meshTransform.p = { 0.0f, 0.5f, 6.0f }; + m_meshTransform.q = b3MakeQuatFromAxisAngle( b3Vec3_axisY, 0.5f * B3_PI ); + // m_meshTransform.q = b3Quat_identity; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = m_meshTransform.p; + bodyDef.rotation = m_meshTransform.q; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.materials = materials; + shapeDef.materialCount = 7; + b3CreateMeshShape( bodyId, &shapeDef, m_meshData, b3Vec3_one ); + } + + // High number of sides to stress the collision code + // Normally the number of sides should be 16 or less. + m_cylinderHull = b3CreateCylinder( 0.3f, 0.15f, 0.0f, 32 ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + // Cylinders + for ( int i = 0; i < 20; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { -8.5f + 0.9f * i, 1.5f, -5.5f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3CreateHullShape( bodyId, &shapeDef, m_cylinderHull ); + } + +#if 0 + // Boxes + b3BoxHull cube = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + for ( int i = 0; i < 5; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { -8.5f + 3.0f * i, 2.0f, -7.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3CreateHullShape( bodyId, &shapeDef, &cube.base ); + } + b3Sphere sphere = { b3Vec3_zero, 0.25f }; + shapeDef.baseMaterial.rollingResistance = 0.1f; + for ( int i = 0; i < 10; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { -8.5f + 2.0f * i, 2.0f, -6.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + } +#endif + +#if 0 + for ( int i = 0; i < 10; ++i ) + { + Human human = {}; + CreateHuman( &human, m_worldId, { -8.0f + 2.0f * i, 3.0f, -5.5f }, 10.0f, 1.0f, 1.0f, 0, nullptr, true ); + } +#endif + } + + ~ConveyorMesh() override + { + b3DestroyMesh( m_meshData ); + b3DestroyHull( m_cylinderHull ); + } + + void Render() override + { + Sample::Render(); + + int triangleCount = m_meshData->triangleCount; + const b3MeshTriangle* triangles = b3GetMeshTriangles( m_meshData ); + const b3Vec3* vertices = b3GetMeshVertices( m_meshData ); + const uint8_t* materialIndices = b3GetMeshMaterialIndices( m_meshData ); + + for ( int i = 0; i < triangleCount; ++i ) + { + const b3MeshTriangle* t = triangles + i; + b3Vec3 v1 = vertices[t->index1]; + b3Vec3 v2 = vertices[t->index2]; + b3Vec3 v3 = vertices[t->index3]; + + b3Vec3 n = b3Cross( v2 - v1, v3 - v1 ); + n = b3Normalize( n ); + + if ( n.y < 0.9f ) + { + continue; + } + + b3Vec3 p = 1.0f / 3.0f * ( v1 + v2 + v3 ); + p = b3TransformPoint( m_meshTransform, p ); + + DrawWorldString( m_camera, p, b3_colorAqua, "%d", i ); + + int materialIndex = materialIndices[i]; + + b3Vec3 v = b3RotateVector( m_meshTransform.q, m_velocities[materialIndex] ); + DrawLine( m_scene, p, p + v, b3_colorBlueViolet ); + } + + DrawTransform( m_scene, b3Transform_identity, 0.5f ); + } + + static Sample* Create( SampleContext* context ) + { + return new ConveyorMesh( context ); + } + + b3Transform m_meshTransform; + b3MeshData* m_meshData; + b3Hull* m_cylinderHull; + b3Vec3 m_velocities[7]; +}; + +static int sampleConveyorMesh = SampleManager::Register( "Shapes", "Conveyor Mesh", ConveyorMesh::Create ); + +class Wind : public Sample +{ +public: + enum ShapeType + { + e_sphereShape = 0, + e_capsuleShape, + e_boxShape + }; + + explicit Wind( SampleContext* context ) + : Sample( context ) + , m_bodyIds{} + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 0.0f, 5.0f, { 0.0f, 1.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + m_groundId = b3CreateBody( m_worldId, &bodyDef ); + } + + m_shapeType = e_sphereShape; + m_wind = { 6.0f, 0.0f }; + m_drag = 1.0f; + m_lift = 0.75f; + m_count = 10; + m_noise = { 0.0f, 0.0f }; + + // Need this to be false for debug draw to work + m_stepWhilePaused = false; + + CreateScene(); + } + + void CreateScene() + { + for ( int i = 0; i < m_maxCount; ++i ) + { + if ( B3_IS_NON_NULL( m_bodyIds[i] ) ) + { + b3DestroyBody( m_bodyIds[i] ); + m_bodyIds[i] = b3_nullBodyId; + } + } + + float radius = 0.1f; + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, radius }; + b3Capsule capsule = { { 0.0f, -radius }, { 0.0f, radius }, 0.25f * radius }; + b3BoxHull box = b3MakeBoxHull( 0.25f * radius, 1.25f * radius, 0.5f * radius ); + + b3SphericalJointDef jointDef = b3DefaultSphericalJointDef(); + jointDef.base.bodyIdA = m_groundId; + jointDef.base.localFrameA.p = { 0.0f, 2.0f + radius }; + jointDef.base.drawScale = 0.1f; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 20.0f; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.gravityScale = 0.5f; + bodyDef.enableSleep = false; + + for ( int i = 0; i < m_count; ++i ) + { + bodyDef.position = { 0.0f, 2.0f - 2.0f * radius * i }; + m_bodyIds[i] = b3CreateBody( m_worldId, &bodyDef ); + + if ( m_shapeType == e_sphereShape ) + { + b3CreateSphereShape( m_bodyIds[i], &shapeDef, &sphere ); + } + else if ( m_shapeType == e_capsuleShape ) + { + b3CreateCapsuleShape( m_bodyIds[i], &shapeDef, &capsule ); + } + else + { + b3CreateHullShape( m_bodyIds[i], &shapeDef, &box.base ); + } + + jointDef.base.bodyIdB = m_bodyIds[i]; + jointDef.base.localFrameB.p = { 0.0f, radius }; + b3CreateSphericalJoint( m_worldId, &jointDef ); + + jointDef.base.bodyIdA = m_bodyIds[i]; + jointDef.base.localFrameA.p = { 0.0f, -radius }; + } + } + + void UpdateUI() override + { + float fontSize = ImGui::GetFontSize(); + float height = 15.0f * fontSize; + ImGui::SetNextWindowPos( { 0.5f * fontSize, m_camera->m_height - height - 2.0f * fontSize }, ImGuiCond_Once ); + ImGui::SetNextWindowSize( { 24.0f * fontSize, height } ); + + ImGui::Begin( "Wind", nullptr, ImGuiWindowFlags_NoResize ); + ImGui::PushItemWidth( 18.0f * fontSize ); + + const char* shapeTypes[] = { "Circle", "Capsule", "Box" }; + int shapeType = int( m_shapeType ); + if ( ImGui::Combo( "Shape", &shapeType, shapeTypes, IM_ARRAYSIZE( shapeTypes ) ) ) + { + m_shapeType = ShapeType( shapeType ); + CreateScene(); + } + + ImGui::SliderFloat2( "Wind", &m_wind.x, -50.0f, 50.0f, "%.1f" ); + ImGui::SliderFloat( "Drag", &m_drag, 0.0f, 1.0f, "%.2f" ); + ImGui::SliderFloat( "Lift", &m_lift, 0.0f, 4.0f, "%.2f" ); + if ( ImGui::SliderInt( "Count", &m_count, 1, m_maxCount, "%d" ) ) + { + CreateScene(); + } + + ImGui::PopItemWidth(); + ImGui::End(); + } + + void Render() override + { + Sample::Render(); + + DrawGrid( m_scene, 20 ); + } + + void Step() override + { + bool shouldStep = m_context->pause == false || m_context->singleStep > 0; + Sample::Step(); + + if ( shouldStep == true ) + { + float speed; + b3Vec3 direction = b3GetLengthAndNormalize( &speed, m_wind ); + b3Vec3 wind = b3MulSV( speed, b3Add( direction, m_noise ) ); + + for ( int i = 0; i < m_count; ++i ) + { + b3ShapeId shapeIds[1]; + int count = b3Body_GetShapes( m_bodyIds[i], shapeIds, 1 ); + for ( int j = 0; j < count; ++j ) + { + b3Shape_ApplyWind( shapeIds[j], wind, m_drag, m_lift, 10.0f, true ); + } + } + + b3Vec3 rand = RandomVec3( { -0.3f, -0.3f, -0.3f }, { 0.3f, 0.3f, 0.3f } ); + m_noise = b3Lerp( m_noise, rand, 0.05f ); + + DrawLine( m_scene, b3Vec3_zero, b3MulSV( 0.2f, wind ), b3_colorFuchsia ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new Wind( context ); + } + + static constexpr int m_maxCount = 60; + + ShapeType m_shapeType; + b3Vec3 m_wind; + float m_drag; + float m_lift; + b3Vec3 m_noise; + b3BodyId m_groundId; + b3BodyId m_bodyIds[m_maxCount]; + int m_count; +}; + +static int sampleWind = SampleManager::Register( "Shapes", "Wind", Wind::Create ); + +class WindDrop : public Sample +{ +public: + explicit WindDrop( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, -10.0f, 20.0f, { 0.0f, 5.0f, 0.0f } ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "ground"; + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull hull = b3MakeBoxHull( 15.0f, 1.0f, 15.0f ); + b3CreateHullShape( groundId, &shapeDef, &hull.base ); + } + + m_drag = 1.0f; + m_lift = 4.0f; + + // Need this to be false for debug draw to work + m_stepWhilePaused = false; + + float radius = 0.1f; + // b3BoxHull box = b3MakeBoxHull( 0.25f * radius, 1.25f * radius, 0.25f * radius ); + b3BoxHull box = b3MakeBoxHull( 4.0f * radius, 0.1f * radius, 4.0f * radius ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 2.0f; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.linearVelocity = { 0.0f, 0.0f, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisX, 0.25f ); + bodyDef.gravityScale = 0.5f; + + bodyDef.position = { 0.0f, 10.0f, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + m_shapeId = b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + + void Render() override + { + Sample::Render(); + + b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 1.0f ); + } + + void Step() override + { + bool shouldStep = m_context->pause == false || m_context->singleStep > 0; + Sample::Step(); + + if ( shouldStep == true ) + { + b3Shape_ApplyWind( m_shapeId, b3Vec3_zero, m_drag, m_lift, 10.0f, true ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new WindDrop( context ); + } + + float m_drag; + float m_lift; + b3ShapeId m_shapeId; +}; + +static int sampleWindDrop = SampleManager::Register( "Shapes", "Wind Drop", WindDrop::Create ); + +class WindFlap : public Sample +{ +public: + explicit WindFlap( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, -10.0f, 20.0f, { 0.0f, 5.0f, 0.0f } ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "ground"; + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull hull = b3MakeBoxHull( 50.0f, 1.0f, 50.0f ); + b3CreateHullShape( groundId, &shapeDef, &hull.base ); + } + + m_drag = 1.0f; + m_lift = 2.0f; + + // Need this to be false for debug draw to work + m_stepWhilePaused = false; + + float a = 0.4f; + // b3BoxHull box = b3MakeBoxHull( 0.25f * radius, 1.25f * radius, 0.25f * radius ); + b3Capsule capsule = { { 0.0f, 0.0f, -a }, { 0.0f, 0.0f, a }, 0.25f * a }; + //b3BoxHull box = b3MakeBoxHull( 2.0f * a, 0.01f, a ); + b3Transform wingTransform1 = {b3Vec3_zero, b3MakeQuatFromAxisAngle(b3Vec3_axisX, 0.1f)}; + b3BoxHull box1 = b3MakeTransformedBoxHull( 2.0f * a, 0.01f, a, wingTransform1 ); + b3Transform wingTransform2 = {b3Vec3_zero, b3MakeQuatFromAxisAngle(b3Vec3_axisX, 0.1f)}; + b3BoxHull box2 = b3MakeTransformedBoxHull( 2.0f * a, 0.01f, a, wingTransform2 ); + + float y = 20.0f; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + //bodyDef.gravityScale = 0.5f; + + shapeDef.density = 5.0f; + bodyDef.position = { -2.0f * a, y, 0.0f }; + b3BodyId wingBodyId1 = b3CreateBody( m_worldId, &bodyDef ); + m_shapeId1 = b3CreateHullShape( wingBodyId1, &shapeDef, &box1.base ); + + bodyDef.position = { 2.0f * a, y, 0.0f }; + b3BodyId wingBodyId2 = b3CreateBody( m_worldId, &bodyDef ); + m_shapeId2 = b3CreateHullShape( wingBodyId2, &shapeDef, &box2.base ); + + bodyDef.position = { 0.0f, y, 0.0f }; + //bodyDef.type = b3_staticBody; + b3BodyId torsoBodyId = b3CreateBody( m_worldId, &bodyDef ); + + shapeDef.density = 10.0f; + m_torsoShapeId = b3CreateCapsuleShape( torsoBodyId, &shapeDef, &capsule ); + + b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef(); + jointDef.base.drawScale = 0.1f; + jointDef.base.bodyIdA = torsoBodyId; + jointDef.base.localFrameA.p = { 0.0f, 0.0f, 0.0f }; + jointDef.base.bodyIdB = wingBodyId1; + jointDef.base.localFrameB.p = { 2.0f * a, 0.0f, 0.0f }; + jointDef.enableSpring = true; + jointDef.hertz = 6.0f; + jointDef.dampingRatio = 0.5f; + jointDef.enableLimit = true; + jointDef.lowerAngle = -30.0f * B3_PI / 180.0f; + jointDef.upperAngle = 30.0f * B3_PI / 180.0f; + m_jointId1 = b3CreateRevoluteJoint( m_worldId, &jointDef ); + + jointDef.base.bodyIdB = wingBodyId2; + jointDef.base.localFrameB.p = { -2.0f * a, 0.0f, 0.0f }; + m_jointId2 = b3CreateRevoluteJoint( m_worldId, &jointDef ); + + b3FilterJointDef filterDef = b3DefaultFilterJointDef(); + filterDef.base.bodyIdA = wingBodyId1; + filterDef.base.bodyIdB = wingBodyId2; + b3CreateFilterJoint( m_worldId, &filterDef ); + + m_time = 0.0f; + } + + void Render() override + { + Sample::Render(); + + b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 1.0f ); + } + + void Step() override + { + bool shouldStep = m_context->pause == false || m_context->singleStep > 0; + Sample::Step(); + + if ( shouldStep == true ) + { + float maxSpeed = 10.0f; + bool wake = false; + b3Shape_ApplyWind( m_shapeId1, b3Vec3_zero, m_drag, m_lift, maxSpeed, wake ); + b3Shape_ApplyWind( m_shapeId2, b3Vec3_zero, m_drag, m_lift, maxSpeed, wake ); + //b3Shape_ApplyWind( m_torsoShapeId, b3Vec3_zero, m_drag, m_lift, maxSpeed, wake ); + + float angle = b3Sin( 10.0f * m_time ); + b3RevoluteJoint_SetTargetAngle(m_jointId1, angle); + b3RevoluteJoint_SetTargetAngle(m_jointId2, -angle); + + m_time += m_context->hertz > 0.0f ? 1.0f / m_context->hertz : 0.0f; + } + } + + static Sample* Create( SampleContext* context ) + { + return new WindFlap( context ); + } + + float m_time; + float m_drag; + float m_lift; + b3ShapeId m_shapeId1; + b3ShapeId m_shapeId2; + b3ShapeId m_torsoShapeId; + b3JointId m_jointId1; + b3JointId m_jointId2; +}; + +static int sampleWindFlap = SampleManager::Register( "Shapes", "Wind Flap", WindFlap::Create ); diff --git a/samples/sample_stacking.cpp b/samples/sample_stacking.cpp new file mode 100644 index 0000000..c58de83 --- /dev/null +++ b/samples/sample_stacking.cpp @@ -0,0 +1,955 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "camera.h" +#include "human.h" +#include "imgui.h" +#include "mesh_loader.h" +#include "sample.h" +#include "scene.h" + +#include "box3d/box3d.h" + +class CardHouseThick : public Sample +{ +public: + explicit CardHouseThick( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 25.0f, 10.0f, { 0.0f, 2.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + const float alpha = 25.0f * B3_DEG_TO_RAD; + const float width = 0.38f; + const float height = 0.98f; + const float depth = 0.08f; + + float offsetX = 0.5f * height * b3Sin( alpha ) + 0.045f; + float offsetY = 0.5f * height * b3Cos( alpha ) + 0.035f; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + b3BoxHull mGround = b3MakeOffsetBoxHull( 10.0f, 1.0f, 10.0f, {0.0f, -1.0f, 0.0f} ); + b3BodyId groundBody = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( groundBody, &shapeDef, &mGround.base ); + + b3BoxHull box = b3MakeBoxHull( 0.5f * depth, 0.5f * height, 0.5f * width ); + AddVerticalRow( 4, -6.0f * offsetX, offsetX, offsetY, alpha, box ); + AddHorizontalRow( 3, -4.0f * offsetX, 4.0f * offsetX, 2.0f * offsetY + 0.04f, box ); + AddVerticalRow( 3, -4.0f * offsetX, offsetX, 3.0f * offsetY + 0.08f, alpha, box ); + AddHorizontalRow( 2, -2.0f * offsetX, 4.0f * offsetX, 4.0f * offsetY + 0.12f, box ); + AddVerticalRow( 2, -2.0f * offsetX, offsetX, 5.0f * offsetY + 0.16f, alpha, box ); + AddHorizontalRow( 1, -0.0f * offsetX, 4.0f * offsetX, 6.0f * offsetY + 0.20f, box ); + AddVerticalRow( 1, -0.0f * offsetX, offsetX, 7.0f * offsetY + 0.24f, alpha, box ); + } + + void AddVerticalRow( int n, float startX, float offsetX, float startY, float alpha, const b3BoxHull& box ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.8f; + + for ( int index = 0; index < n; ++index ) + { + bodyDef.position = { startX - offsetX, startY, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, -alpha ); + b3BodyId body1 = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( body1, &shapeDef, &box.base ); + + bodyDef.position = { startX + offsetX, startY, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, alpha ); + b3BodyId body2 = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( body2, &shapeDef, &box.base ); + + startX += 4.0f * offsetX; + } + } + + void AddHorizontalRow( int n, float startX, float offsetX, float startY, const b3BoxHull& box ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.8f; + + for ( int index = 0; index < n; ++index ) + { + bodyDef.position = { startX + index * offsetX, startY, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.5f * B3_PI ); + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( body, &shapeDef, &box.base ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new CardHouseThick( context ); + } +}; + +static int sampleCardHouseThick = SampleManager::Register( "Stacking", "Card House Thick", CardHouseThick::Create ); + +// From PEEL +class CardHouse : public Sample +{ +public: + explicit CardHouse( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + b3Vec3 pivot = { 0.75, 1.0, 0.4f }; + m_camera->SetView( 0.0f, 15.0f, 3.0f, pivot ); + EnableGrid( m_scene, true ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -2.0f }; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.7f; + + b3BoxHull groundBox = b3MakeBoxHull( 40.0f, 2.0f, 40.0f ); + b3CreateHullShape( groundId, &shapeDef, &groundBox.base ); + + float cardHeight = 0.2f; + float cardThickness = 0.001f; + float cardDepth = 0.1f; + + float angle0 = 25.0f * B3_PI / 180.0f; + float angle1 = -25.0f * B3_PI / 180.0f; + float angle2 = 0.5f * B3_PI; + + // todo box hull is limiting the minimum thickness, breaking this test + b3BoxHull cardBox = b3MakeBoxHull( cardThickness, cardHeight, cardDepth ); + bodyDef.type = b3_dynamicBody; + + int Nb = 5; + float z0 = 0.0f; + float y = cardHeight - 0.02f; + while ( Nb ) + { + float z = z0; + for ( int i = 0; i < Nb; i++ ) + { + if ( i != Nb - 1 ) + { + bodyDef.position = { z + 0.25f, y + cardHeight - 0.015f, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, angle2 ); + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &cardBox.base ); + } + + bodyDef.position = { z, y, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, angle1 ); + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &cardBox.base ); + + z += 0.175f; + + bodyDef.position = { z, y, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, angle0 ); + bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &cardBox.base ); + + z += 0.175f; + } + y += cardHeight * 2.0f - 0.03f; + z0 += 0.175f; + Nb--; + } + } + + static Sample* Create( SampleContext* context ) + { + return new CardHouse( context ); + } +}; + +static int sampleCardHouse = SampleManager::Register( "Stacking", "Card House", CardHouse::Create ); + +class SphereStack : public Sample +{ +public: + explicit SphereStack( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 15.0f, 50.0f, {0.0f, 10.0f, 0.0f} ); + EnableGrid( m_scene, true ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "ground"; + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull hull = b3MakeBoxHull( 15.0f, 1.0f, 15.0f ); + b3CreateHullShape( groundId, &shapeDef, &hull.base ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + float r = 0.5f; + b3Sphere sphere = { b3Vec3_zero, r }; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.rollingResistance = 0.1f; + + float y = 1.5f * r; + + for ( int i = 0; i < 30; ++i ) + { + bodyDef.name = "sphere"; + //bodyDef.position.x = 0.1f * i; + bodyDef.position.y = y; + bodyDef.angularVelocity = { 0.0f, 0.0f, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + if ( i == 0 ) + { + m_bodyId = bodyId; + } + + // shapeDef.density = 1.0f + 4.0f * i; + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + + y += 3.0f * r; + } + } + + void Step() override + { + // b3Vec3 p = b3Body_GetPosition( m_bodyId ); + // printf( "%g %g %g\n", p.x, p.y, p.z ); + + Sample::Step(); + } + + static Sample* Create( SampleContext* context ) + { + return new SphereStack( context ); + } + + b3BodyId m_bodyId; +}; + +static int sampleSphereStack = SampleManager::Register( "Stacking", "Sphere Stack", SphereStack::Create ); + +class SingleBox : public Sample +{ +public: + explicit SingleBox( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 25.0f, 10.0f, b3Vec3_zero ); + EnableGrid( m_scene, true ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "ground"; + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull hull = b3MakeBoxHull( 15.0f, 1.0f, 15.0f ); + b3CreateHullShape( groundId, &shapeDef, &hull.base ); + } + +#if 1 + { + b3BoxHull cube = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "cube"; + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 0.5f, 0.0f }; + //bodyDef.linearVelocity = { 4.0f, 0.0f, 0.0f }; + bodyDef.angularVelocity = { 0.0f, 10.0f, 0.0f }; + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( m_bodyId, &shapeDef, &cube.base ); + } +#else + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "capsule"; + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 2.0f, 0.0f }; + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3Capsule capsule = { { -0.25f, 0.0f }, { 0.25f, 0.0f }, 0.1f }; + b3CreateCapsuleShape( m_bodyId, &shapeDef, &capsule ); + } +#endif + } + + void Step() override + { + Sample::Step(); + + b3Vec3 position = b3Body_GetPosition( m_bodyId ); + DrawTextLine( "(x, y, z) = (%.2g, %.2g, %.2g)", position.x, position.y, position.z ); + } + + static Sample* Create( SampleContext* context ) + { + return new SingleBox( context ); + } + + b3BodyId m_bodyId; +}; + +static int sampleSingleBox = SampleManager::Register( "Stacking", "Single Box", SingleBox::Create ); + +class Cylinder : public Sample +{ +public: + explicit Cylinder( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 15.0f, 10.0f, b3Vec3_zero ); + EnableGrid( m_scene, true ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "ground"; + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull hull = b3MakeBoxHull( 10.0f, 1.0f, 10.0f ); + b3CreateHullShape( groundId, &shapeDef, &hull.base ); + } + + { + m_hull = b3CreateCylinder( 1.0f, 0.25f, 0.0f, 12 ); + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "cylinder"; + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 2.0f, 0.0f }; + bodyDef.linearVelocity = { 0.0f, 0.0f, 0.0f }; + // bodyDef.angularVelocity = { 0.0f, 5.0f, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.rollingResistance = 0.05f; + b3CreateHullShape( bodyId, &shapeDef, m_hull ); + } + + m_context->debugDraw.forceScale = 0.01f; + } + + ~Cylinder() override + { + b3DestroyHull( m_hull ); + } + + void Step() override + { + Sample::Step(); + } + + static Sample* Create( SampleContext* context ) + { + return new Cylinder( context ); + } + + b3Hull* m_hull; +}; + +static int sampleCylinder = SampleManager::Register( "Stacking", "Cylinder", Cylinder::Create ); + +class CylinderStack : public Sample +{ +public: + explicit CylinderStack( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 15.0f, 15.0f, {0.0f, 5.0f, 0.0f} ); + EnableGrid( m_scene, true ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "ground"; + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull hull = b3MakeBoxHull( 10.0f, 1.0f, 10.0f ); + b3CreateHullShape( groundId, &shapeDef, &hull.base ); + } + + m_hull = b3CreateCylinder( 1.0f, 0.5f, 0.0f, 15 ); + + b3Vec3 scales[4] = { + b3Vec3_one, + { -0.75f, 1.0f, 1.0f }, + { 1.2f, 1.0f, -0.9f }, + { 0.9f, 0.9f, 0.9f }, + }; + + for ( int i = 0; i < 10; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "cylinder"; + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 0.0f + 1.1f * i, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + //shapeDef.baseMaterial.rollingResistance = 0.1f; + b3CreateTransformedHullShape( bodyId, &shapeDef, m_hull, b3Transform_identity, scales[i % 4] ); + } + + m_context->debugDraw.forceScale = 0.001f; + } + + ~CylinderStack() override + { + b3DestroyHull( m_hull ); + } + + void Step() override + { + Sample::Step(); + } + + static Sample* Create( SampleContext* context ) + { + return new CylinderStack( context ); + } + + b3Hull* m_hull; +}; + +static int sampleCylinderStack = SampleManager::Register( "Stacking", "Cylinder Stack", CylinderStack::Create ); + +class BoxStack : public Sample +{ +public: + explicit BoxStack( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 15.0f, 50.0f, {0.0f, 20.0f, 0.0f} ); + EnableGrid( m_scene, true ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "ground"; + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull hull = b3MakeBoxHull( 40.0f, 1.0f, 40.0f ); + b3CreateHullShape( groundId, &shapeDef, &hull.base ); + } + + float a = 0.5f; + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "cube"; + bodyDef.type = b3_dynamicBody; + + b3BoxHull cube = b3MakeBoxHull( a, a, a ); + // b3Quat q = b3MakeQuatFromAxisAngle( { 1.0f, 0.0f, 0.0f }, 0.5f * B3_PI ); + + for ( int i = 0; i < 40; ++i ) + { + bodyDef.position = { 0.0f, 1.5f * a + 2.5f * a * i, 0.0f }; + // bodyDef.rotation = b3MulQuat( q, bodyDef.rotation ); + + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.rollingResistance = 0.1f; + + b3CreateHullShape( bodyId, &shapeDef, &cube.base ); + } + + //{ + // b3BoxHull wide = b3MakeBoxHull( 2.0f, 0.6f, 0.6f ); + // b3BodyDef bodyDef = b3DefaultBodyDef(); + // bodyDef.name = "wide"; + // bodyDef.type = b3_dynamicBody; + // bodyDef.position = { 0.0f, 0.6f + 1.0f * m_count, 0.0f }; + // b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + // b3ShapeDef shapeDef = b3DefaultShapeDef(); + // b3CreateHullShape( bodyId, &shapeDef, &wide.base ); + //} + } + + static Sample* Create( SampleContext* context ) + { + return new BoxStack( context ); + } +}; + +static int sampleBoxStack = SampleManager::Register( "Stacking", "Box Stack", BoxStack::Create ); + +class JengaStack : public Sample +{ +public: + explicit JengaStack( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 35.0f, 15.0f, 30.0f, { 0.0f, 10.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + m_shapeType = b3_hullShape; + CreateStack(); + } + + void CreateStack() + { + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull groundBox = b3MakeBoxHull( 50.0f, 1.0f, 50.0f ); + b3CreateHullShape( groundId, &shapeDef, &groundBox.base ); + } + + { + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.rollingResistance = m_shapeType == b3_capsuleShape ? 0.1f : 0.01f; + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + b3BoxHull box = b3MakeBoxHull( 2.5f, 0.25f, 0.25f ); + b3Capsule capsule = { { -2.5f, 0.0f, 0.0f }, { 2.5f, 0.0f, 0.0f }, 0.25f }; + + for ( int i = 0; i < m_size; ++i ) + { + float alpha = ( i & 1 ) == 1 ? 0.0f : 0.5f * B3_PI; + + float x = ( i & 1 ) == 0 ? 1.75f : 0.0f; + float z = ( i & 1 ) == 0 ? 0.0f : 1.75f; + + bodyDef.position = { x, 0.5f * i + 0.25f, z }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisY, alpha ); + b3BodyId boxBody1 = b3CreateBody( m_worldId, &bodyDef ); + + bodyDef.position = { -x, 0.5f * i + 0.25f, -z }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisY, alpha ); + b3BodyId boxBody2 = b3CreateBody( m_worldId, &bodyDef ); + + if ( m_shapeType == b3_capsuleShape ) + { + b3CreateCapsuleShape( boxBody1, &shapeDef, &capsule ); + b3CreateCapsuleShape( boxBody2, &shapeDef, &capsule ); + } + else + { + b3CreateHullShape( boxBody1, &shapeDef, &box.base ); + b3CreateHullShape( boxBody2, &shapeDef, &box.base ); + } + } + } + } + + void UpdateUI() override + { + float fontSize = ImGui::GetFontSize(); + float height = 6.0f * fontSize; + ImGui::SetNextWindowPos( ImVec2( 0.5f * fontSize, m_camera->m_height - height - 2.0f * fontSize ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 8.0f * fontSize, height ) ); + + ImGui::Begin( "Jenga Stack", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + b3Capacity capacity = {}; + + if ( ImGui::RadioButton( "Capsule", m_shapeType == b3_capsuleShape ) ) + { + m_shapeType = b3_capsuleShape; + CreateWorld( &capacity ); + CreateStack(); + } + + if ( ImGui::RadioButton( "Hull", m_shapeType == b3_hullShape ) ) + { + m_shapeType = b3_hullShape; + CreateWorld( &capacity ); + CreateStack(); + } + + ImGui::End(); + + Sample::UpdateUI(); + } + + static Sample* Create( SampleContext* context ) + { + return new JengaStack( context ); + } + + static constexpr int m_size = 40; + b3ShapeType m_shapeType; +}; + +static int sampleJengaStack = SampleManager::Register( "Stacking", "Jenga Stack", JengaStack::Create ); + +class Dominoes : public Sample +{ +public: + explicit Dominoes( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 15.0f, 50.0f, b3Vec3_zero ); + EnableGrid( m_scene, true ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundBody = b3CreateBody( m_worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + b3BoxHull groundBox = b3MakeBoxHull( 80.0f, 1.0f, 80.0f ); + b3CreateHullShape( groundBody, &shapeDef, &groundBox.base ); + + constexpr int n = m_isDebug ? 2 : 30; + + b3BoxHull box = b3MakeBoxHull( 0.2f, 0.8f, 0.05f ); + for ( int ring = 0; ring < n; ++ring ) + { + float radius = 7.0f + 1.1f * ring; + CreateRing( radius, box ); + } + } + + void CreateRing( float radius, b3BoxHull& box ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + for ( float alpha = 0; alpha <= 360.0f; alpha += 2.0f ) + // for (float Alpha = 0; Alpha <= 4.0f; Alpha += 4.0f) + { + b3CosSin cs = b3ComputeCosSin( B3_DEG_TO_RAD * alpha ); + b3Vec3 position = { radius * cs.cosine, 0.8f, radius * cs.sine }; + b3Vec3 normal = { cs.cosine, 0.0f, cs.sine }; + position -= alpha / 630.0f * normal; + + b3Quat orientation = b3MakeQuatFromAxisAngle( b3Vec3_axisY, -B3_DEG_TO_RAD * alpha ); + + bodyDef.position = position; + bodyDef.rotation = orientation; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( body, &shapeDef, &box.base ); + + if ( alpha == 0.0f ) + { + b3Body_ApplyLinearImpulse( body, { 0.0f, 0.0f, 25.0f }, position + b3Vec3{ 0.0f, 0.8f, 0.0f }, true ); + } + } + } + + static Sample* Create( SampleContext* context ) + { + return new Dominoes( context ); + } +}; + +static int sampleDominoes = SampleManager::Register( "Stacking", "Dominoes", Dominoes::Create ); + +// This wedge shape can have an incorrect manifold if not handled correctly +class Wedge : public Sample +{ +public: + static Sample* Create( SampleContext* context ) + { + return new Wedge( context ); + } + + explicit Wedge( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 77.0f, 10.0f, 5.0f ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull groundBox = b3MakeBoxHull( 25.0f, 1.0f, 25.0f ); + b3CreateHullShape( groundId, &shapeDef, &groundBox.base ); + + b3Vec3 vertices[] = { + { -1.0, 1.0f, -0.1f }, { 1.0, 1.0f, -0.1f }, { -1.0, 1.0f, 0.1f }, + { 1.0, 1.0f, 0.1f }, { -0.5, 0.5f, 0.0f }, { 0.5, 0.5f, 0.0f }, + }; + + m_wedgeHull = b3CreateHull( vertices, 6, 6 ); + + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 1.0f, 0.0f }; + b3BodyId wedgeBody = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( wedgeBody, &shapeDef, m_wedgeHull ); + } + + ~Wedge() override + { + b3DestroyHull( m_wedgeHull ); + } + + b3Hull* m_wedgeHull; +}; + +static int sampleWedge = SampleManager::Register( "Stacking", "Wedge", Wedge::Create ); + +// jitter +/* + *+ position [ -459.292877, 217.398331, 1.00115335 ] b3Vec3 ++ rotation [ [ -0.00000000, 0.00000000, -0.707106769 ], 0.707106769 ] b3Quat + +points[0] = { -44.8770714, -91.6598053, -1.92012548 }; +points[1] = { -92.5001831, 51.0151291, 15.8006573 }; +points[2] = { -91.0282211, -9.44371605, 15.6148796 }; +points[3] = { 90.2375641, 77.3870087, 15.9356089 }; +points[4] = { -85.5353241, 91.3750992, -1.36629653 }; +points[5] = { 88.9092178, -87.2975464, -1.86754704 }; +points[6] = { 83.7932816, -89.8572235, 15.4168339 }; +points[7] = { 87.0243988, 88.9776535, -1.32423306 }; +points[8] = { -91.6564941, -85.4949493, 15.3782759 }; +points[9] = { -90.2922516, -87.2074127, -1.92012548 }; +points[10] = { -87.2944870, 89.9510498, 15.9215889 }; +points[11] = { 79.2338104, 89.9690781, 15.9724140 }; +points[12] = { -91.6744461, 81.0823212, -1.39959598 }; +points[13] = { 90.3452759, -76.4459610, 15.4588966 }; +points[14] = { -87.4021912, -89.2263107, 15.3677588 }; +points[15] = { 76.3258057, 92.0059967, 1.82873762 }; +*/ + +/* + * + *+ position [ -402.321838, 157.310364, 16.8169250 ] b3Vec3 ++ rotation [ [ -0.00000000, 0.00000000, -0.00152086187 ], 0.999998868 ] b3Quat + + * +points[0] = { 29.5000000, 17.1488495, 0.175081104 }; +points[1] = { 29.5000000, -17.2990532, 0.125000000 }; +points[2] = { 29.4840164, -17.3057766, 24.0200863 }; +points[3] = { 29.4840164, 17.1648350, 24.1781254 }; +points[4] = { -29.1345520, 17.5529804, 0.125000000 }; +points[5] = { -29.1345520, 17.5529804, 23.7899799 }; +points[6] = { -29.1441040, 16.9679585, 24.3750000 }; +points[7] = { -29.1345520, -17.2990532, 24.3750000 }; +points[8] = { -29.1345520, -17.2990532, 0.175081253 }; +points[9] = { 29.0720215, 17.5529785, 0.125000000 }; +points[10] = { 29.0859070, 17.5629406, 23.8120594 }; +points[11] = { 29.1401348, -17.2990532, 24.3750000 }; +points[12] = { 29.1123581, 16.9722290, 24.4027710 }; +points[13] = { 29.3944912, 17.2543602, 24.1206398 }; +points[14] = { -29.1345520, -17.2990532, 24.0759430 }; +points[15] = { -29.1345520, -16.9722252, 24.4027710 }; +points[16] = { 29.1123619, -16.9722271, 24.4027729 }; +points[17] = { 29.5000000, 17.3429642, 24.0000000 }; +*/ + +class Arch : public Sample +{ +public: + explicit Arch( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 15.0f, 30.0f, { 0.0f, 5.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + b3Vec3 ps1[9] = { { 16.0f, 0.0f, 0.0f }, + { 14.93803712795643f, 5.133601056842984f, 0.0f }, + { 13.79871746027416f, 10.24928069555078f, 0.0f }, + { 12.56252963284711f, 15.34107019122473f, 0.0f }, + { 11.20040987372525f, 20.39856541571217f, 0.0f }, + { 9.66521217819836f, 25.40369899225096f, 0.0f }, + { 7.87179930638133f, 30.3179337000085f, 0.0f }, + { 5.635199558196225f, 35.03820717801641f, 0.0f }, + { 2.405937953536585f, 39.09554102558315f, 0.0f } }; + + b3Vec3 ps2[9] = { { 24.0f, 0.0f, 0.0f }, + { 22.33619528222415f, 6.02299846205841f, 0.0f }, + { 20.54936888969905f, 12.00964361211476f, 0.0f }, + { 18.60854610798073f, 17.9470321677465f, 0.0f }, + { 16.46769273811807f, 23.81367936585418f, 0.0f }, + { 14.05325025774858f, 29.57079353071012f, 0.0f }, + { 11.23551045834022f, 35.13775818285372f, 0.0f }, + { 7.752568160730571f, 40.30450679009583f, 0.0f }, + { 3.016931552701656f, 44.28891593799322f, 0.0f } }; + + float scale = 0.25f; + for ( int i = 0; i < 9; ++i ) + { + ps1[i].x *= scale; + ps1[i].y *= scale; + ps2[i].x *= scale; + ps2[i].y *= scale; + } + + const float halfDepth = 0.5f; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + b3BoxHull groundBox = b3MakeBoxHull( 50.0f, 1.0f, 50.0f ); + b3CreateHullShape( groundId, &shapeDef, &groundBox.base ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + for ( int i = 0; i < 8; ++i ) + { + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3Vec3 ps[8] = { + { ps1[i].x, ps1[i].y, -halfDepth }, { ps2[i].x, ps2[i].y, -halfDepth }, + { ps2[i + 1].x, ps2[i + 1].y, -halfDepth }, { ps1[i + 1].x, ps1[i + 1].y, -halfDepth }, + { ps1[i].x, ps1[i].y, halfDepth }, { ps2[i].x, ps2[i].y, halfDepth }, + { ps2[i + 1].x, ps2[i + 1].y, halfDepth }, { ps1[i + 1].x, ps1[i + 1].y, halfDepth }, + }; + b3Hull* hull = b3CreateHull( ps, 8, 8 ); + b3CreateHullShape( bodyId, &shapeDef, hull ); + b3DestroyHull( hull ); + } + + for ( int i = 0; i < 8; ++i ) + { + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3Vec3 ps[8] = { + { -ps2[i].x, ps2[i].y, -halfDepth }, { -ps1[i].x, ps1[i].y, -halfDepth }, + { -ps1[i + 1].x, ps1[i + 1].y, -halfDepth }, { -ps2[i + 1].x, ps2[i + 1].y, -halfDepth }, + { -ps2[i].x, ps2[i].y, halfDepth }, { -ps1[i].x, ps1[i].y, halfDepth }, + { -ps1[i + 1].x, ps1[i + 1].y, halfDepth }, { -ps2[i + 1].x, ps2[i + 1].y, halfDepth }, + }; + b3Hull* hull = b3CreateHull( ps, 8, 8 ); + b3CreateHullShape( bodyId, &shapeDef, hull ); + b3DestroyHull( hull ); + } + + { + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3Vec3 ps[8] = { + { ps1[8].x, ps1[8].y, -halfDepth }, { ps2[8].x, ps2[8].y, -halfDepth }, + { -ps2[8].x, ps2[8].y, -halfDepth }, { -ps1[8].x, ps1[8].y, -halfDepth }, + { ps1[8].x, ps1[8].y, halfDepth }, { ps2[8].x, ps2[8].y, halfDepth }, + { -ps2[8].x, ps2[8].y, halfDepth }, { -ps1[8].x, ps1[8].y, halfDepth }, + }; + b3Hull* hull = b3CreateHull( ps, 8, 8 ); + b3CreateHullShape( bodyId, &shapeDef, hull ); + b3DestroyHull( hull ); + } + + for ( int i = 0; i < 4; ++i ) + { + b3BoxHull box = b3MakeBoxHull( 2.0f, 0.5f, halfDepth ); + bodyDef.position = { 0.0f, 0.5f + ps2[8].y + 1.0f * i, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new Arch( context ); + } +}; + +static int sampleArch = SampleManager::Register( "Stacking", "Arch", Arch::Create ); + +class Pyramid2D : public Sample +{ +public: + explicit Pyramid2D( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 30.0f, 50.0f, { 0.0f, 5.0f, 0.0f } ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull groundBox = b3MakeBoxHull( 50.0f, 1.0f, 50.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + // shapeDef.filter = { 1, ~0u, 0 }; + + b3CreateHullShape( groundId, &shapeDef, &groundBox.base ); + } + + float a = 1.0f; + b3BoxHull box = b3MakeBoxHull( a, a, a ); + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.motionLocks.linearZ = true; + bodyDef.motionLocks.angularX = true; + bodyDef.motionLocks.angularY = true; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + for ( int row = 0; row < m_size; ++row ) + { + for ( int column = 0; column < m_size - row; ++column ) + { + bodyDef.position = { ( -20.0f + 2.0f * column + row ) * a, ( 1.5f + 2.5f * row ) * a, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + } + } + + static Sample* Create( SampleContext* context ) + { + return new Pyramid2D( context ); + } + + static constexpr int m_size = 12; +}; + +static int samplePyramid2D = SampleManager::Register( "Stacking", "Pyramid2D", Pyramid2D::Create ); + diff --git a/samples/sample_tree.cpp b/samples/sample_tree.cpp new file mode 100644 index 0000000..ac03af4 --- /dev/null +++ b/samples/sample_tree.cpp @@ -0,0 +1,848 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include "camera.h" +#include "imgui.h" +#include "utils.h" +#include "renderer.h" +#include "sample.h" +#include "scene.h" + +#include +#include +#include +#include + +constexpr int colorCount = 12; +b3HexColor colors[colorCount] = { + b3_colorRoyalBlue, b3_colorPink, b3_colorLightGreen, b3_colorOrange, b3_colorYellow, b3_colorViolet, + b3_colorAqua, b3_colorCoral, b3_colorLightGray, b3_colorMoccasin, b3_colorLavender, b3_colorMediumOrchid, +}; + +struct FileConfig +{ + float scale; + bool zUp; +}; + +struct Proxy +{ + b3AABB aabb; + int proxyId; + int queryTimeStamp; + int rayTimeStamp; +}; + +struct Ray +{ + b3Vec3 origin, translation; +}; + +#ifdef _WIN32 + +#include + +// Warning: this code has not been validated +void ClosestPointAVX2( const float* aabbMinX, const float* aabbMinY, const float* aabbMinZ, const float* aabbMaxX, + const float* aabbMaxY, const float* aabbMaxZ, int proxyCount, b3Vec3 point, b3Vec3* outClosestPoint, + float* outMinDistSq ) +{ + const __m256 px = _mm256_set1_ps( point.x ); + const __m256 py = _mm256_set1_ps( point.y ); + const __m256 pz = _mm256_set1_ps( point.z ); + + __m256 bestDistSq = _mm256_set1_ps( FLT_MAX ); + __m256i bestIndex = _mm256_set1_epi32( -1 ); + + int j = 0; + for ( ; j + 8 <= proxyCount; j += 8 ) + { + // Load AABB bounds for 8 proxies + __m256 minX = _mm256_loadu_ps( aabbMinX + j ); + __m256 minY = _mm256_loadu_ps( aabbMinY + j ); + __m256 minZ = _mm256_loadu_ps( aabbMinZ + j ); + __m256 maxX = _mm256_loadu_ps( aabbMaxX + j ); + __m256 maxY = _mm256_loadu_ps( aabbMaxY + j ); + __m256 maxZ = _mm256_loadu_ps( aabbMaxZ + j ); + + // Closest point on AABB to point: clamp each coordinate + __m256 cx = _mm256_min_ps( _mm256_max_ps( px, minX ), maxX ); + __m256 cy = _mm256_min_ps( _mm256_max_ps( py, minY ), maxY ); + __m256 cz = _mm256_min_ps( _mm256_max_ps( pz, minZ ), maxZ ); + + // d = point - closest + __m256 dx = _mm256_sub_ps( px, cx ); + __m256 dy = _mm256_sub_ps( py, cy ); + __m256 dz = _mm256_sub_ps( pz, cz ); + + __m256 dx2 = _mm256_mul_ps( dx, dx ); + __m256 dy2 = _mm256_mul_ps( dy, dy ); + __m256 dz2 = _mm256_mul_ps( dz, dz ); + __m256 distSq = _mm256_add_ps( _mm256_add_ps( dx2, dy2 ), dz2 ); + + // Compare to current best + __m256 mask = _mm256_cmp_ps( distSq, bestDistSq, _CMP_LT_OQ ); + bestDistSq = _mm256_blendv_ps( bestDistSq, distSq, mask ); + + __m256i indices = _mm256_set_epi32( j + 7, j + 6, j + 5, j + 4, j + 3, j + 2, j + 1, j + 0 ); + + // Tracks the 8 best indices + bestIndex = + _mm256_castps_si256( _mm256_blendv_ps( _mm256_castsi256_ps( bestIndex ), _mm256_castsi256_ps( indices ), mask ) ); + } + + // Now find the best index among the best 8. + alignas( 32 ) float distBuf[8]; + alignas( 32 ) int indexBuf[8]; + + _mm256_store_ps( distBuf, bestDistSq ); + _mm256_store_si256( (__m256i*)indexBuf, bestIndex ); + + float minDist = distBuf[0]; + int minIdx = indexBuf[0]; + for ( int k = 1; k < 8; ++k ) + { + if ( distBuf[k] < minDist ) + { + minDist = distBuf[k]; + minIdx = indexBuf[k]; + } + } + + // Remainder + for ( ; j < proxyCount; ++j ) + { + float cx = b3MinFloat( b3MaxFloat( point.x, aabbMinX[j] ), aabbMaxX[j] ); + float cy = b3MinFloat( b3MaxFloat( point.y, aabbMinY[j] ), aabbMaxY[j] ); + float cz = b3MinFloat( b3MaxFloat( point.z, aabbMinZ[j] ), aabbMaxZ[j] ); + + float dx = point.x - cx; + float dy = point.y - cy; + float dz = point.z - cz; + float d2 = dx * dx + dy * dy + dz * dz; + + if ( d2 < minDist ) + { + minDist = d2; + minIdx = j; + } + } + + b3Vec3 cp; + cp.x = b3MinFloat( b3MaxFloat( point.x, aabbMinX[minIdx] ), aabbMaxX[minIdx] ); + cp.y = b3MinFloat( b3MaxFloat( point.y, aabbMinY[minIdx] ), aabbMaxY[minIdx] ); + cp.z = b3MinFloat( b3MaxFloat( point.z, aabbMinZ[minIdx] ), aabbMaxZ[minIdx] ); + + *outClosestPoint = cp; + *outMinDistSq = minDist; +} +#endif + +class TreeBenchmark : public Sample +{ +public: + explicit TreeBenchmark( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 45.0f, 250.0f, b3Vec3_zero ); + } + + m_fileNames[0] = { "bounds01" }; + m_fileNames[1] = { "bounds02" }; + m_fileNames[2] = { "bounds03" }; + m_config[0] = { 1.0f, false }; + m_config[1] = { 1.0f, false }; + m_config[2] = { 0.01f, false }; + m_fileIndex = 0; + + m_closestPoint = b3Vec3_zero; + m_timeStamp = 1; + m_doOverlap = false; + m_doClosest = false; + m_doRay = false; + m_testIndex = 0; + m_rayTime = 0.0f; + m_overlapTime = 0.0f; + m_closestTime = 0.0f; + m_haveClosest = false; + m_buildTime = 0.0f; + m_areaRatio = 0.0f; + m_drawDistance = 1.0f; + m_loadScale = 1.0f; + m_height = 0; + m_tree = {}; + + m_aabbMinX = nullptr; + m_aabbMinY = nullptr; + m_aabbMinZ = nullptr; + m_aabbMaxX = nullptr; + m_aabbMaxY = nullptr; + m_aabbMaxZ = nullptr; + + CreateTree(); + } + + ~TreeBenchmark() override + { + b3DynamicTree_Destroy( &m_tree ); + +#ifdef _WIN32 + _aligned_free( m_aabbMinX ); + _aligned_free( m_aabbMinY ); + _aligned_free( m_aabbMinZ ); + _aligned_free( m_aabbMaxX ); + _aligned_free( m_aabbMaxY ); + _aligned_free( m_aabbMaxZ ); +#endif + } + + // Breath-first search to compute node depth values start with 0 at the root. + int ComputeDepths() + { + m_depths.resize( m_tree.nodeCapacity, 0 ); + + int* queue = (int*)malloc( m_tree.nodeCount * sizeof( int ) ); + int front = 0; + int back = 0; + + b3TreeNode* nodes = m_tree.nodes; + queue[0] = m_tree.root; + back += 1; + int depth = 0; + + while ( back > front ) + { + int index = queue[front]; + front += 1; + + b3TreeNode* node = nodes + index; + if ( node->parent == B3_NULL_INDEX ) + { + m_depths[index] = 0; + } + else + { + m_depths[index] = m_depths[node->parent] + 1; + depth = b3MaxInt( depth, m_depths[index] ); + } + + if ( ( node->flags & b3_leafNode ) == 0 ) + { + B3_ASSERT( back < m_tree.nodeCount - 1 ); + queue[back++] = node->children.child1; + queue[back++] = node->children.child2; + } + } + + free( queue ); + + return depth; + } + + void CreateTree() + { + if ( m_tree.nodeCapacity > 0 ) + { + b3DynamicTree_Destroy( &m_tree ); + m_tree = {}; + } + + m_proxies.clear(); + +#ifdef _WIN32 + _aligned_free( m_aabbMinX ); + _aligned_free( m_aabbMinY ); + _aligned_free( m_aabbMinZ ); + _aligned_free( m_aabbMaxX ); + _aligned_free( m_aabbMaxY ); + _aligned_free( m_aabbMaxZ ); +#endif + + m_tree = b3DynamicTree_Create( 512 ); + + char filePath[64] = {}; + snprintf( filePath, 64, "data/trees/%s.txt", m_fileNames[m_fileIndex] ); + float scale = m_config[m_fileIndex].scale; + bool zUp = m_config[m_fileIndex].zUp; + + FILE* file = fopen( filePath, "r" ); + if ( file == nullptr ) + { + return; + } + + constexpr int bufferSize = 512; + char buffer[bufferSize]; + + int maxArea = 0.0f; + int maxAreaIndex = -1; + uint64_t key = 0; + + while ( fgets( buffer, sizeof( buffer ), file ) ) + { + char* trim = buffer; + while ( *trim == ' ' || *trim == '\t' ) + { + ++trim; + } + + if ( *trim == '\0' || *trim == '\n' || *trim == '#' ) + { + continue; + } + + Proxy rec = {}; + float b1, b2, b3, b4, b5, b6; + int parsed = sscanf( buffer, "%f %f %f %f %f %f", &b1, &b2, &b3, &b4, &b5, &b6 ); + + if ( parsed != 6 ) + { + fprintf( stderr, "Skipping malformed line: %s", buffer ); + continue; + } + + if ( zUp ) + { + rec.aabb.lowerBound.x = scale * b1; + rec.aabb.lowerBound.y = scale * b3; + rec.aabb.lowerBound.z = scale * b2; + rec.aabb.upperBound.x = scale * b4; + rec.aabb.upperBound.y = scale * b6; + rec.aabb.upperBound.z = scale * b5; + } + else + { + rec.aabb.lowerBound.x = scale * b1; + rec.aabb.lowerBound.y = scale * b2; + rec.aabb.lowerBound.z = scale * b3; + rec.aabb.upperBound.x = scale * b4; + rec.aabb.upperBound.y = scale * b5; + rec.aabb.upperBound.z = scale * b6; + } + + float area = b3AABB_Area( rec.aabb ); + if ( area > maxArea ) + { + maxArea = area; + maxAreaIndex = (int)m_proxies.size(); + } + + m_proxies[key] = rec; + key += 1; + + // if ( m_proxies.size() == 320 ) + //{ + // break; + // } + } + +#ifdef _WIN32 + int proxyCount = m_proxies.size(); + m_aabbMinX = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 ); + m_aabbMinY = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 ); + m_aabbMinZ = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 ); + m_aabbMaxX = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 ); + m_aabbMaxY = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 ); + m_aabbMaxZ = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 ); + + for ( int i = 0; i < proxyCount; ++i ) + { + b3Vec3 lb = m_proxies[i].aabb.lowerBound; + b3Vec3 ub = m_proxies[i].aabb.upperBound; + + m_aabbMinX[i] = lb.x; + m_aabbMinY[i] = lb.y; + m_aabbMinZ[i] = lb.z; + m_aabbMaxX[i] = ub.x; + m_aabbMaxY[i] = ub.y; + m_aabbMaxZ[i] = ub.z; + } +#endif + + printf( "max index = %d\n", maxAreaIndex ); + + // constexpr int n = m_isDebug ? 1000 : INT_MAX; + + uint64_t ticks = b3GetTicks(); + int count = (int)m_proxies.size(); + for ( int i = 0; i < count; ++i ) + { + // Arbitrary category bits to ensure correctness + m_proxies[i].proxyId = b3DynamicTree_CreateProxy( &m_tree, m_proxies[i].aabb, 1, i ); + m_proxies[i].queryTimeStamp = 0; + m_proxies[i].rayTimeStamp = 0; + } + + m_buildTime = b3GetMilliseconds( ticks ); + b3DynamicTree_Validate( &m_tree ); + m_areaRatio = b3DynamicTree_GetAreaRatio( &m_tree ); + + m_height = ComputeDepths(); + m_drawLevel = -1; + + Generate(); + } + + void LoadTree() + { + if ( m_tree.nodeCapacity > 0 ) + { + b3DynamicTree_Destroy( &m_tree ); + m_tree = {}; + } + + m_proxies.clear(); + +#ifdef _WIN32 + _aligned_free( m_aabbMinX ); + _aligned_free( m_aabbMinY ); + _aligned_free( m_aabbMinZ ); + _aligned_free( m_aabbMaxX ); + _aligned_free( m_aabbMaxY ); + _aligned_free( m_aabbMaxZ ); +#endif + + { + //bool zUp = true; + m_tree = b3DynamicTree_Load( m_saveFileName, m_loadScale ); + } + + constexpr int bufferSize = 512; + char buffer[bufferSize]; + + int maxArea = 0.0f; + int maxAreaIndex = -1; + + for ( int i = 0; i < m_tree.nodeCapacity; ++i ) + { + b3TreeNode* node = m_tree.nodes + i; + + if ( ( node->flags & b3_leafNode ) == 0 ) + { + continue; + } + + assert( node->flags & b3_allocatedNode ); + + Proxy proxy = {}; + proxy.aabb = node->aabb; + + float area = b3AABB_Area( proxy.aabb ); + if ( area > maxArea ) + { + maxArea = area; + maxAreaIndex = (int)m_proxies.size(); + } + + proxy.proxyId = i; + m_proxies[node->userData] = proxy; + } + +#ifdef _WIN32 + int proxyCount = m_proxies.size(); + m_aabbMinX = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 ); + m_aabbMinY = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 ); + m_aabbMinZ = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 ); + m_aabbMaxX = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 ); + m_aabbMaxY = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 ); + m_aabbMaxZ = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 ); + + for ( int i = 0; i < proxyCount; ++i ) + { + b3Vec3 lb = m_proxies[i].aabb.lowerBound; + b3Vec3 ub = m_proxies[i].aabb.upperBound; + + m_aabbMinX[i] = lb.x; + m_aabbMinY[i] = lb.y; + m_aabbMinZ[i] = lb.z; + m_aabbMaxX[i] = ub.x; + m_aabbMaxY[i] = ub.y; + m_aabbMaxZ[i] = ub.z; + } +#endif + + printf( "max index = %d\n", maxAreaIndex ); + + // constexpr int n = m_isDebug ? 1000 : INT_MAX; + + m_buildTime = 0.0f; + b3DynamicTree_Validate( &m_tree ); + m_areaRatio = b3DynamicTree_GetAreaRatio( &m_tree ); + + m_height = ComputeDepths(); + m_drawLevel = -1; + + Generate(); + } + + static float RayCallback( const b3RayCastInput* input, int proxyId, uint64_t userData, void* context ) + { + TreeBenchmark* sample = static_cast( context ); + Proxy& proxy = sample->m_proxies[userData]; + proxy.rayTimeStamp = sample->m_timeStamp; + return 1.0f; + } + + static bool QueryCallback( int proxyId, uint64_t userData, void* context ) + { + TreeBenchmark* sample = static_cast( context ); + Proxy& proxy = sample->m_proxies[userData]; + proxy.queryTimeStamp = sample->m_timeStamp; + return true; + } + + static float ClosetPointCallback( float minDistanceSquared, int proxyId, uint64_t userData, void* context ) + { + (void)proxyId; + + TreeBenchmark* sample = static_cast( context ); + int testIndex = sample->m_testIndex; + Proxy& proxy = sample->m_proxies[userData]; + proxy.queryTimeStamp = sample->m_timeStamp; + b3Vec3 queryPoint = sample->m_closestPointQueries[testIndex].center; + b3Vec3 closestPoint = b3ClosestPointToAABB( queryPoint, proxy.aabb ); + float distanceSquared = b3DistanceSquared( queryPoint, closestPoint ); + if ( distanceSquared < minDistanceSquared ) + { + sample->m_closestPoint = closestPoint; + sample->m_haveClosest = true; + } + + return distanceSquared; + } + + void Generate() + { + srand( 42 ); + + b3AABB bounds = m_tree.nodes[m_tree.root].aabb; + b3Vec3 extents = b3AABB_Extents( bounds ); + float radius = ( extents.x + extents.y + extents.z ) / 3.0f; + + for ( int i = 0; i < m_testCount; ++i ) + { + m_rays[i].origin = RandomVec3( bounds.lowerBound, bounds.upperBound ); + + b3Vec3 end = RandomVec3( bounds.lowerBound, bounds.upperBound ); + m_rays[i].translation = end - m_rays[i].origin; + + float s = RandomFloatRange( 0.01f, 0.2f ); + b3Vec3 c = RandomVec3( bounds.lowerBound, bounds.upperBound ); + b3Vec3 p1 = c - s * extents; + b3Vec3 p2 = c + s * extents; + + m_overlapQueries[i].lowerBound = b3Min( p1, p2 ); + m_overlapQueries[i].upperBound = b3Max( p1, p2 ); + + m_closestPointQueries[i] = { + .center = c, + .radius = s * radius, + }; + } + } + + void Profile() + { + uint64_t ticks = b3GetTicks(); + for ( int i = 0; i < m_testCount; ++i ) + { + Ray ray = m_rays[i]; + b3RayCastInput input = { ray.origin, ray.translation, 1.0f }; + + b3DynamicTree_RayCast( &m_tree, &input, B3_DEFAULT_MASK_BITS, false, RayCallback, this ); + } + m_rayTime = b3GetMillisecondsAndReset( &ticks ); + + for ( int i = 0; i < m_testCount; ++i ) + { + b3DynamicTree_Query( &m_tree, m_overlapQueries[i], B3_DEFAULT_MASK_BITS, false, QueryCallback, this ); + } + m_overlapTime = b3GetMilliseconds( ticks ); + +#if 0 && defined( _WIN32 ) + // brute force avx2 + int proxyCount = (int)m_proxies.size(); + for ( int i = 0; i < m_testCount; ++i ) + { + b3Vec3 point = m_closestPointQueries[i].center; + float minDistSq; + b3Vec3 closest; + + ClosestPointAVX2( m_aabbMinX, m_aabbMinY, m_aabbMinZ, m_aabbMaxX, m_aabbMaxY, m_aabbMaxZ, proxyCount, point, &closest, + &minDistSq ); + + m_closestPoint = closest; + m_haveClosest = true; + } +#elif 0 + // Brute force + int proxyCount = (int)m_proxies.size(); + for ( int i = 0; i < m_testCount; ++i ) + { + b3Vec3 point = m_closestPointQueries[i].center; + float minDistanceSquared = FLT_MAX; + m_haveClosest = false; + for ( int j = 0; j < proxyCount; ++j ) + { + b3Vec3 closestPoint = b3ClosestPointToAABB( point, m_proxies[j].aabb ); + float d = b3DistanceSquared( point, closestPoint ); + if ( d < minDistanceSquared ) + { + m_closestPoint = closestPoint; + m_haveClosest = true; + minDistanceSquared = d; + } + } + } +#else + for ( int i = 0; i < m_testCount; ++i ) + { + b3Vec3 point = m_closestPointQueries[i].center; + float radius = m_closestPointQueries[i].radius; + float distanceSquared = radius * radius; + m_closestPoint = point; + m_haveClosest = false; + b3DynamicTree_QueryClosest( &m_tree, point, B3_DEFAULT_MASK_BITS, false, ClosetPointCallback, this, + &distanceSquared ); + } +#endif + m_closestTime = b3GetMilliseconds( ticks ); + } + + void UpdateUI() override + { + DrawTextLine( "leaves = %d, height = %d, area = %g", m_tree.proxyCount, m_height, m_areaRatio ); + DrawTextLine( "build time = %g ms", m_buildTime ); + DrawTextLine( "total: ray = %g ms, overlap = %g ms, closest = %g ms", m_rayTime, m_overlapTime, m_closestTime ); + + float s = 1000.0f / m_testCount; + DrawTextLine( "ave: ray = %g us, overlap = %g us, closest = %g us", s * m_rayTime, s * m_overlapTime, s * m_closestTime ); + + float fontSize = ImGui::GetFontSize(); + float height = 23.0f * fontSize; + ImGui::SetNextWindowPos( { 0.5f * fontSize, m_camera->m_height - height - 2.0f * fontSize }, ImGuiCond_Once ); + ImGui::SetNextWindowSize( { 18.0f * fontSize, height } ); + + ImGui::Begin( "Tree", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + if ( ImGui::Combo( "File", &m_fileIndex, m_fileNames, m_fileCount ) ) + { + CreateTree(); + } + + if ( ImGui::Button( "Top Down" ) ) + { + uint64_t ticks = b3GetTicks(); + b3DynamicTree_Rebuild( &m_tree, true ); + m_buildTime = b3GetMilliseconds( ticks ); + m_areaRatio = b3DynamicTree_GetAreaRatio( &m_tree ); + m_height = ComputeDepths(); + } + + ImGui::Separator(); + + ImGui::Checkbox( "Ray Cast", &m_doRay ); + ImGui::Checkbox( "Overlap", &m_doOverlap ); + ImGui::Checkbox( "Closet Point", &m_doClosest ); + + if ( ImGui::Button( "Profile" ) ) + { + Profile(); + } + + ImGui::SliderInt( "Test", &m_testIndex, 0, m_testCount - 1 ); + ImGui::SliderInt( "Level", &m_drawLevel, -1, m_height ); + ImGui::SliderFloat( "Kilometers", &m_drawDistance, 0.5f, 20.0f, "%.1f" ); + + if ( ImGui::Button( "Save" ) ) + { + b3DynamicTree_Save( &m_tree, m_saveFileName ); + } + + if ( ImGui::Button( "Load" ) ) + { + LoadTree(); + } + + ImGui::SliderFloat( "Load Scale", &m_loadScale, 0.01f, 1.0f ); + + ImGui::End(); + } + + void Render() override + { + Sample::Render(); + + DrawTransform( m_scene, b3Transform_identity, 2.0f ); + + b3Vec3 cp = m_camera->GetPosition(); + b3TreeNode* nodes = m_tree.nodes; + float distSquared = m_drawDistance * m_drawDistance * 1000.0f * 1000.0f; + + if ( m_drawLevel >= 0 ) + { + constexpr int colorCount = 20; + b3HexColor colors[colorCount] = { + b3_colorAliceBlue, b3_colorAntiqueWhite, b3_colorAqua, b3_colorAquamarine, b3_colorAzure, + b3_colorBeige, b3_colorBisque, b3_colorBlanchedAlmond, b3_colorBlue, b3_colorBlueViolet, + b3_colorBrown, b3_colorBurlywood, b3_colorCadetBlue, b3_colorChartreuse, b3_colorChocolate, + b3_colorCoral, b3_colorCornflowerBlue, b3_colorCornsilk, b3_colorCrimson, b3_colorCyan, + }; + + int capacity = m_tree.nodeCapacity; + for ( int i = 0; i < capacity; ++i ) + { + b3TreeNode* node = nodes + i; + if ( m_depths[i] != m_drawLevel || ( node->flags & b3_allocatedNode ) == 0 ) + { + // skip internal nodes + continue; + } + + b3Vec3 c = b3AABB_Center( node->aabb ); + if ( m_drawLevel < 10 || b3DistanceSquared( c, cp ) < distSquared ) + { + DrawBounds( m_scene, node->aabb, 0.0f, colors[m_drawLevel % colorCount] ); + } + } + } + else + { + uint16_t requiredFlags = b3_allocatedNode | b3_leafNode; + for ( int i = 0; i < m_tree.nodeCapacity; ++i ) + { + b3TreeNode* node = nodes + i; + + if ( node->flags != requiredFlags ) + { + continue; + } + + b3Vec3 c = b3AABB_Center( node->aabb ); + if ( b3DistanceSquared( c, cp ) > distSquared ) + { + continue; + } + + Proxy& proxy = m_proxies[node->userData]; + if ( proxy.queryTimeStamp == m_timeStamp || proxy.rayTimeStamp == m_timeStamp ) + { + DrawBounds( m_scene, node->aabb, 0.0f, b3_colorLightGray ); + } + else + { + // float extension = 0.01f * node->height; + float extension = 0.0f; + DrawBounds( m_scene, node->aabb, extension, b3_colorLightBlue ); + } + } + } + + if ( m_doRay ) + { + Ray ray = m_rays[m_testIndex]; + DrawLine( m_scene, ray.origin, ray.origin + ray.translation, b3_colorRed ); + } + + if ( m_doOverlap ) + { + DrawBounds( m_scene, m_overlapQueries[m_testIndex], 0.0f, b3_colorRed ); + } + + if ( m_doClosest ) + { + DrawSphere( m_scene, b3Transform_identity, m_closestPointQueries[m_testIndex], b3_colorCyan ); + if ( m_haveClosest ) + { + DrawPoint( m_scene, m_closestPoint, 15.0f, b3_colorOrange ); + } + } + } + + void Step() override + { + Sample::Step(); + ++m_timeStamp; + + if ( m_doRay ) + { + Ray ray = m_rays[m_testIndex]; + b3RayCastInput input = { ray.origin, ray.translation, 1.0f }; + + b3DynamicTree_RayCast( &m_tree, &input, B3_DEFAULT_MASK_BITS, false, RayCallback, this ); + } + + if ( m_doOverlap ) + { + b3DynamicTree_Query( &m_tree, m_overlapQueries[m_testIndex], B3_DEFAULT_MASK_BITS, false, QueryCallback, this ); + } + + if ( m_doClosest ) + { + b3Vec3 point = m_closestPointQueries[m_testIndex].center; + float radius = m_closestPointQueries[m_testIndex].radius; + float distanceSquared = radius * radius; + m_closestPoint = point; + m_haveClosest = false; + b3DynamicTree_QueryClosest( &m_tree, point, B3_DEFAULT_MASK_BITS, false, ClosetPointCallback, this, + &distanceSquared ); + } + + // constexpr int n = m_isDebug ? 0 : 100; + // for ( int i = 0; i < n; ++i ) + //{ + // Profile(); + // } + } + + static Sample* Create( SampleContext* context ) + { + return new TreeBenchmark( context ); + } + + static constexpr int m_fileCount = 3; + static constexpr int m_testCount = 1024; + static constexpr const char* m_saveFileName = "tree.dt"; + + const char* m_fileNames[m_fileCount]; + FileConfig m_config[m_fileCount]; + int m_fileIndex; + + float* m_aabbMinX; + float* m_aabbMinY; + float* m_aabbMinZ; + float* m_aabbMaxX; + float* m_aabbMaxY; + float* m_aabbMaxZ; + + std::unordered_map m_proxies; + std::vector m_depths; + b3DynamicTree m_tree; + Ray m_rays[m_testCount]; + b3AABB m_overlapQueries[m_testCount]; + b3Sphere m_closestPointQueries[m_testCount]; + b3Vec3 m_closestPoint; + float m_areaRatio; + float m_rayTime; + float m_overlapTime; + float m_closestTime; + float m_buildTime; + float m_drawDistance; + float m_loadScale; + int m_testIndex; + int m_timeStamp; + int m_drawLevel; + int m_height; + bool m_haveClosest; + bool m_doOverlap; + bool m_doClosest; + bool m_doRay; +}; + +static int sampleTree = SampleManager::Register( "Tree", "Benchmark", TreeBenchmark::Create ); diff --git a/samples/scene.cpp b/samples/scene.cpp new file mode 100644 index 0000000..76f0dd1 --- /dev/null +++ b/samples/scene.cpp @@ -0,0 +1,2320 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "scene.h" + +#include "../src/core.h" +#include "camera.h" +#include "geo_buffer.h" +#include "renderer.h" +#include "shader.h" +#include "ssao_buffer.h" +#include "utility.h" + +#include "box3d/math_functions.h" + +#include +#include +#include +#include +#include +#include +#include + +static const int shadowWidth = 4096; +static const int shadowHeight = 4096; +static constexpr int s_batchSize = 256; + +// The number of sides of the base circle. For spheres and capsules. +static constexpr int s_sideCount = 16; + +// The number of quad rings going from the middle to the sphere end point +// for example, a value of 2 has 2 rings plus the end cone. For spheres and capsules. +static constexpr int s_ringCount = 8; + +struct Cube +{ + b3Transform transform; + RGBAf color; + bool invert; + bool shadowCaster; +}; + +struct DebugCube +{ + b3Transform transform; + RGBAf color; +}; + +struct Edge +{ + int index1; + int index2; + int flags; +}; + +struct EdgeData +{ + b3Vec3 position; + int flags; +}; + +struct InstanceShape +{ + int triangleCount; + uint32_t triangleArrayId; + + // Static and dynamic buffers + uint32_t triangleBufferIds[2]; + + int edgeCount; + uint32_t edgeArrayId; + + // Static and dynamic buffers + uint32_t edgeBufferIds[2]; +}; + +struct InstanceData +{ + RGBA8 rgba8; + Matrix4 matrix; +}; + +struct Scene +{ + Scene() = default; + + Arena arena; + + std::vector debugCubes; + std::vector sphereInstances; + std::vector openHemisphereInstances; + std::vector openCylinderInstances; + + // Instances organized by hash + std::unordered_map> hullInstances; + std::unordered_map> meshInstances; + std::unordered_map> heightFieldInstances; + + std::vector transparentSphereInstances; + std::vector transparentOpenHemisphereInstances; + std::vector transparentOpenCylinderInstances; + std::unordered_map> transparentHullInstances; + + uint32_t shadowShader = 0; + uint32_t meshShader = 0; + uint32_t meshShadowShader = 0; + uint32_t meshEdgeShader = 0; + uint32_t geometryShader = 0; + uint32_t ssaoShader = 0; + uint32_t ssaoBlurShader = 0; + uint32_t deferredShader = 0; + uint32_t meshForwardShader = 0; + uint32_t forwardShader = 0; + uint32_t depthCopyShader = 0; + + uint32_t depthMapFBO = 0; + uint32_t depthMap = 0; + + PointDraw* pointDraw = nullptr; + LineDraw* lineDraw = nullptr; + GeoBuffer* geoBuffer = nullptr; + SsaoBuffer* ssaoBuffer = nullptr; + + InstanceShape sphere; + + // Building blocks for capsules + InstanceShape openHemisphere; + InstanceShape openCylinder; + + std::unordered_map sharedHulls; + std::unordered_map sharedMeshes; + std::unordered_map sharedHeightFields; + + SceneAOSettings ao = {}; + + b3Vec3 lightPosition{ -16.0f, 40.0f, 10.0f }; + + bool useSSAO = false; + bool useShadow = false; + bool useGrid = false; +}; + +// Renders a 1x1 XY quad in NDC +static uint32_t quadVAO = 0; +static uint32_t quadVBO = 0; +static void DrawQuad() +{ + if ( quadVAO == 0 ) + { + float quadVertices[] = { + // positions texture Coords + // (x, y, z, u, v) + -1.0f, 1.0f, 0.0f, 0.0f, 1.0f, -1.0f, -1.0f, 0.0f, 0.0f, 0.0f, + 1.0f, 1.0f, 0.0f, 1.0f, 1.0f, 1.0f, -1.0f, 0.0f, 1.0f, 0.0f, + }; + + glGenVertexArrays( 1, &quadVAO ); + glGenBuffers( 1, &quadVBO ); + glBindVertexArray( quadVAO ); + glBindBuffer( GL_ARRAY_BUFFER, quadVBO ); + glBufferData( GL_ARRAY_BUFFER, sizeof( quadVertices ), &quadVertices, GL_STATIC_DRAW ); + glEnableVertexAttribArray( 0 ); + glVertexAttribPointer( 0, 3, GL_FLOAT, GL_FALSE, 5 * sizeof( float ), (void*)0 ); + glEnableVertexAttribArray( 1 ); + glVertexAttribPointer( 1, 2, GL_FLOAT, GL_FALSE, 5 * sizeof( float ), (void*)( 3 * sizeof( float ) ) ); + } + + glBindVertexArray( quadVAO ); + glDrawArrays( GL_TRIANGLE_STRIP, 0, 4 ); + glBindVertexArray( 0 ); +} + +static uint32_t cubeArrayId = 0; +static uint32_t cubeBufferId = 0; +void DrawCube( Scene* scene, b3Transform transform, b3HexColor color, const Matrix4& view, const Matrix4& projection ) +{ + // initialize (if necessary) + if ( cubeArrayId == 0 ) + { + // position / normal / uv + + float vertices[] = { + // back face + -1.0f, -1.0f, -1.0f, 0.0f, 0.0f, -1.0f, // bottom-left + 1.0f, 1.0f, -1.0f, 0.0f, 0.0f, -1.0f, // top-right + 1.0f, -1.0f, -1.0f, 0.0f, 0.0f, -1.0f, // bottom-right + 1.0f, 1.0f, -1.0f, 0.0f, 0.0f, -1.0f, // top-right + -1.0f, -1.0f, -1.0f, 0.0f, 0.0f, -1.0f, // bottom-left + -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, -1.0f, // top-left + // front face + -1.0f, -1.0f, 1.0f, 0.0f, 0.0f, 1.0f, // bottom-left + 1.0f, -1.0f, 1.0f, 0.0f, 0.0f, 1.0f, // bottom-right + 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, // top-right + 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, // top-right + -1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, // top-left + -1.0f, -1.0f, 1.0f, 0.0f, 0.0f, 1.0f, // bottom-left + // left face + -1.0f, 1.0f, 1.0f, -1.0f, 0.0f, 0.0f, // top-right + -1.0f, 1.0f, -1.0f, -1.0f, 0.0f, 0.0f, // top-left + -1.0f, -1.0f, -1.0f, -1.0f, 0.0f, 0.0f, // bottom-left + -1.0f, -1.0f, -1.0f, -1.0f, 0.0f, 0.0f, // bottom-left + -1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, // bottom-right + -1.0f, 1.0f, 1.0f, -1.0f, 0.0f, 0.0f, // top-right + // right face + 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, // top-left + 1.0f, -1.0f, -1.0f, 1.0f, 0.0f, 0.0f, // bottom-right + 1.0f, 1.0f, -1.0f, 1.0f, 0.0f, 0.0f, // top-right + 1.0f, -1.0f, -1.0f, 1.0f, 0.0f, 0.0f, // bottom-right + 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, // top-left + 1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 0.0f, // bottom-left + // bottom face + -1.0f, -1.0f, -1.0f, 0.0f, -1.0f, 0.0f, // top-right + 1.0f, -1.0f, -1.0f, 0.0f, -1.0f, 0.0f, // top-left + 1.0f, -1.0f, 1.0f, 0.0f, -1.0f, 0.0f, // bottom-left + 1.0f, -1.0f, 1.0f, 0.0f, -1.0f, 0.0f, // bottom-left + -1.0f, -1.0f, 1.0f, 0.0f, -1.0f, 0.0f, // bottom-right + -1.0f, -1.0f, -1.0f, 0.0f, -1.0f, 0.0f, // top-right + // top face + -1.0f, 1.0f, -1.0f, 0.0f, 1.0f, 0.0f, // top-left + 1.0f, 1.0f, 1.0f, 0.0f, 1.0f, 0.0f, // bottom-right + 1.0f, 1.0f, -1.0f, 0.0f, 1.0f, 0.0f, // top-right + 1.0f, 1.0f, 1.0f, 0.0f, 1.0f, 0.0f, // bottom-right + -1.0f, 1.0f, -1.0f, 0.0f, 1.0f, 0.0f, // top-left + -1.0f, 1.0f, 1.0f, 0.0f, 1.0f, 0.0f // bottom-left + }; + + glGenVertexArrays( 1, &cubeArrayId ); + glGenBuffers( 1, &cubeBufferId ); + + // fill buffer + glBindBuffer( GL_ARRAY_BUFFER, cubeBufferId ); + glBufferData( GL_ARRAY_BUFFER, sizeof( vertices ), vertices, GL_STATIC_DRAW ); + + // link vertex attributes + glBindVertexArray( cubeArrayId ); + + // position + glEnableVertexAttribArray( 0 ); + glVertexAttribPointer( 0, 3, GL_FLOAT, GL_FALSE, 6 * sizeof( float ), (void*)0 ); + + // normal + glEnableVertexAttribArray( 1 ); + glVertexAttribPointer( 1, 3, GL_FLOAT, GL_FALSE, 6 * sizeof( float ), (void*)( 3 * sizeof( float ) ) ); + + glBindBuffer( GL_ARRAY_BUFFER, 0 ); + glBindVertexArray( 0 ); + } + + glUseProgram( scene->forwardShader ); + SetUniformMatrix4( scene->forwardShader, "projection", projection ); + SetUniformMatrix4( scene->forwardShader, "view", view ); + + Matrix4 m = MakeMatrix4( transform, 0.25f ); + SetUniformMatrix4( scene->forwardShader, "model", m ); + + RGBAf c = MakeRGBAf( color ); + SetUniformVec3( scene->forwardShader, "color", { c.r, c.g, c.b } ); + + glBindVertexArray( cubeArrayId ); + glDrawArrays( GL_TRIANGLES, 0, 36 ); + glBindVertexArray( 0 ); +} + +struct VertexData +{ + b3Vec3 position; + b3Vec3 normal; + int material; +}; + +// Creates a shape that can be instanced +InstanceShape CreateInstanceShape( const b3Vec3* vertexPositions, const b3Vec3* vertexNormals, const int* triangleIndices, + int triangleCount, const Edge* edges, int edgeCount, const int* materials, Arena arena ) +{ + InstanceShape shape = {}; + + // Vertex buffer + { + int vertexCount = 3 * triangleCount; + VertexData* vertexData = (VertexData*)arena.Allocate( vertexCount * sizeof( VertexData ) ); + + int index = 0; + for ( int triangleIndex = 0; triangleIndex < triangleCount; ++triangleIndex ) + { + int vertexIndex1 = triangleIndices[3 * triangleIndex + 0]; + int vertexIndex2 = triangleIndices[3 * triangleIndex + 1]; + int vertexIndex3 = triangleIndices[3 * triangleIndex + 2]; + + b3Vec3 vertexPosition1 = vertexPositions[vertexIndex1]; + b3Vec3 vertexPosition2 = vertexPositions[vertexIndex2]; + b3Vec3 vertexPosition3 = vertexPositions[vertexIndex3]; + + vertexData[index + 0].position = vertexPosition1; + vertexData[index + 1].position = vertexPosition2; + vertexData[index + 2].position = vertexPosition3; + + b3Vec3 vertexNormal1, vertexNormal2, vertexNormal3; + if ( vertexNormals ) + { + vertexNormal1 = vertexNormals[vertexIndex1]; + vertexNormal2 = vertexNormals[vertexIndex2]; + vertexNormal3 = vertexNormals[vertexIndex3]; + } + else + { + b3Vec3 edge1 = vertexPosition2 - vertexPosition1; + b3Vec3 edge2 = vertexPosition3 - vertexPosition1; + b3Vec3 normal = b3Cross( edge1, edge2 ); + normal = b3Normalize( normal ); + + vertexNormal1 = normal; + vertexNormal2 = normal; + vertexNormal3 = normal; + } + + vertexData[index + 0].normal = vertexNormal1; + vertexData[index + 1].normal = vertexNormal2; + vertexData[index + 2].normal = vertexNormal3; + + if ( materials != nullptr ) + { + vertexData[index + 0].material = materials[triangleIndex]; + vertexData[index + 1].material = materials[triangleIndex]; + vertexData[index + 2].material = materials[triangleIndex]; + } + else + { + vertexData[index + 0].material = -1; + vertexData[index + 1].material = -1; + vertexData[index + 2].material = -1; + } + + index += 3; + } + + assert( index == vertexCount ); + + // Fill vertex buffers + shape.triangleCount = triangleCount; + + glGenVertexArrays( 1, &shape.triangleArrayId ); + glGenBuffers( 2, shape.triangleBufferIds ); + + // Copy static vertex data + glBindVertexArray( shape.triangleArrayId ); + glBindBuffer( GL_ARRAY_BUFFER, shape.triangleBufferIds[0] ); + glBufferData( GL_ARRAY_BUFFER, vertexCount * sizeof( VertexData ), vertexData, GL_STATIC_DRAW ); + + // positions + uint32_t positionAttribute = 0; + glEnableVertexAttribArray( positionAttribute ); + glVertexAttribPointer( positionAttribute, 3, GL_FLOAT, GL_FALSE, sizeof( VertexData ), + (void*)offsetof( VertexData, position ) ); + + // normals + uint32_t normalAttribute = 1; + glEnableVertexAttribArray( normalAttribute ); + glVertexAttribPointer( normalAttribute, 3, GL_FLOAT, GL_FALSE, sizeof( VertexData ), + (void*)offsetof( VertexData, normal ) ); + + // materials + uint32_t materialAttribute = 2; + glEnableVertexAttribArray( materialAttribute ); + glVertexAttribIPointer( materialAttribute, 1, GL_INT, sizeof( VertexData ), (void*)offsetof( VertexData, material ) ); + + // Instance data setup + glBindBuffer( GL_ARRAY_BUFFER, shape.triangleBufferIds[1] ); + glBufferData( GL_ARRAY_BUFFER, s_batchSize * sizeof( InstanceData ), nullptr, GL_DYNAMIC_DRAW ); + + uint32_t colorInstance = 3; + glEnableVertexAttribArray( colorInstance ); + // Save bandwidth by expanding color to floats in the shader + glVertexAttribPointer( colorInstance, 4, GL_UNSIGNED_BYTE, GL_TRUE, sizeof( InstanceData ), + (void*)offsetof( InstanceData, rgba8 ) ); + + // This makes the color an instance + glVertexAttribDivisor( colorInstance, 1 ); + + // Matrix has to use 4 attributes each 16 bytes + uint32_t matrixAttribute = 4; + glEnableVertexAttribArray( matrixAttribute + 0 ); + glEnableVertexAttribArray( matrixAttribute + 1 ); + glEnableVertexAttribArray( matrixAttribute + 2 ); + glEnableVertexAttribArray( matrixAttribute + 3 ); + + glVertexAttribPointer( matrixAttribute + 0, 4, GL_FLOAT, GL_FALSE, sizeof( InstanceData ), + (void*)offsetof( InstanceData, matrix.cx ) ); + glVertexAttribPointer( matrixAttribute + 1, 4, GL_FLOAT, GL_FALSE, sizeof( InstanceData ), + (void*)offsetof( InstanceData, matrix.cy ) ); + glVertexAttribPointer( matrixAttribute + 2, 4, GL_FLOAT, GL_FALSE, sizeof( InstanceData ), + (void*)offsetof( InstanceData, matrix.cz ) ); + glVertexAttribPointer( matrixAttribute + 3, 4, GL_FLOAT, GL_FALSE, sizeof( InstanceData ), + (void*)offsetof( InstanceData, matrix.cw ) ); + + // This makes the matrix an instance + glVertexAttribDivisor( matrixAttribute + 0, 1 ); + glVertexAttribDivisor( matrixAttribute + 1, 1 ); + glVertexAttribDivisor( matrixAttribute + 2, 1 ); + glVertexAttribDivisor( matrixAttribute + 3, 1 ); + + glBindBuffer( GL_ARRAY_BUFFER, 0 ); + glBindVertexArray( 0 ); + } + + CheckOpenGL(); + + // Edges are optional + if ( edgeCount > 0 ) + { + // Create edge buffer + EdgeData* edgeBuffer = (EdgeData*)arena.Allocate( 2 * edgeCount * sizeof( EdgeData ) ); + for ( int edgeIndex = 0; edgeIndex < edgeCount; ++edgeIndex ) + { + int vertexIndex1 = edges[edgeIndex].index1; + int vertexIndex2 = edges[edgeIndex].index2; + int flags = edges[edgeIndex].flags; + + const b3Vec3* vb = vertexPositions; + b3Vec3 vertexPosition1 = vb[vertexIndex1]; + b3Vec3 vertexPosition2 = vb[vertexIndex2]; + + edgeBuffer[2 * edgeIndex + 0].position = vertexPosition1; + edgeBuffer[2 * edgeIndex + 0].flags = flags; + edgeBuffer[2 * edgeIndex + 1].position = vertexPosition2; + edgeBuffer[2 * edgeIndex + 1].flags = flags; + } + + shape.edgeCount = edgeCount; + + glGenVertexArrays( 1, &shape.edgeArrayId ); + glGenBuffers( 2, shape.edgeBufferIds ); + + // Copy static vertex data + glBindVertexArray( shape.edgeArrayId ); + glBindBuffer( GL_ARRAY_BUFFER, shape.edgeBufferIds[0] ); + glBufferData( GL_ARRAY_BUFFER, 2 * edgeCount * sizeof( EdgeData ), edgeBuffer, GL_STATIC_DRAW ); + + uint32_t positionIndex = 0; + glEnableVertexAttribArray( positionIndex ); + glVertexAttribPointer( positionIndex, 3, GL_FLOAT, GL_FALSE, sizeof( EdgeData ), (void*)offsetof( EdgeData, position ) ); + + uint32_t flagsIndex = 1; + glEnableVertexAttribArray( flagsIndex ); + glVertexAttribIPointer( flagsIndex, 1, GL_INT, sizeof( EdgeData ), (void*)offsetof( EdgeData, flags ) ); + + // Instance data setup + glBindBuffer( GL_ARRAY_BUFFER, shape.edgeBufferIds[1] ); + glBufferData( GL_ARRAY_BUFFER, s_batchSize * sizeof( InstanceData ), nullptr, GL_DYNAMIC_DRAW ); + + uint32_t colorInstance = 2; + glEnableVertexAttribArray( colorInstance ); + // Save bandwidth by expanding color to floats in the shader + glVertexAttribPointer( colorInstance, 4, GL_UNSIGNED_BYTE, GL_TRUE, sizeof( InstanceData ), + (void*)offsetof( InstanceData, rgba8 ) ); + + // This makes the color an instance + glVertexAttribDivisor( colorInstance, 1 ); + + // Matrix has to use 4 attributes each 16 bytes + uint32_t matrixAttribute = 3; + glEnableVertexAttribArray( matrixAttribute + 0 ); + glEnableVertexAttribArray( matrixAttribute + 1 ); + glEnableVertexAttribArray( matrixAttribute + 2 ); + glEnableVertexAttribArray( matrixAttribute + 3 ); + + glVertexAttribPointer( matrixAttribute + 0, 4, GL_FLOAT, GL_FALSE, sizeof( InstanceData ), + (void*)offsetof( InstanceData, matrix.cx ) ); + glVertexAttribPointer( matrixAttribute + 1, 4, GL_FLOAT, GL_FALSE, sizeof( InstanceData ), + (void*)offsetof( InstanceData, matrix.cy ) ); + glVertexAttribPointer( matrixAttribute + 2, 4, GL_FLOAT, GL_FALSE, sizeof( InstanceData ), + (void*)offsetof( InstanceData, matrix.cz ) ); + glVertexAttribPointer( matrixAttribute + 3, 4, GL_FLOAT, GL_FALSE, sizeof( InstanceData ), + (void*)offsetof( InstanceData, matrix.cw ) ); + + // This makes the matrix an instance + glVertexAttribDivisor( matrixAttribute + 0, 1 ); + glVertexAttribDivisor( matrixAttribute + 1, 1 ); + glVertexAttribDivisor( matrixAttribute + 2, 1 ); + glVertexAttribDivisor( matrixAttribute + 3, 1 ); + + glBindBuffer( GL_ARRAY_BUFFER, 0 ); + glBindVertexArray( 0 ); + } + + CheckOpenGL(); + + return shape; +} + +void DestroySharedShape( InstanceShape shape ) +{ + glDeleteBuffers( 2, shape.triangleBufferIds ); + glDeleteVertexArrays( 1, &shape.triangleArrayId ); + + if ( shape.edgeCount > 0 ) + { + glDeleteBuffers( 2, shape.edgeBufferIds ); + glDeleteVertexArrays( 1, &shape.edgeArrayId ); + } +} + +static InstanceShape CreateSphere( Arena arena ) +{ + float radius = 1.0f; + + int vertexCount = s_sideCount * s_ringCount + 2; + b3Vec3* vertices = (b3Vec3*)arena.Allocate( vertexCount * sizeof( b3Vec3 ) ); + b3Vec3* normals = (b3Vec3*)arena.Allocate( vertexCount * sizeof( b3Vec3 ) ); + + int triangleCount = s_sideCount * 2 * s_ringCount; + int* indices = (int*)arena.Allocate( 3 * triangleCount * sizeof( int ) ); + + // poles + vertices[s_sideCount * s_ringCount + 0] = b3Vec3{ -radius, 0.0f, 0.0f }; + normals[s_sideCount * s_ringCount + 0] = { -1.0f, 0.0f, 0.0f }; + vertices[s_sideCount * s_ringCount + 1] = b3Vec3{ radius, 0.0f, 0.0f }; + normals[s_sideCount * s_ringCount + 1] = { 1.0f, 0.0f, 0.0f }; + + int j = 0; + + for ( int i = 0; i < s_sideCount; ++i ) + { + float theta = ( -2 * B3_PI * i ) / s_sideCount; + float s0 = b3Sin( theta ); + float c0 = b3Cos( theta ); + + int i1 = ( i + 1 ) % s_sideCount; + + // Sides + for ( int k = 0; k <= s_ringCount; ++k ) + { + float phi = ( k + 1 ) * B3_PI / ( s_ringCount + 1 ) - 0.5f * B3_PI; + float x0 = b3Sin( phi ); + float y0 = b3Cos( phi ); + + b3Vec3 normal = { x0, c0 * y0, s0 * y0 }; + vertices[s_ringCount * i + k] = radius * normal; + normals[s_ringCount * i + k] = normal; + + if ( k + 1 < s_ringCount ) + { + indices[j++] = s_ringCount * i1 + k; + indices[j++] = s_ringCount * i + k; + indices[j++] = s_ringCount * i1 + k + 1; + + indices[j++] = s_ringCount * i1 + k + 1; + indices[j++] = s_ringCount * i + k; + indices[j++] = s_ringCount * i + k + 1; + } + else + { + // Needed the vertices, but not the indices + break; + } + + B3_ASSERT( s_ringCount * i + k + 1 < vertexCount ); + } + + // Caps + indices[j++] = s_ringCount * s_sideCount; + indices[j++] = s_ringCount * i; + indices[j++] = s_ringCount * i1; + + indices[j++] = s_ringCount * s_sideCount + 1; + indices[j++] = s_ringCount * i1 + s_ringCount - 1; + indices[j++] = s_ringCount * i + s_ringCount - 1; + } + + B3_ASSERT( j == 3 * triangleCount ); + + // todo add line discs for transparent view + + return CreateInstanceShape( vertices, normals, indices, triangleCount, nullptr, 0, nullptr, arena ); +} + +// Open hemisphere along x-axis. Radius is 1. +static InstanceShape CreateOpenHemisphere( Arena arena ) +{ + float radius = 1.0f; + + const int sideVertexCount = s_ringCount + 1; + + // Vertex count including pole + int vertexCount = s_sideCount * sideVertexCount + 1; + b3Vec3* vertices = (b3Vec3*)arena.Allocate( vertexCount * sizeof( b3Vec3 ) ); + b3Vec3* normals = (b3Vec3*)arena.Allocate( vertexCount * sizeof( b3Vec3 ) ); + + // Hemisphere quad rings and a cone at the pole + int triangleCount = 2 * s_sideCount * s_ringCount + s_sideCount; + int* indices = (int*)arena.Allocate( 3 * triangleCount * sizeof( int ) ); + + // Pole at the end of the array + vertices[s_sideCount * sideVertexCount] = b3Vec3{ radius, 0.0f, 0.0f }; + normals[s_sideCount * sideVertexCount] = b3Vec3{ 1.0f, 0.0f, 0.0f }; + + int j = 0; + + // loop sides + for ( int i = 0; i < s_sideCount; ++i ) + { + int i1 = ( i + 1 ) % s_sideCount; + + float theta = ( -2.0f * B3_PI * i ) / s_sideCount; + b3CosSin csTheta = b3ComputeCosSin( theta ); + float c0 = csTheta.cosine; + float s0 = csTheta.sine; + + // loop rings + for ( int k = 0;; ++k ) + { + int k1 = k + 1; + + float phi = ( 0.5f * k * B3_PI ) / ( s_ringCount + 1 ); + b3CosSin csPhi = b3ComputeCosSin( phi ); + float y0 = csPhi.cosine; + float x0 = csPhi.sine; + + b3Vec3 normal = { x0, c0 * y0, s0 * y0 }; + b3Vec3 vertex = radius * normal; + vertices[sideVertexCount * i + k] = vertex; + normals[sideVertexCount * i + k] = normal; + + if ( k < s_ringCount ) + { + // ring quad + indices[j++] = sideVertexCount * i1 + k; + indices[j++] = sideVertexCount * i + k; + indices[j++] = sideVertexCount * i1 + k1; + + indices[j++] = sideVertexCount * i1 + k1; + indices[j++] = sideVertexCount * i + k; + indices[j++] = sideVertexCount * i + k1; + } + else + { + // cone + indices[j++] = s_sideCount * sideVertexCount; + indices[j++] = sideVertexCount * i1 + k; + indices[j++] = sideVertexCount * i + k; + break; + } + } + } + + B3_ASSERT( j == 3 * triangleCount ); + + return CreateInstanceShape( vertices, normals, indices, triangleCount, nullptr, 0, nullptr, arena ); +} + +static InstanceShape CreateOpenCylinder( Arena arena ) +{ + float radius = 1.0f; + float height = 1.0f; + + int vertexCount = 2 * s_sideCount; + b3Vec3* vertices = (b3Vec3*)arena.Allocate( vertexCount * sizeof( b3Vec3 ) ); + b3Vec3* normals = (b3Vec3*)arena.Allocate( vertexCount * sizeof( b3Vec3 ) ); + + int triangleCount = 2 * s_sideCount; + int* indices = (int*)arena.Allocate( 3 * triangleCount * sizeof( int ) ); + + int j = 0; + + // loop sides + for ( int i = 0; i < s_sideCount; ++i ) + { + int i1 = ( i + 1 ) % s_sideCount; + + float theta = ( -2.0f * B3_PI * i ) / s_sideCount; + b3CosSin csTheta = b3ComputeCosSin( theta ); + float c0 = csTheta.cosine; + float s0 = csTheta.sine; + + b3Vec3 normal = { 0.0f, c0, s0 }; + b3Vec3 vertex1 = radius * normal - b3Vec3{ 0.5f * height, 0.0f, 0.0f }; + vertices[2 * i + 0] = vertex1; + normals[2 * i + 0] = normal; + + b3Vec3 vertex2 = radius * normal + b3Vec3{ 0.5f * height, 0.0f, 0.0f }; + vertices[2 * i + 1] = vertex2; + normals[2 * i + 1] = normal; + + // side quad + indices[j++] = 2 * i1; + indices[j++] = 2 * i; + indices[j++] = 2 * i1 + 1; + + indices[j++] = 2 * i1 + 1; + indices[j++] = 2 * i; + indices[j++] = 2 * i + 1; + } + + B3_ASSERT( j == 3 * triangleCount ); + + return CreateInstanceShape( vertices, normals, indices, triangleCount, nullptr, 0, nullptr, arena ); +} + +Scene* CreateScene( const Camera* camera, Arena arena ) +{ + Scene* scene = new Scene; + + scene->ao.radius = 1.0f; + scene->ao.bias = 0.25f; + scene->ao.minScale = 0.0f; + scene->ao.power = 2.0f; + scene->ao.direct = 0.5f; + scene->ao.aoOnly = false; + scene->useSSAO = true; + scene->useShadow = true; + scene->useGrid = false; + + scene->pointDraw = CreateDebugPoints(); + scene->lineDraw = CreateDebugLines(); + + scene->shadowShader = CreateProgramFromFiles( "shaders/shadow_map.vs", "shaders/empty.fs" ); + scene->meshShader = CreateProgramFromFiles( "shaders/mesh.vs", "shaders/mesh.fs" ); + scene->meshShadowShader = CreateProgramFromFiles( "shaders/mesh_shadow.vs", "shaders/empty.fs" ); + scene->meshEdgeShader = CreateProgramFromFiles( "shaders/mesh_edge.vs", "shaders/forward.fs" ); + scene->geometryShader = CreateProgramFromFiles( "shaders/geometry_vs.glsl", "shaders/geometry_fs.glsl" ); + scene->ssaoShader = CreateProgramFromFiles( "shaders/quad_vs.glsl", "shaders/ssao_fs.glsl" ); + scene->ssaoBlurShader = CreateProgramFromFiles( "shaders/quad_vs.glsl", "shaders/ssao_blur_fs.glsl" ); + scene->deferredShader = CreateProgramFromFiles( "shaders/quad_vs.glsl", "shaders/deferred_lighting_fs.glsl" ); + scene->meshForwardShader = CreateProgramFromFiles( "shaders/mesh_forward.vs", "shaders/mesh_forward.fs" ); + scene->forwardShader = CreateProgramFromFiles( "shaders/forward.vs", "shaders/forward.fs" ); + scene->depthCopyShader = CreateProgramFromFiles( "shaders/quad_vs.glsl", "shaders/depth_copy_fs.glsl" ); + + scene->sphere = CreateSphere( arena ); + scene->openHemisphere = CreateOpenHemisphere( arena ); + scene->openCylinder = CreateOpenCylinder( arena ); + + if ( scene->shadowShader == 0 || scene->meshShader == 0 || scene->meshEdgeShader == 0 || scene->meshEdgeShader == 0 || + scene->geometryShader == 0 || scene->ssaoShader == 0 || scene->ssaoBlurShader == 0 || scene->deferredShader == 0 || + scene->forwardShader == 0 || scene->depthCopyShader == 0 ) + { + fprintf( stderr, "Failed to create shaders\n" ); + return nullptr; + } + + // configure depth map FBO for shadow map + glGenFramebuffers( 1, &scene->depthMapFBO ); + + // create depth texture + glGenTextures( 1, &scene->depthMap ); + glBindTexture( GL_TEXTURE_2D, scene->depthMap ); + glTexImage2D( GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT24, shadowWidth, shadowHeight, 0, GL_DEPTH_COMPONENT, GL_FLOAT, nullptr ); + // Linear filtering combined with comparison mode gives free hardware 2x2 PCF + // inside each tap when sampled via sampler2DShadow. + glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR ); + glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR ); + glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_BORDER ); + glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_BORDER ); + glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_COMPARE_REF_TO_TEXTURE ); + glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL ); + float borderColor[] = { 1.0f, 1.0f, 1.0f, 1.0f }; + glTexParameterfv( GL_TEXTURE_2D, GL_TEXTURE_BORDER_COLOR, borderColor ); + + // attach depth texture as FBO's depth buffer + glBindFramebuffer( GL_FRAMEBUFFER, scene->depthMapFBO ); + glFramebufferTexture2D( GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, scene->depthMap, 0 ); + glDrawBuffer( GL_NONE ); + glReadBuffer( GL_NONE ); + glBindFramebuffer( GL_FRAMEBUFFER, 0 ); + + scene->geoBuffer = CreateGeoBuffer( camera->m_bufferWidth, camera->m_bufferHeight ); + scene->ssaoBuffer = CreateSsaoBuffer( camera->m_bufferWidth, camera->m_bufferHeight ); + + glUseProgram( scene->ssaoShader ); + SetUniformInt( scene->ssaoShader, "gPosition", 0 ); + SetUniformInt( scene->ssaoShader, "gNormal", 1 ); + SetUniformInt( scene->ssaoShader, "texNoise", 2 ); + + glUseProgram( scene->ssaoBlurShader ); + SetUniformInt( scene->ssaoBlurShader, "ssaoInput", 0 ); + SetUniformInt( scene->ssaoBlurShader, "gPosition", 1 ); + + glUseProgram( scene->deferredShader ); + SetUniformInt( scene->deferredShader, "gPosition", 0 ); + SetUniformInt( scene->deferredShader, "gNormal", 1 ); + SetUniformInt( scene->deferredShader, "gAlbedoSpec", 2 ); + SetUniformInt( scene->deferredShader, "ssao", 3 ); + SetUniformInt( scene->deferredShader, "shadowMap", 4 ); + + glUseProgram( scene->depthCopyShader ); + SetUniformInt( scene->depthCopyShader, "depthTexture", 0 ); + + return scene; +} + +void DestroySharedMeshes( Scene* scene ) +{ + for ( std::unordered_map::iterator shared = scene->sharedHeightFields.begin(); + shared != scene->sharedHeightFields.end(); ++shared ) + { + DestroySharedShape( shared->second ); + } + + for ( std::unordered_map::iterator shared = scene->sharedHulls.begin(); + shared != scene->sharedHulls.end(); ++shared ) + { + DestroySharedShape( shared->second ); + } + + for ( std::unordered_map::iterator shared = scene->sharedMeshes.begin(); + shared != scene->sharedMeshes.end(); ++shared ) + { + DestroySharedShape( shared->second ); + } + + scene->sharedHeightFields.clear(); + scene->sharedHulls.clear(); + scene->sharedMeshes.clear(); + scene->heightFieldInstances.clear(); + scene->hullInstances.clear(); + scene->meshInstances.clear(); + scene->transparentHullInstances.clear(); +} + +void DestroyScene( Scene* scene ) +{ + DestroyDebugPoints( scene->pointDraw ); + DestroyDebugLines( scene->lineDraw ); + DestroyGeoBuffer( scene->geoBuffer ); + DestroySsaoBuffer( scene->ssaoBuffer ); + + DestroySharedShape( scene->sphere ); + DestroySharedShape( scene->openHemisphere ); + DestroySharedShape( scene->openCylinder ); + + DestroySharedMeshes( scene ); + + DestroyShader( scene->shadowShader ); + DestroyShader( scene->meshShader ); + DestroyShader( scene->meshShadowShader ); + DestroyShader( scene->meshEdgeShader ); + DestroyShader( scene->geometryShader ); + DestroyShader( scene->ssaoShader ); + DestroyShader( scene->ssaoBlurShader ); + DestroyShader( scene->deferredShader ); + DestroyShader( scene->meshForwardShader ); + DestroyShader( scene->forwardShader ); + DestroyShader( scene->depthCopyShader ); + + delete scene; +} + +uint32_t CreateInstanceHull( Scene* scene, const b3Hull* hull, Arena arena ) +{ + uint32_t hash = hull->hash; + + std::unordered_map::iterator result = scene->sharedHulls.find( hash ); + if ( result != scene->sharedHulls.end() ) + { + // Already created + return hash; + } + + int vertexCount = hull->vertexCount; + b3Vec3* vertices = (b3Vec3*)arena.Allocate( hull->vertexCount * sizeof( b3Vec3 ) ); + + const b3Vec3* hullPoints = b3GetHullPoints( hull ); + memcpy( vertices, hullPoints, vertexCount * sizeof( b3Vec3 ) ); + + int triangleCount = 0; + int faceCount = hull->faceCount; + const b3HullFace* hullFaces = b3GetHullFaces( hull ); + const b3HullHalfEdge* hullEdges = b3GetHullEdges( hull ); + + // Count triangles + for ( int i = 0; i < faceCount; ++i ) + { + const b3HullFace* face = hullFaces + i; + + const b3HullHalfEdge* edge1 = hullEdges + face->edge; + const b3HullHalfEdge* edge2 = hullEdges + edge1->next; + const b3HullHalfEdge* edge3 = hullEdges + edge2->next; + B3_ASSERT( edge1 != edge3 ); + + do + { + triangleCount += 1; + edge3 = hullEdges + edge3->next; + } + while ( edge1 != edge3 ); + } + + int* indices = (int*)arena.Allocate( 3 * triangleCount * sizeof( int ) ); + int triangleIndex = 0; + + for ( int i = 0; i < faceCount; ++i ) + { + const b3HullFace* face = hullFaces + i; + + const b3HullHalfEdge* edge1 = hullEdges + face->edge; + const b3HullHalfEdge* edge2 = hullEdges + edge1->next; + const b3HullHalfEdge* edge3 = hullEdges + edge2->next; + B3_ASSERT( edge1 != edge3 ); + + do + { + B3_ASSERT( triangleIndex < triangleCount ); + indices[3 * triangleIndex + 0] = edge1->origin; + indices[3 * triangleIndex + 1] = edge2->origin; + indices[3 * triangleIndex + 2] = edge3->origin; + triangleIndex += 1; + + edge2 = edge3; + edge3 = hullEdges + edge3->next; + } + while ( edge1 != edge3 ); + } + + B3_ASSERT( triangleIndex == triangleCount ); + + int edgeCount = hull->edgeCount / 2; + Edge* edges = (Edge*)arena.Allocate( edgeCount * sizeof( Edge ) ); + + for ( int i = 0; i < edgeCount; ++i ) + { + const b3HullHalfEdge* edge = hullEdges + 2 * i + 0; + const b3HullHalfEdge* twin = hullEdges + 2 * i + 1; + + edges[i].index1 = edge->origin; + edges[i].index2 = twin->origin; + edges[i].flags = 0; + } + + InstanceShape shape = CreateInstanceShape( vertices, nullptr, indices, triangleCount, edges, edgeCount, nullptr, arena ); + scene->sharedHulls[hash] = shape; + + // reserve a slot + scene->hullInstances[hash] = {}; + scene->transparentHullInstances[hash] = {}; + + return hash; +} + +static inline bool EdgeLess( Edge e1, Edge e2 ) +{ + if ( e1.index1 == e2.index1 ) + { + return e1.index2 < e2.index2; + } + + return e1.index1 < e2.index1; +} + +static bool EdgeEquals( Edge e1, Edge e2 ) +{ + return e1.index1 == e2.index1 && e1.index2 == e2.index2; +} + +uint32_t CreateInstanceMesh( Scene* scene, const b3MeshData* meshData, Arena arena ) +{ + uint32_t hash = meshData->hash; + + std::unordered_map::iterator result = scene->sharedMeshes.find( hash ); + if ( result != scene->sharedMeshes.end() ) + { + // Already created + return hash; + } + + int vertexCount = meshData->vertexCount; + const b3Vec3* meshVertices = b3GetMeshVertices( meshData ); + + int triangleCount = meshData->triangleCount; + std::vector indices; + indices.resize( 3 * triangleCount ); + + int edgeCount = 3 * triangleCount; + std::vector edges; + edges.resize( edgeCount ); + + std::vector materials; + materials.resize( triangleCount ); + + const b3MeshTriangle* meshTriangles = b3GetMeshTriangles( meshData ); + const uint8_t* materialIndices = b3GetMeshMaterialIndices( meshData ); + const uint8_t* meshFlags = b3GetMeshFlags( meshData ); + + for ( int i = 0; i < triangleCount; ++i ) + { + b3MeshTriangle triangle = meshTriangles[i]; + + int i1 = triangle.index1; + int i2 = triangle.index2; + int i3 = triangle.index3; + + assert( i1 < vertexCount && i2 < vertexCount && i3 < vertexCount ); + + indices[3 * i + 0] = i1; + indices[3 * i + 1] = i2; + indices[3 * i + 2] = i3; + + uint8_t triangleFlags = meshFlags[i]; + + int flag1 = ( triangleFlags & b3_concaveEdge1 ) != 0 ? 0 : 1; + int flag2 = ( triangleFlags & b3_concaveEdge2 ) != 0 ? 0 : 1; + int flag3 = ( triangleFlags & b3_concaveEdge3 ) != 0 ? 0 : 1; + + edges[3 * i + 0] = { i1, i2, flag1 }; + edges[3 * i + 1] = { i2, i3, flag2 }; + edges[3 * i + 2] = { i3, i1, flag3 }; + + if ( materialIndices != nullptr ) + { + materials[i] = materialIndices[i]; + } + else + { + materials[i] = 0; + } + } + + // Remove duplicate edges + std::sort( edges.data(), edges.data() + edges.size(), EdgeLess ); + auto last = std::ranges::unique( edges, EdgeEquals ).begin(); + edges.erase( last, edges.end() ); + edgeCount = (int)edges.size(); + + InstanceShape shape = CreateInstanceShape( meshVertices, nullptr, indices.data(), triangleCount, edges.data(), edgeCount, + materials.data(), arena ); + scene->sharedMeshes[hash] = shape; + + // reserve a slot + scene->meshInstances[hash] = {}; + + return hash; +} + +uint32_t CreateInstanceHeightField( Scene* scene, const b3HeightField* heightField, Arena arena ) +{ + uint32_t hash = heightField->hash; + + std::unordered_map::iterator result = scene->sharedHeightFields.find( hash ); + if ( result != scene->sharedHeightFields.end() ) + { + // Already created + return hash; + } + + int rowCount = heightField->rowCount; + int columnCount = heightField->columnCount; + uint16_t* compressedHeights = heightField->compressedHeights; + uint8_t* materialIndices = heightField->materialIndices; + uint8_t* flags = heightField->flags; + float minHeight = heightField->minHeight; + float heightScale = heightField->heightScale; + b3Vec3 scale = heightField->scale; + + int vertexCount = rowCount * columnCount; + int cellCount = ( rowCount - 1 ) * ( columnCount - 1 ); + + std::vector vertices; + vertices.resize( vertexCount ); + + int vertexIndex = 0; + for ( int row = 0; row < rowCount; ++row ) + { + for ( int column = 0; column < columnCount; ++column ) + { + int heightIndex = row * columnCount + column; + + float x = float( column ); + float y = minHeight + heightScale * compressedHeights[heightIndex]; + float z = float( row ); + + vertices[vertexIndex] = scale * b3Vec3{ x, y, z }; + vertexIndex += 1; + } + } + assert( vertexIndex == vertexCount ); + + int triangleCount = 2 * ( rowCount - 1 ) * ( columnCount - 1 ); + + std::vector materials; + materials.resize( triangleCount ); + + // int cellCount = triangleCount >> 1; + int materialIndex = 0; + for ( int i = 0; i < cellCount; ++i ) + { + uint8_t index = materialIndices[i]; + if ( index != B3_HEIGHT_FIELD_HOLE ) + { + materials[materialIndex + 0] = index; + materials[materialIndex + 1] = index; + materialIndex += 2; + } + } + + int indexCount = 3 * triangleCount; + std::vector indices; + indices.resize( indexCount ); + + // Edge along each column, row, and diagonal + int edgeCount = columnCount * ( rowCount - 1 ) + rowCount * ( columnCount - 1 ) + ( rowCount - 1 ) * ( columnCount - 1 ); + std::vector edges; + edges.resize( edgeCount ); + + int index = 0; + int edgeIndex = 0; + for ( int row = 0; row < rowCount - 1; ++row ) + { + for ( int column = 0; column < columnCount - 1; ++column ) + { + int cellIndex = row * ( columnCount - 1 ) + column; + int triangleIndex = 2 * cellIndex; + assert( triangleIndex < triangleCount ); + + assert( cellIndex < cellCount ); + if ( materialIndices[cellIndex] == B3_HEIGHT_FIELD_HOLE ) + { + continue; + } + + int index11 = row * columnCount + column; + int index12 = index11 + 1; + int index21 = ( row + 1 ) * columnCount + column; + int index22 = index21 + 1; + + indices[index + 0] = index11; + indices[index + 1] = index21; + indices[index + 2] = index12; + + indices[index + 3] = index22; + indices[index + 4] = index12; + indices[index + 5] = index21; + + index += 6; + + int flag = ( flags[triangleIndex] & b3_concaveEdge2 ) != 0 ? 0 : 1; + edges[edgeIndex] = { index12, index21, flag }; + edgeIndex += 1; + } + } + assert( index <= indexCount ); + indexCount = index; + + // Edge rows + for ( int row = 0; row < rowCount; ++row ) + { + for ( int column = 0; column < columnCount - 1; ++column ) + { + int index1 = row * columnCount + column; + int index2 = index1 + 1; + + int flag; + if ( row < rowCount - 1 ) + { + int cellIndex = row * ( columnCount - 1 ) + column; + int triangleIndex = 2 * cellIndex; + + assert( triangleIndex < triangleCount ); + flag = ( flags[triangleIndex] & b3_concaveEdge3 ) != 0 ? 0 : 1; + } + else + { + int cellIndex = ( row - 1 ) * ( columnCount - 1 ) + column; + int triangleIndex = 2 * cellIndex + 1; + assert( triangleIndex < triangleCount ); + flag = ( flags[triangleIndex] & b3_concaveEdge3 ) != 0 ? 0 : 1; + } + + edges[edgeIndex + 0] = { index1, index2, flag }; + edgeIndex += 1; + } + } + + // Edge columns + for ( int column = 0; column < columnCount; ++column ) + { + for ( int row = 0; row < rowCount - 1; ++row ) + { + int index1 = row * columnCount + column; + int index2 = ( row + 1 ) * columnCount + column; + + int flag; + if ( column < columnCount - 1 ) + { + int cellIndex = row * ( columnCount - 1 ) + column; + int triangleIndex = 2 * cellIndex; + assert( triangleIndex < triangleCount ); + flag = ( flags[triangleIndex] & b3_concaveEdge1 ) != 0 ? 0 : 1; + } + else + { + int cellIndex = row * ( columnCount - 1 ) + column - 1; + int triangleIndex = 2 * cellIndex + 1; + assert( triangleIndex < triangleCount ); + flag = ( flags[triangleIndex] & b3_concaveEdge1 ) != 0 ? 0 : 1; + } + + edges[edgeIndex + 0] = { index1, index2, flag }; + edgeIndex += 1; + } + } + assert( edgeIndex <= edgeCount ); + edgeCount = edgeIndex; + + triangleCount = indexCount / 3; + + assert( materialIndex == triangleCount ); + + InstanceShape shape = CreateInstanceShape( vertices.data(), nullptr, indices.data(), triangleCount, edges.data(), edgeCount, + materials.data(), arena ); + scene->sharedHeightFields[hash] = shape; + + // reserve a slot + scene->heightFieldInstances[hash] = {}; + + return hash; +} + +void ResizeScene( Scene* scene, const Camera* camera ) +{ + DestroyGeoBuffer( scene->geoBuffer ); + scene->geoBuffer = CreateGeoBuffer( camera->m_bufferWidth, camera->m_bufferHeight ); + + DestroySsaoBuffer( scene->ssaoBuffer ); + scene->ssaoBuffer = CreateSsaoBuffer( camera->m_bufferWidth, camera->m_bufferHeight ); + + CheckOpenGL(); +} + +#if 0 +UniqueMesh CreateUniqueMesh( const b3Vec3* vertexPositions, const b3Vec3* vertexNormals, int triangleCount, + const int* triangleIndices, const Edge* edges, int edgeCount, const int* materials, Arena arena ) +{ + UniqueMesh shape = {}; + + // Vertex buffers + { + // Create vertex buffers + int vertexCount = 3 * triangleCount; + VertexData* vertexData = (VertexData*)arena.Allocate( vertexCount * sizeof( VertexData ) ); + + int index = 0; + for ( int triangleIndex = 0; triangleIndex < triangleCount; ++triangleIndex ) + { + int vertexIndex1 = triangleIndices[3 * triangleIndex + 0]; + int vertexIndex2 = triangleIndices[3 * triangleIndex + 1]; + int vertexIndex3 = triangleIndices[3 * triangleIndex + 2]; + + b3Vec3 vertexPosition1 = vertexPositions[vertexIndex1]; + b3Vec3 vertexPosition2 = vertexPositions[vertexIndex2]; + b3Vec3 vertexPosition3 = vertexPositions[vertexIndex3]; + + vertexData[index + 0].position = vertexPosition1; + vertexData[index + 1].position = vertexPosition2; + vertexData[index + 2].position = vertexPosition3; + + b3Vec3 vertexNormal1, vertexNormal2, vertexNormal3; + if ( vertexNormals ) + { + vertexNormal1 = vertexNormals[vertexIndex1]; + vertexNormal2 = vertexNormals[vertexIndex2]; + vertexNormal3 = vertexNormals[vertexIndex3]; + } + else + { + b3Vec3 edge1 = vertexPosition2 - vertexPosition1; + b3Vec3 edge2 = vertexPosition3 - vertexPosition1; + b3Vec3 normal = b3Cross( edge1, edge2 ); + normal = b3Normalize( normal ); + + vertexNormal1 = normal; + vertexNormal2 = normal; + vertexNormal3 = normal; + } + + vertexData[index + 0].normal = vertexNormal1; + vertexData[index + 1].normal = vertexNormal2; + vertexData[index + 2].normal = vertexNormal3; + + if ( materials != nullptr ) + { + vertexData[index + 0].material = materials[triangleIndex]; + vertexData[index + 1].material = materials[triangleIndex]; + vertexData[index + 2].material = materials[triangleIndex]; + } + else + { + vertexData[index + 0].material = -1; + vertexData[index + 1].material = -1; + vertexData[index + 2].material = -1; + } + + index += 3; + } + + assert( index == vertexCount ); + + // Fill vertex buffers + shape.triangleCount = triangleCount; + + glGenVertexArrays( 1, &shape.vertexArrayId ); + glGenBuffers( 1, &shape.vertexBufferId ); + + // Copy static vertex data + glBindVertexArray( shape.vertexArrayId ); + glBindBuffer( GL_ARRAY_BUFFER, shape.vertexBufferId ); + glBufferData( GL_ARRAY_BUFFER, vertexCount * sizeof( VertexData ), vertexData, GL_STATIC_DRAW ); + + // positions + uint32_t positionAttribute = 0; + glEnableVertexAttribArray( positionAttribute ); + glVertexAttribPointer( positionAttribute, 3, GL_FLOAT, GL_FALSE, sizeof( VertexData ), + (void*)offsetof( VertexData, position ) ); + + // normals + uint32_t normalAttribute = 1; + glEnableVertexAttribArray( normalAttribute ); + glVertexAttribPointer( normalAttribute, 3, GL_FLOAT, GL_FALSE, sizeof( VertexData ), + (void*)offsetof( VertexData, normal ) ); + + // materials + uint32_t materialAttribute = 2; + glEnableVertexAttribArray( materialAttribute ); + glVertexAttribIPointer( materialAttribute, 1, GL_INT, sizeof( VertexData ), (void*)offsetof( VertexData, material ) ); + + glBindBuffer( GL_ARRAY_BUFFER, 0 ); + glBindVertexArray( 0 ); + + shape.shadowCaster = true; + } + + // Edge buffer + { + // Create edge buffer + EdgeData* edgeBuffer = (EdgeData*)arena.Allocate( 2 * edgeCount * sizeof( EdgeData ) ); + + for ( int edgeIndex = 0; edgeIndex < edgeCount; ++edgeIndex ) + { + int vertexIndex1 = edges[edgeIndex].index1; + int vertexIndex2 = edges[edgeIndex].index2; + int flags = edges[edgeIndex].flags; + + const b3Vec3* vb = vertexPositions; + b3Vec3 vertexPosition1 = vb[vertexIndex1]; + b3Vec3 vertexPosition2 = vb[vertexIndex2]; + + edgeBuffer[2 * edgeIndex + 0].position = vertexPosition1; + edgeBuffer[2 * edgeIndex + 0].flags = flags; + edgeBuffer[2 * edgeIndex + 1].position = vertexPosition2; + edgeBuffer[2 * edgeIndex + 1].flags = flags; + } + + // Fill edge buffer + shape.edgeCount = edgeCount; + + glGenVertexArrays( 1, &shape.edgeArrayId ); + glGenBuffers( 1, &shape.edgeBufferId ); + + glBindVertexArray( shape.edgeArrayId ); + glBindBuffer( GL_ARRAY_BUFFER, shape.edgeBufferId ); + glBufferData( GL_ARRAY_BUFFER, 2 * edgeCount * sizeof( EdgeData ), edgeBuffer, GL_STATIC_DRAW ); + + uint32_t positionIndex = 0; + glEnableVertexAttribArray( positionIndex ); + glVertexAttribPointer( positionIndex, 3, GL_FLOAT, GL_FALSE, sizeof( EdgeData ), (void*)offsetof( EdgeData, position ) ); + + uint32_t flagsIndex = 1; + glEnableVertexAttribArray( flagsIndex ); + glVertexAttribIPointer( flagsIndex, 1, GL_INT, sizeof( EdgeData ), (void*)offsetof( EdgeData, flags ) ); + + glBindBuffer( GL_ARRAY_BUFFER, 0 ); + glBindVertexArray( 0 ); + } + + CheckOpenGL(); + + return shape; +} + +void DestroyUniqueMesh( UniqueMesh* shape ) +{ + if ( shape->vertexBufferId != 0 ) + { + glDeleteBuffers( 1, &shape->vertexBufferId ); + } + + if ( shape->vertexArrayId != 0 ) + { + glDeleteVertexArrays( 1, &shape->vertexArrayId ); + } + + if ( shape->edgeBufferId != 0 ) + { + glDeleteBuffers( 1, &shape->edgeBufferId ); + } + + if ( shape->edgeArrayId != 0 ) + { + glDeleteVertexArrays( 1, &shape->edgeArrayId ); + } +} +#endif + +void DrawDebugCapsule( Scene* scene, DebugCapsule* capsule, b3Transform transform, b3HexColor color, float alpha ) +{ + float radius = capsule->radius; + b3Vec3 c1 = capsule->center1; + b3Vec3 c2 = capsule->center2; + b3Vec3 origin = 0.5f * ( c1 + c2 ); + b3Vec3 d = c2 - c1; + float length = b3Length( d ); + + assert( length > 0.005f ); + + RGBA8 rgba8 = MakeRGBA8( color, alpha ); + b3Vec3 axis = b3Normalize( d ); + + // Capsule mesh is along x-axis. Build from two open hemispheres and an open cylinder. + + // Left cap + b3Quat qc1 = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisX, -axis ); + + b3Transform xfCap1 = { + .p = b3TransformPoint( transform, c1 ), + .q = b3MulQuat( transform.q, qc1 ), + }; + + Matrix4 matrixCap1 = MakeMatrix4( xfCap1, radius ); + + // Right cap + b3Quat qc2 = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisX, axis ); + + b3Transform xfCap2 = { + .p = b3TransformPoint( transform, c2 ), + .q = b3MulQuat( transform.q, qc2 ), + }; + + Matrix4 matrixCap2 = MakeMatrix4( xfCap2, radius ); + + // Middle + b3Transform xfMiddle = { + .p = b3TransformPoint( transform, origin ), + .q = b3MulQuat( transform.q, qc2 ), + }; + + Matrix4 matrixMiddle = MakeMatrix4( xfMiddle, { length, radius, radius } ); + + if ( alpha < 1.0f ) + { + scene->transparentOpenHemisphereInstances.push_back( { + .rgba8 = rgba8, + .matrix = matrixCap1, + } ); + + scene->transparentOpenHemisphereInstances.push_back( { + .rgba8 = rgba8, + .matrix = matrixCap2, + } ); + + scene->transparentOpenCylinderInstances.push_back( { + .rgba8 = rgba8, + .matrix = matrixMiddle, + } ); + } + else + { + scene->openHemisphereInstances.push_back( { + .rgba8 = rgba8, + .matrix = matrixCap1, + } ); + + scene->openHemisphereInstances.push_back( { + .rgba8 = rgba8, + .matrix = matrixCap2, + } ); + + scene->openCylinderInstances.push_back( { + .rgba8 = rgba8, + .matrix = matrixMiddle, + } ); + } +} + +void DrawDebugHull( Scene* scene, DebugHull* hull, b3Transform transform, b3HexColor color, float alpha ) +{ + b3Transform worldTransform = b3MulTransforms( transform, hull->transform ); + Matrix4 matrix = MakeMatrix4( worldTransform, 1.0f ); + InstanceData instanceData = { + .rgba8 = MakeRGBA8( color, alpha ), + .matrix = matrix, + }; + + uint32_t id = hull->id; + if ( alpha < 1.0f ) + { + assert( scene->transparentHullInstances.contains( id ) ); + scene->transparentHullInstances[id].push_back( instanceData ); + } + else + { + assert( scene->hullInstances.contains( id ) ); + scene->hullInstances[id].push_back( instanceData ); + } +} + +void DrawDebugMesh( Scene* scene, DebugMesh* mesh, b3Transform transform, b3HexColor color, float alpha ) +{ + b3Transform worldTransform = b3MulTransforms( transform, mesh->transform ); + Matrix4 matrix = MakeMatrix4( worldTransform, mesh->scale ); + InstanceData instanceData = { + .rgba8 = MakeRGBA8( color, alpha ), + .matrix = matrix, + }; + + uint32_t id = mesh->id; + assert( scene->meshInstances.contains( id ) ); + scene->meshInstances[id].push_back( instanceData ); +} + +void DrawDebugSphere( Scene* scene, DebugSphere* sphere, b3Transform transform, b3HexColor color, float alpha ) +{ + b3Vec3 center = sphere->center; + b3Transform xf = { + .p = b3TransformPoint( transform, center ), + .q = transform.q, + }; + Matrix4 matrix = MakeMatrix4( xf, sphere->radius ); + InstanceData instanceData = { + .rgba8 = MakeRGBA8( color, alpha ), + .matrix = matrix, + }; + + if ( alpha < 1.0f ) + { + scene->transparentSphereInstances.push_back( instanceData ); + } + else + { + scene->sphereInstances.push_back( instanceData ); + } +} + +void DrawDebugShape( Scene* scene, DebugShape* debugShape, b3Transform transform, b3HexColor color, float alpha ) +{ + switch ( debugShape->type ) + { + case DebugShapeType::capsule: + DrawDebugCapsule( scene, &debugShape->capsule, transform, color, alpha ); + break; + + case DebugShapeType::compound: + { + DebugCompound* compound = &debugShape->compound; + for ( int i = 0; i < compound->capsuleCount; ++i ) + { + DrawDebugCapsule( scene, compound->capsules + i, transform, color, alpha ); + } + + for ( int i = 0; i < compound->hullCount; ++i ) + { + DrawDebugHull( scene, compound->hulls + i, transform, color, alpha ); + } + + for ( int i = 0; i < compound->meshCount; ++i ) + { + DrawDebugMesh( scene, compound->meshes + i, transform, color, alpha ); + } + + for ( int i = 0; i < compound->sphereCount; ++i ) + { + DrawDebugSphere( scene, compound->spheres + i, transform, color, alpha ); + } + } + break; + + case DebugShapeType::hull: + DrawDebugHull( scene, &debugShape->hull, transform, color, alpha ); + break; + + case DebugShapeType::heightField: + { + Matrix4 matrix = MakeMatrix4( transform, 1.0f ); + InstanceData instanceData = { + .rgba8 = MakeRGBA8( color, alpha ), + .matrix = matrix, + }; + + uint32_t id = debugShape->heightField.id; + assert( scene->heightFieldInstances.contains( id ) ); + scene->heightFieldInstances[id].push_back( instanceData ); + } + break; + + case DebugShapeType::mesh: + { + b3Transform xf = b3MulTransforms( transform, debugShape->hull.transform ); + Matrix4 matrix = MakeMatrix4( xf, debugShape->mesh.scale ); + InstanceData instanceData = { + .rgba8 = MakeRGBA8( color, alpha ), + .matrix = matrix, + }; + + uint32_t id = debugShape->mesh.id; + assert( scene->meshInstances.contains( id ) ); + scene->meshInstances[id].push_back( instanceData ); + } + break; + + case DebugShapeType::sphere: + DrawDebugSphere( scene, &debugShape->sphere, transform, color, alpha ); + break; + + default: + assert( false ); + break; + } +} + +static void DrawMeshInstances( InstanceShape shape, const InstanceData* instances, int count, bool cull ) +{ + if ( count == 0 ) + { + return; + } + + if ( cull == false ) + { + glDisable( GL_CULL_FACE ); + } + + glBindVertexArray( shape.triangleArrayId ); + glBindBuffer( GL_ARRAY_BUFFER, shape.triangleBufferIds[1] ); + + int base = 0; + while ( count > 0 ) + { + int batchCount = b3MinInt( count, s_batchSize ); + + glBufferData( GL_ARRAY_BUFFER, s_batchSize * sizeof( InstanceData ), nullptr, GL_DYNAMIC_DRAW ); + glBufferSubData( GL_ARRAY_BUFFER, 0, batchCount * sizeof( InstanceData ), &instances[base] ); + glDrawArraysInstanced( GL_TRIANGLES, 0, 3 * shape.triangleCount, batchCount ); + + // CheckOpenGL(); + + count -= s_batchSize; + base += s_batchSize; + } + + glBindBuffer( GL_ARRAY_BUFFER, 0 ); + glBindVertexArray( 0 ); + + if ( cull == false ) + { + glEnable( GL_CULL_FACE ); + } +} + +static void DrawEdgeInstances( InstanceShape shape, const InstanceData* instances, int count ) +{ + if ( count == 0 ) + { + return; + } + + glBindVertexArray( shape.edgeArrayId ); + glBindBuffer( GL_ARRAY_BUFFER, shape.edgeBufferIds[1] ); + + int base = 0; + while ( count > 0 ) + { + int batchCount = b3MinInt( count, s_batchSize ); + + glBufferData( GL_ARRAY_BUFFER, s_batchSize * sizeof( InstanceData ), nullptr, GL_DYNAMIC_DRAW ); + glBufferSubData( GL_ARRAY_BUFFER, 0, batchCount * sizeof( InstanceData ), &instances[base] ); + glDrawArraysInstanced( GL_LINES, 0, 2 * shape.edgeCount, batchCount ); + + // CheckOpenGL(); + + count -= s_batchSize; + base += s_batchSize; + } + + glBindBuffer( GL_ARRAY_BUFFER, 0 ); + glBindVertexArray( 0 ); +} + +static void DrawMeshes( Scene* scene, const Matrix4& view, const Matrix4& projection, bool forShadow ) +{ + if ( forShadow == true ) + { + // Receiver-plane depth bias in the deferred shader handles per-pixel bias + // precisely, so the shadow pass needs no polygon offset. Adding one back + // reintroduces "Peter Panning" — a visible gap between objects and their + // contact shadows. + glDisable( GL_POLYGON_OFFSET_FILL ); + + glUseProgram( scene->meshShadowShader ); + SetUniformMatrix4( scene->meshShadowShader, "lightSpaceMatrix", view ); + } + else + { + glEnable( GL_POLYGON_OFFSET_FILL ); + glPolygonOffset( 2.0f, 2.0f ); + + glUseProgram( scene->meshShader ); + SetUniformMatrix4( scene->meshShader, "view", view ); + SetUniformMatrix4( scene->meshShader, "projection", projection ); + } + + DrawMeshInstances( scene->sphere, scene->sphereInstances.data(), (int)scene->sphereInstances.size(), true ); + DrawMeshInstances( scene->openHemisphere, scene->openHemisphereInstances.data(), (int)scene->openHemisphereInstances.size(), + true ); + DrawMeshInstances( scene->openCylinder, scene->openCylinderInstances.data(), (int)scene->openCylinderInstances.size(), true ); + + for ( std::unordered_map>::iterator instances = scene->heightFieldInstances.begin(); + instances != scene->heightFieldInstances.end(); ++instances ) + { + uint32_t id = instances->first; + InstanceShape shape = scene->sharedHeightFields.at( id ); + DrawMeshInstances( shape, instances->second.data(), (int)instances->second.size(), true ); + } + + for ( std::unordered_map>::iterator instances = scene->hullInstances.begin(); + instances != scene->hullInstances.end(); ++instances ) + { + uint32_t id = instances->first; + InstanceShape shape = scene->sharedHulls.at( id ); + DrawMeshInstances( shape, instances->second.data(), (int)instances->second.size(), true ); + } + + for ( std::unordered_map>::iterator instances = scene->meshInstances.begin(); + instances != scene->meshInstances.end(); ++instances ) + { + uint32_t id = instances->first; + InstanceShape shape = scene->sharedMeshes.at( id ); + DrawMeshInstances( shape, instances->second.data(), (int)instances->second.size(), false ); + } + + glDisable( GL_POLYGON_OFFSET_FILL ); +} + +static void DrawTransparentShapes( Scene* scene, const Matrix4& view, const Matrix4& projection, b3Vec3 sunDirWorld, + b3Vec3 cameraPos ) +{ + glUseProgram( scene->meshForwardShader ); + SetUniformVec3( scene->meshForwardShader, "sunDirWorld", sunDirWorld ); + SetUniformVec3( scene->meshForwardShader, "cameraPos", cameraPos ); + + SetUniformMatrix4( scene->meshForwardShader, "view", view ); + SetUniformMatrix4( scene->meshForwardShader, "projection", projection ); + + glEnable( GL_POLYGON_OFFSET_FILL ); + glPolygonOffset( 2.0, 2.0 ); + + glCullFace( GL_BACK ); + glEnable( GL_CULL_FACE ); + glEnable( GL_BLEND ); + glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA ); + glDepthMask( GL_FALSE ); + + DrawMeshInstances( scene->sphere, scene->transparentSphereInstances.data(), (int)scene->transparentSphereInstances.size(), + true ); + DrawMeshInstances( scene->openHemisphere, scene->transparentOpenHemisphereInstances.data(), + (int)scene->transparentOpenHemisphereInstances.size(), true ); + DrawMeshInstances( scene->openCylinder, scene->transparentOpenCylinderInstances.data(), + (int)scene->transparentOpenCylinderInstances.size(), true ); + + for ( std::unordered_map>::iterator hulls = scene->transparentHullInstances.begin(); + hulls != scene->transparentHullInstances.end(); ++hulls ) + { + uint32_t id = hulls->first; + InstanceShape shape = scene->sharedHulls.at( id ); + DrawMeshInstances( shape, hulls->second.data(), (int)hulls->second.size(), true ); + } + + glDisable( GL_POLYGON_OFFSET_FILL ); + glDisable( GL_BLEND ); + glDepthMask( GL_TRUE ); +} + +#if 0 +static void DrawObjects( Scene* scene, uint32_t shader, bool isShadow ) +{ + CheckOpenGL(); + + // glFrontFace( GL_CCW ); + // glDepthFunc( GL_LESS ); + // glCullFace( GL_BACK ); + glEnable( GL_POLYGON_OFFSET_FILL ); + glPolygonOffset( 2.0, 2.0 ); + + for ( int i = 0; i < scene->uniqueShapes.size(); ++i ) + { + UniqueShape& shape = scene->uniqueShapes[i]; + if ( isShadow && shape.mesh->shadowCaster == false ) + { + continue; + } + + if ( shape.color.a < 1.0f ) + { + continue; + } + + SetUniformMatrix4( shader, "model", shape.matrix ); + SetUniformColor( shader, "objectColor", shape.color ); + SetUniformBool( shader, "invertedNormals", false ); + CheckOpenGL(); + + glBindVertexArray( shape.mesh->vertexArrayId ); + glDrawArrays( GL_TRIANGLES, 0, 3 * shape.mesh->triangleCount ); + glBindVertexArray( 0 ); + + CheckOpenGL(); + } + + glDisable( GL_POLYGON_OFFSET_FILL ); +} +#endif + +static void DrawLines( Scene* scene, const Matrix4& view, const Matrix4& projection ) +{ + // WARNING: very slow + // glEnable( GL_LINE_SMOOTH ); + + glEnable( GL_BLEND ); + glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA ); + + glUseProgram( scene->meshEdgeShader ); + SetUniformMatrix4( scene->meshEdgeShader, "view", view ); + SetUniformMatrix4( scene->meshEdgeShader, "projection", projection ); + + for ( std::unordered_map>::iterator instances = scene->heightFieldInstances.begin(); + instances != scene->heightFieldInstances.end(); ++instances ) + { + uint32_t id = instances->first; + InstanceShape shape = scene->sharedHeightFields.at( id ); + DrawEdgeInstances( shape, instances->second.data(), (int)instances->second.size() ); + } + + for ( std::unordered_map>::iterator instances = scene->hullInstances.begin(); + instances != scene->hullInstances.end(); ++instances ) + { + uint32_t id = instances->first; + InstanceShape shape = scene->sharedHulls.at( id ); + DrawEdgeInstances( shape, instances->second.data(), (int)instances->second.size() ); + } + + for ( std::unordered_map>::iterator instances = scene->meshInstances.begin(); + instances != scene->meshInstances.end(); ++instances ) + { + uint32_t id = instances->first; + InstanceShape shape = scene->sharedMeshes.at( id ); + DrawEdgeInstances( shape, instances->second.data(), (int)instances->second.size() ); + } + + glDisable( GL_BLEND ); +} + +static Matrix4 LookAt( b3Vec3 eye, b3Vec3 center, b3Vec3 up ) +{ + // Build view transform + b3Vec3 axisZ = b3Normalize( center - eye ); + b3Vec3 axisX = b3Normalize( b3Cross( axisZ, up ) ); + b3Vec3 axisY = b3Cross( axisX, axisZ ); + + // Negate + axisZ = -axisZ; + + Matrix4 out; + out.cx = { axisX.x, axisY.x, axisZ.x, 0.0f }; + out.cy = { axisX.y, axisY.y, axisZ.y, 0.0f }; + out.cz = { axisX.z, axisY.z, axisZ.z, 0.0f }; + out.cw = { -b3Dot( axisX, eye ), -b3Dot( axisY, eye ), -b3Dot( axisZ, eye ), 1.0f }; + + return out; +} + +void DrawScene( Scene* scene, Camera* camera ) +{ + // float temp1[2] = {}; + // glGetFloatv( GL_ALIASED_LINE_WIDTH_RANGE, temp1 ); + + // float temp2[2] = {}; + // glGetFloatv( GL_SMOOTH_LINE_WIDTH_RANGE, temp2 ); + + // CheckOpenGL(); + // glDisable( GL_LINE_SMOOTH ); + + // GLboolean test = glIsEnabled( GL_LINE_SMOOTH ); + // B3_UNUSED( test ); + + // todo wirefame + // glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + + glDepthFunc( GL_LESS ); + + // Get camera matrices + Matrix4 projection = camera->GetProjectionMatrix(); + Matrix4 view = camera->GetViewMatrix(); + Matrix4 cameraMatrix = camera->GetWorldMatrix(); + + // 1. render shadow map — fit the ortho frustum to the camera's view frustum + // each frame so all shadow-map texels go to what the camera can actually see. + Matrix4 lightView = LookAt( scene->lightPosition, { 0.0f, 0.0f, 0.0f }, { 0.0f, 1.0f, 0.0f } ); + + // Scale the shadow region with camera radius so large scenes still get + // covered. Trades off texel density at long camera distances — true large + // scenes want cascaded shadow maps, but a single fitted cascade plus an edge + // fade in the shader keeps the cutoff invisible at this scale. + // + // In third-person the radius is the orbit distance to the pivot, so + // 1.5 * radius is a tight cover of the actual scene of interest and a small + // floor (8) gives great texel density at close orbits. In first-person the + // radius is a stale value — the camera moves m_position directly while + // m_radius keeps whatever it was last set to. Fall back to a wider fixed + // floor there so the shadow frustum still covers the scene the camera can + // see, at the cost of some texel density. + float minShadowDistance = camera->m_thirdPerson ? 8.0f : 60.0f; + const float shadowDistance = b3ClampFloat( 1.5f * camera->m_radius, minShadowDistance, 500.0f ); + float shadowNear = camera->m_nearPlane; + float shadowFar = b3MinFloat( shadowDistance, camera->m_farPlane ); + + b3Vec3 lookDir = -camera->m_forward; // m_forward is the camera-z axis (away from view) + b3Vec3 nearC = camera->m_position + shadowNear * lookDir; + b3Vec3 farC = camera->m_position + shadowFar * lookDir; + float nearH = shadowNear * camera->m_tanY; + float nearW = camera->m_ratio * nearH; + float farH = shadowFar * camera->m_tanY; + float farW = camera->m_ratio * farH; + + b3Vec3 frustumCorners[8] = { + nearC + ( -nearW ) * camera->m_right + ( -nearH ) * camera->m_up, + nearC + (nearW)*camera->m_right + ( -nearH ) * camera->m_up, + nearC + (nearW)*camera->m_right + (nearH)*camera->m_up, + nearC + ( -nearW ) * camera->m_right + (nearH)*camera->m_up, + farC + ( -farW ) * camera->m_right + ( -farH ) * camera->m_up, + farC + (farW)*camera->m_right + ( -farH ) * camera->m_up, + farC + (farW)*camera->m_right + (farH)*camera->m_up, + farC + ( -farW ) * camera->m_right + (farH)*camera->m_up, + }; + + Vector4 c0 = MulMV( lightView, { frustumCorners[0].x, frustumCorners[0].y, frustumCorners[0].z, 1.0f } ); + float minX = c0.x, maxX = c0.x; + float minY = c0.y, maxY = c0.y; + float minZ = c0.z, maxZ = c0.z; + for ( int i = 1; i < 8; ++i ) + { + Vector4 c = MulMV( lightView, { frustumCorners[i].x, frustumCorners[i].y, frustumCorners[i].z, 1.0f } ); + minX = b3MinFloat( minX, c.x ); + maxX = b3MaxFloat( maxX, c.x ); + minY = b3MinFloat( minY, c.y ); + maxY = b3MaxFloat( maxY, c.y ); + minZ = b3MinFloat( minZ, c.z ); + maxZ = b3MaxFloat( maxZ, c.z ); + } + + // Texel-snap the fitted ortho origin so the shadow texel grid stays + // world-locked frame-to-frame. Without this, sub-texel drift as the camera + // moves creates a wandering pattern on lit surfaces that reads as banding. + // Span is held constant within a frame, so each shadow texel maps to exactly + // (texelX, texelY) world-space units perpendicular to the light direction. + float texelX = ( maxX - minX ) / float( shadowWidth ); + float texelY = ( maxY - minY ) / float( shadowHeight ); + minX = floorf( minX / texelX ) * texelX; + minY = floorf( minY / texelY ) * texelY; + maxX = minX + texelX * float( shadowWidth ); + maxY = minY + texelY * float( shadowHeight ); + + // Pull the near plane back so off-camera geometry between the light and the + // view frustum still casts shadows into the visible region. + const float zPad = 50.0f; + float lightNearPlane = -maxZ - zPad; + float lightFarPlane = -minZ; + + Matrix4 lightProjection = MakeOrthographicMatrix( minX, maxX, minY, maxY, lightNearPlane, lightFarPlane ); + Matrix4 lightSpaceMatrix = MulMM( lightProjection, lightView ); + + // World-space size of one shadow texel (light view is rigid, so light-space + // distances equal world-space distances). The deferred shader uses this for + // normal-offset bias; max() keeps the offset conservative when texels are + // non-square. + float worldTexelSize = b3MaxFloat( texelX, texelY ); + + // render scene from light's point of view + glUseProgram( scene->shadowShader ); + SetUniformMatrix4( scene->shadowShader, "lightSpaceMatrix", lightSpaceMatrix ); + CheckOpenGL(); + + glViewport( 0, 0, shadowWidth, shadowHeight ); + glBindFramebuffer( GL_FRAMEBUFFER, scene->depthMapFBO ); + glClear( GL_DEPTH_BUFFER_BIT ); + glActiveTexture( GL_TEXTURE0 ); + + // Back-face cull during the shadow pass: combined with slope-scaled polygon + // offset (applied in DrawMeshes) this eliminates self-shadow acne on solid + // hulls without the gap that front-face culling introduces. + glCullFace( GL_BACK ); + glEnable( GL_CULL_FACE ); + + CheckOpenGL(); + + // DrawObjects( scene, scene->shadowShader, true ); + DrawMeshes( scene, lightSpaceMatrix, Matrix4{}, true ); + + glBindFramebuffer( GL_FRAMEBUFFER, 0 ); + + // reset viewport + glViewport( 0, 0, camera->m_bufferWidth, camera->m_bufferHeight ); + glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); + + // 2. geometry pass: render scene's geometry/color data into geoBuffer + BindGeoBuffer( scene->geoBuffer ); + glUseProgram( scene->geometryShader ); + SetUniformMatrix4( scene->geometryShader, "view", view ); + SetUniformMatrix4( scene->geometryShader, "projection", projection ); + + glCullFace( GL_BACK ); + glEnable( GL_CULL_FACE ); + + // DrawObjects( scene, scene->geometryShader, false ); + DrawMeshes( scene, view, projection, false ); + + glBindFramebuffer( GL_FRAMEBUFFER, 0 ); + + // 3. generate SSAO texture (full resolution — sharp silhouettes) + BindSsaoBuffer( scene->ssaoBuffer ); + glUseProgram( scene->ssaoShader ); + SetUniformMatrix4( scene->ssaoShader, "projection", projection ); + SetUniformFloat( scene->ssaoShader, "radius", scene->ao.radius ); + SetUniformFloat( scene->ssaoShader, "bias", scene->ao.bias ); + SetUniformFloat( scene->ssaoShader, "minScale", scene->ao.minScale ); + { + int loc = glGetUniformLocation( scene->ssaoShader, "noiseScale" ); + glUniform2f( loc, float( camera->m_bufferWidth ) / 4.0f, float( camera->m_bufferHeight ) / 4.0f ); + } + SetSsaoSamples( scene->ssaoBuffer, scene->ssaoShader ); + BindGeoTextures( scene->geoBuffer ); + BindSsaoTextures( scene->ssaoBuffer ); + DrawQuad(); + glBindFramebuffer( GL_FRAMEBUFFER, 0 ); + + CheckOpenGL(); + + // 4. bilateral blur of SSAO — depth-weighted so the blur doesn't leak across silhouettes. + BindSsaoBlurBuffer( scene->ssaoBuffer ); + glUseProgram( scene->ssaoBlurShader ); + BindSsaoColorTexture( scene->ssaoBuffer ); + BindGeoPosition( scene->geoBuffer, 1 ); + DrawQuad(); + glBindFramebuffer( GL_FRAMEBUFFER, 0 ); + + CheckOpenGL(); + + // 5. lighting pass: calculate lighting by iterating over a screen filled quad pixel-by-pixel using the geoBuffer's content. + glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); + glUseProgram( scene->deferredShader ); + BindGeoTextures( scene->geoBuffer ); + BindSsaoBlurTexture( scene->ssaoBuffer ); + + glActiveTexture( GL_TEXTURE4 ); + glBindTexture( GL_TEXTURE_2D, scene->depthMap ); + + SetUniformBool( scene->deferredShader, "useSSAO", scene->useSSAO ); + SetUniformBool( scene->deferredShader, "useShadow", scene->useShadow ); + SetUniformBool( scene->deferredShader, "useGrid", scene->useGrid ); + SetUniformFloat( scene->deferredShader, "aoPower", scene->ao.power ); + SetUniformFloat( scene->deferredShader, "aoDirect", scene->ao.direct ); + SetUniformBool( scene->deferredShader, "aoOnly", scene->ao.aoOnly ); + + // Sun direction in world space (from scene toward sun) — shared with the shadow projection, + // which already uses scene->lightPosition as the light source. + b3Vec3 sunDirWorld = b3Normalize( scene->lightPosition ); + SetUniformVec3( scene->deferredShader, "sunDirWorld", sunDirWorld ); + + // View-ray reconstruction parameters for the sky/fog path. + SetUniformFloat( scene->deferredShader, "tanHalfFovY", camera->m_tanY ); + SetUniformFloat( scene->deferredShader, "aspectRatio", camera->m_ratio ); + + SetUniformMatrix4( scene->deferredShader, "lightSpaceMatrix", lightSpaceMatrix ); + SetUniformMatrix4( scene->deferredShader, "cameraMatrix", cameraMatrix ); + SetUniformFloat( scene->deferredShader, "worldTexelSize", worldTexelSize ); + SetUniformFloat( scene->deferredShader, "lightZRange", lightFarPlane - lightNearPlane ); + + // finally render quad + DrawQuad(); + + CheckOpenGL(); + + // 5.5. Copy geometry depth to the default framebuffer using a depth-only draw pass. + // This avoids glBlitFramebuffer which is very slow on Apple Silicon. + glUseProgram( scene->depthCopyShader ); + BindGeoDepthTexture( scene->geoBuffer, 0 ); + glColorMask( GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE ); + glDepthFunc( GL_ALWAYS ); + glDepthMask( GL_TRUE ); + DrawQuad(); + glColorMask( GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE ); + glDepthFunc( GL_LESS ); + + CheckOpenGL(); + + // 6. forward render lights on top of scene + // glUseProgram( scene->forwardShader ); + // SetUniformMatrix4( scene->forwardShader, "projection", projection ); + // SetUniformMatrix4( scene->forwardShader, "view", view ); + + // b3Transform transform; + // transform.p = scene->lightPosition; + // transform.q = b3Quat_identity; + // Matrix4 m = MakeMatrix4( transform, 0.25f ); + // SetUniformMatrix4( scene->forwardShader, "model", m ); + // SetUniformVec3( scene->forwardShader, "color", { 1.0f, 1.0f, 1.0f } ); + // DrawCube( scene, transform, b3_colorWhite, view, projection ); + + // Draw mesh edges + DrawLines( scene, view, projection ); + + // Draw transparent hulls + DrawTransparentShapes( scene, view, projection, b3Normalize( scene->lightPosition ), camera->GetPosition() ); + + // 7. Debug draw lines and points + FlushLines( scene->lineDraw, camera ); + FlushPoints( scene->pointDraw, camera ); + + // 8. Clear instances + // scene->uniqueShapes.clear(); + scene->sphereInstances.clear(); + scene->openHemisphereInstances.clear(); + scene->openCylinderInstances.clear(); + + // Clear instances + for ( std::unordered_map>::iterator instance = scene->heightFieldInstances.begin(); + instance != scene->heightFieldInstances.end(); ++instance ) + { + instance->second.clear(); + } + + for ( std::unordered_map>::iterator instance = scene->hullInstances.begin(); + instance != scene->hullInstances.end(); ++instance ) + { + instance->second.clear(); + } + + for ( std::unordered_map>::iterator instance = scene->meshInstances.begin(); + instance != scene->meshInstances.end(); ++instance ) + { + instance->second.clear(); + } + + scene->transparentSphereInstances.clear(); + scene->transparentOpenHemisphereInstances.clear(); + scene->transparentOpenCylinderInstances.clear(); + + for ( std::unordered_map>::iterator hulls = scene->transparentHullInstances.begin(); + hulls != scene->transparentHullInstances.end(); ++hulls ) + { + hulls->second.clear(); + } +} + +void DrawDebugCube( Scene* scene, b3Transform transform, RGBAf color ) +{ + int index = (int)scene->debugCubes.size(); + scene->debugCubes[index] = { transform, color }; +} + +void EnableShadows( Scene* scene, bool useShadows ) +{ + scene->useShadow = useShadows; +} + +bool AreShadowsEnabled( Scene* scene ) +{ + return scene->useShadow; +} + +void EnableSSAO( Scene* scene, bool enable ) +{ + scene->useSSAO = enable; +} + +bool IsSSAOEnabled( Scene* scene ) +{ + return scene->useSSAO; +} + +void EnableGrid( Scene* scene, bool enable ) +{ + scene->useGrid = enable; +} + +bool IsGridEnabled( Scene* scene ) +{ + return scene->useGrid; +} + +SceneAOSettings* GetAOSettings( Scene* scene ) +{ + return &scene->ao; +} + +void DrawPoint( Scene* scene, b3Vec3 point, float size, b3HexColor color ) +{ + AddPoint( scene->pointDraw, point, size, color ); +} + +void DrawLine( Scene* scene, b3Vec3 point1, b3Vec3 point2, b3HexColor color ) +{ + AddLine( scene->lineDraw, point1, point2, color ); +} + +void DrawPlane( Scene* scene, b3Vec3 normal, b3Vec3 point, b3HexColor color ) +{ + b3Vec3 perp1 = b3Perp( normal ); + b3Vec3 perp2 = b3Cross( perp1, normal ); + b3Vec3 p = point; + float a = 1.0f; + b3Vec3 p1 = p + a * perp1 + a * perp2; + b3Vec3 p2 = p - a * perp1 + a * perp2; + b3Vec3 p3 = p - a * perp1 - a * perp2; + b3Vec3 p4 = p + a * perp1 - a * perp2; + + AddLine( scene->lineDraw, p1, p2, color ); + AddLine( scene->lineDraw, p2, p3, color ); + AddLine( scene->lineDraw, p3, p4, color ); + AddLine( scene->lineDraw, p4, p1, color ); + AddLine( scene->lineDraw, p, p + 0.5f * normal, color ); + AddPoint( scene->pointDraw, p, 10.0f, color ); +} + +void DrawArrow( Scene* scene, b3Vec3 point1, b3Vec3 point2, float size, b3HexColor color ) +{ + AddLine( scene->lineDraw, point1, point2, color ); + b3Vec3 axis = b3Normalize( point2 - point1 ); + b3Vec3 perp = b3Perp( axis ); + AddLine( scene->lineDraw, point2, point2 - size * axis + size * perp, color ); + AddLine( scene->lineDraw, point2, point2 - size * axis - size * perp, color ); +} + +void DrawBounds( Scene* scene, b3AABB bounds, float extension, b3HexColor color ) +{ + LineDraw* draw = scene->lineDraw; + + b3Vec3 extents = b3AABB_Extents( bounds ); + b3Vec3 center = b3AABB_Center( bounds ); + float hx = extents.x + extension; + float hy = extents.y + extension; + float hz = extents.z + extension; + + b3Vec3 p1 = b3Vec3{ -hx, -hy, hz } + center; + b3Vec3 p2 = b3Vec3{ -hx, hy, hz } + center; + b3Vec3 p3 = b3Vec3{ -hx, hy, -hz } + center; + b3Vec3 p4 = b3Vec3{ -hx, -hy, -hz } + center; + + b3Vec3 p5 = b3Vec3{ hx, -hy, hz } + center; + b3Vec3 p6 = b3Vec3{ hx, hy, hz } + center; + b3Vec3 p7 = b3Vec3{ hx, hy, -hz } + center; + b3Vec3 p8 = b3Vec3{ hx, -hy, -hz } + center; + + // negative x loop + AddLine( draw, p1, p2, color ); + AddLine( draw, p2, p3, color ); + AddLine( draw, p3, p4, color ); + AddLine( draw, p4, p1, color ); + + // positive x loop + AddLine( draw, p5, p6, color ); + AddLine( draw, p6, p7, color ); + AddLine( draw, p7, p8, color ); + AddLine( draw, p8, p5, color ); + + // connect loops + AddLine( draw, p1, p5, color ); + AddLine( draw, p2, p6, color ); + AddLine( draw, p3, p7, color ); + AddLine( draw, p4, p8, color ); +} + +void DrawBox( Scene* scene, b3Vec3 extents, b3Transform transform, b3HexColor color ) +{ + LineDraw* draw = scene->lineDraw; + + float hx = extents.x; + float hy = extents.y; + float hz = extents.z; + + b3Vec3 p1 = b3TransformPoint( transform, { -hx, -hy, hz } ); + b3Vec3 p2 = b3TransformPoint( transform, { -hx, hy, hz } ); + b3Vec3 p3 = b3TransformPoint( transform, { -hx, hy, -hz } ); + b3Vec3 p4 = b3TransformPoint( transform, { -hx, -hy, -hz } ); + + b3Vec3 p5 = b3TransformPoint( transform, { hx, -hy, hz } ); + b3Vec3 p6 = b3TransformPoint( transform, { hx, hy, hz } ); + b3Vec3 p7 = b3TransformPoint( transform, { hx, hy, -hz } ); + b3Vec3 p8 = b3TransformPoint( transform, { hx, -hy, -hz } ); + + // negative x loop + AddLine( draw, p1, p2, color ); + AddLine( draw, p2, p3, color ); + AddLine( draw, p3, p4, color ); + AddLine( draw, p4, p1, color ); + + // positive x loop + AddLine( draw, p5, p6, color ); + AddLine( draw, p6, p7, color ); + AddLine( draw, p7, p8, color ); + AddLine( draw, p8, p5, color ); + + // connect loops + AddLine( draw, p1, p5, color ); + AddLine( draw, p2, p6, color ); + AddLine( draw, p3, p7, color ); + AddLine( draw, p4, p8, color ); +} + +void DrawFace( Scene* scene, b3Vec3 vertex1, b3Vec3 vertex2, b3Vec3 vertex3, b3HexColor color ) +{ + LineDraw* draw = scene->lineDraw; + + AddLine( draw, vertex1, vertex2, color ); + AddLine( draw, vertex2, vertex3, color ); + AddLine( draw, vertex3, vertex1, color ); +} diff --git a/samples/scene.h b/samples/scene.h new file mode 100644 index 0000000..837bade --- /dev/null +++ b/samples/scene.h @@ -0,0 +1,147 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "utility.h" + +#include "box3d/math_functions.h" + +struct Camera; +struct b3DebugShape; +struct Scene; +struct UniqueMesh; + +enum struct DebugShapeType +{ + capsule, + compound, + hull, + heightField, + mesh, + sphere, +}; + +struct DebugSphere +{ + b3Vec3 center; + float radius; +}; + +struct DebugCapsule +{ + b3Vec3 center1, center2; + float radius; +}; + +struct DebugHeightField +{ + uint32_t id; +}; + +struct DebugHull +{ + b3Transform transform; + uint32_t id; +}; + +struct DebugMesh +{ + b3Transform transform; + b3Vec3 scale; + uint32_t id; +}; + +struct DebugCompound +{ + DebugCapsule* capsules; + int capsuleCount; + + DebugHull* hulls; + int hullCount; + + DebugMesh* meshes; + int meshCount; + + DebugSphere* spheres; + int sphereCount; +}; + +struct DebugShape +{ + DebugShapeType type; + b3BodyType bodyType; + union + { + DebugCapsule capsule; + DebugCompound compound; + DebugHeightField heightField; + DebugHull hull; + DebugMesh mesh; + DebugSphere sphere; + }; +}; + +Scene* CreateScene( const Camera* camera, Arena arena ); +void DestroyScene( Scene* scene ); + +// Register a hull and get an id to render it later +uint32_t CreateInstanceHull( Scene* scene, const b3Hull* hull, Arena arena ); + +// Register a mesh and get an id to render it later +uint32_t CreateInstanceMesh( Scene* scene, const b3MeshData* meshData, Arena arena ); + +// Register a height field and get an id to render it later +uint32_t CreateInstanceHeightField( Scene* scene, const b3HeightField* heightField, Arena arena ); + +// This should be called when a sample is destroyed to avoid leaking instances +void DestroySharedMeshes( Scene* scene ); + +void ResizeScene( Scene* scene, const Camera* camera ); + +// UniqueMesh CreateUniqueMesh( const b3Vec3* vertexPositions, const b3Vec3* vertexNormals, int triangleCount, +// const int* triangleIndices, const Edge* edges, int edgeCount, const int* materials, +// Arena arena ); +// void DestroyUniqueMesh( UniqueMesh* shape ); + +void DrawDebugShape( Scene* scene, DebugShape* debugShape, b3Transform transform, b3HexColor color, float alpha ); + +void DrawScene( Scene* scene, Camera* camera ); + +void DrawCube( Scene* scene, b3Transform transform, b3HexColor color, const Matrix4& view, const Matrix4& projection ); + +void DrawDebugCube( Scene* scene, b3Transform transform, b3HexColor color ); + +struct SceneAOSettings +{ + float radius; + float bias; + float minScale; + float power; + float direct; + bool aoOnly; +}; + +// Mutable view of the scene's SSAO parameters. Pointer is valid for the lifetime of the scene. +SceneAOSettings* GetAOSettings( Scene* scene ); + +void EnableShadows( Scene* scene, bool useShadows ); +bool AreShadowsEnabled( Scene* scene ); + +void EnableSSAO( Scene* scene, bool enable ); +bool IsSSAOEnabled( Scene* scene ); + +// World-space ground grid drawn on near-horizontal upward faces. Useful on +// flat-box ground; should be disabled for samples whose ground is a mesh +// (the mesh's triangle wireframe already provides the grid signal). +void EnableGrid( Scene* scene, bool enable ); +bool IsGridEnabled( Scene* scene ); + +void DrawPoint( Scene* scene, b3Vec3 point, float size, b3HexColor color ); +void DrawLine( Scene* scene, b3Vec3 point1, b3Vec3 point2, b3HexColor color ); +void DrawPlane( Scene* scene, b3Vec3 normal, b3Vec3 point, b3HexColor color ); +void DrawArrow( Scene* scene, b3Vec3 point1, b3Vec3 point2, float size, b3HexColor color ); + +void DrawBounds( Scene* scene, b3AABB bounds, float extension, b3HexColor color ); +void DrawBox( Scene* scene, b3Vec3 extents, b3Transform transform, b3HexColor color ); +void DrawFace( Scene* scene, b3Vec3 vertex1, b3Vec3 vertex2, b3Vec3 vertex3, b3HexColor color ); diff --git a/samples/shader.cpp b/samples/shader.cpp new file mode 100644 index 0000000..c904fd1 --- /dev/null +++ b/samples/shader.cpp @@ -0,0 +1,465 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) + #define _CRT_SECURE_NO_WARNINGS +#endif + +#include "shader.h" + +#include +#include +#include +#include +#include +#include + +#include + +void CheckOpenGL() +{ + GLenum errCode = glGetError(); + if ( errCode != GL_NO_ERROR ) + { + fprintf( stderr, "OpenGL error = %d\n", errCode ); + assert( false ); + } +} + +void DumpGLInfo( bool dumpExtensions ) +{ + const GLubyte* renderer = glGetString( GL_RENDERER ); + const GLubyte* vendor = glGetString( GL_VENDOR ); + const GLubyte* version = glGetString( GL_VERSION ); + const GLubyte* glslVersion = glGetString( GL_SHADING_LANGUAGE_VERSION ); + + int major, minor; + glGetIntegerv( GL_MAJOR_VERSION, &major ); + glGetIntegerv( GL_MINOR_VERSION, &minor ); + + printf( "-------------------------------------------------------------\n" ); + printf( "GL Vendor : %s\n", vendor ); + printf( "GL Renderer : %s\n", renderer ); + printf( "GL Version : %s\n", version ); + printf( "GL Version : %d.%d\n", major, minor ); + printf( "GLSL Version : %s\n", glslVersion ); + printf( "-------------------------------------------------------------\n" ); + + if ( dumpExtensions ) + { + int nExtensions; + glGetIntegerv( GL_NUM_EXTENSIONS, &nExtensions ); + for ( int i = 0; i < nExtensions; i++ ) + { + printf( "%s\n", glGetStringi( GL_EXTENSIONS, i ) ); + } + } +} + +static void PrintLog( uint32_t object ) +{ + int length = 0; + if ( glIsShader( object ) ) + { + glGetShaderiv( object, GL_INFO_LOG_LENGTH, &length ); + } + else if ( glIsProgram( object ) ) + { + glGetProgramiv( object, GL_INFO_LOG_LENGTH, &length ); + } + else + { + fprintf( stderr, "printlog: Not a shader or a program\n" ); + return; + } + + char* log = static_cast( malloc( length * sizeof( char ) ) ); + + if ( glIsShader( object ) ) + { + glGetShaderInfoLog( object, length, nullptr, log ); + } + else if ( glIsProgram( object ) ) + { + glGetProgramInfoLog( object, length, nullptr, log ); + } + + fprintf( stderr, "%s", log ); + free( log ); +} + +static uint32_t CreateShaderFromString( const char* source, GLenum type ) +{ + uint32_t shader = glCreateShader( type ); + const char* sources[] = { source }; + + glShaderSource( shader, 1, sources, nullptr ); + glCompileShader( shader ); + + int success = GL_FALSE; + glGetShaderiv( shader, GL_COMPILE_STATUS, &success ); + + if ( success == GL_FALSE ) + { + fprintf( stderr, "Error compiling shader of type %d!\n", type ); + PrintLog( shader ); + } + + return shader; +} + +static bool ReadFile( const char* filename, std::string& out ) +{ + FILE* file = fopen( filename, "rb" ); + if ( file == nullptr ) + { + return false; + } + + fseek( file, 0, SEEK_END ); + long size = ftell( file ); + fseek( file, 0, SEEK_SET ); + + out.resize( (size_t)size ); + if ( size > 0 ) + { + fread( &out[0], 1, (size_t)size, file ); + } + fclose( file ); + return true; +} + +static std::string DirectoryOf( const char* path ) +{ + const char* lastSep = nullptr; + for ( const char* p = path; *p; ++p ) + { + if ( *p == '/' || *p == '\\' ) + { + lastSep = p; + } + } + return lastSep ? std::string( path, lastSep + 1 ) : std::string(); +} + +// Substitutes lines of the form `#include "name"` with the contents of that +// file (resolved relative to `baseDir`). Recursive but not cyclic-safe — keep +// the include graph a DAG. Emits `#line` directives so compile errors keep +// readable line numbers in both the parent and the included file. +static bool ResolveIncludes( const std::string& source, const std::string& baseDir, std::string& out ) +{ + out.reserve( out.size() + source.size() * 2 ); + + int lineNo = 1; + size_t i = 0; + while ( i < source.size() ) + { + size_t lineStart = i; + size_t lineEnd = source.find( '\n', i ); + if ( lineEnd == std::string::npos ) + { + lineEnd = source.size(); + } + + size_t t = lineStart; + while ( t < lineEnd && ( source[t] == ' ' || source[t] == '\t' ) ) + { + t++; + } + + bool isInclude = false; + std::string includeName; + if ( lineEnd >= t + 9 && source.compare( t, 8, "#include" ) == 0 && + ( source[t + 8] == ' ' || source[t + 8] == '\t' ) ) + { + size_t q1 = source.find( '"', t + 8 ); + size_t q2 = ( q1 != std::string::npos && q1 < lineEnd ) ? source.find( '"', q1 + 1 ) : std::string::npos; + if ( q1 != std::string::npos && q2 != std::string::npos && q2 < lineEnd ) + { + isInclude = true; + includeName = source.substr( q1 + 1, q2 - q1 - 1 ); + } + } + + if ( isInclude ) + { + std::string includePath = baseDir + includeName; + std::string includedRaw; + if ( !ReadFile( includePath.c_str(), includedRaw ) ) + { + fprintf( stderr, "Cannot open include '%s'\n", includePath.c_str() ); + return false; + } + + char hdr[64]; + snprintf( hdr, sizeof( hdr ), "#line 1\n" ); + out += hdr; + + if ( !ResolveIncludes( includedRaw, DirectoryOf( includePath.c_str() ), out ) ) + { + return false; + } + + if ( !out.empty() && out.back() != '\n' ) + { + out += '\n'; + } + + snprintf( hdr, sizeof( hdr ), "#line %d\n", lineNo + 1 ); + out += hdr; + } + else + { + out.append( source, lineStart, lineEnd - lineStart ); + out += '\n'; + } + + i = ( lineEnd < source.size() ) ? lineEnd + 1 : lineEnd; + lineNo++; + } + return true; +} + +static uint32_t CreateShaderFromFile( const char* filename, GLenum type ) +{ + std::string raw; + if ( !ReadFile( filename, raw ) ) + { + fprintf( stderr, "Error opening %s\n", filename ); + return 0; + } + + std::string source; + if ( !ResolveIncludes( raw, DirectoryOf( filename ), source ) ) + { + fprintf( stderr, "Error resolving includes in %s\n", filename ); + return 0; + } + + uint32_t shader = glCreateShader( type ); + const char* sources[] = { source.c_str() }; + + glShaderSource( shader, 1, sources, nullptr ); + glCompileShader( shader ); + + int success = GL_FALSE; + glGetShaderiv( shader, GL_COMPILE_STATUS, &success ); + + if ( success == GL_FALSE ) + { + fprintf( stderr, "Error compiling shader of type %d in %s!\n", type, filename ); + PrintLog( shader ); + } + + return shader; +} + +uint32_t CreateProgramFromFiles( const char* vertexPath, const char* fragmentPath ) +{ + uint32_t vertex = CreateShaderFromFile( vertexPath, GL_VERTEX_SHADER ); + if ( vertex == 0 ) + { + return 0; + } + + uint32_t fragment = CreateShaderFromFile( fragmentPath, GL_FRAGMENT_SHADER ); + if ( fragment == 0 ) + { + return 0; + } + + uint32_t program = glCreateProgram(); + glAttachShader( program, vertex ); + glAttachShader( program, fragment ); + + glLinkProgram( program ); + + int success = GL_FALSE; + glGetProgramiv( program, GL_LINK_STATUS, &success ); + if ( success == GL_FALSE ) + { + fprintf( stderr, "glLinkProgram:" ); + PrintLog( program ); + return 0; + } + + glDeleteShader( vertex ); + glDeleteShader( fragment ); + + return program; +} + +uint32_t CreateProgramFromStrings( const char* vs, const char* fs ) +{ + uint32_t vertex = CreateShaderFromString( vs, GL_VERTEX_SHADER ); + if ( vertex == 0 ) + { + return 0; + } + + uint32_t fragment = CreateShaderFromString( fs, GL_FRAGMENT_SHADER ); + if ( fragment == 0 ) + { + return 0; + } + + uint32_t programId = glCreateProgram(); + glAttachShader( programId, vertex ); + glAttachShader( programId, fragment ); + + glLinkProgram( programId ); + + int success = GL_FALSE; + glGetProgramiv( programId, GL_LINK_STATUS, &success ); + if ( success == GL_FALSE ) + { + fprintf( stderr, "glLinkProgram:" ); + PrintLog( programId ); + return 0; + } + + glDeleteShader( vertex ); + glDeleteShader( fragment ); + + return programId; +} + +void DestroyShader( uint32_t programId ) +{ + glDeleteProgram( programId ); +} + +int GetUniformLocation( uint32_t programId, const char* name ) +{ + return glGetUniformLocation( programId, name ); +} + +void SetUniformBool( int location, bool value ) +{ + glUniform1i( location, (int)value ); +} + +void SetUniformInt( int location, int value ) +{ + glUniform1i( location, value ); +} + +void SetUniformFloat( int location, float value ) +{ + glUniform1f( location, value ); +} + +void SetUniformVec3( int location, b3Vec3 value ) +{ + glUniform3fv( location, 1, &value.x ); +} + +void SetUniformVec3( int location, float x, float y, float z ) +{ + glUniform3f( location, x, y, z ); +} + +void SetUniformVec4( int location, Vector4 value ) +{ + glUniform4fv( location, 1, &value.x ); +} + +void SetUniformColor( int location, RGBAf value ) +{ + glUniform4fv( location, 1, &value.r ); +} + +void SetUniformVec4( int location, float x, float y, float z, float w ) +{ + glUniform4f( location, x, y, z, w ); +} + +void SetUniformMatrix4( int location, const Matrix4& mat ) +{ + glUniformMatrix4fv( location, 1, GL_FALSE, &mat.cx.x ); +} + +void SetUniformBool( uint32_t programId, const char* name, bool value ) +{ + int location = glGetUniformLocation( programId, name ); + if ( location != -1 ) + { + glUniform1i( location, (int)value ); + } + else + { + location += 0; + } +} + +void SetUniformInt( uint32_t programId, const char* name, int value ) +{ + int location = glGetUniformLocation( programId, name ); + if ( location != -1 ) + { + glUniform1i( location, value ); + } +} + +void SetUniformFloat( uint32_t programId, const char* name, float value ) +{ + int location = glGetUniformLocation( programId, name ); + if ( location != -1 ) + { + glUniform1f( location, value ); + } +} + +void SetUniformVec3( uint32_t programId, const char* name, b3Vec3 value ) +{ + int location = glGetUniformLocation( programId, name ); + if ( location != -1 ) + { + glUniform3fv( location, 1, &value.x ); + } +} + +void SetUniformVec3( uint32_t programId, const char* name, float x, float y, float z ) +{ + int location = glGetUniformLocation( programId, name ); + if ( location != -1 ) + { + glUniform3f( location, x, y, z ); + } +} + +void SetUniformVec4( uint32_t programId, const char* name, Vector4 value ) +{ + int location = glGetUniformLocation( programId, name ); + if ( location != -1 ) + { + glUniform4fv( location, 1, &value.x ); + } +} + +void SetUniformVec4( uint32_t programId, const char* name, float x, float y, float z, float w ) +{ + int location = glGetUniformLocation( programId, name ); + if ( location != -1 ) + { + glUniform4f( location, x, y, z, w ); + } +} + +void SetUniformColor( uint32_t programId, const char* name, RGBAf value ) +{ + int location = glGetUniformLocation( programId, name ); + if ( location != -1 ) + { + glUniform4fv( location, 1, &value.r ); + } +} + +void SetUniformMatrix4( uint32_t programId, const char* name, const Matrix4& mat ) +{ + int location = glGetUniformLocation( programId, name ); + if ( location != -1 ) + { + glUniformMatrix4fv( location, 1, GL_FALSE, &mat.cx.x ); + } +} diff --git a/samples/shader.h b/samples/shader.h new file mode 100644 index 0000000..c180333 --- /dev/null +++ b/samples/shader.h @@ -0,0 +1,39 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "utility.h" + +#include "box3d/types.h" + +uint32_t CreateProgramFromFiles( const char* vertexPath, const char* fragmentPath ); +uint32_t CreateProgramFromStrings( const char* vs, const char* fs ); + +void DestroyShader( uint32_t programId ); + +// todo these are low value wrappers +int GetUniformLocation( uint32_t programId, const char* name ); + +void SetUniformBool( int location, bool value ); +void SetUniformInt( int location, int value ); +void SetUniformFloat( int location, float value ); +void SetUniformVec3( int location, b3Vec3 value ); +void SetUniformVec3( int location, float x, float y, float z ); +void SetUniformVec4( int location, Vector4 value ); +void SetUniformVec4( int location, float x, float y, float z, float w ); +void SetUniformColor( int location, RGBAf value ); +void SetUniformMatrix4( int location, const Matrix4& mat ); + +void SetUniformBool( uint32_t programId, const char* name, bool value ); +void SetUniformInt( uint32_t programId, const char* name, int value ); +void SetUniformFloat( uint32_t programId, const char* name, float value ); +void SetUniformVec3( uint32_t programId, const char* name, b3Vec3 value ); +void SetUniformVec3( uint32_t programId, const char* name, float x, float y, float z ); +void SetUniformVec4( uint32_t programId, const char* name, Vector4 value ); +void SetUniformVec4( uint32_t programId, const char* name, float x, float y, float z, float w ); +void SetUniformColor( uint32_t programId, const char* name, RGBAf value ); +void SetUniformMatrix4( uint32_t programId, const char* name, const Matrix4& mat ); + +void CheckOpenGL(); +void DumpGLInfo( bool dumpExtensions ); diff --git a/samples/ssao_buffer.cpp b/samples/ssao_buffer.cpp new file mode 100644 index 0000000..1ff1516 --- /dev/null +++ b/samples/ssao_buffer.cpp @@ -0,0 +1,163 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "ssao_buffer.h" + +#include "box3d/math_functions.h" +#include "shader.h" + +#include +#include +#include +#include +#include + +#include "geo_buffer.h" + +struct SsaoBuffer +{ + int32_t width, height; + uint32_t frameBufferId; + uint32_t blurFrameBufferId; + uint32_t colorBufferId; + uint32_t blurColorBufferId; + + b3Vec3 ssaoKernel[32]; + uint32_t noiseTexture; +}; + +SsaoBuffer* CreateSsaoBuffer(int32_t width, int32_t height) +{ + SsaoBuffer* buffer = (SsaoBuffer*)malloc(sizeof(SsaoBuffer)); + buffer->width = width; + buffer->height = height; + + // frame buffer + glGenFramebuffers(1, &buffer->frameBufferId); + glBindFramebuffer(GL_FRAMEBUFFER, buffer->frameBufferId); + + // color buffer + glGenTextures(1, &buffer->colorBufferId); + glBindTexture(GL_TEXTURE_2D, buffer->colorBufferId); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RED, width, height, 0, GL_RED, GL_FLOAT, NULL); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, buffer->colorBufferId, 0); + + if (glCheckFramebufferStatus(GL_FRAMEBUFFER) != GL_FRAMEBUFFER_COMPLETE) + { + fprintf(stderr, "SSAO Framebuffer not complete!\n"); + free(buffer); + return nullptr; + } + + // blur frame buffer + glGenFramebuffers(1, &buffer->blurFrameBufferId); + glBindFramebuffer(GL_FRAMEBUFFER, buffer->blurFrameBufferId); + + // blur color buffer + glGenTextures(1, &buffer->blurColorBufferId); + glBindTexture(GL_TEXTURE_2D, buffer->blurColorBufferId); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RED, width, height, 0, GL_RED, GL_FLOAT, NULL); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, buffer->blurColorBufferId, 0); + if (glCheckFramebufferStatus(GL_FRAMEBUFFER) != GL_FRAMEBUFFER_COMPLETE) + { + fprintf(stderr, "SSAO Blur Framebuffer not complete!\n"); + free(buffer); + return nullptr; + } + + // unbind frame buffer + glBindFramebuffer(GL_FRAMEBUFFER, 0); + + // generate sample kernel + std::uniform_real_distribution randomFloats(0.0f, 1.0f); // generates random floats between 0.0 and 1.0 + std::default_random_engine generator; + + for (uint32_t i = 0; i < 32; ++i) + { + b3Vec3 sample{randomFloats(generator) * 2.0f - 1.0f, randomFloats(generator) * 2.0f - 1.0f, randomFloats(generator)}; + sample = b3Normalize(sample); + sample = randomFloats(generator) * sample; + float scale = float(i) / 32.0f; + + // scale samples s.t. they're more aligned to center of kernel + scale = b3LerpFloat(0.1f, 1.0f, scale * scale); + sample = scale * sample; + buffer->ssaoKernel[i] = sample; + } + + // random sample kernel tangent vectors stuffed into a texture + b3Vec3 ssaoNoise[16]; + for (b3Vec3& noise : ssaoNoise) + { + // rotate around z-axis (in tangent space) + noise = {randomFloats(generator) * 2.0f - 1.0f, randomFloats(generator) * 2.0f - 1.0f, 0.0f}; + } + + glGenTextures(1, &buffer->noiseTexture); + glBindTexture(GL_TEXTURE_2D, buffer->noiseTexture); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA32F, 4, 4, 0, GL_RGB, GL_FLOAT, &ssaoNoise[0]); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT); + + return buffer; +} + +void BindSsaoBuffer(SsaoBuffer* buffer) +{ + glBindFramebuffer(GL_FRAMEBUFFER, buffer->frameBufferId); + glClear(GL_COLOR_BUFFER_BIT); +} + +void SetSsaoSamples(SsaoBuffer* buffer, uint32_t ssaoShader) +{ + // Send kernel + rotation + for (uint32_t i = 0; i < 32; ++i) + { + char sampleName[32]; + snprintf(sampleName, 32, "samples[%d]", i); + SetUniformVec3(ssaoShader, sampleName, buffer->ssaoKernel[i]); + } +} + +void BindSsaoTextures(SsaoBuffer* buffer) +{ + glActiveTexture(GL_TEXTURE2); + glBindTexture(GL_TEXTURE_2D, buffer->noiseTexture); +} + +void BindSsaoBlurBuffer(SsaoBuffer* buffer) +{ + glBindFramebuffer(GL_FRAMEBUFFER, buffer->blurFrameBufferId); + glClear(GL_COLOR_BUFFER_BIT); +} + +void BindSsaoColorTexture(SsaoBuffer* buffer) +{ + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D, buffer->colorBufferId); +} + +void BindSsaoBlurTexture(SsaoBuffer* buffer) +{ + glActiveTexture(GL_TEXTURE3); + glBindTexture(GL_TEXTURE_2D, buffer->blurColorBufferId); +} + +void DestroySsaoBuffer(SsaoBuffer* buffer) +{ + glDeleteFramebuffers(1, &buffer->frameBufferId); + glDeleteFramebuffers(1, &buffer->blurFrameBufferId); + + glDeleteTextures(1, &buffer->colorBufferId); + glDeleteTextures(1, &buffer->blurColorBufferId); + + glDeleteTextures(1, &buffer->noiseTexture); + + free(buffer); +} diff --git a/samples/ssao_buffer.h b/samples/ssao_buffer.h new file mode 100644 index 0000000..e9c1d11 --- /dev/null +++ b/samples/ssao_buffer.h @@ -0,0 +1,21 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +struct SsaoBuffer; +struct GeoBuffer; + +SsaoBuffer* CreateSsaoBuffer(int32_t width, int32_t height); + +void BindSsaoBuffer(SsaoBuffer* buffer); +void SetSsaoSamples(SsaoBuffer* buffer, uint32_t ssaoShader); +void BindSsaoTextures(SsaoBuffer* buffer); + +void BindSsaoBlurBuffer(SsaoBuffer* buffer); +void BindSsaoColorTexture(SsaoBuffer* buffer); +void BindSsaoBlurTexture(SsaoBuffer* buffer); + +void DestroySsaoBuffer(SsaoBuffer* buffer); diff --git a/samples/stb_image_write.h b/samples/stb_image_write.h new file mode 100644 index 0000000..151eaca --- /dev/null +++ b/samples/stb_image_write.h @@ -0,0 +1,1724 @@ +/* stb_image_write - v1.16 - public domain - http://nothings.org/stb + writes out PNG/BMP/TGA/JPEG/HDR images to C stdio - Sean Barrett 2010-2015 + no warranty implied; use at your own risk + + Before #including, + + #define STB_IMAGE_WRITE_IMPLEMENTATION + + in the file that you want to have the implementation. + + Will probably not work correctly with strict-aliasing optimizations. + +ABOUT: + + This header file is a library for writing images to C stdio or a callback. + + The PNG output is not optimal; it is 20-50% larger than the file + written by a decent optimizing implementation; though providing a custom + zlib compress function (see STBIW_ZLIB_COMPRESS) can mitigate that. + This library is designed for source code compactness and simplicity, + not optimal image file size or run-time performance. + +BUILDING: + + You can #define STBIW_ASSERT(x) before the #include to avoid using assert.h. + You can #define STBIW_MALLOC(), STBIW_REALLOC(), and STBIW_FREE() to replace + malloc,realloc,free. + You can #define STBIW_MEMMOVE() to replace memmove() + You can #define STBIW_ZLIB_COMPRESS to use a custom zlib-style compress function + for PNG compression (instead of the builtin one), it must have the following signature: + unsigned char * my_compress(unsigned char *data, int data_len, int *out_len, int quality); + The returned data will be freed with STBIW_FREE() (free() by default), + so it must be heap allocated with STBIW_MALLOC() (malloc() by default), + +UNICODE: + + If compiling for Windows and you wish to use Unicode filenames, compile + with + #define STBIW_WINDOWS_UTF8 + and pass utf8-encoded filenames. Call stbiw_convert_wchar_to_utf8 to convert + Windows wchar_t filenames to utf8. + +USAGE: + + There are five functions, one for each image file format: + + int stbi_write_png(char const *filename, int w, int h, int comp, const void *data, int stride_in_bytes); + int stbi_write_bmp(char const *filename, int w, int h, int comp, const void *data); + int stbi_write_tga(char const *filename, int w, int h, int comp, const void *data); + int stbi_write_jpg(char const *filename, int w, int h, int comp, const void *data, int quality); + int stbi_write_hdr(char const *filename, int w, int h, int comp, const float *data); + + void stbi_flip_vertically_on_write(int flag); // flag is non-zero to flip data vertically + + There are also five equivalent functions that use an arbitrary write function. You are + expected to open/close your file-equivalent before and after calling these: + + int stbi_write_png_to_func(stbi_write_func *func, void *context, int w, int h, int comp, const void *data, int stride_in_bytes); + int stbi_write_bmp_to_func(stbi_write_func *func, void *context, int w, int h, int comp, const void *data); + int stbi_write_tga_to_func(stbi_write_func *func, void *context, int w, int h, int comp, const void *data); + int stbi_write_hdr_to_func(stbi_write_func *func, void *context, int w, int h, int comp, const float *data); + int stbi_write_jpg_to_func(stbi_write_func *func, void *context, int x, int y, int comp, const void *data, int quality); + + where the callback is: + void stbi_write_func(void *context, void *data, int size); + + You can configure it with these global variables: + int stbi_write_tga_with_rle; // defaults to true; set to 0 to disable RLE + int stbi_write_png_compression_level; // defaults to 8; set to higher for more compression + int stbi_write_force_png_filter; // defaults to -1; set to 0..5 to force a filter mode + + + You can define STBI_WRITE_NO_STDIO to disable the file variant of these + functions, so the library will not use stdio.h at all. However, this will + also disable HDR writing, because it requires stdio for formatted output. + + Each function returns 0 on failure and non-0 on success. + + The functions create an image file defined by the parameters. The image + is a rectangle of pixels stored from left-to-right, top-to-bottom. + Each pixel contains 'comp' channels of data stored interleaved with 8-bits + per channel, in the following order: 1=Y, 2=YA, 3=RGB, 4=RGBA. (Y is + monochrome color.) The rectangle is 'w' pixels wide and 'h' pixels tall. + The *data pointer points to the first byte of the top-left-most pixel. + For PNG, "stride_in_bytes" is the distance in bytes from the first byte of + a row of pixels to the first byte of the next row of pixels. + + PNG creates output files with the same number of components as the input. + The BMP format expands Y to RGB in the file format and does not + output alpha. + + PNG supports writing rectangles of data even when the bytes storing rows of + data are not consecutive in memory (e.g. sub-rectangles of a larger image), + by supplying the stride between the beginning of adjacent rows. The other + formats do not. (Thus you cannot write a native-format BMP through the BMP + writer, both because it is in BGR order and because it may have padding + at the end of the line.) + + PNG allows you to set the deflate compression level by setting the global + variable 'stbi_write_png_compression_level' (it defaults to 8). + + HDR expects linear float data. Since the format is always 32-bit rgb(e) + data, alpha (if provided) is discarded, and for monochrome data it is + replicated across all three channels. + + TGA supports RLE or non-RLE compressed data. To use non-RLE-compressed + data, set the global variable 'stbi_write_tga_with_rle' to 0. + + JPEG does ignore alpha channels in input data; quality is between 1 and 100. + Higher quality looks better but results in a bigger image. + JPEG baseline (no JPEG progressive). + +CREDITS: + + + Sean Barrett - PNG/BMP/TGA + Baldur Karlsson - HDR + Jean-Sebastien Guay - TGA monochrome + Tim Kelsey - misc enhancements + Alan Hickman - TGA RLE + Emmanuel Julien - initial file IO callback implementation + Jon Olick - original jo_jpeg.cpp code + Daniel Gibson - integrate JPEG, allow external zlib + Aarni Koskela - allow choosing PNG filter + + bugfixes: + github:Chribba + Guillaume Chereau + github:jry2 + github:romigrou + Sergio Gonzalez + Jonas Karlsson + Filip Wasil + Thatcher Ulrich + github:poppolopoppo + Patrick Boettcher + github:xeekworx + Cap Petschulat + Simon Rodriguez + Ivan Tikhonov + github:ignotion + Adam Schackart + Andrew Kensler + +LICENSE + + See end of file for license information. + +*/ + +#ifndef INCLUDE_STB_IMAGE_WRITE_H +#define INCLUDE_STB_IMAGE_WRITE_H + +#include + +// if STB_IMAGE_WRITE_STATIC causes problems, try defining STBIWDEF to 'inline' or 'static inline' +#ifndef STBIWDEF +#ifdef STB_IMAGE_WRITE_STATIC +#define STBIWDEF static +#else +#ifdef __cplusplus +#define STBIWDEF extern "C" +#else +#define STBIWDEF extern +#endif +#endif +#endif + +#ifndef STB_IMAGE_WRITE_STATIC // C++ forbids static forward declarations +STBIWDEF int stbi_write_tga_with_rle; +STBIWDEF int stbi_write_png_compression_level; +STBIWDEF int stbi_write_force_png_filter; +#endif + +#ifndef STBI_WRITE_NO_STDIO +STBIWDEF int stbi_write_png(char const *filename, int w, int h, int comp, const void *data, int stride_in_bytes); +STBIWDEF int stbi_write_bmp(char const *filename, int w, int h, int comp, const void *data); +STBIWDEF int stbi_write_tga(char const *filename, int w, int h, int comp, const void *data); +STBIWDEF int stbi_write_hdr(char const *filename, int w, int h, int comp, const float *data); +STBIWDEF int stbi_write_jpg(char const *filename, int x, int y, int comp, const void *data, int quality); + +#ifdef STBIW_WINDOWS_UTF8 +STBIWDEF int stbiw_convert_wchar_to_utf8(char *buffer, size_t bufferlen, const wchar_t* input); +#endif +#endif + +typedef void stbi_write_func(void *context, void *data, int size); + +STBIWDEF int stbi_write_png_to_func(stbi_write_func *func, void *context, int w, int h, int comp, const void *data, int stride_in_bytes); +STBIWDEF int stbi_write_bmp_to_func(stbi_write_func *func, void *context, int w, int h, int comp, const void *data); +STBIWDEF int stbi_write_tga_to_func(stbi_write_func *func, void *context, int w, int h, int comp, const void *data); +STBIWDEF int stbi_write_hdr_to_func(stbi_write_func *func, void *context, int w, int h, int comp, const float *data); +STBIWDEF int stbi_write_jpg_to_func(stbi_write_func *func, void *context, int x, int y, int comp, const void *data, int quality); + +STBIWDEF void stbi_flip_vertically_on_write(int flip_boolean); + +#endif//INCLUDE_STB_IMAGE_WRITE_H + +#ifdef STB_IMAGE_WRITE_IMPLEMENTATION + +#ifdef _WIN32 + #ifndef _CRT_SECURE_NO_WARNINGS + #define _CRT_SECURE_NO_WARNINGS + #endif + #ifndef _CRT_NONSTDC_NO_DEPRECATE + #define _CRT_NONSTDC_NO_DEPRECATE + #endif +#endif + +#ifndef STBI_WRITE_NO_STDIO +#include +#endif // STBI_WRITE_NO_STDIO + +#include +#include +#include +#include + +#if defined(STBIW_MALLOC) && defined(STBIW_FREE) && (defined(STBIW_REALLOC) || defined(STBIW_REALLOC_SIZED)) +// ok +#elif !defined(STBIW_MALLOC) && !defined(STBIW_FREE) && !defined(STBIW_REALLOC) && !defined(STBIW_REALLOC_SIZED) +// ok +#else +#error "Must define all or none of STBIW_MALLOC, STBIW_FREE, and STBIW_REALLOC (or STBIW_REALLOC_SIZED)." +#endif + +#ifndef STBIW_MALLOC +#define STBIW_MALLOC(sz) malloc(sz) +#define STBIW_REALLOC(p,newsz) realloc(p,newsz) +#define STBIW_FREE(p) free(p) +#endif + +#ifndef STBIW_REALLOC_SIZED +#define STBIW_REALLOC_SIZED(p,oldsz,newsz) STBIW_REALLOC(p,newsz) +#endif + + +#ifndef STBIW_MEMMOVE +#define STBIW_MEMMOVE(a,b,sz) memmove(a,b,sz) +#endif + + +#ifndef STBIW_ASSERT +#include +#define STBIW_ASSERT(x) assert(x) +#endif + +#define STBIW_UCHAR(x) (unsigned char) ((x) & 0xff) + +#ifdef STB_IMAGE_WRITE_STATIC +static int stbi_write_png_compression_level = 8; +static int stbi_write_tga_with_rle = 1; +static int stbi_write_force_png_filter = -1; +#else +int stbi_write_png_compression_level = 8; +int stbi_write_tga_with_rle = 1; +int stbi_write_force_png_filter = -1; +#endif + +static int stbi__flip_vertically_on_write = 0; + +STBIWDEF void stbi_flip_vertically_on_write(int flag) +{ + stbi__flip_vertically_on_write = flag; +} + +typedef struct +{ + stbi_write_func *func; + void *context; + unsigned char buffer[64]; + int buf_used; +} stbi__write_context; + +// initialize a callback-based context +static void stbi__start_write_callbacks(stbi__write_context *s, stbi_write_func *c, void *context) +{ + s->func = c; + s->context = context; +} + +#ifndef STBI_WRITE_NO_STDIO + +static void stbi__stdio_write(void *context, void *data, int size) +{ + fwrite(data,1,size,(FILE*) context); +} + +#if defined(_WIN32) && defined(STBIW_WINDOWS_UTF8) +#ifdef __cplusplus +#define STBIW_EXTERN extern "C" +#else +#define STBIW_EXTERN extern +#endif +STBIW_EXTERN __declspec(dllimport) int __stdcall MultiByteToWideChar(unsigned int cp, unsigned long flags, const char *str, int cbmb, wchar_t *widestr, int cchwide); +STBIW_EXTERN __declspec(dllimport) int __stdcall WideCharToMultiByte(unsigned int cp, unsigned long flags, const wchar_t *widestr, int cchwide, char *str, int cbmb, const char *defchar, int *used_default); + +STBIWDEF int stbiw_convert_wchar_to_utf8(char *buffer, size_t bufferlen, const wchar_t* input) +{ + return WideCharToMultiByte(65001 /* UTF8 */, 0, input, -1, buffer, (int) bufferlen, NULL, NULL); +} +#endif + +static FILE *stbiw__fopen(char const *filename, char const *mode) +{ + FILE *f; +#if defined(_WIN32) && defined(STBIW_WINDOWS_UTF8) + wchar_t wMode[64]; + wchar_t wFilename[1024]; + if (0 == MultiByteToWideChar(65001 /* UTF8 */, 0, filename, -1, wFilename, sizeof(wFilename)/sizeof(*wFilename))) + return 0; + + if (0 == MultiByteToWideChar(65001 /* UTF8 */, 0, mode, -1, wMode, sizeof(wMode)/sizeof(*wMode))) + return 0; + +#if defined(_MSC_VER) && _MSC_VER >= 1400 + if (0 != _wfopen_s(&f, wFilename, wMode)) + f = 0; +#else + f = _wfopen(wFilename, wMode); +#endif + +#elif defined(_MSC_VER) && _MSC_VER >= 1400 + if (0 != fopen_s(&f, filename, mode)) + f=0; +#else + f = fopen(filename, mode); +#endif + return f; +} + +static int stbi__start_write_file(stbi__write_context *s, const char *filename) +{ + FILE *f = stbiw__fopen(filename, "wb"); + stbi__start_write_callbacks(s, stbi__stdio_write, (void *) f); + return f != NULL; +} + +static void stbi__end_write_file(stbi__write_context *s) +{ + fclose((FILE *)s->context); +} + +#endif // !STBI_WRITE_NO_STDIO + +typedef unsigned int stbiw_uint32; +typedef int stb_image_write_test[sizeof(stbiw_uint32)==4 ? 1 : -1]; + +static void stbiw__writefv(stbi__write_context *s, const char *fmt, va_list v) +{ + while (*fmt) { + switch (*fmt++) { + case ' ': break; + case '1': { unsigned char x = STBIW_UCHAR(va_arg(v, int)); + s->func(s->context,&x,1); + break; } + case '2': { int x = va_arg(v,int); + unsigned char b[2]; + b[0] = STBIW_UCHAR(x); + b[1] = STBIW_UCHAR(x>>8); + s->func(s->context,b,2); + break; } + case '4': { stbiw_uint32 x = va_arg(v,int); + unsigned char b[4]; + b[0]=STBIW_UCHAR(x); + b[1]=STBIW_UCHAR(x>>8); + b[2]=STBIW_UCHAR(x>>16); + b[3]=STBIW_UCHAR(x>>24); + s->func(s->context,b,4); + break; } + default: + STBIW_ASSERT(0); + return; + } + } +} + +static void stbiw__writef(stbi__write_context *s, const char *fmt, ...) +{ + va_list v; + va_start(v, fmt); + stbiw__writefv(s, fmt, v); + va_end(v); +} + +static void stbiw__write_flush(stbi__write_context *s) +{ + if (s->buf_used) { + s->func(s->context, &s->buffer, s->buf_used); + s->buf_used = 0; + } +} + +static void stbiw__putc(stbi__write_context *s, unsigned char c) +{ + s->func(s->context, &c, 1); +} + +static void stbiw__write1(stbi__write_context *s, unsigned char a) +{ + if ((size_t)s->buf_used + 1 > sizeof(s->buffer)) + stbiw__write_flush(s); + s->buffer[s->buf_used++] = a; +} + +static void stbiw__write3(stbi__write_context *s, unsigned char a, unsigned char b, unsigned char c) +{ + int n; + if ((size_t)s->buf_used + 3 > sizeof(s->buffer)) + stbiw__write_flush(s); + n = s->buf_used; + s->buf_used = n+3; + s->buffer[n+0] = a; + s->buffer[n+1] = b; + s->buffer[n+2] = c; +} + +static void stbiw__write_pixel(stbi__write_context *s, int rgb_dir, int comp, int write_alpha, int expand_mono, unsigned char *d) +{ + unsigned char bg[3] = { 255, 0, 255}, px[3]; + int k; + + if (write_alpha < 0) + stbiw__write1(s, d[comp - 1]); + + switch (comp) { + case 2: // 2 pixels = mono + alpha, alpha is written separately, so same as 1-channel case + case 1: + if (expand_mono) + stbiw__write3(s, d[0], d[0], d[0]); // monochrome bmp + else + stbiw__write1(s, d[0]); // monochrome TGA + break; + case 4: + if (!write_alpha) { + // composite against pink background + for (k = 0; k < 3; ++k) + px[k] = bg[k] + ((d[k] - bg[k]) * d[3]) / 255; + stbiw__write3(s, px[1 - rgb_dir], px[1], px[1 + rgb_dir]); + break; + } + /* FALLTHROUGH */ + case 3: + stbiw__write3(s, d[1 - rgb_dir], d[1], d[1 + rgb_dir]); + break; + } + if (write_alpha > 0) + stbiw__write1(s, d[comp - 1]); +} + +static void stbiw__write_pixels(stbi__write_context *s, int rgb_dir, int vdir, int x, int y, int comp, void *data, int write_alpha, int scanline_pad, int expand_mono) +{ + stbiw_uint32 zero = 0; + int i,j, j_end; + + if (y <= 0) + return; + + if (stbi__flip_vertically_on_write) + vdir *= -1; + + if (vdir < 0) { + j_end = -1; j = y-1; + } else { + j_end = y; j = 0; + } + + for (; j != j_end; j += vdir) { + for (i=0; i < x; ++i) { + unsigned char *d = (unsigned char *) data + (j*x+i)*comp; + stbiw__write_pixel(s, rgb_dir, comp, write_alpha, expand_mono, d); + } + stbiw__write_flush(s); + s->func(s->context, &zero, scanline_pad); + } +} + +static int stbiw__outfile(stbi__write_context *s, int rgb_dir, int vdir, int x, int y, int comp, int expand_mono, void *data, int alpha, int pad, const char *fmt, ...) +{ + if (y < 0 || x < 0) { + return 0; + } else { + va_list v; + va_start(v, fmt); + stbiw__writefv(s, fmt, v); + va_end(v); + stbiw__write_pixels(s,rgb_dir,vdir,x,y,comp,data,alpha,pad, expand_mono); + return 1; + } +} + +static int stbi_write_bmp_core(stbi__write_context *s, int x, int y, int comp, const void *data) +{ + if (comp != 4) { + // write RGB bitmap + int pad = (-x*3) & 3; + return stbiw__outfile(s,-1,-1,x,y,comp,1,(void *) data,0,pad, + "11 4 22 4" "4 44 22 444444", + 'B', 'M', 14+40+(x*3+pad)*y, 0,0, 14+40, // file header + 40, x,y, 1,24, 0,0,0,0,0,0); // bitmap header + } else { + // RGBA bitmaps need a v4 header + // use BI_BITFIELDS mode with 32bpp and alpha mask + // (straight BI_RGB with alpha mask doesn't work in most readers) + return stbiw__outfile(s,-1,-1,x,y,comp,1,(void *)data,1,0, + "11 4 22 4" "4 44 22 444444 4444 4 444 444 444 444", + 'B', 'M', 14+108+x*y*4, 0, 0, 14+108, // file header + 108, x,y, 1,32, 3,0,0,0,0,0, 0xff0000,0xff00,0xff,0xff000000u, 0, 0,0,0, 0,0,0, 0,0,0, 0,0,0); // bitmap V4 header + } +} + +STBIWDEF int stbi_write_bmp_to_func(stbi_write_func *func, void *context, int x, int y, int comp, const void *data) +{ + stbi__write_context s = { 0 }; + stbi__start_write_callbacks(&s, func, context); + return stbi_write_bmp_core(&s, x, y, comp, data); +} + +#ifndef STBI_WRITE_NO_STDIO +STBIWDEF int stbi_write_bmp(char const *filename, int x, int y, int comp, const void *data) +{ + stbi__write_context s = { 0 }; + if (stbi__start_write_file(&s,filename)) { + int r = stbi_write_bmp_core(&s, x, y, comp, data); + stbi__end_write_file(&s); + return r; + } else + return 0; +} +#endif //!STBI_WRITE_NO_STDIO + +static int stbi_write_tga_core(stbi__write_context *s, int x, int y, int comp, void *data) +{ + int has_alpha = (comp == 2 || comp == 4); + int colorbytes = has_alpha ? comp-1 : comp; + int format = colorbytes < 2 ? 3 : 2; // 3 color channels (RGB/RGBA) = 2, 1 color channel (Y/YA) = 3 + + if (y < 0 || x < 0) + return 0; + + if (!stbi_write_tga_with_rle) { + return stbiw__outfile(s, -1, -1, x, y, comp, 0, (void *) data, has_alpha, 0, + "111 221 2222 11", 0, 0, format, 0, 0, 0, 0, 0, x, y, (colorbytes + has_alpha) * 8, has_alpha * 8); + } else { + int i,j,k; + int jend, jdir; + + stbiw__writef(s, "111 221 2222 11", 0,0,format+8, 0,0,0, 0,0,x,y, (colorbytes + has_alpha) * 8, has_alpha * 8); + + if (stbi__flip_vertically_on_write) { + j = 0; + jend = y; + jdir = 1; + } else { + j = y-1; + jend = -1; + jdir = -1; + } + for (; j != jend; j += jdir) { + unsigned char *row = (unsigned char *) data + j * x * comp; + int len; + + for (i = 0; i < x; i += len) { + unsigned char *begin = row + i * comp; + int diff = 1; + len = 1; + + if (i < x - 1) { + ++len; + diff = memcmp(begin, row + (i + 1) * comp, comp); + if (diff) { + const unsigned char *prev = begin; + for (k = i + 2; k < x && len < 128; ++k) { + if (memcmp(prev, row + k * comp, comp)) { + prev += comp; + ++len; + } else { + --len; + break; + } + } + } else { + for (k = i + 2; k < x && len < 128; ++k) { + if (!memcmp(begin, row + k * comp, comp)) { + ++len; + } else { + break; + } + } + } + } + + if (diff) { + unsigned char header = STBIW_UCHAR(len - 1); + stbiw__write1(s, header); + for (k = 0; k < len; ++k) { + stbiw__write_pixel(s, -1, comp, has_alpha, 0, begin + k * comp); + } + } else { + unsigned char header = STBIW_UCHAR(len - 129); + stbiw__write1(s, header); + stbiw__write_pixel(s, -1, comp, has_alpha, 0, begin); + } + } + } + stbiw__write_flush(s); + } + return 1; +} + +STBIWDEF int stbi_write_tga_to_func(stbi_write_func *func, void *context, int x, int y, int comp, const void *data) +{ + stbi__write_context s = { 0 }; + stbi__start_write_callbacks(&s, func, context); + return stbi_write_tga_core(&s, x, y, comp, (void *) data); +} + +#ifndef STBI_WRITE_NO_STDIO +STBIWDEF int stbi_write_tga(char const *filename, int x, int y, int comp, const void *data) +{ + stbi__write_context s = { 0 }; + if (stbi__start_write_file(&s,filename)) { + int r = stbi_write_tga_core(&s, x, y, comp, (void *) data); + stbi__end_write_file(&s); + return r; + } else + return 0; +} +#endif + +// ************************************************************************************************* +// Radiance RGBE HDR writer +// by Baldur Karlsson + +#define stbiw__max(a, b) ((a) > (b) ? (a) : (b)) + +#ifndef STBI_WRITE_NO_STDIO + +static void stbiw__linear_to_rgbe(unsigned char *rgbe, float *linear) +{ + int exponent; + float maxcomp = stbiw__max(linear[0], stbiw__max(linear[1], linear[2])); + + if (maxcomp < 1e-32f) { + rgbe[0] = rgbe[1] = rgbe[2] = rgbe[3] = 0; + } else { + float normalize = (float) frexp(maxcomp, &exponent) * 256.0f/maxcomp; + + rgbe[0] = (unsigned char)(linear[0] * normalize); + rgbe[1] = (unsigned char)(linear[1] * normalize); + rgbe[2] = (unsigned char)(linear[2] * normalize); + rgbe[3] = (unsigned char)(exponent + 128); + } +} + +static void stbiw__write_run_data(stbi__write_context *s, int length, unsigned char databyte) +{ + unsigned char lengthbyte = STBIW_UCHAR(length+128); + STBIW_ASSERT(length+128 <= 255); + s->func(s->context, &lengthbyte, 1); + s->func(s->context, &databyte, 1); +} + +static void stbiw__write_dump_data(stbi__write_context *s, int length, unsigned char *data) +{ + unsigned char lengthbyte = STBIW_UCHAR(length); + STBIW_ASSERT(length <= 128); // inconsistent with spec but consistent with official code + s->func(s->context, &lengthbyte, 1); + s->func(s->context, data, length); +} + +static void stbiw__write_hdr_scanline(stbi__write_context *s, int width, int ncomp, unsigned char *scratch, float *scanline) +{ + unsigned char scanlineheader[4] = { 2, 2, 0, 0 }; + unsigned char rgbe[4]; + float linear[3]; + int x; + + scanlineheader[2] = (width&0xff00)>>8; + scanlineheader[3] = (width&0x00ff); + + /* skip RLE for images too small or large */ + if (width < 8 || width >= 32768) { + for (x=0; x < width; x++) { + switch (ncomp) { + case 4: /* fallthrough */ + case 3: linear[2] = scanline[x*ncomp + 2]; + linear[1] = scanline[x*ncomp + 1]; + linear[0] = scanline[x*ncomp + 0]; + break; + default: + linear[0] = linear[1] = linear[2] = scanline[x*ncomp + 0]; + break; + } + stbiw__linear_to_rgbe(rgbe, linear); + s->func(s->context, rgbe, 4); + } + } else { + int c,r; + /* encode into scratch buffer */ + for (x=0; x < width; x++) { + switch(ncomp) { + case 4: /* fallthrough */ + case 3: linear[2] = scanline[x*ncomp + 2]; + linear[1] = scanline[x*ncomp + 1]; + linear[0] = scanline[x*ncomp + 0]; + break; + default: + linear[0] = linear[1] = linear[2] = scanline[x*ncomp + 0]; + break; + } + stbiw__linear_to_rgbe(rgbe, linear); + scratch[x + width*0] = rgbe[0]; + scratch[x + width*1] = rgbe[1]; + scratch[x + width*2] = rgbe[2]; + scratch[x + width*3] = rgbe[3]; + } + + s->func(s->context, scanlineheader, 4); + + /* RLE each component separately */ + for (c=0; c < 4; c++) { + unsigned char *comp = &scratch[width*c]; + + x = 0; + while (x < width) { + // find first run + r = x; + while (r+2 < width) { + if (comp[r] == comp[r+1] && comp[r] == comp[r+2]) + break; + ++r; + } + if (r+2 >= width) + r = width; + // dump up to first run + while (x < r) { + int len = r-x; + if (len > 128) len = 128; + stbiw__write_dump_data(s, len, &comp[x]); + x += len; + } + // if there's a run, output it + if (r+2 < width) { // same test as what we break out of in search loop, so only true if we break'd + // find next byte after run + while (r < width && comp[r] == comp[x]) + ++r; + // output run up to r + while (x < r) { + int len = r-x; + if (len > 127) len = 127; + stbiw__write_run_data(s, len, comp[x]); + x += len; + } + } + } + } + } +} + +static int stbi_write_hdr_core(stbi__write_context *s, int x, int y, int comp, float *data) +{ + if (y <= 0 || x <= 0 || data == NULL) + return 0; + else { + // Each component is stored separately. Allocate scratch space for full output scanline. + unsigned char *scratch = (unsigned char *) STBIW_MALLOC(x*4); + int i, len; + char buffer[128]; + char header[] = "#?RADIANCE\n# Written by stb_image_write.h\nFORMAT=32-bit_rle_rgbe\n"; + s->func(s->context, header, sizeof(header)-1); + +#ifdef __STDC_LIB_EXT1__ + len = sprintf_s(buffer, sizeof(buffer), "EXPOSURE= 1.0000000000000\n\n-Y %d +X %d\n", y, x); +#else + len = snprintf(buffer, sizeof(buffer), "EXPOSURE= 1.0000000000000\n\n-Y %d +X %d\n", y, x); +#endif + s->func(s->context, buffer, len); + + for(i=0; i < y; i++) + stbiw__write_hdr_scanline(s, x, comp, scratch, data + comp*x*(stbi__flip_vertically_on_write ? y-1-i : i)); + STBIW_FREE(scratch); + return 1; + } +} + +STBIWDEF int stbi_write_hdr_to_func(stbi_write_func *func, void *context, int x, int y, int comp, const float *data) +{ + stbi__write_context s = { 0 }; + stbi__start_write_callbacks(&s, func, context); + return stbi_write_hdr_core(&s, x, y, comp, (float *) data); +} + +STBIWDEF int stbi_write_hdr(char const *filename, int x, int y, int comp, const float *data) +{ + stbi__write_context s = { 0 }; + if (stbi__start_write_file(&s,filename)) { + int r = stbi_write_hdr_core(&s, x, y, comp, (float *) data); + stbi__end_write_file(&s); + return r; + } else + return 0; +} +#endif // STBI_WRITE_NO_STDIO + + +////////////////////////////////////////////////////////////////////////////// +// +// PNG writer +// + +#ifndef STBIW_ZLIB_COMPRESS +// stretchy buffer; stbiw__sbpush() == vector<>::push_back() -- stbiw__sbcount() == vector<>::size() +#define stbiw__sbraw(a) ((int *) (void *) (a) - 2) +#define stbiw__sbm(a) stbiw__sbraw(a)[0] +#define stbiw__sbn(a) stbiw__sbraw(a)[1] + +#define stbiw__sbneedgrow(a,n) ((a)==0 || stbiw__sbn(a)+n >= stbiw__sbm(a)) +#define stbiw__sbmaybegrow(a,n) (stbiw__sbneedgrow(a,(n)) ? stbiw__sbgrow(a,n) : 0) +#define stbiw__sbgrow(a,n) stbiw__sbgrowf((void **) &(a), (n), sizeof(*(a))) + +#define stbiw__sbpush(a, v) (stbiw__sbmaybegrow(a,1), (a)[stbiw__sbn(a)++] = (v)) +#define stbiw__sbcount(a) ((a) ? stbiw__sbn(a) : 0) +#define stbiw__sbfree(a) ((a) ? STBIW_FREE(stbiw__sbraw(a)),0 : 0) + +static void *stbiw__sbgrowf(void **arr, int increment, int itemsize) +{ + int m = *arr ? 2*stbiw__sbm(*arr)+increment : increment+1; + void *p = STBIW_REALLOC_SIZED(*arr ? stbiw__sbraw(*arr) : 0, *arr ? (stbiw__sbm(*arr)*itemsize + sizeof(int)*2) : 0, itemsize * m + sizeof(int)*2); + STBIW_ASSERT(p); + if (p) { + if (!*arr) ((int *) p)[1] = 0; + *arr = (void *) ((int *) p + 2); + stbiw__sbm(*arr) = m; + } + return *arr; +} + +static unsigned char *stbiw__zlib_flushf(unsigned char *data, unsigned int *bitbuffer, int *bitcount) +{ + while (*bitcount >= 8) { + stbiw__sbpush(data, STBIW_UCHAR(*bitbuffer)); + *bitbuffer >>= 8; + *bitcount -= 8; + } + return data; +} + +static int stbiw__zlib_bitrev(int code, int codebits) +{ + int res=0; + while (codebits--) { + res = (res << 1) | (code & 1); + code >>= 1; + } + return res; +} + +static unsigned int stbiw__zlib_countm(unsigned char *a, unsigned char *b, int limit) +{ + int i; + for (i=0; i < limit && i < 258; ++i) + if (a[i] != b[i]) break; + return i; +} + +static unsigned int stbiw__zhash(unsigned char *data) +{ + stbiw_uint32 hash = data[0] + (data[1] << 8) + (data[2] << 16); + hash ^= hash << 3; + hash += hash >> 5; + hash ^= hash << 4; + hash += hash >> 17; + hash ^= hash << 25; + hash += hash >> 6; + return hash; +} + +#define stbiw__zlib_flush() (out = stbiw__zlib_flushf(out, &bitbuf, &bitcount)) +#define stbiw__zlib_add(code,codebits) \ + (bitbuf |= (code) << bitcount, bitcount += (codebits), stbiw__zlib_flush()) +#define stbiw__zlib_huffa(b,c) stbiw__zlib_add(stbiw__zlib_bitrev(b,c),c) +// default huffman tables +#define stbiw__zlib_huff1(n) stbiw__zlib_huffa(0x30 + (n), 8) +#define stbiw__zlib_huff2(n) stbiw__zlib_huffa(0x190 + (n)-144, 9) +#define stbiw__zlib_huff3(n) stbiw__zlib_huffa(0 + (n)-256,7) +#define stbiw__zlib_huff4(n) stbiw__zlib_huffa(0xc0 + (n)-280,8) +#define stbiw__zlib_huff(n) ((n) <= 143 ? stbiw__zlib_huff1(n) : (n) <= 255 ? stbiw__zlib_huff2(n) : (n) <= 279 ? stbiw__zlib_huff3(n) : stbiw__zlib_huff4(n)) +#define stbiw__zlib_huffb(n) ((n) <= 143 ? stbiw__zlib_huff1(n) : stbiw__zlib_huff2(n)) + +#define stbiw__ZHASH 16384 + +#endif // STBIW_ZLIB_COMPRESS + +STBIWDEF unsigned char * stbi_zlib_compress(unsigned char *data, int data_len, int *out_len, int quality) +{ +#ifdef STBIW_ZLIB_COMPRESS + // user provided a zlib compress implementation, use that + return STBIW_ZLIB_COMPRESS(data, data_len, out_len, quality); +#else // use builtin + static unsigned short lengthc[] = { 3,4,5,6,7,8,9,10,11,13,15,17,19,23,27,31,35,43,51,59,67,83,99,115,131,163,195,227,258, 259 }; + static unsigned char lengtheb[]= { 0,0,0,0,0,0,0, 0, 1, 1, 1, 1, 2, 2, 2, 2, 3, 3, 3, 3, 4, 4, 4, 4, 5, 5, 5, 5, 0 }; + static unsigned short distc[] = { 1,2,3,4,5,7,9,13,17,25,33,49,65,97,129,193,257,385,513,769,1025,1537,2049,3073,4097,6145,8193,12289,16385,24577, 32768 }; + static unsigned char disteb[] = { 0,0,0,0,1,1,2,2,3,3,4,4,5,5,6,6,7,7,8,8,9,9,10,10,11,11,12,12,13,13 }; + unsigned int bitbuf=0; + int i,j, bitcount=0; + unsigned char *out = NULL; + unsigned char ***hash_table = (unsigned char***) STBIW_MALLOC(stbiw__ZHASH * sizeof(unsigned char**)); + if (hash_table == NULL) + return NULL; + if (quality < 5) quality = 5; + + stbiw__sbpush(out, 0x78); // DEFLATE 32K window + stbiw__sbpush(out, 0x5e); // FLEVEL = 1 + stbiw__zlib_add(1,1); // BFINAL = 1 + stbiw__zlib_add(1,2); // BTYPE = 1 -- fixed huffman + + for (i=0; i < stbiw__ZHASH; ++i) + hash_table[i] = NULL; + + i=0; + while (i < data_len-3) { + // hash next 3 bytes of data to be compressed + int h = stbiw__zhash(data+i)&(stbiw__ZHASH-1), best=3; + unsigned char *bestloc = 0; + unsigned char **hlist = hash_table[h]; + int n = stbiw__sbcount(hlist); + for (j=0; j < n; ++j) { + if (hlist[j]-data > i-32768) { // if entry lies within window + int d = stbiw__zlib_countm(hlist[j], data+i, data_len-i); + if (d >= best) { best=d; bestloc=hlist[j]; } + } + } + // when hash table entry is too long, delete half the entries + if (hash_table[h] && stbiw__sbn(hash_table[h]) == 2*quality) { + STBIW_MEMMOVE(hash_table[h], hash_table[h]+quality, sizeof(hash_table[h][0])*quality); + stbiw__sbn(hash_table[h]) = quality; + } + stbiw__sbpush(hash_table[h],data+i); + + if (bestloc) { + // "lazy matching" - check match at *next* byte, and if it's better, do cur byte as literal + h = stbiw__zhash(data+i+1)&(stbiw__ZHASH-1); + hlist = hash_table[h]; + n = stbiw__sbcount(hlist); + for (j=0; j < n; ++j) { + if (hlist[j]-data > i-32767) { + int e = stbiw__zlib_countm(hlist[j], data+i+1, data_len-i-1); + if (e > best) { // if next match is better, bail on current match + bestloc = NULL; + break; + } + } + } + } + + if (bestloc) { + int d = (int) (data+i - bestloc); // distance back + STBIW_ASSERT(d <= 32767 && best <= 258); + for (j=0; best > lengthc[j+1]-1; ++j); + stbiw__zlib_huff(j+257); + if (lengtheb[j]) stbiw__zlib_add(best - lengthc[j], lengtheb[j]); + for (j=0; d > distc[j+1]-1; ++j); + stbiw__zlib_add(stbiw__zlib_bitrev(j,5),5); + if (disteb[j]) stbiw__zlib_add(d - distc[j], disteb[j]); + i += best; + } else { + stbiw__zlib_huffb(data[i]); + ++i; + } + } + // write out final bytes + for (;i < data_len; ++i) + stbiw__zlib_huffb(data[i]); + stbiw__zlib_huff(256); // end of block + // pad with 0 bits to byte boundary + while (bitcount) + stbiw__zlib_add(0,1); + + for (i=0; i < stbiw__ZHASH; ++i) + (void) stbiw__sbfree(hash_table[i]); + STBIW_FREE(hash_table); + + // store uncompressed instead if compression was worse + if (stbiw__sbn(out) > data_len + 2 + ((data_len+32766)/32767)*5) { + stbiw__sbn(out) = 2; // truncate to DEFLATE 32K window and FLEVEL = 1 + for (j = 0; j < data_len;) { + int blocklen = data_len - j; + if (blocklen > 32767) blocklen = 32767; + stbiw__sbpush(out, data_len - j == blocklen); // BFINAL = ?, BTYPE = 0 -- no compression + stbiw__sbpush(out, STBIW_UCHAR(blocklen)); // LEN + stbiw__sbpush(out, STBIW_UCHAR(blocklen >> 8)); + stbiw__sbpush(out, STBIW_UCHAR(~blocklen)); // NLEN + stbiw__sbpush(out, STBIW_UCHAR(~blocklen >> 8)); + memcpy(out+stbiw__sbn(out), data+j, blocklen); + stbiw__sbn(out) += blocklen; + j += blocklen; + } + } + + { + // compute adler32 on input + unsigned int s1=1, s2=0; + int blocklen = (int) (data_len % 5552); + j=0; + while (j < data_len) { + for (i=0; i < blocklen; ++i) { s1 += data[j+i]; s2 += s1; } + s1 %= 65521; s2 %= 65521; + j += blocklen; + blocklen = 5552; + } + stbiw__sbpush(out, STBIW_UCHAR(s2 >> 8)); + stbiw__sbpush(out, STBIW_UCHAR(s2)); + stbiw__sbpush(out, STBIW_UCHAR(s1 >> 8)); + stbiw__sbpush(out, STBIW_UCHAR(s1)); + } + *out_len = stbiw__sbn(out); + // make returned pointer freeable + STBIW_MEMMOVE(stbiw__sbraw(out), out, *out_len); + return (unsigned char *) stbiw__sbraw(out); +#endif // STBIW_ZLIB_COMPRESS +} + +static unsigned int stbiw__crc32(unsigned char *buffer, int len) +{ +#ifdef STBIW_CRC32 + return STBIW_CRC32(buffer, len); +#else + static unsigned int crc_table[256] = + { + 0x00000000, 0x77073096, 0xEE0E612C, 0x990951BA, 0x076DC419, 0x706AF48F, 0xE963A535, 0x9E6495A3, + 0x0eDB8832, 0x79DCB8A4, 0xE0D5E91E, 0x97D2D988, 0x09B64C2B, 0x7EB17CBD, 0xE7B82D07, 0x90BF1D91, + 0x1DB71064, 0x6AB020F2, 0xF3B97148, 0x84BE41DE, 0x1ADAD47D, 0x6DDDE4EB, 0xF4D4B551, 0x83D385C7, + 0x136C9856, 0x646BA8C0, 0xFD62F97A, 0x8A65C9EC, 0x14015C4F, 0x63066CD9, 0xFA0F3D63, 0x8D080DF5, + 0x3B6E20C8, 0x4C69105E, 0xD56041E4, 0xA2677172, 0x3C03E4D1, 0x4B04D447, 0xD20D85FD, 0xA50AB56B, + 0x35B5A8FA, 0x42B2986C, 0xDBBBC9D6, 0xACBCF940, 0x32D86CE3, 0x45DF5C75, 0xDCD60DCF, 0xABD13D59, + 0x26D930AC, 0x51DE003A, 0xC8D75180, 0xBFD06116, 0x21B4F4B5, 0x56B3C423, 0xCFBA9599, 0xB8BDA50F, + 0x2802B89E, 0x5F058808, 0xC60CD9B2, 0xB10BE924, 0x2F6F7C87, 0x58684C11, 0xC1611DAB, 0xB6662D3D, + 0x76DC4190, 0x01DB7106, 0x98D220BC, 0xEFD5102A, 0x71B18589, 0x06B6B51F, 0x9FBFE4A5, 0xE8B8D433, + 0x7807C9A2, 0x0F00F934, 0x9609A88E, 0xE10E9818, 0x7F6A0DBB, 0x086D3D2D, 0x91646C97, 0xE6635C01, + 0x6B6B51F4, 0x1C6C6162, 0x856530D8, 0xF262004E, 0x6C0695ED, 0x1B01A57B, 0x8208F4C1, 0xF50FC457, + 0x65B0D9C6, 0x12B7E950, 0x8BBEB8EA, 0xFCB9887C, 0x62DD1DDF, 0x15DA2D49, 0x8CD37CF3, 0xFBD44C65, + 0x4DB26158, 0x3AB551CE, 0xA3BC0074, 0xD4BB30E2, 0x4ADFA541, 0x3DD895D7, 0xA4D1C46D, 0xD3D6F4FB, + 0x4369E96A, 0x346ED9FC, 0xAD678846, 0xDA60B8D0, 0x44042D73, 0x33031DE5, 0xAA0A4C5F, 0xDD0D7CC9, + 0x5005713C, 0x270241AA, 0xBE0B1010, 0xC90C2086, 0x5768B525, 0x206F85B3, 0xB966D409, 0xCE61E49F, + 0x5EDEF90E, 0x29D9C998, 0xB0D09822, 0xC7D7A8B4, 0x59B33D17, 0x2EB40D81, 0xB7BD5C3B, 0xC0BA6CAD, + 0xEDB88320, 0x9ABFB3B6, 0x03B6E20C, 0x74B1D29A, 0xEAD54739, 0x9DD277AF, 0x04DB2615, 0x73DC1683, + 0xE3630B12, 0x94643B84, 0x0D6D6A3E, 0x7A6A5AA8, 0xE40ECF0B, 0x9309FF9D, 0x0A00AE27, 0x7D079EB1, + 0xF00F9344, 0x8708A3D2, 0x1E01F268, 0x6906C2FE, 0xF762575D, 0x806567CB, 0x196C3671, 0x6E6B06E7, + 0xFED41B76, 0x89D32BE0, 0x10DA7A5A, 0x67DD4ACC, 0xF9B9DF6F, 0x8EBEEFF9, 0x17B7BE43, 0x60B08ED5, + 0xD6D6A3E8, 0xA1D1937E, 0x38D8C2C4, 0x4FDFF252, 0xD1BB67F1, 0xA6BC5767, 0x3FB506DD, 0x48B2364B, + 0xD80D2BDA, 0xAF0A1B4C, 0x36034AF6, 0x41047A60, 0xDF60EFC3, 0xA867DF55, 0x316E8EEF, 0x4669BE79, + 0xCB61B38C, 0xBC66831A, 0x256FD2A0, 0x5268E236, 0xCC0C7795, 0xBB0B4703, 0x220216B9, 0x5505262F, + 0xC5BA3BBE, 0xB2BD0B28, 0x2BB45A92, 0x5CB36A04, 0xC2D7FFA7, 0xB5D0CF31, 0x2CD99E8B, 0x5BDEAE1D, + 0x9B64C2B0, 0xEC63F226, 0x756AA39C, 0x026D930A, 0x9C0906A9, 0xEB0E363F, 0x72076785, 0x05005713, + 0x95BF4A82, 0xE2B87A14, 0x7BB12BAE, 0x0CB61B38, 0x92D28E9B, 0xE5D5BE0D, 0x7CDCEFB7, 0x0BDBDF21, + 0x86D3D2D4, 0xF1D4E242, 0x68DDB3F8, 0x1FDA836E, 0x81BE16CD, 0xF6B9265B, 0x6FB077E1, 0x18B74777, + 0x88085AE6, 0xFF0F6A70, 0x66063BCA, 0x11010B5C, 0x8F659EFF, 0xF862AE69, 0x616BFFD3, 0x166CCF45, + 0xA00AE278, 0xD70DD2EE, 0x4E048354, 0x3903B3C2, 0xA7672661, 0xD06016F7, 0x4969474D, 0x3E6E77DB, + 0xAED16A4A, 0xD9D65ADC, 0x40DF0B66, 0x37D83BF0, 0xA9BCAE53, 0xDEBB9EC5, 0x47B2CF7F, 0x30B5FFE9, + 0xBDBDF21C, 0xCABAC28A, 0x53B39330, 0x24B4A3A6, 0xBAD03605, 0xCDD70693, 0x54DE5729, 0x23D967BF, + 0xB3667A2E, 0xC4614AB8, 0x5D681B02, 0x2A6F2B94, 0xB40BBE37, 0xC30C8EA1, 0x5A05DF1B, 0x2D02EF8D + }; + + unsigned int crc = ~0u; + int i; + for (i=0; i < len; ++i) + crc = (crc >> 8) ^ crc_table[buffer[i] ^ (crc & 0xff)]; + return ~crc; +#endif +} + +#define stbiw__wpng4(o,a,b,c,d) ((o)[0]=STBIW_UCHAR(a),(o)[1]=STBIW_UCHAR(b),(o)[2]=STBIW_UCHAR(c),(o)[3]=STBIW_UCHAR(d),(o)+=4) +#define stbiw__wp32(data,v) stbiw__wpng4(data, (v)>>24,(v)>>16,(v)>>8,(v)); +#define stbiw__wptag(data,s) stbiw__wpng4(data, s[0],s[1],s[2],s[3]) + +static void stbiw__wpcrc(unsigned char **data, int len) +{ + unsigned int crc = stbiw__crc32(*data - len - 4, len+4); + stbiw__wp32(*data, crc); +} + +static unsigned char stbiw__paeth(int a, int b, int c) +{ + int p = a + b - c, pa = abs(p-a), pb = abs(p-b), pc = abs(p-c); + if (pa <= pb && pa <= pc) return STBIW_UCHAR(a); + if (pb <= pc) return STBIW_UCHAR(b); + return STBIW_UCHAR(c); +} + +// @OPTIMIZE: provide an option that always forces left-predict or paeth predict +static void stbiw__encode_png_line(unsigned char *pixels, int stride_bytes, int width, int height, int y, int n, int filter_type, signed char *line_buffer) +{ + static int mapping[] = { 0,1,2,3,4 }; + static int firstmap[] = { 0,1,0,5,6 }; + int *mymap = (y != 0) ? mapping : firstmap; + int i; + int type = mymap[filter_type]; + unsigned char *z = pixels + stride_bytes * (stbi__flip_vertically_on_write ? height-1-y : y); + int signed_stride = stbi__flip_vertically_on_write ? -stride_bytes : stride_bytes; + + if (type==0) { + memcpy(line_buffer, z, width*n); + return; + } + + // first loop isn't optimized since it's just one pixel + for (i = 0; i < n; ++i) { + switch (type) { + case 1: line_buffer[i] = z[i]; break; + case 2: line_buffer[i] = z[i] - z[i-signed_stride]; break; + case 3: line_buffer[i] = z[i] - (z[i-signed_stride]>>1); break; + case 4: line_buffer[i] = (signed char) (z[i] - stbiw__paeth(0,z[i-signed_stride],0)); break; + case 5: line_buffer[i] = z[i]; break; + case 6: line_buffer[i] = z[i]; break; + } + } + switch (type) { + case 1: for (i=n; i < width*n; ++i) line_buffer[i] = z[i] - z[i-n]; break; + case 2: for (i=n; i < width*n; ++i) line_buffer[i] = z[i] - z[i-signed_stride]; break; + case 3: for (i=n; i < width*n; ++i) line_buffer[i] = z[i] - ((z[i-n] + z[i-signed_stride])>>1); break; + case 4: for (i=n; i < width*n; ++i) line_buffer[i] = z[i] - stbiw__paeth(z[i-n], z[i-signed_stride], z[i-signed_stride-n]); break; + case 5: for (i=n; i < width*n; ++i) line_buffer[i] = z[i] - (z[i-n]>>1); break; + case 6: for (i=n; i < width*n; ++i) line_buffer[i] = z[i] - stbiw__paeth(z[i-n], 0,0); break; + } +} + +STBIWDEF unsigned char *stbi_write_png_to_mem(const unsigned char *pixels, int stride_bytes, int x, int y, int n, int *out_len) +{ + int force_filter = stbi_write_force_png_filter; + int ctype[5] = { -1, 0, 4, 2, 6 }; + unsigned char sig[8] = { 137,80,78,71,13,10,26,10 }; + unsigned char *out,*o, *filt, *zlib; + signed char *line_buffer; + int j,zlen; + + if (stride_bytes == 0) + stride_bytes = x * n; + + if (force_filter >= 5) { + force_filter = -1; + } + + filt = (unsigned char *) STBIW_MALLOC((x*n+1) * y); if (!filt) return 0; + line_buffer = (signed char *) STBIW_MALLOC(x * n); if (!line_buffer) { STBIW_FREE(filt); return 0; } + for (j=0; j < y; ++j) { + int filter_type; + if (force_filter > -1) { + filter_type = force_filter; + stbiw__encode_png_line((unsigned char*)(pixels), stride_bytes, x, y, j, n, force_filter, line_buffer); + } else { // Estimate the best filter by running through all of them: + int best_filter = 0, best_filter_val = 0x7fffffff, est, i; + for (filter_type = 0; filter_type < 5; filter_type++) { + stbiw__encode_png_line((unsigned char*)(pixels), stride_bytes, x, y, j, n, filter_type, line_buffer); + + // Estimate the entropy of the line using this filter; the less, the better. + est = 0; + for (i = 0; i < x*n; ++i) { + est += abs((signed char) line_buffer[i]); + } + if (est < best_filter_val) { + best_filter_val = est; + best_filter = filter_type; + } + } + if (filter_type != best_filter) { // If the last iteration already got us the best filter, don't redo it + stbiw__encode_png_line((unsigned char*)(pixels), stride_bytes, x, y, j, n, best_filter, line_buffer); + filter_type = best_filter; + } + } + // when we get here, filter_type contains the filter type, and line_buffer contains the data + filt[j*(x*n+1)] = (unsigned char) filter_type; + STBIW_MEMMOVE(filt+j*(x*n+1)+1, line_buffer, x*n); + } + STBIW_FREE(line_buffer); + zlib = stbi_zlib_compress(filt, y*( x*n+1), &zlen, stbi_write_png_compression_level); + STBIW_FREE(filt); + if (!zlib) return 0; + + // each tag requires 12 bytes of overhead + out = (unsigned char *) STBIW_MALLOC(8 + 12+13 + 12+zlen + 12); + if (!out) return 0; + *out_len = 8 + 12+13 + 12+zlen + 12; + + o=out; + STBIW_MEMMOVE(o,sig,8); o+= 8; + stbiw__wp32(o, 13); // header length + stbiw__wptag(o, "IHDR"); + stbiw__wp32(o, x); + stbiw__wp32(o, y); + *o++ = 8; + *o++ = STBIW_UCHAR(ctype[n]); + *o++ = 0; + *o++ = 0; + *o++ = 0; + stbiw__wpcrc(&o,13); + + stbiw__wp32(o, zlen); + stbiw__wptag(o, "IDAT"); + STBIW_MEMMOVE(o, zlib, zlen); + o += zlen; + STBIW_FREE(zlib); + stbiw__wpcrc(&o, zlen); + + stbiw__wp32(o,0); + stbiw__wptag(o, "IEND"); + stbiw__wpcrc(&o,0); + + STBIW_ASSERT(o == out + *out_len); + + return out; +} + +#ifndef STBI_WRITE_NO_STDIO +STBIWDEF int stbi_write_png(char const *filename, int x, int y, int comp, const void *data, int stride_bytes) +{ + FILE *f; + int len; + unsigned char *png = stbi_write_png_to_mem((const unsigned char *) data, stride_bytes, x, y, comp, &len); + if (png == NULL) return 0; + + f = stbiw__fopen(filename, "wb"); + if (!f) { STBIW_FREE(png); return 0; } + fwrite(png, 1, len, f); + fclose(f); + STBIW_FREE(png); + return 1; +} +#endif + +STBIWDEF int stbi_write_png_to_func(stbi_write_func *func, void *context, int x, int y, int comp, const void *data, int stride_bytes) +{ + int len; + unsigned char *png = stbi_write_png_to_mem((const unsigned char *) data, stride_bytes, x, y, comp, &len); + if (png == NULL) return 0; + func(context, png, len); + STBIW_FREE(png); + return 1; +} + + +/* *************************************************************************** + * + * JPEG writer + * + * This is based on Jon Olick's jo_jpeg.cpp: + * public domain Simple, Minimalistic JPEG writer - http://www.jonolick.com/code.html + */ + +static const unsigned char stbiw__jpg_ZigZag[] = { 0,1,5,6,14,15,27,28,2,4,7,13,16,26,29,42,3,8,12,17,25,30,41,43,9,11,18, + 24,31,40,44,53,10,19,23,32,39,45,52,54,20,22,33,38,46,51,55,60,21,34,37,47,50,56,59,61,35,36,48,49,57,58,62,63 }; + +static void stbiw__jpg_writeBits(stbi__write_context *s, int *bitBufP, int *bitCntP, const unsigned short *bs) { + int bitBuf = *bitBufP, bitCnt = *bitCntP; + bitCnt += bs[1]; + bitBuf |= bs[0] << (24 - bitCnt); + while(bitCnt >= 8) { + unsigned char c = (bitBuf >> 16) & 255; + stbiw__putc(s, c); + if(c == 255) { + stbiw__putc(s, 0); + } + bitBuf <<= 8; + bitCnt -= 8; + } + *bitBufP = bitBuf; + *bitCntP = bitCnt; +} + +static void stbiw__jpg_DCT(float *d0p, float *d1p, float *d2p, float *d3p, float *d4p, float *d5p, float *d6p, float *d7p) { + float d0 = *d0p, d1 = *d1p, d2 = *d2p, d3 = *d3p, d4 = *d4p, d5 = *d5p, d6 = *d6p, d7 = *d7p; + float z1, z2, z3, z4, z5, z11, z13; + + float tmp0 = d0 + d7; + float tmp7 = d0 - d7; + float tmp1 = d1 + d6; + float tmp6 = d1 - d6; + float tmp2 = d2 + d5; + float tmp5 = d2 - d5; + float tmp3 = d3 + d4; + float tmp4 = d3 - d4; + + // Even part + float tmp10 = tmp0 + tmp3; // phase 2 + float tmp13 = tmp0 - tmp3; + float tmp11 = tmp1 + tmp2; + float tmp12 = tmp1 - tmp2; + + d0 = tmp10 + tmp11; // phase 3 + d4 = tmp10 - tmp11; + + z1 = (tmp12 + tmp13) * 0.707106781f; // c4 + d2 = tmp13 + z1; // phase 5 + d6 = tmp13 - z1; + + // Odd part + tmp10 = tmp4 + tmp5; // phase 2 + tmp11 = tmp5 + tmp6; + tmp12 = tmp6 + tmp7; + + // The rotator is modified from fig 4-8 to avoid extra negations. + z5 = (tmp10 - tmp12) * 0.382683433f; // c6 + z2 = tmp10 * 0.541196100f + z5; // c2-c6 + z4 = tmp12 * 1.306562965f + z5; // c2+c6 + z3 = tmp11 * 0.707106781f; // c4 + + z11 = tmp7 + z3; // phase 5 + z13 = tmp7 - z3; + + *d5p = z13 + z2; // phase 6 + *d3p = z13 - z2; + *d1p = z11 + z4; + *d7p = z11 - z4; + + *d0p = d0; *d2p = d2; *d4p = d4; *d6p = d6; +} + +static void stbiw__jpg_calcBits(int val, unsigned short bits[2]) { + int tmp1 = val < 0 ? -val : val; + val = val < 0 ? val-1 : val; + bits[1] = 1; + while(tmp1 >>= 1) { + ++bits[1]; + } + bits[0] = val & ((1<0)&&(DU[end0pos]==0); --end0pos) { + } + // end0pos = first element in reverse order !=0 + if(end0pos == 0) { + stbiw__jpg_writeBits(s, bitBuf, bitCnt, EOB); + return DU[0]; + } + for(i = 1; i <= end0pos; ++i) { + int startpos = i; + int nrzeroes; + unsigned short bits[2]; + for (; DU[i]==0 && i<=end0pos; ++i) { + } + nrzeroes = i-startpos; + if ( nrzeroes >= 16 ) { + int lng = nrzeroes>>4; + int nrmarker; + for (nrmarker=1; nrmarker <= lng; ++nrmarker) + stbiw__jpg_writeBits(s, bitBuf, bitCnt, M16zeroes); + nrzeroes &= 15; + } + stbiw__jpg_calcBits(DU[i], bits); + stbiw__jpg_writeBits(s, bitBuf, bitCnt, HTAC[(nrzeroes<<4)+bits[1]]); + stbiw__jpg_writeBits(s, bitBuf, bitCnt, bits); + } + if(end0pos != 63) { + stbiw__jpg_writeBits(s, bitBuf, bitCnt, EOB); + } + return DU[0]; +} + +static int stbi_write_jpg_core(stbi__write_context *s, int width, int height, int comp, const void* data, int quality) { + // Constants that don't pollute global namespace + static const unsigned char std_dc_luminance_nrcodes[] = {0,0,1,5,1,1,1,1,1,1,0,0,0,0,0,0,0}; + static const unsigned char std_dc_luminance_values[] = {0,1,2,3,4,5,6,7,8,9,10,11}; + static const unsigned char std_ac_luminance_nrcodes[] = {0,0,2,1,3,3,2,4,3,5,5,4,4,0,0,1,0x7d}; + static const unsigned char std_ac_luminance_values[] = { + 0x01,0x02,0x03,0x00,0x04,0x11,0x05,0x12,0x21,0x31,0x41,0x06,0x13,0x51,0x61,0x07,0x22,0x71,0x14,0x32,0x81,0x91,0xa1,0x08, + 0x23,0x42,0xb1,0xc1,0x15,0x52,0xd1,0xf0,0x24,0x33,0x62,0x72,0x82,0x09,0x0a,0x16,0x17,0x18,0x19,0x1a,0x25,0x26,0x27,0x28, + 0x29,0x2a,0x34,0x35,0x36,0x37,0x38,0x39,0x3a,0x43,0x44,0x45,0x46,0x47,0x48,0x49,0x4a,0x53,0x54,0x55,0x56,0x57,0x58,0x59, + 0x5a,0x63,0x64,0x65,0x66,0x67,0x68,0x69,0x6a,0x73,0x74,0x75,0x76,0x77,0x78,0x79,0x7a,0x83,0x84,0x85,0x86,0x87,0x88,0x89, + 0x8a,0x92,0x93,0x94,0x95,0x96,0x97,0x98,0x99,0x9a,0xa2,0xa3,0xa4,0xa5,0xa6,0xa7,0xa8,0xa9,0xaa,0xb2,0xb3,0xb4,0xb5,0xb6, + 0xb7,0xb8,0xb9,0xba,0xc2,0xc3,0xc4,0xc5,0xc6,0xc7,0xc8,0xc9,0xca,0xd2,0xd3,0xd4,0xd5,0xd6,0xd7,0xd8,0xd9,0xda,0xe1,0xe2, + 0xe3,0xe4,0xe5,0xe6,0xe7,0xe8,0xe9,0xea,0xf1,0xf2,0xf3,0xf4,0xf5,0xf6,0xf7,0xf8,0xf9,0xfa + }; + static const unsigned char std_dc_chrominance_nrcodes[] = {0,0,3,1,1,1,1,1,1,1,1,1,0,0,0,0,0}; + static const unsigned char std_dc_chrominance_values[] = {0,1,2,3,4,5,6,7,8,9,10,11}; + static const unsigned char std_ac_chrominance_nrcodes[] = {0,0,2,1,2,4,4,3,4,7,5,4,4,0,1,2,0x77}; + static const unsigned char std_ac_chrominance_values[] = { + 0x00,0x01,0x02,0x03,0x11,0x04,0x05,0x21,0x31,0x06,0x12,0x41,0x51,0x07,0x61,0x71,0x13,0x22,0x32,0x81,0x08,0x14,0x42,0x91, + 0xa1,0xb1,0xc1,0x09,0x23,0x33,0x52,0xf0,0x15,0x62,0x72,0xd1,0x0a,0x16,0x24,0x34,0xe1,0x25,0xf1,0x17,0x18,0x19,0x1a,0x26, + 0x27,0x28,0x29,0x2a,0x35,0x36,0x37,0x38,0x39,0x3a,0x43,0x44,0x45,0x46,0x47,0x48,0x49,0x4a,0x53,0x54,0x55,0x56,0x57,0x58, + 0x59,0x5a,0x63,0x64,0x65,0x66,0x67,0x68,0x69,0x6a,0x73,0x74,0x75,0x76,0x77,0x78,0x79,0x7a,0x82,0x83,0x84,0x85,0x86,0x87, + 0x88,0x89,0x8a,0x92,0x93,0x94,0x95,0x96,0x97,0x98,0x99,0x9a,0xa2,0xa3,0xa4,0xa5,0xa6,0xa7,0xa8,0xa9,0xaa,0xb2,0xb3,0xb4, + 0xb5,0xb6,0xb7,0xb8,0xb9,0xba,0xc2,0xc3,0xc4,0xc5,0xc6,0xc7,0xc8,0xc9,0xca,0xd2,0xd3,0xd4,0xd5,0xd6,0xd7,0xd8,0xd9,0xda, + 0xe2,0xe3,0xe4,0xe5,0xe6,0xe7,0xe8,0xe9,0xea,0xf2,0xf3,0xf4,0xf5,0xf6,0xf7,0xf8,0xf9,0xfa + }; + // Huffman tables + static const unsigned short YDC_HT[256][2] = { {0,2},{2,3},{3,3},{4,3},{5,3},{6,3},{14,4},{30,5},{62,6},{126,7},{254,8},{510,9}}; + static const unsigned short UVDC_HT[256][2] = { {0,2},{1,2},{2,2},{6,3},{14,4},{30,5},{62,6},{126,7},{254,8},{510,9},{1022,10},{2046,11}}; + static const unsigned short YAC_HT[256][2] = { + {10,4},{0,2},{1,2},{4,3},{11,4},{26,5},{120,7},{248,8},{1014,10},{65410,16},{65411,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {12,4},{27,5},{121,7},{502,9},{2038,11},{65412,16},{65413,16},{65414,16},{65415,16},{65416,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {28,5},{249,8},{1015,10},{4084,12},{65417,16},{65418,16},{65419,16},{65420,16},{65421,16},{65422,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {58,6},{503,9},{4085,12},{65423,16},{65424,16},{65425,16},{65426,16},{65427,16},{65428,16},{65429,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {59,6},{1016,10},{65430,16},{65431,16},{65432,16},{65433,16},{65434,16},{65435,16},{65436,16},{65437,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {122,7},{2039,11},{65438,16},{65439,16},{65440,16},{65441,16},{65442,16},{65443,16},{65444,16},{65445,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {123,7},{4086,12},{65446,16},{65447,16},{65448,16},{65449,16},{65450,16},{65451,16},{65452,16},{65453,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {250,8},{4087,12},{65454,16},{65455,16},{65456,16},{65457,16},{65458,16},{65459,16},{65460,16},{65461,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {504,9},{32704,15},{65462,16},{65463,16},{65464,16},{65465,16},{65466,16},{65467,16},{65468,16},{65469,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {505,9},{65470,16},{65471,16},{65472,16},{65473,16},{65474,16},{65475,16},{65476,16},{65477,16},{65478,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {506,9},{65479,16},{65480,16},{65481,16},{65482,16},{65483,16},{65484,16},{65485,16},{65486,16},{65487,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {1017,10},{65488,16},{65489,16},{65490,16},{65491,16},{65492,16},{65493,16},{65494,16},{65495,16},{65496,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {1018,10},{65497,16},{65498,16},{65499,16},{65500,16},{65501,16},{65502,16},{65503,16},{65504,16},{65505,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {2040,11},{65506,16},{65507,16},{65508,16},{65509,16},{65510,16},{65511,16},{65512,16},{65513,16},{65514,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {65515,16},{65516,16},{65517,16},{65518,16},{65519,16},{65520,16},{65521,16},{65522,16},{65523,16},{65524,16},{0,0},{0,0},{0,0},{0,0},{0,0}, + {2041,11},{65525,16},{65526,16},{65527,16},{65528,16},{65529,16},{65530,16},{65531,16},{65532,16},{65533,16},{65534,16},{0,0},{0,0},{0,0},{0,0},{0,0} + }; + static const unsigned short UVAC_HT[256][2] = { + {0,2},{1,2},{4,3},{10,4},{24,5},{25,5},{56,6},{120,7},{500,9},{1014,10},{4084,12},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {11,4},{57,6},{246,8},{501,9},{2038,11},{4085,12},{65416,16},{65417,16},{65418,16},{65419,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {26,5},{247,8},{1015,10},{4086,12},{32706,15},{65420,16},{65421,16},{65422,16},{65423,16},{65424,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {27,5},{248,8},{1016,10},{4087,12},{65425,16},{65426,16},{65427,16},{65428,16},{65429,16},{65430,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {58,6},{502,9},{65431,16},{65432,16},{65433,16},{65434,16},{65435,16},{65436,16},{65437,16},{65438,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {59,6},{1017,10},{65439,16},{65440,16},{65441,16},{65442,16},{65443,16},{65444,16},{65445,16},{65446,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {121,7},{2039,11},{65447,16},{65448,16},{65449,16},{65450,16},{65451,16},{65452,16},{65453,16},{65454,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {122,7},{2040,11},{65455,16},{65456,16},{65457,16},{65458,16},{65459,16},{65460,16},{65461,16},{65462,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {249,8},{65463,16},{65464,16},{65465,16},{65466,16},{65467,16},{65468,16},{65469,16},{65470,16},{65471,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {503,9},{65472,16},{65473,16},{65474,16},{65475,16},{65476,16},{65477,16},{65478,16},{65479,16},{65480,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {504,9},{65481,16},{65482,16},{65483,16},{65484,16},{65485,16},{65486,16},{65487,16},{65488,16},{65489,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {505,9},{65490,16},{65491,16},{65492,16},{65493,16},{65494,16},{65495,16},{65496,16},{65497,16},{65498,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {506,9},{65499,16},{65500,16},{65501,16},{65502,16},{65503,16},{65504,16},{65505,16},{65506,16},{65507,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {2041,11},{65508,16},{65509,16},{65510,16},{65511,16},{65512,16},{65513,16},{65514,16},{65515,16},{65516,16},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}, + {16352,14},{65517,16},{65518,16},{65519,16},{65520,16},{65521,16},{65522,16},{65523,16},{65524,16},{65525,16},{0,0},{0,0},{0,0},{0,0},{0,0}, + {1018,10},{32707,15},{65526,16},{65527,16},{65528,16},{65529,16},{65530,16},{65531,16},{65532,16},{65533,16},{65534,16},{0,0},{0,0},{0,0},{0,0},{0,0} + }; + static const int YQT[] = {16,11,10,16,24,40,51,61,12,12,14,19,26,58,60,55,14,13,16,24,40,57,69,56,14,17,22,29,51,87,80,62,18,22, + 37,56,68,109,103,77,24,35,55,64,81,104,113,92,49,64,78,87,103,121,120,101,72,92,95,98,112,100,103,99}; + static const int UVQT[] = {17,18,24,47,99,99,99,99,18,21,26,66,99,99,99,99,24,26,56,99,99,99,99,99,47,66,99,99,99,99,99,99, + 99,99,99,99,99,99,99,99,99,99,99,99,99,99,99,99,99,99,99,99,99,99,99,99,99,99,99,99,99,99,99,99}; + static const float aasf[] = { 1.0f * 2.828427125f, 1.387039845f * 2.828427125f, 1.306562965f * 2.828427125f, 1.175875602f * 2.828427125f, + 1.0f * 2.828427125f, 0.785694958f * 2.828427125f, 0.541196100f * 2.828427125f, 0.275899379f * 2.828427125f }; + + int row, col, i, k, subsample; + float fdtbl_Y[64], fdtbl_UV[64]; + unsigned char YTable[64], UVTable[64]; + + if(!data || !width || !height || comp > 4 || comp < 1) { + return 0; + } + + quality = quality ? quality : 90; + subsample = quality <= 90 ? 1 : 0; + quality = quality < 1 ? 1 : quality > 100 ? 100 : quality; + quality = quality < 50 ? 5000 / quality : 200 - quality * 2; + + for(i = 0; i < 64; ++i) { + int uvti, yti = (YQT[i]*quality+50)/100; + YTable[stbiw__jpg_ZigZag[i]] = (unsigned char) (yti < 1 ? 1 : yti > 255 ? 255 : yti); + uvti = (UVQT[i]*quality+50)/100; + UVTable[stbiw__jpg_ZigZag[i]] = (unsigned char) (uvti < 1 ? 1 : uvti > 255 ? 255 : uvti); + } + + for(row = 0, k = 0; row < 8; ++row) { + for(col = 0; col < 8; ++col, ++k) { + fdtbl_Y[k] = 1 / (YTable [stbiw__jpg_ZigZag[k]] * aasf[row] * aasf[col]); + fdtbl_UV[k] = 1 / (UVTable[stbiw__jpg_ZigZag[k]] * aasf[row] * aasf[col]); + } + } + + // Write Headers + { + static const unsigned char head0[] = { 0xFF,0xD8,0xFF,0xE0,0,0x10,'J','F','I','F',0,1,1,0,0,1,0,1,0,0,0xFF,0xDB,0,0x84,0 }; + static const unsigned char head2[] = { 0xFF,0xDA,0,0xC,3,1,0,2,0x11,3,0x11,0,0x3F,0 }; + const unsigned char head1[] = { 0xFF,0xC0,0,0x11,8,(unsigned char)(height>>8),STBIW_UCHAR(height),(unsigned char)(width>>8),STBIW_UCHAR(width), + 3,1,(unsigned char)(subsample?0x22:0x11),0,2,0x11,1,3,0x11,1,0xFF,0xC4,0x01,0xA2,0 }; + s->func(s->context, (void*)head0, sizeof(head0)); + s->func(s->context, (void*)YTable, sizeof(YTable)); + stbiw__putc(s, 1); + s->func(s->context, UVTable, sizeof(UVTable)); + s->func(s->context, (void*)head1, sizeof(head1)); + s->func(s->context, (void*)(std_dc_luminance_nrcodes+1), sizeof(std_dc_luminance_nrcodes)-1); + s->func(s->context, (void*)std_dc_luminance_values, sizeof(std_dc_luminance_values)); + stbiw__putc(s, 0x10); // HTYACinfo + s->func(s->context, (void*)(std_ac_luminance_nrcodes+1), sizeof(std_ac_luminance_nrcodes)-1); + s->func(s->context, (void*)std_ac_luminance_values, sizeof(std_ac_luminance_values)); + stbiw__putc(s, 1); // HTUDCinfo + s->func(s->context, (void*)(std_dc_chrominance_nrcodes+1), sizeof(std_dc_chrominance_nrcodes)-1); + s->func(s->context, (void*)std_dc_chrominance_values, sizeof(std_dc_chrominance_values)); + stbiw__putc(s, 0x11); // HTUACinfo + s->func(s->context, (void*)(std_ac_chrominance_nrcodes+1), sizeof(std_ac_chrominance_nrcodes)-1); + s->func(s->context, (void*)std_ac_chrominance_values, sizeof(std_ac_chrominance_values)); + s->func(s->context, (void*)head2, sizeof(head2)); + } + + // Encode 8x8 macroblocks + { + static const unsigned short fillBits[] = {0x7F, 7}; + int DCY=0, DCU=0, DCV=0; + int bitBuf=0, bitCnt=0; + // comp == 2 is grey+alpha (alpha is ignored) + int ofsG = comp > 2 ? 1 : 0, ofsB = comp > 2 ? 2 : 0; + const unsigned char *dataR = (const unsigned char *)data; + const unsigned char *dataG = dataR + ofsG; + const unsigned char *dataB = dataR + ofsB; + int x, y, pos; + if(subsample) { + for(y = 0; y < height; y += 16) { + for(x = 0; x < width; x += 16) { + float Y[256], U[256], V[256]; + for(row = y, pos = 0; row < y+16; ++row) { + // row >= height => use last input row + int clamped_row = (row < height) ? row : height - 1; + int base_p = (stbi__flip_vertically_on_write ? (height-1-clamped_row) : clamped_row)*width*comp; + for(col = x; col < x+16; ++col, ++pos) { + // if col >= width => use pixel from last input column + int p = base_p + ((col < width) ? col : (width-1))*comp; + float r = dataR[p], g = dataG[p], b = dataB[p]; + Y[pos]= +0.29900f*r + 0.58700f*g + 0.11400f*b - 128; + U[pos]= -0.16874f*r - 0.33126f*g + 0.50000f*b; + V[pos]= +0.50000f*r - 0.41869f*g - 0.08131f*b; + } + } + DCY = stbiw__jpg_processDU(s, &bitBuf, &bitCnt, Y+0, 16, fdtbl_Y, DCY, YDC_HT, YAC_HT); + DCY = stbiw__jpg_processDU(s, &bitBuf, &bitCnt, Y+8, 16, fdtbl_Y, DCY, YDC_HT, YAC_HT); + DCY = stbiw__jpg_processDU(s, &bitBuf, &bitCnt, Y+128, 16, fdtbl_Y, DCY, YDC_HT, YAC_HT); + DCY = stbiw__jpg_processDU(s, &bitBuf, &bitCnt, Y+136, 16, fdtbl_Y, DCY, YDC_HT, YAC_HT); + + // subsample U,V + { + float subU[64], subV[64]; + int yy, xx; + for(yy = 0, pos = 0; yy < 8; ++yy) { + for(xx = 0; xx < 8; ++xx, ++pos) { + int j = yy*32+xx*2; + subU[pos] = (U[j+0] + U[j+1] + U[j+16] + U[j+17]) * 0.25f; + subV[pos] = (V[j+0] + V[j+1] + V[j+16] + V[j+17]) * 0.25f; + } + } + DCU = stbiw__jpg_processDU(s, &bitBuf, &bitCnt, subU, 8, fdtbl_UV, DCU, UVDC_HT, UVAC_HT); + DCV = stbiw__jpg_processDU(s, &bitBuf, &bitCnt, subV, 8, fdtbl_UV, DCV, UVDC_HT, UVAC_HT); + } + } + } + } else { + for(y = 0; y < height; y += 8) { + for(x = 0; x < width; x += 8) { + float Y[64], U[64], V[64]; + for(row = y, pos = 0; row < y+8; ++row) { + // row >= height => use last input row + int clamped_row = (row < height) ? row : height - 1; + int base_p = (stbi__flip_vertically_on_write ? (height-1-clamped_row) : clamped_row)*width*comp; + for(col = x; col < x+8; ++col, ++pos) { + // if col >= width => use pixel from last input column + int p = base_p + ((col < width) ? col : (width-1))*comp; + float r = dataR[p], g = dataG[p], b = dataB[p]; + Y[pos]= +0.29900f*r + 0.58700f*g + 0.11400f*b - 128; + U[pos]= -0.16874f*r - 0.33126f*g + 0.50000f*b; + V[pos]= +0.50000f*r - 0.41869f*g - 0.08131f*b; + } + } + + DCY = stbiw__jpg_processDU(s, &bitBuf, &bitCnt, Y, 8, fdtbl_Y, DCY, YDC_HT, YAC_HT); + DCU = stbiw__jpg_processDU(s, &bitBuf, &bitCnt, U, 8, fdtbl_UV, DCU, UVDC_HT, UVAC_HT); + DCV = stbiw__jpg_processDU(s, &bitBuf, &bitCnt, V, 8, fdtbl_UV, DCV, UVDC_HT, UVAC_HT); + } + } + } + + // Do the bit alignment of the EOI marker + stbiw__jpg_writeBits(s, &bitBuf, &bitCnt, fillBits); + } + + // EOI + stbiw__putc(s, 0xFF); + stbiw__putc(s, 0xD9); + + return 1; +} + +STBIWDEF int stbi_write_jpg_to_func(stbi_write_func *func, void *context, int x, int y, int comp, const void *data, int quality) +{ + stbi__write_context s = { 0 }; + stbi__start_write_callbacks(&s, func, context); + return stbi_write_jpg_core(&s, x, y, comp, (void *) data, quality); +} + + +#ifndef STBI_WRITE_NO_STDIO +STBIWDEF int stbi_write_jpg(char const *filename, int x, int y, int comp, const void *data, int quality) +{ + stbi__write_context s = { 0 }; + if (stbi__start_write_file(&s,filename)) { + int r = stbi_write_jpg_core(&s, x, y, comp, data, quality); + stbi__end_write_file(&s); + return r; + } else + return 0; +} +#endif + +#endif // STB_IMAGE_WRITE_IMPLEMENTATION + +/* Revision history + 1.16 (2021-07-11) + make Deflate code emit uncompressed blocks when it would otherwise expand + support writing BMPs with alpha channel + 1.15 (2020-07-13) unknown + 1.14 (2020-02-02) updated JPEG writer to downsample chroma channels + 1.13 + 1.12 + 1.11 (2019-08-11) + + 1.10 (2019-02-07) + support utf8 filenames in Windows; fix warnings and platform ifdefs + 1.09 (2018-02-11) + fix typo in zlib quality API, improve STB_I_W_STATIC in C++ + 1.08 (2018-01-29) + add stbi__flip_vertically_on_write, external zlib, zlib quality, choose PNG filter + 1.07 (2017-07-24) + doc fix + 1.06 (2017-07-23) + writing JPEG (using Jon Olick's code) + 1.05 ??? + 1.04 (2017-03-03) + monochrome BMP expansion + 1.03 ??? + 1.02 (2016-04-02) + avoid allocating large structures on the stack + 1.01 (2016-01-16) + STBIW_REALLOC_SIZED: support allocators with no realloc support + avoid race-condition in crc initialization + minor compile issues + 1.00 (2015-09-14) + installable file IO function + 0.99 (2015-09-13) + warning fixes; TGA rle support + 0.98 (2015-04-08) + added STBIW_MALLOC, STBIW_ASSERT etc + 0.97 (2015-01-18) + fixed HDR asserts, rewrote HDR rle logic + 0.96 (2015-01-17) + add HDR output + fix monochrome BMP + 0.95 (2014-08-17) + add monochrome TGA output + 0.94 (2014-05-31) + rename private functions to avoid conflicts with stb_image.h + 0.93 (2014-05-27) + warning fixes + 0.92 (2010-08-01) + casts to unsigned char to fix warnings + 0.91 (2010-07-17) + first public release + 0.90 first internal release +*/ + +/* +------------------------------------------------------------------------------ +This software is available under 2 licenses -- choose whichever you prefer. +------------------------------------------------------------------------------ +ALTERNATIVE A - MIT License +Copyright (c) 2017 Sean Barrett +Permission is hereby granted, free of charge, to any person obtaining a copy of +this software and associated documentation files (the "Software"), to deal in +the Software without restriction, including without limitation the rights to +use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies +of the Software, and to permit persons to whom the Software is furnished to do +so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +------------------------------------------------------------------------------ +ALTERNATIVE B - Public Domain (www.unlicense.org) +This is free and unencumbered software released into the public domain. +Anyone is free to copy, modify, publish, use, compile, sell, or distribute this +software, either in source code form or as a compiled binary, for any purpose, +commercial or non-commercial, and by any means. +In jurisdictions that recognize copyright laws, the author or authors of this +software dedicate any and all copyright interest in the software to the public +domain. We make this dedication for the benefit of the public at large and to +the detriment of our heirs and successors. We intend this dedication to be an +overt act of relinquishment in perpetuity of all present and future rights to +this software under copyright law. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN +ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION +WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +------------------------------------------------------------------------------ +*/ diff --git a/samples/stb_truetype.h b/samples/stb_truetype.h new file mode 100644 index 0000000..90a5c2e --- /dev/null +++ b/samples/stb_truetype.h @@ -0,0 +1,5079 @@ +// stb_truetype.h - v1.26 - public domain +// authored from 2009-2021 by Sean Barrett / RAD Game Tools +// +// ======================================================================= +// +// NO SECURITY GUARANTEE -- DO NOT USE THIS ON UNTRUSTED FONT FILES +// +// This library does no range checking of the offsets found in the file, +// meaning an attacker can use it to read arbitrary memory. +// +// ======================================================================= +// +// This library processes TrueType files: +// parse files +// extract glyph metrics +// extract glyph shapes +// render glyphs to one-channel bitmaps with antialiasing (box filter) +// render glyphs to one-channel SDF bitmaps (signed-distance field/function) +// +// Todo: +// non-MS cmaps +// crashproof on bad data +// hinting? (no longer patented) +// cleartype-style AA? +// optimize: use simple memory allocator for intermediates +// optimize: build edge-list directly from curves +// optimize: rasterize directly from curves? +// +// ADDITIONAL CONTRIBUTORS +// +// Mikko Mononen: compound shape support, more cmap formats +// Tor Andersson: kerning, subpixel rendering +// Dougall Johnson: OpenType / Type 2 font handling +// Daniel Ribeiro Maciel: basic GPOS-based kerning +// +// Misc other: +// Ryan Gordon +// Simon Glass +// github:IntellectualKitty +// Imanol Celaya +// Daniel Ribeiro Maciel +// +// Bug/warning reports/fixes: +// "Zer" on mollyrocket Fabian "ryg" Giesen github:NiLuJe +// Cass Everitt Martins Mozeiko github:aloucks +// stoiko (Haemimont Games) Cap Petschulat github:oyvindjam +// Brian Hook Omar Cornut github:vassvik +// Walter van Niftrik Ryan Griege +// David Gow Peter LaValle +// David Given Sergey Popov +// Ivan-Assen Ivanov Giumo X. Clanjor +// Anthony Pesch Higor Euripedes +// Johan Duparc Thomas Fields +// Hou Qiming Derek Vinyard +// Rob Loach Cort Stratton +// Kenney Phillis Jr. Brian Costabile +// Ken Voskuil (kaesve) Yakov Galka +// +// VERSION HISTORY +// +// 1.26 (2021-08-28) fix broken rasterizer +// 1.25 (2021-07-11) many fixes +// 1.24 (2020-02-05) fix warning +// 1.23 (2020-02-02) query SVG data for glyphs; query whole kerning table (but only kern not GPOS) +// 1.22 (2019-08-11) minimize missing-glyph duplication; fix kerning if both 'GPOS' and 'kern' are defined +// 1.21 (2019-02-25) fix warning +// 1.20 (2019-02-07) PackFontRange skips missing codepoints; GetScaleFontVMetrics() +// 1.19 (2018-02-11) GPOS kerning, STBTT_fmod +// 1.18 (2018-01-29) add missing function +// 1.17 (2017-07-23) make more arguments const; doc fix +// 1.16 (2017-07-12) SDF support +// 1.15 (2017-03-03) make more arguments const +// 1.14 (2017-01-16) num-fonts-in-TTC function +// 1.13 (2017-01-02) support OpenType fonts, certain Apple fonts +// 1.12 (2016-10-25) suppress warnings about casting away const with -Wcast-qual +// 1.11 (2016-04-02) fix unused-variable warning +// 1.10 (2016-04-02) user-defined fabs(); rare memory leak; remove duplicate typedef +// 1.09 (2016-01-16) warning fix; avoid crash on outofmem; use allocation userdata properly +// 1.08 (2015-09-13) document stbtt_Rasterize(); fixes for vertical & horizontal edges +// 1.07 (2015-08-01) allow PackFontRanges to accept arrays of sparse codepoints; +// variant PackFontRanges to pack and render in separate phases; +// fix stbtt_GetFontOFfsetForIndex (never worked for non-0 input?); +// fixed an assert() bug in the new rasterizer +// replace assert() with STBTT_assert() in new rasterizer +// +// Full history can be found at the end of this file. +// +// LICENSE +// +// See end of file for license information. +// +// USAGE +// +// Include this file in whatever places need to refer to it. In ONE C/C++ +// file, write: +// #define STB_TRUETYPE_IMPLEMENTATION +// before the #include of this file. This expands out the actual +// implementation into that C/C++ file. +// +// To make the implementation private to the file that generates the implementation, +// #define STBTT_STATIC +// +// Simple 3D API (don't ship this, but it's fine for tools and quick start) +// stbtt_BakeFontBitmap() -- bake a font to a bitmap for use as texture +// stbtt_GetBakedQuad() -- compute quad to draw for a given char +// +// Improved 3D API (more shippable): +// #include "stb_rect_pack.h" -- optional, but you really want it +// stbtt_PackBegin() +// stbtt_PackSetOversampling() -- for improved quality on small fonts +// stbtt_PackFontRanges() -- pack and renders +// stbtt_PackEnd() +// stbtt_GetPackedQuad() +// +// "Load" a font file from a memory buffer (you have to keep the buffer loaded) +// stbtt_InitFont() +// stbtt_GetFontOffsetForIndex() -- indexing for TTC font collections +// stbtt_GetNumberOfFonts() -- number of fonts for TTC font collections +// +// Render a unicode codepoint to a bitmap +// stbtt_GetCodepointBitmap() -- allocates and returns a bitmap +// stbtt_MakeCodepointBitmap() -- renders into bitmap you provide +// stbtt_GetCodepointBitmapBox() -- how big the bitmap must be +// +// Character advance/positioning +// stbtt_GetCodepointHMetrics() +// stbtt_GetFontVMetrics() +// stbtt_GetFontVMetricsOS2() +// stbtt_GetCodepointKernAdvance() +// +// Starting with version 1.06, the rasterizer was replaced with a new, +// faster and generally-more-precise rasterizer. The new rasterizer more +// accurately measures pixel coverage for anti-aliasing, except in the case +// where multiple shapes overlap, in which case it overestimates the AA pixel +// coverage. Thus, anti-aliasing of intersecting shapes may look wrong. If +// this turns out to be a problem, you can re-enable the old rasterizer with +// #define STBTT_RASTERIZER_VERSION 1 +// which will incur about a 15% speed hit. +// +// ADDITIONAL DOCUMENTATION +// +// Immediately after this block comment are a series of sample programs. +// +// After the sample programs is the "header file" section. This section +// includes documentation for each API function. +// +// Some important concepts to understand to use this library: +// +// Codepoint +// Characters are defined by unicode codepoints, e.g. 65 is +// uppercase A, 231 is lowercase c with a cedilla, 0x7e30 is +// the hiragana for "ma". +// +// Glyph +// A visual character shape (every codepoint is rendered as +// some glyph) +// +// Glyph index +// A font-specific integer ID representing a glyph +// +// Baseline +// Glyph shapes are defined relative to a baseline, which is the +// bottom of uppercase characters. Characters extend both above +// and below the baseline. +// +// Current Point +// As you draw text to the screen, you keep track of a "current point" +// which is the origin of each character. The current point's vertical +// position is the baseline. Even "baked fonts" use this model. +// +// Vertical Font Metrics +// The vertical qualities of the font, used to vertically position +// and space the characters. See docs for stbtt_GetFontVMetrics. +// +// Font Size in Pixels or Points +// The preferred interface for specifying font sizes in stb_truetype +// is to specify how tall the font's vertical extent should be in pixels. +// If that sounds good enough, skip the next paragraph. +// +// Most font APIs instead use "points", which are a common typographic +// measurement for describing font size, defined as 72 points per inch. +// stb_truetype provides a point API for compatibility. However, true +// "per inch" conventions don't make much sense on computer displays +// since different monitors have different number of pixels per +// inch. For example, Windows traditionally uses a convention that +// there are 96 pixels per inch, thus making 'inch' measurements have +// nothing to do with inches, and thus effectively defining a point to +// be 1.333 pixels. Additionally, the TrueType font data provides +// an explicit scale factor to scale a given font's glyphs to points, +// but the author has observed that this scale factor is often wrong +// for non-commercial fonts, thus making fonts scaled in points +// according to the TrueType spec incoherently sized in practice. +// +// DETAILED USAGE: +// +// Scale: +// Select how high you want the font to be, in points or pixels. +// Call ScaleForPixelHeight or ScaleForMappingEmToPixels to compute +// a scale factor SF that will be used by all other functions. +// +// Baseline: +// You need to select a y-coordinate that is the baseline of where +// your text will appear. Call GetFontBoundingBox to get the baseline-relative +// bounding box for all characters. SF*-y0 will be the distance in pixels +// that the worst-case character could extend above the baseline, so if +// you want the top edge of characters to appear at the top of the +// screen where y=0, then you would set the baseline to SF*-y0. +// +// Current point: +// Set the current point where the first character will appear. The +// first character could extend left of the current point; this is font +// dependent. You can either choose a current point that is the leftmost +// point and hope, or add some padding, or check the bounding box or +// left-side-bearing of the first character to be displayed and set +// the current point based on that. +// +// Displaying a character: +// Compute the bounding box of the character. It will contain signed values +// relative to . I.e. if it returns x0,y0,x1,y1, +// then the character should be displayed in the rectangle from +// to = 32 && *text < 128) { + stbtt_aligned_quad q; + stbtt_GetBakedQuad(cdata, 512,512, *text-32, &x,&y,&q,1);//1=opengl & d3d10+,0=d3d9 + glTexCoord2f(q.s0,q.t0); glVertex2f(q.x0,q.y0); + glTexCoord2f(q.s1,q.t0); glVertex2f(q.x1,q.y0); + glTexCoord2f(q.s1,q.t1); glVertex2f(q.x1,q.y1); + glTexCoord2f(q.s0,q.t1); glVertex2f(q.x0,q.y1); + } + ++text; + } + glEnd(); +} +#endif +// +// +////////////////////////////////////////////////////////////////////////////// +// +// Complete program (this compiles): get a single bitmap, print as ASCII art +// +#if 0 +#include +#define STB_TRUETYPE_IMPLEMENTATION // force following include to generate implementation +#include "stb_truetype.h" + +char ttf_buffer[1<<25]; + +int main(int argc, char **argv) +{ + stbtt_fontinfo font; + unsigned char *bitmap; + int w,h,i,j,c = (argc > 1 ? atoi(argv[1]) : 'a'), s = (argc > 2 ? atoi(argv[2]) : 20); + + fread(ttf_buffer, 1, 1<<25, fopen(argc > 3 ? argv[3] : "c:/windows/fonts/arialbd.ttf", "rb")); + + stbtt_InitFont(&font, ttf_buffer, stbtt_GetFontOffsetForIndex(ttf_buffer,0)); + bitmap = stbtt_GetCodepointBitmap(&font, 0,stbtt_ScaleForPixelHeight(&font, s), c, &w, &h, 0,0); + + for (j=0; j < h; ++j) { + for (i=0; i < w; ++i) + putchar(" .:ioVM@"[bitmap[j*w+i]>>5]); + putchar('\n'); + } + return 0; +} +#endif +// +// Output: +// +// .ii. +// @@@@@@. +// V@Mio@@o +// :i. V@V +// :oM@@M +// :@@@MM@M +// @@o o@M +// :@@. M@M +// @@@o@@@@ +// :M@@V:@@. +// +////////////////////////////////////////////////////////////////////////////// +// +// Complete program: print "Hello World!" banner, with bugs +// +#if 0 +char buffer[24<<20]; +unsigned char screen[20][79]; + +int main(int arg, char **argv) +{ + stbtt_fontinfo font; + int i,j,ascent,baseline,ch=0; + float scale, xpos=2; // leave a little padding in case the character extends left + char *text = "Heljo World!"; // intentionally misspelled to show 'lj' brokenness + + fread(buffer, 1, 1000000, fopen("c:/windows/fonts/arialbd.ttf", "rb")); + stbtt_InitFont(&font, buffer, 0); + + scale = stbtt_ScaleForPixelHeight(&font, 15); + stbtt_GetFontVMetrics(&font, &ascent,0,0); + baseline = (int) (ascent*scale); + + while (text[ch]) { + int advance,lsb,x0,y0,x1,y1; + float x_shift = xpos - (float) floor(xpos); + stbtt_GetCodepointHMetrics(&font, text[ch], &advance, &lsb); + stbtt_GetCodepointBitmapBoxSubpixel(&font, text[ch], scale,scale,x_shift,0, &x0,&y0,&x1,&y1); + stbtt_MakeCodepointBitmapSubpixel(&font, &screen[baseline + y0][(int) xpos + x0], x1-x0,y1-y0, 79, scale,scale,x_shift,0, text[ch]); + // note that this stomps the old data, so where character boxes overlap (e.g. 'lj') it's wrong + // because this API is really for baking character bitmaps into textures. if you want to render + // a sequence of characters, you really need to render each bitmap to a temp buffer, then + // "alpha blend" that into the working buffer + xpos += (advance * scale); + if (text[ch+1]) + xpos += scale*stbtt_GetCodepointKernAdvance(&font, text[ch],text[ch+1]); + ++ch; + } + + for (j=0; j < 20; ++j) { + for (i=0; i < 78; ++i) + putchar(" .:ioVM@"[screen[j][i]>>5]); + putchar('\n'); + } + + return 0; +} +#endif + + +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +//// +//// INTEGRATION WITH YOUR CODEBASE +//// +//// The following sections allow you to supply alternate definitions +//// of C library functions used by stb_truetype, e.g. if you don't +//// link with the C runtime library. + +#ifdef STB_TRUETYPE_IMPLEMENTATION + // #define your own (u)stbtt_int8/16/32 before including to override this + #ifndef stbtt_uint8 + typedef unsigned char stbtt_uint8; + typedef signed char stbtt_int8; + typedef unsigned short stbtt_uint16; + typedef signed short stbtt_int16; + typedef unsigned int stbtt_uint32; + typedef signed int stbtt_int32; + #endif + + typedef char stbtt__check_size32[sizeof(stbtt_int32)==4 ? 1 : -1]; + typedef char stbtt__check_size16[sizeof(stbtt_int16)==2 ? 1 : -1]; + + // e.g. #define your own STBTT_ifloor/STBTT_iceil() to avoid math.h + #ifndef STBTT_ifloor + #include + #define STBTT_ifloor(x) ((int) floor(x)) + #define STBTT_iceil(x) ((int) ceil(x)) + #endif + + #ifndef STBTT_sqrt + #include + #define STBTT_sqrt(x) sqrt(x) + #define STBTT_pow(x,y) pow(x,y) + #endif + + #ifndef STBTT_fmod + #include + #define STBTT_fmod(x,y) fmod(x,y) + #endif + + #ifndef STBTT_cos + #include + #define STBTT_cos(x) cos(x) + #define STBTT_acos(x) acos(x) + #endif + + #ifndef STBTT_fabs + #include + #define STBTT_fabs(x) fabs(x) + #endif + + // #define your own functions "STBTT_malloc" / "STBTT_free" to avoid malloc.h + #ifndef STBTT_malloc + #include + #define STBTT_malloc(x,u) ((void)(u),malloc(x)) + #define STBTT_free(x,u) ((void)(u),free(x)) + #endif + + #ifndef STBTT_assert + #include + #define STBTT_assert(x) assert(x) + #endif + + #ifndef STBTT_strlen + #include + #define STBTT_strlen(x) strlen(x) + #endif + + #ifndef STBTT_memcpy + #include + #define STBTT_memcpy memcpy + #define STBTT_memset memset + #endif +#endif + +/////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////// +//// +//// INTERFACE +//// +//// + +#ifndef __STB_INCLUDE_STB_TRUETYPE_H__ +#define __STB_INCLUDE_STB_TRUETYPE_H__ + +#ifdef STBTT_STATIC +#define STBTT_DEF static +#else +#define STBTT_DEF extern +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +// private structure +typedef struct +{ + unsigned char *data; + int cursor; + int size; +} stbtt__buf; + +////////////////////////////////////////////////////////////////////////////// +// +// TEXTURE BAKING API +// +// If you use this API, you only have to call two functions ever. +// + +typedef struct +{ + unsigned short x0,y0,x1,y1; // coordinates of bbox in bitmap + float xoff,yoff,xadvance; +} stbtt_bakedchar; + +STBTT_DEF int stbtt_BakeFontBitmap(const unsigned char *data, int offset, // font location (use offset=0 for plain .ttf) + float pixel_height, // height of font in pixels + unsigned char *pixels, int pw, int ph, // bitmap to be filled in + int first_char, int num_chars, // characters to bake + stbtt_bakedchar *chardata); // you allocate this, it's num_chars long +// if return is positive, the first unused row of the bitmap +// if return is negative, returns the negative of the number of characters that fit +// if return is 0, no characters fit and no rows were used +// This uses a very crappy packing. + +typedef struct +{ + float x0,y0,s0,t0; // top-left + float x1,y1,s1,t1; // bottom-right +} stbtt_aligned_quad; + +STBTT_DEF void stbtt_GetBakedQuad(const stbtt_bakedchar *chardata, int pw, int ph, // same data as above + int char_index, // character to display + float *xpos, float *ypos, // pointers to current position in screen pixel space + stbtt_aligned_quad *q, // output: quad to draw + int opengl_fillrule); // true if opengl fill rule; false if DX9 or earlier +// Call GetBakedQuad with char_index = 'character - first_char', and it +// creates the quad you need to draw and advances the current position. +// +// The coordinate system used assumes y increases downwards. +// +// Characters will extend both above and below the current position; +// see discussion of "BASELINE" above. +// +// It's inefficient; you might want to c&p it and optimize it. + +STBTT_DEF void stbtt_GetScaledFontVMetrics(const unsigned char *fontdata, int index, float size, float *ascent, float *descent, float *lineGap); +// Query the font vertical metrics without having to create a font first. + + +////////////////////////////////////////////////////////////////////////////// +// +// NEW TEXTURE BAKING API +// +// This provides options for packing multiple fonts into one atlas, not +// perfectly but better than nothing. + +typedef struct +{ + unsigned short x0,y0,x1,y1; // coordinates of bbox in bitmap + float xoff,yoff,xadvance; + float xoff2,yoff2; +} stbtt_packedchar; + +typedef struct stbtt_pack_context stbtt_pack_context; +typedef struct stbtt_fontinfo stbtt_fontinfo; +#ifndef STB_RECT_PACK_VERSION +typedef struct stbrp_rect stbrp_rect; +#endif + +STBTT_DEF int stbtt_PackBegin(stbtt_pack_context *spc, unsigned char *pixels, int width, int height, int stride_in_bytes, int padding, void *alloc_context); +// Initializes a packing context stored in the passed-in stbtt_pack_context. +// Future calls using this context will pack characters into the bitmap passed +// in here: a 1-channel bitmap that is width * height. stride_in_bytes is +// the distance from one row to the next (or 0 to mean they are packed tightly +// together). "padding" is the amount of padding to leave between each +// character (normally you want '1' for bitmaps you'll use as textures with +// bilinear filtering). +// +// Returns 0 on failure, 1 on success. + +STBTT_DEF void stbtt_PackEnd (stbtt_pack_context *spc); +// Cleans up the packing context and frees all memory. + +#define STBTT_POINT_SIZE(x) (-(x)) + +STBTT_DEF int stbtt_PackFontRange(stbtt_pack_context *spc, const unsigned char *fontdata, int font_index, float font_size, + int first_unicode_char_in_range, int num_chars_in_range, stbtt_packedchar *chardata_for_range); +// Creates character bitmaps from the font_index'th font found in fontdata (use +// font_index=0 if you don't know what that is). It creates num_chars_in_range +// bitmaps for characters with unicode values starting at first_unicode_char_in_range +// and increasing. Data for how to render them is stored in chardata_for_range; +// pass these to stbtt_GetPackedQuad to get back renderable quads. +// +// font_size is the full height of the character from ascender to descender, +// as computed by stbtt_ScaleForPixelHeight. To use a point size as computed +// by stbtt_ScaleForMappingEmToPixels, wrap the point size in STBTT_POINT_SIZE() +// and pass that result as 'font_size': +// ..., 20 , ... // font max minus min y is 20 pixels tall +// ..., STBTT_POINT_SIZE(20), ... // 'M' is 20 pixels tall + +typedef struct +{ + float font_size; + int first_unicode_codepoint_in_range; // if non-zero, then the chars are continuous, and this is the first codepoint + int *array_of_unicode_codepoints; // if non-zero, then this is an array of unicode codepoints + int num_chars; + stbtt_packedchar *chardata_for_range; // output + unsigned char h_oversample, v_oversample; // don't set these, they're used internally +} stbtt_pack_range; + +STBTT_DEF int stbtt_PackFontRanges(stbtt_pack_context *spc, const unsigned char *fontdata, int font_index, stbtt_pack_range *ranges, int num_ranges); +// Creates character bitmaps from multiple ranges of characters stored in +// ranges. This will usually create a better-packed bitmap than multiple +// calls to stbtt_PackFontRange. Note that you can call this multiple +// times within a single PackBegin/PackEnd. + +STBTT_DEF void stbtt_PackSetOversampling(stbtt_pack_context *spc, unsigned int h_oversample, unsigned int v_oversample); +// Oversampling a font increases the quality by allowing higher-quality subpixel +// positioning, and is especially valuable at smaller text sizes. +// +// This function sets the amount of oversampling for all following calls to +// stbtt_PackFontRange(s) or stbtt_PackFontRangesGatherRects for a given +// pack context. The default (no oversampling) is achieved by h_oversample=1 +// and v_oversample=1. The total number of pixels required is +// h_oversample*v_oversample larger than the default; for example, 2x2 +// oversampling requires 4x the storage of 1x1. For best results, render +// oversampled textures with bilinear filtering. Look at the readme in +// stb/tests/oversample for information about oversampled fonts +// +// To use with PackFontRangesGather etc., you must set it before calls +// call to PackFontRangesGatherRects. + +STBTT_DEF void stbtt_PackSetSkipMissingCodepoints(stbtt_pack_context *spc, int skip); +// If skip != 0, this tells stb_truetype to skip any codepoints for which +// there is no corresponding glyph. If skip=0, which is the default, then +// codepoints without a glyph recived the font's "missing character" glyph, +// typically an empty box by convention. + +STBTT_DEF void stbtt_GetPackedQuad(const stbtt_packedchar *chardata, int pw, int ph, // same data as above + int char_index, // character to display + float *xpos, float *ypos, // pointers to current position in screen pixel space + stbtt_aligned_quad *q, // output: quad to draw + int align_to_integer); + +STBTT_DEF int stbtt_PackFontRangesGatherRects(stbtt_pack_context *spc, const stbtt_fontinfo *info, stbtt_pack_range *ranges, int num_ranges, stbrp_rect *rects); +STBTT_DEF void stbtt_PackFontRangesPackRects(stbtt_pack_context *spc, stbrp_rect *rects, int num_rects); +STBTT_DEF int stbtt_PackFontRangesRenderIntoRects(stbtt_pack_context *spc, const stbtt_fontinfo *info, stbtt_pack_range *ranges, int num_ranges, stbrp_rect *rects); +// Calling these functions in sequence is roughly equivalent to calling +// stbtt_PackFontRanges(). If you more control over the packing of multiple +// fonts, or if you want to pack custom data into a font texture, take a look +// at the source to of stbtt_PackFontRanges() and create a custom version +// using these functions, e.g. call GatherRects multiple times, +// building up a single array of rects, then call PackRects once, +// then call RenderIntoRects repeatedly. This may result in a +// better packing than calling PackFontRanges multiple times +// (or it may not). + +// this is an opaque structure that you shouldn't mess with which holds +// all the context needed from PackBegin to PackEnd. +struct stbtt_pack_context { + void *user_allocator_context; + void *pack_info; + int width; + int height; + int stride_in_bytes; + int padding; + int skip_missing; + unsigned int h_oversample, v_oversample; + unsigned char *pixels; + void *nodes; +}; + +////////////////////////////////////////////////////////////////////////////// +// +// FONT LOADING +// +// + +STBTT_DEF int stbtt_GetNumberOfFonts(const unsigned char *data); +// This function will determine the number of fonts in a font file. TrueType +// collection (.ttc) files may contain multiple fonts, while TrueType font +// (.ttf) files only contain one font. The number of fonts can be used for +// indexing with the previous function where the index is between zero and one +// less than the total fonts. If an error occurs, -1 is returned. + +STBTT_DEF int stbtt_GetFontOffsetForIndex(const unsigned char *data, int index); +// Each .ttf/.ttc file may have more than one font. Each font has a sequential +// index number starting from 0. Call this function to get the font offset for +// a given index; it returns -1 if the index is out of range. A regular .ttf +// file will only define one font and it always be at offset 0, so it will +// return '0' for index 0, and -1 for all other indices. + +// The following structure is defined publicly so you can declare one on +// the stack or as a global or etc, but you should treat it as opaque. +struct stbtt_fontinfo +{ + void * userdata; + unsigned char * data; // pointer to .ttf file + int fontstart; // offset of start of font + + int numGlyphs; // number of glyphs, needed for range checking + + int loca,head,glyf,hhea,hmtx,kern,gpos,svg; // table locations as offset from start of .ttf + int index_map; // a cmap mapping for our chosen character encoding + int indexToLocFormat; // format needed to map from glyph index to glyph + + stbtt__buf cff; // cff font data + stbtt__buf charstrings; // the charstring index + stbtt__buf gsubrs; // global charstring subroutines index + stbtt__buf subrs; // private charstring subroutines index + stbtt__buf fontdicts; // array of font dicts + stbtt__buf fdselect; // map from glyph to fontdict +}; + +STBTT_DEF int stbtt_InitFont(stbtt_fontinfo *info, const unsigned char *data, int offset); +// Given an offset into the file that defines a font, this function builds +// the necessary cached info for the rest of the system. You must allocate +// the stbtt_fontinfo yourself, and stbtt_InitFont will fill it out. You don't +// need to do anything special to free it, because the contents are pure +// value data with no additional data structures. Returns 0 on failure. + + +////////////////////////////////////////////////////////////////////////////// +// +// CHARACTER TO GLYPH-INDEX CONVERSIOn + +STBTT_DEF int stbtt_FindGlyphIndex(const stbtt_fontinfo *info, int unicode_codepoint); +// If you're going to perform multiple operations on the same character +// and you want a speed-up, call this function with the character you're +// going to process, then use glyph-based functions instead of the +// codepoint-based functions. +// Returns 0 if the character codepoint is not defined in the font. + + +////////////////////////////////////////////////////////////////////////////// +// +// CHARACTER PROPERTIES +// + +STBTT_DEF float stbtt_ScaleForPixelHeight(const stbtt_fontinfo *info, float pixels); +// computes a scale factor to produce a font whose "height" is 'pixels' tall. +// Height is measured as the distance from the highest ascender to the lowest +// descender; in other words, it's equivalent to calling stbtt_GetFontVMetrics +// and computing: +// scale = pixels / (ascent - descent) +// so if you prefer to measure height by the ascent only, use a similar calculation. + +STBTT_DEF float stbtt_ScaleForMappingEmToPixels(const stbtt_fontinfo *info, float pixels); +// computes a scale factor to produce a font whose EM size is mapped to +// 'pixels' tall. This is probably what traditional APIs compute, but +// I'm not positive. + +STBTT_DEF void stbtt_GetFontVMetrics(const stbtt_fontinfo *info, int *ascent, int *descent, int *lineGap); +// ascent is the coordinate above the baseline the font extends; descent +// is the coordinate below the baseline the font extends (i.e. it is typically negative) +// lineGap is the spacing between one row's descent and the next row's ascent... +// so you should advance the vertical position by "*ascent - *descent + *lineGap" +// these are expressed in unscaled coordinates, so you must multiply by +// the scale factor for a given size + +STBTT_DEF int stbtt_GetFontVMetricsOS2(const stbtt_fontinfo *info, int *typoAscent, int *typoDescent, int *typoLineGap); +// analogous to GetFontVMetrics, but returns the "typographic" values from the OS/2 +// table (specific to MS/Windows TTF files). +// +// Returns 1 on success (table present), 0 on failure. + +STBTT_DEF void stbtt_GetFontBoundingBox(const stbtt_fontinfo *info, int *x0, int *y0, int *x1, int *y1); +// the bounding box around all possible characters + +STBTT_DEF void stbtt_GetCodepointHMetrics(const stbtt_fontinfo *info, int codepoint, int *advanceWidth, int *leftSideBearing); +// leftSideBearing is the offset from the current horizontal position to the left edge of the character +// advanceWidth is the offset from the current horizontal position to the next horizontal position +// these are expressed in unscaled coordinates + +STBTT_DEF int stbtt_GetCodepointKernAdvance(const stbtt_fontinfo *info, int ch1, int ch2); +// an additional amount to add to the 'advance' value between ch1 and ch2 + +STBTT_DEF int stbtt_GetCodepointBox(const stbtt_fontinfo *info, int codepoint, int *x0, int *y0, int *x1, int *y1); +// Gets the bounding box of the visible part of the glyph, in unscaled coordinates + +STBTT_DEF void stbtt_GetGlyphHMetrics(const stbtt_fontinfo *info, int glyph_index, int *advanceWidth, int *leftSideBearing); +STBTT_DEF int stbtt_GetGlyphKernAdvance(const stbtt_fontinfo *info, int glyph1, int glyph2); +STBTT_DEF int stbtt_GetGlyphBox(const stbtt_fontinfo *info, int glyph_index, int *x0, int *y0, int *x1, int *y1); +// as above, but takes one or more glyph indices for greater efficiency + +typedef struct stbtt_kerningentry +{ + int glyph1; // use stbtt_FindGlyphIndex + int glyph2; + int advance; +} stbtt_kerningentry; + +STBTT_DEF int stbtt_GetKerningTableLength(const stbtt_fontinfo *info); +STBTT_DEF int stbtt_GetKerningTable(const stbtt_fontinfo *info, stbtt_kerningentry* table, int table_length); +// Retrieves a complete list of all of the kerning pairs provided by the font +// stbtt_GetKerningTable never writes more than table_length entries and returns how many entries it did write. +// The table will be sorted by (a.glyph1 == b.glyph1)?(a.glyph2 < b.glyph2):(a.glyph1 < b.glyph1) + +////////////////////////////////////////////////////////////////////////////// +// +// GLYPH SHAPES (you probably don't need these, but they have to go before +// the bitmaps for C declaration-order reasons) +// + +#ifndef STBTT_vmove // you can predefine these to use different values (but why?) + enum { + STBTT_vmove=1, + STBTT_vline, + STBTT_vcurve, + STBTT_vcubic + }; +#endif + +#ifndef stbtt_vertex // you can predefine this to use different values + // (we share this with other code at RAD) + #define stbtt_vertex_type short // can't use stbtt_int16 because that's not visible in the header file + typedef struct + { + stbtt_vertex_type x,y,cx,cy,cx1,cy1; + unsigned char type,padding; + } stbtt_vertex; +#endif + +STBTT_DEF int stbtt_IsGlyphEmpty(const stbtt_fontinfo *info, int glyph_index); +// returns non-zero if nothing is drawn for this glyph + +STBTT_DEF int stbtt_GetCodepointShape(const stbtt_fontinfo *info, int unicode_codepoint, stbtt_vertex **vertices); +STBTT_DEF int stbtt_GetGlyphShape(const stbtt_fontinfo *info, int glyph_index, stbtt_vertex **vertices); +// returns # of vertices and fills *vertices with the pointer to them +// these are expressed in "unscaled" coordinates +// +// The shape is a series of contours. Each one starts with +// a STBTT_moveto, then consists of a series of mixed +// STBTT_lineto and STBTT_curveto segments. A lineto +// draws a line from previous endpoint to its x,y; a curveto +// draws a quadratic bezier from previous endpoint to +// its x,y, using cx,cy as the bezier control point. + +STBTT_DEF void stbtt_FreeShape(const stbtt_fontinfo *info, stbtt_vertex *vertices); +// frees the data allocated above + +STBTT_DEF unsigned char *stbtt_FindSVGDoc(const stbtt_fontinfo *info, int gl); +STBTT_DEF int stbtt_GetCodepointSVG(const stbtt_fontinfo *info, int unicode_codepoint, const char **svg); +STBTT_DEF int stbtt_GetGlyphSVG(const stbtt_fontinfo *info, int gl, const char **svg); +// fills svg with the character's SVG data. +// returns data size or 0 if SVG not found. + +////////////////////////////////////////////////////////////////////////////// +// +// BITMAP RENDERING +// + +STBTT_DEF void stbtt_FreeBitmap(unsigned char *bitmap, void *userdata); +// frees the bitmap allocated below + +STBTT_DEF unsigned char *stbtt_GetCodepointBitmap(const stbtt_fontinfo *info, float scale_x, float scale_y, int codepoint, int *width, int *height, int *xoff, int *yoff); +// allocates a large-enough single-channel 8bpp bitmap and renders the +// specified character/glyph at the specified scale into it, with +// antialiasing. 0 is no coverage (transparent), 255 is fully covered (opaque). +// *width & *height are filled out with the width & height of the bitmap, +// which is stored left-to-right, top-to-bottom. +// +// xoff/yoff are the offset it pixel space from the glyph origin to the top-left of the bitmap + +STBTT_DEF unsigned char *stbtt_GetCodepointBitmapSubpixel(const stbtt_fontinfo *info, float scale_x, float scale_y, float shift_x, float shift_y, int codepoint, int *width, int *height, int *xoff, int *yoff); +// the same as stbtt_GetCodepoitnBitmap, but you can specify a subpixel +// shift for the character + +STBTT_DEF void stbtt_MakeCodepointBitmap(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, int codepoint); +// the same as stbtt_GetCodepointBitmap, but you pass in storage for the bitmap +// in the form of 'output', with row spacing of 'out_stride' bytes. the bitmap +// is clipped to out_w/out_h bytes. Call stbtt_GetCodepointBitmapBox to get the +// width and height and positioning info for it first. + +STBTT_DEF void stbtt_MakeCodepointBitmapSubpixel(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, float shift_x, float shift_y, int codepoint); +// same as stbtt_MakeCodepointBitmap, but you can specify a subpixel +// shift for the character + +STBTT_DEF void stbtt_MakeCodepointBitmapSubpixelPrefilter(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, float shift_x, float shift_y, int oversample_x, int oversample_y, float *sub_x, float *sub_y, int codepoint); +// same as stbtt_MakeCodepointBitmapSubpixel, but prefiltering +// is performed (see stbtt_PackSetOversampling) + +STBTT_DEF void stbtt_GetCodepointBitmapBox(const stbtt_fontinfo *font, int codepoint, float scale_x, float scale_y, int *ix0, int *iy0, int *ix1, int *iy1); +// get the bbox of the bitmap centered around the glyph origin; so the +// bitmap width is ix1-ix0, height is iy1-iy0, and location to place +// the bitmap top left is (leftSideBearing*scale,iy0). +// (Note that the bitmap uses y-increases-down, but the shape uses +// y-increases-up, so CodepointBitmapBox and CodepointBox are inverted.) + +STBTT_DEF void stbtt_GetCodepointBitmapBoxSubpixel(const stbtt_fontinfo *font, int codepoint, float scale_x, float scale_y, float shift_x, float shift_y, int *ix0, int *iy0, int *ix1, int *iy1); +// same as stbtt_GetCodepointBitmapBox, but you can specify a subpixel +// shift for the character + +// the following functions are equivalent to the above functions, but operate +// on glyph indices instead of Unicode codepoints (for efficiency) +STBTT_DEF unsigned char *stbtt_GetGlyphBitmap(const stbtt_fontinfo *info, float scale_x, float scale_y, int glyph, int *width, int *height, int *xoff, int *yoff); +STBTT_DEF unsigned char *stbtt_GetGlyphBitmapSubpixel(const stbtt_fontinfo *info, float scale_x, float scale_y, float shift_x, float shift_y, int glyph, int *width, int *height, int *xoff, int *yoff); +STBTT_DEF void stbtt_MakeGlyphBitmap(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, int glyph); +STBTT_DEF void stbtt_MakeGlyphBitmapSubpixel(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, float shift_x, float shift_y, int glyph); +STBTT_DEF void stbtt_MakeGlyphBitmapSubpixelPrefilter(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, float shift_x, float shift_y, int oversample_x, int oversample_y, float *sub_x, float *sub_y, int glyph); +STBTT_DEF void stbtt_GetGlyphBitmapBox(const stbtt_fontinfo *font, int glyph, float scale_x, float scale_y, int *ix0, int *iy0, int *ix1, int *iy1); +STBTT_DEF void stbtt_GetGlyphBitmapBoxSubpixel(const stbtt_fontinfo *font, int glyph, float scale_x, float scale_y,float shift_x, float shift_y, int *ix0, int *iy0, int *ix1, int *iy1); + + +// @TODO: don't expose this structure +typedef struct +{ + int w,h,stride; + unsigned char *pixels; +} stbtt__bitmap; + +// rasterize a shape with quadratic beziers into a bitmap +STBTT_DEF void stbtt_Rasterize(stbtt__bitmap *result, // 1-channel bitmap to draw into + float flatness_in_pixels, // allowable error of curve in pixels + stbtt_vertex *vertices, // array of vertices defining shape + int num_verts, // number of vertices in above array + float scale_x, float scale_y, // scale applied to input vertices + float shift_x, float shift_y, // translation applied to input vertices + int x_off, int y_off, // another translation applied to input + int invert, // if non-zero, vertically flip shape + void *userdata); // context for to STBTT_MALLOC + +////////////////////////////////////////////////////////////////////////////// +// +// Signed Distance Function (or Field) rendering + +STBTT_DEF void stbtt_FreeSDF(unsigned char *bitmap, void *userdata); +// frees the SDF bitmap allocated below + +STBTT_DEF unsigned char * stbtt_GetGlyphSDF(const stbtt_fontinfo *info, float scale, int glyph, int padding, unsigned char onedge_value, float pixel_dist_scale, int *width, int *height, int *xoff, int *yoff); +STBTT_DEF unsigned char * stbtt_GetCodepointSDF(const stbtt_fontinfo *info, float scale, int codepoint, int padding, unsigned char onedge_value, float pixel_dist_scale, int *width, int *height, int *xoff, int *yoff); +// These functions compute a discretized SDF field for a single character, suitable for storing +// in a single-channel texture, sampling with bilinear filtering, and testing against +// larger than some threshold to produce scalable fonts. +// info -- the font +// scale -- controls the size of the resulting SDF bitmap, same as it would be creating a regular bitmap +// glyph/codepoint -- the character to generate the SDF for +// padding -- extra "pixels" around the character which are filled with the distance to the character (not 0), +// which allows effects like bit outlines +// onedge_value -- value 0-255 to test the SDF against to reconstruct the character (i.e. the isocontour of the character) +// pixel_dist_scale -- what value the SDF should increase by when moving one SDF "pixel" away from the edge (on the 0..255 scale) +// if positive, > onedge_value is inside; if negative, < onedge_value is inside +// width,height -- output height & width of the SDF bitmap (including padding) +// xoff,yoff -- output origin of the character +// return value -- a 2D array of bytes 0..255, width*height in size +// +// pixel_dist_scale & onedge_value are a scale & bias that allows you to make +// optimal use of the limited 0..255 for your application, trading off precision +// and special effects. SDF values outside the range 0..255 are clamped to 0..255. +// +// Example: +// scale = stbtt_ScaleForPixelHeight(22) +// padding = 5 +// onedge_value = 180 +// pixel_dist_scale = 180/5.0 = 36.0 +// +// This will create an SDF bitmap in which the character is about 22 pixels +// high but the whole bitmap is about 22+5+5=32 pixels high. To produce a filled +// shape, sample the SDF at each pixel and fill the pixel if the SDF value +// is greater than or equal to 180/255. (You'll actually want to antialias, +// which is beyond the scope of this example.) Additionally, you can compute +// offset outlines (e.g. to stroke the character border inside & outside, +// or only outside). For example, to fill outside the character up to 3 SDF +// pixels, you would compare against (180-36.0*3)/255 = 72/255. The above +// choice of variables maps a range from 5 pixels outside the shape to +// 2 pixels inside the shape to 0..255; this is intended primarily for apply +// outside effects only (the interior range is needed to allow proper +// antialiasing of the font at *smaller* sizes) +// +// The function computes the SDF analytically at each SDF pixel, not by e.g. +// building a higher-res bitmap and approximating it. In theory the quality +// should be as high as possible for an SDF of this size & representation, but +// unclear if this is true in practice (perhaps building a higher-res bitmap +// and computing from that can allow drop-out prevention). +// +// The algorithm has not been optimized at all, so expect it to be slow +// if computing lots of characters or very large sizes. + + + +////////////////////////////////////////////////////////////////////////////// +// +// Finding the right font... +// +// You should really just solve this offline, keep your own tables +// of what font is what, and don't try to get it out of the .ttf file. +// That's because getting it out of the .ttf file is really hard, because +// the names in the file can appear in many possible encodings, in many +// possible languages, and e.g. if you need a case-insensitive comparison, +// the details of that depend on the encoding & language in a complex way +// (actually underspecified in truetype, but also gigantic). +// +// But you can use the provided functions in two possible ways: +// stbtt_FindMatchingFont() will use *case-sensitive* comparisons on +// unicode-encoded names to try to find the font you want; +// you can run this before calling stbtt_InitFont() +// +// stbtt_GetFontNameString() lets you get any of the various strings +// from the file yourself and do your own comparisons on them. +// You have to have called stbtt_InitFont() first. + + +STBTT_DEF int stbtt_FindMatchingFont(const unsigned char *fontdata, const char *name, int flags); +// returns the offset (not index) of the font that matches, or -1 if none +// if you use STBTT_MACSTYLE_DONTCARE, use a font name like "Arial Bold". +// if you use any other flag, use a font name like "Arial"; this checks +// the 'macStyle' header field; i don't know if fonts set this consistently +#define STBTT_MACSTYLE_DONTCARE 0 +#define STBTT_MACSTYLE_BOLD 1 +#define STBTT_MACSTYLE_ITALIC 2 +#define STBTT_MACSTYLE_UNDERSCORE 4 +#define STBTT_MACSTYLE_NONE 8 // <= not same as 0, this makes us check the bitfield is 0 + +STBTT_DEF int stbtt_CompareUTF8toUTF16_bigendian(const char *s1, int len1, const char *s2, int len2); +// returns 1/0 whether the first string interpreted as utf8 is identical to +// the second string interpreted as big-endian utf16... useful for strings from next func + +STBTT_DEF const char *stbtt_GetFontNameString(const stbtt_fontinfo *font, int *length, int platformID, int encodingID, int languageID, int nameID); +// returns the string (which may be big-endian double byte, e.g. for unicode) +// and puts the length in bytes in *length. +// +// some of the values for the IDs are below; for more see the truetype spec: +// http://developer.apple.com/textfonts/TTRefMan/RM06/Chap6name.html +// http://www.microsoft.com/typography/otspec/name.htm + +enum { // platformID + STBTT_PLATFORM_ID_UNICODE =0, + STBTT_PLATFORM_ID_MAC =1, + STBTT_PLATFORM_ID_ISO =2, + STBTT_PLATFORM_ID_MICROSOFT =3 +}; + +enum { // encodingID for STBTT_PLATFORM_ID_UNICODE + STBTT_UNICODE_EID_UNICODE_1_0 =0, + STBTT_UNICODE_EID_UNICODE_1_1 =1, + STBTT_UNICODE_EID_ISO_10646 =2, + STBTT_UNICODE_EID_UNICODE_2_0_BMP=3, + STBTT_UNICODE_EID_UNICODE_2_0_FULL=4 +}; + +enum { // encodingID for STBTT_PLATFORM_ID_MICROSOFT + STBTT_MS_EID_SYMBOL =0, + STBTT_MS_EID_UNICODE_BMP =1, + STBTT_MS_EID_SHIFTJIS =2, + STBTT_MS_EID_UNICODE_FULL =10 +}; + +enum { // encodingID for STBTT_PLATFORM_ID_MAC; same as Script Manager codes + STBTT_MAC_EID_ROMAN =0, STBTT_MAC_EID_ARABIC =4, + STBTT_MAC_EID_JAPANESE =1, STBTT_MAC_EID_HEBREW =5, + STBTT_MAC_EID_CHINESE_TRAD =2, STBTT_MAC_EID_GREEK =6, + STBTT_MAC_EID_KOREAN =3, STBTT_MAC_EID_RUSSIAN =7 +}; + +enum { // languageID for STBTT_PLATFORM_ID_MICROSOFT; same as LCID... + // problematic because there are e.g. 16 english LCIDs and 16 arabic LCIDs + STBTT_MS_LANG_ENGLISH =0x0409, STBTT_MS_LANG_ITALIAN =0x0410, + STBTT_MS_LANG_CHINESE =0x0804, STBTT_MS_LANG_JAPANESE =0x0411, + STBTT_MS_LANG_DUTCH =0x0413, STBTT_MS_LANG_KOREAN =0x0412, + STBTT_MS_LANG_FRENCH =0x040c, STBTT_MS_LANG_RUSSIAN =0x0419, + STBTT_MS_LANG_GERMAN =0x0407, STBTT_MS_LANG_SPANISH =0x0409, + STBTT_MS_LANG_HEBREW =0x040d, STBTT_MS_LANG_SWEDISH =0x041D +}; + +enum { // languageID for STBTT_PLATFORM_ID_MAC + STBTT_MAC_LANG_ENGLISH =0 , STBTT_MAC_LANG_JAPANESE =11, + STBTT_MAC_LANG_ARABIC =12, STBTT_MAC_LANG_KOREAN =23, + STBTT_MAC_LANG_DUTCH =4 , STBTT_MAC_LANG_RUSSIAN =32, + STBTT_MAC_LANG_FRENCH =1 , STBTT_MAC_LANG_SPANISH =6 , + STBTT_MAC_LANG_GERMAN =2 , STBTT_MAC_LANG_SWEDISH =5 , + STBTT_MAC_LANG_HEBREW =10, STBTT_MAC_LANG_CHINESE_SIMPLIFIED =33, + STBTT_MAC_LANG_ITALIAN =3 , STBTT_MAC_LANG_CHINESE_TRAD =19 +}; + +#ifdef __cplusplus +} +#endif + +#endif // __STB_INCLUDE_STB_TRUETYPE_H__ + +/////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////// +//// +//// IMPLEMENTATION +//// +//// + +#ifdef STB_TRUETYPE_IMPLEMENTATION + +#ifndef STBTT_MAX_OVERSAMPLE +#define STBTT_MAX_OVERSAMPLE 8 +#endif + +#if STBTT_MAX_OVERSAMPLE > 255 +#error "STBTT_MAX_OVERSAMPLE cannot be > 255" +#endif + +typedef int stbtt__test_oversample_pow2[(STBTT_MAX_OVERSAMPLE & (STBTT_MAX_OVERSAMPLE-1)) == 0 ? 1 : -1]; + +#ifndef STBTT_RASTERIZER_VERSION +#define STBTT_RASTERIZER_VERSION 2 +#endif + +#ifdef _MSC_VER +#define STBTT__NOTUSED(v) (void)(v) +#else +#define STBTT__NOTUSED(v) (void)sizeof(v) +#endif + +////////////////////////////////////////////////////////////////////////// +// +// stbtt__buf helpers to parse data from file +// + +static stbtt_uint8 stbtt__buf_get8(stbtt__buf *b) +{ + if (b->cursor >= b->size) + return 0; + return b->data[b->cursor++]; +} + +static stbtt_uint8 stbtt__buf_peek8(stbtt__buf *b) +{ + if (b->cursor >= b->size) + return 0; + return b->data[b->cursor]; +} + +static void stbtt__buf_seek(stbtt__buf *b, int o) +{ + STBTT_assert(!(o > b->size || o < 0)); + b->cursor = (o > b->size || o < 0) ? b->size : o; +} + +static void stbtt__buf_skip(stbtt__buf *b, int o) +{ + stbtt__buf_seek(b, b->cursor + o); +} + +static stbtt_uint32 stbtt__buf_get(stbtt__buf *b, int n) +{ + stbtt_uint32 v = 0; + int i; + STBTT_assert(n >= 1 && n <= 4); + for (i = 0; i < n; i++) + v = (v << 8) | stbtt__buf_get8(b); + return v; +} + +static stbtt__buf stbtt__new_buf(const void *p, size_t size) +{ + stbtt__buf r; + STBTT_assert(size < 0x40000000); + r.data = (stbtt_uint8*) p; + r.size = (int) size; + r.cursor = 0; + return r; +} + +#define stbtt__buf_get16(b) stbtt__buf_get((b), 2) +#define stbtt__buf_get32(b) stbtt__buf_get((b), 4) + +static stbtt__buf stbtt__buf_range(const stbtt__buf *b, int o, int s) +{ + stbtt__buf r = stbtt__new_buf(NULL, 0); + if (o < 0 || s < 0 || o > b->size || s > b->size - o) return r; + r.data = b->data + o; + r.size = s; + return r; +} + +static stbtt__buf stbtt__cff_get_index(stbtt__buf *b) +{ + int count, start, offsize; + start = b->cursor; + count = stbtt__buf_get16(b); + if (count) { + offsize = stbtt__buf_get8(b); + STBTT_assert(offsize >= 1 && offsize <= 4); + stbtt__buf_skip(b, offsize * count); + stbtt__buf_skip(b, stbtt__buf_get(b, offsize) - 1); + } + return stbtt__buf_range(b, start, b->cursor - start); +} + +static stbtt_uint32 stbtt__cff_int(stbtt__buf *b) +{ + int b0 = stbtt__buf_get8(b); + if (b0 >= 32 && b0 <= 246) return b0 - 139; + else if (b0 >= 247 && b0 <= 250) return (b0 - 247)*256 + stbtt__buf_get8(b) + 108; + else if (b0 >= 251 && b0 <= 254) return -(b0 - 251)*256 - stbtt__buf_get8(b) - 108; + else if (b0 == 28) return stbtt__buf_get16(b); + else if (b0 == 29) return stbtt__buf_get32(b); + STBTT_assert(0); + return 0; +} + +static void stbtt__cff_skip_operand(stbtt__buf *b) { + int v, b0 = stbtt__buf_peek8(b); + STBTT_assert(b0 >= 28); + if (b0 == 30) { + stbtt__buf_skip(b, 1); + while (b->cursor < b->size) { + v = stbtt__buf_get8(b); + if ((v & 0xF) == 0xF || (v >> 4) == 0xF) + break; + } + } else { + stbtt__cff_int(b); + } +} + +static stbtt__buf stbtt__dict_get(stbtt__buf *b, int key) +{ + stbtt__buf_seek(b, 0); + while (b->cursor < b->size) { + int start = b->cursor, end, op; + while (stbtt__buf_peek8(b) >= 28) + stbtt__cff_skip_operand(b); + end = b->cursor; + op = stbtt__buf_get8(b); + if (op == 12) op = stbtt__buf_get8(b) | 0x100; + if (op == key) return stbtt__buf_range(b, start, end-start); + } + return stbtt__buf_range(b, 0, 0); +} + +static void stbtt__dict_get_ints(stbtt__buf *b, int key, int outcount, stbtt_uint32 *out) +{ + int i; + stbtt__buf operands = stbtt__dict_get(b, key); + for (i = 0; i < outcount && operands.cursor < operands.size; i++) + out[i] = stbtt__cff_int(&operands); +} + +static int stbtt__cff_index_count(stbtt__buf *b) +{ + stbtt__buf_seek(b, 0); + return stbtt__buf_get16(b); +} + +static stbtt__buf stbtt__cff_index_get(stbtt__buf b, int i) +{ + int count, offsize, start, end; + stbtt__buf_seek(&b, 0); + count = stbtt__buf_get16(&b); + offsize = stbtt__buf_get8(&b); + STBTT_assert(i >= 0 && i < count); + STBTT_assert(offsize >= 1 && offsize <= 4); + stbtt__buf_skip(&b, i*offsize); + start = stbtt__buf_get(&b, offsize); + end = stbtt__buf_get(&b, offsize); + return stbtt__buf_range(&b, 2+(count+1)*offsize+start, end - start); +} + +////////////////////////////////////////////////////////////////////////// +// +// accessors to parse data from file +// + +// on platforms that don't allow misaligned reads, if we want to allow +// truetype fonts that aren't padded to alignment, define ALLOW_UNALIGNED_TRUETYPE + +#define ttBYTE(p) (* (stbtt_uint8 *) (p)) +#define ttCHAR(p) (* (stbtt_int8 *) (p)) +#define ttFixed(p) ttLONG(p) + +static stbtt_uint16 ttUSHORT(stbtt_uint8 *p) { return p[0]*256 + p[1]; } +static stbtt_int16 ttSHORT(stbtt_uint8 *p) { return p[0]*256 + p[1]; } +static stbtt_uint32 ttULONG(stbtt_uint8 *p) { return (p[0]<<24) + (p[1]<<16) + (p[2]<<8) + p[3]; } +static stbtt_int32 ttLONG(stbtt_uint8 *p) { return (p[0]<<24) + (p[1]<<16) + (p[2]<<8) + p[3]; } + +#define stbtt_tag4(p,c0,c1,c2,c3) ((p)[0] == (c0) && (p)[1] == (c1) && (p)[2] == (c2) && (p)[3] == (c3)) +#define stbtt_tag(p,str) stbtt_tag4(p,str[0],str[1],str[2],str[3]) + +static int stbtt__isfont(stbtt_uint8 *font) +{ + // check the version number + if (stbtt_tag4(font, '1',0,0,0)) return 1; // TrueType 1 + if (stbtt_tag(font, "typ1")) return 1; // TrueType with type 1 font -- we don't support this! + if (stbtt_tag(font, "OTTO")) return 1; // OpenType with CFF + if (stbtt_tag4(font, 0,1,0,0)) return 1; // OpenType 1.0 + if (stbtt_tag(font, "true")) return 1; // Apple specification for TrueType fonts + return 0; +} + +// @OPTIMIZE: binary search +static stbtt_uint32 stbtt__find_table(stbtt_uint8 *data, stbtt_uint32 fontstart, const char *tag) +{ + stbtt_int32 num_tables = ttUSHORT(data+fontstart+4); + stbtt_uint32 tabledir = fontstart + 12; + stbtt_int32 i; + for (i=0; i < num_tables; ++i) { + stbtt_uint32 loc = tabledir + 16*i; + if (stbtt_tag(data+loc+0, tag)) + return ttULONG(data+loc+8); + } + return 0; +} + +static int stbtt_GetFontOffsetForIndex_internal(unsigned char *font_collection, int index) +{ + // if it's just a font, there's only one valid index + if (stbtt__isfont(font_collection)) + return index == 0 ? 0 : -1; + + // check if it's a TTC + if (stbtt_tag(font_collection, "ttcf")) { + // version 1? + if (ttULONG(font_collection+4) == 0x00010000 || ttULONG(font_collection+4) == 0x00020000) { + stbtt_int32 n = ttLONG(font_collection+8); + if (index >= n) + return -1; + return ttULONG(font_collection+12+index*4); + } + } + return -1; +} + +static int stbtt_GetNumberOfFonts_internal(unsigned char *font_collection) +{ + // if it's just a font, there's only one valid font + if (stbtt__isfont(font_collection)) + return 1; + + // check if it's a TTC + if (stbtt_tag(font_collection, "ttcf")) { + // version 1? + if (ttULONG(font_collection+4) == 0x00010000 || ttULONG(font_collection+4) == 0x00020000) { + return ttLONG(font_collection+8); + } + } + return 0; +} + +static stbtt__buf stbtt__get_subrs(stbtt__buf cff, stbtt__buf fontdict) +{ + stbtt_uint32 subrsoff = 0, private_loc[2] = { 0, 0 }; + stbtt__buf pdict; + stbtt__dict_get_ints(&fontdict, 18, 2, private_loc); + if (!private_loc[1] || !private_loc[0]) return stbtt__new_buf(NULL, 0); + pdict = stbtt__buf_range(&cff, private_loc[1], private_loc[0]); + stbtt__dict_get_ints(&pdict, 19, 1, &subrsoff); + if (!subrsoff) return stbtt__new_buf(NULL, 0); + stbtt__buf_seek(&cff, private_loc[1]+subrsoff); + return stbtt__cff_get_index(&cff); +} + +// since most people won't use this, find this table the first time it's needed +static int stbtt__get_svg(stbtt_fontinfo *info) +{ + stbtt_uint32 t; + if (info->svg < 0) { + t = stbtt__find_table(info->data, info->fontstart, "SVG "); + if (t) { + stbtt_uint32 offset = ttULONG(info->data + t + 2); + info->svg = t + offset; + } else { + info->svg = 0; + } + } + return info->svg; +} + +static int stbtt_InitFont_internal(stbtt_fontinfo *info, unsigned char *data, int fontstart) +{ + stbtt_uint32 cmap, t; + stbtt_int32 i,numTables; + + info->data = data; + info->fontstart = fontstart; + info->cff = stbtt__new_buf(NULL, 0); + + cmap = stbtt__find_table(data, fontstart, "cmap"); // required + info->loca = stbtt__find_table(data, fontstart, "loca"); // required + info->head = stbtt__find_table(data, fontstart, "head"); // required + info->glyf = stbtt__find_table(data, fontstart, "glyf"); // required + info->hhea = stbtt__find_table(data, fontstart, "hhea"); // required + info->hmtx = stbtt__find_table(data, fontstart, "hmtx"); // required + info->kern = stbtt__find_table(data, fontstart, "kern"); // not required + info->gpos = stbtt__find_table(data, fontstart, "GPOS"); // not required + + if (!cmap || !info->head || !info->hhea || !info->hmtx) + return 0; + if (info->glyf) { + // required for truetype + if (!info->loca) return 0; + } else { + // initialization for CFF / Type2 fonts (OTF) + stbtt__buf b, topdict, topdictidx; + stbtt_uint32 cstype = 2, charstrings = 0, fdarrayoff = 0, fdselectoff = 0; + stbtt_uint32 cff; + + cff = stbtt__find_table(data, fontstart, "CFF "); + if (!cff) return 0; + + info->fontdicts = stbtt__new_buf(NULL, 0); + info->fdselect = stbtt__new_buf(NULL, 0); + + // @TODO this should use size from table (not 512MB) + info->cff = stbtt__new_buf(data+cff, 512*1024*1024); + b = info->cff; + + // read the header + stbtt__buf_skip(&b, 2); + stbtt__buf_seek(&b, stbtt__buf_get8(&b)); // hdrsize + + // @TODO the name INDEX could list multiple fonts, + // but we just use the first one. + stbtt__cff_get_index(&b); // name INDEX + topdictidx = stbtt__cff_get_index(&b); + topdict = stbtt__cff_index_get(topdictidx, 0); + stbtt__cff_get_index(&b); // string INDEX + info->gsubrs = stbtt__cff_get_index(&b); + + stbtt__dict_get_ints(&topdict, 17, 1, &charstrings); + stbtt__dict_get_ints(&topdict, 0x100 | 6, 1, &cstype); + stbtt__dict_get_ints(&topdict, 0x100 | 36, 1, &fdarrayoff); + stbtt__dict_get_ints(&topdict, 0x100 | 37, 1, &fdselectoff); + info->subrs = stbtt__get_subrs(b, topdict); + + // we only support Type 2 charstrings + if (cstype != 2) return 0; + if (charstrings == 0) return 0; + + if (fdarrayoff) { + // looks like a CID font + if (!fdselectoff) return 0; + stbtt__buf_seek(&b, fdarrayoff); + info->fontdicts = stbtt__cff_get_index(&b); + info->fdselect = stbtt__buf_range(&b, fdselectoff, b.size-fdselectoff); + } + + stbtt__buf_seek(&b, charstrings); + info->charstrings = stbtt__cff_get_index(&b); + } + + t = stbtt__find_table(data, fontstart, "maxp"); + if (t) + info->numGlyphs = ttUSHORT(data+t+4); + else + info->numGlyphs = 0xffff; + + info->svg = -1; + + // find a cmap encoding table we understand *now* to avoid searching + // later. (todo: could make this installable) + // the same regardless of glyph. + numTables = ttUSHORT(data + cmap + 2); + info->index_map = 0; + for (i=0; i < numTables; ++i) { + stbtt_uint32 encoding_record = cmap + 4 + 8 * i; + // find an encoding we understand: + switch(ttUSHORT(data+encoding_record)) { + case STBTT_PLATFORM_ID_MICROSOFT: + switch (ttUSHORT(data+encoding_record+2)) { + case STBTT_MS_EID_UNICODE_BMP: + case STBTT_MS_EID_UNICODE_FULL: + // MS/Unicode + info->index_map = cmap + ttULONG(data+encoding_record+4); + break; + } + break; + case STBTT_PLATFORM_ID_UNICODE: + // Mac/iOS has these + // all the encodingIDs are unicode, so we don't bother to check it + info->index_map = cmap + ttULONG(data+encoding_record+4); + break; + } + } + if (info->index_map == 0) + return 0; + + info->indexToLocFormat = ttUSHORT(data+info->head + 50); + return 1; +} + +STBTT_DEF int stbtt_FindGlyphIndex(const stbtt_fontinfo *info, int unicode_codepoint) +{ + stbtt_uint8 *data = info->data; + stbtt_uint32 index_map = info->index_map; + + stbtt_uint16 format = ttUSHORT(data + index_map + 0); + if (format == 0) { // apple byte encoding + stbtt_int32 bytes = ttUSHORT(data + index_map + 2); + if (unicode_codepoint < bytes-6) + return ttBYTE(data + index_map + 6 + unicode_codepoint); + return 0; + } else if (format == 6) { + stbtt_uint32 first = ttUSHORT(data + index_map + 6); + stbtt_uint32 count = ttUSHORT(data + index_map + 8); + if ((stbtt_uint32) unicode_codepoint >= first && (stbtt_uint32) unicode_codepoint < first+count) + return ttUSHORT(data + index_map + 10 + (unicode_codepoint - first)*2); + return 0; + } else if (format == 2) { + STBTT_assert(0); // @TODO: high-byte mapping for japanese/chinese/korean + return 0; + } else if (format == 4) { // standard mapping for windows fonts: binary search collection of ranges + stbtt_uint16 segcount = ttUSHORT(data+index_map+6) >> 1; + stbtt_uint16 searchRange = ttUSHORT(data+index_map+8) >> 1; + stbtt_uint16 entrySelector = ttUSHORT(data+index_map+10); + stbtt_uint16 rangeShift = ttUSHORT(data+index_map+12) >> 1; + + // do a binary search of the segments + stbtt_uint32 endCount = index_map + 14; + stbtt_uint32 search = endCount; + + if (unicode_codepoint > 0xffff) + return 0; + + // they lie from endCount .. endCount + segCount + // but searchRange is the nearest power of two, so... + if (unicode_codepoint >= ttUSHORT(data + search + rangeShift*2)) + search += rangeShift*2; + + // now decrement to bias correctly to find smallest + search -= 2; + while (entrySelector) { + stbtt_uint16 end; + searchRange >>= 1; + end = ttUSHORT(data + search + searchRange*2); + if (unicode_codepoint > end) + search += searchRange*2; + --entrySelector; + } + search += 2; + + { + stbtt_uint16 offset, start, last; + stbtt_uint16 item = (stbtt_uint16) ((search - endCount) >> 1); + + start = ttUSHORT(data + index_map + 14 + segcount*2 + 2 + 2*item); + last = ttUSHORT(data + endCount + 2*item); + if (unicode_codepoint < start || unicode_codepoint > last) + return 0; + + offset = ttUSHORT(data + index_map + 14 + segcount*6 + 2 + 2*item); + if (offset == 0) + return (stbtt_uint16) (unicode_codepoint + ttSHORT(data + index_map + 14 + segcount*4 + 2 + 2*item)); + + return ttUSHORT(data + offset + (unicode_codepoint-start)*2 + index_map + 14 + segcount*6 + 2 + 2*item); + } + } else if (format == 12 || format == 13) { + stbtt_uint32 ngroups = ttULONG(data+index_map+12); + stbtt_int32 low,high; + low = 0; high = (stbtt_int32)ngroups; + // Binary search the right group. + while (low < high) { + stbtt_int32 mid = low + ((high-low) >> 1); // rounds down, so low <= mid < high + stbtt_uint32 start_char = ttULONG(data+index_map+16+mid*12); + stbtt_uint32 end_char = ttULONG(data+index_map+16+mid*12+4); + if ((stbtt_uint32) unicode_codepoint < start_char) + high = mid; + else if ((stbtt_uint32) unicode_codepoint > end_char) + low = mid+1; + else { + stbtt_uint32 start_glyph = ttULONG(data+index_map+16+mid*12+8); + if (format == 12) + return start_glyph + unicode_codepoint-start_char; + else // format == 13 + return start_glyph; + } + } + return 0; // not found + } + // @TODO + STBTT_assert(0); + return 0; +} + +STBTT_DEF int stbtt_GetCodepointShape(const stbtt_fontinfo *info, int unicode_codepoint, stbtt_vertex **vertices) +{ + return stbtt_GetGlyphShape(info, stbtt_FindGlyphIndex(info, unicode_codepoint), vertices); +} + +static void stbtt_setvertex(stbtt_vertex *v, stbtt_uint8 type, stbtt_int32 x, stbtt_int32 y, stbtt_int32 cx, stbtt_int32 cy) +{ + v->type = type; + v->x = (stbtt_int16) x; + v->y = (stbtt_int16) y; + v->cx = (stbtt_int16) cx; + v->cy = (stbtt_int16) cy; +} + +static int stbtt__GetGlyfOffset(const stbtt_fontinfo *info, int glyph_index) +{ + int g1,g2; + + STBTT_assert(!info->cff.size); + + if (glyph_index >= info->numGlyphs) return -1; // glyph index out of range + if (info->indexToLocFormat >= 2) return -1; // unknown index->glyph map format + + if (info->indexToLocFormat == 0) { + g1 = info->glyf + ttUSHORT(info->data + info->loca + glyph_index * 2) * 2; + g2 = info->glyf + ttUSHORT(info->data + info->loca + glyph_index * 2 + 2) * 2; + } else { + g1 = info->glyf + ttULONG (info->data + info->loca + glyph_index * 4); + g2 = info->glyf + ttULONG (info->data + info->loca + glyph_index * 4 + 4); + } + + return g1==g2 ? -1 : g1; // if length is 0, return -1 +} + +static int stbtt__GetGlyphInfoT2(const stbtt_fontinfo *info, int glyph_index, int *x0, int *y0, int *x1, int *y1); + +STBTT_DEF int stbtt_GetGlyphBox(const stbtt_fontinfo *info, int glyph_index, int *x0, int *y0, int *x1, int *y1) +{ + if (info->cff.size) { + stbtt__GetGlyphInfoT2(info, glyph_index, x0, y0, x1, y1); + } else { + int g = stbtt__GetGlyfOffset(info, glyph_index); + if (g < 0) return 0; + + if (x0) *x0 = ttSHORT(info->data + g + 2); + if (y0) *y0 = ttSHORT(info->data + g + 4); + if (x1) *x1 = ttSHORT(info->data + g + 6); + if (y1) *y1 = ttSHORT(info->data + g + 8); + } + return 1; +} + +STBTT_DEF int stbtt_GetCodepointBox(const stbtt_fontinfo *info, int codepoint, int *x0, int *y0, int *x1, int *y1) +{ + return stbtt_GetGlyphBox(info, stbtt_FindGlyphIndex(info,codepoint), x0,y0,x1,y1); +} + +STBTT_DEF int stbtt_IsGlyphEmpty(const stbtt_fontinfo *info, int glyph_index) +{ + stbtt_int16 numberOfContours; + int g; + if (info->cff.size) + return stbtt__GetGlyphInfoT2(info, glyph_index, NULL, NULL, NULL, NULL) == 0; + g = stbtt__GetGlyfOffset(info, glyph_index); + if (g < 0) return 1; + numberOfContours = ttSHORT(info->data + g); + return numberOfContours == 0; +} + +static int stbtt__close_shape(stbtt_vertex *vertices, int num_vertices, int was_off, int start_off, + stbtt_int32 sx, stbtt_int32 sy, stbtt_int32 scx, stbtt_int32 scy, stbtt_int32 cx, stbtt_int32 cy) +{ + if (start_off) { + if (was_off) + stbtt_setvertex(&vertices[num_vertices++], STBTT_vcurve, (cx+scx)>>1, (cy+scy)>>1, cx,cy); + stbtt_setvertex(&vertices[num_vertices++], STBTT_vcurve, sx,sy,scx,scy); + } else { + if (was_off) + stbtt_setvertex(&vertices[num_vertices++], STBTT_vcurve,sx,sy,cx,cy); + else + stbtt_setvertex(&vertices[num_vertices++], STBTT_vline,sx,sy,0,0); + } + return num_vertices; +} + +static int stbtt__GetGlyphShapeTT(const stbtt_fontinfo *info, int glyph_index, stbtt_vertex **pvertices) +{ + stbtt_int16 numberOfContours; + stbtt_uint8 *endPtsOfContours; + stbtt_uint8 *data = info->data; + stbtt_vertex *vertices=0; + int num_vertices=0; + int g = stbtt__GetGlyfOffset(info, glyph_index); + + *pvertices = NULL; + + if (g < 0) return 0; + + numberOfContours = ttSHORT(data + g); + + if (numberOfContours > 0) { + stbtt_uint8 flags=0,flagcount; + stbtt_int32 ins, i,j=0,m,n, next_move, was_off=0, off, start_off=0; + stbtt_int32 x,y,cx,cy,sx,sy, scx,scy; + stbtt_uint8 *points; + endPtsOfContours = (data + g + 10); + ins = ttUSHORT(data + g + 10 + numberOfContours * 2); + points = data + g + 10 + numberOfContours * 2 + 2 + ins; + + n = 1+ttUSHORT(endPtsOfContours + numberOfContours*2-2); + + m = n + 2*numberOfContours; // a loose bound on how many vertices we might need + vertices = (stbtt_vertex *) STBTT_malloc(m * sizeof(vertices[0]), info->userdata); + if (vertices == 0) + return 0; + + next_move = 0; + flagcount=0; + + // in first pass, we load uninterpreted data into the allocated array + // above, shifted to the end of the array so we won't overwrite it when + // we create our final data starting from the front + + off = m - n; // starting offset for uninterpreted data, regardless of how m ends up being calculated + + // first load flags + + for (i=0; i < n; ++i) { + if (flagcount == 0) { + flags = *points++; + if (flags & 8) + flagcount = *points++; + } else + --flagcount; + vertices[off+i].type = flags; + } + + // now load x coordinates + x=0; + for (i=0; i < n; ++i) { + flags = vertices[off+i].type; + if (flags & 2) { + stbtt_int16 dx = *points++; + x += (flags & 16) ? dx : -dx; // ??? + } else { + if (!(flags & 16)) { + x = x + (stbtt_int16) (points[0]*256 + points[1]); + points += 2; + } + } + vertices[off+i].x = (stbtt_int16) x; + } + + // now load y coordinates + y=0; + for (i=0; i < n; ++i) { + flags = vertices[off+i].type; + if (flags & 4) { + stbtt_int16 dy = *points++; + y += (flags & 32) ? dy : -dy; // ??? + } else { + if (!(flags & 32)) { + y = y + (stbtt_int16) (points[0]*256 + points[1]); + points += 2; + } + } + vertices[off+i].y = (stbtt_int16) y; + } + + // now convert them to our format + num_vertices=0; + sx = sy = cx = cy = scx = scy = 0; + for (i=0; i < n; ++i) { + flags = vertices[off+i].type; + x = (stbtt_int16) vertices[off+i].x; + y = (stbtt_int16) vertices[off+i].y; + + if (next_move == i) { + if (i != 0) + num_vertices = stbtt__close_shape(vertices, num_vertices, was_off, start_off, sx,sy,scx,scy,cx,cy); + + // now start the new one + start_off = !(flags & 1); + if (start_off) { + // if we start off with an off-curve point, then when we need to find a point on the curve + // where we can start, and we need to save some state for when we wraparound. + scx = x; + scy = y; + if (!(vertices[off+i+1].type & 1)) { + // next point is also a curve point, so interpolate an on-point curve + sx = (x + (stbtt_int32) vertices[off+i+1].x) >> 1; + sy = (y + (stbtt_int32) vertices[off+i+1].y) >> 1; + } else { + // otherwise just use the next point as our start point + sx = (stbtt_int32) vertices[off+i+1].x; + sy = (stbtt_int32) vertices[off+i+1].y; + ++i; // we're using point i+1 as the starting point, so skip it + } + } else { + sx = x; + sy = y; + } + stbtt_setvertex(&vertices[num_vertices++], STBTT_vmove,sx,sy,0,0); + was_off = 0; + next_move = 1 + ttUSHORT(endPtsOfContours+j*2); + ++j; + } else { + if (!(flags & 1)) { // if it's a curve + if (was_off) // two off-curve control points in a row means interpolate an on-curve midpoint + stbtt_setvertex(&vertices[num_vertices++], STBTT_vcurve, (cx+x)>>1, (cy+y)>>1, cx, cy); + cx = x; + cy = y; + was_off = 1; + } else { + if (was_off) + stbtt_setvertex(&vertices[num_vertices++], STBTT_vcurve, x,y, cx, cy); + else + stbtt_setvertex(&vertices[num_vertices++], STBTT_vline, x,y,0,0); + was_off = 0; + } + } + } + num_vertices = stbtt__close_shape(vertices, num_vertices, was_off, start_off, sx,sy,scx,scy,cx,cy); + } else if (numberOfContours < 0) { + // Compound shapes. + int more = 1; + stbtt_uint8 *comp = data + g + 10; + num_vertices = 0; + vertices = 0; + while (more) { + stbtt_uint16 flags, gidx; + int comp_num_verts = 0, i; + stbtt_vertex *comp_verts = 0, *tmp = 0; + float mtx[6] = {1,0,0,1,0,0}, m, n; + + flags = ttSHORT(comp); comp+=2; + gidx = ttSHORT(comp); comp+=2; + + if (flags & 2) { // XY values + if (flags & 1) { // shorts + mtx[4] = ttSHORT(comp); comp+=2; + mtx[5] = ttSHORT(comp); comp+=2; + } else { + mtx[4] = ttCHAR(comp); comp+=1; + mtx[5] = ttCHAR(comp); comp+=1; + } + } + else { + // @TODO handle matching point + STBTT_assert(0); + } + if (flags & (1<<3)) { // WE_HAVE_A_SCALE + mtx[0] = mtx[3] = ttSHORT(comp)/16384.0f; comp+=2; + mtx[1] = mtx[2] = 0; + } else if (flags & (1<<6)) { // WE_HAVE_AN_X_AND_YSCALE + mtx[0] = ttSHORT(comp)/16384.0f; comp+=2; + mtx[1] = mtx[2] = 0; + mtx[3] = ttSHORT(comp)/16384.0f; comp+=2; + } else if (flags & (1<<7)) { // WE_HAVE_A_TWO_BY_TWO + mtx[0] = ttSHORT(comp)/16384.0f; comp+=2; + mtx[1] = ttSHORT(comp)/16384.0f; comp+=2; + mtx[2] = ttSHORT(comp)/16384.0f; comp+=2; + mtx[3] = ttSHORT(comp)/16384.0f; comp+=2; + } + + // Find transformation scales. + m = (float) STBTT_sqrt(mtx[0]*mtx[0] + mtx[1]*mtx[1]); + n = (float) STBTT_sqrt(mtx[2]*mtx[2] + mtx[3]*mtx[3]); + + // Get indexed glyph. + comp_num_verts = stbtt_GetGlyphShape(info, gidx, &comp_verts); + if (comp_num_verts > 0) { + // Transform vertices. + for (i = 0; i < comp_num_verts; ++i) { + stbtt_vertex* v = &comp_verts[i]; + stbtt_vertex_type x,y; + x=v->x; y=v->y; + v->x = (stbtt_vertex_type)(m * (mtx[0]*x + mtx[2]*y + mtx[4])); + v->y = (stbtt_vertex_type)(n * (mtx[1]*x + mtx[3]*y + mtx[5])); + x=v->cx; y=v->cy; + v->cx = (stbtt_vertex_type)(m * (mtx[0]*x + mtx[2]*y + mtx[4])); + v->cy = (stbtt_vertex_type)(n * (mtx[1]*x + mtx[3]*y + mtx[5])); + } + // Append vertices. + tmp = (stbtt_vertex*)STBTT_malloc((num_vertices+comp_num_verts)*sizeof(stbtt_vertex), info->userdata); + if (!tmp) { + if (vertices) STBTT_free(vertices, info->userdata); + if (comp_verts) STBTT_free(comp_verts, info->userdata); + return 0; + } + if (num_vertices > 0 && vertices) STBTT_memcpy(tmp, vertices, num_vertices*sizeof(stbtt_vertex)); + STBTT_memcpy(tmp+num_vertices, comp_verts, comp_num_verts*sizeof(stbtt_vertex)); + if (vertices) STBTT_free(vertices, info->userdata); + vertices = tmp; + STBTT_free(comp_verts, info->userdata); + num_vertices += comp_num_verts; + } + // More components ? + more = flags & (1<<5); + } + } else { + // numberOfCounters == 0, do nothing + } + + *pvertices = vertices; + return num_vertices; +} + +typedef struct +{ + int bounds; + int started; + float first_x, first_y; + float x, y; + stbtt_int32 min_x, max_x, min_y, max_y; + + stbtt_vertex *pvertices; + int num_vertices; +} stbtt__csctx; + +#define STBTT__CSCTX_INIT(bounds) {bounds,0, 0,0, 0,0, 0,0,0,0, NULL, 0} + +static void stbtt__track_vertex(stbtt__csctx *c, stbtt_int32 x, stbtt_int32 y) +{ + if (x > c->max_x || !c->started) c->max_x = x; + if (y > c->max_y || !c->started) c->max_y = y; + if (x < c->min_x || !c->started) c->min_x = x; + if (y < c->min_y || !c->started) c->min_y = y; + c->started = 1; +} + +static void stbtt__csctx_v(stbtt__csctx *c, stbtt_uint8 type, stbtt_int32 x, stbtt_int32 y, stbtt_int32 cx, stbtt_int32 cy, stbtt_int32 cx1, stbtt_int32 cy1) +{ + if (c->bounds) { + stbtt__track_vertex(c, x, y); + if (type == STBTT_vcubic) { + stbtt__track_vertex(c, cx, cy); + stbtt__track_vertex(c, cx1, cy1); + } + } else { + stbtt_setvertex(&c->pvertices[c->num_vertices], type, x, y, cx, cy); + c->pvertices[c->num_vertices].cx1 = (stbtt_int16) cx1; + c->pvertices[c->num_vertices].cy1 = (stbtt_int16) cy1; + } + c->num_vertices++; +} + +static void stbtt__csctx_close_shape(stbtt__csctx *ctx) +{ + if (ctx->first_x != ctx->x || ctx->first_y != ctx->y) + stbtt__csctx_v(ctx, STBTT_vline, (int)ctx->first_x, (int)ctx->first_y, 0, 0, 0, 0); +} + +static void stbtt__csctx_rmove_to(stbtt__csctx *ctx, float dx, float dy) +{ + stbtt__csctx_close_shape(ctx); + ctx->first_x = ctx->x = ctx->x + dx; + ctx->first_y = ctx->y = ctx->y + dy; + stbtt__csctx_v(ctx, STBTT_vmove, (int)ctx->x, (int)ctx->y, 0, 0, 0, 0); +} + +static void stbtt__csctx_rline_to(stbtt__csctx *ctx, float dx, float dy) +{ + ctx->x += dx; + ctx->y += dy; + stbtt__csctx_v(ctx, STBTT_vline, (int)ctx->x, (int)ctx->y, 0, 0, 0, 0); +} + +static void stbtt__csctx_rccurve_to(stbtt__csctx *ctx, float dx1, float dy1, float dx2, float dy2, float dx3, float dy3) +{ + float cx1 = ctx->x + dx1; + float cy1 = ctx->y + dy1; + float cx2 = cx1 + dx2; + float cy2 = cy1 + dy2; + ctx->x = cx2 + dx3; + ctx->y = cy2 + dy3; + stbtt__csctx_v(ctx, STBTT_vcubic, (int)ctx->x, (int)ctx->y, (int)cx1, (int)cy1, (int)cx2, (int)cy2); +} + +static stbtt__buf stbtt__get_subr(stbtt__buf idx, int n) +{ + int count = stbtt__cff_index_count(&idx); + int bias = 107; + if (count >= 33900) + bias = 32768; + else if (count >= 1240) + bias = 1131; + n += bias; + if (n < 0 || n >= count) + return stbtt__new_buf(NULL, 0); + return stbtt__cff_index_get(idx, n); +} + +static stbtt__buf stbtt__cid_get_glyph_subrs(const stbtt_fontinfo *info, int glyph_index) +{ + stbtt__buf fdselect = info->fdselect; + int nranges, start, end, v, fmt, fdselector = -1, i; + + stbtt__buf_seek(&fdselect, 0); + fmt = stbtt__buf_get8(&fdselect); + if (fmt == 0) { + // untested + stbtt__buf_skip(&fdselect, glyph_index); + fdselector = stbtt__buf_get8(&fdselect); + } else if (fmt == 3) { + nranges = stbtt__buf_get16(&fdselect); + start = stbtt__buf_get16(&fdselect); + for (i = 0; i < nranges; i++) { + v = stbtt__buf_get8(&fdselect); + end = stbtt__buf_get16(&fdselect); + if (glyph_index >= start && glyph_index < end) { + fdselector = v; + break; + } + start = end; + } + } + if (fdselector == -1) stbtt__new_buf(NULL, 0); + return stbtt__get_subrs(info->cff, stbtt__cff_index_get(info->fontdicts, fdselector)); +} + +static int stbtt__run_charstring(const stbtt_fontinfo *info, int glyph_index, stbtt__csctx *c) +{ + int in_header = 1, maskbits = 0, subr_stack_height = 0, sp = 0, v, i, b0; + int has_subrs = 0, clear_stack; + float s[48]; + stbtt__buf subr_stack[10], subrs = info->subrs, b; + float f; + +#define STBTT__CSERR(s) (0) + + // this currently ignores the initial width value, which isn't needed if we have hmtx + b = stbtt__cff_index_get(info->charstrings, glyph_index); + while (b.cursor < b.size) { + i = 0; + clear_stack = 1; + b0 = stbtt__buf_get8(&b); + switch (b0) { + // @TODO implement hinting + case 0x13: // hintmask + case 0x14: // cntrmask + if (in_header) + maskbits += (sp / 2); // implicit "vstem" + in_header = 0; + stbtt__buf_skip(&b, (maskbits + 7) / 8); + break; + + case 0x01: // hstem + case 0x03: // vstem + case 0x12: // hstemhm + case 0x17: // vstemhm + maskbits += (sp / 2); + break; + + case 0x15: // rmoveto + in_header = 0; + if (sp < 2) return STBTT__CSERR("rmoveto stack"); + stbtt__csctx_rmove_to(c, s[sp-2], s[sp-1]); + break; + case 0x04: // vmoveto + in_header = 0; + if (sp < 1) return STBTT__CSERR("vmoveto stack"); + stbtt__csctx_rmove_to(c, 0, s[sp-1]); + break; + case 0x16: // hmoveto + in_header = 0; + if (sp < 1) return STBTT__CSERR("hmoveto stack"); + stbtt__csctx_rmove_to(c, s[sp-1], 0); + break; + + case 0x05: // rlineto + if (sp < 2) return STBTT__CSERR("rlineto stack"); + for (; i + 1 < sp; i += 2) + stbtt__csctx_rline_to(c, s[i], s[i+1]); + break; + + // hlineto/vlineto and vhcurveto/hvcurveto alternate horizontal and vertical + // starting from a different place. + + case 0x07: // vlineto + if (sp < 1) return STBTT__CSERR("vlineto stack"); + goto vlineto; + case 0x06: // hlineto + if (sp < 1) return STBTT__CSERR("hlineto stack"); + for (;;) { + if (i >= sp) break; + stbtt__csctx_rline_to(c, s[i], 0); + i++; + vlineto: + if (i >= sp) break; + stbtt__csctx_rline_to(c, 0, s[i]); + i++; + } + break; + + case 0x1F: // hvcurveto + if (sp < 4) return STBTT__CSERR("hvcurveto stack"); + goto hvcurveto; + case 0x1E: // vhcurveto + if (sp < 4) return STBTT__CSERR("vhcurveto stack"); + for (;;) { + if (i + 3 >= sp) break; + stbtt__csctx_rccurve_to(c, 0, s[i], s[i+1], s[i+2], s[i+3], (sp - i == 5) ? s[i + 4] : 0.0f); + i += 4; + hvcurveto: + if (i + 3 >= sp) break; + stbtt__csctx_rccurve_to(c, s[i], 0, s[i+1], s[i+2], (sp - i == 5) ? s[i+4] : 0.0f, s[i+3]); + i += 4; + } + break; + + case 0x08: // rrcurveto + if (sp < 6) return STBTT__CSERR("rcurveline stack"); + for (; i + 5 < sp; i += 6) + stbtt__csctx_rccurve_to(c, s[i], s[i+1], s[i+2], s[i+3], s[i+4], s[i+5]); + break; + + case 0x18: // rcurveline + if (sp < 8) return STBTT__CSERR("rcurveline stack"); + for (; i + 5 < sp - 2; i += 6) + stbtt__csctx_rccurve_to(c, s[i], s[i+1], s[i+2], s[i+3], s[i+4], s[i+5]); + if (i + 1 >= sp) return STBTT__CSERR("rcurveline stack"); + stbtt__csctx_rline_to(c, s[i], s[i+1]); + break; + + case 0x19: // rlinecurve + if (sp < 8) return STBTT__CSERR("rlinecurve stack"); + for (; i + 1 < sp - 6; i += 2) + stbtt__csctx_rline_to(c, s[i], s[i+1]); + if (i + 5 >= sp) return STBTT__CSERR("rlinecurve stack"); + stbtt__csctx_rccurve_to(c, s[i], s[i+1], s[i+2], s[i+3], s[i+4], s[i+5]); + break; + + case 0x1A: // vvcurveto + case 0x1B: // hhcurveto + if (sp < 4) return STBTT__CSERR("(vv|hh)curveto stack"); + f = 0.0; + if (sp & 1) { f = s[i]; i++; } + for (; i + 3 < sp; i += 4) { + if (b0 == 0x1B) + stbtt__csctx_rccurve_to(c, s[i], f, s[i+1], s[i+2], s[i+3], 0.0); + else + stbtt__csctx_rccurve_to(c, f, s[i], s[i+1], s[i+2], 0.0, s[i+3]); + f = 0.0; + } + break; + + case 0x0A: // callsubr + if (!has_subrs) { + if (info->fdselect.size) + subrs = stbtt__cid_get_glyph_subrs(info, glyph_index); + has_subrs = 1; + } + // FALLTHROUGH + case 0x1D: // callgsubr + if (sp < 1) return STBTT__CSERR("call(g|)subr stack"); + v = (int) s[--sp]; + if (subr_stack_height >= 10) return STBTT__CSERR("recursion limit"); + subr_stack[subr_stack_height++] = b; + b = stbtt__get_subr(b0 == 0x0A ? subrs : info->gsubrs, v); + if (b.size == 0) return STBTT__CSERR("subr not found"); + b.cursor = 0; + clear_stack = 0; + break; + + case 0x0B: // return + if (subr_stack_height <= 0) return STBTT__CSERR("return outside subr"); + b = subr_stack[--subr_stack_height]; + clear_stack = 0; + break; + + case 0x0E: // endchar + stbtt__csctx_close_shape(c); + return 1; + + case 0x0C: { // two-byte escape + float dx1, dx2, dx3, dx4, dx5, dx6, dy1, dy2, dy3, dy4, dy5, dy6; + float dx, dy; + int b1 = stbtt__buf_get8(&b); + switch (b1) { + // @TODO These "flex" implementations ignore the flex-depth and resolution, + // and always draw beziers. + case 0x22: // hflex + if (sp < 7) return STBTT__CSERR("hflex stack"); + dx1 = s[0]; + dx2 = s[1]; + dy2 = s[2]; + dx3 = s[3]; + dx4 = s[4]; + dx5 = s[5]; + dx6 = s[6]; + stbtt__csctx_rccurve_to(c, dx1, 0, dx2, dy2, dx3, 0); + stbtt__csctx_rccurve_to(c, dx4, 0, dx5, -dy2, dx6, 0); + break; + + case 0x23: // flex + if (sp < 13) return STBTT__CSERR("flex stack"); + dx1 = s[0]; + dy1 = s[1]; + dx2 = s[2]; + dy2 = s[3]; + dx3 = s[4]; + dy3 = s[5]; + dx4 = s[6]; + dy4 = s[7]; + dx5 = s[8]; + dy5 = s[9]; + dx6 = s[10]; + dy6 = s[11]; + //fd is s[12] + stbtt__csctx_rccurve_to(c, dx1, dy1, dx2, dy2, dx3, dy3); + stbtt__csctx_rccurve_to(c, dx4, dy4, dx5, dy5, dx6, dy6); + break; + + case 0x24: // hflex1 + if (sp < 9) return STBTT__CSERR("hflex1 stack"); + dx1 = s[0]; + dy1 = s[1]; + dx2 = s[2]; + dy2 = s[3]; + dx3 = s[4]; + dx4 = s[5]; + dx5 = s[6]; + dy5 = s[7]; + dx6 = s[8]; + stbtt__csctx_rccurve_to(c, dx1, dy1, dx2, dy2, dx3, 0); + stbtt__csctx_rccurve_to(c, dx4, 0, dx5, dy5, dx6, -(dy1+dy2+dy5)); + break; + + case 0x25: // flex1 + if (sp < 11) return STBTT__CSERR("flex1 stack"); + dx1 = s[0]; + dy1 = s[1]; + dx2 = s[2]; + dy2 = s[3]; + dx3 = s[4]; + dy3 = s[5]; + dx4 = s[6]; + dy4 = s[7]; + dx5 = s[8]; + dy5 = s[9]; + dx6 = dy6 = s[10]; + dx = dx1+dx2+dx3+dx4+dx5; + dy = dy1+dy2+dy3+dy4+dy5; + if (STBTT_fabs(dx) > STBTT_fabs(dy)) + dy6 = -dy; + else + dx6 = -dx; + stbtt__csctx_rccurve_to(c, dx1, dy1, dx2, dy2, dx3, dy3); + stbtt__csctx_rccurve_to(c, dx4, dy4, dx5, dy5, dx6, dy6); + break; + + default: + return STBTT__CSERR("unimplemented"); + } + } break; + + default: + if (b0 != 255 && b0 != 28 && b0 < 32) + return STBTT__CSERR("reserved operator"); + + // push immediate + if (b0 == 255) { + f = (float)(stbtt_int32)stbtt__buf_get32(&b) / 0x10000; + } else { + stbtt__buf_skip(&b, -1); + f = (float)(stbtt_int16)stbtt__cff_int(&b); + } + if (sp >= 48) return STBTT__CSERR("push stack overflow"); + s[sp++] = f; + clear_stack = 0; + break; + } + if (clear_stack) sp = 0; + } + return STBTT__CSERR("no endchar"); + +#undef STBTT__CSERR +} + +static int stbtt__GetGlyphShapeT2(const stbtt_fontinfo *info, int glyph_index, stbtt_vertex **pvertices) +{ + // runs the charstring twice, once to count and once to output (to avoid realloc) + stbtt__csctx count_ctx = STBTT__CSCTX_INIT(1); + stbtt__csctx output_ctx = STBTT__CSCTX_INIT(0); + if (stbtt__run_charstring(info, glyph_index, &count_ctx)) { + *pvertices = (stbtt_vertex*)STBTT_malloc(count_ctx.num_vertices*sizeof(stbtt_vertex), info->userdata); + output_ctx.pvertices = *pvertices; + if (stbtt__run_charstring(info, glyph_index, &output_ctx)) { + STBTT_assert(output_ctx.num_vertices == count_ctx.num_vertices); + return output_ctx.num_vertices; + } + } + *pvertices = NULL; + return 0; +} + +static int stbtt__GetGlyphInfoT2(const stbtt_fontinfo *info, int glyph_index, int *x0, int *y0, int *x1, int *y1) +{ + stbtt__csctx c = STBTT__CSCTX_INIT(1); + int r = stbtt__run_charstring(info, glyph_index, &c); + if (x0) *x0 = r ? c.min_x : 0; + if (y0) *y0 = r ? c.min_y : 0; + if (x1) *x1 = r ? c.max_x : 0; + if (y1) *y1 = r ? c.max_y : 0; + return r ? c.num_vertices : 0; +} + +STBTT_DEF int stbtt_GetGlyphShape(const stbtt_fontinfo *info, int glyph_index, stbtt_vertex **pvertices) +{ + if (!info->cff.size) + return stbtt__GetGlyphShapeTT(info, glyph_index, pvertices); + else + return stbtt__GetGlyphShapeT2(info, glyph_index, pvertices); +} + +STBTT_DEF void stbtt_GetGlyphHMetrics(const stbtt_fontinfo *info, int glyph_index, int *advanceWidth, int *leftSideBearing) +{ + stbtt_uint16 numOfLongHorMetrics = ttUSHORT(info->data+info->hhea + 34); + if (glyph_index < numOfLongHorMetrics) { + if (advanceWidth) *advanceWidth = ttSHORT(info->data + info->hmtx + 4*glyph_index); + if (leftSideBearing) *leftSideBearing = ttSHORT(info->data + info->hmtx + 4*glyph_index + 2); + } else { + if (advanceWidth) *advanceWidth = ttSHORT(info->data + info->hmtx + 4*(numOfLongHorMetrics-1)); + if (leftSideBearing) *leftSideBearing = ttSHORT(info->data + info->hmtx + 4*numOfLongHorMetrics + 2*(glyph_index - numOfLongHorMetrics)); + } +} + +STBTT_DEF int stbtt_GetKerningTableLength(const stbtt_fontinfo *info) +{ + stbtt_uint8 *data = info->data + info->kern; + + // we only look at the first table. it must be 'horizontal' and format 0. + if (!info->kern) + return 0; + if (ttUSHORT(data+2) < 1) // number of tables, need at least 1 + return 0; + if (ttUSHORT(data+8) != 1) // horizontal flag must be set in format + return 0; + + return ttUSHORT(data+10); +} + +STBTT_DEF int stbtt_GetKerningTable(const stbtt_fontinfo *info, stbtt_kerningentry* table, int table_length) +{ + stbtt_uint8 *data = info->data + info->kern; + int k, length; + + // we only look at the first table. it must be 'horizontal' and format 0. + if (!info->kern) + return 0; + if (ttUSHORT(data+2) < 1) // number of tables, need at least 1 + return 0; + if (ttUSHORT(data+8) != 1) // horizontal flag must be set in format + return 0; + + length = ttUSHORT(data+10); + if (table_length < length) + length = table_length; + + for (k = 0; k < length; k++) + { + table[k].glyph1 = ttUSHORT(data+18+(k*6)); + table[k].glyph2 = ttUSHORT(data+20+(k*6)); + table[k].advance = ttSHORT(data+22+(k*6)); + } + + return length; +} + +static int stbtt__GetGlyphKernInfoAdvance(const stbtt_fontinfo *info, int glyph1, int glyph2) +{ + stbtt_uint8 *data = info->data + info->kern; + stbtt_uint32 needle, straw; + int l, r, m; + + // we only look at the first table. it must be 'horizontal' and format 0. + if (!info->kern) + return 0; + if (ttUSHORT(data+2) < 1) // number of tables, need at least 1 + return 0; + if (ttUSHORT(data+8) != 1) // horizontal flag must be set in format + return 0; + + l = 0; + r = ttUSHORT(data+10) - 1; + needle = glyph1 << 16 | glyph2; + while (l <= r) { + m = (l + r) >> 1; + straw = ttULONG(data+18+(m*6)); // note: unaligned read + if (needle < straw) + r = m - 1; + else if (needle > straw) + l = m + 1; + else + return ttSHORT(data+22+(m*6)); + } + return 0; +} + +static stbtt_int32 stbtt__GetCoverageIndex(stbtt_uint8 *coverageTable, int glyph) +{ + stbtt_uint16 coverageFormat = ttUSHORT(coverageTable); + switch (coverageFormat) { + case 1: { + stbtt_uint16 glyphCount = ttUSHORT(coverageTable + 2); + + // Binary search. + stbtt_int32 l=0, r=glyphCount-1, m; + int straw, needle=glyph; + while (l <= r) { + stbtt_uint8 *glyphArray = coverageTable + 4; + stbtt_uint16 glyphID; + m = (l + r) >> 1; + glyphID = ttUSHORT(glyphArray + 2 * m); + straw = glyphID; + if (needle < straw) + r = m - 1; + else if (needle > straw) + l = m + 1; + else { + return m; + } + } + break; + } + + case 2: { + stbtt_uint16 rangeCount = ttUSHORT(coverageTable + 2); + stbtt_uint8 *rangeArray = coverageTable + 4; + + // Binary search. + stbtt_int32 l=0, r=rangeCount-1, m; + int strawStart, strawEnd, needle=glyph; + while (l <= r) { + stbtt_uint8 *rangeRecord; + m = (l + r) >> 1; + rangeRecord = rangeArray + 6 * m; + strawStart = ttUSHORT(rangeRecord); + strawEnd = ttUSHORT(rangeRecord + 2); + if (needle < strawStart) + r = m - 1; + else if (needle > strawEnd) + l = m + 1; + else { + stbtt_uint16 startCoverageIndex = ttUSHORT(rangeRecord + 4); + return startCoverageIndex + glyph - strawStart; + } + } + break; + } + + default: return -1; // unsupported + } + + return -1; +} + +static stbtt_int32 stbtt__GetGlyphClass(stbtt_uint8 *classDefTable, int glyph) +{ + stbtt_uint16 classDefFormat = ttUSHORT(classDefTable); + switch (classDefFormat) + { + case 1: { + stbtt_uint16 startGlyphID = ttUSHORT(classDefTable + 2); + stbtt_uint16 glyphCount = ttUSHORT(classDefTable + 4); + stbtt_uint8 *classDef1ValueArray = classDefTable + 6; + + if (glyph >= startGlyphID && glyph < startGlyphID + glyphCount) + return (stbtt_int32)ttUSHORT(classDef1ValueArray + 2 * (glyph - startGlyphID)); + break; + } + + case 2: { + stbtt_uint16 classRangeCount = ttUSHORT(classDefTable + 2); + stbtt_uint8 *classRangeRecords = classDefTable + 4; + + // Binary search. + stbtt_int32 l=0, r=classRangeCount-1, m; + int strawStart, strawEnd, needle=glyph; + while (l <= r) { + stbtt_uint8 *classRangeRecord; + m = (l + r) >> 1; + classRangeRecord = classRangeRecords + 6 * m; + strawStart = ttUSHORT(classRangeRecord); + strawEnd = ttUSHORT(classRangeRecord + 2); + if (needle < strawStart) + r = m - 1; + else if (needle > strawEnd) + l = m + 1; + else + return (stbtt_int32)ttUSHORT(classRangeRecord + 4); + } + break; + } + + default: + return -1; // Unsupported definition type, return an error. + } + + // "All glyphs not assigned to a class fall into class 0". (OpenType spec) + return 0; +} + +// Define to STBTT_assert(x) if you want to break on unimplemented formats. +#define STBTT_GPOS_TODO_assert(x) + +static stbtt_int32 stbtt__GetGlyphGPOSInfoAdvance(const stbtt_fontinfo *info, int glyph1, int glyph2) +{ + stbtt_uint16 lookupListOffset; + stbtt_uint8 *lookupList; + stbtt_uint16 lookupCount; + stbtt_uint8 *data; + stbtt_int32 i, sti; + + if (!info->gpos) return 0; + + data = info->data + info->gpos; + + if (ttUSHORT(data+0) != 1) return 0; // Major version 1 + if (ttUSHORT(data+2) != 0) return 0; // Minor version 0 + + lookupListOffset = ttUSHORT(data+8); + lookupList = data + lookupListOffset; + lookupCount = ttUSHORT(lookupList); + + for (i=0; i= pairSetCount) return 0; + + needle=glyph2; + r=pairValueCount-1; + l=0; + + // Binary search. + while (l <= r) { + stbtt_uint16 secondGlyph; + stbtt_uint8 *pairValue; + m = (l + r) >> 1; + pairValue = pairValueArray + (2 + valueRecordPairSizeInBytes) * m; + secondGlyph = ttUSHORT(pairValue); + straw = secondGlyph; + if (needle < straw) + r = m - 1; + else if (needle > straw) + l = m + 1; + else { + stbtt_int16 xAdvance = ttSHORT(pairValue + 2); + return xAdvance; + } + } + } else + return 0; + break; + } + + case 2: { + stbtt_uint16 valueFormat1 = ttUSHORT(table + 4); + stbtt_uint16 valueFormat2 = ttUSHORT(table + 6); + if (valueFormat1 == 4 && valueFormat2 == 0) { // Support more formats? + stbtt_uint16 classDef1Offset = ttUSHORT(table + 8); + stbtt_uint16 classDef2Offset = ttUSHORT(table + 10); + int glyph1class = stbtt__GetGlyphClass(table + classDef1Offset, glyph1); + int glyph2class = stbtt__GetGlyphClass(table + classDef2Offset, glyph2); + + stbtt_uint16 class1Count = ttUSHORT(table + 12); + stbtt_uint16 class2Count = ttUSHORT(table + 14); + stbtt_uint8 *class1Records, *class2Records; + stbtt_int16 xAdvance; + + if (glyph1class < 0 || glyph1class >= class1Count) return 0; // malformed + if (glyph2class < 0 || glyph2class >= class2Count) return 0; // malformed + + class1Records = table + 16; + class2Records = class1Records + 2 * (glyph1class * class2Count); + xAdvance = ttSHORT(class2Records + 2 * glyph2class); + return xAdvance; + } else + return 0; + break; + } + + default: + return 0; // Unsupported position format + } + } + } + + return 0; +} + +STBTT_DEF int stbtt_GetGlyphKernAdvance(const stbtt_fontinfo *info, int g1, int g2) +{ + int xAdvance = 0; + + if (info->gpos) + xAdvance += stbtt__GetGlyphGPOSInfoAdvance(info, g1, g2); + else if (info->kern) + xAdvance += stbtt__GetGlyphKernInfoAdvance(info, g1, g2); + + return xAdvance; +} + +STBTT_DEF int stbtt_GetCodepointKernAdvance(const stbtt_fontinfo *info, int ch1, int ch2) +{ + if (!info->kern && !info->gpos) // if no kerning table, don't waste time looking up both codepoint->glyphs + return 0; + return stbtt_GetGlyphKernAdvance(info, stbtt_FindGlyphIndex(info,ch1), stbtt_FindGlyphIndex(info,ch2)); +} + +STBTT_DEF void stbtt_GetCodepointHMetrics(const stbtt_fontinfo *info, int codepoint, int *advanceWidth, int *leftSideBearing) +{ + stbtt_GetGlyphHMetrics(info, stbtt_FindGlyphIndex(info,codepoint), advanceWidth, leftSideBearing); +} + +STBTT_DEF void stbtt_GetFontVMetrics(const stbtt_fontinfo *info, int *ascent, int *descent, int *lineGap) +{ + if (ascent ) *ascent = ttSHORT(info->data+info->hhea + 4); + if (descent) *descent = ttSHORT(info->data+info->hhea + 6); + if (lineGap) *lineGap = ttSHORT(info->data+info->hhea + 8); +} + +STBTT_DEF int stbtt_GetFontVMetricsOS2(const stbtt_fontinfo *info, int *typoAscent, int *typoDescent, int *typoLineGap) +{ + int tab = stbtt__find_table(info->data, info->fontstart, "OS/2"); + if (!tab) + return 0; + if (typoAscent ) *typoAscent = ttSHORT(info->data+tab + 68); + if (typoDescent) *typoDescent = ttSHORT(info->data+tab + 70); + if (typoLineGap) *typoLineGap = ttSHORT(info->data+tab + 72); + return 1; +} + +STBTT_DEF void stbtt_GetFontBoundingBox(const stbtt_fontinfo *info, int *x0, int *y0, int *x1, int *y1) +{ + *x0 = ttSHORT(info->data + info->head + 36); + *y0 = ttSHORT(info->data + info->head + 38); + *x1 = ttSHORT(info->data + info->head + 40); + *y1 = ttSHORT(info->data + info->head + 42); +} + +STBTT_DEF float stbtt_ScaleForPixelHeight(const stbtt_fontinfo *info, float height) +{ + int fheight = ttSHORT(info->data + info->hhea + 4) - ttSHORT(info->data + info->hhea + 6); + return (float) height / fheight; +} + +STBTT_DEF float stbtt_ScaleForMappingEmToPixels(const stbtt_fontinfo *info, float pixels) +{ + int unitsPerEm = ttUSHORT(info->data + info->head + 18); + return pixels / unitsPerEm; +} + +STBTT_DEF void stbtt_FreeShape(const stbtt_fontinfo *info, stbtt_vertex *v) +{ + STBTT_free(v, info->userdata); +} + +STBTT_DEF stbtt_uint8 *stbtt_FindSVGDoc(const stbtt_fontinfo *info, int gl) +{ + int i; + stbtt_uint8 *data = info->data; + stbtt_uint8 *svg_doc_list = data + stbtt__get_svg((stbtt_fontinfo *) info); + + int numEntries = ttUSHORT(svg_doc_list); + stbtt_uint8 *svg_docs = svg_doc_list + 2; + + for(i=0; i= ttUSHORT(svg_doc)) && (gl <= ttUSHORT(svg_doc + 2))) + return svg_doc; + } + return 0; +} + +STBTT_DEF int stbtt_GetGlyphSVG(const stbtt_fontinfo *info, int gl, const char **svg) +{ + stbtt_uint8 *data = info->data; + stbtt_uint8 *svg_doc; + + if (info->svg == 0) + return 0; + + svg_doc = stbtt_FindSVGDoc(info, gl); + if (svg_doc != NULL) { + *svg = (char *) data + info->svg + ttULONG(svg_doc + 4); + return ttULONG(svg_doc + 8); + } else { + return 0; + } +} + +STBTT_DEF int stbtt_GetCodepointSVG(const stbtt_fontinfo *info, int unicode_codepoint, const char **svg) +{ + return stbtt_GetGlyphSVG(info, stbtt_FindGlyphIndex(info, unicode_codepoint), svg); +} + +////////////////////////////////////////////////////////////////////////////// +// +// antialiasing software rasterizer +// + +STBTT_DEF void stbtt_GetGlyphBitmapBoxSubpixel(const stbtt_fontinfo *font, int glyph, float scale_x, float scale_y,float shift_x, float shift_y, int *ix0, int *iy0, int *ix1, int *iy1) +{ + int x0=0,y0=0,x1,y1; // =0 suppresses compiler warning + if (!stbtt_GetGlyphBox(font, glyph, &x0,&y0,&x1,&y1)) { + // e.g. space character + if (ix0) *ix0 = 0; + if (iy0) *iy0 = 0; + if (ix1) *ix1 = 0; + if (iy1) *iy1 = 0; + } else { + // move to integral bboxes (treating pixels as little squares, what pixels get touched)? + if (ix0) *ix0 = STBTT_ifloor( x0 * scale_x + shift_x); + if (iy0) *iy0 = STBTT_ifloor(-y1 * scale_y + shift_y); + if (ix1) *ix1 = STBTT_iceil ( x1 * scale_x + shift_x); + if (iy1) *iy1 = STBTT_iceil (-y0 * scale_y + shift_y); + } +} + +STBTT_DEF void stbtt_GetGlyphBitmapBox(const stbtt_fontinfo *font, int glyph, float scale_x, float scale_y, int *ix0, int *iy0, int *ix1, int *iy1) +{ + stbtt_GetGlyphBitmapBoxSubpixel(font, glyph, scale_x, scale_y,0.0f,0.0f, ix0, iy0, ix1, iy1); +} + +STBTT_DEF void stbtt_GetCodepointBitmapBoxSubpixel(const stbtt_fontinfo *font, int codepoint, float scale_x, float scale_y, float shift_x, float shift_y, int *ix0, int *iy0, int *ix1, int *iy1) +{ + stbtt_GetGlyphBitmapBoxSubpixel(font, stbtt_FindGlyphIndex(font,codepoint), scale_x, scale_y,shift_x,shift_y, ix0,iy0,ix1,iy1); +} + +STBTT_DEF void stbtt_GetCodepointBitmapBox(const stbtt_fontinfo *font, int codepoint, float scale_x, float scale_y, int *ix0, int *iy0, int *ix1, int *iy1) +{ + stbtt_GetCodepointBitmapBoxSubpixel(font, codepoint, scale_x, scale_y,0.0f,0.0f, ix0,iy0,ix1,iy1); +} + +////////////////////////////////////////////////////////////////////////////// +// +// Rasterizer + +typedef struct stbtt__hheap_chunk +{ + struct stbtt__hheap_chunk *next; +} stbtt__hheap_chunk; + +typedef struct stbtt__hheap +{ + struct stbtt__hheap_chunk *head; + void *first_free; + int num_remaining_in_head_chunk; +} stbtt__hheap; + +static void *stbtt__hheap_alloc(stbtt__hheap *hh, size_t size, void *userdata) +{ + if (hh->first_free) { + void *p = hh->first_free; + hh->first_free = * (void **) p; + return p; + } else { + if (hh->num_remaining_in_head_chunk == 0) { + int count = (size < 32 ? 2000 : size < 128 ? 800 : 100); + stbtt__hheap_chunk *c = (stbtt__hheap_chunk *) STBTT_malloc(sizeof(stbtt__hheap_chunk) + size * count, userdata); + if (c == NULL) + return NULL; + c->next = hh->head; + hh->head = c; + hh->num_remaining_in_head_chunk = count; + } + --hh->num_remaining_in_head_chunk; + return (char *) (hh->head) + sizeof(stbtt__hheap_chunk) + size * hh->num_remaining_in_head_chunk; + } +} + +static void stbtt__hheap_free(stbtt__hheap *hh, void *p) +{ + *(void **) p = hh->first_free; + hh->first_free = p; +} + +static void stbtt__hheap_cleanup(stbtt__hheap *hh, void *userdata) +{ + stbtt__hheap_chunk *c = hh->head; + while (c) { + stbtt__hheap_chunk *n = c->next; + STBTT_free(c, userdata); + c = n; + } +} + +typedef struct stbtt__edge { + float x0,y0, x1,y1; + int invert; +} stbtt__edge; + + +typedef struct stbtt__active_edge +{ + struct stbtt__active_edge *next; + #if STBTT_RASTERIZER_VERSION==1 + int x,dx; + float ey; + int direction; + #elif STBTT_RASTERIZER_VERSION==2 + float fx,fdx,fdy; + float direction; + float sy; + float ey; + #else + #error "Unrecognized value of STBTT_RASTERIZER_VERSION" + #endif +} stbtt__active_edge; + +#if STBTT_RASTERIZER_VERSION == 1 +#define STBTT_FIXSHIFT 10 +#define STBTT_FIX (1 << STBTT_FIXSHIFT) +#define STBTT_FIXMASK (STBTT_FIX-1) + +static stbtt__active_edge *stbtt__new_active(stbtt__hheap *hh, stbtt__edge *e, int off_x, float start_point, void *userdata) +{ + stbtt__active_edge *z = (stbtt__active_edge *) stbtt__hheap_alloc(hh, sizeof(*z), userdata); + float dxdy = (e->x1 - e->x0) / (e->y1 - e->y0); + STBTT_assert(z != NULL); + if (!z) return z; + + // round dx down to avoid overshooting + if (dxdy < 0) + z->dx = -STBTT_ifloor(STBTT_FIX * -dxdy); + else + z->dx = STBTT_ifloor(STBTT_FIX * dxdy); + + z->x = STBTT_ifloor(STBTT_FIX * e->x0 + z->dx * (start_point - e->y0)); // use z->dx so when we offset later it's by the same amount + z->x -= off_x * STBTT_FIX; + + z->ey = e->y1; + z->next = 0; + z->direction = e->invert ? 1 : -1; + return z; +} +#elif STBTT_RASTERIZER_VERSION == 2 +static stbtt__active_edge *stbtt__new_active(stbtt__hheap *hh, stbtt__edge *e, int off_x, float start_point, void *userdata) +{ + stbtt__active_edge *z = (stbtt__active_edge *) stbtt__hheap_alloc(hh, sizeof(*z), userdata); + float dxdy = (e->x1 - e->x0) / (e->y1 - e->y0); + STBTT_assert(z != NULL); + //STBTT_assert(e->y0 <= start_point); + if (!z) return z; + z->fdx = dxdy; + z->fdy = dxdy != 0.0f ? (1.0f/dxdy) : 0.0f; + z->fx = e->x0 + dxdy * (start_point - e->y0); + z->fx -= off_x; + z->direction = e->invert ? 1.0f : -1.0f; + z->sy = e->y0; + z->ey = e->y1; + z->next = 0; + return z; +} +#else +#error "Unrecognized value of STBTT_RASTERIZER_VERSION" +#endif + +#if STBTT_RASTERIZER_VERSION == 1 +// note: this routine clips fills that extend off the edges... ideally this +// wouldn't happen, but it could happen if the truetype glyph bounding boxes +// are wrong, or if the user supplies a too-small bitmap +static void stbtt__fill_active_edges(unsigned char *scanline, int len, stbtt__active_edge *e, int max_weight) +{ + // non-zero winding fill + int x0=0, w=0; + + while (e) { + if (w == 0) { + // if we're currently at zero, we need to record the edge start point + x0 = e->x; w += e->direction; + } else { + int x1 = e->x; w += e->direction; + // if we went to zero, we need to draw + if (w == 0) { + int i = x0 >> STBTT_FIXSHIFT; + int j = x1 >> STBTT_FIXSHIFT; + + if (i < len && j >= 0) { + if (i == j) { + // x0,x1 are the same pixel, so compute combined coverage + scanline[i] = scanline[i] + (stbtt_uint8) ((x1 - x0) * max_weight >> STBTT_FIXSHIFT); + } else { + if (i >= 0) // add antialiasing for x0 + scanline[i] = scanline[i] + (stbtt_uint8) (((STBTT_FIX - (x0 & STBTT_FIXMASK)) * max_weight) >> STBTT_FIXSHIFT); + else + i = -1; // clip + + if (j < len) // add antialiasing for x1 + scanline[j] = scanline[j] + (stbtt_uint8) (((x1 & STBTT_FIXMASK) * max_weight) >> STBTT_FIXSHIFT); + else + j = len; // clip + + for (++i; i < j; ++i) // fill pixels between x0 and x1 + scanline[i] = scanline[i] + (stbtt_uint8) max_weight; + } + } + } + } + + e = e->next; + } +} + +static void stbtt__rasterize_sorted_edges(stbtt__bitmap *result, stbtt__edge *e, int n, int vsubsample, int off_x, int off_y, void *userdata) +{ + stbtt__hheap hh = { 0, 0, 0 }; + stbtt__active_edge *active = NULL; + int y,j=0; + int max_weight = (255 / vsubsample); // weight per vertical scanline + int s; // vertical subsample index + unsigned char scanline_data[512], *scanline; + + if (result->w > 512) + scanline = (unsigned char *) STBTT_malloc(result->w, userdata); + else + scanline = scanline_data; + + y = off_y * vsubsample; + e[n].y0 = (off_y + result->h) * (float) vsubsample + 1; + + while (j < result->h) { + STBTT_memset(scanline, 0, result->w); + for (s=0; s < vsubsample; ++s) { + // find center of pixel for this scanline + float scan_y = y + 0.5f; + stbtt__active_edge **step = &active; + + // update all active edges; + // remove all active edges that terminate before the center of this scanline + while (*step) { + stbtt__active_edge * z = *step; + if (z->ey <= scan_y) { + *step = z->next; // delete from list + STBTT_assert(z->direction); + z->direction = 0; + stbtt__hheap_free(&hh, z); + } else { + z->x += z->dx; // advance to position for current scanline + step = &((*step)->next); // advance through list + } + } + + // resort the list if needed + for(;;) { + int changed=0; + step = &active; + while (*step && (*step)->next) { + if ((*step)->x > (*step)->next->x) { + stbtt__active_edge *t = *step; + stbtt__active_edge *q = t->next; + + t->next = q->next; + q->next = t; + *step = q; + changed = 1; + } + step = &(*step)->next; + } + if (!changed) break; + } + + // insert all edges that start before the center of this scanline -- omit ones that also end on this scanline + while (e->y0 <= scan_y) { + if (e->y1 > scan_y) { + stbtt__active_edge *z = stbtt__new_active(&hh, e, off_x, scan_y, userdata); + if (z != NULL) { + // find insertion point + if (active == NULL) + active = z; + else if (z->x < active->x) { + // insert at front + z->next = active; + active = z; + } else { + // find thing to insert AFTER + stbtt__active_edge *p = active; + while (p->next && p->next->x < z->x) + p = p->next; + // at this point, p->next->x is NOT < z->x + z->next = p->next; + p->next = z; + } + } + } + ++e; + } + + // now process all active edges in XOR fashion + if (active) + stbtt__fill_active_edges(scanline, result->w, active, max_weight); + + ++y; + } + STBTT_memcpy(result->pixels + j * result->stride, scanline, result->w); + ++j; + } + + stbtt__hheap_cleanup(&hh, userdata); + + if (scanline != scanline_data) + STBTT_free(scanline, userdata); +} + +#elif STBTT_RASTERIZER_VERSION == 2 + +// the edge passed in here does not cross the vertical line at x or the vertical line at x+1 +// (i.e. it has already been clipped to those) +static void stbtt__handle_clipped_edge(float *scanline, int x, stbtt__active_edge *e, float x0, float y0, float x1, float y1) +{ + if (y0 == y1) return; + STBTT_assert(y0 < y1); + STBTT_assert(e->sy <= e->ey); + if (y0 > e->ey) return; + if (y1 < e->sy) return; + if (y0 < e->sy) { + x0 += (x1-x0) * (e->sy - y0) / (y1-y0); + y0 = e->sy; + } + if (y1 > e->ey) { + x1 += (x1-x0) * (e->ey - y1) / (y1-y0); + y1 = e->ey; + } + + if (x0 == x) + STBTT_assert(x1 <= x+1); + else if (x0 == x+1) + STBTT_assert(x1 >= x); + else if (x0 <= x) + STBTT_assert(x1 <= x); + else if (x0 >= x+1) + STBTT_assert(x1 >= x+1); + else + STBTT_assert(x1 >= x && x1 <= x+1); + + if (x0 <= x && x1 <= x) + scanline[x] += e->direction * (y1-y0); + else if (x0 >= x+1 && x1 >= x+1) + ; + else { + STBTT_assert(x0 >= x && x0 <= x+1 && x1 >= x && x1 <= x+1); + scanline[x] += e->direction * (y1-y0) * (1-((x0-x)+(x1-x))/2); // coverage = 1 - average x position + } +} + +static float stbtt__sized_trapezoid_area(float height, float top_width, float bottom_width) +{ + STBTT_assert(top_width >= 0); + STBTT_assert(bottom_width >= 0); + return (top_width + bottom_width) / 2.0f * height; +} + +static float stbtt__position_trapezoid_area(float height, float tx0, float tx1, float bx0, float bx1) +{ + return stbtt__sized_trapezoid_area(height, tx1 - tx0, bx1 - bx0); +} + +static float stbtt__sized_triangle_area(float height, float width) +{ + return height * width / 2; +} + +static void stbtt__fill_active_edges_new(float *scanline, float *scanline_fill, int len, stbtt__active_edge *e, float y_top) +{ + float y_bottom = y_top+1; + + while (e) { + // brute force every pixel + + // compute intersection points with top & bottom + STBTT_assert(e->ey >= y_top); + + if (e->fdx == 0) { + float x0 = e->fx; + if (x0 < len) { + if (x0 >= 0) { + stbtt__handle_clipped_edge(scanline,(int) x0,e, x0,y_top, x0,y_bottom); + stbtt__handle_clipped_edge(scanline_fill-1,(int) x0+1,e, x0,y_top, x0,y_bottom); + } else { + stbtt__handle_clipped_edge(scanline_fill-1,0,e, x0,y_top, x0,y_bottom); + } + } + } else { + float x0 = e->fx; + float dx = e->fdx; + float xb = x0 + dx; + float x_top, x_bottom; + float sy0,sy1; + float dy = e->fdy; + STBTT_assert(e->sy <= y_bottom && e->ey >= y_top); + + // compute endpoints of line segment clipped to this scanline (if the + // line segment starts on this scanline. x0 is the intersection of the + // line with y_top, but that may be off the line segment. + if (e->sy > y_top) { + x_top = x0 + dx * (e->sy - y_top); + sy0 = e->sy; + } else { + x_top = x0; + sy0 = y_top; + } + if (e->ey < y_bottom) { + x_bottom = x0 + dx * (e->ey - y_top); + sy1 = e->ey; + } else { + x_bottom = xb; + sy1 = y_bottom; + } + + if (x_top >= 0 && x_bottom >= 0 && x_top < len && x_bottom < len) { + // from here on, we don't have to range check x values + + if ((int) x_top == (int) x_bottom) { + float height; + // simple case, only spans one pixel + int x = (int) x_top; + height = (sy1 - sy0) * e->direction; + STBTT_assert(x >= 0 && x < len); + scanline[x] += stbtt__position_trapezoid_area(height, x_top, x+1.0f, x_bottom, x+1.0f); + scanline_fill[x] += height; // everything right of this pixel is filled + } else { + int x,x1,x2; + float y_crossing, y_final, step, sign, area; + // covers 2+ pixels + if (x_top > x_bottom) { + // flip scanline vertically; signed area is the same + float t; + sy0 = y_bottom - (sy0 - y_top); + sy1 = y_bottom - (sy1 - y_top); + t = sy0, sy0 = sy1, sy1 = t; + t = x_bottom, x_bottom = x_top, x_top = t; + dx = -dx; + dy = -dy; + t = x0, x0 = xb, xb = t; + } + STBTT_assert(dy >= 0); + STBTT_assert(dx >= 0); + + x1 = (int) x_top; + x2 = (int) x_bottom; + // compute intersection with y axis at x1+1 + y_crossing = y_top + dy * (x1+1 - x0); + + // compute intersection with y axis at x2 + y_final = y_top + dy * (x2 - x0); + + // x1 x_top x2 x_bottom + // y_top +------|-----+------------+------------+--------|---+------------+ + // | | | | | | + // | | | | | | + // sy0 | Txxxxx|............|............|............|............| + // y_crossing | *xxxxx.......|............|............|............| + // | | xxxxx..|............|............|............| + // | | /- xx*xxxx........|............|............| + // | | dy < | xxxxxx..|............|............| + // y_final | | \- | xx*xxx.........|............| + // sy1 | | | | xxxxxB...|............| + // | | | | | | + // | | | | | | + // y_bottom +------------+------------+------------+------------+------------+ + // + // goal is to measure the area covered by '.' in each pixel + + // if x2 is right at the right edge of x1, y_crossing can blow up, github #1057 + // @TODO: maybe test against sy1 rather than y_bottom? + if (y_crossing > y_bottom) + y_crossing = y_bottom; + + sign = e->direction; + + // area of the rectangle covered from sy0..y_crossing + area = sign * (y_crossing-sy0); + + // area of the triangle (x_top,sy0), (x1+1,sy0), (x1+1,y_crossing) + scanline[x1] += stbtt__sized_triangle_area(area, x1+1 - x_top); + + // check if final y_crossing is blown up; no test case for this + if (y_final > y_bottom) { + y_final = y_bottom; + dy = (y_final - y_crossing ) / (x2 - (x1+1)); // if denom=0, y_final = y_crossing, so y_final <= y_bottom + } + + // in second pixel, area covered by line segment found in first pixel + // is always a rectangle 1 wide * the height of that line segment; this + // is exactly what the variable 'area' stores. it also gets a contribution + // from the line segment within it. the THIRD pixel will get the first + // pixel's rectangle contribution, the second pixel's rectangle contribution, + // and its own contribution. the 'own contribution' is the same in every pixel except + // the leftmost and rightmost, a trapezoid that slides down in each pixel. + // the second pixel's contribution to the third pixel will be the + // rectangle 1 wide times the height change in the second pixel, which is dy. + + step = sign * dy * 1; // dy is dy/dx, change in y for every 1 change in x, + // which multiplied by 1-pixel-width is how much pixel area changes for each step in x + // so the area advances by 'step' every time + + for (x = x1+1; x < x2; ++x) { + scanline[x] += area + step/2; // area of trapezoid is 1*step/2 + area += step; + } + STBTT_assert(STBTT_fabs(area) <= 1.01f); // accumulated error from area += step unless we round step down + STBTT_assert(sy1 > y_final-0.01f); + + // area covered in the last pixel is the rectangle from all the pixels to the left, + // plus the trapezoid filled by the line segment in this pixel all the way to the right edge + scanline[x2] += area + sign * stbtt__position_trapezoid_area(sy1-y_final, (float) x2, x2+1.0f, x_bottom, x2+1.0f); + + // the rest of the line is filled based on the total height of the line segment in this pixel + scanline_fill[x2] += sign * (sy1-sy0); + } + } else { + // if edge goes outside of box we're drawing, we require + // clipping logic. since this does not match the intended use + // of this library, we use a different, very slow brute + // force implementation + // note though that this does happen some of the time because + // x_top and x_bottom can be extrapolated at the top & bottom of + // the shape and actually lie outside the bounding box + int x; + for (x=0; x < len; ++x) { + // cases: + // + // there can be up to two intersections with the pixel. any intersection + // with left or right edges can be handled by splitting into two (or three) + // regions. intersections with top & bottom do not necessitate case-wise logic. + // + // the old way of doing this found the intersections with the left & right edges, + // then used some simple logic to produce up to three segments in sorted order + // from top-to-bottom. however, this had a problem: if an x edge was epsilon + // across the x border, then the corresponding y position might not be distinct + // from the other y segment, and it might ignored as an empty segment. to avoid + // that, we need to explicitly produce segments based on x positions. + + // rename variables to clearly-defined pairs + float y0 = y_top; + float x1 = (float) (x); + float x2 = (float) (x+1); + float x3 = xb; + float y3 = y_bottom; + + // x = e->x + e->dx * (y-y_top) + // (y-y_top) = (x - e->x) / e->dx + // y = (x - e->x) / e->dx + y_top + float y1 = (x - x0) / dx + y_top; + float y2 = (x+1 - x0) / dx + y_top; + + if (x0 < x1 && x3 > x2) { // three segments descending down-right + stbtt__handle_clipped_edge(scanline,x,e, x0,y0, x1,y1); + stbtt__handle_clipped_edge(scanline,x,e, x1,y1, x2,y2); + stbtt__handle_clipped_edge(scanline,x,e, x2,y2, x3,y3); + } else if (x3 < x1 && x0 > x2) { // three segments descending down-left + stbtt__handle_clipped_edge(scanline,x,e, x0,y0, x2,y2); + stbtt__handle_clipped_edge(scanline,x,e, x2,y2, x1,y1); + stbtt__handle_clipped_edge(scanline,x,e, x1,y1, x3,y3); + } else if (x0 < x1 && x3 > x1) { // two segments across x, down-right + stbtt__handle_clipped_edge(scanline,x,e, x0,y0, x1,y1); + stbtt__handle_clipped_edge(scanline,x,e, x1,y1, x3,y3); + } else if (x3 < x1 && x0 > x1) { // two segments across x, down-left + stbtt__handle_clipped_edge(scanline,x,e, x0,y0, x1,y1); + stbtt__handle_clipped_edge(scanline,x,e, x1,y1, x3,y3); + } else if (x0 < x2 && x3 > x2) { // two segments across x+1, down-right + stbtt__handle_clipped_edge(scanline,x,e, x0,y0, x2,y2); + stbtt__handle_clipped_edge(scanline,x,e, x2,y2, x3,y3); + } else if (x3 < x2 && x0 > x2) { // two segments across x+1, down-left + stbtt__handle_clipped_edge(scanline,x,e, x0,y0, x2,y2); + stbtt__handle_clipped_edge(scanline,x,e, x2,y2, x3,y3); + } else { // one segment + stbtt__handle_clipped_edge(scanline,x,e, x0,y0, x3,y3); + } + } + } + } + e = e->next; + } +} + +// directly AA rasterize edges w/o supersampling +static void stbtt__rasterize_sorted_edges(stbtt__bitmap *result, stbtt__edge *e, int n, int vsubsample, int off_x, int off_y, void *userdata) +{ + stbtt__hheap hh = { 0, 0, 0 }; + stbtt__active_edge *active = NULL; + int y,j=0, i; + float scanline_data[129], *scanline, *scanline2; + + STBTT__NOTUSED(vsubsample); + + if (result->w > 64) + scanline = (float *) STBTT_malloc((result->w*2+1) * sizeof(float), userdata); + else + scanline = scanline_data; + + scanline2 = scanline + result->w; + + y = off_y; + e[n].y0 = (float) (off_y + result->h) + 1; + + while (j < result->h) { + // find center of pixel for this scanline + float scan_y_top = y + 0.0f; + float scan_y_bottom = y + 1.0f; + stbtt__active_edge **step = &active; + + STBTT_memset(scanline , 0, result->w*sizeof(scanline[0])); + STBTT_memset(scanline2, 0, (result->w+1)*sizeof(scanline[0])); + + // update all active edges; + // remove all active edges that terminate before the top of this scanline + while (*step) { + stbtt__active_edge * z = *step; + if (z->ey <= scan_y_top) { + *step = z->next; // delete from list + STBTT_assert(z->direction); + z->direction = 0; + stbtt__hheap_free(&hh, z); + } else { + step = &((*step)->next); // advance through list + } + } + + // insert all edges that start before the bottom of this scanline + while (e->y0 <= scan_y_bottom) { + if (e->y0 != e->y1) { + stbtt__active_edge *z = stbtt__new_active(&hh, e, off_x, scan_y_top, userdata); + if (z != NULL) { + if (j == 0 && off_y != 0) { + if (z->ey < scan_y_top) { + // this can happen due to subpixel positioning and some kind of fp rounding error i think + z->ey = scan_y_top; + } + } + STBTT_assert(z->ey >= scan_y_top); // if we get really unlucky a tiny bit of an edge can be out of bounds + // insert at front + z->next = active; + active = z; + } + } + ++e; + } + + // now process all active edges + if (active) + stbtt__fill_active_edges_new(scanline, scanline2+1, result->w, active, scan_y_top); + + { + float sum = 0; + for (i=0; i < result->w; ++i) { + float k; + int m; + sum += scanline2[i]; + k = scanline[i] + sum; + k = (float) STBTT_fabs(k)*255 + 0.5f; + m = (int) k; + if (m > 255) m = 255; + result->pixels[j*result->stride + i] = (unsigned char) m; + } + } + // advance all the edges + step = &active; + while (*step) { + stbtt__active_edge *z = *step; + z->fx += z->fdx; // advance to position for current scanline + step = &((*step)->next); // advance through list + } + + ++y; + ++j; + } + + stbtt__hheap_cleanup(&hh, userdata); + + if (scanline != scanline_data) + STBTT_free(scanline, userdata); +} +#else +#error "Unrecognized value of STBTT_RASTERIZER_VERSION" +#endif + +#define STBTT__COMPARE(a,b) ((a)->y0 < (b)->y0) + +static void stbtt__sort_edges_ins_sort(stbtt__edge *p, int n) +{ + int i,j; + for (i=1; i < n; ++i) { + stbtt__edge t = p[i], *a = &t; + j = i; + while (j > 0) { + stbtt__edge *b = &p[j-1]; + int c = STBTT__COMPARE(a,b); + if (!c) break; + p[j] = p[j-1]; + --j; + } + if (i != j) + p[j] = t; + } +} + +static void stbtt__sort_edges_quicksort(stbtt__edge *p, int n) +{ + /* threshold for transitioning to insertion sort */ + while (n > 12) { + stbtt__edge t; + int c01,c12,c,m,i,j; + + /* compute median of three */ + m = n >> 1; + c01 = STBTT__COMPARE(&p[0],&p[m]); + c12 = STBTT__COMPARE(&p[m],&p[n-1]); + /* if 0 >= mid >= end, or 0 < mid < end, then use mid */ + if (c01 != c12) { + /* otherwise, we'll need to swap something else to middle */ + int z; + c = STBTT__COMPARE(&p[0],&p[n-1]); + /* 0>mid && midn => n; 0 0 */ + /* 0n: 0>n => 0; 0 n */ + z = (c == c12) ? 0 : n-1; + t = p[z]; + p[z] = p[m]; + p[m] = t; + } + /* now p[m] is the median-of-three */ + /* swap it to the beginning so it won't move around */ + t = p[0]; + p[0] = p[m]; + p[m] = t; + + /* partition loop */ + i=1; + j=n-1; + for(;;) { + /* handling of equality is crucial here */ + /* for sentinels & efficiency with duplicates */ + for (;;++i) { + if (!STBTT__COMPARE(&p[i], &p[0])) break; + } + for (;;--j) { + if (!STBTT__COMPARE(&p[0], &p[j])) break; + } + /* make sure we haven't crossed */ + if (i >= j) break; + t = p[i]; + p[i] = p[j]; + p[j] = t; + + ++i; + --j; + } + /* recurse on smaller side, iterate on larger */ + if (j < (n-i)) { + stbtt__sort_edges_quicksort(p,j); + p = p+i; + n = n-i; + } else { + stbtt__sort_edges_quicksort(p+i, n-i); + n = j; + } + } +} + +static void stbtt__sort_edges(stbtt__edge *p, int n) +{ + stbtt__sort_edges_quicksort(p, n); + stbtt__sort_edges_ins_sort(p, n); +} + +typedef struct +{ + float x,y; +} stbtt__point; + +static void stbtt__rasterize(stbtt__bitmap *result, stbtt__point *pts, int *wcount, int windings, float scale_x, float scale_y, float shift_x, float shift_y, int off_x, int off_y, int invert, void *userdata) +{ + float y_scale_inv = invert ? -scale_y : scale_y; + stbtt__edge *e; + int n,i,j,k,m; +#if STBTT_RASTERIZER_VERSION == 1 + int vsubsample = result->h < 8 ? 15 : 5; +#elif STBTT_RASTERIZER_VERSION == 2 + int vsubsample = 1; +#else + #error "Unrecognized value of STBTT_RASTERIZER_VERSION" +#endif + // vsubsample should divide 255 evenly; otherwise we won't reach full opacity + + // now we have to blow out the windings into explicit edge lists + n = 0; + for (i=0; i < windings; ++i) + n += wcount[i]; + + e = (stbtt__edge *) STBTT_malloc(sizeof(*e) * (n+1), userdata); // add an extra one as a sentinel + if (e == 0) return; + n = 0; + + m=0; + for (i=0; i < windings; ++i) { + stbtt__point *p = pts + m; + m += wcount[i]; + j = wcount[i]-1; + for (k=0; k < wcount[i]; j=k++) { + int a=k,b=j; + // skip the edge if horizontal + if (p[j].y == p[k].y) + continue; + // add edge from j to k to the list + e[n].invert = 0; + if (invert ? p[j].y > p[k].y : p[j].y < p[k].y) { + e[n].invert = 1; + a=j,b=k; + } + e[n].x0 = p[a].x * scale_x + shift_x; + e[n].y0 = (p[a].y * y_scale_inv + shift_y) * vsubsample; + e[n].x1 = p[b].x * scale_x + shift_x; + e[n].y1 = (p[b].y * y_scale_inv + shift_y) * vsubsample; + ++n; + } + } + + // now sort the edges by their highest point (should snap to integer, and then by x) + //STBTT_sort(e, n, sizeof(e[0]), stbtt__edge_compare); + stbtt__sort_edges(e, n); + + // now, traverse the scanlines and find the intersections on each scanline, use xor winding rule + stbtt__rasterize_sorted_edges(result, e, n, vsubsample, off_x, off_y, userdata); + + STBTT_free(e, userdata); +} + +static void stbtt__add_point(stbtt__point *points, int n, float x, float y) +{ + if (!points) return; // during first pass, it's unallocated + points[n].x = x; + points[n].y = y; +} + +// tessellate until threshold p is happy... @TODO warped to compensate for non-linear stretching +static int stbtt__tesselate_curve(stbtt__point *points, int *num_points, float x0, float y0, float x1, float y1, float x2, float y2, float objspace_flatness_squared, int n) +{ + // midpoint + float mx = (x0 + 2*x1 + x2)/4; + float my = (y0 + 2*y1 + y2)/4; + // versus directly drawn line + float dx = (x0+x2)/2 - mx; + float dy = (y0+y2)/2 - my; + if (n > 16) // 65536 segments on one curve better be enough! + return 1; + if (dx*dx+dy*dy > objspace_flatness_squared) { // half-pixel error allowed... need to be smaller if AA + stbtt__tesselate_curve(points, num_points, x0,y0, (x0+x1)/2.0f,(y0+y1)/2.0f, mx,my, objspace_flatness_squared,n+1); + stbtt__tesselate_curve(points, num_points, mx,my, (x1+x2)/2.0f,(y1+y2)/2.0f, x2,y2, objspace_flatness_squared,n+1); + } else { + stbtt__add_point(points, *num_points,x2,y2); + *num_points = *num_points+1; + } + return 1; +} + +static void stbtt__tesselate_cubic(stbtt__point *points, int *num_points, float x0, float y0, float x1, float y1, float x2, float y2, float x3, float y3, float objspace_flatness_squared, int n) +{ + // @TODO this "flatness" calculation is just made-up nonsense that seems to work well enough + float dx0 = x1-x0; + float dy0 = y1-y0; + float dx1 = x2-x1; + float dy1 = y2-y1; + float dx2 = x3-x2; + float dy2 = y3-y2; + float dx = x3-x0; + float dy = y3-y0; + float longlen = (float) (STBTT_sqrt(dx0*dx0+dy0*dy0)+STBTT_sqrt(dx1*dx1+dy1*dy1)+STBTT_sqrt(dx2*dx2+dy2*dy2)); + float shortlen = (float) STBTT_sqrt(dx*dx+dy*dy); + float flatness_squared = longlen*longlen-shortlen*shortlen; + + if (n > 16) // 65536 segments on one curve better be enough! + return; + + if (flatness_squared > objspace_flatness_squared) { + float x01 = (x0+x1)/2; + float y01 = (y0+y1)/2; + float x12 = (x1+x2)/2; + float y12 = (y1+y2)/2; + float x23 = (x2+x3)/2; + float y23 = (y2+y3)/2; + + float xa = (x01+x12)/2; + float ya = (y01+y12)/2; + float xb = (x12+x23)/2; + float yb = (y12+y23)/2; + + float mx = (xa+xb)/2; + float my = (ya+yb)/2; + + stbtt__tesselate_cubic(points, num_points, x0,y0, x01,y01, xa,ya, mx,my, objspace_flatness_squared,n+1); + stbtt__tesselate_cubic(points, num_points, mx,my, xb,yb, x23,y23, x3,y3, objspace_flatness_squared,n+1); + } else { + stbtt__add_point(points, *num_points,x3,y3); + *num_points = *num_points+1; + } +} + +// returns number of contours +static stbtt__point *stbtt_FlattenCurves(stbtt_vertex *vertices, int num_verts, float objspace_flatness, int **contour_lengths, int *num_contours, void *userdata) +{ + stbtt__point *points=0; + int num_points=0; + + float objspace_flatness_squared = objspace_flatness * objspace_flatness; + int i,n=0,start=0, pass; + + // count how many "moves" there are to get the contour count + for (i=0; i < num_verts; ++i) + if (vertices[i].type == STBTT_vmove) + ++n; + + *num_contours = n; + if (n == 0) return 0; + + *contour_lengths = (int *) STBTT_malloc(sizeof(**contour_lengths) * n, userdata); + + if (*contour_lengths == 0) { + *num_contours = 0; + return 0; + } + + // make two passes through the points so we don't need to realloc + for (pass=0; pass < 2; ++pass) { + float x=0,y=0; + if (pass == 1) { + points = (stbtt__point *) STBTT_malloc(num_points * sizeof(points[0]), userdata); + if (points == NULL) goto error; + } + num_points = 0; + n= -1; + for (i=0; i < num_verts; ++i) { + switch (vertices[i].type) { + case STBTT_vmove: + // start the next contour + if (n >= 0) + (*contour_lengths)[n] = num_points - start; + ++n; + start = num_points; + + x = vertices[i].x, y = vertices[i].y; + stbtt__add_point(points, num_points++, x,y); + break; + case STBTT_vline: + x = vertices[i].x, y = vertices[i].y; + stbtt__add_point(points, num_points++, x, y); + break; + case STBTT_vcurve: + stbtt__tesselate_curve(points, &num_points, x,y, + vertices[i].cx, vertices[i].cy, + vertices[i].x, vertices[i].y, + objspace_flatness_squared, 0); + x = vertices[i].x, y = vertices[i].y; + break; + case STBTT_vcubic: + stbtt__tesselate_cubic(points, &num_points, x,y, + vertices[i].cx, vertices[i].cy, + vertices[i].cx1, vertices[i].cy1, + vertices[i].x, vertices[i].y, + objspace_flatness_squared, 0); + x = vertices[i].x, y = vertices[i].y; + break; + } + } + (*contour_lengths)[n] = num_points - start; + } + + return points; +error: + STBTT_free(points, userdata); + STBTT_free(*contour_lengths, userdata); + *contour_lengths = 0; + *num_contours = 0; + return NULL; +} + +STBTT_DEF void stbtt_Rasterize(stbtt__bitmap *result, float flatness_in_pixels, stbtt_vertex *vertices, int num_verts, float scale_x, float scale_y, float shift_x, float shift_y, int x_off, int y_off, int invert, void *userdata) +{ + float scale = scale_x > scale_y ? scale_y : scale_x; + int winding_count = 0; + int *winding_lengths = NULL; + stbtt__point *windings = stbtt_FlattenCurves(vertices, num_verts, flatness_in_pixels / scale, &winding_lengths, &winding_count, userdata); + if (windings) { + stbtt__rasterize(result, windings, winding_lengths, winding_count, scale_x, scale_y, shift_x, shift_y, x_off, y_off, invert, userdata); + STBTT_free(winding_lengths, userdata); + STBTT_free(windings, userdata); + } +} + +STBTT_DEF void stbtt_FreeBitmap(unsigned char *bitmap, void *userdata) +{ + STBTT_free(bitmap, userdata); +} + +STBTT_DEF unsigned char *stbtt_GetGlyphBitmapSubpixel(const stbtt_fontinfo *info, float scale_x, float scale_y, float shift_x, float shift_y, int glyph, int *width, int *height, int *xoff, int *yoff) +{ + int ix0,iy0,ix1,iy1; + stbtt__bitmap gbm; + stbtt_vertex *vertices; + int num_verts = stbtt_GetGlyphShape(info, glyph, &vertices); + + if (scale_x == 0) scale_x = scale_y; + if (scale_y == 0) { + if (scale_x == 0) { + STBTT_free(vertices, info->userdata); + return NULL; + } + scale_y = scale_x; + } + + stbtt_GetGlyphBitmapBoxSubpixel(info, glyph, scale_x, scale_y, shift_x, shift_y, &ix0,&iy0,&ix1,&iy1); + + // now we get the size + gbm.w = (ix1 - ix0); + gbm.h = (iy1 - iy0); + gbm.pixels = NULL; // in case we error + + if (width ) *width = gbm.w; + if (height) *height = gbm.h; + if (xoff ) *xoff = ix0; + if (yoff ) *yoff = iy0; + + if (gbm.w && gbm.h) { + gbm.pixels = (unsigned char *) STBTT_malloc(gbm.w * gbm.h, info->userdata); + if (gbm.pixels) { + gbm.stride = gbm.w; + + stbtt_Rasterize(&gbm, 0.35f, vertices, num_verts, scale_x, scale_y, shift_x, shift_y, ix0, iy0, 1, info->userdata); + } + } + STBTT_free(vertices, info->userdata); + return gbm.pixels; +} + +STBTT_DEF unsigned char *stbtt_GetGlyphBitmap(const stbtt_fontinfo *info, float scale_x, float scale_y, int glyph, int *width, int *height, int *xoff, int *yoff) +{ + return stbtt_GetGlyphBitmapSubpixel(info, scale_x, scale_y, 0.0f, 0.0f, glyph, width, height, xoff, yoff); +} + +STBTT_DEF void stbtt_MakeGlyphBitmapSubpixel(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, float shift_x, float shift_y, int glyph) +{ + int ix0,iy0; + stbtt_vertex *vertices; + int num_verts = stbtt_GetGlyphShape(info, glyph, &vertices); + stbtt__bitmap gbm; + + stbtt_GetGlyphBitmapBoxSubpixel(info, glyph, scale_x, scale_y, shift_x, shift_y, &ix0,&iy0,0,0); + gbm.pixels = output; + gbm.w = out_w; + gbm.h = out_h; + gbm.stride = out_stride; + + if (gbm.w && gbm.h) + stbtt_Rasterize(&gbm, 0.35f, vertices, num_verts, scale_x, scale_y, shift_x, shift_y, ix0,iy0, 1, info->userdata); + + STBTT_free(vertices, info->userdata); +} + +STBTT_DEF void stbtt_MakeGlyphBitmap(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, int glyph) +{ + stbtt_MakeGlyphBitmapSubpixel(info, output, out_w, out_h, out_stride, scale_x, scale_y, 0.0f,0.0f, glyph); +} + +STBTT_DEF unsigned char *stbtt_GetCodepointBitmapSubpixel(const stbtt_fontinfo *info, float scale_x, float scale_y, float shift_x, float shift_y, int codepoint, int *width, int *height, int *xoff, int *yoff) +{ + return stbtt_GetGlyphBitmapSubpixel(info, scale_x, scale_y,shift_x,shift_y, stbtt_FindGlyphIndex(info,codepoint), width,height,xoff,yoff); +} + +STBTT_DEF void stbtt_MakeCodepointBitmapSubpixelPrefilter(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, float shift_x, float shift_y, int oversample_x, int oversample_y, float *sub_x, float *sub_y, int codepoint) +{ + stbtt_MakeGlyphBitmapSubpixelPrefilter(info, output, out_w, out_h, out_stride, scale_x, scale_y, shift_x, shift_y, oversample_x, oversample_y, sub_x, sub_y, stbtt_FindGlyphIndex(info,codepoint)); +} + +STBTT_DEF void stbtt_MakeCodepointBitmapSubpixel(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, float shift_x, float shift_y, int codepoint) +{ + stbtt_MakeGlyphBitmapSubpixel(info, output, out_w, out_h, out_stride, scale_x, scale_y, shift_x, shift_y, stbtt_FindGlyphIndex(info,codepoint)); +} + +STBTT_DEF unsigned char *stbtt_GetCodepointBitmap(const stbtt_fontinfo *info, float scale_x, float scale_y, int codepoint, int *width, int *height, int *xoff, int *yoff) +{ + return stbtt_GetCodepointBitmapSubpixel(info, scale_x, scale_y, 0.0f,0.0f, codepoint, width,height,xoff,yoff); +} + +STBTT_DEF void stbtt_MakeCodepointBitmap(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, int codepoint) +{ + stbtt_MakeCodepointBitmapSubpixel(info, output, out_w, out_h, out_stride, scale_x, scale_y, 0.0f,0.0f, codepoint); +} + +////////////////////////////////////////////////////////////////////////////// +// +// bitmap baking +// +// This is SUPER-CRAPPY packing to keep source code small + +static int stbtt_BakeFontBitmap_internal(unsigned char *data, int offset, // font location (use offset=0 for plain .ttf) + float pixel_height, // height of font in pixels + unsigned char *pixels, int pw, int ph, // bitmap to be filled in + int first_char, int num_chars, // characters to bake + stbtt_bakedchar *chardata) +{ + float scale; + int x,y,bottom_y, i; + stbtt_fontinfo f; + f.userdata = NULL; + if (!stbtt_InitFont(&f, data, offset)) + return -1; + STBTT_memset(pixels, 0, pw*ph); // background of 0 around pixels + x=y=1; + bottom_y = 1; + + scale = stbtt_ScaleForPixelHeight(&f, pixel_height); + + for (i=0; i < num_chars; ++i) { + int advance, lsb, x0,y0,x1,y1,gw,gh; + int g = stbtt_FindGlyphIndex(&f, first_char + i); + stbtt_GetGlyphHMetrics(&f, g, &advance, &lsb); + stbtt_GetGlyphBitmapBox(&f, g, scale,scale, &x0,&y0,&x1,&y1); + gw = x1-x0; + gh = y1-y0; + if (x + gw + 1 >= pw) + y = bottom_y, x = 1; // advance to next row + if (y + gh + 1 >= ph) // check if it fits vertically AFTER potentially moving to next row + return -i; + STBTT_assert(x+gw < pw); + STBTT_assert(y+gh < ph); + stbtt_MakeGlyphBitmap(&f, pixels+x+y*pw, gw,gh,pw, scale,scale, g); + chardata[i].x0 = (stbtt_int16) x; + chardata[i].y0 = (stbtt_int16) y; + chardata[i].x1 = (stbtt_int16) (x + gw); + chardata[i].y1 = (stbtt_int16) (y + gh); + chardata[i].xadvance = scale * advance; + chardata[i].xoff = (float) x0; + chardata[i].yoff = (float) y0; + x = x + gw + 1; + if (y+gh+1 > bottom_y) + bottom_y = y+gh+1; + } + return bottom_y; +} + +STBTT_DEF void stbtt_GetBakedQuad(const stbtt_bakedchar *chardata, int pw, int ph, int char_index, float *xpos, float *ypos, stbtt_aligned_quad *q, int opengl_fillrule) +{ + float d3d_bias = opengl_fillrule ? 0 : -0.5f; + float ipw = 1.0f / pw, iph = 1.0f / ph; + const stbtt_bakedchar *b = chardata + char_index; + int round_x = STBTT_ifloor((*xpos + b->xoff) + 0.5f); + int round_y = STBTT_ifloor((*ypos + b->yoff) + 0.5f); + + q->x0 = round_x + d3d_bias; + q->y0 = round_y + d3d_bias; + q->x1 = round_x + b->x1 - b->x0 + d3d_bias; + q->y1 = round_y + b->y1 - b->y0 + d3d_bias; + + q->s0 = b->x0 * ipw; + q->t0 = b->y0 * iph; + q->s1 = b->x1 * ipw; + q->t1 = b->y1 * iph; + + *xpos += b->xadvance; +} + +////////////////////////////////////////////////////////////////////////////// +// +// rectangle packing replacement routines if you don't have stb_rect_pack.h +// + +#ifndef STB_RECT_PACK_VERSION + +typedef int stbrp_coord; + +//////////////////////////////////////////////////////////////////////////////////// +// // +// // +// COMPILER WARNING ?!?!? // +// // +// // +// if you get a compile warning due to these symbols being defined more than // +// once, move #include "stb_rect_pack.h" before #include "stb_truetype.h" // +// // +//////////////////////////////////////////////////////////////////////////////////// + +typedef struct +{ + int width,height; + int x,y,bottom_y; +} stbrp_context; + +typedef struct +{ + unsigned char x; +} stbrp_node; + +struct stbrp_rect +{ + stbrp_coord x,y; + int id,w,h,was_packed; +}; + +static void stbrp_init_target(stbrp_context *con, int pw, int ph, stbrp_node *nodes, int num_nodes) +{ + con->width = pw; + con->height = ph; + con->x = 0; + con->y = 0; + con->bottom_y = 0; + STBTT__NOTUSED(nodes); + STBTT__NOTUSED(num_nodes); +} + +static void stbrp_pack_rects(stbrp_context *con, stbrp_rect *rects, int num_rects) +{ + int i; + for (i=0; i < num_rects; ++i) { + if (con->x + rects[i].w > con->width) { + con->x = 0; + con->y = con->bottom_y; + } + if (con->y + rects[i].h > con->height) + break; + rects[i].x = con->x; + rects[i].y = con->y; + rects[i].was_packed = 1; + con->x += rects[i].w; + if (con->y + rects[i].h > con->bottom_y) + con->bottom_y = con->y + rects[i].h; + } + for ( ; i < num_rects; ++i) + rects[i].was_packed = 0; +} +#endif + +////////////////////////////////////////////////////////////////////////////// +// +// bitmap baking +// +// This is SUPER-AWESOME (tm Ryan Gordon) packing using stb_rect_pack.h. If +// stb_rect_pack.h isn't available, it uses the BakeFontBitmap strategy. + +STBTT_DEF int stbtt_PackBegin(stbtt_pack_context *spc, unsigned char *pixels, int pw, int ph, int stride_in_bytes, int padding, void *alloc_context) +{ + stbrp_context *context = (stbrp_context *) STBTT_malloc(sizeof(*context) ,alloc_context); + int num_nodes = pw - padding; + stbrp_node *nodes = (stbrp_node *) STBTT_malloc(sizeof(*nodes ) * num_nodes,alloc_context); + + if (context == NULL || nodes == NULL) { + if (context != NULL) STBTT_free(context, alloc_context); + if (nodes != NULL) STBTT_free(nodes , alloc_context); + return 0; + } + + spc->user_allocator_context = alloc_context; + spc->width = pw; + spc->height = ph; + spc->pixels = pixels; + spc->pack_info = context; + spc->nodes = nodes; + spc->padding = padding; + spc->stride_in_bytes = stride_in_bytes != 0 ? stride_in_bytes : pw; + spc->h_oversample = 1; + spc->v_oversample = 1; + spc->skip_missing = 0; + + stbrp_init_target(context, pw-padding, ph-padding, nodes, num_nodes); + + if (pixels) + STBTT_memset(pixels, 0, pw*ph); // background of 0 around pixels + + return 1; +} + +STBTT_DEF void stbtt_PackEnd (stbtt_pack_context *spc) +{ + STBTT_free(spc->nodes , spc->user_allocator_context); + STBTT_free(spc->pack_info, spc->user_allocator_context); +} + +STBTT_DEF void stbtt_PackSetOversampling(stbtt_pack_context *spc, unsigned int h_oversample, unsigned int v_oversample) +{ + STBTT_assert(h_oversample <= STBTT_MAX_OVERSAMPLE); + STBTT_assert(v_oversample <= STBTT_MAX_OVERSAMPLE); + if (h_oversample <= STBTT_MAX_OVERSAMPLE) + spc->h_oversample = h_oversample; + if (v_oversample <= STBTT_MAX_OVERSAMPLE) + spc->v_oversample = v_oversample; +} + +STBTT_DEF void stbtt_PackSetSkipMissingCodepoints(stbtt_pack_context *spc, int skip) +{ + spc->skip_missing = skip; +} + +#define STBTT__OVER_MASK (STBTT_MAX_OVERSAMPLE-1) + +static void stbtt__h_prefilter(unsigned char *pixels, int w, int h, int stride_in_bytes, unsigned int kernel_width) +{ + unsigned char buffer[STBTT_MAX_OVERSAMPLE]; + int safe_w = w - kernel_width; + int j; + STBTT_memset(buffer, 0, STBTT_MAX_OVERSAMPLE); // suppress bogus warning from VS2013 -analyze + for (j=0; j < h; ++j) { + int i; + unsigned int total; + STBTT_memset(buffer, 0, kernel_width); + + total = 0; + + // make kernel_width a constant in common cases so compiler can optimize out the divide + switch (kernel_width) { + case 2: + for (i=0; i <= safe_w; ++i) { + total += pixels[i] - buffer[i & STBTT__OVER_MASK]; + buffer[(i+kernel_width) & STBTT__OVER_MASK] = pixels[i]; + pixels[i] = (unsigned char) (total / 2); + } + break; + case 3: + for (i=0; i <= safe_w; ++i) { + total += pixels[i] - buffer[i & STBTT__OVER_MASK]; + buffer[(i+kernel_width) & STBTT__OVER_MASK] = pixels[i]; + pixels[i] = (unsigned char) (total / 3); + } + break; + case 4: + for (i=0; i <= safe_w; ++i) { + total += pixels[i] - buffer[i & STBTT__OVER_MASK]; + buffer[(i+kernel_width) & STBTT__OVER_MASK] = pixels[i]; + pixels[i] = (unsigned char) (total / 4); + } + break; + case 5: + for (i=0; i <= safe_w; ++i) { + total += pixels[i] - buffer[i & STBTT__OVER_MASK]; + buffer[(i+kernel_width) & STBTT__OVER_MASK] = pixels[i]; + pixels[i] = (unsigned char) (total / 5); + } + break; + default: + for (i=0; i <= safe_w; ++i) { + total += pixels[i] - buffer[i & STBTT__OVER_MASK]; + buffer[(i+kernel_width) & STBTT__OVER_MASK] = pixels[i]; + pixels[i] = (unsigned char) (total / kernel_width); + } + break; + } + + for (; i < w; ++i) { + STBTT_assert(pixels[i] == 0); + total -= buffer[i & STBTT__OVER_MASK]; + pixels[i] = (unsigned char) (total / kernel_width); + } + + pixels += stride_in_bytes; + } +} + +static void stbtt__v_prefilter(unsigned char *pixels, int w, int h, int stride_in_bytes, unsigned int kernel_width) +{ + unsigned char buffer[STBTT_MAX_OVERSAMPLE]; + int safe_h = h - kernel_width; + int j; + STBTT_memset(buffer, 0, STBTT_MAX_OVERSAMPLE); // suppress bogus warning from VS2013 -analyze + for (j=0; j < w; ++j) { + int i; + unsigned int total; + STBTT_memset(buffer, 0, kernel_width); + + total = 0; + + // make kernel_width a constant in common cases so compiler can optimize out the divide + switch (kernel_width) { + case 2: + for (i=0; i <= safe_h; ++i) { + total += pixels[i*stride_in_bytes] - buffer[i & STBTT__OVER_MASK]; + buffer[(i+kernel_width) & STBTT__OVER_MASK] = pixels[i*stride_in_bytes]; + pixels[i*stride_in_bytes] = (unsigned char) (total / 2); + } + break; + case 3: + for (i=0; i <= safe_h; ++i) { + total += pixels[i*stride_in_bytes] - buffer[i & STBTT__OVER_MASK]; + buffer[(i+kernel_width) & STBTT__OVER_MASK] = pixels[i*stride_in_bytes]; + pixels[i*stride_in_bytes] = (unsigned char) (total / 3); + } + break; + case 4: + for (i=0; i <= safe_h; ++i) { + total += pixels[i*stride_in_bytes] - buffer[i & STBTT__OVER_MASK]; + buffer[(i+kernel_width) & STBTT__OVER_MASK] = pixels[i*stride_in_bytes]; + pixels[i*stride_in_bytes] = (unsigned char) (total / 4); + } + break; + case 5: + for (i=0; i <= safe_h; ++i) { + total += pixels[i*stride_in_bytes] - buffer[i & STBTT__OVER_MASK]; + buffer[(i+kernel_width) & STBTT__OVER_MASK] = pixels[i*stride_in_bytes]; + pixels[i*stride_in_bytes] = (unsigned char) (total / 5); + } + break; + default: + for (i=0; i <= safe_h; ++i) { + total += pixels[i*stride_in_bytes] - buffer[i & STBTT__OVER_MASK]; + buffer[(i+kernel_width) & STBTT__OVER_MASK] = pixels[i*stride_in_bytes]; + pixels[i*stride_in_bytes] = (unsigned char) (total / kernel_width); + } + break; + } + + for (; i < h; ++i) { + STBTT_assert(pixels[i*stride_in_bytes] == 0); + total -= buffer[i & STBTT__OVER_MASK]; + pixels[i*stride_in_bytes] = (unsigned char) (total / kernel_width); + } + + pixels += 1; + } +} + +static float stbtt__oversample_shift(int oversample) +{ + if (!oversample) + return 0.0f; + + // The prefilter is a box filter of width "oversample", + // which shifts phase by (oversample - 1)/2 pixels in + // oversampled space. We want to shift in the opposite + // direction to counter this. + return (float)-(oversample - 1) / (2.0f * (float)oversample); +} + +// rects array must be big enough to accommodate all characters in the given ranges +STBTT_DEF int stbtt_PackFontRangesGatherRects(stbtt_pack_context *spc, const stbtt_fontinfo *info, stbtt_pack_range *ranges, int num_ranges, stbrp_rect *rects) +{ + int i,j,k; + int missing_glyph_added = 0; + + k=0; + for (i=0; i < num_ranges; ++i) { + float fh = ranges[i].font_size; + float scale = fh > 0 ? stbtt_ScaleForPixelHeight(info, fh) : stbtt_ScaleForMappingEmToPixels(info, -fh); + ranges[i].h_oversample = (unsigned char) spc->h_oversample; + ranges[i].v_oversample = (unsigned char) spc->v_oversample; + for (j=0; j < ranges[i].num_chars; ++j) { + int x0,y0,x1,y1; + int codepoint = ranges[i].array_of_unicode_codepoints == NULL ? ranges[i].first_unicode_codepoint_in_range + j : ranges[i].array_of_unicode_codepoints[j]; + int glyph = stbtt_FindGlyphIndex(info, codepoint); + if (glyph == 0 && (spc->skip_missing || missing_glyph_added)) { + rects[k].w = rects[k].h = 0; + } else { + stbtt_GetGlyphBitmapBoxSubpixel(info,glyph, + scale * spc->h_oversample, + scale * spc->v_oversample, + 0,0, + &x0,&y0,&x1,&y1); + rects[k].w = (stbrp_coord) (x1-x0 + spc->padding + spc->h_oversample-1); + rects[k].h = (stbrp_coord) (y1-y0 + spc->padding + spc->v_oversample-1); + if (glyph == 0) + missing_glyph_added = 1; + } + ++k; + } + } + + return k; +} + +STBTT_DEF void stbtt_MakeGlyphBitmapSubpixelPrefilter(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, float shift_x, float shift_y, int prefilter_x, int prefilter_y, float *sub_x, float *sub_y, int glyph) +{ + stbtt_MakeGlyphBitmapSubpixel(info, + output, + out_w - (prefilter_x - 1), + out_h - (prefilter_y - 1), + out_stride, + scale_x, + scale_y, + shift_x, + shift_y, + glyph); + + if (prefilter_x > 1) + stbtt__h_prefilter(output, out_w, out_h, out_stride, prefilter_x); + + if (prefilter_y > 1) + stbtt__v_prefilter(output, out_w, out_h, out_stride, prefilter_y); + + *sub_x = stbtt__oversample_shift(prefilter_x); + *sub_y = stbtt__oversample_shift(prefilter_y); +} + +// rects array must be big enough to accommodate all characters in the given ranges +STBTT_DEF int stbtt_PackFontRangesRenderIntoRects(stbtt_pack_context *spc, const stbtt_fontinfo *info, stbtt_pack_range *ranges, int num_ranges, stbrp_rect *rects) +{ + int i,j,k, missing_glyph = -1, return_value = 1; + + // save current values + int old_h_over = spc->h_oversample; + int old_v_over = spc->v_oversample; + + k = 0; + for (i=0; i < num_ranges; ++i) { + float fh = ranges[i].font_size; + float scale = fh > 0 ? stbtt_ScaleForPixelHeight(info, fh) : stbtt_ScaleForMappingEmToPixels(info, -fh); + float recip_h,recip_v,sub_x,sub_y; + spc->h_oversample = ranges[i].h_oversample; + spc->v_oversample = ranges[i].v_oversample; + recip_h = 1.0f / spc->h_oversample; + recip_v = 1.0f / spc->v_oversample; + sub_x = stbtt__oversample_shift(spc->h_oversample); + sub_y = stbtt__oversample_shift(spc->v_oversample); + for (j=0; j < ranges[i].num_chars; ++j) { + stbrp_rect *r = &rects[k]; + if (r->was_packed && r->w != 0 && r->h != 0) { + stbtt_packedchar *bc = &ranges[i].chardata_for_range[j]; + int advance, lsb, x0,y0,x1,y1; + int codepoint = ranges[i].array_of_unicode_codepoints == NULL ? ranges[i].first_unicode_codepoint_in_range + j : ranges[i].array_of_unicode_codepoints[j]; + int glyph = stbtt_FindGlyphIndex(info, codepoint); + stbrp_coord pad = (stbrp_coord) spc->padding; + + // pad on left and top + r->x += pad; + r->y += pad; + r->w -= pad; + r->h -= pad; + stbtt_GetGlyphHMetrics(info, glyph, &advance, &lsb); + stbtt_GetGlyphBitmapBox(info, glyph, + scale * spc->h_oversample, + scale * spc->v_oversample, + &x0,&y0,&x1,&y1); + stbtt_MakeGlyphBitmapSubpixel(info, + spc->pixels + r->x + r->y*spc->stride_in_bytes, + r->w - spc->h_oversample+1, + r->h - spc->v_oversample+1, + spc->stride_in_bytes, + scale * spc->h_oversample, + scale * spc->v_oversample, + 0,0, + glyph); + + if (spc->h_oversample > 1) + stbtt__h_prefilter(spc->pixels + r->x + r->y*spc->stride_in_bytes, + r->w, r->h, spc->stride_in_bytes, + spc->h_oversample); + + if (spc->v_oversample > 1) + stbtt__v_prefilter(spc->pixels + r->x + r->y*spc->stride_in_bytes, + r->w, r->h, spc->stride_in_bytes, + spc->v_oversample); + + bc->x0 = (stbtt_int16) r->x; + bc->y0 = (stbtt_int16) r->y; + bc->x1 = (stbtt_int16) (r->x + r->w); + bc->y1 = (stbtt_int16) (r->y + r->h); + bc->xadvance = scale * advance; + bc->xoff = (float) x0 * recip_h + sub_x; + bc->yoff = (float) y0 * recip_v + sub_y; + bc->xoff2 = (x0 + r->w) * recip_h + sub_x; + bc->yoff2 = (y0 + r->h) * recip_v + sub_y; + + if (glyph == 0) + missing_glyph = j; + } else if (spc->skip_missing) { + return_value = 0; + } else if (r->was_packed && r->w == 0 && r->h == 0 && missing_glyph >= 0) { + ranges[i].chardata_for_range[j] = ranges[i].chardata_for_range[missing_glyph]; + } else { + return_value = 0; // if any fail, report failure + } + + ++k; + } + } + + // restore original values + spc->h_oversample = old_h_over; + spc->v_oversample = old_v_over; + + return return_value; +} + +STBTT_DEF void stbtt_PackFontRangesPackRects(stbtt_pack_context *spc, stbrp_rect *rects, int num_rects) +{ + stbrp_pack_rects((stbrp_context *) spc->pack_info, rects, num_rects); +} + +STBTT_DEF int stbtt_PackFontRanges(stbtt_pack_context *spc, const unsigned char *fontdata, int font_index, stbtt_pack_range *ranges, int num_ranges) +{ + stbtt_fontinfo info; + int i,j,n, return_value = 1; + //stbrp_context *context = (stbrp_context *) spc->pack_info; + stbrp_rect *rects; + + // flag all characters as NOT packed + for (i=0; i < num_ranges; ++i) + for (j=0; j < ranges[i].num_chars; ++j) + ranges[i].chardata_for_range[j].x0 = + ranges[i].chardata_for_range[j].y0 = + ranges[i].chardata_for_range[j].x1 = + ranges[i].chardata_for_range[j].y1 = 0; + + n = 0; + for (i=0; i < num_ranges; ++i) + n += ranges[i].num_chars; + + rects = (stbrp_rect *) STBTT_malloc(sizeof(*rects) * n, spc->user_allocator_context); + if (rects == NULL) + return 0; + + info.userdata = spc->user_allocator_context; + stbtt_InitFont(&info, fontdata, stbtt_GetFontOffsetForIndex(fontdata,font_index)); + + n = stbtt_PackFontRangesGatherRects(spc, &info, ranges, num_ranges, rects); + + stbtt_PackFontRangesPackRects(spc, rects, n); + + return_value = stbtt_PackFontRangesRenderIntoRects(spc, &info, ranges, num_ranges, rects); + + STBTT_free(rects, spc->user_allocator_context); + return return_value; +} + +STBTT_DEF int stbtt_PackFontRange(stbtt_pack_context *spc, const unsigned char *fontdata, int font_index, float font_size, + int first_unicode_codepoint_in_range, int num_chars_in_range, stbtt_packedchar *chardata_for_range) +{ + stbtt_pack_range range; + range.first_unicode_codepoint_in_range = first_unicode_codepoint_in_range; + range.array_of_unicode_codepoints = NULL; + range.num_chars = num_chars_in_range; + range.chardata_for_range = chardata_for_range; + range.font_size = font_size; + return stbtt_PackFontRanges(spc, fontdata, font_index, &range, 1); +} + +STBTT_DEF void stbtt_GetScaledFontVMetrics(const unsigned char *fontdata, int index, float size, float *ascent, float *descent, float *lineGap) +{ + int i_ascent, i_descent, i_lineGap; + float scale; + stbtt_fontinfo info; + stbtt_InitFont(&info, fontdata, stbtt_GetFontOffsetForIndex(fontdata, index)); + scale = size > 0 ? stbtt_ScaleForPixelHeight(&info, size) : stbtt_ScaleForMappingEmToPixels(&info, -size); + stbtt_GetFontVMetrics(&info, &i_ascent, &i_descent, &i_lineGap); + *ascent = (float) i_ascent * scale; + *descent = (float) i_descent * scale; + *lineGap = (float) i_lineGap * scale; +} + +STBTT_DEF void stbtt_GetPackedQuad(const stbtt_packedchar *chardata, int pw, int ph, int char_index, float *xpos, float *ypos, stbtt_aligned_quad *q, int align_to_integer) +{ + float ipw = 1.0f / pw, iph = 1.0f / ph; + const stbtt_packedchar *b = chardata + char_index; + + if (align_to_integer) { + float x = (float) STBTT_ifloor((*xpos + b->xoff) + 0.5f); + float y = (float) STBTT_ifloor((*ypos + b->yoff) + 0.5f); + q->x0 = x; + q->y0 = y; + q->x1 = x + b->xoff2 - b->xoff; + q->y1 = y + b->yoff2 - b->yoff; + } else { + q->x0 = *xpos + b->xoff; + q->y0 = *ypos + b->yoff; + q->x1 = *xpos + b->xoff2; + q->y1 = *ypos + b->yoff2; + } + + q->s0 = b->x0 * ipw; + q->t0 = b->y0 * iph; + q->s1 = b->x1 * ipw; + q->t1 = b->y1 * iph; + + *xpos += b->xadvance; +} + +////////////////////////////////////////////////////////////////////////////// +// +// sdf computation +// + +#define STBTT_min(a,b) ((a) < (b) ? (a) : (b)) +#define STBTT_max(a,b) ((a) < (b) ? (b) : (a)) + +static int stbtt__ray_intersect_bezier(float orig[2], float ray[2], float q0[2], float q1[2], float q2[2], float hits[2][2]) +{ + float q0perp = q0[1]*ray[0] - q0[0]*ray[1]; + float q1perp = q1[1]*ray[0] - q1[0]*ray[1]; + float q2perp = q2[1]*ray[0] - q2[0]*ray[1]; + float roperp = orig[1]*ray[0] - orig[0]*ray[1]; + + float a = q0perp - 2*q1perp + q2perp; + float b = q1perp - q0perp; + float c = q0perp - roperp; + + float s0 = 0., s1 = 0.; + int num_s = 0; + + if (a != 0.0) { + float discr = b*b - a*c; + if (discr > 0.0) { + float rcpna = -1 / a; + float d = (float) STBTT_sqrt(discr); + s0 = (b+d) * rcpna; + s1 = (b-d) * rcpna; + if (s0 >= 0.0 && s0 <= 1.0) + num_s = 1; + if (d > 0.0 && s1 >= 0.0 && s1 <= 1.0) { + if (num_s == 0) s0 = s1; + ++num_s; + } + } + } else { + // 2*b*s + c = 0 + // s = -c / (2*b) + s0 = c / (-2 * b); + if (s0 >= 0.0 && s0 <= 1.0) + num_s = 1; + } + + if (num_s == 0) + return 0; + else { + float rcp_len2 = 1 / (ray[0]*ray[0] + ray[1]*ray[1]); + float rayn_x = ray[0] * rcp_len2, rayn_y = ray[1] * rcp_len2; + + float q0d = q0[0]*rayn_x + q0[1]*rayn_y; + float q1d = q1[0]*rayn_x + q1[1]*rayn_y; + float q2d = q2[0]*rayn_x + q2[1]*rayn_y; + float rod = orig[0]*rayn_x + orig[1]*rayn_y; + + float q10d = q1d - q0d; + float q20d = q2d - q0d; + float q0rd = q0d - rod; + + hits[0][0] = q0rd + s0*(2.0f - 2.0f*s0)*q10d + s0*s0*q20d; + hits[0][1] = a*s0+b; + + if (num_s > 1) { + hits[1][0] = q0rd + s1*(2.0f - 2.0f*s1)*q10d + s1*s1*q20d; + hits[1][1] = a*s1+b; + return 2; + } else { + return 1; + } + } +} + +static int equal(float *a, float *b) +{ + return (a[0] == b[0] && a[1] == b[1]); +} + +static int stbtt__compute_crossings_x(float x, float y, int nverts, stbtt_vertex *verts) +{ + int i; + float orig[2], ray[2] = { 1, 0 }; + float y_frac; + int winding = 0; + + // make sure y never passes through a vertex of the shape + y_frac = (float) STBTT_fmod(y, 1.0f); + if (y_frac < 0.01f) + y += 0.01f; + else if (y_frac > 0.99f) + y -= 0.01f; + + orig[0] = x; + orig[1] = y; + + // test a ray from (-infinity,y) to (x,y) + for (i=0; i < nverts; ++i) { + if (verts[i].type == STBTT_vline) { + int x0 = (int) verts[i-1].x, y0 = (int) verts[i-1].y; + int x1 = (int) verts[i ].x, y1 = (int) verts[i ].y; + if (y > STBTT_min(y0,y1) && y < STBTT_max(y0,y1) && x > STBTT_min(x0,x1)) { + float x_inter = (y - y0) / (y1 - y0) * (x1-x0) + x0; + if (x_inter < x) + winding += (y0 < y1) ? 1 : -1; + } + } + if (verts[i].type == STBTT_vcurve) { + int x0 = (int) verts[i-1].x , y0 = (int) verts[i-1].y ; + int x1 = (int) verts[i ].cx, y1 = (int) verts[i ].cy; + int x2 = (int) verts[i ].x , y2 = (int) verts[i ].y ; + int ax = STBTT_min(x0,STBTT_min(x1,x2)), ay = STBTT_min(y0,STBTT_min(y1,y2)); + int by = STBTT_max(y0,STBTT_max(y1,y2)); + if (y > ay && y < by && x > ax) { + float q0[2],q1[2],q2[2]; + float hits[2][2]; + q0[0] = (float)x0; + q0[1] = (float)y0; + q1[0] = (float)x1; + q1[1] = (float)y1; + q2[0] = (float)x2; + q2[1] = (float)y2; + if (equal(q0,q1) || equal(q1,q2)) { + x0 = (int)verts[i-1].x; + y0 = (int)verts[i-1].y; + x1 = (int)verts[i ].x; + y1 = (int)verts[i ].y; + if (y > STBTT_min(y0,y1) && y < STBTT_max(y0,y1) && x > STBTT_min(x0,x1)) { + float x_inter = (y - y0) / (y1 - y0) * (x1-x0) + x0; + if (x_inter < x) + winding += (y0 < y1) ? 1 : -1; + } + } else { + int num_hits = stbtt__ray_intersect_bezier(orig, ray, q0, q1, q2, hits); + if (num_hits >= 1) + if (hits[0][0] < 0) + winding += (hits[0][1] < 0 ? -1 : 1); + if (num_hits >= 2) + if (hits[1][0] < 0) + winding += (hits[1][1] < 0 ? -1 : 1); + } + } + } + } + return winding; +} + +static float stbtt__cuberoot( float x ) +{ + if (x<0) + return -(float) STBTT_pow(-x,1.0f/3.0f); + else + return (float) STBTT_pow( x,1.0f/3.0f); +} + +// x^3 + a*x^2 + b*x + c = 0 +static int stbtt__solve_cubic(float a, float b, float c, float* r) +{ + float s = -a / 3; + float p = b - a*a / 3; + float q = a * (2*a*a - 9*b) / 27 + c; + float p3 = p*p*p; + float d = q*q + 4*p3 / 27; + if (d >= 0) { + float z = (float) STBTT_sqrt(d); + float u = (-q + z) / 2; + float v = (-q - z) / 2; + u = stbtt__cuberoot(u); + v = stbtt__cuberoot(v); + r[0] = s + u + v; + return 1; + } else { + float u = (float) STBTT_sqrt(-p/3); + float v = (float) STBTT_acos(-STBTT_sqrt(-27/p3) * q / 2) / 3; // p3 must be negative, since d is negative + float m = (float) STBTT_cos(v); + float n = (float) STBTT_cos(v-3.141592/2)*1.732050808f; + r[0] = s + u * 2 * m; + r[1] = s - u * (m + n); + r[2] = s - u * (m - n); + + //STBTT_assert( STBTT_fabs(((r[0]+a)*r[0]+b)*r[0]+c) < 0.05f); // these asserts may not be safe at all scales, though they're in bezier t parameter units so maybe? + //STBTT_assert( STBTT_fabs(((r[1]+a)*r[1]+b)*r[1]+c) < 0.05f); + //STBTT_assert( STBTT_fabs(((r[2]+a)*r[2]+b)*r[2]+c) < 0.05f); + return 3; + } +} + +STBTT_DEF unsigned char * stbtt_GetGlyphSDF(const stbtt_fontinfo *info, float scale, int glyph, int padding, unsigned char onedge_value, float pixel_dist_scale, int *width, int *height, int *xoff, int *yoff) +{ + float scale_x = scale, scale_y = scale; + int ix0,iy0,ix1,iy1; + int w,h; + unsigned char *data; + + if (scale == 0) return NULL; + + stbtt_GetGlyphBitmapBoxSubpixel(info, glyph, scale, scale, 0.0f,0.0f, &ix0,&iy0,&ix1,&iy1); + + // if empty, return NULL + if (ix0 == ix1 || iy0 == iy1) + return NULL; + + ix0 -= padding; + iy0 -= padding; + ix1 += padding; + iy1 += padding; + + w = (ix1 - ix0); + h = (iy1 - iy0); + + if (width ) *width = w; + if (height) *height = h; + if (xoff ) *xoff = ix0; + if (yoff ) *yoff = iy0; + + // invert for y-downwards bitmaps + scale_y = -scale_y; + + { + // distance from singular values (in the same units as the pixel grid) + const float eps = 1./1024, eps2 = eps*eps; + int x,y,i,j; + float *precompute; + stbtt_vertex *verts; + int num_verts = stbtt_GetGlyphShape(info, glyph, &verts); + data = (unsigned char *) STBTT_malloc(w * h, info->userdata); + precompute = (float *) STBTT_malloc(num_verts * sizeof(float), info->userdata); + + for (i=0,j=num_verts-1; i < num_verts; j=i++) { + if (verts[i].type == STBTT_vline) { + float x0 = verts[i].x*scale_x, y0 = verts[i].y*scale_y; + float x1 = verts[j].x*scale_x, y1 = verts[j].y*scale_y; + float dist = (float) STBTT_sqrt((x1-x0)*(x1-x0) + (y1-y0)*(y1-y0)); + precompute[i] = (dist < eps) ? 0.0f : 1.0f / dist; + } else if (verts[i].type == STBTT_vcurve) { + float x2 = verts[j].x *scale_x, y2 = verts[j].y *scale_y; + float x1 = verts[i].cx*scale_x, y1 = verts[i].cy*scale_y; + float x0 = verts[i].x *scale_x, y0 = verts[i].y *scale_y; + float bx = x0 - 2*x1 + x2, by = y0 - 2*y1 + y2; + float len2 = bx*bx + by*by; + if (len2 >= eps2) + precompute[i] = 1.0f / len2; + else + precompute[i] = 0.0f; + } else + precompute[i] = 0.0f; + } + + for (y=iy0; y < iy1; ++y) { + for (x=ix0; x < ix1; ++x) { + float val; + float min_dist = 999999.0f; + float sx = (float) x + 0.5f; + float sy = (float) y + 0.5f; + float x_gspace = (sx / scale_x); + float y_gspace = (sy / scale_y); + + int winding = stbtt__compute_crossings_x(x_gspace, y_gspace, num_verts, verts); // @OPTIMIZE: this could just be a rasterization, but needs to be line vs. non-tesselated curves so a new path + + for (i=0; i < num_verts; ++i) { + float x0 = verts[i].x*scale_x, y0 = verts[i].y*scale_y; + + if (verts[i].type == STBTT_vline && precompute[i] != 0.0f) { + float x1 = verts[i-1].x*scale_x, y1 = verts[i-1].y*scale_y; + + float dist,dist2 = (x0-sx)*(x0-sx) + (y0-sy)*(y0-sy); + if (dist2 < min_dist*min_dist) + min_dist = (float) STBTT_sqrt(dist2); + + // coarse culling against bbox + //if (sx > STBTT_min(x0,x1)-min_dist && sx < STBTT_max(x0,x1)+min_dist && + // sy > STBTT_min(y0,y1)-min_dist && sy < STBTT_max(y0,y1)+min_dist) + dist = (float) STBTT_fabs((x1-x0)*(y0-sy) - (y1-y0)*(x0-sx)) * precompute[i]; + STBTT_assert(i != 0); + if (dist < min_dist) { + // check position along line + // x' = x0 + t*(x1-x0), y' = y0 + t*(y1-y0) + // minimize (x'-sx)*(x'-sx)+(y'-sy)*(y'-sy) + float dx = x1-x0, dy = y1-y0; + float px = x0-sx, py = y0-sy; + // minimize (px+t*dx)^2 + (py+t*dy)^2 = px*px + 2*px*dx*t + t^2*dx*dx + py*py + 2*py*dy*t + t^2*dy*dy + // derivative: 2*px*dx + 2*py*dy + (2*dx*dx+2*dy*dy)*t, set to 0 and solve + float t = -(px*dx + py*dy) / (dx*dx + dy*dy); + if (t >= 0.0f && t <= 1.0f) + min_dist = dist; + } + } else if (verts[i].type == STBTT_vcurve) { + float x2 = verts[i-1].x *scale_x, y2 = verts[i-1].y *scale_y; + float x1 = verts[i ].cx*scale_x, y1 = verts[i ].cy*scale_y; + float box_x0 = STBTT_min(STBTT_min(x0,x1),x2); + float box_y0 = STBTT_min(STBTT_min(y0,y1),y2); + float box_x1 = STBTT_max(STBTT_max(x0,x1),x2); + float box_y1 = STBTT_max(STBTT_max(y0,y1),y2); + // coarse culling against bbox to avoid computing cubic unnecessarily + if (sx > box_x0-min_dist && sx < box_x1+min_dist && sy > box_y0-min_dist && sy < box_y1+min_dist) { + int num=0; + float ax = x1-x0, ay = y1-y0; + float bx = x0 - 2*x1 + x2, by = y0 - 2*y1 + y2; + float mx = x0 - sx, my = y0 - sy; + float res[3] = {0.f,0.f,0.f}; + float px,py,t,it,dist2; + float a_inv = precompute[i]; + if (a_inv == 0.0) { // if a_inv is 0, it's 2nd degree so use quadratic formula + float a = 3*(ax*bx + ay*by); + float b = 2*(ax*ax + ay*ay) + (mx*bx+my*by); + float c = mx*ax+my*ay; + if (STBTT_fabs(a) < eps2) { // if a is 0, it's linear + if (STBTT_fabs(b) >= eps2) { + res[num++] = -c/b; + } + } else { + float discriminant = b*b - 4*a*c; + if (discriminant < 0) + num = 0; + else { + float root = (float) STBTT_sqrt(discriminant); + res[0] = (-b - root)/(2*a); + res[1] = (-b + root)/(2*a); + num = 2; // don't bother distinguishing 1-solution case, as code below will still work + } + } + } else { + float b = 3*(ax*bx + ay*by) * a_inv; // could precompute this as it doesn't depend on sample point + float c = (2*(ax*ax + ay*ay) + (mx*bx+my*by)) * a_inv; + float d = (mx*ax+my*ay) * a_inv; + num = stbtt__solve_cubic(b, c, d, res); + } + dist2 = (x0-sx)*(x0-sx) + (y0-sy)*(y0-sy); + if (dist2 < min_dist*min_dist) + min_dist = (float) STBTT_sqrt(dist2); + + if (num >= 1 && res[0] >= 0.0f && res[0] <= 1.0f) { + t = res[0], it = 1.0f - t; + px = it*it*x0 + 2*t*it*x1 + t*t*x2; + py = it*it*y0 + 2*t*it*y1 + t*t*y2; + dist2 = (px-sx)*(px-sx) + (py-sy)*(py-sy); + if (dist2 < min_dist * min_dist) + min_dist = (float) STBTT_sqrt(dist2); + } + if (num >= 2 && res[1] >= 0.0f && res[1] <= 1.0f) { + t = res[1], it = 1.0f - t; + px = it*it*x0 + 2*t*it*x1 + t*t*x2; + py = it*it*y0 + 2*t*it*y1 + t*t*y2; + dist2 = (px-sx)*(px-sx) + (py-sy)*(py-sy); + if (dist2 < min_dist * min_dist) + min_dist = (float) STBTT_sqrt(dist2); + } + if (num >= 3 && res[2] >= 0.0f && res[2] <= 1.0f) { + t = res[2], it = 1.0f - t; + px = it*it*x0 + 2*t*it*x1 + t*t*x2; + py = it*it*y0 + 2*t*it*y1 + t*t*y2; + dist2 = (px-sx)*(px-sx) + (py-sy)*(py-sy); + if (dist2 < min_dist * min_dist) + min_dist = (float) STBTT_sqrt(dist2); + } + } + } + } + if (winding == 0) + min_dist = -min_dist; // if outside the shape, value is negative + val = onedge_value + pixel_dist_scale * min_dist; + if (val < 0) + val = 0; + else if (val > 255) + val = 255; + data[(y-iy0)*w+(x-ix0)] = (unsigned char) val; + } + } + STBTT_free(precompute, info->userdata); + STBTT_free(verts, info->userdata); + } + return data; +} + +STBTT_DEF unsigned char * stbtt_GetCodepointSDF(const stbtt_fontinfo *info, float scale, int codepoint, int padding, unsigned char onedge_value, float pixel_dist_scale, int *width, int *height, int *xoff, int *yoff) +{ + return stbtt_GetGlyphSDF(info, scale, stbtt_FindGlyphIndex(info, codepoint), padding, onedge_value, pixel_dist_scale, width, height, xoff, yoff); +} + +STBTT_DEF void stbtt_FreeSDF(unsigned char *bitmap, void *userdata) +{ + STBTT_free(bitmap, userdata); +} + +////////////////////////////////////////////////////////////////////////////// +// +// font name matching -- recommended not to use this +// + +// check if a utf8 string contains a prefix which is the utf16 string; if so return length of matching utf8 string +static stbtt_int32 stbtt__CompareUTF8toUTF16_bigendian_prefix(stbtt_uint8 *s1, stbtt_int32 len1, stbtt_uint8 *s2, stbtt_int32 len2) +{ + stbtt_int32 i=0; + + // convert utf16 to utf8 and compare the results while converting + while (len2) { + stbtt_uint16 ch = s2[0]*256 + s2[1]; + if (ch < 0x80) { + if (i >= len1) return -1; + if (s1[i++] != ch) return -1; + } else if (ch < 0x800) { + if (i+1 >= len1) return -1; + if (s1[i++] != 0xc0 + (ch >> 6)) return -1; + if (s1[i++] != 0x80 + (ch & 0x3f)) return -1; + } else if (ch >= 0xd800 && ch < 0xdc00) { + stbtt_uint32 c; + stbtt_uint16 ch2 = s2[2]*256 + s2[3]; + if (i+3 >= len1) return -1; + c = ((ch - 0xd800) << 10) + (ch2 - 0xdc00) + 0x10000; + if (s1[i++] != 0xf0 + (c >> 18)) return -1; + if (s1[i++] != 0x80 + ((c >> 12) & 0x3f)) return -1; + if (s1[i++] != 0x80 + ((c >> 6) & 0x3f)) return -1; + if (s1[i++] != 0x80 + ((c ) & 0x3f)) return -1; + s2 += 2; // plus another 2 below + len2 -= 2; + } else if (ch >= 0xdc00 && ch < 0xe000) { + return -1; + } else { + if (i+2 >= len1) return -1; + if (s1[i++] != 0xe0 + (ch >> 12)) return -1; + if (s1[i++] != 0x80 + ((ch >> 6) & 0x3f)) return -1; + if (s1[i++] != 0x80 + ((ch ) & 0x3f)) return -1; + } + s2 += 2; + len2 -= 2; + } + return i; +} + +static int stbtt_CompareUTF8toUTF16_bigendian_internal(char *s1, int len1, char *s2, int len2) +{ + return len1 == stbtt__CompareUTF8toUTF16_bigendian_prefix((stbtt_uint8*) s1, len1, (stbtt_uint8*) s2, len2); +} + +// returns results in whatever encoding you request... but note that 2-byte encodings +// will be BIG-ENDIAN... use stbtt_CompareUTF8toUTF16_bigendian() to compare +STBTT_DEF const char *stbtt_GetFontNameString(const stbtt_fontinfo *font, int *length, int platformID, int encodingID, int languageID, int nameID) +{ + stbtt_int32 i,count,stringOffset; + stbtt_uint8 *fc = font->data; + stbtt_uint32 offset = font->fontstart; + stbtt_uint32 nm = stbtt__find_table(fc, offset, "name"); + if (!nm) return NULL; + + count = ttUSHORT(fc+nm+2); + stringOffset = nm + ttUSHORT(fc+nm+4); + for (i=0; i < count; ++i) { + stbtt_uint32 loc = nm + 6 + 12 * i; + if (platformID == ttUSHORT(fc+loc+0) && encodingID == ttUSHORT(fc+loc+2) + && languageID == ttUSHORT(fc+loc+4) && nameID == ttUSHORT(fc+loc+6)) { + *length = ttUSHORT(fc+loc+8); + return (const char *) (fc+stringOffset+ttUSHORT(fc+loc+10)); + } + } + return NULL; +} + +static int stbtt__matchpair(stbtt_uint8 *fc, stbtt_uint32 nm, stbtt_uint8 *name, stbtt_int32 nlen, stbtt_int32 target_id, stbtt_int32 next_id) +{ + stbtt_int32 i; + stbtt_int32 count = ttUSHORT(fc+nm+2); + stbtt_int32 stringOffset = nm + ttUSHORT(fc+nm+4); + + for (i=0; i < count; ++i) { + stbtt_uint32 loc = nm + 6 + 12 * i; + stbtt_int32 id = ttUSHORT(fc+loc+6); + if (id == target_id) { + // find the encoding + stbtt_int32 platform = ttUSHORT(fc+loc+0), encoding = ttUSHORT(fc+loc+2), language = ttUSHORT(fc+loc+4); + + // is this a Unicode encoding? + if (platform == 0 || (platform == 3 && encoding == 1) || (platform == 3 && encoding == 10)) { + stbtt_int32 slen = ttUSHORT(fc+loc+8); + stbtt_int32 off = ttUSHORT(fc+loc+10); + + // check if there's a prefix match + stbtt_int32 matchlen = stbtt__CompareUTF8toUTF16_bigendian_prefix(name, nlen, fc+stringOffset+off,slen); + if (matchlen >= 0) { + // check for target_id+1 immediately following, with same encoding & language + if (i+1 < count && ttUSHORT(fc+loc+12+6) == next_id && ttUSHORT(fc+loc+12) == platform && ttUSHORT(fc+loc+12+2) == encoding && ttUSHORT(fc+loc+12+4) == language) { + slen = ttUSHORT(fc+loc+12+8); + off = ttUSHORT(fc+loc+12+10); + if (slen == 0) { + if (matchlen == nlen) + return 1; + } else if (matchlen < nlen && name[matchlen] == ' ') { + ++matchlen; + if (stbtt_CompareUTF8toUTF16_bigendian_internal((char*) (name+matchlen), nlen-matchlen, (char*)(fc+stringOffset+off),slen)) + return 1; + } + } else { + // if nothing immediately following + if (matchlen == nlen) + return 1; + } + } + } + + // @TODO handle other encodings + } + } + return 0; +} + +static int stbtt__matches(stbtt_uint8 *fc, stbtt_uint32 offset, stbtt_uint8 *name, stbtt_int32 flags) +{ + stbtt_int32 nlen = (stbtt_int32) STBTT_strlen((char *) name); + stbtt_uint32 nm,hd; + if (!stbtt__isfont(fc+offset)) return 0; + + // check italics/bold/underline flags in macStyle... + if (flags) { + hd = stbtt__find_table(fc, offset, "head"); + if ((ttUSHORT(fc+hd+44) & 7) != (flags & 7)) return 0; + } + + nm = stbtt__find_table(fc, offset, "name"); + if (!nm) return 0; + + if (flags) { + // if we checked the macStyle flags, then just check the family and ignore the subfamily + if (stbtt__matchpair(fc, nm, name, nlen, 16, -1)) return 1; + if (stbtt__matchpair(fc, nm, name, nlen, 1, -1)) return 1; + if (stbtt__matchpair(fc, nm, name, nlen, 3, -1)) return 1; + } else { + if (stbtt__matchpair(fc, nm, name, nlen, 16, 17)) return 1; + if (stbtt__matchpair(fc, nm, name, nlen, 1, 2)) return 1; + if (stbtt__matchpair(fc, nm, name, nlen, 3, -1)) return 1; + } + + return 0; +} + +static int stbtt_FindMatchingFont_internal(unsigned char *font_collection, char *name_utf8, stbtt_int32 flags) +{ + stbtt_int32 i; + for (i=0;;++i) { + stbtt_int32 off = stbtt_GetFontOffsetForIndex(font_collection, i); + if (off < 0) return off; + if (stbtt__matches((stbtt_uint8 *) font_collection, off, (stbtt_uint8*) name_utf8, flags)) + return off; + } +} + +#if defined(__GNUC__) || defined(__clang__) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-qual" +#endif + +STBTT_DEF int stbtt_BakeFontBitmap(const unsigned char *data, int offset, + float pixel_height, unsigned char *pixels, int pw, int ph, + int first_char, int num_chars, stbtt_bakedchar *chardata) +{ + return stbtt_BakeFontBitmap_internal((unsigned char *) data, offset, pixel_height, pixels, pw, ph, first_char, num_chars, chardata); +} + +STBTT_DEF int stbtt_GetFontOffsetForIndex(const unsigned char *data, int index) +{ + return stbtt_GetFontOffsetForIndex_internal((unsigned char *) data, index); +} + +STBTT_DEF int stbtt_GetNumberOfFonts(const unsigned char *data) +{ + return stbtt_GetNumberOfFonts_internal((unsigned char *) data); +} + +STBTT_DEF int stbtt_InitFont(stbtt_fontinfo *info, const unsigned char *data, int offset) +{ + return stbtt_InitFont_internal(info, (unsigned char *) data, offset); +} + +STBTT_DEF int stbtt_FindMatchingFont(const unsigned char *fontdata, const char *name, int flags) +{ + return stbtt_FindMatchingFont_internal((unsigned char *) fontdata, (char *) name, flags); +} + +STBTT_DEF int stbtt_CompareUTF8toUTF16_bigendian(const char *s1, int len1, const char *s2, int len2) +{ + return stbtt_CompareUTF8toUTF16_bigendian_internal((char *) s1, len1, (char *) s2, len2); +} + +#if defined(__GNUC__) || defined(__clang__) +#pragma GCC diagnostic pop +#endif + +#endif // STB_TRUETYPE_IMPLEMENTATION + + +// FULL VERSION HISTORY +// +// 1.25 (2021-07-11) many fixes +// 1.24 (2020-02-05) fix warning +// 1.23 (2020-02-02) query SVG data for glyphs; query whole kerning table (but only kern not GPOS) +// 1.22 (2019-08-11) minimize missing-glyph duplication; fix kerning if both 'GPOS' and 'kern' are defined +// 1.21 (2019-02-25) fix warning +// 1.20 (2019-02-07) PackFontRange skips missing codepoints; GetScaleFontVMetrics() +// 1.19 (2018-02-11) OpenType GPOS kerning (horizontal only), STBTT_fmod +// 1.18 (2018-01-29) add missing function +// 1.17 (2017-07-23) make more arguments const; doc fix +// 1.16 (2017-07-12) SDF support +// 1.15 (2017-03-03) make more arguments const +// 1.14 (2017-01-16) num-fonts-in-TTC function +// 1.13 (2017-01-02) support OpenType fonts, certain Apple fonts +// 1.12 (2016-10-25) suppress warnings about casting away const with -Wcast-qual +// 1.11 (2016-04-02) fix unused-variable warning +// 1.10 (2016-04-02) allow user-defined fabs() replacement +// fix memory leak if fontsize=0.0 +// fix warning from duplicate typedef +// 1.09 (2016-01-16) warning fix; avoid crash on outofmem; use alloc userdata for PackFontRanges +// 1.08 (2015-09-13) document stbtt_Rasterize(); fixes for vertical & horizontal edges +// 1.07 (2015-08-01) allow PackFontRanges to accept arrays of sparse codepoints; +// allow PackFontRanges to pack and render in separate phases; +// fix stbtt_GetFontOFfsetForIndex (never worked for non-0 input?); +// fixed an assert() bug in the new rasterizer +// replace assert() with STBTT_assert() in new rasterizer +// 1.06 (2015-07-14) performance improvements (~35% faster on x86 and x64 on test machine) +// also more precise AA rasterizer, except if shapes overlap +// remove need for STBTT_sort +// 1.05 (2015-04-15) fix misplaced definitions for STBTT_STATIC +// 1.04 (2015-04-15) typo in example +// 1.03 (2015-04-12) STBTT_STATIC, fix memory leak in new packing, various fixes +// 1.02 (2014-12-10) fix various warnings & compile issues w/ stb_rect_pack, C++ +// 1.01 (2014-12-08) fix subpixel position when oversampling to exactly match +// non-oversampled; STBTT_POINT_SIZE for packed case only +// 1.00 (2014-12-06) add new PackBegin etc. API, w/ support for oversampling +// 0.99 (2014-09-18) fix multiple bugs with subpixel rendering (ryg) +// 0.9 (2014-08-07) support certain mac/iOS fonts without an MS platformID +// 0.8b (2014-07-07) fix a warning +// 0.8 (2014-05-25) fix a few more warnings +// 0.7 (2013-09-25) bugfix: subpixel glyph bug fixed in 0.5 had come back +// 0.6c (2012-07-24) improve documentation +// 0.6b (2012-07-20) fix a few more warnings +// 0.6 (2012-07-17) fix warnings; added stbtt_ScaleForMappingEmToPixels, +// stbtt_GetFontBoundingBox, stbtt_IsGlyphEmpty +// 0.5 (2011-12-09) bugfixes: +// subpixel glyph renderer computed wrong bounding box +// first vertex of shape can be off-curve (FreeSans) +// 0.4b (2011-12-03) fixed an error in the font baking example +// 0.4 (2011-12-01) kerning, subpixel rendering (tor) +// bugfixes for: +// codepoint-to-glyph conversion using table fmt=12 +// codepoint-to-glyph conversion using table fmt=4 +// stbtt_GetBakedQuad with non-square texture (Zer) +// updated Hello World! sample to use kerning and subpixel +// fixed some warnings +// 0.3 (2009-06-24) cmap fmt=12, compound shapes (MM) +// userdata, malloc-from-userdata, non-zero fill (stb) +// 0.2 (2009-03-11) Fix unsigned/signed char warnings +// 0.1 (2009-03-09) First public release +// + +/* +------------------------------------------------------------------------------ +This software is available under 2 licenses -- choose whichever you prefer. +------------------------------------------------------------------------------ +ALTERNATIVE A - MIT License +Copyright (c) 2017 Sean Barrett +Permission is hereby granted, free of charge, to any person obtaining a copy of +this software and associated documentation files (the "Software"), to deal in +the Software without restriction, including without limitation the rights to +use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies +of the Software, and to permit persons to whom the Software is furnished to do +so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +------------------------------------------------------------------------------ +ALTERNATIVE B - Public Domain (www.unlicense.org) +This is free and unencumbered software released into the public domain. +Anyone is free to copy, modify, publish, use, compile, sell, or distribute this +software, either in source code form or as a compiled binary, for any purpose, +commercial or non-commercial, and by any means. +In jurisdictions that recognize copyright laws, the author or authors of this +software dedicate any and all copyright interest in the software to the public +domain. We make this dedication for the benefit of the public at large and to +the detriment of our heirs and successors. We intend this dedication to be an +overt act of relinquishment in perpetuity of all present and future rights to +this software under copyright law. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN +ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION +WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +------------------------------------------------------------------------------ +*/ diff --git a/samples/tiny_obj_loader.h b/samples/tiny_obj_loader.h new file mode 100644 index 0000000..942b149 --- /dev/null +++ b/samples/tiny_obj_loader.h @@ -0,0 +1,3499 @@ +/* +The MIT License (MIT) + +Copyright (c) 2012-Present, Syoyo Fujita and many contributors. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +*/ + +// +// version 2.0.0 : Add new object oriented API. 1.x API is still provided. +// * Add python binding. +// * Support line primitive. +// * Support points primitive. +// * Support multiple search path for .mtl(v1 API). +// * Support vertex skinning weight `vw`(as an tinyobj +// extension). Note that this differs vertex weight([w] +// component in `v` line) +// * Support escaped whitespece in mtllib +// * Add robust triangulation using Mapbox +// earcut(TINYOBJLOADER_USE_MAPBOX_EARCUT). +// version 1.4.0 : Modifed ParseTextureNameAndOption API +// version 1.3.1 : Make ParseTextureNameAndOption API public +// version 1.3.0 : Separate warning and error message(breaking API of LoadObj) +// version 1.2.3 : Added color space extension('-colorspace') to tex opts. +// version 1.2.2 : Parse multiple group names. +// version 1.2.1 : Added initial support for line('l') primitive(PR #178) +// version 1.2.0 : Hardened implementation(#175) +// version 1.1.1 : Support smoothing groups(#162) +// version 1.1.0 : Support parsing vertex color(#144) +// version 1.0.8 : Fix parsing `g` tag just after `usemtl`(#138) +// version 1.0.7 : Support multiple tex options(#126) +// version 1.0.6 : Add TINYOBJLOADER_USE_DOUBLE option(#124) +// version 1.0.5 : Ignore `Tr` when `d` exists in MTL(#43) +// version 1.0.4 : Support multiple filenames for 'mtllib'(#112) +// version 1.0.3 : Support parsing texture options(#85) +// version 1.0.2 : Improve parsing speed by about a factor of 2 for large +// files(#105) +// version 1.0.1 : Fixes a shape is lost if obj ends with a 'usemtl'(#104) +// version 1.0.0 : Change data structure. Change license from BSD to MIT. +// + +// +// Use this in *one* .cc +// #define TINYOBJLOADER_IMPLEMENTATION +// #include "tiny_obj_loader.h" +// + +#ifndef TINY_OBJ_LOADER_H_ +#define TINY_OBJ_LOADER_H_ + +#include +#include +#include + +namespace tinyobj { + +// TODO(syoyo): Better C++11 detection for older compiler +#if __cplusplus > 199711L +#define TINYOBJ_OVERRIDE override +#else +#define TINYOBJ_OVERRIDE +#endif + +#ifdef __clang__ +#pragma clang diagnostic push +#if __has_warning("-Wzero-as-null-pointer-constant") +#pragma clang diagnostic ignored "-Wzero-as-null-pointer-constant" +#endif + +#pragma clang diagnostic ignored "-Wpadded" + +#endif + +// https://en.wikipedia.org/wiki/Wavefront_.obj_file says ... +// +// -blendu on | off # set horizontal texture blending +// (default on) +// -blendv on | off # set vertical texture blending +// (default on) +// -boost real_value # boost mip-map sharpness +// -mm base_value gain_value # modify texture map values (default +// 0 1) +// # base_value = brightness, +// gain_value = contrast +// -o u [v [w]] # Origin offset (default +// 0 0 0) +// -s u [v [w]] # Scale (default +// 1 1 1) +// -t u [v [w]] # Turbulence (default +// 0 0 0) +// -texres resolution # texture resolution to create +// -clamp on | off # only render texels in the clamped +// 0-1 range (default off) +// # When unclamped, textures are +// repeated across a surface, +// # when clamped, only texels which +// fall within the 0-1 +// # range are rendered. +// -bm mult_value # bump multiplier (for bump maps +// only) +// +// -imfchan r | g | b | m | l | z # specifies which channel of the file +// is used to +// # create a scalar or bump texture. +// r:red, g:green, +// # b:blue, m:matte, l:luminance, +// z:z-depth.. +// # (the default for bump is 'l' and +// for decal is 'm') +// bump -imfchan r bumpmap.tga # says to use the red channel of +// bumpmap.tga as the bumpmap +// +// For reflection maps... +// +// -type sphere # specifies a sphere for a "refl" +// reflection map +// -type cube_top | cube_bottom | # when using a cube map, the texture +// file for each +// cube_front | cube_back | # side of the cube is specified +// separately +// cube_left | cube_right +// +// TinyObjLoader extension. +// +// -colorspace SPACE # Color space of the texture. e.g. +// 'sRGB` or 'linear' +// + +#ifdef TINYOBJLOADER_USE_DOUBLE +//#pragma message "using double" +typedef double real_t; +#else +//#pragma message "using float" +typedef float real_t; +#endif + +typedef enum { + TEXTURE_TYPE_NONE, // default + TEXTURE_TYPE_SPHERE, + TEXTURE_TYPE_CUBE_TOP, + TEXTURE_TYPE_CUBE_BOTTOM, + TEXTURE_TYPE_CUBE_FRONT, + TEXTURE_TYPE_CUBE_BACK, + TEXTURE_TYPE_CUBE_LEFT, + TEXTURE_TYPE_CUBE_RIGHT +} texture_type_t; + +struct texture_option_t { + texture_type_t type; // -type (default TEXTURE_TYPE_NONE) + real_t sharpness; // -boost (default 1.0?) + real_t brightness; // base_value in -mm option (default 0) + real_t contrast; // gain_value in -mm option (default 1) + real_t origin_offset[3]; // -o u [v [w]] (default 0 0 0) + real_t scale[3]; // -s u [v [w]] (default 1 1 1) + real_t turbulence[3]; // -t u [v [w]] (default 0 0 0) + int texture_resolution; // -texres resolution (No default value in the spec. + // We'll use -1) + bool clamp; // -clamp (default false) + char imfchan; // -imfchan (the default for bump is 'l' and for decal is 'm') + bool blendu; // -blendu (default on) + bool blendv; // -blendv (default on) + real_t bump_multiplier; // -bm (for bump maps only, default 1.0) + + // extension + std::string colorspace; // Explicitly specify color space of stored texel + // value. Usually `sRGB` or `linear` (default empty). +}; + +struct material_t { + std::string name; + + real_t ambient[3]; + real_t diffuse[3]; + real_t specular[3]; + real_t transmittance[3]; + real_t emission[3]; + real_t shininess; + real_t ior; // index of refraction + real_t dissolve; // 1 == opaque; 0 == fully transparent + // illumination model (see http://www.fileformat.info/format/material/) + int illum; + + int dummy; // Suppress padding warning. + + std::string ambient_texname; // map_Ka. For ambient or ambient occlusion. + std::string diffuse_texname; // map_Kd + std::string specular_texname; // map_Ks + std::string specular_highlight_texname; // map_Ns + std::string bump_texname; // map_bump, map_Bump, bump + std::string displacement_texname; // disp + std::string alpha_texname; // map_d + std::string reflection_texname; // refl + + texture_option_t ambient_texopt; + texture_option_t diffuse_texopt; + texture_option_t specular_texopt; + texture_option_t specular_highlight_texopt; + texture_option_t bump_texopt; + texture_option_t displacement_texopt; + texture_option_t alpha_texopt; + texture_option_t reflection_texopt; + + // PBR extension + // http://exocortex.com/blog/extending_wavefront_mtl_to_support_pbr + real_t roughness; // [0, 1] default 0 + real_t metallic; // [0, 1] default 0 + real_t sheen; // [0, 1] default 0 + real_t clearcoat_thickness; // [0, 1] default 0 + real_t clearcoat_roughness; // [0, 1] default 0 + real_t anisotropy; // aniso. [0, 1] default 0 + real_t anisotropy_rotation; // anisor. [0, 1] default 0 + real_t pad0; + std::string roughness_texname; // map_Pr + std::string metallic_texname; // map_Pm + std::string sheen_texname; // map_Ps + std::string emissive_texname; // map_Ke + std::string normal_texname; // norm. For normal mapping. + + texture_option_t roughness_texopt; + texture_option_t metallic_texopt; + texture_option_t sheen_texopt; + texture_option_t emissive_texopt; + texture_option_t normal_texopt; + + int pad2; + + std::map unknown_parameter; + +#ifdef TINY_OBJ_LOADER_PYTHON_BINDING + // For pybind11 + std::array GetDiffuse() { + std::array values; + values[0] = double(diffuse[0]); + values[1] = double(diffuse[1]); + values[2] = double(diffuse[2]); + + return values; + } + + std::array GetSpecular() { + std::array values; + values[0] = double(specular[0]); + values[1] = double(specular[1]); + values[2] = double(specular[2]); + + return values; + } + + std::array GetTransmittance() { + std::array values; + values[0] = double(transmittance[0]); + values[1] = double(transmittance[1]); + values[2] = double(transmittance[2]); + + return values; + } + + std::array GetEmission() { + std::array values; + values[0] = double(emission[0]); + values[1] = double(emission[1]); + values[2] = double(emission[2]); + + return values; + } + + std::array GetAmbient() { + std::array values; + values[0] = double(ambient[0]); + values[1] = double(ambient[1]); + values[2] = double(ambient[2]); + + return values; + } + + void SetDiffuse(std::array &a) { + diffuse[0] = real_t(a[0]); + diffuse[1] = real_t(a[1]); + diffuse[2] = real_t(a[2]); + } + + void SetAmbient(std::array &a) { + ambient[0] = real_t(a[0]); + ambient[1] = real_t(a[1]); + ambient[2] = real_t(a[2]); + } + + void SetSpecular(std::array &a) { + specular[0] = real_t(a[0]); + specular[1] = real_t(a[1]); + specular[2] = real_t(a[2]); + } + + void SetTransmittance(std::array &a) { + transmittance[0] = real_t(a[0]); + transmittance[1] = real_t(a[1]); + transmittance[2] = real_t(a[2]); + } + + std::string GetCustomParameter(const std::string &key) { + std::map::const_iterator it = + unknown_parameter.find(key); + + if (it != unknown_parameter.end()) { + return it->second; + } + return std::string(); + } + +#endif +}; + +struct tag_t { + std::string name; + + std::vector intValues; + std::vector floatValues; + std::vector stringValues; +}; + +struct joint_and_weight_t { + int joint_id; + real_t weight; +}; + +struct skin_weight_t { + int vertex_id; // Corresponding vertex index in `attrib_t::vertices`. + // Compared to `index_t`, this index must be positive and + // start with 0(does not allow relative indexing) + std::vector weightValues; +}; + +// Index struct to support different indices for vtx/normal/texcoord. +// -1 means not used. +struct index_t { + int vertex_index; + int normal_index; + int texcoord_index; +}; + +struct mesh_t { + std::vector indices; + std::vector + num_face_vertices; // The number of vertices per + // face. 3 = triangle, 4 = quad, ... + std::vector material_ids; // per-face material ID + std::vector smoothing_group_ids; // per-face smoothing group + // ID(0 = off. positive value + // = group id) + std::vector tags; // SubD tag +}; + +// struct path_t { +// std::vector indices; // pairs of indices for lines +//}; + +struct lines_t { + // Linear flattened indices. + std::vector indices; // indices for vertices(poly lines) + std::vector num_line_vertices; // The number of vertices per line. +}; + +struct points_t { + std::vector indices; // indices for points +}; + +struct shape_t { + std::string name; + mesh_t mesh; + lines_t lines; + points_t points; +}; + +// Vertex attributes +struct attrib_t { + std::vector vertices; // 'v'(xyz) + + // For backward compatibility, we store vertex weight in separate array. + std::vector vertex_weights; // 'v'(w) + std::vector normals; // 'vn' + std::vector texcoords; // 'vt'(uv) + + // For backward compatibility, we store texture coordinate 'w' in separate + // array. + std::vector texcoord_ws; // 'vt'(w) + std::vector colors; // extension: vertex colors + + // + // TinyObj extension. + // + + // NOTE(syoyo): array index is based on the appearance order. + // To get a corresponding skin weight for a specific vertex id `vid`, + // Need to reconstruct a look up table: `skin_weight_t::vertex_id` == `vid` + // (e.g. using std::map, std::unordered_map) + std::vector skin_weights; + + attrib_t() {} + + // + // For pybind11 + // + const std::vector &GetVertices() const { return vertices; } + + const std::vector &GetVertexWeights() const { return vertex_weights; } +}; + +struct callback_t { + // W is optional and set to 1 if there is no `w` item in `v` line + void (*vertex_cb)(void *user_data, real_t x, real_t y, real_t z, real_t w); + void (*vertex_color_cb)(void *user_data, real_t x, real_t y, real_t z, + real_t r, real_t g, real_t b, bool has_color); + void (*normal_cb)(void *user_data, real_t x, real_t y, real_t z); + + // y and z are optional and set to 0 if there is no `y` and/or `z` item(s) in + // `vt` line. + void (*texcoord_cb)(void *user_data, real_t x, real_t y, real_t z); + + // called per 'f' line. num_indices is the number of face indices(e.g. 3 for + // triangle, 4 for quad) + // 0 will be passed for undefined index in index_t members. + void (*index_cb)(void *user_data, index_t *indices, int num_indices); + // `name` material name, `material_id` = the array index of material_t[]. -1 + // if + // a material not found in .mtl + void (*usemtl_cb)(void *user_data, const char *name, int material_id); + // `materials` = parsed material data. + void (*mtllib_cb)(void *user_data, const material_t *materials, + int num_materials); + // There may be multiple group names + void (*group_cb)(void *user_data, const char **names, int num_names); + void (*object_cb)(void *user_data, const char *name); + + callback_t() + : vertex_cb(NULL), + vertex_color_cb(NULL), + normal_cb(NULL), + texcoord_cb(NULL), + index_cb(NULL), + usemtl_cb(NULL), + mtllib_cb(NULL), + group_cb(NULL), + object_cb(NULL) {} +}; + +class MaterialReader { + public: + MaterialReader() {} + virtual ~MaterialReader(); + + virtual bool operator()(const std::string &matId, + std::vector *materials, + std::map *matMap, std::string *warn, + std::string *err) = 0; +}; + +/// +/// Read .mtl from a file. +/// +class MaterialFileReader : public MaterialReader { + public: + // Path could contain separator(';' in Windows, ':' in Posix) + explicit MaterialFileReader(const std::string &mtl_basedir) + : m_mtlBaseDir(mtl_basedir) {} + virtual ~MaterialFileReader() TINYOBJ_OVERRIDE {} + virtual bool operator()(const std::string &matId, + std::vector *materials, + std::map *matMap, std::string *warn, + std::string *err) TINYOBJ_OVERRIDE; + + private: + std::string m_mtlBaseDir; +}; + +/// +/// Read .mtl from a stream. +/// +class MaterialStreamReader : public MaterialReader { + public: + explicit MaterialStreamReader(std::istream &inStream) + : m_inStream(inStream) {} + virtual ~MaterialStreamReader() TINYOBJ_OVERRIDE {} + virtual bool operator()(const std::string &matId, + std::vector *materials, + std::map *matMap, std::string *warn, + std::string *err) TINYOBJ_OVERRIDE; + + private: + std::istream &m_inStream; +}; + +// v2 API +struct ObjReaderConfig { + bool triangulate; // triangulate polygon? + + // Currently not used. + // "simple" or empty: Create triangle fan + // "earcut": Use the algorithm based on Ear clipping + std::string triangulation_method; + + /// Parse vertex color. + /// If vertex color is not present, its filled with default value. + /// false = no vertex color + /// This will increase memory of parsed .obj + bool vertex_color; + + /// + /// Search path to .mtl file. + /// Default = "" = search from the same directory of .obj file. + /// Valid only when loading .obj from a file. + /// + std::string mtl_search_path; + + ObjReaderConfig() + : triangulate(true), triangulation_method("simple"), vertex_color(true) {} +}; + +/// +/// Wavefront .obj reader class(v2 API) +/// +class ObjReader { + public: + ObjReader() : valid_(false) {} + + /// + /// Load .obj and .mtl from a file. + /// + /// @param[in] filename wavefront .obj filename + /// @param[in] config Reader configuration + /// + bool ParseFromFile(const std::string &filename, + const ObjReaderConfig &config = ObjReaderConfig()); + + /// + /// Parse .obj from a text string. + /// Need to supply .mtl text string by `mtl_text`. + /// This function ignores `mtllib` line in .obj text. + /// + /// @param[in] obj_text wavefront .obj filename + /// @param[in] mtl_text wavefront .mtl filename + /// @param[in] config Reader configuration + /// + bool ParseFromString(const std::string &obj_text, const std::string &mtl_text, + const ObjReaderConfig &config = ObjReaderConfig()); + + /// + /// .obj was loaded or parsed correctly. + /// + bool Valid() const { return valid_; } + + const attrib_t &GetAttrib() const { return attrib_; } + + const std::vector &GetShapes() const { return shapes_; } + + const std::vector &GetMaterials() const { return materials_; } + + /// + /// Warning message(may be filled after `Load` or `Parse`) + /// + const std::string &Warning() const { return warning_; } + + /// + /// Error message(filled when `Load` or `Parse` failed) + /// + const std::string &Error() const { return error_; } + + private: + bool valid_; + + attrib_t attrib_; + std::vector shapes_; + std::vector materials_; + + std::string warning_; + std::string error_; +}; + +/// ==>>========= Legacy v1 API ============================================= + +/// Loads .obj from a file. +/// 'attrib', 'shapes' and 'materials' will be filled with parsed shape data +/// 'shapes' will be filled with parsed shape data +/// Returns true when loading .obj become success. +/// Returns warning message into `warn`, and error message into `err` +/// 'mtl_basedir' is optional, and used for base directory for .mtl file. +/// In default(`NULL'), .mtl file is searched from an application's working +/// directory. +/// 'triangulate' is optional, and used whether triangulate polygon face in .obj +/// or not. +/// Option 'default_vcols_fallback' specifies whether vertex colors should +/// always be defined, even if no colors are given (fallback to white). +bool LoadObj(attrib_t *attrib, std::vector *shapes, + std::vector *materials, std::string *warn, + std::string *err, const char *filename, + const char *mtl_basedir = NULL, bool triangulate = true, + bool default_vcols_fallback = true); + +/// Loads .obj from a file with custom user callback. +/// .mtl is loaded as usual and parsed material_t data will be passed to +/// `callback.mtllib_cb`. +/// Returns true when loading .obj/.mtl become success. +/// Returns warning message into `warn`, and error message into `err` +/// See `examples/callback_api/` for how to use this function. +bool LoadObjWithCallback(std::istream &inStream, const callback_t &callback, + void *user_data = NULL, + MaterialReader *readMatFn = NULL, + std::string *warn = NULL, std::string *err = NULL); + +/// Loads object from a std::istream, uses `readMatFn` to retrieve +/// std::istream for materials. +/// Returns true when loading .obj become success. +/// Returns warning and error message into `err` +bool LoadObj(attrib_t *attrib, std::vector *shapes, + std::vector *materials, std::string *warn, + std::string *err, std::istream *inStream, + MaterialReader *readMatFn = NULL, bool triangulate = true, + bool default_vcols_fallback = true); + +/// Loads materials into std::map +void LoadMtl(std::map *material_map, + std::vector *materials, std::istream *inStream, + std::string *warning, std::string *err); + +/// +/// Parse texture name and texture option for custom texture parameter through +/// material::unknown_parameter +/// +/// @param[out] texname Parsed texture name +/// @param[out] texopt Parsed texopt +/// @param[in] linebuf Input string +/// +bool ParseTextureNameAndOption(std::string *texname, texture_option_t *texopt, + const char *linebuf); + +/// =<<========== Legacy v1 API ============================================= + +} // namespace tinyobj + +#endif // TINY_OBJ_LOADER_H_ + +#ifdef TINYOBJLOADER_IMPLEMENTATION +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef TINYOBJLOADER_USE_MAPBOX_EARCUT + +#ifdef TINYOBJLOADER_DONOT_INCLUDE_MAPBOX_EARCUT +// Assume earcut.hpp is included outside of tiny_obj_loader.h +#else + +#ifdef __clang__ +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Weverything" +#endif + +#include + +#include "earcut.h" + +#ifdef __clang__ +#pragma clang diagnostic pop +#endif + +#endif + +#endif // TINYOBJLOADER_USE_MAPBOX_EARCUT + +namespace tinyobj { + +MaterialReader::~MaterialReader() {} + +struct vertex_index_t { + int v_idx, vt_idx, vn_idx; + vertex_index_t() : v_idx(-1), vt_idx(-1), vn_idx(-1) {} + explicit vertex_index_t(int idx) : v_idx(idx), vt_idx(idx), vn_idx(idx) {} + vertex_index_t(int vidx, int vtidx, int vnidx) + : v_idx(vidx), vt_idx(vtidx), vn_idx(vnidx) {} +}; + +// Internal data structure for face representation +// index + smoothing group. +struct face_t { + unsigned int + smoothing_group_id; // smoothing group id. 0 = smoothing groupd is off. + int pad_; + std::vector vertex_indices; // face vertex indices. + + face_t() : smoothing_group_id(0), pad_(0) {} +}; + +// Internal data structure for line representation +struct __line_t { + // l v1/vt1 v2/vt2 ... + // In the specification, line primitrive does not have normal index, but + // TinyObjLoader allow it + std::vector vertex_indices; +}; + +// Internal data structure for points representation +struct __points_t { + // p v1 v2 ... + // In the specification, point primitrive does not have normal index and + // texture coord index, but TinyObjLoader allow it. + std::vector vertex_indices; +}; + +struct tag_sizes { + tag_sizes() : num_ints(0), num_reals(0), num_strings(0) {} + int num_ints; + int num_reals; + int num_strings; +}; + +struct obj_shape { + std::vector v; + std::vector vn; + std::vector vt; +}; + +// +// Manages group of primitives(face, line, points, ...) +struct PrimGroup { + std::vector faceGroup; + std::vector<__line_t> lineGroup; + std::vector<__points_t> pointsGroup; + + void clear() { + faceGroup.clear(); + lineGroup.clear(); + pointsGroup.clear(); + } + + bool IsEmpty() const { + return faceGroup.empty() && lineGroup.empty() && pointsGroup.empty(); + } + + // TODO(syoyo): bspline, surface, ... +}; + +// See +// http://stackoverflow.com/questions/6089231/getting-std-ifstream-to-handle-lf-cr-and-crlf +static std::istream &safeGetline(std::istream &is, std::string &t) { + t.clear(); + + // The characters in the stream are read one-by-one using a std::streambuf. + // That is faster than reading them one-by-one using the std::istream. + // Code that uses streambuf this way must be guarded by a sentry object. + // The sentry object performs various tasks, + // such as thread synchronization and updating the stream state. + + std::istream::sentry se(is, true); + std::streambuf *sb = is.rdbuf(); + + if (se) { + for (;;) { + int c = sb->sbumpc(); + switch (c) { + case '\n': + return is; + case '\r': + if (sb->sgetc() == '\n') sb->sbumpc(); + return is; + case EOF: + // Also handle the case when the last line has no line ending + if (t.empty()) is.setstate(std::ios::eofbit); + return is; + default: + t += static_cast(c); + } + } + } + + return is; +} + +#define IS_SPACE(x) (((x) == ' ') || ((x) == '\t')) +#define IS_DIGIT(x) \ + (static_cast((x) - '0') < static_cast(10)) +#define IS_NEW_LINE(x) (((x) == '\r') || ((x) == '\n') || ((x) == '\0')) + +template +static inline std::string toString(const T &t) { + std::stringstream ss; + ss << t; + return ss.str(); +} + +struct warning_context { + std::string *warn; + size_t line_number; +}; + +// Make index zero-base, and also support relative index. +static inline bool fixIndex(int idx, int n, int *ret, bool allow_zero, + const warning_context &context) { + if (!ret) { + return false; + } + + if (idx > 0) { + (*ret) = idx - 1; + return true; + } + + if (idx == 0) { + // zero is not allowed according to the spec. + if (context.warn) { + (*context.warn) += + "A zero value index found (will have a value of -1 for normal and " + "tex indices. Line " + + toString(context.line_number) + ").\n"; + } + + (*ret) = idx - 1; + return allow_zero; + } + + if (idx < 0) { + (*ret) = n + idx; // negative value = relative + if ((*ret) < 0) { + return false; // invalid relative index + } + return true; + } + + return false; // never reach here. +} + +static inline std::string parseString(const char **token) { + std::string s; + (*token) += strspn((*token), " \t"); + size_t e = strcspn((*token), " \t\r"); + s = std::string((*token), &(*token)[e]); + (*token) += e; + return s; +} + +static inline int parseInt(const char **token) { + (*token) += strspn((*token), " \t"); + int i = atoi((*token)); + (*token) += strcspn((*token), " \t\r"); + return i; +} + +// Tries to parse a floating point number located at s. +// +// s_end should be a location in the string where reading should absolutely +// stop. For example at the end of the string, to prevent buffer overflows. +// +// Parses the following EBNF grammar: +// sign = "+" | "-" ; +// END = ? anything not in digit ? +// digit = "0" | "1" | "2" | "3" | "4" | "5" | "6" | "7" | "8" | "9" ; +// integer = [sign] , digit , {digit} ; +// decimal = integer , ["." , integer] ; +// float = ( decimal , END ) | ( decimal , ("E" | "e") , integer , END ) ; +// +// Valid strings are for example: +// -0 +3.1417e+2 -0.0E-3 1.0324 -1.41 11e2 +// +// If the parsing is a success, result is set to the parsed value and true +// is returned. +// +// The function is greedy and will parse until any of the following happens: +// - a non-conforming character is encountered. +// - s_end is reached. +// +// The following situations triggers a failure: +// - s >= s_end. +// - parse failure. +// +static bool tryParseDouble(const char *s, const char *s_end, double *result) { + if (s >= s_end) { + return false; + } + + double mantissa = 0.0; + // This exponent is base 2 rather than 10. + // However the exponent we parse is supposed to be one of ten, + // thus we must take care to convert the exponent/and or the + // mantissa to a * 2^E, where a is the mantissa and E is the + // exponent. + // To get the final double we will use ldexp, it requires the + // exponent to be in base 2. + int exponent = 0; + + // NOTE: THESE MUST BE DECLARED HERE SINCE WE ARE NOT ALLOWED + // TO JUMP OVER DEFINITIONS. + char sign = '+'; + char exp_sign = '+'; + char const *curr = s; + + // How many characters were read in a loop. + int read = 0; + // Tells whether a loop terminated due to reaching s_end. + bool end_not_reached = false; + bool leading_decimal_dots = false; + + /* + BEGIN PARSING. + */ + + // Find out what sign we've got. + if (*curr == '+' || *curr == '-') { + sign = *curr; + curr++; + if ((curr != s_end) && (*curr == '.')) { + // accept. Somethig like `.7e+2`, `-.5234` + leading_decimal_dots = true; + } + } else if (IS_DIGIT(*curr)) { /* Pass through. */ + } else if (*curr == '.') { + // accept. Somethig like `.7e+2`, `-.5234` + leading_decimal_dots = true; + } else { + goto fail; + } + + // Read the integer part. + end_not_reached = (curr != s_end); + if (!leading_decimal_dots) { + while (end_not_reached && IS_DIGIT(*curr)) { + mantissa *= 10; + mantissa += static_cast(*curr - 0x30); + curr++; + read++; + end_not_reached = (curr != s_end); + } + + // We must make sure we actually got something. + if (read == 0) goto fail; + } + + // We allow numbers of form "#", "###" etc. + if (!end_not_reached) goto assemble; + + // Read the decimal part. + if (*curr == '.') { + curr++; + read = 1; + end_not_reached = (curr != s_end); + while (end_not_reached && IS_DIGIT(*curr)) { + static const double pow_lut[] = { + 1.0, 0.1, 0.01, 0.001, 0.0001, 0.00001, 0.000001, 0.0000001, + }; + const int lut_entries = sizeof pow_lut / sizeof pow_lut[0]; + + // NOTE: Don't use powf here, it will absolutely murder precision. + mantissa += static_cast(*curr - 0x30) * + (read < lut_entries ? pow_lut[read] : std::pow(10.0, -read)); + read++; + curr++; + end_not_reached = (curr != s_end); + } + } else if (*curr == 'e' || *curr == 'E') { + } else { + goto assemble; + } + + if (!end_not_reached) goto assemble; + + // Read the exponent part. + if (*curr == 'e' || *curr == 'E') { + curr++; + // Figure out if a sign is present and if it is. + end_not_reached = (curr != s_end); + if (end_not_reached && (*curr == '+' || *curr == '-')) { + exp_sign = *curr; + curr++; + } else if (IS_DIGIT(*curr)) { /* Pass through. */ + } else { + // Empty E is not allowed. + goto fail; + } + + read = 0; + end_not_reached = (curr != s_end); + while (end_not_reached && IS_DIGIT(*curr)) { + // To avoid annoying MSVC's min/max macro definiton, + // Use hardcoded int max value + if (exponent > + (2147483647 / 10)) { // 2147483647 = std::numeric_limits::max() + // Integer overflow + goto fail; + } + exponent *= 10; + exponent += static_cast(*curr - 0x30); + curr++; + read++; + end_not_reached = (curr != s_end); + } + exponent *= (exp_sign == '+' ? 1 : -1); + if (read == 0) goto fail; + } + +assemble: + *result = (sign == '+' ? 1 : -1) * + (exponent ? std::ldexp(mantissa * std::pow(5.0, exponent), exponent) + : mantissa); + return true; +fail: + return false; +} + +static inline real_t parseReal(const char **token, double default_value = 0.0) { + (*token) += strspn((*token), " \t"); + const char *end = (*token) + strcspn((*token), " \t\r"); + double val = default_value; + tryParseDouble((*token), end, &val); + real_t f = static_cast(val); + (*token) = end; + return f; +} + +static inline bool parseReal(const char **token, real_t *out) { + (*token) += strspn((*token), " \t"); + const char *end = (*token) + strcspn((*token), " \t\r"); + double val; + bool ret = tryParseDouble((*token), end, &val); + if (ret) { + real_t f = static_cast(val); + (*out) = f; + } + (*token) = end; + return ret; +} + +static inline void parseReal2(real_t *x, real_t *y, const char **token, + const double default_x = 0.0, + const double default_y = 0.0) { + (*x) = parseReal(token, default_x); + (*y) = parseReal(token, default_y); +} + +static inline void parseReal3(real_t *x, real_t *y, real_t *z, + const char **token, const double default_x = 0.0, + const double default_y = 0.0, + const double default_z = 0.0) { + (*x) = parseReal(token, default_x); + (*y) = parseReal(token, default_y); + (*z) = parseReal(token, default_z); +} + +#if 0 // not used +static inline void parseV(real_t *x, real_t *y, real_t *z, real_t *w, + const char **token, const double default_x = 0.0, + const double default_y = 0.0, + const double default_z = 0.0, + const double default_w = 1.0) { + (*x) = parseReal(token, default_x); + (*y) = parseReal(token, default_y); + (*z) = parseReal(token, default_z); + (*w) = parseReal(token, default_w); +} +#endif + +// Extension: parse vertex with colors(6 items) +// Return 3: xyz, 4: xyzw, 6: xyzrgb +// `r`: red(case 6) or [w](case 4) +static inline int parseVertexWithColor(real_t *x, real_t *y, real_t *z, + real_t *r, real_t *g, real_t *b, + const char **token, + const double default_x = 0.0, + const double default_y = 0.0, + const double default_z = 0.0) { + // TODO: Check error + (*x) = parseReal(token, default_x); + (*y) = parseReal(token, default_y); + (*z) = parseReal(token, default_z); + + // - 4 components(x, y, z, w) ot 6 components + bool has_r = parseReal(token, r); + + if (!has_r) { + (*r) = (*g) = (*b) = 1.0; + return 3; + } + + bool has_g = parseReal(token, g); + + if (!has_g) { + (*g) = (*b) = 1.0; + return 4; + } + + bool has_b = parseReal(token, b); + + if (!has_b) { + (*r) = (*g) = (*b) = 1.0; + return 3; // treated as xyz + } + + return 6; +} + +static inline bool parseOnOff(const char **token, bool default_value = true) { + (*token) += strspn((*token), " \t"); + const char *end = (*token) + strcspn((*token), " \t\r"); + + bool ret = default_value; + if ((0 == strncmp((*token), "on", 2))) { + ret = true; + } else if ((0 == strncmp((*token), "off", 3))) { + ret = false; + } + + (*token) = end; + return ret; +} + +static inline texture_type_t parseTextureType( + const char **token, texture_type_t default_value = TEXTURE_TYPE_NONE) { + (*token) += strspn((*token), " \t"); + const char *end = (*token) + strcspn((*token), " \t\r"); + texture_type_t ty = default_value; + + if ((0 == strncmp((*token), "cube_top", strlen("cube_top")))) { + ty = TEXTURE_TYPE_CUBE_TOP; + } else if ((0 == strncmp((*token), "cube_bottom", strlen("cube_bottom")))) { + ty = TEXTURE_TYPE_CUBE_BOTTOM; + } else if ((0 == strncmp((*token), "cube_left", strlen("cube_left")))) { + ty = TEXTURE_TYPE_CUBE_LEFT; + } else if ((0 == strncmp((*token), "cube_right", strlen("cube_right")))) { + ty = TEXTURE_TYPE_CUBE_RIGHT; + } else if ((0 == strncmp((*token), "cube_front", strlen("cube_front")))) { + ty = TEXTURE_TYPE_CUBE_FRONT; + } else if ((0 == strncmp((*token), "cube_back", strlen("cube_back")))) { + ty = TEXTURE_TYPE_CUBE_BACK; + } else if ((0 == strncmp((*token), "sphere", strlen("sphere")))) { + ty = TEXTURE_TYPE_SPHERE; + } + + (*token) = end; + return ty; +} + +static tag_sizes parseTagTriple(const char **token) { + tag_sizes ts; + + (*token) += strspn((*token), " \t"); + ts.num_ints = atoi((*token)); + (*token) += strcspn((*token), "/ \t\r"); + if ((*token)[0] != '/') { + return ts; + } + + (*token)++; // Skip '/' + + (*token) += strspn((*token), " \t"); + ts.num_reals = atoi((*token)); + (*token) += strcspn((*token), "/ \t\r"); + if ((*token)[0] != '/') { + return ts; + } + (*token)++; // Skip '/' + + ts.num_strings = parseInt(token); + + return ts; +} + +// Parse triples with index offsets: i, i/j/k, i//k, i/j +static bool parseTriple(const char **token, int vsize, int vnsize, int vtsize, + vertex_index_t *ret, const warning_context &context) { + if (!ret) { + return false; + } + + vertex_index_t vi(-1); + + if (!fixIndex(atoi((*token)), vsize, &vi.v_idx, false, context)) { + return false; + } + + (*token) += strcspn((*token), "/ \t\r"); + if ((*token)[0] != '/') { + (*ret) = vi; + return true; + } + (*token)++; + + // i//k + if ((*token)[0] == '/') { + (*token)++; + if (!fixIndex(atoi((*token)), vnsize, &vi.vn_idx, true, context)) { + return false; + } + (*token) += strcspn((*token), "/ \t\r"); + (*ret) = vi; + return true; + } + + // i/j/k or i/j + if (!fixIndex(atoi((*token)), vtsize, &vi.vt_idx, true, context)) { + return false; + } + + (*token) += strcspn((*token), "/ \t\r"); + if ((*token)[0] != '/') { + (*ret) = vi; + return true; + } + + // i/j/k + (*token)++; // skip '/' + if (!fixIndex(atoi((*token)), vnsize, &vi.vn_idx, true, context)) { + return false; + } + (*token) += strcspn((*token), "/ \t\r"); + + (*ret) = vi; + + return true; +} + +// Parse raw triples: i, i/j/k, i//k, i/j +static vertex_index_t parseRawTriple(const char **token) { + vertex_index_t vi(static_cast(0)); // 0 is an invalid index in OBJ + + vi.v_idx = atoi((*token)); + (*token) += strcspn((*token), "/ \t\r"); + if ((*token)[0] != '/') { + return vi; + } + (*token)++; + + // i//k + if ((*token)[0] == '/') { + (*token)++; + vi.vn_idx = atoi((*token)); + (*token) += strcspn((*token), "/ \t\r"); + return vi; + } + + // i/j/k or i/j + vi.vt_idx = atoi((*token)); + (*token) += strcspn((*token), "/ \t\r"); + if ((*token)[0] != '/') { + return vi; + } + + // i/j/k + (*token)++; // skip '/' + vi.vn_idx = atoi((*token)); + (*token) += strcspn((*token), "/ \t\r"); + return vi; +} + +bool ParseTextureNameAndOption(std::string *texname, texture_option_t *texopt, + const char *linebuf) { + // @todo { write more robust lexer and parser. } + bool found_texname = false; + std::string texture_name; + + const char *token = linebuf; // Assume line ends with NULL + + while (!IS_NEW_LINE((*token))) { + token += strspn(token, " \t"); // skip space + if ((0 == strncmp(token, "-blendu", 7)) && IS_SPACE((token[7]))) { + token += 8; + texopt->blendu = parseOnOff(&token, /* default */ true); + } else if ((0 == strncmp(token, "-blendv", 7)) && IS_SPACE((token[7]))) { + token += 8; + texopt->blendv = parseOnOff(&token, /* default */ true); + } else if ((0 == strncmp(token, "-clamp", 6)) && IS_SPACE((token[6]))) { + token += 7; + texopt->clamp = parseOnOff(&token, /* default */ true); + } else if ((0 == strncmp(token, "-boost", 6)) && IS_SPACE((token[6]))) { + token += 7; + texopt->sharpness = parseReal(&token, 1.0); + } else if ((0 == strncmp(token, "-bm", 3)) && IS_SPACE((token[3]))) { + token += 4; + texopt->bump_multiplier = parseReal(&token, 1.0); + } else if ((0 == strncmp(token, "-o", 2)) && IS_SPACE((token[2]))) { + token += 3; + parseReal3(&(texopt->origin_offset[0]), &(texopt->origin_offset[1]), + &(texopt->origin_offset[2]), &token); + } else if ((0 == strncmp(token, "-s", 2)) && IS_SPACE((token[2]))) { + token += 3; + parseReal3(&(texopt->scale[0]), &(texopt->scale[1]), &(texopt->scale[2]), + &token, 1.0, 1.0, 1.0); + } else if ((0 == strncmp(token, "-t", 2)) && IS_SPACE((token[2]))) { + token += 3; + parseReal3(&(texopt->turbulence[0]), &(texopt->turbulence[1]), + &(texopt->turbulence[2]), &token); + } else if ((0 == strncmp(token, "-type", 5)) && IS_SPACE((token[5]))) { + token += 5; + texopt->type = parseTextureType((&token), TEXTURE_TYPE_NONE); + } else if ((0 == strncmp(token, "-texres", 7)) && IS_SPACE((token[7]))) { + token += 7; + // TODO(syoyo): Check if arg is int type. + texopt->texture_resolution = parseInt(&token); + } else if ((0 == strncmp(token, "-imfchan", 8)) && IS_SPACE((token[8]))) { + token += 9; + token += strspn(token, " \t"); + const char *end = token + strcspn(token, " \t\r"); + if ((end - token) == 1) { // Assume one char for -imfchan + texopt->imfchan = (*token); + } + token = end; + } else if ((0 == strncmp(token, "-mm", 3)) && IS_SPACE((token[3]))) { + token += 4; + parseReal2(&(texopt->brightness), &(texopt->contrast), &token, 0.0, 1.0); + } else if ((0 == strncmp(token, "-colorspace", 11)) && + IS_SPACE((token[11]))) { + token += 12; + texopt->colorspace = parseString(&token); + } else { +// Assume texture filename +#if 0 + size_t len = strcspn(token, " \t\r"); // untile next space + texture_name = std::string(token, token + len); + token += len; + + token += strspn(token, " \t"); // skip space +#else + // Read filename until line end to parse filename containing whitespace + // TODO(syoyo): Support parsing texture option flag after the filename. + texture_name = std::string(token); + token += texture_name.length(); +#endif + + found_texname = true; + } + } + + if (found_texname) { + (*texname) = texture_name; + return true; + } else { + return false; + } +} + +static void InitTexOpt(texture_option_t *texopt, const bool is_bump) { + if (is_bump) { + texopt->imfchan = 'l'; + } else { + texopt->imfchan = 'm'; + } + texopt->bump_multiplier = static_cast(1.0); + texopt->clamp = false; + texopt->blendu = true; + texopt->blendv = true; + texopt->sharpness = static_cast(1.0); + texopt->brightness = static_cast(0.0); + texopt->contrast = static_cast(1.0); + texopt->origin_offset[0] = static_cast(0.0); + texopt->origin_offset[1] = static_cast(0.0); + texopt->origin_offset[2] = static_cast(0.0); + texopt->scale[0] = static_cast(1.0); + texopt->scale[1] = static_cast(1.0); + texopt->scale[2] = static_cast(1.0); + texopt->turbulence[0] = static_cast(0.0); + texopt->turbulence[1] = static_cast(0.0); + texopt->turbulence[2] = static_cast(0.0); + texopt->texture_resolution = -1; + texopt->type = TEXTURE_TYPE_NONE; +} + +static void InitMaterial(material_t *material) { + InitTexOpt(&material->ambient_texopt, /* is_bump */ false); + InitTexOpt(&material->diffuse_texopt, /* is_bump */ false); + InitTexOpt(&material->specular_texopt, /* is_bump */ false); + InitTexOpt(&material->specular_highlight_texopt, /* is_bump */ false); + InitTexOpt(&material->bump_texopt, /* is_bump */ true); + InitTexOpt(&material->displacement_texopt, /* is_bump */ false); + InitTexOpt(&material->alpha_texopt, /* is_bump */ false); + InitTexOpt(&material->reflection_texopt, /* is_bump */ false); + InitTexOpt(&material->roughness_texopt, /* is_bump */ false); + InitTexOpt(&material->metallic_texopt, /* is_bump */ false); + InitTexOpt(&material->sheen_texopt, /* is_bump */ false); + InitTexOpt(&material->emissive_texopt, /* is_bump */ false); + InitTexOpt(&material->normal_texopt, + /* is_bump */ false); // @fixme { is_bump will be true? } + material->name = ""; + material->ambient_texname = ""; + material->diffuse_texname = ""; + material->specular_texname = ""; + material->specular_highlight_texname = ""; + material->bump_texname = ""; + material->displacement_texname = ""; + material->reflection_texname = ""; + material->alpha_texname = ""; + for (int i = 0; i < 3; i++) { + material->ambient[i] = static_cast(0.0); + material->diffuse[i] = static_cast(0.0); + material->specular[i] = static_cast(0.0); + material->transmittance[i] = static_cast(0.0); + material->emission[i] = static_cast(0.0); + } + material->illum = 0; + material->dissolve = static_cast(1.0); + material->shininess = static_cast(1.0); + material->ior = static_cast(1.0); + + material->roughness = static_cast(0.0); + material->metallic = static_cast(0.0); + material->sheen = static_cast(0.0); + material->clearcoat_thickness = static_cast(0.0); + material->clearcoat_roughness = static_cast(0.0); + material->anisotropy_rotation = static_cast(0.0); + material->anisotropy = static_cast(0.0); + material->roughness_texname = ""; + material->metallic_texname = ""; + material->sheen_texname = ""; + material->emissive_texname = ""; + material->normal_texname = ""; + + material->unknown_parameter.clear(); +} + +// code from https://wrf.ecse.rpi.edu//Research/Short_Notes/pnpoly.html +template +static int pnpoly(int nvert, T *vertx, T *verty, T testx, T testy) { + int i, j, c = 0; + for (i = 0, j = nvert - 1; i < nvert; j = i++) { + if (((verty[i] > testy) != (verty[j] > testy)) && + (testx < + (vertx[j] - vertx[i]) * (testy - verty[i]) / (verty[j] - verty[i]) + + vertx[i])) + c = !c; + } + return c; +} + +struct TinyObjPoint { + real_t x, y, z; + TinyObjPoint() : x(0), y(0), z(0) {} + TinyObjPoint(real_t x_, real_t y_, real_t z_) : x(x_), y(y_), z(z_) {} +}; + +inline TinyObjPoint cross(const TinyObjPoint &v1, const TinyObjPoint &v2) { + return TinyObjPoint(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, + v1.x * v2.y - v1.y * v2.x); +} + +inline real_t dot(const TinyObjPoint &v1, const TinyObjPoint &v2) { + return (v1.x * v2.x + v1.y * v2.y + v1.z * v2.z); +} + +inline real_t GetLength(TinyObjPoint &e) { + return std::sqrt(e.x * e.x + e.y * e.y + e.z * e.z); +} + +inline TinyObjPoint Normalize(TinyObjPoint e) { + real_t inv_length = real_t(1) / GetLength(e); + return TinyObjPoint(e.x * inv_length, e.y * inv_length, e.z * inv_length); +} + +inline TinyObjPoint WorldToLocal(const TinyObjPoint &a, const TinyObjPoint &u, + const TinyObjPoint &v, const TinyObjPoint &w) { + return TinyObjPoint(dot(a, u), dot(a, v), dot(a, w)); +} + +// TODO(syoyo): refactor function. +static bool exportGroupsToShape(shape_t *shape, const PrimGroup &prim_group, + const std::vector &tags, + const int material_id, const std::string &name, + bool triangulate, const std::vector &v, + std::string *warn) { + if (prim_group.IsEmpty()) { + return false; + } + + shape->name = name; + + // polygon + if (!prim_group.faceGroup.empty()) { + // Flatten vertices and indices + for (size_t i = 0; i < prim_group.faceGroup.size(); i++) { + const face_t &face = prim_group.faceGroup[i]; + + size_t npolys = face.vertex_indices.size(); + + if (npolys < 3) { + // Face must have 3+ vertices. + if (warn) { + (*warn) += "Degenerated face found\n."; + } + continue; + } + + if (triangulate && npolys != 3) { + if (npolys == 4) { + vertex_index_t i0 = face.vertex_indices[0]; + vertex_index_t i1 = face.vertex_indices[1]; + vertex_index_t i2 = face.vertex_indices[2]; + vertex_index_t i3 = face.vertex_indices[3]; + + size_t vi0 = size_t(i0.v_idx); + size_t vi1 = size_t(i1.v_idx); + size_t vi2 = size_t(i2.v_idx); + size_t vi3 = size_t(i3.v_idx); + + if (((3 * vi0 + 2) >= v.size()) || ((3 * vi1 + 2) >= v.size()) || + ((3 * vi2 + 2) >= v.size()) || ((3 * vi3 + 2) >= v.size())) { + // Invalid triangle. + // FIXME(syoyo): Is it ok to simply skip this invalid triangle? + if (warn) { + (*warn) += "Face with invalid vertex index found.\n"; + } + continue; + } + + real_t v0x = v[vi0 * 3 + 0]; + real_t v0y = v[vi0 * 3 + 1]; + real_t v0z = v[vi0 * 3 + 2]; + real_t v1x = v[vi1 * 3 + 0]; + real_t v1y = v[vi1 * 3 + 1]; + real_t v1z = v[vi1 * 3 + 2]; + real_t v2x = v[vi2 * 3 + 0]; + real_t v2y = v[vi2 * 3 + 1]; + real_t v2z = v[vi2 * 3 + 2]; + real_t v3x = v[vi3 * 3 + 0]; + real_t v3y = v[vi3 * 3 + 1]; + real_t v3z = v[vi3 * 3 + 2]; + + // There are two candidates to split the quad into two triangles. + // + // Choose the shortest edge. + // TODO: Is it better to determine the edge to split by calculating + // the area of each triangle? + // + // +---+ + // |\ | + // | \ | + // | \| + // +---+ + // + // +---+ + // | /| + // | / | + // |/ | + // +---+ + + real_t e02x = v2x - v0x; + real_t e02y = v2y - v0y; + real_t e02z = v2z - v0z; + real_t e13x = v3x - v1x; + real_t e13y = v3y - v1y; + real_t e13z = v3z - v1z; + + real_t sqr02 = e02x * e02x + e02y * e02y + e02z * e02z; + real_t sqr13 = e13x * e13x + e13y * e13y + e13z * e13z; + + index_t idx0, idx1, idx2, idx3; + + idx0.vertex_index = i0.v_idx; + idx0.normal_index = i0.vn_idx; + idx0.texcoord_index = i0.vt_idx; + idx1.vertex_index = i1.v_idx; + idx1.normal_index = i1.vn_idx; + idx1.texcoord_index = i1.vt_idx; + idx2.vertex_index = i2.v_idx; + idx2.normal_index = i2.vn_idx; + idx2.texcoord_index = i2.vt_idx; + idx3.vertex_index = i3.v_idx; + idx3.normal_index = i3.vn_idx; + idx3.texcoord_index = i3.vt_idx; + + if (sqr02 < sqr13) { + // [0, 1, 2], [0, 2, 3] + shape->mesh.indices.push_back(idx0); + shape->mesh.indices.push_back(idx1); + shape->mesh.indices.push_back(idx2); + + shape->mesh.indices.push_back(idx0); + shape->mesh.indices.push_back(idx2); + shape->mesh.indices.push_back(idx3); + } else { + // [0, 1, 3], [1, 2, 3] + shape->mesh.indices.push_back(idx0); + shape->mesh.indices.push_back(idx1); + shape->mesh.indices.push_back(idx3); + + shape->mesh.indices.push_back(idx1); + shape->mesh.indices.push_back(idx2); + shape->mesh.indices.push_back(idx3); + } + + // Two triangle faces + shape->mesh.num_face_vertices.push_back(3); + shape->mesh.num_face_vertices.push_back(3); + + shape->mesh.material_ids.push_back(material_id); + shape->mesh.material_ids.push_back(material_id); + + shape->mesh.smoothing_group_ids.push_back(face.smoothing_group_id); + shape->mesh.smoothing_group_ids.push_back(face.smoothing_group_id); + + } else { +#ifdef TINYOBJLOADER_USE_MAPBOX_EARCUT + vertex_index_t i0 = face.vertex_indices[0]; + vertex_index_t i0_2 = i0; + + // TMW change: Find the normal axis of the polygon using Newell's + // method + TinyObjPoint n; + for (size_t k = 0; k < npolys; ++k) { + i0 = face.vertex_indices[k % npolys]; + size_t vi0 = size_t(i0.v_idx); + + size_t j = (k + 1) % npolys; + i0_2 = face.vertex_indices[j]; + size_t vi0_2 = size_t(i0_2.v_idx); + + real_t v0x = v[vi0 * 3 + 0]; + real_t v0y = v[vi0 * 3 + 1]; + real_t v0z = v[vi0 * 3 + 2]; + + real_t v0x_2 = v[vi0_2 * 3 + 0]; + real_t v0y_2 = v[vi0_2 * 3 + 1]; + real_t v0z_2 = v[vi0_2 * 3 + 2]; + + const TinyObjPoint point1(v0x, v0y, v0z); + const TinyObjPoint point2(v0x_2, v0y_2, v0z_2); + + TinyObjPoint a(point1.x - point2.x, point1.y - point2.y, + point1.z - point2.z); + TinyObjPoint b(point1.x + point2.x, point1.y + point2.y, + point1.z + point2.z); + + n.x += (a.y * b.z); + n.y += (a.z * b.x); + n.z += (a.x * b.y); + } + real_t length_n = GetLength(n); + // Check if zero length normal + if (length_n <= 0) { + continue; + } + // Negative is to flip the normal to the correct direction + real_t inv_length = -real_t(1.0) / length_n; + n.x *= inv_length; + n.y *= inv_length; + n.z *= inv_length; + + TinyObjPoint axis_w, axis_v, axis_u; + axis_w = n; + TinyObjPoint a; + if (std::fabs(axis_w.x) > real_t(0.9999999)) { + a = TinyObjPoint(0, 1, 0); + } else { + a = TinyObjPoint(1, 0, 0); + } + axis_v = Normalize(cross(axis_w, a)); + axis_u = cross(axis_w, axis_v); + using Point = std::array; + + // first polyline define the main polygon. + // following polylines define holes(not used in tinyobj). + std::vector > polygon; + + std::vector polyline; + + // TMW change: Find best normal and project v0x and v0y to those + // coordinates, instead of picking a plane aligned with an axis (which + // can flip polygons). + + // Fill polygon data(facevarying vertices). + for (size_t k = 0; k < npolys; k++) { + i0 = face.vertex_indices[k]; + size_t vi0 = size_t(i0.v_idx); + + assert(((3 * vi0 + 2) < v.size())); + + real_t v0x = v[vi0 * 3 + 0]; + real_t v0y = v[vi0 * 3 + 1]; + real_t v0z = v[vi0 * 3 + 2]; + + TinyObjPoint polypoint(v0x, v0y, v0z); + TinyObjPoint loc = WorldToLocal(polypoint, axis_u, axis_v, axis_w); + + polyline.push_back({loc.x, loc.y}); + } + + polygon.push_back(polyline); + std::vector indices = mapbox::earcut(polygon); + // => result = 3 * faces, clockwise + + assert(indices.size() % 3 == 0); + + // Reconstruct vertex_index_t + for (size_t k = 0; k < indices.size() / 3; k++) { + { + index_t idx0, idx1, idx2; + idx0.vertex_index = face.vertex_indices[indices[3 * k + 0]].v_idx; + idx0.normal_index = + face.vertex_indices[indices[3 * k + 0]].vn_idx; + idx0.texcoord_index = + face.vertex_indices[indices[3 * k + 0]].vt_idx; + idx1.vertex_index = face.vertex_indices[indices[3 * k + 1]].v_idx; + idx1.normal_index = + face.vertex_indices[indices[3 * k + 1]].vn_idx; + idx1.texcoord_index = + face.vertex_indices[indices[3 * k + 1]].vt_idx; + idx2.vertex_index = face.vertex_indices[indices[3 * k + 2]].v_idx; + idx2.normal_index = + face.vertex_indices[indices[3 * k + 2]].vn_idx; + idx2.texcoord_index = + face.vertex_indices[indices[3 * k + 2]].vt_idx; + + shape->mesh.indices.push_back(idx0); + shape->mesh.indices.push_back(idx1); + shape->mesh.indices.push_back(idx2); + + shape->mesh.num_face_vertices.push_back(3); + shape->mesh.material_ids.push_back(material_id); + shape->mesh.smoothing_group_ids.push_back( + face.smoothing_group_id); + } + } + +#else // Built-in ear clipping triangulation + vertex_index_t i0 = face.vertex_indices[0]; + vertex_index_t i1(-1); + vertex_index_t i2 = face.vertex_indices[1]; + + // find the two axes to work in + size_t axes[2] = {1, 2}; + for (size_t k = 0; k < npolys; ++k) { + i0 = face.vertex_indices[(k + 0) % npolys]; + i1 = face.vertex_indices[(k + 1) % npolys]; + i2 = face.vertex_indices[(k + 2) % npolys]; + size_t vi0 = size_t(i0.v_idx); + size_t vi1 = size_t(i1.v_idx); + size_t vi2 = size_t(i2.v_idx); + + if (((3 * vi0 + 2) >= v.size()) || ((3 * vi1 + 2) >= v.size()) || + ((3 * vi2 + 2) >= v.size())) { + // Invalid triangle. + // FIXME(syoyo): Is it ok to simply skip this invalid triangle? + continue; + } + real_t v0x = v[vi0 * 3 + 0]; + real_t v0y = v[vi0 * 3 + 1]; + real_t v0z = v[vi0 * 3 + 2]; + real_t v1x = v[vi1 * 3 + 0]; + real_t v1y = v[vi1 * 3 + 1]; + real_t v1z = v[vi1 * 3 + 2]; + real_t v2x = v[vi2 * 3 + 0]; + real_t v2y = v[vi2 * 3 + 1]; + real_t v2z = v[vi2 * 3 + 2]; + real_t e0x = v1x - v0x; + real_t e0y = v1y - v0y; + real_t e0z = v1z - v0z; + real_t e1x = v2x - v1x; + real_t e1y = v2y - v1y; + real_t e1z = v2z - v1z; + real_t cx = std::fabs(e0y * e1z - e0z * e1y); + real_t cy = std::fabs(e0z * e1x - e0x * e1z); + real_t cz = std::fabs(e0x * e1y - e0y * e1x); + const real_t epsilon = std::numeric_limits::epsilon(); + // std::cout << "cx " << cx << ", cy " << cy << ", cz " << cz << + // "\n"; + if (cx > epsilon || cy > epsilon || cz > epsilon) { + // std::cout << "corner\n"; + // found a corner + if (cx > cy && cx > cz) { + // std::cout << "pattern0\n"; + } else { + // std::cout << "axes[0] = 0\n"; + axes[0] = 0; + if (cz > cx && cz > cy) { + // std::cout << "axes[1] = 1\n"; + axes[1] = 1; + } + } + break; + } + } + + face_t remainingFace = face; // copy + size_t guess_vert = 0; + vertex_index_t ind[3]; + real_t vx[3]; + real_t vy[3]; + + // How many iterations can we do without decreasing the remaining + // vertices. + size_t remainingIterations = face.vertex_indices.size(); + size_t previousRemainingVertices = + remainingFace.vertex_indices.size(); + + while (remainingFace.vertex_indices.size() > 3 && + remainingIterations > 0) { + // std::cout << "remainingIterations " << remainingIterations << + // "\n"; + + npolys = remainingFace.vertex_indices.size(); + if (guess_vert >= npolys) { + guess_vert -= npolys; + } + + if (previousRemainingVertices != npolys) { + // The number of remaining vertices decreased. Reset counters. + previousRemainingVertices = npolys; + remainingIterations = npolys; + } else { + // We didn't consume a vertex on previous iteration, reduce the + // available iterations. + remainingIterations--; + } + + for (size_t k = 0; k < 3; k++) { + ind[k] = remainingFace.vertex_indices[(guess_vert + k) % npolys]; + size_t vi = size_t(ind[k].v_idx); + if (((vi * 3 + axes[0]) >= v.size()) || + ((vi * 3 + axes[1]) >= v.size())) { + // ??? + vx[k] = static_cast(0.0); + vy[k] = static_cast(0.0); + } else { + vx[k] = v[vi * 3 + axes[0]]; + vy[k] = v[vi * 3 + axes[1]]; + } + } + + // + // area is calculated per face + // + real_t e0x = vx[1] - vx[0]; + real_t e0y = vy[1] - vy[0]; + real_t e1x = vx[2] - vx[1]; + real_t e1y = vy[2] - vy[1]; + real_t cross = e0x * e1y - e0y * e1x; + // std::cout << "axes = " << axes[0] << ", " << axes[1] << "\n"; + // std::cout << "e0x, e0y, e1x, e1y " << e0x << ", " << e0y << ", " + // << e1x << ", " << e1y << "\n"; + + real_t area = + (vx[0] * vy[1] - vy[0] * vx[1]) * static_cast(0.5); + // std::cout << "cross " << cross << ", area " << area << "\n"; + // if an internal angle + if (cross * area < static_cast(0.0)) { + // std::cout << "internal \n"; + guess_vert += 1; + // std::cout << "guess vert : " << guess_vert << "\n"; + continue; + } + + // check all other verts in case they are inside this triangle + bool overlap = false; + for (size_t otherVert = 3; otherVert < npolys; ++otherVert) { + size_t idx = (guess_vert + otherVert) % npolys; + + if (idx >= remainingFace.vertex_indices.size()) { + // std::cout << "???0\n"; + // ??? + continue; + } + + size_t ovi = size_t(remainingFace.vertex_indices[idx].v_idx); + + if (((ovi * 3 + axes[0]) >= v.size()) || + ((ovi * 3 + axes[1]) >= v.size())) { + // std::cout << "???1\n"; + // ??? + continue; + } + real_t tx = v[ovi * 3 + axes[0]]; + real_t ty = v[ovi * 3 + axes[1]]; + if (pnpoly(3, vx, vy, tx, ty)) { + // std::cout << "overlap\n"; + overlap = true; + break; + } + } + + if (overlap) { + // std::cout << "overlap2\n"; + guess_vert += 1; + continue; + } + + // this triangle is an ear + { + index_t idx0, idx1, idx2; + idx0.vertex_index = ind[0].v_idx; + idx0.normal_index = ind[0].vn_idx; + idx0.texcoord_index = ind[0].vt_idx; + idx1.vertex_index = ind[1].v_idx; + idx1.normal_index = ind[1].vn_idx; + idx1.texcoord_index = ind[1].vt_idx; + idx2.vertex_index = ind[2].v_idx; + idx2.normal_index = ind[2].vn_idx; + idx2.texcoord_index = ind[2].vt_idx; + + shape->mesh.indices.push_back(idx0); + shape->mesh.indices.push_back(idx1); + shape->mesh.indices.push_back(idx2); + + shape->mesh.num_face_vertices.push_back(3); + shape->mesh.material_ids.push_back(material_id); + shape->mesh.smoothing_group_ids.push_back( + face.smoothing_group_id); + } + + // remove v1 from the list + size_t removed_vert_index = (guess_vert + 1) % npolys; + while (removed_vert_index + 1 < npolys) { + remainingFace.vertex_indices[removed_vert_index] = + remainingFace.vertex_indices[removed_vert_index + 1]; + removed_vert_index += 1; + } + remainingFace.vertex_indices.pop_back(); + } + + // std::cout << "remainingFace.vi.size = " << + // remainingFace.vertex_indices.size() << "\n"; + if (remainingFace.vertex_indices.size() == 3) { + i0 = remainingFace.vertex_indices[0]; + i1 = remainingFace.vertex_indices[1]; + i2 = remainingFace.vertex_indices[2]; + { + index_t idx0, idx1, idx2; + idx0.vertex_index = i0.v_idx; + idx0.normal_index = i0.vn_idx; + idx0.texcoord_index = i0.vt_idx; + idx1.vertex_index = i1.v_idx; + idx1.normal_index = i1.vn_idx; + idx1.texcoord_index = i1.vt_idx; + idx2.vertex_index = i2.v_idx; + idx2.normal_index = i2.vn_idx; + idx2.texcoord_index = i2.vt_idx; + + shape->mesh.indices.push_back(idx0); + shape->mesh.indices.push_back(idx1); + shape->mesh.indices.push_back(idx2); + + shape->mesh.num_face_vertices.push_back(3); + shape->mesh.material_ids.push_back(material_id); + shape->mesh.smoothing_group_ids.push_back( + face.smoothing_group_id); + } + } +#endif + } // npolys + } else { + for (size_t k = 0; k < npolys; k++) { + index_t idx; + idx.vertex_index = face.vertex_indices[k].v_idx; + idx.normal_index = face.vertex_indices[k].vn_idx; + idx.texcoord_index = face.vertex_indices[k].vt_idx; + shape->mesh.indices.push_back(idx); + } + + shape->mesh.num_face_vertices.push_back( + static_cast(npolys)); + shape->mesh.material_ids.push_back(material_id); // per face + shape->mesh.smoothing_group_ids.push_back( + face.smoothing_group_id); // per face + } + } + + shape->mesh.tags = tags; + } + + // line + if (!prim_group.lineGroup.empty()) { + // Flatten indices + for (size_t i = 0; i < prim_group.lineGroup.size(); i++) { + for (size_t j = 0; j < prim_group.lineGroup[i].vertex_indices.size(); + j++) { + const vertex_index_t &vi = prim_group.lineGroup[i].vertex_indices[j]; + + index_t idx; + idx.vertex_index = vi.v_idx; + idx.normal_index = vi.vn_idx; + idx.texcoord_index = vi.vt_idx; + + shape->lines.indices.push_back(idx); + } + + shape->lines.num_line_vertices.push_back( + int(prim_group.lineGroup[i].vertex_indices.size())); + } + } + + // points + if (!prim_group.pointsGroup.empty()) { + // Flatten & convert indices + for (size_t i = 0; i < prim_group.pointsGroup.size(); i++) { + for (size_t j = 0; j < prim_group.pointsGroup[i].vertex_indices.size(); + j++) { + const vertex_index_t &vi = prim_group.pointsGroup[i].vertex_indices[j]; + + index_t idx; + idx.vertex_index = vi.v_idx; + idx.normal_index = vi.vn_idx; + idx.texcoord_index = vi.vt_idx; + + shape->points.indices.push_back(idx); + } + } + } + + return true; +} + +// Split a string with specified delimiter character and escape character. +// https://rosettacode.org/wiki/Tokenize_a_string_with_escaping#C.2B.2B +static void SplitString(const std::string &s, char delim, char escape, + std::vector &elems) { + std::string token; + + bool escaping = false; + for (size_t i = 0; i < s.size(); ++i) { + char ch = s[i]; + if (escaping) { + escaping = false; + } else if (ch == escape) { + escaping = true; + continue; + } else if (ch == delim) { + if (!token.empty()) { + elems.push_back(token); + } + token.clear(); + continue; + } + token += ch; + } + + elems.push_back(token); +} + +static std::string JoinPath(const std::string &dir, + const std::string &filename) { + if (dir.empty()) { + return filename; + } else { + // check '/' + char lastChar = *dir.rbegin(); + if (lastChar != '/') { + return dir + std::string("/") + filename; + } else { + return dir + filename; + } + } +} + +void LoadMtl(std::map *material_map, + std::vector *materials, std::istream *inStream, + std::string *warning, std::string *err) { + (void)err; + + // Create a default material anyway. + material_t material; + InitMaterial(&material); + + // Issue 43. `d` wins against `Tr` since `Tr` is not in the MTL specification. + bool has_d = false; + bool has_tr = false; + + // has_kd is used to set a default diffuse value when map_Kd is present + // and Kd is not. + bool has_kd = false; + + std::stringstream warn_ss; + + size_t line_no = 0; + std::string linebuf; + while (inStream->peek() != -1) { + safeGetline(*inStream, linebuf); + line_no++; + + // Trim trailing whitespace. + if (linebuf.size() > 0) { + linebuf = linebuf.substr(0, linebuf.find_last_not_of(" \t") + 1); + } + + // Trim newline '\r\n' or '\n' + if (linebuf.size() > 0) { + if (linebuf[linebuf.size() - 1] == '\n') + linebuf.erase(linebuf.size() - 1); + } + if (linebuf.size() > 0) { + if (linebuf[linebuf.size() - 1] == '\r') + linebuf.erase(linebuf.size() - 1); + } + + // Skip if empty line. + if (linebuf.empty()) { + continue; + } + + // Skip leading space. + const char *token = linebuf.c_str(); + token += strspn(token, " \t"); + + assert(token); + if (token[0] == '\0') continue; // empty line + + if (token[0] == '#') continue; // comment line + + // new mtl + if ((0 == strncmp(token, "newmtl", 6)) && IS_SPACE((token[6]))) { + // flush previous material. + if (!material.name.empty()) { + material_map->insert(std::pair( + material.name, static_cast(materials->size()))); + materials->push_back(material); + } + + // initial temporary material + InitMaterial(&material); + + has_d = false; + has_tr = false; + + // set new mtl name + token += 7; + { + std::string namebuf = parseString(&token); + // TODO: empty name check? + if (namebuf.empty()) { + if (warning) { + (*warning) += "empty material name in `newmtl`\n"; + } + } + material.name = namebuf; + } + continue; + } + + // ambient + if (token[0] == 'K' && token[1] == 'a' && IS_SPACE((token[2]))) { + token += 2; + real_t r, g, b; + parseReal3(&r, &g, &b, &token); + material.ambient[0] = r; + material.ambient[1] = g; + material.ambient[2] = b; + continue; + } + + // diffuse + if (token[0] == 'K' && token[1] == 'd' && IS_SPACE((token[2]))) { + token += 2; + real_t r, g, b; + parseReal3(&r, &g, &b, &token); + material.diffuse[0] = r; + material.diffuse[1] = g; + material.diffuse[2] = b; + has_kd = true; + continue; + } + + // specular + if (token[0] == 'K' && token[1] == 's' && IS_SPACE((token[2]))) { + token += 2; + real_t r, g, b; + parseReal3(&r, &g, &b, &token); + material.specular[0] = r; + material.specular[1] = g; + material.specular[2] = b; + continue; + } + + // transmittance + if ((token[0] == 'K' && token[1] == 't' && IS_SPACE((token[2]))) || + (token[0] == 'T' && token[1] == 'f' && IS_SPACE((token[2])))) { + token += 2; + real_t r, g, b; + parseReal3(&r, &g, &b, &token); + material.transmittance[0] = r; + material.transmittance[1] = g; + material.transmittance[2] = b; + continue; + } + + // ior(index of refraction) + if (token[0] == 'N' && token[1] == 'i' && IS_SPACE((token[2]))) { + token += 2; + material.ior = parseReal(&token); + continue; + } + + // emission + if (token[0] == 'K' && token[1] == 'e' && IS_SPACE(token[2])) { + token += 2; + real_t r, g, b; + parseReal3(&r, &g, &b, &token); + material.emission[0] = r; + material.emission[1] = g; + material.emission[2] = b; + continue; + } + + // shininess + if (token[0] == 'N' && token[1] == 's' && IS_SPACE(token[2])) { + token += 2; + material.shininess = parseReal(&token); + continue; + } + + // illum model + if (0 == strncmp(token, "illum", 5) && IS_SPACE(token[5])) { + token += 6; + material.illum = parseInt(&token); + continue; + } + + // dissolve + if ((token[0] == 'd' && IS_SPACE(token[1]))) { + token += 1; + material.dissolve = parseReal(&token); + + if (has_tr) { + warn_ss << "Both `d` and `Tr` parameters defined for \"" + << material.name + << "\". Use the value of `d` for dissolve (line " << line_no + << " in .mtl.)\n"; + } + has_d = true; + continue; + } + if (token[0] == 'T' && token[1] == 'r' && IS_SPACE(token[2])) { + token += 2; + if (has_d) { + // `d` wins. Ignore `Tr` value. + warn_ss << "Both `d` and `Tr` parameters defined for \"" + << material.name + << "\". Use the value of `d` for dissolve (line " << line_no + << " in .mtl.)\n"; + } else { + // We invert value of Tr(assume Tr is in range [0, 1]) + // NOTE: Interpretation of Tr is application(exporter) dependent. For + // some application(e.g. 3ds max obj exporter), Tr = d(Issue 43) + material.dissolve = static_cast(1.0) - parseReal(&token); + } + has_tr = true; + continue; + } + + // PBR: roughness + if (token[0] == 'P' && token[1] == 'r' && IS_SPACE(token[2])) { + token += 2; + material.roughness = parseReal(&token); + continue; + } + + // PBR: metallic + if (token[0] == 'P' && token[1] == 'm' && IS_SPACE(token[2])) { + token += 2; + material.metallic = parseReal(&token); + continue; + } + + // PBR: sheen + if (token[0] == 'P' && token[1] == 's' && IS_SPACE(token[2])) { + token += 2; + material.sheen = parseReal(&token); + continue; + } + + // PBR: clearcoat thickness + if (token[0] == 'P' && token[1] == 'c' && IS_SPACE(token[2])) { + token += 2; + material.clearcoat_thickness = parseReal(&token); + continue; + } + + // PBR: clearcoat roughness + if ((0 == strncmp(token, "Pcr", 3)) && IS_SPACE(token[3])) { + token += 4; + material.clearcoat_roughness = parseReal(&token); + continue; + } + + // PBR: anisotropy + if ((0 == strncmp(token, "aniso", 5)) && IS_SPACE(token[5])) { + token += 6; + material.anisotropy = parseReal(&token); + continue; + } + + // PBR: anisotropy rotation + if ((0 == strncmp(token, "anisor", 6)) && IS_SPACE(token[6])) { + token += 7; + material.anisotropy_rotation = parseReal(&token); + continue; + } + + // ambient or ambient occlusion texture + if ((0 == strncmp(token, "map_Ka", 6)) && IS_SPACE(token[6])) { + token += 7; + ParseTextureNameAndOption(&(material.ambient_texname), + &(material.ambient_texopt), token); + continue; + } + + // diffuse texture + if ((0 == strncmp(token, "map_Kd", 6)) && IS_SPACE(token[6])) { + token += 7; + ParseTextureNameAndOption(&(material.diffuse_texname), + &(material.diffuse_texopt), token); + + // Set a decent diffuse default value if a diffuse texture is specified + // without a matching Kd value. + if (!has_kd) { + material.diffuse[0] = static_cast(0.6); + material.diffuse[1] = static_cast(0.6); + material.diffuse[2] = static_cast(0.6); + } + + continue; + } + + // specular texture + if ((0 == strncmp(token, "map_Ks", 6)) && IS_SPACE(token[6])) { + token += 7; + ParseTextureNameAndOption(&(material.specular_texname), + &(material.specular_texopt), token); + continue; + } + + // specular highlight texture + if ((0 == strncmp(token, "map_Ns", 6)) && IS_SPACE(token[6])) { + token += 7; + ParseTextureNameAndOption(&(material.specular_highlight_texname), + &(material.specular_highlight_texopt), token); + continue; + } + + // bump texture + if (((0 == strncmp(token, "map_bump", 8)) || + (0 == strncmp(token, "map_Bump", 8))) && + IS_SPACE(token[8])) { + token += 9; + ParseTextureNameAndOption(&(material.bump_texname), + &(material.bump_texopt), token); + continue; + } + + // bump texture + if ((0 == strncmp(token, "bump", 4)) && IS_SPACE(token[4])) { + token += 5; + ParseTextureNameAndOption(&(material.bump_texname), + &(material.bump_texopt), token); + continue; + } + + // alpha texture + if ((0 == strncmp(token, "map_d", 5)) && IS_SPACE(token[5])) { + token += 6; + material.alpha_texname = token; + ParseTextureNameAndOption(&(material.alpha_texname), + &(material.alpha_texopt), token); + continue; + } + + // displacement texture + if (((0 == strncmp(token, "map_disp", 8)) || + (0 == strncmp(token, "map_Disp", 8))) && + IS_SPACE(token[8])) { + token += 9; + ParseTextureNameAndOption(&(material.displacement_texname), + &(material.displacement_texopt), token); + continue; + } + + // displacement texture + if ((0 == strncmp(token, "disp", 4)) && IS_SPACE(token[4])) { + token += 5; + ParseTextureNameAndOption(&(material.displacement_texname), + &(material.displacement_texopt), token); + continue; + } + + // reflection map + if ((0 == strncmp(token, "refl", 4)) && IS_SPACE(token[4])) { + token += 5; + ParseTextureNameAndOption(&(material.reflection_texname), + &(material.reflection_texopt), token); + continue; + } + + // PBR: roughness texture + if ((0 == strncmp(token, "map_Pr", 6)) && IS_SPACE(token[6])) { + token += 7; + ParseTextureNameAndOption(&(material.roughness_texname), + &(material.roughness_texopt), token); + continue; + } + + // PBR: metallic texture + if ((0 == strncmp(token, "map_Pm", 6)) && IS_SPACE(token[6])) { + token += 7; + ParseTextureNameAndOption(&(material.metallic_texname), + &(material.metallic_texopt), token); + continue; + } + + // PBR: sheen texture + if ((0 == strncmp(token, "map_Ps", 6)) && IS_SPACE(token[6])) { + token += 7; + ParseTextureNameAndOption(&(material.sheen_texname), + &(material.sheen_texopt), token); + continue; + } + + // PBR: emissive texture + if ((0 == strncmp(token, "map_Ke", 6)) && IS_SPACE(token[6])) { + token += 7; + ParseTextureNameAndOption(&(material.emissive_texname), + &(material.emissive_texopt), token); + continue; + } + + // PBR: normal map texture + if ((0 == strncmp(token, "norm", 4)) && IS_SPACE(token[4])) { + token += 5; + ParseTextureNameAndOption(&(material.normal_texname), + &(material.normal_texopt), token); + continue; + } + + // unknown parameter + const char *_space = strchr(token, ' '); + if (!_space) { + _space = strchr(token, '\t'); + } + if (_space) { + std::ptrdiff_t len = _space - token; + std::string key(token, static_cast(len)); + std::string value = _space + 1; + material.unknown_parameter.insert( + std::pair(key, value)); + } + } + // flush last material. + material_map->insert(std::pair( + material.name, static_cast(materials->size()))); + materials->push_back(material); + + if (warning) { + (*warning) = warn_ss.str(); + } +} + +bool MaterialFileReader::operator()(const std::string &matId, + std::vector *materials, + std::map *matMap, + std::string *warn, std::string *err) { + if (!m_mtlBaseDir.empty()) { +#ifdef _WIN32 + char sep = ';'; +#else + char sep = ':'; +#endif + + // https://stackoverflow.com/questions/5167625/splitting-a-c-stdstring-using-tokens-e-g + std::vector paths; + std::istringstream f(m_mtlBaseDir); + + std::string s; + while (getline(f, s, sep)) { + paths.push_back(s); + } + + for (size_t i = 0; i < paths.size(); i++) { + std::string filepath = JoinPath(paths[i], matId); + + std::ifstream matIStream(filepath.c_str()); + if (matIStream) { + LoadMtl(matMap, materials, &matIStream, warn, err); + + return true; + } + } + + std::stringstream ss; + ss << "Material file [ " << matId + << " ] not found in a path : " << m_mtlBaseDir << "\n"; + if (warn) { + (*warn) += ss.str(); + } + return false; + + } else { + std::string filepath = matId; + std::ifstream matIStream(filepath.c_str()); + if (matIStream) { + LoadMtl(matMap, materials, &matIStream, warn, err); + + return true; + } + + std::stringstream ss; + ss << "Material file [ " << filepath + << " ] not found in a path : " << m_mtlBaseDir << "\n"; + if (warn) { + (*warn) += ss.str(); + } + + return false; + } +} + +bool MaterialStreamReader::operator()(const std::string &matId, + std::vector *materials, + std::map *matMap, + std::string *warn, std::string *err) { + (void)err; + (void)matId; + if (!m_inStream) { + std::stringstream ss; + ss << "Material stream in error state. \n"; + if (warn) { + (*warn) += ss.str(); + } + return false; + } + + LoadMtl(matMap, materials, &m_inStream, warn, err); + + return true; +} + +bool LoadObj(attrib_t *attrib, std::vector *shapes, + std::vector *materials, std::string *warn, + std::string *err, const char *filename, const char *mtl_basedir, + bool triangulate, bool default_vcols_fallback) { + attrib->vertices.clear(); + attrib->normals.clear(); + attrib->texcoords.clear(); + attrib->colors.clear(); + shapes->clear(); + + std::stringstream errss; + + std::ifstream ifs(filename); + if (!ifs) { + errss << "Cannot open file [" << filename << "]\n"; + if (err) { + (*err) = errss.str(); + } + return false; + } + + std::string baseDir = mtl_basedir ? mtl_basedir : ""; + if (!baseDir.empty()) { +#ifndef _WIN32 + const char dirsep = '/'; +#else + const char dirsep = '\\'; +#endif + if (baseDir[baseDir.length() - 1] != dirsep) baseDir += dirsep; + } + MaterialFileReader matFileReader(baseDir); + + return LoadObj(attrib, shapes, materials, warn, err, &ifs, &matFileReader, + triangulate, default_vcols_fallback); +} + +bool LoadObj(attrib_t *attrib, std::vector *shapes, + std::vector *materials, std::string *warn, + std::string *err, std::istream *inStream, + MaterialReader *readMatFn /*= NULL*/, bool triangulate, + bool default_vcols_fallback) { + std::stringstream errss; + + std::vector v; + std::vector vertex_weights; // optional [w] component in `v` + std::vector vn; + std::vector vt; + std::vector vc; + std::vector vw; // tinyobj extension: vertex skin weights + std::vector tags; + PrimGroup prim_group; + std::string name; + + // material + std::set material_filenames; + std::map material_map; + int material = -1; + + // smoothing group id + unsigned int current_smoothing_id = + 0; // Initial value. 0 means no smoothing. + + int greatest_v_idx = -1; + int greatest_vn_idx = -1; + int greatest_vt_idx = -1; + + shape_t shape; + + bool found_all_colors = true; // check if all 'v' line has color info + + size_t line_num = 0; + std::string linebuf; + while (inStream->peek() != -1) { + safeGetline(*inStream, linebuf); + + line_num++; + + // Trim newline '\r\n' or '\n' + if (linebuf.size() > 0) { + if (linebuf[linebuf.size() - 1] == '\n') + linebuf.erase(linebuf.size() - 1); + } + if (linebuf.size() > 0) { + if (linebuf[linebuf.size() - 1] == '\r') + linebuf.erase(linebuf.size() - 1); + } + + // Skip if empty line. + if (linebuf.empty()) { + continue; + } + + // Skip leading space. + const char *token = linebuf.c_str(); + token += strspn(token, " \t"); + + assert(token); + if (token[0] == '\0') continue; // empty line + + if (token[0] == '#') continue; // comment line + + // vertex + if (token[0] == 'v' && IS_SPACE((token[1]))) { + token += 2; + real_t x, y, z; + real_t r, g, b; + + int num_components = parseVertexWithColor(&x, &y, &z, &r, &g, &b, &token); + found_all_colors &= (num_components == 6); + + v.push_back(x); + v.push_back(y); + v.push_back(z); + + vertex_weights.push_back( + r); // r = w, and initialized to 1.0 when `w` component is not found. + + if ((num_components == 6) || default_vcols_fallback) { + vc.push_back(r); + vc.push_back(g); + vc.push_back(b); + } + + continue; + } + + // normal + if (token[0] == 'v' && token[1] == 'n' && IS_SPACE((token[2]))) { + token += 3; + real_t x, y, z; + parseReal3(&x, &y, &z, &token); + vn.push_back(x); + vn.push_back(y); + vn.push_back(z); + continue; + } + + // texcoord + if (token[0] == 'v' && token[1] == 't' && IS_SPACE((token[2]))) { + token += 3; + real_t x, y; + parseReal2(&x, &y, &token); + vt.push_back(x); + vt.push_back(y); + continue; + } + + // skin weight. tinyobj extension + if (token[0] == 'v' && token[1] == 'w' && IS_SPACE((token[2]))) { + token += 3; + + // vw ... + // example: + // vw 0 0 0.25 1 0.25 2 0.5 + + // TODO(syoyo): Add syntax check + int vid = 0; + vid = parseInt(&token); + + skin_weight_t sw; + + sw.vertex_id = vid; + + while (!IS_NEW_LINE(token[0])) { + real_t j, w; + // joint_id should not be negative, weight may be negative + // TODO(syoyo): # of elements check + parseReal2(&j, &w, &token, -1.0); + + if (j < static_cast(0)) { + if (err) { + std::stringstream ss; + ss << "Failed parse `vw' line. joint_id is negative. " + "line " + << line_num << ".)\n"; + (*err) += ss.str(); + } + return false; + } + + joint_and_weight_t jw; + + jw.joint_id = int(j); + jw.weight = w; + + sw.weightValues.push_back(jw); + + size_t n = strspn(token, " \t\r"); + token += n; + } + + vw.push_back(sw); + } + + warning_context context; + context.warn = warn; + context.line_number = line_num; + + // line + if (token[0] == 'l' && IS_SPACE((token[1]))) { + token += 2; + + __line_t line; + + while (!IS_NEW_LINE(token[0])) { + vertex_index_t vi; + if (!parseTriple(&token, static_cast(v.size() / 3), + static_cast(vn.size() / 3), + static_cast(vt.size() / 2), &vi, context)) { + if (err) { + (*err) += + "Failed to parse `l' line (e.g. a zero value for vertex index. " + "Line " + + toString(line_num) + ").\n"; + } + return false; + } + + line.vertex_indices.push_back(vi); + + size_t n = strspn(token, " \t\r"); + token += n; + } + + prim_group.lineGroup.push_back(line); + + continue; + } + + // points + if (token[0] == 'p' && IS_SPACE((token[1]))) { + token += 2; + + __points_t pts; + + while (!IS_NEW_LINE(token[0])) { + vertex_index_t vi; + if (!parseTriple(&token, static_cast(v.size() / 3), + static_cast(vn.size() / 3), + static_cast(vt.size() / 2), &vi, context)) { + if (err) { + (*err) += + "Failed to parse `p' line (e.g. a zero value for vertex index. " + "Line " + + toString(line_num) + ").\n"; + } + return false; + } + + pts.vertex_indices.push_back(vi); + + size_t n = strspn(token, " \t\r"); + token += n; + } + + prim_group.pointsGroup.push_back(pts); + + continue; + } + + // face + if (token[0] == 'f' && IS_SPACE((token[1]))) { + token += 2; + token += strspn(token, " \t"); + + face_t face; + + face.smoothing_group_id = current_smoothing_id; + face.vertex_indices.reserve(3); + + while (!IS_NEW_LINE(token[0])) { + vertex_index_t vi; + if (!parseTriple(&token, static_cast(v.size() / 3), + static_cast(vn.size() / 3), + static_cast(vt.size() / 2), &vi, context)) { + if (err) { + (*err) += + "Failed to parse `f' line (e.g. a zero value for vertex index " + "or invalid relative vertex index). Line " + + toString(line_num) + ").\n"; + } + return false; + } + + greatest_v_idx = greatest_v_idx > vi.v_idx ? greatest_v_idx : vi.v_idx; + greatest_vn_idx = + greatest_vn_idx > vi.vn_idx ? greatest_vn_idx : vi.vn_idx; + greatest_vt_idx = + greatest_vt_idx > vi.vt_idx ? greatest_vt_idx : vi.vt_idx; + + face.vertex_indices.push_back(vi); + size_t n = strspn(token, " \t\r"); + token += n; + } + + // replace with emplace_back + std::move on C++11 + prim_group.faceGroup.push_back(face); + + continue; + } + + // use mtl + if ((0 == strncmp(token, "usemtl", 6))) { + token += 6; + std::string namebuf = parseString(&token); + + int newMaterialId = -1; + std::map::const_iterator it = + material_map.find(namebuf); + if (it != material_map.end()) { + newMaterialId = it->second; + } else { + // { error!! material not found } + if (warn) { + (*warn) += "material [ '" + namebuf + "' ] not found in .mtl\n"; + } + } + + if (newMaterialId != material) { + // Create per-face material. Thus we don't add `shape` to `shapes` at + // this time. + // just clear `faceGroup` after `exportGroupsToShape()` call. + exportGroupsToShape(&shape, prim_group, tags, material, name, + triangulate, v, warn); + prim_group.faceGroup.clear(); + material = newMaterialId; + } + + continue; + } + + // load mtl + if ((0 == strncmp(token, "mtllib", 6)) && IS_SPACE((token[6]))) { + if (readMatFn) { + token += 7; + + std::vector filenames; + SplitString(std::string(token), ' ', '\\', filenames); + + if (filenames.empty()) { + if (warn) { + std::stringstream ss; + ss << "Looks like empty filename for mtllib. Use default " + "material (line " + << line_num << ".)\n"; + + (*warn) += ss.str(); + } + } else { + bool found = false; + for (size_t s = 0; s < filenames.size(); s++) { + if (material_filenames.count(filenames[s]) > 0) { + found = true; + continue; + } + + std::string warn_mtl; + std::string err_mtl; + bool ok = (*readMatFn)(filenames[s].c_str(), materials, + &material_map, &warn_mtl, &err_mtl); + if (warn && (!warn_mtl.empty())) { + (*warn) += warn_mtl; + } + + if (err && (!err_mtl.empty())) { + (*err) += err_mtl; + } + + if (ok) { + found = true; + material_filenames.insert(filenames[s]); + break; + } + } + + if (!found) { + if (warn) { + (*warn) += + "Failed to load material file(s). Use default " + "material.\n"; + } + } + } + } + + continue; + } + + // group name + if (token[0] == 'g' && IS_SPACE((token[1]))) { + // flush previous face group. + bool ret = exportGroupsToShape(&shape, prim_group, tags, material, name, + triangulate, v, warn); + (void)ret; // return value not used. + + if (shape.mesh.indices.size() > 0) { + shapes->push_back(shape); + } + + shape = shape_t(); + + // material = -1; + prim_group.clear(); + + std::vector names; + + while (!IS_NEW_LINE(token[0])) { + std::string str = parseString(&token); + names.push_back(str); + token += strspn(token, " \t\r"); // skip tag + } + + // names[0] must be 'g' + + if (names.size() < 2) { + // 'g' with empty names + if (warn) { + std::stringstream ss; + ss << "Empty group name. line: " << line_num << "\n"; + (*warn) += ss.str(); + name = ""; + } + } else { + std::stringstream ss; + ss << names[1]; + + // tinyobjloader does not support multiple groups for a primitive. + // Currently we concatinate multiple group names with a space to get + // single group name. + + for (size_t i = 2; i < names.size(); i++) { + ss << " " << names[i]; + } + + name = ss.str(); + } + + continue; + } + + // object name + if (token[0] == 'o' && IS_SPACE((token[1]))) { + // flush previous face group. + bool ret = exportGroupsToShape(&shape, prim_group, tags, material, name, + triangulate, v, warn); + (void)ret; // return value not used. + + if (shape.mesh.indices.size() > 0 || shape.lines.indices.size() > 0 || + shape.points.indices.size() > 0) { + shapes->push_back(shape); + } + + // material = -1; + prim_group.clear(); + shape = shape_t(); + + // @todo { multiple object name? } + token += 2; + std::stringstream ss; + ss << token; + name = ss.str(); + + continue; + } + + if (token[0] == 't' && IS_SPACE(token[1])) { + const int max_tag_nums = 8192; // FIXME(syoyo): Parameterize. + tag_t tag; + + token += 2; + + tag.name = parseString(&token); + + tag_sizes ts = parseTagTriple(&token); + + if (ts.num_ints < 0) { + ts.num_ints = 0; + } + if (ts.num_ints > max_tag_nums) { + ts.num_ints = max_tag_nums; + } + + if (ts.num_reals < 0) { + ts.num_reals = 0; + } + if (ts.num_reals > max_tag_nums) { + ts.num_reals = max_tag_nums; + } + + if (ts.num_strings < 0) { + ts.num_strings = 0; + } + if (ts.num_strings > max_tag_nums) { + ts.num_strings = max_tag_nums; + } + + tag.intValues.resize(static_cast(ts.num_ints)); + + for (size_t i = 0; i < static_cast(ts.num_ints); ++i) { + tag.intValues[i] = parseInt(&token); + } + + tag.floatValues.resize(static_cast(ts.num_reals)); + for (size_t i = 0; i < static_cast(ts.num_reals); ++i) { + tag.floatValues[i] = parseReal(&token); + } + + tag.stringValues.resize(static_cast(ts.num_strings)); + for (size_t i = 0; i < static_cast(ts.num_strings); ++i) { + tag.stringValues[i] = parseString(&token); + } + + tags.push_back(tag); + + continue; + } + + if (token[0] == 's' && IS_SPACE(token[1])) { + // smoothing group id + token += 2; + + // skip space. + token += strspn(token, " \t"); // skip space + + if (token[0] == '\0') { + continue; + } + + if (token[0] == '\r' || token[1] == '\n') { + continue; + } + + if (strlen(token) >= 3 && token[0] == 'o' && token[1] == 'f' && + token[2] == 'f') { + current_smoothing_id = 0; + } else { + // assume number + int smGroupId = parseInt(&token); + if (smGroupId < 0) { + // parse error. force set to 0. + // FIXME(syoyo): Report warning. + current_smoothing_id = 0; + } else { + current_smoothing_id = static_cast(smGroupId); + } + } + + continue; + } // smoothing group id + + // Ignore unknown command. + } + + // not all vertices have colors, no default colors desired? -> clear colors + if (!found_all_colors && !default_vcols_fallback) { + vc.clear(); + } + + if (greatest_v_idx >= static_cast(v.size() / 3)) { + if (warn) { + std::stringstream ss; + ss << "Vertex indices out of bounds (line " << line_num << ".)\n\n"; + (*warn) += ss.str(); + } + } + if (greatest_vn_idx >= static_cast(vn.size() / 3)) { + if (warn) { + std::stringstream ss; + ss << "Vertex normal indices out of bounds (line " << line_num + << ".)\n\n"; + (*warn) += ss.str(); + } + } + if (greatest_vt_idx >= static_cast(vt.size() / 2)) { + if (warn) { + std::stringstream ss; + ss << "Vertex texcoord indices out of bounds (line " << line_num + << ".)\n\n"; + (*warn) += ss.str(); + } + } + + bool ret = exportGroupsToShape(&shape, prim_group, tags, material, name, + triangulate, v, warn); + // exportGroupsToShape return false when `usemtl` is called in the last + // line. + // we also add `shape` to `shapes` when `shape.mesh` has already some + // faces(indices) + if (ret || shape.mesh.indices + .size()) { // FIXME(syoyo): Support other prims(e.g. lines) + shapes->push_back(shape); + } + prim_group.clear(); // for safety + + if (err) { + (*err) += errss.str(); + } + + attrib->vertices.swap(v); + attrib->vertex_weights.swap(vertex_weights); + attrib->normals.swap(vn); + attrib->texcoords.swap(vt); + attrib->texcoord_ws.swap(vt); + attrib->colors.swap(vc); + attrib->skin_weights.swap(vw); + + return true; +} + +bool LoadObjWithCallback(std::istream &inStream, const callback_t &callback, + void *user_data /*= NULL*/, + MaterialReader *readMatFn /*= NULL*/, + std::string *warn, /* = NULL*/ + std::string *err /*= NULL*/) { + std::stringstream errss; + + // material + std::set material_filenames; + std::map material_map; + int material_id = -1; // -1 = invalid + + std::vector indices; + std::vector materials; + std::vector names; + names.reserve(2); + std::vector names_out; + + std::string linebuf; + while (inStream.peek() != -1) { + safeGetline(inStream, linebuf); + + // Trim newline '\r\n' or '\n' + if (linebuf.size() > 0) { + if (linebuf[linebuf.size() - 1] == '\n') + linebuf.erase(linebuf.size() - 1); + } + if (linebuf.size() > 0) { + if (linebuf[linebuf.size() - 1] == '\r') + linebuf.erase(linebuf.size() - 1); + } + + // Skip if empty line. + if (linebuf.empty()) { + continue; + } + + // Skip leading space. + const char *token = linebuf.c_str(); + token += strspn(token, " \t"); + + assert(token); + if (token[0] == '\0') continue; // empty line + + if (token[0] == '#') continue; // comment line + + // vertex + if (token[0] == 'v' && IS_SPACE((token[1]))) { + token += 2; + real_t x, y, z; + real_t r, g, b; + + int num_components = parseVertexWithColor(&x, &y, &z, &r, &g, &b, &token); + if (callback.vertex_cb) { + callback.vertex_cb(user_data, x, y, z, r); // r=w is optional + } + if (callback.vertex_color_cb) { + bool found_color = (num_components == 6); + callback.vertex_color_cb(user_data, x, y, z, r, g, b, found_color); + } + continue; + } + + // normal + if (token[0] == 'v' && token[1] == 'n' && IS_SPACE((token[2]))) { + token += 3; + real_t x, y, z; + parseReal3(&x, &y, &z, &token); + if (callback.normal_cb) { + callback.normal_cb(user_data, x, y, z); + } + continue; + } + + // texcoord + if (token[0] == 'v' && token[1] == 't' && IS_SPACE((token[2]))) { + token += 3; + real_t x, y, z; // y and z are optional. default = 0.0 + parseReal3(&x, &y, &z, &token); + if (callback.texcoord_cb) { + callback.texcoord_cb(user_data, x, y, z); + } + continue; + } + + // face + if (token[0] == 'f' && IS_SPACE((token[1]))) { + token += 2; + token += strspn(token, " \t"); + + indices.clear(); + while (!IS_NEW_LINE(token[0])) { + vertex_index_t vi = parseRawTriple(&token); + + index_t idx; + idx.vertex_index = vi.v_idx; + idx.normal_index = vi.vn_idx; + idx.texcoord_index = vi.vt_idx; + + indices.push_back(idx); + size_t n = strspn(token, " \t\r"); + token += n; + } + + if (callback.index_cb && indices.size() > 0) { + callback.index_cb(user_data, &indices.at(0), + static_cast(indices.size())); + } + + continue; + } + + // use mtl + if ((0 == strncmp(token, "usemtl", 6)) && IS_SPACE((token[6]))) { + token += 7; + std::stringstream ss; + ss << token; + std::string namebuf = ss.str(); + + int newMaterialId = -1; + std::map::const_iterator it = + material_map.find(namebuf); + if (it != material_map.end()) { + newMaterialId = it->second; + } else { + // { warn!! material not found } + if (warn && (!callback.usemtl_cb)) { + (*warn) += "material [ " + namebuf + " ] not found in .mtl\n"; + } + } + + if (newMaterialId != material_id) { + material_id = newMaterialId; + } + + if (callback.usemtl_cb) { + callback.usemtl_cb(user_data, namebuf.c_str(), material_id); + } + + continue; + } + + // load mtl + if ((0 == strncmp(token, "mtllib", 6)) && IS_SPACE((token[6]))) { + if (readMatFn) { + token += 7; + + std::vector filenames; + SplitString(std::string(token), ' ', '\\', filenames); + + if (filenames.empty()) { + if (warn) { + (*warn) += + "Looks like empty filename for mtllib. Use default " + "material. \n"; + } + } else { + bool found = false; + for (size_t s = 0; s < filenames.size(); s++) { + if (material_filenames.count(filenames[s]) > 0) { + found = true; + continue; + } + + std::string warn_mtl; + std::string err_mtl; + bool ok = (*readMatFn)(filenames[s].c_str(), &materials, + &material_map, &warn_mtl, &err_mtl); + + if (warn && (!warn_mtl.empty())) { + (*warn) += warn_mtl; // This should be warn message. + } + + if (err && (!err_mtl.empty())) { + (*err) += err_mtl; + } + + if (ok) { + found = true; + material_filenames.insert(filenames[s]); + break; + } + } + + if (!found) { + if (warn) { + (*warn) += + "Failed to load material file(s). Use default " + "material.\n"; + } + } else { + if (callback.mtllib_cb) { + callback.mtllib_cb(user_data, &materials.at(0), + static_cast(materials.size())); + } + } + } + } + + continue; + } + + // group name + if (token[0] == 'g' && IS_SPACE((token[1]))) { + names.clear(); + + while (!IS_NEW_LINE(token[0])) { + std::string str = parseString(&token); + names.push_back(str); + token += strspn(token, " \t\r"); // skip tag + } + + assert(names.size() > 0); + + if (callback.group_cb) { + if (names.size() > 1) { + // create const char* array. + names_out.resize(names.size() - 1); + for (size_t j = 0; j < names_out.size(); j++) { + names_out[j] = names[j + 1].c_str(); + } + callback.group_cb(user_data, &names_out.at(0), + static_cast(names_out.size())); + + } else { + callback.group_cb(user_data, NULL, 0); + } + } + + continue; + } + + // object name + if (token[0] == 'o' && IS_SPACE((token[1]))) { + // @todo { multiple object name? } + token += 2; + + std::stringstream ss; + ss << token; + std::string object_name = ss.str(); + + if (callback.object_cb) { + callback.object_cb(user_data, object_name.c_str()); + } + + continue; + } + +#if 0 // @todo + if (token[0] == 't' && IS_SPACE(token[1])) { + tag_t tag; + + token += 2; + std::stringstream ss; + ss << token; + tag.name = ss.str(); + + token += tag.name.size() + 1; + + tag_sizes ts = parseTagTriple(&token); + + tag.intValues.resize(static_cast(ts.num_ints)); + + for (size_t i = 0; i < static_cast(ts.num_ints); ++i) { + tag.intValues[i] = atoi(token); + token += strcspn(token, "/ \t\r") + 1; + } + + tag.floatValues.resize(static_cast(ts.num_reals)); + for (size_t i = 0; i < static_cast(ts.num_reals); ++i) { + tag.floatValues[i] = parseReal(&token); + token += strcspn(token, "/ \t\r") + 1; + } + + tag.stringValues.resize(static_cast(ts.num_strings)); + for (size_t i = 0; i < static_cast(ts.num_strings); ++i) { + std::stringstream ss; + ss << token; + tag.stringValues[i] = ss.str(); + token += tag.stringValues[i].size() + 1; + } + + tags.push_back(tag); + } +#endif + + // Ignore unknown command. + } + + if (err) { + (*err) += errss.str(); + } + + return true; +} + +bool ObjReader::ParseFromFile(const std::string &filename, + const ObjReaderConfig &config) { + std::string mtl_search_path; + + if (config.mtl_search_path.empty()) { + // + // split at last '/'(for unixish system) or '\\'(for windows) to get + // the base directory of .obj file + // + size_t pos = filename.find_last_of("/\\"); + if (pos != std::string::npos) { + mtl_search_path = filename.substr(0, pos); + } + } else { + mtl_search_path = config.mtl_search_path; + } + + valid_ = LoadObj(&attrib_, &shapes_, &materials_, &warning_, &error_, + filename.c_str(), mtl_search_path.c_str(), + config.triangulate, config.vertex_color); + + return valid_; +} + +bool ObjReader::ParseFromString(const std::string &obj_text, + const std::string &mtl_text, + const ObjReaderConfig &config) { + std::stringbuf obj_buf(obj_text); + std::stringbuf mtl_buf(mtl_text); + + std::istream obj_ifs(&obj_buf); + std::istream mtl_ifs(&mtl_buf); + + MaterialStreamReader mtl_ss(mtl_ifs); + + valid_ = LoadObj(&attrib_, &shapes_, &materials_, &warning_, &error_, + &obj_ifs, &mtl_ss, config.triangulate, config.vertex_color); + + return valid_; +} + +#ifdef __clang__ +#pragma clang diagnostic pop +#endif +} // namespace tinyobj + +#endif diff --git a/samples/utility.h b/samples/utility.h new file mode 100644 index 0000000..4b92bb4 --- /dev/null +++ b/samples/utility.h @@ -0,0 +1,118 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box3d/types.h" + +struct Vector4 +{ + float x, y, z, w; +}; + +struct Matrix4 +{ + Vector4 cx, cy, cz, cw; +}; + +inline Matrix4 MakeMatrix4( const b3Transform& transform, b3Vec3 scale ) +{ + b3Matrix3 m3 = b3MakeMatrixFromQuat( transform.q ); + Matrix4 m = {}; + m.cx = { scale.x * m3.cx.x, scale.x * m3.cx.y, scale.x * m3.cx.z, 0.0f }; + m.cy = { scale.y * m3.cy.x, scale.y * m3.cy.y, scale.y * m3.cy.z, 0.0f }; + m.cz = { scale.z * m3.cz.x, scale.z * m3.cz.y, scale.z * m3.cz.z, 0.0f }; + m.cw = { transform.p.x, transform.p.y, transform.p.z, 1.0f }; + return m; +} + +inline Matrix4 MakeMatrix4( const b3Transform& transform, float scale ) +{ + b3Matrix3 m3 = b3MakeMatrixFromQuat( transform.q ); + Matrix4 m = {}; + m.cx = { scale * m3.cx.x, scale * m3.cx.y, scale * m3.cx.z, 0.0f }; + m.cy = { scale * m3.cy.x, scale * m3.cy.y, scale * m3.cy.z, 0.0f }; + m.cz = { scale * m3.cz.x, scale * m3.cz.y, scale * m3.cz.z, 0.0f }; + m.cw = { transform.p.x, transform.p.y, transform.p.z, 1.0f }; + return m; +} + +inline Vector4 MulMV( const Matrix4& m, Vector4 a ) +{ + Vector4 b; + b.x = m.cx.x * a.x + m.cy.x * a.y + m.cz.x * a.z + m.cw.x * a.w; + b.y = m.cx.y * a.x + m.cy.y * a.y + m.cz.y * a.z + m.cw.y * a.w; + b.z = m.cx.z * a.x + m.cy.z * a.y + m.cz.z * a.z + m.cw.z * a.w; + b.w = m.cx.w * a.x + m.cy.w * a.y + m.cz.w * a.z + m.cw.w * a.w; + return b; +} + +inline Matrix4 MulMM( const Matrix4& a, const Matrix4& b ) +{ + Matrix4 c; + c.cx = MulMV( a, b.cx ); + c.cy = MulMV( a, b.cy ); + c.cz = MulMV( a, b.cz ); + c.cw = MulMV( a, b.cw ); + return c; +} + +struct RGBA8 +{ + uint8_t r, g, b, a; +}; + +inline RGBA8 MakeRGBA8( b3HexColor h, float alpha ) +{ + RGBA8 color; + color.r = ( h >> 16 ) & 0xFF; + color.g = ( h >> 8 ) & 0xFF; + color.b = h & 0xFF; + color.a = uint8_t( 255.0f * alpha + 0.5f ); + return color; +} + +inline RGBA8 MakeRGBA8( b3HexColor h ) +{ + RGBA8 color; + color.r = ( h >> 16 ) & 0xFF; + color.g = ( h >> 8 ) & 0xFF; + color.b = h & 0xFF; + color.a = 0xFF; + return color; +} + +struct RGBAf +{ + float r, g, b, a; +}; + +inline RGBAf MakeRGBAf( b3HexColor h, float alpha ) +{ + RGBAf color; + color.r = ( ( h >> 16 ) & 0xFF ) / 255.0f; + color.g = ( ( h >> 8 ) & 0xFF ) / 255.0f; + color.b = ( h & 0xFF ) / 255.0f; + color.a = alpha; + return color; +} + +inline RGBAf MakeRGBAf( b3HexColor h ) +{ + return MakeRGBAf( h, 1.0f ); +} + +Matrix4 MakeOrthographicMatrix( float left, float right, float bottom, float top, float near, float far ); + +struct Arena +{ + void Create( int byteCount ); + void Destroy(); + + void* Allocate( int byteCount ); + void Clear(); + + char* data; + int capacity; + int index; +}; diff --git a/shaders/common.glsl b/shaders/common.glsl new file mode 100644 index 0000000..4479268 --- /dev/null +++ b/shaders/common.glsl @@ -0,0 +1,80 @@ +// Shared constants and helpers used across deferred and forward fragment shaders. +// Included via the loader's `#include "common.glsl"` directive (no `#version` +// here; the including shader provides it). +// +// All color constants are authored in sRGB (display) space and converted to +// linear at use. Intensities are linear-space scalars. + +vec3 SrgbToLinear(vec3 c) { return pow(c, vec3(2.2)); } +vec3 LinearToSrgb(vec3 c) { return pow(c, vec3(1.0 / 2.2)); } + +vec3 ACESFilm(vec3 x) +{ + float a = 2.51, b = 0.03, c = 2.43, d = 0.59, e = 0.14; + return clamp((x * (a * x + b)) / (x * (c * x + d) + e), 0.0, 1.0); +} + +const vec3 SUN_COLOR_SRGB = vec3(1.00, 0.97, 0.92); +const float SUN_INTENSITY = 0.5; +const vec3 SKY_AMBIENT_SRGB = vec3(0.55, 0.60, 0.68); +const vec3 GROUND_AMBIENT_SRGB = vec3(0.22, 0.20, 0.17); +const float AMBIENT_INTENSITY = 0.4; +const float SPEC_POWER = 32.0; +const float SPEC_SCALE = 0.20; + +const vec3 SKY_ZENITH_SRGB = vec3(0.28, 0.48, 0.78); +const vec3 SKY_HORIZON_SRGB = vec3(0.55, 0.65, 0.78); +const vec3 SKY_GROUND_SRGB = vec3(0.28, 0.25, 0.22); +const vec3 SUN_DISK_SRGB = vec3(1.00, 0.97, 0.85); +const float SUN_DISK_BRIGHT = 8.0; +const vec3 SUN_HALO_SRGB = vec3(1.00, 0.92, 0.78); +const float SUN_HALO_BRIGHT = 0.35; + +const float FOG_DENSITY = 0.002; + +vec3 SkyColor(vec3 rayDir, vec3 sunDir) +{ + float t = clamp(rayDir.y, -1.0, 1.0); + vec3 skySrgb = (t >= 0.0) + ? mix(SKY_HORIZON_SRGB, SKY_ZENITH_SRGB, pow(t, 1.2)) + : mix(SKY_HORIZON_SRGB, SKY_GROUND_SRGB, pow(-t, 0.50)); + vec3 sky = SrgbToLinear(skySrgb); + + float sunCos = max(dot(rayDir, sunDir), 0.0); + sky += SrgbToLinear(SUN_DISK_SRGB) * SUN_DISK_BRIGHT * pow(sunCos, 800.0); + sky += SrgbToLinear(SUN_HALO_SRGB) * SUN_HALO_BRIGHT * pow(sunCos, 8.0); + return sky; +} + +// Hemispheric ambient term for a +Y-up world. N is world-space normal. +vec3 HemisphericAmbient(vec3 N) +{ + float hemi = 0.5 + 0.5 * N.y; + return AMBIENT_INTENSITY * mix(SrgbToLinear(GROUND_AMBIENT_SRGB), + SrgbToLinear(SKY_AMBIENT_SRGB), hemi); +} + +// TPDF (triangular) dither for 8-bit sRGB output. Two independent IGN samples +// give noise in [-1, +1] LSB; converting smooth gradients (sky, fog) from +// hard contour bands into high-frequency texture the eye averages out. Apply +// AFTER LinearToSrgb so quantization happens in display space. +vec3 DitherSrgb(vec3 c, vec2 fragCoord) +{ + const vec2 magic = vec2(0.06711056, 0.00583715); + const float k = 52.9829189; + float r1 = fract(k * fract(dot(fragCoord, magic))); + float r2 = fract(k * fract(dot(fragCoord + 7.0, magic))); + float t = r1 + r2 - 1.0; + return c + vec3(t) * (1.0 / 255.0); +} + +// Anti-aliased 1-pixel-wide world-space grid. uv is the tiled coordinate +// (e.g. worldXZ divided by the desired cell size). Returns ~1.0 on a line, +// ~0.0 in the cell interior; fwidth filtering keeps the line crisp at any +// distance and prevents the grid from aliasing on far receivers. +float WorldGrid(vec2 uv) +{ + vec2 grid = abs(fract(uv - 0.5) - 0.5); + vec2 line = grid / fwidth(uv); + return 1.0 - smoothstep(0.0, 1.5, min(line.x, line.y)); +} diff --git a/shaders/deferred_lighting_fs.glsl b/shaders/deferred_lighting_fs.glsl new file mode 100644 index 0000000..f01ebf1 --- /dev/null +++ b/shaders/deferred_lighting_fs.glsl @@ -0,0 +1,253 @@ +#version 330 core + +#include "common.glsl" + +out vec4 outColor; + +in vec2 TexCoords; + +uniform sampler2D gPosition; // view-space position (xyz=0 for sky) +uniform sampler2D gNormal; // view-space normal +uniform sampler2D gAlbedoSpec; // sRGB albedo, a specular strength +uniform sampler2D ssao; +uniform sampler2DShadow shadowMap; + +uniform mat4 lightSpaceMatrix; +uniform mat4 cameraMatrix; // camera-to-world +uniform float worldTexelSize; // world-space span of one shadow-map texel +uniform float lightZRange; // lightFarPlane - lightNearPlane, in world units + +// Sun + sky +uniform vec3 sunDirWorld; // normalized, points from scene toward sun +uniform float tanHalfFovY; +uniform float aspectRatio; + +uniform bool useSSAO; +uniform bool useShadow; +uniform bool useGrid; +uniform float aoPower; +uniform float aoDirect; +uniform bool aoOnly; + +// Vogel disk (phyllotaxis spiral) sampling. Produces naturally well-distributed +// taps for any sample count, and the rotation phase is just an additive angle +// per tap — no rotation matrix needed. +const float GOLDEN_ANGLE = 2.39996323; +const int PCF_TAP_COUNT = 32; +const float PCF_RADIUS_TEXELS = 3.0; + +float ShadowCalculation(vec4 lightSpacePos, vec3 worldNormal, vec3 worldPos) +{ + vec3 projCoords = lightSpacePos.xyz / lightSpacePos.w; + projCoords = projCoords * 0.5 + 0.5; + + // dFdx/dFdy must run before any early return so neighboring fragments in the + // 2x2 quad always reach this point with valid projCoords values. + vec3 ddxCoords = dFdx(projCoords); + vec3 ddyCoords = dFdy(projCoords); + + if (projCoords.z > 1.0 || projCoords.z < 0.0) return 0.0; + + // Soft-fade near the shadow map boundary so geometry just outside the fitted + // frustum doesn't produce a hard cutoff line. Falls off over the outer 6% + // of the xy range and outer 10% of the depth range. + vec2 edge = min(projCoords.xy, 1.0 - projCoords.xy); + float xyFade = smoothstep(0.0, 0.06, min(edge.x, edge.y)); + float zFade = 1.0 - smoothstep(0.90, 1.0, projCoords.z); + float fade = xyFade * zFade; + if (fade <= 0.0) return 0.0; + + // Receiver-plane depth bias: convert the screen-space gradient of projCoords.z + // into a gradient with respect to shadow-map UV via the inverse Jacobian. + // Each PCF tap then gets the depth the receiver surface would *actually* have + // at that tap's offset, so per-texel acne disappears without lifting shadows + // off contact via a large constant bias. + // + // Silhouette guard: at object edges, dFdx/dFdy mix samples from two unrelated + // surfaces, producing a meaningless gradient that would over-bias the test + // and paint a bright fringe along the silhouette. fwidth flags that case and + // we fall back to a constant bias for those fragments. + vec2 dzduv = vec2(0.0); + // True silhouettes (adjacent pixels on different surfaces) have huge zWidth, + // close to 1.0. Continuous surfaces even at grazing angles stay well below. + float zWidth = abs(ddxCoords.z) + abs(ddyCoords.z); + bool onSilhouette = zWidth > 0.3; + if (!onSilhouette) + { + float det = ddxCoords.x * ddyCoords.y - ddxCoords.y * ddyCoords.x; + if (abs(det) > 1e-8) + { + float invDet = 1.0 / det; + dzduv.x = (ddyCoords.y * ddxCoords.z - ddxCoords.y * ddyCoords.z) * invDet; + dzduv.y = (ddxCoords.x * ddyCoords.z - ddyCoords.x * ddxCoords.z) * invDet; + } + // Clamp loose enough to cover legitimate steep slopes (e.g. surfaces + // nearly aligned with the light direction) but still finite. + dzduv = clamp(dzduv, vec2(-2.0), vec2(2.0)); + } + + vec2 texelSize = 1.0 / textureSize(shadowMap, 0); + + // Adapt PCF kernel radius to the screen pixel's footprint in shadow texels. + // At far view distances each screen pixel covers many shadow texels; a + // fixed 3-texel disk fits *inside* a single pixel, so adjacent pixels see + // nearly the same texel set just rotated differently — that resonates with + // the texel grid into the swirly moire visible on distant flat receivers. + // Growing the kernel with the footprint makes the taps actually average + // over the pixel's shadow-texel coverage, and as a bonus distant shadows + // get the gentle softening they should naturally have. Clamped so very + // distant geometry doesn't blow the kernel up to a useless smear. + float footprintTexels = max(length(ddxCoords.xy), length(ddyCoords.xy)) / texelSize.x; + float radiusTexels = clamp(footprintTexels, PCF_RADIUS_TEXELS, 8.0); + vec2 sampleScale = radiusTexels * texelSize; + + // Hardware bilinear PCF samples a 2x2 texel neighborhood against a single + // refZ, but the four stored depths differ by up to dzduv * 0.5 * texelSize. + // Without absorbing that, half the bilinear neighbors pass and half fail, + // producing the residual banding RPDB alone can't kill. Scaling the bias + // with the receiver slope keeps adjacent texels on the same side of the + // comparison. + float slopeBias = (abs(dzduv.x) + abs(dzduv.y)) * texelSize.x; + // World-unit bias floor: the existing constants are in normalized [0,1] + // light-space depth, but the geometric scale that actually causes acne + // (stacked-brick seams, hull-on-hull contacts) is in world units. A + // 1-world-unit caster spans `1.0 / lightZRange` of normalized depth, so + // any bias smaller than that lets self-shadow bands appear at every + // brick interface. Floor it at ~half a world unit so the bias adapts + // automatically to camera distance (which controls lightZRange). + float minBias = 0.5 / max(lightZRange, 1.0); + float bias = max(minBias, onSilhouette ? 0.003 : (slopeBias + 0.0005)); + + // Per-pixel rotation phase. At extreme grazing angles each screen pixel + // covers many shadow texels and rotation makes adjacent pixels sample + // wildly different texel subsets — that resonates with the texel grid into + // visible moire. Fade rotation out as the receiver becomes foreshortened. + // + // Use Interleaved Gradient Noise (Jimenez 2014) for the angle. The basic + // fract(sin(dot(...))*43758.5453) hash has low-frequency structure that + // shows through as concentric/swirl moire when each screen pixel covers + // many shadow texels (distant view of a flat receiver). IGN was designed + // for shader dithering and decorrelates much better at neighboring pixels. + float rotStrength = 1.0 - smoothstep(0.05, 0.20, zWidth); + float ign = fract(52.9829189 * fract(dot(gl_FragCoord.xy, vec2(0.06711056, 0.00583715)))); + float phi = ign * 6.2831853 * rotStrength; + + // Vogel-disk PCF: each tap is one rotation step further around the golden- + // angle spiral, with radius growing as sqrt(i/N) so density is uniform + // across the disk. sampler2DShadow + GL_LEQUAL returns 1.0 when refZ <= + // storedDepth (lit), with hardware bilinear PCF inside each tap thanks to + // GL_LINEAR filtering on the depth texture. + float lit = 0.0; + float invN = 1.0 / float(PCF_TAP_COUNT); + for (int i = 0; i < PCF_TAP_COUNT; ++i) + { + float r = sqrt((float(i) + 0.5) * invN); + float theta = float(i) * GOLDEN_ANGLE + phi; + vec2 offset = vec2(cos(theta), sin(theta)) * r * sampleScale; + float refZ = projCoords.z + dot(dzduv, offset) - bias; + lit += texture(shadowMap, vec3(projCoords.xy + offset, refZ)); + } + return (1.0 - lit * invN) * fade; +} + +void main() +{ + vec2 ndc = TexCoords * 2.0 - 1.0; + vec3 camRight = cameraMatrix[0].xyz; + vec3 camUp = cameraMatrix[1].xyz; + vec3 camForward = -cameraMatrix[2].xyz; + vec3 viewDirWorld = normalize( + camForward + + ndc.x * tanHalfFovY * aspectRatio * camRight + + ndc.y * tanHalfFovY * camUp + ); + + vec3 viewPos = texture(gPosition, TexCoords).rgb; + vec3 viewNormal = texture(gNormal, TexCoords).rgb; + vec3 albedoSrgb = texture(gAlbedoSpec, TexCoords).rgb; + float specStrength = texture(gAlbedoSpec, TexCoords).a; + float ao = useSSAO ? texture(ssao, TexCoords).r : 1.0; + + if (viewPos.z == 0.0) + { + // AO-only debug still shows sky as sky so the geometry framing reads. + vec3 sky = LinearToSrgb(ACESFilm(SkyColor(viewDirWorld, sunDirWorld))); + outColor = vec4(DitherSrgb(sky, gl_FragCoord.xy), 1.0); + return; + } + + if (aoOnly) + { + // Bypass tonemap so AO of 1.0 reads as pure white. + outColor = vec4(vec3(ao), 1.0); + return; + } + + vec3 baseColor = SrgbToLinear(albedoSrgb); + vec3 worldPos = (cameraMatrix * vec4(viewPos, 1.0)).xyz; + vec3 worldNormal = normalize(mat3(cameraMatrix) * viewNormal); + + // World-space grid on near-horizontal upward faces. Two tiers so distance + // reads at any zoom: minor lines every 1 unit, major every 5. fwidth keeps + // each line ~1 pixel wide regardless of camera distance, which both + // avoids aliasing and produces a natural "fade with distance" since lines + // sub-pixel-merge into the surface tint as they recede. The orientation + // gate lets the same grid apply to stacked-cube tops without leaking onto + // walls or sloped faces. Disabled per-sample on mesh-collision grounds + // where the wireframe overlay already provides the same signal. + if (useGrid && abs(worldPos.y) < 0.5) + { + float groundFactor = smoothstep(0.95, 0.99, worldNormal.y); + if (groundFactor > 0.0) + { + float minor = WorldGrid(worldPos.xz); + float major = WorldGrid(worldPos.xz * 0.2); + float gridDarken = (1.0 - 0.15 * minor) * (1.0 - 0.40 * major); + baseColor *= mix(1.0, gridDarken, groundFactor); + } + } + + float aoFactor = pow(ao, aoPower); + + vec3 ambient = HemisphericAmbient(worldNormal) * aoFactor; + + vec3 sunColor = SrgbToLinear(SUN_COLOR_SRGB) * SUN_INTENSITY; + + float NdotL = max(dot(worldNormal, sunDirWorld), 0.0); + vec3 diffuse = NdotL * sunColor; + + vec3 viewDirToCam = -viewDirWorld; + vec3 halfDir = normalize(sunDirWorld + viewDirToCam); + float spec = pow(max(dot(worldNormal, halfDir), 0.0), SPEC_POWER); + vec3 specular = spec * specStrength * SPEC_SCALE * sunColor; + + float shadow = 0.0; + if (useShadow) + { + // Normal-offset bias: push the receiver point along the world normal + // before projecting to light space. RPDB inside ShadowCalculation + // handles per-tap bias well on flat receivers, but its Jacobian breaks + // down at grazing angles where dzduv saturates the ±2 clamp. Offsetting + // along N concentrates extra bias exactly on faces near-perpendicular + // to the light (NdotL near 0), killing the residual acne RPDB misses. + vec3 biasedWorldPos = worldPos + worldNormal * worldTexelSize * 1.5 * (1.0 - NdotL); + vec4 lightSpacePos = lightSpaceMatrix * vec4(biasedWorldPos, 1.0); + shadow = ShadowCalculation(lightSpacePos, worldNormal, worldPos); + } + + // todo for disabling shadows + // shadow = 0.0f; + + // Physically AO should only modulate indirect light, but direct sun overwhelms ambient on + // sunlit surfaces and the contact darkening disappears. aoDirect dials in a controllable + // amount of AO on the direct term — a fudge, but the only way to get visible contact AO + // on a brightly lit floor without a separate contact-shadow pass. + float directAO = mix(1.0, aoFactor, aoDirect); + vec3 color = baseColor * (ambient + (1.0 - shadow) * (diffuse + specular) * directAO); + + float dist = length(viewPos); + float fog = 1.0 - exp(-FOG_DENSITY * dist); + color = mix(color, SkyColor(viewDirWorld, sunDirWorld), fog); + + outColor = vec4(DitherSrgb(LinearToSrgb(ACESFilm(color)), gl_FragCoord.xy), 1.0); +} diff --git a/shaders/depth_copy_fs.glsl b/shaders/depth_copy_fs.glsl new file mode 100644 index 0000000..3661142 --- /dev/null +++ b/shaders/depth_copy_fs.glsl @@ -0,0 +1,10 @@ +#version 330 core + +in vec2 TexCoords; + +uniform sampler2D depthTexture; + +void main() +{ + gl_FragDepth = texture(depthTexture, TexCoords).r; +} diff --git a/shaders/empty.fs b/shaders/empty.fs new file mode 100644 index 0000000..cd61250 --- /dev/null +++ b/shaders/empty.fs @@ -0,0 +1,5 @@ +#version 330 core + +void main() +{ +} diff --git a/shaders/forward.fs b/shaders/forward.fs new file mode 100644 index 0000000..cea36dc --- /dev/null +++ b/shaders/forward.fs @@ -0,0 +1,10 @@ +#version 330 core + +layout (location = 0) out vec4 fragmentColor; + +in vec3 overrideColor; + +void main() +{ + fragmentColor = vec4(overrideColor, 1.0); +} diff --git a/shaders/forward.vs b/shaders/forward.vs new file mode 100644 index 0000000..d45a38a --- /dev/null +++ b/shaders/forward.vs @@ -0,0 +1,17 @@ +#version 330 core + +layout (location = 0) in vec3 v_position; +layout (location = 1) in int v_flags; + +uniform mat4 projection; +uniform mat4 view; +uniform mat4 model; +uniform vec3 color; + +out vec3 overrideColor; + +void main() +{ + overrideColor = v_flags != 0 ? vec3(0.9, 0.9, 0.9) : color; + gl_Position = projection * view * model * vec4(v_position, 1.0); +} diff --git a/shaders/geometry_fs.glsl b/shaders/geometry_fs.glsl new file mode 100644 index 0000000..0ffb3d9 --- /dev/null +++ b/shaders/geometry_fs.glsl @@ -0,0 +1,20 @@ +#version 330 core + +layout (location = 0) out vec3 gPosition; +layout (location = 1) out vec3 gNormal; +layout (location = 2) out vec4 gAlbedoSpec; + +in vec3 fragmentPosition; +in vec3 normal; +in vec4 color; + +void main() +{ + // store the fragment position vector in the first gbuffer texture + gPosition = fragmentPosition; + + // also store the per-fragment normals into the gbuffer + gNormal = normalize(normal); + + gAlbedoSpec = color; +} \ No newline at end of file diff --git a/shaders/geometry_vs.glsl b/shaders/geometry_vs.glsl new file mode 100644 index 0000000..bc74a2a --- /dev/null +++ b/shaders/geometry_vs.glsl @@ -0,0 +1,46 @@ +#version 330 core + +layout (location = 0) in vec3 aPos; +layout (location = 1) in vec3 aNormal; +layout (location = 2) in int aMaterial; + +out vec3 fragmentPosition; +out vec3 normal; +out vec4 color; + +// for drawing a hollow cube +uniform bool invertedNormals; + +uniform mat4 model; +uniform mat4 view; +uniform mat4 projection; +uniform vec4 objectColor; + +void main() +{ + // Define an array of 10 colors + vec3 colors[10]; + colors[0] = vec3(0.4706, 0.6118, 0.2); + colors[1] = vec3(0.5255, 0.2941, 0.3529); + colors[2] = vec3(0.2, 0.2, 0.4588); + colors[3] = vec3(0.7451, 0.5725, 0.2471); + colors[4] = vec3(0.6784, 0.2627, 0.6784); + colors[5] = vec3(0.2784, 0.6, 0.6); + colors[6] = vec3(0.502, 0.251, 0.0039); + colors[7] = vec3(0.698, 0.5608, 0.8314); + colors[8] = vec3(0.0, 0.2941, 0.0); + colors[9] = vec3(0.5, 0.5, 0.5); + + vec4 viewPos = view * model * vec4(aPos, 1.0); + fragmentPosition = viewPos.xyz; + + mat3 normalMatrix = transpose(inverse(mat3(view * model))); + normal = normalMatrix * (invertedNormals ? -aNormal : aNormal); + + int colorIndex = max(0, aMaterial) % 10; + color = aMaterial >= 0 ? vec4(colors[colorIndex], 1.0) : objectColor; + + // color = vec4(colors[2], 1.0f); + + gl_Position = projection * viewPos; +} \ No newline at end of file diff --git a/shaders/mesh.fs b/shaders/mesh.fs new file mode 100644 index 0000000..0ffb3d9 --- /dev/null +++ b/shaders/mesh.fs @@ -0,0 +1,20 @@ +#version 330 core + +layout (location = 0) out vec3 gPosition; +layout (location = 1) out vec3 gNormal; +layout (location = 2) out vec4 gAlbedoSpec; + +in vec3 fragmentPosition; +in vec3 normal; +in vec4 color; + +void main() +{ + // store the fragment position vector in the first gbuffer texture + gPosition = fragmentPosition; + + // also store the per-fragment normals into the gbuffer + gNormal = normalize(normal); + + gAlbedoSpec = color; +} \ No newline at end of file diff --git a/shaders/mesh.vs b/shaders/mesh.vs new file mode 100644 index 0000000..8c50074 --- /dev/null +++ b/shaders/mesh.vs @@ -0,0 +1,48 @@ +#version 330 core + +// Static +layout (location = 0) in vec3 v_localPosition; +layout (location = 1) in vec3 v_localNormal; +layout (location = 2) in int v_material; + +// Instance +layout (location = 3) in vec4 v_instanceColor; +layout (location = 4) in mat4 v_instanceMatrix; + +out vec3 fragmentPosition; +out vec3 normal; +out vec4 color; + +uniform mat4 view; +uniform mat4 projection; + +void main() +{ + // Define an array of 10 colors + vec3 colors[10]; + colors[0] = vec3(0.4706, 0.6118, 0.2); + colors[1] = vec3(0.5255, 0.2941, 0.3529); + colors[2] = vec3(0.2, 0.2, 0.4588); + colors[3] = vec3(0.7451, 0.5725, 0.2471); + colors[4] = vec3(0.6784, 0.2627, 0.6784); + colors[5] = vec3(0.2784, 0.6, 0.6); + colors[6] = vec3(0.502, 0.251, 0.0039); + colors[7] = vec3(0.698, 0.5608, 0.8314); + colors[8] = vec3(0.0, 0.2941, 0.0); + colors[9] = vec3(0.5, 0.5, 0.5); + + vec4 viewPos = view * v_instanceMatrix * vec4(v_localPosition, 1.0); + fragmentPosition = viewPos.xyz; + + mat3 normalMatrix = transpose(inverse(mat3(view * v_instanceMatrix))); + normal = normalMatrix * v_localNormal; + + int colorIndex = max(0, v_material) % 10; + + // alpha is specular + color = v_material >= 0 ? vec4(colors[colorIndex], 1.0) : v_instanceColor; + + //color = vec4(colors[4], 1.0f); + + gl_Position = projection * viewPos; +} \ No newline at end of file diff --git a/shaders/mesh_edge.vs b/shaders/mesh_edge.vs new file mode 100644 index 0000000..7667f77 --- /dev/null +++ b/shaders/mesh_edge.vs @@ -0,0 +1,20 @@ +#version 330 core + +// Static +layout (location = 0) in vec3 v_localPosition; +layout (location = 1) in int v_flags; + +// Instance +layout (location = 2) in vec4 v_instanceColor; +layout (location = 3) in mat4 v_instanceMatrix; + +out vec3 overrideColor; + +uniform mat4 projection; +uniform mat4 view; + +void main() +{ + overrideColor = v_flags != 0 ? vec3(0.6, 0.6, 0.6) : 0.5 * v_instanceColor.rgb; + gl_Position = projection * view * v_instanceMatrix * vec4(v_localPosition, 1.0); +} diff --git a/shaders/mesh_forward.fs b/shaders/mesh_forward.fs new file mode 100644 index 0000000..96c17f6 --- /dev/null +++ b/shaders/mesh_forward.fs @@ -0,0 +1,34 @@ +#version 330 core + +#include "common.glsl" + +layout (location = 0) out vec4 fragmentColor; + +in vec3 position; // world space +in vec3 normal; // world space +in vec4 color; // sRGB albedo, alpha is opacity + +uniform vec3 sunDirWorld; +uniform vec3 cameraPos; + +void main() +{ + vec3 N = normalize(normal); + vec3 viewRay = normalize(position - cameraPos); + + vec3 baseColor = SrgbToLinear(color.rgb); + + vec3 ambient = HemisphericAmbient(N); + + vec3 sunColor = SrgbToLinear(SUN_COLOR_SRGB) * SUN_INTENSITY; + float NdotL = max(dot(N, sunDirWorld), 0.0); + vec3 diffuse = NdotL * sunColor; + + vec3 lit = baseColor * (ambient + diffuse); + + float dist = length(position - cameraPos); + float fog = 1.0 - exp(-FOG_DENSITY * dist); + lit = mix(lit, SkyColor(viewRay, sunDirWorld), fog); + + fragmentColor = vec4(DitherSrgb(LinearToSrgb(ACESFilm(lit)), gl_FragCoord.xy), color.a); +} diff --git a/shaders/mesh_forward.vs b/shaders/mesh_forward.vs new file mode 100644 index 0000000..19b203d --- /dev/null +++ b/shaders/mesh_forward.vs @@ -0,0 +1,47 @@ +#version 330 core + +layout (location = 0) in vec3 v_localPosition; +layout (location = 1) in vec3 v_localNormal; +layout (location = 2) in int v_material; +layout (location = 3) in vec4 v_instanceColor; +layout (location = 4) in mat4 v_instanceMatrix; + +out vec3 position; +out vec3 normal; +out vec4 color; + +uniform mat4 view; +uniform mat4 projection; + +void main() +{ + // Define an array of 10 colors + vec3 colors[10]; + colors[0] = vec3(0.4706, 0.6118, 0.2); + colors[1] = vec3(0.5255, 0.2941, 0.3529); + colors[2] = vec3(0.2, 0.2, 0.4588); + colors[3] = vec3(0.7451, 0.5725, 0.2471); + colors[4] = vec3(0.6784, 0.2627, 0.6784); + colors[5] = vec3(0.2784, 0.6, 0.6); + colors[6] = vec3(0.502, 0.251, 0.0039); + colors[7] = vec3(0.698, 0.5608, 0.8314); + colors[8] = vec3(0.0, 0.2941, 0.0); + colors[9] = vec3(0.5, 0.5, 0.5); + + // world space position + position = vec3(v_instanceMatrix * vec4(v_localPosition, 1.0)); + + vec4 viewPos = view * vec4(position, 1.0); + + // world space normal, accounting for non-uniform scale + mat3 normalMatrix = transpose(inverse(mat3(v_instanceMatrix))); + normal = normalMatrix * v_localNormal; + + int colorIndex = max(0, v_material) % 10; + color = v_material >= 0 ? vec4(colors[colorIndex], v_instanceColor.a) : v_instanceColor; + + // color = vec4(colors[2], 0.5f); + + gl_Position = projection * viewPos; + +} \ No newline at end of file diff --git a/shaders/mesh_shadow.vs b/shaders/mesh_shadow.vs new file mode 100644 index 0000000..551471f --- /dev/null +++ b/shaders/mesh_shadow.vs @@ -0,0 +1,14 @@ +#version 330 core + +layout (location = 0) in vec3 v_localPosition; +layout (location = 1) in vec3 v_localNormal; +layout (location = 2) in int v_material; +layout (location = 3) in vec4 v_instanceColor; +layout (location = 4) in mat4 v_instanceMatrix; + +uniform mat4 lightSpaceMatrix; + +void main() +{ + gl_Position = lightSpaceMatrix * v_instanceMatrix * vec4(v_localPosition, 1.0); +} diff --git a/shaders/quad_vs.glsl b/shaders/quad_vs.glsl new file mode 100644 index 0000000..9f93e29 --- /dev/null +++ b/shaders/quad_vs.glsl @@ -0,0 +1,11 @@ +#version 330 core +layout (location = 0) in vec3 aPos; +layout (location = 1) in vec2 aTexCoords; + +out vec2 TexCoords; + +void main() +{ + TexCoords = aTexCoords; + gl_Position = vec4(aPos, 1.0); +} \ No newline at end of file diff --git a/shaders/shadow_map.vs b/shaders/shadow_map.vs new file mode 100644 index 0000000..947562f --- /dev/null +++ b/shaders/shadow_map.vs @@ -0,0 +1,10 @@ +#version 330 core +layout (location = 0) in vec3 aPos; + +uniform mat4 lightSpaceMatrix; +uniform mat4 model; + +void main() +{ + gl_Position = lightSpaceMatrix * model * vec4(aPos, 1.0); +} \ No newline at end of file diff --git a/shaders/ssao_blur_fs.glsl b/shaders/ssao_blur_fs.glsl new file mode 100644 index 0000000..24ec446 --- /dev/null +++ b/shaders/ssao_blur_fs.glsl @@ -0,0 +1,47 @@ +#version 330 core +out float FragColor; + +in vec2 TexCoords; + +uniform sampler2D ssaoInput; +uniform sampler2D gPosition; + +// Depth difference (in view-space units) over which a tap's weight halves. +// Tight enough to keep silhouette edges sharp, loose enough not to leave +// per-tap noise behind on smooth surfaces. +const float DEPTH_SIGMA = 0.25; + +void main() +{ + vec2 texelSize = 1.0 / vec2(textureSize(ssaoInput, 0)); + float centerZ = texture(gPosition, TexCoords).z; + + // Sky / no-geometry: pass through. Avoids contaminating real geometry's + // bilateral weights with the zero-init sentinel from the G-buffer. + if (centerZ == 0.0) + { + FragColor = texture(ssaoInput, TexCoords).r; + return; + } + + float invTwoSigmaSq = 1.0 / (2.0 * DEPTH_SIGMA * DEPTH_SIGMA); + + float result = 0.0; + float weightSum = 0.0; + for (int x = -2; x < 2; ++x) + { + for (int y = -2; y < 2; ++y) + { + vec2 sampleUV = TexCoords + vec2(float(x), float(y)) * texelSize; + float tapZ = texture(gPosition, sampleUV).z; + // Drop sky taps so they don't pull the weighted average toward 1.0 + // along silhouettes against the background. + if (tapZ == 0.0) continue; + float dz = centerZ - tapZ; + float w = exp(-dz * dz * invTwoSigmaSq); + result += texture(ssaoInput, sampleUV).r * w; + weightSum += w; + } + } + FragColor = (weightSum > 0.0) ? (result / weightSum) : texture(ssaoInput, TexCoords).r; +} diff --git a/shaders/ssao_fs.glsl b/shaders/ssao_fs.glsl new file mode 100644 index 0000000..3ae4423 --- /dev/null +++ b/shaders/ssao_fs.glsl @@ -0,0 +1,67 @@ +#version 330 core +out float FragColor; + +in vec2 TexCoords; + +uniform sampler2D gPosition; +uniform sampler2D gNormal; +uniform sampler2D texNoise; + +uniform vec3 samples[32]; +uniform float radius = 0.5; +uniform float bias = 0.025; +uniform float minScale = 0.1; + +// parameters (you'd probably want to use them as uniforms to more easily tweak the effect) +int kernelSize = 32; + +// tile noise texture over screen based on screen dimensions divided by noise size +uniform vec2 noiseScale; + +uniform mat4 projection; + +void main() +{ + // get input for SSAO algorithm + vec3 fragPos = texture(gPosition, TexCoords).xyz; + + // early-out for sky/background fragments (no geometry in G-buffer) + if (fragPos.z == 0.0) + { + FragColor = 1.0; + return; + } + + vec3 normal = normalize(texture(gNormal, TexCoords).rgb); + vec3 randomVec = normalize(texture(texNoise, TexCoords * noiseScale).xyz); + + // create TBN change-of-basis matrix: from tangent-space to view-space + vec3 tangent = normalize(randomVec - normal * dot(randomVec, normal)); + vec3 bitangent = cross(normal, tangent); + mat3 TBN = mat3(tangent, bitangent, normal); + + // iterate over the sample kernel and calculate occlusion factor + float occlusion = 0.0; + for(int i = 0; i < kernelSize; ++i) + { + // get sample position + vec3 samplePos = TBN * samples[i]; // from tangent to view-space + samplePos = fragPos + samplePos * radius; + + // project sample position (to sample texture) (to get position on screen/texture) + vec4 offset = vec4(samplePos, 1.0); + offset = projection * offset; // from view to clip-space + offset.xyz /= offset.w; // perspective divide + offset.xyz = offset.xyz * 0.5 + 0.5; // transform to range 0.0 - 1.0 + + // get sample depth + float sampleDepth = texture(gPosition, offset.xy).z; // get depth value of kernel sample + + // range check & accumulate + float rangeCheck = smoothstep(0.0, 1.0, radius / abs(fragPos.z - sampleDepth)); + occlusion += (sampleDepth >= samplePos.z + bias ? 1.0 : 0.0) * rangeCheck; + } + occlusion = max(1.0 - (occlusion / kernelSize), minScale); + + FragColor = occlusion; +} diff --git a/shared/CMakeLists.txt b/shared/CMakeLists.txt new file mode 100644 index 0000000..aba87f1 --- /dev/null +++ b/shared/CMakeLists.txt @@ -0,0 +1,25 @@ +# Shared code for samples, benchmarks, and unit tests + +set(BOX3D_SHARED_FILES + benchmarks.c + benchmarks.h + determinism.c + determinism.h + human.c + human.h + stability.c + stability.h + utils.c + utils.h +) + +add_library(shared ${BOX3D_SHARED_FILES}) + +set_target_properties(shared PROPERTIES + C_STANDARD 17 +) + +target_include_directories(shared PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(shared PRIVATE box3d) + +source_group(TREE ${CMAKE_CURRENT_SOURCE_DIR} PREFIX "" FILES ${BOX3D_SHARED_FILES}) \ No newline at end of file diff --git a/shared/benchmarks.c b/shared/benchmarks.c new file mode 100644 index 0000000..7fb9587 --- /dev/null +++ b/shared/benchmarks.c @@ -0,0 +1,1005 @@ +// SPDX-FileCopyrightText: 2022 Erin Catto +// SPDX-License-Identifier: MIT + +#include "benchmarks.h" + +#include "human.h" +#include "utils.h" + +#include "box3d/box3d.h" + +#include +#include +#include + +#ifdef NDEBUG +#define BENCHMARK_DEBUG 0 +#else +#define BENCHMARK_DEBUG 1 +#endif + +void CreateJointGrid( b3WorldId worldId ) +{ + b3World_EnableSleeping( worldId, false ); + + int n = BENCHMARK_DEBUG ? 10 : 100; + + // Allocate to avoid huge stack usage + b3BodyId* bodies = malloc( n * n * sizeof( b3BodyId ) ); + int index = 0; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.filter.categoryBits = 2; + shapeDef.filter.maskBits = ~2u; + + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.4f }; + + b3SphericalJointDef jointDef = b3DefaultSphericalJointDef(); + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.enableSleep = false; + + for ( int k = 0; k < n; ++k ) + { + for ( int i = 0; i < n; ++i ) + { + float fk = (float)k; + float fi = (float)i; + + if ( i == 0 ) + { + bodyDef.type = b3_staticBody; + } + else + { + bodyDef.type = b3_dynamicBody; + } + + bodyDef.position = (b3Vec3){ fk, -fi, 0.0f }; + + b3BodyId body = b3CreateBody( worldId, &bodyDef ); + + b3CreateSphereShape( body, &shapeDef, &sphere ); + + if ( i > 0 ) + { + jointDef.base.bodyIdA = bodies[index - 1]; + jointDef.base.bodyIdB = body; + jointDef.base.localFrameA.p = (b3Vec3){ 0.0f, -0.5f, 0.0f }; + jointDef.base.localFrameB.p = (b3Vec3){ 0.0f, 0.5f, 0.0f }; + b3CreateSphericalJoint( worldId, &jointDef ); + } + + if ( k > 0 ) + { + jointDef.base.bodyIdA = bodies[index - n]; + jointDef.base.bodyIdB = body; + jointDef.base.localFrameA.p = (b3Vec3){ 0.5f, 0.0f, 0.0f }; + jointDef.base.localFrameB.p = (b3Vec3){ -0.5f, 0.0f, 0.0f }; + b3CreateSphericalJoint( worldId, &jointDef ); + } + + bodies[index++] = body; + } + } + + free( bodies ); +} + +// The 80 block version falls over after 1000 steps. +void CreateLargePyramid( b3WorldId worldId ) +{ + b3World_EnableSleeping( worldId, false ); + + int baseCount = BENCHMARK_DEBUG ? 20 : 90; + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = (b3Vec3){ 0.0f, -1.0f }; + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 100.0f, 1.0f, 100.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + + float h = 0.5f; + b3BoxHull box = b3MakeBoxHull( h, h, h ); + + float shift = 1.0f * h; + + for ( int i = 0; i < baseCount; ++i ) + { + float y = ( 2.0f * i + 1.0f ) * shift; + + for ( int j = i; j < baseCount; ++j ) + { + float x = ( i + 1.0f ) * shift + 2.0f * ( j - i ) * shift - h * baseCount; + + bodyDef.position = (b3Vec3){ x, y, 0.0f }; + + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + } +} + +void CreateWidePyramid( b3WorldId worldId ) +{ + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = (b3Vec3){ 0.0f, -1.0f }; + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 100.0f, 1.0f, 100.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + + const float boxSize = 2.0f; + const float boxSeparation = 0.5f; + const float halfBoxSize = 0.5f * boxSize; + const int pyramidHeight = BENCHMARK_DEBUG ? 5 : 15; + + float h = halfBoxSize - 0.025; + b3BoxHull box = b3MakeBoxHull( h, h, h ); + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + for ( int i = 0; i < pyramidHeight; ++i ) + { + for ( int j = i / 2; j < pyramidHeight - ( i + 1 ) / 2; ++j ) + { + for ( int k = i / 2; k < pyramidHeight - ( i + 1 ) / 2; ++k ) + { + float x = -pyramidHeight + boxSize * j + ( i & 1 ? halfBoxSize : 0.0f ); + float y = 1.0f + ( boxSize + boxSeparation ) * i; + float z = -pyramidHeight + boxSize * k + ( i & 1 ? halfBoxSize : 0.0f ); + bodyDef.position = (b3Vec3){ x, y, z }; + + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + } + } +} + +static void CreateSmallPyramid( b3WorldId worldId, int baseCount, float extent, float centerX, float baseZ ) +{ + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.enableSleep = false; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + b3BoxHull box = b3MakeBoxHull( extent, extent, extent ); + + for ( int i = 0; i < baseCount; ++i ) + { + float y = ( 2.0f * i + 1.0f ) * extent; + + for ( int j = i; j < baseCount; ++j ) + { + float x = ( i + 1.0f ) * extent + 2.0f * ( j - i ) * extent + centerX - 0.5f; + bodyDef.position = (b3Vec3){ x, y, baseZ }; + + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + } +} + +void CreateManyPyramids( b3WorldId worldId ) +{ + int baseCount = 10; + float extent = 0.5f; + int rowCount = BENCHMARK_DEBUG ? 3 : 14; + int columnCount = BENCHMARK_DEBUG ? 3 : 14; + float groundExtent = extent * columnCount * ( baseCount + 1.0f ); + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = (b3Vec3){ 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + b3BoxHull box = b3MakeBoxHull( groundExtent, 1.0f, groundExtent ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + + float baseWidth = 2.0f * extent * baseCount; + float baseZ = -groundExtent + 2.0f * extent; + float deltaZ = 2.0f * ( groundExtent - 2.0f * extent ) / ( rowCount - 1.0f ); + + for ( int i = 0; i < rowCount; ++i ) + { + for ( int j = 0; j < columnCount; ++j ) + { + float centerX = -groundExtent + j * ( baseWidth + 2.0f * extent ) + 2.0f * extent; + CreateSmallPyramid( worldId, baseCount, extent, centerX, baseZ ); + } + + baseZ += deltaZ; + } +} + +#define RAIN_GRID_SIZE 15.0f +#define RAIN_LARGE_WORLD 0 +#ifdef NDEBUG +#if RAIN_LARGE_WORLD == 1 +#define RAIN_GRID_COUNT 700 +#define RAIN_GROUP_SIZE 1 +#else +#define RAIN_GRID_COUNT 10 +#define RAIN_GROUP_SIZE 3 +#endif + +#else + +#if RAIN_LARGE_WORLD == 1 +#define RAIN_GRID_COUNT 12 +#define RAIN_GROUP_SIZE 1 +#else +#define RAIN_GRID_COUNT 3 +#define RAIN_GROUP_SIZE 2 +#endif +#endif + +typedef struct Group +{ + Human humans[RAIN_GROUP_SIZE]; +} Group; + +typedef struct RainData +{ + Group* groups; + b3MeshData* gridMesh; + b3MeshData* torusMesh; + int columnCount; + int columnIndex; +} RainData; + +RainData g_rainData; + +void GetRainCapacity( b3Capacity* capacity ) +{ +#if RAIN_LARGE_WORLD == 1 +#ifdef NDEBUG + capacity->staticShapeCount = 1000 * 1024; + capacity->dynamicShapeCount = 2 * 1024; + capacity->staticBodyCount = 500 * 1024; + capacity->dynamicBodyCount = 2 * 1024; +#else + capacity->staticShapeCount = 512; + capacity->dynamicShapeCount = 128; + capacity->staticBodyCount = 256; + capacity->dynamicBodyCount = 128; +#endif +#endif +} + +void CreateRain( b3WorldId worldId ) +{ + memset( &g_rainData, 0, sizeof( g_rainData ) ); + + g_rainData.groups = malloc( RAIN_GRID_COUNT * RAIN_GRID_COUNT * sizeof( Group ) ); + memset( g_rainData.groups, 0, RAIN_GRID_COUNT * RAIN_GRID_COUNT * sizeof( Group ) ); + + int halfMeshGridRows = 4; + float meshGridCellWidth = RAIN_GRID_SIZE / ( 2.0f * halfMeshGridRows ); + g_rainData.gridMesh = b3CreateGridMesh( 2 * halfMeshGridRows, 2 * halfMeshGridRows, meshGridCellWidth, 1, true ); + g_rainData.torusMesh = b3CreateTorusMesh( 16, 16, 0.25f * RAIN_GRID_SIZE, 1.0f ); + + float span = RAIN_GRID_SIZE * RAIN_GRID_COUNT; + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + bodyDef.position.x = -0.5f * span + 0.5f * RAIN_GRID_SIZE; + for ( int i = 0; i < RAIN_GRID_COUNT; ++i ) + { + bodyDef.position.z = -0.5f * span + 0.5f * RAIN_GRID_SIZE; + for ( int j = 0; j < RAIN_GRID_COUNT; ++j ) + { + b3BodyId body = b3CreateBody( worldId, &bodyDef ); + b3CreateMeshShape( body, &shapeDef, g_rainData.gridMesh, b3Vec3_one ); + b3CreateMeshShape( body, &shapeDef, g_rainData.torusMesh, b3Vec3_one ); + + bodyDef.position.z += RAIN_GRID_SIZE; + } + + bodyDef.position.x += RAIN_GRID_SIZE; + } + + // b3World_SetJointTuning( worldId, 60.0f, 1.0f ); +} + +void DestroyRain( void ) +{ + b3DestroyMesh( g_rainData.gridMesh ); + b3DestroyMesh( g_rainData.torusMesh ); + + free( g_rainData.groups ); + g_rainData.groups = NULL; +} + +void CreateGroup( b3WorldId worldId, int rowIndex, int columnIndex ) +{ + assert( rowIndex < RAIN_GRID_COUNT && columnIndex < RAIN_GRID_COUNT ); + + int groupIndex = rowIndex * RAIN_GRID_COUNT + columnIndex; + + float span = RAIN_GRID_COUNT * RAIN_GRID_SIZE; + float groupDistance = 1.0f * span / RAIN_GRID_COUNT; + + b3Vec3 position; + position.x = -0.5f * span + groupDistance * ( columnIndex + 0.5f ); + position.y = 20.0f; + position.z = -0.5f * span + groupDistance * ( rowIndex + 0.5f ); + + float frictionTorque = 5.0f; + float hertz = 1.0f; + float dampingRatio = 0.7f; + bool colorize = false; + + for ( int i = 0; i < RAIN_GROUP_SIZE; ++i ) + { + Human* human = g_rainData.groups[groupIndex].humans + i; + CreateHuman( human, worldId, position, frictionTorque, hertz, dampingRatio, groupIndex, NULL, colorize ); + position.x += 0.75f; + } +} + +void DestroyGroup( int rowIndex, int columnIndex ) +{ + assert( rowIndex < RAIN_GRID_COUNT && columnIndex < RAIN_GRID_COUNT ); + + int groupIndex = rowIndex * RAIN_GRID_COUNT + columnIndex; + + for ( int i = 0; i < RAIN_GROUP_SIZE; ++i ) + { + DestroyHuman( g_rainData.groups[groupIndex].humans + i ); + } +} + +void StepRain( b3WorldId worldId, int stepCount ) +{ + int delay = BENCHMARK_DEBUG ? 0x7F : 0x2F; + int increment = RAIN_LARGE_WORLD ? 100 : 1; + + if ( ( stepCount & delay ) == 0 ) + { + if ( g_rainData.columnCount < RAIN_GRID_COUNT ) + { + for ( int i = 0; i < RAIN_GRID_COUNT; i += increment ) + { + CreateGroup( worldId, i, g_rainData.columnCount ); + } + + g_rainData.columnCount = b3MinInt( g_rainData.columnCount + increment, RAIN_GRID_COUNT ); + } + else + { + for ( int i = 0; i < RAIN_GRID_COUNT; i += increment ) + { + DestroyGroup( i, g_rainData.columnIndex ); + CreateGroup( worldId, i, g_rainData.columnIndex ); + } + + g_rainData.columnIndex = g_rainData.columnIndex + increment; + if ( g_rainData.columnIndex >= RAIN_GRID_COUNT ) + { + g_rainData.columnIndex = 0; + } + } + } +} + +void GetWasherCapacity( b3Capacity* capacity ) +{ + capacity->staticShapeCount = 16; + capacity->dynamicShapeCount = 10000; + capacity->staticBodyCount = 16; + capacity->dynamicBodyCount = 10000; + capacity->contactCount = 60000; +} + +void CreateWasher( b3WorldId worldId ) +{ + bool kinematic = true; + + b3BodyId groundId; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position.y = -1.0f; + groundId = b3CreateBody( worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 60.0f, 1.0f, 60.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + + { + float motorSpeed = 25.0f; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = (b3Vec3){ 0.0f, 21.0f, 0.0f }; + + if ( kinematic == true ) + { + bodyDef.type = b3_kinematicBody; + bodyDef.angularVelocity = (b3Vec3){ 0.0f, 0.0f, ( B3_PI / 180.0f ) * motorSpeed }; + bodyDef.linearVelocity = (b3Vec3){ 0.001f, -0.002f, 0.0f }; + } + else + { + bodyDef.type = b3_dynamicBody; + } + + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + float r0 = 14.0f; + float r1 = 16.0f; + float r2 = 18.0f; + b3Vec3 nd = { 0.0f, 0.0f, -10.0f }; + b3Vec3 pd = { 0.0f, 0.0f, 10.0f }; + + float angle = B3_PI / 18.0f; + b3Quat q = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, angle ); + b3Quat qo = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.1f * angle ); + b3Vec3 u1 = { 1.0f, 0.0f, 0.0f }; + for ( int i = 0; i < 36; ++i ) + { + b3Vec3 u2; + if ( i == 35 ) + { + u2 = (b3Vec3){ 1.0f, 0.0f, 0.0f }; + } + else + { + u2 = b3RotateVector( q, u1 ); + } + + { + b3Vec3 a1 = b3InvRotateVector( qo, u1 ); + b3Vec3 a2 = b3RotateVector( qo, u2 ); + b3Vec3 p1 = b3MulAdd( nd, r1, a1 ); + b3Vec3 p2 = b3MulAdd( nd, r2, a1 ); + b3Vec3 p3 = b3MulAdd( nd, r1, a2 ); + b3Vec3 p4 = b3MulAdd( nd, r2, a2 ); + b3Vec3 p5 = b3MulAdd( pd, r1, a1 ); + b3Vec3 p6 = b3MulAdd( pd, r2, a1 ); + b3Vec3 p7 = b3MulAdd( pd, r1, a2 ); + b3Vec3 p8 = b3MulAdd( pd, r2, a2 ); + + b3Vec3 points[8] = { p1, p2, p3, p4, p5, p6, p7, p8 }; + b3Hull* hull = b3CreateHull( points, 8, 8 ); + b3CreateHullShape( bodyId, &shapeDef, hull ); + b3DestroyHull( hull ); + } + + if ( i % 9 == 0 ) + { + b3Vec3 p1 = b3MulAdd( nd, r0, u1 ); + b3Vec3 p2 = b3MulAdd( nd, r1, u1 ); + b3Vec3 p3 = b3MulAdd( nd, r0, u2 ); + b3Vec3 p4 = b3MulAdd( nd, r1, u2 ); + b3Vec3 p5 = b3MulAdd( pd, r0, u1 ); + b3Vec3 p6 = b3MulAdd( pd, r1, u1 ); + b3Vec3 p7 = b3MulAdd( pd, r0, u2 ); + b3Vec3 p8 = b3MulAdd( pd, r1, u2 ); + + b3Vec3 points[8] = { p1, p2, p3, p4, p5, p6, p7, p8 }; + b3Hull* hull = b3CreateHull( points, 8, 8 ); + b3CreateHullShape( bodyId, &shapeDef, hull ); + b3DestroyHull( hull ); + } + + u1 = u2; + } + + if ( kinematic == false ) + { + b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = bodyId; + jointDef.base.localFrameA.p.y = 10.0f; + jointDef.motorSpeed = ( B3_PI / 180.0f ) * motorSpeed; + jointDef.maxMotorTorque = 1e8f; + jointDef.enableMotor = true; + + b3CreateRevoluteJoint( worldId, &jointDef ); + } + } + + int gridCount = BENCHMARK_DEBUG ? 8 : 20; + float a = 0.2f; + + b3BoxHull cube = b3MakeBoxHull( a, a, a ); + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + float x = -2.0f * a * gridCount; + for ( int i = 0; i < gridCount; ++i ) + { + float y = -2.0f * a * gridCount + 21.0f; + for ( int j = 0; j < gridCount; ++j ) + { + float z = -2.0f * a * gridCount; + for ( int k = 0; k < gridCount; ++k ) + { + bodyDef.position = (b3Vec3){ x, y, z }; + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + b3CreateHullShape( bodyId, &shapeDef, &cube.base ); + z += 4.0f * a; + } + + y += 4.0f * a; + } + + x += 4.0f * a; + } +} + +struct +{ + b3MeshData* meshData; +} g_compoundCapsulesData; + +void CreateCompoundCapsules( b3WorldId worldId ) +{ + memset( &g_compoundCapsulesData, 0, sizeof( g_compoundCapsulesData ) ); + + // float tilt = 0.15f * B3_PI; + float tilt = 0.0f * B3_PI; + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = (b3Vec3){ 0.0f, -1.0f, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( (b3Vec3){ 1.0f, 0.0f, 0.0 }, tilt ); + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + + int xCount = 200; + int zCount = 400; + + float cellWidth = 1.0f; + float amplitude = 0.4f; + float rowHz = 0.05f; + float columnHz = 0.1f; + + g_compoundCapsulesData.meshData = b3CreateWaveMesh( xCount, zCount, cellWidth, amplitude, rowHz, columnHz ); + + // g_compoundCapsulesData.meshData = b3CreateWaveMesh( xCount, zCount, 1.0f, 2.5f, 0.05f, 0.01f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateMeshShape( groundId, &shapeDef, g_compoundCapsulesData.meshData, b3Vec3_one ); + + bodyDef.rotation = b3Quat_identity; + bodyDef.type = b3_dynamicBody; + bodyDef.sleepThreshold = 0.2f; + + int bodyCount = BENCHMARK_DEBUG ? 10 : 90; + + shapeDef.baseMaterial.friction = 0.9f; + shapeDef.baseMaterial.rollingResistance = 0.2f; + shapeDef.updateBodyMass = false; + shapeDef.density = 1.0f; + + float angularVelocity = -0.5f; + float z = BENCHMARK_DEBUG ? -15.0f : -140.0f; + b3CosSin cs = b3ComputeCosSin( tilt ); + float yTilt = cs.sine / cs.cosine; + //b3CosSin stubCS = b3ComputeCosSin( 50.0f * B3_PI / 180.0f ); + for ( int bodyIndex = 0; bodyIndex < bodyCount; ++bodyIndex ) + { + bodyDef.position = (b3Vec3){ 0.0, 0.5f - z * yTilt, z }; + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + float y = 1.0f; + float r = 0.75f; + float l = 1.5f; + float offset = 0.05f; + //float stubX = 1.4f; + //float stubZ = 0.0f; + for ( int shapeIndex = 0; shapeIndex < 22; ++shapeIndex ) + { + b3Capsule capsule = { { offset, y, 0.0f }, { 0.0f, y + l, -offset }, r }; + b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + +#if 0 + if ( ( shapeIndex & 1 ) == 0 ) + { + b3Capsule stub = { { 0.0f, y + 0.5f * l, 0.0f }, { 1.4f * r, y + 0.5f * l, 0.0f }, 0.2f }; + b3CreateCapsuleShape( bodyId, &shapeDef, &stub ); + } + else + { + } +#elif 0 + b3Capsule stub = { { 0.0f, y + 0.5f * l, 0.0f }, { stubX * r, y + 0.5f * l, stubZ * r }, 0.2f }; + b3CreateCapsuleShape( bodyId, &shapeDef, &stub ); + stubCS = b3ComputeCosSin( B3_PI * RandomFloat() ); + float sx = stubX * stubCS.cosine - stubZ * stubCS.sine; + float sz = stubX * stubCS.sine + stubZ * stubCS.cosine; + stubX = sx; + stubZ = sz; +#endif + y += l + 2.0f * r; + r = 0.95f * r; + offset = -offset; + } + + float velocityScale = 0.5f + ( 0.5f * bodyIndex ) / bodyCount; + b3Body_ApplyMassFromShapes( bodyId ); + b3Vec3 center = b3Body_GetWorldCenterOfMass( bodyId ); + b3Vec3 omega = { 0.0f, 0.0f, velocityScale * angularVelocity }; + b3Vec3 v = b3Cross( omega, b3Sub( center, bodyDef.position ) ); + b3Body_SetAngularVelocity( bodyId, omega ); + b3Body_SetLinearVelocity( bodyId, v ); + + z += 3.0f; + angularVelocity = -angularVelocity; + } +} + +void DestroyCompoundCapsules( void ) +{ + b3DestroyMesh( g_compoundCapsulesData.meshData ); + memset( &g_compoundCapsulesData, 0, sizeof( g_compoundCapsulesData ) ); +} + +struct +{ + b3MeshData* meshData; +} g_compoundHullsData; + +void CreateCompoundHulls( b3WorldId worldId ) +{ + memset( &g_compoundHullsData, 0, sizeof( g_compoundHullsData ) ); + + // float tilt = 0.15f * B3_PI; + float tilt = 0.0f * B3_PI; + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = (b3Vec3){ 0.0f, 0.0f, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( (b3Vec3){ 1.0f, 0.0f, 0.0 }, tilt ); + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + + int xCount = 200; + // int zCount = BENCHMARK_DEBUG ? 100 : 400; + int zCount = 400; + + float cellWidth = 1.0f; + float amplitude = 0.4f; + float rowHz = 0.05f; + float columnHz = 0.1f; + + g_compoundHullsData.meshData = b3CreateWaveMesh( xCount, zCount, cellWidth, amplitude, rowHz, columnHz ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateMeshShape( groundId, &shapeDef, g_compoundHullsData.meshData, b3Vec3_one ); + + bodyDef.type = b3_dynamicBody; + bodyDef.sleepThreshold = 0.2f; + bodyDef.rotation = b3Quat_identity; + + int bodyCount = BENCHMARK_DEBUG ? 10 : 90; + + shapeDef.baseMaterial.friction = 0.9f; + shapeDef.baseMaterial.rollingResistance = 0.05f; + shapeDef.updateBodyMass = false; + shapeDef.density = 1.0f; + + int hullCount = 22; + b3Hull* hulls[22] = { 0 }; + + float y = 1.0f; + float r = 0.75f; + float l = 1.5f; + for ( int i = 0; i < hullCount; ++i ) + { + hulls[i] = b3CreateCylinder( l + 2.0f * r, r, y - r, 6 ); + y += l + 2.0f * r; + r = 0.95f * r; + } + + float angularVelocity = -0.5f; + float z = BENCHMARK_DEBUG ? -15.0f : -140.0f; + b3CosSin cs = b3ComputeCosSin( tilt ); + float yTilt = cs.sine / cs.cosine; + for ( int bodyIndex = 0; bodyIndex < bodyCount; ++bodyIndex ) + { + bodyDef.position = (b3Vec3){ 0.0, 1.0f - z * yTilt, z }; + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + for ( int shapeIndex = 0; shapeIndex < 22; ++shapeIndex ) + { + // Random rotation provided no benefit to behavior. + // float angle = B3_PI * RandomFloat(); + // b3Transform xf; + // xf.p = b3Vec3_zero; + // xf.q = b3MakeQuatFromAxisAngle( (b3Vec3){ 0.0f, 1.0f, 0.0f }, angle ); + // b3Vec3 scale = {1.0f, 1.0f, 1.0f}; + // b3CreateTransformedHullShape( bodyId, &shapeDef, hulls[shapeIndex], xf, scale ); + b3CreateHullShape( bodyId, &shapeDef, hulls[shapeIndex] ); + } + + float velocityScale = 0.5f + ( 0.5f * bodyIndex ) / bodyCount; + b3Body_ApplyMassFromShapes( bodyId ); + b3Vec3 center = b3Body_GetWorldCenterOfMass( bodyId ); + b3Vec3 omega = { 0.0f, 0.0f, velocityScale * angularVelocity }; + b3Vec3 v = b3Cross( omega, b3Sub( center, bodyDef.position ) ); + b3Body_SetAngularVelocity( bodyId, omega ); + b3Body_SetLinearVelocity( bodyId, v ); + + z += 3.0f; + angularVelocity = -angularVelocity; + } + + for ( int i = 0; i < hullCount; ++i ) + { + b3DestroyHull( hulls[i] ); + } +} + +void DestroyCompoundHulls( void ) +{ + b3DestroyMesh( g_compoundHullsData.meshData ); + memset( &g_compoundHullsData, 0, sizeof( g_compoundHullsData ) ); +} + +struct +{ + b3MeshData* meshData; +} g_weldedHullsData; + +void CreateWeldedHulls( b3WorldId worldId ) +{ + memset( &g_weldedHullsData, 0, sizeof( g_weldedHullsData ) ); + + float tilt = 0.0f * B3_PI; + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = (b3Vec3){ 0.0f, 0.0f, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( (b3Vec3){ 1.0f, 0.0f, 0.0 }, tilt ); + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + + int xCount = 200; + // int zCount = BENCHMARK_DEBUG ? 100 : 400; + int zCount = 400; + + float cellWidth = 1.0f; + float amplitude = 0.4f; + float rowHz = 0.05f; + float columnHz = 0.1f; + + g_weldedHullsData.meshData = b3CreateWaveMesh( xCount, zCount, cellWidth, amplitude, rowHz, columnHz ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateMeshShape( groundId, &shapeDef, g_weldedHullsData.meshData, b3Vec3_one ); + + int entityCount = BENCHMARK_DEBUG ? 10 : 90; + + int shapeCount = 22; + b3Hull* hulls[22] = { 0 }; + + float y = 1.0f; + float r = 0.75f; + float l = 1.5f; + for ( int i = 0; i < shapeCount; ++i ) + { + hulls[i] = b3CreateCylinder( l + 2.0f * r, r, y - r, 6 ); + y += l + 2.0f * r; + r = 0.95f * r; + } + + b3WeldJointDef jointDef = b3DefaultWeldJointDef(); + jointDef.angularHertz = 6.0f; + jointDef.angularDampingRatio = 2.0f; + + float angularVelocity = -0.5f; + float z = BENCHMARK_DEBUG ? -15.0f : -140.0f; + b3CosSin cs = b3ComputeCosSin( tilt ); + float yTilt = cs.sine / cs.cosine; + + int shapesPerBody = 5; + + shapeDef.baseMaterial.friction = 0.9f; + shapeDef.baseMaterial.rollingResistance = 0.1f; + shapeDef.updateBodyMass = false; + shapeDef.density = 1.0f; + + bodyDef.type = b3_dynamicBody; + bodyDef.sleepThreshold = 0.2f; + + for ( int entityIndex = 0; entityIndex < entityCount; ++entityIndex ) + { + b3BodyId prevBodyId = b3_nullBodyId; + + y = 1.0f; + r = 0.75f; + b3Vec3 base = (b3Vec3){ 0.0, 1.0f - z * yTilt, z }; + bodyDef.position = base; + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + float velocityScale = 0.5f + ( 0.5f * entityIndex ) / entityCount; + + for ( int i = 0; i < shapeCount; ++i ) + { + b3CreateHullShape( bodyId, &shapeDef, hulls[i] ); + + if ( ( i + 1 ) % shapesPerBody == 0 || i == shapeCount - 1 ) + { + b3Body_ApplyMassFromShapes( bodyId ); + + b3Vec3 center = b3Body_GetWorldCenterOfMass( bodyId ); + b3Vec3 omega = { 0.0f, 0.0f, angularVelocity * velocityScale }; + b3Vec3 v = b3Cross( omega, b3Sub( center, base ) ); + b3Body_SetAngularVelocity( bodyId, omega ); + b3Body_SetLinearVelocity( bodyId, v ); + + if ( i < shapeCount - 1 ) + { + prevBodyId = bodyId; + + if ( i < shapeCount - 1 ) + { + bodyId = b3CreateBody( worldId, &bodyDef ); + + if ( B3_IS_NON_NULL( prevBodyId ) ) + { + jointDef.base.bodyIdA = prevBodyId; + jointDef.base.bodyIdB = bodyId; + jointDef.base.localFrameA.p = (b3Vec3){ 0.0f, y + l + r, 0.0f }; + jointDef.base.localFrameB.p = (b3Vec3){ 0.0f, y + l + r, 0.0f }; + + b3CreateWeldJoint( worldId, &jointDef ); + } + + velocityScale *= 0.75f; + } + } + } + + y += l + 2.0f * r; + r = 0.95f * r; + } + + z += 3.0f; + angularVelocity = -angularVelocity; + } + + for ( int i = 0; i < shapeCount; ++i ) + { + b3DestroyHull( hulls[i] ); + } +} + +void DestroyWeldedHulls( void ) +{ + b3DestroyMesh( g_weldedHullsData.meshData ); + memset( &g_weldedHullsData, 0, sizeof( g_weldedHullsData ) ); +} + +struct JunkyardData +{ + b3BodyId pusherId; + float degrees; + float radius; +} g_junkyardData; + +void CreateJunkyard( b3WorldId worldId ) +{ + b3BodyId groundId; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position.y = -1.0f; + groundId = b3CreateBody( worldId, &bodyDef ); + } + + { + b3ShapeDef shapeDef = b3DefaultShapeDef(); + { + b3BoxHull box = b3MakeBoxHull( 120.0f, 1.0f, 120.0f ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + { + b3Vec3 offset = { -50.0f, 8.0f, 0.0f }; + b3BoxHull box = b3MakeOffsetBoxHull( 1.0f, 8.0f, 50.0f, offset ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + { + b3Vec3 offset = { 50.0f, 8.0f, 0.0f }; + b3BoxHull box = b3MakeOffsetBoxHull( 1.0f, 8.0f, 50.0f, offset ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + { + b3Vec3 offset = { 0.0f, 8.0f, -50.0f }; + b3BoxHull box = b3MakeOffsetBoxHull( 50.0f, 8.0f, 1.0f, offset ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + { + b3Vec3 offset = { 0.0f, 8.0f, 50.0f }; + b3BoxHull box = b3MakeOffsetBoxHull( 50.0f, 8.0f, 1.0f, offset ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + } + { + b3Hull* hull; + { + float radius = 1.5f; + int pointCount = 10; + + // Golden ratio + const float phi = ( 1.0f + sqrtf( 5.0f ) ) / 2.0f; + + // Random points on sphere (Fibonacci lattice) + b3Vec3 points[10]; + for ( int i = 0; i < pointCount; ++i ) + { + float Theta = 2.0f * B3_PI * i / phi; // Azimuthal angle + float Z = 1.0f - ( 2.0f * i + 1.0f ) / pointCount; // Z coordinate + float Radius_XY = sqrtf( 1.0f - Z * Z ); // Radius in xy-plane + + points[i].x = radius * Radius_XY * cosf( Theta ); + points[i].y = radius * Radius_XY * sinf( Theta ); + points[i].z = radius * Z; + } + + hull = b3CreateHull( points, pointCount, pointCount ); + } + + int count = BENCHMARK_DEBUG ? 2 : 24; + float height = 24.0f; + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + for ( int Y = 0; Y < count; ++Y ) + { + for ( int X = 0; X <= 20; ++X ) + { + for ( int Z = 0; Z <= 20; ++Z ) + { + bodyDef.position.x = -40.0f + 4.0f * X; + bodyDef.position.y = 4.0f * Y + height + 1.0f; + bodyDef.position.z = -40.0f + 4.0f * Z; + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, hull ); + } + } + } + + b3DestroyHull( hull ); + } + + g_junkyardData.radius = 35.0f; + float mHeight = 24.0f; + + b3Hull* hull = b3CreateCylinder( mHeight, 4.0f, 0.0f, 16 ); + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_kinematicBody; + bodyDef.position = (b3Vec3){ g_junkyardData.radius, 0.0f, 0.0f }; + g_junkyardData.pusherId = b3CreateBody( worldId, &bodyDef ); + g_junkyardData.degrees = 0.0f; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( g_junkyardData.pusherId, &shapeDef, hull ); + b3DestroyHull( hull ); +} + +void StepJunkyard( b3WorldId worldId, int stepCount ) +{ + (void)worldId; + (void)stepCount; + + float timeStep = 1.0f / 60.0f; + const float omega = -6.0f; + g_junkyardData.degrees += omega * timeStep; + b3CosSin cs = b3ComputeCosSin( g_junkyardData.degrees * B3_PI / 180.0f ); + float r = g_junkyardData.radius; + b3Vec3 targetPos = { r * cs.cosine, 0.0f, r * cs.sine }; + b3Transform target = { .p = targetPos, .q = b3Quat_identity }; + b3Body_SetTargetTransform( g_junkyardData.pusherId, target, timeStep, false ); +} diff --git a/shared/benchmarks.h b/shared/benchmarks.h new file mode 100644 index 0000000..8f07af5 --- /dev/null +++ b/shared/benchmarks.h @@ -0,0 +1,47 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT +#pragma once + +#include "box3d/id.h" + +#include + +// This allows benchmarks to be tested on the benchmark app and also visualized in the samples app + +typedef struct b3Capacity b3Capacity; + +#ifdef __cplusplus +extern "C" +{ +#endif + +void CreateCompoundCapsules( b3WorldId worldId ); +void DestroyCompoundCapsules( void ); +void CreateCompoundHulls( b3WorldId worldId ); +void DestroyCompoundHulls( void ); +void CreateJointGrid( b3WorldId worldId ); +void CreateJunkyard( b3WorldId worldId ); +void StepJunkyard( b3WorldId worldId, int stepCount ); +void CreateLargePyramid( b3WorldId worldId ); +//void CreateLargeWorld( b3WorldId worldId ); +//void StepLargeWorld( b3WorldId worldId, int stepCount ); +//void DestroyLargeWorld( void ); +void CreateWidePyramid( b3WorldId worldId ); +void CreateManyPyramids( b3WorldId worldId ); +void GetRainCapacity( b3Capacity* capacity ); +void CreateRain( b3WorldId worldId ); +void StepRain( b3WorldId worldId, int stepCount ); +void DestroyRain( void ); +void GetWasherCapacity( b3Capacity* capacity ); +void CreateWasher( b3WorldId worldId ); +void CreateWeldedHulls( b3WorldId worldId ); +void DestroyWeldedHulls( void ); + +// void CreateSpinner( b3WorldId worldId ); +// float StepSpinner( b3WorldId worldId, int stepCount ); +// void CreateSmash( b3WorldId worldId ); +// void CreateTumbler( b3WorldId worldId ); + +#ifdef __cplusplus +} +#endif diff --git a/shared/determinism.c b/shared/determinism.c new file mode 100644 index 0000000..011cac1 --- /dev/null +++ b/shared/determinism.c @@ -0,0 +1,124 @@ +// SPDX-FileCopyrightText: 2022 Erin Catto +// SPDX-License-Identifier: MIT + +#include "determinism.h" + +#include "human.h" + +#include "box3d/box3d.h" + +#include +#include + +#define GRID_SIZE 15.0f + +static void CreateGroup( FallingRagdollData* data, b3WorldId worldId, int rowIndex, int columnIndex ) +{ + assert( rowIndex < RAGDOLL_GRID_COUNT && columnIndex < RAGDOLL_GRID_COUNT ); + + int groupIndex = rowIndex * RAGDOLL_GRID_COUNT + columnIndex; + + float span = RAGDOLL_GRID_COUNT * GRID_SIZE; + float groupDistance = 1.0f * span / RAGDOLL_GRID_COUNT; + + b3Vec3 position; + position.x = -0.5f * span + groupDistance * ( columnIndex + 0.5f ); + position.y = 15.0f; + position.z = -0.5f * span + groupDistance * ( rowIndex + 0.5f ); + + float frictionTorque = 5.0f; + float hertz = 1.0f; + float dampingRatio = 0.7f; + bool colorize = false; + + for ( int i = 0; i < RAGDOLL_GROUP_SIZE; ++i ) + { + Human* human = data->groups[groupIndex].humans + i; + CreateHuman( human, worldId, position, frictionTorque, hertz, dampingRatio, groupIndex, NULL, colorize ); + position.x += 0.75f; + } +} + +FallingRagdollData CreateFallingRagdolls( b3WorldId worldId ) +{ + FallingRagdollData data = { 0 }; + + int halfMeshGridRows = 4; + float meshGridCellWidth = GRID_SIZE / ( 2.0f * halfMeshGridRows ); + data.gridMesh = b3CreateGridMesh( 2 * halfMeshGridRows, 2 * halfMeshGridRows, meshGridCellWidth, 0, true ); + data.torusMesh = b3CreateTorusMesh( 16, 16, 0.25f * GRID_SIZE, 1.0f ); + + float span = GRID_SIZE * RAGDOLL_GRID_COUNT; + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + bodyDef.position.x = -0.5f * span + 0.5f * GRID_SIZE; + for ( int i = 0; i < RAGDOLL_GRID_COUNT; ++i ) + { + bodyDef.position.z = -0.5f * span + 0.5f * GRID_SIZE; + for ( int j = 0; j < RAGDOLL_GRID_COUNT; ++j ) + { + b3BodyId body = b3CreateBody( worldId, &bodyDef ); + b3CreateMeshShape( body, &shapeDef, data.gridMesh, b3Vec3_one ); + b3CreateMeshShape( body, &shapeDef, data.torusMesh, b3Vec3_one ); + + CreateGroup( &data, worldId, i, j ); + + bodyDef.position.z += GRID_SIZE; + } + + bodyDef.position.x += GRID_SIZE; + } + + return data; +} + +bool UpdateFallingRagdolls( b3WorldId worldId, FallingRagdollData* data ) +{ + if ( data->hash == 0 ) + { + b3BodyEvents bodyEvents = b3World_GetBodyEvents( worldId ); + + if ( bodyEvents.moveCount == 0 ) + { + int awakeCount = b3World_GetAwakeBodyCount( worldId ); + assert( awakeCount == 0 ); + + data->hash = B3_HASH_INIT; + for ( int i = 0; i < RAGDOLL_GRID_COUNT; ++i ) + { + for ( int j = 0; j < RAGDOLL_GRID_COUNT; ++j ) + { + for ( int k = 0; k < RAGDOLL_GROUP_SIZE; ++k ) + { + int groupIndex = i * RAGDOLL_GRID_COUNT + j; + + Human* human = data->groups[groupIndex].humans + k; + + for ( int b = 0; b < bone_count; ++b ) + { + b3BodyId bodyId = human->bones[b].bodyId; + b3Transform xf = b3Body_GetTransform( bodyId ); + data->hash = b3Hash( data->hash, (uint8_t*)( &xf ), sizeof( b3Transform ) ); + } + } + } + } + + data->sleepStep = data->stepCount; + } + } + + data->stepCount += 1; + + return data->hash != 0; +} + +void DestroyFallingRagdolls( FallingRagdollData* data ) +{ + b3DestroyMesh( data->gridMesh ); + b3DestroyMesh( data->torusMesh ); + + data->gridMesh = NULL; + data->torusMesh = NULL; +} diff --git a/shared/determinism.h b/shared/determinism.h new file mode 100644 index 0000000..0839fbf --- /dev/null +++ b/shared/determinism.h @@ -0,0 +1,41 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT +#pragma once + +#include "box3d/id.h" + +#include "human.h" +#include + +#ifdef __cplusplus +extern "C" +{ +#endif + +#define RAGDOLL_GROUP_SIZE 2 +#define RAGDOLL_GRID_COUNT 2 + +typedef struct RagdollGroup +{ + Human humans[RAGDOLL_GROUP_SIZE]; +} RagdollGroup; + +typedef struct FallingRagdollData +{ + RagdollGroup groups[RAGDOLL_GRID_COUNT * RAGDOLL_GRID_COUNT]; + b3MeshData* gridMesh; + b3MeshData* torusMesh; + int columnCount; + int columnIndex; + int stepCount; + int sleepStep; + uint32_t hash; +} FallingRagdollData; + +FallingRagdollData CreateFallingRagdolls( b3WorldId worldId ); +bool UpdateFallingRagdolls( b3WorldId worldId, FallingRagdollData* data ); +void DestroyFallingRagdolls( FallingRagdollData* data ); + +#ifdef __cplusplus +} +#endif diff --git a/shared/human.c b/shared/human.c new file mode 100644 index 0000000..ea87668 --- /dev/null +++ b/shared/human.c @@ -0,0 +1,775 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "human.h" + +#include "utils.h" + +#include "box3d/box3d.h" + +#include +#include + +void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float frictionTorque, float hertz, + float dampingRatio, int groupIndex, void* userData, bool colorize ) +{ + assert( human->isSpawned == false ); + + for ( int i = 0; i < bone_count; ++i ) + { + human->bones[i].bodyId = b3_nullBodyId; + human->bones[i].anchorId = b3_nullBodyId; + human->bones[i].jointId = b3_nullJointId; + human->bones[i].jointFriction = 1.0f; + human->bones[i].parentIndex = -1; + } + + for ( int i = 0; i < FILTER_JOINT_COUNT; ++i ) + { + human->filterJoints[i] = b3_nullJointId; + } + + human->frictionTorque = frictionTorque; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.userData = userData; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + // shapeDef.friction = 0.2f; + shapeDef.baseMaterial.rollingResistance = 0.2f; + + b3HexColor shirtColor = b3_colorMediumTurquoise; + b3HexColor pantColor = b3_colorDodgerBlue; + b3HexColor skinColors[4] = { b3_colorNavajoWhite, b3_colorLightYellow, b3_colorPeru, b3_colorTan }; + b3HexColor skinColor = skinColors[groupIndex % 4]; + + { + Bone* bone = human->bones + bone_pelvis; + + bone->parentIndex = -1; + + bodyDef.name = "pelvis"; + bone->referenceFrame = ( b3Transform ){ { 0.0f, 0.932087f, -0.051708f }, { { 0.739169f, 0.0f, 0.0f }, 0.673520f } }; + bodyDef.rotation = bone->referenceFrame.q; + bodyDef.position = b3Add( bone->referenceFrame.p, position ); + bone->bodyId = b3CreateBody( worldId, &bodyDef ); + + b3Capsule capsule = { { 0.07f, 0.0f, -0.08f }, { -0.07f, 0.0f, -0.08f }, 0.13f }; + shapeDef.filter.groupIndex = 0; + shapeDef.baseMaterial.customColor = colorize ? pantColor : 0; + + b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + } + + { + Bone* bone = human->bones + bone_spine_01; + bone->parentIndex = bone_pelvis; + + bodyDef.name = "spine_01"; + bone->referenceFrame = ( b3Transform ){ { 0.0f, 1.113505f, -0.03481f }, { { 0.739973f, 0.0f, 0.0f }, 0.672637f } }; + bodyDef.rotation = bone->referenceFrame.q; + bodyDef.position = b3Add( bone->referenceFrame.p, position ); + //bodyDef.type = b3_staticBody; + bone->bodyId = b3CreateBody( worldId, &bodyDef ); + bodyDef.type = b3_dynamicBody; + + b3Capsule capsule = { { 0.06f, -0.0f, -0.052264f }, { -0.06f, 0.0f, -0.052264f }, 0.12f }; + shapeDef.filter.groupIndex = -groupIndex; + shapeDef.baseMaterial.customColor = colorize ? shirtColor : 0; + b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + bone->jointType = b3_sphericalJoint; + bone->localFrameA = + ( b3Transform ){ { 0.000000, 0.000000, -0.182204 }, { { -0.999999, 0.000000, -0.000000 }, 0.001194 } }; + bone->localFrameB = + ( b3Transform ){ { 0.000000, 0.000000, -0.007736 }, { { -1.000000, 0.000000, -0.000000 }, 0.000000 } }; + bone->swingLimit = 25.0f * B3_DEG_TO_RAD; + bone->twistLimit = ( b3Vec2 ){ -15.0f * B3_DEG_TO_RAD, 15.0f * B3_DEG_TO_RAD }; + } + + { + Bone* bone = human->bones + bone_spine_02; + bone->parentIndex = bone_spine_01; + + // bodyDef.name = "spine_02"; + bone->referenceFrame = ( b3Transform ){ { 0.0f, 1.194336f, -0.027087f }, { { 0.703611f, 0.0f, 0.0f }, 0.710586f } }; + bodyDef.rotation = bone->referenceFrame.q; + bodyDef.position = b3Add( bone->referenceFrame.p, position ); + bone->bodyId = b3CreateBody( worldId, &bodyDef ); + + b3Capsule capsule = { { 0.08f, -0.015133f, -0.091801f }, { -0.08f, -0.015133f, -0.091801f }, 0.10f }; + shapeDef.filter.groupIndex = 0; + shapeDef.baseMaterial.customColor = colorize ? shirtColor : 0; + b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + bone->jointType = b3_sphericalJoint; + bone->localFrameA = + ( b3Transform ){ { 0.000000, -0.000000, -0.088935 }, { { -0.998619, -0.000000, 0.000000 }, -0.052540 } }; + bone->localFrameB = + ( b3Transform ){ { -0.000000, 0.000000, -0.008199 }, { { -1.000000, 0.000000, -0.000000 }, 0.000000 } }; + bone->swingLimit = 25.0f * B3_DEG_TO_RAD; + bone->twistLimit = ( b3Vec2 ){ -15.0f * B3_DEG_TO_RAD, 15.0f * B3_DEG_TO_RAD }; + } + + { + Bone* bone = human->bones + bone_spine_03; + bone->parentIndex = bone_spine_02; + + bodyDef.name = "spine_03"; + bone->referenceFrame = + ( b3Transform ){ { -0.0f, 1.31043f, -0.028232f }, { { 0.669856f, 0.000001f, -0.000001f }, 0.742491f } }; + bodyDef.rotation = bone->referenceFrame.q; + bodyDef.position = b3Add( bone->referenceFrame.p, position ); + bone->bodyId = b3CreateBody( worldId, &bodyDef ); + + b3Capsule capsule = { { 0.11f, -0.039753f, -0.13f }, { -0.11f, -0.039753f, -0.13f }, 0.145f }; + shapeDef.filter.groupIndex = 0; + shapeDef.baseMaterial.customColor = colorize ? shirtColor : 0; + b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + bone->jointType = b3_sphericalJoint; + bone->localFrameA = + ( b3Transform ){ { -0.000000, 0.000000, -0.124298 }, { { -0.998921, 0.000001, -0.000001 }, -0.046434 } }; + bone->localFrameB = ( b3Transform ){ { 0.000000, 0.000000, 0.000000 }, { { -1.000000, 0.000000, -0.000001 }, 0.000000 } }; + bone->swingLimit = 15.0f * B3_DEG_TO_RAD; + bone->twistLimit = ( b3Vec2 ){ -10.0f * B3_DEG_TO_RAD, 10.0f * B3_DEG_TO_RAD }; + } + + { + Bone* bone = human->bones + bone_neck; + bone->parentIndex = bone_spine_03; + + bodyDef.name = "neck"; + bone->referenceFrame = ( b3Transform ){ { 0.0f, 1.575582f, -0.055837f }, { { 0.879922f, 0.0f, 0.0f }, 0.475118f } }; + bodyDef.rotation = bone->referenceFrame.q; + bodyDef.position = b3Add( bone->referenceFrame.p, position ); + bone->bodyId = b3CreateBody( worldId, &bodyDef ); + + b3Capsule capsule = { { -0.000001f, -0.0f, -0.02f }, { 0.0f, -0.005f, -0.08f }, 0.07f }; + shapeDef.filter.groupIndex = 0; + shapeDef.baseMaterial.customColor = colorize ? skinColor : 0; + b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + bone->jointType = b3_sphericalJoint; + bone->localFrameA = + ( b3Transform ){ { 0.000001, -0.000259, -0.266585 }, { { -0.942192, -0.000001, 0.000000 }, 0.335074 } }; + bone->localFrameB = ( b3Transform ){ { 0.000000, 0.000000, 0.000000 }, { { -1.000000, 0.000000, -0.000001 }, 0.000000 } }; + bone->swingLimit = 45.0f * B3_DEG_TO_RAD; + bone->twistLimit = ( b3Vec2 ){ -15.0f * B3_DEG_TO_RAD, 15.0f * B3_DEG_TO_RAD }; + bone->jointFriction = 0.8f; + } + + { + Bone* bone = human->bones + bone_head; + bone->parentIndex = bone_neck; + + bodyDef.name = "head"; + bone->referenceFrame = ( b3Transform ){ { 0.0f, 1.653348f, -0.003241f }, { { 0.750288f, 0.0f, 0.0f }, 0.661111f } }; + bodyDef.rotation = bone->referenceFrame.q; + bodyDef.position = b3Add( bone->referenceFrame.p, position ); + bone->bodyId = b3CreateBody( worldId, &bodyDef ); + + b3Capsule capsule = { { -0.000001f, 0.016892f, -0.05869f }, { 0.0f, -0.003629f, -0.115072f }, 0.0975f }; + shapeDef.filter.groupIndex = 0; + shapeDef.baseMaterial.customColor = colorize ? skinColor : 0; + b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + bone->jointType = b3_sphericalJoint; + bone->localFrameA = + ( b3Transform ){ { 0.000000, 0.001321, -0.093873 }, { { -0.974301, -0.000000, -0.000000 }, -0.225251 } }; + bone->localFrameB = ( b3Transform ){ { 0.000000, 0.001268, -0.005104 }, { { -1.000000, 0.000000, -0.00000 }, 0.000000 } }; + bone->swingLimit = 15.0f * B3_DEG_TO_RAD; + bone->twistLimit = ( b3Vec2 ){ -15.0f * B3_DEG_TO_RAD, 15.0f * B3_DEG_TO_RAD }; + bone->jointFriction = 0.4f; + } + + { + Bone* bone = human->bones + bone_thigh_l; + bone->parentIndex = bone_pelvis; + + bodyDef.name = "thigh_l"; + bone->referenceFrame = + ( b3Transform ){ { 0.090416f, 0.986104f, -0.035090f }, { { -0.703287f, -0.070715f, 0.053866f }, 0.705327f } }; + bodyDef.rotation = bone->referenceFrame.q; + bodyDef.position = b3Add( bone->referenceFrame.p, position ); + bone->bodyId = b3CreateBody( worldId, &bodyDef ); + + b3Capsule capsule = { { 0.023719f, 0.006008f, -0.039068f }, { -0.064492f, -0.004664f, -0.424718f }, 0.09f }; + shapeDef.filter.groupIndex = -groupIndex; + shapeDef.baseMaterial.customColor = colorize ? pantColor : 0; + b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + bone->jointType = b3_sphericalJoint; + bone->localFrameA = + ( b3Transform ){ { 0.05f, 0.011537f, -0.055325f }, { { -0.714896, -0.022305, -0.698361 }, -0.026790 } }; + bone->localFrameB = ( b3Transform ){ { 0.0f, 0.0f, 0.0f }, { { -0.002064, 0.758987, 0.017046 }, 0.650880 } }; + bone->swingLimit = 10.0f * B3_DEG_TO_RAD; + bone->twistLimit = ( b3Vec2 ){ -60.0f * B3_DEG_TO_RAD, 40.0f * B3_DEG_TO_RAD }; + } + + { + Bone* bone = human->bones + bone_calf_l; + bone->parentIndex = bone_thigh_l; + + bodyDef.name = "calf_l"; + bone->referenceFrame = + ( b3Transform ){ { 0.101198f, 0.527027f, -0.037374f }, { { -0.653328f, -0.066860f, 0.058582f }, 0.751838f } }; + bodyDef.rotation = bone->referenceFrame.q; + bodyDef.position = b3Add( bone->referenceFrame.p, position ); + bone->bodyId = b3CreateBody( worldId, &bodyDef ); + + b3Capsule capsule = { { 0.001778f, 0.0f, 0.009841f }, { -0.078577f, 0.014707f, -0.41816f }, 0.075f }; + shapeDef.filter.groupIndex = 0; + shapeDef.baseMaterial.customColor = colorize ? pantColor : 0; + b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + bone->jointType = b3_revoluteJoint; + bone->localFrameA = + ( b3Transform ){ { -0.069989, 0.000253, -0.453844 }, { { -0.000677, 0.760087, 0.105674 }, 0.641171 } }; + bone->localFrameB = ( b3Transform ){ { 0.0f, 0.0f, 0.0f }, { { -0.044589, 0.765540, 0.053368 }, 0.639619 } }; + bone->twistLimit = ( b3Vec2 ){ -5.0f * B3_DEG_TO_RAD, 45.0f * B3_DEG_TO_RAD }; + } + + { + Bone* bone = human->bones + bone_thigh_r; + bone->parentIndex = bone_pelvis; + + bodyDef.name = "thigh_r"; + bone->referenceFrame = + ( b3Transform ){ { -0.090416f, 0.986104f, -0.03509f }, { { -0.703287f, 0.070715f, -0.053865f }, 0.705326f } }; + bodyDef.rotation = bone->referenceFrame.q; + bodyDef.position = b3Add( bone->referenceFrame.p, position ); + bone->bodyId = b3CreateBody( worldId, &bodyDef ); + + b3Capsule capsule = { { -0.023719f, 0.006008f, -0.039068f }, { 0.064492f, -0.004664f, -0.424718f }, 0.09f }; + shapeDef.filter.groupIndex = -groupIndex; + shapeDef.baseMaterial.customColor = colorize ? pantColor : 0; + b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + bone->jointType = b3_sphericalJoint; + bone->localFrameA = ( b3Transform ){ { -0.05, 0.011537, -0.055326 }, { { -0.039089, -0.714094, 0.043177 }, 0.697623 } }; + bone->localFrameB = ( b3Transform ){ { 0.0f, 0.0f, 0.0f }, { { 0.758805, -0.019886, -0.651012 }, -0.001759 } }; + bone->swingLimit = 10.0f * B3_DEG_TO_RAD; + bone->twistLimit = ( b3Vec2 ){ -30.0f * B3_DEG_TO_RAD, 60.0f * B3_DEG_TO_RAD }; + } + + { + Bone* bone = human->bones + bone_calf_r; + bone->parentIndex = bone_thigh_r; + + bodyDef.name = "calf_r"; + bone->referenceFrame = + ( b3Transform ){ { -0.101198f, 0.527027f, -0.037373f }, { { -0.653327f, 0.06686f, -0.058582f }, 0.751839f } }; + bodyDef.rotation = bone->referenceFrame.q; + bodyDef.position = b3Add( bone->referenceFrame.p, position ); + bone->bodyId = b3CreateBody( worldId, &bodyDef ); + + b3Capsule capsule = { { -0.001820f, 0.0f, 0.010071f }, { 0.077883f, 0.014825f, -0.418047f }, 0.075f }; + shapeDef.filter.groupIndex = 0; + shapeDef.baseMaterial.customColor = colorize ? pantColor : 0; + b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + bone->jointType = b3_revoluteJoint; + bone->localFrameA = + ( b3Transform ){ { 0.069988, 0.000253, -0.453844 }, { { 0.760086, -0.000675, -0.641171 }, -0.105676 } }; + bone->localFrameB = ( b3Transform ){ { 0.0f, 0.0f, 0.0f }, { { 0.765540, -0.044589, -0.639619 }, -0.053368 } }; + bone->twistLimit = ( b3Vec2 ){ -45.0f * B3_DEG_TO_RAD, 5.0f * B3_DEG_TO_RAD }; + } + + { + Bone* bone = human->bones + bone_upper_arm_l; + bone->parentIndex = bone_spine_03; + + bodyDef.name = "upper_arm_l"; + bone->referenceFrame = + ( b3Transform ){ { 0.20378f, 1.484275f, -0.115897f }, { { 0.143082f, 0.695980f, -0.690130f }, 0.13733f } }; + bodyDef.rotation = bone->referenceFrame.q; + bodyDef.position = b3Add( bone->referenceFrame.p, position ); + bone->bodyId = b3CreateBody( worldId, &bodyDef ); + + b3Capsule capsule = { { 0.0f, 0.0f, 0.0f }, { -0.091118f, 0.037775f, 0.229719f }, 0.075f }; + shapeDef.filter.groupIndex = 0; + shapeDef.baseMaterial.customColor = colorize ? shirtColor : 0; + b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + bone->jointType = b3_sphericalJoint; + bone->localFrameA = + ( b3Transform ){ { 0.203780, -0.069369, -0.181921 }, { { -0.278486, 0.445600, -0.097014 }, 0.845266 } }; + bone->localFrameB = ( b3Transform ){ { 0.000000, 0.000000, 0.000000 }, { { -0.201396, -0.001586, 0.901850 }, 0.382234 } }; + bone->swingLimit = 60.0f * B3_DEG_TO_RAD; + bone->twistLimit = ( b3Vec2 ){ -5.0f * B3_DEG_TO_RAD, 5.0f * B3_DEG_TO_RAD }; + } + + { + Bone* bone = human->bones + bone_lower_arm_l; + bone->parentIndex = bone_upper_arm_l; + + bodyDef.name = "lower_arm_l"; + bone->referenceFrame = + ( b3Transform ){ { 0.305614f, 1.242908f, -0.117599f }, { { 0.165048f, 0.563437f, -0.802002f }, 0.109959f } }; + bodyDef.rotation = bone->referenceFrame.q; + bodyDef.position = b3Add( bone->referenceFrame.p, position ); + bone->bodyId = b3CreateBody( worldId, &bodyDef ); + + b3Capsule capsule = { { 0.0f, 0.0f, 0.0f }, { -0.142406f, 0.039392f, 0.261092f }, 0.05f }; + shapeDef.filter.groupIndex = 0; + shapeDef.baseMaterial.customColor = colorize ? skinColor : 0; + b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + bone->jointType = b3_revoluteJoint; + bone->localFrameA = ( b3Transform ){ { -0.095482, 0.039584, 0.240723 }, { { 0.512487, -0.180629, 0.839474 }, 0.003742 } }; + bone->localFrameB = ( b3Transform ){ { 0.0f, 0.0f, 0.0f }, { { 0.503803, -0.029831, 0.858168 }, 0.094017 } }; + bone->twistLimit = ( b3Vec2 ){ -5.0f * B3_DEG_TO_RAD, 60.0f * B3_DEG_TO_RAD }; + } + + { + Bone* bone = human->bones + bone_upper_arm_r; + bone->parentIndex = bone_spine_03; + + bodyDef.name = "upper_arm_r"; + bone->referenceFrame = + ( b3Transform ){ { -0.20378f, 1.484276f, -0.115899f }, { { 0.143083f, -0.695978f, 0.690132f }, 0.137329f } }; + bodyDef.rotation = bone->referenceFrame.q; + bodyDef.position = b3Add( bone->referenceFrame.p, position ); + bone->bodyId = b3CreateBody( worldId, &bodyDef ); + + b3Capsule capsule = { { 0.0f, 0.0f, 0.0f }, { 0.091118f, 0.037775f, 0.229718f }, 0.075f }; + shapeDef.filter.groupIndex = 0; + shapeDef.baseMaterial.customColor = colorize ? shirtColor : 0; + b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + bone->jointType = b3_sphericalJoint; + bone->localFrameA = + ( b3Transform ){ { -0.203779, -0.069371, -0.181922 }, { { -0.253621, -0.414842, 0.106962 }, 0.867261 } }; + bone->localFrameB = ( b3Transform ){ { 0.000000, 0.000000, 0.000000 }, { { -0.201397, 0.001587, -0.901850 }, 0.382233 } }; + bone->swingLimit = 60.0f * B3_DEG_TO_RAD; + bone->twistLimit = ( b3Vec2 ){ -5.0f * B3_DEG_TO_RAD, 5.0f * B3_DEG_TO_RAD }; + } + + { + Bone* bone = human->bones + bone_lower_arm_r; + bone->parentIndex = bone_upper_arm_r; + + bodyDef.name = "lower_arm_r"; + bone->referenceFrame = + ( b3Transform ){ { -0.305614f, 1.242907f, -0.117599f }, { { 0.165048f, -0.563437f, 0.802002f }, 0.109959f } }; + bodyDef.rotation = bone->referenceFrame.q; + bodyDef.position = b3Add( bone->referenceFrame.p, position ); + bone->bodyId = b3CreateBody( worldId, &bodyDef ); + + b3Capsule capsule = { { 0.0f, 0.0f, 0.0f }, { 0.142406f, 0.039392f, 0.261092f }, 0.05f }; + shapeDef.filter.groupIndex = 0; + shapeDef.baseMaterial.customColor = colorize ? skinColor : 0; + b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + bone->jointType = b3_revoluteJoint; + bone->localFrameA = + ( b3Transform ){ { 0.095484, 0.039585, 0.240723 }, { { -0.180627, 0.512487, -0.003744 }, -0.839474 } }; + bone->localFrameB = ( b3Transform ){ { 0.0f, 0.0f, 0.0f }, { { -0.029831, 0.503803, -0.094017 }, -0.858169 } }; + bone->twistLimit = ( b3Vec2 ){ -60.0f * B3_DEG_TO_RAD, 5.0f * B3_DEG_TO_RAD }; + } + + // float dampingRatio = 0.9f; + for ( int i = 1; i < bone_count; ++i ) + { + Bone* bone = human->bones + i; + Bone* parent = human->bones + bone->parentIndex; + + b3BodyId bodyIdA = parent->bodyId; + b3BodyId bodyIdB = bone->bodyId; + + b3Transform frameA = b3NormalizeTransform( bone->localFrameA ); + b3Transform frameB = b3NormalizeTransform( bone->localFrameB ); + + if ( bone->jointType == b3_revoluteJoint ) + { + b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef(); + jointDef.base.bodyIdA = bodyIdA; + jointDef.base.bodyIdB = bodyIdB; + jointDef.base.localFrameA = frameA; + jointDef.base.localFrameB = frameB; + jointDef.enableLimit = true; + jointDef.lowerAngle = bone->twistLimit.x; + jointDef.upperAngle = bone->twistLimit.y; + jointDef.enableSpring = hertz > 0.0f; + jointDef.hertz = hertz; + jointDef.dampingRatio = dampingRatio; + jointDef.enableMotor = true; + jointDef.maxMotorTorque = bone->jointFriction * frictionTorque; + bone->jointId = b3CreateRevoluteJoint( worldId, &jointDef ); + } + else if ( bone->jointType == b3_sphericalJoint ) + { + b3SphericalJointDef jointDef = b3DefaultSphericalJointDef(); + jointDef.base.bodyIdA = bodyIdA; + jointDef.base.bodyIdB = bodyIdB; + jointDef.base.localFrameA = frameA; + jointDef.base.localFrameB = frameB; + jointDef.enableConeLimit = true; + jointDef.coneAngle = bone->swingLimit; + jointDef.enableTwistLimit = true; + jointDef.lowerTwistAngle = bone->twistLimit.x; + jointDef.upperTwistAngle = bone->twistLimit.y; + jointDef.enableSpring = hertz > 0.0f; + jointDef.hertz = hertz; + jointDef.dampingRatio = dampingRatio; + jointDef.enableMotor = true; + jointDef.maxMotorTorque = bone->jointFriction * frictionTorque; + bone->jointId = b3CreateSphericalJoint( worldId, &jointDef ); + } + } + + // Disable some collisions + b3FilterJointDef filterDef = b3DefaultFilterJointDef(); + filterDef.base.bodyIdA = human->bones[bone_thigh_l].bodyId; + filterDef.base.bodyIdB = human->bones[bone_thigh_r].bodyId; + human->filterJoints[0] = b3CreateFilterJoint( worldId, &filterDef ); + human->filterJointCount = 1; + + human->isSpawned = true; +} + +void DestroyHuman( Human* human ) +{ + assert( human->isSpawned == true ); + + for ( int i = 0; i < human->filterJointCount; ++i ) + { + b3DestroyJoint( human->filterJoints[i], false ); + human->filterJoints[i] = b3_nullJointId; + } + + for ( int i = 0; i < bone_count; ++i ) + { + if ( B3_IS_NULL( human->bones[i].jointId ) ) + { + continue; + } + + b3DestroyJoint( human->bones[i].jointId, false ); + human->bones[i].jointId = b3_nullJointId; + } + + for ( int i = 0; i < bone_count; ++i ) + { + if ( B3_IS_NULL( human->bones[i].bodyId ) ) + { + continue; + } + + b3DestroyBody( human->bones[i].bodyId ); + human->bones[i].bodyId = b3_nullBodyId; + } + + human->isSpawned = false; +} + +void Human_SetVelocity( Human* human, b3Vec3 velocity ) +{ + for ( int i = 0; i < bone_count; ++i ) + { + b3BodyId bodyId = human->bones[i].bodyId; + + if ( B3_IS_NULL( bodyId ) ) + { + continue; + } + + b3Body_SetLinearVelocity( bodyId, velocity ); + } +} + +void Human_ApplyRandomAngularImpulse( Human* human, float magnitude ) +{ + assert( human->isSpawned == true ); + b3Vec3 range = { magnitude, magnitude, magnitude }; + b3Vec3 impulse = RandomVec3( b3Neg( range ), range ); + b3Body_ApplyAngularImpulse( human->bones[bone_spine_01].bodyId, impulse, true ); +} + +void Human_SetJointFrictionTorque( Human* human, float torque ) +{ + assert( human->isSpawned == true ); + human->frictionTorque = torque; + + for ( int i = 1; i < bone_count; ++i ) + { + Bone* bone = human->bones + i; + if ( bone->jointType == b3_revoluteJoint ) + { + b3RevoluteJoint_SetMaxMotorTorque( bone->jointId, bone->jointFriction * torque ); + } + else + { + b3SphericalJoint_SetMaxMotorTorque( bone->jointId, bone->jointFriction * torque ); + } + } +} + +void Human_SetJointSpringHertz( Human* human, float hertz ) +{ + assert( human->isSpawned == true ); + for ( int i = 1; i < bone_count; ++i ) + { + Bone* bone = human->bones + i; + if ( bone->jointType == b3_revoluteJoint ) + { + b3RevoluteJoint_SetSpringHertz( bone->jointId, hertz ); + } + else + { + b3SphericalJoint_SetSpringHertz( bone->jointId, hertz ); + } + } +} + +void Human_SetJointDampingRatio( Human* human, float dampingRatio ) +{ + assert( human->isSpawned == true ); + for ( int i = 1; i < bone_count; ++i ) + { + Bone* bone = human->bones + i; + if ( bone->jointType == b3_revoluteJoint ) + { + b3RevoluteJoint_SetSpringDampingRatio( bone->jointId, dampingRatio ); + } + else + { + b3SphericalJoint_SetSpringDampingRatio( bone->jointId, dampingRatio ); + } + } +} + +void Human_AlignSpring(Human* human, b3WorldId worldId, b3BodyId groundId, float hertz, float dampingRatio) +{ + assert( human->isSpawned == true ); + + Bone* bone = human->bones + bone_pelvis; + assert( B3_IS_NULL( bone->jointId ) == true ); + b3Quat q = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisZ, b3Vec3_axisY ); + b3Quat qb = b3Body_GetRotation( bone->bodyId ); + + b3ParallelJointDef jointDef = b3DefaultParallelJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = bone->bodyId; + jointDef.base.localFrameA.q = q; + jointDef.base.localFrameB.q = b3InvMulQuat( qb, q ); + jointDef.base.drawScale = 2.0f; + jointDef.base.collideConnected = true; + jointDef.hertz = hertz; + jointDef.dampingRatio = dampingRatio; + + bone->jointId = b3CreateParallelJoint( worldId, &jointDef ); +} + +void Human_CreateMotorAnchors( Human* human, b3WorldId worldId ) +{ + b3BodyDef anchorDef = b3DefaultBodyDef(); + anchorDef.type = b3_kinematicBody; + + b3MotorJointDef motorDef = b3DefaultMotorJointDef(); + motorDef.angularHertz = 5.0f; + motorDef.angularDampingRatio = 1.0f; + motorDef.linearHertz = 5.0f; + motorDef.linearDampingRatio = 1.0f; + motorDef.maxSpringForce = FLT_MAX; + motorDef.maxSpringTorque = FLT_MAX; + + for ( int i = 0; i < bone_count; ++i ) + { + Bone* bone = human->bones + i; + + b3Transform bodyTransform = b3Body_GetTransform( bone->bodyId ); + anchorDef.position = bodyTransform.p; + anchorDef.rotation = bodyTransform.q; + bone->anchorId = b3CreateBody(worldId, &anchorDef ); + + motorDef.base.bodyIdA = bone->anchorId; + motorDef.base.bodyIdB = bone->bodyId; + + bone->anchorJointId = b3CreateMotorJoint( worldId, &motorDef ); + } +} + +void Human_CreateParallelAnchors( Human* human, b3WorldId worldId ) +{ + b3BodyDef anchorDef = b3DefaultBodyDef(); + anchorDef.type = b3_kinematicBody; + + b3Quat qFrameWorld = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisZ, b3Vec3_axisY ); + b3ParallelJointDef jointDef = b3DefaultParallelJointDef(); + jointDef.hertz = 8.0f; + jointDef.dampingRatio = 1.0f; + jointDef.maxTorque = 800.0f; + + for ( int i = 0; i < bone_count; ++i ) + { + Bone* bone = human->bones + i; + + b3Transform bodyTransform = b3Body_GetTransform( bone->bodyId ); + anchorDef.position = bodyTransform.p; + anchorDef.rotation = bodyTransform.q; + bone->anchorId = b3CreateBody(worldId, &anchorDef ); + + jointDef.base.bodyIdA = bone->anchorId; + jointDef.base.bodyIdB = bone->bodyId; + + b3Quat frameQuat = b3InvMulQuat( bodyTransform.q, qFrameWorld ); + jointDef.base.localFrameA.q = frameQuat; + jointDef.base.localFrameB.q = frameQuat; + + bone->anchorJointId = b3CreateParallelJoint( worldId, &jointDef ); + } +} + +#if 0 +void Human::EnablePoseControl( b3World* world, float springHertz, bool poseControl ) +{ + if ( poseControl == m_poseControl ) + { + return; + } + + if ( m_poseControl ) + { + for ( int i = 0; i < bone_count; ++i ) + { + Bone* bone = human->bones + i; + + world->RemoveJoint( bone->poseJoint, true ); + bone->poseJoint = nullptr; + } + + world->DestroyBody( m_rootBody, true ); + m_rootBody = nullptr; + } + else + { + b3BodyDef rootDef = b3DefaultBodyDef(); + rootDef.type = b3_kinematicBody; + rootDef.position = m_baseTransform.p; + rootDef.rotation = m_baseTransform.q; + m_rootBody = world->CreateBody( &rootDef ); + + float dampingRatio = 0.9f; + + for ( int i = 0; i < bone_count; ++i ) + { + Bone* bone = human->bones + i; + + b3Body* bodyIdA = m_rootBody; + b3Body* bodyIdB = bone->bodyId; + + bool enableCollision = false; + bool useBlockSolver = false; + + // bone->poseJoint = world->AddRigidJoint( bodyIdA, bodyIdB, bodyIdB->GetPosition(), enableCollision, useBlockSolver ); + bone->poseJoint = world->AddRigidJoint( bodyIdA, bone->referenceFrame, bodyIdB, b3Transform_identity, + b3Transform_identity, enableCollision, useBlockSolver ); + bone->poseJoint->SetAngularFrequency( springHertz ); + bone->poseJoint->SetAngularDampingRatio( dampingRatio ); + bone->poseJoint->SetLinearFrequency( springHertz ); + bone->poseJoint->SetLinearDampingRatio( dampingRatio ); + } + } + + m_poseControl = poseControl; +} + +void Human::AdjustPoseControl( float springHertz ) +{ + if ( m_poseControl == false ) + { + return; + } + + for ( int i = 0; i < bone_count; ++i ) + { + Bone* bone = human->bones + i; + + bone->poseJoint->SetAngularFrequency( springHertz ); + bone->poseJoint->SetLinearFrequency( springHertz ); + } +} + +void Human::DriveBase( const b3Transform& transform, float timeStep ) +{ + m_rootBody->SetVelocityFromKeyframe( transform, timeStep ); + m_baseTransform = transform; +} + +void Human::EnableMotors( bool enableMotors ) +{ + if ( enableMotors == m_motorized ) + { + return; + } + + for ( int i = 1; i < bone_count; ++i ) + { + Bone* bone = human->bones + i; + + if ( bone->joint == nullptr ) + { + continue; + } + + if ( bone->joint->mType == b3_revoluteJoint ) + { + b3RevoluteJoint* joint = static_cast( bone->joint ); + if ( enableMotors ) + { + joint->SetMotorMode( B3_POSITION_MODE ); + } + else + { + joint->SetMotorFriction( bone->jointFriction ); + } + } + else if ( bone->joint->mType == b3_sphericalJoint ) + { + b3SphericalJoint* joint = static_cast( bone->joint ); + if ( enableMotors ) + { + joint->SetMotorMode( B3_POSITION_MODE ); + } + else + { + joint->SetMotorFriction( bone->jointFriction ); + } + } + } + + m_motorized = enableMotors; +} + +void Human::AdjustMotors( float springHertz ) +{ + if ( m_motorized == false ) + { + return; + } + + for ( int i = 1; i < bone_count; ++i ) + { + Bone* bone = human->bones + i; + + if ( bone->joint == nullptr ) + { + continue; + } + + if ( bone->joint->mType == b3_revoluteJoint ) + { + b3RevoluteJoint* joint = static_cast( bone->joint ); + joint->mAngularSpring.Frequency = springHertz; + } + else if ( bone->joint->mType == b3_sphericalJoint ) + { + b3SphericalJoint* joint = static_cast( bone->joint ); + joint->mAngularSpring.Frequency = springHertz; + } + } +} +#endif diff --git a/shared/human.h b/shared/human.h new file mode 100644 index 0000000..9e8e6be --- /dev/null +++ b/shared/human.h @@ -0,0 +1,82 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box3d/types.h" + +typedef enum BoneId +{ + bone_pelvis, + bone_spine_01, + bone_spine_02, + bone_spine_03, + bone_neck, + bone_head, + bone_thigh_l, + bone_calf_l, + bone_thigh_r, + bone_calf_r, + bone_upper_arm_l, + bone_lower_arm_l, + bone_upper_arm_r, + bone_lower_arm_r, + bone_count, +} BoneId; + +typedef struct Bone +{ + b3BodyId bodyId; + b3JointId jointId; + b3BodyId anchorId; + b3JointId anchorJointId; + b3Transform localFrameA; + b3Transform localFrameB; + b3Transform referenceFrame; + b3JointType jointType; + float swingLimit; + b3Vec2 twistLimit; + float jointFriction; + int parentIndex; +} Bone; + +#define FILTER_JOINT_COUNT 8 + +// This must be zero initialized +typedef struct Human +{ + Bone bones[bone_count]; + b3JointId filterJoints[FILTER_JOINT_COUNT]; + int filterJointCount; + float frictionTorque; + bool isSpawned; +} Human; + +#ifdef __cplusplus +extern "C" +{ +#endif + +void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float frictionTorque, float hertz, float dampingRatio, + int groupIndex, void* userData, bool colorize ); + +// There are no allocations so this does not need to be called when the world is destroyed. +void DestroyHuman( Human* human ); + +void Human_SetVelocity( Human* human, b3Vec3 velocity ); +void Human_ApplyRandomAngularImpulse( Human* human, float magnitude ); +void Human_SetJointFrictionTorque( Human* human, float torque ); +void Human_SetJointSpringHertz( Human* human, float hertz ); +void Human_SetJointDampingRatio( Human* human, float dampingRatio ); + +void Human_AlignSpring( Human* human, b3WorldId worldId, b3BodyId groundId, float hertz, float dampingRatio ); +void Human_CreateMotorAnchors( Human* human, b3WorldId worldId ); +void Human_CreateParallelAnchors( Human* human, b3WorldId worldId ); + +// void Human_EnablePoseControl( Human* human, float springHertz, bool enablePoseControl ); +// void Human_AdjustPoseControl( Human* human, float springHertz ); +// void Human_DriveBase( Human* human, b3Transform transform, float timeStep ); + +#ifdef __cplusplus +} +#endif diff --git a/shared/stability.c b/shared/stability.c new file mode 100644 index 0000000..a23b4cc --- /dev/null +++ b/shared/stability.c @@ -0,0 +1,74 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "stability.h" + +#include "utils.h" + +#include "box3d/box3d.h" + +#include +#include + +#define MESH_DROP_GRID_COUNT 32 + +MeshDropData CreateMeshDrop( b3WorldId worldId ) +{ + MeshDropData data = { 0 }; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + + int gridCount = 40; + float cellWidth = 1.0f; + float rowHz = 0.1f; + float columnHz = 0.2f; + float groundAmplitude = 0.5f; + + data.mesh = b3CreateWaveMesh( gridCount, gridCount, cellWidth, groundAmplitude, rowHz, columnHz ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.filter.categoryBits = 1; + b3CreateMeshShape( groundId, &shapeDef, data.mesh, b3Vec3_one ); + } + + { + b3BoxHull box = b3MakeBoxHull( 0.02f, 0.2f, 0.04f ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.rollingResistance = 0.1f; + + // Don't allow shapes to collide with each other. + shapeDef.filter.categoryBits = 2; + shapeDef.filter.maskBits = 1; + + g_randomSeed = 3963634789; + + int gridCount = MESH_DROP_GRID_COUNT; + + for ( int i = 0; i < gridCount; ++i ) + { + for ( int j = 0; j < gridCount; ++j ) + { + b3Vec3 linearVelocity = RandomVec3Uniform( -1.0f, 1.0f ); + b3Vec3 angularVelocity = RandomVec3Uniform( -5.0f, 5.0f ); + + bodyDef.position = (b3Vec3){ 0.5f * ( i - 0.5f * gridCount ), 5.0f, 0.5f * ( j - 0.5f * gridCount ) }; + bodyDef.linearVelocity = linearVelocity; + bodyDef.angularVelocity = angularVelocity; + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + } + } + + return data; +} + +void DestroyMeshDrop( MeshDropData* data ) +{ + b3DestroyMesh( data->mesh ); +} diff --git a/shared/stability.h b/shared/stability.h new file mode 100644 index 0000000..a512a61 --- /dev/null +++ b/shared/stability.h @@ -0,0 +1,24 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT +#pragma once + +#include "box3d/id.h" + +#include + +#ifdef __cplusplus +extern "C" +{ +#endif + +typedef struct MeshDropData +{ + struct b3MeshData* mesh; +} MeshDropData; + +MeshDropData CreateMeshDrop( b3WorldId worldId ); +void DestroyMeshDrop( MeshDropData* data ); + +#ifdef __cplusplus +} +#endif diff --git a/shared/utils.c b/shared/utils.c new file mode 100644 index 0000000..ac19aff --- /dev/null +++ b/shared/utils.c @@ -0,0 +1,33 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "utils.h" + +#if defined( _WIN64 ) +#include +#elif defined( __APPLE__ ) +#include +#elif defined( __linux__ ) +#include +#elif defined( __EMSCRIPTEN__ ) +#include +#endif + +uint32_t g_randomSeed = RAND_SEED; + +int GetNumberOfCores( void ) +{ +#if defined( _WIN64 ) + SYSTEM_INFO sysinfo; + GetSystemInfo( &sysinfo ); + return sysinfo.dwNumberOfProcessors; +#elif defined( __APPLE__ ) + return (int)sysconf( _SC_NPROCESSORS_ONLN ); +#elif defined( __linux__ ) + return (int)sysconf( _SC_NPROCESSORS_ONLN ); +#elif defined( __EMSCRIPTEN__ ) + return (int)sysconf( _SC_NPROCESSORS_ONLN ); +#else + return 1; +#endif +} diff --git a/shared/utils.h b/shared/utils.h new file mode 100644 index 0000000..80b9ec4 --- /dev/null +++ b/shared/utils.h @@ -0,0 +1,122 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box3d/math_functions.h" + +#define RAND_LIMIT 32767 +#define RAND_SEED 12345 + +// Global seed for simple random number generator. + +#ifdef __cplusplus +extern "C" +{ +#endif + +extern uint32_t g_randomSeed; + +int GetNumberOfCores( void ); + +#ifdef __cplusplus +} +#endif + +// Simple random number generator. Using this instead of rand() for cross platform determinism. +B3_INLINE int RandomInt() +{ + // XorShift32 algorithm + uint32_t x = g_randomSeed; + x ^= x << 13; + x ^= x >> 17; + x ^= x << 5; + g_randomSeed = x; + + // Map the 32-bit value to the range 0 to RAND_LIMIT + return (int)( x % ( RAND_LIMIT + 1 ) ); +} + +// Random integer in range [lo, hi] +B3_INLINE float RandomIntRange( int lo, int hi ) +{ + return lo + RandomInt() % ( hi - lo + 1 ); +} + +// Random number in range [-1,1] +B3_INLINE float RandomFloat() +{ + float r = (float)( RandomInt() & ( RAND_LIMIT ) ); + r /= RAND_LIMIT; + r = 2.0f * r - 1.0f; + return r; +} + +// Random floating point number in range [lo, hi] +B3_INLINE float RandomFloatRange( float lo, float hi ) +{ + float r = (float)( RandomInt() & ( RAND_LIMIT ) ); + r /= RAND_LIMIT; + r = ( hi - lo ) * r + lo; + return r; +} + +// Random vector with coordinates in range [lo, hi] +B3_INLINE b3Vec3 RandomVec3( b3Vec3 lo, b3Vec3 hi ) +{ + b3Vec3 v; + v.x = RandomFloatRange( lo.x, hi.x ); + v.y = RandomFloatRange( lo.y, hi.y ); + v.z = RandomFloatRange( lo.z, hi.z ); + return v; +} + +B3_INLINE b3Vec3 RandomVec3Uniform( float lo, float hi ) +{ + b3Vec3 v; + v.x = RandomFloatRange( lo, hi ); + v.y = RandomFloatRange( lo, hi ); + v.z = RandomFloatRange( lo, hi ); + return v; +} + +B3_INLINE b3Vec3 RandomUnitVector() +{ + // Generate uniformly distributed random quaternion using Shoemake's method + // Reference: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III, 1992 + + float u1 = RandomFloatRange( 0.0f, 1.0f ); + float u2 = RandomFloatRange( 0.0f, 2.0f * B3_PI ); + float u3 = RandomFloatRange( 0.0f, 2.0f * B3_PI ); + + float sqrt1MinusU1 = sqrtf( 1.0f - u1 ); + float sqrtU1 = sqrtf( u1 ); + + b3Vec3 v; + v.x = sqrt1MinusU1 * sinf( u2 ); + v.y = sqrt1MinusU1 * cosf( u2 ); + v.z = sqrtU1 * sinf( u3 ); + + return v; +} + +B3_INLINE b3Quat RandomQuat() +{ + // Generate uniformly distributed random quaternion using Shoemake's method + // Reference: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III, 1992 + + float u1 = RandomFloatRange( 0.0f, 1.0f ); + float u2 = RandomFloatRange( 0.0f, 2.0f * B3_PI ); + float u3 = RandomFloatRange( 0.0f, 2.0f * B3_PI ); + + float sqrt1MinusU1 = sqrtf( 1.0f - u1 ); + float sqrtU1 = sqrtf( u1 ); + + b3Quat q; + q.v.x = sqrt1MinusU1 * sinf( u2 ); + q.v.y = sqrt1MinusU1 * cosf( u2 ); + q.v.z = sqrtU1 * sinf( u3 ); + q.s = sqrtU1 * cosf( u3 ); + + return q; +} diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt new file mode 100644 index 0000000..5ba4eb3 --- /dev/null +++ b/src/CMakeLists.txt @@ -0,0 +1,240 @@ +include(GNUInstallDirs) +include(FetchContent) + +set(BOX3D_SOURCE_FILES + aabb.c + aabb.h + algorithm.h + arena_allocator.c + arena_allocator.h + bitset.c + bitset.h + block_allocator.c + block_allocator.h + body.c + body.h + broad_phase.c + broad_phase.h + capsule.c + compound.c + compound.h + constraint_graph.c + constraint_graph.h + contact.c + contact.h + contact_solver.c + contact_solver.h + container.h + convex_manifold.c + core.c + core.h + ctz.h + distance.c + distance_joint.c + dynamic_tree.c + height_field.c + hull.c + id_pool.c + id_pool.h + island.c + island.h + joint.c + joint.h + manifold.c + manifold.h + math_functions.c + math_internal.h + mesh.c + mesh_contact.c + motor_joint.c + mover.c + parallel_for.c + parallel_for.h + parallel_joint.c + physics_world.c + physics_world.h + platform.h + prismatic_joint.c + qsort.h + revolute_joint.c + scheduler.c + scheduler.h + sensor.c + sensor.h + shape.c + shape.h + simd.c + simd.h + solver.c + solver.h + solver_set.c + solver_set.h + sphere.c + spherical_joint.c + table.c + table.h + timer.c + triangle_manifold.c + types.c + verstable.h + weld_joint.c + wheel_joint.c + + box3d.natvis +) + +set(BOX3D_INCLUDE_FILES + ../include/box3d/base.h + ../include/box3d/box3d.h + ../include/box3d/collision.h + ../include/box3d/constants.h + ../include/box3d/id.h + ../include/box3d/math_functions.h + ../include/box3d/types.h +) + +# Hide internal functions +# https://gcc.gnu.org/wiki/Visibility +set(CMAKE_C_VISIBILITY_PRESET hidden) +set(CMAKE_VISIBILITY_INLINES_HIDDEN 1) + +add_library(box3d STATIC ${BOX3D_SOURCE_FILES} ${BOX3D_INCLUDE_FILES}) + +# target_link_libraries(box3d PRIVATE eastl) + +target_include_directories(box3d + PUBLIC + $ + $ + $ + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR} +) + +set(CMAKE_DEBUG_POSTFIX "d") + +# Box3D uses C17 for _Static_assert and anonymous unions +set_target_properties(box3d PROPERTIES + C_STANDARD 17 + C_STANDARD_REQUIRED YES + C_EXTENSIONS YES + VERSION ${PROJECT_VERSION} + SOVERSION ${PROJECT_VERSION_MAJOR} + DEBUG_POSTFIX ${CMAKE_DEBUG_POSTFIX} +) + +if (BOX3D_COMPILE_WARNING_AS_ERROR) + set_target_properties(box3d PROPERTIES COMPILE_WARNING_AS_ERROR ON) +endif() + +if (BOX3D_PROFILE) + target_compile_definitions(box3d PRIVATE BOX3D_PROFILE) + set(TRACY_DELAYED_INIT ON CACHE BOOL "Enable delayed init for Tracy" FORCE) + set(TRACY_MANUAL_LIFETIME ON CACHE BOOL "Enable manual lifetime for Tracy" FORCE) + + FetchContent_Declare( + tracy + GIT_REPOSITORY https://github.com/wolfpld/tracy.git + GIT_TAG v0.13.1 + GIT_SHALLOW TRUE + GIT_PROGRESS TRUE + ) + FetchContent_MakeAvailable(tracy) + + target_link_libraries(box3d PUBLIC TracyClient) +endif() + +if(BOX3D_VALIDATE) + message(STATUS "Box3D validation ON") + target_compile_definitions(box3d PRIVATE BOX3D_VALIDATE) +endif() + +if (BOX3D_DISABLE_SIMD) + message(STATUS "Box3D SIMD disabled") + target_compile_definitions(box3d PRIVATE BOX3D_DISABLE_SIMD) +endif() + +if (MSVC) + message(STATUS "Box3D on MSVC") + if (BUILD_SHARED_LIBS) + # this is needed by DLL users to import Box3D symbols + target_compile_definitions(box3d INTERFACE BOX3D_DLL) + endif() + + # Visual Studio won't load the natvis unless it is in the project + target_sources(box3d PRIVATE box3d.natvis) + + # Enable asserts in release with debug info + target_compile_definitions(box3d PUBLIC "$<$:B3_ENABLE_ASSERT>") + + # Warnings Wall is problematic in mixed C/C++, so using W4 + target_compile_options(box3d PRIVATE /W4) + # target_compile_options(box3d PRIVATE /wd4820 /wd5045 /wd4061 /wd4711 /wd4514 /wd4365 /wd5219 /wd5039) + target_compile_options(box3d PRIVATE /wd4820 /wd5045 /wd4061 /wd4711) + # 4710 - warn about inline functions that are not inlined + target_compile_options(box3d PRIVATE /wd4710 ) + + if (${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") + target_compile_options(box3d PRIVATE -Wmissing-prototypes) + endif() + +elseif (MINGW) + message(STATUS "Box3D on MinGW") +elseif (APPLE) + message(STATUS "Box3D on Apple") + target_compile_options(box3d PRIVATE -Wmissing-prototypes -Wall -Wextra -pedantic) +elseif (EMSCRIPTEN) + message(STATUS "Box3D on Emscripten") + target_compile_options(box3d PRIVATE -msimd128 -msse2) +elseif (UNIX) + message(STATUS "Box3D using Unix") + target_compile_options(box3d PRIVATE -Wmissing-prototypes -Wall -Wextra -pedantic -Wno-unused-value) + if ("${CMAKE_HOST_SYSTEM_PROCESSOR}" STREQUAL "aarch64") + # raspberry pi + # -mfpu=neon + # target_compile_options(box3d PRIVATE) + else() + endif() +endif() + +source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "src" FILES ${BOX3D_SOURCE_FILES}) +source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}/../include" PREFIX "include" FILES ${BOX3D_INCLUDE_FILES}) + +add_library(box3d::box3d ALIAS box3d) + +# libm is required on many Unix toolchains for sqrtf, etc. +if(UNIX AND NOT APPLE AND NOT EMSCRIPTEN) + target_link_libraries(box3d PUBLIC m) +endif() + +install( + TARGETS box3d + EXPORT box3dConfig + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} +) + +install( + EXPORT box3dConfig + NAMESPACE box3d:: + DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/box3d" +) + +install( + FILES ${BOX3D_API_FILES} + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/box3d +) + +include(CMakePackageConfigHelpers) + +write_basic_package_version_file( + "${CMAKE_CURRENT_BINARY_DIR}/box3dConfigVersion.cmake" + VERSION ${BOX3D_VERSION} + COMPATIBILITY SameMajorVersion +) + +install( + FILES "${CMAKE_CURRENT_BINARY_DIR}/box3dConfigVersion.cmake" + DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/box3d" +) diff --git a/src/aabb.c b/src/aabb.c new file mode 100644 index 0000000..c1d5342 --- /dev/null +++ b/src/aabb.c @@ -0,0 +1,124 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "aabb.h" + +#include "box3d/math_functions.h" + +#include + +// From Real-time Collision Detection, p179. +b3CastOutput b3RayCastAABB( b3AABB a, b3Vec3 p1, b3Vec3 p2 ) +{ + // Radius not handled + b3CastOutput output = { 0 }; + + float tmin = -FLT_MAX; + float tmax = FLT_MAX; + + b3Vec3 p = p1; + b3Vec3 d = b3Sub( p2, p1 ); + b3Vec3 absD = b3Abs( d ); + + b3Vec3 normal = b3Vec3_zero; + + // x-coordinate + if ( absD.x < FLT_EPSILON ) + { + // parallel + if ( p.x < a.lowerBound.x || a.upperBound.x < p.x ) + { + return output; + } + } + else + { + float inv_d = 1.0f / d.x; + float t1 = ( a.lowerBound.x - p.x ) * inv_d; + float t2 = ( a.upperBound.x - p.x ) * inv_d; + + // Sign of the normal vector. + float s = -1.0f; + + if ( t1 > t2 ) + { + float tmp = t1; + t1 = t2; + t2 = tmp; + s = 1.0f; + } + + // Push the min up + if ( t1 > tmin ) + { + normal.y = 0.0f; + normal.x = s; + tmin = t1; + } + + // Pull the max down + tmax = b3MinFloat( tmax, t2 ); + + if ( tmin > tmax ) + { + return output; + } + } + + // y-coordinate + if ( absD.y < FLT_EPSILON ) + { + // parallel + if ( p.y < a.lowerBound.y || a.upperBound.y < p.y ) + { + return output; + } + } + else + { + float inv_d = 1.0f / d.y; + float t1 = ( a.lowerBound.y - p.y ) * inv_d; + float t2 = ( a.upperBound.y - p.y ) * inv_d; + + // Sign of the normal vector. + float s = -1.0f; + + if ( t1 > t2 ) + { + float tmp = t1; + t1 = t2; + t2 = tmp; + s = 1.0f; + } + + // Push the min up + if ( t1 > tmin ) + { + normal.x = 0.0f; + normal.y = s; + tmin = t1; + } + + // Pull the max down + tmax = b3MinFloat( tmax, t2 ); + + if ( tmin > tmax ) + { + return output; + } + } + + // Does the ray start inside the box? + // Does the ray intersect beyond the max fraction? + if ( tmin < 0.0f || 1.0f < tmin ) + { + return output; + } + + // Intersection. + output.fraction = tmin; + output.normal = normal; + output.point = b3Lerp( p1, p2, tmin ); + output.hit = true; + return output; +} diff --git a/src/aabb.h b/src/aabb.h new file mode 100644 index 0000000..c20dca3 --- /dev/null +++ b/src/aabb.h @@ -0,0 +1,80 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box3d/types.h" + +// Ray cast an AABB +b3CastOutput b3RayCastAABB( b3AABB a, b3Vec3 p1, b3Vec3 p2 ); + +// Get the surface area (perimeter) +static inline float b3Perimeter( b3AABB a ) +{ + float wx = a.upperBound.x - a.lowerBound.x; + float wy = a.upperBound.y - a.lowerBound.y; + float wz = a.upperBound.z - a.lowerBound.z; + return 2.0f * ( wx * wz + wy * wx + wz * wy ); +} + +/// Enlarge a to contain b +/// @return true if the AABB grew +static inline bool b3EnlargeAABB( b3AABB* a, b3AABB b ) +{ + bool changed = false; + if ( b.lowerBound.x < a->lowerBound.x ) + { + a->lowerBound.x = b.lowerBound.x; + changed = true; + } + + if ( b.lowerBound.y < a->lowerBound.y ) + { + a->lowerBound.y = b.lowerBound.y; + changed = true; + } + + if ( b.lowerBound.z < a->lowerBound.z ) + { + a->lowerBound.z = b.lowerBound.z; + changed = true; + } + + if ( a->upperBound.x < b.upperBound.x ) + { + a->upperBound.x = b.upperBound.x; + changed = true; + } + + if ( a->upperBound.y < b.upperBound.y ) + { + a->upperBound.y = b.upperBound.y; + changed = true; + } + + if ( a->upperBound.z < b.upperBound.z ) + { + a->upperBound.z = b.upperBound.z; + changed = true; + } + + return changed; +} + +#if 0 +/// Do a and b overlap +inline bool b3OverlapAABBs( b3AABB a, b3AABB b ) +{ + return !( b.lowerBound.x > a.upperBound.x || b.lowerBound.y > a.upperBound.y || b.lowerBound.z > a.upperBound.z || + a.lowerBound.x > b.upperBound.x || a.lowerBound.y > b.upperBound.y || a.lowerBound.z > b.upperBound.z ); +} +#endif + +static inline b3Vec3 b3FarthestPointOnAABB( b3AABB b, b3Vec3 p ) +{ + return (b3Vec3){ + .x = ( p.x - b.lowerBound.x ) > ( b.upperBound.x - p.x ) ? b.lowerBound.x : b.upperBound.x, + .y = ( p.y - b.lowerBound.y ) > ( b.upperBound.y - p.y ) ? b.lowerBound.y : b.upperBound.y, + .z = ( p.z - b.lowerBound.z ) > ( b.upperBound.z - p.z ) ? b.lowerBound.z : b.upperBound.z, + }; +} diff --git a/src/algorithm.h b/src/algorithm.h new file mode 100644 index 0000000..67d4b46 --- /dev/null +++ b/src/algorithm.h @@ -0,0 +1,21 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +#ifdef __cplusplus +#define B3_TYPE_OF( A ) decltype( A ) +#else +#define B3_TYPE_OF( A ) __typeof__( A ) +#endif + +#define B3_SWAP( x, y ) \ + do \ + { \ + B3_TYPE_OF( x ) B3_SWAP_TEMP = x; \ + x = y; \ + y = B3_SWAP_TEMP; \ + } \ + while ( 0 ) diff --git a/src/arena_allocator.c b/src/arena_allocator.c new file mode 100644 index 0000000..b7031f0 --- /dev/null +++ b/src/arena_allocator.c @@ -0,0 +1,127 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "arena_allocator.h" + +#include "core.h" + +#include +#include + +b3Stack b3CreateStack( int capacity ) +{ + B3_ASSERT( capacity >= 0 ); + b3Stack stack = { 0 }; + stack.capacity = capacity; + stack.memory = (char*)b3Alloc( capacity ); + return stack; +} + +void b3DestroyStack( b3Stack* stack ) +{ + b3Free( stack->memory, stack->capacity ); +} + +void* b3StackAlloc( b3Stack* stack, int size, const char* name ) +{ + if ( stack->entryCount == B3_MAX_STACK_ENTRIES ) + { + B3_ASSERT( false ); + return NULL; + } + + // ensure allocation is 32 byte aligned to support 256-bit SIMD + int size32 = ( ( size - 1 ) | 0x1F ) + 1; + + b3StackEntry entry; + entry.size = size32; + entry.name = name; + if ( stack->index + size32 > stack->capacity ) + { + // fall back to the heap (undesirable) + entry.data = (char*)b3Alloc( size32 ); + entry.usedMalloc = true; + + B3_ASSERT( ( (uintptr_t)entry.data & 0x1F ) == 0 ); + } + else + { + entry.data = stack->memory + stack->index; + entry.usedMalloc = false; + stack->index += size32; + + B3_ASSERT( ( (uintptr_t)entry.data & 0x1F ) == 0 ); + } + + stack->allocation += size32; + if ( stack->allocation > stack->maxAllocation ) + { + stack->maxAllocation = stack->allocation; + } + + stack->entries[stack->entryCount] = entry; + stack->entryCount += 1; + return entry.data; +} + +void b3StackFree( b3Stack* stack, void* mem ) +{ + int entryCount = stack->entryCount; + B3_ASSERT( entryCount > 0 ); + b3StackEntry* entry = stack->entries + ( entryCount - 1 ); + B3_ASSERT( mem == entry->data ); + if ( entry->usedMalloc ) + { + b3Free( mem, entry->size ); + } + else + { + stack->index -= entry->size; + } + stack->allocation -= entry->size; + stack->entryCount -= 1; +} + +void b3GrowStack( b3Stack* stack ) +{ + // Stack must not be in use + B3_ASSERT( stack->allocation == 0 ); + + if ( stack->maxAllocation > stack->capacity ) + { + b3Free( stack->memory, stack->capacity ); + stack->capacity = stack->maxAllocation + stack->maxAllocation / 2; + stack->memory = (char*)b3Alloc( stack->capacity ); + } +} + +int b3GetStackCapacity( b3Stack* stack ) +{ + return stack->capacity; +} + +int b3GetStackAllocation( b3Stack* stack ) +{ + return stack->allocation; +} + +int b3GetMaxStackAllocation( b3Stack* stack ) +{ + return stack->maxAllocation; +} + +b3Arena b3CreateArena( int capacity ) +{ + int c = capacity > 8 ? capacity : 8; + return (b3Arena){ + .memory = b3Alloc( c ), + .capacity = c, + .index = 0, + }; +} + +void b3DestroyArena(b3Arena* arena) +{ + b3Free( arena->memory, arena->capacity ); + *arena = (b3Arena){ 0 }; +} diff --git a/src/arena_allocator.h b/src/arena_allocator.h new file mode 100644 index 0000000..f5e59f9 --- /dev/null +++ b/src/arena_allocator.h @@ -0,0 +1,86 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once +#include "box3d/base.h" + +#include +#include + +#define B3_MAX_STACK_ENTRIES 32 + +typedef struct b3StackEntry +{ + char* data; + const char* name; + int size; + bool usedMalloc; +} b3StackEntry; + +// This is a stack-like arena allocator used for fast per step allocations. +// You must nest allocate/free pairs. The code will B3_ASSERT +// if you try to interleave multiple allocate/free pairs. +// This allocator uses the heap if space is insufficient. +// I could remove the need to free entries individually. +typedef struct b3Stack +{ + char* memory; + int capacity; + int index; + + int allocation; + int maxAllocation; + + b3StackEntry entries[B3_MAX_STACK_ENTRIES]; + int entryCount; +} b3Stack; + +typedef struct b3Arena +{ + char* memory; + int capacity; + int index; +} b3Arena; + +#ifdef __cplusplus +extern "C" +{ +#endif + +b3Stack b3CreateStack( int capacity ); +void b3DestroyStack( b3Stack* stack ); + +void* b3StackAlloc( b3Stack* stack, int size, const char* name ); +void b3StackFree( b3Stack* stack, void* mem ); + +// Grow the stack based on usage +void b3GrowStack( b3Stack* stack ); + +int b3GetStackCapacity( b3Stack* stack ); +int b3GetStackAllocation( b3Stack* stack ); +int b3GetMaxStackAllocation( b3Stack* stack ); + +b3Arena b3CreateArena( int capacity ); +void b3DestroyArena( b3Arena* arena ); + +static inline void* b3Bump( b3Arena* arena, int size ) +{ + if (size == 0) + { + return NULL; + } + + if ( size + arena->index > arena->capacity ) + { + B3_ASSERT( false ); + return NULL; + } + + void* memory = arena->memory + arena->index; + arena->index += size; + return memory; +} + +#ifdef __cplusplus +} +#endif diff --git a/src/bitset.c b/src/bitset.c new file mode 100644 index 0000000..319725b --- /dev/null +++ b/src/bitset.c @@ -0,0 +1,66 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "bitset.h" + +#include + +b3BitSet b3CreateBitSet( uint32_t bitCapacity ) +{ + b3BitSet bitSet = { 0 }; + bitSet.blockCapacity = ( bitCapacity + sizeof( uint64_t ) * 8 - 1 ) / ( sizeof( uint64_t ) * 8 ); + bitSet.blockCount = 0; + bitSet.bits = (uint64_t*)b3Alloc( bitSet.blockCapacity * sizeof( uint64_t ) ); + memset( bitSet.bits, 0, bitSet.blockCapacity * sizeof( uint64_t ) ); + return bitSet; +} + +void b3DestroyBitSet( b3BitSet* bitSet ) +{ + b3Free( bitSet->bits, bitSet->blockCapacity * sizeof( uint64_t ) ); + bitSet->blockCapacity = 0; + bitSet->blockCount = 0; + bitSet->bits = NULL; +} + +void b3SetBitCountAndClear( b3BitSet* bitSet, uint32_t bitCount ) +{ + uint32_t blockCount = ( bitCount + sizeof( uint64_t ) * 8 - 1 ) / ( sizeof( uint64_t ) * 8 ); + if ( bitSet->blockCapacity < blockCount ) + { + b3DestroyBitSet( bitSet ); + uint32_t newBitCapacity = bitCount + ( bitCount >> 1 ); + *bitSet = b3CreateBitSet( newBitCapacity ); + } + + bitSet->blockCount = blockCount; + memset( bitSet->bits, 0, bitSet->blockCount * sizeof( uint64_t ) ); +} + +void b3GrowBitSet( b3BitSet* bitSet, uint32_t blockCount ) +{ + B3_ASSERT( blockCount > bitSet->blockCount ); + if ( blockCount > bitSet->blockCapacity ) + { + uint32_t oldCapacity = bitSet->blockCapacity; + bitSet->blockCapacity = blockCount + blockCount / 2; + uint64_t* newBits = (uint64_t*)b3Alloc( bitSet->blockCapacity * sizeof( uint64_t ) ); + memset( newBits, 0, bitSet->blockCapacity * sizeof( uint64_t ) ); + B3_ASSERT( bitSet->bits != NULL ); + memcpy( newBits, bitSet->bits, oldCapacity * sizeof( uint64_t ) ); + b3Free( bitSet->bits, oldCapacity * sizeof( uint64_t ) ); + bitSet->bits = newBits; + } + + bitSet->blockCount = blockCount; +} + +void b3InPlaceUnion( b3BitSet* __restrict setA, const b3BitSet* __restrict setB ) +{ + B3_ASSERT( setA->blockCount == setB->blockCount ); + uint32_t blockCount = setA->blockCount; + for ( uint32_t i = 0; i < blockCount; ++i ) + { + setA->bits[i] |= setB->bits[i]; + } +} diff --git a/src/bitset.h b/src/bitset.h new file mode 100644 index 0000000..8eb9bef --- /dev/null +++ b/src/bitset.h @@ -0,0 +1,76 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "core.h" + +#include +#include + +// Bit set provides fast operations on large arrays of bits. +typedef struct b3BitSet +{ + uint64_t* bits; + uint32_t blockCapacity; + uint32_t blockCount; +} b3BitSet; + + +#ifdef __cplusplus +extern "C" +{ +#endif + +b3BitSet b3CreateBitSet( uint32_t bitCapacity ); +void b3DestroyBitSet( b3BitSet* bitSet ); +void b3SetBitCountAndClear( b3BitSet* bitSet, uint32_t bitCount ); +void b3InPlaceUnion( b3BitSet* setA, const b3BitSet* setB ); +void b3GrowBitSet( b3BitSet* bitSet, uint32_t blockCount ); +int b3CountSetBits( b3BitSet* bitSet ); + +#ifdef __cplusplus +} +#endif + +static inline void b3SetBit( b3BitSet* bitSet, uint32_t bitIndex ) +{ + uint32_t blockIndex = bitIndex / 64; + B3_ASSERT( blockIndex < bitSet->blockCount ); + bitSet->bits[blockIndex] |= ( (uint64_t)1 << bitIndex % 64 ); +} + +static inline void b3SetBitGrow( b3BitSet* bitSet, uint32_t bitIndex ) +{ + uint32_t blockIndex = bitIndex / 64; + if ( blockIndex >= bitSet->blockCount ) + { + b3GrowBitSet( bitSet, blockIndex + 1 ); + } + bitSet->bits[blockIndex] |= ( (uint64_t)1 << bitIndex % 64 ); +} + +static inline void b3ClearBit( b3BitSet* bitSet, uint32_t bitIndex ) +{ + uint32_t blockIndex = bitIndex / 64; + if ( blockIndex >= bitSet->blockCount ) + { + return; + } + bitSet->bits[blockIndex] &= ~( (uint64_t)1 << bitIndex % 64 ); +} + +static inline bool b3GetBit( const b3BitSet* bitSet, uint32_t bitIndex ) +{ + uint32_t blockIndex = bitIndex / 64; + if ( blockIndex >= bitSet->blockCount ) + { + return false; + } + return ( bitSet->bits[blockIndex] & ( (uint64_t)1 << bitIndex % 64 ) ) != 0; +} + +static inline int b3GetBitSetBytes( b3BitSet* bitSet ) +{ + return bitSet->blockCapacity * sizeof( uint64_t ); +} diff --git a/src/block_allocator.c b/src/block_allocator.c new file mode 100644 index 0000000..b008595 --- /dev/null +++ b/src/block_allocator.c @@ -0,0 +1,99 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "block_allocator.h" + +#include "core.h" + +b3BlockAllocator b3CreateBlockAllocator( int elementSize, int initialCount ) +{ + B3_ASSERT( elementSize >= (int)sizeof( void* ) ); + + b3BlockAllocator allocator = { 0 }; + + b3Array_Create( allocator.blocks ); + allocator.elementSize = elementSize; + allocator.freeList = NULL; + allocator.nextIndex = 0; + allocator.allocationCount = 0; + + if ( initialCount > 0 ) + { + int count = ( initialCount + B3_BLOCK_SIZE - 1 ) >> B3_BLOCK_EXPONENT; + b3Array_Resize( allocator.blocks, count ); + + for ( int i = 0; i < allocator.blocks.count; ++i ) + { + allocator.blocks.data[i].memory = b3Alloc( B3_BLOCK_SIZE * elementSize ); + } + } + + return allocator; +} + +void b3DestroyBlockAllocator( b3BlockAllocator* allocator ) +{ + for ( int i = 0; i < allocator->blocks.count; ++i ) + { + b3Free( allocator->blocks.data[i].memory, B3_BLOCK_SIZE * allocator->elementSize ); + } + + b3Array_Destroy( allocator->blocks ); +} + +void* b3AllocateElement( b3BlockAllocator* allocator ) +{ + B3_ASSERT( allocator != NULL ); + + allocator->allocationCount += 1; + + // Pop from free list first + if ( allocator->freeList != NULL ) + { + void* element = allocator->freeList; + allocator->freeList = *(void**)element; + return element; + } + + int startIndex = allocator->nextIndex; + int startOffset = startIndex & ( B3_BLOCK_SIZE - 1 ); + + if ( startOffset + 1 > B3_BLOCK_SIZE ) + { + startIndex = ( startIndex + B3_BLOCK_SIZE ) & ~( B3_BLOCK_SIZE - 1 ); + } + + allocator->nextIndex = startIndex + 1; + + int lastIndex = allocator->nextIndex - 1; + int requiredBlockCount = ( lastIndex >> B3_BLOCK_EXPONENT ) + 1; + + if ( requiredBlockCount > allocator->blocks.count ) + { + int oldCount = allocator->blocks.count; + b3Array_Resize( allocator->blocks, requiredBlockCount ); + + for ( int i = oldCount; i < requiredBlockCount; ++i ) + { + allocator->blocks.data[i].memory = b3Alloc( B3_BLOCK_SIZE * allocator->elementSize ); + } + } + + int blockIndex = startIndex >> B3_BLOCK_EXPONENT; + int blockOffset = startIndex & ( B3_BLOCK_SIZE - 1 ); + + return allocator->blocks.data[blockIndex].memory + blockOffset * allocator->elementSize; +} + +void b3FreeElement( b3BlockAllocator* allocator, void* element ) +{ + B3_ASSERT( allocator != NULL ); + B3_ASSERT( element != NULL ); + B3_ASSERT( allocator->allocationCount > 0 ); + + allocator->allocationCount -= 1; + + // Push onto free list + *(void**)element = allocator->freeList; + allocator->freeList = element; +} diff --git a/src/block_allocator.h b/src/block_allocator.h new file mode 100644 index 0000000..77ee538 --- /dev/null +++ b/src/block_allocator.h @@ -0,0 +1,44 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "container.h" + +#include "box3d/base.h" + +#define B3_BLOCK_EXPONENT 8 +#define B3_BLOCK_SIZE ( 1 << B3_BLOCK_EXPONENT ) + +typedef struct b3Block +{ + char* memory; +} b3Block; + +b3DeclareArray( b3Block ); + +typedef struct b3BlockAllocator +{ + b3ArrayC( b3Block ) blocks; + void* freeList; + int elementSize; + int nextIndex; + int allocationCount; +} b3BlockAllocator; + +#ifdef __cplusplus +extern "C" +{ +#endif + +// Element must be large enough to hold a pointer +b3BlockAllocator b3CreateBlockAllocator( int elementSize, int initialCount ); +void b3DestroyBlockAllocator( b3BlockAllocator* allocator ); + +// Elements are guaranteed contiguous. +void* b3AllocateElement( b3BlockAllocator* allocator ); +void b3FreeElement( b3BlockAllocator* allocator, void* element ); + +#ifdef __cplusplus +} +#endif diff --git a/src/body.c b/src/body.c new file mode 100644 index 0000000..d3e4e37 --- /dev/null +++ b/src/body.c @@ -0,0 +1,2323 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" + +#include "aabb.h" +#include "contact.h" +#include "core.h" +#include "id_pool.h" +#include "island.h" +#include "joint.h" +#include "physics_world.h" +#include "sensor.h" +#include "shape.h" +#include "solver_set.h" + +// needed for dll export +#include "box3d/box3d.h" +#include "box3d/id.h" + +#include +#include + +// Get a validated body from a world using an id. +b3Body* b3GetBodyFullId( b3World* world, b3BodyId bodyId ) +{ + B3_ASSERT( b3Body_IsValid( bodyId ) ); + + // id index starts at one so that zero can represent null + // id index starts at one so that zero can represent null + return b3Array_Get( world->bodies, bodyId.index1 - 1 ); +} + +b3Transform b3GetBodyTransformQuick( b3World* world, b3Body* body ) +{ + b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, body->localIndex ); + return bodySim->transform; +} + +b3Transform b3GetBodyTransform( b3World* world, int bodyId ) +{ + b3Body* body = b3Array_Get( world->bodies, bodyId ); + return b3GetBodyTransformQuick( world, body ); +} + +// Create a b3BodyId from a raw id. +b3BodyId b3MakeBodyId( b3World* world, int bodyId ) +{ + b3Body* body = b3Array_Get( world->bodies, bodyId ); + return (b3BodyId){ bodyId + 1, world->worldId, body->generation }; +} + +b3BodySim* b3GetBodySim( b3World* world, b3Body* body ) +{ + b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, body->localIndex ); + return bodySim; +} + +b3BodyState* b3GetBodyState( b3World* world, b3Body* body ) +{ + if ( body->setIndex == b3_awakeSet ) + { + b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + return b3Array_Get( set->bodyStates, body->localIndex ); + } + + return NULL; +} + +static void b3CreateIslandForBody( b3World* world, int setIndex, b3Body* body ) +{ + B3_ASSERT( body->islandId == B3_NULL_INDEX ); + B3_ASSERT( setIndex != b3_disabledSet ); + + b3Island* island = b3CreateIsland( world, setIndex ); + b3Array_Push( island->bodies, body->id ); + body->islandId = island->islandId; + body->islandIndex = 0; + + b3ValidateIsland( world, island->islandId ); +} + +static void b3RemoveBodyFromIsland( b3World* world, b3Body* body ) +{ + if ( body->islandId == B3_NULL_INDEX ) + { + B3_ASSERT( body->islandIndex == B3_NULL_INDEX ); + return; + } + + int islandId = body->islandId; + b3Island* island = b3Array_Get( world->islands, islandId ); + { + int localIndex = body->islandIndex; + int movedBodyId = island->bodies.data[island->bodies.count - 1]; + island->bodies.data[localIndex] = movedBodyId; + B3_VALIDATE( world->bodies.data[movedBodyId].islandIndex == island->bodies.count - 1 ); + world->bodies.data[movedBodyId].islandIndex = localIndex; + island->bodies.count -= 1; + } + + if ( island->bodies.count == 0 ) + { + // Destroy empty island + B3_ASSERT( island->contacts.count == 0 ); + B3_ASSERT( island->joints.count == 0 ); + + // Free the island + b3DestroyIsland( world, island->islandId ); + } + else + { + b3ValidateIsland( world, islandId ); + } + + body->islandId = B3_NULL_INDEX; + body->islandIndex = B3_NULL_INDEX; +} + +static void b3DestroyBodyContacts( b3World* world, b3Body* body, bool wakeBodies ) +{ + // Destroy the attached contacts + int edgeKey = body->headContactKey; + while ( edgeKey != B3_NULL_INDEX ) + { + int contactId = edgeKey >> 1; + int edgeIndex = edgeKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + edgeKey = contact->edges[edgeIndex].nextKey; + b3DestroyContact( world, contact, wakeBodies ); + } + + b3ValidateSolverSets( world ); +} + +b3BodyId b3CreateBody( b3WorldId worldId, const b3BodyDef* def ) +{ + B3_CHECK_DEF( def ); + B3_ASSERT( b3IsValidVec3( def->position ) ); + B3_ASSERT( b3IsValidQuat( def->rotation ) ); + B3_ASSERT( b3IsValidVec3( def->linearVelocity ) ); + B3_ASSERT( b3IsValidVec3( def->angularVelocity ) ); + B3_ASSERT( b3IsValidFloat( def->linearDamping ) && def->linearDamping >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->angularDamping ) && def->angularDamping >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->sleepThreshold ) && def->sleepThreshold >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->gravityScale ) ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + + if ( world == NULL ) + { + return b3_nullBodyId; + } + + world->locked = true; + + bool isAwake = ( def->isAwake || def->enableSleep == false ) && def->isEnabled; + + // determine the solver set + int setId; + if ( def->isEnabled == false ) + { + // any body type can be disabled + setId = b3_disabledSet; + } + else if ( def->type == b3_staticBody ) + { + setId = b3_staticSet; + } + else if ( isAwake == true ) + { + setId = b3_awakeSet; + } + else + { + // new set for a sleeping body in its own island + setId = b3AllocId( &world->solverSetIdPool ); + if ( setId == world->solverSets.count ) + { + // Create a zero initialized solver set. All sub-arrays are also zero initialized. + b3Array_Push( world->solverSets, (b3SolverSet){ 0 } ); + } + else + { + B3_ASSERT( world->solverSets.data[setId].setIndex == B3_NULL_INDEX ); + } + + world->solverSets.data[setId].setIndex = setId; + } + + B3_ASSERT( 0 <= setId && setId < world->solverSets.count ); + + int bodyId = b3AllocId( &world->bodyIdPool ); + + uint32_t lockFlags = 0; + lockFlags |= def->motionLocks.linearX ? b3_lockLinearX : 0; + lockFlags |= def->motionLocks.linearY ? b3_lockLinearY : 0; + lockFlags |= def->motionLocks.linearZ ? b3_lockLinearZ : 0; + lockFlags |= def->motionLocks.angularX ? b3_lockAngularX : 0; + lockFlags |= def->motionLocks.angularY ? b3_lockAngularY : 0; + lockFlags |= def->motionLocks.angularZ ? b3_lockAngularZ : 0; + + b3SolverSet* set = b3Array_Get( world->solverSets, setId ); + b3BodySim* bodySim = b3Array_Emplace( set->bodySims ); + *bodySim = (b3BodySim){ 0 }; + bodySim->transform.p = def->position; + bodySim->transform.q = def->rotation; + bodySim->center = def->position; + bodySim->rotation0 = bodySim->transform.q; + bodySim->center0 = bodySim->center; + bodySim->localCenter = b3Vec3_zero; + bodySim->force = b3Vec3_zero; + bodySim->torque = b3Vec3_zero; + bodySim->invMass = 0.0f; + bodySim->invInertiaLocal = b3Mat3_zero; + bodySim->minExtent = B3_HUGE; + bodySim->maxExtent = b3Vec3_zero; + bodySim->linearDamping = def->linearDamping; + bodySim->angularDamping = def->angularDamping; + bodySim->gravityScale = def->gravityScale; + bodySim->bodyId = bodyId; + bodySim->flags = lockFlags; + bodySim->flags |= def->isBullet ? b3_isBullet : 0; + bodySim->flags |= def->allowFastRotation ? b3_allowFastRotation : 0; + bodySim->flags |= def->type == b3_dynamicBody ? b3_dynamicFlag : 0; + + if ( setId == b3_awakeSet ) + { + b3BodyState* bodyState = b3Array_Emplace( set->bodyStates ); + + *bodyState = (b3BodyState){ 0 }; + bodyState->linearVelocity = def->linearVelocity; + bodyState->angularVelocity = def->angularVelocity; + bodyState->deltaRotation = b3Quat_identity; + bodyState->flags = bodySim->flags; + + bodySim->maxAngularVelocity = b3Length( def->angularVelocity ) + 5.0f; + } + + if ( bodyId == world->bodies.count ) + { + b3Array_Push( world->bodies, (b3Body){ 0 } ); + } + else + { + B3_ASSERT( world->bodies.data[bodyId].id == B3_NULL_INDEX ); + } + + b3Body* body = b3Array_Get( world->bodies, bodyId ); + + if ( def->name ) + { + int i = 0; + while ( i < B3_NAME_LENGTH - 1 && def->name[i] != 0 ) + { + body->name[i] = def->name[i]; + i += 1; + } + + while ( i < B3_NAME_LENGTH ) + { + body->name[i] = 0; + i += 1; + } + } + else + { + memset( body->name, 0, B3_NAME_LENGTH * sizeof( char ) ); + } + + body->userData = def->userData; + body->setIndex = setId; + body->localIndex = set->bodySims.count - 1; + body->generation += 1; + body->headShapeId = B3_NULL_INDEX; + body->shapeCount = 0; + body->headChainId = B3_NULL_INDEX; + body->headContactKey = B3_NULL_INDEX; + body->contactCount = 0; + body->headJointKey = B3_NULL_INDEX; + body->jointCount = 0; + body->islandId = B3_NULL_INDEX; + body->islandIndex = B3_NULL_INDEX; + body->bodyMoveIndex = B3_NULL_INDEX; + body->id = bodyId; + body->sleepThreshold = def->sleepThreshold; + body->sleepTime = 0.0f; + body->mass = 0.0f; + body->inertia = b3Mat3_zero; + body->type = def->type; + body->flags = bodySim->flags; + body->enableSleep = def->enableSleep; + + // dynamic and kinematic bodies that are enabled need a island + if ( setId >= b3_awakeSet ) + { + b3CreateIslandForBody( world, setId, body ); + } + + b3ValidateSolverSets( world ); + + b3BodyId id = { bodyId + 1, world->worldId, body->generation }; + + world->locked = false; + + return id; +} + +bool b3IsBodyAwake( b3World* world, b3Body* body ) +{ + B3_UNUSED( world ); + return body->setIndex == b3_awakeSet; +} + +bool b3WakeBody( b3World* world, b3Body* body ) +{ + if ( body->setIndex >= b3_firstSleepingSet ) + { + b3WakeSolverSet( world, body->setIndex ); + b3ValidateSolverSets( world ); + return true; + } + + return false; +} + +bool b3WakeBodyWithLock( b3World* world, b3Body* body ) +{ + B3_ASSERT( world->locked == false ); + world->locked = true; + bool woke = b3WakeBody( world, body ); + world->locked = false; + return woke; +} + +void b3DestroyBody( b3BodyId bodyId ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + world->locked = true; + + b3Body* body = b3GetBodyFullId( 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 != B3_NULL_INDEX ) + { + int jointId = edgeKey >> 1; + int edgeIndex = edgeKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + edgeKey = joint->edges[edgeIndex].nextKey; + + // Careful because this modifies the list being traversed + b3DestroyJointInternal( world, joint, wakeBodies ); + } + + // Destroy all contacts attached to this body. + b3DestroyBodyContacts( world, body, wakeBodies ); + + // Destroy the attached shapes and their broad-phase proxies. + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + if ( shape->sensorIndex != B3_NULL_INDEX ) + { + b3DestroySensor( world, shape ); + } + + b3DestroyShapeProxy( shape, &world->broadPhase ); + + b3DestroyShapeAllocations( world, shape ); + + // Return shape to free list. + b3FreeId( &world->shapeIdPool, shapeId ); + shape->id = B3_NULL_INDEX; + + shapeId = shape->nextShapeId; + } + + b3RemoveBodyFromIsland( world, body ); + + // Remove body sim from solver set that owns it + b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + int movedIndex = b3Array_RemoveSwap( set->bodySims, body->localIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fix moved body index + b3BodySim* movedSim = set->bodySims.data + body->localIndex; + int movedId = movedSim->bodyId; + b3Body* movedBody = b3Array_Get( world->bodies, movedId ); + B3_ASSERT( movedBody->localIndex == movedIndex ); + movedBody->localIndex = body->localIndex; + } + + // Remove body state from awake set + if ( body->setIndex == b3_awakeSet ) + { + int result = b3Array_RemoveSwap( set->bodyStates, body->localIndex ); + B3_UNUSED( result ); + B3_ASSERT( result == movedIndex ); + } + else if ( set->setIndex >= b3_firstSleepingSet && set->bodySims.count == 0 ) + { + // Remove solver set if it's now an orphan. + b3DestroySolverSet( world, set->setIndex ); + } + + // Free body and id (preserve body revision) + b3FreeId( &world->bodyIdPool, body->id ); + + body->setIndex = B3_NULL_INDEX; + body->localIndex = B3_NULL_INDEX; + body->id = B3_NULL_INDEX; + + b3ValidateSolverSets( world ); + + world->locked = false; +} + +int b3Body_GetContactCapacity( b3BodyId bodyId ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + // Conservative and fast + return body->contactCount; +} + +int b3Body_GetContactData( b3BodyId bodyId, b3ContactData* contactData, int capacity ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + int contactKey = body->headContactKey; + int index = 0; + while ( contactKey != B3_NULL_INDEX && index < capacity ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + + // Is contact touching? + if ( contact->flags & b3_contactTouchingFlag ) + { + b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); + b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB ); + + contactData[index].contactId = (b3ContactId){ contact->contactId + 1, bodyId.world0, 0, contact->generation }; + contactData[index].shapeIdA = (b3ShapeId){ shapeA->id + 1, bodyId.world0, shapeA->generation }; + contactData[index].shapeIdB = (b3ShapeId){ shapeB->id + 1, bodyId.world0, shapeB->generation }; + contactData[index].manifolds = contact->manifolds; + contactData[index].manifoldCount = contact->manifoldCount; + index += 1; + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + + B3_ASSERT( index <= capacity ); + + return index; +} + +b3AABB b3Body_ComputeAABB( b3BodyId bodyId ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return (b3AABB){ 0 }; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + if ( body->headShapeId == B3_NULL_INDEX ) + { + b3Transform transform = b3GetBodyTransform( world, body->id ); + return (b3AABB){ transform.p, transform.p }; + } + + b3Shape* shape = b3Array_Get( world->shapes, body->headShapeId ); + b3AABB aabb = shape->aabb; + while ( shape->nextShapeId != B3_NULL_INDEX ) + { + shape = b3Array_Get( world->shapes, shape->nextShapeId ); + aabb = b3AABB_Union( aabb, shape->aabb ); + } + + return aabb; +} + +float b3Body_GetClosestPoint( b3BodyId bodyId, b3Vec3* result, b3Vec3 target ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + *result = (b3Vec3){ 0 }; + return 0.0f; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3Transform transform = b3GetBodyTransform( world, body->id ); + + float closestDistance = FLT_MAX; + b3Vec3 closestPoint = transform.p; + + b3DistanceInput input = { 0 }; + input.proxyA = (b3ShapeProxy){ &target, 1, 0.0f }; + input.transformA.q = b3Quat_identity; + input.transformB = transform; + input.useRadii = false; + + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shapeId = shape->nextShapeId; + + b3ShapeType type = shape->type; + if ( type != b3_sphereShape && type != b3_capsuleShape && type != b3_hullShape ) + { + continue; + } + + input.proxyB = b3MakeShapeProxy( shape ); + + b3SimplexCache cache = { 0 }; + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + if ( output.distance < closestDistance ) + { + closestDistance = output.distance; + closestPoint = output.pointB; + } + } + + *result = closestPoint; + return closestDistance; +} + +b3BodyCastResult b3Body_CastRay( b3BodyId bodyId, const b3BodyRayCastInput* input, b3Transform bodyTransform ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return (b3BodyCastResult){ 0 }; + } + + b3BodyCastResult result = { 0 }; + b3Body* body = b3GetBodyFullId( world, bodyId ); + + b3RayCastInput shapeInput = { 0 }; + shapeInput.origin = input->origin; + shapeInput.translation = input->translation; + shapeInput.maxFraction = input->maxFraction; + + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shapeId = shape->nextShapeId; + + if ( b3ShouldQueryCollide( &shape->filter, &input->filter ) == false ) + { + continue; + } + + b3CastOutput shapeOutput = b3RayCastShape( shape, bodyTransform, &shapeInput ); + + if ( shapeOutput.hit == false ) + { + continue; + } + + if ( shapeOutput.fraction > shapeInput.maxFraction ) + { + continue; + } + + // Careful with id, shapeId is the next shape. + b3ShapeId id = { shape->id + 1, bodyId.world0, shape->generation }; + + int materialIndex = b3ClampInt( shapeOutput.materialIndex, 0, shape->materialCount - 1 ); + uint64_t userMaterialId = shape->materials[materialIndex].userMaterialId; + + result = (b3BodyCastResult){ + .shapeId = id, + .point = shapeOutput.point, + .normal = shapeOutput.normal, + .fraction = shapeOutput.fraction, + .triangleIndex = shapeOutput.triangleIndex, + .userMaterialId = userMaterialId, + .iterations = shapeOutput.iterations, + .hit = true, + }; + + shapeInput.maxFraction = shapeOutput.fraction; + } + + return result; +} + +b3BodyCastResult b3Body_CastShape( b3BodyId bodyId, const b3BodyShapeCastInput* input, b3Transform bodyTransform ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return (b3BodyCastResult){ 0 }; + } + + b3BodyCastResult result = { 0 }; + b3Body* body = b3GetBodyFullId( world, bodyId ); + + b3ShapeCastInput shapeInput = { 0 }; + shapeInput.proxy = input->proxy; + shapeInput.translation = input->translation; + shapeInput.maxFraction = input->maxFraction; + shapeInput.canEncroach = input->canEncroach; + + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shapeId = shape->nextShapeId; + + if ( b3ShouldQueryCollide( &shape->filter, &input->filter ) == false ) + { + continue; + } + + b3CastOutput shapeOutput = b3ShapeCastShape( shape, bodyTransform, &shapeInput ); + + if ( shapeOutput.hit == false ) + { + continue; + } + + if ( shapeOutput.fraction > shapeInput.maxFraction ) + { + continue; + } + + // Careful with id, shapeId is the next shape. + b3ShapeId id = { shape->id + 1, bodyId.world0, shape->generation }; + int materialIndex = b3ClampInt( shapeOutput.materialIndex, 0, shape->materialCount - 1 ); + uint64_t userMaterialId = shape->materials[materialIndex].userMaterialId; + + result = (b3BodyCastResult){ + .shapeId = id, + .point = shapeOutput.point, + .normal = shapeOutput.normal, + .fraction = shapeOutput.fraction, + .triangleIndex = shapeOutput.triangleIndex, + .userMaterialId = userMaterialId, + .iterations = shapeOutput.iterations, + .hit = true, + }; + + shapeInput.maxFraction = shapeOutput.fraction; + } + + return result; +} + +bool b3Body_OverlapShape( b3BodyId bodyId, const b3ShapeProxy* proxy, b3QueryFilter filter, b3Transform bodyTransform ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return false; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shapeId = shape->nextShapeId; + + if ( b3ShouldQueryCollide( &shape->filter, &filter ) == false ) + { + continue; + } + + bool overlaps = b3OverlapShape( shape, bodyTransform, proxy ); + if ( overlaps ) + { + return true; + } + } + + return false; +} + +int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int planeCapacity, const b3Capsule* mover, + b3QueryFilter filter, b3Transform bodyTransform ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return 0; + } + + if ( planeCapacity == 0 ) + { + return 0; + } + + int resultCount = 0; + b3Body* body = b3GetBodyFullId( world, bodyId ); + + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shapeId = shape->nextShapeId; + + if ( b3ShouldQueryCollide( &shape->filter, &filter ) == false ) + { + continue; + } + + b3ShapeType type = shape->type; + if ( type != b3_sphereShape && type != b3_capsuleShape && type != b3_hullShape ) + { + continue; + } + + b3PlaneResult plane; + int count = b3CollideMover( &plane, 1, shape, bodyTransform, mover ); + + if ( count > 0 ) + { + b3ShapeId id = { shape->id + 1, world->worldId, shape->generation }; + bodyPlanes[resultCount] = (b3BodyPlaneResult){ .shapeId = id, .result = plane }; + resultCount += 1; + if ( resultCount == planeCapacity ) + { + return resultCount; + } + } + } + + return resultCount; +} + +void b3UpdateBodyMassData( b3World* world, b3Body* body ) +{ + b3BodySim* bodySim = b3GetBodySim( world, body ); + + // Compute mass data from shapes. Each shape has its own density. + body->mass = 0.0f; + body->inertia = b3Mat3_zero; + + bodySim->invMass = 0.0f; + bodySim->invInertiaLocal = b3Mat3_zero; + bodySim->invInertiaWorld = b3Mat3_zero; + bodySim->localCenter = b3Vec3_zero; + bodySim->minExtent = B3_HUGE; + bodySim->maxExtent = b3Vec3_zero; + + if ( body->headShapeId == B3_NULL_INDEX ) + { + return; + } + + // Static and kinematic sims have zero mass. + if ( body->type != b3_dynamicBody ) + { + bodySim->center = bodySim->transform.p; + bodySim->center0 = bodySim->center; + + // Need extents for kinematic bodies for sleeping to work correctly. + if ( body->type == b3_kinematicBody ) + { + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + const b3Shape* s = b3Array_Get( world->shapes, shapeId ); + + b3ShapeExtent extent = b3ComputeShapeExtent( s, b3Vec3_zero ); + bodySim->minExtent = b3MinFloat( bodySim->minExtent, extent.minExtent ); + bodySim->maxExtent = b3Max( bodySim->maxExtent, extent.maxExtent ); + + shapeId = s->nextShapeId; + } + } + + return; + } + + // Accumulate mass over all shapes. + b3Vec3 localCenter = b3Vec3_zero; + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + const b3Shape* s = b3Array_Get( world->shapes, shapeId ); + shapeId = s->nextShapeId; + + if ( s->density == 0.0f ) + { + continue; + } + + b3MassData massData = b3ComputeShapeMass( s ); + body->mass += massData.mass; + localCenter = b3MulAdd( localCenter, massData.mass, massData.center ); + body->inertia = b3AddMM( body->inertia, massData.inertia ); + } + + // Compute center of mass. + if ( body->mass > 0.0f ) + { + bodySim->invMass = 1.0f / body->mass; + localCenter = b3MulSV( bodySim->invMass, localCenter ); + + // Shift inertia to mass center + body->inertia = b3SubMM( body->inertia, b3Steiner( body->mass, localCenter ) ); + + // This call is faster than b3Invert + bodySim->invInertiaLocal = b3InvertT( body->inertia ); + + b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( bodySim->transform.q ); + bodySim->invInertiaWorld = b3MulMM( b3MulMM( rotationMatrix, bodySim->invInertiaLocal ), b3Transpose( rotationMatrix ) ); + } + + // Move center of mass. + b3Vec3 oldCenter = bodySim->center; + bodySim->localCenter = localCenter; + bodySim->center = b3TransformPoint( bodySim->transform, bodySim->localCenter ); + bodySim->center0 = bodySim->center; + + // Update center of mass velocity + b3BodyState* state = b3GetBodyState( world, body ); + if ( state != NULL ) + { + b3Vec3 deltaLinear = b3Cross( state->angularVelocity, b3Sub( bodySim->center, oldCenter ) ); + state->linearVelocity = b3Add( state->linearVelocity, deltaLinear ); + } + + // Compute body extents relative to center of mass + shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* s = b3Array_Get( world->shapes, shapeId ); + + b3ShapeExtent extent = b3ComputeShapeExtent( s, localCenter ); + bodySim->minExtent = b3MinFloat( bodySim->minExtent, extent.minExtent ); + bodySim->maxExtent = b3Max( bodySim->maxExtent, extent.maxExtent ); + + shapeId = s->nextShapeId; + } +} + +void b3DumpBody( b3World* world, b3Body* body ) +{ + // int32 bodyIndex = m_islandIndex; + + b3BodySim* sim = b3GetBodySim( world, body ); + b3Vec3 p = sim->transform.p; + b3Quat q = sim->transform.q; + + b3Vec3 v = b3Vec3_zero; + b3Vec3 w = b3Vec3_zero; + b3BodyState* state = b3GetBodyState( world, body ); + if ( state != NULL ) + { + v = state->linearVelocity; + w = state->angularVelocity; + } + + // %.9g is sufficient to save and load the same value using text + // FLT_DECIMAL_DIG == 9 + b3Dump( "{\n" ); + b3Dump( " b3BodyDef bd = b3DefaultBodyDef();\n" ); + if ( body->name[0] != 0 ) + { + b3Dump( " bd.name = \"%s\";\n", body->name ); + } + b3Dump( " bd.type = b3BodyType(%d);\n", body->type ); + b3Dump( " bd.position = {%.9g, %.9g, %.9g};\n", p.x, p.y, p.z ); + b3Dump( " bd.rotation = {{%.9g, %.9g, %.9g}, %.9g};\n", q.v.x, q.v.y, q.v.z, q.s ); + b3Dump( " bd.linearVelocity = {%.9g, %.9g, %.9g};\n", v.x, v.y, v.z ); + b3Dump( " bd.angularVelocity = {%.9g, %.9g, %.9g};\n", w.x, w.y, w.z ); + b3Dump( " bd.linearDamping = %.9g;\n", sim->linearDamping ); + b3Dump( " bd.angularDamping = %.9g;\n", sim->angularDamping ); + b3Dump( " bd.enableSleep = bool(%d);\n", body->enableSleep ); + b3Dump( " bd.isAwake = bool(%d);\n", body->setIndex == b3_awakeSet ); + b3Dump( " bd.gravityScale = %.9g;\n", sim->gravityScale ); + b3Dump( " b3BodyId bodyId = b3CreateBody(m_worldId, &bd);\n" ); + b3Dump( " bodies.push_back(bodyId);\n" ); + b3Dump( "\n" ); + + int shapeIndex = body->headShapeId; + while ( shapeIndex != B3_NULL_INDEX ) + { + b3Dump( " {\n" ); + b3DumpShape( world, shapeIndex ); + b3Dump( " }\n" ); + + b3Shape* shape = b3Array_Get( world->shapes, shapeIndex ); + shapeIndex = shape->nextShapeId; + } + b3Dump( "}\n" ); +} + +b3Vec3 b3Body_GetPosition( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3Transform transform = b3GetBodyTransformQuick( world, body ); + return transform.p; +} + +b3Quat b3Body_GetRotation( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3Transform transform = b3GetBodyTransformQuick( world, body ); + return transform.q; +} + +b3Transform b3Body_GetTransform( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return b3GetBodyTransformQuick( world, body ); +} + +b3Vec3 b3Body_GetLocalPoint( b3BodyId bodyId, b3Vec3 worldPoint ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3Transform transform = b3GetBodyTransformQuick( world, body ); + return b3InvTransformPoint( transform, worldPoint ); +} + +b3Vec3 b3Body_GetWorldPoint( b3BodyId bodyId, b3Vec3 localPoint ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3Transform transform = b3GetBodyTransformQuick( world, body ); + return b3TransformPoint( transform, localPoint ); +} + +b3Vec3 b3Body_GetLocalVector( b3BodyId bodyId, b3Vec3 worldVector ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3Transform transform = b3GetBodyTransformQuick( world, body ); + return b3InvRotateVector( transform.q, worldVector ); +} + +b3Vec3 b3Body_GetWorldVector( b3BodyId bodyId, b3Vec3 localVector ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3Transform transform = b3GetBodyTransformQuick( world, body ); + return b3RotateVector( transform.q, localVector ); +} + +void b3Body_SetTransform( b3BodyId bodyId, b3Vec3 position, b3Quat rotation ) +{ + B3_ASSERT( b3IsValidVec3( position ) ); + B3_ASSERT( b3IsValidQuat( rotation ) ); + B3_ASSERT( b3Body_IsValid( bodyId ) ); + b3World* world = b3GetWorld( bodyId.world0 ); + B3_ASSERT( world->locked == false ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + + bodySim->transform.p = position; + bodySim->transform.q = rotation; + bodySim->center = b3TransformPoint( bodySim->transform, bodySim->localCenter ); + + b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( bodySim->transform.q ); + bodySim->invInertiaWorld = b3MulMM( b3MulMM( rotationMatrix, bodySim->invInertiaLocal ), b3Transpose( rotationMatrix ) ); + + bodySim->rotation0 = bodySim->transform.q; + bodySim->center0 = bodySim->center; + + b3BroadPhase* broadPhase = &world->broadPhase; + + b3Transform transform = bodySim->transform; + const float margin = B3_AABB_MARGIN; + const float speculativeDistance = B3_SPECULATIVE_DISTANCE; + + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + b3AABB aabb = b3ComputeShapeAABB( shape, transform ); + aabb.lowerBound.x -= speculativeDistance; + aabb.lowerBound.y -= speculativeDistance; + aabb.lowerBound.z -= speculativeDistance; + aabb.upperBound.x += speculativeDistance; + aabb.upperBound.y += speculativeDistance; + aabb.upperBound.z += speculativeDistance; + shape->aabb = aabb; + + if ( b3AABB_Contains( shape->fatAABB, aabb ) == false ) + { + b3AABB fatAABB; + fatAABB.lowerBound.x = aabb.lowerBound.x - margin; + fatAABB.lowerBound.y = aabb.lowerBound.y - margin; + fatAABB.lowerBound.z = aabb.lowerBound.z - margin; + fatAABB.upperBound.x = aabb.upperBound.x + margin; + fatAABB.upperBound.y = aabb.upperBound.y + margin; + fatAABB.upperBound.z = aabb.upperBound.z + margin; + shape->fatAABB = fatAABB; + + // The body could be disabled + if ( shape->proxyKey != B3_NULL_INDEX ) + { + b3BroadPhase_MoveProxy( broadPhase, shape->proxyKey, fatAABB ); + } + } + + shapeId = shape->nextShapeId; + } +} + +b3Vec3 b3Body_GetLinearVelocity( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodyState* state = b3GetBodyState( world, body ); + if ( state != NULL ) + { + return state->linearVelocity; + } + return b3Vec3_zero; +} + +b3Vec3 b3Body_GetAngularVelocity( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodyState* state = b3GetBodyState( world, body ); + if ( state != NULL ) + { + return state->angularVelocity; + } + return b3Vec3_zero; +} + +void b3Body_SetLinearVelocity( b3BodyId bodyId, b3Vec3 linearVelocity ) +{ + B3_ASSERT( b3IsValidVec3( linearVelocity ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( body->type == b3_staticBody ) + { + return; + } + + if ( b3LengthSquared( linearVelocity ) > 0.0f ) + { + b3WakeBodyWithLock( world, body ); + } + + b3BodyState* state = b3GetBodyState( world, body ); + if ( state == NULL ) + { + return; + } + + state->linearVelocity = linearVelocity; +} + +void b3Body_SetAngularVelocity( b3BodyId bodyId, b3Vec3 angularVelocity ) +{ + B3_ASSERT( b3IsValidVec3( angularVelocity ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( body->type == b3_staticBody ) + { + return; + } + + // Apply locks to avoid waking + b3Vec3 w; + w.x = ( body->flags & b3_lockAngularX ) ? 0.0f : angularVelocity.x; + w.y = ( body->flags & b3_lockAngularY ) ? 0.0f : angularVelocity.y; + w.z = ( body->flags & b3_lockAngularZ ) ? 0.0f : angularVelocity.z; + + if ( b3LengthSquared( w ) != 0.0f ) + { + b3WakeBodyWithLock( world, body ); + } + + b3BodyState* state = b3GetBodyState( world, body ); + if ( state == NULL ) + { + return; + } + + state->angularVelocity = w; +} + +void b3Body_SetTargetTransform( b3BodyId bodyId, b3Transform target, float timeStep, bool wake ) +{ + B3_ASSERT( b3IsValidTransform( target ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( body->setIndex == b3_disabledSet ) + { + return; + } + + if ( body->type == b3_staticBody || timeStep <= 0.0f ) + { + return; + } + + if ( body->setIndex != b3_awakeSet && wake == false ) + { + return; + } + + b3BodySim* sim = b3GetBodySim( world, body ); + + // Compute linear velocity + b3Vec3 center1 = sim->center; + b3Vec3 center2 = b3TransformPoint( target, sim->localCenter ); + float invTimeStep = 1.0f / timeStep; + b3Vec3 linearVelocity = b3MulSV( invTimeStep, b3Sub( center2, center1 ) ); + + // Compute angular velocity: + // q' = 0.5 * w * q + // <~> ( q2 - q1 ) / dt = 0.5 * w * q1 + // <=> w = 2 * ( q2 - q1 ) * Conjugate( q1 ) / dt + b3Quat q1 = sim->transform.q; + b3Quat q2 = target.q; + + // Use the shortest arc quaternion + if ( b3DotQuat( q1, q2 ) < 0.0f ) + { + q2 = b3NegateQuat( q2 ); + } + + b3Quat dq = { b3Sub( q2.v, q1.v ), q2.s - q1.s }; + b3Quat omega = b3MulQuat( dq, b3Conjugate( q1 ) ); + b3Vec3 angularVelocity = b3MulSV( 2.0f * invTimeStep, omega.v ); + + // Early out if the body is asleep already and the desired movement is small + if ( body->setIndex != b3_awakeSet ) + { + float maxVelocity = b3Length( linearVelocity ) + b3Length( b3Mul( angularVelocity, sim->maxExtent ) ); + + // Return if velocity would be sleepy + if ( maxVelocity < body->sleepThreshold ) + { + return; + } + + // Must wake for state to exist + b3WakeBodyWithLock( world, body ); + } + + B3_ASSERT( body->setIndex == b3_awakeSet ); + + b3BodyState* state = b3GetBodyState( world, body ); + state->linearVelocity = linearVelocity; + state->angularVelocity = angularVelocity; +} + +b3Vec3 b3Body_GetLocalPointVelocity( b3BodyId bodyId, b3Vec3 localPoint ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodyState* state = b3GetBodyState( world, body ); + if ( state == NULL ) + { + return b3Vec3_zero; + } + + b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, body->localIndex ); + + b3Vec3 r = b3RotateVector( bodySim->transform.q, b3Sub( localPoint, bodySim->localCenter ) ); + b3Vec3 v = b3Add( state->linearVelocity, b3Cross( state->angularVelocity, r ) ); + return v; +} + +b3Vec3 b3Body_GetWorldPointVelocity( b3BodyId bodyId, b3Vec3 worldPoint ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodyState* state = b3GetBodyState( world, body ); + if ( state == NULL ) + { + return b3Vec3_zero; + } + + b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, body->localIndex ); + + b3Vec3 r = b3Sub( worldPoint, bodySim->center ); + b3Vec3 v = b3Add( state->linearVelocity, b3Cross( state->angularVelocity, r ) ); + return v; +} + +void b3Body_ApplyForce( b3BodyId bodyId, b3Vec3 force, b3Vec3 point, bool wake ) +{ + B3_ASSERT( b3IsValidVec3( force ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b3_firstSleepingSet ) + { + b3WakeBodyWithLock( world, body ); + } + + if ( body->setIndex == b3_awakeSet ) + { + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->force = b3Add( bodySim->force, force ); + bodySim->torque = b3Add( bodySim->torque, b3Cross( b3Sub( point, bodySim->center ), force ) ); + } +} + +void b3Body_ApplyForceToCenter( b3BodyId bodyId, b3Vec3 force, bool wake ) +{ + B3_ASSERT( b3IsValidVec3( force ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b3_firstSleepingSet ) + { + b3WakeBodyWithLock( world, body ); + } + + if ( body->setIndex == b3_awakeSet ) + { + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->force = b3Add( bodySim->force, force ); + } +} + +void b3Body_ApplyTorque( b3BodyId bodyId, b3Vec3 torque, bool wake ) +{ + B3_ASSERT( b3IsValidVec3( torque ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b3_firstSleepingSet ) + { + b3WakeBodyWithLock( world, body ); + } + + if ( body->setIndex == b3_awakeSet ) + { + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->torque = b3Add( bodySim->torque, torque ); + } +} + +void b3Body_ApplyLinearImpulse( b3BodyId bodyId, b3Vec3 impulse, b3Vec3 point, bool wake ) +{ + B3_ASSERT( b3IsValidVec3( impulse ) ); + B3_ASSERT( b3IsValidVec3( point ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b3_firstSleepingSet ) + { + b3WakeBodyWithLock( world, body ); + } + + if ( body->setIndex == b3_awakeSet ) + { + int localIndex = body->localIndex; + b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + b3BodyState* state = b3Array_Get( set->bodyStates, localIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, localIndex ); + + state->linearVelocity = b3MulAdd( state->linearVelocity, bodySim->invMass, impulse ); + + float maxLinearSpeed = world->maxLinearSpeed; + if ( b3LengthSquared( state->linearVelocity ) > maxLinearSpeed * maxLinearSpeed ) + { + state->linearVelocity = b3MulSV( maxLinearSpeed, b3Normalize( state->linearVelocity ) ); + } + + b3Vec3 delta = b3MulMV( bodySim->invInertiaWorld, b3Cross( b3Sub( point, bodySim->center ), impulse ) ); + state->angularVelocity = b3Add( state->angularVelocity, delta ); + } +} + +void b3Body_ApplyLinearImpulseToCenter( b3BodyId bodyId, b3Vec3 impulse, bool wake ) +{ + B3_ASSERT( b3IsValidVec3( impulse ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b3_firstSleepingSet ) + { + b3WakeBodyWithLock( world, body ); + } + + if ( body->setIndex == b3_awakeSet ) + { + int localIndex = body->localIndex; + b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + b3BodyState* state = b3Array_Get( set->bodyStates, localIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, localIndex ); + state->linearVelocity = b3MulAdd( state->linearVelocity, bodySim->invMass, impulse ); + + float maxLinearSpeed = world->maxLinearSpeed; + if ( b3LengthSquared( state->linearVelocity ) > maxLinearSpeed * maxLinearSpeed ) + { + state->linearVelocity = b3MulSV( maxLinearSpeed, b3Normalize( state->linearVelocity ) ); + } + } +} + +void b3Body_ApplyAngularImpulse( b3BodyId bodyId, b3Vec3 impulse, bool wake ) +{ + B3_ASSERT( b3IsValidVec3( impulse ) ); + B3_ASSERT( b3Body_IsValid( bodyId ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + + int id = bodyId.index1 - 1; + b3Body* body = b3Array_Get( world->bodies, id ); + B3_ASSERT( body->generation == bodyId.generation ); + + if ( wake && body->setIndex >= b3_firstSleepingSet ) + { + // this will not invalidate body pointer + b3WakeBodyWithLock( world, body ); + } + + if ( body->setIndex == b3_awakeSet ) + { + int localIndex = body->localIndex; + b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + b3BodyState* state = b3Array_Get( set->bodyStates, localIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, localIndex ); + + b3Vec3 localImpulse = b3InvRotateVector( bodySim->transform.q, impulse ); + b3Vec3 localAngularVelocityDelta = b3MulMV( bodySim->invInertiaLocal, localImpulse ); + state->angularVelocity = + b3Add( state->angularVelocity, b3RotateVector( bodySim->transform.q, localAngularVelocityDelta ) ); + } +} + +b3BodyType b3Body_GetType( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->type; +} + +// This should follow similar steps as you would get destroying and recreating the body, shapes, and joints. +// Contacts are difficult to preserve because the broad-phase pairs change, so I just destroy them. +// todo with a bit more effort I could support an option to let the body sleep +// +// Revised steps: +// 1 Skip disabled bodies +// 2 Destroy all contacts on the body +// 3 Wake the body +// 4 For all joints attached to the body +// - wake attached bodies +// - remove from island +// - move to static set temporarily +// 5 Change the body type and transfer the body +// 6 If the body was static +// - create an island for the body +// Else if the body is becoming static +// - remove it from the island +// 7 For all joints +// - if either body is non-static +// - link into island +// - transfer to constraint graph +// 8 For all shapes +// - Destroy proxy in old tree +// - Create proxy in new tree +// Notes: +// - the implementation below tries to minimize the number of predicates, so some +// operations may have no effect, such as transferring a joint to the same set +void b3Body_SetType( b3BodyId bodyId, b3BodyType type ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + world->locked = true; + b3Body* body = b3GetBodyFullId( world, bodyId ); + + b3BodyType originalType = body->type; + if ( originalType == type ) + { + world->locked = false; + return; + } + + if ( type != b3_staticBody ) + { + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + if ( shape->type == b3_compoundShape || shape->type == b3_heightShape ) + { + // Setting the body type is not supported for bodies with compound shapes + return; + } + + shapeId = shape->nextShapeId; + } + } + + // Stage 1: skip disabled bodies + if ( body->setIndex == b3_disabledSet ) + { + // Disabled bodies don't change solver sets or islands when they change type. + body->type = type; + + if ( type == b3_dynamicBody ) + { + body->flags |= b3_dynamicFlag; + } + else + { + body->flags &= ~b3_dynamicFlag; + } + + // Body type affects the mass properties + b3UpdateBodyMassData( world, body ); + world->locked = false; + return; + } + + // Stage 2: destroy all contacts but don't wake bodies (because we don't need to) + bool wakeBodies = false; + b3DestroyBodyContacts( world, body, wakeBodies ); + + // Stage 3: wake this body (does nothing if body is static), otherwise it will also wake + // all bodies in the same sleeping solver set. + b3WakeBody( world, body ); + + // Stage 4: move joints to temporary storage + b3SolverSet* staticSet = b3Array_Get( world->solverSets, b3_staticSet ); + + int jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + jointKey = joint->edges[edgeIndex].nextKey; + + // Joint may be disabled by other body + if ( joint->setIndex == b3_disabledSet ) + { + continue; + } + + // Wake attached bodies. The b3WakeBody call above does not wake bodies + // attached to a static body. But it is necessary because the body may have + // no joints. + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + b3WakeBody( world, bodyA ); + b3WakeBody( world, bodyB ); + + // Remove joint from island + b3UnlinkJoint( world, joint ); + + // It is necessary to transfer all joints to the static set + // so they can be added to the constraint graph below and acquire consistent colors. + b3SolverSet* jointSourceSet = b3Array_Get( world->solverSets, joint->setIndex ); + b3TransferJoint( world, staticSet, jointSourceSet, joint ); + } + + // Stage 5: change the body type and transfer body + body->type = type; + + if ( type == b3_dynamicBody ) + { + body->flags |= b3_dynamicFlag; + } + else + { + body->flags &= ~b3_dynamicFlag; + } + + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + b3SolverSet* sourceSet = b3Array_Get( world->solverSets, body->setIndex ); + b3SolverSet* targetSet = type == b3_staticBody ? staticSet : awakeSet; + + // Transfer body + b3TransferBody( world, targetSet, sourceSet, body ); + + // Stage 6: update island participation for the body + if ( originalType == b3_staticBody ) + { + // Create island for body + b3CreateIslandForBody( world, b3_awakeSet, body ); + } + else if ( type == b3_staticBody ) + { + // Remove body from island. + b3RemoveBodyFromIsland( world, body ); + } + + // Stage 7: Transfer joints to the target set + jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + + jointKey = joint->edges[edgeIndex].nextKey; + + // Joint may be disabled by other body + if ( joint->setIndex == b3_disabledSet ) + { + continue; + } + + // All joints were transferred to the static set in an earlier stage + B3_ASSERT( joint->setIndex == b3_staticSet ); + + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + B3_ASSERT( bodyA->setIndex == b3_staticSet || bodyA->setIndex == b3_awakeSet ); + B3_ASSERT( bodyB->setIndex == b3_staticSet || bodyB->setIndex == b3_awakeSet ); + + if ( bodyA->type == b3_dynamicBody || bodyB->type == b3_dynamicBody ) + { + b3TransferJoint( world, awakeSet, staticSet, joint ); + } + } + + // Recreate shape proxies in broadphase + b3Transform transform = b3GetBodyTransformQuick( world, body ); + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + // Setting the body type is not supported for bodies with compound shapes + B3_ASSERT( shape->type != b3_compoundShape ); + + shapeId = shape->nextShapeId; + b3DestroyShapeProxy( shape, &world->broadPhase ); + bool forcePairCreation = true; + b3CreateShapeProxy( shape, &world->broadPhase, type, transform, forcePairCreation ); + } + + // Relink all joints + jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + jointKey = joint->edges[edgeIndex].nextKey; + + int otherEdgeIndex = edgeIndex ^ 1; + int otherBodyId = joint->edges[otherEdgeIndex].bodyId; + b3Body* otherBody = b3Array_Get( world->bodies, otherBodyId ); + + if ( otherBody->setIndex == b3_disabledSet ) + { + continue; + } + + if ( body->type != b3_dynamicBody && otherBody->type != b3_dynamicBody ) + { + continue; + } + + b3LinkJoint( world, joint ); + } + + // Body type affects the mass + b3UpdateBodyMassData( world, body ); + + b3BodyState* state = b3GetBodyState( world, body ); + if ( state != NULL ) + { + // Ensure flags are in sync (b2_skipSolverWrite) + state->flags = body->flags; + } + + b3ValidateSolverSets( world ); + b3ValidateIsland( world, body->islandId ); + + world->locked = false; +} + +void b3Body_SetName( b3BodyId bodyId, const char* name ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( name ) + { + int i = 0; + while ( i < B3_NAME_LENGTH - 1 && name[i] != 0 ) + { + body->name[i] = name[i]; + i += 1; + } + + while ( i < B3_NAME_LENGTH ) + { + body->name[i] = 0; + i += 1; + } + } + else + { + memset( body->name, 0, B3_NAME_LENGTH * sizeof( char ) ); + } +} + +const char* b3Body_GetName( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->name; +} + +void b3Body_SetUserData( b3BodyId bodyId, void* userData ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + body->userData = userData; +} + +void* b3Body_GetUserData( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->userData; +} + +float b3Body_GetMass( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->mass; +} + +b3Matrix3 b3Body_GetLocalRotationalInertia( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->inertia; +} + +float b3Body_GetInverseMass( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* sim = b3GetBodySim( world, body ); + return sim->invMass; +} + +b3Matrix3 b3Body_GetWorldInverseRotationalInertia( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* sim = b3GetBodySim( world, body ); + return sim->invInertiaWorld; +} + +b3Vec3 b3Body_GetLocalCenterOfMass( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + return bodySim->localCenter; +} + +b3Vec3 b3Body_GetWorldCenterOfMass( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + return bodySim->center; +} + +void b3Body_SetMassData( b3BodyId bodyId, b3MassData massData ) +{ + B3_ASSERT( b3IsValidFloat( massData.mass ) && massData.mass >= 0.0f ); + B3_ASSERT( b3IsValidMatrix3( massData.inertia ) ); + B3_ASSERT( b3IsValidVec3( massData.center ) ); + + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + + body->mass = massData.mass; + body->inertia = massData.inertia; + bodySim->localCenter = massData.center; + + b3Vec3 center = b3TransformPoint( bodySim->transform, massData.center ); + bodySim->center = center; + bodySim->center0 = center; + + bodySim->invMass = body->mass > 0.0f ? 1.0f / body->mass : 0.0f; + bodySim->invInertiaLocal = b3Det( body->inertia ) > 0.0f ? b3InvertT( body->inertia ) : b3Mat3_zero; +} + +b3MassData b3Body_GetMassData( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + b3MassData massData = { body->mass, bodySim->localCenter, body->inertia }; + return massData; +} + +void b3Body_ApplyMassFromShapes( b3BodyId bodyId ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3UpdateBodyMassData( world, body ); +} + +void b3Body_SetLinearDamping( b3BodyId bodyId, float linearDamping ) +{ + B3_ASSERT( b3IsValidFloat( linearDamping ) && linearDamping >= 0.0f ); + + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->linearDamping = linearDamping; +} + +float b3Body_GetLinearDamping( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + return bodySim->linearDamping; +} + +void b3Body_SetAngularDamping( b3BodyId bodyId, float angularDamping ) +{ + B3_ASSERT( b3IsValidFloat( angularDamping ) && angularDamping >= 0.0f ); + + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->angularDamping = angularDamping; +} + +float b3Body_GetAngularDamping( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + return bodySim->angularDamping; +} + +void b3Body_SetGravityScale( b3BodyId bodyId, float gravityScale ) +{ + B3_ASSERT( b3Body_IsValid( bodyId ) ); + B3_ASSERT( b3IsValidFloat( gravityScale ) ); + + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->gravityScale = gravityScale; +} + +float b3Body_GetGravityScale( b3BodyId bodyId ) +{ + B3_ASSERT( b3Body_IsValid( bodyId ) ); + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + return bodySim->gravityScale; +} + +bool b3Body_IsAwake( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->setIndex == b3_awakeSet; +} + +void b3Body_SetAwake( b3BodyId bodyId, bool awake ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + world->locked = true; + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( awake && body->setIndex >= b3_firstSleepingSet ) + { + b3WakeBody( world, body ); + } + else if ( awake == false && body->setIndex == b3_awakeSet ) + { + b3Island* island = b3Array_Get( world->islands, body->islandId ); + if ( island->constraintRemoveCount > 0 ) + { + // Must split the island before sleeping. This is expensive. + b3SplitIsland( world, body->islandId ); + } + + b3TrySleepIsland( world, body->islandId ); + } + + world->locked = false; +} + +bool b3Body_IsEnabled( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->setIndex != b3_disabledSet; +} + +bool b3Body_IsSleepEnabled( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->enableSleep; +} + +void b3Body_SetSleepThreshold( b3BodyId bodyId, float sleepThreshold ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + body->sleepThreshold = sleepThreshold; +} + +float b3Body_GetSleepThreshold( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->sleepThreshold; +} + +void b3Body_EnableSleep( b3BodyId bodyId, bool enableSleep ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + world->locked = true; + + b3Body* body = b3GetBodyFullId( world, bodyId ); + body->enableSleep = enableSleep; + + if ( enableSleep == false ) + { + b3WakeBody( world, body ); + } + + world->locked = false; +} + +// Disabling a body requires a lot of detailed bookkeeping, but it is a valuable feature. +// The most challenging aspect that joints may connect to bodies that are not disabled. +void b3Body_Disable( b3BodyId bodyId ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + world->locked = true; + + b3Body* body = b3GetBodyFullId( world, bodyId ); + if ( body->setIndex == b3_disabledSet ) + { + world->locked = false; + return; + } + + // Destroy contacts and wake bodies touching this body. This avoid floating bodies. + // This is necessary even for static bodies. + bool wakeBodies = true; + b3DestroyBodyContacts( world, body, wakeBodies ); + + // The current solver set of the body + b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + + // Disabled bodies and connected joints are moved to the disabled set + b3SolverSet* disabledSet = b3Array_Get( world->solverSets, b3_disabledSet ); + + // Unlink joints and transfer them to the disabled set + int jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + jointKey = joint->edges[edgeIndex].nextKey; + + // joint may already be disabled by other body + if ( joint->setIndex == b3_disabledSet ) + { + continue; + } + + B3_ASSERT( joint->setIndex == set->setIndex || set->setIndex == b3_staticSet ); + + // Remove joint from island + b3UnlinkJoint( world, joint ); + + // Transfer joint to disabled set + b3SolverSet* jointSet = b3Array_Get( world->solverSets, joint->setIndex ); + b3TransferJoint( world, disabledSet, jointSet, joint ); + } + + // Remove shapes from broad-phase + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shapeId = shape->nextShapeId; + b3DestroyShapeProxy( shape, &world->broadPhase ); + } + + // Disabled bodies are not in an island. If the island becomes empty it will be destroyed. + b3RemoveBodyFromIsland( world, body ); + + // Transfer body sim + b3TransferBody( world, disabledSet, set, body ); + + b3ValidateConnectivity( world ); + b3ValidateSolverSets( world ); + + world->locked = false; +} + +void b3Body_Enable( b3BodyId bodyId ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + if ( body->setIndex != b3_disabledSet ) + { + return; + } + + b3SolverSet* disabledSet = b3Array_Get( world->solverSets, b3_disabledSet ); + int setId = body->type == b3_staticBody ? b3_staticSet : b3_awakeSet; + b3SolverSet* targetSet = b3Array_Get( world->solverSets, setId ); + + b3TransferBody( world, targetSet, disabledSet, body ); + + b3Transform transform = b3GetBodyTransformQuick( world, body ); + + // Add shapes to broad-phase + b3BodyType proxyType = body->type; + bool forcePairCreation = true; + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shapeId = shape->nextShapeId; + + b3CreateShapeProxy( shape, &world->broadPhase, proxyType, transform, forcePairCreation ); + } + + if ( setId != b3_staticSet ) + { + b3CreateIslandForBody( world, setId, body ); + } + + // Transfer joints. If the other body is disabled, don't transfer. + // If the other body is sleeping, wake it. + int jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + B3_ASSERT( joint->setIndex == b3_disabledSet ); + B3_ASSERT( joint->islandId == B3_NULL_INDEX ); + + jointKey = joint->edges[edgeIndex].nextKey; + + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + + if ( bodyA->setIndex == b3_disabledSet || bodyB->setIndex == b3_disabledSet ) + { + // one body is still disabled + continue; + } + + // Transfer joint first + int jointSetId; + if ( bodyA->setIndex == b3_staticSet && bodyB->setIndex == b3_staticSet ) + { + jointSetId = b3_staticSet; + } + else if ( bodyA->setIndex == b3_staticSet ) + { + jointSetId = bodyB->setIndex; + } + else + { + jointSetId = bodyA->setIndex; + } + + b3SolverSet* jointSet = b3Array_Get( world->solverSets, jointSetId ); + b3TransferJoint( world, jointSet, disabledSet, joint ); + + // Now that the joint is in the correct set, I can link the joint in the island. + if ( jointSetId != b3_staticSet ) + { + b3LinkJoint( world, joint ); + } + } + + b3ValidateSolverSets( world ); +} + +void b3Body_SetMotionLocks( b3BodyId bodyId, b3MotionLocks locks ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + uint32_t newFlags = 0; + newFlags |= locks.linearX ? b3_lockLinearX : 0; + newFlags |= locks.linearY ? b3_lockLinearY : 0; + newFlags |= locks.linearZ ? b3_lockLinearZ : 0; + newFlags |= locks.angularX ? b3_lockAngularX : 0; + newFlags |= locks.angularY ? b3_lockAngularY : 0; + newFlags |= locks.angularZ ? b3_lockAngularZ : 0; + + b3Body* body = b3GetBodyFullId( world, bodyId ); + if ( ( body->flags & b3_allLocks ) != newFlags ) + { + body->flags &= ~b3_allLocks; + body->flags |= newFlags; + + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->flags &= ~b3_allLocks; + bodySim->flags |= newFlags; + + b3BodyState* state = b3GetBodyState( world, body ); + + if ( state != NULL ) + { + state->flags = body->flags; + + if ( locks.linearX ) + { + state->linearVelocity.x = 0.0f; + } + + if ( locks.linearY ) + { + state->linearVelocity.y = 0.0f; + } + + if ( locks.linearZ ) + { + state->linearVelocity.z = 0.0f; + } + + if ( locks.angularX ) + { + state->angularVelocity.x = 0.0f; + } + + if ( locks.angularY ) + { + state->angularVelocity.y = 0.0f; + } + + if ( locks.angularZ ) + { + state->angularVelocity.z = 0.0f; + } + } + } +} + +b3MotionLocks b3Body_GetMotionLocks( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + + b3MotionLocks locks; + locks.linearX = ( body->flags & b3_lockLinearX ); + locks.linearY = ( body->flags & b3_lockLinearY ); + locks.linearZ = ( body->flags & b3_lockLinearZ ); + locks.angularX = ( body->flags & b3_lockAngularX ); + locks.angularY = ( body->flags & b3_lockAngularY ); + locks.angularZ = ( body->flags & b3_lockAngularZ ); + return locks; +} + +void b3Body_SetBullet( b3BodyId bodyId, bool flag ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + + if ( flag ) + { + bodySim->flags |= b3_isBullet; + } + else + { + bodySim->flags &= ~b3_isBullet; + } +} + +bool b3Body_IsBullet( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + return ( bodySim->flags & b3_isBullet ) != 0; +} + +void b3Body_EnableHitEvents( b3BodyId bodyId, bool enableHitEvents ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shape->enableHitEvents = enableHitEvents; + shapeId = shape->nextShapeId; + } +} + +b3WorldId b3Body_GetWorld( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + return (b3WorldId){ (uint16_t)( bodyId.world0 + 1 ), world->generation }; +} + +int b3Body_GetShapeCount( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->shapeCount; +} + +int b3Body_GetShapes( b3BodyId bodyId, b3ShapeId* shapeArray, int capacity ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + int shapeId = body->headShapeId; + int shapeCount = 0; + while ( shapeId != B3_NULL_INDEX && shapeCount < capacity ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + b3ShapeId id = { shape->id + 1, bodyId.world0, shape->generation }; + shapeArray[shapeCount] = id; + shapeCount += 1; + + shapeId = shape->nextShapeId; + } + + return shapeCount; +} + +int b3Body_GetJointCount( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->jointCount; +} + +int b3Body_GetJoints( b3BodyId bodyId, b3JointId* jointArray, int capacity ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + int jointKey = body->headJointKey; + + int jointCount = 0; + while ( jointKey != B3_NULL_INDEX && jointCount < capacity ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + + b3JointId id = { jointId + 1, bodyId.world0, joint->generation }; + jointArray[jointCount] = id; + jointCount += 1; + + jointKey = joint->edges[edgeIndex].nextKey; + } + + return jointCount; +} + +bool b3ShouldBodiesCollide( b3World* world, b3Body* bodyA, b3Body* bodyB ) +{ + if ( bodyA->type != b3_dynamicBody && bodyB->type != b3_dynamicBody ) + { + return false; + } + + int jointKey; + int otherBodyId; + if ( bodyA->jointCount < bodyB->jointCount ) + { + jointKey = bodyA->headJointKey; + otherBodyId = bodyB->id; + } + else + { + jointKey = bodyB->headJointKey; + otherBodyId = bodyA->id; + } + + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + int otherEdgeIndex = edgeIndex ^ 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + if ( joint->collideConnected == false && joint->edges[otherEdgeIndex].bodyId == otherBodyId ) + { + return false; + } + + jointKey = joint->edges[edgeIndex].nextKey; + } + + return true; +} diff --git a/src/body.h b/src/body.h new file mode 100644 index 0000000..e094895 --- /dev/null +++ b/src/body.h @@ -0,0 +1,254 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box3d/constants.h" +#include "box3d/math_functions.h" +#include "box3d/types.h" + +typedef struct b3World b3World; + +enum b3BodyFlags +{ + // This body has fixed translation along the x-axis + b3_lockLinearX = 0x00000001, + + // This body has fixed translation along the y-axis + b3_lockLinearY = 0x00000002, + + // This body has fixed translation along the z-axis + b3_lockLinearZ = 0x00000004, + + // This body has fixed rotation around the x-axis + b3_lockAngularX = 0x00000008, + + // This body has fixed rotation around the y-axis + b3_lockAngularY = 0x00000010, + + // This body has fixed rotation around the z-axis + b3_lockAngularZ = 0x00000020, + + // This flag is used for debug draw + b3_isFast = 0x00000040, + + // This dynamic body does a final CCD pass against all body types, but not other bullets + b3_isBullet = 0x00000080, + + // This body was speed capped in the current time step + b3_isSpeedCapped = 0x00000100, + + // This body had a time of impact event in the current time step + b3_hadTimeOfImpact = 0x00000200, + + // This body has no limit on angular velocity + b3_allowFastRotation = 0x00000400, + + // This body need's to have its AABB increased + b3_enlargeBounds = 0x00000800, + + // This body is dynamic so the solver should write to it. + // This prevents writing to kinematic bodies that causes a multithreaded sharing + // cache coherence problem even when the values are not changing. + // Used for b3BodyState flags. + b3_dynamicFlag = 0x00001000, + + // All lock flags + b3_allLocks = b3_lockLinearX | b3_lockLinearY | b3_lockLinearZ | b3_lockAngularX | b3_lockAngularY | b3_lockAngularZ, +}; + +// Body organizational details that are not used in the solver. +typedef struct b3Body +{ + char name[B3_NAME_LENGTH]; + + void* userData; + + // index of solver set stored in b3World + // may be B3_NULL_INDEX + int setIndex; + + // body sim and state index within set + // may be B3_NULL_INDEX + int localIndex; + + // [31 : contactId | 1 : edgeIndex] + int headContactKey; + int contactCount; + + // todo maybe move this to the body sim + int headShapeId; + int shapeCount; + + int headChainId; + + // [31 : jointId | 1 : edgeIndex] + int headJointKey; + int jointCount; + + // All enabled dynamic and kinematic bodies are in an island. + int islandId; + + // Index into the island's bodies array for O(1) swap-removal. + // B3_NULL_INDEX when not in an island. + int islandIndex; + + float sleepThreshold; + float sleepTime; + + float mass; + + // local space inertia + b3Matrix3 inertia; + + // this is used to adjust the fellAsleep flag in the body move array + int bodyMoveIndex; + + int id; + + // b3BodyFlags + uint32_t flags; + + b3BodyType type; + + // This is monotonically advanced when a body is allocated in this slot + // Used to check for invalid b3BodyId + uint16_t generation; + + // todo move into flags + bool enableSleep; +} b3Body; + +// Body State +// The body state is designed for fast conversion to and from SIMD via scatter-gather. +// Only awake dynamic and kinematic bodies have a body state. +// This is used in the performance critical constraint solver +// +// The solver operates on the body state. The body state array does not hold static bodies. Static bodies are shared +// across worker threads. It would be okay to read their states, but writing to them would cause cache thrashing across +// workers, even if the values don't change. +// This causes some trouble when computing anchors. I rotate joint anchors using the body rotation every sub-step. For static +// bodies the anchor doesn't rotate. Body A or B could be static and this can lead to lots of branching. This branching +// should be minimized. +// +// Solution 1: +// Use delta rotations. This means anchors need to be prepared in world space. The delta rotation for static bodies will be +// identity using a dummy state. Base separation and angles need to be computed. Manifolds will be behind a frame, but that +// is probably best if bodies move fast. +// +// Solution 2: +// Use full rotation. The anchors for static bodies will be in world space while the anchors for dynamic bodies will be in local +// space. Potentially confusing and bug prone. +// +// Note: +// I rotate joint anchors each sub-step but not contact anchors. Joint stability improves a lot by rotating joint anchors +// according to substep progress. Contacts have reduced stability when anchors are rotated during substeps, especially for +// round shapes. +// +// 56 bytes +// todo_erin measure perf padding to 64 bytes +typedef struct b3BodyState +{ + b3Vec3 linearVelocity; // 12 + b3Vec3 angularVelocity; // 12 + + // Using delta position reduces round-off error far from the origin + b3Vec3 deltaPosition; // 12 + + // Using delta rotation because I cannot access the full rotation on static bodies in + // the solver and must use zero delta rotation for static bodies (c,s) = (1,0) + b3Quat deltaRotation; // 16 + + // b3BodyFlags + // Important flags: locking, dynamic + uint32_t flags; // 4 +} b3BodyState; + +// Identity body state, notice the deltaRotation is identity +static const b3BodyState b3_identityBodyState = { + { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f }, { { 0.0f, 0.0f, 0.0f }, 1.0f }, 0, +}; + +// Body simulation data used for integration of position and velocity +// Transform data used for collision and solver preparation. +typedef struct b3BodySim +{ + // transform for body origin + b3Transform transform; + + // center of mass position in world space + b3Vec3 center; + + // previous rotation and COM for TOI + b3Quat rotation0; + b3Vec3 center0; + + // location of center of mass relative to the body origin + b3Vec3 localCenter; + + b3Vec3 force; + b3Vec3 torque; + + float invMass; + + // Rotational inertia about the center of mass. The world space inverse inertia tensor + // must be updated whenever the body rotation is modified. + b3Matrix3 invInertiaLocal; + b3Matrix3 invInertiaWorld; + + float minExtent; + b3Vec3 maxExtent; + float maxAngularVelocity; + float linearDamping; + float angularDamping; + float gravityScale; + + // Index of b3Body + int bodyId; + + // b3BodyFlags + uint32_t flags; +} b3BodySim; + +#ifdef __cplusplus +extern "C" +{ +#endif + +// Get a validated body from a world using an id. +b3Body* b3GetBodyFullId( b3World* world, b3BodyId bodyId ); + +b3Transform b3GetBodyTransformQuick( b3World* world, b3Body* body ); +b3Transform b3GetBodyTransform( b3World* world, int bodyId ); + +// Create a b3BodyId from a raw id. +b3BodyId b3MakeBodyId( b3World* world, int bodyId ); + +bool b3ShouldBodiesCollide( b3World* world, b3Body* bodyA, b3Body* bodyB ); +bool b3IsBodyAwake( b3World* world, b3Body* body ); + +b3BodySim* b3GetBodySim( b3World* world, b3Body* body ); +b3BodyState* b3GetBodyState( b3World* world, b3Body* body ); + +// careful calling this because it can invalidate body, state, joint, and contact pointers +bool b3WakeBody( b3World* world, b3Body* body ); +bool b3WakeBodyWithLock( b3World* world, b3Body* body ); + +void b3UpdateBodyMassData( b3World* world, b3Body* body ); +void b3DumpBody( b3World* world, b3Body* body ); + +#ifdef __cplusplus +} +#endif + +B3_INLINE b3Sweep b3MakeSweep( const b3BodySim* bodySim ) +{ + b3Sweep s; + s.c1 = bodySim->center0; + s.c2 = bodySim->center; + s.q1 = bodySim->rotation0; + s.q2 = bodySim->transform.q; + s.localCenter = bodySim->localCenter; + return s; +} + diff --git a/src/box3d.natvis b/src/box3d.natvis new file mode 100644 index 0000000..a5d1d7e --- /dev/null +++ b/src/box3d.natvis @@ -0,0 +1,138 @@ + + + + + + [ {x}, {y} ] + + + + [ {x}, {y}, {z} ] + + + + [ {v}, {s} ] + + + + index: {index1} + + b3_worlds[ index1 - 1 ] + + + + + + {b3_worlds[world0].bodies.data[index1-1].name,na}, + {b3_worlds[world0].bodies.data[index1-1].type} + + + b3_worlds[ world0 ].bodies.data[index1-1] + + + + + + {b3_worlds[world0].shapes.data[index1-1].type} + + + + + + {point}, sep = {separation} + + + + + + {int(owner1)}:{int(index1)}, {int(owner2)}:{int(index2)} + + + + + count: { pointCount }, triangle: { triangleIndex }, cluster: {clusterIndex} + + normal + feature + triangleFlags + + pointCount + points + + + + + + {{{count} : {radius}}} + + count + radius + + count + points + + + + + + type: {type} + + witnessA + witnessB + sweepA + sweepB + proxyA + proxyB + + + + + {{{count} / {capacity}}} + + count + capacity + + count + data + + + + + + + {{{count} / {capacity}}} + + count + capacity + + count + data + + + + + + {{{count} / {capacity}}} + + count + capacity + + count + data + + + + + + {{{count} / {capacity}}} + + count + capacity + + count + data + + + + + diff --git a/src/broad_phase.c b/src/broad_phase.c new file mode 100644 index 0000000..bd7b855 --- /dev/null +++ b/src/broad_phase.c @@ -0,0 +1,534 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "broad_phase.h" + +#include "aabb.h" +#include "arena_allocator.h" +#include "body.h" +#include "contact.h" +#include "core.h" +#include "parallel_for.h" +#include "physics_world.h" +#include "platform.h" +#include "shape.h" + +#include + +void b3CreateBroadPhase( b3BroadPhase* bp, const b3Capacity* capacity ) +{ + _Static_assert( b3_bodyTypeCount == 3, "must be three body types" ); + + bp->moveSet = b3CreateSet( 2 * capacity->dynamicShapeCount ); + b3Array_Reserve( bp->moveArray, capacity->dynamicShapeCount ); + bp->moveResults = NULL; + bp->movePairs = NULL; + bp->movePairCapacity = 0; + b3AtomicStoreInt( &bp->movePairIndex, 0 ); + bp->pairSet = b3CreateSet( 2 * capacity->contactCount ); + + int staticCapacity = b3MaxInt( 16, capacity->staticShapeCount ); + bp->trees[b3_staticBody] = b3DynamicTree_Create( staticCapacity ); + + int kinematicCapacity = 16; + bp->trees[b3_kinematicBody] = b3DynamicTree_Create( kinematicCapacity ); + + int dynamicCapacity = b3MaxInt( 16, capacity->dynamicShapeCount ); + bp->trees[b3_dynamicBody] = b3DynamicTree_Create( dynamicCapacity ); +} + +void b3DestroyBroadPhase( b3BroadPhase* bp ) +{ + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3DynamicTree_Destroy( bp->trees + i ); + } + + b3DestroySet( &bp->moveSet ); + b3Array_Destroy( bp->moveArray ); + b3DestroySet( &bp->pairSet ); + + *bp = (b3BroadPhase){ 0 }; + + memset( bp, 0, sizeof( b3BroadPhase ) ); +} + +static void b3UnBufferMove( b3BroadPhase* bp, int proxyKey ) +{ + bool found = b3RemoveKey( &bp->moveSet, proxyKey + 1 ); + + if ( found ) + { + // Purge from move buffer. Linear search. + // todo if I can iterate the move set then I don't need the moveArray + int count = bp->moveArray.count; + for ( int i = 0; i < count; ++i ) + { + if ( bp->moveArray.data[i] == proxyKey ) + { + b3Array_RemoveSwap( bp->moveArray, i ); + break; + } + } + } +} + +int b3BroadPhase_CreateProxy( b3BroadPhase* bp, b3BodyType proxyType, b3AABB aabb, uint64_t categoryBits, int shapeIndex, + bool forcePairCreation ) +{ + B3_ASSERT( 0 <= proxyType && proxyType < b3_bodyTypeCount ); + int proxyId = b3DynamicTree_CreateProxy( bp->trees + proxyType, aabb, categoryBits, shapeIndex ); + int proxyKey = B3_PROXY_KEY( proxyId, proxyType ); + if ( proxyType != b3_staticBody || forcePairCreation ) + { + b3BufferMove( bp, proxyKey ); + } + return proxyKey; +} + +void b3BroadPhase_DestroyProxy( b3BroadPhase* bp, int proxyKey ) +{ + B3_ASSERT( bp->moveArray.count == (int)bp->moveSet.count ); + b3UnBufferMove( bp, proxyKey ); + + b3BodyType proxyType = B3_PROXY_TYPE( proxyKey ); + int proxyId = B3_PROXY_ID( proxyKey ); + + B3_ASSERT( 0 <= proxyType && proxyType <= b3_bodyTypeCount ); + b3DynamicTree_DestroyProxy( bp->trees + proxyType, proxyId ); +} + +void b3BroadPhase_MoveProxy( b3BroadPhase* bp, int proxyKey, b3AABB aabb ) +{ + b3BodyType proxyType = B3_PROXY_TYPE( proxyKey ); + int proxyId = B3_PROXY_ID( proxyKey ); + + b3DynamicTree_MoveProxy( bp->trees + proxyType, proxyId, aabb ); + b3BufferMove( bp, proxyKey ); +} + +void b3BroadPhase_EnlargeProxy( b3BroadPhase* bp, int proxyKey, b3AABB aabb ) +{ + B3_ASSERT( proxyKey != B3_NULL_INDEX ); + int typeIndex = B3_PROXY_TYPE( proxyKey ); + int proxyId = B3_PROXY_ID( proxyKey ); + + B3_ASSERT( typeIndex != b3_staticBody ); + + b3DynamicTree_EnlargeProxy( bp->trees + typeIndex, proxyId, aabb ); + b3BufferMove( bp, proxyKey ); +} + +typedef struct b3MovePair +{ + int shapeIndexA; + int shapeIndexB; + int childIndex; + b3MovePair* next; + bool heap; +} b3MovePair; + +typedef struct b3MoveResult +{ + b3MovePair* pairList; +} b3MoveResult; + +typedef struct b3QueryPairContext +{ + b3World* world; + b3MoveResult* moveResult; + b3AABB aabb; + b3BodyType queryTreeType; + int queryProxyKey; + int queryShapeIndex; + + int compoundProxyId; + int compoundShapeIndex; +} b3QueryPairContext; + +// This is called from b3DynamicTree::Query when we are gathering pairs. +static bool b3PairQueryCallback( int proxyId, uint64_t userData, void* context ) +{ + b3QueryPairContext* queryContext = (b3QueryPairContext*)context; + b3World* world = queryContext->world; + int shapeIndex; + int childIndex = 0; + + if ( queryContext->compoundShapeIndex == B3_NULL_INDEX ) + { + // Outer query: userData is a shape index. + shapeIndex = (int)userData; + + // A proxy cannot form a pair with itself. + if ( shapeIndex == queryContext->queryShapeIndex ) + { + return true; + } + + b3Shape* shape = b3Array_Get( world->shapes, shapeIndex ); + if ( shape->type == b3_compoundShape ) + { + b3Transform compoundTransform = b3GetBodyTransform( world, shape->bodyId ); + b3AABB localAABB = b3AABB_Transform( b3InvertTransform( compoundTransform ), queryContext->aabb ); + + // recurse + queryContext->compoundShapeIndex = shapeIndex; + queryContext->compoundProxyId = proxyId; + + b3DynamicTree_Query( &shape->compound->tree, localAABB, B3_DEFAULT_MASK_BITS, false, b3PairQueryCallback, context ); + queryContext->compoundShapeIndex = B3_NULL_INDEX; + queryContext->compoundProxyId = B3_NULL_INDEX; + return true; + } + } + else + { + // Inner query into a compound shape: userData is the compound child index, not a shape + // index, so do not compare it against queryShapeIndex. + shapeIndex = queryContext->compoundShapeIndex; + proxyId = queryContext->compoundProxyId; + childIndex = (int)userData; + } + + b3BroadPhase* broadPhase = &queryContext->world->broadPhase; + + int proxyKey = B3_PROXY_KEY( proxyId, queryContext->queryTreeType ); + int queryProxyKey = queryContext->queryProxyKey; + + // A proxy cannot form a pair with itself. + B3_ASSERT( proxyKey != queryContext->queryProxyKey ); + + b3BodyType treeType = queryContext->queryTreeType; + b3BodyType queryProxyType = B3_PROXY_TYPE( queryProxyKey ); + + // De-duplication + // It is important to prevent duplicate contacts from being created. Ideally I can prevent duplicates + // early and in the worker. Most of the time the moveSet contains dynamic and kinematic proxies, but + // sometimes it has static proxies. + + // I had an optimization here to skip checking the move set if this is a query into + // the static tree. The assumption is that the static proxies are never in the move set + // so there is no risk of duplication. However, this is not true with + // b3ShapeDef::invokeContactCreation or when a static shape is modified. + // There can easily be scenarios where the static proxy is in the moveSet but the dynamic proxy is not. + // I could have some flag to indicate that there are any static bodies in the moveSet. + + // Is this proxy also moving? + if ( queryProxyType == b3_dynamicBody ) + { + if ( treeType == b3_dynamicBody && proxyKey < queryProxyKey ) + { + bool moved = b3ContainsKey( &broadPhase->moveSet, proxyKey + 1 ); + if ( moved ) + { + // Both proxies are moving. Avoid duplicate pairs. + return true; + } + } + } + else + { + B3_ASSERT( treeType == b3_dynamicBody ); + bool moved = b3ContainsKey( &broadPhase->moveSet, proxyKey + 1 ); + if ( moved ) + { + // Both proxies are moving. Avoid duplicate pairs. + return true; + } + } + + uint64_t pairKey = b3ShapePairKey( shapeIndex, queryContext->queryShapeIndex, childIndex ); + if ( b3ContainsKey( &broadPhase->pairSet, pairKey ) ) + { + // contact exists + return true; + } + + // Order shapes so that B3_SHAPE_PAIR_KEY works correctly + int shapeIdA = shapeIndex; + int shapeIdB = queryContext->queryShapeIndex; + b3Shape* shapeA = b3Array_Get( world->shapes, shapeIdA ); + b3Shape* shapeB = b3Array_Get( world->shapes, shapeIdB ); + int bodyIdA = shapeA->bodyId; + int bodyIdB = shapeB->bodyId; + + // Are the shapes on the same body? + if ( bodyIdA == bodyIdB ) + { + return true; + } + + // Sensors are handled elsewhere + if ( shapeA->sensorIndex != B3_NULL_INDEX || shapeB->sensorIndex != B3_NULL_INDEX ) + { + return true; + } + + if ( b3ShouldShapesCollide( shapeA->filter, shapeB->filter ) == false ) + { + return true; + } + + // Does a joint override collision? + b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB ); + if ( b3ShouldBodiesCollide( world, bodyA, bodyB ) == false ) + { + return true; + } + + // Custom user filter + if ( shapeA->enableCustomFiltering || shapeB->enableCustomFiltering ) + { + b3CustomFilterFcn* customFilterFcn = queryContext->world->customFilterFcn; + if ( customFilterFcn != NULL ) + { + b3ShapeId idA = { shapeIdA + 1, world->worldId, shapeA->generation }; + b3ShapeId idB = { shapeIdB + 1, world->worldId, shapeB->generation }; + bool shouldCollide = customFilterFcn( idA, idB, queryContext->world->customFilterContext ); + if ( shouldCollide == false ) + { + return true; + } + } + } + + // todo per thread to eliminate atomic? + int pairIndex = b3AtomicFetchAddInt( &broadPhase->movePairIndex, 1 ); + + b3MovePair* pair; + if ( pairIndex < broadPhase->movePairCapacity ) + { + pair = broadPhase->movePairs + pairIndex; + pair->heap = false; + } + else + { + // todo experimenting with ignoring this pair if we ran out of space + return true; + // pair = (b3MovePair*)b3Alloc( sizeof( b3MovePair ) ); + // pair->heap = true; + } + + pair->shapeIndexA = shapeIdA; + pair->shapeIndexB = shapeIdB; + pair->childIndex = childIndex; + pair->next = queryContext->moveResult->pairList; + queryContext->moveResult->pairList = pair; + + // continue the query + return true; +} + +static void b3FindPairsTask( int startIndex, int endIndex, int workerIndex, void* context ) +{ + b3TracyCZoneNC( pair_task, "Pair Task", b3_colorAquamarine, true ); + + B3_UNUSED( workerIndex ); + + b3World* world = (b3World*)context; + b3BroadPhase* bp = &world->broadPhase; + + b3QueryPairContext queryContext = { 0 }; + queryContext.world = world; + queryContext.compoundShapeIndex = B3_NULL_INDEX; + + for ( int i = startIndex; i < endIndex; ++i ) + { + // Initialize move result for this moved proxy + queryContext.moveResult = bp->moveResults + i; + queryContext.moveResult->pairList = NULL; + + int proxyKey = bp->moveArray.data[i]; + if ( proxyKey == B3_NULL_INDEX ) + { + // proxy was destroyed after it moved + continue; + } + + b3BodyType proxyType = B3_PROXY_TYPE( proxyKey ); + + int proxyId = B3_PROXY_ID( proxyKey ); + queryContext.queryProxyKey = proxyKey; + + const b3DynamicTree* baseTree = bp->trees + proxyType; + + // We have to query the tree with the fat AABB so that + // we don't fail to create a contact that may touch later. + b3AABB fatAABB = b3DynamicTree_GetAABB( baseTree, proxyId ); + queryContext.queryShapeIndex = (int)b3DynamicTree_GetUserData( baseTree, proxyId ); + queryContext.aabb = fatAABB; + + // Compound shape collision invocation is not supported + B3_VALIDATE( world->shapes.data[queryContext.queryShapeIndex].type != b3_compoundShape ); + + // Query trees. Only dynamic proxies collide with kinematic and static proxies. + // Using B3_DEFAULT_MASK_BITS so that b3Filter::groupIndex works. + // consider using bits = groupIndex > 0 ? B3_DEFAULT_MASK_BITS : maskBits + bool requireAllBits = false; + if ( proxyType == b3_dynamicBody ) + { + queryContext.queryTreeType = b3_kinematicBody; + b3DynamicTree_Query( bp->trees + b3_kinematicBody, fatAABB, B3_DEFAULT_MASK_BITS, requireAllBits, b3PairQueryCallback, + &queryContext ); + + queryContext.queryTreeType = b3_staticBody; + b3DynamicTree_Query( bp->trees + b3_staticBody, fatAABB, B3_DEFAULT_MASK_BITS, requireAllBits, b3PairQueryCallback, + &queryContext ); + } + + // All proxies collide with dynamic proxies + // Using B3_DEFAULT_MASK_BITS so that b3Filter::groupIndex works. + queryContext.queryTreeType = b3_dynamicBody; + b3DynamicTree_Query( bp->trees + b3_dynamicBody, fatAABB, B3_DEFAULT_MASK_BITS, requireAllBits, b3PairQueryCallback, + &queryContext ); + } + + b3TracyCZoneEnd( pair_task ); +} + +static void b3UpdateTreesTask( void* context ) +{ + b3TracyCZoneNC( tree_task, "Rebuild Trees", b3_colorFireBrick, true ); + + b3World* world = (b3World*)context; + b3DynamicTree_Rebuild( world->broadPhase.trees + b3_dynamicBody, false ); + b3DynamicTree_Rebuild( world->broadPhase.trees + b3_kinematicBody, false ); + + b3TracyCZoneEnd( tree_task ); +} + +void b3UpdateBroadPhasePairs( b3World* world ) +{ + b3BroadPhase* bp = &world->broadPhase; + + int moveCount = bp->moveArray.count; + B3_ASSERT( moveCount == (int)bp->moveSet.count ); + + if ( moveCount == 0 ) + { + return; + } + + b3TracyCZoneNC( update_pairs, "Pairs", b3_colorMediumSlateBlue, true ); + + b3Stack* alloc = &world->stack; + + // todo these could be in the step context + bp->moveResults = (b3MoveResult*)b3StackAlloc( alloc, moveCount * sizeof( b3MoveResult ), "move results" ); + bp->movePairCapacity = 16 * moveCount; + bp->movePairs = (b3MovePair*)b3StackAlloc( alloc, bp->movePairCapacity * sizeof( b3MovePair ), "move pairs" ); + + b3AtomicStoreInt( &bp->movePairIndex, 0 ); + +#ifndef NDEBUG + extern b3AtomicInt b3_probeCount; + b3AtomicStoreInt( &b3_probeCount, 0 ); +#endif + + int minRange = 64; + b3ParallelFor( world, b3FindPairsTask, moveCount, minRange, world, "pairs" ); + + b3TracyCZoneNC( create_contacts, "Create Contacts", b3_colorCoral, true ); + + // Task that can be done in parallel with the narrow-phase + // - rebuild the collision tree for dynamic and kinematic bodies to keep their query performance good + if ( world->taskCount < B3_MAX_TASKS ) + { + world->userTreeTask = world->enqueueTaskFcn( &b3UpdateTreesTask, world, world->userTaskContext, "rebuild tree" ); + world->taskCount += 1; + world->activeTaskCount += world->userTreeTask == NULL ? 0 : 1; + } + else + { + world->userTreeTask = NULL; + b3UpdateTreesTask( world ); + } + + // Single-threaded work + // - Clear move flags + // - Create contacts in deterministic order + for ( int i = 0; i < moveCount; ++i ) + { + b3MoveResult* result = bp->moveResults + i; + b3MovePair* pair = result->pairList; + while ( pair != NULL ) + { + int shapeIdA = pair->shapeIndexA; + int shapeIdB = pair->shapeIndexB; + int childIndex = pair->childIndex; + + b3Shape* shapeA = b3Array_Get( world->shapes, shapeIdA ); + b3Shape* shapeB = b3Array_Get( world->shapes, shapeIdB ); + + b3CreateContact( world, shapeA, shapeB, childIndex ); + + if ( pair->heap ) + { + b3MovePair* temp = pair; + pair = pair->next; + b3Free( temp, sizeof( b3MovePair ) ); + } + else + { + pair = pair->next; + } + } + } + + // Reset move buffer + b3Array_Clear( bp->moveArray ); + b3ClearSet( &bp->moveSet ); + + b3StackFree( alloc, bp->movePairs ); + bp->movePairs = NULL; + b3StackFree( alloc, bp->moveResults ); + bp->moveResults = NULL; + + b3ValidateSolverSets( world ); + + b3TracyCZoneEnd( create_contacts ); + + b3TracyCZoneEnd( update_pairs ); +} + +bool b3BroadPhase_TestOverlap( const b3BroadPhase* bp, int proxyKeyA, int proxyKeyB ) +{ + int typeIndexA = B3_PROXY_TYPE( proxyKeyA ); + int proxyIdA = B3_PROXY_ID( proxyKeyA ); + int typeIndexB = B3_PROXY_TYPE( proxyKeyB ); + int proxyIdB = B3_PROXY_ID( proxyKeyB ); + + b3AABB aabbA = b3DynamicTree_GetAABB( bp->trees + typeIndexA, proxyIdA ); + b3AABB aabbB = b3DynamicTree_GetAABB( bp->trees + typeIndexB, proxyIdB ); + return b3AABB_Overlaps( aabbA, aabbB ); +} + +int b3BroadPhase_GetShapeIndex( b3BroadPhase* bp, int proxyKey ) +{ + int typeIndex = B3_PROXY_TYPE( proxyKey ); + int proxyId = B3_PROXY_ID( proxyKey ); + + return (int)b3DynamicTree_GetUserData( bp->trees + typeIndex, proxyId ); +} + +void b3ValidateBroadphase( const b3BroadPhase* bp ) +{ + b3DynamicTree_Validate( bp->trees + b3_dynamicBody ); + b3DynamicTree_Validate( bp->trees + b3_kinematicBody ); + + // todo validate every shape AABB is contained in tree AABB +} + +void b3ValidateNoEnlarged( const b3BroadPhase* bp ) +{ +#if B3_ENABLE_VALIDATION == 1 + for ( int j = 0; j < b3_bodyTypeCount; ++j ) + { + const b3DynamicTree* tree = bp->trees + j; + b3DynamicTree_ValidateNoEnlarged( tree ); + } +#else + B3_UNUSED( bp ); +#endif +} diff --git a/src/broad_phase.h b/src/broad_phase.h new file mode 100644 index 0000000..660d91f --- /dev/null +++ b/src/broad_phase.h @@ -0,0 +1,88 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "container.h" +#include "table.h" + +#include "box3d/collision.h" +#include "box3d/types.h" + +typedef struct b3Shape b3Shape; +typedef struct b3MovePair b3MovePair; +typedef struct b3MoveResult b3MoveResult; +typedef struct b3Stack b3Stack; +typedef struct b3World b3World; + +// Store the proxy type in the lower 2 bits of the proxy key. This leaves 30 bits for the id. +#define B3_PROXY_TYPE( KEY ) ( (b3BodyType)( ( KEY ) & 3 ) ) +#define B3_PROXY_ID( KEY ) ( ( KEY ) >> 2 ) +#define B3_PROXY_KEY( ID, TYPE ) ( ( ( ID ) << 2 ) | ( TYPE ) ) + +/// The broad-phase is used for computing pairs and performing volume queries and ray casts. +/// This broad-phase does not persist pairs. Instead, this reports potentially new pairs. +/// It is up to the client to consume the new pairs and to track subsequent overlap. +typedef struct b3BroadPhase +{ + b3DynamicTree trees[b3_bodyTypeCount]; + + // The move set and array are used to track shapes that have moved significantly + // and need a pair query for new contacts. The array has a deterministic order. + // todo perhaps just a move set? + // todo implement a 32bit hash set for faster lookup + // todo moveSet can grow quite large on the first time step and remain large + b3HashSet moveSet; + b3ArrayC( int ) moveArray; + + // These are the results from the pair query and are used to create new contacts + // in deterministic order. + // todo these could be in the step context + b3MoveResult* moveResults; + b3MovePair* movePairs; + int movePairCapacity; + b3AtomicInt movePairIndex; + + // Tracks shape pairs that have a b3Contact + // todo pairSet can grow quite large on the first time step and remain large + b3HashSet pairSet; +} b3BroadPhase; + +#ifdef __cplusplus +extern "C" +{ +#endif + +void b3CreateBroadPhase( b3BroadPhase* bp, const b3Capacity* capacity ); +void b3DestroyBroadPhase( b3BroadPhase* bp ); + +int b3BroadPhase_CreateProxy( b3BroadPhase* bp, b3BodyType proxyType, b3AABB aabb, uint64_t categoryBits, int shapeIndex, + bool forcePairCreation ); +void b3BroadPhase_DestroyProxy( b3BroadPhase* bp, int proxyKey ); + +void b3BroadPhase_MoveProxy( b3BroadPhase* bp, int proxyKey, b3AABB aabb ); +void b3BroadPhase_EnlargeProxy( b3BroadPhase* bp, int proxyKey, b3AABB aabb ); + +int b3BroadPhase_GetShapeIndex( b3BroadPhase* bp, int proxyKey ); + +void b3UpdateBroadPhasePairs( b3World* world ); +bool b3BroadPhase_TestOverlap( const b3BroadPhase* bp, int proxyKeyA, int proxyKeyB ); + +void b3ValidateBroadphase( const b3BroadPhase* bp ); +void b3ValidateNoEnlarged( const b3BroadPhase* bp ); + +#ifdef __cplusplus +} +#endif + +// This is what triggers new contact pairs to be created +// Warning: this must be called in deterministic order +static inline void b3BufferMove( b3BroadPhase* bp, int queryProxy ) +{ + // Adding 1 because 0 is the sentinel + bool alreadyAdded = b3AddKey( &bp->moveSet, queryProxy + 1 ); + if ( alreadyAdded == false ) + { + b3Array_Push( bp->moveArray, queryProxy ); + } +} diff --git a/src/capsule.c b/src/capsule.c new file mode 100644 index 0000000..ea6c457 --- /dev/null +++ b/src/capsule.c @@ -0,0 +1,290 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "math_internal.h" +#include "shape.h" + +#include "box3d/base.h" +#include "box3d/collision.h" +#include "box3d/constants.h" + +b3MassData b3ComputeCapsuleMass( const b3Capsule* shape, float density ) +{ + b3Vec3 c1 = shape->center1; + b3Vec3 c2 = shape->center2; + float r = shape->radius; + + // Cylinder + float cylinderHeight = b3Distance( c1, c2 ); + float cylinderVolume = B3_PI * r * r * cylinderHeight; + float cylinderMass = cylinderVolume * density; + + // Sphere + float sphereVolume = ( 4.0f / 3.0f ) * B3_PI * r * r * r; + float sphereMass = sphereVolume * density; + + // Local accumulated inertia + b3Matrix3 inertia = b3AddMM( b3CylinderInertia( cylinderMass, r, cylinderHeight ), b3SphereInertia( sphereMass, r ) ); + + float steiner = 0.125f * sphereMass * ( 3.0f * r + 2.0f * cylinderHeight ) * cylinderHeight; + inertia.cx.x += steiner; + inertia.cz.z += steiner; + + // Align capsule axis with chosen up-axis + b3Matrix3 rotation = b3Mat3_identity; + if ( cylinderHeight * cylinderHeight > 1000.0f * FLT_MIN ) + { + b3Vec3 direction = b3Normalize( b3Sub( c2, c1 ) ); + b3Quat q = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisY, direction ); + rotation = b3MakeMatrixFromQuat( q ); + } + + // Parallel Axis Theorem capsule + float mass = sphereMass + cylinderMass; + b3Vec3 center = b3MulSV( 0.5f, b3Add( c1, c2 ) ); + + b3MassData out; + out.mass = mass; + out.center = center; + out.inertia = b3AddMM( b3MulMM( rotation, b3MulMM( inertia, b3Transpose( rotation ) ) ), b3Steiner( mass, center ) ); + + return out; +} + +b3AABB b3ComputeCapsuleAABB( const b3Capsule* shape, b3Transform transform ) +{ + float r = shape->radius; + + b3Vec3 center1 = b3TransformPoint( transform, shape->center1 ); + b3Vec3 center2 = b3TransformPoint( transform, shape->center2 ); + b3Vec3 extent = { r, r, r }; + + b3AABB aabb; + aabb.lowerBound = b3Sub( b3Min( center1, center2 ), extent ); + aabb.upperBound = b3Add( b3Max( center1, center2 ), extent ); + return aabb; +} + +b3AABB b3ComputeSweptCapsuleAABB( const b3Capsule* shape, b3Transform xf1, b3Transform xf2 ) +{ + b3Vec3 r = { shape->radius, shape->radius, shape->radius }; + b3Vec3 a = b3TransformPoint( xf1, shape->center1 ); + b3Vec3 b = b3TransformPoint( xf1, shape->center2 ); + b3Vec3 c = b3TransformPoint( xf2, shape->center1 ); + b3Vec3 d = b3TransformPoint( xf2, shape->center2 ); + + b3AABB aabb = { + .lowerBound = b3Sub( b3Min( b3Min( a, b ), b3Min( c, d ) ), r ), + .upperBound = b3Add( b3Max( b3Max( a, b ), b3Max( c, d ) ), r ), + }; + return aabb; +} + +static b3CastOutput b3CastRayAgainstCap( b3Vec3 p, b3Vec3 q, b3Vec3 c, float r, float maxFraction ) +{ + b3Sphere sphere = { c, r }; + b3RayCastInput input = { + .origin = p, + .translation = b3Sub( q, p ), + .maxFraction = maxFraction, + }; + + b3CastOutput output = b3RayCastSphere( &sphere, &input ); + return output; +} + +static b3CastOutput b3TransformCapsuleResult( b3CastOutput result, b3Quat rotation, b3Vec3 translation ) +{ + b3CastOutput out = result; + out.point = b3Add( b3RotateVector( rotation, result.point ), translation ); + out.normal = b3RotateVector( rotation, result.normal ); + return out; +} + +bool b3OverlapCapsule( const b3Capsule* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ) +{ + b3DistanceInput input; + input.proxyA = (b3ShapeProxy){ &shape->center1, 2, shape->radius }; + input.proxyB = *proxy; + input.transformA = shapeTransform; + input.transformB = b3Transform_identity; + input.useRadii = true; + + b3SimplexCache cache = { 0 }; + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + return output.distance < B3_OVERLAP_SLOP; +} + +// todo_erin implement precision improvement +// Precision Improvements for Ray / Sphere Intersection - Ray Tracing Gems 2019 +// http://www.codercorner.com/blog/?p=321 +b3CastOutput b3RayCastCapsule( const b3Capsule* shape, const b3RayCastInput* input ) +{ + b3Vec3 c1 = shape->center1; + b3Vec3 c2 = shape->center2; + float r = shape->radius; + + // Initialize result structure + b3CastOutput output = { 0 }; + + // Compute height and handle degenerate capsules + float height = b3Distance( c1, c2 ); + if ( height < 1000.0f * FLT_MIN ) + { + b3Vec3 sphereCenter = b3MulSV( 0.5f, b3Add( shape->center1, shape->center2 ) ); + b3Sphere sphere = { sphereCenter, shape->radius }; + return b3RayCastSphere( &sphere, input ); + } + + // Transform ray and capsule into local space capsule space + b3Quat rotation = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisY, b3MulSV( 1.0f / height, b3Sub( c2, c1 ) ) ); + b3Vec3 offset = c1; + + // Capsule starts at the origin and is along the y-axis + b3Vec3 a = b3Vec3_zero; + b3Vec3 b = { 0.0f, height, 0.0f }; + b3Vec3 ab = b3Sub( b, a ); + + // Ray expressed relative to capsule space (capsule along y-axis) + b3Vec3 p = b3InvRotateVector( rotation, b3Sub( input->origin, offset ) ); + b3Vec3 q = b3Add( p, b3InvRotateVector( rotation, input->translation ) ); + b3Vec3 pq = b3Sub( q, p ); + + // Ray 2D translation length squared + float k1 = pq.x * pq.x + pq.z * pq.z; + + // Ray start point 2D separation squared from circle + float k3 = p.x * p.x + p.z * p.z - r * r; + + // Parallel case (2D ray translation is zero) + if ( k1 < 1000.0f * FLT_MIN ) + { + if ( k3 > 0.0f ) + { + // Parallel and outside + return output; + } + + if ( 0.0f <= p.y && p.y <= height ) + { + // Parallel and inside + output.hit = true; + output.point = input->origin; + return output; + } + + // Below cylinder and casting upwards + if ( p.y < 0.0f && pq.y > 0.0f ) + { + output = b3CastRayAgainstCap( p, q, a, r, input->maxFraction ); + return b3TransformCapsuleResult( output, rotation, offset ); + } + + // Above cylinder and casting downwards + if ( p.y > height && pq.y < 0.0f ) + { + output = b3CastRayAgainstCap( p, q, b, r, input->maxFraction ); + return b3TransformCapsuleResult( output, rotation, offset ); + } + + // Above or below and casting away from cylinder + return output; + } + + // Non-parallel case + float k2 = pq.x * p.x + pq.z * p.z; + + float discriminant = k2 * k2 - k1 * k3; + if ( discriminant < 0.0f ) + { + // No real roots - no intersection + return output; + } + + float t = ( -k2 - sqrtf( discriminant ) ) / k1; + if ( t > input->maxFraction ) + { + // Segment approaching cylinder, but not quite getting there. + return output; + } + + // Don't skip t < 0. This means that we start in the *infinite* cylinder and still might hit a cap + + // This is the point on the ray that hits the infinite cylinder + b3Vec3 c = b3MulAdd( p, t, pq ); + + // This is the cylinder hit point relative to the capsule base + b3Vec3 ac = b3Sub( c, a ); + + // Fraction of the cylinder hit point along the capsule axis + float s = b3Dot( ac, ab ) / ( height * height ); + + if ( s < 0.0f ) + { + // X projects outside A, run test through sphere at A + output = b3CastRayAgainstCap( p, q, a, r, input->maxFraction ); + return b3TransformCapsuleResult( output, rotation, offset ); + } + + if ( s > 1.0f ) + { + // X projects outside B, run test through sphere at B + output = b3CastRayAgainstCap( p, q, b, r, input->maxFraction ); + return b3TransformCapsuleResult( output, rotation, offset ); + } + + if ( t < 0.0f ) + { + // Ray starts inside + output.hit = true; + output.point = input->origin; + return output; + } + + // Ray hits cylinder inside segment AB + output.fraction = t; + output.point = c; + output.normal = b3Normalize( (b3Vec3){ c.x, 0.0f, c.z } ); + output.hit = true; + + return b3TransformCapsuleResult( output, rotation, offset ); +} + +b3CastOutput b3ShapeCastCapsule( const b3Capsule* capsule, const b3ShapeCastInput* input ) +{ + b3ShapeCastPairInput pairInput; + pairInput.proxyA = (b3ShapeProxy){ &capsule->center1, 2, capsule->radius }; + pairInput.proxyB = input->proxy; + pairInput.transformA = b3Transform_identity; + pairInput.transformB = b3Transform_identity; + pairInput.translationB = input->translation; + pairInput.maxFraction = input->maxFraction; + pairInput.canEncroach = input->canEncroach; + + b3CastOutput output = b3ShapeCast( &pairInput ); + return output; +} + +int b3CollideMoverAndCapsule( b3PlaneResult* result, const b3Capsule* shape, const b3Capsule* mover ) +{ + b3DistanceInput distanceInput; + distanceInput.proxyA = (b3ShapeProxy){ &shape->center1, 2, 0.0f }; + distanceInput.proxyB = (b3ShapeProxy){ &mover->center1, 2, mover->radius }; + distanceInput.transformA = b3Transform_identity; + distanceInput.transformB = b3Transform_identity; + distanceInput.useRadii = false; + + float totalRadius = mover->radius + shape->radius; + + b3SimplexCache cache = { 0 }; + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance <= totalRadius ) + { + b3Plane plane = { distanceOutput.normal, totalRadius - distanceOutput.distance }; + *result = (b3PlaneResult){ plane, distanceOutput.pointA }; + return 1; + } + + return 0; +} diff --git a/src/compound.c b/src/compound.c new file mode 100644 index 0000000..71c3f7f --- /dev/null +++ b/src/compound.c @@ -0,0 +1,1210 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "compound.h" + +#include "math_internal.h" +#include "shape.h" + +#include "box3d/base.h" +#include "box3d/collision.h" +#include "box3d/constants.h" +#include "box3d/types.h" + +#include +#include + +typedef struct b3SharedHull +{ + const b3Hull* hull; + int hullOffset; +} b3SharedHull; + +typedef struct b3SharedMesh +{ + const b3MeshData* meshData; + int meshOffset; +} b3SharedMesh; + +typedef struct b3HullInstance +{ + b3Transform transform; + uint32_t hullOffset; + uint32_t materialIndex; +} b3HullInstance; + +typedef struct b3MeshInstance +{ + b3Transform transform; + b3Vec3 scale; + uint32_t meshOffset; + uint32_t materialIndices[B3_MAX_COMPOUND_MESH_MATERIALS]; +} b3MeshInstance; + +static inline b3TreeNode* b3GetCompoundNodes( b3Compound* compound ) +{ + if ( compound->nodeOffset == 0 ) + { + return NULL; + } + + return (b3TreeNode*)( (intptr_t)compound + compound->nodeOffset ); +} + +const b3SurfaceMaterial* b3GetCompoundMaterials( const b3Compound* compound ) +{ + if ( compound->materialOffset == 0 ) + { + return NULL; + } + + return (b3SurfaceMaterial*)( (intptr_t)compound + compound->materialOffset ); +} + +b3CompoundCapsule b3GetCompoundCapsule( const b3Compound* compound, int index ) +{ + B3_ASSERT( 0 <= index && index < compound->capsuleCount && compound->capsuleOffset > 0 ); + + b3CompoundCapsule result = { 0 }; + if ( compound->capsuleOffset == 0 ) + { + return result; + } + + const b3CompoundCapsule* capsules = (const b3CompoundCapsule*)( (intptr_t)compound + compound->capsuleOffset ); + return capsules[index]; +} + +b3CompoundHull b3GetCompoundHull( const b3Compound* compound, int index ) +{ + B3_ASSERT( 0 <= index && index < compound->hullCount && compound->hullOffset > 0 ); + + b3CompoundHull result = { 0 }; + if ( compound->hullOffset == 0 ) + { + return result; + } + + const b3HullInstance* hullInstances = (const b3HullInstance*)( (intptr_t)compound + compound->hullOffset ); + uint32_t hullOffset = hullInstances[index].hullOffset; + B3_ASSERT( hullOffset >= compound->hullOffset + compound->hullCount * sizeof( b3HullInstance ) ); + result.hull = (const b3Hull*)( (intptr_t)compound + hullOffset ); + result.transform = hullInstances[index].transform; + result.materialIndex = hullInstances[index].materialIndex; + return result; +} + +b3CompoundMesh b3GetCompoundMesh( const b3Compound* compound, int index ) +{ + B3_ASSERT( 0 <= index && index < compound->meshCount && compound->meshOffset > 0 ); + + b3CompoundMesh result = { 0 }; + if ( compound->meshOffset == 0 ) + { + return result; + } + + const b3MeshInstance* meshInstances = (const b3MeshInstance*)( (intptr_t)compound + compound->meshOffset ); + uint32_t meshOffset = meshInstances[index].meshOffset; + B3_ASSERT( meshOffset >= compound->meshOffset + compound->meshCount * sizeof( b3HullInstance ) ); + result.meshData = (const b3MeshData*)( (intptr_t)compound + meshOffset ); + result.transform = meshInstances[index].transform; + result.scale = meshInstances[index].scale; + for ( int i = 0; i < B3_MAX_COMPOUND_MESH_MATERIALS; ++i ) + { + result.materialIndices[i] = meshInstances[index].materialIndices[i]; + } + return result; +} + +b3CompoundSphere b3GetCompoundSphere( const b3Compound* compound, int index ) +{ + B3_ASSERT( 0 <= index && index < compound->sphereCount && compound->sphereOffset > 0 ); + + b3CompoundSphere result = { 0 }; + if ( compound->sphereOffset == 0 ) + { + return result; + } + + const b3CompoundSphere* spheres = (const b3CompoundSphere*)( (intptr_t)compound + compound->sphereOffset ); + return spheres[index]; +} + +b3ChildShape b3GetCompoundChild( const b3Compound* compound, int childIndex ) +{ + // Capsule? + if ( 0 <= childIndex && childIndex < compound->capsuleCount ) + { + b3CompoundCapsule compoundCapsule = b3GetCompoundCapsule( compound, childIndex ); + return (b3ChildShape){ + .capsule = compoundCapsule.capsule, + .transform = b3Transform_identity, + .materialIndices = { compoundCapsule.materialIndex }, + .type = b3_capsuleShape, + }; + } + childIndex -= compound->capsuleCount; + + // Hull? + if ( 0 <= childIndex && childIndex < compound->hullCount ) + { + b3CompoundHull compoundHull = b3GetCompoundHull( compound, childIndex ); + return (b3ChildShape){ + .hull = compoundHull.hull, + .transform = compoundHull.transform, + .materialIndices = { compoundHull.materialIndex }, + .type = b3_hullShape, + }; + } + childIndex -= compound->hullCount; + + // Mesh? + if ( 0 <= childIndex && childIndex < compound->meshCount ) + { + b3CompoundMesh compoundMesh = b3GetCompoundMesh( compound, childIndex ); + const int* m = compoundMesh.materialIndices; + _Static_assert( B3_MAX_COMPOUND_MESH_MATERIALS == 4, "too many materials in compound mesh" ); + + return (b3ChildShape){ + .mesh = + { + .data = compoundMesh.meshData, + .scale = compoundMesh.scale, + }, + .transform = compoundMesh.transform, + .materialIndices = { m[0], m[1], m[2], m[3] }, + .type = b3_meshShape, + }; + } + childIndex -= compound->meshCount; + + B3_ASSERT( 0 <= childIndex && childIndex < compound->sphereCount ); + + // Sphere? + { + b3CompoundSphere compoundSphere = b3GetCompoundSphere( compound, childIndex ); + return (b3ChildShape){ + .sphere = compoundSphere.sphere, + .transform = b3Transform_identity, + .materialIndices = { compoundSphere.materialIndex }, + .type = b3_sphereShape, + }; + } +} + +static inline size_t vt_wyhash( const void* key, size_t len ); + +static inline uint64_t b3HashHull( const b3Hull* hull ) +{ + return vt_wyhash( hull, hull->byteCount ); +} + +static bool b3CompareHulls( const b3Hull* hull1, const b3Hull* hull2 ) +{ + if ( hull1 == hull2 ) + { + return true; + } + + if ( hull1->byteCount != hull2->byteCount ) + { + return false; + } + + int result = memcmp( hull1, hull2, hull1->byteCount ); + return result == 0; +} + +#define NAME b3HullMap +#define KEY_TY const b3Hull* +#define VAL_TY int +#define HASH_FN b3HashHull +#define CMPR_FN b3CompareHulls +#define MALLOC_FN b3Alloc +#define FREE_FN b3Free +#include "verstable.h" + +static inline uint64_t b3HashMesh( const b3MeshData* mesh ) +{ + return vt_wyhash( mesh, mesh->byteCount ); +} + +static bool b3CompareMeshes( const b3MeshData* mesh1, const b3MeshData* mesh2 ) +{ + if ( mesh1 == mesh2 ) + { + return true; + } + + if ( mesh1->byteCount != mesh2->byteCount ) + { + return false; + } + + int result = memcmp( mesh1, mesh2, mesh1->byteCount ); + return result == 0; +} + +#define NAME b3MeshMap +#define KEY_TY const b3MeshData* +#define VAL_TY int +#define HASH_FN b3HashMesh +#define CMPR_FN b3CompareMeshes +#define MALLOC_FN b3Alloc +#define FREE_FN b3Free +#include "verstable.h" + +static inline uint64_t b3HashMaterial( const b3SurfaceMaterial* material ) +{ + return vt_wyhash( material, sizeof( b3SurfaceMaterial ) ); +} + +static bool b3CompareMaterials( const b3SurfaceMaterial* mat1, const b3SurfaceMaterial* mat2 ) +{ + if ( mat1 == mat2 ) + { + return true; + } + + int result = memcmp( mat1, mat2, sizeof( b3SurfaceMaterial ) ); + return result == 0; +} + +#define NAME b3MaterialMap +#define KEY_TY const b3SurfaceMaterial* +#define VAL_TY int +#define HASH_FN b3HashMaterial +#define CMPR_FN b3CompareMaterials +#define MALLOC_FN b3Alloc +#define FREE_FN b3Free +#include "verstable.h" + +b3Compound* b3CreateCompound( const b3CompoundDef* def ) +{ + int convexCount = def->capsuleCount + def->hullCount + def->sphereCount; + int shapeCount = convexCount + def->meshCount; + + if ( shapeCount >= B3_MAX_CHILD_SHAPES ) + { + B3_ASSERT( false ); + return NULL; + } + + b3DynamicTree tree = b3DynamicTree_Create( shapeCount ); + + int childIndex = 0; + + // Instances + int capsuleCount = def->capsuleCount; + b3CompoundCapsule* capsuleInstances = b3AllocZeroed( capsuleCount * sizeof( b3CompoundCapsule ) ); + int hullCount = def->hullCount; + b3HullInstance* hullInstances = b3AllocZeroed( hullCount * sizeof( b3HullInstance ) ); + int meshCount = def->meshCount; + b3MeshInstance* meshInstances = b3AllocZeroed( meshCount * sizeof( b3MeshInstance ) ); + int sphereCount = def->sphereCount; + b3CompoundSphere* sphereInstances = b3AllocZeroed( sphereCount * sizeof( b3CompoundSphere ) ); + + // Determine material capacity + int materialCapacity = convexCount; + for ( int i = 0; i < def->meshCount; ++i ) + { + B3_ASSERT( def->meshes[i].materialCount > 0 ); + materialCapacity += def->meshes[i].materialCount; + } + + // Material map for convex material sharing. Mesh materials are not shared for simplicity. + b3MaterialMap materialMap; + b3MaterialMap_init( &materialMap ); + b3MaterialMap_reserve( &materialMap, materialCapacity ); + b3SurfaceMaterial* materials = b3AllocZeroed( materialCapacity * sizeof( b3SurfaceMaterial ) ); + int materialCount = 0; + + for ( int i = 0; i < def->capsuleCount; ++i ) + { + const b3CompoundCapsuleDef* capsuleDef = def->capsules + i; + capsuleInstances[i].capsule = capsuleDef->capsule; + + // Look for an existing material + b3MaterialMap_itr materialItr = b3MaterialMap_get_or_insert( &materialMap, &capsuleDef->material, materialCount ); + + // Get the shared material index + int materialIndex = materialItr.data->val; + capsuleInstances[i].materialIndex = materialIndex; + + // Is this a new material? + if ( materialIndex == materialCount ) + { + materials[materialIndex] = capsuleDef->material; + materialCount += 1; + } + + b3AABB aabb = b3ComputeCapsuleAABB( &capsuleDef->capsule, b3Transform_identity ); + b3DynamicTree_CreateProxy( &tree, aabb, ~0ull, childIndex ); + childIndex += 1; + } + + // Hulls + b3SharedHull* sharedHulls = b3AllocZeroed( hullCount * sizeof( b3SharedHull ) ); + int sharedHullCount = 0; + + if ( hullCount > 0 ) + { + b3HullMap hullMap; + b3HullMap_init( &hullMap ); + b3HullMap_reserve( &hullMap, hullCount ); + + for ( int i = 0; i < hullCount; ++i ) + { + const b3CompoundHullDef* hullDef = def->hulls + i; + const b3Hull* hull = hullDef->hull; + b3AABB aabb = b3ComputeHullAABB( hull, hullDef->transform ); + b3DynamicTree_CreateProxy( &tree, aabb, ~0ull, childIndex ); + childIndex += 1; + + // Look for an existing material + b3MaterialMap_itr materialItr = b3MaterialMap_get_or_insert( &materialMap, &hullDef->material, materialCount ); + + // Get the shared material index + int materialIndex = materialItr.data->val; + hullInstances[i].materialIndex = materialIndex; + + // Is this a new material? + if ( materialIndex == materialCount ) + { + materials[materialIndex] = hullDef->material; + materialCount += 1; + } + + hullInstances[i].transform = hullDef->transform; + + // Look for an existing matching hull + b3HullMap_itr itr = b3HullMap_get_or_insert( &hullMap, hull, sharedHullCount ); + + // Get the unique index for this hull + int sharedHullIndex = itr.data->val; + + // The offset isn't known yet, so store the index of the shared hull + hullInstances[i].hullOffset = sharedHullIndex; + + // Is this a new hull? + if ( sharedHullIndex == sharedHullCount ) + { + // Create a shared hull. The offset is determined below. + sharedHulls[sharedHullIndex].hull = hull; + sharedHulls[sharedHullIndex].hullOffset = B3_NULL_INDEX; + sharedHullCount += 1; + } + } + + b3HullMap_cleanup( &hullMap ); + } + + // Meshes + b3SharedMesh* sharedMeshes = b3AllocZeroed( meshCount * sizeof( b3SharedMesh ) ); + int sharedMeshCount = 0; + + if ( meshCount > 0 ) + { + b3MeshMap meshMap; + b3MeshMap_init( &meshMap ); + b3MeshMap_reserve( &meshMap, meshCount ); + + for ( int i = 0; i < meshCount; ++i ) + { + const b3CompoundMeshDef* meshDef = def->meshes + i; + + const b3MeshData* meshData = meshDef->meshData; + b3AABB aabb = b3ComputeMeshAABB( meshData, meshDef->transform, meshDef->scale ); + b3DynamicTree_CreateProxy( &tree, aabb, ~0ull, childIndex ); + childIndex += 1; + + // No effort to share mesh materials. It would be easier to do if the number of materials was limited. + B3_ASSERT( meshData->materialCount == meshDef->materialCount ); + + for ( int j = 0; j < meshDef->materialCount; ++j ) + { + // Look for an existing material + b3MaterialMap_itr materialItr = + b3MaterialMap_get_or_insert( &materialMap, &meshDef->materials[j], materialCount ); + + // Get the shared material index + int materialIndex = materialItr.data->val; + meshInstances[i].materialIndices[j] = materialIndex; + + // Is this a new material? + if ( materialIndex == materialCount ) + { + materials[materialIndex] = meshDef->materials[j]; + materialCount += 1; + } + } + + // Look for an existing matching mesh + b3MeshMap_itr itr = b3MeshMap_get_or_insert( &meshMap, meshData, sharedMeshCount ); + + // Get the shared mesh index + int sharedMeshIndex = itr.data->val; + + // Create mesh instance + meshInstances[i].transform = def->meshes[i].transform; + meshInstances[i].scale = def->meshes[i].scale; + + // The offset isn't known yet, so store the index of the shared mesh + meshInstances[i].meshOffset = sharedMeshIndex; + + // Is this a new mesh? + if ( sharedMeshIndex == sharedMeshCount ) + { + // Create a shared mesh. The offset is determined below. + sharedMeshes[sharedMeshIndex].meshData = meshData; + sharedMeshes[sharedMeshIndex].meshOffset = B3_NULL_INDEX; + sharedMeshCount += 1; + } + } + + b3MeshMap_cleanup( &meshMap ); + } + + // Spheres + for ( int i = 0; i < def->sphereCount; ++i ) + { + const b3CompoundSphereDef* sphereDef = def->spheres + i; + sphereInstances[i].sphere = sphereDef->sphere; + + // Look for an existing material + b3MaterialMap_itr materialItr = b3MaterialMap_get_or_insert( &materialMap, &sphereDef->material, materialCount ); + + // Get the shared material index + int materialIndex = materialItr.data->val; + sphereInstances[i].materialIndex = materialIndex; + + // Is this a new material? + if ( materialIndex == materialCount ) + { + materials[materialIndex] = sphereDef->material; + materialCount += 1; + } + + b3AABB aabb = b3ComputeSphereAABB( &sphereDef->sphere, b3Transform_identity ); + b3DynamicTree_CreateProxy( &tree, aabb, ~0ull, childIndex ); + childIndex += 1; + } + + B3_ASSERT( materialCount <= materialCapacity ); + B3_ASSERT( tree.nodeCount > 0 ); + + b3DynamicTree_Rebuild( &tree, true ); + + int byteCount = sizeof( b3Compound ); + + // Tree nodes - todo 64 byte alignment + int nodeOffset = byteCount; + byteCount += tree.nodeCapacity * sizeof( b3TreeNode ); + + int materialOffset = byteCount; + byteCount += materialCount * sizeof( b3SurfaceMaterial ); + + int capsuleOffset = byteCount; + byteCount += def->capsuleCount * sizeof( b3CompoundCapsule ); + + // Hull data layout has another level of indirection to allow for tight data packing + // 1. hull instance array : hull count array of b3HullInstance with individual hull transforms and offsets + // 2. heterogeneous array of shared hull data : each shared hull can have a different byte count, so direct indexing is not + // possible + int hullArrayOffset = byteCount; + + // Array of hull instances + byteCount += hullCount * sizeof( b3HullInstance ); + + // Packed shared hull blobs + for ( int i = 0; i < sharedHullCount; ++i ) + { + sharedHulls[i].hullOffset = byteCount; + byteCount += sharedHulls[i].hull->byteCount; + } + + // Mesh data layout has another level of indirection to allow for tight data packing + // 1. mesh instance array : mesh count array of b3MeshInstance with individual mesh transform, scale, and offset + // 2. heterogeneous array of shared mesh data : each shared mesh can have a different byte count, so direct indexing is not + // possible + int meshArrayOffset = byteCount; + + // Array of mesh instances + byteCount += meshCount * sizeof( b3MeshInstance ); + + // Packed shared mesh blobs + for ( int i = 0; i < sharedMeshCount; ++i ) + { + sharedMeshes[i].meshOffset = byteCount; + byteCount += sharedMeshes[i].meshData->byteCount; + } + + int sphereOffset = byteCount; + byteCount += def->sphereCount * sizeof( b3CompoundSphere ); + + b3Compound* compound = b3Alloc( byteCount ); + memset( compound, 0, byteCount ); + + compound->version = B3_COMPOUND_VERSION; + compound->byteCount = byteCount; + compound->nodeOffset = nodeOffset; + memcpy( &compound->tree, &tree, sizeof( b3DynamicTree ) ); + + // todo clean up this mess + compound->tree.freeList = 0; + compound->tree.leafIndices = NULL; + compound->tree.leafBoxes = NULL; + compound->tree.leafCenters = NULL; + compound->tree.binIndices = NULL; + compound->tree.rebuildCapacity = 0; + + compound->tree.nodes = NULL; + compound->materialOffset = materialOffset; + compound->materialCount = materialCount; + compound->capsuleOffset = capsuleOffset; + compound->capsuleCount = capsuleCount; + compound->hullOffset = hullArrayOffset; + compound->hullCount = hullCount; + compound->meshOffset = meshArrayOffset; + compound->meshCount = meshCount; + compound->sphereOffset = sphereOffset; + compound->sphereCount = sphereCount; + + // Tree nodes + b3TreeNode* nodes = b3GetCompoundNodes( compound ); + memcpy( nodes, tree.nodes, tree.nodeCapacity * sizeof( b3TreeNode ) ); + compound->tree.nodes = nodes; + + // Materials + B3_ASSERT( materialCount > 0 ); + b3SurfaceMaterial* destinationMaterials = (b3SurfaceMaterial*)( (intptr_t)compound + compound->materialOffset ); + if ( materials != NULL ) + { + memcpy( destinationMaterials, materials, materialCount * sizeof( b3SurfaceMaterial ) ); + } + + // Capsules + if ( def->capsuleCount > 0 ) + { + B3_ASSERT( compound->capsuleOffset > 0 ); + b3CompoundCapsule* capsules = (b3CompoundCapsule*)( (intptr_t)compound + compound->capsuleOffset ); + memcpy( capsules, capsuleInstances, capsuleCount * sizeof( b3CompoundCapsule ) ); + } + + // Hulls + for ( int i = 0; i < hullCount; ++i ) + { + // Fix up offsets + int sharedIndex = hullInstances[i].hullOffset; + B3_ASSERT( 0 <= sharedIndex && sharedIndex < sharedHullCount ); + hullInstances[i].hullOffset = sharedHulls[sharedIndex].hullOffset; + } + + b3HullInstance* destinationHullInstances = (b3HullInstance*)( (intptr_t)compound + hullArrayOffset ); + memcpy( destinationHullInstances, hullInstances, hullCount * sizeof( b3HullInstance ) ); + + for ( int i = 0; i < sharedHullCount; ++i ) + { + int offset = sharedHulls[i].hullOffset; + b3Hull* destinationHull = (b3Hull*)( (intptr_t)compound + offset ); + memcpy( destinationHull, sharedHulls[i].hull, sharedHulls[i].hull->byteCount ); + } + + compound->sharedHullCount = sharedHullCount; + + // Meshes + for ( int i = 0; i < meshCount; ++i ) + { + // Fix up offsets + int sharedIndex = meshInstances[i].meshOffset; + B3_ASSERT( 0 <= sharedIndex && sharedIndex < sharedMeshCount ); + meshInstances[i].meshOffset = sharedMeshes[sharedIndex].meshOffset; + } + + b3MeshInstance* destinationMeshInstances = (b3MeshInstance*)( (intptr_t)compound + meshArrayOffset ); + memcpy( destinationMeshInstances, meshInstances, meshCount * sizeof( b3MeshInstance ) ); + + for ( int i = 0; i < sharedMeshCount; ++i ) + { + int offset = sharedMeshes[i].meshOffset; + b3MeshData* destinationMesh = (b3MeshData*)( (intptr_t)compound + offset ); + memcpy( destinationMesh, sharedMeshes[i].meshData, sharedMeshes[i].meshData->byteCount ); + } + + compound->sharedMeshCount = sharedMeshCount; + + // Spheres + if ( def->sphereCount > 0 ) + { + B3_ASSERT( compound->sphereOffset > 0 ); + b3CompoundSphere* spheres = (b3CompoundSphere*)( (intptr_t)compound + compound->sphereOffset ); + memcpy( spheres, sphereInstances, sphereCount * sizeof( b3CompoundSphere ) ); + } + + b3MaterialMap_cleanup( &materialMap ); + b3Free( sharedHulls, hullCount * sizeof( b3SharedHull ) ); + b3Free( sharedMeshes, meshCount * sizeof( b3SharedMesh ) ); + b3Free( capsuleInstances, capsuleCount * sizeof( b3CompoundCapsule ) ); + b3Free( hullInstances, hullCount * sizeof( b3HullInstance ) ); + b3Free( meshInstances, meshCount * sizeof( b3MeshInstance ) ); + b3Free( sphereInstances, sphereCount * sizeof( b3CompoundSphere ) ); + b3Free( materials, materialCapacity * sizeof( b3SurfaceMaterial ) ); + b3DynamicTree_Destroy( &tree ); + + return compound; +} + +void b3DestroyCompound( b3Compound* compound ) +{ + b3Free( compound, compound->byteCount ); +} + +uint8_t* b3ConvertCompoundToBytes( b3Compound* compound ) +{ + // scrub this pointer before serialization + compound->tree.nodes = NULL; + return (uint8_t*)compound; +} + +b3Compound* b3ConvertBytesToCompound( uint8_t* bytes, int byteCount ) +{ + b3Compound* compound = (b3Compound*)bytes; + if ( compound->version != B3_COMPOUND_VERSION ) + { + return NULL; + } + + if ( compound->byteCount < (int)sizeof( b3Compound ) ) + { + return NULL; + } + + if ( byteCount != compound->byteCount ) + { + return NULL; + } + + if ( compound->nodeOffset <= 0 ) + { + return NULL; + } + + // this mutates the input bytes + compound->tree.nodes = (b3TreeNode*)( (intptr_t)compound + compound->nodeOffset ); + return compound; +} + +b3AABB b3ComputeCompoundAABB( const b3Compound* shape, b3Transform transform ) +{ + B3_ASSERT( shape->nodeOffset > 0 ); + + const b3TreeNode* nodes = (const b3TreeNode*)( (intptr_t)shape + shape->nodeOffset ); + int root = shape->tree.root; + b3AABB aabb = nodes[root].aabb; + return b3AABB_Transform( transform, aabb ); +} + +struct b3CompoundOverlapContext +{ + const b3Compound* compound; + // transform of the compound + b3Transform transform; + b3ShapeProxy proxy; + bool overlap; +}; + +static bool b3CompoundOverlapCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int childIndex = (int)userData; + struct b3CompoundOverlapContext* overlapContext = context; + b3ChildShape child = b3GetCompoundChild( overlapContext->compound, childIndex ); + + b3Transform transform = b3MulTransforms( overlapContext->transform, child.transform ); + + bool overlap = false; + switch ( child.type ) + { + case b3_capsuleShape: + overlap = b3OverlapCapsule( &child.capsule, transform, &overlapContext->proxy ); + break; + + case b3_hullShape: + overlap = b3OverlapHull( child.hull, transform, &overlapContext->proxy ); + break; + + case b3_meshShape: + overlap = b3OverlapMesh( &child.mesh, transform, &overlapContext->proxy ); + break; + + case b3_sphereShape: + overlap = b3OverlapSphere( &child.sphere, transform, &overlapContext->proxy ); + break; + + default: + B3_ASSERT( false ); + break; + } + + if ( overlap ) + { + // Done + overlapContext->overlap = true; + return false; + } + + // Continue the query if there is no overlap + return true; +} + +bool b3OverlapCompound( const b3Compound* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ) +{ + struct b3CompoundOverlapContext context = { + .compound = shape, + .transform = shapeTransform, + .proxy = *proxy, + .overlap = false, + }; + + b3AABB aabb = { proxy->points[0], proxy->points[0] }; + for ( int i = 1; i < proxy->count; ++i ) + { + aabb.lowerBound = b3Min( aabb.lowerBound, proxy->points[i] ); + aabb.upperBound = b3Max( aabb.upperBound, proxy->points[i] ); + } + + b3Vec3 r = { proxy->radius, proxy->radius, proxy->radius }; + aabb.lowerBound = b3Sub( aabb.lowerBound, r ); + aabb.upperBound = b3Add( aabb.upperBound, r ); + + (void)b3DynamicTree_Query( &shape->tree, aabb, ~0ull, false, b3CompoundOverlapCallback, &context ); + + return context.overlap; +} + +struct b3CompoundCastContext +{ + const b3Compound* compound; + b3CastOutput* output; +}; + +static float b3CompoundRayCastCallback( const b3RayCastInput* input, int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + struct b3CompoundCastContext* castContext = context; + const b3Compound* compound = castContext->compound; + + int childIndex = (int)userData; + + b3ChildShape child = b3GetCompoundChild( compound, childIndex ); + + b3RayCastInput localInput = *input; + localInput.origin = b3InvTransformPoint( child.transform, input->origin ); + localInput.translation = b3InvRotateVector( child.transform.q, input->translation ); + + b3CastOutput output = { 0 }; + + switch ( child.type ) + { + case b3_capsuleShape: + output = b3RayCastCapsule( &child.capsule, &localInput ); + output.materialIndex = child.materialIndices[0]; + break; + + case b3_hullShape: + output = b3RayCastHull( child.hull, &localInput ); + output.materialIndex = child.materialIndices[0]; + break; + + case b3_meshShape: + { + output = b3RayCastMesh( &child.mesh, &localInput ); + B3_ASSERT( 0 <= output.materialIndex ); + int childMaterialIndex = b3MinInt( output.materialIndex, B3_MAX_COMPOUND_MESH_MATERIALS - 1 ); + output.materialIndex = child.materialIndices[childMaterialIndex]; + } + break; + + case b3_sphereShape: + output = b3RayCastSphere( &child.sphere, &localInput ); + output.materialIndex = child.materialIndices[0]; + break; + + default: + B3_ASSERT( false ); + break; + } + + if ( output.hit ) + { + output.point = b3TransformPoint( child.transform, output.point ); + output.normal = b3RotateVector( child.transform.q, output.normal ); + output.childIndex = childIndex; + *castContext->output = output; + return output.fraction; + } + + return input->maxFraction; +} + +b3CastOutput b3RayCastCompound( const b3Compound* shape, const b3RayCastInput* input ) +{ + b3CastOutput result = { 0 }; + + struct b3CompoundCastContext context = { + .compound = shape, + .output = &result, + }; + (void)b3DynamicTree_RayCast( &shape->tree, input, ~0ull, false, b3CompoundRayCastCallback, &context ); + return result; +} + +static float b3CompoundShapeCastCallback( const b3ShapeCastInput* input, int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + struct b3CompoundCastContext* castContext = context; + const b3Compound* compound = castContext->compound; + + int childIndex = (int)userData; + + b3ChildShape child = b3GetCompoundChild( compound, childIndex ); + + b3ShapeCastInput localInput = *input; + b3Vec3 localPoints[B3_MAX_SHAPE_CAST_POINTS]; + + localInput.proxy.count = b3MinInt( input->proxy.count, B3_MAX_SHAPE_CAST_POINTS ); + + b3Transform invTransform = b3InvertTransform( child.transform ); + b3Matrix3 R = b3MakeMatrixFromQuat( invTransform.q ); + + for ( int i = 0; i < localInput.proxy.count; ++i ) + { + localPoints[i] = b3Add( b3MulMV( R, input->proxy.points[i] ), invTransform.p ); + } + + localInput.proxy.points = localPoints; + localInput.translation = b3MulMV( R, input->translation ); + + b3CastOutput output = { 0 }; + + switch ( child.type ) + { + case b3_capsuleShape: + output = b3ShapeCastCapsule( &child.capsule, &localInput ); + output.materialIndex = child.materialIndices[0]; + break; + + case b3_hullShape: + output = b3ShapeCastHull( child.hull, &localInput ); + output.materialIndex = child.materialIndices[0]; + break; + + case b3_meshShape: + { + output = b3ShapeCastMesh( &child.mesh, &localInput ); + B3_ASSERT( 0 <= output.materialIndex ); + int childMaterialIndex = b3MinInt( output.materialIndex, B3_MAX_COMPOUND_MESH_MATERIALS - 1 ); + output.materialIndex = child.materialIndices[childMaterialIndex]; + } + break; + + case b3_sphereShape: + output = b3ShapeCastSphere( &child.sphere, &localInput ); + output.materialIndex = child.materialIndices[0]; + break; + + default: + B3_ASSERT( false ); + break; + } + + if ( output.hit ) + { + output.point = b3TransformPoint( child.transform, output.point ); + output.normal = b3RotateVector( child.transform.q, output.normal ); + output.childIndex = childIndex; + *castContext->output = output; + return output.fraction; + } + + return input->maxFraction; +} + +b3CastOutput b3ShapeCastCompound( const b3Compound* shape, const b3ShapeCastInput* input ) +{ + b3CastOutput result = { 0 }; + + struct b3CompoundCastContext context = { + .compound = shape, + .output = &result, + }; + (void)b3DynamicTree_ShapeCast( &shape->tree, input, ~0ull, false, b3CompoundShapeCastCallback, &context ); + return result; +} + +struct b3CompoundQueryContext +{ + const b3Compound* compound; + b3CompoundQueryFcn* fcn; + void* userContext; +}; + +static bool TreeQueryCallbackFcn( int proxyId, uint64_t userData, void* treeContext ) +{ + B3_UNUSED( proxyId ); + struct b3CompoundQueryContext* context = treeContext; + return context->fcn( context->compound, (int)userData, context->userContext ); +} + +void b3QueryCompound( const b3Compound* compound, b3AABB aabb, b3CompoundQueryFcn* fcn, void* context ) +{ + struct b3CompoundQueryContext compoundContext = { + .compound = compound, + .fcn = fcn, + .userContext = context, + }; + + b3DynamicTree_Query( &compound->tree, aabb, B3_DEFAULT_MASK_BITS, false, TreeQueryCallbackFcn, &compoundContext ); +} + +#if 0 +struct b3CompoundImpactContext +{ + b3TOIInput toiInput; + b3TOIOutput toiOutput; + b3Transform compoundTransform; + + // Bounds local to compound + b3AABB localSweepBoundsB; + + // Centroid of shape in body B local space + b3Vec3 localCentroidB; + float fallbackRadius; +}; + +static bool b3CompoundTimeOfImpactFcn( const b3Compound* compound, int childIndex, void* context ) +{ + b3CompoundImpactContext* toiContext = (b3CompoundImpactContext*)context; + + b3ChildShape child = b3GetCompoundChild( compound, childIndex ); + + b3TOIOutput output = {0 }; + toiContext->toiInput.sweepA = b3MakeCompoundChildSweep( toiContext->compoundTransform, child.transform ); + + switch ( child.type ) + { + case b3_capsuleShape: + { + toiContext->toiInput.proxyA.points = &child.capsule.center1; + toiContext->toiInput.proxyA.count = 2; + toiContext->toiInput.proxyA.radius = child.capsule.radius; + output = b3TimeOfImpact( &toiContext->toiInput ); + } + break; + + case b3_hullShape: + { + toiContext->toiInput.proxyA.points = b3GetHullPoints( child.hull ); + toiContext->toiInput.proxyA.count = child.hull->vertexCount; + toiContext->toiInput.proxyA.radius = 0.0f; + output = b3TimeOfImpact( &toiContext->toiInput ); + } + break; + + case b3_meshShape: + { + b3MeshImpactContext meshContext = {0}; + meshContext.toiInput = toiContext->toiInput; + meshContext.isSensor = false; + meshContext.localCentroidB = toiContext->localCentroidB; + meshContext.fallbackRadius = toiContext->fallbackRadius; + + b3Transform meshWorldTransform = b3MulTransforms( toiContext->compoundTransform, child.transform ); + + const b3Sweep* sweepB = &toiContext->toiInput.sweepB; + b3Transform xfB1 = { + .p = sweepB->c1 - b3RotateVector( sweepB->q1, sweepB->localCenter ), + .q = sweepB->q1, + }; + + b3Transform xfB2 = { + .p = sweepB->c2 - b3RotateVector( sweepB->q2, sweepB->localCenter ), + .q = sweepB->q2, + }; + + meshContext.meshLocalCentroidB1 = + b3InvTransformPoint( meshWorldTransform, b3TransformPoint( xfB1, meshContext.localCentroidB ) ); + meshContext.meshLocalCentroidB2 = + b3InvTransformPoint( meshWorldTransform, b3TransformPoint( xfB2, meshContext.localCentroidB ) ); + + // Bounds local to mesh + b3AABB localBounds = b3AABB_Transform( b3InvertTransform( child.transform ), toiContext->localSweepBoundsB ); + + b3QueryMesh( &child.mesh, localBounds, b3MeshTimeOfImpactFcn, &meshContext ); + + output = meshContext.toiOutput; + } + break; + + case b3_sphereShape: + { + toiContext->toiInput.proxyA.points = &child.sphere.center; + toiContext->toiInput.proxyA.count = 1; + toiContext->toiInput.proxyA.radius = child.sphere.radius; + output = b3TimeOfImpact( &toiContext->toiInput ); + } + break; + + default: + B3_ASSERT( false ); + break; + } + + if ( 0.0f < output.fraction && output.fraction < toiContext->toiInput.maxFraction ) + { + toiContext->toiOutput = output; + toiContext->toiInput.maxFraction = output.fraction; + } + + // Clear this to be safe + toiContext->toiInput.proxyA = {0}; + + // Continue the query + return true; +} + +b3TOIOutput b3CompoundTimeOfImpact(const b3Compound* compound, b3Transform transform, const b3ShapeProxy* proxy, + const b3Sweep* sweep, float maxFraction) +{ + b3CompoundImpactContext context = {0}; + context.toiInput.proxyB = b3MakeShapeProxy( shapeB ); + context.toiInput.sweepB = *sweepB; + context.toiInput.maxFraction = maxFraction; + + context.compoundTransform = { + .p = sweepA->c1, + .q = sweepA->q1, + }; + + b3Vec3 localCentroidB = b3GetShapeCentroid( shapeB ); + context.localCentroidB = localCentroidB; + + b3ShapeExtent extents = b3ComputeShapeExtent( shapeB, context.localCentroidB ); + context.fallbackRadius = b3MaxFloat( 0.5f * extents.minExtent, B3_SPECULATIVE_DISTANCE ); + + // Swept bounds of shapeB + b3AABB aabb = b3ComputeSweptShapeAABB( shapeB, sweepB, maxFraction ); + + // Bounds local to mesh + b3AABB localBounds = b3AABB_Transform( b3InvertTransform( context.compoundTransform ), bounds ); + context.localSweepBoundsB = localBounds; + + b3DynamicTree_Query( &compound->tree, aabb, B3_DEFAULT_MASK_BITS, false, TreeQueryCallbackFcn, &compoundContext ); + + return context.toiOutput; +} +#endif + +// xf = xfP * xfC +b3Sweep b3MakeCompoundChildSweep( b3Transform compoundTransform, b3Transform childTransform ) +{ + b3Transform xf = b3MulTransforms( compoundTransform, childTransform ); + return (b3Sweep){ + .localCenter = b3Vec3_zero, + .c1 = xf.p, + .c2 = xf.p, + .q1 = xf.q, + .q2 = xf.q, + }; +} + +struct b3CompoundMoverContext +{ + const b3Compound* compound; + b3PlaneResult* planes; + int planeCapacity; + int planeCount; + b3Capsule mover; +}; + +static bool b3CompoundMoverCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int childIndex = (int)userData; + struct b3CompoundMoverContext* moverContext = context; + b3ChildShape child = b3GetCompoundChild( moverContext->compound, childIndex ); + + // Transform mover to child space + b3Capsule localMover; + localMover.center1 = b3InvTransformPoint( child.transform, moverContext->mover.center1 ); + localMover.center2 = b3InvTransformPoint( child.transform, moverContext->mover.center2 ); + localMover.radius = moverContext->mover.radius; + + int capacity = moverContext->planeCapacity - moverContext->planeCount; + B3_ASSERT( capacity > 0 ); + + b3PlaneResult* planes = moverContext->planes + moverContext->planeCount; + int planeCount = 0; + + switch ( child.type ) + { + case b3_capsuleShape: + planeCount = b3CollideMoverAndCapsule( planes, &child.capsule, &localMover ); + break; + + case b3_hullShape: + planeCount = b3CollideMoverAndHull( planes, child.hull, &localMover ); + break; + + case b3_meshShape: + planeCount = b3CollideMoverAndMesh( planes, capacity, &child.mesh, &localMover ); + break; + + case b3_sphereShape: + planeCount = b3CollideMoverAndSphere( planes, &child.sphere, &localMover ); + break; + + default: + B3_ASSERT( false ); + break; + } + + // Transform results back to shape space + for ( int i = 0; i < planeCount; ++i ) + { + planes[i].plane.normal = b3RotateVector( child.transform.q, planes[i].plane.normal ); + planes[i].point = b3TransformPoint( child.transform, planes[i].point ); + } + + moverContext->planeCount += planeCount; + + // Continue query while there is room for more planes + return moverContext->planeCount < moverContext->planeCapacity; +} + +int b3CollideMoverAndCompound( b3PlaneResult* planes, int capacity, const b3Compound* shape, const b3Capsule* mover ) +{ + struct b3CompoundMoverContext context = { + .compound = shape, + .planes = planes, + .planeCapacity = capacity, + .planeCount = 0, + .mover = *mover, + }; + + b3AABB aabb; + aabb.lowerBound = b3Min( mover->center1, mover->center2 ); + aabb.upperBound = b3Max( mover->center1, mover->center2 ); + b3Vec3 r = { mover->radius, mover->radius, mover->radius }; + aabb.lowerBound = b3Sub( aabb.lowerBound, r ); + aabb.upperBound = b3Add( aabb.upperBound, r ); + + (void)b3DynamicTree_Query( &shape->tree, aabb, ~0ull, false, b3CompoundMoverCallback, &context ); + + return context.planeCount; +} diff --git a/src/compound.h b/src/compound.h new file mode 100644 index 0000000..237556c --- /dev/null +++ b/src/compound.h @@ -0,0 +1,46 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box3d/types.h" + +typedef struct b3ChildShape +{ + union + { + b3Capsule capsule; + const b3Hull* hull; + b3Mesh mesh; + b3Sphere sphere; + }; + + b3Transform transform; + + // Index 0 is used for convex shapes. + // todo limit to 64K? + int materialIndices[B3_MAX_COMPOUND_MESH_MATERIALS]; + b3ShapeType type; +} b3ChildShape; + +typedef bool b3CompoundQueryFcn( const b3Compound* compound, int childIndex, void* context ); + +#ifdef __cplusplus +extern "C" +{ +#endif + +b3ChildShape b3GetCompoundChild( const b3Compound* compound, int childIndex ); +void b3QueryCompound( const b3Compound* compound, b3AABB aabb, b3CompoundQueryFcn* fcn, void* context ); + +b3TOIOutput b3CompoundTimeOfImpact( const b3Compound* compound, b3Transform transform, const b3ShapeProxy* proxy, + const b3Sweep* sweep, float maxFraction ); + +// Transforms a sweep for a compound child shape +b3Sweep b3MakeCompoundChildSweep( b3Transform compoundTransform, b3Transform childTransform ); + +int b3CollideMoverAndCompound( b3PlaneResult* planes, int capacity, const b3Compound* shape, const b3Capsule* mover ); + +#ifdef __cplusplus +} +#endif diff --git a/src/constraint_graph.c b/src/constraint_graph.c new file mode 100644 index 0000000..9f06e9e --- /dev/null +++ b/src/constraint_graph.c @@ -0,0 +1,328 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "constraint_graph.h" + +#include "bitset.h" +#include "body.h" +#include "contact.h" +#include "joint.h" +#include "physics_world.h" + +#include + +// Solver using graph coloring. Islands are only used for sleep. +// High-Performance Physical Simulations on Next-Generation Architecture with Many Cores +// http://web.eecs.umich.edu/~msmelyan/papers/physsim_onmanycore_itj.pdf + +// Kinematic bodies have to be treated like dynamic bodies in graph coloring. Unlike static bodies, we cannot use a dummy solver +// body for kinematic bodies. We cannot access a kinematic body from multiple threads efficiently because the SIMD solver body +// scatter would write to the same kinematic body from multiple threads. Even if these writes don't modify the body, they will +// cause horrible cache stalls. To make this feasible I would need a way to block these writes. + +// This is used for debugging by making all constraints be assigned to overflow. +#define B3_FORCE_OVERFLOW 0 + +static const b3HexColor b3_graphColors[B3_GRAPH_COLOR_COUNT] = { + b3_colorRed, b3_colorOrange, b3_colorYellow, b3_colorLimeGreen, b3_colorSpringGreen, + b3_colorAqua, b3_colorDodgerBlue, b3_colorBlueViolet, b3_colorMagenta, b3_colorDeepPink, + b3_colorCrimson, b3_colorCoral, b3_colorGold, b3_colorGreenYellow, b3_colorMediumSeaGreen, + b3_colorTurquoise, b3_colorDeepSkyBlue, b3_colorCornflowerBlue, b3_colorMediumSlateBlue, b3_colorMediumOrchid, + b3_colorHotPink, b3_colorTomato, b3_colorKhaki, b3_colorSilver, +}; + +b3HexColor b3GetGraphColor( int index ) +{ + B3_ASSERT( 0 <= index && index < B3_GRAPH_COLOR_COUNT ); + return b3_graphColors[index]; +} + +void b3CreateGraph( b3ConstraintGraph* graph, int bodyCapacity ) +{ + _Static_assert( B3_GRAPH_COLOR_COUNT >= 2, "must have at least two constraint graph colors" ); + _Static_assert( B3_OVERFLOW_INDEX == B3_GRAPH_COLOR_COUNT - 1, "bad over flow index" ); + + *graph = (b3ConstraintGraph){ 0 }; + + bodyCapacity = b3MaxInt( bodyCapacity, 8 ); + + // Initialize graph color bit set. + // No bitset for overflow color. + for ( int i = 0; i < B3_OVERFLOW_INDEX; ++i ) + { + b3GraphColor* color = graph->colors + i; + color->bodySet = b3CreateBitSet( bodyCapacity ); + b3SetBitCountAndClear( &color->bodySet, bodyCapacity ); + } +} + +void b3DestroyGraph( b3ConstraintGraph* graph ) +{ + for ( int i = 0; i < B3_GRAPH_COLOR_COUNT; ++i ) + { + b3GraphColor* color = graph->colors + i; + + // The bit set should never be used on the overflow color + B3_ASSERT( i != B3_OVERFLOW_INDEX || color->bodySet.bits == NULL ); + + b3DestroyBitSet( &color->bodySet ); + + b3Array_Destroy( color->convexContacts ); + b3Array_Destroy( color->contacts ); + b3Array_Destroy( color->jointSims ); + } +} + +// Contacts are always created as non-touching. They get cloned into the constraint +// graph once they are found to be touching. +void b3AddContactToGraph( b3World* world, b3Contact* contact ) +{ + B3_ASSERT( contact->manifoldCount > 0 ); + B3_ASSERT( contact->flags & b3_contactTouchingFlag ); + + b3ConstraintGraph* graph = &world->constraintGraph; + int colorIndex = B3_OVERFLOW_INDEX; + + int bodyIdA = contact->edges[0].bodyId; + int bodyIdB = contact->edges[1].bodyId; + b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB ); + + b3BodyType typeA = bodyA->type; + b3BodyType typeB = bodyB->type; + B3_ASSERT( typeA == b3_dynamicBody || typeB == b3_dynamicBody ); + +#if B3_FORCE_OVERFLOW == 0 + if ( typeA == b3_dynamicBody && typeB == b3_dynamicBody ) + { + // Dynamic constraint colors cannot encroach on colors reserved for static constraints + for ( int i = 0; i < B3_DYNAMIC_COLOR_COUNT; ++i ) + { + b3GraphColor* color = graph->colors + i; + if ( b3GetBit( &color->bodySet, bodyIdA ) || b3GetBit( &color->bodySet, bodyIdB ) ) + { + continue; + } + + b3SetBitGrow( &color->bodySet, bodyIdA ); + b3SetBitGrow( &color->bodySet, bodyIdB ); + colorIndex = i; + break; + } + } + else if ( typeA == b3_dynamicBody ) + { + // Static constraint colors build from the end to get higher priority than dyn-dyn constraints + for ( int i = B3_OVERFLOW_INDEX - 1; i >= 1; --i ) + { + b3GraphColor* color = graph->colors + i; + if ( b3GetBit( &color->bodySet, bodyIdA ) ) + { + continue; + } + + b3SetBitGrow( &color->bodySet, bodyIdA ); + colorIndex = i; + break; + } + } + else if ( typeB == b3_dynamicBody ) + { + // Static constraint colors build from the end to get higher priority than dyn-dyn constraints + for ( int i = B3_OVERFLOW_INDEX - 1; i >= 1; --i ) + { + b3GraphColor* color = graph->colors + i; + if ( b3GetBit( &color->bodySet, bodyIdB ) ) + { + continue; + } + + b3SetBitGrow( &color->bodySet, bodyIdB ); + colorIndex = i; + break; + } + } +#endif + + bool isScalar = ( contact->flags & b3_simMeshContact ) || colorIndex == B3_OVERFLOW_INDEX; + + b3GraphColor* color = graph->colors + colorIndex; + contact->colorIndex = colorIndex; + contact->localIndex = isScalar ? color->contacts.count : color->convexContacts.count; + contact->bodySimIndexA = bodyA->type == b3_staticBody ? B3_NULL_INDEX : bodyA->localIndex; + contact->bodySimIndexB = bodyB->type == b3_staticBody ? B3_NULL_INDEX : bodyB->localIndex; + + if ( isScalar ) + { + B3_ASSERT( contact->manifoldCount < UINT16_MAX ); + b3ContactSpec spec = { + .contactId = contact->contactId, + .manifoldStart = 0, + .manifoldCount = (uint16_t)contact->manifoldCount, + }; + b3Array_Push( color->contacts, spec ); + } + else + { + b3Array_Push( color->convexContacts, contact->contactId ); + } +} + +void b3RemoveContactFromGraph( b3World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex, bool meshContact ) +{ + b3ConstraintGraph* graph = &world->constraintGraph; + + B3_ASSERT( 0 <= colorIndex && colorIndex < B3_GRAPH_COLOR_COUNT ); + b3GraphColor* color = graph->colors + colorIndex; + + if ( colorIndex != B3_OVERFLOW_INDEX ) + { + // This might clear a bit for a static body, but this has no effect + b3ClearBit( &color->bodySet, bodyIdA ); + b3ClearBit( &color->bodySet, bodyIdB ); + } + + if ( meshContact || colorIndex == B3_OVERFLOW_INDEX ) + { + int movedIndex = b3Array_RemoveSwap( color->contacts, localIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fix index on swapped contact + int movedContactId = color->contacts.data[localIndex].contactId; + b3Contact* movedContact = b3Array_Get( world->contacts, movedContactId ); + B3_ASSERT( movedContact->setIndex == b3_awakeSet ); + B3_ASSERT( movedContact->colorIndex == colorIndex ); + B3_ASSERT( movedContact->localIndex == movedIndex ); + movedContact->localIndex = localIndex; + } + } + else + { + int movedIndex = b3Array_RemoveSwap( color->convexContacts, localIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fix index on swapped contact + int movedContactId = color->convexContacts.data[localIndex]; + b3Contact* movedContact = b3Array_Get( world->contacts, movedContactId ); + B3_ASSERT( movedContact->setIndex == b3_awakeSet ); + B3_ASSERT( movedContact->colorIndex == colorIndex ); + B3_ASSERT( movedContact->localIndex == movedIndex ); + B3_ASSERT( ( movedContact->flags & b3_simMeshContact ) == 0 ); + movedContact->localIndex = localIndex; + } + } +} + +static int b3AssignJointColor( b3ConstraintGraph* graph, int bodyIdA, int bodyIdB, b3BodyType typeA, b3BodyType typeB ) +{ + B3_ASSERT( typeA == b3_dynamicBody || typeB == b3_dynamicBody ); + B3_UNUSED( graph ); + B3_UNUSED( bodyIdA ); + B3_UNUSED( bodyIdB ); + B3_UNUSED( typeA ); + B3_UNUSED( typeB ); + +#if B3_FORCE_OVERFLOW == 0 + if ( typeA == b3_dynamicBody && typeB == b3_dynamicBody ) + { + // Dynamic constraint colors cannot encroach on colors reserved for static constraints + for ( int i = 0; i < B3_DYNAMIC_COLOR_COUNT; ++i ) + { + b3GraphColor* color = graph->colors + i; + if ( b3GetBit( &color->bodySet, bodyIdA ) || b3GetBit( &color->bodySet, bodyIdB ) ) + { + continue; + } + + b3SetBitGrow( &color->bodySet, bodyIdA ); + b3SetBitGrow( &color->bodySet, bodyIdB ); + return i; + } + } + else if ( typeA == b3_dynamicBody ) + { + // Static constraint colors build from the end to get higher priority than dyn-dyn constraints + for ( int i = B3_OVERFLOW_INDEX - 1; i >= 1; --i ) + { + b3GraphColor* color = graph->colors + i; + if ( b3GetBit( &color->bodySet, bodyIdA ) ) + { + continue; + } + + b3SetBitGrow( &color->bodySet, bodyIdA ); + return i; + } + } + else if ( typeB == b3_dynamicBody ) + { + // Static constraint colors build from the end to get higher priority than dyn-dyn constraints + for ( int i = B3_OVERFLOW_INDEX - 1; i >= 1; --i ) + { + b3GraphColor* color = graph->colors + i; + if ( b3GetBit( &color->bodySet, bodyIdB ) ) + { + continue; + } + + b3SetBitGrow( &color->bodySet, bodyIdB ); + return i; + } + } +#endif + + return B3_OVERFLOW_INDEX; +} + +b3JointSim* b3CreateJointInGraph( b3World* world, b3Joint* joint ) +{ + b3ConstraintGraph* graph = &world->constraintGraph; + + int bodyIdA = joint->edges[0].bodyId; + int bodyIdB = joint->edges[1].bodyId; + b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB ); + + int colorIndex = b3AssignJointColor( graph, bodyIdA, bodyIdB, bodyA->type, bodyB->type ); + + b3JointSim* jointSim = b3Array_Emplace( graph->colors[colorIndex].jointSims ); + memset( jointSim, 0, sizeof( b3JointSim ) ); + + joint->colorIndex = colorIndex; + joint->localIndex = graph->colors[colorIndex].jointSims.count - 1; + return jointSim; +} + +void b3AddJointToGraph( b3World* world, b3JointSim* jointSim, b3Joint* joint ) +{ + b3JointSim* jointDst = b3CreateJointInGraph( world, joint ); + memcpy( jointDst, jointSim, sizeof( b3JointSim ) ); +} + +void b3RemoveJointFromGraph( b3World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex ) +{ + b3ConstraintGraph* graph = &world->constraintGraph; + + B3_ASSERT( 0 <= colorIndex && colorIndex < B3_GRAPH_COLOR_COUNT ); + b3GraphColor* color = graph->colors + colorIndex; + + if ( colorIndex != B3_OVERFLOW_INDEX ) + { + // May clear static bodies, no effect + b3ClearBit( &color->bodySet, bodyIdA ); + b3ClearBit( &color->bodySet, bodyIdB ); + } + + int movedIndex = b3Array_RemoveSwap( color->jointSims, localIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fix moved joint + b3JointSim* movedJointSim = color->jointSims.data + localIndex; + int movedId = movedJointSim->jointId; + b3Joint* movedJoint = b3Array_Get( world->joints, movedId ); + B3_ASSERT( movedJoint->setIndex == b3_awakeSet ); + B3_ASSERT( movedJoint->colorIndex == colorIndex ); + B3_ASSERT( movedJoint->localIndex == movedIndex ); + movedJoint->localIndex = localIndex; + } +} diff --git a/src/constraint_graph.h b/src/constraint_graph.h new file mode 100644 index 0000000..1776cc0 --- /dev/null +++ b/src/constraint_graph.h @@ -0,0 +1,86 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "bitset.h" +#include "contact.h" +#include "container.h" +#include "solver.h" +#include "solver_set.h" +#include "box3d/constants.h" + +typedef struct b3Body b3Body; +typedef struct b3Contact b3Contact; +typedef struct b3JointSim b3JointSim; +typedef struct b3Joint b3Joint; +typedef struct b3StepContext b3StepContext; +typedef struct b3World b3World; + +// This holds constraints that cannot fit the graph color limit. This happens when a single dynamic body +// is touching many other bodies. +#define B3_OVERFLOW_INDEX ( B3_GRAPH_COLOR_COUNT - 1 ) + +// This keeps constraints involving two dynamic bodies at a lower solver priority than constraints +// involving a dynamic and static bodies. This reduces tunneling due to push through. +#define B3_DYNAMIC_COLOR_COUNT ( B3_GRAPH_COLOR_COUNT - 4 ) + +// todo optimize mesh contact constraints +// They can be lumped with convex contacts and I can use the bitset event to re-link if the manifold count increases +// Could also create a group for two wide manifolds and use bitset event +// This could create ping-pong jitter so may need to pin to high water mark or introduce hysteresis somehow +// +// Dirk has the idea to do graph coloring based on manifolds. This suggests mesh contact will have manifolds +// in multiple graph colors. So each manifold with have a color and local index. +// Some concerns about this: +// - manifolds don't have a strong identity, would this affect stability/jitter? +// - this creates a lot of static graph colors and can overflow + +typedef struct b3GraphColor +{ + // This bitset is indexed by bodyId so this is over-sized to encompass static bodies + // however I never traverse these bits or use the bit count for anything + // This bitset is unused on the overflow color. + b3BitSet bodySet; + + // cache friendly arrays + b3ArrayC( b3JointSim ) jointSims; + + b3ArrayC( int ) convexContacts; + b3ArrayC( b3ContactSpec ) contacts; + + // These are used for convex contacts + struct b3ContactConstraintWide* wideConstraints; + int wideConstraintCount; + + // These are used for mesh and overflow contacts + struct b3ManifoldConstraint* manifoldConstraints; + int manifoldConstraintCount; + struct b3ContactConstraint* contactConstraints; + int contactConstraintCount; +} b3GraphColor; + +typedef struct b3ConstraintGraph +{ + // including overflow at the end + b3GraphColor colors[B3_GRAPH_COLOR_COUNT]; +} b3ConstraintGraph; + +#ifdef __cplusplus +extern "C" +{ +#endif + +void b3CreateGraph( b3ConstraintGraph* graph, int bodyCapacity ); +void b3DestroyGraph( b3ConstraintGraph* graph ); + +void b3AddContactToGraph( b3World* world, b3Contact* contact ); +void b3RemoveContactFromGraph( b3World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex, bool meshContact ); + +b3JointSim* b3CreateJointInGraph( b3World* world, b3Joint* joint ); +void b3AddJointToGraph( b3World* world, b3JointSim* jointSim, b3Joint* joint ); +void b3RemoveJointFromGraph( b3World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex ); + +#ifdef __cplusplus +} +#endif diff --git a/src/contact.c b/src/contact.c new file mode 100644 index 0000000..ac3929f --- /dev/null +++ b/src/contact.c @@ -0,0 +1,862 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "contact.h" + +#include "algorithm.h" +#include "body.h" +#include "compound.h" +#include "island.h" +#include "manifold.h" +#include "physics_world.h" +#include "shape.h" +#include "solver_set.h" +#include "table.h" + +#include "box3d/box3d.h" + +// Contacts and determinism +// A deterministic simulation requires contacts to exist in the same order in b3Island no matter the thread count. +// The order must reproduce from run to run. This is necessary because the Gauss-Seidel constraint solver is order dependent. +// +// Creation: +// - Contacts are created using results from b3UpdateBroadPhasePairs +// - These results are ordered according to the order of the broad-phase move array +// - The move array is ordered according to the shape creation order using a bitset. +// - The island/shape/body order is determined by creation order +// - Logically contacts are only created for awake bodies, so they are immediately added to the awake contact array (serially) +// +// Island linking: +// - The awake contact array is built from the body-contact graph for all awake bodies in awake islands. +// - Awake contacts are solved in parallel and they generate contact state changes. +// - These state changes may link islands together using union find. +// - The state changes are ordered using a bit array that encompasses all contacts +// - As long as contacts are created in deterministic order, island link order is deterministic. +// - This keeps the order of contacts in islands deterministic + +// Manifold functions should compute important results in local space to improve precision. However, this +// interface function takes two world transforms instead of a relative transform for these reasons: +// +// First: +// The anchors need to be computed relative to the shape origin in world space. This is necessary so the +// solver does not need to access static body transforms. Not even in constraint preparation. This approach +// has world space vectors yet retains precision. +// +// Second: +// b3ManifoldPoint::point is very useful for debugging and it is in world space. +// +// Third: +// The user may call the manifold functions directly and they should be easy to use and have easy to use +// results. +// typedef b3Manifold b3ManifoldFcn( const b3Shape* shapeA, b3Transform xfA, const b3Shape* shapeB, b3Transform xfB, +// b3ContactCache* cache ); + +static b3Contact* b3GetContactFullId( b3World* world, b3ContactId contactId ) +{ + int id = contactId.index1 - 1; + b3Contact* contact = b3Array_Get( world->contacts, id ); + B3_ASSERT( contact->contactId == id && contact->generation == contactId.generation ); + return contact; +} + +b3ContactData b3Contact_GetData( b3ContactId contactId ) +{ + b3World* world = b3GetWorld( contactId.world0 ); + b3Contact* contact = b3GetContactFullId( world, contactId ); + + const b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); + const b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB ); + + b3ContactData data = { 0 }; + data.contactId = contactId; + data.shapeIdA = (b3ShapeId){ + .index1 = shapeA->id + 1, + .world0 = contactId.world0, + .generation = shapeA->generation, + }; + data.shapeIdB = (b3ShapeId){ + .index1 = shapeB->id + 1, + .world0 = contactId.world0, + .generation = shapeB->generation, + }; + + if ( contact->manifoldCount > 0 ) + { + data.manifolds = contact->manifolds; + data.manifoldCount = contact->manifoldCount; + } + else + { + data.manifolds = NULL; + data.manifoldCount = 0; + } + + return data; +} + +struct b3ContactRegister +{ + // b3ManifoldFcn* fcn; + bool supported; + bool primary; +}; + +static struct b3ContactRegister s_registers[b3_shapeTypeCount][b3_shapeTypeCount]; +static bool s_initialized = false; + +static void b3AddType( b3ShapeType type1, b3ShapeType type2 ) +{ + B3_ASSERT( 0 <= type1 && type1 < b3_shapeTypeCount ); + B3_ASSERT( 0 <= type2 && type2 < b3_shapeTypeCount ); + + s_registers[type1][type2].supported = true; + s_registers[type1][type2].primary = true; + + if ( type1 != type2 ) + { + s_registers[type2][type1].supported = true; + s_registers[type2][type1].primary = false; + } +} + +void b3InitializeContactRegisters( void ) +{ + if ( s_initialized == false ) + { + b3AddType( b3_sphereShape, b3_sphereShape ); + b3AddType( b3_capsuleShape, b3_sphereShape ); + b3AddType( b3_capsuleShape, b3_capsuleShape ); + b3AddType( b3_compoundShape, b3_sphereShape ); + b3AddType( b3_compoundShape, b3_capsuleShape ); + b3AddType( b3_compoundShape, b3_hullShape ); + b3AddType( b3_hullShape, b3_sphereShape ); + b3AddType( b3_hullShape, b3_capsuleShape ); + b3AddType( b3_hullShape, b3_hullShape ); + b3AddType( b3_meshShape, b3_sphereShape ); + b3AddType( b3_meshShape, b3_capsuleShape ); + b3AddType( b3_meshShape, b3_hullShape ); + b3AddType( b3_heightShape, b3_sphereShape ); + b3AddType( b3_heightShape, b3_capsuleShape ); + b3AddType( b3_heightShape, b3_hullShape ); + s_initialized = true; + } +} + +void b3CreateContact( b3World* world, b3Shape* shapeA, b3Shape* shapeB, int childIndex ) +{ + b3ShapeType typeA = shapeA->type; + b3ShapeType typeB = shapeB->type; + + B3_ASSERT( 0 <= typeA && typeA < b3_shapeTypeCount ); + B3_ASSERT( 0 <= typeB && typeB < b3_shapeTypeCount ); + + if ( s_registers[typeA][typeB].supported == false ) + { + // For example, no mesh vs mesh collision + return; + } + + if ( s_registers[typeA][typeB].primary == false ) + { + // flip order + b3CreateContact( world, shapeB, shapeA, childIndex ); + return; + } + + b3Body* bodyA = b3Array_Get( world->bodies, shapeA->bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, shapeB->bodyId ); + + B3_ASSERT( bodyA->setIndex != b3_disabledSet && bodyB->setIndex != b3_disabledSet ); + B3_ASSERT( bodyA->setIndex != b3_staticSet || bodyB->setIndex != b3_staticSet ); + + int setIndex; + if ( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ) + { + setIndex = b3_awakeSet; + } + else + { + // sleeping and non-touching contacts live in the disabled set + // later if this set is found to be touching then the sleeping + // islands will be linked and the contact moved to the merged island + + // This is possible if a shape moves slightly then falls asleep + setIndex = b3_disabledSet; + } + + b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + + // Create contact key and contact + int contactId = b3AllocId( &world->contactIdPool ); + if ( contactId == world->contacts.count ) + { + b3Contact emptyContact = { 0 }; + b3Array_Push( world->contacts, emptyContact ); + } + + int shapeIdA = shapeA->id; + int shapeIdB = shapeB->id; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + int generation = contact->generation; + *contact = (b3Contact){ 0 }; + contact->contactId = contactId; + contact->generation = generation + 1; + contact->setIndex = setIndex; + contact->colorIndex = B3_NULL_INDEX; + contact->localIndex = set->contactIndices.count; + contact->islandId = B3_NULL_INDEX; + contact->islandIndex = B3_NULL_INDEX; + contact->shapeIdA = shapeIdA; + contact->shapeIdB = shapeIdB; + contact->childIndex = childIndex; + + if ( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape ) + { + contact->flags |= b3_simMeshContact; + } + else if ( shapeA->type == b3_compoundShape ) + { + b3ChildShape child = b3GetCompoundChild( shapeA->compound, childIndex ); + if ( child.type == b3_meshShape ) + { + contact->flags |= b3_simMeshContact; + } + } + + // todo impose these restrictions to make life easier + B3_ASSERT( shapeB->type == b3_sphereShape || shapeB->type == b3_capsuleShape || shapeB->type == b3_hullShape ); + // B3_ASSERT( bodyB->type != b3_staticBody ); + + // Is either body static? + // Note: it is possible to have a dynamic mesh collide with a static convex shape. Maybe I should disallow this. + if ( bodyA->type == b3_staticBody || bodyB->type == b3_staticBody ) + { + contact->flags |= b3_contactStaticFlag; + } + + B3_ASSERT( shapeA->sensorIndex == B3_NULL_INDEX && shapeB->sensorIndex == B3_NULL_INDEX ); + + if ( shapeA->enableContactEvents || shapeB->enableContactEvents ) + { + contact->flags |= b3_contactEnableContactEvents; + } + + // Connect to body A + { + contact->edges[0].bodyId = shapeA->bodyId; + contact->edges[0].prevKey = B3_NULL_INDEX; + contact->edges[0].nextKey = bodyA->headContactKey; + + int keyA = ( contactId << 1 ) | 0; + int headContactKey = bodyA->headContactKey; + if ( headContactKey != B3_NULL_INDEX ) + { + b3Contact* headContact = b3Array_Get( world->contacts, headContactKey >> 1 ); + headContact->edges[headContactKey & 1].prevKey = keyA; + } + bodyA->headContactKey = keyA; + bodyA->contactCount += 1; + } + + // Connect to body B + { + contact->edges[1].bodyId = shapeB->bodyId; + contact->edges[1].prevKey = B3_NULL_INDEX; + contact->edges[1].nextKey = bodyB->headContactKey; + + int keyB = ( contactId << 1 ) | 1; + int headContactKey = bodyB->headContactKey; + if ( bodyB->headContactKey != B3_NULL_INDEX ) + { + b3Contact* headContact = b3Array_Get( world->contacts, headContactKey >> 1 ); + headContact->edges[headContactKey & 1].prevKey = keyB; + } + bodyB->headContactKey = keyB; + bodyB->contactCount += 1; + } + + // Add to pair set for fast lookup + uint64_t pairKey = b3ShapePairKey( shapeIdA, shapeIdB, childIndex ); + b3AddKey( &world->broadPhase.pairSet, pairKey ); + + // Contacts are created as non-touching. Later if they are found to be touching + // they will link islands and be moved into the constraint graph. + b3Array_Push( set->contactIndices, contactId ); + + float radiusA = 0.0f; + if ( typeA == b3_sphereShape ) + { + radiusA = shapeA->sphere.radius; + } + else if ( typeA == b3_capsuleShape ) + { + radiusA = shapeA->capsule.radius; + } + + float radiusB = 0.0f; + if ( typeB == b3_sphereShape ) + { + radiusB = shapeB->sphere.radius; + } + else if ( typeB == b3_capsuleShape ) + { + radiusB = shapeB->capsule.radius; + } + + float maxRadius = b3MaxFloat( radiusA, radiusB ); + + // Assuming the rolling resistance doesn't change + contact->rollingResistance = + b3MaxFloat( shapeA->materials[0].rollingResistance, shapeB->materials[0].rollingResistance ) * maxRadius; + + if ( shapeA->enablePreSolveEvents || shapeB->enablePreSolveEvents ) + { + contact->flags |= b3_simEnablePreSolveEvents; + } +} + +// A contact is destroyed when: +// - broad-phase proxies stop overlapping +// - a body is destroyed +// - a body is disabled +// - a body changes type from dynamic to kinematic or static +// - a shape is destroyed +// - contact filtering is modified +void b3DestroyContact( b3World* world, b3Contact* contact, bool wakeBodies ) +{ + // Remove pair from set + uint64_t pairKey = b3ShapePairKey( contact->shapeIdA, contact->shapeIdB, contact->childIndex ); + b3RemoveKey( &world->broadPhase.pairSet, pairKey ); + + b3FreeManifolds( world, contact->manifolds, contact->manifoldCount ); + contact->manifolds = NULL; + contact->manifoldCount = 0; + + b3ContactEdge* edgeA = contact->edges + 0; + b3ContactEdge* edgeB = contact->edges + 1; + + int bodyIdA = edgeA->bodyId; + int bodyIdB = edgeB->bodyId; + b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB ); + + uint32_t flags = contact->flags; + bool touching = ( flags & b3_contactTouchingFlag ) != 0; + + // End touch event + if ( touching && ( flags & b3_contactEnableContactEvents ) != 0 ) + { + uint16_t worldId = world->worldId; + const b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); + const b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB ); + b3ShapeId shapeIdA = { shapeA->id + 1, worldId, shapeA->generation }; + b3ShapeId shapeIdB = { shapeB->id + 1, worldId, shapeB->generation }; + + b3ContactId contactId = { + .index1 = contact->contactId + 1, + .world0 = world->worldId, + .padding = 0, + .generation = contact->generation, + }; + + b3ContactEndTouchEvent event = { + .shapeIdA = shapeIdA, + .shapeIdB = shapeIdB, + .contactId = contactId, + }; + + b3Array_Push( world->contactEndEvents[world->endEventArrayIndex], event ); + } + + // Remove from body A + if ( edgeA->prevKey != B3_NULL_INDEX ) + { + b3Contact* prevContact = b3Array_Get( world->contacts, edgeA->prevKey >> 1 ); + b3ContactEdge* prevEdge = prevContact->edges + ( edgeA->prevKey & 1 ); + prevEdge->nextKey = edgeA->nextKey; + } + + if ( edgeA->nextKey != B3_NULL_INDEX ) + { + b3Contact* nextContact = b3Array_Get( world->contacts, edgeA->nextKey >> 1 ); + b3ContactEdge* nextEdge = nextContact->edges + ( edgeA->nextKey & 1 ); + nextEdge->prevKey = edgeA->prevKey; + } + + int contactId = contact->contactId; + + int edgeKeyA = ( contactId << 1 ) | 0; + if ( bodyA->headContactKey == edgeKeyA ) + { + bodyA->headContactKey = edgeA->nextKey; + } + + bodyA->contactCount -= 1; + + // Remove from body B + if ( edgeB->prevKey != B3_NULL_INDEX ) + { + b3Contact* prevContact = b3Array_Get( world->contacts, edgeB->prevKey >> 1 ); + b3ContactEdge* prevEdge = prevContact->edges + ( edgeB->prevKey & 1 ); + prevEdge->nextKey = edgeB->nextKey; + } + + if ( edgeB->nextKey != B3_NULL_INDEX ) + { + b3Contact* nextContact = b3Array_Get( world->contacts, edgeB->nextKey >> 1 ); + b3ContactEdge* nextEdge = nextContact->edges + ( edgeB->nextKey & 1 ); + nextEdge->prevKey = edgeB->prevKey; + } + + int edgeKeyB = ( contactId << 1 ) | 1; + if ( bodyB->headContactKey == edgeKeyB ) + { + bodyB->headContactKey = edgeB->nextKey; + } + + bodyB->contactCount -= 1; + + if ( contact->flags & b3_simMeshContact ) + { + b3Array_Destroy( contact->meshContact.triangleCache ); + } + + // Remove contact from the array that owns it + if ( contact->islandId != B3_NULL_INDEX ) + { + b3UnlinkContact( world, contact ); + } + + if ( contact->colorIndex != B3_NULL_INDEX ) + { + // contact is an active constraint + B3_ASSERT( contact->setIndex == b3_awakeSet ); + bool meshContact = contact->flags & b3_simMeshContact; + b3RemoveContactFromGraph( world, bodyIdA, bodyIdB, contact->colorIndex, contact->localIndex, meshContact ); + } + else + { + // contact is non-touching or is sleeping or is a sensor + B3_ASSERT( contact->setIndex != b3_awakeSet || ( contact->flags & b3_contactTouchingFlag ) == 0 ); + b3SolverSet* set = b3Array_Get( world->solverSets, contact->setIndex ); + + int localIndex = contact->localIndex; + int movedIndex = b3Array_RemoveSwap( set->contactIndices, localIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + int movedContactIndex = set->contactIndices.data[localIndex]; + b3Contact* movedContact = b3Array_Get( world->contacts, movedContactIndex ); + movedContact->localIndex = localIndex; + } + } + + // Free contact and id (preserve generation) + contact->contactId = B3_NULL_INDEX; + contact->setIndex = B3_NULL_INDEX; + contact->colorIndex = B3_NULL_INDEX; + contact->localIndex = B3_NULL_INDEX; + b3FreeId( &world->contactIdPool, contactId ); + + if ( wakeBodies && touching ) + { + b3WakeBody( world, bodyA ); + b3WakeBody( world, bodyB ); + } +} + +static bool b3ComputeConvexManifold( b3World* world, int workerIndex, b3Contact* contact, const b3Shape* shapeA, b3Transform xfA, + const b3Shape* shapeB, b3Transform xfB, b3Arena arena ) +{ + b3ShapeType typeA = shapeA->type; + b3ShapeType typeB = shapeB->type; + + b3ContactCache* cache = &contact->convexContact.cache; + + int pointCapacity = 32; + b3LocalManifoldPoint* pointBuffer = (b3LocalManifoldPoint*)b3Bump( &arena, pointCapacity * sizeof( b3LocalManifoldPoint ) ); + + b3LocalManifold geomManifold = { 0 }; + geomManifold.points = pointBuffer; + + b3Transform transformBtoA = b3InvMulTransforms( xfA, xfB ); + + if ( typeA == b3_sphereShape ) + { + B3_ASSERT( typeB == b3_sphereShape ); + b3CollideSpheres( &geomManifold, pointCapacity, &shapeA->sphere, &shapeB->sphere, transformBtoA ); + } + else if ( typeA == b3_capsuleShape ) + { + if ( typeB == b3_sphereShape ) + { + b3CollideCapsuleAndSphere( &geomManifold, pointCapacity, &shapeA->capsule, &shapeB->sphere, transformBtoA ); + } + else + { + B3_ASSERT( typeB == b3_capsuleShape ); + b3CollideCapsules( &geomManifold, pointCapacity, &shapeA->capsule, &shapeB->capsule, transformBtoA ); + } + } + else + { + B3_ASSERT( typeA == b3_hullShape ); + + if ( typeB == b3_sphereShape ) + { + b3CollideHullAndSphere( &geomManifold, pointCapacity, shapeA->hull, &shapeB->sphere, transformBtoA, + &cache->simplexCache ); + } + else if ( typeB == b3_capsuleShape ) + { + b3CollideHullAndCapsule( &geomManifold, pointCapacity, shapeA->hull, &shapeB->capsule, transformBtoA, + &cache->simplexCache ); + } + else + { + B3_ASSERT( typeB == b3_hullShape ); + b3CollideHulls( &geomManifold, pointCapacity, shapeA->hull, shapeB->hull, transformBtoA, &cache->satCache ); + world->taskContexts.data[workerIndex].satCallCount += 1; + world->taskContexts.data[workerIndex].satCacheHitCount += cache->satCache.hit; + } + } + + if ( geomManifold.pointCount == 0 ) + { + if ( contact->manifoldCount > 0 ) + { + b3FreeManifolds( world, contact->manifolds, contact->manifoldCount ); + contact->manifolds = NULL; + contact->manifoldCount = 0; + } + + return false; + } + + b3ManifoldPoint oldPoints[B3_MAX_MANIFOLD_POINTS]; + int oldCount = 0; + + if ( contact->manifoldCount == 0 ) + { + contact->manifolds = b3AllocateManifolds( world, 1 ); + contact->manifoldCount = 1; + } + else + { + oldCount = contact->manifolds[0].pointCount; + memcpy( oldPoints, contact->manifolds[0].points, oldCount * sizeof( b3ManifoldPoint ) ); + } + + b3Manifold* manifold = contact->manifolds; + manifold->pointCount = geomManifold.pointCount; + + b3Matrix3 matrixA = b3MakeMatrixFromQuat( xfA.q ); + manifold->normal = b3MulMV( matrixA, geomManifold.normal ); + + // Store point data in contact + for ( int i = 0; i < geomManifold.pointCount; ++i ) + { + const b3LocalManifoldPoint* source = geomManifold.points + i; + b3ManifoldPoint* target = manifold->points + i; + + // Contact points are computed in frame A + target->anchorA = b3MulMV( matrixA, source->point ); + target->anchorB = b3Add( target->anchorA, b3Sub( xfA.p, xfB.p ) ); + target->separation = source->separation; + target->featureId = b3MakeFeatureId( source->pair ); + target->triangleIndex = B3_NULL_INDEX; + target->normalVelocity = 0.0f; + } + + // Copy impulses from old points + for ( int i = 0; i < geomManifold.pointCount; ++i ) + { + b3ManifoldPoint* pt2 = manifold->points + i; + pt2->totalNormalImpulse = 0.0f; + pt2->persisted = false; + + for ( int j = 0; j < oldCount; ++j ) + { + b3ManifoldPoint* pt1 = oldPoints + j; + + if ( pt2->featureId == pt1->featureId ) + { + pt2->normalImpulse = pt1->normalImpulse; + pt2->persisted = true; + + // claimed + pt1->featureId = UINT32_MAX; + + break; + } + } + + if ( pt2->persisted == false ) + { + pt2->normalImpulse = 0.0f; + } + } + + return true; +} + +static bool b3UpdateConvexContact( b3World* world, int workerIndex, b3Contact* contact, b3Shape* shapeA, b3Transform xfA, + b3Shape* shapeB, b3Transform xfB, bool flip, b3Arena arena ) +{ + // Compute new manifold + bool touching = b3ComputeConvexManifold( world, workerIndex, contact, shapeA, xfA, shapeB, xfB, arena ); + + if ( touching == false ) + { + B3_ASSERT( contact->manifolds == NULL && contact->manifoldCount == 0 ); + return false; + } + + B3_ASSERT( contact->manifoldCount == 1 ); + + if ( flip ) + { + // Not flipping the feature ids because they just need to match and flipping is consistent. + b3Manifold* manifold = contact->manifolds + 0; + manifold->normal = b3Neg( manifold->normal ); + int pointCount = manifold->pointCount; + for ( int i = 0; i < pointCount; ++i ) + { + b3ManifoldPoint* mp = manifold->points + i; + B3_SWAP( mp->anchorA, mp->anchorB ); + } + } + + const b3SurfaceMaterial* materialA = shapeA->materials + 0; + const b3SurfaceMaterial* materialB = shapeB->materials + 0; + + // Keep these updated in case the values on the shapes are modified + contact->friction = + world->frictionCallback( materialA->friction, materialA->userMaterialId, materialB->friction, materialB->userMaterialId ); + contact->restitution = world->restitutionCallback( materialA->restitution, materialA->userMaterialId, materialB->restitution, + materialB->userMaterialId ); + + if ( materialA->rollingResistance > 0.0f || materialB->rollingResistance > 0.0f ) + { + b3ShapeType typeA = shapeA->type; + b3ShapeType typeB = shapeB->type; + + float radiusA = 0.0f; + if ( typeA == b3_sphereShape ) + { + radiusA = shapeA->sphere.radius; + } + else if ( typeA == b3_capsuleShape ) + { + radiusA = shapeA->capsule.radius; + } + else if ( typeA == b3_hullShape ) + { + radiusA = 0.25f * shapeA->hull->innerRadius; + } + + float radiusB = 0.0f; + if ( typeB == b3_sphereShape ) + { + radiusB = shapeB->sphere.radius; + } + else if ( typeB == b3_capsuleShape ) + { + radiusB = shapeB->capsule.radius; + } + else if ( typeB == b3_hullShape ) + { + radiusB = 0.25f * shapeB->hull->innerRadius; + } + + float maxRadius = b3MaxFloat( radiusA, radiusB ); + contact->rollingResistance = b3MaxFloat( materialA->rollingResistance, materialB->rollingResistance ) * maxRadius; + } + else + { + contact->rollingResistance = 0.0f; + } + + b3Vec3 tangentVelocityA = b3RotateVector( xfA.q, materialA->tangentVelocity ); + b3Vec3 tangentVelocityB = b3RotateVector( xfB.q, materialB->tangentVelocity ); + contact->tangentVelocity = b3Sub( tangentVelocityA, tangentVelocityB ); + + if ( world->preSolveFcn && ( contact->flags & b3_simEnablePreSolveEvents ) != 0 ) + { + b3ShapeId shapeIdA = { shapeA->id + 1, world->worldId, shapeA->generation }; + b3ShapeId shapeIdB = { shapeB->id + 1, world->worldId, shapeB->generation }; + + // this call assumes thread safety + b3Vec3 point = b3Add( contact->manifolds[0].points[0].anchorA, xfA.p ); + b3Vec3 normal = contact->manifolds[0].normal; + touching = world->preSolveFcn( shapeIdA, shapeIdB, point, normal, world->preSolveContext ); + if ( touching == false ) + { + // disable contact + b3FreeManifolds( world, contact->manifolds, contact->manifoldCount ); + contact->manifolds = NULL; + contact->manifoldCount = 0; + return false; + } + } + + if ( shapeA->enableHitEvents || shapeB->enableHitEvents ) + { + contact->flags |= b3_simEnableHitEvent; + } + else + { + contact->flags &= ~b3_simEnableHitEvent; + } + + return true; +} + +// Update the contact manifold and touching status. +// Note: do not assume the shape AABBs are overlapping or are valid. +bool b3UpdateContact( b3World* world, int workerIndex, b3Contact* contact, b3Shape* shapeA, b3Vec3 localCenterA, b3Transform xfA, + b3Shape* shapeB, b3Vec3 localCenterB, b3Transform xfB, bool isFast, b3Arena arena ) +{ + bool touching; + + B3_ASSERT( shapeB->type != b3_compoundShape ); + + if ( shapeA->type == b3_compoundShape ) + { + int childIndex = contact->childIndex; + b3ChildShape child = b3GetCompoundChild( shapeA->compound, childIndex ); + + // Temporary child shape to match existing function signatures + b3Shape childShapeA; + memcpy( &childShapeA, shapeA, sizeof( b3Shape ) ); + + childShapeA.type = child.type; + + if ( child.type == b3_capsuleShape ) + { + childShapeA.capsule = child.capsule; + if ( shapeB->type == b3_hullShape ) + { + // Flip + bool flip = true; + touching = b3UpdateConvexContact( world, workerIndex, contact, shapeB, xfB, &childShapeA, xfA, flip, arena ); + } + else + { + bool flip = false; + touching = b3UpdateConvexContact( world, workerIndex, contact, &childShapeA, xfA, shapeB, xfB, flip, arena ); + } + } + else if ( child.type == b3_hullShape ) + { + childShapeA.hull = child.hull; + b3Transform xfChild = b3MulTransforms( xfA, child.transform ); + bool flip = false; + touching = b3UpdateConvexContact( world, workerIndex, contact, &childShapeA, xfChild, shapeB, xfB, flip, arena ); + } + else if ( child.type == b3_meshShape ) + { + childShapeA.mesh = child.mesh; + b3Transform xfChild = b3MulTransforms( xfA, child.transform ); + + touching = b3ComputeMeshManifolds( world, workerIndex, contact, &childShapeA, child.materialIndices, xfChild, shapeB, + xfB, isFast, arena ); + + if ( touching && ( shapeA->enableHitEvents || shapeB->enableHitEvents ) ) + { + contact->flags |= b3_simEnableHitEvent; + } + else + { + contact->flags &= ~b3_simEnableHitEvent; + } + + B3_ASSERT( ( touching == true && contact->manifoldCount > 0 ) || + ( touching == false && contact->manifoldCount == 0 ) ); + } + else + { + B3_ASSERT( child.type == b3_sphereShape ); + + childShapeA.sphere = child.sphere; + if ( shapeB->type == b3_capsuleShape || shapeB->type == b3_hullShape ) + { + // Flip + bool flip = true; + touching = b3UpdateConvexContact( world, workerIndex, contact, shapeB, xfB, &childShapeA, xfA, flip, arena ); + } + else + { + bool flip = false; + touching = b3UpdateConvexContact( world, workerIndex, contact, &childShapeA, xfA, shapeB, xfB, flip, arena ); + } + } + + // The anchor is relative to the child origin but oriented in world space. + // Offset the anchor to be relative to the compound origin. + int manifoldCount = contact->manifoldCount; + b3Vec3 offset = b3RotateVector( xfA.q, child.transform.p ); + for ( int i = 0; i < manifoldCount; ++i ) + { + b3Manifold* manifold = contact->manifolds + i; + int pointCount = manifold->pointCount; + for ( int j = 0; j < pointCount; ++j ) + { + b3ManifoldPoint* mp = manifold->points + j; + mp->anchorA = b3Add( mp->anchorA, offset ); + } + } + } + else if ( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape ) + { + // Does this contact touch a mesh or height-field? + + // Compute mesh manifolds + touching = b3ComputeMeshManifolds( world, workerIndex, contact, shapeA, NULL, xfA, shapeB, xfB, isFast, arena ); + + if ( touching && ( shapeA->enableHitEvents || shapeB->enableHitEvents ) ) + { + contact->flags |= b3_simEnableHitEvent; + } + else + { + contact->flags &= ~b3_simEnableHitEvent; + } + + // B3_ASSERT( ( touching == true && contact->manifoldCount > 0 ) || ( touching == false && contact->manifoldCount == 0 ) + // ); + B3_ASSERT( ( touching == true && contact->manifoldCount > 0 ) || ( touching == false && contact->manifoldCount == 0 ) ); + } + else + { + // Convex-vs-convex + bool flip = false; + touching = b3UpdateConvexContact( world, workerIndex, contact, shapeA, xfA, shapeB, xfB, flip, arena ); + } + + if ( touching ) + { + b3Vec3 centerA = b3RotateVector( xfA.q, localCenterA ); + b3Vec3 centerB = b3RotateVector( xfB.q, localCenterB ); + + // Adjust anchors to be relative to center of mass + for ( int i = 0; i < contact->manifoldCount; ++i ) + { + b3Manifold* manifold = contact->manifolds + i; + for ( int j = 0; j < manifold->pointCount; ++j ) + { + b3ManifoldPoint* mp = manifold->points + j; + mp->anchorA = b3Sub( mp->anchorA, centerA ); + mp->anchorB = b3Sub( mp->anchorB, centerB ); + } + } + + contact->flags |= b3_simTouchingFlag; + } + else + { + contact->flags &= ~b3_simTouchingFlag; + } + + return touching; +} diff --git a/src/contact.h b/src/contact.h new file mode 100644 index 0000000..1b8cbed --- /dev/null +++ b/src/contact.h @@ -0,0 +1,185 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "arena_allocator.h" +#include "container.h" + +#include "box3d/collision.h" +#include "box3d/types.h" + +#define B3_FORCE_GHOST_COLLISIONS 0 + +typedef struct b3Shape b3Shape; +typedef struct b3World b3World; + +typedef union b3ContactCache +{ + b3SATCache satCache; + b3SimplexCache simplexCache; +} b3ContactCache; + +typedef struct b3TriangleCache +{ + int triangleIndex; + b3ContactCache cache; +} b3TriangleCache; + +b3DeclareArray( b3TriangleCache ); + +enum b3ContactFlags +{ + // Set when the solid shapes are touching. + b3_contactTouchingFlag = 0x00000001, + + // Contact has a hit event + b3_contactHitEventFlag = 0x00000002, + + // This contact wants contact events + b3_contactEnableContactEvents = 0x00000004, + + // This is contact is between a dynamic and static body + b3_contactStaticFlag = 0x00000008, + + // Set when the shapes are touching + b3_simTouchingFlag = 0x00010000, + + // This contact no longer has overlapping AABBs + b3_simDisjoint = 0x00020000, + + // This contact started touching + b3_simStartedTouching = 0x00040000, + + // This contact stopped touching + b3_simStoppedTouching = 0x00080000, + + // This contact has a hit event + b3_simEnableHitEvent = 0x00100000, + + // This contact wants pre-solve events + b3_simEnablePreSolveEvents = 0x00200000, + + // This is a mesh contact + b3_simMeshContact = 0x00400000, + + b3_relativeTransformValid = 0x00800000, +}; + +// A contact edge is used to connect bodies and contacts together +// in a contact graph where each body is a node and each contact +// is an edge. A contact edge belongs to a doubly linked list +// maintained in each attached body. Each contact has two contact +// edges, one for each attached body. +typedef struct b3ContactEdge +{ + int bodyId; + int prevKey; + int nextKey; +} b3ContactEdge; + +typedef struct b3MeshContact +{ + // todo use block allocator for this for faster shutdown + b3ArrayC( b3TriangleCache ) triangleCache; + b3AABB queryBounds; +} b3MeshContact; + +typedef struct b3ConvexContact +{ + b3ContactCache cache; +} b3ConvexContact; + +// Represents the persistent interaction between two shapes +typedef struct b3Contact +{ + // index of simulation set stored in b3World + // B3_NULL_INDEX when slot is free + int setIndex; + + // index into the constraint graph color array + // B3_NULL_INDEX for non-touching or sleeping contacts + // B3_NULL_INDEX when slot is free + int colorIndex; + + // contact index within set or graph color + // B3_NULL_INDEX when slot is free + int localIndex; + + b3ContactEdge edges[2]; + int shapeIdA; + int shapeIdB; + int childIndex; + + // A contact only belongs to an island if touching, otherwise B3_NULL_INDEX. + int islandId; + + // Index into the island's contacts array for O(1) swap-removal. + // B3_NULL_INDEX when not in an island. + int islandIndex; + + // Back index into b3World::contacts + int contactId; + + // These are transient and cached for improved performance. B3_NULL_INDEX for static bodies. + int bodySimIndexA; + int bodySimIndexB; + + // b3ContactFlags + uint32_t flags; + + b3Manifold* manifolds; + int manifoldCount; + + b3Transform cachedTransformA; + b3Transform cachedTransformB; + + // Mixed friction and restitution + float friction; + + // Usage determined by b3_simMeshContact in simFlags + union + { + b3ConvexContact convexContact; + b3MeshContact meshContact; + }; + + float restitution; + float rollingResistance; + b3Vec3 tangentVelocity; + + // This is monotonically advanced when a contact is allocated in this slot + // Used to check for invalid b3ContactId + uint32_t generation; +} b3Contact; + +typedef struct b3ContactSpec +{ + int contactId; + + // Start of the global manifold constraint array + int manifoldStart; + uint16_t manifoldCount; +} b3ContactSpec; + +b3DeclareArray( b3ContactSpec ); + +#ifdef __cplusplus +extern "C" +{ +#endif + +void b3InitializeContactRegisters( void ); + +void b3CreateContact( b3World* world, b3Shape* shapeA, b3Shape* shapeB, int childIndex ); +void b3DestroyContact( b3World* world, b3Contact* contact, bool wakeBodies ); + +bool b3UpdateContact( b3World* world, int workerIndex, b3Contact* contact, b3Shape* shapeA, b3Vec3 localCenterA, b3Transform xfA, + b3Shape* shapeB, b3Vec3 localCenterB, b3Transform xfB, bool isFast, b3Arena arena ); + +bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact, const b3Shape* shapeA, const int* materialMap, + b3Transform xfA, const b3Shape* shapeB, b3Transform xfB, bool isFast, b3Arena arena ); + +#ifdef __cplusplus +} +#endif diff --git a/src/contact_solver.c b/src/contact_solver.c new file mode 100644 index 0000000..218dbb8 --- /dev/null +++ b/src/contact_solver.c @@ -0,0 +1,2428 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "contact_solver.h" + +#include "body.h" +#include "constraint_graph.h" +#include "contact.h" +#include "core.h" +#include "math_internal.h" +#include "physics_world.h" +#include "platform.h" +#include "solver_set.h" + +#if B3_ENABLE_VALIDATION +#include "shape.h" +#endif + +#define FIXED_ANCHORS 1 + +// contact separation for sub-stepping +// s = s0 + dot(cB + rB - cA - rA, normal) +// normal is held constant +// body positions c can translation and anchors r can rotate +// s(t) = s0 + dot(cB(t) + rB(t) - cA(t) - rA(t), normal) +// s(t) = s0 + dot(cB0 + dpB + rot(dqB, rB0) - cA0 - dpA - rot(dqA, rA0), normal) +// s(t) = s0 + dot(cB0 - cA0, normal) + dot(dpB - dpA + rot(dqB, rB0) - rot(dqA, rA0), normal) +// s_base = s0 + dot(cB0 - cA0, normal) + +// Prepare a mesh constraints +void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( prepare_contact, "Prepare Contact", b3_colorYellow, true ); + + b3World* world = context->world; + b3BodySim* bodySims = context->sims; + b3BodyState* bodyStates = context->states; + + float warmStartScale = world->enableWarmStarting ? 1.0f : 0.0f; + + // Need to use spans in order to find the associated b2Contact, which is per color + b3ContactPrepareSpan* spans = context->contactPrepareSpans; + b3ManifoldConstraint* manifoldBase = context->manifoldConstraints; + b3ContactConstraint* base = context->contactConstraints; + + if ( block.blockType == b3_overflowBlock ) + { + b3GraphColor* overflow = world->constraintGraph.colors + B3_OVERFLOW_INDEX; + spans = context->overflowSpans; + manifoldBase = overflow->manifoldConstraints; + base = overflow->contactConstraints; + } + + int index = block.startIndex; + int endIndex = block.startIndex + block.count; + + // Find color for start index. Linear search but fast. + int colorIndex = 0; + while ( spans[colorIndex + 1].start <= index ) + { + colorIndex += 1; + } + + // Loop over block + while ( index < endIndex ) + { + int colorStart = spans[colorIndex].start; + int colorEndIndex = b3MinInt( spans[colorIndex + 1].start, endIndex ); + b3ContactSpec* specs = spans[colorIndex].contacts; + + // Loop over color + for ( ; index < colorEndIndex; ++index ) + { + b3ContactConstraint* contactConstraint = base + index; + + int localIndex = index - colorStart; + B3_ASSERT( 0 <= localIndex && localIndex < spans[colorIndex].count ); + int contactId = specs[localIndex].contactId; + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + + int indexA = contact->bodySimIndexA; + int indexB = contact->bodySimIndexB; + +#if B3_ENABLE_VALIDATION + if ( indexA != B3_NULL_INDEX ) + { + b3Body* bodyA = b3Array_Get( world->bodies, contact->edges[0].bodyId ); + B3_ASSERT( indexA == bodyA->localIndex ); + } + + if ( indexB != B3_NULL_INDEX ) + { + b3Body* bodyB = b3Array_Get( world->bodies, contact->edges[1].bodyId ); + B3_ASSERT( indexB == bodyB->localIndex ); + } +#endif + + // Body A data + float mA; + b3Matrix3 iA; + b3Vec3 vA; + b3Vec3 wA; + + if ( indexA == B3_NULL_INDEX ) + { + mA = 0.0f; + iA = b3Mat3_zero; + vA = b3Vec3_zero; + wA = b3Vec3_zero; + } + else + { + b3BodySim* simA = bodySims + indexA; + mA = simA->invMass; + iA = simA->invInertiaWorld; + + b3BodyState* stateA = bodyStates + indexA; + vA = stateA->linearVelocity; + wA = stateA->angularVelocity; + } + + // Body B data + float mB; + b3Matrix3 iB; + b3Vec3 vB; + b3Vec3 wB; + + if ( indexB == B3_NULL_INDEX ) + { + mB = 0.0f; + iB = b3Mat3_zero; + vB = b3Vec3_zero; + wB = b3Vec3_zero; + } + else + { + b3BodySim* simB = bodySims + indexB; + mB = simB->invMass; + iB = simB->invInertiaWorld; + + b3BodyState* stateB = bodyStates + indexB; + vB = stateB->linearVelocity; + wB = stateB->angularVelocity; + } + + int manifoldCount = contact->manifoldCount; + contactConstraint->contact = contact; + contactConstraint->manifoldCount = manifoldCount; + contactConstraint->indexA = indexA; + contactConstraint->indexB = indexB; + contactConstraint->invIA = iA; + contactConstraint->invMassA = mA; + contactConstraint->invIB = iB; + contactConstraint->invMassB = mB; + contactConstraint->rollingMass = b3InvertMatrix( b3AddMM( iA, iB ) ); + contactConstraint->softness = + ( contact->flags & b3_contactStaticFlag ) != 0 ? context->staticSoftness : context->contactSoftness; + contactConstraint->friction = contact->friction; + contactConstraint->restitution = contact->restitution; + contactConstraint->rollingResistance = contact->rollingResistance; + + b3ManifoldConstraint* manifoldConstraints = manifoldBase + specs[localIndex].manifoldStart; + contactConstraint->constraints = manifoldConstraints; + + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) + { + b3Manifold* manifold = contact->manifolds + manifoldIndex; + b3ManifoldConstraint* constraint = manifoldConstraints + manifoldIndex; + int pointCount = manifold->pointCount; + b3Vec3 normal = manifold->normal; + b3Vec3 tangent1 = b3Perp( normal ); + b3Vec3 tangent2 = b3Cross( tangent1, normal ); + + constraint->pointCount = pointCount; + constraint->normal = normal; + constraint->tangent1 = tangent1; + constraint->tangent2 = tangent2; + + // Stiffer for static contacts to avoid bodies getting pushed through the ground + constraint->tangentVelocity1 = b3Dot( contact->tangentVelocity, constraint->tangent1 ); + constraint->tangentVelocity2 = b3Dot( contact->tangentVelocity, constraint->tangent2 ); + + b3Vec3 centerA = b3Vec3_zero; + b3Vec3 centerB = b3Vec3_zero; + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + b3ManifoldConstraintPoint* cp = constraint->points + pointIndex; + + // Copy data from manifold point + b3ManifoldPoint* mp = manifold->points + pointIndex; + cp->rA = mp->anchorA; + cp->rB = mp->anchorB; + cp->baseSeparation = mp->separation - b3Dot( b3Sub( cp->rB, cp->rA ), normal ); + cp->normalImpulse = warmStartScale * mp->normalImpulse; + cp->totalNormalImpulse = 0.0f; + + b3Vec3 rA = cp->rA; + b3Vec3 rB = cp->rB; + + b3Vec3 rnA = b3Cross( rA, normal ); + b3Vec3 rnB = b3Cross( rB, normal ); + float kNormal = mA + mB + b3Dot( rnA, b3MulMV( iA, rnA ) ) + b3Dot( rnB, b3MulMV( iB, rnB ) ); + cp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; + + // Save relative velocity for restitution + b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); + b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); + cp->relativeVelocity = b3Dot( normal, b3Sub( vrB, vrA ) ); + + centerA = b3Add( centerA, rA ); + centerB = b3Add( centerB, rB ); + } + + float invCount = 1.0f / pointCount; + centerA = b3MulSV( invCount, centerA ); + centerB = b3MulSV( invCount, centerB ); + constraint->originA = centerA; + constraint->originB = centerB; + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + b3ManifoldConstraintPoint* cp = constraint->points + pointIndex; + cp->leverArm = b3Distance( cp->rA, centerA ); + } + + b3Vec3 rtA1 = b3Cross( centerA, tangent1 ); + b3Vec3 rtA2 = b3Cross( centerA, tangent2 ); + b3Vec3 rtB1 = b3Cross( centerB, tangent1 ); + b3Vec3 rtB2 = b3Cross( centerB, tangent2 ); + + { + b3Matrix2 k; + k.cx.x = mA + mB + b3Dot( rtA1, b3MulMV( iA, rtA1 ) ) + b3Dot( rtB1, b3MulMV( iB, rtB1 ) ); + k.cy.y = mA + mB + b3Dot( rtA2, b3MulMV( iA, rtA2 ) ) + b3Dot( rtB2, b3MulMV( iB, rtB2 ) ); + k.cx.y = k.cy.x = b3Dot( rtA1, b3MulMV( iA, rtA2 ) ) + b3Dot( rtB1, b3MulMV( iB, rtB2 ) ); + + constraint->tangentMass = b3Invert2( k ); + constraint->frictionImpulse.x = warmStartScale * b3Dot( manifold->frictionImpulse, tangent1 ); + constraint->frictionImpulse.y = warmStartScale * b3Dot( manifold->frictionImpulse, tangent2 ); + } + + { + float k = b3Dot( normal, b3MulMV( b3AddMM( iA, iB ), normal ) ); + constraint->twistMass = k > 0.0f ? 1.0f / k : 0.0f; + constraint->twistImpulse = warmStartScale * manifold->twistImpulse; + } + + { + constraint->rollingImpulse = b3MulSV( warmStartScale, manifold->rollingImpulse ); + } + } + } + + // Advance to next color + colorIndex += 1; + } + + b3TracyCZoneEnd( prepare_contact ); +} + +void b3WarmStartContacts_Mesh( b3SolverBlock block, b3StepContext* context ) +{ + b3World* world = context->world; + b3GraphColor* color = world->constraintGraph.colors + block.colorIndex; + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + b3BodyState* states = awakeSet->bodyStates.data; + b3ContactConstraint* constraints = color->contactConstraints; + + // This is a dummy state to represent a static body because static bodies don't have a solver body. + b3BodyState dummyState = b3_identityBodyState; + + int startIndex = block.startIndex; + int endIndex = startIndex + block.count; + + for ( int constraintIndex = startIndex; constraintIndex < endIndex; ++constraintIndex ) + { + const b3ContactConstraint* contactConstraint = constraints + constraintIndex; + int indexA = contactConstraint->indexA; + int indexB = contactConstraint->indexB; + + b3BodyState* stateA = indexA == B3_NULL_INDEX ? &dummyState : states + indexA; + b3BodyState* stateB = indexB == B3_NULL_INDEX ? &dummyState : states + indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + float mA = contactConstraint->invMassA; + b3Matrix3 iA = contactConstraint->invIA; + float mB = contactConstraint->invMassB; + b3Matrix3 iB = contactConstraint->invIB; + + int manifoldCount = contactConstraint->manifoldCount; + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) + { + b3ManifoldConstraint* constraint = contactConstraint->constraints + manifoldIndex; + + // Normal impulses + b3Vec3 normal = constraint->normal; + int pointCount = constraint->pointCount; + for ( int j = 0; j < pointCount; ++j ) + { + const b3ManifoldConstraintPoint* cp = constraint->points + j; + + // fixed anchors + b3Vec3 rA = cp->rA; + b3Vec3 rB = cp->rB; + + b3Vec3 impulse = b3MulSV( cp->normalImpulse, normal ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) ); + vA = b3MulSub( vA, mA, impulse ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) ); + vB = b3MulAdd( vB, mB, impulse ); + } + + // Central friction + { + b3Vec3 rA = constraint->originA; + b3Vec3 rB = constraint->originB; + b3Vec3 impulse = b3MulSV( constraint->frictionImpulse.x, constraint->tangent1 ); + impulse = b3Add( impulse, b3MulSV( constraint->frictionImpulse.y, constraint->tangent2 ) ); + + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) ); + vA = b3MulSub( vA, mA, impulse ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) ); + vB = b3MulAdd( vB, mB, impulse ); + } + + // Central twist friction + { + b3Vec3 impulse = b3MulSV( constraint->twistImpulse, constraint->normal ); + wA = b3Sub( wA, b3MulMV( iA, impulse ) ); + wB = b3Add( wB, b3MulMV( iB, impulse ) ); + } + + // Rolling resistance + { + b3Vec3 impulse = constraint->rollingImpulse; + wA = b3Sub( wA, b3MulMV( iA, impulse ) ); + wB = b3Add( wB, b3MulMV( iB, impulse ) ); + } + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } + } +} + +// Merged normal and friction loops. This is much more stable for the Jenga stack. +void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context, bool useBias ) +{ + b3World* world = context->world; + b3GraphColor* color = world->constraintGraph.colors + block.colorIndex; + b3ContactConstraint* contactConstraints = color->contactConstraints; + b3BodyState* states = context->states; + + // This is a dummy state to represent a static body because static bodies don't have a solver body. + b3BodyState dummyState = b3_identityBodyState; + + // The last block might not be full + int startIndex = block.startIndex; + int endIndex = startIndex + block.count; + + float inv_h = context->inv_h; + const float contactSpeed = context->world->contactSpeed; + + for ( int i = startIndex; i < endIndex; ++i ) + { + b3ContactConstraint* contactConstraint = contactConstraints + i; + int manifoldCount = contactConstraint->manifoldCount; + + int indexA = contactConstraint->indexA; + int indexB = contactConstraint->indexB; + + float mA = contactConstraint->invMassA; + b3Matrix3 iA = contactConstraint->invIA; + float mB = contactConstraint->invMassB; + b3Matrix3 iB = contactConstraint->invIB; + + b3BodyState* stateA = indexA == B3_NULL_INDEX ? &dummyState : states + indexA; + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Quat dqA = stateA->deltaRotation; + + b3BodyState* stateB = indexB == B3_NULL_INDEX ? &dummyState : states + indexB; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + b3Quat dqB = stateB->deltaRotation; + + b3Vec3 dp = b3Sub( stateB->deltaPosition, stateA->deltaPosition ); + b3Softness softness = contactConstraint->softness; + float friction = contactConstraint->friction; + float rollingResistance = contactConstraint->rollingResistance; + + for ( int j = 0; j < manifoldCount; ++j ) + { + b3ManifoldConstraint* constraint = contactConstraint->constraints + j; + + int pointCount = constraint->pointCount; + b3Vec3 normal = constraint->normal; + + float totalNormalImpulse = 0.0f; + float totalTwistLimit = 0.0f; + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + b3ManifoldConstraintPoint* cp = constraint->points + pointIndex; + + // Fixed anchor points for applying impulses + b3Vec3 rA = cp->rA; + b3Vec3 rB = cp->rB; + + // compute current separation + // this is subject to round-off error if the anchor is far from the body center of mass + b3Vec3 ds = b3Add( dp, b3Sub( b3RotateVector( dqB, rB ), b3RotateVector( dqA, rA ) ) ); + float s = b3Dot( ds, normal ) + cp->baseSeparation; + + float velocityBias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( s > 0.0f ) + { + // speculative bias + velocityBias = s * inv_h; + } + else if ( useBias ) + { + velocityBias = b3MaxFloat( softness.massScale * softness.biasRate * s, -contactSpeed ); + massScale = softness.massScale; + impulseScale = softness.impulseScale; + } + + // relative normal velocity at contact + b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); + b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); + float vn = b3Dot( b3Sub( vrB, vrA ), normal ); + + // incremental normal impulse + float deltaImpulse = -cp->normalMass * ( massScale * vn + velocityBias ) - impulseScale * cp->normalImpulse; + + // clamp the accumulated impulse + float newImpulse = b3MaxFloat( cp->normalImpulse + deltaImpulse, 0.0f ); + deltaImpulse = newImpulse - cp->normalImpulse; + cp->normalImpulse = newImpulse; + cp->totalNormalImpulse += newImpulse; + + totalNormalImpulse += newImpulse; + totalTwistLimit += cp->leverArm * cp->normalImpulse; + + // apply normal impulse + b3Vec3 P = b3MulSV( deltaImpulse, normal ); + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) ); + + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); + } + + // No friction when applying bias + if ( useBias == true ) + { + // Go to next manifold + continue; + } + + // Central twist friction + { + float twistSpeed = b3Dot( constraint->normal, b3Sub( wB, wA ) ); + float maxImpulse = friction * totalTwistLimit; + float deltaImpulse = -constraint->twistMass * twistSpeed; + float oldImpulse = constraint->twistImpulse; + constraint->twistImpulse = b3ClampFloat( oldImpulse + deltaImpulse, -maxImpulse, maxImpulse ); + deltaImpulse = constraint->twistImpulse - oldImpulse; + + wA = b3Sub( wA, b3MulMV( iA, b3MulSV( deltaImpulse, constraint->normal ) ) ); + wB = b3Add( wB, b3MulMV( iB, b3MulSV( deltaImpulse, constraint->normal ) ) ); + } + + // Rolling resistance + if ( rollingResistance > 0.0f ) + { + b3Vec3 deltaImpulse = b3Neg( b3MulMV( contactConstraint->rollingMass, b3Sub( wB, wA ) ) ); + b3Vec3 oldImpulse = constraint->rollingImpulse; + constraint->rollingImpulse = b3Add( oldImpulse, deltaImpulse ); + + float maxImpulse = rollingResistance * totalNormalImpulse; + float magSqr = b3Dot( constraint->rollingImpulse, constraint->rollingImpulse ); + if ( magSqr > maxImpulse * maxImpulse + FLT_EPSILON ) + { + constraint->rollingImpulse = b3MulSV( maxImpulse / sqrtf( magSqr ), constraint->rollingImpulse ); + } + + deltaImpulse = b3Sub( constraint->rollingImpulse, oldImpulse ); + + wA = b3Sub( wA, b3MulMV( iA, deltaImpulse ) ); + wB = b3Add( wB, b3MulMV( iB, deltaImpulse ) ); + } + + // Central friction + { + b3Vec3 tangent1 = constraint->tangent1; + b3Vec3 tangent2 = constraint->tangent2; + + // Fixed anchor points for applying impulses + b3Vec3 rA = constraint->originA; + b3Vec3 rB = constraint->originB; + + // Relative tangent velocity at contact + b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); + b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); + b3Vec3 vr = b3Sub( vrB, vrA ); + b3Vec2 vt = { + b3Dot( vr, tangent1 ) - constraint->tangentVelocity1, + b3Dot( vr, tangent2 ) - constraint->tangentVelocity2, + }; + + // Incremental tangent impulse + b3Vec2 tm = b3MulMV2( constraint->tangentMass, vt ); + b3Vec2 deltaImpulse = { -tm.x, -tm.y }; + b3Vec2 newImpulse = { + constraint->frictionImpulse.x + deltaImpulse.x, + constraint->frictionImpulse.y + deltaImpulse.y, + }; + + float maxImpulse = friction * totalNormalImpulse; + + // Clamp the accumulated impulse + float lengthSquared = b3Dot2( newImpulse, newImpulse ); + if ( lengthSquared > maxImpulse * maxImpulse ) + { + float scale = maxImpulse / sqrtf( lengthSquared ); + newImpulse.x *= scale; + newImpulse.y *= scale; + } + deltaImpulse = b3Sub2( newImpulse, constraint->frictionImpulse ); + constraint->frictionImpulse = newImpulse; + + // Apply delta impulse + b3Vec3 P = b3Blend2( deltaImpulse.x, tangent1, deltaImpulse.y, tangent2 ); + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); + } + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } + } +} + +void b3ApplyRestitution_Mesh( b3SolverBlock block, b3StepContext* context ) +{ + b3World* world = context->world; + b3GraphColor* color = world->constraintGraph.colors + block.colorIndex; + b3BodyState* states = context->states; + b3ContactConstraint* constraints = color->contactConstraints; + + // This is a dummy state to represent a static body because static bodies don't have a solver body. + b3BodyState dummyState = b3_identityBodyState; + + int startIndex = block.startIndex; + int endIndex = startIndex + block.count; + + float threshold = context->world->restitutionThreshold; + + for ( int constraintIndex = startIndex; constraintIndex < endIndex; ++constraintIndex ) + { + const b3ContactConstraint* contactConstraint = constraints + constraintIndex; + float restitution = contactConstraint->restitution; + if ( restitution == 0.0f ) + { + continue; + } + + int indexA = contactConstraint->indexA; + int indexB = contactConstraint->indexB; + + b3BodyState* stateA = indexA == B3_NULL_INDEX ? &dummyState : states + indexA; + b3BodyState* stateB = indexB == B3_NULL_INDEX ? &dummyState : states + indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + float mA = contactConstraint->invMassA; + b3Matrix3 iA = contactConstraint->invIA; + float mB = contactConstraint->invMassB; + b3Matrix3 iB = contactConstraint->invIB; + + int manifoldCount = contactConstraint->manifoldCount; + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) + { + b3ManifoldConstraint* cm = contactConstraint->constraints + manifoldIndex; + + b3Vec3 normal = cm->normal; + int pointCount = cm->pointCount; + B3_ASSERT( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + b3ManifoldConstraintPoint* cp = cm->points + pointIndex; + + // If the total normal impulse is zero then there was no collision + // this skips speculative contact points that didn't generate an impulse + // The max normal impulse is used in case there was a collision that moved away within the sub-step + // process + if ( cp->relativeVelocity > -threshold || cp->totalNormalImpulse == 0.0f ) + { + continue; + } + + // fixed anchor points + b3Vec3 rA = cp->rA; + b3Vec3 rB = cp->rB; + + // relative normal velocity at contact + b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); + b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); + float vn = b3Dot( b3Sub( vrB, vrA ), normal ); + + // compute normal impulse + float impulse = -cp->normalMass * ( vn + restitution * cp->relativeVelocity ); + + // clamp the accumulated impulse + float newImpulse = b3MaxFloat( cp->normalImpulse + impulse, 0.0f ); + impulse = newImpulse - cp->normalImpulse; + cp->normalImpulse = newImpulse; + cp->totalNormalImpulse += impulse; + + // apply contact impulse + b3Vec3 P = b3MulSV( impulse, normal ); + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } + } + } +} + +// Don't need to use spans for colors for this because the constraint to contact association +// is already linked by pointer. +void b3StoreImpulses_Mesh( b3SolverBlock block, b3StepContext* context, int workerIndex ) +{ + b3World* world = context->world; + b3ContactConstraint* base = context->contactConstraints; + b3TaskContext* taskContext = world->taskContexts.data + workerIndex; + b3BitSet* hitEventBitSet = &taskContext->hitEventBitSet; + bool hasHitEvents = taskContext->hasHitEvents; + float negHitThreshold = -world->hitEventThreshold; + + for ( int i = block.startIndex; i < block.startIndex + block.count; ++i ) + { + b3ContactConstraint* contactConstraint = base + i; + + // Having this contact pointer simplifies impulse storage + b3Contact* contact = contactConstraint->contact; + + int manifoldCount = contactConstraint->manifoldCount; + B3_ASSERT( manifoldCount == contact->manifoldCount ); + + bool checkHitEvents = ( contact->flags & b3_simEnableHitEvent ) != 0; + bool flagged = false; + + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) + { + b3Manifold* manifold = contact->manifolds + manifoldIndex; + b3ManifoldConstraint* constraint = contactConstraint->constraints + manifoldIndex; + manifold->twistImpulse = constraint->twistImpulse; + manifold->frictionImpulse = b3Blend2( constraint->frictionImpulse.x, constraint->tangent1, + constraint->frictionImpulse.y, constraint->tangent2 ); + manifold->rollingImpulse = constraint->rollingImpulse; + + int count = constraint->pointCount; + B3_ASSERT( count == manifold->pointCount ); + for ( int pointIndex = 0; pointIndex < count; ++pointIndex ) + { + b3ManifoldConstraintPoint* cp = constraint->points + pointIndex; + b3ManifoldPoint* mp = manifold->points + pointIndex; + mp->normalImpulse = cp->normalImpulse; + mp->totalNormalImpulse = cp->totalNormalImpulse; + mp->normalVelocity = cp->relativeVelocity; + + if ( checkHitEvents && flagged == false && + mp->normalVelocity < negHitThreshold && mp->totalNormalImpulse > 0.0f ) + { + b3SetBit( hitEventBitSet, contact->contactId ); + hasHitEvents = true; + flagged = true; + } + } + } + } + + taskContext->hasHitEvents = hasHitEvents; +} + +#if defined( B3_SIMD_NEON ) + +#include + +// wide float holds 4 numbers +typedef float32x4_t b3FloatW; + +#elif defined( B3_SIMD_SSE2 ) + +#include + +// wide float holds 4 numbers +typedef __m128 b3FloatW; + +#else + +// scalar math +typedef struct b3FloatW +{ + float x, y, z, w; +} b3FloatW; + +#endif + +// Wide vec2 +typedef struct b3Vec2W +{ + b3FloatW x, y; +} b3Vec2W; + +// Wide vec3 +typedef struct b3Vec3W +{ + b3FloatW X, Y, Z; +} b3Vec3W; + +// Wide quaternion +typedef struct b3QuatW +{ + b3Vec3W V; + b3FloatW S; +} b3QuatW; + +// Wide symmetric matrix2 +typedef struct b3SymMatrix2W +{ + b3FloatW cxx, cxy, cyy; +} b3SymMatrix2W; + +// Wide symmetric matrix3 +typedef struct b3SymMatrix3W +{ + b3FloatW cxx, cxy, cxz, cyy, cyz, czz; +} b3SymMatrix3W; + +#if defined( B3_SIMD_NEON ) + +static inline b3FloatW b3ZeroW( void ) +{ + return vdupq_n_f32( 0.0f ); +} + +static inline b3FloatW b3SplatW( float scalar ) +{ + return vdupq_n_f32( scalar ); +} + +static inline b3FloatW b3NegW( b3FloatW a ) +{ + return vnegq_f32( a ); +} + +static inline b3FloatW b3SetW( float a, float b, float c, float d ) +{ + float32_t array[4] = { a, b, c, d }; + return vld1q_f32( array ); +} + +static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b ) +{ + return vaddq_f32( a, b ); +} + +static inline b3FloatW b3SubW( b3FloatW a, b3FloatW b ) +{ + return vsubq_f32( a, b ); +} + +static inline b3FloatW b3MulW( b3FloatW a, b3FloatW b ) +{ + return vmulq_f32( a, b ); +} + +static inline b3FloatW b3DivW( b3FloatW a, b3FloatW b ) +{ + return vdivq_f32( a, b ); +} + +static inline b3FloatW b3SqrtW( b3FloatW a ) +{ + return vsqrtq_f32( a ); +} + +// Cannot use real FMA because it doesn't match the non-SIMD path +static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c ) +{ + return vaddq_f32( a, vmulq_f32( b, c ) ); +} + +// static inline b3FloatW b3MulSubW( b3FloatW a, b3FloatW b, b3FloatW c ) +//{ +// return vsubq_f32( a, vmulq_f32( b, c ) ); +// } + +static inline b3FloatW b3MinW( b3FloatW a, b3FloatW b ) +{ + return vminq_f32( a, b ); +} + +static inline b3FloatW b3MaxW( b3FloatW a, b3FloatW b ) +{ + return vmaxq_f32( a, b ); +} + +// clamp a to [-b, b] +static inline b3FloatW b3SymClampW( b3FloatW a, b3FloatW b ) +{ + b3FloatW nb = b3NegW( b ); + b3FloatW c = b3MaxW( nb, a ); + return b3MinW( c, b ); +} + +static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b ) +{ + return vreinterpretq_f32_u32( vorrq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) ); +} + +static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b ) +{ + return vreinterpretq_f32_u32( vcgtq_f32( a, b ) ); +} + +static inline b3FloatW b3EqualsW( b3FloatW a, b3FloatW b ) +{ + return vreinterpretq_f32_u32( vceqq_f32( a, b ) ); +} + +static inline bool b3AllZeroW( b3FloatW a ) +{ + // Create a zero vector for comparison + b3FloatW zero = vdupq_n_f32( 0.0f ); + + // Compare the input vector with zero + uint32x4_t cmp_result = vceqq_f32( a, zero ); + +// Check if all comparison results are non-zero using vminvq +#ifdef __ARM_FEATURE_SVE + // ARM v8.2+ has horizontal minimum instruction + return vminvq_u32( cmp_result ) != 0; +#else + // For older ARM architectures, we need to manually check all lanes + return vgetq_lane_u32( cmp_result, 0 ) != 0 && vgetq_lane_u32( cmp_result, 1 ) != 0 && vgetq_lane_u32( cmp_result, 2 ) != 0 && + vgetq_lane_u32( cmp_result, 3 ) != 0; +#endif +} + +// component-wise returns mask ? b : a +static inline b3FloatW b3BlendW( b3FloatW a, b3FloatW b, b3FloatW mask ) +{ + uint32x4_t mask32 = vreinterpretq_u32_f32( mask ); + return vbslq_f32( mask32, b, a ); +} + +#elif defined( B3_SIMD_SSE2 ) + +static inline b3FloatW b3ZeroW( void ) +{ + return _mm_setzero_ps(); +} + +static inline b3FloatW b3SplatW( float scalar ) +{ + return _mm_set1_ps( scalar ); +} + +static inline b3FloatW b3NegW( b3FloatW a ) +{ + // Create a mask with the sign bit set for each element + __m128 mask = _mm_set1_ps( -0.0f ); + + // XOR the input with the mask to negate each element + return _mm_xor_ps( a, mask ); +} + +static inline b3FloatW b3SetW( float a, float b, float c, float d ) +{ + return _mm_setr_ps( a, b, c, d ); +} + +static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b ) +{ + return _mm_add_ps( a, b ); +} + +static inline b3FloatW b3SubW( b3FloatW a, b3FloatW b ) +{ + return _mm_sub_ps( a, b ); +} + +static inline b3FloatW b3MulW( b3FloatW a, b3FloatW b ) +{ + return _mm_mul_ps( a, b ); +} + +static inline b3FloatW b3DivW( b3FloatW a, b3FloatW b ) +{ + return _mm_div_ps( a, b ); +} + +static inline b3FloatW b3SqrtW( b3FloatW a ) +{ + return _mm_sqrt_ps( a ); +} + +static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c ) +{ + return _mm_add_ps( a, _mm_mul_ps( b, c ) ); +} + +// static inline b3FloatW b3MulSubW( b3FloatW a, b3FloatW b, b3FloatW c ) +//{ +// return _mm_sub_ps( a, _mm_mul_ps( b, c ) ); +// } + +static inline b3FloatW b3MinW( b3FloatW a, b3FloatW b ) +{ + return _mm_min_ps( a, b ); +} + +static inline b3FloatW b3MaxW( b3FloatW a, b3FloatW b ) +{ + return _mm_max_ps( a, b ); +} + +// clamp a to [-b, b] +static inline b3FloatW b3SymClampW( b3FloatW a, b3FloatW b ) +{ + b3FloatW nb = b3NegW( b ); + b3FloatW c = b3MaxW( nb, a ); + return b3MinW( c, b ); +} + +static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b ) +{ + return _mm_or_ps( a, b ); +} + +static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b ) +{ + return _mm_cmpgt_ps( a, b ); +} + +static inline b3FloatW b3EqualsW( b3FloatW a, b3FloatW b ) +{ + return _mm_cmpeq_ps( a, b ); +} + +static inline bool b3AllZeroW( b3FloatW a ) +{ + // Compare each element with zero + b3FloatW zero = _mm_setzero_ps(); + b3FloatW cmp = _mm_cmpeq_ps( a, zero ); + + // Create a mask from the comparison results + int mask = _mm_movemask_ps( cmp ); + + // If all elements are zero, the mask will be 0xF (1111 in binary) + return mask == 0xF; +} + +// component-wise returns mask ? b : a +static inline b3FloatW b3BlendW( b3FloatW a, b3FloatW b, b3FloatW mask ) +{ + return _mm_or_ps( _mm_and_ps( mask, b ), _mm_andnot_ps( mask, a ) ); +} + +#else + +static inline b3FloatW b3ZeroW( void ) +{ + return (b3FloatW){ 0.0f, 0.0f, 0.0f, 0.0f }; +} + +static inline b3FloatW b3SplatW( float scalar ) +{ + return (b3FloatW){ scalar, scalar, scalar, scalar }; +} + +static inline b3FloatW b3NegW( b3FloatW a ) +{ + return (b3FloatW){ -a.x, -a.y, -a.z, -a.w }; +} + +static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b ) +{ + return (b3FloatW){ a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w }; +} + +static inline b3FloatW b3SubW( b3FloatW a, b3FloatW b ) +{ + return (b3FloatW){ a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w }; +} + +static inline b3FloatW b3MulW( b3FloatW a, b3FloatW b ) +{ + return (b3FloatW){ a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w }; +} + +static inline b3FloatW b3DivW( b3FloatW a, b3FloatW b ) +{ + return (b3FloatW){ a.x / b.x, a.y / b.y, a.z / b.z, a.w / b.w }; +} + +static inline b3FloatW b3SqrtW( b3FloatW a ) +{ + return (b3FloatW){ sqrtf( a.x ), sqrtf( a.y ), sqrtf( a.z ), sqrtf( a.w ) }; +} + +static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c ) +{ + return (b3FloatW){ a.x + b.x * c.x, a.y + b.y * c.y, a.z + b.z * c.z, a.w + b.w * c.w }; +} + +// static inline b3FloatW b3MulSubW( b3FloatW a, b3FloatW b, b3FloatW c ) +//{ +// return { a.x - b.x * c.x, a.y - b.y * c.y, a.z - b.z * c.z, a.w - b.w * c.w }; +// } + +// static inline b3FloatW b3MinW( b3FloatW a, b3FloatW b ) +//{ +// b3FloatW r; +// r.x = a.x <= b.x ? a.x : b.x; +// r.y = a.y <= b.y ? a.y : b.y; +// r.z = a.z <= b.z ? a.z : b.z; +// r.w = a.w <= b.w ? a.w : b.w; +// return r; +// } + +static inline b3FloatW b3MaxW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x >= b.x ? a.x : b.x; + r.y = a.y >= b.y ? a.y : b.y; + r.z = a.z >= b.z ? a.z : b.z; + r.w = a.w >= b.w ? a.w : b.w; + return r; +} + +// clamp a to [-b, b] +static inline b3FloatW b3SymClampW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x <= b.x ? a.x : b.x; + r.y = a.y <= b.y ? a.y : b.y; + r.z = a.z <= b.z ? a.z : b.z; + r.w = a.w <= b.w ? a.w : b.w; + r.x = r.x <= -b.x ? -b.x : r.x; + r.y = r.y <= -b.y ? -b.y : r.y; + r.z = r.z <= -b.z ? -b.z : r.z; + r.w = r.w <= -b.w ? -b.w : r.w; + return r; +} + +static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x != 0.0f || b.x != 0.0f ? 1.0f : 0.0f; + r.y = a.y != 0.0f || b.y != 0.0f ? 1.0f : 0.0f; + r.z = a.z != 0.0f || b.z != 0.0f ? 1.0f : 0.0f; + r.w = a.w != 0.0f || b.w != 0.0f ? 1.0f : 0.0f; + return r; +} + +static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x > b.x ? 1.0f : 0.0f; + r.y = a.y > b.y ? 1.0f : 0.0f; + r.z = a.z > b.z ? 1.0f : 0.0f; + r.w = a.w > b.w ? 1.0f : 0.0f; + return r; +} + +static inline b3FloatW b3EqualsW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x == b.x ? 1.0f : 0.0f; + r.y = a.y == b.y ? 1.0f : 0.0f; + r.z = a.z == b.z ? 1.0f : 0.0f; + r.w = a.w == b.w ? 1.0f : 0.0f; + return r; +} + +static inline bool b3AllZeroW( b3FloatW a ) +{ + return a.x == 0.0f && a.y == 0.0f && a.z == 0.0f && a.w == 0.0f; +} + +// component-wise returns mask ? b : a +static inline b3FloatW b3BlendW( b3FloatW a, b3FloatW b, b3FloatW mask ) +{ + b3FloatW r; + r.x = mask.x != 0.0f ? b.x : a.x; + r.y = mask.y != 0.0f ? b.y : a.y; + r.z = mask.z != 0.0f ? b.z : a.z; + r.w = mask.w != 0.0f ? b.w : a.w; + return r; +} + +#endif + +// s * a +static inline b3Vec3W b3MulSVW( b3FloatW s, b3Vec3W a ) +{ + return (b3Vec3W){ b3MulW( s, a.X ), b3MulW( s, a.Y ), b3MulW( s, a.Z ) }; +} + +// a - s * b +static inline b3Vec3W b3MulSubSVW( b3Vec3W a, b3FloatW s, b3Vec3W b ) +{ + return (b3Vec3W){ b3SubW( a.X, b3MulW( s, b.X ) ), b3SubW( a.Y, b3MulW( s, b.Y ) ), b3SubW( a.Z, b3MulW( s, b.Z ) ) }; +} + +// a + s * b +static inline b3Vec3W b3MulAddSVW( b3Vec3W a, b3FloatW s, b3Vec3W b ) +{ + return (b3Vec3W){ b3AddW( a.X, b3MulW( s, b.X ) ), b3AddW( a.Y, b3MulW( s, b.Y ) ), b3AddW( a.Z, b3MulW( s, b.Z ) ) }; +} + +// a + b +static inline b3Vec2W b3AddV2W( b3Vec2W a, b3Vec2W b ) +{ + return (b3Vec2W){ + b3AddW( a.x, b.x ), + b3AddW( a.y, b.y ), + }; +} + +// a - b +static inline b3Vec3W b3SubVW( b3Vec3W a, b3Vec3W b ) +{ + return (b3Vec3W){ + b3SubW( a.X, b.X ), + b3SubW( a.Y, b.Y ), + b3SubW( a.Z, b.Z ), + }; +} + +// a + b +static inline b3Vec3W b3AddVW( b3Vec3W a, b3Vec3W b ) +{ + return (b3Vec3W){ + b3AddW( a.X, b.X ), + b3AddW( a.Y, b.Y ), + b3AddW( a.Z, b.Z ), + }; +} + +// m * a +static inline b3Vec2W b3MulMV2W( b3SymMatrix2W m, b3Vec2W a ) +{ + b3Vec2W b = { + b3AddW( b3MulW( m.cxx, a.x ), b3MulW( m.cxy, a.y ) ), + b3AddW( b3MulW( m.cxy, a.x ), b3MulW( m.cyy, a.y ) ), + }; + + return b; +} + +// m * a +static inline b3Vec3W b3MulMVW( b3SymMatrix3W m, b3Vec3W a ) +{ + b3Vec3W b = { + b3AddW( b3MulW( m.cxx, a.X ), b3AddW( b3MulW( m.cxy, a.Y ), b3MulW( m.cxz, a.Z ) ) ), + b3AddW( b3MulW( m.cxy, a.X ), b3AddW( b3MulW( m.cyy, a.Y ), b3MulW( m.cyz, a.Z ) ) ), + b3AddW( b3MulW( m.cxz, a.X ), b3AddW( b3MulW( m.cyz, a.Y ), b3MulW( m.czz, a.Z ) ) ), + }; + + return b; +} + +// a - m * b +static inline b3Vec3W b3MulSubMVW( b3Vec3W a, b3SymMatrix3W m, b3Vec3W b ) +{ + b3Vec3W c = { + b3AddW( b3MulW( m.cxx, b.X ), b3AddW( b3MulW( m.cxy, b.Y ), b3MulW( m.cxz, b.Z ) ) ), + b3AddW( b3MulW( m.cxy, b.X ), b3AddW( b3MulW( m.cyy, b.Y ), b3MulW( m.cyz, b.Z ) ) ), + b3AddW( b3MulW( m.cxz, b.X ), b3AddW( b3MulW( m.cyz, b.Y ), b3MulW( m.czz, b.Z ) ) ), + }; + + return (b3Vec3W){ b3SubW( a.X, c.X ), b3SubW( a.Y, c.Y ), b3SubW( a.Z, c.Z ) }; +} + +// a + m * b +static inline b3Vec3W b3MulAddMVW( b3Vec3W a, b3SymMatrix3W m, b3Vec3W b ) +{ + b3Vec3W c = { + b3AddW( b3MulW( m.cxx, b.X ), b3AddW( b3MulW( m.cxy, b.Y ), b3MulW( m.cxz, b.Z ) ) ), + b3AddW( b3MulW( m.cxy, b.X ), b3AddW( b3MulW( m.cyy, b.Y ), b3MulW( m.cyz, b.Z ) ) ), + b3AddW( b3MulW( m.cxz, b.X ), b3AddW( b3MulW( m.cyz, b.Y ), b3MulW( m.czz, b.Z ) ) ), + }; + + return (b3Vec3W){ b3AddW( a.X, c.X ), b3AddW( a.Y, c.Y ), b3AddW( a.Z, c.Z ) }; +} + +static inline b3FloatW b3DotW( b3Vec3W a, b3Vec3W b ) +{ + return b3AddW( b3AddW( b3MulW( a.X, b.X ), b3MulW( a.Y, b.Y ) ), b3MulW( a.Z, b.Z ) ); +} + +static inline b3Vec3W b3CrossW( b3Vec3W a, b3Vec3W b ) +{ + b3Vec3W c; + c.X = b3SubW( b3MulW( a.Y, b.Z ), b3MulW( a.Z, b.Y ) ); + c.Y = b3SubW( b3MulW( a.Z, b.X ), b3MulW( a.X, b.Z ) ); + c.Z = b3SubW( b3MulW( a.X, b.Y ), b3MulW( a.Y, b.X ) ); + return c; +} + +static inline b3Vec3W b3RotateVectorW( b3QuatW q, b3Vec3W a ) +{ + b3Vec3W t1 = b3CrossW( q.V, a ); + b3Vec3W t2; + t2.X = b3MulAddW( t1.X, q.S, a.X ); + t2.Y = b3MulAddW( t1.Y, q.S, a.Y ); + t2.Z = b3MulAddW( t1.Z, q.S, a.Z ); + b3Vec3W t3 = b3CrossW( q.V, t2 ); + b3FloatW two = b3SplatW( 2.0f ); + b3Vec3W b; + b.X = b3MulAddW( a.X, two, t3.X ); + b.Y = b3MulAddW( a.Y, two, t3.Y ); + b.Z = b3MulAddW( a.Z, two, t3.Z ); + return b; +} + +// Soft contact constraints with sub-stepping support +// Uses fixed anchors for Jacobians for better behavior on rolling shapes (circles & capsules) +// http://mmacklin.com/smallsteps.pdf +// https://box2d.org/files/ErinCatto_SoftConstraints_GDC2011.pdf + +typedef struct b3ContactConstraintPointWide +{ + b3Vec3W anchorAs, anchorBs; + b3FloatW baseSeparations; + b3FloatW normalImpulses; + b3FloatW totalNormalImpulses; + b3FloatW normalMasses; + b3FloatW leverArms; + b3FloatW relativeVelocities; +} b3ContactConstraintPointWide; + +// Solves four points +typedef struct b3ContactConstraintWide +{ + // These are base 1 + int indexA[B3_SIMD_WIDTH]; + int indexB[B3_SIMD_WIDTH]; + + b3FloatW invMassA, invMassB; + b3SymMatrix3W invIA, invIB; + b3Vec3W normal; + + // todo test computing the tangents on the fly, at least tangent2 + b3Vec3W tangent1; + b3Vec3W tangent2; + + b3Vec3W originA, originB; + b3FloatW twistMass; + b3FloatW twistImpulse; + b3SymMatrix2W tangentMass; + b3Vec2W frictionImpulse; + b3SymMatrix3W rollingMass; + b3Vec3W rollingImpulse; + b3FloatW friction; + b3FloatW rollingResistance; + b3FloatW tangentVelocity1; + b3FloatW tangentVelocity2; + + b3FloatW biasRate; + b3FloatW massScale; + b3FloatW impulseScale; + b3FloatW restitution; + + b3Manifold* manifolds[B3_SIMD_WIDTH]; + + // todo store the maximum point count per wide constraint + // to make this work I need zero initialization which is too + // expensive for all the wide constraint data. Instead + // the graph color should store the point count as a compact secondary + // transient array with zero initialization. + b3ContactConstraintPointWide points[B3_MAX_MANIFOLD_POINTS]; + +} b3ContactConstraintWide; + +int b3GetWideContactConstraintByteCount( void ) +{ + return sizeof( b3ContactConstraintWide ); +} + +// wide version of b3BodyState +typedef struct b3BodyStateW +{ + b3Vec3W v; + b3Vec3W w; + b3Vec3W dp; + b3QuatW dq; +} b3BodyStateW; + +#if defined( B3_SIMD_SSE2 ) || defined( B3_SIMD_NEON ) + +static b3BodyStateW b3GatherBodies( const b3BodyState* states, int* indices ) +{ + b3BodyState dummy = { 0 }; + dummy.deltaRotation.s = 1.0f; + + // Indices are 0 for null + b3BodyState b1 = indices[0] == 0 ? dummy : states[indices[0] - 1]; + b3BodyState b2 = indices[1] == 0 ? dummy : states[indices[1] - 1]; + b3BodyState b3 = indices[2] == 0 ? dummy : states[indices[2] - 1]; + b3BodyState b4 = indices[3] == 0 ? dummy : states[indices[3] - 1]; + + b3BodyStateW s; + s.v.X = b3SetW( b1.linearVelocity.x, b2.linearVelocity.x, b3.linearVelocity.x, b4.linearVelocity.x ); + s.v.Y = b3SetW( b1.linearVelocity.y, b2.linearVelocity.y, b3.linearVelocity.y, b4.linearVelocity.y ); + s.v.Z = b3SetW( b1.linearVelocity.z, b2.linearVelocity.z, b3.linearVelocity.z, b4.linearVelocity.z ); + + s.w.X = b3SetW( b1.angularVelocity.x, b2.angularVelocity.x, b3.angularVelocity.x, b4.angularVelocity.x ); + s.w.Y = b3SetW( b1.angularVelocity.y, b2.angularVelocity.y, b3.angularVelocity.y, b4.angularVelocity.y ); + s.w.Z = b3SetW( b1.angularVelocity.z, b2.angularVelocity.z, b3.angularVelocity.z, b4.angularVelocity.z ); + + s.dp.X = b3SetW( b1.deltaPosition.x, b2.deltaPosition.x, b3.deltaPosition.x, b4.deltaPosition.x ); + s.dp.Y = b3SetW( b1.deltaPosition.y, b2.deltaPosition.y, b3.deltaPosition.y, b4.deltaPosition.y ); + s.dp.Z = b3SetW( b1.deltaPosition.z, b2.deltaPosition.z, b3.deltaPosition.z, b4.deltaPosition.z ); + + s.dq.V.X = b3SetW( b1.deltaRotation.v.x, b2.deltaRotation.v.x, b3.deltaRotation.v.x, b4.deltaRotation.v.x ); + s.dq.V.Y = b3SetW( b1.deltaRotation.v.y, b2.deltaRotation.v.y, b3.deltaRotation.v.y, b4.deltaRotation.v.y ); + s.dq.V.Z = b3SetW( b1.deltaRotation.v.z, b2.deltaRotation.v.z, b3.deltaRotation.v.z, b4.deltaRotation.v.z ); + s.dq.S = b3SetW( b1.deltaRotation.s, b2.deltaRotation.s, b3.deltaRotation.s, b4.deltaRotation.s ); + return s; +} + +// This writes only the velocities back to the solver bodies +static void b3ScatterBodies( b3BodyState* states, int* indices, const b3BodyStateW* simdBody ) +{ + const float* vx = (const float*)&simdBody->v.X; + const float* vy = (const float*)&simdBody->v.Y; + const float* vz = (const float*)&simdBody->v.Z; + const float* wx = (const float*)&simdBody->w.X; + const float* wy = (const float*)&simdBody->w.Y; + const float* wz = (const float*)&simdBody->w.Z; + + // I don't use any dummy body in the body array because this will lead to multithreaded sharing and the + // associated cache flushing. + + // Warning: indices start at 1 with 0 indicating null + + if ( indices[0] != 0 && ( states[indices[0] - 1].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* s = states + ( indices[0] - 1 ); + + b3Vec3 v = { vx[0], vy[0], vz[0] }; + b3Vec3 w = { wx[0], wy[0], wz[0] }; + + uint32_t flags = s->flags; + if ( flags & b3_allLocks ) + { + v.x = ( flags & b3_lockLinearX ) ? 0.0f : v.x; + v.y = ( flags & b3_lockLinearY ) ? 0.0f : v.y; + v.z = ( flags & b3_lockLinearZ ) ? 0.0f : v.z; + w.x = ( flags & b3_lockAngularX ) ? 0.0f : w.x; + w.y = ( flags & b3_lockAngularY ) ? 0.0f : w.y; + w.z = ( flags & b3_lockAngularZ ) ? 0.0f : w.z; + } + + s->linearVelocity = v; + s->angularVelocity = w; + } + + if ( indices[1] != 0 && ( states[indices[1] - 1].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* s = states + ( indices[1] - 1 ); + + b3Vec3 v = { vx[1], vy[1], vz[1] }; + b3Vec3 w = { wx[1], wy[1], wz[1] }; + + uint32_t flags = s->flags; + if ( flags & b3_allLocks ) + { + v.x = ( flags & b3_lockLinearX ) ? 0.0f : v.x; + v.y = ( flags & b3_lockLinearY ) ? 0.0f : v.y; + v.z = ( flags & b3_lockLinearZ ) ? 0.0f : v.z; + w.x = ( flags & b3_lockAngularX ) ? 0.0f : w.x; + w.y = ( flags & b3_lockAngularY ) ? 0.0f : w.y; + w.z = ( flags & b3_lockAngularZ ) ? 0.0f : w.z; + } + + s->linearVelocity = v; + s->angularVelocity = w; + } + + if ( indices[2] != 0 && ( states[indices[2] - 1].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* s = states + ( indices[2] - 1 ); + + b3Vec3 v = { vx[2], vy[2], vz[2] }; + b3Vec3 w = { wx[2], wy[2], wz[2] }; + + uint32_t flags = s->flags; + if ( flags & b3_allLocks ) + { + v.x = ( flags & b3_lockLinearX ) ? 0.0f : v.x; + v.y = ( flags & b3_lockLinearY ) ? 0.0f : v.y; + v.z = ( flags & b3_lockLinearZ ) ? 0.0f : v.z; + w.x = ( flags & b3_lockAngularX ) ? 0.0f : w.x; + w.y = ( flags & b3_lockAngularY ) ? 0.0f : w.y; + w.z = ( flags & b3_lockAngularZ ) ? 0.0f : w.z; + } + + s->linearVelocity = v; + s->angularVelocity = w; + } + + if ( indices[3] != 0 && ( states[indices[3] - 1].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* s = states + ( indices[3] - 1 ); + + b3Vec3 v = { vx[3], vy[3], vz[3] }; + b3Vec3 w = { wx[3], wy[3], wz[3] }; + + uint32_t flags = s->flags; + if ( flags & b3_allLocks ) + { + v.x = ( flags & b3_lockLinearX ) ? 0.0f : v.x; + v.y = ( flags & b3_lockLinearY ) ? 0.0f : v.y; + v.z = ( flags & b3_lockLinearZ ) ? 0.0f : v.z; + w.x = ( flags & b3_lockAngularX ) ? 0.0f : w.x; + w.y = ( flags & b3_lockAngularY ) ? 0.0f : w.y; + w.z = ( flags & b3_lockAngularZ ) ? 0.0f : w.z; + } + + s->linearVelocity = v; + s->angularVelocity = w; + } +} + +#else // non-simd + +static b3BodyStateW b3GatherBodies( const b3BodyState* states, int* indices ) +{ + b3BodyState identity = b3_identityBodyState; + + b3BodyState s1 = indices[0] == 0 ? identity : states[indices[0] - 1]; + b3BodyState s2 = indices[1] == 0 ? identity : states[indices[1] - 1]; + b3BodyState s3 = indices[2] == 0 ? identity : states[indices[2] - 1]; + b3BodyState s4 = indices[3] == 0 ? identity : states[indices[3] - 1]; + + b3BodyStateW simdBody; + simdBody.v.X = (b3FloatW){ s1.linearVelocity.x, s2.linearVelocity.x, s3.linearVelocity.x, s4.linearVelocity.x }; + simdBody.v.Y = (b3FloatW){ s1.linearVelocity.y, s2.linearVelocity.y, s3.linearVelocity.y, s4.linearVelocity.y }; + simdBody.v.Z = (b3FloatW){ s1.linearVelocity.z, s2.linearVelocity.z, s3.linearVelocity.z, s4.linearVelocity.z }; + simdBody.w.X = (b3FloatW){ s1.angularVelocity.x, s2.angularVelocity.x, s3.angularVelocity.x, s4.angularVelocity.x }; + simdBody.w.Y = (b3FloatW){ s1.angularVelocity.y, s2.angularVelocity.y, s3.angularVelocity.y, s4.angularVelocity.y }; + simdBody.w.Z = (b3FloatW){ s1.angularVelocity.z, s2.angularVelocity.z, s3.angularVelocity.z, s4.angularVelocity.z }; + simdBody.dp.X = (b3FloatW){ s1.deltaPosition.x, s2.deltaPosition.x, s3.deltaPosition.x, s4.deltaPosition.x }; + simdBody.dp.Y = (b3FloatW){ s1.deltaPosition.y, s2.deltaPosition.y, s3.deltaPosition.y, s4.deltaPosition.y }; + simdBody.dp.Z = (b3FloatW){ s1.deltaPosition.z, s2.deltaPosition.z, s3.deltaPosition.z, s4.deltaPosition.z }; + simdBody.dq.V.X = (b3FloatW){ s1.deltaRotation.v.x, s2.deltaRotation.v.x, s3.deltaRotation.v.x, s4.deltaRotation.v.x }; + simdBody.dq.V.Y = (b3FloatW){ s1.deltaRotation.v.y, s2.deltaRotation.v.y, s3.deltaRotation.v.y, s4.deltaRotation.v.y }; + simdBody.dq.V.Z = (b3FloatW){ s1.deltaRotation.v.z, s2.deltaRotation.v.z, s3.deltaRotation.v.z, s4.deltaRotation.v.z }; + simdBody.dq.S = (b3FloatW){ s1.deltaRotation.s, s2.deltaRotation.s, s3.deltaRotation.s, s4.deltaRotation.s }; + + return simdBody; +} + +// This writes only the velocities back to the solver bodies +static void b3ScatterBodies( b3BodyState* states, int* indices, const b3BodyStateW* simdBody ) +{ + int index1 = indices[0] - 1; + if ( index1 != -1 && ( states[index1].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* state = states + index1; + state->linearVelocity.x = simdBody->v.X.x; + state->linearVelocity.y = simdBody->v.Y.x; + state->linearVelocity.z = simdBody->v.Z.x; + state->angularVelocity.x = simdBody->w.X.x; + state->angularVelocity.y = simdBody->w.Y.x; + state->angularVelocity.z = simdBody->w.Z.x; + } + + int index2 = indices[1] - 1; + if ( index2 != -1 && ( states[index2].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* state = states + index2; + state->linearVelocity.x = simdBody->v.X.y; + state->linearVelocity.y = simdBody->v.Y.y; + state->linearVelocity.z = simdBody->v.Z.y; + state->angularVelocity.x = simdBody->w.X.y; + state->angularVelocity.y = simdBody->w.Y.y; + state->angularVelocity.z = simdBody->w.Z.y; + } + + int index3 = indices[2] - 1; + if ( index3 != -1 && ( states[index3].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* state = states + index3; + state->linearVelocity.x = simdBody->v.X.z; + state->linearVelocity.y = simdBody->v.Y.z; + state->linearVelocity.z = simdBody->v.Z.z; + state->angularVelocity.x = simdBody->w.X.z; + state->angularVelocity.y = simdBody->w.Y.z; + state->angularVelocity.z = simdBody->w.Z.z; + } + + int index4 = indices[3] - 1; + if ( index4 != -1 && ( states[index4].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* state = states + index4; + state->linearVelocity.x = simdBody->v.X.w; + state->linearVelocity.y = simdBody->v.Y.w; + state->linearVelocity.z = simdBody->v.Z.w; + state->angularVelocity.x = simdBody->w.X.w; + state->angularVelocity.y = simdBody->w.Y.w; + state->angularVelocity.z = simdBody->w.Z.w; + } +} +#endif + +// Prepare convex contact constraints +void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( prepare_contact, "Prepare Contact", b3_colorYellow, true ); + b3World* world = context->world; + b3BodySim* sims = context->sims; + b3BodyState* states = context->states; +#if B3_ENABLE_VALIDATION + b3Body* bodies = world->bodies.data; +#endif + b3WidePrepareSpan* spans = context->widePrepareSpans; + b3ContactConstraintWide* wideBase = context->wideConstraints; + + // Stiffer for static contacts to avoid bodies getting pushed through the ground + b3Softness contactSoftness = context->contactSoftness; + b3Softness staticSoftness = context->staticSoftness; + + float warmStartScale = world->enableWarmStarting ? 1.0f : 0.0f; + + int wideIndex = block.startIndex; + int endWideIndex = block.startIndex + block.count; + + // Find color for start index. Linear search but fast. + int colorIndex = 0; + while ( spans[colorIndex + 1].start <= wideIndex ) + { + colorIndex += 1; + } + + // Loop over block + while ( wideIndex < endWideIndex ) + { + int colorWideStart = spans[colorIndex].start; + int colorWideEndIndex = b3MinInt( spans[colorIndex + 1].start, endWideIndex ); + int colorContactCount = spans[colorIndex].count; + int* contactIds = spans[colorIndex].contacts; + + // Loop over color + for ( ; wideIndex < colorWideEndIndex; ++wideIndex ) + { + b3ContactConstraintWide* constraint = wideBase + wideIndex; + int localWideIndex = wideIndex - colorWideStart; + + for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) + { + int contactIndex = B3_SIMD_WIDTH * localWideIndex + lane; + if ( contactIndex >= colorContactCount ) + { + // Remainder lanes were zeroed in solver setup. + break; + } + + int contactId = contactIds[contactIndex]; + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + B3_ASSERT( contact->manifoldCount == 1 ); + b3Manifold* manifold = contact->manifolds + 0; + + int indexA = contact->bodySimIndexA; + int indexB = contact->bodySimIndexB; + +#if B3_ENABLE_VALIDATION + b3Body* bodyA = bodies + contact->edges[0].bodyId; + int validIndexA = bodyA->setIndex == b3_awakeSet ? bodyA->localIndex : B3_NULL_INDEX; + b3Body* bodyB = bodies + contact->edges[1].bodyId; + int validIndexB = bodyB->setIndex == b3_awakeSet ? bodyB->localIndex : B3_NULL_INDEX; + B3_ASSERT( indexA == validIndexA ); + B3_ASSERT( indexB == validIndexB ); +#endif + + // 0 for null + constraint->indexA[lane] = indexA + 1; + constraint->indexB[lane] = indexB + 1; + constraint->manifolds[lane] = manifold; + + // Body A data + float mA; + b3Matrix3 iA; + b3Vec3 vA; + b3Vec3 wA; + + if ( indexA == B3_NULL_INDEX ) + { + mA = 0.0f; + iA = b3Mat3_zero; + vA = b3Vec3_zero; + wA = b3Vec3_zero; + } + else + { + b3BodySim* simA = sims + indexA; + mA = simA->invMass; + iA = simA->invInertiaWorld; + + b3BodyState* stateA = states + indexA; + vA = stateA->linearVelocity; + wA = stateA->angularVelocity; + } + + // Body B data + float mB; + b3Matrix3 iB; + b3Vec3 vB; + b3Vec3 wB; + + if ( indexB == B3_NULL_INDEX ) + { + mB = 0.0f; + iB = b3Mat3_zero; + vB = b3Vec3_zero; + wB = b3Vec3_zero; + } + else + { + b3BodySim* simB = sims + indexB; + mB = simB->invMass; + iB = simB->invInertiaWorld; + + b3BodyState* stateB = states + indexB; + vB = stateB->linearVelocity; + wB = stateB->angularVelocity; + } + + ( (float*)&constraint->invMassA )[lane] = mA; + ( (float*)&constraint->invMassB )[lane] = mB; + + ( (float*)&constraint->invIA.cxx )[lane] = iA.cx.x; + ( (float*)&constraint->invIA.cxy )[lane] = iA.cx.y; + ( (float*)&constraint->invIA.cxz )[lane] = iA.cx.z; + ( (float*)&constraint->invIA.cyy )[lane] = iA.cy.y; + ( (float*)&constraint->invIA.cyz )[lane] = iA.cy.z; + ( (float*)&constraint->invIA.czz )[lane] = iA.cz.z; + + ( (float*)&constraint->invIB.cxx )[lane] = iB.cx.x; + ( (float*)&constraint->invIB.cxy )[lane] = iB.cx.y; + ( (float*)&constraint->invIB.cxz )[lane] = iB.cx.z; + ( (float*)&constraint->invIB.cyy )[lane] = iB.cy.y; + ( (float*)&constraint->invIB.cyz )[lane] = iB.cy.z; + ( (float*)&constraint->invIB.czz )[lane] = iB.cz.z; + + b3Softness soft = ( indexA == B3_NULL_INDEX || indexB == B3_NULL_INDEX ) ? staticSoftness : contactSoftness; + + b3Vec3 normal = manifold->normal; + ( (float*)&constraint->normal.X )[lane] = normal.x; + ( (float*)&constraint->normal.Y )[lane] = normal.y; + ( (float*)&constraint->normal.Z )[lane] = normal.z; + + b3Vec3 tangent1 = b3Perp( normal ); + ( (float*)&constraint->tangent1.X )[lane] = tangent1.x; + ( (float*)&constraint->tangent1.Y )[lane] = tangent1.y; + ( (float*)&constraint->tangent1.Z )[lane] = tangent1.z; + + b3Vec3 tangent2 = b3Cross( tangent1, normal ); + ( (float*)&constraint->tangent2.X )[lane] = tangent2.x; + ( (float*)&constraint->tangent2.Y )[lane] = tangent2.y; + ( (float*)&constraint->tangent2.Z )[lane] = tangent2.z; + + ( (float*)&constraint->friction )[lane] = contact->friction; + ( (float*)&constraint->restitution )[lane] = contact->restitution; + ( (float*)&constraint->rollingResistance )[lane] = contact->rollingResistance; + + ( (float*)&constraint->tangentVelocity1 )[lane] = b3Dot( contact->tangentVelocity, tangent1 ); + ( (float*)&constraint->tangentVelocity2 )[lane] = b3Dot( contact->tangentVelocity, tangent2 ); + + ( (float*)&constraint->biasRate )[lane] = soft.biasRate; + ( (float*)&constraint->massScale )[lane] = soft.massScale; + ( (float*)&constraint->impulseScale )[lane] = soft.impulseScale; + + int pointCount = manifold->pointCount; + b3Vec3 originA = b3Vec3_zero; + b3Vec3 originB = b3Vec3_zero; + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + const b3ManifoldPoint* mp = manifold->points + pointIndex; + b3ContactConstraintPointWide* cp = constraint->points + pointIndex; + + b3Vec3 rA = mp->anchorA; + b3Vec3 rB = mp->anchorB; + originA = b3Add( originA, rA ); + originB = b3Add( originB, rB ); + + ( (float*)&cp->anchorAs.X )[lane] = rA.x; + ( (float*)&cp->anchorAs.Y )[lane] = rA.y; + ( (float*)&cp->anchorAs.Z )[lane] = rA.z; + + ( (float*)&cp->anchorBs.X )[lane] = rB.x; + ( (float*)&cp->anchorBs.Y )[lane] = rB.y; + ( (float*)&cp->anchorBs.Z )[lane] = rB.z; + + float baseSeparation = mp->separation - b3Dot( b3Sub( rB, rA ), normal ); + ( (float*)&cp->baseSeparations )[lane] = baseSeparation; + + ( (float*)&cp->normalImpulses )[lane] = warmStartScale * mp->normalImpulse; + ( (float*)&cp->totalNormalImpulses )[lane] = 0.0f; + + b3Vec3 rnA = b3Cross( rA, normal ); + b3Vec3 rnB = b3Cross( rB, normal ); + float kNormal = mA + mB + b3Dot( rnA, b3MulMV( iA, rnA ) ) + b3Dot( rnB, b3MulMV( iB, rnB ) ); + ( (float*)&cp->normalMasses )[lane] = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; + + // Save relative velocity for restitution + b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); + b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); + ( (float*)&cp->relativeVelocities )[lane] = b3Dot( normal, b3Sub( vrB, vrA ) ); + } + + float invCount = 1.0f / pointCount; + originA = b3MulSV( invCount, originA ); + originB = b3MulSV( invCount, originB ); + + ( (float*)&constraint->originA.X )[lane] = originA.x; + ( (float*)&constraint->originA.Y )[lane] = originA.y; + ( (float*)&constraint->originA.Z )[lane] = originA.z; + ( (float*)&constraint->originB.X )[lane] = originB.x; + ( (float*)&constraint->originB.Y )[lane] = originB.y; + ( (float*)&constraint->originB.Z )[lane] = originB.z; + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + const b3ManifoldPoint* mp = manifold->points + pointIndex; + b3ContactConstraintPointWide* cp = constraint->points + pointIndex; + ( (float*)&cp->leverArms )[lane] = b3Distance( mp->anchorA, originA ); + } + + b3Vec3 rtA1 = b3Cross( originA, tangent1 ); + b3Vec3 rtA2 = b3Cross( originA, tangent2 ); + + b3Vec3 rtB1 = b3Cross( originB, tangent1 ); + b3Vec3 rtB2 = b3Cross( originB, tangent2 ); + + { + b3Matrix2 k; + k.cx.x = mA + mB + b3Dot( rtA1, b3MulMV( iA, rtA1 ) ) + b3Dot( rtB1, b3MulMV( iB, rtB1 ) ); + k.cy.y = mA + mB + b3Dot( rtA2, b3MulMV( iA, rtA2 ) ) + b3Dot( rtB2, b3MulMV( iB, rtB2 ) ); + k.cx.y = k.cy.x = b3Dot( rtA1, b3MulMV( iA, rtA2 ) ) + b3Dot( rtB1, b3MulMV( iB, rtB2 ) ); + b3Matrix2 tangentMass = b3Invert2( k ); + + ( (float*)&constraint->tangentMass.cxx )[lane] = tangentMass.cx.x; + ( (float*)&constraint->tangentMass.cxy )[lane] = tangentMass.cx.y; + ( (float*)&constraint->tangentMass.cyy )[lane] = tangentMass.cy.y; + + ( (float*)&constraint->frictionImpulse.x )[lane] = + warmStartScale * b3Dot( manifold->frictionImpulse, tangent1 ); + ( (float*)&constraint->frictionImpulse.y )[lane] = + warmStartScale * b3Dot( manifold->frictionImpulse, tangent2 ); + } + + { + float k = b3Dot( normal, b3MulMV( b3AddMM( iA, iB ), normal ) ); + ( (float*)&constraint->twistMass )[lane] = k > 0.0f ? 1.0f / k : 0.0f; + ( (float*)&constraint->twistImpulse )[lane] = warmStartScale * manifold->twistImpulse; + } + + { + b3Matrix3 rollingMass = b3InvertMatrix( b3AddMM( iA, iB ) ); + + ( (float*)&constraint->rollingMass.cxx )[lane] = rollingMass.cx.x; + ( (float*)&constraint->rollingMass.cxy )[lane] = rollingMass.cx.y; + ( (float*)&constraint->rollingMass.cxz )[lane] = rollingMass.cx.z; + ( (float*)&constraint->rollingMass.cyy )[lane] = rollingMass.cy.y; + ( (float*)&constraint->rollingMass.cyz )[lane] = rollingMass.cy.z; + ( (float*)&constraint->rollingMass.czz )[lane] = rollingMass.cz.z; + + ( (float*)&constraint->rollingImpulse.X )[lane] = warmStartScale * manifold->rollingImpulse.x; + ( (float*)&constraint->rollingImpulse.Y )[lane] = warmStartScale * manifold->rollingImpulse.y; + ( (float*)&constraint->rollingImpulse.Z )[lane] = warmStartScale * manifold->rollingImpulse.z; + } + + // zero remaining points + for ( int pointIndex = pointCount; pointIndex < B3_MAX_MANIFOLD_POINTS; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = constraint->points + pointIndex; + ( (float*)&cp->anchorAs.X )[lane] = 0.0f; + ( (float*)&cp->anchorAs.Y )[lane] = 0.0f; + ( (float*)&cp->anchorAs.Z )[lane] = 0.0f; + ( (float*)&cp->anchorBs.X )[lane] = 0.0f; + ( (float*)&cp->anchorBs.Y )[lane] = 0.0f; + ( (float*)&cp->anchorBs.Z )[lane] = 0.0f; + ( (float*)&cp->baseSeparations )[lane] = 0.0f; + ( (float*)&cp->normalImpulses )[lane] = 0.0f; + ( (float*)&cp->totalNormalImpulses )[lane] = 0.0f; + ( (float*)&cp->normalMasses )[lane] = 0.0f; + ( (float*)&cp->relativeVelocities )[lane] = 0.0f; + ( (float*)&cp->leverArms )[lane] = 0.0f; + } + } + } + + // Advance to next color + colorIndex += 1; + } + + b3TracyCZoneEnd( prepare_contact ); +} + +void b3WarmStartContacts_Convex( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( warm_start_contact, "Warm Start", b3_colorGreen, true ); + + b3BodyState* states = context->states; + b3ContactConstraintWide* constraints = context->graph->colors[block.colorIndex].wideConstraints; + + for ( int i = block.startIndex; i < block.startIndex + block.count; ++i ) + { + b3ContactConstraintWide* c = constraints + i; + b3BodyStateW bA = b3GatherBodies( states, c->indexA ); + b3BodyStateW bB = b3GatherBodies( states, c->indexB ); + + // Normal impulses + for ( int j = 0; j < B3_MAX_MANIFOLD_POINTS; ++j ) + { + b3ContactConstraintPointWide* cp = c->points + j; + + b3Vec3W rA = cp->anchorAs; + b3Vec3W rB = cp->anchorBs; + + b3Vec3W impulse; + impulse.X = b3MulW( cp->normalImpulses, c->normal.X ); + impulse.Y = b3MulW( cp->normalImpulses, c->normal.Y ); + impulse.Z = b3MulW( cp->normalImpulses, c->normal.Z ); + + bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, impulse ) ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, impulse ); + bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, impulse ) ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, impulse ); + } + + // Central friction + { + b3Vec3W rA = c->originA; + b3Vec3W rB = c->originB; + b3Vec3W impulse = b3MulSVW( c->frictionImpulse.x, c->tangent1 ); + impulse = b3MulAddSVW( impulse, c->frictionImpulse.y, c->tangent2 ); + + bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, impulse ) ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, impulse ); + bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, impulse ) ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, impulse ); + } + + // Central twist friction + { + b3Vec3W impulse = b3MulSVW( c->twistImpulse, c->normal ); + bA.w = b3MulSubMVW( bA.w, c->invIA, impulse ); + bB.w = b3MulAddMVW( bB.w, c->invIB, impulse ); + } + + // Rolling resistance + { + b3Vec3W impulse = c->rollingImpulse; + bA.w = b3MulSubMVW( bA.w, c->invIA, impulse ); + bB.w = b3MulAddMVW( bB.w, c->invIB, impulse ); + } + + b3ScatterBodies( states, c->indexA, &bA ); + b3ScatterBodies( states, c->indexB, &bB ); + } + + b3TracyCZoneEnd( warm_start_contact ); +} + +void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context, bool useBias ) +{ + b3TracyCZoneNC( solve_contact, "Solve Contact", b3_colorAliceBlue, true ); + + b3BodyState* states = context->states; + b3ContactConstraintWide* constraints = context->graph->colors[block.colorIndex].wideConstraints; + b3FloatW inv_h = b3SplatW( context->inv_h ); + b3FloatW contactSpeed = b3SplatW( -context->world->contactSpeed ); + b3FloatW oneW = b3SplatW( 1.0f ); + b3FloatW epsilonW = b3SplatW( FLT_EPSILON ); + + for ( int wideIndex = block.startIndex; wideIndex < block.startIndex + block.count; ++wideIndex ) + { + b3ContactConstraintWide* c = constraints + wideIndex; + + b3BodyStateW bA = b3GatherBodies( states, c->indexA ); + b3BodyStateW bB = b3GatherBodies( states, c->indexB ); + + b3FloatW biasRate, massScale, impulseScale; + if ( useBias ) + { + biasRate = b3MulW( c->massScale, c->biasRate ); + massScale = c->massScale; + impulseScale = c->impulseScale; + } + else + { + biasRate = b3ZeroW(); + massScale = oneW; + impulseScale = b3ZeroW(); + } + + b3Vec3W dp = b3SubVW( bB.dp, bA.dp ); + + b3FloatW totalNormalImpulse = b3ZeroW(); + b3FloatW totalTwistLimit = b3ZeroW(); + + // todo_erin use the max point count of the four manifolds + for ( int pointIndex = 0; pointIndex < B3_MAX_MANIFOLD_POINTS; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = c->points + pointIndex; + + // Fixed anchor points for applying impulses + b3Vec3W rA = cp->anchorAs; + b3Vec3W rB = cp->anchorBs; + + // Moving anchors for current separation + // todo speed this up using matrices + b3Vec3W rsA = b3RotateVectorW( bA.dq, rA ); + b3Vec3W rsB = b3RotateVectorW( bB.dq, rB ); + + // compute current separation + // this is subject to round-off error if the anchor is far from the body center of mass + b3Vec3W ds = b3AddVW( dp, b3SubVW( rsB, rsA ) ); + b3FloatW s = b3AddW( b3DotW( c->normal, ds ), cp->baseSeparations ); + + // Apply speculative bias if separation is greater than zero, otherwise apply soft constraint bias + b3FloatW mask = b3GreaterThanW( s, b3ZeroW() ); + b3FloatW specBias = b3MulW( s, inv_h ); + b3FloatW softBias = b3MaxW( b3MulW( biasRate, s ), contactSpeed ); + b3FloatW bias = b3BlendW( softBias, specBias, mask ); + + b3FloatW pointMassScale = b3BlendW( massScale, oneW, mask ); + b3FloatW pointImpulseScale = b3BlendW( impulseScale, b3ZeroW(), mask ); + + // Relative velocity at contact + b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); + b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); + b3FloatW vn = b3DotW( b3SubVW( vrB, vrA ), c->normal ); + + // Compute normal impulse + b3FloatW negImpulse = b3AddW( b3MulW( cp->normalMasses, b3AddW( b3MulW( pointMassScale, vn ), bias ) ), + b3MulW( pointImpulseScale, cp->normalImpulses ) ); + + // Clamp the accumulated impulse + b3FloatW newImpulse = b3MaxW( b3SubW( cp->normalImpulses, negImpulse ), b3ZeroW() ); + b3FloatW deltaImpulse = b3SubW( newImpulse, cp->normalImpulses ); + cp->normalImpulses = newImpulse; + cp->totalNormalImpulses = b3AddW( cp->totalNormalImpulses, newImpulse ); + + totalNormalImpulse = b3AddW( totalNormalImpulse, newImpulse ); + totalTwistLimit = b3AddW( totalTwistLimit, b3MulW( cp->leverArms, newImpulse ) ); + + // Apply contact impulse + b3Vec3W P = b3MulSVW( deltaImpulse, c->normal ); + bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); + bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); + } + + // No friction when applying bias + if ( useBias == false ) + { + // Rolling resistance + if ( b3AllZeroW( c->rollingResistance ) == false ) + { + // flip A/B order to negate + b3Vec3W deltaImpulse = b3MulMVW( c->rollingMass, b3SubVW( bA.w, bB.w ) ); + b3Vec3W oldImpulse = c->rollingImpulse; + c->rollingImpulse = b3AddVW( oldImpulse, deltaImpulse ); + + b3FloatW maxImpulse = b3MulW( c->rollingResistance, totalNormalImpulse ); + b3FloatW lengthSquared = b3DotW( c->rollingImpulse, c->rollingImpulse ); + + // if ( magSqr > maxLambda * maxLambda + FLT_EPSILON ) + //{ + // c->rollingImpulse *= maxLambda / sqrtf( magSqr ); + // } + + b3FloatW mask = b3GreaterThanW( lengthSquared, b3MulAddW( epsilonW, maxImpulse, maxImpulse ) ); + + // No approximate _mm_rsqrt_ps here to maintain cross-platform determinism + b3FloatW normalize = b3DivW( maxImpulse, b3AddW( b3SqrtW( lengthSquared ), epsilonW ) ); + b3FloatW scale = b3BlendW( oneW, normalize, mask ); + + // Ensure zero rolling resistance yields no impulse + b3FloatW rollingMask = b3GreaterThanW( c->rollingResistance, b3ZeroW() ); + scale = b3BlendW( b3ZeroW(), scale, rollingMask ); + + c->rollingImpulse = b3MulSVW( scale, c->rollingImpulse ); + + deltaImpulse = b3SubVW( c->rollingImpulse, oldImpulse ); + + bA.w = b3MulSubMVW( bA.w, c->invIA, deltaImpulse ); + bB.w = b3MulAddMVW( bB.w, c->invIB, deltaImpulse ); + } + + // Central twist friction + { + b3FloatW twistSpeed = b3DotW( c->normal, b3SubVW( bB.w, bA.w ) ); + b3FloatW maxLambda = b3MulW( c->friction, totalTwistLimit ); + b3FloatW deltaImpulse = b3NegW( b3MulW( c->twistMass, twistSpeed ) ); + b3FloatW oldImpulse = c->twistImpulse; + c->twistImpulse = b3SymClampW( b3AddW( oldImpulse, deltaImpulse ), maxLambda ); + deltaImpulse = b3SubW( c->twistImpulse, oldImpulse ); + + b3Vec3W L = b3MulSVW( deltaImpulse, c->normal ); + bA.w = b3MulSubMVW( bA.w, c->invIA, L ); + bB.w = b3MulAddMVW( bB.w, c->invIB, L ); + } + + // Central friction + { + b3Vec3W tangent1 = c->tangent1; + b3Vec3W tangent2 = c->tangent2; + + // Fixed anchor points for applying impulses + b3Vec3W rA = c->originA; + b3Vec3W rB = c->originB; + + // Relative tangent velocity at contact + b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); + b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); + b3Vec3W vr = b3SubVW( vrB, vrA ); + b3Vec2W vt = { + b3SubW( b3DotW( vr, tangent1 ), c->tangentVelocity1 ), + b3SubW( b3DotW( vr, tangent2 ), c->tangentVelocity2 ), + }; + + // Incremental tangent impulse + b3Vec2W deltaImpulse = b3MulMV2W( c->tangentMass, vt ); + deltaImpulse = (b3Vec2W){ b3NegW( deltaImpulse.x ), b3NegW( deltaImpulse.y ) }; + b3Vec2W newImpulse = b3AddV2W( c->frictionImpulse, deltaImpulse ); + + b3FloatW friction = c->friction; + b3FloatW maxImpulse = b3MulW( friction, totalNormalImpulse ); + + // Clamp the accumulated impulse + b3FloatW lengthSquared = b3AddW( b3MulW( newImpulse.x, newImpulse.x ), b3MulW( newImpulse.y, newImpulse.y ) ); + + // Max impulse can be zero + b3FloatW mask = b3GreaterThanW( lengthSquared, b3MulW( maxImpulse, maxImpulse ) ); + + // No approximate _mm_rsqrt_ps here to maintain cross-platform determinism. Add epsilon to avoid divide by + // zero. + b3FloatW normalize = b3DivW( maxImpulse, b3AddW( b3SqrtW( lengthSquared ), epsilonW ) ); + b3FloatW scale = b3BlendW( oneW, normalize, mask ); + newImpulse = (b3Vec2W){ + b3MulW( scale, newImpulse.x ), + b3MulW( scale, newImpulse.y ), + }; + + deltaImpulse = (b3Vec2W){ + b3SubW( newImpulse.x, c->frictionImpulse.x ), + b3SubW( newImpulse.y, c->frictionImpulse.y ), + }; + + c->frictionImpulse = newImpulse; + + // Apply delta impulse + b3Vec3W P = b3AddVW( b3MulSVW( deltaImpulse.x, tangent1 ), b3MulSVW( deltaImpulse.y, tangent2 ) ); + bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); + bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); + } + } + + b3ScatterBodies( states, c->indexA, &bA ); + b3ScatterBodies( states, c->indexB, &bB ); + } + + b3TracyCZoneEnd( solve_contact ); +} + +void b3ApplyRestitution_Convex( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( restitution, "Restitution", b3_colorDodgerBlue, true ); + + b3BodyState* states = context->states; + b3ContactConstraintWide* constraints = context->graph->colors[block.colorIndex].wideConstraints; + b3FloatW threshold = b3SplatW( context->world->restitutionThreshold ); + b3FloatW zero = b3ZeroW(); + + for ( int i = block.startIndex; i < block.startIndex + block.count; ++i ) + { + b3ContactConstraintWide* c = constraints + i; + + if ( b3AllZeroW( c->restitution ) ) + { + // No lanes have restitution. Common case. + continue; + } + + // Single gather for all manifolds + b3BodyStateW bA = b3GatherBodies( states, c->indexA ); + b3BodyStateW bB = b3GatherBodies( states, c->indexB ); + + // Create a mask based on restitution so that lanes with no restitution are not affected + // by the calculations below. + b3FloatW restitutionMask = b3EqualsW( c->restitution, zero ); + + for ( int pointIndex = 0; pointIndex < B3_MAX_MANIFOLD_POINTS; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = c->points + pointIndex; + + // Set effective mass to zero if restitution should not be applied + b3FloatW mask1 = b3GreaterThanW( b3AddW( cp->relativeVelocities, threshold ), zero ); + b3FloatW mask2 = b3EqualsW( cp->totalNormalImpulses, zero ); + b3FloatW mask = b3OrW( b3OrW( mask1, mask2 ), restitutionMask ); + b3FloatW mass = b3BlendW( cp->normalMasses, zero, mask ); + + // Fixed anchors for impulses + b3Vec3W rA = cp->anchorAs; + b3Vec3W rB = cp->anchorBs; + + // Relative velocity at contact + b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); + b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); + b3FloatW vn = b3DotW( b3SubVW( vrB, vrA ), c->normal ); + + // Compute normal impulse + b3FloatW negImpulse = b3MulW( mass, b3AddW( vn, b3MulW( c->restitution, cp->relativeVelocities ) ) ); + + // Clamp the accumulated impulse + b3FloatW newImpulse = b3MaxW( b3SubW( cp->normalImpulses, negImpulse ), b3ZeroW() ); + b3FloatW deltaImpulse = b3SubW( newImpulse, cp->normalImpulses ); + cp->normalImpulses = newImpulse; + cp->totalNormalImpulses = b3AddW( cp->totalNormalImpulses, deltaImpulse ); + + // Apply contact impulse + b3Vec3W P = b3MulSVW( deltaImpulse, c->normal ); + bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); + bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); + } + + b3ScatterBodies( states, c->indexA, &bA ); + b3ScatterBodies( states, c->indexB, &bB ); + } + + b3TracyCZoneEnd( restitution ); +} + +// Store impulses by contact constraint +void b3StoreImpulses_Convex( b3SolverBlock block, b3StepContext* context, int workerIndex ) +{ + b3TracyCZoneNC( store_impulses, "Store", b3_colorFireBrick, true ); + + b3World* world = context->world; + b3WidePrepareSpan* spans = context->widePrepareSpans; + const b3ContactConstraintWide* wideBase = context->wideConstraints; + b3TaskContext* taskContext = world->taskContexts.data + workerIndex; + b3BitSet* hitEventBitSet = &taskContext->hitEventBitSet; + bool hasHitEvents = taskContext->hasHitEvents; + float negHitThreshold = -world->hitEventThreshold; + + int wideIndex = block.startIndex; + int endWideIndex = block.startIndex + block.count; + + // Find color for start index + int colorIndex = 0; + while ( spans[colorIndex + 1].start <= wideIndex ) + { + colorIndex += 1; + } + + while ( wideIndex < endWideIndex ) + { + int colorWideStart = spans[colorIndex].start; + int colorWideEndIndex = b3MinInt( spans[colorIndex + 1].start, endWideIndex ); + int colorContactCount = spans[colorIndex].count; + int* contactIds = spans[colorIndex].contacts; + + for ( ; wideIndex < colorWideEndIndex; ++wideIndex ) + { + const b3ContactConstraintWide* c = wideBase + wideIndex; + const float* frictionImpulse1 = (float*)&c->frictionImpulse.x; + const float* frictionImpulse2 = (float*)&c->frictionImpulse.y; + const float* tangent1X = (float*)&c->tangent1.X; + const float* tangent1Y = (float*)&c->tangent1.Y; + const float* tangent1Z = (float*)&c->tangent1.Z; + const float* tangent2X = (float*)&c->tangent2.X; + const float* tangent2Y = (float*)&c->tangent2.Y; + const float* tangent2Z = (float*)&c->tangent2.Z; + const float* twistImpulse = (float*)&c->twistImpulse; + const float* rollingImpulseX = (float*)&c->rollingImpulse.X; + const float* rollingImpulseY = (float*)&c->rollingImpulse.Y; + const float* rollingImpulseZ = (float*)&c->rollingImpulse.Z; + + int localWideIndex = wideIndex - colorWideStart; + + for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) + { + int contactIndex = B3_SIMD_WIDTH * localWideIndex + lane; + if ( contactIndex >= colorContactCount ) + { + break; + } + + b3Manifold* m = c->manifolds[lane]; + if ( m == NULL ) + { + continue; + } + + float f1 = frictionImpulse1[lane]; + float f2 = frictionImpulse2[lane]; + m->frictionImpulse = (b3Vec3){ + f1 * tangent1X[lane] + f2 * tangent2X[lane], + f1 * tangent1Y[lane] + f2 * tangent2Y[lane], + f1 * tangent1Z[lane] + f2 * tangent2Z[lane], + }; + m->twistImpulse = twistImpulse[lane]; + m->rollingImpulse = (b3Vec3){ + rollingImpulseX[lane], + rollingImpulseY[lane], + rollingImpulseZ[lane], + }; + + int pointCount = m->pointCount; + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + const b3ContactConstraintPointWide* cp = c->points + pointIndex; + const float* normalImpulse = (float*)&cp->normalImpulses; + const float* totalNormalImpulse = (float*)&cp->totalNormalImpulses; + const float* normalVelocity = (float*)&cp->relativeVelocities; + + b3ManifoldPoint* mp = m->points + pointIndex; + mp->normalImpulse = normalImpulse[lane]; + mp->totalNormalImpulse = totalNormalImpulse[lane]; + mp->normalVelocity = normalVelocity[lane]; + } + + int contactId = contactIds[contactIndex]; + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + if ( ( contact->flags & b3_simEnableHitEvent ) != 0 ) + { + for ( int k = 0; k < pointCount; ++k ) + { + b3ManifoldPoint* mp = m->points + k; + + // Need to check total impulse because the point may be speculative and not colliding + if ( mp->normalVelocity < negHitThreshold && mp->totalNormalImpulse > 0.0f ) + { + b3SetBit( hitEventBitSet, contact->contactId ); + hasHitEvents = true; + break; + } + } + } + } + } + + colorIndex += 1; + } + + taskContext->hasHitEvents = hasHitEvents; + + b3TracyCZoneEnd( store_impulses ); +} + +void b3PrepareContacts_Overflow( b3StepContext* context ) +{ + b3ConstraintGraph* graph = context->graph; + b3GraphColor* color = graph->colors + B3_OVERFLOW_INDEX; + + uint16_t count = (uint16_t)color->contacts.count; + if (count == 0) + { + return; + } + + b3SolverBlock block = { + .startIndex = 0, + .count = count, + .blockType = b3_overflowBlock, + .colorIndex = B3_OVERFLOW_INDEX, + }; + + b3PrepareContacts_Mesh( block, context ); +} + +void b3WarmStartContacts_Overflow( b3StepContext* context ) +{ + b3ConstraintGraph* graph = context->graph; + b3GraphColor* color = graph->colors + B3_OVERFLOW_INDEX; + + uint16_t count = (uint16_t)color->contacts.count; + if ( count == 0 ) + { + return; + } + + b3SolverBlock block = { + .startIndex = 0, + .count = count, + .blockType = b3_overflowBlock, + .colorIndex = B3_OVERFLOW_INDEX, + }; + + b3WarmStartContacts_Mesh( block, context ); +} + +void b3SolveContacts_Overflow( b3StepContext* context, bool useBias ) +{ + b3ConstraintGraph* graph = context->graph; + b3GraphColor* color = graph->colors + B3_OVERFLOW_INDEX; + + uint16_t count = (uint16_t)color->contacts.count; + if ( count == 0 ) + { + return; + } + + b3SolverBlock block = { + .startIndex = 0, + .count = count, + .blockType = b3_overflowBlock, + .colorIndex = B3_OVERFLOW_INDEX, + }; + + b3SolveContacts_Mesh( block, context, useBias ); +} + +void b3ApplyRestitution_Overflow( b3StepContext* context ) +{ + b3ConstraintGraph* graph = context->graph; + b3GraphColor* color = graph->colors + B3_OVERFLOW_INDEX; + + uint16_t count = (uint16_t)color->contacts.count; + if ( count == 0 ) + { + return; + } + + b3SolverBlock block = { + .startIndex = 0, + .count = count, + .blockType = b3_overflowBlock, + .colorIndex = B3_OVERFLOW_INDEX, + }; + + b3ApplyRestitution_Mesh( block, context ); +} + +void b3StoreImpulses_Overflow( b3StepContext* context ) +{ + b3ConstraintGraph* graph = context->graph; + b3GraphColor* color = graph->colors + B3_OVERFLOW_INDEX; + + uint16_t count = (uint16_t)color->contacts.count; + if ( count == 0 ) + { + return; + } + + b3SolverBlock block = { + .startIndex = 0, + .count = count, + .blockType = b3_overflowBlock, + .colorIndex = B3_OVERFLOW_INDEX, + }; + + // Overflow runs serially on the orchestrator (worker 0) + b3StoreImpulses_Mesh( block, context, 0 ); +} diff --git a/src/contact_solver.h b/src/contact_solver.h new file mode 100644 index 0000000..67c767f --- /dev/null +++ b/src/contact_solver.h @@ -0,0 +1,82 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "math_internal.h" +#include "solver.h" + +typedef struct b3ManifoldConstraintPoint +{ + b3Vec3 rA, rB; + float baseSeparation; + float relativeVelocity; + float normalImpulse; + float totalNormalImpulse; + float normalMass; + float leverArm; +} b3ManifoldConstraintPoint; + +typedef struct b3ManifoldConstraint +{ + // todo use pointer buffer + b3ManifoldConstraintPoint points[4]; + int pointCount; + b3Vec3 normal; + b3Vec3 tangent1; + b3Vec3 tangent2; + b3Vec3 originA, originB; + float twistMass; + float twistImpulse; + b3Matrix2 tangentMass; + b3Vec2 frictionImpulse; + b3Vec3 rollingImpulse; + float tangentVelocity1; + float tangentVelocity2; +} b3ManifoldConstraint; + +typedef struct b3ContactConstraint +{ + b3ManifoldConstraint* constraints; + struct b3Contact* contact; + int indexA; + int indexB; + float invMassA, invMassB; + b3Matrix3 invIA, invIB; + b3Softness softness; + b3Matrix3 rollingMass; + float friction; + float restitution; + float rollingResistance; + int manifoldCount; +} b3ContactConstraint; + +#ifdef __cplusplus +extern "C" +{ +#endif + +int b3GetWideContactConstraintByteCount( void ); + +// Overflow contacts don't fit into the constraint graph coloring +void b3PrepareContacts_Overflow( b3StepContext* context ); +void b3WarmStartContacts_Overflow( b3StepContext* context ); +void b3SolveContacts_Overflow( b3StepContext* context, bool useBias ); +void b3ApplyRestitution_Overflow( b3StepContext* context ); +void b3StoreImpulses_Overflow( b3StepContext* context ); + +void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ); +void b3WarmStartContacts_Mesh( b3SolverBlock block, b3StepContext* context ); +void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context, bool useBias ); +void b3ApplyRestitution_Mesh( b3SolverBlock block, b3StepContext* context ); +void b3StoreImpulses_Mesh( b3SolverBlock block, b3StepContext* context, int workerIndex ); + +void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ); +void b3WarmStartContacts_Convex( b3SolverBlock block, b3StepContext* context ); +void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context, bool useBias ); +void b3ApplyRestitution_Convex( b3SolverBlock block, b3StepContext* context ); +void b3StoreImpulses_Convex( b3SolverBlock block, b3StepContext* context, int workerIndex ); + +#ifdef __cplusplus +} +#endif diff --git a/src/container.h b/src/container.h new file mode 100644 index 0000000..77f484f --- /dev/null +++ b/src/container.h @@ -0,0 +1,184 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "algorithm.h" +#include "core.h" + +#include +#include + +#define b3DeclareArray( T ) \ + typedef struct b3DynamicArray_##T \ + { \ + struct T* data; \ + int count; \ + int capacity; \ + } b3DynamicArray_##T + +#define b3DeclareArrayNative( T ) \ + typedef struct b3DynamicArray_##T \ + { \ + T* data; \ + int count; \ + int capacity; \ + } b3DynamicArray_##T + +// Define an array. +// It may be zero initialized: +// b3ArrayC(int) myArray = { 0 }; +#define b3ArrayC( T ) b3DynamicArray_##T + +// Alternative to zero initialization +#define b3Array_Create( a ) \ + do \ + { \ + ( a ).data = NULL; \ + ( a ).count = 0; \ + ( a ).capacity = 0; \ + } \ + while ( 0 ) + +#define b3Array_CreateN( a, n ) \ + do \ + { \ + ( a ).data = ( n ) > 0 ? (B3_TYPE_OF( ( a ).data ))b3GrowAlloc( NULL, 0, ( n ) * sizeof( *( a ).data ) ) : NULL; \ + ( a ).count = 0; \ + ( a ).capacity = ( n ); \ + } \ + while ( 0 ) + +#define b3Array_Destroy( a ) \ + do \ + { \ + b3Free( ( a ).data, ( a ).capacity * sizeof( *( a ).data ) ); \ + ( a ).data = NULL; \ + ( a ).count = 0; \ + ( a ).capacity = 0; \ + } \ + while ( 0 ) + +#define b3Array_Reserve( a, n ) \ + do \ + { \ + if ( ( a ).capacity < n ) \ + { \ + int oldSize = ( a ).capacity * sizeof( *( a ).data ); \ + int newSize = ( n ) * sizeof( *( a ).data ); \ + ( a ).data = (B3_TYPE_OF( ( a ).data ))b3GrowAlloc( ( a ).data, oldSize, newSize ); \ + ( a ).capacity = ( n ); \ + } \ + } \ + while ( 0 ) + +#define b3Array_Resize( a, n ) \ + do \ + { \ + b3Array_Reserve( a, n ); \ + ( a ).count = ( n ); \ + } \ + while ( 0 ) + +// Push a new element by value +#define b3Array_Push( a, value ) \ + do \ + { \ + if ( ( a ).count >= ( a ).capacity ) \ + { \ + int oldSize = ( a ).capacity * sizeof( *( a ).data ); \ + int newCapacity = ( a ).capacity == 0 ? 8 : 2 * ( a ).capacity; \ + int newSize = newCapacity * sizeof( *( a ).data ); \ + ( a ).data = (B3_TYPE_OF( ( a ).data ))b3GrowAlloc( ( a ).data, oldSize, newSize ); \ + ( a ).capacity = newCapacity; \ + } \ + ( a ).data[( a ).count++] = ( value ); \ + } \ + while ( 0 ) + +// Get a pointer to an element +#define b3Array_Get( a, index ) ( B3_ASSERT( 0 <= ( index ) && ( index ) < ( a ).count ), ( a ).data + ( index ) ) + +// Create a new uninitialized element and return a pointer to it +#define b3Array_Emplace( a ) \ + ( (B3_TYPE_OF( ( a ).data ))b3EmplaceHelper( (void**)&( a ).data, &( a ).count, &( a ).capacity, sizeof( *( a ).data ) ) ) + +// Remove the last element and return it by value. +#define b3Array_Pop( a ) ( B3_ASSERT( 0 < ( a ).count ), ( a ).data[-1 + ( a ).count--] ) + +// Add an uninitialized element and return its index. +#define b3Array_AddIndex( a ) \ + ( b3EmplaceHelper( (void**)&( a ).data, &( a ).count, &( a ).capacity, sizeof( *( a ).data ) ), ( a ).count - 1 ) + +// Append a contiguous run of values. +#define b3Array_Append( a, src, n ) \ + do \ + { \ + int b3_n = ( n ); \ + if ( ( a ).count + b3_n > ( a ).capacity ) \ + { \ + int req = ( a ).count + b3_n; \ + int newCapacity = req > 2 ? req + ( req >> 1 ) : 8; \ + int oldSize = ( a ).capacity * sizeof( *( a ).data ); \ + int newSize = newCapacity * sizeof( *( a ).data ); \ + ( a ).data = (B3_TYPE_OF( ( a ).data ))b3GrowAlloc( ( a ).data, oldSize, newSize ); \ + ( a ).capacity = newCapacity; \ + } \ + memcpy( ( a ).data + ( a ).count, ( src ), b3_n * sizeof( *( a ).data ) ); \ + ( a ).count += b3_n; \ + } \ + while ( 0 ) + +// Zero the entire allocated buffer (capacity, not just count). +#define b3Array_MemZero( a ) \ + do \ + { \ + if ( ( a ).capacity > 0 ) \ + { \ + memset( ( a ).data, 0, ( a ).capacity * sizeof( *( a ).data ) ); \ + } \ + } \ + while ( 0 ) + +// Remove and element by swapping with the last element. If the index is the last element it returns +// B3_NULL_INDEX, otherwise it returns the index of the last element (which is now out of bounds). +#define b3Array_RemoveSwap( a, index ) b3RemoveHelper( ( a ).data, &( a ).count, ( index ), sizeof( *( a ).data ) ) + +B3_INLINE void* b3EmplaceHelper( void** data, int* count, int* capacity, int elem_size ) +{ + if ( *count >= *capacity ) + { + int oldCapacity = *capacity; + int oldSize = oldCapacity * elem_size; + int newCapacity = ( oldCapacity == 0 ? 16 : 2 * oldCapacity ); + int newSize = newCapacity * elem_size; + *data = b3GrowAlloc( *data, oldSize, newSize ); + *capacity = newCapacity; + } + return (char*)*data + ( *count )++ * elem_size; +} + +B3_INLINE int b3RemoveHelper( void* data, int* count, int index, int elementSize ) +{ + B3_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 B3_NULL_INDEX; +} + +#define b3Array_Clear( a ) \ + do \ + { \ + ( a ).count = 0; \ + } \ + while ( 0 ) + +#define b3Array_ByteCount( a ) ( ( a ).capacity * (int)sizeof( *( a ).data ) ) + +b3DeclareArrayNative( int ); diff --git a/src/convex_manifold.c b/src/convex_manifold.c new file mode 100644 index 0000000..6639539 --- /dev/null +++ b/src/convex_manifold.c @@ -0,0 +1,1603 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "algorithm.h" +#include "manifold.h" +#include "shape.h" + +#include "box3d/base.h" +#include "box3d/collision.h" +#include "box3d/constants.h" + +#include +#include + +static inline bool b3IsMinkowskiFaceIsolated( b3Vec3 a, b3Vec3 b, b3Vec3 n ) +{ + // An isolated edge (e.g. like in a capsule) defines a circle through the + // origin on the Gauss map. So testing for overlap between this circle and + // the arc AB simplifies to a simple plane test. + float an = b3Dot( a, n ); + float bn = b3Dot( b, n ); + + return an * bn <= 0.0f; +} + +// bxa = cross(b, a) and dxc = cross(d, c) +// but in practice we use the edge vector between the faces for robustness +static inline bool b3IsMinkowskiFace( b3Vec3 a, b3Vec3 b, b3Vec3 bxa, b3Vec3 c, b3Vec3 d, b3Vec3 dxc ) +{ + // Two edges build a face on the Minkowski sum if the associated arcs ab and cd intersect on the Gauss map. + // The associated arcs are defined by the adjacent face normals of each edge. + float cba = b3Dot( c, bxa ); + float dba = b3Dot( d, bxa ); + float adc = b3Dot( a, dxc ); + float bdc = b3Dot( b, dxc ); + + return cba * dba < 0.0f && adc * bdc < 0.0f && cba * bdc > 0.0f; +} + +static int b3ClipSegment( b3ClipVertex segment[2], b3Plane plane ) +{ + int vertexCount = 0; + b3ClipVertex vertex1 = segment[0]; + b3ClipVertex vertex2 = segment[1]; + + float distance1 = b3PlaneSeparation( plane, vertex1.position ); + float distance2 = b3PlaneSeparation( plane, vertex2.position ); + + // If the points are behind the plane + if ( distance1 <= 0.0f ) + { + segment[vertexCount++] = vertex1; + } + if ( distance2 <= 0.0f ) + { + segment[vertexCount++] = vertex2; + } + + // If the points are on different sides of the plane + if ( distance1 * distance2 < 0.0f ) + { + // Find intersection point of edge and plane + float t = distance1 / ( distance1 - distance2 ); + segment[vertexCount].position = b3Add( b3MulSV( 1.0f - t, vertex1.position ), b3MulSV( t, vertex2.position ) ); + segment[vertexCount].pair = distance1 > 0.0f ? vertex1.pair : vertex2.pair; + vertexCount++; + } + + return vertexCount; +} + +static int b3ClipSegmentToHullFace( b3ClipVertex segment[2], const b3Hull* hull, int refFace ) +{ + const b3HullFace* faces = b3GetHullFaces( hull ); + const b3Plane* planes = b3GetHullPlanes( hull ); + const b3HullHalfEdge* edges = b3GetHullEdges( hull ); + const b3Vec3* points = b3GetHullPoints( hull ); + + b3Plane refPlane = planes[refFace]; + + const b3HullFace* face = faces + refFace; + + int edgeIndex = face->edge; + + do + { + const b3HullHalfEdge* edge = edges + edgeIndex; + int nextEdgeIndex = edge->next; + const b3HullHalfEdge* next = edges + nextEdgeIndex; + + b3Vec3 vertex1 = points[edge->origin]; + b3Vec3 vertex2 = points[next->origin]; + b3Vec3 tangent = b3Normalize( b3Sub( vertex2, vertex1 ) ); + b3Vec3 binormal = b3Cross( tangent, refPlane.normal ); + + int pointCount = b3ClipSegment( segment, b3MakePlaneFromNormalAndPoint( binormal, vertex1 ) ); + if ( pointCount < 2 ) + { + return 0; + } + + edgeIndex = nextEdgeIndex; + } + while ( edgeIndex != face->edge ); + + return 2; +} + +static b3FaceQuery b3QueryFaceDirectionHullAndCapsule( const b3Hull* hull, const b3Capsule* capsule, + b3Transform capsuleTransform ) +{ + int maxFaceIndex = -1; + int maxVertexIndex = -1; + float maxFaceSeparation = -FLT_MAX; + const b3Plane* planes = b3GetHullPlanes( hull ); + + b3Vec3 capsulePoints[2] = { + b3TransformPoint( capsuleTransform, capsule->center1 ), + b3TransformPoint( capsuleTransform, capsule->center2 ), + }; + + for ( int faceIndex = 0; faceIndex < hull->faceCount; ++faceIndex ) + { + b3Plane plane = planes[faceIndex]; + + int vertexIndex = b3GetPointSupport( capsulePoints, 2, b3Neg( plane.normal ) ); + b3Vec3 support = capsulePoints[vertexIndex]; + float separation = b3PlaneSeparation( plane, support ); + if ( separation > maxFaceSeparation ) + { + maxVertexIndex = vertexIndex; + maxFaceIndex = faceIndex; + maxFaceSeparation = separation; + } + } + + return (b3FaceQuery){ + .separation = maxFaceSeparation, + .faceIndex = (uint8_t)maxFaceIndex, + .vertexIndex = (uint8_t)maxVertexIndex, + }; +} + +static b3FaceQuery b3QueryFaceDirections( const b3Hull* hullA, const b3Hull* hullB, b3Transform relativeTransform ) +{ + // We perform all computations in local space of the second hull + b3Transform transform = b3InvertTransform( relativeTransform ); + const b3Plane* planesA = b3GetHullPlanes( hullA ); + const b3Vec3* pointsB = b3GetHullPoints( hullB ); + + int maxFaceIndex = -1; + int maxVertexIndex = -1; + float maxFaceSeparation = -FLT_MAX; + + for ( int faceIndex = 0; faceIndex < hullA->faceCount; ++faceIndex ) + { + b3Plane plane = b3TransformPlane( transform, planesA[faceIndex] ); + + int vertexIndex = b3FindHullSupportVertex( hullB, b3Neg( plane.normal ) ); + b3Vec3 support = pointsB[vertexIndex]; + float separation = b3PlaneSeparation( plane, support ); + if ( separation > maxFaceSeparation ) + { + maxFaceIndex = faceIndex; + maxVertexIndex = vertexIndex; + maxFaceSeparation = separation; + } + } + + return (b3FaceQuery){ + .separation = maxFaceSeparation, + .faceIndex = (uint8_t)maxFaceIndex, + .vertexIndex = (uint8_t)maxVertexIndex, + }; +} + +static b3EdgeQuery b3QueryEdgeDirectionHullAndCapsule( const b3Hull* hull, const b3Capsule* capsule, + b3Transform capsuleTransform ) +{ + // Find axis of minimum penetration + float maxSeparation = -FLT_MAX; + int maxIndex1 = -1; + int maxIndex2 = -1; + + // We perform all computations in local space of the hull + b3Vec3 p1 = b3TransformPoint( capsuleTransform, capsule->center1 ); + b3Vec3 q1 = b3TransformPoint( capsuleTransform, capsule->center2 ); + b3Vec3 e1 = b3Sub( q1, p1 ); + + const b3HullHalfEdge* edges = b3GetHullEdges( hull ); + const b3Vec3* points = b3GetHullPoints( hull ); + const b3Plane* planes = b3GetHullPlanes( hull ); + + for ( int index = 0; index < hull->edgeCount; index += 2 ) + { + const b3HullHalfEdge* edge = edges + index; + const b3HullHalfEdge* twin = edges + index + 1; + B3_ASSERT( edge->twin == index + 1 && twin->twin == index ); + + b3Vec3 p2 = points[edge->origin]; + b3Vec3 q2 = points[twin->origin]; + b3Vec3 e2 = b3Sub( q2, p2 ); + + b3Vec3 u2 = planes[edge->face].normal; + b3Vec3 v2 = planes[twin->face].normal; + + if ( b3IsMinkowskiFaceIsolated( u2, v2, e1 ) ) + { + // We can pass any point on the edge and choose + // the edge centers for better numerical precision. + b3Vec3 c1 = b3MulSV( 0.5f, b3Add( q1, p1 ) ); + b3Vec3 c2 = hull->center; + float separation = b3EdgeEdgeSeparation( q1, e1, c1, q2, e2, c2 ); + if ( separation > maxSeparation ) + { + // Note: We don't exit early if we find a separating axis here since we want to + // find the best one for caching and account for the convex radius later. + maxSeparation = separation; + maxIndex1 = 0; + maxIndex2 = index; + } + } + } + + // Save result + return (b3EdgeQuery){ + .separation = maxSeparation, + .indexA = (uint8_t)maxIndex1, + .indexB = (uint8_t)maxIndex2, + }; +} + +static b3EdgeQuery b3QueryEdgeDirections( const b3Hull* hullA, const b3Hull* hullB, b3Transform transformBtoA ) +{ + // Find axis of minimum penetration + float maxSeparation = -FLT_MAX; + int maxIndexA = B3_NULL_INDEX; + int maxIndexB = B3_NULL_INDEX; + + const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA ); + const b3Vec3* pointsA = b3GetHullPoints( hullA ); + const b3Plane* planesA = b3GetHullPlanes( hullA ); + const b3HullHalfEdge* edgesB = b3GetHullEdges( hullB ); + const b3Vec3* pointsB = b3GetHullPoints( hullB ); + const b3Plane* planesB = b3GetHullPlanes( hullB ); + + // Work in frame A + b3Matrix3 matrix = b3MakeMatrixFromQuat( transformBtoA.q ); + + // Arranged to minimize transform operations + for ( int indexB = 0; indexB < hullB->edgeCount; indexB += 2 ) + { + const b3HullHalfEdge* edgeB = edgesB + indexB; + const b3HullHalfEdge* twinB = edgesB + indexB + 1; + B3_ASSERT( edgeB->twin == indexB + 1 && twinB->twin == indexB ); + + b3Vec3 qB = pointsB[twinB->origin]; + b3Vec3 eB = b3MulMV( matrix, b3Sub( qB, pointsB[edgeB->origin] ) ); + qB = b3Add( b3MulMV( matrix, qB ), transformBtoA.p ); + + b3Vec3 uB = b3MulMV( matrix, planesB[edgeB->face].normal ); + b3Vec3 vB = b3MulMV( matrix, planesB[twinB->face].normal ); + + for ( int indexA = 0; indexA < hullA->edgeCount; indexA += 2 ) + { + const b3HullHalfEdge* edgeA = edgesA + indexA; + const b3HullHalfEdge* twinA = edgesA + indexA + 1; + B3_ASSERT( edgeA->twin == indexA + 1 && twinA->twin == indexA ); + + b3Vec3 qA = pointsA[twinA->origin]; + b3Vec3 eA = b3Sub( qA, pointsA[edgeA->origin] ); + b3Vec3 uA = planesA[edgeA->face].normal; + b3Vec3 vA = planesA[twinA->face].normal; + + bool isMinkowski; + { + // Two edges build a face on the Minkowski sum if the associated arcs AB and CD intersect on the Gauss map. + // The associated arcs are defined by the adjacent face normals of each edge. + float cba = b3Dot( uB, eA ); + float dba = b3Dot( vB, eA ); + float adc = -b3Dot( uA, eB ); + float bdc = -b3Dot( vA, eB ); + + isMinkowski = cba * dba < 0.0f && adc * bdc < 0.0f && cba * bdc > 0.0f; + } + + if ( isMinkowski ) + { + b3Vec3 centerA = hullA->center; + b3Vec3 centerB = b3TransformPoint( transformBtoA, hullB->center ); + float separation = b3EdgeEdgeSeparation( qA, eA, centerA, qB, eB, centerB ); + + if ( separation > maxSeparation ) + { + // Continues to find the maximum separating axis + maxSeparation = separation; + maxIndexA = indexA; + maxIndexB = indexB; + } + } + } + } + + return (b3EdgeQuery){ + .separation = maxSeparation, + .indexA = maxIndexA, + .indexB = maxIndexB, + }; +} + +// Reduce the manifold points to a maximum of 4 points. +// Note: this modifies the input point array to improve performance +static void b3ReduceManifoldPoints( b3LocalManifold* manifold, int capacity, b3LocalManifoldPoint* points, int count ) +{ + if ( capacity < 4 ) + { + return; + } + + if ( count <= 4 ) + { + for ( int i = 0; i < count; ++i ) + { + manifold->points[i] = points[i]; + } + + manifold->pointCount = count; + return; + } + + b3Vec3 normal = manifold->normal; + // float linearSlop = B3_LINEAR_SLOP; + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + float tolSqr = speculativeDistance * speculativeDistance; + + // This bias is very important for contact point consistency across time steps. + // It creates a pecking order to avoid flickering between candidates with similar scores. + float bias = 0.95f; + + // Step 1: find extreme point that is touching + int bestIndex = B3_NULL_INDEX; + float bestScore = -FLT_MAX; + + // Arbitrary tangent direction + // b3Vec3 perp1 = b3Perp( normal ); + // b3Vec3 perp2 = b3Cross( perp1, normal ); + // b3Vec3 searchDirection = -0.4535961214255773f * perp1 + 0.8912073600614354f * perp2; + b3Vec3 searchDirection = b3ArbitraryPerp( normal ); + for ( int index = 0; index < count; ++index ) + { + b3LocalManifoldPoint* pt = points + index; + + if ( pt->separation > speculativeDistance ) + { + continue; + } + + // The deeper the better + float score = -pt->separation + b3Dot( searchDirection, pt->point ); + if ( bias * score > bestScore ) + { + bestIndex = index; + bestScore = score; + } + } + + B3_VALIDATE( 0 <= bestIndex && bestIndex < count ); + if ( bestIndex == B3_NULL_INDEX ) + { + manifold->pointCount = 0; + return; + } + + manifold->points[0] = points[bestIndex]; + manifold->pointCount = 1; + + // Remove best point from array + points[bestIndex] = points[count - 1]; + count -= 1; + + b3Vec3 a = manifold->points[0].point; + + // Step 2: Find farthest point in 2D + bestScore = 0.0f; + bestIndex = B3_NULL_INDEX; + float maxDistanceSquared = 0.0f; + + for ( int index = 0; index < count; ++index ) + { + b3Vec3 p = points[index].point; + b3Vec3 d = b3Sub( p, a ); + b3Vec3 v = b3MulSub( d, b3Dot( d, normal ), normal ); + float distanceSquared = b3LengthSquared( v ); + maxDistanceSquared = b3MaxFloat( maxDistanceSquared, distanceSquared ); + float separation = b3MaxFloat( 0.0f, -points[index].separation ); + float score = distanceSquared + 4.0f * separation * separation; + if ( bias * score > bestScore ) + { + bestScore = score; + bestIndex = index; + } + } + + if ( bestScore < tolSqr ) + { + return; + } + + B3_ASSERT( 0 <= bestIndex && bestIndex < count ); + manifold->points[1] = points[bestIndex]; + manifold->pointCount = 2; + + // Remove best point from array + points[bestIndex] = points[count - 1]; + count -= 1; + + b3Vec3 b = manifold->points[1].point; + + // Step 3: Find the point with the maximum triangular area + bestScore = tolSqr; + bestIndex = B3_NULL_INDEX; + float bestSignedArea = 0.0f; + b3Vec3 ba = b3Sub( b, a ); + for ( int index = 0; index < count; ++index ) + { + b3Vec3 p = points[index].point; + float signedArea = b3Dot( normal, b3Cross( ba, b3Sub( p, a ) ) ); + float score = b3AbsFloat( signedArea ); + if ( bias * score >= bestScore ) + { + bestScore = score; + bestIndex = index; + bestSignedArea = signedArea; + } + } + + if ( bestIndex == B3_NULL_INDEX ) + { + return; + } + + B3_ASSERT( bestIndex != B3_NULL_INDEX ); + + manifold->points[2] = points[bestIndex]; + manifold->pointCount = 3; + points[bestIndex] = points[count - 1]; + count -= 1; + + b3Vec3 c = manifold->points[2].point; + + // Step 4: get the point that adds the most area outside the current triangle + bestScore = tolSqr; + bestIndex = B3_NULL_INDEX; + float sign = bestSignedArea < 0.0f ? -1.0f : 1.0f; + for ( int index = 0; index < count; ++index ) + { + b3Vec3 p = points[index].point; + float u1 = sign * b3Dot( normal, b3Cross( b3Sub( p, a ), ba ) ); + float u2 = sign * b3Dot( normal, b3Cross( b3Sub( p, b ), b3Sub( c, b ) ) ); + float u3 = sign * b3Dot( normal, b3Cross( b3Sub( p, c ), b3Sub( a, c ) ) ); + float score = b3MaxFloat( u1, b3MaxFloat( u2, u3 ) ); + + if ( bias * score > bestScore ) + { + bestScore = score; + bestIndex = index; + } + } + + if ( bestIndex != B3_NULL_INDEX ) + { + manifold->points[manifold->pointCount] = points[bestIndex]; + manifold->pointCount += 1; + } +} + +void b3CollideSpheres( b3LocalManifold* manifold, int capacity, const b3Sphere* sphereA, const b3Sphere* sphereB, + b3Transform transformBtoA ) +{ + if ( capacity == 0 ) + { + return; + } + + // Work in shapeB coordinates + b3Vec3 center1 = sphereA->center; + b3Vec3 center2 = b3TransformPoint( transformBtoA, sphereB->center ); + + float totalRadius = sphereA->radius + sphereB->radius; + b3Vec3 offset = b3Sub( center2, center1 ); + float distanceSq = b3LengthSquared( offset ); + + if ( distanceSq > totalRadius * totalRadius ) + { + // We found a separating axis + return; + } + + b3Vec3 normal = { 0.0f, 1.0f, 0.0f }; + float distance = sqrtf( distanceSq ); + if ( distance * distance > 1000.0f * FLT_MIN ) + { + normal = b3MulSV( 1.0f / distance, offset ); + } + + // Contact at the midpoint + // 0.5 * ( ((c1 + rA*n) + c2) - rB*n ) + b3Vec3 point = + b3MulSV( 0.5f, b3MulSub( b3Add( b3MulAdd( center1, sphereA->radius, normal ), center2 ), sphereB->radius, normal ) ); + + // Manifold in frame B + manifold->normal = normal; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = distance - totalRadius; + pt->pair = b3FeaturePair_single; +} + +void b3CollideCapsuleAndSphere( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA, const b3Sphere* sphereB, + b3Transform transformBtoA ) +{ + manifold->pointCount = 0; + + if ( capacity < 1 ) + { + return; + } + + // Work in shape B coordinates + b3Vec3 center = b3TransformPoint( transformBtoA, sphereB->center ); + b3Vec3 center1 = capsuleA->center1; + b3Vec3 center2 = capsuleA->center2; + + float totalRadius = sphereB->radius + capsuleA->radius; + + b3Vec3 closestPoint = b3ClosestPointOnSegment( center1, center2, center ); + b3Vec3 offset = b3Sub( center, closestPoint ); + float distanceSq = b3LengthSquared( offset ); + + if ( distanceSq > totalRadius * totalRadius ) + { + // We found a separating axis + return; + } + + b3Vec3 normal = { 0.0f, 1.0f, 0.0f }; + float distance = sqrtf( distanceSq ); + if ( distance * distance > 1000.0f * FLT_MIN ) + { + normal = b3MulSV( 1.0f / distance, offset ); + } + + // Contact at the midpoint + // 0.5 * (((center - sB*n) + closestPoint) + cA*n) + b3Vec3 point = + b3MulSV( 0.5f, b3MulAdd( b3Add( b3MulSub( center, sphereB->radius, normal ), closestPoint ), capsuleA->radius, normal ) ); + + // Manifold in frame B + manifold->normal = normal; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = distance - totalRadius; + pt->pair = b3FeaturePair_single; +} + +void b3CollideHullAndSphere( b3LocalManifold* manifold, int capacity, const b3Hull* hullA, const b3Sphere* sphereB, + b3Transform transformBtoA, b3SimplexCache* cache ) +{ + manifold->pointCount = 0; + + if ( capacity == 0 ) + { + return; + } + + b3Vec3 center = b3TransformPoint( transformBtoA, sphereB->center ); + + const float speculativeDistance = B3_SPECULATIVE_DISTANCE; + + // Work in shapeA coordinates + + b3DistanceInput distanceInput; + distanceInput.proxyA = (b3ShapeProxy){ b3GetHullPoints( hullA ), hullA->vertexCount, 0.0f }; + distanceInput.proxyB = (b3ShapeProxy){ ¢er, 1, 0.0f }; + distanceInput.transformA = b3Transform_identity; + distanceInput.transformB = b3Transform_identity; + distanceInput.useRadii = false; + + float radiusA = 0.0f; + float radiusB = sphereB->radius; + float radius = radiusA + radiusB; + + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, cache, NULL, 0 ); + + if ( distanceOutput.distance > radius + speculativeDistance ) + { + // We found a separating axis + *cache = (b3SimplexCache){ 0 }; + return; + } + + if ( distanceOutput.distance > 100.0f * FLT_EPSILON ) + { + // Shallow penetration + b3Vec3 normal = b3Normalize( b3Sub( distanceOutput.pointB, distanceOutput.pointA ) ); + + // cA is the projection of the sphere center onto to the hull (pointA if radiusA == 0). + b3Vec3 cA = b3MulAdd( center, radiusA - b3Dot( b3Sub( center, distanceOutput.pointA ), normal ), normal ); + + // cB is the deepest point on the sphere with respect to the reference f + b3Vec3 cB = b3MulSub( center, radiusB, normal ); + + b3Vec3 point = b3Lerp( cA, cB, 0.5f ); + + // Manifold in frame A + manifold->normal = normal; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = distanceOutput.distance - radius; + pt->pair = b3FeaturePair_single; + } + else + { + // Deep penetration + int bestIndex = -1; + float bestDistance = -FLT_MAX; + const b3Plane* planes = b3GetHullPlanes( hullA ); + + for ( int index = 0; index < hullA->faceCount; ++index ) + { + b3Plane plane = planes[index]; + + float distance = b3PlaneSeparation( plane, center ); + if ( distance > bestDistance ) + { + bestIndex = index; + bestDistance = distance; + } + } + B3_ASSERT( bestIndex >= 0 ); + + b3Vec3 normal = planes[bestIndex].normal; + + // cA is the projection of the sphere center onto to the hull + b3Vec3 cA = b3MulAdd( center, radiusA - b3Dot( b3Sub( center, distanceOutput.pointA ), normal ), normal ); + + // cB is the deepest point on the sphere with respect to the reference f + b3Vec3 cB = b3MulSub( center, radiusB, normal ); + + b3Vec3 point = b3Lerp( cA, cB, 0.5f ); + + // Manifold in frame A + manifold->normal = normal; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = bestDistance - radius; + pt->pair = b3FeaturePair_single; + } +} + +void b3CollideCapsules( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA, const b3Capsule* capsuleB, + b3Transform transformBtoA ) +{ + manifold->pointCount = 0; + + if ( capacity < 2 ) + { + return; + } + + // Work in shapeA coordinates + b3Vec3 centerA1 = capsuleA->center1; + b3Vec3 centerA2 = capsuleA->center2; + b3Vec3 centerB1 = b3TransformPoint( transformBtoA, capsuleB->center1 ); + b3Vec3 centerB2 = b3TransformPoint( transformBtoA, capsuleB->center2 ); + + float radius = capsuleA->radius + capsuleB->radius; + float maxDistance = radius + B3_SPECULATIVE_DISTANCE; + + b3ClosestApproachResult result = b3ClosestApproachSegments( centerA1, centerA2, centerB1, centerB2 ); + b3Vec3 offset = b3Sub( result.point2, result.point1 ); + float distanceSquared = b3LengthSquared( offset ); + float linearSlop = B3_LINEAR_SLOP; + float minDistance = 0.01f * linearSlop; + + if ( distanceSquared > maxDistance * maxDistance || distanceSquared < minDistance * minDistance ) + { + // We found a separating axis + return; + } + + float lengthA; + b3Vec3 segmentA = b3Sub( centerA2, centerA1 ); + b3Vec3 edgeA = b3GetLengthAndNormalize( &lengthA, segmentA ); + if ( lengthA < B3_MIN_CAPSULE_LENGTH ) + { + return; + } + + float lengthB; + b3Vec3 segmentB = b3Sub( centerB2, centerB1 ); + b3Vec3 edgeB = b3GetLengthAndNormalize( &lengthB, segmentB ); + if ( lengthB < B3_MIN_CAPSULE_LENGTH ) + { + return; + } + + // Parallel edges: |eA x eB| = sin(alpha) + const float alphaTol = 0.05f; + const float alphaTolSqr = alphaTol * alphaTol; + b3Vec3 axis = b3Cross( edgeA, edgeB ); + + // Try to create two contact points if the capsules are nearly parallel + if ( b3LengthSquared( axis ) < alphaTolSqr ) + { + // Clip segment B against side planes of segment A + + // Sides planes of A + b3Plane planesA[2]; + planesA[0].normal = b3Neg( edgeA ); + planesA[0].offset = -b3Dot( edgeA, capsuleA->center1 ); + planesA[1].normal = edgeA; + planesA[1].offset = b3Dot( edgeA, capsuleA->center2 ); + + // Clip points for B + b3ClipVertex verticesB[2]; + verticesB[0].position = centerB1; + verticesB[0].separation = 0.0f; + verticesB[0].pair = b3MakeFeaturePair( b3_featureShapeA, 0, b3_featureShapeA, 0 ); + verticesB[1].position = centerB2; + verticesB[1].separation = 0.0f; + verticesB[1].pair = b3MakeFeaturePair( b3_featureShapeA, 1, b3_featureShapeA, 1 ); + + int pointCount = b3ClipSegment( verticesB, planesA[0] ); + if ( pointCount == 2 ) + { + pointCount = b3ClipSegment( verticesB, planesA[1] ); + } + + if ( pointCount == 2 ) + { + // Closest points on A to the clipped points on B. + b3Vec3 closestPoint1 = b3ClosestPointOnSegment( centerA1, centerA2, verticesB[0].position ); + b3Vec3 closestPoint2 = b3ClosestPointOnSegment( centerA1, centerA2, verticesB[1].position ); + + float distance1 = b3Distance( closestPoint1, verticesB[0].position ); + float distance2 = b3Distance( closestPoint2, verticesB[1].position ); + if ( distance1 <= radius && distance2 <= radius ) + { + if ( distance1 < minDistance || distance2 < minDistance ) + { + // Avoid divide by zero + return; + } + + b3Vec3 normal1 = b3MulSV( 1.0f / distance1, b3Sub( closestPoint1, verticesB[0].position ) ); + b3Vec3 normal2 = b3MulSV( 1.0f / distance2, b3Sub( closestPoint2, verticesB[1].position ) ); + b3Vec3 normal = b3Normalize( b3Add( normal1, normal2 ) ); + float radiusA = capsuleA->radius; + float radiusB = capsuleB->radius; + + // Contact is at the midpoint: 0.5 * (((vB.pos + rA*nK) + cP) - rB*n) + b3Vec3 point1 = + b3MulSV( 0.5f, b3MulSub( b3Add( b3MulAdd( verticesB[0].position, radiusA, normal1 ), closestPoint1 ), radiusB, + normal ) ); + b3Vec3 point2 = + b3MulSV( 0.5f, b3MulSub( b3Add( b3MulAdd( verticesB[1].position, radiusA, normal2 ), closestPoint2 ), radiusB, + normal ) ); + + // Manifold in frame A + manifold->normal = normal; + manifold->pointCount = 2; + + b3LocalManifoldPoint* pt1 = manifold->points + 0; + pt1->point = point1; + pt1->separation = distance1 - radius; + pt1->pair = verticesB[0].pair; + + b3LocalManifoldPoint* pt2 = manifold->points + 1; + pt2->point = point2; + pt2->separation = distance2 - radius; + pt2->pair = verticesB[1].pair; + + return; + } + } + } + + float distance; + b3Vec3 normal = b3GetLengthAndNormalize( &distance, offset ); + // Contact at the midpoint 0.5 * (((p1 + rA*n) + p2) - rB*n) + b3Vec3 point = b3MulSV( + 0.5f, b3MulSub( b3Add( b3MulAdd( result.point1, capsuleA->radius, normal ), result.point2 ), capsuleB->radius, normal ) ); + + // Manifold in frame A + manifold->normal = normal; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = distance - radius; + pt->pair = b3FeaturePair_single; +} + +static bool b3BuildHullFaceAndCapsuleContact( b3LocalManifold* manifold, const b3Hull* hullA, const b3Capsule* capsuleB, + b3Transform transformBtoA, b3FaceQuery query ) +{ + // Work in shapeA coordinates + const b3Plane* planes = b3GetHullPlanes( hullA ); + + // Clip the capsule edge against the side planes of the reference face + int refFace = query.faceIndex; + b3Plane refPlane = planes[refFace]; + + b3ClipVertex segmentB[2]; + segmentB[0].position = b3TransformPoint( transformBtoA, capsuleB->center1 ); + segmentB[0].separation = 0.0f; + segmentB[0].pair = b3MakeFeaturePair( b3_featureShapeA, 0, b3_featureShapeA, 0 ); + segmentB[1].position = b3TransformPoint( transformBtoA, capsuleB->center2 ); + segmentB[1].separation = 0.0f; + segmentB[1].pair = b3MakeFeaturePair( b3_featureShapeA, 1, b3_featureShapeA, 1 ); + + int pointCount = b3ClipSegmentToHullFace( segmentB, hullA, refFace ); + if ( pointCount < 2 ) + { + return false; + } + + float distance1 = b3PlaneSeparation( refPlane, segmentB[0].position ); + float distance2 = b3PlaneSeparation( refPlane, segmentB[1].position ); + const float speculativeDistance = B3_SPECULATIVE_DISTANCE; + + if ( distance1 <= speculativeDistance || distance2 <= speculativeDistance ) + { + b3Vec3 normal = refPlane.normal; + b3Vec3 point1 = b3MulSub( segmentB[0].position, 0.5f * ( distance1 + capsuleB->radius ), normal ); + b3Vec3 point2 = b3MulSub( segmentB[1].position, 0.5f * ( distance2 + capsuleB->radius ), normal ); + + // Manifold in frame A + manifold->normal = normal; + manifold->pointCount = 2; + + b3LocalManifoldPoint* pt1 = manifold->points + 0; + pt1->point = point1; + pt1->separation = distance1 - capsuleB->radius; + pt1->pair = segmentB[0].pair; + + b3LocalManifoldPoint* pt2 = manifold->points + 1; + pt2->point = point2; + pt2->separation = distance2 - capsuleB->radius; + pt2->pair = segmentB[1].pair; + + return true; + } + + return false; +} + +static inline float b3DeepestPointSeparation( const b3LocalManifold* manifold ) +{ + // Deepest point + float minSeparation = FLT_MAX; + int pointCount = manifold->pointCount; + for ( int i = 0; i < pointCount; ++i ) + { + minSeparation = b3MinFloat( minSeparation, manifold->points[i].separation ); + } + + return minSeparation; +} + +static bool b3BuildHullAndCapsuleEdgeContact( b3LocalManifold* manifold, int capacity, const b3Hull* hullA, + const b3Capsule* capsuleB, b3Transform transformBtoA, b3EdgeQuery query ) +{ + if ( capacity < 1 ) + { + return false; + } + + // Work in shapeA coordinates + + b3Vec3 pc = b3TransformPoint( transformBtoA, capsuleB->center1 ); + b3Vec3 qc = b3TransformPoint( transformBtoA, capsuleB->center2 ); + b3Vec3 ec = b3Sub( qc, pc ); + + const b3HullHalfEdge* edges = b3GetHullEdges( hullA ); + const b3Vec3* points = b3GetHullPoints( hullA ); + + const b3HullHalfEdge* edge2 = edges + query.indexB; + const b3HullHalfEdge* twin2 = edges + edge2->twin; + b3Vec3 ch = hullA->center; + b3Vec3 ph = points[edge2->origin]; + b3Vec3 qh = points[twin2->origin]; + b3Vec3 eh = b3Sub( qh, ph ); + + b3Vec3 normal = b3Cross( ec, eh ); + normal = b3Normalize( normal ); + + // Normal should point outward from hull + if ( b3Dot( normal, b3Sub( ph, ch ) ) < 0.0f ) + { + normal = b3Neg( normal ); + } + + b3ClosestApproachResult result = b3ClosestApproachLines( ph, eh, pc, ec ); + + if ( b3IsWithinSegments( &result ) == false ) + { + // closest point beyond end points + return false; + } + + b3Vec3 point = b3MulSV( 0.5f, b3Add( b3MulSub( result.point1, capsuleB->radius, normal ), result.point2 ) ); + + float separation = b3Dot( normal, b3Sub( result.point2, result.point1 ) ); + B3_VALIDATE( b3AbsFloat( separation - query.separation ) < B3_LINEAR_SLOP ); + + // Manifold in frame A + manifold->normal = normal; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = separation - capsuleB->radius; + pt->pair = b3MakeFeaturePair( b3_featureShapeA, query.indexA, b3_featureShapeB, query.indexB ); + return true; +} + +void b3CollideHullAndCapsule( b3LocalManifold* manifold, int capacity, const b3Hull* hullA, const b3Capsule* capsuleB, + b3Transform transformBtoA, b3SimplexCache* cache ) +{ + manifold->pointCount = 0; + + if ( capacity < 2 ) + { + return; + } + + // Work in shapeA coordinates + b3DistanceInput distanceInput; + distanceInput.proxyA = (b3ShapeProxy){ b3GetHullPoints( hullA ), hullA->vertexCount, 0.0f }; + distanceInput.proxyB = (b3ShapeProxy){ &capsuleB->center1, 2, 0.0f }; + distanceInput.transformA = b3Transform_identity; + distanceInput.transformB = transformBtoA; + distanceInput.useRadii = false; + + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, cache, NULL, 0 ); + const float speculativeDistance = B3_SPECULATIVE_DISTANCE; + + if ( distanceOutput.distance > capsuleB->radius + speculativeDistance ) + { + // We found a separating axis + *cache = (b3SimplexCache){ 0 }; + return; + } + + if ( distanceOutput.distance > 100.0f * FLT_EPSILON ) + { + const b3Plane* planes = b3GetHullPlanes( hullA ); + + // Shallow penetration + b3Vec3 delta = distanceOutput.normal; + int refFace = b3FindHullSupportFace( hullA, delta ); + b3Plane refPlane = planes[refFace]; + + // Try to create two contact points if closest + // points difference is nearly parallel to face normal + const float kTolerance = 0.998f; + if ( b3AbsFloat( b3Dot( refPlane.normal, delta ) ) > kTolerance ) + { + // Clip capsule segment against side planes of reference face + b3ClipVertex verticesB[2]; + verticesB[0].position = b3TransformPoint( transformBtoA, capsuleB->center1 ); + verticesB[0].separation = 0.0f; + verticesB[0].pair = b3MakeFeaturePair( b3_featureShapeA, 0, b3_featureShapeA, 0 ); + verticesB[1].position = b3TransformPoint( transformBtoA, capsuleB->center2 ); + verticesB[1].separation = 0.0f; + verticesB[1].pair = b3MakeFeaturePair( b3_featureShapeA, 1, b3_featureShapeA, 1 ); + + int pointCount = b3ClipSegmentToHullFace( verticesB, hullA, refFace ); + + if ( pointCount == 2 ) + { + float distance1 = b3PlaneSeparation( refPlane, verticesB[0].position ); + float distance2 = b3PlaneSeparation( refPlane, verticesB[1].position ); + if ( distance1 <= capsuleB->radius + speculativeDistance || distance2 <= capsuleB->radius + speculativeDistance ) + { + b3Vec3 normal = refPlane.normal; + b3Vec3 point1 = b3MulSub( verticesB[0].position, 0.5f * ( capsuleB->radius + distance1 ), normal ); + b3Vec3 point2 = b3MulSub( verticesB[1].position, 0.5f * ( capsuleB->radius + distance2 ), normal ); + + // Manifold in frame A + manifold->normal = normal; + manifold->pointCount = 2; + + b3LocalManifoldPoint* pt1 = manifold->points + 0; + pt1->point = point1; + pt1->separation = distance1 - capsuleB->radius; + pt1->pair = verticesB[0].pair; + + b3LocalManifoldPoint* pt2 = manifold->points + 1; + pt2->point = point2; + pt2->separation = distance2 - capsuleB->radius; + pt2->pair = verticesB[1].pair; + + return; + } + } + } + + // Create contact from closest points + b3Vec3 point = + b3MulSV( 0.5f, b3Add( b3MulSub( distanceOutput.pointA, capsuleB->radius, delta ), distanceOutput.pointB ) ); + + // Manifold in frame A + manifold->normal = delta; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = distanceOutput.distance - capsuleB->radius; + pt->pair = b3FeaturePair_single; + return; + } + + // Deep penetration + + b3FaceQuery faceQuery = b3QueryFaceDirectionHullAndCapsule( hullA, capsuleB, transformBtoA ); + if ( faceQuery.separation > capsuleB->radius ) + { + // We found a separating axis + return; + } + + b3EdgeQuery edgeQuery = b3QueryEdgeDirectionHullAndCapsule( hullA, capsuleB, transformBtoA ); + if ( edgeQuery.separation > capsuleB->radius ) + { + // We found a separating axis + return; + } + + // Create face contact + float faceSeparation = faceQuery.separation - capsuleB->radius; + b3BuildHullFaceAndCapsuleContact( manifold, hullA, capsuleB, transformBtoA, faceQuery ); + if ( manifold->pointCount > 1 ) + { + // If ( Out.PointCount <= 1 ) -> Compare with unclipped separation + // If ( Out.PointCount > 1 ) -> Be aggressive and compare with clipped separation + // Face contact can be empty if it does not realize the axis of minimum penetration + faceSeparation = b3DeepestPointSeparation( manifold ); + } + B3_VALIDATE( faceSeparation <= 0.0f ); + + // Face contact can be empty if it does not realize the axis of minimum penetration. + // Create edge contact if face contact fails or edge contact is significantly better! + const float kRelEdgeTolerance = 0.90f; + const float kAbsTolerance = 0.5f * B3_LINEAR_SLOP; + float edgeSeparation = edgeQuery.separation - capsuleB->radius; + if ( manifold->pointCount == 0 || edgeSeparation > kRelEdgeTolerance * faceSeparation + kAbsTolerance ) + { + // Edge contact + b3BuildHullAndCapsuleEdgeContact( manifold, capacity, hullA, capsuleB, transformBtoA, edgeQuery ); + } +} + +static int b3BuildPolygon( b3ClipVertex* out, b3Transform transform, const b3Hull* hull, int incFace, b3Plane refPlane ) +{ + const b3HullFace* faces = b3GetHullFaces( hull ); + const b3HullHalfEdge* edges = b3GetHullEdges( hull ); + const b3Vec3* points = b3GetHullPoints( hull ); + + const b3HullFace* face = faces + incFace; + int edgeIndex = face->edge; + B3_ASSERT( edges[edgeIndex].face == incFace ); + + int outCount = 0; + + b3Matrix3 matrix = b3MakeMatrixFromQuat( transform.q ); + + do + { + const b3HullHalfEdge* edge = edges + edgeIndex; + + int nextEdgeIndex = edge->next; + const b3HullHalfEdge* next = edges + nextEdgeIndex; + + b3ClipVertex vertex; + vertex.position = b3Add( b3MulMV( matrix, points[next->origin] ), transform.p ); + vertex.separation = b3PlaneSeparation( refPlane, vertex.position ); + vertex.pair = b3MakeFeaturePair( b3_featureShapeB, edgeIndex, b3_featureShapeB, nextEdgeIndex ); + + out[outCount] = vertex; + outCount += 1; + + edgeIndex = nextEdgeIndex; + } + while ( edgeIndex != face->edge && outCount < B3_MAX_CLIP_POINTS ); + + B3_VALIDATE( b3ValidatePolygon( out, outCount ) ); + + return outCount; +} + +static bool b3BuildFaceAContact( b3LocalManifold* manifold, int capacity, const b3Hull* hullA, const b3Hull* hullB, + b3Transform transformBtoA, b3FaceQuery query, b3SATCache* cache ) +{ + const b3HullFace* facesA = b3GetHullFaces( hullA ); + const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA ); + const b3Plane* planesA = b3GetHullPlanes( hullA ); + const b3Vec3* pointsA = b3GetHullPoints( hullA ); + + // Reference face + int refFace = query.faceIndex; + b3Plane refPlane = planesA[refFace]; + + // Find incident face + b3Vec3 refNormalInB = b3InvRotateVector( transformBtoA.q, refPlane.normal ); + int incFace = b3FindIncidentFace( hullB, refNormalInB, query.vertexIndex ); + + // Build clip polygon from incident face in frame A + b3ClipVertex buffer1[B3_MAX_CLIP_POINTS], buffer2[B3_MAX_CLIP_POINTS]; + int pointCount = b3BuildPolygon( buffer1, transformBtoA, hullB, incFace, refPlane ); + + // Clip incident face against side planes of reference face + b3ClipVertex* input = buffer1; + b3ClipVertex* output = buffer2; + + const b3HullFace* face = facesA + refFace; + int edgeIndex = face->edge; + + do + { + const b3HullHalfEdge* edge = edgesA + edgeIndex; + int nextEdgeIndex = edge->next; + const b3HullHalfEdge* next = edgesA + nextEdgeIndex; + b3Vec3 vertex1 = pointsA[edge->origin]; + b3Vec3 vertex2 = pointsA[next->origin]; + b3Vec3 tangent = b3Normalize( b3Sub( vertex2, vertex1 ) ); + b3Vec3 binormal = b3Cross( tangent, refPlane.normal ); + + b3Plane clipPlane = b3MakePlaneFromNormalAndPoint( binormal, vertex1 ); + + pointCount = b3ClipPolygon( output, input, pointCount, clipPlane, edgeIndex, refPlane ); + B3_ASSERT( pointCount <= B3_MAX_CLIP_POINTS ); + + B3_SWAP( output, input ); + + if ( pointCount < 3 ) + { + *cache = (b3SATCache){ 0 }; + return false; + } + + edgeIndex = nextEdgeIndex; + } + while ( edgeIndex != face->edge ); + + pointCount = b3MinInt( pointCount, B3_MAX_CLIP_POINTS ); + + b3LocalManifoldPoint points[B3_MAX_CLIP_POINTS]; + float minSeparation = FLT_MAX; + + manifold->normal = refPlane.normal; + + for ( int i = 0; i < pointCount; ++i ) + { + b3ClipVertex* clipPoint = input + i; + b3LocalManifoldPoint* pt = points + i; + *pt = (b3LocalManifoldPoint){ 0 }; + + // Using the half-way point keeps the points in the same position when swapping reference face from A to B. + b3Vec3 point = b3MulSub( clipPoint->position, 0.5f * clipPoint->separation, refPlane.normal ); + + // Old way of pushing onto the reference face. + // b3Vec3 point = clipPoint->position - clipPoint->separation * refPlane.normal; + + pt->normal = manifold->normal; + pt->point = point; + pt->separation = clipPoint->separation; + pt->pair = clipPoint->pair; + + minSeparation = b3MinFloat( minSeparation, clipPoint->separation ); + } + + if ( minSeparation >= B3_SPECULATIVE_DISTANCE ) + { + *cache = (b3SATCache){ 0 }; + return false; + } + + b3ReduceManifoldPoints( manifold, capacity, points, pointCount ); + + // Save cache + cache->separation = minSeparation; + cache->type = (uint8_t)b3_faceAxisA; + cache->indexA = (uint8_t)query.faceIndex; + cache->indexB = (uint8_t)query.vertexIndex; + + return true; +} + +static bool b3BuildFaceBContact( b3LocalManifold* manifold, int capacity, const b3Hull* hullA, const b3Hull* hullB, + b3Transform transformBtoA, b3FaceQuery query, b3SATCache* cache ) +{ + b3Transform transformAtoB = b3InvertTransform( transformBtoA ); + bool touching = b3BuildFaceAContact( manifold, capacity, hullB, hullA, transformAtoB, query, cache ); + if ( touching == false ) + { + return false; + } + + // Results are in frame B, need to transform them into frame A + b3Matrix3 matrix = b3MakeMatrixFromQuat( transformBtoA.q ); + + // Transform and flip normal so it points from A to B, even though the B has the reference face. + manifold->normal = b3Neg( b3MulMV( matrix, manifold->normal ) ); + cache->type = (uint8_t)b3_faceAxisB; + cache->indexA = (uint8_t)query.vertexIndex; + cache->indexB = (uint8_t)query.faceIndex; + + // Transform points from frame B to frame A. + // Also flip the pairs to ensure correct matches. + for ( int i = 0; i < manifold->pointCount; ++i ) + { + b3LocalManifoldPoint* pt = manifold->points + i; + pt->point = b3Add( b3MulMV( matrix, pt->point ), transformBtoA.p ); + pt->pair = b3FlipPair( pt->pair ); + } + + return true; +} + +static bool b3BuildEdgeContact( b3LocalManifold* manifold, const b3Hull* hullA, const b3Hull* hullB, b3Transform transformBtoA, + b3EdgeQuery query, b3SATCache* cache ) +{ + // Work in shapeA coordinates + const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA ); + const b3Vec3* pointsA = b3GetHullPoints( hullA ); + + const b3HullHalfEdge* edgesB = b3GetHullEdges( hullB ); + const b3Vec3* pointsB = b3GetHullPoints( hullB ); + + // B3_VALIDATE( query.separation <= 2.0f * B3_SPECULATIVE_DISTANCE ); + + const b3HullHalfEdge* edgeA = edgesA + query.indexA; + const b3HullHalfEdge* twinA = edgesA + edgeA->twin; + b3Vec3 centerA = hullA->center; + b3Vec3 pA = pointsA[edgeA->origin]; + b3Vec3 qA = pointsA[twinA->origin]; + b3Vec3 eA = b3Sub( qA, pA ); + + const b3HullHalfEdge* edgeB = edgesB + query.indexB; + const b3HullHalfEdge* twinB = edgesB + edgeB->twin; + b3Vec3 pB = b3TransformPoint( transformBtoA, pointsB[edgeB->origin] ); + b3Vec3 qB = b3TransformPoint( transformBtoA, pointsB[twinB->origin] ); + b3Vec3 eB = b3Sub( qB, pB ); + + b3Vec3 normal = b3Cross( eA, eB ); + normal = b3Normalize( normal ); + + if ( b3Dot( normal, b3Sub( pA, centerA ) ) < 0.0f ) + { + normal = b3Neg( normal ); + } + + b3ClosestApproachResult result = b3ClosestApproachLines( pA, eA, pB, eB ); + + if ( b3IsWithinSegments( &result ) == false ) + { + *cache = (b3SATCache){ 0 }; + return false; + } + + // This can slide off the end from caching + float separation = b3Dot( normal, b3Sub( result.point2, result.point1 ) ); + + // todo I suspect this could trip if the cache becomes invalid + // B3_VALIDATE( b3AbsFloat( separation - query.separation ) < B3_LINEAR_SLOP ); + + b3Vec3 point = b3MulSV( 0.5f, b3Add( result.point1, result.point2 ) ); + + // Result in frame A + manifold->normal = normal; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = separation; + pt->pair = b3MakeFeaturePair( b3_featureShapeA, query.indexA, b3_featureShapeB, query.indexB ); + + // Save cache + cache->separation = separation; + cache->type = (uint8_t)b3_edgePairAxis; + cache->indexA = (uint8_t)query.indexA; + cache->indexB = (uint8_t)query.indexB; + + return true; +} + +void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3Hull* hullA, const b3Hull* hullB, b3Transform transformBtoA, + b3SATCache* cache ) +{ + manifold->pointCount = 0; + + if ( capacity < 4 ) + { + return; + } + + // Work in shapeA coordinates + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + + float linearSlop = B3_LINEAR_SLOP; + const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA ); + const b3Plane* planesA = b3GetHullPlanes( hullA ); + const b3Vec3* pointsA = b3GetHullPoints( hullA ); + + const b3HullHalfEdge* edgesB = b3GetHullEdges( hullB ); + const b3Plane* planesB = b3GetHullPlanes( hullB ); + const b3Vec3* pointsB = b3GetHullPoints( hullB ); + + // Attempt to use the cache to speed up collision + switch ( cache->type ) + { + case b3_invalidAxis: + *cache = (b3SATCache){ 0 }; + break; + + case b3_faceAxisA: + { + B3_ASSERT( cache->indexA < hullA->faceCount ); + + // Check for separation using cached face + b3Plane plane = planesA[cache->indexA]; + b3Vec3 searchDirectionInB = b3Neg( b3InvRotateVector( transformBtoA.q, plane.normal ) ); + int vertexIndex = b3FindHullSupportVertex( hullB, searchDirectionInB ); + b3Vec3 support = b3TransformPoint( transformBtoA, pointsB[vertexIndex] ); + float separation = b3PlaneSeparation( plane, support ); + + if ( separation >= speculativeDistance ) + { + // Cache hit, shapes are separated + return; + } + + // if ( cache->separation < speculativeDistance ) + { + // Attempt face contact using cached feature + b3FaceQuery faceQuery; + faceQuery.separation = 0.0f; + faceQuery.faceIndex = cache->indexA; + faceQuery.vertexIndex = vertexIndex; + + b3SATCache localCache = { 0 }; + bool touching = b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, &localCache ); + if ( touching == true && b3AbsFloat( cache->separation - localCache.separation ) < linearSlop ) + { + // Cache hit, contact points generated + return; + } + } + } + break; + + case b3_faceAxisB: + { + B3_ASSERT( cache->indexB < hullB->faceCount ); + + // Check for separation using cached face + b3Plane plane = planesB[cache->indexB]; + b3Vec3 searchDirectionInA = b3Neg( b3RotateVector( transformBtoA.q, plane.normal ) ); + int vertexIndex = b3FindHullSupportVertex( hullA, searchDirectionInA ); + b3Vec3 support = b3InvTransformPoint( transformBtoA, pointsA[vertexIndex] ); + float separation = b3PlaneSeparation( plane, support ); + + if ( separation >= speculativeDistance ) + { + // Cache hit, shapes are separated + return; + } + + // if ( cache->separation < speculativeDistance ) + { + // Attempt face contact using cached feature + b3FaceQuery faceQuery; + faceQuery.separation = 0.0f; + faceQuery.faceIndex = cache->indexB; + faceQuery.vertexIndex = vertexIndex; + + b3SATCache localCache = { 0 }; + bool touching = b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, &localCache ); + if ( touching == true && b3AbsFloat( cache->separation - localCache.separation ) < linearSlop ) + { + // Cache hit, contact points generated + return; + } + } + } + break; + + case b3_edgePairAxis: + { + int index1 = cache->indexA; + const b3HullHalfEdge* edge1 = edgesA + index1; + const b3HullHalfEdge* twin1 = edgesA + index1 + 1; + B3_ASSERT( edge1->twin == index1 + 1 && twin1->twin == index1 ); + + b3Vec3 p1 = pointsA[edge1->origin]; + b3Vec3 q1 = pointsA[twin1->origin]; + b3Vec3 e1 = b3Sub( q1, p1 ); + + b3Vec3 u1 = planesA[edge1->face].normal; + b3Vec3 v1 = planesA[twin1->face].normal; + + int index2 = cache->indexB; + const b3HullHalfEdge* edge2 = edgesB + index2; + const b3HullHalfEdge* twin2 = edgesB + index2 + 1; + B3_ASSERT( edge2->twin == index2 + 1 && twin2->twin == index2 ); + + b3Vec3 p2 = b3TransformPoint( transformBtoA, pointsB[edge2->origin] ); + b3Vec3 q2 = b3TransformPoint( transformBtoA, pointsB[twin2->origin] ); + b3Vec3 e2 = b3Sub( q2, p2 ); + + b3Vec3 u2 = b3RotateVector( transformBtoA.q, planesB[edge2->face].normal ); + b3Vec3 v2 = b3RotateVector( transformBtoA.q, planesB[twin2->face].normal ); + + // flipping the signs of u2 and v2 + // cross(v2, u2) == cross(-v2, -u2) + // so we still use -e2 + // but we can also use e1 = cross(u1, v1) and e2 = cross(u2, v2) + bool isMinkowski = b3IsMinkowskiFace( u1, v1, e1, b3Neg( u2 ), b3Neg( v2 ), e2 ); + if ( isMinkowski == true ) + { + // Transform reference center of the first hull into local space of the second hull + b3Vec3 c1 = hullA->center; + b3Vec3 c2 = b3TransformPoint( transformBtoA, hullB->center ); + + float separation = b3EdgeEdgeSeparation( p1, e1, c1, p2, e2, c2 ); + if ( separation > speculativeDistance ) + { + // Cache hit, shapes are separated + return; + } + + // if ( cache->separation <= speculativeDistance ) + { + // Try to rebuild contact from last features + b3EdgeQuery edgeQuery; + edgeQuery.indexA = cache->indexA; + edgeQuery.indexB = cache->indexB; + edgeQuery.separation = 0.0f; + + b3SATCache localCache = { 0 }; + bool touching = b3BuildEdgeContact( manifold, hullA, hullB, transformBtoA, edgeQuery, &localCache ); + if ( touching && b3AbsFloat( cache->separation - localCache.separation ) < linearSlop ) + { + // Cache hit, contact point generated + return; + } + } + } + } + break; + + // This case is for testing + case b3_manualFaceAxisA: + { + b3FaceQuery faceQueryA = b3QueryFaceDirections( hullA, hullB, transformBtoA ); + b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQueryA, cache ); + return; + } + + // This case is for testing + case b3_manualFaceAxisB: + { + b3FaceQuery faceQueryB = b3QueryFaceDirections( hullB, hullA, b3InvertTransform( transformBtoA ) ); + b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQueryB, cache ); + return; + } + + // This case is for testing + case b3_manualEdgePairAxis: + { + b3EdgeQuery edgeQuery = b3QueryEdgeDirections( hullA, hullB, transformBtoA ); + if ( edgeQuery.indexA != B3_NULL_INDEX ) + { + b3BuildEdgeContact( manifold, hullA, hullB, transformBtoA, edgeQuery, cache ); + } + return; + } + + default: + B3_ASSERT( false ); + break; + } + + manifold->pointCount = 0; + *cache = (b3SATCache){ 0 }; + + // Find axis of minimum penetration + b3FaceQuery faceQueryA = b3QueryFaceDirections( hullA, hullB, transformBtoA ); + if ( faceQueryA.separation > speculativeDistance ) + { + B3_ASSERT( faceQueryA.faceIndex < hullA->faceCount ); + B3_ASSERT( faceQueryA.vertexIndex < hullB->vertexCount ); + + // We found a separating axis + cache->separation = faceQueryA.separation; + cache->type = (uint8_t)b3_faceAxisA; + cache->indexA = (uint8_t)faceQueryA.faceIndex; + cache->indexB = (uint8_t)faceQueryA.vertexIndex; + return; + } + + b3FaceQuery faceQueryB = b3QueryFaceDirections( hullB, hullA, b3InvertTransform( transformBtoA ) ); + if ( faceQueryB.separation > speculativeDistance ) + { + B3_ASSERT( faceQueryB.faceIndex < hullB->faceCount ); + B3_ASSERT( faceQueryB.vertexIndex < hullA->vertexCount ); + + // We found a separating axis + cache->separation = faceQueryB.separation; + cache->type = (uint8_t)b3_faceAxisB; + cache->indexA = (uint8_t)faceQueryB.vertexIndex; + cache->indexB = (uint8_t)faceQueryB.faceIndex; + return; + } + + b3EdgeQuery edgeQuery = b3QueryEdgeDirections( hullA, hullB, transformBtoA ); + if ( edgeQuery.separation > speculativeDistance ) + { + // We found a separating axis + cache->separation = edgeQuery.separation; + cache->type = (uint8_t)b3_edgePairAxis; + cache->indexA = (uint8_t)edgeQuery.indexA; + cache->indexB = (uint8_t)edgeQuery.indexB; + return; + } + + // Always build a face contact (e.g. Jenga problem) + float faceSeparationA = faceQueryA.separation; + float faceSeparationB = faceQueryB.separation; + B3_VALIDATE( faceSeparationA <= speculativeDistance && faceSeparationB <= speculativeDistance ); + + if ( faceSeparationB > faceSeparationA + 0.5f * linearSlop ) + { + // Face contact B + b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQueryB, cache ); + } + else + { + // Face contact A + b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQueryA, cache ); + } + + if ( edgeQuery.indexA == B3_NULL_INDEX ) + { + // There are no valid edge pairs (all edges parallel) + return; + } + + float clippedFaceSeparation = cache->separation; + + B3_VALIDATE( edgeQuery.separation <= speculativeDistance ); + + // todo get rid of relative tolerance + const float kRelEdgeTolerance = 0.90f; + // const float kRelFaceTolerance = 0.98f; + const float kAbsTolerance = 0.5f * linearSlop; + + // Face contact can be empty if it does not realize the axis of minimum penetration. + // Create edge contact if face contact fails or edge contact is significantly better! + if ( manifold->pointCount == 0 || edgeQuery.separation > kRelEdgeTolerance * clippedFaceSeparation + kAbsTolerance ) + { + // Edge contact + b3LocalManifold edgeManifold = { 0 }; + b3LocalManifoldPoint edgePoint = { 0 }; + edgeManifold.points = &edgePoint; + + b3BuildEdgeContact( &edgeManifold, hullA, hullB, transformBtoA, edgeQuery, cache ); + + // It is possible with speculation to have vertex-vertex collision that is missed by SAT. + // todo I doubt this backup scheme matters because I'm using the clipped face separation. + // B3_VALIDATE( edgeManifold.pointCount == 1 ); + + if ( edgeManifold.pointCount == 1 ) + { + // Copy edge manifold out, being careful to preserve manifold point buffer. + b3LocalManifoldPoint* points = manifold->points; + *manifold = edgeManifold; + manifold->points = points; + manifold->points[0] = edgePoint; + } + } +} diff --git a/src/core.c b/src/core.c new file mode 100644 index 0000000..63fd099 --- /dev/null +++ b/src/core.c @@ -0,0 +1,345 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#if defined( B3_COMPILER_MSVC ) +// CRTDBG requires these to be included first +#define _CRTDBG_MAP_ALLOC +#include +#include +#else +#include +#endif + +#include "core.h" + +#include "box3d/constants.h" +#include "box3d/math_functions.h" + +#include +#include + +#ifdef BOX2D_PROFILE + +#include +#define b3TracyCAlloc( ptr, size ) TracyCAlloc( ptr, size ) +#define b3TracyCFree( ptr ) TracyCFree( ptr ) + +#else + +#define b3TracyCAlloc( ptr, size ) +#define b3TracyCFree( ptr ) + +#endif + +#include "platform.h" + +#include + +// This allows the user to change the length units at runtime +float b3_lengthUnitsPerMeter = 1.0f; + +void b3SetLengthUnitsPerMeter( float lengthUnits ) +{ + B3_ASSERT( b3IsValidFloat( lengthUnits ) && lengthUnits > 0.0f ); + b3_lengthUnitsPerMeter = lengthUnits; +} + +float b3GetLengthUnitsPerMeter( void ) +{ + return b3_lengthUnitsPerMeter; +} + +static int b3DefaultAssertFcn( const char* condition, const char* fileName, int lineNumber ) +{ + printf( "BOX3D ASSERTION: %s, %s, line %d\n", condition, fileName, lineNumber ); + + // return non-zero to break to debugger + return 1; +} + +b3AssertFcn* b3AssertHandler = b3DefaultAssertFcn; + +void b3SetAssertFcn( b3AssertFcn* assertFcn ) +{ + B3_ASSERT( assertFcn != NULL ); + b3AssertHandler = assertFcn; +} + +#if !defined( NDEBUG ) || defined( B3_ENABLE_ASSERT ) +int b3InternalAssert( const char* condition, const char* fileName, int lineNumber ) +{ + int result = b3AssertHandler( condition, fileName, lineNumber ); + if ( result ) + { + B3_BREAKPOINT; + } + return result; +} +#endif + +static void b3DefaultLogFcn( const char* message ) +{ + printf( "Box3D: %s\n", message ); +} + +b3LogFcn* b3LogHandler = b3DefaultLogFcn; + +void b3SetLogFcn( b3LogFcn* logFcn ) +{ + B3_ASSERT( logFcn != NULL ); + b3LogHandler = logFcn; +} + +void b3Log( const char* format, ... ) +{ + va_list args; + va_start( args, format ); + char buffer[512]; + vsnprintf( buffer, sizeof( buffer ), format, args ); + b3LogHandler( buffer ); + va_end( args ); +} + +b3Version b3GetVersion( void ) +{ + return (b3Version){ 0, 2, 0 }; +} + +static b3AllocFcn* b3_allocFcn = NULL; +static b3FreeFcn* b3_freeFcn = NULL; + +b3AtomicInt b3_byteCount; + +void b3SetAllocator( b3AllocFcn* allocFcn, b3FreeFcn* freeFcn ) +{ + b3_allocFcn = allocFcn; + b3_freeFcn = freeFcn; +} + +// Use 64 byte alignment for everything to align to 64 byte cache line and works with 256bit SIMD. +#define B3_ALIGNMENT 64 + +void* b3Alloc( size_t size ) +{ + if ( size == 0 ) + { + return NULL; + } + + // This could cause some sharing issues, however Box3D rarely calls b3Alloc. + // todo this is not true, Box3D allocates a lot. + b3AtomicFetchAddInt( &b3_byteCount, (int)size ); + + // Allocation must be a multiple of 32 or risk a seg fault + // https://en.cppreference.com/w/c/memory/aligned_alloc + int size64 = ( ( (int)size - 1 ) | 0x3F ) + 1; + + if ( b3_allocFcn != NULL ) + { + void* ptr = b3_allocFcn( size64, B3_ALIGNMENT ); + b3TracyCAlloc( ptr, size ); + + B3_ASSERT( ptr != NULL ); + B3_ASSERT( ( (uintptr_t)ptr & 0x3F ) == 0 ); + + return ptr; + } + +#ifdef B3_PLATFORM_WINDOWS + void* ptr = _aligned_malloc( size64, B3_ALIGNMENT ); +#elif defined( B3_PLATFORM_ANDROID ) + void* ptr = NULL; + if ( posix_memalign( &ptr, B3_ALIGNMENT, size64 ) != 0 ) + { + // allocation failed, exit the application + exit( EXIT_FAILURE ); + } +#else + void* ptr = aligned_alloc( B3_ALIGNMENT, size64 ); +#endif + + b3TracyCAlloc( ptr, size ); + + B3_ASSERT( ptr != NULL ); + B3_ASSERT( ( (uintptr_t)ptr & 0x3F ) == 0 ); + + return ptr; +} + +void b3Free( void* mem, size_t size ) +{ + if ( mem == NULL ) + { + return; + } + + b3TracyCFree( mem ); + + if ( b3_freeFcn != NULL ) + { + b3_freeFcn( mem ); + } + else + { +#ifdef B3_PLATFORM_WINDOWS + _aligned_free( mem ); +#else + free( mem ); +#endif + } + + b3AtomicFetchAddInt( &b3_byteCount, -(int)size ); +} + +void* b3GrowAlloc( void* oldMem, int oldSize, int newSize ) +{ + // todo try _aligned_realloc + + B3_ASSERT( newSize > oldSize ); + void* newMem = b3Alloc( newSize ); + if ( oldSize > 0 ) + { + memcpy( newMem, oldMem, oldSize ); + b3Free( oldMem, oldSize ); + } + return newMem; +} + +int b3GetByteCount( void ) +{ + return b3AtomicLoadInt( &b3_byteCount ); +} + +void* b3AllocZeroed( size_t size ) +{ + void* mem = b3Alloc( size ); + memset( mem, 0, size ); + return mem; +} + +static FILE* b3OpenFile( const char* fileName, const char* mode ) +{ + FILE* file = NULL; + +#if defined( _MSC_VER ) + errno_t e = fopen_s( &file, fileName, mode ); + if ( e != 0 ) + { + return NULL; + } +#else + file = fopen( fileName, mode ); + if ( file == NULL ) + { + return NULL; + } +#endif + + return file; +} + +FILE* b3_dumpFile = NULL; +int b3_meshIndex = 0; + +void b3OpenDump( const char* fileName ) +{ + B3_ASSERT( b3_dumpFile == NULL ); + b3_dumpFile = b3OpenFile( fileName, "w" ); +} + +void b3Dump( const char* string, ... ) +{ + if ( b3_dumpFile == NULL ) + { + return; + } + + va_list args; + va_start( args, string ); + vfprintf( b3_dumpFile, string, args ); + va_end( args ); +} + +void b3CloseDump( void ) +{ + fclose( b3_dumpFile ); + b3_dumpFile = NULL; +} + +int b3FetchAddMeshDumpIndex( void ) +{ + int result = b3_meshIndex; + b3_meshIndex += 1; + return result; +} + +void b3WriteBinaryFile( void* data, int size, const char* fileName ) +{ + if ( data == NULL || size <= 0 || fileName == NULL ) + { + return; + } + + FILE* file = b3OpenFile( fileName, "wb" ); + if ( file == NULL ) + { + return; + } + + // Write binary blob; ignore partial-write errors for simplicity + (void)fwrite( data, 1, (size_t)size, file ); + fclose( file ); +} + +void* b3ReadBinaryFile( const char* prefix, const char* fileName, int* memSize ) +{ + *memSize = 0; + + if ( prefix == NULL || fileName == NULL ) + { + return NULL; + } + + char buffer[128]; + snprintf( buffer, sizeof( buffer ), "%s%s", prefix, fileName ); + + FILE* file = b3OpenFile( buffer, "rb" ); + if ( file == NULL ) + { + return NULL; + } + + // Determine file size + if ( fseek( file, 0, SEEK_END ) != 0 ) + { + fclose( file ); + return NULL; + } + + long size = ftell( file ); + if ( size <= 0 ) + { + fclose( file ); + return NULL; + } + + // Rewind + if ( fseek( file, 0, SEEK_SET ) != 0 ) + { + fclose( file ); + return NULL; + } + + void* data = b3Alloc( (size_t)size ); + size_t readCount = fread( data, 1, (size_t)size, file ); + fclose( file ); + + if ( readCount != (size_t)size ) + { + b3Free( data, (size_t)size ); + return NULL; + } + + *memSize = (int)size; + return data; +} diff --git a/src/core.h b/src/core.h new file mode 100644 index 0000000..a4d8599 --- /dev/null +++ b/src/core.h @@ -0,0 +1,166 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box3d/base.h" + +#include + +// clang-format off + +#ifdef NDEBUG + #define B3_DEBUG 0 +#else + #define B3_DEBUG 1 +#endif + +// Define platform +#if defined(_WIN32) || defined(_WIN64) + #define B3_PLATFORM_WINDOWS +#elif defined( __ANDROID__ ) + #define B3_PLATFORM_ANDROID +#elif defined( __linux__ ) + #define B3_PLATFORM_LINUX +#elif defined( __APPLE__ ) + #include + #if defined( TARGET_OS_IPHONE ) && !TARGET_OS_IPHONE + #define B3_PLATFORM_MACOS + #else + #define B3_PLATFORM_IOS + #endif +#elif defined( __EMSCRIPTEN__ ) + #define B3_PLATFORM_WASM +#else + #define B3_PLATFORM_UNKNOWN +#endif + +// Define CPU +#if defined( __x86_64__ ) || defined( _M_X64 ) || defined( __i386__ ) || defined( _M_IX86 ) + #define B3_CPU_X86_X64 +#elif defined( __aarch64__ ) || defined( _M_ARM64 ) || defined( __arm__ ) || defined( _M_ARM ) + #define B3_CPU_ARM +#elif defined( __EMSCRIPTEN__ ) + #define B3_CPU_WASM +#else + #define B3_CPU_UNKNOWN +#endif + +// Define SIMD +#if defined( BOX3D_DISABLE_SIMD ) + #define B3_SIMD_NONE + #define B3_SIMD_WIDTH 4 + //#pragma message("B3_SIMD_NONE") +#else + #if defined( B3_CPU_X86_X64 ) + #define B3_SIMD_SSE2 + #define B3_SIMD_WIDTH 4 + //#pragma message("B3_SIMD_SSE2") + #elif defined( B3_CPU_ARM ) + #define B3_SIMD_NEON + #define B3_SIMD_WIDTH 4 + //#pragma message("B3_SIMD_NEON") + #elif defined( B3_CPU_WASM ) + #define B3_CPU_WASM + #define B3_SIMD_SSE2 + #define B3_SIMD_WIDTH 4 + //#pragma message("B3_SIMD_SSE2") + #else + #define B3_SIMD_NONE + #define B3_SIMD_WIDTH 4 + //#pragma message("B3_SIMD_NONE") + #endif +#endif + +// Define compiler +#if defined( __clang__ ) + #define B3_COMPILER_CLANG +#elif defined( __GNUC__ ) + #define B3_COMPILER_GCC +#elif defined( _MSC_VER ) + #define B3_COMPILER_MSVC +#endif + +/// Tracy profiler instrumentation +/// https://github.com/wolfpld/tracy +#ifdef BOX3D_PROFILE + #include + #define b3TracyCZoneC( ctx, color, active ) TracyCZoneC( ctx, color, active ) + #define b3TracyCZoneNC( ctx, name, color, active ) TracyCZoneNC( ctx, name, color, active ) + #define b3TracyCZoneEnd( ctx ) TracyCZoneEnd( ctx ) + #define b3TracyCFrame TracyCFrameMark +#else + #define b3TracyCZoneC( ctx, color, active ) + #define b3TracyCZoneNC( ctx, name, color, active ) + #define b3TracyCZoneEnd( ctx ) + #define b3TracyCFrame +#endif + +// clang-format on + +typedef struct b3AtomicInt +{ + int value; +} b3AtomicInt; + +typedef struct b3AtomicU32 +{ + uint32_t value; +} b3AtomicU32; + +// Returns the number of elements of an array +#define B3_ARRAY_COUNT( A ) (int)( sizeof( A ) / sizeof( A[0] ) ) + +// Used to prevent the compiler from warning about unused variables +#define B3_UNUSED( ... ) (void)sizeof( ( __VA_ARGS__, 0 ) ) + +// Use to validate definitions. Do not take my cookie. +#define B3_SECRET_COOKIE 1152023 + +#define B3_CHECK_DEF( DEF ) B3_ASSERT( DEF->internalValue == B3_SECRET_COOKIE ) +#define B3_CHECK_JOINT_DEF( DEF ) B3_ASSERT( DEF->base.internalValue == B3_SECRET_COOKIE ) + +// These macros help avoid sizeof bugs +#define B3_ALLOC( T, N ) (T*)b3Alloc( N * sizeof( T ) ); +#define B3_FREE( M, T, N ) b3Free( M, N * sizeof( T ) ); + + +#ifdef __cplusplus +extern "C" +{ +#endif + +void* b3Alloc( size_t size ); +void* b3AllocZeroed( size_t size ); +void b3Free( void* mem, size_t size ); +void* b3GrowAlloc( void* oldMem, int oldSize, int newSize ); + +void b3Log( const char* format, ... ); + +typedef struct b3Mutex b3Mutex; +b3Mutex* b3CreateMutex( void ); +void b3DestroyMutex( b3Mutex* m ); +void b3LockMutex( b3Mutex* m ); +void b3UnlockMutex( b3Mutex* m ); + +typedef struct b3Semaphore b3Semaphore; +b3Semaphore* b3CreateSemaphore( int initCount ); +void b3DestroySemaphore( b3Semaphore* s ); +void b3WaitSemaphore( b3Semaphore* s ); +void b3SignalSemaphore( b3Semaphore* s ); + +typedef void b3ThreadFunction( void* context ); +typedef struct b3Thread b3Thread; +// Name may be NULL, otherwise it is copied. +b3Thread* b3CreateThread( b3ThreadFunction* function, void* context, const char* name ); +void b3JoinThread( b3Thread* t ); + +// Dump to a file. Only one dump file allowed at a time. +void b3OpenDump( const char* fileName ); +void b3Dump( const char* string, ... ); +void b3CloseDump( void ); +int b3FetchAddMeshDumpIndex( void ); + +#ifdef __cplusplus +} +#endif diff --git a/src/ctz.h b/src/ctz.h new file mode 100644 index 0000000..cfdc23f --- /dev/null +++ b/src/ctz.h @@ -0,0 +1,131 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +#if defined( _MSC_VER ) && !defined( __clang__ ) + +#include + +// https://en.wikipedia.org/wiki/Find_first_set + +B3_INLINE uint32_t b3CTZ32( uint32_t block ) +{ + unsigned long index; + _BitScanForward( &index, block ); + return index; +} + +// This function doesn't need to be fast, so using the Ivy Bridge fallback. +B3_INLINE uint32_t b3CLZ32( uint32_t value ) +{ +#if 0 + + // Use BSR (Bit Scan Reverse) which is available on Ivy Bridge + unsigned long index; + if ( _BitScanReverse( &index, value ) ) + { + // BSR gives the index of the most significant 1-bit + // We need to invert this to get the number of leading zeros + return 31 - index; + } + else + { + // If x is 0, BSR sets the zero flag and doesn't modify index + // LZCNT should return 32 for an input of 0 + return 32; + } + +#else + + return __lzcnt( value ); + +#endif +} + +B3_INLINE uint32_t b3CTZ64( uint64_t block ) +{ + unsigned long index; + +#ifdef _WIN64 + _BitScanForward64( &index, block ); +#else + // 32-bit fall back + if ( (uint32_t)block != 0 ) + { + _BitScanForward( &index, (uint32_t)block ); + } + else + { + _BitScanForward( &index, (uint32_t)( block >> 32 ) ); + index += 32; + } +#endif + + return index; +} + +B3_INLINE int b3PopCount64( uint64_t block ) +{ + return (int)__popcnt64( block ); +} + +#else + +B3_INLINE uint32_t b3CTZ32( uint32_t block ) +{ + return __builtin_ctz( block ); +} + +B3_INLINE uint32_t b3CLZ32( uint32_t value ) +{ + return __builtin_clz( value ); +} + +B3_INLINE uint32_t b3CTZ64( uint64_t block ) +{ + return __builtin_ctzll( block ); +} + +B3_INLINE int b3PopCount64( uint64_t block ) +{ + return __builtin_popcountll( block ); +} + +#endif + +B3_INLINE bool b3IsPowerOf2( int x ) +{ + return ( x & ( x - 1 ) ) == 0; +} + +B3_INLINE int b3BoundingPowerOf2( int x ) +{ + if ( x <= 1 ) + { + return 1; + } + + return 32 - (int)b3CLZ32( (uint32_t)x - 1 ); +} + +B3_INLINE int b3RoundUpPowerOf2( int x ) +{ + if ( x <= 1 ) + { + return 1; + } + + return 1 << ( 32 - (int)b3CLZ32( (uint32_t)x - 1 ) ); +} + +B3_INLINE int b3LowerPowerOf2Exponent( int x ) +{ + B3_ASSERT( x > 0 ); + int clz = (int)b3CLZ32( (uint32_t)x ); + + // Position of most significant bit = floor(log2(M)) + return 31 - clz; +} diff --git a/src/distance.c b/src/distance.c new file mode 100644 index 0000000..b0fc17e --- /dev/null +++ b/src/distance.c @@ -0,0 +1,1994 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +// Dirk Gregorius contributed portions of this code + +#include "math_internal.h" + +#include "box3d/collision.h" +#include "box3d/constants.h" + +#define B3_MAX_SIMPLEX_VERTICES 4 +#define B3_MAX_GJK_ITERATIONS 32 + +int b3GetProxySupport( const b3ShapeProxy* proxy, b3Vec3 axis ) +{ + int count = proxy->count; + const b3Vec3* points = proxy->points; + + B3_ASSERT( count > 0 ); + B3_ASSERT( points != NULL ); + + // We move the first vertex into the origin for improved precision. + // This is necessary since we don't have shape transforms and + // vertices can potentially be far away from the origin (large). + b3Vec3 origin = points[0]; + int maxIndex = 0; + float maxProjection = 0.0f; + + for ( int index = 1; index < count; ++index ) + { + // We subtract the first vertex since we are shifting into the origin. + float projection = b3Dot( axis, b3Sub( points[index], origin ) ); + if ( projection > maxProjection ) + { + maxIndex = index; + maxProjection = projection; + } + } + + return maxIndex; +} + +int b3GetPointSupport( const b3Vec3* points, int count, b3Vec3 axis ) +{ + B3_ASSERT( count > 0 ); + B3_ASSERT( points != NULL ); + + // We move the first vertex into the origin for improved precision. + // This is necessary since we don't have shape transforms and + // vertices can potentially be far away from the origin (large). + b3Vec3 origin = points[0]; + int maxIndex = 0; + float maxProjection = 0.0f; + + for ( int index = 1; index < count; ++index ) + { + // We subtract the first vertex since we are shifting into the origin. + float projection = b3Dot( axis, b3Sub( points[index], origin ) ); + if ( projection > maxProjection ) + { + maxIndex = index; + maxProjection = projection; + } + } + + return maxIndex; +} + +// Similar to Real-time Collision Detection, p179. +// todo try https://www.scratchapixel.com/lessons/3d-basic-rendering/minimal-ray-tracer-rendering-simple-shapes/ray-box-intersection.html +bool b3IntersectRayAndAABB( b3Vec3 lowerBound, b3Vec3 upperBound, b3Vec3 p1, b3Vec3 p2, float* minFraction, + float* maxFraction ) +{ + // Ray direction and length + b3Vec3 d = b3Sub( p2, p1 ); + float rayLength = b3Length( d ); + + // Handle degenerate ray + if ( rayLength < FLT_EPSILON ) + { + // Check if point is inside AABB + if ( p1.x >= lowerBound.x && p1.x <= upperBound.x && p1.y >= lowerBound.y && p1.y <= upperBound.y && + p1.z >= lowerBound.z && p1.z <= upperBound.z ) + { + *minFraction = 0.0f; + *maxFraction = 0.0f; + return true; + } + else + { + return false; + } + } + + b3Vec3 rayDir = b3MulSV( 1.0f / rayLength, d ); + + // Slab method for ray-AABB intersection + float tMin = 0.0f; + float tMax = rayLength; + + // Test each axis + for ( int i = 0; i < 3; ++i ) + { + float rayComponent = b3GetByIndex( rayDir, i ); + float rayStart = b3GetByIndex( p1, i ); + float boxMin = b3GetByIndex( lowerBound, i ); + float boxMax = b3GetByIndex( upperBound, i ); + + if ( b3AbsFloat( rayComponent ) < FLT_EPSILON ) + { + // Ray is parallel to slab, check if ray origin is within slab + if ( rayStart < boxMin || rayStart > boxMax ) + { + return false; + } + } + else + { + // Compute intersection distances + float t1 = ( boxMin - rayStart ) / rayComponent; + float t2 = ( boxMax - rayStart ) / rayComponent; + + // Ensure t1 <= t2 + if ( t1 > t2 ) + { + float temp = t1; + t1 = t2; + t2 = temp; + } + + // Update intersection interval + tMin = b3MaxFloat( tMin, t1 ); + tMax = b3MinFloat( tMax, t2 ); + + // Check for no intersection + if ( tMin > tMax ) + { + return false; + } + } + } + + // Check if intersection is behind ray start + if ( tMax < 0.0f ) + { + return false; + } + + // Convert distances to fractions + *minFraction = b3ClampFloat( tMin / rayLength, 0.0f, 1.0f ); + *maxFraction = b3ClampFloat( tMax / rayLength, 0.0f, 1.0f ); + + return true; +} + +static void b3BarycentricCoordsEdge( float out[3], b3Vec3 a, b3Vec3 b ) +{ + b3Vec3 ab = b3Sub( b, a ); + + // Last element is divisor + float divisor = b3Dot( ab, ab ); + + out[0] = b3Dot( b, ab ); + out[1] = -b3Dot( a, ab ); + out[2] = divisor; +} + +static void b3BarycentricCoordsTri( float out[4], b3Vec3 a, b3Vec3 b, b3Vec3 c ) +{ + b3Vec3 ab = b3Sub( b, a ); + b3Vec3 ac = b3Sub( c, a ); + + b3Vec3 bXC = b3Cross( b, c ); + b3Vec3 cXA = b3Cross( c, a ); + b3Vec3 aXB = b3Cross( a, b ); + + b3Vec3 abXAc = b3Cross( ab, ac ); + + // Last element is divisor + float divisor = b3Dot( abXAc, abXAc ); + + out[0] = b3Dot( bXC, abXAc ); + out[1] = b3Dot( cXA, abXAc ); + out[2] = b3Dot( aXB, abXAc ); + out[3] = divisor; +} + +static void b3BarycentricCoordsTet( float out[5], b3Vec3 a, b3Vec3 b, b3Vec3 c, b3Vec3 d ) +{ + b3Vec3 ab = b3Sub( b, a ); + b3Vec3 ac = b3Sub( c, a ); + b3Vec3 ad = b3Sub( d, a ); + + // Last element is divisor (forced to be positive) + float divisor = b3ScalarTripleProduct( ab, ac, ad ); + + float sign = divisor < 0.0f ? -1.0f : 1.0f; + out[0] = sign * b3ScalarTripleProduct( b, c, d ); + out[1] = sign * b3ScalarTripleProduct( a, d, c ); + out[2] = sign * b3ScalarTripleProduct( a, b, d ); + out[3] = sign * b3ScalarTripleProduct( a, c, b ); + out[4] = sign * divisor; +} + +static float b3GetMetric( const b3Simplex* simplex ) +{ + int count = simplex->count; + B3_ASSERT( 1 <= count && count <= 4 ); + + const b3SimplexVertex* vertices = simplex->vertices; + + switch ( count ) + { + case 1: + { + return 0.0f; + } + + case 2: + { + b3Vec3 a = vertices[0].w; + b3Vec3 b = vertices[1].w; + return b3Distance( a, b ); + } + + case 3: + { + b3Vec3 a = vertices[0].w; + b3Vec3 b = vertices[1].w; + b3Vec3 c = vertices[2].w; + return b3Length( b3Cross( b3Sub( b, a ), b3Sub( c, a ) ) ) / 2.0f; + } + + case 4: + { + b3Vec3 a = vertices[0].w; + b3Vec3 b = vertices[1].w; + b3Vec3 c = vertices[2].w; + b3Vec3 d = vertices[3].w; + return b3ScalarTripleProduct( b3Sub( b, a ), b3Sub( c, a ), b3Sub( d, a ) ) / 6.0f; + } + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + return 0.0f; +} + +static void b3WriteCache( b3SimplexCache* cache, const b3Simplex* simplex ) +{ + int count = simplex->count; + cache->metric = b3GetMetric( simplex ); + cache->count = (uint16_t)count; + for ( int index = 0; index < count; ++index ) + { + cache->indexA[index] = (uint8_t)simplex->vertices[index].indexA; + cache->indexB[index] = (uint8_t)simplex->vertices[index].indexB; + } +} + +static bool b3SolveSimplex2( b3Simplex* simplex ) +{ + b3SimplexVertex* vs = simplex->vertices; + B3_ASSERT( simplex->count == 2 ); + + // Vertex regions + //float wAB[3]; + + b3Vec3 a = vs[0].w; + b3Vec3 b = vs[1].w; + b3Vec3 ab = b3Sub( b, a ); + + // Last element is divisor + float divisor = b3Dot( ab, ab ); + + float u = b3Dot( b, ab ); + float v = -b3Dot( a, ab ); + //wAB[2] = divisor; + + // V( A ) + if ( v <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0].a = 1.0f; + + return true; + } + + // V( B ) + if ( u <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = vs[1]; + vs[0].a = 1.0f; + + return true; + } + + // Edge region + if ( divisor <= 0.0f ) + { + return false; + } + + // VR( AB ) + float denominator = 1.0f / divisor; + vs[0].a = denominator * u; + vs[1].a = denominator * v; + + return true; +} + +static bool b3SolveSimplex3( b3Simplex* simplex ) +{ + b3SimplexVertex* vs = simplex->vertices; + B3_ASSERT( simplex->count == 3 ); + + // Get simplex (be aware of aliasing here!) + b3SimplexVertex v1 = vs[0]; + b3SimplexVertex v2 = vs[1]; + b3SimplexVertex v3 = vs[2]; + + // Vertex regions + float wAB[3], wBC[3], wCA[3]; + b3BarycentricCoordsEdge( wAB, v1.w, v2.w ); + b3BarycentricCoordsEdge( wBC, v2.w, v3.w ); + b3BarycentricCoordsEdge( wCA, v3.w, v1.w ); + + // VR( A ) + if ( wAB[1] <= 0.0f && wCA[0] <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = v1; + vs[0].a = 1.0f; + + return true; + } + + // VR( B ) + if ( wBC[1] <= 0.0f && wAB[0] <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = v2; + vs[0].a = 1.0f; + + return true; + } + + // VR( C ) + if ( wCA[1] <= 0.0f && wBC[0] <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = v3; + vs[0].a = 1.0f; + + return true; + } + + // Edge regions + float wABC[4]; + b3BarycentricCoordsTri( wABC, v1.w, v2.w, v3.w ); + + // VR( AB ) + if ( wABC[2] <= 0.0f && wAB[0] > 0.0f && wAB[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = v1; + vs[1] = v2; + + // Normalize + float divisor = wAB[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wAB[0] / divisor; + vs[1].a = wAB[1] / divisor; + + return true; + } + + // VR( BC ) + if ( wABC[0] <= 0.0f && wBC[0] > 0.0f && wBC[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = v2; + vs[1] = v3; + + // Normalize + float divisor = wBC[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wBC[0] / divisor; + vs[1].a = wBC[1] / divisor; + + return true; + } + + // VR( CA ) + if ( wABC[1] <= 0.0f && wCA[0] > 0.0f && wCA[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = v3; + vs[1] = v1; + + // Normalize + float divisor = wCA[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wCA[0] / divisor; + vs[1].a = wCA[1] / divisor; + + return true; + } + + // Face region + float divisor = wABC[3]; + if ( divisor <= 0.0f ) + { + return false; + } + + // VR( ABC ) + vs[0].a = wABC[0] / divisor; + vs[1].a = wABC[1] / divisor; + vs[2].a = wABC[2] / divisor; + + return true; +} + +static bool b3SolveSimplex4( b3Simplex* simplex ) +{ + b3SimplexVertex* vs = simplex->vertices; + + // Get simplex (be aware of aliasing here!) + B3_ASSERT( simplex->count == 4 ); + b3SimplexVertex vertexA = vs[0]; + b3SimplexVertex vertexB = vs[1]; + b3SimplexVertex vertexC = vs[2]; + b3SimplexVertex vertexD = vs[3]; + + // Vertex region + float wAB[3], wAC[3], wAD[3], wBC[3], wCD[3], wDB[3]; + b3BarycentricCoordsEdge( wAB, vertexA.w, vertexB.w ); + b3BarycentricCoordsEdge( wAC, vertexA.w, vertexC.w ); + b3BarycentricCoordsEdge( wAD, vertexA.w, vertexD.w ); + b3BarycentricCoordsEdge( wBC, vertexB.w, vertexC.w ); + b3BarycentricCoordsEdge( wCD, vertexC.w, vertexD.w ); + b3BarycentricCoordsEdge( wDB, vertexD.w, vertexB.w ); + + // VR( A ) + if ( wAB[1] <= 0.0f && wAC[1] <= 0.0f && wAD[1] <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = vertexA; + + vs[0].a = 1.0f; + + return true; + } + + // VR( B ) + if ( wAB[0] <= 0.0f && wDB[0] <= 0.0f && wBC[1] <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = vertexB; + + vs[0].a = 1.0f; + + return true; + } + + // VR( C ) + if ( wAC[0] <= 0.0f && wBC[0] <= 0.0f && wCD[1] <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = vertexC; + + vs[0].a = 1.0f; + + return true; + } + + // VR( D ) + if ( wAD[0] <= 0.0f && wCD[0] <= 0.0f && wDB[1] <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = vertexD; + + vs[0].a = 1.0f; + + return true; + } + + // Edge region + float wACB[4], wABD[4], wADC[4], wBCD[4]; + b3BarycentricCoordsTri( wACB, vertexA.w, vertexC.w, vertexB.w ); + b3BarycentricCoordsTri( wABD, vertexA.w, vertexB.w, vertexD.w ); + b3BarycentricCoordsTri( wADC, vertexA.w, vertexD.w, vertexC.w ); + b3BarycentricCoordsTri( wBCD, vertexB.w, vertexC.w, vertexD.w ); + + // VR( AB ) + if ( wABD[2] <= 0.0f && wACB[1] <= 0.0f && wAB[0] > 0.0f && wAB[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = vertexA; + vs[1] = vertexB; + + // Normalize + float divisor = wAB[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wAB[0] / divisor; + vs[1].a = wAB[1] / divisor; + + return true; + } + + // VR( AC ) + if ( wACB[2] <= 0.0f && wADC[1] <= 0.0f && wAC[0] > 0.0f && wAC[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = vertexA; + vs[1] = vertexC; + + // Normalize + float divisor = wAC[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wAC[0] / divisor; + vs[1].a = wAC[1] / divisor; + + return true; + } + + // VR( AD ) + if ( wADC[2] <= 0.0f && wABD[1] <= 0.0f && wAD[0] > 0.0f && wAD[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = vertexA; + vs[1] = vertexD; + + // Normalize + float divisor = wAD[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wAD[0] / divisor; + vs[1].a = wAD[1] / divisor; + + return true; + } + + // VR( BC ) + if ( wACB[0] <= 0.0f && wBCD[2] <= 0.0f && wBC[0] > 0.0f && wBC[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = vertexB; + vs[1] = vertexC; + + // Normalize + float divisor = wBC[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wBC[0] / divisor; + vs[1].a = wBC[1] / divisor; + + return true; + } + + // VR( CD ) + if ( wADC[0] <= 0.0f && wBCD[0] <= 0.0f && wCD[0] > 0.0f && wCD[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = vertexC; + vs[1] = vertexD; + + // Normalize + float divisor = wCD[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wCD[0] / divisor; + vs[1].a = wCD[1] / divisor; + + return true; + } + + // VR( DB ) + if ( wABD[0] <= 0.0f && wBCD[1] <= 0.0f && wDB[0] > 0.0f && wDB[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = vertexD; + vs[1] = vertexB; + + // Normalize + float divisor = wDB[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wDB[0] / divisor; + vs[1].a = wDB[1] / divisor; + + return true; + } + + // Face regions + float wABCD[5]; + b3BarycentricCoordsTet( wABCD, vertexA.w, vertexB.w, vertexC.w, vertexD.w ); + + // VR( ACB ) + if ( wABCD[3] < 0.0f && wACB[0] > 0.0f && wACB[1] > 0.0f && wACB[2] > 0.0f ) + { + // Reduce simplex + simplex->count = 3; + vs[0] = vertexA; + vs[1] = vertexC; + vs[2] = vertexB; + + // Normalize + float divisor = wACB[3]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wACB[0] / divisor; + vs[1].a = wACB[1] / divisor; + vs[2].a = wACB[2] / divisor; + + return true; + } + + // VR( ABD ) + if ( wABCD[2] < 0.0f && wABD[0] > 0.0f && wABD[1] > 0.0f && wABD[2] > 0.0f ) + { + // Reduce simplex + simplex->count = 3; + vs[0] = vertexA; + vs[1] = vertexB; + vs[2] = vertexD; + + // Normalize + float divisor = wABD[3]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wABD[0] / divisor; + vs[1].a = wABD[1] / divisor; + vs[2].a = wABD[2] / divisor; + + return true; + } + + // VR( ADC ) + if ( wABCD[1] < 0.0f && wADC[0] > 0.0f && wADC[1] > 0.0f && wADC[2] > 0.0f ) + { + // Reduce simplex + simplex->count = 3; + vs[0] = vertexA; + vs[1] = vertexD; + vs[2] = vertexC; + + // Normalize + float divisor = wADC[3]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wADC[0] / divisor; + vs[1].a = wADC[1] / divisor; + vs[2].a = wADC[2] / divisor; + + return true; + } + + // VR( BCD ) + if ( wABCD[0] < 0.0f && wBCD[0] > 0.0f && wBCD[1] > 0.0f && wBCD[2] > 0.0f ) + { + // Reduce simplex + simplex->count = 3; + vs[0] = vertexB; + vs[1] = vertexC; + vs[2] = vertexD; + + // Normalize + float divisor = wBCD[3]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wBCD[0] / divisor; + vs[1].a = wBCD[1] / divisor; + vs[2].a = wBCD[2] / divisor; + + return true; + } + + // *** Inside tetrahedron *** + float divisor = wABCD[4]; + if ( divisor <= 0.0f ) + { + return false; + } + + // VR( ABCD ) + vs[0].a = wABCD[0] / divisor; + vs[1].a = wABCD[1] / divisor; + vs[2].a = wABCD[2] / divisor; + vs[3].a = wABCD[3] / divisor; + + return true; +} + +static void b3ComputeWitnessPoints( const b3Simplex* simplex, b3Vec3* vertexA, b3Vec3* vertexB ) +{ + const b3SimplexVertex* vs = simplex->vertices; + int count = simplex->count; + B3_ASSERT( 1 <= count && count <= 4 ); + + switch ( count ) + { + case 1: + *vertexA = vs[0].wA; + *vertexB = vs[0].wB; + break; + + case 2: + *vertexA = b3Blend2( vs[0].a, vs[0].wA, vs[1].a, vs[1].wA ); + *vertexB = b3Blend2( vs[0].a, vs[0].wB, vs[1].a, vs[1].wB ); + break; + + case 3: + *vertexA = b3Blend3( vs[0].a, vs[0].wA, vs[1].a, vs[1].wA, vs[2].a, vs[2].wA ); + *vertexB = b3Blend3( vs[0].a, vs[0].wB, vs[1].a, vs[1].wB, vs[2].a, vs[2].wB ); + break; + + case 4: + { + // Force identical points and *zero* distance + b3Vec3 sum = b3Add( b3Blend2( vs[0].a, vs[0].wA, vs[1].a, vs[1].wA ), + b3Blend2( vs[2].a, vs[2].wA, vs[3].a, vs[3].wA ) ); + *vertexA = sum; + *vertexB = sum; + } + break; + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } +} + +b3DistanceOutput b3ShapeDistance( const b3DistanceInput* input, b3SimplexCache* cache, b3Simplex* simplexes, int simplexCapacity ) +{ + // Build initial simplex + b3Transform transformA = input->transformA; + b3Transform transformB = input->transformB; + + // Work in relative space + b3Transform xf = b3InvMulTransforms( transformA, transformB ); + + // Use matrices for faster math + b3Matrix3 m = b3MakeMatrixFromQuat( xf.q ); + b3Matrix3 mt = b3Transpose( m ); + b3Matrix3 mA = b3MakeMatrixFromQuat( transformA.q ); + + const b3ShapeProxy* proxyA = &input->proxyA; + const b3ShapeProxy* proxyB = &input->proxyB; + + // Compute initial simplex from cache + B3_ASSERT( cache->count <= B3_MAX_SIMPLEX_VERTICES ); + + b3Simplex simplex = { 0 }; + b3SimplexVertex* vs = simplex.vertices; + + simplex.count = cache->count; + for ( int i = 0; i < cache->count; ++i ) + { + int index1 = cache->indexA[i]; + int index2 = cache->indexB[i]; + + B3_ASSERT( 0 <= index1 && index1 < proxyA->count ); + B3_ASSERT( 0 <= index2 && index2 < proxyB->count ); + + b3Vec3 vertex1 = proxyA->points[index1]; + b3Vec3 vertex2 = b3Add( b3MulMV( m, proxyB->points[index2] ), xf.p ); + + vs[i].indexA = index1; + vs[i].indexB = index2; + vs[i].wA = vertex1; + vs[i].wB = vertex2; + vs[i].w = b3Sub( vertex2, vertex1 ); + vs[i].a = 0.0f; + } + + // Compute the new simplex metric, if it is substantially + // different than the old metric flush the simplex. + if ( simplex.count > 0 ) + { + float metric1 = cache->metric; + float metric2 = b3GetMetric( &simplex ); + + // todo the tetrahedron metric can be negative + if ( 2.0f * metric1 < metric2 || metric2 < 0.5f * metric1 || metric2 < FLT_EPSILON ) + { + // Flush the simplex + simplex.count = 0; + } + } + + // If the cache is invalid or empty + if ( simplex.count == 0 ) + { + b3Vec3 vertex1 = proxyA->points[0]; + b3Vec3 vertex2 = b3Add( b3MulMV( m, proxyB->points[0] ), xf.p ); + + simplex.count = 1; + simplex.vertices[0].indexA = 0; + simplex.vertices[0].indexB = 0; + simplex.vertices[0].wA = vertex1; + simplex.vertices[0].wB = vertex2; + simplex.vertices[0].w = b3Sub( vertex2, vertex1 ); + simplex.vertices[0].a = 0.0f; + } + + b3Simplex backup = { 0 }; + + int simplexIndex = 0; + if ( simplexes != NULL && simplexIndex < simplexCapacity ) + { + simplexes[simplexIndex] = simplex; + simplexIndex += 1; + } + + b3DistanceOutput distanceOutput = { 0 }; + + // Keep track of squared distance + float distanceSq = FLT_MAX; + + b3Vec3 normal = b3Vec3_zero; + + // Run GJK + int iteration = 0; + for ( ; iteration < B3_MAX_GJK_ITERATIONS; ++iteration ) + { + // Solve simplex + bool solved = false; + switch ( simplex.count ) + { + case 1: + simplex.vertices[0].a = 1.0f; + solved = true; + break; + + case 2: + solved = b3SolveSimplex2( &simplex ); + break; + + case 3: + solved = b3SolveSimplex3( &simplex ); + break; + + case 4: + solved = b3SolveSimplex4( &simplex ); + break; + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + if ( solved == false ) + { + // No progress - reconstruct last simplex + B3_ASSERT( backup.count != 0 ); + simplex = backup; + break; + } + + if ( simplexes != NULL && simplexIndex < simplexCapacity ) + { + simplexes[simplexIndex] = simplex; + simplexIndex += 1; + distanceOutput.iterations = iteration; + distanceOutput.simplexCount = simplexIndex; + } + + if ( simplex.count == B3_MAX_SIMPLEX_VERTICES ) + { + // Overlap + b3Vec3 localPointA, localPointB; + b3ComputeWitnessPoints( &simplex, &localPointA, &localPointB ); + distanceOutput.pointA = b3Add( b3MulMV( mA, localPointA ), transformA.p ); + distanceOutput.pointB = b3Add( b3MulMV( mA, localPointB ), transformA.p ); + return distanceOutput; + } + + // Assure distance progression + float oldDistanceSq = distanceSq; + + // Compute closest point + b3Vec3 closestPoint = { 0 }; + + switch ( simplex.count ) + { + case 1: + closestPoint = vs[0].w; + break; + + case 2: + closestPoint = b3Blend2( vs[0].a, vs[0].w, vs[1].a, vs[1].w ); + break; + + case 3: + closestPoint = b3Blend3( vs[0].a, vs[0].w, vs[1].a, vs[1].w, vs[2].a, vs[2].w ); + break; + + case 4: + closestPoint = b3Add( b3Blend2( vs[0].a, vs[0].w, vs[1].a, vs[1].w ), + b3Blend2( vs[2].a, vs[2].w, vs[3].a, vs[3].w ) ); + break; + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + distanceSq = b3Dot( closestPoint, closestPoint ); + + if ( distanceSq >= oldDistanceSq ) + { + // No progress - reconstruct last simplex + B3_ASSERT( backup.count != 0 ); + simplex = backup; + break; + } + + // Build new tentative support point + b3Vec3 searchDirection = { 0 }; + + switch ( simplex.count ) + { + case 1: + { + // v = -A + searchDirection = b3Neg( vs[0].w ); + } + break; + + case 2: + { + // v = (AB x AO) x AB + b3Vec3 a = vs[0].w; + b3Vec3 b = vs[1].w; + + b3Vec3 ab = b3Sub( b, a ); + + searchDirection = b3Cross( b3Cross( ab, b3Neg( a ) ), ab ); + } + break; + + case 3: + { + // v = AB x AC or v = AC x AB + b3Vec3 a = vs[0].w; + b3Vec3 b = vs[1].w; + b3Vec3 c = vs[2].w; + + b3Vec3 ab = b3Sub( b, a ); + b3Vec3 ac = b3Sub( c, a ); + + b3Vec3 n = b3Cross( ab, ac ); + + searchDirection = b3Dot( n, a ) < 0.0f ? n : b3Neg( n ); + } + break; + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + if ( b3LengthSquared( searchDirection ) < 1000.0f * FLT_MIN ) + { + // The origin is probably contained by a line segment or triangle. + // Thus the shapes are overlapped. + b3Vec3 localPointA, localPointB; + b3ComputeWitnessPoints( &simplex, &localPointA, &localPointB ); + distanceOutput.pointA = b3Add( b3MulMV( mA, localPointA ), transformA.p ); + distanceOutput.pointB = b3Add( b3MulMV( mA, localPointB ), transformA.p ); + B3_VALIDATE( b3Distance( localPointA, localPointB ) < FLT_EPSILON ); + return distanceOutput; + } + + normal = b3Neg( searchDirection ); + + // Get new support points + b3Vec3 searchDirection1 = searchDirection; + int indexA = b3GetProxySupport( &input->proxyA, b3Neg( searchDirection1 ) ); + b3Vec3 supportA = input->proxyA.points[indexA]; + b3Vec3 searchDirection2 = b3MulMV( mt, searchDirection ); + int indexB = b3GetProxySupport( &input->proxyB, searchDirection2 ); + b3Vec3 supportB = b3Add( b3MulMV( m, input->proxyB.points[indexB] ), xf.p ); + + // Save current simplex and add new vertex - this can fail if we detect cycling + backup = simplex; + + // Check for duplicate support points. This is the main termination criteria. + bool duplicate = false; + for ( int i = 0; i < simplex.count; ++i ) + { + if ( vs[i].indexA == indexA && vs[i].indexB == indexB ) + { + duplicate = true; + break; + } + } + + if ( duplicate ) + { + break; + } + + vs[simplex.count].indexA = indexA; + vs[simplex.count].indexB = indexB; + vs[simplex.count].wA = supportA; + vs[simplex.count].wB = supportB; + vs[simplex.count].w = b3Sub( supportB, supportA ); + simplex.count += 1; + } + + normal = b3Normalize( normal ); + if ( b3IsNormalized( normal ) == false ) + { + // Treat as overlap + return distanceOutput; + } + + // Build witness points and safe cache + b3Vec3 localPointA, localPointB; + b3ComputeWitnessPoints( &simplex, &localPointA, &localPointB ); + b3WriteCache( cache, &simplex ); + + // Convert results from frame A into world space + normal = b3MulMV( mA, normal ); + distanceOutput.pointA = b3Add( b3MulMV( mA, localPointA ), transformA.p ); + distanceOutput.pointB = b3Add( b3MulMV( mA, localPointB ), transformA.p ); + distanceOutput.distance = b3Distance( localPointA, localPointB ); + distanceOutput.normal = normal; + distanceOutput.iterations = iteration; + distanceOutput.simplexCount = simplexIndex; + + // Apply radii if requested + if ( input->useRadii ) + { + float rA = input->proxyA.radius; + float rB = input->proxyB.radius; + distanceOutput.distance = b3MaxFloat( 0.0f, distanceOutput.distance - rA - rB ); + + // Keep closest points on perimeter even if overlapped, this way the points move smoothly. + distanceOutput.pointA = b3Add( distanceOutput.pointA, b3MulSV( rA, normal ) ); + distanceOutput.pointB = b3Sub( distanceOutput.pointB, b3MulSV( rB, normal ) ); + } + + return distanceOutput; +} + + +// Separation function: +// f(t) = (c2 + t * dp2 - c1 - t * dp1 ) * n + +// Root finding : f(t) - target = 0 +// (c2 + t * dp2 - c1 - t * dp1 ) * n - target = 0 +// (c2 - c1) * n + t * (dp2 - dp1) * n - target = 0 +// t = [target - (c2 - c1) * n] / [(dp2 - dp1) * n] +// t = (target - d) / [(dp2 - dp1) * n] + +b3CastOutput b3ShapeCast( const b3ShapeCastPairInput* input ) +{ + // Compute tolerance + float linearSlop = B3_LINEAR_SLOP; + float totalRadius = input->proxyA.radius + input->proxyB.radius; + float target = b3MaxFloat( linearSlop, totalRadius - linearSlop ); + float tolerance = 0.25f * linearSlop; + + B3_ASSERT( target > tolerance ); + + // Prepare input for distance query + b3SimplexCache cache = { 0 }; + + float alpha = 0.0f; + + b3DistanceInput distanceInput = { 0 }; + distanceInput.proxyA = input->proxyA; + distanceInput.proxyB = input->proxyB; + distanceInput.transformA = input->transformA; + distanceInput.transformB = input->transformB; + distanceInput.useRadii = false; + + b3Vec3 delta2 = input->translationB; + b3DistanceOutput distanceOutput = { 0 }; + b3CastOutput output = { 0 }; + output.triangleIndex = B3_NULL_INDEX; + + int iteration = 0; + const int maxIterations = 20; + + for ( ; iteration < maxIterations; ++iteration ) + { + output.iterations += 1; + + distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance < target + tolerance ) + { + if ( iteration == 0 ) + { + if ( input->canEncroach && distanceOutput.distance > 2.0f * linearSlop ) + { + target = distanceOutput.distance - linearSlop; + } + else + { + // Initial overlap + output.hit = true; + + // Compute a common point + b3Vec3 c1 = b3MulAdd( distanceOutput.pointA, input->proxyA.radius, distanceOutput.normal ); + b3Vec3 c2 = b3MulAdd( distanceOutput.pointB, -input->proxyB.radius, distanceOutput.normal ); + output.point = b3Lerp( c1, c2, 0.5f ); + return output; + } + } + else + { + // Logging for bad input data + if ( distanceOutput.distance > 0.0f && b3IsNormalized( distanceOutput.normal ) == false ) + { + for ( int i = 0; i < input->proxyA.count; ++i ) + { + b3Vec3 p = input->proxyA.points[i]; + b3Log( "pointA[%d] = {%.9f, %.9f, %.9f}", i, p.x, p.y, p.z ); + } + b3Log( "radiusA = %.9f", input->proxyA.radius ); + + for ( int i = 0; i < input->proxyB.count; ++i ) + { + b3Vec3 p = input->proxyB.points[i]; + b3Log( "pointB[%d] = {%.9f, %.9f, %.9f}", i, p.x, p.y, p.z ); + } + b3Log( "radiusB = %.9f", input->proxyB.radius ); + + { + b3Transform xfA = input->transformA; + b3Log( "xfA = {{%.9f, %.9f, %.9f}, {{%.9f, %.9f, %.9f}, %.9f}", xfA.p.x, xfA.p.y, xfA.p.z, xfA.q.v.x, + xfA.q.v.y, xfA.q.v.z, xfA.q.s ); + } + { + b3Transform xfB = input->transformB; + b3Log( "xfB = {{%.9f, %.9f, %.9f}, {{%.9f, %.9f, %.9f}, %.9f}", xfB.p.x, xfB.p.y, xfB.p.z, xfB.q.v.x, + xfB.q.v.y, xfB.q.v.z, xfB.q.s ); + } + + { + b3Vec3 t = input->translationB; + b3Log( "t = {%.9f, %.9f, %.9f}", t.x, t.y, t.z ); + } + + b3Log( "maxFraction = %.9f, canEncroach = %d", input->maxFraction, input->canEncroach ); + + // Numerical problem. Likely extreme input. + return output; + } + + // Hitting this assert implies that the algorithm brought the shapes too close. + // B3_ASSERT( distanceOutput.distance > 0.0f && b3IsNormalized( distanceOutput.normal ) ); + + output.fraction = alpha; + output.point = b3MulAdd( distanceOutput.pointA, input->proxyA.radius, distanceOutput.normal ); + output.normal = distanceOutput.normal; + output.hit = true; + return output; + } + } + + B3_ASSERT( distanceOutput.distance > 0.0f ); + B3_ASSERT( b3IsNormalized( distanceOutput.normal ) ); + + // Check if shapes are approaching each other + float denominator = b3Dot( delta2, distanceOutput.normal ); + if ( denominator >= 0.0f ) + { + // Miss + return output; + } + + // Advance sweep + alpha += ( target - distanceOutput.distance ) / denominator; + if ( alpha >= input->maxFraction ) + { + // Success! + return output; + } + + distanceInput.transformB.p = b3MulAdd( input->transformB.p, alpha, delta2 ); + } + + // Failure! + return output; +} + +b3Transform b3GetSweepTransform( const b3Sweep* sweep, float time ) +{ + b3Transform transform; + transform.q = b3NLerp( sweep->q1, sweep->q2, time ); + transform.p = b3Sub( b3Lerp( sweep->c1, sweep->c2, time ), b3RotateVector( transform.q, sweep->localCenter ) ); + return transform; +} + +static inline b3Transform b3GetFinalSweepTransform( const b3Sweep* sweep ) +{ + b3Transform transform; + transform.q = sweep->q2; + transform.p = b3Sub( sweep->c2, b3RotateVector( transform.q, sweep->localCenter ) ); + return transform; +} + +static int b3UniqueCount( int vertexCount, int vertices[3] ) +{ + B3_ASSERT( 1 <= vertexCount && vertexCount <= 3 ); + + switch ( vertexCount ) + { + case 1: + return 1; + + case 2: + return vertices[0] != vertices[1] ? 2 : 1; + + case 3: + if ( vertices[0] != vertices[1] && vertices[0] != vertices[2] && vertices[1] != vertices[2] ) + { + // All different + return 3; + } + + if ( vertices[0] == vertices[1] && vertices[0] == vertices[2] && vertices[1] == vertices[2] ) + { + // All equal + return 1; + } + + return 2; + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + return 0; +} + +// This checks if the cross product of two edges switches direction. +static inline bool b3CheckFastEdges( b3Transform xfA, b3Vec3 localEdgeA, b3Transform xfB, b3Vec3 localEdgeB, b3Vec3 axis0 ) +{ + // By taking the local witness axes we make sure that we + // get the correct orientations (e.g. if one axis was flipped)! + b3Vec3 edgeA = b3RotateVector( xfA.q, localEdgeA ); + b3Vec3 edgeB = b3RotateVector( xfB.q, localEdgeB ); + b3Vec3 axis = b3Cross( edgeA, edgeB ); + return b3Dot( axis, axis0 ) < 0.0f; +} + +typedef enum b3SeparationType +{ + b3_separationUnknown = 0, + b3_separationVertices, + b3_separationEdges, + b3_separationFaceA, + b3_separationFaceB, +} b3SeparationType; + +typedef struct b3SeparationFunction +{ + const b3ShapeProxy* proxyA; + const b3ShapeProxy* proxyB; + b3Sweep sweepA; + b3Sweep sweepB; + + // These are associated with different bodies depending on the separation function type. + // It could be two local vectors/points on the same body (for example, both on bodyA). + b3Vec3 witness1; + b3Vec3 witness2; + + b3SeparationType type; +} b3SeparationFunction; + +static b3SeparationFunction b3MakeSeparationFunction( const b3SimplexCache cache, const b3ShapeProxy* proxyA, + const b3Sweep* sweepA, const b3ShapeProxy* proxyB, const b3Sweep* sweepB, + const b3DistanceOutput* query, float t1 ) +{ + B3_ASSERT( 1 <= cache.count && cache.count <= 3 ); + B3_VALIDATE( b3IsNormalized( query->normal ) ); + + b3SeparationFunction fcn = { 0 }; + fcn.proxyA = proxyA; + fcn.proxyB = proxyB; + fcn.sweepA = *sweepA; + fcn.sweepB = *sweepB; + fcn.type = b3_separationUnknown; + + int indexA[3] = { cache.indexA[0], cache.indexA[1], cache.indexA[2] }; + int indexB[3] = { cache.indexB[0], cache.indexB[1], cache.indexB[2] }; + + int uniqueCountA = b3UniqueCount( cache.count, indexA ); + int uniqueCountB = b3UniqueCount( cache.count, indexB ); + + b3Transform xfA1 = b3GetSweepTransform( sweepA, t1 ); + b3Transform xfB1 = b3GetSweepTransform( sweepB, t1 ); + + b3Quat qA = xfA1.q; + b3Quat qB = xfB1.q; + + // Minimize round-off + b3Vec3 deltaP = b3Sub( xfB1.p, xfA1.p ); + + switch ( cache.count ) + { + case 1: + { + // Witness is the world space direction + fcn.type = b3_separationVertices; + fcn.witness1 = query->normal; + } + break; + + case 2: + { + if ( uniqueCountA == 2 && uniqueCountB == 2 ) + { + // Edge/Edge + b3Vec3 vA1 = proxyA->points[indexA[0]]; + b3Vec3 localEdgeA = b3Sub( proxyA->points[indexA[1]], vA1 ); + localEdgeA = b3Normalize( localEdgeA ); + b3Vec3 edgeA = b3RotateVector( qA, localEdgeA ); + + b3Vec3 vB1 = proxyB->points[indexB[0]]; + b3Vec3 localEdgeB = b3Sub( proxyB->points[indexB[1]], vB1 ); + localEdgeB = b3Normalize( localEdgeB ); + b3Vec3 edgeB = b3RotateVector( qB, localEdgeB ); + + b3Vec3 axis = b3Cross( edgeA, edgeB ); + float lengthSquared = b3LengthSquared( axis ); + + // Skip near parallel edges: |e1 x e1| = sin(alpha) * |e1| * |e2| + const float kToleranceSquared = 0.05f * 0.05f; + if ( lengthSquared < kToleranceSquared ) + { + // The axis is not safe to normalize so we use a world axis instead! + fcn.type = b3_separationVertices; + fcn.witness1 = query->normal; + } + else + { + b3Vec3 delta = b3Add( b3Sub( b3RotateVector( qB, vB1 ), b3RotateVector( qA, vA1 ) ), deltaP ); + if ( b3Dot( delta, axis ) < 0.0f ) + { + // Make axis point from A to B + axis = b3Neg( axis ); + localEdgeB = b3Neg( localEdgeB ); + } + + // Check for possible sign flip in edge/edge cross product + b3Transform xfA2 = b3GetFinalSweepTransform( sweepA ); + b3Transform xfB2 = b3GetFinalSweepTransform( sweepB ); + bool fastEdges = b3CheckFastEdges( xfA2, localEdgeA, xfB2, localEdgeB, axis ); + if ( fastEdges == true ) + { + // Not safe to use local edges, fall back to initial world space axis instead + fcn.type = b3_separationVertices; + fcn.witness1 = b3Normalize( axis ); + } + else + { + // Edge cross product is safe. This converges faster than a fixed axis. + fcn.type = b3_separationEdges; + fcn.witness1 = localEdgeA; + fcn.witness2 = localEdgeB; + } + } + } + else + { + B3_VALIDATE( b3IsNormalized( query->normal ) ); + + // Vertex versus edge, use world axis witness + fcn.type = b3_separationVertices; + fcn.witness1 = query->normal; + } + } + break; + + case 3: + { + if ( uniqueCountA == 3 ) + { + b3Vec3 vA1 = proxyA->points[indexA[0]]; + b3Vec3 vA2 = proxyA->points[indexA[1]]; + b3Vec3 vA3 = proxyA->points[indexA[2]]; + b3Vec3 localAxisA = b3Cross( b3Sub( vA2, vA1 ), b3Sub( vA3, vA1 ) ); + localAxisA = b3Normalize( localAxisA ); + b3Vec3 axisA = b3RotateVector( qA, localAxisA ); + + b3Vec3 localPointA = b3MulSV( 1.0f / 3.0f, b3Add( b3Add( vA1, vA2 ), vA3 ) ); + b3Vec3 localPointB = proxyB->points[indexB[0]]; + b3Vec3 delta = b3Add( b3Sub( b3RotateVector( qB, localPointB ), b3RotateVector( qA, localPointA ) ), deltaP ); + + if ( b3Dot( delta, axisA ) < 0.0f ) + { + // Make axis point from A to B + localAxisA = b3Neg( localAxisA ); + } + + // Witness is the local plane of faceA + fcn.type = b3_separationFaceA; + fcn.witness1 = localAxisA; + fcn.witness2 = localPointA; + } + else if ( uniqueCountB == 3 ) + { + b3Vec3 vB1 = proxyB->points[indexB[0]]; + b3Vec3 vB2 = proxyB->points[indexB[1]]; + b3Vec3 vB3 = proxyB->points[indexB[2]]; + b3Vec3 localAxisB = b3Cross( b3Sub( vB2, vB1 ), b3Sub( vB3, vB1 ) ); + localAxisB = b3Normalize( localAxisB ); + b3Vec3 axisB = b3RotateVector( qB, localAxisB ); + + b3Vec3 localPointA = proxyA->points[indexA[0]]; + b3Vec3 localPointB = b3MulSV( 1.0f / 3.0f, b3Add( b3Add( vB1, vB2 ), vB3 ) ); + b3Vec3 delta = b3Sub( b3Sub( b3RotateVector( qA, localPointA ), b3RotateVector( qB, localPointB ) ), deltaP ); + + if ( b3Dot( delta, axisB ) < 0.0f ) + { + // Make axis point from B to A + localAxisB = b3Neg( localAxisB ); + } + + // Witness is the local plane of faceB + fcn.type = b3_separationFaceB; + fcn.witness1 = localAxisB; + fcn.witness2 = localPointB; + } + else + { + B3_ASSERT( uniqueCountA == 2 && uniqueCountB == 2 ); + + if ( indexA[0] == indexA[1] ) + { + // Make first two indices are unique + indexA[1] = indexA[2]; + B3_ASSERT( indexA[0] != indexA[1] ); + } + + b3Vec3 vA1 = proxyA->points[indexA[0]]; + b3Vec3 vA2 = proxyA->points[indexA[1]]; + b3Vec3 localEdgeA = b3Normalize( b3Sub( vA2, vA1 ) ); + b3Vec3 edgeA = b3RotateVector( qA, localEdgeA ); + + if ( indexB[0] == indexB[1] ) + { + // Make first two indices are unique + indexB[1] = indexB[2]; + B3_ASSERT( indexB[0] != indexB[1] ); + } + + b3Vec3 vB1 = proxyB->points[indexB[0]]; + b3Vec3 vB2 = proxyB->points[indexB[1]]; + b3Vec3 localEdgeB = b3Normalize( b3Sub( vB2, vB1 ) ); + b3Vec3 edgeB = b3RotateVector( qB, localEdgeB ); + + b3Vec3 axis = b3Cross( edgeA, edgeB ); + float lengthSquared = b3LengthSquared( axis ); + + // Skip near parallel edges: |e1 x e1| = sin(alpha) * |e1| * |e2| + const float kToleranceSquared = 0.005f * 0.005f; + if ( lengthSquared < kToleranceSquared ) + { + // The axis is not safe to normalize so we use a world axis instead! + fcn.type = b3_separationVertices; + fcn.witness1 = query->normal; + } + else + { + b3Vec3 delta = b3Add( b3Sub( b3RotateVector( qB, vB1 ), b3RotateVector( qA, vA1 ) ), deltaP ); + if ( b3Dot( delta, axis ) < 0.0f ) + { + // Make axis point from A to B + axis = b3Neg( axis ); + localEdgeB = b3Neg( localEdgeB ); + } + + // Check for possible sign flip in edge/edge cross product + b3Transform xfA2 = b3GetFinalSweepTransform( sweepA ); + b3Transform xfB2 = b3GetFinalSweepTransform( sweepB ); + bool fastEdges = b3CheckFastEdges( xfA2, localEdgeA, xfB2, localEdgeB, axis ); + if ( fastEdges ) + { + // Not safe to use local edges, fall back to initial world space axis instead + fcn.type = b3_separationVertices; + fcn.witness1 = b3Normalize( axis ); + } + else + { + // Edge cross product is safe. This converges faster than a fixed axis. + fcn.type = b3_separationEdges; + fcn.witness1 = localEdgeA; + fcn.witness2 = localEdgeB; + } + } + } + } + break; + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + return fcn; +} + +static float b3FindMinSeparation( b3SeparationFunction* fcn, int* indexA, int* indexB, float t ) +{ + b3Transform xfA = b3GetSweepTransform( &fcn->sweepA, t ); + b3Transform xfB = b3GetSweepTransform( &fcn->sweepB, t ); + + switch ( fcn->type ) + { + case b3_separationVertices: + { + b3Vec3 axis = fcn->witness1; + + b3Vec3 localAxisA = b3InvRotateVector( xfA.q, axis ); + b3Vec3 localAxisB = b3InvRotateVector( xfB.q, b3Neg( axis ) ); + + *indexA = b3GetPointSupport( fcn->proxyA->points, fcn->proxyA->count, localAxisA ); + *indexB = b3GetPointSupport( fcn->proxyB->points, fcn->proxyB->count, localAxisB ); + + b3Vec3 deltaP = b3Sub( xfB.p, xfA.p ); + b3Vec3 localPointA = fcn->proxyA->points[*indexA]; + b3Vec3 localPointB = fcn->proxyB->points[*indexB]; + b3Vec3 delta = b3Add( b3Sub( b3RotateVector( xfB.q, localPointB ), b3RotateVector( xfA.q, localPointA ) ), deltaP ); + return b3Dot( delta, axis ); + } + + case b3_separationEdges: + { + b3Vec3 edgeA = b3RotateVector( xfA.q, fcn->witness1 ); + b3Vec3 edgeB = b3RotateVector( xfB.q, fcn->witness2 ); + b3Vec3 axis = b3Cross( edgeA, edgeB ); + B3_ASSERT( axis.x != 0.0f || axis.y != 0.0f || axis.z != 0.0f ); + axis = b3Normalize( axis ); + + b3Vec3 axisA = b3InvRotateVector( xfA.q, axis ); + *indexA = b3GetPointSupport( fcn->proxyA->points, fcn->proxyA->count, axisA ); + + b3Vec3 axisB = b3InvRotateVector( xfB.q, axis ); + *indexB = b3GetPointSupport( fcn->proxyB->points, fcn->proxyB->count, b3Neg( axisB ) ); + + b3Vec3 deltaP = b3Sub( xfB.p, xfA.p ); + b3Vec3 localPointA = fcn->proxyA->points[*indexA]; + b3Vec3 localPointB = fcn->proxyB->points[*indexB]; + b3Vec3 delta = b3Add( b3Sub( b3RotateVector( xfB.q, localPointB ), b3RotateVector( xfA.q, localPointA ) ), deltaP ); + + return b3Dot( delta, axis ); + } + + case b3_separationFaceA: + { + b3Vec3 normal = b3RotateVector( xfA.q, fcn->witness1 ); + *indexA = -1; + b3Vec3 pointA = b3TransformPoint( xfA, fcn->witness2 ); + + b3Vec3 axisB = b3InvRotateVector( xfB.q, normal ); + *indexB = b3GetPointSupport( fcn->proxyB->points, fcn->proxyB->count, b3Neg( axisB ) ); + b3Vec3 pointB = b3TransformPoint( xfB, fcn->proxyB->points[*indexB] ); + + return b3Dot( b3Sub( pointB, pointA ), normal ); + } + + case b3_separationFaceB: + { + b3Vec3 normal = b3RotateVector( xfB.q, fcn->witness1 ); + + b3Vec3 axisA = b3InvRotateVector( xfA.q, normal ); + *indexA = b3GetPointSupport( fcn->proxyA->points, fcn->proxyA->count, b3Neg( axisA ) ); + b3Vec3 pointA = b3TransformPoint( xfA, fcn->proxyA->points[*indexA] ); + + *indexB = -1; + b3Vec3 pointB = b3TransformPoint( xfB, fcn->witness2 ); + + return b3Dot( b3Sub( pointA, pointB ), normal ); + } + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + return 0.0f; +} + +static float b3EvaluateSeparation( b3SeparationFunction* fcn, int index1, int index2, float beta ) +{ + b3Transform transform1 = b3GetSweepTransform( &fcn->sweepA, beta ); + b3Transform transform2 = b3GetSweepTransform( &fcn->sweepB, beta ); + + switch ( fcn->type ) + { + case b3_separationVertices: + { + b3Vec3 axis = fcn->witness1; + + b3Vec3 point1 = b3TransformPoint( transform1, fcn->proxyA->points[index1] ); + b3Vec3 point2 = b3TransformPoint( transform2, fcn->proxyB->points[index2] ); + + return b3Dot( b3Sub( point2, point1 ), axis ); + } + + case b3_separationEdges: + { + b3Vec3 edge1 = b3RotateVector( transform1.q, fcn->witness1 ); + b3Vec3 edge2 = b3RotateVector( transform2.q, fcn->witness2 ); + b3Vec3 axis = b3Cross( edge1, edge2 ); + axis = b3Normalize( axis ); + + b3Vec3 point1 = b3TransformPoint( transform1, fcn->proxyA->points[index1] ); + b3Vec3 point2 = b3TransformPoint( transform2, fcn->proxyB->points[index2] ); + + return b3Dot( b3Sub( point2, point1 ), axis ); + } + + case b3_separationFaceA: + { + b3Vec3 axis = b3RotateVector( transform1.q, fcn->witness1 ); + + b3Vec3 point1 = b3TransformPoint( transform1, fcn->witness2 ); + b3Vec3 point2 = b3TransformPoint( transform2, fcn->proxyB->points[index2] ); + + return b3Dot( b3Sub( point2, point1 ), axis ); + } + + case b3_separationFaceB: + { + b3Vec3 axis = b3RotateVector( transform2.q, fcn->witness1 ); + + b3Vec3 point1 = b3TransformPoint( transform1, fcn->proxyA->points[index1] ); + b3Vec3 point2 = b3TransformPoint( transform2, fcn->witness2 ); + + return b3Dot( b3Sub( point1, point2 ), axis ); + } + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + return 0.0f; +} + +static void b3ForceFixedAxis( b3SeparationFunction* fcn, float beta ) +{ + B3_ASSERT( fcn->type == b3_separationEdges ); + + b3Transform transform1 = b3GetSweepTransform( &fcn->sweepA, beta ); + b3Transform transform2 = b3GetSweepTransform( &fcn->sweepB, beta ); + + b3Vec3 edge1 = b3RotateVector( transform1.q, fcn->witness1 ); + b3Vec3 edge2 = b3RotateVector( transform2.q, fcn->witness2 ); + b3Vec3 axis = b3Cross( edge1, edge2 ); + axis = b3Normalize( axis ); + + fcn->type = b3_separationVertices; + fcn->witness1 = axis; + fcn->witness2 = b3Vec3_zero; +} + +// Time of Impact using root finding +b3TOIOutput b3TimeOfImpact( const b3TOIInput* input ) +{ + b3TOIOutput output = { 0 }; + + // Set these to invalid values so they can be validated on exit + output.state = b3_toiStateUnknown; + output.fraction = -1.0f; + + b3Sweep sweepA = input->sweepA; + b3Sweep sweepB = input->sweepB; + + // Shift to origin + b3Vec3 origin = sweepA.c1; + sweepA.c1 = b3Vec3_zero; + sweepA.c2 = b3Sub( sweepA.c2, origin ); + sweepB.c1 = b3Sub( sweepB.c1, origin ); + sweepB.c2 = b3Sub( sweepB.c2, origin ); + + b3ShapeProxy proxyA = input->proxyA; + b3ShapeProxy proxyB = input->proxyB; + + int maxPushBackIterations = proxyA.count + proxyB.count; + float tMax = input->maxFraction; + + // Setup target distance and tolerance + float linearSlop = B3_LINEAR_SLOP; + float totalRadius = proxyA.radius + proxyB.radius; + float target = b3MaxFloat( linearSlop, totalRadius - linearSlop ); + float tolerance = 0.25f * linearSlop; + B3_ASSERT( target > tolerance ); + + float t1 = 0.0f; + const int maxIterations = 25; + int distanceIterations = 0; + + // Prepare input for distance query. + b3SimplexCache cache = { 0 }; + b3DistanceInput distanceInput = { 0 }; + distanceInput.proxyA = proxyA; + distanceInput.proxyB = proxyB; + distanceInput.useRadii = false; + + // The outer loop progressively attempts to compute new separating axes. + // This loop terminates when an axis is repeated (no progress is made). + for ( ;; ) + { + // Get the distance between shapes. We can also use the results to get a separating axis. + distanceInput.transformA = b3GetSweepTransform( &sweepA, t1 ); + distanceInput.transformB = b3GetSweepTransform( &sweepB, t1 ); + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); + output.distance = distanceOutput.distance; + + output.distanceIterations += 1; + distanceIterations += 1; + + // If the shapes are overlapped, we give up on continuous collision. + if ( distanceOutput.distance <= 0.0f ) + { + output.state = b3_toiStateOverlapped; + output.fraction = 0.0f; + break; + } + + if ( distanceOutput.distance <= target + tolerance ) + { + // Success! + output.state = b3_toiStateHit; + + // Averaged hit point + b3Vec3 pA = b3MulAdd( distanceOutput.pointA, proxyA.radius, distanceOutput.normal ); + b3Vec3 pB = b3MulAdd( distanceOutput.pointB, -proxyB.radius, distanceOutput.normal ); + output.point = b3Lerp( pA, pB, 0.5f ); + output.point = b3Add( output.point, origin ); + output.normal = distanceOutput.normal; + output.fraction = t1; + break; + } + + if ( distanceIterations == maxIterations ) + { + // Progress too slow. This can happen when a capsule rotates around a + // triangle vertex. + output.state = b3_toiStateFailed; + output.fraction = t1; + + // Averaged hit point + b3Vec3 pA = b3MulAdd( distanceOutput.pointA, input->proxyA.radius, distanceOutput.normal ); + b3Vec3 pB = b3MulAdd( distanceOutput.pointB, -input->proxyB.radius, distanceOutput.normal ); + output.point = b3Lerp( pA, pB, 0.5f ); + output.point = b3Add( output.point, origin ); + output.normal = distanceOutput.normal; + break; + } + + // Initialize the separating axis. + b3SeparationFunction function = + b3MakeSeparationFunction( cache, &proxyA, &sweepA, &proxyB, &sweepB, &distanceOutput, t1 ); + +#if B3_ENABLE_VALIDATION && 0 + // todo this can give a negative value for diagonal edge contact on faces, typical GJK problem + // to fix this I think the separation function would need to identify faces + { + int index1, index2; + float minSeparation = b3FindMinSeparation( &function, &index1, &index2, t1 ); + // SAT should give a closer result than GJK + B3_VALIDATE( minSeparation > target - tolerance && minSeparation - distanceOutput.distance < 0.1f * B3_LINEAR_SLOP ); + } +#endif + + // Compute the TOI on the separating axis. We do this by successively resolving the deepest point. + bool done = false; + float t2 = tMax; + int pushBackIterations = 0; + for ( ;; ) + { + int indexA, indexB; + float s2 = b3FindMinSeparation( &function, &indexA, &indexB, t2 ); + + // Dump the function seen by the root finder + // for ( int Step = 0; Step <= 100; ++Step ) + // { + // float Alpha = 0.01f * Step; + // float Separation = Function.Evaluate( Index1, Index2, Alpha ); + // + // b3Report( "s(%4.2g) = %g\n", Alpha, Separation ); + // } + + // Is the final configuration separated? + if ( s2 - target > tolerance ) + { + // Success! + output.state = b3_toiStateSeparated; + output.fraction = input->maxFraction; + done = true; + break; + } + + // Has the separation reached tolerance? + if ( s2 >= target - tolerance ) + { + // Advance the sweeps + t1 = t2; + break; + } + + // Compute the initial separation of the witness points + float s1 = b3EvaluateSeparation( &function, indexA, indexB, t1 ); + + // Check for overlap. This might happen if the root finder runs out of iterations. + if ( s1 < target - tolerance ) + { + // Failed! + B3_VALIDATE( false ); + output.state = b3_toiStateFailed; + output.fraction = t1; + done = true; + break; + } + + // Has the separation reached tolerance? + if ( s1 <= target + tolerance ) + { + // Success! t1 should hold the TOI (could be 0.0) + output.state = b3_toiStateHit; + output.fraction = t1; + done = true; + break; + } + + // Compute 1D root of: f(x) - target = 0 + int rootIterationCount = 0; + int maxRootIterations = 50; + float a1 = t1; + float a2 = t2; + for ( ;; ) + { + // Use a mix of false position and bisection. + float t; + if ( rootIterationCount & 1 ) + { + // False position to improve convergence. + t = a1 + ( target - s1 ) * ( a2 - a1 ) / ( s2 - s1 ); + } + else + { + // Bisection to guarantee progress. + t = 0.5f * ( a1 + a2 ); + } + + output.rootIterations += 1; + rootIterationCount += 1; + + float s = b3EvaluateSeparation( &function, indexA, indexB, t ); + + // Has the separation reached tolerance? + if ( b3AbsFloat( s - target ) <= tolerance ) + { + // t2 holds a tentative value for t1 + t2 = t; + break; + } + + // Ensure we continue to bracket the root. + if ( s > target ) + { + a1 = t; + s1 = s; + } + else + { + a2 = t; + s2 = s; + } + + if ( rootIterationCount == maxRootIterations ) + { + B3_VALIDATE( false ); + break; + } + } + + // Restart the inner loop if we have a failing edge case. + if ( rootIterationCount == maxRootIterations - 1 && function.type == b3_separationEdges ) + { + B3_VALIDATE( false ); + + rootIterationCount = 0; + t2 = input->maxFraction; + b3ForceFixedAxis( &function, t1 ); + B3_ASSERT( function.type != b3_separationEdges ); + } + + output.pushBackIterations += 1; + pushBackIterations += 1; + + if ( pushBackIterations == maxPushBackIterations ) + { + break; + } + } + + if ( done ) + { + // Averaged hit point + b3Vec3 pA = b3MulAdd( distanceOutput.pointA, input->proxyA.radius, distanceOutput.normal ); + b3Vec3 pB = b3MulAdd( distanceOutput.pointB, -input->proxyB.radius, distanceOutput.normal ); + output.point = b3Lerp( pA, pB, 0.5f ); + output.point = b3Add( output.point, origin ); + output.normal = distanceOutput.normal; + break; + } + } + + // It is expected that the state and fraction are set before reaching this + B3_ASSERT( output.state != b3_toiStateUnknown ); + B3_ASSERT( output.fraction >= 0.0f ); + + return output; +} diff --git a/src/distance_joint.c b/src/distance_joint.c new file mode 100644 index 0000000..382b80f --- /dev/null +++ b/src/distance_joint.c @@ -0,0 +1,558 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "physics_world.h" +#include "solver.h" +#include "solver_set.h" + +// needed for dll export +#include "box3d/box3d.h" + +void b3DistanceJoint_SetLength( b3JointId jointId, float length ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + b3DistanceJoint* joint = &base->distanceJoint; + + joint->length = b3ClampFloat( length, B3_LINEAR_SLOP, B3_HUGE ); + joint->impulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; +} + +float b3DistanceJoint_GetLength( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + b3DistanceJoint* joint = &base->distanceJoint; + return joint->length; +} + +void b3DistanceJoint_EnableLimit( b3JointId jointId, bool enableLimit ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + b3DistanceJoint* joint = &base->distanceJoint; + joint->enableLimit = enableLimit; +} + +bool b3DistanceJoint_IsLimitEnabled( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + return joint->distanceJoint.enableLimit; +} + +void b3DistanceJoint_SetLengthRange( b3JointId jointId, float minLength, float maxLength ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + b3DistanceJoint* joint = &base->distanceJoint; + + minLength = b3ClampFloat( minLength, B3_LINEAR_SLOP, B3_HUGE ); + maxLength = b3ClampFloat( maxLength, B3_LINEAR_SLOP, B3_HUGE ); + joint->minLength = b3MinFloat( minLength, maxLength ); + joint->maxLength = b3MaxFloat( minLength, maxLength ); + joint->impulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; +} + +float b3DistanceJoint_GetMinLength( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + b3DistanceJoint* joint = &base->distanceJoint; + return joint->minLength; +} + +float b3DistanceJoint_GetMaxLength( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + b3DistanceJoint* joint = &base->distanceJoint; + return joint->maxLength; +} + +float b3DistanceJoint_GetCurrentLength( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + + b3World* world = b3GetUnlockedWorld( jointId.world0 ); + if ( world == NULL ) + { + return 0.0f; + } + + b3Transform transformA = b3GetBodyTransform( world, base->bodyIdA ); + b3Transform transformB = b3GetBodyTransform( world, base->bodyIdB ); + + b3Vec3 pA = b3TransformPoint( transformA, base->localFrameA.p ); + b3Vec3 pB = b3TransformPoint( transformB, base->localFrameB.p ); + b3Vec3 d = b3Sub( pB, pA ); + float length = b3Length( d ); + return length; +} + +void b3DistanceJoint_EnableSpring( b3JointId jointId, bool enableSpring ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + base->distanceJoint.enableSpring = enableSpring; +} + +bool b3DistanceJoint_IsSpringEnabled( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + return base->distanceJoint.enableSpring; +} + + +void b3DistanceJoint_SetSpringForceRange( b3JointId jointId, float lowerForce, float upperForce ) +{ + B3_ASSERT( lowerForce <= upperForce ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + base->distanceJoint.lowerSpringForce = lowerForce; + base->distanceJoint.upperSpringForce = upperForce; +} + +void b3DistanceJoint_GetSpringForceRange( b3JointId jointId, float* lowerForce, float* upperForce ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + *lowerForce = base->distanceJoint.lowerSpringForce; + *upperForce = base->distanceJoint.upperSpringForce; +} + +void b3DistanceJoint_SetSpringHertz( b3JointId jointId, float hertz ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + base->distanceJoint.hertz = hertz; +} + +void b3DistanceJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + base->distanceJoint.dampingRatio = dampingRatio; +} + +float b3DistanceJoint_GetSpringHertz( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + b3DistanceJoint* joint = &base->distanceJoint; + return joint->hertz; +} + +float b3DistanceJoint_GetSpringDampingRatio( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + b3DistanceJoint* joint = &base->distanceJoint; + return joint->dampingRatio; +} + +void b3DistanceJoint_EnableMotor( b3JointId jointId, bool enableMotor ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + if ( enableMotor != joint->distanceJoint.enableMotor ) + { + joint->distanceJoint.enableMotor = enableMotor; + joint->distanceJoint.motorImpulse = 0.0f; + } +} + +bool b3DistanceJoint_IsMotorEnabled( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + return joint->distanceJoint.enableMotor; +} + +void b3DistanceJoint_SetMotorSpeed( b3JointId jointId, float motorSpeed ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + joint->distanceJoint.motorSpeed = motorSpeed; +} + +float b3DistanceJoint_GetMotorSpeed( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + return joint->distanceJoint.motorSpeed; +} + +float b3DistanceJoint_GetMotorForce( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + return world->inv_h * base->distanceJoint.motorImpulse; +} + +void b3DistanceJoint_SetMaxMotorForce( b3JointId jointId, float force ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + joint->distanceJoint.maxMotorForce = force; +} + +float b3DistanceJoint_GetMaxMotorForce( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + return joint->distanceJoint.maxMotorForce; +} + +b3Vec3 b3GetDistanceJointForce( b3World* world, b3JointSim* base ) +{ + b3DistanceJoint* joint = &base->distanceJoint; + + b3Transform transformA = b3GetBodyTransform( world, base->bodyIdA ); + b3Transform transformB = b3GetBodyTransform( world, base->bodyIdB ); + + b3Vec3 pA = b3TransformPoint( transformA, base->localFrameA.p ); + b3Vec3 pB = b3TransformPoint( transformB, base->localFrameB.p ); + b3Vec3 d = b3Sub( pB, pA ); + b3Vec3 axis = b3Normalize( d ); + float force = ( joint->impulse + joint->lowerImpulse - joint->upperImpulse + joint->motorImpulse ) * world->inv_h; + return b3MulSV( force, axis ); +} + +// 1-D constrained system +// m (v2 - v1) = lambda +// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass. +// x2 = x1 + h * v2 + +// 1-D mass-damper-spring system +// m (v2 - v1) + h * d * v2 + h * k * + +// C = norm(p2 - p1) - L +// u = (p2 - p1) / norm(p2 - p1) +// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1)) +// J = [-u -cross(r1, u) u cross(r2, u)] +// K = J * invM * JT +// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2 + +void b3PrepareDistanceJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_distanceJoint ); + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b3World* world = context->world; + b3Body* bodyA = b3Array_Get( world->bodies, idA ); + b3Body* bodyB = b3Array_Get( world->bodies, idB ); + + B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); + + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + float mA = bodySimA->invMass; + b3Matrix3 iA = bodySimA->invInertiaWorld; + float mB = bodySimB->invMass; + b3Matrix3 iB = bodySimB->invInertiaWorld; + + base->invMassA = mA; + base->invMassB = mB; + base->invIA = iA; + base->invIB = iB; + + b3DistanceJoint* joint = &base->distanceJoint; + + joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX; + joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX; + + // initial anchors in world space + joint->anchorA = b3RotateVector( bodySimA->transform.q, b3Sub( base->localFrameA.p, bodySimA->localCenter ) ); + joint->anchorB = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) ); + joint->deltaCenter = b3Sub( bodySimB->center, bodySimA->center ); + + b3Vec3 rA = joint->anchorA; + b3Vec3 rB = joint->anchorB; + b3Vec3 separation = b3Add( b3Sub( rB, rA ), joint->deltaCenter ); + b3Vec3 axis = b3Normalize( separation ); + + // compute effective mass + b3Vec3 crA = b3Cross( rA, axis ); + b3Vec3 crB = b3Cross( rB, axis ); + float k = mA + mB + b3Dot( crA, b3MulMV( iA, crA ) ) + b3Dot( crB, b3MulMV( iB, crB ) ); + joint->axialMass = k > 0.0f ? 1.0f / k : 0.0f; + + joint->distanceSoftness = b3MakeSoft( joint->hertz, joint->dampingRatio, context->h ); + + if ( context->enableWarmStarting == false ) + { + joint->impulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; + joint->motorImpulse = 0.0f; + } +} + +void b3WarmStartDistanceJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_distanceJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3DistanceJoint* joint = &base->distanceJoint; + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->anchorA ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->anchorB ); + + b3Vec3 ds = b3Add( b3Sub( stateB->deltaPosition, stateA->deltaPosition ), b3Sub( rB, rA ) ); + b3Vec3 separation = b3Add( joint->deltaCenter, ds ); + b3Vec3 axis = b3Normalize( separation ); + + float axialImpulse = joint->impulse + joint->lowerImpulse - joint->upperImpulse + joint->motorImpulse; + b3Vec3 P = b3MulSV( axialImpulse, axis ); + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = b3MulSub( stateA->linearVelocity, mA, P ); + stateA->angularVelocity = b3Sub( stateA->angularVelocity, b3MulMV( iA, b3Cross( rA, P ) ) ); + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = b3MulAdd( stateB->linearVelocity, mB, P ); + stateB->angularVelocity = b3Add( stateB->angularVelocity, b3MulMV( iB, b3Cross( rB, P ) ) ); + } +} + +void b3SolveDistanceJoint( b3JointSim* base, b3StepContext* context, bool useBias ) +{ + B3_ASSERT( base->type == b3_distanceJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3DistanceJoint* joint = &base->distanceJoint; + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + // current anchors + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->anchorA ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->anchorB ); + + // current separation + b3Vec3 ds = b3Add( b3Sub( stateB->deltaPosition, stateA->deltaPosition ), b3Sub( rB, rA ) ); + b3Vec3 separation = b3Add( joint->deltaCenter, ds ); + + float length = b3Length( separation ); + b3Vec3 axis = b3Normalize( separation ); + + // joint is soft if + // - spring is enabled + // - and (joint limit is disabled or limits are not equal) + if ( joint->enableSpring && ( joint->minLength < joint->maxLength || joint->enableLimit == false ) ) + { + // spring + if ( joint->hertz > 0.0f ) + { + // Cdot = dot(u, v + cross(w, r)) + b3Vec3 vr = b3Add( b3Sub( vB, vA ), b3Sub( b3Cross( wB, rB ), b3Cross( wA, rA ) ) ); + float Cdot = b3Dot( 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 h = context->h; + joint->impulse = b3ClampFloat( joint->impulse + impulse, joint->lowerSpringForce * h, joint->upperSpringForce * h ); + impulse = joint->impulse - oldImpulse; + + b3Vec3 P = b3MulSV( impulse, axis ); + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); + } + + if ( joint->enableLimit ) + { + // lower limit + { + b3Vec3 vr = b3Add( b3Sub( vB, vA ), b3Sub( b3Cross( wB, rB ), b3Cross( wA, rA ) ) ); + float Cdot = b3Dot( axis, vr ); + + float C = length - joint->minLength; + + float bias = 0.0f; + float massCoeff = 1.0f; + float impulseCoeff = 0.0f; + if ( C > 0.0f ) + { + // speculative + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * C; + massCoeff = base->constraintSoftness.massScale; + impulseCoeff = base->constraintSoftness.impulseScale; + } + + float impulse = -massCoeff * joint->axialMass * ( Cdot + bias ) - impulseCoeff * joint->lowerImpulse; + float newImpulse = b3MaxFloat( 0.0f, joint->lowerImpulse + impulse ); + impulse = newImpulse - joint->lowerImpulse; + joint->lowerImpulse = newImpulse; + + b3Vec3 P = b3MulSV( impulse, axis ); + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); + } + + // upper + { + b3Vec3 vr = b3Add( b3Sub( vA, vB ), b3Sub( b3Cross( wA, rA ), b3Cross( wB, rB ) ) ); + float Cdot = b3Dot( axis, vr ); + + float C = joint->maxLength - length; + + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( C > 0.0f ) + { + // speculative + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * C; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->upperImpulse; + float newImpulse = b3MaxFloat( 0.0f, joint->upperImpulse + impulse ); + impulse = newImpulse - joint->upperImpulse; + joint->upperImpulse = newImpulse; + + b3Vec3 P = b3MulSV( -impulse, axis ); + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); + } + } + + if ( joint->enableMotor ) + { + b3Vec3 vr = b3Add( b3Sub( vB, vA ), b3Sub( b3Cross( wB, rB ), b3Cross( wA, rA ) ) ); + float Cdot = b3Dot( axis, vr ); + float impulse = joint->axialMass * ( joint->motorSpeed - Cdot ); + float oldImpulse = joint->motorImpulse; + float maxImpulse = context->h * joint->maxMotorForce; + joint->motorImpulse = b3ClampFloat( joint->motorImpulse + impulse, -maxImpulse, maxImpulse ); + impulse = joint->motorImpulse - oldImpulse; + + b3Vec3 P = b3MulSV( impulse, axis ); + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); + } + } + else + { + // rigid constraint + b3Vec3 vr = b3Add( b3Sub( vB, vA ), b3Sub( b3Cross( wB, rB ), b3Cross( wA, rA ) ) ); + float Cdot = b3Dot( axis, vr ); + + float C = length - joint->length; + + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias ) + { + bias = base->constraintSoftness.biasRate * C; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->impulse; + joint->impulse += impulse; + + b3Vec3 P = b3MulSV( impulse, axis ); + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +void b3DrawDistanceJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB ) +{ + B3_ASSERT( base->type == b3_distanceJoint ); + + b3DistanceJoint* joint = &base->distanceJoint; + + b3Vec3 pA = b3TransformPoint( transformA, base->localFrameA.p ); + b3Vec3 pB = b3TransformPoint( transformB, base->localFrameB.p ); + + b3Vec3 axis = b3Normalize( b3Sub( pB, pA ) ); + + if ( joint->minLength < joint->maxLength && joint->enableLimit ) + { + b3Vec3 pMin = b3MulAdd( pA, joint->minLength, axis ); + b3Vec3 pMax = b3MulAdd( pA, joint->maxLength, axis ); + + if ( joint->minLength > B3_LINEAR_SLOP ) + { + draw->DrawPointFcn( pMin, 6.0f, b3_colorLightGreen, draw->context ); + } + + if ( joint->maxLength < B3_HUGE ) + { + draw->DrawPointFcn(pMax, 6.0f, b3_colorRed, draw->context); + } + + if ( joint->minLength > B3_LINEAR_SLOP && joint->maxLength < B3_HUGE ) + { + draw->DrawSegmentFcn( pMin, pMax, b3_colorGray, draw->context ); + } + } + + draw->DrawSegmentFcn( pA, pB, b3_colorWhite, draw->context ); + draw->DrawPointFcn( pA, 4.0f, b3_colorWhite, draw->context ); + draw->DrawPointFcn( pB, 4.0f, b3_colorWhite, draw->context ); + + if ( joint->hertz > 0.0f && joint->enableSpring ) + { + b3Vec3 pRest = b3MulAdd( pA, joint->length, axis ); + draw->DrawPointFcn( pRest, 4.0f, b3_colorBlue, draw->context ); + } +} diff --git a/src/dynamic_tree.c b/src/dynamic_tree.c new file mode 100644 index 0000000..3f34b67 --- /dev/null +++ b/src/dynamic_tree.c @@ -0,0 +1,2217 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "aabb.h" +#include "core.h" +#include "joint.h" +#include "simd.h" + +#include "box3d/collision.h" +#include "box3d/constants.h" +#include "box3d/math_functions.h" + +#include +#include +#include + +#define B3_TREE_STACK_SIZE 1024 + +static b3TreeNode b3_defaultTreeNode = { + .aabb = { { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f } }, + .categoryBits = B3_DEFAULT_CATEGORY_BITS, + .children = + { + .child1 = B3_NULL_INDEX, + .child2 = B3_NULL_INDEX, + }, + .parent = B3_NULL_INDEX, + .height = 0, + .flags = b3_allocatedNode, +}; + +static inline bool b3IsLeaf( const b3TreeNode* node ) +{ + return node->flags & b3_leafNode; +} + +static inline bool b3IsAllocated( const b3TreeNode* node ) +{ + return node->flags & b3_allocatedNode; +} + +static inline uint16_t b3MaxUInt16( uint16_t a, uint16_t b ) +{ + return a > b ? a : b; +} + +b3DynamicTree b3DynamicTree_Create( int proxyCapacity ) +{ + int capacity = b3MaxInt( proxyCapacity, 16 ); + + b3DynamicTree tree; + + // memset needed for deterministic serialization + memset( &tree, 0, sizeof( b3DynamicTree ) ); + + tree.version = B3_DYNAMIC_TREE_VERSION; + tree.root = B3_NULL_INDEX; + + // maximum node count for a full binary tree is 2 * leafCount - 1 + tree.nodeCapacity = 2 * capacity - 1; + tree.nodeCount = 0; + + tree.nodes = (b3TreeNode*)b3Alloc( tree.nodeCapacity * sizeof( b3TreeNode ) ); + + // Better to have 64 byte alignment + B3_VALIDATE( ( (uintptr_t)tree.nodes & 0x3F ) == 0 ); + + memset( tree.nodes, 0, tree.nodeCapacity * sizeof( b3TreeNode ) ); + + // Build a linked list for the free list. + // todo use a bump allocator until the capacity is consumed (see b3PoolAllocator) + for ( int i = 0; i < tree.nodeCapacity - 1; ++i ) + { + tree.nodes[i].next = i + 1; + } + + tree.nodes[tree.nodeCapacity - 1].next = B3_NULL_INDEX; + tree.freeList = 0; + + tree.proxyCount = 0; + + tree.leafIndices = NULL; + tree.leafBoxes = NULL; + tree.leafCenters = NULL; + tree.binIndices = NULL; + tree.rebuildCapacity = 0; + + return tree; +} + +void b3DynamicTree_Destroy( b3DynamicTree* tree ) +{ + b3Free( tree->nodes, tree->nodeCapacity * sizeof( b3TreeNode ) ); + b3Free( tree->leafIndices, tree->rebuildCapacity * sizeof( int ) ); + b3Free( tree->leafBoxes, tree->rebuildCapacity * sizeof( b3AABB ) ); + b3Free( tree->leafCenters, tree->rebuildCapacity * sizeof( b3Vec3 ) ); + b3Free( tree->binIndices, tree->rebuildCapacity * sizeof( int ) ); + + memset( tree, 0, sizeof( b3DynamicTree ) ); +} + +// Allocate a node from the pool. Grow the pool if necessary. +static int b3AllocateNode( b3DynamicTree* tree ) +{ + // Expand the node pool as needed. + if ( tree->freeList == B3_NULL_INDEX ) + { + B3_ASSERT( tree->nodeCount == tree->nodeCapacity ); + + // The free list is empty. Rebuild a bigger pool. + b3TreeNode* oldNodes = tree->nodes; + int oldCapacity = tree->nodeCapacity; + tree->nodeCapacity += oldCapacity >> 1; + tree->nodes = (b3TreeNode*)b3Alloc( tree->nodeCapacity * sizeof( b3TreeNode ) ); + B3_ASSERT( oldNodes != NULL ); + memcpy( tree->nodes, oldNodes, tree->nodeCount * sizeof( b3TreeNode ) ); + memset( tree->nodes + tree->nodeCount, 0, ( tree->nodeCapacity - tree->nodeCount ) * sizeof( b3TreeNode ) ); + b3Free( oldNodes, oldCapacity * sizeof( b3TreeNode ) ); + + // Build a linked list for the free list. The parent pointer becomes the "next" pointer. + // todo avoid building freelist? + for ( int i = tree->nodeCount; i < tree->nodeCapacity - 1; ++i ) + { + tree->nodes[i].next = i + 1; + } + + tree->nodes[tree->nodeCapacity - 1].next = B3_NULL_INDEX; + tree->freeList = tree->nodeCount; + } + + // Peel a node off the free list. + int nodeIndex = tree->freeList; + b3TreeNode* node = tree->nodes + nodeIndex; + tree->freeList = node->next; + *node = b3_defaultTreeNode; + ++tree->nodeCount; + return nodeIndex; +} + +// Return a node to the pool. +static void b3FreeNode( b3DynamicTree* tree, int nodeId ) +{ + B3_ASSERT( 0 <= nodeId && nodeId < tree->nodeCapacity ); + B3_ASSERT( 0 < tree->nodeCount ); + tree->nodes[nodeId].next = tree->freeList; + tree->nodes[nodeId].flags = 0; + tree->freeList = nodeId; + --tree->nodeCount; +} + +// Greedy algorithm for sibling selection using the SAH +// We have three nodes A-(B,C) and want to add a leaf D, there are three choices. +// 1: make a new parent for A and D : E-(A-(B,C), D) +// 2: associate D with B +// a: B is a leaf : A-(E-(B,D), C) +// b: B is an internal node: A-(B{D},C) +// 3: associate D with C +// a: C is a leaf : A-(B, E-(C,D)) +// b: C is an internal node: A-(B, C{D}) +// All of these have a clear cost except when B or C is an internal node. Hence we need to be greedy. + +// The cost for cases 1, 2a, and 3a can be computed using the sibling cost formula. +// cost of sibling H = area(union(H, D)) + increased area of ancestors + +// Suppose B (or C) is an internal node, then the lowest cost would be one of two cases: +// case1: D becomes a sibling of B +// case2: D becomes a descendant of B along with a new internal node of area(D). +static int b3FindBestSibling( const b3DynamicTree* tree, b3AABB boxD ) +{ + b3Vec3 centerD = b3AABB_Center( boxD ); + float areaD = b3Perimeter( boxD ); + + const b3TreeNode* nodes = tree->nodes; + int rootIndex = tree->root; + + b3AABB rootBox = nodes[rootIndex].aabb; + + // Area of current node + float areaBase = b3Perimeter( rootBox ); + + // Area of inflated node + float directCost = b3Perimeter( b3AABB_Union( rootBox, boxD ) ); + float inheritedCost = 0.0f; + + int bestSibling = rootIndex; + float bestCost = directCost; + + // Descend the tree from root, following a single greedy path. + int index = rootIndex; + while ( b3IsLeaf( nodes + index ) == false ) + { + int child1 = nodes[index].children.child1; + int child2 = nodes[index].children.child2; + + // Cost of creating a new parent for this node and the new leaf + float cost = directCost + inheritedCost; + + // Sometimes there are multiple identical costs within tolerance. + // This breaks the ties using the centroid distance. + if ( cost < bestCost ) + { + bestSibling = index; + bestCost = cost; + } + + // Inheritance cost seen by children + inheritedCost += directCost - areaBase; + + bool leaf1 = b3IsLeaf( nodes + child1 ); + bool leaf2 = b3IsLeaf( nodes + child2 ); + + // Cost of descending into child 1 + float lowerCost1 = FLT_MAX; + b3AABB box1 = nodes[child1].aabb; + float directCost1 = b3Perimeter( b3AABB_Union( box1, boxD ) ); + float area1 = 0.0f; + if ( leaf1 ) + { + // Child 1 is a leaf + // Cost of creating new node and increasing area of node P + float cost1 = directCost1 + inheritedCost; + + // Need this here due to while condition above + if ( cost1 < bestCost ) + { + bestSibling = child1; + bestCost = cost1; + } + } + else + { + // Child 1 is an internal node + area1 = b3Perimeter( box1 ); + + // Lower bound cost of inserting under child 1. + lowerCost1 = inheritedCost + directCost1 + b3MinFloat( areaD - area1, 0.0f ); + } + + // Cost of descending into child 2 + float lowerCost2 = FLT_MAX; + b3AABB box2 = nodes[child2].aabb; + float directCost2 = b3Perimeter( b3AABB_Union( box2, boxD ) ); + float area2 = 0.0f; + if ( leaf2 ) + { + // Child 2 is a leaf + // Cost of creating new node and increasing area of node P + float cost2 = directCost2 + inheritedCost; + + // Need this here due to while condition above + if ( cost2 < bestCost ) + { + bestSibling = child2; + bestCost = cost2; + } + } + else + { + // Child 2 is an internal node + area2 = b3Perimeter( box2 ); + + // Lower bound cost of inserting under child 2. This is not the cost + // of child 2, it is the best we can hope for under child 2. + lowerCost2 = inheritedCost + directCost2 + b3MinFloat( areaD - area2, 0.0f ); + } + + if ( leaf1 && leaf2 ) + { + break; + } + + // Can the cost possibly be decreased? + if ( bestCost <= lowerCost1 && bestCost <= lowerCost2 ) + { + break; + } + + if ( lowerCost1 == lowerCost2 && leaf1 == false ) + { + B3_ASSERT( lowerCost1 < FLT_MAX ); + B3_ASSERT( lowerCost2 < FLT_MAX ); + + // No clear choice based on lower bound surface area. This can happen when both + // children fully contain D. Fall back to node distance. + b3Vec3 d1 = b3Sub( b3AABB_Center( box1 ), centerD ); + b3Vec3 d2 = b3Sub( b3AABB_Center( box2 ), centerD ); + lowerCost1 = b3LengthSquared( d1 ); + lowerCost2 = b3LengthSquared( d2 ); + } + + // Descend + if ( lowerCost1 < lowerCost2 && leaf1 == false ) + { + index = child1; + areaBase = area1; + directCost = directCost1; + } + else + { + index = child2; + areaBase = area2; + directCost = directCost2; + } + + B3_ASSERT( b3IsLeaf( nodes + index ) == false ); + } + + return bestSibling; +} + +enum b3RotateType +{ + b3_rotateNone, + b3_rotateBF, + b3_rotateBG, + b3_rotateCD, + b3_rotateCE +}; + +// Perform a left or right rotation if node A is imbalanced. +// Returns the new root index. +static void b3RotateNodes( b3DynamicTree* tree, int iA ) +{ + B3_ASSERT( iA != B3_NULL_INDEX ); + + b3TreeNode* nodes = tree->nodes; + + b3TreeNode* A = nodes + iA; + if ( b3IsLeaf( A ) == true ) + { + return; + } + + int iB = A->children.child1; + int iC = A->children.child2; + B3_ASSERT( 0 <= iB && iB < tree->nodeCapacity ); + B3_ASSERT( 0 <= iC && iC < tree->nodeCapacity ); + + b3TreeNode* B = nodes + iB; + b3TreeNode* C = nodes + iC; + + bool isLeafB = b3IsLeaf( B ); + bool isLeafC = b3IsLeaf( C ); + + if ( isLeafB == true && isLeafC == false ) + { + int iF = C->children.child1; + int iG = C->children.child2; + b3TreeNode* F = nodes + iF; + b3TreeNode* G = nodes + iG; + B3_ASSERT( 0 <= iF && iF < tree->nodeCapacity ); + B3_ASSERT( 0 <= iG && iG < tree->nodeCapacity ); + + // Base cost + float costBase = b3Perimeter( C->aabb ); + + // Cost of swapping B and F + b3AABB aabbBG = b3AABB_Union( B->aabb, G->aabb ); + float costBF = b3Perimeter( aabbBG ); + + // Cost of swapping B and G + b3AABB aabbBF = b3AABB_Union( B->aabb, F->aabb ); + float costBG = b3Perimeter( aabbBF ); + + if ( costBase < costBF && costBase < costBG ) + { + // Rotation does not improve cost + return; + } + + if ( costBF < costBG ) + { + // Swap B and F + A->children.child1 = iF; + C->children.child1 = iB; + + B->parent = iC; + F->parent = iA; + + C->aabb = aabbBG; + + C->height = 1 + b3MaxUInt16( B->height, G->height ); + A->height = 1 + b3MaxUInt16( C->height, F->height ); + C->categoryBits = B->categoryBits | G->categoryBits; + A->categoryBits = C->categoryBits | F->categoryBits; + C->flags |= ( B->flags | G->flags ) & b3_enlargedNode; + A->flags |= ( C->flags | F->flags ) & b3_enlargedNode; + } + else + { + // Swap B and G + A->children.child1 = iG; + C->children.child2 = iB; + + B->parent = iC; + G->parent = iA; + + C->aabb = aabbBF; + + C->height = 1 + b3MaxUInt16( B->height, F->height ); + A->height = 1 + b3MaxUInt16( C->height, G->height ); + C->categoryBits = B->categoryBits | F->categoryBits; + A->categoryBits = C->categoryBits | G->categoryBits; + C->flags |= ( B->flags | F->flags ) & b3_enlargedNode; + A->flags |= ( C->flags | G->flags ) & b3_enlargedNode; + } + } + else if ( isLeafC == true && isLeafB == false ) + { + // C is a leaf and B is internal + + int iD = B->children.child1; + int iE = B->children.child2; + b3TreeNode* D = nodes + iD; + b3TreeNode* E = nodes + iE; + B3_ASSERT( 0 <= iD && iD < tree->nodeCapacity ); + B3_ASSERT( 0 <= iE && iE < tree->nodeCapacity ); + + // Base cost + float costBase = b3Perimeter( B->aabb ); + + // Cost of swapping C and D + b3AABB aabbCE = b3AABB_Union( C->aabb, E->aabb ); + float costCD = b3Perimeter( aabbCE ); + + // Cost of swapping C and E + b3AABB aabbCD = b3AABB_Union( C->aabb, D->aabb ); + float costCE = b3Perimeter( aabbCD ); + + if ( costBase < costCD && costBase < costCE ) + { + // Rotation does not improve cost + return; + } + + if ( costCD < costCE ) + { + // Swap C and D + A->children.child2 = iD; + B->children.child1 = iC; + + C->parent = iB; + D->parent = iA; + + B->aabb = aabbCE; + + B->height = 1 + b3MaxUInt16( C->height, E->height ); + A->height = 1 + b3MaxUInt16( B->height, D->height ); + B->categoryBits = C->categoryBits | E->categoryBits; + A->categoryBits = B->categoryBits | D->categoryBits; + B->flags |= ( C->flags | E->flags ) & b3_enlargedNode; + A->flags |= ( B->flags | D->flags ) & b3_enlargedNode; + } + else + { + // Swap C and E + A->children.child2 = iE; + B->children.child2 = iC; + + C->parent = iB; + E->parent = iA; + + B->aabb = aabbCD; + + B->height = 1 + b3MaxUInt16( C->height, D->height ); + A->height = 1 + b3MaxUInt16( B->height, E->height ); + B->categoryBits = C->categoryBits | D->categoryBits; + A->categoryBits = B->categoryBits | E->categoryBits; + B->flags |= ( C->flags | D->flags ) & b3_enlargedNode; + A->flags |= ( B->flags | E->flags ) & b3_enlargedNode; + } + } + else if ( isLeafB == false && isLeafC == false ) + { + // All grand children exist so there are many options for rotation + int iD = B->children.child1; + int iE = B->children.child2; + int iF = C->children.child1; + int iG = C->children.child2; + + B3_ASSERT( 0 <= iD && iD < tree->nodeCapacity ); + B3_ASSERT( 0 <= iE && iE < tree->nodeCapacity ); + B3_ASSERT( 0 <= iF && iF < tree->nodeCapacity ); + B3_ASSERT( 0 <= iG && iG < tree->nodeCapacity ); + + b3TreeNode* D = nodes + iD; + b3TreeNode* E = nodes + iE; + b3TreeNode* F = nodes + iF; + b3TreeNode* G = nodes + iG; + + // Base cost + float areaB = b3Perimeter( B->aabb ); + float areaC = b3Perimeter( C->aabb ); + float costBase = areaB + areaC; + enum b3RotateType bestRotation = b3_rotateNone; + float bestCost = costBase; + + // Cost of swapping B and F + b3AABB aabbBG = b3AABB_Union( B->aabb, G->aabb ); + float costBF = areaB + b3Perimeter( aabbBG ); + if ( costBF < bestCost ) + { + bestRotation = b3_rotateBF; + bestCost = costBF; + } + + // Cost of swapping B and G + b3AABB aabbBF = b3AABB_Union( B->aabb, F->aabb ); + float costBG = areaB + b3Perimeter( aabbBF ); + if ( costBG < bestCost ) + { + bestRotation = b3_rotateBG; + bestCost = costBG; + } + + // Cost of swapping C and D + b3AABB aabbCE = b3AABB_Union( C->aabb, E->aabb ); + float costCD = areaC + b3Perimeter( aabbCE ); + if ( costCD < bestCost ) + { + bestRotation = b3_rotateCD; + bestCost = costCD; + } + + // Cost of swapping C and E + b3AABB aabbCD = b3AABB_Union( C->aabb, D->aabb ); + float costCE = areaC + b3Perimeter( aabbCD ); + if ( costCE < bestCost ) + { + bestRotation = b3_rotateCE; + // bestCost = costCE; + } + + switch ( bestRotation ) + { + case b3_rotateNone: + break; + + case b3_rotateBF: + A->children.child1 = iF; + C->children.child1 = iB; + + B->parent = iC; + F->parent = iA; + + C->aabb = aabbBG; + + C->height = 1 + b3MaxUInt16( B->height, G->height ); + A->height = 1 + b3MaxUInt16( C->height, F->height ); + C->categoryBits = B->categoryBits | G->categoryBits; + A->categoryBits = C->categoryBits | F->categoryBits; + C->flags |= ( B->flags | G->flags ) & b3_enlargedNode; + A->flags |= ( C->flags | F->flags ) & b3_enlargedNode; + break; + + case b3_rotateBG: + A->children.child1 = iG; + C->children.child2 = iB; + + B->parent = iC; + G->parent = iA; + + C->aabb = aabbBF; + + C->height = 1 + b3MaxUInt16( B->height, F->height ); + A->height = 1 + b3MaxUInt16( C->height, G->height ); + C->categoryBits = B->categoryBits | F->categoryBits; + A->categoryBits = C->categoryBits | G->categoryBits; + C->flags |= ( B->flags | F->flags ) & b3_enlargedNode; + A->flags |= ( C->flags | G->flags ) & b3_enlargedNode; + break; + + case b3_rotateCD: + A->children.child2 = iD; + B->children.child1 = iC; + + C->parent = iB; + D->parent = iA; + + B->aabb = aabbCE; + + B->height = 1 + b3MaxUInt16( C->height, E->height ); + A->height = 1 + b3MaxUInt16( B->height, D->height ); + B->categoryBits = C->categoryBits | E->categoryBits; + A->categoryBits = B->categoryBits | D->categoryBits; + B->flags |= ( C->flags | E->flags ) & b3_enlargedNode; + A->flags |= ( B->flags | D->flags ) & b3_enlargedNode; + break; + + case b3_rotateCE: + A->children.child2 = iE; + B->children.child2 = iC; + + C->parent = iB; + E->parent = iA; + + B->aabb = aabbCD; + + B->height = 1 + b3MaxUInt16( C->height, D->height ); + A->height = 1 + b3MaxUInt16( B->height, E->height ); + B->categoryBits = C->categoryBits | D->categoryBits; + A->categoryBits = B->categoryBits | E->categoryBits; + B->flags |= ( C->flags | D->flags ) & b3_enlargedNode; + A->flags |= ( B->flags | E->flags ) & b3_enlargedNode; + break; + + default: + B3_ASSERT( false ); + break; + } + } +} + +// It would be nicer if the root had zero height but maintaining this would drastically increase +// insertion cost because whole sub-trees would need the height to be updated. +static void b3InsertLeaf( b3DynamicTree* tree, int leaf, bool shouldRotate ) +{ + if ( tree->root == B3_NULL_INDEX ) + { + tree->root = leaf; + tree->nodes[tree->root].parent = B3_NULL_INDEX; + return; + } + + // Stage 1: find the best sibling for this node + b3AABB leafAABB = tree->nodes[leaf].aabb; + int sibling = b3FindBestSibling( tree, leafAABB ); + + // Stage 2: create a new parent for the leaf and sibling + int oldParent = tree->nodes[sibling].parent; + int newParent = b3AllocateNode( tree ); + + // warning: node pointer can change after allocation + b3TreeNode* nodes = tree->nodes; + nodes[newParent].parent = oldParent; + nodes[newParent].userData = UINT64_MAX; + nodes[newParent].aabb = b3AABB_Union( leafAABB, nodes[sibling].aabb ); + nodes[newParent].categoryBits = nodes[leaf].categoryBits | nodes[sibling].categoryBits; + nodes[newParent].height = nodes[sibling].height + 1; + + if ( oldParent != B3_NULL_INDEX ) + { + // The sibling was not the root. + if ( nodes[oldParent].children.child1 == sibling ) + { + nodes[oldParent].children.child1 = newParent; + } + else + { + nodes[oldParent].children.child2 = newParent; + } + + nodes[newParent].children.child1 = sibling; + nodes[newParent].children.child2 = leaf; + nodes[sibling].parent = newParent; + nodes[leaf].parent = newParent; + } + else + { + // The sibling was the root. + nodes[newParent].children.child1 = sibling; + nodes[newParent].children.child2 = leaf; + nodes[sibling].parent = newParent; + nodes[leaf].parent = newParent; + tree->root = newParent; + } + + // Stage 3: walk back up the tree fixing heights and AABBs + int index = nodes[leaf].parent; + while ( index != B3_NULL_INDEX ) + { + int child1 = nodes[index].children.child1; + int child2 = nodes[index].children.child2; + + B3_ASSERT( child1 != B3_NULL_INDEX ); + B3_ASSERT( child2 != B3_NULL_INDEX ); + + nodes[index].aabb = b3AABB_Union( nodes[child1].aabb, nodes[child2].aabb ); + nodes[index].categoryBits = nodes[child1].categoryBits | nodes[child2].categoryBits; + nodes[index].height = 1 + b3MaxUInt16( nodes[child1].height, nodes[child2].height ); + nodes[index].flags |= ( nodes[child1].flags | nodes[child2].flags ) & b3_enlargedNode; + + if ( shouldRotate ) + { + b3RotateNodes( tree, index ); + } + + index = nodes[index].parent; + } +} + +static void b3RemoveLeaf( b3DynamicTree* tree, int leaf ) +{ + if ( leaf == tree->root ) + { + tree->root = B3_NULL_INDEX; + return; + } + + b3TreeNode* nodes = tree->nodes; + + int parent = nodes[leaf].parent; + int grandParent = nodes[parent].parent; + int sibling; + if ( nodes[parent].children.child1 == leaf ) + { + sibling = nodes[parent].children.child2; + } + else + { + sibling = nodes[parent].children.child1; + } + + if ( grandParent != B3_NULL_INDEX ) + { + // Destroy parent and connect sibling to grandParent. + if ( nodes[grandParent].children.child1 == parent ) + { + nodes[grandParent].children.child1 = sibling; + } + else + { + nodes[grandParent].children.child2 = sibling; + } + nodes[sibling].parent = grandParent; + b3FreeNode( tree, parent ); + + // Adjust ancestor bounds. + int index = grandParent; + while ( index != B3_NULL_INDEX ) + { + b3TreeNode* node = nodes + index; + b3TreeNode* child1 = nodes + node->children.child1; + b3TreeNode* child2 = nodes + node->children.child2; + + // Fast union using SSE + //__m128 aabb1 = _mm_load_ps(&child1->aabb.lowerBound.x); + //__m128 aabb2 = _mm_load_ps(&child2->aabb.lowerBound.x); + //__m128 lower = _mm_min_ps(aabb1, aabb2); + //__m128 upper = _mm_max_ps(aabb1, aabb2); + //__m128 aabb = _mm_shuffle_ps(lower, upper, _MM_SHUFFLE(3, 2, 1, 0)); + //_mm_store_ps(&node->aabb.lowerBound.x, aabb); + + node->aabb = b3AABB_Union( child1->aabb, child2->aabb ); + node->categoryBits = child1->categoryBits | child2->categoryBits; + node->height = 1 + b3MaxUInt16( child1->height, child2->height ); + + index = node->parent; + } + } + else + { + tree->root = sibling; + tree->nodes[sibling].parent = B3_NULL_INDEX; + b3FreeNode( tree, parent ); + } +} + +// Create a proxy in the tree as a leaf node. We return the index of the node instead of a pointer so that we can grow +// the node pool. +int b3DynamicTree_CreateProxy( b3DynamicTree* tree, b3AABB aabb, uint64_t categoryBits, uint64_t userData ) +{ + B3_ASSERT( b3IsValidAABB( aabb ) ); + B3_VALIDATE( -B3_HUGE < aabb.lowerBound.x && aabb.lowerBound.x < B3_HUGE ); + B3_VALIDATE( -B3_HUGE < aabb.lowerBound.y && aabb.lowerBound.y < B3_HUGE ); + B3_VALIDATE( -B3_HUGE < aabb.lowerBound.z && aabb.lowerBound.z < B3_HUGE ); + B3_VALIDATE( -B3_HUGE < aabb.upperBound.x && aabb.upperBound.x < B3_HUGE ); + B3_VALIDATE( -B3_HUGE < aabb.upperBound.y && aabb.upperBound.y < B3_HUGE ); + B3_VALIDATE( -B3_HUGE < aabb.upperBound.z && aabb.upperBound.z < B3_HUGE ); + + int proxyId = b3AllocateNode( tree ); + b3TreeNode* node = tree->nodes + proxyId; + + node->aabb = aabb; + node->userData = userData; + node->categoryBits = categoryBits; + node->height = 0; + node->flags = b3_allocatedNode | b3_leafNode; + + bool shouldRotate = true; + b3InsertLeaf( tree, proxyId, shouldRotate ); + + tree->proxyCount += 1; + + return proxyId; +} + +void b3DynamicTree_DestroyProxy( b3DynamicTree* tree, int proxyId ) +{ + B3_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); + B3_ASSERT( b3IsLeaf( tree->nodes + proxyId ) ); + + b3RemoveLeaf( tree, proxyId ); + b3FreeNode( tree, proxyId ); + + B3_ASSERT( tree->proxyCount > 0 ); + tree->proxyCount -= 1; +} + +int b3DynamicTree_GetProxyCount( const b3DynamicTree* tree ) +{ + return tree->proxyCount; +} + +void b3DynamicTree_MoveProxy( b3DynamicTree* tree, int proxyId, b3AABB aabb ) +{ + B3_ASSERT( b3IsValidAABB( aabb ) ); + B3_VALIDATE( -B3_HUGE < aabb.lowerBound.x && aabb.lowerBound.x < B3_HUGE ); + B3_VALIDATE( -B3_HUGE < aabb.lowerBound.y && aabb.lowerBound.y < B3_HUGE ); + B3_VALIDATE( -B3_HUGE < aabb.lowerBound.z && aabb.lowerBound.z < B3_HUGE ); + B3_VALIDATE( -B3_HUGE < aabb.upperBound.x && aabb.upperBound.x < B3_HUGE ); + B3_VALIDATE( -B3_HUGE < aabb.upperBound.y && aabb.upperBound.y < B3_HUGE ); + B3_VALIDATE( -B3_HUGE < aabb.upperBound.z && aabb.upperBound.z < B3_HUGE ); + B3_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); + B3_ASSERT( b3IsLeaf( tree->nodes + proxyId ) ); + + b3RemoveLeaf( tree, proxyId ); + + tree->nodes[proxyId].aabb = aabb; + + bool shouldRotate = false; + b3InsertLeaf( tree, proxyId, shouldRotate ); +} + +void b3DynamicTree_EnlargeProxy( b3DynamicTree* tree, int proxyId, b3AABB aabb ) +{ + b3TreeNode* nodes = tree->nodes; + B3_VALIDATE( b3IsValidAABB( aabb ) ); + B3_VALIDATE( aabb.upperBound.x - aabb.lowerBound.x < B3_HUGE ); + B3_VALIDATE( aabb.upperBound.y - aabb.lowerBound.y < B3_HUGE ); + B3_VALIDATE( aabb.upperBound.z - aabb.lowerBound.z < B3_HUGE ); + B3_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); + B3_VALIDATE( b3IsLeaf( tree->nodes + proxyId ) ); + + // Caller must ensure this + B3_VALIDATE( b3AABB_Contains( nodes[proxyId].aabb, aabb ) == false ); + + b3TreeNode* node = nodes + proxyId; + node->aabb = aabb; + + int parentIndex = node->parent; + while ( parentIndex != B3_NULL_INDEX ) + { + node = nodes + parentIndex; + bool changed = b3EnlargeAABB( &node->aabb, aabb ); + + // todo not sure why this node is marked as enlarged even if it didn't change + node->flags |= b3_enlargedNode; + + parentIndex = node->parent; + + if ( changed == false ) + { + break; + } + } + + while ( parentIndex != B3_NULL_INDEX ) + { + node = nodes + parentIndex; + if ( node->flags & b3_enlargedNode ) + { + // early out because this ancestor was previously ascended and marked as enlarged + break; + } + + node->flags |= b3_enlargedNode; + parentIndex = node->parent; + } +} + +void b3DynamicTree_SetCategoryBits( b3DynamicTree* tree, int proxyId, uint64_t categoryBits ) +{ + b3TreeNode* nodes = tree->nodes; + + B3_ASSERT( b3IsLeaf( nodes + proxyId ) ); + + nodes[proxyId].categoryBits = categoryBits; + + // Fix up category bits in ancestor internal nodes + int nodeIndex = nodes[proxyId].parent; + while ( nodeIndex != B3_NULL_INDEX ) + { + b3TreeNode* node = nodes + nodeIndex; + int child1 = node->children.child1; + B3_ASSERT( child1 != B3_NULL_INDEX ); + int child2 = node->children.child2; + B3_ASSERT( child2 != B3_NULL_INDEX ); + node->categoryBits = nodes[child1].categoryBits | nodes[child2].categoryBits; + + nodeIndex = node->parent; + } +} + +uint64_t b3DynamicTree_GetCategoryBits( b3DynamicTree* tree, int proxyId ) +{ + B3_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); + return tree->nodes[proxyId].categoryBits; +} + +int b3DynamicTree_GetHeight( const b3DynamicTree* tree ) +{ + if ( tree->root == B3_NULL_INDEX ) + { + return 0; + } + + return tree->nodes[tree->root].height; +} + +float b3DynamicTree_GetAreaRatio( const b3DynamicTree* tree ) +{ + if ( tree->root == B3_NULL_INDEX ) + { + return 0.0f; + } + + const b3TreeNode* root = tree->nodes + tree->root; + float rootArea = b3Perimeter( root->aabb ); + + float totalArea = 0.0f; + for ( int i = 0; i < tree->nodeCapacity; ++i ) + { + const b3TreeNode* node = tree->nodes + i; + if ( b3IsAllocated( node ) == false || b3IsLeaf( node ) || i == tree->root ) + { + continue; + } + + totalArea += b3Perimeter( node->aabb ); + } + + return totalArea / rootArea; +} + +b3AABB b3DynamicTree_GetRootBounds( const b3DynamicTree* tree ) +{ + if ( tree->root != B3_NULL_INDEX ) + { + return tree->nodes[tree->root].aabb; + } + + b3AABB empty = { b3Vec3_zero, b3Vec3_zero }; + return empty; +} + +#if B3_ENABLE_VALIDATION +// Compute the height of a sub-tree. +static int b3ComputeHeightRecurse( const b3DynamicTree* tree, int nodeId ) +{ + B3_ASSERT( 0 <= nodeId && nodeId < tree->nodeCapacity ); + b3TreeNode* node = tree->nodes + nodeId; + + if ( b3IsLeaf( node ) ) + { + return 0; + } + + int height1 = b3ComputeHeightRecurse( tree, node->children.child1 ); + int height2 = b3ComputeHeightRecurse( tree, node->children.child2 ); + return 1 + b3MaxInt( height1, height2 ); +} + +static int b3ComputeHeight( const b3DynamicTree* tree ) +{ + int height = b3ComputeHeightRecurse( tree, tree->root ); + return height; +} + +static void b3ValidateStructure( const b3DynamicTree* tree, int index ) +{ + if ( index == B3_NULL_INDEX ) + { + return; + } + + if ( index == tree->root ) + { + B3_ASSERT( tree->nodes[index].parent == B3_NULL_INDEX ); + } + + const b3TreeNode* node = tree->nodes + index; + + B3_ASSERT( node->flags == 0 || ( node->flags & b3_allocatedNode ) != 0 ); + + if ( b3IsLeaf( node ) ) + { + B3_ASSERT( node->height == 0 ); + return; + } + + int child1 = node->children.child1; + int child2 = node->children.child2; + + B3_ASSERT( 0 <= child1 && child1 < tree->nodeCapacity ); + B3_ASSERT( 0 <= child2 && child2 < tree->nodeCapacity ); + + B3_ASSERT( tree->nodes[child1].parent == index ); + B3_ASSERT( tree->nodes[child2].parent == index ); + + if ( ( tree->nodes[child1].flags | tree->nodes[child2].flags ) & b3_enlargedNode ) + { + B3_ASSERT( node->flags & b3_enlargedNode ); + } + + b3ValidateStructure( tree, child1 ); + b3ValidateStructure( tree, child2 ); +} + +static void b3ValidateMetrics( const b3DynamicTree* tree, int index ) +{ + if ( index == B3_NULL_INDEX ) + { + return; + } + + const b3TreeNode* node = tree->nodes + index; + + B3_VALIDATE( b3IsSaneAABB( node->aabb ) ); + + if ( b3IsLeaf( node ) ) + { + B3_ASSERT( node->height == 0 ); + return; + } + + int child1 = node->children.child1; + int child2 = node->children.child2; + + B3_ASSERT( 0 <= child1 && child1 < tree->nodeCapacity ); + B3_ASSERT( 0 <= child2 && child2 < tree->nodeCapacity ); + + int height1 = tree->nodes[child1].height; + int height2 = tree->nodes[child2].height; + int height = 1 + b3MaxInt( height1, height2 ); + B3_ASSERT( node->height == height ); + + // b3AABB aabb = b3AABB_Union(tree->nodes[child1].aabb, tree->nodes[child2].aabb); + + B3_ASSERT( b3AABB_Contains( node->aabb, tree->nodes[child1].aabb ) ); + B3_ASSERT( b3AABB_Contains( node->aabb, tree->nodes[child2].aabb ) ); + + // B3_ASSERT(aabb.lowerBound.x == node->aabb.lowerBound.x); + // B3_ASSERT(aabb.lowerBound.y == node->aabb.lowerBound.y); + // B3_ASSERT(aabb.upperBound.x == node->aabb.upperBound.x); + // B3_ASSERT(aabb.upperBound.y == node->aabb.upperBound.y); + + uint64_t categoryBits = tree->nodes[child1].categoryBits | tree->nodes[child2].categoryBits; + B3_ASSERT( node->categoryBits == categoryBits ); + + b3ValidateMetrics( tree, child1 ); + b3ValidateMetrics( tree, child2 ); +} +#endif + +void b3DynamicTree_Validate( const b3DynamicTree* tree ) +{ +#if B3_ENABLE_VALIDATION + if ( tree->root == B3_NULL_INDEX ) + { + return; + } + + b3ValidateStructure( tree, tree->root ); + b3ValidateMetrics( tree, tree->root ); + + int freeCount = 0; + int freeIndex = tree->freeList; + while ( freeIndex != B3_NULL_INDEX ) + { + B3_ASSERT( 0 <= freeIndex && freeIndex < tree->nodeCapacity ); + freeIndex = tree->nodes[freeIndex].next; + ++freeCount; + } + + int height = b3DynamicTree_GetHeight( tree ); + int computedHeight = b3ComputeHeight( tree ); + B3_ASSERT( height == computedHeight ); + + B3_ASSERT( tree->nodeCount + freeCount == tree->nodeCapacity ); +#else + B3_UNUSED( tree ); +#endif +} + +void b3DynamicTree_ValidateNoEnlarged( const b3DynamicTree* tree ) +{ +#if B3_ENABLE_VALIDATION == 1 + int capacity = tree->nodeCapacity; + const b3TreeNode* nodes = tree->nodes; + for ( int i = 0; i < capacity; ++i ) + { + const b3TreeNode* node = nodes + i; + if ( node->flags & b3_allocatedNode ) + { + B3_ASSERT( ( node->flags & b3_enlargedNode ) == 0 ); + } + } +#else + B3_UNUSED( tree ); +#endif +} + +int b3DynamicTree_GetByteCount( const b3DynamicTree* tree ) +{ + size_t size = sizeof( b3DynamicTree ) + sizeof( b3TreeNode ) * tree->nodeCapacity + + tree->rebuildCapacity * ( sizeof( int ) + sizeof( b3AABB ) + sizeof( b3Vec3 ) + sizeof( int ) ); + + return (int)size; +} + +b3TreeStats b3DynamicTree_Query( const b3DynamicTree* tree, b3AABB aabb, uint64_t maskBits, bool requireAllBits, + b3TreeQueryCallbackFcn* callback, void* context ) +{ + b3TreeStats result = { 0 }; + + if ( tree->nodeCount == 0 ) + { + return result; + } + + int stack[B3_TREE_STACK_SIZE]; + int stackCount = 0; + stack[stackCount++] = tree->root; + + while ( stackCount > 0 ) + { + int nodeId = stack[--stackCount]; + if ( nodeId == B3_NULL_INDEX ) + { + // todo huh? + B3_ASSERT( false ); + continue; + } + + const b3TreeNode* node = tree->nodes + nodeId; + result.nodeVisits += 1; + + // Assuming branch prediction deals with requireAllBits well + uint64_t bitMatch = requireAllBits ? ( node->categoryBits & maskBits ) == maskBits : ( node->categoryBits & maskBits ); + + if ( bitMatch && b3AABB_Overlaps( node->aabb, aabb ) ) + { + if ( b3IsLeaf( node ) ) + { + // callback to user code with proxy id + bool proceed = callback( nodeId, node->userData, context ); + result.leafVisits += 1; + + if ( proceed == false ) + { + return result; + } + } + else + { + B3_ASSERT( stackCount < B3_TREE_STACK_SIZE - 1 ); + if ( stackCount < B3_TREE_STACK_SIZE - 1 ) + { + stack[stackCount++] = node->children.child1; + stack[stackCount++] = node->children.child2; + } + } + } + } + + return result; +} + +B3_FORCE_INLINE float b3DistanceToNodeSqr( b3Vec3 point, const b3TreeNode* node ) +{ + b3Vec3 r = b3Sub( point, b3Clamp( point, node->aabb.lowerBound, node->aabb.upperBound ) ); + return b3Dot( r, r ); +} + +struct b3QueryClosestItem +{ + int nodeIndex; + float distanceToNodeSqr; +}; + +b3TreeStats b3DynamicTree_QueryClosest( const b3DynamicTree* tree, b3Vec3 point, uint64_t maskBits, bool requireAllBits, + b3TreeQueryClosestCallbackFcn* callback, void* context, float* minDistanceSqr ) +{ + b3TreeStats result = { 0 }; + + if ( tree->nodeCount == 0 ) + { + return result; + } + + float minSqr = *minDistanceSqr; + struct b3QueryClosestItem stack[B3_TREE_STACK_SIZE]; + int stackCount = 0; + + float rootDistanceSqr = b3DistanceToNodeSqr( point, tree->nodes + tree->root ); + stack[stackCount++] = (struct b3QueryClosestItem){ + .nodeIndex = tree->root, + .distanceToNodeSqr = rootDistanceSqr, + }; + + while ( stackCount > 0 ) + { + struct b3QueryClosestItem item = stack[--stackCount]; + const b3TreeNode* node = tree->nodes + item.nodeIndex; + result.nodeVisits += 1; + + uint64_t bitMatch = requireAllBits ? ( node->categoryBits & maskBits ) == maskBits : ( node->categoryBits & maskBits ); + + if ( bitMatch ) + { + if ( item.distanceToNodeSqr < minSqr ) + { + if ( b3IsLeaf( node ) ) + { + // callback to user code with minimum distance squared so far and proxy id + float dd = callback( minSqr, item.nodeIndex, node->userData, context ); + + if ( dd < minSqr ) + { + minSqr = dd; + } + + result.leafVisits += 1; + } + else + { + B3_ASSERT( stackCount < B3_TREE_STACK_SIZE - 1 ); + if ( stackCount < B3_TREE_STACK_SIZE - 1 ) + { + int child1 = node->children.child1; + int child2 = node->children.child2; + + // Store the distance to node in the stack instead of recomputing after pop + struct b3QueryClosestItem item1 = { + .nodeIndex = child1, + .distanceToNodeSqr = b3DistanceToNodeSqr( point, tree->nodes + child1 ), + }; + + struct b3QueryClosestItem item2 = { + .nodeIndex = child2, + .distanceToNodeSqr = b3DistanceToNodeSqr( point, tree->nodes + child2 ), + }; + + // Ensure we iterate the closest child first as we pop off the stack + if ( item2.distanceToNodeSqr < item1.distanceToNodeSqr ) + { + stack[stackCount++] = item1; + stack[stackCount++] = item2; + } + else + { + stack[stackCount++] = item2; + stack[stackCount++] = item1; + } + } + } + } + } + } + + *minDistanceSqr = minSqr; + + return result; +} + +b3TreeStats b3DynamicTree_RayCast( const b3DynamicTree* tree, const b3RayCastInput* input, uint64_t maskBits, bool requireAllBits, + b3TreeRayCastCallbackFcn* callback, void* context ) +{ + b3TreeStats result = { 0 }; + + if ( tree->nodeCount == 0 ) + { + return result; + } + + b3Vec3 p1 = input->origin; + b3Vec3 d = input->translation; + + b3V32 pv1 = b3LoadV( &p1.x ); + b3V32 dv = b3LoadV( &d.x ); + + float maxFraction = input->maxFraction; + + b3Vec3 p2 = b3MulAdd( p1, maxFraction, d ); + + // Build a bounding box for the segment. + b3AABB segmentAABB = { b3Min( p1, p2 ), b3Max( p1, p2 ) }; + + int stack[B3_TREE_STACK_SIZE]; + int stackCount = 0; + stack[stackCount++] = tree->root; + + const b3TreeNode* nodes = tree->nodes; + + b3RayCastInput subInput = *input; + + while ( stackCount > 0 ) + { + int nodeId = stack[--stackCount]; + if ( nodeId == B3_NULL_INDEX ) + { + // todo is this possible? + B3_ASSERT( false ); + continue; + } + + const b3TreeNode* node = nodes + nodeId; + result.nodeVisits += 1; + + b3AABB nodeAABB = node->aabb; + + // todo look at disassembly + uint64_t bitMatch = requireAllBits ? ( node->categoryBits & maskBits ) == maskBits : ( node->categoryBits & maskBits ); + + if ( bitMatch == 0 || b3AABB_Overlaps( nodeAABB, segmentAABB ) == false ) + { + continue; + } + + b3V32 lower = b3LoadV( &nodeAABB.lowerBound.x ); + b3V32 upper = b3LoadV( &nodeAABB.upperBound.x ); + + bool edgeOverlap = b3TestBoundsRayOverlap( lower, upper, pv1, dv ); + if ( edgeOverlap == false ) + { + continue; + } + + if ( b3IsLeaf( node ) ) + { + subInput.maxFraction = maxFraction; + + float value = callback( &subInput, nodeId, node->userData, context ); + result.leafVisits += 1; + + // The user may return -1 to indicate this shape should be skipped + + if ( value == 0.0f ) + { + // The client has terminated the ray cast. + return result; + } + + if ( 0.0f < value && value <= maxFraction ) + { + // Update segment bounding box. + maxFraction = value; + p2 = b3MulAdd( p1, maxFraction, d ); + segmentAABB.lowerBound = b3Min( p1, p2 ); + segmentAABB.upperBound = b3Max( p1, p2 ); + } + } + else + { + B3_ASSERT( stackCount < B3_TREE_STACK_SIZE - 1 ); + if ( stackCount < B3_TREE_STACK_SIZE - 1 ) + { + b3Vec3 c1 = b3AABB_Center( nodes[node->children.child1].aabb ); + b3Vec3 c2 = b3AABB_Center( nodes[node->children.child2].aabb ); + if ( b3DistanceSquared( c1, p1 ) < b3DistanceSquared( c2, p1 ) ) + { + stack[stackCount++] = node->children.child2; + stack[stackCount++] = node->children.child1; + } + else + { + stack[stackCount++] = node->children.child1; + stack[stackCount++] = node->children.child2; + } + } + } + } + + return result; +} + +b3TreeStats b3DynamicTree_ShapeCast( const b3DynamicTree* tree, const b3ShapeCastInput* input, uint64_t maskBits, + bool requireAllBits, b3TreeShapeCastCallbackFcn* callback, void* context ) +{ + b3TreeStats stats = { 0 }; + + if ( tree->nodeCount == 0 || input->proxy.count == 0 ) + { + return stats; + } + + b3AABB originAABB = { input->proxy.points[0], input->proxy.points[0] }; + for ( int i = 1; i < input->proxy.count; ++i ) + { + originAABB.lowerBound = b3Min( originAABB.lowerBound, input->proxy.points[i] ); + originAABB.upperBound = b3Max( originAABB.upperBound, input->proxy.points[i] ); + } + + b3Vec3 radius = { input->proxy.radius, input->proxy.radius, input->proxy.radius }; + + originAABB.lowerBound = b3Sub( originAABB.lowerBound, radius ); + originAABB.upperBound = b3Add( originAABB.upperBound, radius ); + + b3Vec3 p1 = b3AABB_Center( originAABB ); + b3Vec3 extension = b3AABB_Extents( originAABB ); + + // v is perpendicular to the segment. + b3Vec3 d = input->translation; + + b3V32 pv1 = b3LoadV( &p1.x ); + b3V32 dv = b3LoadV( &d.x ); + b3V32 ev = b3LoadV( &extension.x ); + + float maxFraction = input->maxFraction; + + // Build total box for the shape cast + b3Vec3 t = b3MulSV( maxFraction, input->translation ); + b3AABB totalAABB = { + b3Min( originAABB.lowerBound, b3Add( originAABB.lowerBound, t ) ), + b3Max( originAABB.upperBound, b3Add( originAABB.upperBound, t ) ), + }; + + b3ShapeCastInput subInput = *input; + const b3TreeNode* nodes = tree->nodes; + + int stack[B3_TREE_STACK_SIZE]; + int stackCount = 0; + stack[stackCount++] = tree->root; + + while ( stackCount > 0 ) + { + int nodeId = stack[--stackCount]; + if ( nodeId == B3_NULL_INDEX ) + { + // todo is this possible? + B3_ASSERT( false ); + continue; + } + + const b3TreeNode* node = nodes + nodeId; + stats.nodeVisits += 1; + + // todo look at disassembly + uint64_t bitMatch = requireAllBits ? ( node->categoryBits & maskBits ) == maskBits : ( node->categoryBits & maskBits ); + + if ( bitMatch == 0 || b3AABB_Overlaps( node->aabb, totalAABB ) == false ) + { + continue; + } + + // radius extension is added to the node in this case + b3V32 lower = b3SubV( b3LoadV( &node->aabb.lowerBound.x ), ev ); + b3V32 upper = b3AddV( b3LoadV( &node->aabb.upperBound.x ), ev ); + bool edgeOverlap = b3TestBoundsRayOverlap( lower, upper, pv1, dv ); + if ( edgeOverlap == false ) + { + continue; + } + + if ( b3IsLeaf( node ) ) + { + subInput.maxFraction = maxFraction; + + float value = callback( &subInput, nodeId, node->userData, context ); + stats.leafVisits += 1; + + if ( value == 0.0f ) + { + // The client has terminated the ray cast. + return stats; + } + + if ( 0.0f < value && value < maxFraction ) + { + // Update segment bounding box. + maxFraction = value; + t = b3MulSV( maxFraction, input->translation ); + totalAABB.lowerBound = b3Min( originAABB.lowerBound, b3Add( originAABB.lowerBound, t ) ); + totalAABB.upperBound = b3Max( originAABB.upperBound, b3Add( originAABB.upperBound, t ) ); + } + } + else + { + B3_ASSERT( stackCount < B3_TREE_STACK_SIZE - 1 ); + if ( stackCount < B3_TREE_STACK_SIZE - 1 ) + { + b3Vec3 c1 = b3AABB_Center( nodes[node->children.child1].aabb ); + b3Vec3 c2 = b3AABB_Center( nodes[node->children.child2].aabb ); + if ( b3DistanceSquared( c1, p1 ) < b3DistanceSquared( c2, p1 ) ) + { + stack[stackCount++] = node->children.child2; + stack[stackCount++] = node->children.child1; + } + else + { + stack[stackCount++] = node->children.child1; + stack[stackCount++] = node->children.child2; + } + } + } + } + + return stats; +} + +// Median split == 0, Surface area heuristic == 1 +#define B3_TREE_HEURISTIC 0 + +#if B3_TREE_HEURISTIC == 0 + +// Median split heuristic +static int b3PartitionMid( int* indices, b3Vec3* centers, int count ) +{ + // Handle trivial case + if ( count <= 2 ) + { + return count / 2; + } + + b3Vec3 lowerBound = centers[0]; + b3Vec3 upperBound = centers[0]; + + for ( int i = 1; i < count; ++i ) + { + lowerBound = b3Min( lowerBound, centers[i] ); + upperBound = b3Max( upperBound, centers[i] ); + } + + b3Vec3 d = b3Sub( upperBound, lowerBound ); + b3Vec3 c = b3MulSV( 0.5f, b3Add( lowerBound, upperBound ) ); + + // Partition longest axis using the Hoare partition scheme + // https://en.wikipedia.org/wiki/Quicksort + // https://nicholasvadivelu.com/2021/01/11/array-partition/ + int i1 = 0, i2 = count; + if ( d.x >= d.y && d.x >= d.z ) + { + float pivot = c.x; + + while ( i1 < i2 ) + { + while ( i1 < i2 && centers[i1].x < pivot ) + { + i1 += 1; + }; + + while ( i1 < i2 && centers[i2 - 1].x >= pivot ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap indices + { + int temp = indices[i1]; + indices[i1] = indices[i2 - 1]; + indices[i2 - 1] = temp; + } + + // Swap centers + { + b3Vec3 temp = centers[i1]; + centers[i1] = centers[i2 - 1]; + centers[i2 - 1] = temp; + } + + i1 += 1; + i2 -= 1; + } + } + } + else if ( d.y >= d.z ) + { + float pivot = c.y; + + while ( i1 < i2 ) + { + while ( i1 < i2 && centers[i1].y < pivot ) + { + i1 += 1; + }; + + while ( i1 < i2 && centers[i2 - 1].y >= pivot ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap indices + { + int temp = indices[i1]; + indices[i1] = indices[i2 - 1]; + indices[i2 - 1] = temp; + } + + // Swap centers + { + b3Vec3 temp = centers[i1]; + centers[i1] = centers[i2 - 1]; + centers[i2 - 1] = temp; + } + + i1 += 1; + i2 -= 1; + } + } + } + else + { + float pivot = c.z; + + while ( i1 < i2 ) + { + while ( i1 < i2 && centers[i1].z < pivot ) + { + i1 += 1; + }; + + while ( i1 < i2 && centers[i2 - 1].z >= pivot ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap indices + { + int temp = indices[i1]; + indices[i1] = indices[i2 - 1]; + indices[i2 - 1] = temp; + } + + // Swap centers + { + b3Vec3 temp = centers[i1]; + centers[i1] = centers[i2 - 1]; + centers[i2 - 1] = temp; + } + + i1 += 1; + i2 -= 1; + } + } + } + B3_ASSERT( i1 == i2 ); + + if ( i1 > 0 && i1 < count ) + { + return i1; + } + + return count / 2; +} + +#else + +#define B3_BIN_COUNT 8 + +typedef struct b3TreeBin +{ + b3AABB aabb; + int count; +} b3TreeBin; + +typedef struct b3TreePlane +{ + b3AABB leftAABB; + b3AABB rightAABB; + int leftCount; + int rightCount; +} b3TreePlane; + +// "On Fast Construction of SAH-based Bounding Volume Hierarchies" by Ingo Wald +// Returns the left child count +static int b3PartitionSAH( int* indices, int* binIndices, b3AABB* boxes, int count ) +{ + B3_ASSERT( count > 0 ); + + b3TreeBin bins[B3_BIN_COUNT]; + b3TreePlane planes[B3_BIN_COUNT - 1]; + + b3Vec3 center = b3AABB_Center( boxes[0] ); + b3AABB centroidAABB; + centroidAABB.lowerBound = center; + centroidAABB.upperBound = center; + + for ( int i = 1; i < count; ++i ) + { + center = b3AABB_Center( boxes[i] ); + centroidAABB.lowerBound = b3Min( centroidAABB.lowerBound, center ); + centroidAABB.upperBound = b3Max( centroidAABB.upperBound, center ); + } + + b3Vec3 d = b3Sub( centroidAABB.upperBound, centroidAABB.lowerBound ); + + // Find longest axis + int axisIndex; + float invD; + if ( d.x > d.y ) + { + axisIndex = 0; + invD = d.x; + } + else + { + axisIndex = 1; + invD = d.y; + } + + invD = invD > 0.0f ? 1.0f / invD : 0.0f; + + // Initialize bin bounds and count + for ( int i = 0; i < B3_BIN_COUNT; ++i ) + { + bins[i].aabb.lowerBound = (b3Vec3){ FLT_MAX, FLT_MAX }; + bins[i].aabb.upperBound = (b3Vec3){ -FLT_MAX, -FLT_MAX }; + bins[i].count = 0; + } + + // Assign boxes to bins and compute bin boxes + // TODO_ERIN optimize + float binCount = B3_BIN_COUNT; + float lowerBoundArray[2] = { centroidAABB.lowerBound.x, centroidAABB.lowerBound.y }; + float minC = lowerBoundArray[axisIndex]; + for ( int i = 0; i < count; ++i ) + { + b3Vec3 c = b3AABB_Center( boxes[i] ); + float cArray[2] = { c.x, c.y }; + int binIndex = (int)( binCount * ( cArray[axisIndex] - minC ) * invD ); + binIndex = b3ClampInt( binIndex, 0, B3_BIN_COUNT - 1 ); + binIndices[i] = binIndex; + bins[binIndex].count += 1; + bins[binIndex].aabb = b3AABB_Union( bins[binIndex].aabb, boxes[i] ); + } + + int planeCount = B3_BIN_COUNT - 1; + + // Prepare all the left planes, candidates for left child + planes[0].leftCount = bins[0].count; + planes[0].leftAABB = bins[0].aabb; + for ( int i = 1; i < planeCount; ++i ) + { + planes[i].leftCount = planes[i - 1].leftCount + bins[i].count; + planes[i].leftAABB = b3AABB_Union( planes[i - 1].leftAABB, bins[i].aabb ); + } + + // Prepare all the right planes, candidates for right child + planes[planeCount - 1].rightCount = bins[planeCount].count; + planes[planeCount - 1].rightAABB = bins[planeCount].aabb; + for ( int i = planeCount - 2; i >= 0; --i ) + { + planes[i].rightCount = planes[i + 1].rightCount + bins[i + 1].count; + planes[i].rightAABB = b3AABB_Union( planes[i + 1].rightAABB, bins[i + 1].aabb ); + } + + // Find best split to minimize SAH + float minCost = FLT_MAX; + int bestPlane = 0; + for ( int i = 0; i < planeCount; ++i ) + { + float leftArea = b3Perimeter( planes[i].leftAABB ); + float rightArea = b3Perimeter( planes[i].rightAABB ); + int leftCount = planes[i].leftCount; + int rightCount = planes[i].rightCount; + + float cost = leftCount * leftArea + rightCount * rightArea; + if ( cost < minCost ) + { + bestPlane = i; + minCost = cost; + } + } + + // Partition node indices and boxes using the Hoare partition scheme + // https://en.wikipedia.org/wiki/Quicksort + // https://nicholasvadivelu.com/2021/01/11/array-partition/ + int i1 = 0, i2 = count; + while ( i1 < i2 ) + { + while ( i1 < i2 && binIndices[i1] < bestPlane ) + { + i1 += 1; + }; + + while ( i1 < i2 && binIndices[i2 - 1] >= bestPlane ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap indices + { + int temp = indices[i1]; + indices[i1] = indices[i2 - 1]; + indices[i2 - 1] = temp; + } + + // Swap boxes + { + b3AABB temp = boxes[i1]; + boxes[i1] = boxes[i2 - 1]; + boxes[i2 - 1] = temp; + } + + i1 += 1; + i2 -= 1; + } + } + B3_ASSERT( i1 == i2 ); + + if ( i1 > 0 && i1 < count ) + { + return i1; + } + else + { + return count / 2; + } +} + +#endif + +// Temporary data used to track the rebuild of a tree node +struct b3RebuildItem +{ + int nodeIndex; + int childCount; + + // Leaf indices + int startIndex; + int splitIndex; + int endIndex; +}; + +// Returns root node index +static int b3BuildTree( b3DynamicTree* tree, int leafCount ) +{ + b3TreeNode* nodes = tree->nodes; + int* leafIndices = tree->leafIndices; + + if ( leafCount == 1 ) + { + nodes[leafIndices[0]].parent = B3_NULL_INDEX; + return leafIndices[0]; + } + +#if B3_TREE_HEURISTIC == 0 + b3Vec3* leafCenters = tree->leafCenters; +#else + b3AABB* leafBoxes = tree->leafBoxes; + int* binIndices = tree->binIndices; +#endif + + // todo large stack item + struct b3RebuildItem stack[B3_TREE_STACK_SIZE]; + int top = 0; + + stack[0].nodeIndex = b3AllocateNode( tree ); + stack[0].childCount = -1; + stack[0].startIndex = 0; + stack[0].endIndex = leafCount; +#if B3_TREE_HEURISTIC == 0 + stack[0].splitIndex = b3PartitionMid( leafIndices, leafCenters, leafCount ); +#else + stack[0].splitIndex = b3PartitionSAH( leafIndices, binIndices, leafBoxes, leafCount ); +#endif + + while ( true ) + { + struct b3RebuildItem* item = stack + top; + + item->childCount += 1; + + if ( item->childCount == 2 ) + { + // This internal node has both children established + + if ( top == 0 ) + { + // all done + break; + } + + struct b3RebuildItem* parentItem = stack + ( top - 1 ); + b3TreeNode* parentNode = nodes + parentItem->nodeIndex; + + if ( parentItem->childCount == 0 ) + { + B3_ASSERT( parentNode->children.child1 == B3_NULL_INDEX ); + parentNode->children.child1 = item->nodeIndex; + } + else + { + B3_ASSERT( parentItem->childCount == 1 ); + B3_ASSERT( parentNode->children.child2 == B3_NULL_INDEX ); + parentNode->children.child2 = item->nodeIndex; + } + + b3TreeNode* node = nodes + item->nodeIndex; + + B3_ASSERT( node->parent == B3_NULL_INDEX ); + node->parent = parentItem->nodeIndex; + + B3_ASSERT( node->children.child1 != B3_NULL_INDEX ); + B3_ASSERT( node->children.child2 != B3_NULL_INDEX ); + b3TreeNode* child1 = nodes + node->children.child1; + b3TreeNode* child2 = nodes + node->children.child2; + + node->aabb = b3AABB_Union( child1->aabb, child2->aabb ); + node->height = 1 + b3MaxUInt16( child1->height, child2->height ); + node->categoryBits = child1->categoryBits | child2->categoryBits; + + // Pop stack + top -= 1; + } + else + { + int startIndex, endIndex; + if ( item->childCount == 0 ) + { + startIndex = item->startIndex; + endIndex = item->splitIndex; + } + else + { + B3_ASSERT( item->childCount == 1 ); + startIndex = item->splitIndex; + endIndex = item->endIndex; + } + + int count = endIndex - startIndex; + + if ( count == 1 ) + { + int childIndex = leafIndices[startIndex]; + b3TreeNode* node = nodes + item->nodeIndex; + + if ( item->childCount == 0 ) + { + B3_ASSERT( node->children.child1 == B3_NULL_INDEX ); + node->children.child1 = childIndex; + } + else + { + B3_ASSERT( item->childCount == 1 ); + B3_ASSERT( node->children.child2 == B3_NULL_INDEX ); + node->children.child2 = childIndex; + } + + b3TreeNode* childNode = nodes + childIndex; + B3_ASSERT( childNode->parent == B3_NULL_INDEX ); + childNode->parent = item->nodeIndex; + } + else + { + B3_ASSERT( count > 0 ); + B3_ASSERT( top < B3_TREE_STACK_SIZE ); + + top += 1; + struct b3RebuildItem* newItem = stack + top; + newItem->nodeIndex = b3AllocateNode( tree ); + newItem->childCount = -1; + newItem->startIndex = startIndex; + newItem->endIndex = endIndex; +#if B3_TREE_HEURISTIC == 0 + newItem->splitIndex = b3PartitionMid( leafIndices + startIndex, leafCenters + startIndex, count ); +#else + newItem->splitIndex = + b3PartitionSAH( leafIndices + startIndex, binIndices + startIndex, leafBoxes + startIndex, count ); +#endif + newItem->splitIndex += startIndex; + } + } + } + + b3TreeNode* rootNode = nodes + stack[0].nodeIndex; + B3_ASSERT( rootNode->parent == B3_NULL_INDEX ); + B3_ASSERT( rootNode->children.child1 != B3_NULL_INDEX ); + B3_ASSERT( rootNode->children.child2 != B3_NULL_INDEX ); + + b3TreeNode* child1 = nodes + rootNode->children.child1; + b3TreeNode* child2 = nodes + rootNode->children.child2; + + rootNode->aabb = b3AABB_Union( child1->aabb, child2->aabb ); + rootNode->height = 1 + b3MaxUInt16( child1->height, child2->height ); + rootNode->categoryBits = child1->categoryBits | child2->categoryBits; + + return stack[0].nodeIndex; +} + +// Not safe to access tree during this operation because it may grow +int b3DynamicTree_Rebuild( b3DynamicTree* tree, bool fullBuild ) +{ + int proxyCount = tree->proxyCount; + if ( proxyCount == 0 ) + { + return 0; + } + + // Ensure capacity for rebuild space + if ( proxyCount > tree->rebuildCapacity ) + { + int newCapacity = proxyCount + proxyCount / 2; + + b3Free( tree->leafIndices, tree->rebuildCapacity * sizeof( int ) ); + tree->leafIndices = (int*)b3Alloc( newCapacity * sizeof( int ) ); + +#if B3_TREE_HEURISTIC == 0 + b3Free( tree->leafCenters, tree->rebuildCapacity * sizeof( b3Vec3 ) ); + tree->leafCenters = (b3Vec3*)b3Alloc( newCapacity * sizeof( b3Vec3 ) ); +#else + b3Free( tree->leafBoxes, tree->rebuildCapacity * sizeof( b3AABB ) ); + tree->leafBoxes = (b3AABB*)b3Alloc( newCapacity * sizeof( b3AABB ) ); + b3Free( tree->binIndices, tree->rebuildCapacity * sizeof( int ) ); + tree->binIndices = (int*)b3Alloc( newCapacity * sizeof( int ) ); +#endif + tree->rebuildCapacity = newCapacity; + } + + int leafCount = 0; + int stack[B3_TREE_STACK_SIZE]; + int stackCount = 0; + + int nodeIndex = tree->root; + b3TreeNode* nodes = tree->nodes; + b3TreeNode* node = nodes + nodeIndex; + + // These are the nodes that get sorted to rebuild the tree. + // I'm using indices because the node pool may grow during the build. + int* leafIndices = tree->leafIndices; + +#if B3_TREE_HEURISTIC == 0 + b3Vec3* leafCenters = tree->leafCenters; +#else + b3AABB* leafBoxes = tree->leafBoxes; +#endif + + // Gather all proxy nodes that have grown and all internal nodes that haven't grown. Both are + // considered leaves in the tree rebuild. + // Free all internal nodes that have grown. + // todo use a node growth metric instead of simply enlarged to reduce rebuild size and frequency + // this should be weighed against b3_aabbMargin + while ( true ) + { + if ( b3IsLeaf( node ) == true || ( ( node->flags & b3_enlargedNode ) == 0 && fullBuild == false ) ) + { + leafIndices[leafCount] = nodeIndex; +#if B3_TREE_HEURISTIC == 0 + leafCenters[leafCount] = b3AABB_Center( node->aabb ); +#else + leafBoxes[leafCount] = node->aabb; +#endif + leafCount += 1; + + // Detach + node->parent = B3_NULL_INDEX; + } + else + { + int doomedNodeIndex = nodeIndex; + + // Handle children + nodeIndex = node->children.child1; + + B3_ASSERT( stackCount < B3_TREE_STACK_SIZE ); + if ( stackCount < B3_TREE_STACK_SIZE ) + { + stack[stackCount++] = node->children.child2; + } + + node = nodes + nodeIndex; + + // Remove doomed node + b3FreeNode( tree, doomedNodeIndex ); + + continue; + } + + if ( stackCount == 0 ) + { + break; + } + + nodeIndex = stack[--stackCount]; + node = nodes + nodeIndex; + } + +#if B3_ENABLE_VALIDATION == 1 + int capacity = tree->nodeCapacity; + for ( int i = 0; i < capacity; ++i ) + { + if ( nodes[i].flags & b3_allocatedNode ) + { + B3_ASSERT( ( nodes[i].flags & b3_enlargedNode ) == 0 ); + } + } +#endif + + B3_ASSERT( leafCount <= proxyCount ); + + tree->root = b3BuildTree( tree, leafCount ); + + b3DynamicTree_Validate( tree ); + + return leafCount; +} + +static FILE* b3OpenTreeFile( const char* fileName, const char* mode ) +{ + FILE* file = NULL; + +#if defined( _MSC_VER ) + errno_t e = fopen_s( &file, fileName, mode ); + if ( e != 0 ) + { + return NULL; + } +#else + file = fopen( fileName, mode ); + if ( file == NULL ) + { + return NULL; + } +#endif + + return file; +} + +void b3DynamicTree_Save( const b3DynamicTree* tree, const char* fileName ) +{ + FILE* file = b3OpenTreeFile( fileName, "wb" ); + if ( file == NULL ) + { + return; + } + + // Copy to allow setting some fields to zero + b3DynamicTree temp = *tree; + + // Zero pointers and temp data + temp.nodes = NULL; + temp.leafIndices = NULL; + temp.leafBoxes = NULL; + temp.leafCenters = NULL; + temp.binIndices = NULL; + temp.rebuildCapacity = 0; + + // Write tree struct + fwrite( &temp, sizeof( b3DynamicTree ), 1, file ); + + // Write the node array, this includes the free list + if ( tree->nodeCapacity > 0 && tree->nodes != NULL ) + { + fwrite( tree->nodes, sizeof( b3TreeNode ), tree->nodeCapacity, file ); + } + + fclose( file ); +} + +b3DynamicTree b3DynamicTree_Load( const char* fileName, float scale ) +{ + b3DynamicTree tree = { 0 }; + + FILE* file = b3OpenTreeFile( fileName, "rb" ); + if ( file == NULL ) + { + return tree; + } + + int readCount = (int)fread( &tree, sizeof( b3DynamicTree ), 1, file ); + if ( readCount != 1 ) + { + fclose( file ); + return tree; + } + + if ( tree.version != B3_DYNAMIC_TREE_VERSION ) + { + fclose( file ); + memset( &tree, 0, sizeof( b3DynamicTree ) ); + return tree; + } + + if ( tree.nodeCapacity > 0 ) + { + tree.nodes = (b3TreeNode*)b3Alloc( tree.nodeCapacity * sizeof( b3TreeNode ) ); + readCount = (int)fread( tree.nodes, sizeof( b3TreeNode ), tree.nodeCapacity, file ); + if ( readCount != tree.nodeCapacity ) + { + b3Free( tree.nodes, tree.nodeCapacity * sizeof( b3TreeNode ) ); + fclose( file ); + memset( &tree, 0, sizeof( b3DynamicTree ) ); + return tree; + } + + for ( int i = 0; i < tree.nodeCapacity; ++i ) + { + b3TreeNode* node = tree.nodes + i; + node->aabb.lowerBound = b3MulSV( scale, node->aabb.lowerBound ); + node->aabb.upperBound = b3MulSV( scale, node->aabb.upperBound ); + } + } + else + { + tree.nodes = NULL; + } + + // Zero temp data fields + tree.leafIndices = NULL; + tree.leafBoxes = NULL; + tree.leafCenters = NULL; + tree.binIndices = NULL; + tree.rebuildCapacity = 0; + + fclose( file ); + + return tree; +} diff --git a/src/height_field.c b/src/height_field.c new file mode 100644 index 0000000..93d2462 --- /dev/null +++ b/src/height_field.c @@ -0,0 +1,1601 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "algorithm.h" +#include "body.h" +#include "core.h" +#include "shape.h" +#include "simd.h" + +#include "box3d/collision.h" +#include "box3d/constants.h" +#include "box3d/math_functions.h" + +#include +#include +#include + +/* + Convention + + index = row * columnCount + column + height = minHeight + heightScale * compressedHeights[index]; + + column = index % columnCount; + row = index / columnCount; + + x-axis : columns + z-axis : rows + + 00 --- 01 --- 02 --- 03 X + | 0 | 1 | 2 | + 04 --- 05 --- 06 --- 07 + | 3 | 4 | 5 | + 08 --- 09 --- 10 --- 11 + | 6 | 7 | 8 | + 12 --- 13 --- 14 --- 15 + Z + + The quads exist before the column and row ends: row < rowCount - 1 and column < columnCount - 1 + quadIndex = row * (columnCount - 1) + column + + Quad origin index from quad index (needs row): + index = quadIndex + row * columnCount + + Triangle index is related to the quad index + triangleIndex = 2 * quadIndex + (0/1) + quadIndex = triangleIndex / 2 + + Row and column from quad index: + row = quadIndex / (columnCount - 1) + column = quadIndex - row * (columnCount - 1) + + The triangle diagonal is fixed. + + triangle0 = {00, 04, 01} -> {11, 21, 12} + triangle1 = {04, 05, 01} -> {22, 12, 21} + + 11 12 + 00 ---- 01 + | / | + | 0 / 1 | 1 + | / | + 04 ---- 05 + 21 22 + + For adjacency we have + + NA + 00 ---- 01 ---- 02 + | / | / | + NA | 0 / 1 | 2 / 3 | + | / | / | + 04 ---- 05 ---- 06 + | / | / | + NA | 6 / 7 | 8 / 9 | + | / | / | + 08 ---- 09 ---- 10 + + 0: NA, NA, 1 + 1: 0, 6, 2 + + Triangle layouts + + 11 3 12 + 1 ---- 3 + | / + 1 | 0 / 2 + | / + 2 + 21 + + 12 + 2 + / | + 2 / 1 | 1 + / | + 3 ---- 1 + 21 3 22 + */ + +b3HeightField* b3CreateHeightField( const b3HeightFieldDef* data ) +{ + b3HeightField* hf = (b3HeightField*)b3Alloc( sizeof( b3HeightField ) ); + + int columnCount = data->countX; + int rowCount = data->countZ; + + hf->scale = data->scale; + hf->columnCount = columnCount; + hf->rowCount = rowCount; + + int heightCount = columnCount * rowCount; + B3_ASSERT( heightCount >= 4 ); + + hf->compressedHeights = (uint16_t*)b3Alloc( heightCount * sizeof( uint16_t ) ); + + const float* heights = data->heights; + + B3_ASSERT( data->globalMinimumHeight <= data->globalMaximumHeight ); + hf->minHeight = data->globalMinimumHeight; + hf->maxHeight = data->globalMaximumHeight; + + float height = b3MaxFloat( hf->maxHeight - hf->minHeight, B3_LINEAR_SLOP ); + hf->heightScale = height / UINT16_MAX; + + float lowerHeightBound = hf->maxHeight; + float upperHeightBound = hf->minHeight; + + float invHeightScale = 1.0f / hf->heightScale; + for ( int i = 0; i < heightCount; ++i ) + { + float clampedHeight = b3ClampFloat( heights[i], hf->minHeight, hf->maxHeight ); + float scaledHeight = ( clampedHeight - hf->minHeight ) * invHeightScale; + hf->compressedHeights[i] = (uint16_t)( b3MinFloat( scaledHeight, (float)UINT16_MAX ) ); + + lowerHeightBound = b3MinFloat( lowerHeightBound, clampedHeight ); + upperHeightBound = b3MaxFloat( upperHeightBound, clampedHeight ); + } + + // Use decompressed heights for accurate convexity metrics. + float* decompressedHeights = (float*)b3Alloc( heightCount * sizeof( float ) ); + for ( int i = 0; i < heightCount; ++i ) + { + decompressedHeights[i] = hf->minHeight + hf->heightScale * hf->compressedHeights[i]; + } + heights = decompressedHeights; + + int cellCount = ( hf->columnCount - 1 ) * ( hf->rowCount - 1 ); + + hf->materialIndices = (uint8_t*)b3Alloc( cellCount * sizeof( uint8_t ) ); + + if ( data->materialIndices != NULL ) + { + for ( int i = 0; i < cellCount; ++i ) + { + hf->materialIndices[i] = data->materialIndices[i]; + } + } + else + { + for ( int i = 0; i < cellCount; ++i ) + { + hf->materialIndices[i] = 0; + } + } + + hf->aabb.lowerBound = (b3Vec3){ 0.0f, hf->scale.y * lowerHeightBound, 0.0f }; + hf->aabb.upperBound = + (b3Vec3){ hf->scale.x * ( hf->columnCount - 1 ), hf->scale.y * upperHeightBound, hf->scale.z * ( hf->rowCount - 1 ) }; + + hf->version = B3_HEIGHT_FIELD_VERSION; + + hf->clockwise = data->clockwiseWinding; + + int triangleCount = 2 * cellCount; + hf->flags = (uint8_t*)b3Alloc( triangleCount * sizeof( uint8_t ) ); + + memset( hf->flags, 0, triangleCount * sizeof( uint8_t ) ); + + float cos5Deg = 0.9962f; + b3Vec3 scale = hf->scale; + + int triangleIndex = 0; + for ( int row = 0; row < rowCount - 1; ++row ) + { + for ( int column = 0; column < columnCount - 1; ++column ) + { + // todo compute convexity flags + // This requires a couple things + // - determine all 3 adjacent triangles for each triangle + // - consider clockwise winding + // - consider borders where there is no adjacent triangle + + int triangleIndex1 = triangleIndex; + int triangleIndex2 = triangleIndex + 1; + triangleIndex += 2; + + int cellIndex = row * ( columnCount - 1 ) + column; + + if ( hf->materialIndices[cellIndex] == B3_HEIGHT_FIELD_HOLE ) + { + continue; + } + + int flags1 = 0; + int flags2 = 0; + + b3Plane plane1, plane2; + b3Vec3 center1, center2; + + int index11 = row * columnCount + column; + int index12 = index11 + 1; + int index21 = ( row + 1 ) * columnCount + column; + int index22 = index21 + 1; + + { + float height11 = heights[index11]; + float height12 = heights[index12]; + float height21 = heights[index21]; + float height22 = heights[index22]; + + float x1 = (float)( column ); + float x2 = (float)( column + 1 ); + float z1 = (float)( row ); + float z2 = (float)( row + 1 ); + + // triangle 0 : 11, 21, 12 + b3Vec3 vs0[3]; + vs0[0] = b3Mul( scale, (b3Vec3){ x1, height11, z1 } ); + vs0[1] = b3Mul( scale, (b3Vec3){ x1, height21, z2 } ); + vs0[2] = b3Mul( scale, (b3Vec3){ x2, height12, z1 } ); + plane1 = b3MakePlaneFromPoints( vs0[0], vs0[1], vs0[2] ); + + center1 = b3MulSV( 1.0f / 3.0f, b3Add( b3Add( vs0[0], vs0[1] ), vs0[2] ) ); + + // triangle 1 : 22, 12, 21 + b3Vec3 vs1[3]; + vs1[0] = b3Mul( scale, (b3Vec3){ x2, height22, z2 } ); + vs1[1] = b3Mul( scale, (b3Vec3){ x2, height12, z1 } ); + vs1[2] = b3Mul( scale, (b3Vec3){ x1, height21, z2 } ); + plane2 = b3MakePlaneFromPoints( vs1[0], vs1[1], vs1[2] ); + + center2 = b3MulSV( 1.0f / 3.0f, b3Add( b3Add( vs1[0], vs1[1] ), vs1[2] ) ); + + float separation = b3PlaneSeparation( plane1, vs1[0] ); + float cosAngle = b3Dot( plane1.normal, plane2.normal ); + if ( separation > 0.0f || cosAngle > cos5Deg ) + { + flags1 |= b3_concaveEdge2; + flags2 |= b3_concaveEdge2; + } + } + + B3_UNUSED( center1 ); + B3_UNUSED( center2 ); + + // top + int topCellIndex = ( row - 1 ) * ( columnCount - 1 ) + column; + if ( row > 0 && hf->materialIndices[topCellIndex] != B3_HEIGHT_FIELD_HOLE ) + { + B3_ASSERT( 0 <= topCellIndex && topCellIndex < cellCount ); + + int r = row - 1; + int c = column; + + int i11 = r * columnCount + c; + int i12 = i11 + 1; + int i21 = ( r + 1 ) * columnCount + c; + int i22 = i21 + 1; + + B3_ASSERT( i21 == index11 ); + B3_ASSERT( i22 == index12 ); + + // float h11 = heights[i11]; + float h12 = heights[i12]; + float h21 = heights[i21]; + float h22 = heights[i22]; + + float x1 = (float)( c ); + float x2 = (float)( c + 1 ); + float z1 = (float)( r ); + float z2 = (float)( r + 1 ); + + // triangle 1 + b3Vec3 vs[3]; + vs[0] = b3Mul( scale, (b3Vec3){ x2, h22, z2 } ); + vs[1] = b3Mul( scale, (b3Vec3){ x2, h12, z1 } ); + vs[2] = b3Mul( scale, (b3Vec3){ x1, h21, z2 } ); + + b3Vec3 n = b3Normalize( b3Cross( b3Sub( vs[1], vs[0] ), b3Sub( vs[2], vs[0] ) ) ); + + float separation = b3PlaneSeparation( plane1, vs[1] ); + float cosAngle = b3Dot( plane1.normal, n ); + if ( separation > 0.0f || cosAngle > cos5Deg ) + { + flags1 |= b3_concaveEdge3; + } + } + + int bottomCellIndex = ( row + 1 ) * ( columnCount - 1 ) + column; + if ( row + 1 < rowCount - 1 && hf->materialIndices[bottomCellIndex] != B3_HEIGHT_FIELD_HOLE ) + { + B3_ASSERT( 0 <= bottomCellIndex && bottomCellIndex < cellCount ); + + int r = row + 1; + int c = column; + + int i11 = r * columnCount + c; + int i12 = i11 + 1; + int i21 = ( r + 1 ) * columnCount + c; + // int i22 = i21 + 1; + + B3_ASSERT( i11 == index21 ); + B3_ASSERT( i12 == index22 ); + + float h11 = heights[i11]; + float h12 = heights[i12]; + float h21 = heights[i21]; + // float h22 = heights[i22]; + + float x1 = (float)( c ); + float x2 = (float)( c + 1 ); + float z1 = (float)( r ); + float z2 = (float)( r + 1 ); + + // triangle 0 + b3Vec3 vs[3]; + vs[0] = b3Mul( scale, (b3Vec3){ x1, h11, z1 } ); + vs[1] = b3Mul( scale, (b3Vec3){ x1, h21, z2 } ); + vs[2] = b3Mul( scale, (b3Vec3){ x2, h12, z1 } ); + + b3Vec3 n = b3Normalize( b3Cross( b3Sub( vs[1], vs[0] ), b3Sub( vs[2], vs[0] ) ) ); + + float separation = b3PlaneSeparation( plane2, vs[1] ); + float cosAngle = b3Dot( plane2.normal, n ); + if ( separation > 0.0f || cosAngle > cos5Deg ) + { + flags2 |= b3_concaveEdge3; + } + } + + int leftCellIndex = row * ( columnCount - 1 ) + column - 1; + if ( column - 1 >= 0 && hf->materialIndices[leftCellIndex] != B3_HEIGHT_FIELD_HOLE ) + { + B3_ASSERT( 0 <= leftCellIndex && leftCellIndex < cellCount ); + + int r = row; + int c = column - 1; + + int i11 = r * columnCount + c; + int i12 = i11 + 1; + int i21 = ( r + 1 ) * columnCount + c; + int i22 = i21 + 1; + + B3_ASSERT( i12 == index11 ); + B3_ASSERT( i22 == index21 ); + + // float h11 = heights[i11]; + float h12 = heights[i12]; + float h21 = heights[i21]; + float h22 = heights[i22]; + + float x1 = (float)( c ); + float x2 = (float)( c + 1 ); + float z1 = (float)( r ); + float z2 = (float)( r + 1 ); + + // triangle 1 + b3Vec3 vs[3]; + vs[0] = b3Mul( scale, (b3Vec3){ x2, h22, z2 } ); + vs[1] = b3Mul( scale, (b3Vec3){ x2, h12, z1 } ); + vs[2] = b3Mul( scale, (b3Vec3){ x1, h21, z2 } ); + + b3Vec3 n = b3Normalize( b3Cross( b3Sub( vs[1], vs[0] ), b3Sub( vs[2], vs[0] ) ) ); + + float separation = b3PlaneSeparation( plane1, vs[2] ); + float cosAngle = b3Dot( plane1.normal, n ); + if ( separation > 0.0f || cosAngle > cos5Deg ) + { + flags1 |= b3_concaveEdge1; + } + } + + int rightCellIndex = row * ( columnCount - 1 ) + column + 1; + if ( column + 1 < columnCount - 1 && hf->materialIndices[rightCellIndex] != B3_HEIGHT_FIELD_HOLE ) + { + B3_ASSERT( 0 <= rightCellIndex && rightCellIndex < cellCount ); + + int r = row; + int c = column + 1; + + int i11 = r * columnCount + c; + int i12 = i11 + 1; + int i21 = ( r + 1 ) * columnCount + c; + // int i22 = i21 + 1; + + B3_ASSERT( i11 == index12 ); + B3_ASSERT( i21 == index22 ); + + float h11 = heights[i11]; + float h12 = heights[i12]; + float h21 = heights[i21]; + // float h22 = heights[i22]; + + float x1 = (float)( c ); + float x2 = (float)( c + 1 ); + float z1 = (float)( r ); + float z2 = (float)( r + 1 ); + + // triangle 0 + b3Vec3 vs[3]; + vs[0] = b3Mul( scale, (b3Vec3){ x1, h11, z1 } ); + vs[1] = b3Mul( scale, (b3Vec3){ x1, h21, z2 } ); + vs[2] = b3Mul( scale, (b3Vec3){ x2, h12, z1 } ); + + b3Vec3 n = b3Normalize( b3Cross( b3Sub( vs[1], vs[0] ), b3Sub( vs[2], vs[0] ) ) ); + + float separation = b3PlaneSeparation( plane2, vs[2] ); + float cosAngle = b3Dot( plane2.normal, n ); + if ( separation > 0.0f || cosAngle > cos5Deg ) + { + flags2 |= b3_concaveEdge1; + } + } + + B3_ASSERT( 0 <= flags1 && flags1 <= UINT8_MAX ); + B3_ASSERT( 0 <= flags2 && flags2 <= UINT8_MAX ); + + hf->flags[triangleIndex1] = (uint8_t)flags1; + hf->flags[triangleIndex2] = (uint8_t)flags2; + } + } + + B3_ASSERT( triangleIndex == triangleCount ); + + b3Free( decompressedHeights, heightCount * sizeof( float ) ); + + hf->hash = 0; + uint32_t h = B3_HASH_INIT; + h = b3Hash( h, (const uint8_t*)hf->compressedHeights, heightCount * (int)sizeof( uint16_t ) ); + h = b3Hash( h, (const uint8_t*)hf->materialIndices, cellCount * (int)sizeof( uint8_t ) ); + h = b3Hash( h, (const uint8_t*)hf->flags, triangleCount * (int)sizeof( uint8_t ) ); + h = b3Hash( h, (const uint8_t*)&hf->aabb, (int)( sizeof( b3HeightField ) - offsetof( b3HeightField, aabb ) ) ); + hf->hash = h; + + return hf; +} + +_Static_assert( b3_concaveEdge3 == 4 * b3_concaveEdge1, "bit math" ); +_Static_assert( b3_inverseConcaveEdge3 == 4 * b3_inverseConcaveEdge1, "bit math" ); + +b3Triangle b3GetHeightFieldTriangle( const b3HeightField* heightField, int triangleIndex ) +{ + B3_ASSERT( 0 <= triangleIndex ); + B3_ASSERT( triangleIndex < 2 * ( heightField->columnCount - 1 ) * ( heightField->rowCount - 1 ) ); + + b3Triangle triangle; + triangle.flags = heightField->flags[triangleIndex]; + + int columnCount = heightField->columnCount; + int quadIndex = triangleIndex >> 1; + int row = quadIndex / ( columnCount - 1 ); + int column = quadIndex - row * ( columnCount - 1 ); + + int index11 = row * columnCount + column; + int index12 = index11 + 1; + int index21 = ( row + 1 ) * columnCount + column; + int index22 = index21 + 1; + + int cellIndex = row * ( columnCount - 1 ) + column; + + B3_ASSERT( quadIndex == cellIndex ); + B3_ASSERT( heightField->materialIndices[cellIndex] != B3_HEIGHT_FIELD_HOLE ); + B3_UNUSED( cellIndex ); + + float minHeight = heightField->minHeight; + float heightScale = heightField->heightScale; + const uint16_t* heights = heightField->compressedHeights; + + float height11 = minHeight + heightScale * heights[index11]; + float height12 = minHeight + heightScale * heights[index12]; + float height21 = minHeight + heightScale * heights[index21]; + float height22 = minHeight + heightScale * heights[index22]; + + float x1 = (float)( column ); + float x2 = (float)( column + 1 ); + float z1 = (float)( row ); + float z2 = (float)( row + 1 ); + + b3Vec3 scale = heightField->scale; + if ( ( triangleIndex & 1 ) == 0 ) + { + triangle.vertices[0] = b3Mul( scale, (b3Vec3){ x1, height11, z1 } ); + triangle.vertices[1] = b3Mul( scale, (b3Vec3){ x1, height21, z2 } ); + triangle.vertices[2] = b3Mul( scale, (b3Vec3){ x2, height12, z1 } ); + triangle.i1 = index11; + triangle.i2 = index21; + triangle.i3 = index12; + } + else + { + triangle.vertices[0] = b3Mul( scale, (b3Vec3){ x2, height22, z2 } ); + triangle.vertices[1] = b3Mul( scale, (b3Vec3){ x2, height12, z1 } ); + triangle.vertices[2] = b3Mul( scale, (b3Vec3){ x1, height21, z2 } ); + triangle.i1 = index22; + triangle.i2 = index12; + triangle.i3 = index21; + } + + if ( heightField->clockwise ) + { + B3_SWAP( triangle.vertices[1], triangle.vertices[2] ); + B3_SWAP( triangle.i2, triangle.i3 ); + + // Reversing winding swaps edge1 and edge3; edge2 (the diagonal) is preserved. + int flags = triangle.flags; + int edge1Bits = flags & ( b3_concaveEdge1 | b3_inverseConcaveEdge1 ); + int edge3Bits = flags & ( b3_concaveEdge3 | b3_inverseConcaveEdge3 ); + flags &= ~( b3_concaveEdge1 | b3_concaveEdge3 | b3_inverseConcaveEdge1 | b3_inverseConcaveEdge3 ); + flags |= edge1Bits << 2; + flags |= edge3Bits >> 2; + triangle.flags = flags; + } + + return triangle; +} + +int b3GetHeightFieldMaterial( const b3HeightField* heightField, int triangleIndex ) +{ + B3_ASSERT( 0 <= triangleIndex ); + B3_ASSERT( triangleIndex < 2 * ( heightField->columnCount - 1 ) * ( heightField->rowCount - 1 ) ); + + int cellIndex = triangleIndex >> 1; + return heightField->materialIndices[cellIndex]; +} + +b3AABB b3ComputeHeightFieldAABB( const b3HeightField* shape, b3Transform transform ) +{ + return b3AABB_Transform( transform, shape->aabb ); +} + +b3CastOutput b3RayCastHeightField( const b3HeightField* heightField, const b3RayCastInput* input ) +{ + b3ShapeCastInput shapeCastInput = { 0 }; + shapeCastInput.proxy = (b3ShapeProxy){ &input->origin, 1, 0.0f }; + shapeCastInput.translation = input->translation; + shapeCastInput.maxFraction = input->maxFraction; + + return b3ShapeCastHeightField( heightField, &shapeCastInput ); +} + +// todo advance cast to the grid border immediately if it starts outside the row/column range +// todo terminate the cast immediately if it leaves the row/column range +b3CastOutput b3ShapeCastHeightField( const b3HeightField* heightField, const b3ShapeCastInput* input ) +{ + b3AABB shapeBounds = b3MakeAABB( input->proxy.points, input->proxy.count, input->proxy.radius ); + b3Vec3 shapeTranslation = input->translation; + b3Vec3 scale = heightField->scale; + + b3Vec3 shapeStart = b3AABB_Center( shapeBounds ); + b3Vec3 shapeDelta = b3MulSV( input->maxFraction, shapeTranslation ); + b3Vec3 shapeEnd = b3Add( shapeStart, shapeDelta ); + + b3CastOutput result = { 0 }; + + b3Vec3 shapeExtents = b3AABB_Extents( shapeBounds ); + b3Vec3 margin = { B3_AABB_MARGIN, B3_AABB_MARGIN, B3_AABB_MARGIN }; + b3AABB combinedBounds = { b3Sub( b3Sub( heightField->aabb.lowerBound, shapeExtents ), margin ), + b3Add( b3Add( heightField->aabb.upperBound, shapeExtents ), margin ) }; + + float minFraction, maxFraction; + bool intersects = b3IntersectRayAndAABB( combinedBounds.lowerBound, combinedBounds.upperBound, shapeStart, shapeEnd, + &minFraction, &maxFraction ); + if ( intersects == false ) + { + return result; + } + + // These are for walking the grid, not the triangle cast. + // The triangle cast uses the unclamped ray and fraction. + b3Vec3 clampedStart = b3MulAdd( shapeStart, minFraction, shapeDelta ); + b3Vec3 clampedDelta = b3MulSV( maxFraction - minFraction, shapeDelta ); + b3Vec3 clampedEnd = b3Add( clampedStart, clampedDelta ); + + // The grid traversal starts from the leading shape bounds corner + float signX, signZ; + if ( shapeTranslation.x >= 0.0f ) + { + clampedStart.x += shapeExtents.x; + signX = 1.0f; + } + else + { + clampedStart.x -= shapeExtents.x; + signX = -1.0f; + } + + if ( shapeTranslation.z >= 0.0f ) + { + clampedStart.z += shapeExtents.z; + signZ = 1.0f; + } + else + { + clampedStart.z -= shapeExtents.z; + signZ = -1.0f; + } + + // Shift the end as well + clampedEnd = b3Add( clampedStart, clampedDelta ); + + // Row and column range for the shape cast + int columnStart = (int)floorf( clampedStart.x / scale.x ); + int columnEnd = (int)floorf( clampedEnd.x / scale.x ); + int rowStart = (int)floorf( clampedStart.z / scale.z ); + int rowEnd = (int)floorf( clampedEnd.z / scale.z ); + + b3Vec3 absClampedDelta = b3Abs( clampedDelta ); + + // Precompute increments for row and column traversal. + // The ray can be slightly tilted yet remain within a single row or column + // once rasterized. + float deltaAlphaX; + float nextFractionX; + int deltaColumn; + + if ( columnStart < columnEnd ) + { + B3_ASSERT( absClampedDelta.x > 0.0f ); + + // Going forward on x columns + deltaAlphaX = scale.x / absClampedDelta.x; + nextFractionX = ( scale.x * ( columnStart + 1 ) - clampedStart.x ) / absClampedDelta.x; + deltaColumn = 1; + } + else if ( columnEnd < columnStart ) + { + B3_ASSERT( absClampedDelta.x > 0.0f ); + + // Going backwards on x columns + deltaAlphaX = scale.x / absClampedDelta.x; + nextFractionX = ( clampedStart.x - scale.x * columnStart ) / absClampedDelta.x; + deltaColumn = -1; + } + else + { + // Cast stays in a single column + deltaAlphaX = 0.0f; + nextFractionX = FLT_MAX; + deltaColumn = 0; + } + + float deltaAlphaZ; + float nextFractionZ; + int deltaRow; + + if ( rowStart < rowEnd ) + { + B3_ASSERT( absClampedDelta.z > 0.0f ); + + // Going forward on z rows + deltaAlphaZ = scale.z / absClampedDelta.z; + nextFractionZ = ( scale.z * ( rowStart + 1 ) - clampedStart.z ) / absClampedDelta.z; + deltaRow = 1; + } + else if ( rowEnd < rowStart ) + { + B3_ASSERT( absClampedDelta.z > 0.0f ); + + // Going backwards on z rows + deltaAlphaZ = scale.z / absClampedDelta.z; + nextFractionZ = ( clampedStart.z - scale.z * rowStart ) / absClampedDelta.z; + deltaRow = -1; + } + else + { + // Cast stays in a single row + deltaAlphaZ = 0.0f; + nextFractionZ = FLT_MAX; + deltaRow = 0; + } + + // Column and row range for 2D projected initial shape bounds + int boxColumnHead = columnStart; + int boxRowHead = rowStart; + + int boxColumnTail = (int)floorf( ( clampedStart.x - 2.0f * signX * shapeExtents.x ) / scale.x ); + int boxRowTail = (int)floorf( ( clampedStart.z - 2.0f * signZ * shapeExtents.z ) / scale.z ); + + float bestFraction = input->maxFraction; + + int rowCount = heightField->rowCount; + int columnCount = heightField->columnCount; + int cellCount = ( heightField->rowCount - 1 ) * ( heightField->columnCount - 1 ); + B3_UNUSED( cellCount ); + + float minHeight = heightField->minHeight; + float heightScale = heightField->heightScale; + const uint16_t* heights = heightField->compressedHeights; + + b3ShapeCastPairInput pairInput = { 0 }; + pairInput.transformA = b3Transform_identity; + pairInput.proxyB = input->proxy; + pairInput.transformB = b3Transform_identity; + pairInput.translationB = input->translation; + pairInput.canEncroach = input->canEncroach; + + b3AABB castBounds; + castBounds.lowerBound = b3Sub( b3Min( clampedStart, clampedEnd ), shapeExtents ); + castBounds.upperBound = b3Add( b3Max( clampedStart, clampedEnd ), shapeExtents ); + + b3V32 rayOrigin = b3LoadV( &shapeStart.x ); + b3V32 rayTranslation = b3LoadV( &shapeTranslation.x ); + + while ( true ) + { + int column1, column2; + if ( boxColumnTail < boxColumnHead ) + { + column1 = boxColumnTail; + column2 = boxColumnHead; + } + else + { + column1 = boxColumnHead; + column2 = boxColumnTail; + } + + int row1, row2; + if ( boxRowTail < boxRowHead ) + { + row1 = boxRowTail; + row2 = boxRowHead; + } + else + { + row1 = boxRowHead; + row2 = boxRowTail; + } + + for ( int row = row1; row <= row2; ++row ) + { + if ( row < 0 || rowCount - 1 <= row ) + { + continue; + } + + for ( int column = column1; column <= column2; ++column ) + { + if ( column < 0 || columnCount - 1 <= column ) + { + continue; + } + + int cellIndex = row * ( columnCount - 1 ) + column; + B3_ASSERT( cellIndex < cellCount ); + + uint8_t materialIndex = heightField->materialIndices[cellIndex]; + if ( materialIndex == B3_HEIGHT_FIELD_HOLE ) + { + continue; + } + + // index = row * columnCount + column + int index11 = row * columnCount + column; + + int index12 = index11 + 1; + int index21 = ( row + 1 ) * columnCount + column; + int index22 = index21 + 1; + + float height11 = minHeight + heightScale * heights[index11]; + float height12 = minHeight + heightScale * heights[index12]; + float height21 = minHeight + heightScale * heights[index21]; + float height22 = minHeight + heightScale * heights[index22]; + + float x1 = (float)( column ); + float x2 = (float)( column + 1 ); + float z1 = (float)( row ); + float z2 = (float)( row + 1 ); + + b3Vec3 point11 = b3Mul( scale, (b3Vec3){ x1, height11, z1 } ); + b3Vec3 point12 = b3Mul( scale, (b3Vec3){ x2, height12, z1 } ); + b3Vec3 point21 = b3Mul( scale, (b3Vec3){ x1, height21, z2 } ); + b3Vec3 point22 = b3Mul( scale, (b3Vec3){ x2, height22, z2 } ); + + // I know the min/max x and z values, but not the min/max heights. + b3AABB bounds; + bounds.lowerBound = b3Min( b3Min( point11, point12 ), b3Min( point21, point22 ) ); + bounds.upperBound = b3Max( b3Max( point11, point12 ), b3Max( point21, point22 ) ); + + if ( b3AABB_Overlaps( castBounds, bounds ) == false ) + { + continue; + } + + int quadIndex = row * ( columnCount - 1 ) + column; + int triangleIndex1 = 2 * quadIndex; + int triangleIndex2 = triangleIndex1 + 1; + + if ( input->proxy.count == 1 && input->proxy.radius == 0.0f ) + { + // Ray cast + { + b3V32 vertex1 = b3LoadV( &point11.x ); + b3V32 vertex2, vertex3; + + if ( heightField->clockwise ) + { + vertex2 = b3LoadV( &point12.x ); + vertex3 = b3LoadV( &point21.x ); + } + else + { + vertex2 = b3LoadV( &point21.x ); + vertex3 = b3LoadV( &point12.x ); + } + + float alpha = b3IntersectRayTriangle( rayOrigin, rayTranslation, vertex1, vertex2, vertex3 ); + B3_ASSERT( 0 <= alpha && alpha <= 1.0f ); + + if ( alpha < bestFraction ) + { + b3Vec3 edge1 = b3Sub( point21, point11 ); + b3Vec3 edge2 = b3Sub( point12, point11 ); + b3Vec3 normal = heightField->clockwise ? b3Cross( edge2, edge1 ) : b3Cross( edge1, edge2 ); + + result.point = b3MulAdd( shapeStart, alpha, shapeTranslation ); + result.normal = b3Normalize( normal ); + result.fraction = alpha; + result.triangleIndex = triangleIndex1; + result.materialIndex = materialIndex; + result.hit = true; + bestFraction = alpha; + } + } + + { + b3V32 vertex1 = b3LoadV( &point22.x ); + b3V32 vertex2, vertex3; + + if ( heightField->clockwise ) + { + vertex2 = b3LoadV( &point21.x ); + vertex3 = b3LoadV( &point12.x ); + } + else + { + vertex2 = b3LoadV( &point12.x ); + vertex3 = b3LoadV( &point21.x ); + } + + float alpha = b3IntersectRayTriangle( rayOrigin, rayTranslation, vertex1, vertex2, vertex3 ); + B3_ASSERT( 0 <= alpha && alpha <= 1.0f ); + + if ( alpha < bestFraction ) + { + b3Vec3 edge1 = b3Sub( point22, point21 ); + b3Vec3 edge2 = b3Sub( point12, point21 ); + b3Vec3 normal = heightField->clockwise ? b3Cross( edge2, edge1 ) : b3Cross( edge1, edge2 ); + + result.point = b3MulAdd( shapeStart, alpha, shapeTranslation ); + result.normal = b3Normalize( normal ); + result.fraction = alpha; + result.triangleIndex = triangleIndex2; + result.materialIndex = materialIndex; + result.hit = true; + bestFraction = alpha; + } + } + } + else + { + // Shape cast + // todo back-side culling + { + // Shift origin to first vertex + b3Vec3 origin = point11; + b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( point21, origin ), b3Sub( point12, origin ) }; + pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + pairInput.maxFraction = bestFraction; + pairInput.transformB.p = b3Neg( origin ); + + b3CastOutput pairOutput = b3ShapeCast( &pairInput ); + + if ( pairOutput.hit ) + { + bestFraction = pairOutput.fraction; + result = pairOutput; + result.point = b3Add( result.point, origin ); + result.triangleIndex = triangleIndex1; + result.materialIndex = materialIndex; + } + } + + { + // Shift origin to first vertex + b3Vec3 origin = point21; + b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( point22, origin ), b3Sub( point12, origin ) }; + pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + pairInput.maxFraction = bestFraction; + pairInput.transformB.p = b3Neg( origin ); + + b3CastOutput pairOutput = b3ShapeCast( &pairInput ); + + if ( pairOutput.hit ) + { + bestFraction = pairOutput.fraction; + result = pairOutput; + result.point = b3Add( result.point, origin ); + result.triangleIndex = triangleIndex2; + result.materialIndex = materialIndex; + } + } + } + } + } + + // These fractions always increase to guarantee the loop eventually exits + if ( nextFractionX > bestFraction && nextFractionZ > bestFraction ) + { + break; + } + + // Advance the cast to the next column or row + if ( nextFractionX <= nextFractionZ ) + { + if ( boxColumnHead == columnEnd ) + { + // Hit the end already + break; + } + + // Advance to next column + boxColumnHead += deltaColumn; + + // Build a single column to cast + boxColumnTail = boxColumnHead; + + if ( shapeExtents.z == 0.0f ) + { + // Single row + boxRowTail = boxRowHead; + } + else + { + // Rasterize shape row + float rowIntercept = clampedStart.z + nextFractionX * clampedDelta.z; + boxRowTail = (int)floorf( ( rowIntercept - 2.0f * signZ * shapeExtents.z ) / scale.z ); + } + + nextFractionX += deltaAlphaX; + } + else + { + if ( boxRowHead == rowEnd ) + { + // Hit the end already + break; + } + + // Advance to next row + boxRowHead += deltaRow; + + // Build a single row to cast + boxRowTail = boxRowHead; + + if ( shapeExtents.x == 0.0f ) + { + // Single column + boxColumnTail = boxColumnHead; + } + else + { + // Rasterize shape column + float columnIntercept = clampedStart.x + nextFractionZ * clampedDelta.x; + boxColumnTail = (int)floorf( ( columnIntercept - 2.0f * signX * shapeExtents.x ) / scale.x ); + } + + nextFractionZ += deltaAlphaZ; + } + } + + return result; +} + +bool b3OverlapHeightField( const b3HeightField* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ) +{ + b3Vec3 buffer[B3_MAX_SHAPE_CAST_POINTS]; + b3ShapeProxy localProxy = b3MakeLocalProxy( proxy, shapeTransform, buffer ); + b3AABB aabb = b3ComputeProxyAABB( &localProxy ); + + b3Vec3 scale = shape->scale; + int minRow = (int)floorf( aabb.lowerBound.z / scale.z ); + int maxRow = (int)floorf( aabb.upperBound.z / scale.z ); + int minCol = (int)floorf( aabb.lowerBound.x / scale.x ); + int maxCol = (int)floorf( aabb.upperBound.x / scale.x ); + + b3V32 boundsMin = b3LoadV( &aabb.lowerBound.x ); + b3V32 boundsMax = b3LoadV( &aabb.upperBound.x ); + b3V32 boundsCenter = b3MulV( b3_halfV, b3AddV( boundsMin, boundsMax ) ); + b3V32 boundsExtent = b3SubV( boundsMax, boundsCenter ); + + b3DistanceInput input; + input.proxyB = localProxy; + input.transformA = b3Transform_identity; + input.transformB = b3Transform_identity; + input.useRadii = true; + + b3SimplexCache cache = { 0 }; + + float minHeight = shape->minHeight; + float heightScale = shape->heightScale; + + const uint16_t* heights = shape->compressedHeights; + + // Outer loop on rows and inner loop on columns so that triangle indices + // increase monotonically. + for ( int row = minRow; row <= maxRow; ++row ) + { + if ( row < 0 || shape->rowCount - 1 <= row ) + { + continue; + } + + for ( int column = minCol; column <= maxCol; ++column ) + { + if ( column < 0 || shape->columnCount - 1 <= column ) + { + continue; + } + + int cellIndex = row * ( shape->columnCount - 1 ) + column; + B3_ASSERT( cellIndex < ( shape->rowCount - 1 ) * ( shape->columnCount - 1 ) ); + uint8_t material = shape->materialIndices[cellIndex]; + if ( material == B3_HEIGHT_FIELD_HOLE ) + { + continue; + } + + // index = row * columnCount + column + int index11 = row * shape->columnCount + column; + int index12 = index11 + 1; + int index21 = ( row + 1 ) * shape->columnCount + column; + int index22 = index21 + 1; + + float height11 = minHeight + heightScale * heights[index11]; + float height12 = minHeight + heightScale * heights[index12]; + float height21 = minHeight + heightScale * heights[index21]; + float height22 = minHeight + heightScale * heights[index22]; + + float x1 = (float)( column ); + float x2 = (float)( column + 1 ); + float z1 = (float)( row ); + float z2 = (float)( row + 1 ); + + b3Vec3 point11 = b3Mul( scale, (b3Vec3){ x1, height11, z1 } ); + b3Vec3 point12 = b3Mul( scale, (b3Vec3){ x2, height12, z1 } ); + b3Vec3 point21 = b3Mul( scale, (b3Vec3){ x1, height21, z2 } ); + b3Vec3 point22 = b3Mul( scale, (b3Vec3){ x2, height22, z2 } ); + + b3V32 v11 = b3LoadV( &point11.x ); + b3V32 v12 = b3LoadV( &point12.x ); + b3V32 v21 = b3LoadV( &point21.x ); + b3V32 v22 = b3LoadV( &point22.x ); + + if ( b3TestBoundsTriangleOverlap( boundsCenter, boundsExtent, v11, v21, v12 ) ) + { + b3Vec3 triangleVertices[] = { point11, point21, point12 }; + input.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + + // reset the cache + cache.count = 0; + + // get distance between triangle and query shape + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + + float tolerance = 0.1f * B3_LINEAR_SLOP; + if ( output.distance < tolerance ) + { + // overlap detected + return true; + } + } + + if ( b3TestBoundsTriangleOverlap( boundsCenter, boundsExtent, v21, v22, v12 ) ) + { + b3Vec3 triangleVertices[] = { point22, point12, point21 }; + input.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + + // reset the cache + cache.count = 0; + + // get distance between triangle and query shape + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + + float tolerance = 0.1f * B3_LINEAR_SLOP; + if ( output.distance < tolerance ) + { + // overlap detected + return true; + } + } + } + } + + return false; +} + +void b3QueryHeightField( const b3HeightField* heightField, b3AABB bounds, b3MeshQueryFcn* fcn, void* context ) +{ + b3Vec3 scale = heightField->scale; + + int minRow = (int)floorf( bounds.lowerBound.z / scale.z ); + int maxRow = (int)floorf( bounds.upperBound.z / scale.z ); + int minCol = (int)floorf( bounds.lowerBound.x / scale.x ); + int maxCol = (int)floorf( bounds.upperBound.x / scale.x ); + + float minHeight = heightField->minHeight; + float heightScale = heightField->heightScale; + + const uint16_t* heights = heightField->compressedHeights; + + // Outer loop on rows and inner loop on columns so that triangle indices + // increase monotonically. + for ( int row = minRow; row <= maxRow; ++row ) + { + if ( row < 0 || heightField->rowCount - 1 <= row ) + { + continue; + } + + for ( int column = minCol; column <= maxCol; ++column ) + { + if ( column < 0 || heightField->columnCount - 1 <= column ) + { + continue; + } + + int cellIndex = row * ( heightField->columnCount - 1 ) + column; + B3_ASSERT( cellIndex < ( heightField->rowCount - 1 ) * ( heightField->columnCount - 1 ) ); + uint8_t material = heightField->materialIndices[cellIndex]; + if ( material == B3_HEIGHT_FIELD_HOLE ) + { + continue; + } + + // index = row * columnCount + column + int index11 = row * heightField->columnCount + column; + int index12 = index11 + 1; + int index21 = ( row + 1 ) * heightField->columnCount + column; + int index22 = index21 + 1; + + float height11 = minHeight + heightScale * heights[index11]; + float height12 = minHeight + heightScale * heights[index12]; + float height21 = minHeight + heightScale * heights[index21]; + float height22 = minHeight + heightScale * heights[index22]; + + float x1 = (float)( column ); + float x2 = (float)( column + 1 ); + float z1 = (float)( row ); + float z2 = (float)( row + 1 ); + + b3Vec3 point11 = b3Mul( scale, (b3Vec3){ x1, height11, z1 } ); + b3Vec3 point12 = b3Mul( scale, (b3Vec3){ x2, height12, z1 } ); + b3Vec3 point21 = b3Mul( scale, (b3Vec3){ x1, height21, z2 } ); + b3Vec3 point22 = b3Mul( scale, (b3Vec3){ x2, height22, z2 } ); + + // I know the min/max x and z values, but not the min/max heights. + // This could be done with no branching in SIMD. + b3AABB cellBound; + cellBound.lowerBound = b3Min( b3Min( point11, point12 ), b3Min( point21, point22 ) ); + cellBound.upperBound = b3Max( b3Max( point11, point12 ), b3Max( point21, point22 ) ); + + if ( b3AABB_Overlaps( bounds, cellBound ) ) + { + int quadIndex = row * ( heightField->columnCount - 1 ) + column; + int triangleIndex = 2 * quadIndex; + + if ( heightField->clockwise ) + { + fcn( point11, point12, point21, triangleIndex, context ); + fcn( point22, point21, point12, triangleIndex + 1, context ); + } + else + { + fcn( point11, point21, point12, triangleIndex, context ); + fcn( point22, point12, point21, triangleIndex + 1, context ); + } + } + } + } +} + +int b3CollideMoverAndHeightField( b3PlaneResult* planes, int capacity, const b3HeightField* shape, const b3Capsule* mover ) +{ + b3DistanceInput distanceInput = { 0 }; + distanceInput.proxyB = (b3ShapeProxy){ &mover->center1, 2, 0.0f }; + distanceInput.transformA = b3Transform_identity; + distanceInput.transformB = b3Transform_identity; + distanceInput.useRadii = false; + + b3SimplexCache cache = { 0 }; + + float radius = mover->radius; + b3V32 center1 = b3LoadV( &mover->center1.x ); + b3V32 center2 = b3LoadV( &mover->center2.x ); + b3V32 r = b3SplatV( radius ); + b3V32 boundsMin = b3SubV( b3MinV( center1, center2 ), r ); + b3V32 boundsMax = b3AddV( b3MaxV( center1, center2 ), r ); + b3V32 boundsCenter = b3MulV( b3_halfV, b3AddV( boundsMin, boundsMax ) ); + b3V32 boundsExtent = b3SubV( boundsMax, boundsCenter ); + + float localMinX = b3GetXV( boundsMin ); + float localMinZ = b3GetZV( boundsMin ); + float localMaxX = b3GetXV( boundsMax ); + float localMaxZ = b3GetZV( boundsMax ); + + b3Vec3 scale = shape->scale; + int minRow = (int)floorf( localMinZ / scale.z ); + int maxRow = (int)floorf( localMaxZ / scale.z ); + int minCol = (int)floorf( localMinX / scale.x ); + int maxCol = (int)floorf( localMaxX / scale.x ); + + float minHeight = shape->minHeight; + float heightScale = shape->heightScale; + const uint16_t* heights = shape->compressedHeights; + int planeCount = 0; + + // Outer loop on rows and inner loop on columns so that triangle indices + // increase monotonically. + for ( int row = minRow; row <= maxRow; ++row ) + { + if ( row < 0 || shape->rowCount - 1 <= row ) + { + continue; + } + + for ( int column = minCol; column <= maxCol; ++column ) + { + if ( column < 0 || shape->columnCount - 1 <= column ) + { + continue; + } + + int cellIndex = row * ( shape->columnCount - 1 ) + column; + B3_ASSERT( cellIndex < ( shape->rowCount - 1 ) * ( shape->columnCount - 1 ) ); + uint8_t material = shape->materialIndices[cellIndex]; + if ( material == B3_HEIGHT_FIELD_HOLE ) + { + continue; + } + + // index = row * columnCount + column + int index11 = row * shape->columnCount + column; + int index12 = index11 + 1; + int index21 = ( row + 1 ) * shape->columnCount + column; + int index22 = index21 + 1; + + float height11 = minHeight + heightScale * heights[index11]; + float height12 = minHeight + heightScale * heights[index12]; + float height21 = minHeight + heightScale * heights[index21]; + float height22 = minHeight + heightScale * heights[index22]; + + float x1 = (float)( column ); + float x2 = (float)( column + 1 ); + float z1 = (float)( row ); + float z2 = (float)( row + 1 ); + + b3Vec3 point11 = b3Mul( scale, (b3Vec3){ x1, height11, z1 } ); + b3Vec3 point12 = b3Mul( scale, (b3Vec3){ x2, height12, z1 } ); + b3Vec3 point21 = b3Mul( scale, (b3Vec3){ x1, height21, z2 } ); + b3Vec3 point22 = b3Mul( scale, (b3Vec3){ x2, height22, z2 } ); + + b3V32 v11 = b3LoadV( &point11.x ); + b3V32 v12 = b3LoadV( &point12.x ); + b3V32 v21 = b3LoadV( &point21.x ); + b3V32 v22 = b3LoadV( &point22.x ); + + if ( b3TestBoundsTriangleOverlap( boundsCenter, boundsExtent, v11, v21, v12 ) ) + { + b3Vec3 triangleVertices[] = { point11, point21, point12 }; + distanceInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + + // reset the cache + cache.count = 0; + + // get distance between triangle and mover + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance == 0.0f ) + { + // todo SAT + } + else if ( distanceOutput.distance <= mover->radius ) + { + b3Plane plane = { distanceOutput.normal, mover->radius - distanceOutput.distance }; + planes[planeCount] = (b3PlaneResult){ plane, distanceOutput.pointA }; + planeCount += 1; + + if ( planeCount == capacity ) + { + return planeCount; + } + } + } + + if ( b3TestBoundsTriangleOverlap( boundsCenter, boundsExtent, v21, v22, v12 ) ) + { + b3Vec3 triangleVertices[] = { point22, point12, point21 }; + distanceInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + + // reset the cache + cache.count = 0; + + // get distance between triangle and mover + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance == 0.0f ) + { + // todo SAT + } + else if ( distanceOutput.distance <= mover->radius ) + { + b3Plane plane = { distanceOutput.normal, mover->radius - distanceOutput.distance }; + planes[planeCount] = (b3PlaneResult){ plane, distanceOutput.pointA }; + planeCount += 1; + + if ( planeCount == capacity ) + { + return planeCount; + } + } + } + } + } + + return planeCount; +} + +b3HeightField* b3CreateGrid( int rowCount, int columnCount, b3Vec3 scale, bool makeHoles ) +{ + int heightCount = rowCount * columnCount; + float* heights = (float*)b3Alloc( heightCount * sizeof( float ) ); + + for ( int i = 0; i < rowCount; ++i ) + { + for ( int j = 0; j < columnCount; ++j ) + { + int k = i * columnCount + j; + heights[k] = 0.0f; + } + } + + int cellCount = ( rowCount - 1 ) * ( columnCount - 1 ); + uint8_t* materialIndices = (uint8_t*)b3Alloc( cellCount * sizeof( uint8_t ) ); + + for ( int i = 0; i < rowCount - 1; ++i ) + { + for ( int j = 0; j < columnCount - 1; ++j ) + { + int k = i * ( columnCount - 1 ) + j; + + if ( makeHoles && k > 0 && k % 16 == 0 ) + { + materialIndices[k] = B3_HEIGHT_FIELD_HOLE; + } + else + { + materialIndices[k] = 0; + } + } + } + + b3HeightFieldDef data = { 0 }; + data.heights = heights; + data.materialIndices = materialIndices; + data.scale = scale; + data.countX = columnCount; + data.countZ = rowCount; + data.globalMinimumHeight = -256.0f; + data.globalMaximumHeight = 256.0f; + data.clockwiseWinding = false; + + b3HeightField* heightField = b3CreateHeightField( &data ); + + b3Free( heights, heightCount * sizeof( float ) ); + b3Free( materialIndices, cellCount * sizeof( uint8_t ) ); + + return heightField; +} + +b3HeightField* b3CreateWave( int rowCount, int columnCount, b3Vec3 scale, float rowFrequency, float columnFrequency, + bool makeHoles ) +{ + int heightCount = rowCount * columnCount; + float* heights = (float*)b3Alloc( heightCount * sizeof( float ) ); + + float omegaZ = 2.0f * B3_PI * rowFrequency; + float omegaX = 2.0f * B3_PI * columnFrequency; + + for ( int i = 0; i < rowCount; ++i ) + { + float rowHeight = sinf( omegaZ * i ); + + for ( int j = 0; j < columnCount; ++j ) + { + int k = i * columnCount + j; + float columnHeight = sinf( omegaX * j ); + heights[k] = rowHeight * columnHeight; + } + } + + int cellCount = ( rowCount - 1 ) * ( columnCount - 1 ); + uint8_t* materialIndices = (uint8_t*)b3Alloc( cellCount * sizeof( uint8_t ) ); + + for ( int i = 0; i < rowCount - 1; ++i ) + { + for ( int j = 0; j < columnCount - 1; ++j ) + { + int k = i * ( columnCount - 1 ) + j; + + if ( makeHoles && k > 0 && k % 16 == 0 ) + { + materialIndices[k] = B3_HEIGHT_FIELD_HOLE; + } + else + { + int materialCount = 3; + materialIndices[k] = (uint8_t)( ( i + j ) % materialCount ); + } + } + } + + b3HeightFieldDef data = { 0 }; + data.heights = heights; + data.materialIndices = materialIndices; + data.scale = scale; + data.countX = columnCount; + data.countZ = rowCount; + data.globalMinimumHeight = -256.0f; + data.globalMaximumHeight = 256.0f; + data.clockwiseWinding = false; + + b3HeightField* heightField = b3CreateHeightField( &data ); + + b3Free( heights, heightCount * sizeof( float ) ); + b3Free( materialIndices, cellCount * sizeof( uint8_t ) ); + + return heightField; +} + +void b3DestroyHeightField( b3HeightField* heightField ) +{ + int heightCount = heightField->rowCount * heightField->columnCount; + b3Free( heightField->compressedHeights, heightCount * sizeof( uint16_t ) ); + + int cellCount = ( heightField->rowCount - 1 ) * ( heightField->columnCount - 1 ); + b3Free( heightField->materialIndices, cellCount * sizeof( uint8_t ) ); + + int triangleCount = 2 * cellCount; + b3Free( heightField->flags, triangleCount * sizeof( uint8_t ) ); + + b3Free( heightField, sizeof( b3HeightField ) ); +} + +void b3DumpHeightData( const b3HeightFieldDef* data, const char* fileName ) +{ + FILE* file = NULL; + +#if defined( _MSC_VER ) + errno_t e = fopen_s( &file, fileName, "w" ); + if ( e != 0 ) + { + return; + } +#else + file = fopen( fileName, "w" ); + if ( file == NULL ) + { + return; + } +#endif + + fprintf( file, "%d %d\n", data->countX, data->countZ ); + fprintf( file, "%.9f %.9f %.9f\n", data->scale.x, data->scale.y, data->scale.z ); + fprintf( file, "%.9f %.9f\n", data->globalMinimumHeight, data->globalMaximumHeight ); + fprintf( file, "%d\n", data->clockwiseWinding ); + + int heightCount = data->countX * data->countZ; + for ( int i = 0; i < heightCount; ++i ) + { + fprintf( file, "%.9f\n", data->heights[i] ); + } + + int materialCount = ( data->countX - 1 ) * ( data->countZ - 1 ); + for ( int i = 0; i < materialCount; ++i ) + { + fprintf( file, "%d\n", data->materialIndices[i] ); + } + + fclose( file ); +} + +#if defined( _MSC_VER ) +#define B3_FILE_SCAN fscanf_s +#else +#define B3_FILE_SCAN fscanf +#endif + +b3HeightField* b3LoadHeightField( const char* fileName ) +{ + FILE* file = NULL; + +#if defined( _MSC_VER ) + errno_t e = fopen_s( &file, fileName, "r" ); + if ( e != 0 ) + { + return NULL; + } +#else + file = fopen( fileName, "r" ); + if ( file == NULL ) + { + return NULL; + } +#endif + + b3HeightFieldDef data = { 0 }; + + // Read dimensions + if ( B3_FILE_SCAN( file, "%d %d", &data.countX, &data.countZ ) != 2 ) + { + fclose( file ); + return NULL; + } + + // Read scale + if ( B3_FILE_SCAN( file, "%f %f %f", &data.scale.x, &data.scale.y, &data.scale.z ) != 3 ) + { + fclose( file ); + return NULL; + } + + // Read global height bounds + if ( B3_FILE_SCAN( file, "%f %f", &data.globalMinimumHeight, &data.globalMaximumHeight ) != 2 ) + { + fclose( file ); + return NULL; + } + + // Read clockwise winding + int clockwise; + if ( B3_FILE_SCAN( file, "%d", &clockwise ) != 1 ) + { + fclose( file ); + return NULL; + } + data.clockwiseWinding = clockwise != 0; + + // Allocate and read height data + int heightCount = data.countX * data.countZ; + data.heights = (float*)b3Alloc( heightCount * sizeof( float ) ); + + for ( int i = 0; i < heightCount; ++i ) + { + if ( B3_FILE_SCAN( file, "%f", &data.heights[i] ) != 1 ) + { + b3Free( data.heights, heightCount * sizeof( float ) ); + fclose( file ); + return NULL; + } + } + + // Allocate and read material indices + int materialCount = ( data.countX - 1 ) * ( data.countZ - 1 ); + data.materialIndices = (uint8_t*)b3Alloc( materialCount * sizeof( uint8_t ) ); + + for ( int i = 0; i < materialCount; ++i ) + { + int materialIndex; + if ( B3_FILE_SCAN( file, "%d", &materialIndex ) != 1 ) + { + b3Free( data.heights, heightCount * sizeof( float ) ); + b3Free( data.materialIndices, materialCount * sizeof( uint8_t ) ); + fclose( file ); + return NULL; + } + data.materialIndices[i] = (uint8_t)materialIndex; + } + + fclose( file ); + + // Create height field from loaded data + b3HeightField* heightField = b3CreateHeightField( &data ); + + // Clean up temporary allocations + b3Free( data.heights, heightCount * sizeof( float ) ); + b3Free( data.materialIndices, materialCount * sizeof( uint8_t ) ); + + return heightField; +} diff --git a/src/hull.c b/src/hull.c new file mode 100644 index 0000000..47f0d39 --- /dev/null +++ b/src/hull.c @@ -0,0 +1,2910 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +// Dirk Gregorius contributed portions of this code + +#include "algorithm.h" +#include "math_internal.h" +#include "shape.h" + +#include "box3d/collision.h" +#include "box3d/constants.h" +#include "box3d/math_functions.h" + +#include +#include +#include +#include +#include +#include + +#define B3_AXIS_X 0 +#define B3_AXIS_Y 1 +#define B3_AXIS_Z 2 + +#define B3_MARK_VISIBLE 0 +#define B3_MARK_DELETE 1 + +// Final hull is index-encoded with uint8_t, so vertex/edge/face counts are capped at UINT8_MAX. +#define B3_HULL_LIMIT UINT8_MAX + +typedef struct b3QHListNode +{ + struct b3QHListNode* prev; + struct b3QHListNode* next; +} b3QHListNode; + +typedef struct b3QHFace b3QHFace; + +typedef struct b3QHVertex +{ + // Intrusive list link. Must be first so (b3QHVertex*)nodePtr is valid. + b3QHListNode link; + + b3QHFace* conflictFace; + b3Vec3 position; + + // Index in the finalized hull, stamped during emit. B3_NULL_INDEX until then. + int finalIndex; + bool reachable; +} b3QHVertex; + +typedef struct b3QHHalfEdge +{ + // Edge ring (CCW) around the owning face. Not an external list. + struct b3QHHalfEdge* prev; + struct b3QHHalfEdge* next; + + b3QHVertex* origin; + b3QHFace* face; + struct b3QHHalfEdge* twin; + + // Index in the finalized hull, stamped during emit. B3_NULL_INDEX until then. + int finalIndex; +} b3QHHalfEdge; + +struct b3QHFace +{ + // Intrusive list link. Must be first so (b3QHFace*)nodePtr is valid. + b3QHListNode link; + + b3QHHalfEdge* edge; + + int mark; + float area; + b3Plane plane; + b3Vec3 centroid; + float maxConflictDistance; + + // Sentinel head for this face's conflict list of b3QHVertex. + b3QHVertex conflictListHead; + + // Cached farthest conflict vertex (above b3HullBuilder::minOutside). + // NULL when no conflict above threshold; maxConflictDistance is then minOutside. + b3QHVertex* maxConflict; + + // Index in the finalized hull, stamped during emit. B3_NULL_INDEX until then. + int finalIndex; + bool flipped; +}; + +// One frame of the iterative horizon DFS. Replaces a recursive call to b3HullBuilder_BuildHorizon. +typedef struct b3HorizonFrame +{ + b3QHFace* face; + b3QHHalfEdge* startEdge; // ring termination sentinel + b3QHHalfEdge* edge; // next edge to process + bool started; // false until the first edge of this ring has been processed +} b3HorizonFrame; + +// All working memory for one hull build, carved from a single b3Alloc block. +typedef struct b3HullBuilder +{ + float tolerance; + float minRadius; + float minOutside; + + b3Vec3 interiorPoint; + + // List sentinels. Only the link is meaningful; other fields are unused. + b3QHVertex orphanedList; + b3QHVertex vertexList; + b3QHFace faceList; + + // Bump-allocated pools with pointer-based free lists for faces and edges. + b3QHVertex* vertexBase; + int vertexCapacity; + int vertexCount; + + b3QHHalfEdge* edgeBase; + int edgeCapacity; + int edgeCount; + b3QHHalfEdge* edgeFreeHead; // LIFO free list; overlays edge->next + + b3QHFace* faceBase; + int faceCapacity; + int faceCount; + b3QHFace* faceFreeHead; // LIFO free list; overlays face->link.next + + // Reusable scratch buffers. + b3QHHalfEdge** horizon; + int horizonCapacity; + int horizonCount; + + b3QHFace** cone; + int coneCapacity; + int coneCount; + + b3QHFace** mergedFaces; + int mergedFacesCapacity; + int mergedFacesCount; + + // DFS stack used by the iterative b3HullBuilder_BuildHorizon. Depth bounded by live faces. + b3HorizonFrame* horizonStack; + int horizonStackCapacity; + + // Final counts of the constructed hull (vertexList / faceList / half-edges around faces). + // Populated by CleanHull; zero until then. + int finalVertexCount; + int finalHalfEdgeCount; + int finalFaceCount; +} b3HullBuilder; + +static inline void b3QHList_Init( b3QHListNode* head ) +{ + head->prev = head; + head->next = head; +} + +static inline bool b3QHList_Empty( const b3QHListNode* head ) +{ + return head->next == head; +} + +static inline bool b3QHList_Contains( const b3QHListNode* node ) +{ + return node->prev != NULL && node->next != NULL; +} + +// Insert node before `where`. +static inline void b3QHList_Insert( b3QHListNode* node, b3QHListNode* where ) +{ + B3_ASSERT( !b3QHList_Contains( node ) && b3QHList_Contains( where ) ); + + node->prev = where->prev; + node->next = where; + + node->prev->next = node; + node->next->prev = node; +} + +static inline void b3QHList_Remove( b3QHListNode* node ) +{ + B3_ASSERT( b3QHList_Contains( node ) ); + + node->prev->next = node->next; + node->next->prev = node->prev; + + node->prev = NULL; + node->next = NULL; +} + +static inline void b3QHList_PushBack( b3QHListNode* head, b3QHListNode* node ) +{ + b3QHList_Insert( node, head->prev ); +} + +static b3QHVertex* b3HullBuilder_NewVertex( b3HullBuilder* b, b3Vec3 position) +{ + B3_ASSERT( b->vertexCount < b->vertexCapacity ); + b3QHVertex* vertex = b->vertexBase + b->vertexCount++; + + vertex->link.prev = NULL; + vertex->link.next = NULL; + vertex->conflictFace = NULL; + vertex->position = position; + vertex->finalIndex = B3_NULL_INDEX; + vertex->reachable = false; + + return vertex; +} + +static b3QHHalfEdge* b3HullBuilder_NewEdge( b3HullBuilder* b ) +{ + b3QHHalfEdge* edge; + if ( b->edgeFreeHead != NULL ) + { + edge = b->edgeFreeHead; + b->edgeFreeHead = edge->next; + } + else + { + B3_ASSERT( b->edgeCount < b->edgeCapacity ); + edge = b->edgeBase + b->edgeCount++; + } + // All other fields (prev/next/origin/face/twin) are written by NewFace immediately after. + edge->finalIndex = B3_NULL_INDEX; + return edge; +} + +static void b3HullBuilder_RetireEdge( b3HullBuilder* b, b3QHHalfEdge* edge ) +{ + edge->next = b->edgeFreeHead; + b->edgeFreeHead = edge; +} + +static b3QHFace* b3HullBuilder_NewFace( b3HullBuilder* b, b3QHVertex* v1, b3QHVertex* v2, b3QHVertex* v3 ) +{ + b3QHFace* face; + if ( b->faceFreeHead != NULL ) + { + face = b->faceFreeHead; + // link.next was used as free-list pointer; recover next head before we clobber. + b->faceFreeHead = (b3QHFace*)face->link.next; + } + else + { + B3_ASSERT( b->faceCount < b->faceCapacity ); + face = b->faceBase + b->faceCount++; + } + + // link.prev: NULL on retired faces (cleared by Remove); NULL here so PushBack's + // !b3QHList_Contains assert holds for fresh bump slots too. + // link.next: was the free-list pointer on reused slots, now stale; PushBack overwrites. + face->link.prev = NULL; + face->link.next = NULL; + face->maxConflict = NULL; + face->maxConflictDistance = 0.0f; + face->finalIndex = B3_NULL_INDEX; + + b3QHHalfEdge* edge1 = b3HullBuilder_NewEdge( b ); + b3QHHalfEdge* edge2 = b3HullBuilder_NewEdge( b ); + b3QHHalfEdge* edge3 = b3HullBuilder_NewEdge( b ); + + b3Vec3 p1 = v1->position; + b3Vec3 p2 = v2->position; + b3Vec3 p3 = v3->position; + + b3Plane plane; + plane.normal = b3Cross( b3Sub( p2, p1 ), b3Sub( p3, p1 ) ); + float length; + plane.normal = b3GetLengthAndNormalize( &length, plane.normal ); + plane.offset = b3Dot( plane.normal, p1 ); + + float area = 0.5f * length; + + face->edge = edge1; + face->mark = B3_MARK_VISIBLE; + face->area = area; + face->centroid = b3MulSV( 1.0f / 3.0f, b3Add( v1->position, b3Add( v2->position, v3->position ) ) ); + face->plane = plane; + face->flipped = b3PlaneSeparation( plane, b->interiorPoint ) > 0.0f; + b3QHList_Init( &face->conflictListHead.link ); + + edge1->prev = edge3; + edge1->next = edge2; + edge1->origin = v1; + edge1->face = face; + edge1->twin = NULL; + + edge2->prev = edge1; + edge2->next = edge3; + edge2->origin = v2; + edge2->face = face; + edge2->twin = NULL; + + edge3->prev = edge2; + edge3->next = edge1; + edge3->origin = v3; + edge3->face = face; + edge3->twin = NULL; + + return face; +} + +// Remove face from faceList if still linked, clear its edge pointer, then push onto faceFreeHead. +// Uses face->link.next as the free-list next pointer (link.prev stays NULL, so b3QHList_Contains +// returns false on a free slot, as required by the retire-guard in ResolveFaces). +static void b3HullBuilder_RetireFace( b3HullBuilder* b, b3QHFace* face ) +{ + if ( b3QHList_Contains( &face->link ) ) + { + b3QHList_Remove( &face->link ); + } + face->edge = NULL; + // link.prev is already NULL after Remove (or was never set). link.next holds free-list ptr. + face->link.next = (b3QHListNode*)b->faceFreeHead; + b->faceFreeHead = face; +} + +static b3AABB b3BuildBounds( int vertexCount, const b3Vec3* vertices ) +{ + b3AABB bounds = B3_BOUNDS3_EMPTY; + for ( int i = 0; i < vertexCount; ++i ) + { + bounds.lowerBound = b3Min( bounds.lowerBound, vertices[i] ); + bounds.upperBound = b3Max( bounds.upperBound, vertices[i] ); + } + return bounds; +} + +static void b3FindFarthestPointsAlongCardinalAxes( int* index1Out, int* index2Out, float tolerance, int vertexCount, + const b3Vec3* vertexBase ) +{ + *index1Out = B3_NULL_INDEX; + *index2Out = B3_NULL_INDEX; + + b3Vec3 v0 = vertexBase[0]; + b3Vec3 minPt[3] = { v0, v0, v0 }; + b3Vec3 maxPt[3] = { v0, v0, v0 }; + + int minIndex[3] = { 0, 0, 0 }; + int maxIndex[3] = { 0, 0, 0 }; + + for ( int i = 1; i < vertexCount; ++i ) + { + b3Vec3 v = vertexBase[i]; + + if ( v.x < minPt[B3_AXIS_X].x ) + { + minPt[B3_AXIS_X] = v; + minIndex[B3_AXIS_X] = i; + } + else if ( v.x > maxPt[B3_AXIS_X].x ) + { + maxPt[B3_AXIS_X] = v; + maxIndex[B3_AXIS_X] = i; + } + + if ( v.y < minPt[B3_AXIS_Y].y ) + { + minPt[B3_AXIS_Y] = v; + minIndex[B3_AXIS_Y] = i; + } + else if ( v.y > maxPt[B3_AXIS_Y].y ) + { + maxPt[B3_AXIS_Y] = v; + maxIndex[B3_AXIS_Y] = i; + } + + if ( v.z < minPt[B3_AXIS_Z].z ) + { + minPt[B3_AXIS_Z] = v; + minIndex[B3_AXIS_Z] = i; + } + else if ( v.z > maxPt[B3_AXIS_Z].z ) + { + maxPt[B3_AXIS_Z] = v; + maxIndex[B3_AXIS_Z] = i; + } + } + + b3Vec3 distance; + distance.x = maxPt[B3_AXIS_X].x - minPt[B3_AXIS_X].x; + distance.y = maxPt[B3_AXIS_Y].y - minPt[B3_AXIS_Y].y; + distance.z = maxPt[B3_AXIS_Z].z - minPt[B3_AXIS_Z].z; + + float distanceArray[3] = { distance.x, distance.y, distance.z }; + int maxElement = b3MaxElementIndex( distance ); + + if ( distanceArray[maxElement] > 2.0f * tolerance ) + { + *index1Out = minIndex[maxElement]; + *index2Out = maxIndex[maxElement]; + } +} + +static int b3FindFarthestPointFromLine( int index1, int index2, float tolerance, int vertexCount, const b3Vec3* vertexBase ) +{ + b3Vec3 a = vertexBase[index1]; + b3Vec3 b = vertexBase[index2]; + + // |ap x ab|^2 / |ab|^2 is the squared perpendicular distance from p to the line. + // Compares against (2 * tolerance)^2 + b3Vec3 ab = b3Sub( b, a ); + float abLengthSqr = b3Dot( ab, ab ); + B3_ASSERT( abLengthSqr > 0.0f ); + + float invAbLengthSqr = 1.0f / abLengthSqr; + float maxDistanceSqr = 4.0f * tolerance * tolerance; + int maxIndex = B3_NULL_INDEX; + + for ( int i = 0; i < vertexCount; ++i ) + { + if ( i == index1 || i == index2 ) + { + continue; + } + + b3Vec3 ap = b3Sub( vertexBase[i], a ); + b3Vec3 cross = b3Cross( ap, ab ); + float distanceSqr = b3Dot( cross, cross ) * invAbLengthSqr; + if ( distanceSqr > maxDistanceSqr ) + { + maxDistanceSqr = distanceSqr; + maxIndex = i; + } + } + + return maxIndex; +} + +static int b3FindFarthestPointFromPlane( int index1, int index2, int index3, float tolerance, int vertexCount, + const b3Vec3* vertexBase ) +{ + b3Vec3 a = vertexBase[index1]; + b3Vec3 b = vertexBase[index2]; + b3Vec3 c = vertexBase[index3]; + + b3Plane plane = b3MakePlaneFromPoints( a, b, c ); + + float maxDistance = 2.0f * tolerance; + int maxIndex = B3_NULL_INDEX; + + for ( int i = 0; i < vertexCount; ++i ) + { + if ( i == index1 || i == index2 || i == index3 ) + { + continue; + } + + float distance = b3AbsFloat( b3PlaneSeparation( plane, vertexBase[i] ) ); + if ( distance > maxDistance ) + { + maxDistance = distance; + maxIndex = i; + } + } + + return maxIndex; +} + +static bool b3IsEdgeConvex( const b3QHHalfEdge* edge, float tolerance ) +{ + float distance = b3PlaneSeparation( edge->face->plane, edge->twin->face->centroid ); + return distance < -tolerance; +} + +static bool b3IsEdgeConcave( const b3QHHalfEdge* edge, float tolerance ) +{ + float distance = b3PlaneSeparation( edge->face->plane, edge->twin->face->centroid ); + return distance > tolerance; +} + +static int b3VertexCountOfFace( const b3QHFace* face ) +{ + int count = 0; + const b3QHHalfEdge* edge = face->edge; + do + { + count++; + edge = edge->next; + } + while ( edge != face->edge ); + + return count; +} + +static void b3LinkFace( b3QHFace* face, int index, b3QHHalfEdge* twin ) +{ + B3_ASSERT( face != twin->face ); + + b3QHHalfEdge* edge = face->edge; + while ( index-- > 0 ) + { + B3_ASSERT( edge->face == face ); + edge = edge->next; + } + + B3_ASSERT( edge != twin ); + edge->twin = twin; + twin->twin = edge; +} + +static void b3LinkFaces( b3QHFace* face1, int index1, b3QHFace* face2, int index2 ) +{ + B3_ASSERT( face1 != face2 ); + + b3QHHalfEdge* edge1 = face1->edge; + while ( index1-- > 0 ) + { + edge1 = edge1->next; + } + + b3QHHalfEdge* edge2 = face2->edge; + while ( index2-- > 0 ) + { + edge2 = edge2->next; + } + + B3_ASSERT( edge1 != edge2 ); + edge1->twin = edge2; + edge2->twin = edge1; +} + +static void b3NewellPlane( b3QHFace* face ) +{ + int count = 0; + b3Vec3 centroid = b3Vec3_zero; + b3Vec3 normal = b3Vec3_zero; + + b3QHHalfEdge* edge = face->edge; + B3_ASSERT( edge->face == face ); + + // Use the first vertex as the origin to reduce round-off + b3Vec3 origin = edge->origin->position; + + do + { + b3QHHalfEdge* twin = edge->twin; + B3_ASSERT( twin->twin == edge ); + + b3Vec3 v1 = b3Sub( edge->origin->position, origin ); + b3Vec3 v2 = b3Sub( twin->origin->position, origin ); + + count++; + centroid = b3Add( centroid, v1 ); + normal.x += ( v1.y - v2.y ) * ( v1.z + v2.z ); + normal.y += ( v1.z - v2.z ) * ( v1.x + v2.x ); + normal.z += ( v1.x - v2.x ) * ( v1.y + v2.y ); + + edge = edge->next; + } + while ( edge != face->edge ); + + B3_ASSERT( count > 0 ); + centroid = b3MulSV( 1.0f / (float)count, centroid ); + centroid = b3Add( centroid, origin ); + + float length = b3Length( normal ); + B3_VALIDATE( length > 0.0f ); + normal = b3MulSV( 1.0f / length, normal ); + + face->centroid = centroid; + face->plane = b3MakePlaneFromNormalAndPoint( normal, centroid ); + face->area = 0.5f * length; +} + +#if B3_DEBUG +static bool b3CheckConsistency( const b3QHFace* face ) +{ + if ( face->mark == B3_MARK_DELETE ) + { + return false; + } + + if ( b3VertexCountOfFace( face ) < 3 ) + { + return false; + } + + const b3QHHalfEdge* edge = face->edge; + + do + { + const b3QHHalfEdge* twin = edge->twin; + + if ( twin == NULL ) + { + return false; + } + if ( twin->face == NULL ) + { + return false; + } + if ( twin->face == face ) + { + return false; + } + if ( twin->face->mark == B3_MARK_DELETE ) + { + return false; + } + if ( twin->twin != edge ) + { + return false; + } + if ( edge->next->origin != twin->origin ) + { + return false; + } + if ( edge->origin != twin->next->origin ) + { + return false; + } + if ( edge->face != face ) + { + return false; + } + + edge = edge->next; + } + while ( edge != face->edge ); + + return true; +} +#endif + +static void b3HullBuilder_ComputeTolerance( b3HullBuilder* b, int pointCount, const b3Vec3* points ) +{ + b3AABB bounds = b3BuildBounds( pointCount, points ); + b3Vec3 maxAbs = b3Max( b3Abs( bounds.lowerBound ), b3Abs( bounds.upperBound ) ); + + float maxSum = maxAbs.x + maxAbs.y + maxAbs.z; + float maxCoord = b3MaxFloat( maxAbs.x, b3MaxFloat( maxAbs.y, maxAbs.z ) ); + float maxDistance = b3MinFloat( B3_SQRT3 * maxCoord, maxSum ); + + float tolerance = ( 3.0f * maxDistance * 1.01f + maxCoord ) * FLT_EPSILON; + + b->tolerance = tolerance; + b->minRadius = 4.0f * b->tolerance; + b->minOutside = 2.0f * b->minRadius; + B3_ASSERT( b->minRadius < b->minOutside + 3.0f * FLT_EPSILON ); +} + +static bool b3HullBuilder_BuildInitialHull( b3HullBuilder* b, int pointCount, const b3Vec3* points ) +{ + int index1, index2; + b3FindFarthestPointsAlongCardinalAxes( &index1, &index2, b->tolerance, pointCount, points ); + if ( index1 < 0 || index2 < 0 ) + { + return false; + } + + int index3 = b3FindFarthestPointFromLine( index1, index2, b->tolerance, pointCount, points ); + if ( index3 < 0 ) + { + return false; + } + + int index4 = b3FindFarthestPointFromPlane( index1, index2, index3, b->tolerance, pointCount, points ); + if ( index4 < 0 ) + { + return false; + } + + b3Vec3 v1 = b3Sub( points[index1], points[index4] ); + b3Vec3 v2 = b3Sub( points[index2], points[index4] ); + b3Vec3 v3 = b3Sub( points[index3], points[index4] ); + + if ( b3ScalarTripleProduct( v1, v2, v3 ) < 0.0f ) + { + int temp = index2; + index2 = index3; + index3 = temp; + } + + b->interiorPoint = b3Vec3_zero; + b->interiorPoint = b3Add( b->interiorPoint, points[index1] ); + b->interiorPoint = b3Add( b->interiorPoint, points[index2] ); + b->interiorPoint = b3Add( b->interiorPoint, points[index3] ); + b->interiorPoint = b3Add( b->interiorPoint, points[index4] ); + b->interiorPoint = b3MulSV( 0.25f, b->interiorPoint ); + + b3QHVertex* vertex1 = b3HullBuilder_NewVertex( b, points[index1] ); + b3QHList_PushBack( &b->vertexList.link, &vertex1->link ); + b3QHVertex* vertex2 = b3HullBuilder_NewVertex( b, points[index2] ); + b3QHList_PushBack( &b->vertexList.link, &vertex2->link ); + b3QHVertex* vertex3 = b3HullBuilder_NewVertex( b, points[index3] ); + b3QHList_PushBack( &b->vertexList.link, &vertex3->link ); + b3QHVertex* vertex4 = b3HullBuilder_NewVertex( b, points[index4] ); + b3QHList_PushBack( &b->vertexList.link, &vertex4->link ); + + b3QHFace* face1 = b3HullBuilder_NewFace( b, vertex1, vertex2, vertex3 ); + b3QHList_PushBack( &b->faceList.link, &face1->link ); + b3QHFace* face2 = b3HullBuilder_NewFace( b, vertex4, vertex2, vertex1 ); + b3QHList_PushBack( &b->faceList.link, &face2->link ); + b3QHFace* face3 = b3HullBuilder_NewFace( b, vertex4, vertex3, vertex2 ); + b3QHList_PushBack( &b->faceList.link, &face3->link ); + b3QHFace* face4 = b3HullBuilder_NewFace( b, vertex4, vertex1, vertex3 ); + b3QHList_PushBack( &b->faceList.link, &face4->link ); + + b3LinkFaces( face1, 0, face2, 1 ); + b3LinkFaces( face1, 1, face3, 1 ); + b3LinkFaces( face1, 2, face4, 1 ); + + b3LinkFaces( face2, 0, face3, 2 ); + b3LinkFaces( face3, 0, face4, 2 ); + b3LinkFaces( face4, 0, face2, 2 ); + +#if B3_DEBUG + B3_ASSERT( b3CheckConsistency( face1 ) ); + B3_ASSERT( b3CheckConsistency( face2 ) ); + B3_ASSERT( b3CheckConsistency( face3 ) ); + B3_ASSERT( b3CheckConsistency( face4 ) ); +#endif + + for ( int index = 0; index < pointCount; ++index ) + { + if ( index == index1 || index == index2 || index == index3 || index == index4 ) + { + continue; + } + + b3Vec3 point = points[index]; + + float maxDistance = b->minOutside; + b3QHFace* maxFace = NULL; + + for ( b3QHListNode* node = b->faceList.link.next; node != &b->faceList.link; node = node->next ) + { + b3QHFace* face = (b3QHFace*)node; + float distance = b3PlaneSeparation( face->plane, point ); + if ( distance > maxDistance ) + { + maxDistance = distance; + maxFace = face; + } + } + + if ( maxFace != NULL ) + { + b3QHVertex* vertex = b3HullBuilder_NewVertex( b, point ); + vertex->conflictFace = maxFace; + b3QHList_PushBack( &maxFace->conflictListHead.link, &vertex->link ); + if ( maxDistance > maxFace->maxConflictDistance ) + { + maxFace->maxConflictDistance = maxDistance; + maxFace->maxConflict = vertex; + } + } + } + + return true; +} + +// Recompute the farthest-conflict cache after a face's plane changes. +// Walks the existing conflict list once; cost is bounded by that list, not the global pool. +static void b3HullBuilder_RecacheConflicts( b3QHFace* face, float minOutside ) +{ + b3QHVertex* maxVertex = NULL; + float maxDistance = minOutside; + + for ( b3QHListNode* node = face->conflictListHead.link.next; node != &face->conflictListHead.link; node = node->next ) + { + b3QHVertex* vertex = (b3QHVertex*)node; + float distance = b3PlaneSeparation( face->plane, vertex->position ); + if ( distance > maxDistance ) + { + maxDistance = distance; + maxVertex = vertex; + } + } + + face->maxConflict = maxVertex; + face->maxConflictDistance = maxDistance; +} + +static b3QHVertex* b3HullBuilder_NextConflictVertex( const b3HullBuilder* b ) +{ + b3QHVertex* maxVertex = NULL; + float maxDistance = b->minOutside; + + for ( const b3QHListNode* faceNode = b->faceList.link.next; faceNode != &b->faceList.link; faceNode = faceNode->next ) + { + const b3QHFace* face = (const b3QHFace*)faceNode; + if ( face->maxConflict != NULL && face->maxConflictDistance > maxDistance ) + { + maxDistance = face->maxConflictDistance; + maxVertex = face->maxConflict; + } + } + + return maxVertex; +} + +// Move every conflict vertex of `face` onto the orphaned list and clear their conflictFace. +static void b3HullBuilder_DrainConflictList( b3HullBuilder* b, b3QHFace* face ) +{ + b3QHListNode* node = face->conflictListHead.link.next; + while ( node != &face->conflictListHead.link ) + { + b3QHVertex* orphan = (b3QHVertex*)node; + node = node->next; + + orphan->conflictFace = NULL; + b3QHList_Remove( &orphan->link ); + b3QHList_PushBack( &b->orphanedList.link, &orphan->link ); + } + B3_ASSERT( b3QHList_Empty( &face->conflictListHead.link ) ); +} + +// Mark a face for deletion, drain its conflict list, and populate a fresh DFS frame for it. +// `entryEdge` is the half-edge in `face` whose twin lies in the just-deleted parent face, or +// NULL for the seed. The frame skips `entryEdge` on recursive entries (it would be ignored +// anyway since the parent's mark is now DELETE, but skipping saves one iteration). +static void b3HullBuilder_EnterHorizonFace( b3HullBuilder* b, b3QHFace* face, b3QHHalfEdge* entryEdge, + b3HorizonFrame* frameOut ) +{ + face->mark = B3_MARK_DELETE; + b3HullBuilder_DrainConflictList( b, face ); + + frameOut->face = face; + frameOut->started = false; + if ( entryEdge != NULL ) + { + frameOut->startEdge = entryEdge; + frameOut->edge = entryEdge->next; + } + else + { + frameOut->startEdge = face->edge; + frameOut->edge = face->edge; + } +} + +static void b3HullBuilder_BuildHorizon( b3HullBuilder* b, b3QHVertex* apex, b3QHFace* seed ) +{ + b3HorizonFrame* stack = b->horizonStack; + int top = 0; + + B3_ASSERT( top < b->horizonStackCapacity ); + b3HullBuilder_EnterHorizonFace( b, seed, NULL, &stack[top++] ); + + while ( top > 0 ) + { + b3HorizonFrame* f = &stack[top - 1]; + + if ( f->started && f->edge == f->startEdge ) + { + top--; + continue; + } + f->started = true; + + b3QHHalfEdge* edge = f->edge; + b3QHHalfEdge* twin = edge->twin; + f->edge = edge->next; + + if ( twin->face->mark != B3_MARK_VISIBLE ) + { + continue; + } + + float distance = b3PlaneSeparation( twin->face->plane, apex->position ); + if ( distance > b->minRadius ) + { + B3_ASSERT( top < b->horizonStackCapacity ); + b3HullBuilder_EnterHorizonFace( b, twin->face, twin, &stack[top++] ); + } + else + { + B3_ASSERT( b->horizonCount < b->horizonCapacity ); + b->horizon[b->horizonCount++] = edge; + } + } +} + +static void b3HullBuilder_BuildCone( b3HullBuilder* b, b3QHVertex* apex ) +{ + for ( int i = 0; i < b->horizonCount; ++i ) + { + b3QHHalfEdge* edge = b->horizon[i]; + B3_ASSERT( edge->twin->twin == edge ); + + b3QHFace* face = b3HullBuilder_NewFace( b, apex, edge->origin, edge->twin->origin ); + B3_ASSERT( b->coneCount < b->coneCapacity ); + b->cone[b->coneCount++] = face; + + b3LinkFace( face, 1, edge->twin ); + } + + b3QHFace* face1 = b->cone[b->coneCount - 1]; + for ( int i = 0; i < b->coneCount; ++i ) + { + b3QHFace* face2 = b->cone[i]; + b3LinkFaces( face1, 2, face2, 0 ); + face1 = face2; + } +} + +// Retire half-edges in the half-open ring range [begin, end) by pushing each onto edgeFreeHead. +// The caller has already detached these edges from the live face ring (rewire happens before +// destroy), so they are unreachable from the live hull. +static void b3HullBuilder_DestroyEdges( b3HullBuilder* b, b3QHHalfEdge* begin, b3QHHalfEdge* end ) +{ + b3QHHalfEdge* edge = begin; + while ( edge != end ) + { + b3QHHalfEdge* next = edge->next; + b3HullBuilder_RetireEdge( b, edge ); + edge = next; + } +} + +static void b3HullBuilder_ConnectEdges( b3HullBuilder* b, b3QHHalfEdge* prev, b3QHHalfEdge* next ) +{ + B3_ASSERT( prev != next ); + B3_ASSERT( prev->face == next->face ); + + // If both shared neighbors are the same face, prev and next together would orphan that face. + if ( prev->twin->face == next->twin->face ) + { + // next is redundant. + if ( next->face->edge == next ) + { + next->face->edge = prev; + } + + b3QHHalfEdge* twin; + if ( b3VertexCountOfFace( prev->twin->face ) == 3 ) + { + // Capture all 3 half-edges of the dead triangle before the rewire overwrites prev->twin. + b3QHHalfEdge* deadEdge0 = prev->twin; // prev->twin (will be rewired below) + b3QHHalfEdge* deadEdge1 = next->twin; // next->twin + b3QHHalfEdge* deadEdge2 = next->twin->prev; // third edge of the dead triangle + + twin = deadEdge2->twin; + B3_ASSERT( twin->face->mark != B3_MARK_DELETE ); + + b3QHFace* opposingFace = prev->twin->face; + opposingFace->mark = B3_MARK_DELETE; + B3_ASSERT( b->mergedFacesCount < b->mergedFacesCapacity ); + b->mergedFaces[b->mergedFacesCount++] = opposingFace; + + prev->next = next->next; + prev->next->prev = prev; + + prev->twin = twin; + twin->twin = prev; + + // Drop the redundant vertex (slot abandoned in the bump allocator). + b3QHList_Remove( &next->origin->link ); + + // Retire the 3 half-edges of the dead triangle now that the rewire is complete. + b3HullBuilder_RetireEdge( b, deadEdge0 ); + b3HullBuilder_RetireEdge( b, deadEdge1 ); + b3HullBuilder_RetireEdge( b, deadEdge2 ); + } + else + { + twin = next->twin; + + if ( twin->face->edge == prev->twin ) + { + twin->face->edge = twin; + } + + twin->next = prev->twin->next; + twin->next->prev = twin; + // prev->twin slot is retired to the edge free list. + b3HullBuilder_RetireEdge( b, prev->twin ); + + prev->next = next->next; + prev->next->prev = prev; + + prev->twin = twin; + twin->twin = prev; + + // Drop the redundant vertex (slot abandoned in the bump allocator). + b3QHList_Remove( &next->origin->link ); + } + + // Twin->face changed shape; recompute its plane and refresh its cached max conflict. + b3NewellPlane( twin->face ); + b3HullBuilder_RecacheConflicts( twin->face, b->minOutside ); + } + else + { + prev->next = next; + next->prev = prev; + } +} + + +static void b3HullBuilder_AbsorbFaces( b3HullBuilder* b, b3QHFace* face ) +{ + for ( int i = 0; i < b->mergedFacesCount; ++i ) + { + B3_ASSERT( b->mergedFaces[i]->mark == B3_MARK_DELETE ); + b3QHListNode* head = &b->mergedFaces[i]->conflictListHead.link; + + b3QHListNode* node = head->next; + while ( node != head ) + { + b3QHVertex* vertex = (b3QHVertex*)node; + node = node->next; + + b3QHList_Remove( &vertex->link ); + + float distance = b3PlaneSeparation( face->plane, vertex->position ); + if ( distance > b->minOutside ) + { + b3QHList_PushBack( &face->conflictListHead.link, &vertex->link ); + vertex->conflictFace = face; + if ( distance > face->maxConflictDistance ) + { + face->maxConflictDistance = distance; + face->maxConflict = vertex; + } + } + else + { + b3QHList_PushBack( &b->orphanedList.link, &vertex->link ); + vertex->conflictFace = NULL; + } + } + + B3_ASSERT( b3QHList_Empty( head ) ); + + // Conflict list is now drained. Retire this face to the free list. + b3HullBuilder_RetireFace( b, b->mergedFaces[i] ); + } +} + +static void b3HullBuilder_ConnectFaces( b3HullBuilder* b, b3QHHalfEdge* edge ) +{ + b3QHFace* face = edge->face; + + b3QHHalfEdge* twin = edge->twin; + + b3QHHalfEdge* edgePrev = edge->prev; + b3QHHalfEdge* edgeNext = edge->next; + b3QHHalfEdge* twinPrev = twin->prev; + b3QHHalfEdge* twinNext = twin->next; + + while ( edgePrev->twin->face == twin->face ) + { + B3_ASSERT( edgePrev->twin == twinNext ); + B3_ASSERT( twinNext->twin == edgePrev ); + + edgePrev = edgePrev->prev; + twinNext = twinNext->next; + } + B3_ASSERT( edgePrev->face != twinNext->face ); + + while ( edgeNext->twin->face == twin->face ) + { + B3_ASSERT( edgeNext->twin == twinPrev ); + B3_ASSERT( twinPrev->twin == edgeNext ); + + edgeNext = edgeNext->next; + twinPrev = twinPrev->prev; + } + B3_ASSERT( edgeNext->face != twinPrev->face ); + + face->edge = edgePrev; + + // Discard opposing face. mergedFaces is single-buffered: ConnectFaces does not nest. + b->mergedFacesCount = 0; + B3_ASSERT( b->mergedFacesCount < b->mergedFacesCapacity ); + b->mergedFaces[b->mergedFacesCount++] = twin->face; + twin->face->mark = B3_MARK_DELETE; + twin->face->edge = NULL; + + for ( b3QHHalfEdge* absorbed = twinNext; absorbed != twinPrev->next; absorbed = absorbed->next ) + { + absorbed->face = face; + } + + b3HullBuilder_DestroyEdges( b, edgePrev->next, edgeNext ); + b3HullBuilder_DestroyEdges( b, twinPrev->next, twinNext ); + + b3HullBuilder_ConnectEdges( b, edgePrev, twinNext ); + b3HullBuilder_ConnectEdges( b, twinPrev, edgeNext ); + + b3NewellPlane( face ); + // Existing conflicts now have stale distances under the new plane; AbsorbFaces will then + // add more incrementally and update the cache as it goes. + b3HullBuilder_RecacheConflicts( face, b->minOutside ); +#if B3_DEBUG + B3_ASSERT( b3CheckConsistency( face ) ); +#endif + + b3HullBuilder_AbsorbFaces( b, face ); +} + +static bool b3HullBuilder_MergeConcave( b3HullBuilder* b, b3QHFace* face ) +{ + b3QHHalfEdge* edge = face->edge; + + do + { + b3QHHalfEdge* twin = edge->twin; + + if ( b3IsEdgeConcave( edge, b->minRadius ) || b3IsEdgeConcave( twin, b->minRadius ) ) + { + b3HullBuilder_ConnectFaces( b, edge ); + return true; + } + + edge = edge->next; + } + while ( edge != face->edge ); + + return false; +} + +static bool b3HullBuilder_MergeCoplanar( b3HullBuilder* b, b3QHFace* face ) +{ + b3QHHalfEdge* edge = face->edge; + + do + { + b3QHHalfEdge* twin = edge->twin; + + if ( !b3IsEdgeConvex( edge, b->minRadius ) || !b3IsEdgeConvex( twin, b->minRadius ) ) + { + b3HullBuilder_ConnectFaces( b, edge ); + return true; + } + + edge = edge->next; + } + while ( edge != face->edge ); + + return false; +} + +static void b3HullBuilder_MergeFaces( b3HullBuilder* b ) +{ + for ( int i = 0; i < b->coneCount; ++i ) + { + b3QHFace* face = b->cone[i]; + if ( face->mark == B3_MARK_VISIBLE && face->flipped ) + { + face->flipped = false; + + float bestArea = 0; + b3QHHalfEdge* bestEdge = NULL; + + b3QHHalfEdge* edge = face->edge; + do + { + b3QHHalfEdge* twin = edge->twin; + float area = twin->face->area; + if ( area > bestArea ) + { + bestArea = area; + bestEdge = edge; + } + + edge = edge->next; + } + while ( edge != face->edge ); + + B3_ASSERT( bestEdge != NULL ); + b3HullBuilder_ConnectFaces( b, bestEdge ); + } + } + + for ( int i = 0; i < b->coneCount; ++i ) + { + b3QHFace* face = b->cone[i]; + if ( face->mark == B3_MARK_VISIBLE ) + { + while ( b3HullBuilder_MergeConcave( b, face ) ) + { + } + } + } + + for ( int i = 0; i < b->coneCount; ++i ) + { + b3QHFace* face = b->cone[i]; + if ( face->mark == B3_MARK_VISIBLE ) + { + while ( b3HullBuilder_MergeCoplanar( b, face ) ) + { + } + } + } +} + +static void b3HullBuilder_ResolveVertices( b3HullBuilder* b ) +{ + b3QHListNode* node = b->orphanedList.link.next; + while ( node != &b->orphanedList.link ) + { + b3QHVertex* vertex = (b3QHVertex*)node; + node = node->next; + b3QHList_Remove( &vertex->link ); + + float maxDistance = b->minOutside; + b3QHFace* maxFace = NULL; + + for ( int i = 0; i < b->coneCount; ++i ) + { + if ( b->cone[i]->mark == B3_MARK_VISIBLE ) + { + float distance = b3PlaneSeparation( b->cone[i]->plane, vertex->position ); + if ( distance > maxDistance ) + { + maxDistance = distance; + maxFace = b->cone[i]; + } + } + } + + if ( maxFace != NULL ) + { + B3_ASSERT( maxFace->mark == B3_MARK_VISIBLE ); + b3QHList_PushBack( &maxFace->conflictListHead.link, &vertex->link ); + vertex->conflictFace = maxFace; + if ( maxDistance > maxFace->maxConflictDistance ) + { + maxFace->maxConflictDistance = maxDistance; + maxFace->maxConflict = vertex; + } + } + // Otherwise: vertex is interior to the hull. Its slot in the bump pool is abandoned. + } + + B3_ASSERT( b3QHList_Empty( &b->orphanedList.link ) ); +} + +static void b3HullBuilder_ResolveFaces( b3HullBuilder* b ) +{ + // Splice deleted faces out of the face list. Faces already retired by AbsorbFaces are no + // longer on faceList, so we guard with b3QHList_Contains before removing. + b3QHListNode* node = b->faceList.link.next; + while ( node != &b->faceList.link ) + { + b3QHFace* face = (b3QHFace*)node; + node = node->next; + + if ( face->mark == B3_MARK_DELETE && b3QHList_Contains( &face->link ) ) + { + B3_ASSERT( b3QHList_Empty( &face->conflictListHead.link ) ); + b3QHList_Remove( &face->link ); + } + } + + for ( int i = 0; i < b->coneCount; ++i ) + { + b3QHFace* face = b->cone[i]; + if ( face->mark == B3_MARK_DELETE ) + { + continue; + } + b3QHList_PushBack( &b->faceList.link, &face->link ); + } +} + +static void b3HullBuilder_AddVertexToHull( b3HullBuilder* b, b3QHVertex* vertex ) +{ + b3QHFace* face = vertex->conflictFace; + vertex->conflictFace = NULL; + b3QHList_Remove( &vertex->link ); + b3QHList_PushBack( &b->vertexList.link, &vertex->link ); + + b->horizonCount = 0; + b3HullBuilder_BuildHorizon( b, vertex, face ); + B3_ASSERT( b->horizonCount >= 3 ); + + b->coneCount = 0; + b3HullBuilder_BuildCone( b, vertex ); + B3_ASSERT( b->coneCount >= 3 ); + + b3HullBuilder_MergeFaces( b ); + b3HullBuilder_ResolveVertices( b ); + b3HullBuilder_ResolveFaces( b ); +} + +static void b3HullBuilder_CleanHull( b3HullBuilder* b, b3Vec3 origin ) +{ + int faceCount = 0; + int halfEdgeCount = 0; + + for ( b3QHListNode* faceNode = b->faceList.link.next; faceNode != &b->faceList.link; faceNode = faceNode->next ) + { + b3QHFace* face = (b3QHFace*)faceNode; + b3QHHalfEdge* edge = face->edge; + + do + { + edge->origin->reachable = true; + edge = edge->next; + halfEdgeCount++; + } + while ( edge != face->edge ); + + face->plane.offset += b3Dot( face->plane.normal, origin ); + face->centroid = b3Add( face->centroid, origin ); + faceCount++; + } + + int vertexCount = 0; + b3QHListNode* node = b->vertexList.link.next; + while ( node != &b->vertexList.link ) + { + b3QHVertex* vertex = (b3QHVertex*)node; + node = node->next; + + if ( !vertex->reachable ) + { + b3QHList_Remove( &vertex->link ); + } + else + { + vertex->position = b3Add( vertex->position, origin ); + vertexCount++; + } + } + + b->interiorPoint = b3Add( b->interiorPoint, origin ); + + b->finalVertexCount = vertexCount; + b->finalHalfEdgeCount = halfEdgeCount; + b->finalFaceCount = faceCount; +} + +#if B3_DEBUG +static bool b3HullBuilder_IsConsistent( const b3HullBuilder* b ) +{ + int v = b->finalVertexCount; + int e = b->finalHalfEdgeCount / 2; + int f = b->finalFaceCount; + + if ( v - e + f != 2 ) + { + return false; + } + + for ( const b3QHListNode* faceNode = b->faceList.link.next; faceNode != &b->faceList.link; faceNode = faceNode->next ) + { + const b3QHFace* face = (const b3QHFace*)faceNode; + if ( face->edge->face != face ) + { + return false; + } + + if ( !b3CheckConsistency( face ) ) + { + return false; + } + + if ( b3PlaneSeparation( face->plane, b->interiorPoint ) > 0 ) + { + return false; + } + + if ( face->mark != B3_MARK_VISIBLE ) + { + return false; + } + + const b3QHHalfEdge* edge = face->edge; + + do + { + if ( edge->next->origin != edge->twin->origin ) + { + return false; + } + if ( edge->prev->next != edge ) + { + return false; + } + if ( edge->next->prev != edge ) + { + return false; + } + if ( edge->twin->twin != edge ) + { + return false; + } + if ( edge->face != face ) + { + return false; + } + if ( b3DistanceSquared( edge->origin->position, edge->twin->origin->position ) < 1000.0f * FLT_MIN ) + { + return false; + } + + edge = edge->next; + } + while ( edge != face->edge ); + } + + return true; +} +#endif + +static bool b3HullBuilder_HasHull( const b3HullBuilder* b ) +{ + int v = b->finalVertexCount; + int e = b->finalHalfEdgeCount / 2; + int f = b->finalFaceCount; + return v - e + f == 2 && f >= 4; +} + +// Build the entire hull. Returns true iff the result satisfies Euler's identity. +static bool b3HullBuilder_Construct( b3HullBuilder* b, const b3Vec3* points, int pointCount, int maxVertexCount, b3Vec3 origin, + b3Vec3* shiftedPoints ) +{ + if ( pointCount < 4 ) + { + return false; + } + + for ( int i = 0; i < pointCount; ++i ) + { + shiftedPoints[i] = b3Sub( points[i], origin ); + } + + b3HullBuilder_ComputeTolerance( b, pointCount, shiftedPoints ); + if ( !b3HullBuilder_BuildInitialHull( b, pointCount, shiftedPoints ) ) + { + return false; + } + + int budget = b3ClampInt( maxVertexCount - 4, 0, B3_HULL_LIMIT - 4 ); + + b3QHVertex* vertex = b3HullBuilder_NextConflictVertex( b ); + while ( vertex && budget > 0 ) + { + b3HullBuilder_AddVertexToHull( b, vertex ); + vertex = b3HullBuilder_NextConflictVertex( b ); + budget -= 1; + } + + b3HullBuilder_CleanHull( b, origin ); + +#if B3_DEBUG + B3_ASSERT( b3HullBuilder_IsConsistent( b ) ); +#endif + + return b3HullBuilder_HasHull( b ); +} + +static inline size_t b3AlignUp8( size_t x ) +{ + return ( x + 7u ) & ~(size_t)7u; +} + +typedef struct b3HullWorkSizes +{ + int N; // pointCount + int M; // clamped maxVertexCount, in [4, B3_HULL_LIMIT] + int vertexCapacity; + int edgeCapacity; + int faceCapacity; + int horizonCapacity; + int coneCapacity; + int mergedFacesCapacity; + int horizonStackCapacity; + size_t totalBytes; + + size_t offsetVertex; + size_t offsetEdge; + size_t offsetFace; + size_t offsetHorizon; + size_t offsetCone; + size_t offsetMergedFaces; + size_t offsetHorizonStack; + size_t offsetShiftedPoints; +} b3HullWorkSizes; + +static b3HullWorkSizes b3ComputeHullWorkSizes( int pointCount, int clampedMaxCount ) +{ + b3HullWorkSizes s; + s.N = pointCount; + s.M = clampedMaxCount; + + // Vertices: 4 initial hull vertices + at most one per remaining input point. No free list. + s.vertexCapacity = pointCount + 4; + + // Edges and faces use free-list recycling; capacity is proportional to live hull size. + // edgeCapacity: peak is ~twice live edges plus cone edges; floor 48. + s.edgeCapacity = 24 * s.M - 48; + if ( s.edgeCapacity < 48 ) + { + s.edgeCapacity = 48; + } + + // faceCapacity: peak intermediate state live faces (<=2*M-4) plus full cone (<=3*M-6); floor 16. + s.faceCapacity = 5 * s.M - 10; + if ( s.faceCapacity < 16 ) + { + s.faceCapacity = 16; + } + + // Horizon/cone bounded by current half-edge count; mergedFaces by face count. + s.horizonCapacity = 3 * s.M - 6; + if ( s.horizonCapacity < 6 ) + { + s.horizonCapacity = 6; + } + s.coneCapacity = s.horizonCapacity; + s.mergedFacesCapacity = 2 * s.M - 4; + if ( s.mergedFacesCapacity < 4 ) + { + s.mergedFacesCapacity = 4; + } + + // Horizon DFS depth is bounded by the number of live faces (Euler: <=2*M-4). + s.horizonStackCapacity = 2 * s.M - 4; + if ( s.horizonStackCapacity < 4 ) + { + s.horizonStackCapacity = 4; + } + + size_t offset = 0; + + s.offsetVertex = offset; + offset = b3AlignUp8( offset + (size_t)s.vertexCapacity * sizeof( b3QHVertex ) ); + + s.offsetEdge = offset; + offset = b3AlignUp8( offset + (size_t)s.edgeCapacity * sizeof( b3QHHalfEdge ) ); + + s.offsetFace = offset; + offset = b3AlignUp8( offset + (size_t)s.faceCapacity * sizeof( b3QHFace ) ); + + s.offsetHorizon = offset; + offset = b3AlignUp8( offset + (size_t)s.horizonCapacity * sizeof( b3QHHalfEdge* ) ); + + s.offsetCone = offset; + offset = b3AlignUp8( offset + (size_t)s.coneCapacity * sizeof( b3QHFace* ) ); + + s.offsetMergedFaces = offset; + offset = b3AlignUp8( offset + (size_t)s.mergedFacesCapacity * sizeof( b3QHFace* ) ); + + s.offsetHorizonStack = offset; + offset = b3AlignUp8( offset + (size_t)s.horizonStackCapacity * sizeof( b3HorizonFrame ) ); + + s.offsetShiftedPoints = offset; + offset += (size_t)pointCount * sizeof( b3Vec3 ); + + s.totalBytes = offset; + return s; +} + +static void b3HullBuilder_Init( b3HullBuilder* b, char* mem, const b3HullWorkSizes* s ) +{ + memset( b, 0, sizeof( *b ) ); + b3QHList_Init( &b->orphanedList.link ); + b3QHList_Init( &b->vertexList.link ); + b3QHList_Init( &b->faceList.link ); + + b->vertexBase = (b3QHVertex*)( mem + s->offsetVertex ); + b->vertexCapacity = s->vertexCapacity; + + b->edgeBase = (b3QHHalfEdge*)( mem + s->offsetEdge ); + b->edgeCapacity = s->edgeCapacity; + + b->faceBase = (b3QHFace*)( mem + s->offsetFace ); + b->faceCapacity = s->faceCapacity; + + b->horizon = (b3QHHalfEdge**)( mem + s->offsetHorizon ); + b->horizonCapacity = s->horizonCapacity; + + b->cone = (b3QHFace**)( mem + s->offsetCone ); + b->coneCapacity = s->coneCapacity; + + b->mergedFaces = (b3QHFace**)( mem + s->offsetMergedFaces ); + b->mergedFacesCapacity = s->mergedFacesCapacity; + + b->horizonStack = (b3HorizonFrame*)( mem + s->offsetHorizonStack ); + b->horizonStackCapacity = s->horizonStackCapacity; +} + +static b3Vec3* b3GetHullPointsWrite( b3Hull* hull ) +{ + if ( hull->pointOffset == 0 ) + { + return NULL; + } + return (b3Vec3*)( (intptr_t)hull + hull->pointOffset ); +} + +static b3Plane* b3GetHullPlanesWrite( b3Hull* hull ) +{ + if ( hull->planeOffset == 0 ) + { + return NULL; + } + return (b3Plane*)( (intptr_t)hull + hull->planeOffset ); +} + +static b3HullVertex* b3GetHullVerticesWrite( b3Hull* hull ) +{ + if ( hull->vertexOffset == 0 ) + { + return NULL; + } + return (b3HullVertex*)( (intptr_t)hull + hull->vertexOffset ); +} + +static b3HullHalfEdge* b3GetHullEdgesWrite( b3Hull* hull ) +{ + if ( hull->edgeOffset == 0 ) + { + return NULL; + } + return (b3HullHalfEdge*)( (intptr_t)hull + hull->edgeOffset ); +} + +int b3FindHullSupportVertex( const b3Hull* hull, b3Vec3 direction ) +{ + int bestIndex = B3_NULL_INDEX; + float bestDot = -FLT_MAX; + + int vertexCount = hull->vertexCount; + const b3Vec3* points = b3GetHullPoints( hull ); + + for ( int index = 0; index < vertexCount; ++index ) + { + float dot = b3Dot( direction, points[index] ); + if ( dot > bestDot ) + { + bestIndex = index; + bestDot = dot; + } + } + B3_ASSERT( bestIndex >= 0 ); + + return bestIndex; +} + +int b3FindHullSupportFace( const b3Hull* hull, b3Vec3 direction ) +{ + int bestIndex = B3_NULL_INDEX; + float bestDot = -FLT_MAX; + + int faceCount = hull->faceCount; + const b3Plane* planes = b3GetHullPlanes( hull ); + + for ( int index = 0; index < faceCount; ++index ) + { + float dot = b3Dot( planes[index].normal, direction ); + if ( dot > bestDot ) + { + bestDot = dot; + bestIndex = index; + } + } + B3_ASSERT( bestIndex >= 0 ); + + return bestIndex; +} + +#if B3_ENABLE_VALIDATION + +bool b3IsValidHull( const b3Hull* hull ) +{ + if ( hull->version != B3_HULL_VERSION ) + { + return false; + } + + int v = hull->vertexCount; + int e = hull->edgeCount / 2; + int f = hull->faceCount; + + if ( v - e + f != 2 ) + { + return false; + } + + const b3HullVertex* vertices = b3GetHullVertices( hull ); + const b3HullHalfEdge* edges = b3GetHullEdges( hull ); + for ( int index = 0; index < hull->vertexCount; ++index ) + { + const b3HullVertex* vertex = vertices + index; + const b3HullHalfEdge* edge = edges + vertex->edge; + + if ( edge->origin != index ) + { + return false; + } + } + + for ( int index = 0; index < hull->edgeCount; index += 2 ) + { + const b3HullHalfEdge* edge = edges + index + 0; + const b3HullHalfEdge* twin = edges + index + 1; + + if ( edge->twin != index + 1 ) + { + return false; + } + + if ( twin->twin != index + 0 ) + { + return false; + } + } + + const b3HullFace* faces = b3GetHullFaces( hull ); + const b3Plane* planes = b3GetHullPlanes( hull ); + for ( int faceIndex = 0; faceIndex < hull->faceCount; ++faceIndex ) + { + const b3HullFace* face = faces + faceIndex; + + int baseEdgeIndex = face->edge; + const b3HullHalfEdge* edge = edges + baseEdgeIndex; + + b3Plane plane = planes[faceIndex]; + if ( b3PlaneSeparation( plane, hull->center ) >= 0.0f ) + { + return false; + } + + int edgeIndex = baseEdgeIndex; + do + { + edge = edges + edgeIndex; + const b3HullHalfEdge* next = edges + edge->next; + const b3HullHalfEdge* twin = edges + edge->twin; + + if ( edge->face != faceIndex ) + { + return false; + } + + if ( twin->twin != edgeIndex ) + { + return false; + } + + if ( next->origin != twin->origin ) + { + return false; + } + + edgeIndex = edge->next; + } + while ( edgeIndex != baseEdgeIndex ); + } + + if ( hull->volume <= 0.0f ) + { + return false; + } + + if ( hull->surfaceArea <= 0.0f ) + { + return false; + } + + if ( hull->innerRadius <= 0.0f ) + { + return false; + } + + return true; +} + +#else + +bool b3IsValidHull( const b3Hull* hull ) +{ + B3_UNUSED( hull ); + return true; +} + +#endif + +b3Hull* b3CreateCylinder( float height, float radius, float yOffset, int sides ) +{ + B3_ASSERT( height > 0.0f ); + B3_ASSERT( radius > 0.0f ); + B3_ASSERT( 3 <= sides && sides <= 32 ); + + int pointCount = 2 * sides; + b3Vec3* points = (b3Vec3*)b3Alloc( pointCount * sizeof( b3Vec3 ) ); + B3_ASSERT( points != NULL ); + + float alpha = 0.0f; + float deltaAlpha = 2.0f * B3_PI / sides; + + for ( int index = 0; index < sides; ++index ) + { + float sinAlpha = b3Sin( alpha ); + float cosAlpha = b3Cos( alpha ); + + points[2 * index + 0] = (b3Vec3){ radius * cosAlpha, yOffset, radius * sinAlpha }; + points[2 * index + 1] = (b3Vec3){ radius * cosAlpha, yOffset + height, radius * sinAlpha }; + + alpha += deltaAlpha; + } + + b3Hull* hull = b3CreateHull( points, pointCount, pointCount ); + B3_ASSERT( hull->vertexCount == pointCount ); + B3_ASSERT( hull->edgeCount == 6 * sides ); + B3_ASSERT( hull->faceCount == sides + 2 ); + + b3Free( points, pointCount * sizeof( b3Vec3 ) ); + + return hull; +} + +b3Hull* b3CreateCone( float height, float radius1, float radius2, int slices ) +{ + B3_ASSERT( height > 0.0f ); + B3_ASSERT( radius1 > 0.0f ); + B3_ASSERT( radius2 > 0.0f ); + B3_ASSERT( 4 <= slices && slices <= 32 ); + + int pointCount = 2 * slices; + b3Vec3* points = (b3Vec3*)b3Alloc( pointCount * sizeof( b3Vec3 ) ); + B3_ASSERT( points != NULL ); + + float alpha = 0.0f; + float deltaAlpha = 2.0f * B3_PI / slices; + + for ( int index = 0; index < slices; ++index ) + { + float sinAlpha = b3Sin( alpha ); + float cosAlpha = b3Cos( alpha ); + + points[2 * index + 0] = (b3Vec3){ radius1 * cosAlpha, 0.0f, radius1 * sinAlpha }; + points[2 * index + 1] = (b3Vec3){ radius2 * cosAlpha, height, radius2 * sinAlpha }; + + alpha += deltaAlpha; + } + + b3Hull* hull = b3CreateHull( points, pointCount, pointCount ); + B3_ASSERT( hull->vertexCount == pointCount ); + B3_ASSERT( hull->edgeCount == 6 * slices ); + B3_ASSERT( hull->faceCount == slices + 2 ); + + b3Free( points, pointCount * sizeof( b3Vec3 ) ); + + return hull; +} + +static void b3UpdateHullBounds( b3Hull* hull ) +{ + const b3Vec3* points = b3GetHullPoints( hull ); + int vertexCount = hull->vertexCount; + + B3_ASSERT( vertexCount > 0 ); + b3AABB bounds; + bounds.lowerBound = points[0]; + bounds.upperBound = points[0]; + + for ( int i = 1; i < vertexCount; ++i ) + { + b3Vec3 p = points[i]; + bounds.lowerBound = b3Min( bounds.lowerBound, p ); + bounds.upperBound = b3Max( bounds.upperBound, p ); + } + + hull->aabb = bounds; +} + +// M. Kallay - "Computing the Moment of Inertia of a Solid Defined by a Triangle Mesh" +static bool b3UpdateHullBulkProperties( b3Hull* hull ) +{ + const b3Vec3* points = b3GetHullPoints( hull ); + const b3HullFace* faces = b3GetHullFaces( hull ); + const b3HullHalfEdge* edges = b3GetHullEdges( hull ); + const b3Plane* planes = b3GetHullPlanes( hull ); + + float area = 0.0f; + float volume = 0.0f; + b3Vec3 center = b3Vec3_zero; + + // Use the first vertex to reduce round-off errors. + b3Vec3 origin = points[0]; + + float xx = 0.0f; + float xy = 0.0f; + float yy = 0.0f; + float xz = 0.0f; + float zz = 0.0f; + float yz = 0.0f; + + int faceCount = hull->faceCount; + + for ( int faceIndex = 0; faceIndex < faceCount; ++faceIndex ) + { + const b3HullFace* face = faces + faceIndex; + const b3HullHalfEdge* edge1 = edges + face->edge; + const b3HullHalfEdge* edge2 = edges + edge1->next; + const b3HullHalfEdge* edge3 = edges + edge2->next; + + B3_ASSERT( edge1 != edge3 ); + B3_ASSERT( edge1->origin < hull->vertexCount ); + + b3Vec3 v1 = b3Sub( points[edge1->origin], origin ); + + do + { + B3_ASSERT( edge2->origin < hull->vertexCount ); + B3_ASSERT( edge3->origin < hull->vertexCount ); + + b3Vec3 v2 = b3Sub( points[edge2->origin], origin ); + b3Vec3 v3 = b3Sub( points[edge3->origin], origin ); + + area += b3Length( b3Cross( b3Sub( v2, v1 ), b3Sub( v3, v1 ) ) ); + + float det = b3ScalarTripleProduct( v1, v2, v3 ); + + volume += det; + + b3Vec3 v4 = b3Add( v1, b3Add( v2, v3 ) ); + center = b3Add( center, b3MulSV( det, v4 ) ); + + xx += det * ( v1.x * v1.x + v2.x * v2.x + v3.x * v3.x + v4.x * v4.x ); + yy += det * ( v1.y * v1.y + v2.y * v2.y + v3.y * v3.y + v4.y * v4.y ); + zz += det * ( v1.z * v1.z + v2.z * v2.z + v3.z * v3.z + v4.z * v4.z ); + xy += det * ( v1.x * v1.y + v2.x * v2.y + v3.x * v3.y + v4.x * v4.y ); + xz += det * ( v1.x * v1.z + v2.x * v2.z + v3.x * v3.z + v4.x * v4.z ); + yz += det * ( v1.y * v1.z + v2.y * v2.z + v3.y * v3.z + v4.y * v4.z ); + + edge2 = edge3; + edge3 = edges + edge3->next; + } + while ( edge1 != edge3 ); + } + + B3_VALIDATE( volume > 0.0f ); + + b3Vec3 localCenter = volume > 0.0f ? b3MulSV( 0.25f / volume, center ) : b3Vec3_zero; + center = b3Add( localCenter, origin ); + + float radius = FLT_MAX; + for ( int faceIndex = 0; faceIndex < faceCount; ++faceIndex ) + { + b3Plane plane = planes[faceIndex]; + float distance = b3PlaneSeparation( plane, center ); + B3_VALIDATE( distance < 0.0f ); + + radius = b3MinFloat( radius, -distance ); + } + + B3_VALIDATE( 0.0f < radius && radius < FLT_MAX ); + + b3Matrix3 inertia; + inertia.cx.x = yy + zz; + inertia.cy.x = -xy; + inertia.cz.x = -xz; + inertia.cx.y = -xy; + inertia.cy.y = xx + zz; + inertia.cz.y = -yz; + inertia.cx.z = -xz; + inertia.cy.z = -yz; + inertia.cz.z = xx + yy; + + float mass = volume / 6.0f; + + b3Matrix3 centralInertia = b3MulSM( 1.0f / 120.0f, inertia ); + centralInertia = b3SubMM( centralInertia, b3Steiner( mass, localCenter ) ); + + hull->center = center; + hull->centralInertia = centralInertia; + hull->volume = mass; + hull->surfaceArea = 0.5f * area; + hull->innerRadius = radius; + + if ( mass <= 0.0f ) + { + return false; + } + + if ( volume <= 0.0f ) + { + return false; + } + + if ( area <= 0.0f ) + { + return false; + } + + if ( radius <= 0.0f ) + { + return false; + } + + return true; +} + +b3Hull* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCount ) +{ + if ( pointCount < 4 ) + { + return NULL; + } + + b3Vec3 origin = points[0]; + int clampedMaxCount = b3ClampInt( maxVertexCount, 4, B3_HULL_LIMIT ); + + // Single allocation for all working memory. + b3HullWorkSizes sizes = b3ComputeHullWorkSizes( pointCount, clampedMaxCount ); + char* work = b3Alloc( sizes.totalBytes ); + + b3HullBuilder builder; + b3HullBuilder_Init( &builder, work, &sizes ); + + b3Vec3* shiftedPoints = (b3Vec3*)( work + sizes.offsetShiftedPoints ); + + bool ok = b3HullBuilder_Construct( &builder, points, pointCount, clampedMaxCount, origin, shiftedPoints ); + if ( !ok ) + { + b3Free( work, sizes.totalBytes ); + return NULL; + } + + if (builder.finalVertexCount >= B3_HULL_LIMIT) + { + b3Log( "hull final vertex count of %d exceeds limit of %d", builder.finalVertexCount, B3_HULL_LIMIT ); + b3Free( work, sizes.totalBytes ); + return NULL; + } + + if (builder.finalFaceCount >= B3_HULL_LIMIT) + { + b3Log( "hull final face count of %d exceeds limit of %d", builder.finalFaceCount, B3_HULL_LIMIT ); + b3Free( work, sizes.totalBytes ); + return NULL; + } + + if (builder.finalHalfEdgeCount >= B3_HULL_LIMIT) + { + b3Log( "hull final half edge count of %d exceeds limit of %d", builder.finalHalfEdgeCount, B3_HULL_LIMIT ); + b3Free( work, sizes.totalBytes ); + return NULL; + } + + // Walk lists into temp arrays bounded by B3_HULL_LIMIT, stamping finalIndex on each node so + // the resolution pass below is O(E + F) instead of O(E^2 + F^2). + const b3QHVertex* tempVertices[B3_HULL_LIMIT]; + int vertexCount = 0; + for ( b3QHListNode* node = builder.vertexList.link.next; node != &builder.vertexList.link; node = node->next ) + { + B3_ASSERT( vertexCount <= B3_HULL_LIMIT - 1 ); + + b3QHVertex* vertex = (b3QHVertex*)node; + vertex->finalIndex = vertexCount; + tempVertices[vertexCount++] = vertex; + } + + // Collect edges in twin-paired order (i, i+1) by stamping each pair as we discover it. + // Replaces b3SortEdges' O(E^2) twin pairing. + const b3QHFace* tempFaces[B3_HULL_LIMIT]; + const b3QHHalfEdge* tempEdges[B3_HULL_LIMIT]; + int faceCount = 0; + int edgeCount = 0; + + for ( b3QHListNode* faceNode = builder.faceList.link.next; faceNode != &builder.faceList.link; faceNode = faceNode->next ) + { + B3_ASSERT( faceCount <= B3_HULL_LIMIT - 1 ); + + b3QHFace* face = (b3QHFace*)faceNode; + face->finalIndex = faceCount; + tempFaces[faceCount++] = face; + + b3QHHalfEdge* edge = face->edge; + do + { + if ( edge->finalIndex < 0 ) + { + B3_ASSERT( edgeCount + 1 <= B3_HULL_LIMIT - 1 ); + + edge->finalIndex = edgeCount; + tempEdges[edgeCount++] = edge; + edge->twin->finalIndex = edgeCount; + tempEdges[edgeCount++] = edge->twin; + } + edge = edge->next; + } + while ( edge != face->edge ); + } + + // Allocate the hull. Arrays hang off the end. + int byteCount = (int)sizeof( b3Hull ); + int vertexOffset = byteCount; + byteCount += vertexCount * (int)sizeof( b3HullVertex ); + int pointOffset = byteCount; + byteCount += vertexCount * (int)sizeof( b3Vec3 ); + int edgeOffset = byteCount; + byteCount += edgeCount * (int)sizeof( b3HullHalfEdge ); + int faceOffset = byteCount; + byteCount += faceCount * (int)sizeof( b3HullFace ); + int planeOffset = byteCount; + byteCount += faceCount * (int)sizeof( b3Plane ); + + b3Hull* hull = b3Alloc( byteCount ); + memset( hull, 0, byteCount ); + + hull->version = B3_HULL_VERSION; + hull->vertexOffset = vertexOffset; + hull->pointOffset = pointOffset; + hull->edgeOffset = edgeOffset; + hull->faceOffset = faceOffset; + hull->planeOffset = planeOffset; + + hull->vertexCount = vertexCount; + hull->edgeCount = edgeCount; + hull->faceCount = faceCount; + + hull->byteCount = byteCount; + + b3HullVertex* vertices = b3GetHullVerticesWrite( hull ); + b3HullHalfEdge* edges = b3GetHullEdgesWrite( hull ); + b3HullFace* faces = (b3HullFace*)( (intptr_t)hull + hull->faceOffset ); + b3Vec3* finalPoints = b3GetHullPointsWrite( hull ); + b3Plane* planes = b3GetHullPlanesWrite( hull ); + + for ( int index = 0; index < vertexCount; ++index ) + { + vertices[index].edge = 0; + finalPoints[index] = tempVertices[index]->position; + } + + for ( int index = 0; index < edgeCount; ++index ) + { + const b3QHHalfEdge* edge = tempEdges[index]; + B3_ASSERT( 0 <= edge->next->finalIndex && edge->next->finalIndex <= UINT8_MAX ); + B3_ASSERT( 0 <= edge->twin->finalIndex && edge->twin->finalIndex <= UINT8_MAX ); + B3_ASSERT( 0 <= edge->face->finalIndex && edge->face->finalIndex <= UINT8_MAX ); + B3_ASSERT( 0 <= edge->origin->finalIndex && edge->origin->finalIndex <= UINT8_MAX ); + + edges[index].next = (uint8_t)edge->next->finalIndex; + edges[index].twin = (uint8_t)edge->twin->finalIndex; + edges[index].face = (uint8_t)edge->face->finalIndex; + edges[index].origin = (uint8_t)edge->origin->finalIndex; + + vertices[edge->origin->finalIndex].edge = (uint8_t)index; + } + + for ( int index = 0; index < faceCount; ++index ) + { + const b3QHFace* face = tempFaces[index]; + B3_ASSERT( 0 <= face->edge->finalIndex && face->edge->finalIndex <= UINT8_MAX ); + + faces[index].edge = (uint8_t)face->edge->finalIndex; + planes[index] = face->plane; + } + + // All builder pointers are dead from here on. + b3Free( work, sizes.totalBytes ); + + b3UpdateHullBounds( hull ); + bool success = b3UpdateHullBulkProperties( hull ); + if ( success == false ) + { + b3DestroyHull( hull ); + return NULL; + } + + if ( b3IsValidHull( hull ) == false ) + { + b3DestroyHull( hull ); + return NULL; + } + + hull->hash = 0; + hull->hash = b3Hash( B3_HASH_INIT, (uint8_t*)hull, hull->byteCount ); + + return hull; +} + +b3Hull* b3CloneHull( const b3Hull* hull ) +{ + if ( hull == NULL || b3IsValidHull( hull ) == false ) + { + return NULL; + } + + b3Hull* clone = (b3Hull*)b3Alloc( hull->byteCount ); + memcpy( clone, hull, hull->byteCount ); + + return clone; +} + +b3Hull* b3CloneAndTransformHull( const b3Hull* original, b3Transform transform, b3Vec3 scale ) +{ + if ( original == NULL || b3IsValidHull( original ) == false ) + { + return NULL; + } + + b3Hull* hull = (b3Hull*)b3Alloc( original->byteCount ); + memcpy( hull, original, original->byteCount ); + + b3Vec3 safeScale = b3SafeScale( scale ); + + b3HullHalfEdge* edges = b3GetHullEdgesWrite( hull ); + const b3HullFace* faces = b3GetHullFaces( hull ); + int faceCount = hull->faceCount; + int vertexCount = hull->vertexCount; + + if ( safeScale.x * safeScale.y * safeScale.z < 0.0f ) + { + // Reflected: reverse edge winding for each face. + for ( int i = 0; i < faceCount; ++i ) + { + const b3HullFace* face = faces + i; + + uint8_t startEdgeIndex = face->edge; + uint8_t currentEdgeIndex = startEdgeIndex; + uint8_t prevEdgeIndex = UINT8_MAX; + + do + { + b3HullHalfEdge* edge = edges + currentEdgeIndex; + + if ( edge->next == startEdgeIndex ) + { + prevEdgeIndex = currentEdgeIndex; + break; + } + + currentEdgeIndex = edge->next; + } + while ( currentEdgeIndex != startEdgeIndex ); + + B3_ASSERT( prevEdgeIndex != UINT8_MAX ); + + currentEdgeIndex = startEdgeIndex; + + do + { + b3HullHalfEdge* edge = edges + currentEdgeIndex; + uint8_t nextIndex = edge->next; + edge->next = prevEdgeIndex; + + if ( currentEdgeIndex < edge->twin ) + { + b3HullHalfEdge* twin = edges + edge->twin; + B3_SWAP( edge->origin, twin->origin ); + } + + prevEdgeIndex = currentEdgeIndex; + currentEdgeIndex = nextIndex; + } + while ( currentEdgeIndex != startEdgeIndex ); + } + + b3HullVertex* vertices = b3GetHullVerticesWrite( hull ); + + for ( int i = 0; i < vertexCount; ++i ) + { + b3HullVertex* vertex = vertices + i; + const b3HullHalfEdge* edge = edges + vertex->edge; + vertex->edge = edge->twin; + } + } + + b3Matrix3 matrix = b3MakeMatrixFromQuat( transform.q ); + b3Vec3* points = b3GetHullPointsWrite( hull ); + + for ( int i = 0; i < vertexCount; ++i ) + { + points[i] = b3Add( b3MulMV( matrix, b3Mul( safeScale, points[i] ) ), transform.p ); + } + + b3Plane* planes = b3GetHullPlanesWrite( hull ); + + for ( int i = 0; i < faceCount; ++i ) + { + int count = 0; + b3Vec3 centroid = b3Vec3_zero; + b3Vec3 normal = b3Vec3_zero; + + const b3HullFace* face = faces + i; + uint8_t startEdgeIndex = face->edge; + uint8_t currentEdgeIndex = startEdgeIndex; + + const b3HullHalfEdge* startEdge = edges + currentEdgeIndex; + B3_ASSERT( startEdge->face == i ); + B3_ASSERT( startEdge->origin < vertexCount ); + + b3Vec3 origin = points[startEdge->origin]; + + do + { + b3HullHalfEdge* edge = edges + currentEdgeIndex; + b3HullHalfEdge* twin = edges + edge->twin; + B3_ASSERT( twin->twin == currentEdgeIndex ); + + b3Vec3 v1 = b3Sub( points[edge->origin], origin ); + b3Vec3 v2 = b3Sub( points[twin->origin], origin ); + + count++; + centroid = b3Add( centroid, v1 ); + normal.x += ( v1.y - v2.y ) * ( v1.z + v2.z ); + normal.y += ( v1.z - v2.z ) * ( v1.x + v2.x ); + normal.z += ( v1.x - v2.x ) * ( v1.y + v2.y ); + + currentEdgeIndex = edge->next; + } + while ( currentEdgeIndex != startEdgeIndex ); + + B3_ASSERT( count > 0 ); + centroid = b3MulSV( 1.0f / (float)count, centroid ); + centroid = b3Add( centroid, origin ); + + float area = b3Length( normal ); + B3_ASSERT( area > 0.0f ); + normal = b3MulSV( 1.0f / area, normal ); + + planes[i] = b3MakePlaneFromNormalAndPoint( normal, centroid ); + } + + b3UpdateHullBounds( hull ); + bool success = b3UpdateHullBulkProperties( hull ); + if ( success == false ) + { + b3Free( hull, original->byteCount ); + return NULL; + } + + hull->hash = 0; + hull->hash = b3Hash( B3_HASH_INIT, (uint8_t*)hull, hull->byteCount ); + + B3_VALIDATE( b3IsValidHull( hull ) ); + + return hull; +} + +void b3DestroyHull( b3Hull* hull ) +{ + b3Free( hull, hull->byteCount ); +} + +b3MassData b3ComputeHullMass( const b3Hull* shape, float density ) +{ + b3MassData out; + out.mass = density * shape->volume; + out.center = shape->center; + + // todo_erin switch to central + out.inertia = b3AddMM( b3MulSM( density, shape->centralInertia ), b3Steiner( out.mass, out.center ) ); + return out; +} + +b3AABB b3ComputeHullAABB( const b3Hull* shape, b3Transform transform ) +{ + return b3AABB_Transform( transform, shape->aabb ); +} + +b3AABB b3ComputeSweptHullAABB( const b3Hull* shape, b3Transform xf1, b3Transform xf2 ) +{ + b3AABB aabb1 = b3AABB_Transform( xf1, shape->aabb ); + b3AABB aabb2 = b3AABB_Transform( xf2, shape->aabb ); + return b3AABB_Union( aabb1, aabb2 ); +} + +bool b3OverlapHull( const b3Hull* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ) +{ + const b3Vec3* points = b3GetHullPoints( shape ); + + b3DistanceInput input; + input.proxyA = (b3ShapeProxy){ points, shape->vertexCount, 0.0f }; + input.proxyB = *proxy; + input.transformA = shapeTransform; + input.transformB = b3Transform_identity; + input.useRadii = true; + + b3SimplexCache cache = { 0 }; + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + return output.distance < B3_OVERLAP_SLOP; +} + +b3CastOutput b3RayCastHull( const b3Hull* shape, const b3RayCastInput* input ) +{ + B3_ASSERT( b3IsValidRay( input ) ); + b3CastOutput output = { 0 }; + + float lower = 0.0f; + float upper = input->maxFraction; + int bestFace = B3_NULL_INDEX; + + const b3Plane* planes = b3GetHullPlanes( shape ); + + for ( int faceIndex = 0; faceIndex < shape->faceCount; ++faceIndex ) + { + b3Plane plane = planes[faceIndex]; + + float distance = plane.offset - b3Dot( plane.normal, input->origin ); + float denominator = b3Dot( plane.normal, input->translation ); + + if ( denominator == 0.0f ) + { + if ( distance < 0.0f ) + { + return output; + } + } + else + { + float fraction = distance / denominator; + + if ( denominator < 0.0f ) + { + if ( fraction > lower ) + { + bestFace = faceIndex; + lower = fraction; + } + } + else + { + if ( fraction < upper ) + { + upper = fraction; + } + } + + if ( upper < lower ) + { + return output; + } + } + } + + if ( bestFace >= 0 ) + { + output.point = b3Add( input->origin, b3MulSV( lower, input->translation ) ); + output.normal = planes[bestFace].normal; + output.fraction = lower; + output.hit = true; + } + else + { + output.point = input->origin; + output.hit = true; + } + + return output; +} + +b3CastOutput b3ShapeCastHull( const b3Hull* shape, const b3ShapeCastInput* input ) +{ + const b3Vec3* points = b3GetHullPoints( shape ); + + b3ShapeCastPairInput pairInput; + pairInput.proxyA = (b3ShapeProxy){ points, shape->vertexCount, 0.0f }; + pairInput.proxyB = input->proxy; + pairInput.transformA = b3Transform_identity; + pairInput.transformB = b3Transform_identity; + pairInput.translationB = input->translation; + pairInput.maxFraction = input->maxFraction; + pairInput.canEncroach = input->canEncroach; + + b3CastOutput output = b3ShapeCast( &pairInput ); + return output; +} + +int b3CollideMoverAndHull( b3PlaneResult* result, const b3Hull* shape, const b3Capsule* mover ) +{ + const b3Vec3* points = b3GetHullPoints( shape ); + b3DistanceInput distanceInput; + distanceInput.proxyA = (b3ShapeProxy){ points, shape->vertexCount, 0.0f }; + distanceInput.proxyB = (b3ShapeProxy){ &mover->center1, 2, mover->radius }; + distanceInput.transformA = b3Transform_identity; + distanceInput.transformB = b3Transform_identity; + distanceInput.useRadii = false; + + float totalRadius = mover->radius; + + b3SimplexCache cache = { 0 }; + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance <= totalRadius ) + { + b3Plane plane = { distanceOutput.normal, totalRadius - distanceOutput.distance }; + *result = (b3PlaneResult){ plane, distanceOutput.pointA }; + return 1; + } + + return 0; +} + +b3ShapeExtent b3ComputeHullExtent( const b3Hull* hull, b3Vec3 origin ) +{ + const b3Vec3* points = b3GetHullPoints( hull ); + + b3ShapeExtent extent; + extent.minExtent = hull->innerRadius; + extent.maxExtent = b3Vec3_zero; + for ( int index = 0; index < hull->vertexCount; ++index ) + { + b3Vec3 point = points[index]; + extent.maxExtent = b3Max( extent.maxExtent, b3Abs( b3Sub( point, origin ) ) ); + } + + return extent; +} + +float b3ComputeHullProjectedArea( const b3Hull* hull, b3Vec3 direction ) +{ + float area = 0.0f; + + int faceCount = hull->faceCount; + const b3HullFace* hullFaces = b3GetHullFaces( hull ); + const b3HullHalfEdge* hullEdges = b3GetHullEdges( hull ); + const b3Vec3* hullPoints = b3GetHullPoints( hull ); + + for ( int i = 0; i < faceCount; ++i ) + { + const b3HullFace* face = hullFaces + i; + + int baseEdge = face->edge; + const b3HullHalfEdge* edge = hullEdges + baseEdge; + b3Vec3 p1 = hullPoints[edge->origin]; + + int edgeIndex = edge->next; + edge = hullEdges + edgeIndex; + b3Vec3 p2 = hullPoints[edge->origin]; + + edgeIndex = edge->next; + + do + { + edge = hullEdges + edgeIndex; + b3Vec3 p3 = hullPoints[edge->origin]; + + b3Vec3 e1 = b3Sub( p2, p1 ); + b3Vec3 e2 = b3Sub( p3, p1 ); + b3Vec3 n = b3Cross( e1, e2 ); + float a = b3Dot( n, direction ); + area += b3MaxFloat( a, 0.0f ); + + p2 = p3; + edgeIndex = edge->next; + } + while ( edgeIndex != baseEdge ); + } + + return 0.5f * area; +} + +// Constant template box (vertex/edge/face/topology). b3MakeTransformedBoxHull copies and +// fills in the runtime-dependent fields (boxPoints, boxPlanes, aabb, mass properties, hash). +static const b3BoxHull s_boxHull = { + .base = { + .version = B3_HULL_VERSION, + .byteCount = sizeof( b3BoxHull ), + .hash = 0, + .vertexCount = 8, + .edgeCount = 24, + .faceCount = 6, + .vertexOffset = offsetof( b3BoxHull, boxVertices ), + .pointOffset = offsetof( b3BoxHull, boxPoints ), + .edgeOffset = offsetof( b3BoxHull, boxEdges ), + .faceOffset = offsetof( b3BoxHull, boxFaces ), + .planeOffset = offsetof( b3BoxHull, boxPlanes ), + }, + .boxVertices = { + [0] = { .edge = 8 }, + [1] = { .edge = 1 }, + [2] = { .edge = 0 }, + [3] = { .edge = 9 }, + [4] = { .edge = 13 }, + [5] = { .edge = 3 }, + [6] = { .edge = 5 }, + [7] = { .edge = 11 }, + }, + .boxEdges = { + [0] = { 2, 1, 2, 0 }, + [1] = { 17, 0, 1, 5 }, + [2] = { 4, 3, 1, 0 }, + [3] = { 20, 2, 5, 3 }, + [4] = { 6, 5, 5, 0 }, + [5] = { 23, 4, 6, 4 }, + [6] = { 0, 7, 6, 0 }, + [7] = { 18, 6, 2, 2 }, + [8] = { 10, 9, 0, 1 }, + [9] = { 21, 8, 3, 5 }, + [10] = { 12, 11, 3, 1 }, + [11] = { 16, 10, 7, 2 }, + [12] = { 14, 13, 7, 1 }, + [13] = { 19, 12, 4, 4 }, + [14] = { 8, 15, 4, 1 }, + [15] = { 22, 14, 0, 3 }, + [16] = { 7, 17, 3, 2 }, + [17] = { 9, 16, 2, 5 }, + [18] = { 11, 19, 6, 2 }, + [19] = { 5, 18, 7, 4 }, + [20] = { 15, 21, 1, 3 }, + [21] = { 1, 20, 0, 5 }, + [22] = { 3, 23, 4, 3 }, + [23] = { 13, 22, 5, 4 }, + }, + .boxFaces = { + [0] = { .edge = 0 }, + [1] = { .edge = 8 }, + [2] = { .edge = 16 }, + [3] = { .edge = 20 }, + [4] = { .edge = 19 }, + [5] = { .edge = 21 }, + }, +}; + +b3BoxHull b3MakeTransformedBoxHull( float hx, float hy, float hz, b3Transform transform ) +{ + b3BoxHull boxHull = s_boxHull; + + float minH = 0.2f * B3_LINEAR_SLOP; + b3Vec3 h = b3Max( (b3Vec3){ minH, minH, minH }, (b3Vec3){ hx, hy, hz } ); + + boxHull.base.aabb = b3AABB_Transform( transform, (b3AABB){ b3Neg( h ), h } ); + boxHull.base.surfaceArea = 8.0f * ( h.x * h.y + h.x * h.z + h.y * h.z ); + boxHull.base.volume = 8.0f * h.x * h.y * h.z; + boxHull.base.innerRadius = b3MinFloat( h.x, b3MinFloat( h.y, h.z ) ); + boxHull.base.center = transform.p; + + b3Matrix3 boxInertia = b3BoxInertia( boxHull.base.volume, b3Neg( h ), h ); + boxHull.base.centralInertia = b3RotateInertia( transform.q, boxInertia ); + + b3Vec3 lower = b3Neg( h ); + b3Vec3 upper = h; + + boxHull.boxPlanes[0] = b3TransformPlane( transform, b3MakePlaneFromNormalAndPoint( b3Neg( b3Vec3_axisX ), lower ) ); + boxHull.boxPlanes[1] = b3TransformPlane( transform, b3MakePlaneFromNormalAndPoint( b3Vec3_axisX, upper ) ); + boxHull.boxPlanes[2] = b3TransformPlane( transform, b3MakePlaneFromNormalAndPoint( b3Neg( b3Vec3_axisY ), lower ) ); + boxHull.boxPlanes[3] = b3TransformPlane( transform, b3MakePlaneFromNormalAndPoint( b3Vec3_axisY, upper ) ); + boxHull.boxPlanes[4] = b3TransformPlane( transform, b3MakePlaneFromNormalAndPoint( b3Neg( b3Vec3_axisZ ), lower ) ); + boxHull.boxPlanes[5] = b3TransformPlane( transform, b3MakePlaneFromNormalAndPoint( b3Vec3_axisZ, upper ) ); + + boxHull.boxPoints[0] = b3TransformPoint( transform, (b3Vec3){ h.x, h.y, h.z } ); + boxHull.boxPoints[1] = b3TransformPoint( transform, (b3Vec3){ -h.x, h.y, h.z } ); + boxHull.boxPoints[2] = b3TransformPoint( transform, (b3Vec3){ -h.x, -h.y, h.z } ); + boxHull.boxPoints[3] = b3TransformPoint( transform, (b3Vec3){ h.x, -h.y, h.z } ); + boxHull.boxPoints[4] = b3TransformPoint( transform, (b3Vec3){ h.x, h.y, -h.z } ); + boxHull.boxPoints[5] = b3TransformPoint( transform, (b3Vec3){ -h.x, h.y, -h.z } ); + boxHull.boxPoints[6] = b3TransformPoint( transform, (b3Vec3){ -h.x, -h.y, -h.z } ); + boxHull.boxPoints[7] = b3TransformPoint( transform, (b3Vec3){ h.x, -h.y, -h.z } ); + + boxHull.base.hash = 0; + boxHull.base.hash = b3Hash( B3_HASH_INIT, (uint8_t*)&boxHull, sizeof( b3BoxHull ) ); + + return boxHull; +} + +b3BoxHull b3MakeCubeHull( float halfWidth ) +{ + return b3MakeBoxHull( halfWidth, halfWidth, halfWidth ); +} + +b3BoxHull b3MakeOffsetBoxHull( float hx, float hy, float hz, b3Vec3 offset ) +{ + b3Transform transform = { .p = offset, .q = b3Quat_identity }; + return b3MakeTransformedBoxHull( hx, hy, hz, transform ); +} + +b3BoxHull b3MakeBoxHull( float hx, float hy, float hz ) +{ + return b3MakeTransformedBoxHull( hx, hy, hz, b3Transform_identity ); +} + +void b3ScaleBox( b3Vec3* halfWidths, b3Transform* transform, b3Vec3 postScale, float minHalfWidth ) +{ + B3_ASSERT( b3IsValidFloat( minHalfWidth ) && minHalfWidth > 0.0f ); + + b3Quat q = transform->q; + + if ( postScale.x < 0.0f || postScale.y < 0.0f || postScale.z < 0.0f ) + { + // todo this might be unnecessary if rotation is identity + // todo compare with polar decomposition (much more expensive) + b3Matrix3 m = b3MakeMatrixFromQuat( q ); + m.cx.x *= postScale.x; + m.cy.x *= postScale.x; + m.cz.x *= postScale.x; + m.cx.y *= postScale.y; + m.cy.y *= postScale.y; + m.cz.y *= postScale.y; + m.cx.z *= postScale.z; + m.cy.z *= postScale.z; + m.cz.z *= postScale.z; + m.cx = b3Normalize( m.cx ); + m.cy = b3Normalize( m.cy ); + m.cz = b3Normalize( m.cz ); + m.cx = postScale.x < 0.0f ? b3Neg( m.cx ) : m.cx; + m.cy = postScale.y < 0.0f ? b3Neg( m.cy ) : m.cy; + m.cz = postScale.z < 0.0f ? b3Neg( m.cz ) : m.cz; + q = b3MakeQuatFromMatrix( &m ); + } + + b3Vec3 absScale = b3Abs( postScale ); + + b3Vec3 h = *halfWidths; + b3Vec3 p1 = b3Mul( absScale, b3RotateVector( q, b3Neg( h ) ) ); + b3Vec3 p2 = b3Mul( absScale, b3RotateVector( q, h ) ); + + b3Vec3 localP1 = b3InvRotateVector( q, p1 ); + b3Vec3 localP2 = b3InvRotateVector( q, p2 ); + + b3Vec3 lower = b3Min( localP1, localP2 ); + b3Vec3 upper = b3Max( localP1, localP2 ); + + b3Vec3 scaledHalfWidth = b3MulSV( 0.5f, b3Sub( upper, lower ) ); + + b3Vec3 mLimit = { minHalfWidth, minHalfWidth, minHalfWidth }; + *halfWidths = b3Max( scaledHalfWidth, mLimit ); + transform->p = b3Mul( postScale, transform->p ); + transform->q = q; +} + +// todo use new hull scaling technique +b3BoxHull b3MakeScaledBoxHull( b3Vec3 halfWidths, b3Transform transform, b3Vec3 postScale ) +{ + b3Vec3 h = halfWidths; + b3Transform xf = transform; + b3ScaleBox( &h, &xf, postScale, 4.0f * B3_LINEAR_SLOP ); + return b3MakeTransformedBoxHull( h.x, h.y, h.z, xf ); +} + +// ============================================================================= +// 2D hull (Andrew's monotone chain) +// ============================================================================= + +// cross(b - a, c - a) +static inline float b3Cross2D( b3Vec2 a, b3Vec2 b, b3Vec2 c ) +{ + return ( b.x - a.x ) * ( c.y - a.y ) - ( b.y - a.y ) * ( c.x - a.x ); +} + +// Iteratively remove the vertices contributing the least area. +int b3SimplifyHull2D( b3Point2D* hull, int count1, int target ) +{ + B3_ASSERT( target >= 3 ); + + if ( count1 <= 3 ) + { + return count1; + } + + float linearSlop = B3_LINEAR_SLOP; + float areaTol = 0.25f * linearSlop * linearSlop; + int count2 = count1; + + for ( ;; ) + { + float minArea = FLT_MAX; + int minIndex = B3_NULL_INDEX; + + for ( int i = 0; i < count2; i++ ) + { + int prev = ( i + count2 - 1 ) % count2; + int next = ( i + 1 ) % count2; + float area = b3Cross2D( hull[prev].p, hull[i].p, hull[next].p ); + B3_VALIDATE( area >= -FLT_EPSILON ); + + if ( area + areaTol < minArea ) + { + minArea = area; + minIndex = i; + } + } + + if ( minArea > areaTol && count2 <= target ) + { + break; + } + + B3_ASSERT( minIndex != B3_NULL_INDEX ); + + count2 -= 1; + if ( minIndex < count2 ) + { + memmove( hull + minIndex, hull + ( minIndex + 1 ), ( count2 - minIndex ) * sizeof( b3Point2D ) ); + } + } + + return count2; +} + +// Sort comparator: by x, then by y. Insertion since n is typically 20 or less. +static void b3Sort2D( b3Point2D* pts, int count ) +{ + for ( int i = 1; i < count; ++i ) + { + b3Point2D base = pts[i]; + int j = i - 1; + while ( j >= 0 && ( pts[j].p.x > base.p.x || ( pts[j].p.x == base.p.x && pts[j].p.y > base.p.y ) ) ) + { + pts[j + 1] = pts[j]; + j -= 1; + } + pts[j + 1] = base; + } +} + +// Welds points that have been sorted O(n) time +static int b3Weld2D( b3Point2D* pts, int count1 ) +{ + if ( count1 <= 1 ) + { + return count1; + } + + float linearSlop = 0.25f * B3_LINEAR_SLOP; + float tolSqr = linearSlop * linearSlop; + + int count2 = 1; + int baseIndex = 0; + for ( int i = 1; i < count1; i++ ) + { + float distanceSqr = b3DistanceSquared2( pts[i].p, pts[baseIndex].p ); + if ( distanceSqr < tolSqr ) + { + if ( pts[i].originalIndex < pts[baseIndex].originalIndex ) + { + pts[baseIndex] = pts[i]; + } + } + else + { + pts[count2] = pts[i]; + baseIndex = count2; + count2 += 1; + } + } + + return count2; +} + +// Andrew's monotone chain convex hull algorithm +// hull must have space for 2 * count1 points +int b3Hull2D( b3Point2D* pts, int count1, b3Point2D* hull ) +{ + if ( count1 <= 0 ) + { + return 0; + } + + if ( count1 == 1 ) + { + hull[0] = pts[0]; + return 1; + } + + b3Sort2D( pts, count1 ); + count1 = b3Weld2D( pts, count1 ); + + if ( count1 == 1 ) + { + hull[0] = pts[0]; + return 1; + } + + if ( count1 == 2 ) + { + hull[0] = pts[0]; + hull[1] = pts[1]; + return 2; + } + + int count2 = 0; + + for ( int i = 0; i < count1; i++ ) + { + while ( count2 >= 2 ) + { + float area = b3Cross2D( hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p ); + if ( area > 0.0f ) + { + break; + } + + count2 -= 1; + } + + B3_VALIDATE( count2 < 2 * count1 ); + hull[count2] = pts[i]; + count2 += 1; + } + + int lowerCount = count2 + 1; + + for ( int i = count1 - 2; i >= 0; i-- ) + { + while ( count2 >= lowerCount ) + { + float area = b3Cross2D( hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p ); + if ( area > 0.0f ) + { + break; + } + + count2 -= 1; + } + + B3_VALIDATE( count2 < 2 * count1 ); + hull[count2] = pts[i]; + count2 += 1; + } + + B3_VALIDATE( hull[0].originalIndex == hull[count2 - 1].originalIndex ); + return count2 - 1; +} diff --git a/src/id_pool.c b/src/id_pool.c new file mode 100644 index 0000000..185b6ee --- /dev/null +++ b/src/id_pool.c @@ -0,0 +1,73 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "id_pool.h" + +b3IdPool b3CreateIdPool( void ) +{ + b3IdPool pool = { 0 }; + b3Array_Reserve( pool.freeArray, 32 ); + return pool; +} + +void b3DestroyIdPool( b3IdPool* pool ) +{ + b3Array_Destroy( pool->freeArray ); + *pool = (b3IdPool){ 0 }; +} + +int b3AllocId( b3IdPool* pool ) +{ + int count = pool->freeArray.count; + if ( count > 0 ) + { + int id = b3Array_Pop( pool->freeArray ); + return id; + } + + int id = pool->nextIndex; + pool->nextIndex += 1; + return id; +} + +void b3FreeId( b3IdPool* pool, int id ) +{ + B3_ASSERT( pool->nextIndex > 0 ); + B3_ASSERT( 0 <= id && id < pool->nextIndex ); + + // todo does not work with assertion above + // should probably be `id == pool->nextIndex - 1` + if ( id == pool->nextIndex ) + { + pool->nextIndex -= 1; + return; + } + + b3Array_Push( pool->freeArray, id ); +} + +#if B3_ENABLE_VALIDATION + +void b3ValidateFreeId( const b3IdPool* pool, int id ) +{ + int freeCount = pool->freeArray.count; + for ( int i = 0; i < freeCount; ++i ) + { + if ( pool->freeArray.data[i] == id ) + { + return; + } + } + + B3_ASSERT( 0 ); +} + +#else + +void b3ValidateFreeId( const b3IdPool* pool, int id ) +{ + B3_UNUSED( pool ); + B3_UNUSED( id ); +} + +#endif diff --git a/src/id_pool.h b/src/id_pool.h new file mode 100644 index 0000000..7659a38 --- /dev/null +++ b/src/id_pool.h @@ -0,0 +1,43 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "container.h" + +typedef struct b3IdPool +{ + b3ArrayC( int ) freeArray; + int nextIndex; +} b3IdPool; + +#ifdef __cplusplus +extern "C" +{ +#endif + +b3IdPool b3CreateIdPool( void ); +void b3DestroyIdPool( b3IdPool* pool ); + +int b3AllocId( b3IdPool* pool ); +void b3FreeId( b3IdPool* pool, int id ); +void b3ValidateFreeId( const b3IdPool* pool, int id ); + +#ifdef __cplusplus +} +#endif + +B3_INLINE int b3GetIdCount( const b3IdPool* pool ) +{ + return pool->nextIndex - pool->freeArray.count; +} + +B3_INLINE int b3GetIdCapacity( const b3IdPool* pool ) +{ + return pool->nextIndex; +} + +B3_INLINE int b3GetIdBytes( const b3IdPool* pool ) +{ + return b3Array_ByteCount( pool->freeArray ); +} diff --git a/src/island.c b/src/island.c new file mode 100644 index 0000000..625a1b3 --- /dev/null +++ b/src/island.c @@ -0,0 +1,735 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "island.h" + +#include "body.h" +#include "contact.h" +#include "core.h" +#include "id_pool.h" +#include "joint.h" +#include "physics_world.h" +#include "solver_set.h" + +#include + +b3Island* b3CreateIsland( b3World* world, int setIndex ) +{ + B3_ASSERT( setIndex == b3_awakeSet || setIndex >= b3_firstSleepingSet ); + + int islandId = b3AllocId( &world->islandIdPool ); + + if ( islandId == world->islands.count ) + { + b3Island emptyIsland = { 0 }; + b3Array_Push( world->islands, emptyIsland ); + } + else + { + B3_ASSERT( world->islands.data[islandId].setIndex == B3_NULL_INDEX ); + } + + b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + + b3Island* island = b3Array_Get( world->islands, islandId ); + island->setIndex = setIndex; + island->localIndex = set->islandSims.count; + island->islandId = islandId; + b3Array_Create( island->bodies ); + b3Array_Create( island->contacts ); + b3Array_Create( island->joints ); + island->constraintRemoveCount = 0; + + b3IslandSim* islandSim = b3Array_Emplace( set->islandSims ); + islandSim->islandId = islandId; + + return island; +} + +void b3DestroyIsland( b3World* world, int islandId ) +{ + if ( world->splitIslandId == islandId ) + { + world->splitIslandId = B3_NULL_INDEX; + } + + // assume island is empty + b3Island* island = b3Array_Get( world->islands, islandId ); + b3SolverSet* set = b3Array_Get( world->solverSets, island->setIndex ); + { + int localIndex = island->localIndex; + int lastIndex = set->islandSims.count - 1; + B3_ASSERT( 0 <= localIndex && localIndex <= lastIndex ); + int moveIslandId = set->islandSims.data[lastIndex].islandId; + set->islandSims.data[localIndex] = set->islandSims.data[lastIndex]; + world->islands.data[moveIslandId].localIndex = localIndex; + set->islandSims.count -= 1; + } + + // Free island and id (preserve island generation) + b3Array_Destroy( island->bodies ); + b3Array_Destroy( island->contacts ); + b3Array_Destroy( island->joints ); + island->constraintRemoveCount = 0; + island->localIndex = B3_NULL_INDEX; + island->islandId = B3_NULL_INDEX; + island->setIndex = B3_NULL_INDEX; + + b3FreeId( &world->islandIdPool, islandId ); +} + +static int b3MergeIslands( b3World* world, int islandIdA, int islandIdB ) +{ + if ( islandIdA == islandIdB ) + { + return islandIdA; + } + + if ( islandIdA == B3_NULL_INDEX ) + { + B3_ASSERT( islandIdB != B3_NULL_INDEX ); + return islandIdB; + } + + if ( islandIdB == B3_NULL_INDEX ) + { + B3_ASSERT( islandIdA != B3_NULL_INDEX ); + return islandIdA; + } + + b3Island* smallIsland; + b3Island* bigIsland; + { + b3Island* islandA = b3Array_Get( world->islands, islandIdA ); + b3Island* islandB = b3Array_Get( world->islands, islandIdB ); + + // Keep the biggest island to reduce cache misses + if ( islandA->bodies.count >= islandB->bodies.count ) + { + bigIsland = islandA; + smallIsland = islandB; + } + else + { + bigIsland = islandB; + smallIsland = islandA; + } + } + + int bigIslandId = bigIsland->islandId; + b3Array_Reserve( bigIsland->bodies, bigIsland->bodies.count + smallIsland->bodies.count ); + + // Move bodies from smaller island to larger island + for ( int i = 0; i < smallIsland->bodies.count; ++i ) + { + int bodyId = smallIsland->bodies.data[i]; + b3Body* body = b3Array_Get( world->bodies, bodyId ); + B3_VALIDATE( body->islandId == smallIsland->islandId ); + body->islandId = bigIslandId; + body->islandIndex = bigIsland->bodies.count; + b3Array_Push( bigIsland->bodies, bodyId ); + } + + // Migrate contacts from smaller island to larger island + if ( smallIsland->contacts.count > 0 ) + { + b3Array_Reserve( bigIsland->contacts, bigIsland->contacts.count + smallIsland->contacts.count ); + + for ( int i = 0; i < smallIsland->contacts.count; ++i ) + { + b3ContactLink* link = smallIsland->contacts.data + i; + b3Contact* contact = b3Array_Get( world->contacts, link->contactId ); + contact->islandId = bigIslandId; + contact->islandIndex = bigIsland->contacts.count; + b3Array_Push( bigIsland->contacts, *link ); + } + } + + // Migrate joints from smaller island to larger island + if ( smallIsland->joints.count > 0 ) + { + b3Array_Reserve( bigIsland->joints, bigIsland->joints.count + smallIsland->joints.count ); + + for ( int i = 0; i < smallIsland->joints.count; ++i ) + { + b3JointLink* link = smallIsland->joints.data + i; + b3Joint* joint = b3Array_Get( world->joints, link->jointId ); + joint->islandId = bigIslandId; + joint->islandIndex = bigIsland->joints.count; + b3Array_Push( bigIsland->joints, *link ); + } + } + + // Track removed constraints + bigIsland->constraintRemoveCount += smallIsland->constraintRemoveCount; + + b3DestroyIsland( world, smallIsland->islandId ); + + b3ValidateIsland( world, bigIslandId ); + + return bigIslandId; +} + +static void b3AddContactToIsland( b3World* world, int islandId, b3Contact* contact ) +{ + B3_ASSERT( contact->islandId == B3_NULL_INDEX ); + B3_ASSERT( contact->islandIndex == B3_NULL_INDEX ); + + b3Island* island = b3Array_Get( world->islands, islandId ); + + contact->islandId = islandId; + contact->islandIndex = island->contacts.count; + + b3ContactLink link; + link.contactId = contact->contactId; + link.bodyIdA = contact->edges[0].bodyId; + link.bodyIdB = contact->edges[1].bodyId; + b3Array_Push( island->contacts, link ); + + b3ValidateIsland( world, islandId ); +} + +// Link a contact into an island. +void b3LinkContact( b3World* world, b3Contact* contact ) +{ + B3_ASSERT( ( contact->flags & b3_contactTouchingFlag ) != 0 ); + + int bodyIdA = contact->edges[0].bodyId; + int bodyIdB = contact->edges[1].bodyId; + + b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB ); + + B3_ASSERT( bodyA->setIndex != b3_disabledSet && bodyB->setIndex != b3_disabledSet ); + B3_ASSERT( bodyA->setIndex != b3_staticSet || bodyB->setIndex != b3_staticSet ); + + // Wake bodyB if bodyA is awake and bodyB is sleeping + if ( bodyA->setIndex == b3_awakeSet && bodyB->setIndex >= b3_firstSleepingSet ) + { + b3WakeSolverSet( world, bodyB->setIndex ); + } + + // Wake bodyA if bodyB is awake and bodyA is sleeping + if ( bodyB->setIndex == b3_awakeSet && bodyA->setIndex >= b3_firstSleepingSet ) + { + b3WakeSolverSet( world, bodyA->setIndex ); + } + + int islandIdA = bodyA->islandId; + int islandIdB = bodyB->islandId; + + // Static bodies have null island indices. + B3_ASSERT( bodyA->setIndex != b3_staticSet || islandIdA == B3_NULL_INDEX ); + B3_ASSERT( bodyB->setIndex != b3_staticSet || islandIdB == B3_NULL_INDEX ); + B3_ASSERT( islandIdA != B3_NULL_INDEX || islandIdB != B3_NULL_INDEX ); + + // Merge islands. This will destroy one of the islands. + int finalIslandId = b3MergeIslands( world, islandIdA, islandIdB ); + + // Add contact to the island that survived + b3AddContactToIsland( world, finalIslandId, contact ); +} + +// This is called when a contact no longer has contact points or when a contact is destroyed. +void b3UnlinkContact( b3World* world, b3Contact* contact ) +{ + B3_ASSERT( contact->islandId != B3_NULL_INDEX ); + + // remove from island + int islandId = contact->islandId; + b3Island* island = b3Array_Get( world->islands, islandId ); + + int removeIndex = contact->islandIndex; + B3_ASSERT( 0 <= removeIndex && removeIndex < island->contacts.count ); + B3_ASSERT( island->contacts.data[removeIndex].contactId == contact->contactId ); + + int movedIndex = b3Array_RemoveSwap( island->contacts, removeIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fix islandIndex on the contact that was swapped into removeIndex + b3ContactLink* movedLink = island->contacts.data + removeIndex; + b3Contact* movedContact = b3Array_Get( world->contacts, movedLink->contactId ); + B3_ASSERT( movedContact->islandIndex == movedIndex ); + movedContact->islandIndex = removeIndex; + } + + contact->islandId = B3_NULL_INDEX; + contact->islandIndex = B3_NULL_INDEX; + island->constraintRemoveCount += 1; + + b3ValidateIsland( world, islandId ); +} + +static void b3AddJointToIsland( b3World* world, int islandId, b3Joint* joint ) +{ + B3_ASSERT( joint->islandId == B3_NULL_INDEX ); + B3_ASSERT( joint->islandIndex == B3_NULL_INDEX ); + + b3Island* island = b3Array_Get( world->islands, islandId ); + + joint->islandId = islandId; + joint->islandIndex = island->joints.count; + + b3JointLink link; + link.jointId = joint->jointId; + link.bodyIdA = joint->edges[0].bodyId; + link.bodyIdB = joint->edges[1].bodyId; + b3Array_Push( island->joints, link ); + + b3ValidateIsland( world, islandId ); +} + +void b3LinkJoint( b3World* world, b3Joint* joint ) +{ + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + + B3_ASSERT( bodyA->type == b3_dynamicBody || bodyB->type == b3_dynamicBody ); + + if ( bodyA->setIndex == b3_awakeSet && bodyB->setIndex >= b3_firstSleepingSet ) + { + b3WakeSolverSet( world, bodyB->setIndex ); + } + else if ( bodyB->setIndex == b3_awakeSet && bodyA->setIndex >= b3_firstSleepingSet ) + { + b3WakeSolverSet( world, bodyA->setIndex ); + } + + int islandIdA = bodyA->islandId; + int islandIdB = bodyB->islandId; + + B3_ASSERT( islandIdA != B3_NULL_INDEX || islandIdB != B3_NULL_INDEX ); + + // Merge islands. This will destroy one of the islands. + int finalIslandId = b3MergeIslands( world, islandIdA, islandIdB ); + + // Add joint the island that survived + b3AddJointToIsland( world, finalIslandId, joint ); +} + +void b3UnlinkJoint( b3World* world, b3Joint* joint ) +{ + if ( joint->islandId == B3_NULL_INDEX ) + { + return; + } + + // remove from island + int islandId = joint->islandId; + b3Island* island = b3Array_Get( world->islands, islandId ); + + int removeIndex = joint->islandIndex; + B3_ASSERT( 0 <= removeIndex && removeIndex < island->joints.count ); + B3_ASSERT( island->joints.data[removeIndex].jointId == joint->jointId ); + + int movedIndex = b3Array_RemoveSwap( island->joints, removeIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fix islandIndex on the joint that was swapped into removeIndex + b3JointLink* movedLink = island->joints.data + removeIndex; + b3Joint* movedJoint = b3Array_Get( world->joints, movedLink->jointId ); + B3_ASSERT( movedJoint->islandIndex == movedIndex ); + movedJoint->islandIndex = removeIndex; + } + + joint->islandId = B3_NULL_INDEX; + joint->islandIndex = B3_NULL_INDEX; + island->constraintRemoveCount += 1; + + b3ValidateIsland( world, islandId ); +} + +// Find parent of a node. Use path halving to speed up further queries. +static inline int b3IslandFindParent( int* parents, int node ) +{ + // Walk the chain of parents to find the node that is its own parent (the root) + while ( parents[node] != node ) + { + int grandParent = parents[parents[node]]; + parents[node] = grandParent; + node = grandParent; + } + + return node; +} + +// Connect the components containing node1 and node2. +// Uses rank to keep tree balanced. Tracks per-component contact and joint counts. +static inline void b3IslandUnion( int* parents, int* ranks, int node1, int node2, int* contactCounts, int* jointCounts ) +{ + int root1 = b3IslandFindParent( parents, node1 ); + int root2 = b3IslandFindParent( parents, node2 ); + if ( root1 != root2 ) + { + if ( ranks[root1] < ranks[root2] ) + { + parents[root1] = root2; + contactCounts[root2] += contactCounts[root1]; + jointCounts[root2] += jointCounts[root1]; + } + else if ( ranks[root1] > ranks[root2] ) + { + parents[root2] = root1; + contactCounts[root1] += contactCounts[root2]; + jointCounts[root1] += jointCounts[root2]; + } + else + { + parents[root2] = root1; + ranks[root1] += 1; + contactCounts[root1] += contactCounts[root2]; + jointCounts[root1] += jointCounts[root2]; + } + } +} + +// This uses union-find. +// https://en.wikipedia.org/wiki/Disjoint-set_data_structure +void b3SplitIsland( b3World* world, int baseId ) +{ + b3Island* baseIsland = b3Array_Get( world->islands, baseId ); + B3_ASSERT( baseIsland->constraintRemoveCount > 0 ); + B3_ASSERT( baseIsland->setIndex == b3_awakeSet ); + + b3ValidateIsland( world, baseId ); + + // Cache base island fields before b3CreateIsland, which may reallocate + // world->islands and invalidate the baseIsland pointer. + int baseBodyCount = baseIsland->bodies.count; + int* baseBodyIds = baseIsland->bodies.data; + int baseBodyCapacity = baseIsland->bodies.capacity; + + int baseContactCount = baseIsland->contacts.count; + b3ContactLink* baseContacts = baseIsland->contacts.data; + int baseContactCapacity = baseIsland->contacts.capacity; + + int baseJointCount = baseIsland->joints.count; + b3JointLink* baseJoints = baseIsland->joints.data; + int baseJointCapacity = baseIsland->joints.capacity; + + b3Stack* alloc = &world->stack; + + // No lock is needed because I ensure the allocator is not used while this task is active. + // Allocate contactCounts and jointCounts before ranks so ranks can be freed first (LIFO arena). + int* parents = b3StackAlloc( alloc, baseBodyCount * sizeof( int ), "parents" ); + int* contactCounts = b3StackAlloc( alloc, baseBodyCount * sizeof( int ), "contact counts" ); + int* jointCounts = b3StackAlloc( alloc, baseBodyCount * sizeof( int ), "joint counts" ); + int* ranks = b3StackAlloc( alloc, baseBodyCount * sizeof( int ), "ranks" ); + for ( int i = 0; i < baseBodyCount; ++i ) + { + parents[i] = i; + ranks[i] = 0; + contactCounts[i] = 0; + jointCounts[i] = 0; + } + + b3Body* bodies = world->bodies.data; + + // Union over contacts, tracking per-component contact counts + for ( int i = 0; i < baseContactCount; ++i ) + { + int bodyIdA = baseContacts[i].bodyIdA; + int bodyIdB = baseContacts[i].bodyIdB; + B3_VALIDATE( 0 <= bodyIdA && bodyIdA < world->bodies.count ); + B3_VALIDATE( 0 <= bodyIdB && bodyIdB < world->bodies.count ); + b3Body* bodyA = bodies + bodyIdA; + b3Body* bodyB = bodies + bodyIdB; + int islandIndexA = bodyA->islandIndex; + int islandIndexB = bodyB->islandIndex; + + // Only connect non-static bodies + if ( islandIndexA != B3_NULL_INDEX && islandIndexB != B3_NULL_INDEX ) + { + B3_VALIDATE( 0 <= islandIndexA && islandIndexA < baseBodyCount ); + B3_VALIDATE( 0 <= islandIndexB && islandIndexB < baseBodyCount ); + b3IslandUnion( parents, ranks, islandIndexA, islandIndexB, contactCounts, jointCounts ); + int root = b3IslandFindParent( parents, islandIndexA ); + contactCounts[root] += 1; + } + else + { + int islandIndex = islandIndexA != B3_NULL_INDEX ? islandIndexA : islandIndexB; + int root = b3IslandFindParent( parents, islandIndex ); + contactCounts[root] += 1; + } + } + + // Union over joints, tracking per-component joint counts + for ( int i = 0; i < baseJointCount; ++i ) + { + int bodyIdA = baseJoints[i].bodyIdA; + int bodyIdB = baseJoints[i].bodyIdB; + B3_VALIDATE( 0 <= bodyIdA && bodyIdA < world->bodies.count ); + B3_VALIDATE( 0 <= bodyIdB && bodyIdB < world->bodies.count ); + b3Body* bodyA = bodies + bodyIdA; + b3Body* bodyB = bodies + bodyIdB; + int islandIndexA = bodyA->islandIndex; + int islandIndexB = bodyB->islandIndex; + + // Only connect non-static bodies + if ( islandIndexA != B3_NULL_INDEX && islandIndexB != B3_NULL_INDEX ) + { + B3_VALIDATE( 0 <= islandIndexA && islandIndexA < baseBodyCount ); + B3_VALIDATE( 0 <= islandIndexB && islandIndexB < baseBodyCount ); + b3IslandUnion( parents, ranks, islandIndexA, islandIndexB, contactCounts, jointCounts ); + int root = b3IslandFindParent( parents, islandIndexA ); + jointCounts[root] += 1; + } + else + { + int islandIndex = islandIndexA != B3_NULL_INDEX ? islandIndexA : islandIndexB; + int root = b3IslandFindParent( parents, islandIndex ); + jointCounts[root] += 1; + } + } + + // Done with ranks + b3StackFree( alloc, ranks ); + ranks = NULL; + + // Flatten all parent indices and count connected components. + int componentCount = 0; + for ( int i = 0; i < baseBodyCount; ++i ) + { + parents[i] = b3IslandFindParent( parents, i ); + if ( parents[i] == i ) + { + componentCount += 1; + } + } + + // Early return — island is still fully connected, no split needed. + if ( componentCount == 1 ) + { + baseIsland->constraintRemoveCount = 0; + b3StackFree( alloc, jointCounts ); + b3StackFree( alloc, contactCounts ); + b3StackFree( alloc, parents ); + return; + } + + // Detach body/contact/joint arrays from base island so b3DestroyIsland won't free them + baseIsland->bodies.data = NULL; + baseIsland->bodies.count = 0; + baseIsland->bodies.capacity = 0; + + baseIsland->contacts.data = NULL; + baseIsland->contacts.count = 0; + baseIsland->contacts.capacity = 0; + + baseIsland->joints.data = NULL; + baseIsland->joints.count = 0; + baseIsland->joints.capacity = 0; + + // Null so code below doesn't accidentally use this. + baseIsland = NULL; + + // Map from body index to new island index. Only set for root bodies. + int* rootMap = b3StackAlloc( alloc, baseBodyCount * sizeof( int ), "root map" ); + for ( int i = 0; i < baseBodyCount; ++i ) + { + rootMap[i] = B3_NULL_INDEX; + } + + int* componentBodyCounts = b3StackAlloc( alloc, componentCount * sizeof( int ), "component body counts" ); + int* componentContactCounts = b3StackAlloc( alloc, componentCount * sizeof( int ), "component contact counts" ); + int* componentJointCounts = b3StackAlloc( alloc, componentCount * sizeof( int ), "component joint counts" ); + int islandCount = 0; + + // Find the root body for each body and create islands as needed. + // Extract per-component counts from the root nodes' accumulated counts. + for ( int i = 0; i < baseBodyCount; ++i ) + { + int rootIndex = parents[i]; + if ( rootMap[rootIndex] == B3_NULL_INDEX ) + { + rootMap[rootIndex] = islandCount; + componentBodyCounts[islandCount] = 0; + componentContactCounts[islandCount] = contactCounts[rootIndex]; + componentJointCounts[islandCount] = jointCounts[rootIndex]; + islandCount += 1; + } + + componentBodyCounts[rootMap[rootIndex]] += 1; + } + + B3_ASSERT( islandCount == componentCount ); + + // Map from new island index to island id + int* islandIds = b3StackAlloc( alloc, islandCount * sizeof( int ), "island ids" ); + + // Create new islands and reserve body/contact/joint arrays + for ( int i = 0; i < islandCount; ++i ) + { + // WARNING: this invalidates baseIsland pointer + b3Island* newIsland = b3CreateIsland( world, b3_awakeSet ); + islandIds[i] = newIsland->islandId; + + // Reserve arrays to avoid wasteful growth and memcpy. + b3Array_Reserve( newIsland->bodies, componentBodyCounts[i] ); + b3Array_Reserve( newIsland->contacts, componentContactCounts[i] ); + b3Array_Reserve( newIsland->joints, componentJointCounts[i] ); + } + + // Assign bodies to new islands + for ( int i = 0; i < baseBodyCount; ++i ) + { + int bodyId = baseBodyIds[i]; + int root = b3IslandFindParent( parents, i ); + int newIslandId = islandIds[rootMap[root]]; + + b3Body* body = b3Array_Get( world->bodies, bodyId ); + b3Island* newIsland = b3Array_Get( world->islands, newIslandId ); + + body->islandId = newIslandId; + body->islandIndex = newIsland->bodies.count; + + // Ensure the array has the correct capacity + B3_VALIDATE( newIsland->bodies.count < newIsland->bodies.capacity ); + b3Array_Push( newIsland->bodies, bodyId ); + } + + // Assign contacts to the island of their bodies + for ( int i = 0; i < baseContactCount; ++i ) + { + b3ContactLink* link = baseContacts + i; + b3Contact* contact = b3Array_Get( world->contacts, link->contactId ); + + // Static bodies don't have an island id. + b3Body* bodyA = b3Array_Get( world->bodies, link->bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, link->bodyIdB ); + int targetIslandId = bodyA->islandId != B3_NULL_INDEX ? bodyA->islandId : bodyB->islandId; + + b3Island* targetIsland = b3Array_Get( world->islands, targetIslandId ); + contact->islandId = targetIslandId; + contact->islandIndex = targetIsland->contacts.count; + + // Ensure the array has the correct capacity + B3_VALIDATE( targetIsland->contacts.count < targetIsland->contacts.capacity ); + b3Array_Push( targetIsland->contacts, *link ); + } + + // Assign joints to the island of their bodies + for ( int i = 0; i < baseJointCount; ++i ) + { + b3JointLink* link = baseJoints + i; + b3Joint* joint = b3Array_Get( world->joints, link->jointId ); + + // Static bodies don't have an island id. + b3Body* bodyA = b3Array_Get( world->bodies, link->bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, link->bodyIdB ); + int targetIslandId = bodyA->islandId != B3_NULL_INDEX ? bodyA->islandId : bodyB->islandId; + + b3Island* targetIsland = b3Array_Get( world->islands, targetIslandId ); + joint->islandId = targetIslandId; + joint->islandIndex = targetIsland->joints.count; + + // Ensure the array has the correct capacity + B3_VALIDATE( targetIsland->joints.count < targetIsland->joints.capacity ); + b3Array_Push( targetIsland->joints, *link ); + } + + // Destroy the base island + b3DestroyIsland( world, baseId ); + + // Free the detached arrays manually + b3Free( baseBodyIds, baseBodyCapacity * sizeof( int ) ); + b3Free( baseContacts, baseContactCapacity * sizeof( b3ContactLink ) ); + b3Free( baseJoints, baseJointCapacity * sizeof( b3JointLink ) ); + + // Free arena items in LIFO order + b3StackFree( alloc, islandIds ); + b3StackFree( alloc, componentJointCounts ); + b3StackFree( alloc, componentContactCounts ); + b3StackFree( alloc, componentBodyCounts ); + b3StackFree( alloc, rootMap ); + b3StackFree( alloc, jointCounts ); + b3StackFree( alloc, contactCounts ); + b3StackFree( alloc, parents ); +} + +// Split an island because some contacts and/or joints have been removed. +// This is called during the constraint solve while islands are not being touched. This uses union find and +// touches a lot of memory, so it can be slow. +// Note: contacts/joints connected to static bodies must belong to an island but don't affect island connectivity +// Note: static bodies are never in an island +// Note: this task interacts with some allocators without locks under the assumption that no other tasks +// are interacting with these data structures. +void b3SplitIslandTask( void* context ) +{ + b3TracyCZoneNC( split, "Split Island", b3_colorOlive, true ); + + uint64_t ticks = b3GetTicks(); + b3World* world = context; + + B3_ASSERT( world->splitIslandId != B3_NULL_INDEX ); + + b3SplitIsland( world, world->splitIslandId ); + + world->splitIslandId = B3_NULL_INDEX; + world->profile.splitIslands += b3GetMilliseconds( ticks ); + b3TracyCZoneEnd( split ); +} + +#if B3_ENABLE_VALIDATION +void b3ValidateIsland( b3World* world, int islandId ) +{ + if ( islandId == B3_NULL_INDEX ) + { + return; + } + + b3Island* island = b3Array_Get( world->islands, islandId ); + B3_ASSERT( island->islandId == islandId ); + B3_ASSERT( island->setIndex != B3_NULL_INDEX ); + + { + B3_ASSERT( island->bodies.count > 0 ); + B3_ASSERT( island->bodies.count <= b3GetIdCount( &world->bodyIdPool ) ); + + for ( int i = 0; i < island->bodies.count; ++i ) + { + b3Body* body = b3Array_Get( world->bodies, island->bodies.data[i] ); + B3_ASSERT( body->islandId == islandId ); + B3_ASSERT( body->islandIndex == i ); + B3_ASSERT( body->setIndex == island->setIndex ); + } + } + + if ( island->contacts.count > 0 ) + { + B3_ASSERT( island->contacts.count <= b3GetIdCount( &world->contactIdPool ) ); + + for ( int i = 0; i < island->contacts.count; ++i ) + { + b3ContactLink* link = island->contacts.data + i; + b3Contact* contact = b3Array_Get( world->contacts, link->contactId ); + B3_ASSERT( contact->setIndex == island->setIndex ); + B3_ASSERT( contact->islandId == islandId ); + B3_ASSERT( contact->islandIndex == i ); + } + } + + if ( island->joints.count > 0 ) + { + B3_ASSERT( island->joints.count <= b3GetIdCount( &world->jointIdPool ) ); + + for ( int i = 0; i < island->joints.count; ++i ) + { + b3JointLink* link = island->joints.data + i; + b3Joint* joint = b3Array_Get( world->joints, link->jointId ); + B3_ASSERT( joint->setIndex == island->setIndex ); + B3_ASSERT( joint->islandId == islandId ); + B3_ASSERT( joint->islandIndex == i ); + } + } +} + +#else + +void b3ValidateIsland( b3World* world, int islandId ) +{ + B3_UNUSED( world ); + B3_UNUSED( islandId ); +} +#endif diff --git a/src/island.h b/src/island.h new file mode 100644 index 0000000..d37bb09 --- /dev/null +++ b/src/island.h @@ -0,0 +1,110 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "container.h" + +#include + +typedef struct b3Contact b3Contact; +typedef struct b3Joint b3Joint; +typedef struct b3World b3World; + +// Cached contact data stored in the island for fast contiguous iteration. +// Avoids touching b3Contact during union-find in b3SplitIsland. +typedef struct b3ContactLink +{ + int contactId; + int bodyIdA; + int bodyIdB; +} b3ContactLink; + +b3DeclareArray( b3ContactLink ); + +// Cached joint data stored in the island for fast contiguous iteration. +typedef struct b3JointLink +{ + int jointId; + int bodyIdA; + int bodyIdB; +} b3JointLink; + +b3DeclareArray( b3JointLink ); + +// Deterministic solver +// +// Collide all awake contacts +// Use bit array to emit start/stop touching events in defined order, per thread. Try using contact index, assuming contacts are +// created in a deterministic order. bit-wise OR together bit arrays and issue changes: +// - start touching: merge islands - temporary linked list - mark root island dirty - wake all - largest island is root +// - stop touching: increment constraintRemoveCount + +// Persistent island for awake bodies, joints, and contacts. +// Contacts are touching. +// Contacts and joints may connect to static bodies, but static bodies are not in the island. +// https://en.wikipedia.org/wiki/Component_(graph_theory) +// https://en.wikipedia.org/wiki/Dynamic_connectivity +typedef struct b3Island +{ + // index of solver set stored in b3World + // may be B3_NULL_INDEX + int setIndex; + + // island index within set + // may be B3_NULL_INDEX + int localIndex; + + int islandId; + + // Keeps track of how many contacts have been removed from this island. + // This is used to determine if an island is a candidate for splitting. + int constraintRemoveCount; + + // I tried using a stack array for this but the data pointer goes out of + // sync when the world island array grows. + b3ArrayC( int ) bodies; + + // Contacts and joints that belong to this island. May connect to static + // bodies not in the island. + // Each link has the two body ids so that b3SplitIsland's union-find pass + // never needs to touch b3Contact/b3Joint. + b3ArrayC( b3ContactLink ) contacts; + b3ArrayC( b3JointLink ) joints; + +} b3Island; + +// This is used to move islands across solver sets +typedef struct b3IslandSim +{ + int islandId; +} b3IslandSim; + +#ifdef __cplusplus +extern "C" +{ +#endif + +b3Island* b3CreateIsland( b3World* world, int setIndex ); +void b3DestroyIsland( b3World* world, int islandId ); + +// Link contacts into the island graph when it starts having contact points +void b3LinkContact( b3World* world, b3Contact* contact ); + +// Unlink contact from the island graph when it stops having contact points +void b3UnlinkContact( b3World* world, b3Contact* contact ); + +// Link a joint into the island graph when it is created +void b3LinkJoint( b3World* world, b3Joint* joint ); + +// Unlink a joint from the island graph when it is destroyed +void b3UnlinkJoint( b3World* world, b3Joint* joint ); + +void b3SplitIsland( b3World* world, int baseId ); +void b3SplitIslandTask( void* context ); + +void b3ValidateIsland( b3World* world, int islandId ); + +#ifdef __cplusplus +} +#endif diff --git a/src/joint.c b/src/joint.c new file mode 100644 index 0000000..3a7e832 --- /dev/null +++ b/src/joint.c @@ -0,0 +1,1725 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "joint.h" + +#include "body.h" +#include "contact.h" +#include "core.h" +#include "island.h" +#include "physics_world.h" +#include "shape.h" +#include "solver.h" +#include "solver_set.h" + +// needed for dll export +#include "box3d/box3d.h" + +#include +#include +#include + +static b3JointDef b3DefaultJointDef( void ) +{ + b3JointDef def = { 0 }; + def.localFrameA.q = b3Quat_identity; + def.localFrameB.q = b3Quat_identity; + def.forceThreshold = FLT_MAX; + def.torqueThreshold = FLT_MAX; + def.constraintHertz = 60.0f; + def.constraintDampingRatio = 2.0f; + def.drawScale = b3GetLengthUnitsPerMeter(); + def.internalValue = B3_SECRET_COOKIE; + return def; +} + +b3ParallelJointDef b3DefaultParallelJointDef( void ) +{ + b3ParallelJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + def.hertz = 1.0f; + def.dampingRatio = 1.0f; + def.maxTorque = FLT_MAX; + return def; +} + +b3DistanceJointDef b3DefaultDistanceJointDef( void ) +{ + b3DistanceJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + def.lowerSpringForce = -FLT_MAX; + def.upperSpringForce = FLT_MAX; + def.length = 1.0f; + def.maxLength = B3_HUGE; + return def; +} + +b3MotorJointDef b3DefaultMotorJointDef( void ) +{ + b3MotorJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + return def; +} + +b3FilterJointDef b3DefaultFilterJointDef( void ) +{ + b3FilterJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + return def; +} + +b3PrismaticJointDef b3DefaultPrismaticJointDef( void ) +{ + b3PrismaticJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + return def; +} + +b3RevoluteJointDef b3DefaultRevoluteJointDef( void ) +{ + b3RevoluteJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + return def; +} + +b3SphericalJointDef b3DefaultSphericalJointDef( void ) +{ + b3SphericalJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + def.targetRotation = b3Quat_identity; + return def; +} + +b3WeldJointDef b3DefaultWeldJointDef( void ) +{ + b3WeldJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + return def; +} + +b3WheelJointDef b3DefaultWheelJointDef( void ) +{ + b3WheelJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + def.enableSuspensionSpring = true; + def.suspensionHertz = 1.0f; + def.suspensionDampingRatio = 0.7f; + def.steeringHertz = 1.0f; + def.steeringDampingRatio = 0.7f; + return def; +} + +b3ExplosionDef b3DefaultExplosionDef( void ) +{ + b3ExplosionDef def = { 0 }; + def.maskBits = B3_DEFAULT_MASK_BITS; + return def; +} + +b3Joint* b3GetJointFullId( b3World* world, b3JointId jointId ) +{ + int id = jointId.index1 - 1; + b3Joint* joint = b3Array_Get( world->joints, id ); + B3_ASSERT( joint->jointId == id && joint->generation == jointId.generation ); + return joint; +} + +b3JointSim* b3GetJointSim( b3World* world, b3Joint* joint ) +{ + if ( joint->setIndex == b3_awakeSet ) + { + B3_ASSERT( 0 <= joint->colorIndex && joint->colorIndex < B3_GRAPH_COLOR_COUNT ); + b3GraphColor* color = world->constraintGraph.colors + joint->colorIndex; + return b3Array_Get( color->jointSims, joint->localIndex ); + } + + b3SolverSet* set = b3Array_Get( world->solverSets, joint->setIndex ); + return b3Array_Get( set->jointSims, joint->localIndex ); +} + +b3JointSim* b3GetJointSimCheckType( b3JointId jointId, b3JointType type ) +{ + B3_UNUSED( type ); + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + B3_ASSERT( joint->type == type ); + b3JointSim* jointSim = b3GetJointSim( world, joint ); + B3_ASSERT( jointSim->type == type ); + return jointSim; +} + +typedef struct b3JointPair +{ + b3Joint* joint; + b3JointSim* jointSim; +} b3JointPair; + +static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3JointType type ) +{ + B3_ASSERT( b3IsValidTransform( def->localFrameA ) ); + B3_ASSERT( b3IsValidTransform( def->localFrameB ) ); + + b3Body* bodyA = b3GetBodyFullId( world, def->bodyIdA ); + b3Body* bodyB = b3GetBodyFullId( world, def->bodyIdB ); + + int bodyIdA = bodyA->id; + int bodyIdB = bodyB->id; + int maxSetIndex = b3MaxInt( bodyA->setIndex, bodyB->setIndex ); + + // Create joint id and joint + int jointId = b3AllocId( &world->jointIdPool ); + if ( jointId == world->joints.count ) + { + b3Array_Push( world->joints, (b3Joint){ 0 } ); + } + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + joint->jointId = jointId; + joint->userData = def->userData; + joint->generation += 1; + joint->setIndex = B3_NULL_INDEX; + joint->colorIndex = B3_NULL_INDEX; + joint->localIndex = B3_NULL_INDEX; + joint->islandId = B3_NULL_INDEX; + joint->islandIndex = B3_NULL_INDEX; + joint->drawScale = def->drawScale; + joint->type = type; + joint->collideConnected = def->collideConnected; + + // Doubly linked list on bodyA + joint->edges[0].bodyId = bodyIdA; + joint->edges[0].prevKey = B3_NULL_INDEX; + joint->edges[0].nextKey = bodyA->headJointKey; + + int keyA = ( jointId << 1 ) | 0; + if ( bodyA->headJointKey != B3_NULL_INDEX ) + { + b3Joint* jointA = b3Array_Get( world->joints, bodyA->headJointKey >> 1 ); + b3JointEdge* edgeA = jointA->edges + ( bodyA->headJointKey & 1 ); + edgeA->prevKey = keyA; + } + bodyA->headJointKey = keyA; + bodyA->jointCount += 1; + + // Doubly linked list on bodyB + joint->edges[1].bodyId = bodyIdB; + joint->edges[1].prevKey = B3_NULL_INDEX; + joint->edges[1].nextKey = bodyB->headJointKey; + + int keyB = ( jointId << 1 ) | 1; + if ( bodyB->headJointKey != B3_NULL_INDEX ) + { + b3Joint* jointB = b3Array_Get( world->joints, bodyB->headJointKey >> 1 ); + b3JointEdge* edgeB = jointB->edges + ( bodyB->headJointKey & 1 ); + edgeB->prevKey = keyB; + } + bodyB->headJointKey = keyB; + bodyB->jointCount += 1; + + b3JointSim* jointSim; + + if ( bodyA->setIndex == b3_disabledSet || bodyB->setIndex == b3_disabledSet ) + { + // if either body is disabled, create in disabled set + b3SolverSet* set = b3Array_Get( world->solverSets, b3_disabledSet ); + joint->setIndex = b3_disabledSet; + joint->localIndex = set->jointSims.count; + + jointSim = b3Array_Emplace( set->jointSims ); + memset( jointSim, 0, sizeof( b3JointSim ) ); + + jointSim->jointId = jointId; + jointSim->bodyIdA = bodyIdA; + jointSim->bodyIdB = bodyIdB; + } + else if ( bodyA->type != b3_dynamicBody && bodyB->type != b3_dynamicBody ) + { + // joint is not attached to a dynamic body + b3SolverSet* set = b3Array_Get( world->solverSets, b3_staticSet ); + joint->setIndex = b3_staticSet; + joint->localIndex = set->jointSims.count; + + jointSim = b3Array_Emplace( set->jointSims ); + memset( jointSim, 0, sizeof( b3JointSim ) ); + + jointSim->jointId = jointId; + jointSim->bodyIdA = bodyIdA; + jointSim->bodyIdB = bodyIdB; + } + else if ( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ) + { + // if either body is sleeping, wake it + if ( maxSetIndex >= b3_firstSleepingSet ) + { + b3WakeSolverSet( world, maxSetIndex ); + } + + joint->setIndex = b3_awakeSet; + + jointSim = b3CreateJointInGraph( world, joint ); + jointSim->jointId = jointId; + jointSim->bodyIdA = bodyIdA; + jointSim->bodyIdB = bodyIdB; + } + else + { + // joint connected between sleeping and/or static bodies + B3_ASSERT( bodyA->setIndex >= b3_firstSleepingSet || bodyB->setIndex >= b3_firstSleepingSet ); + B3_ASSERT( bodyA->setIndex != b3_staticSet || bodyB->setIndex != b3_staticSet ); + + // joint should go into the sleeping set (not static set) + int setIndex = maxSetIndex; + + b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + joint->setIndex = setIndex; + joint->localIndex = set->jointSims.count; + + jointSim = b3Array_Emplace( set->jointSims ); + memset( jointSim, 0, sizeof( b3JointSim ) ); + + jointSim->jointId = jointId; + jointSim->bodyIdA = bodyIdA; + jointSim->bodyIdB = bodyIdB; + + if ( bodyA->setIndex != bodyB->setIndex && bodyA->setIndex >= b3_firstSleepingSet && + bodyB->setIndex >= b3_firstSleepingSet ) + { + // merge sleeping sets + b3MergeSolverSets( world, bodyA->setIndex, bodyB->setIndex ); + B3_ASSERT( bodyA->setIndex == bodyB->setIndex ); + + // fix potentially invalid set index + setIndex = bodyA->setIndex; + + b3SolverSet* mergedSet = b3Array_Get( world->solverSets, setIndex ); + + // Careful! The joint sim pointer was orphaned by the set merge. + jointSim = b3Array_Get( mergedSet->jointSims, joint->localIndex ); + } + + B3_ASSERT( joint->setIndex == setIndex ); + } + + jointSim->localFrameA = def->localFrameA; + jointSim->localFrameB = def->localFrameB; + jointSim->type = type; + jointSim->constraintHertz = def->constraintHertz; + jointSim->constraintDampingRatio = def->constraintDampingRatio; + jointSim->constraintSoftness = (b3Softness){ + .biasRate = 0.0f, + .massScale = 1.0f, + .impulseScale = 0.0f, + }; + + B3_ASSERT( b3IsValidFloat( def->forceThreshold ) && def->forceThreshold >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->torqueThreshold ) && def->torqueThreshold >= 0.0f ); + + jointSim->forceThreshold = def->forceThreshold; + jointSim->torqueThreshold = def->torqueThreshold; + + B3_ASSERT( jointSim->jointId == jointId ); + B3_ASSERT( jointSim->bodyIdA == bodyIdA ); + B3_ASSERT( jointSim->bodyIdB == bodyIdB ); + + if ( joint->setIndex > b3_disabledSet ) + { + // Add edge to island graph + b3LinkJoint( world, joint ); + } + + b3ValidateSolverSets( world ); + + return (b3JointPair){ joint, jointSim }; +} + +static void b3DestroyContactsBetweenBodies( b3World* world, b3Body* bodyA, b3Body* bodyB ) +{ + int contactKey; + int otherBodyId; + + // use the smaller of the two contact lists + if ( bodyA->contactCount < bodyB->contactCount ) + { + contactKey = bodyA->headContactKey; + otherBodyId = bodyB->id; + } + else + { + contactKey = bodyB->headContactKey; + otherBodyId = bodyA->id; + } + + // no need to wake bodies when a joint removes collision between them + bool wakeBodies = false; + + // destroy the contacts + while ( contactKey != B3_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + int otherEdgeIndex = edgeIndex ^ 1; + if ( contact->edges[otherEdgeIndex].bodyId == otherBodyId ) + { + // Careful, this removes the contact from the current doubly linked list + b3DestroyContact( world, contact, wakeBodies ); + } + } + + b3ValidateSolverSets( world ); +} + +void b3Joint_SetConstraintTuning( b3JointId jointId, float hertz, float dampingRatio ) +{ + B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f ); + B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f ); + + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + base->constraintHertz = hertz; + base->constraintDampingRatio = dampingRatio; +} + +void b3Joint_GetConstraintTuning( b3JointId jointId, float* hertz, float* dampingRatio ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + *hertz = base->constraintHertz; + *dampingRatio = base->constraintDampingRatio; +} + +void b3Joint_SetForceThreshold( b3JointId jointId, float threshold ) +{ + B3_ASSERT( b3IsValidFloat( threshold ) && threshold >= 0.0f ); + + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + base->forceThreshold = threshold; +} + +float b3Joint_GetForceThreshold( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + return base->forceThreshold; +} + +void b3Joint_SetTorqueThreshold( b3JointId jointId, float threshold ) +{ + B3_ASSERT( b3IsValidFloat( threshold ) && threshold >= 0.0f ); + + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + base->torqueThreshold = threshold; +} + +float b3Joint_GetTorqueThreshold( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + return base->torqueThreshold; +} + +b3JointId b3CreateDistanceJoint( b3WorldId worldId, const b3DistanceJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + B3_ASSERT( b3IsValidFloat( def->length ) && def->length > 0.0f ); + B3_ASSERT( def->lowerSpringForce <= def->upperSpringForce ); + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_distanceJoint ); + + b3JointSim* joint = pair.jointSim; + + joint->distanceJoint = (b3DistanceJoint){ 0 }; + joint->distanceJoint.length = b3MaxFloat( def->length, B3_LINEAR_SLOP ); + joint->distanceJoint.hertz = def->hertz; + joint->distanceJoint.dampingRatio = def->dampingRatio; + joint->distanceJoint.lowerSpringForce = def->lowerSpringForce; + joint->distanceJoint.upperSpringForce = def->upperSpringForce; + joint->distanceJoint.minLength = b3MaxFloat( def->minLength, B3_LINEAR_SLOP ); + joint->distanceJoint.maxLength = b3MaxFloat( def->minLength, def->maxLength ); + joint->distanceJoint.maxMotorForce = def->maxMotorForce; + joint->distanceJoint.motorSpeed = def->motorSpeed; + joint->distanceJoint.enableSpring = def->enableSpring; + joint->distanceJoint.enableLimit = def->enableLimit; + joint->distanceJoint.enableMotor = def->enableMotor; + joint->distanceJoint.impulse = 0.0f; + joint->distanceJoint.lowerImpulse = 0.0f; + joint->distanceJoint.upperImpulse = 0.0f; + joint->distanceJoint.motorImpulse = 0.0f; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + return jointId; +} + +b3JointId b3CreateMotorJoint( b3WorldId worldId, const b3MotorJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_motorJoint ); + b3JointSim* joint = pair.jointSim; + + joint->motorJoint = (b3MotorJoint){ 0 }; + joint->motorJoint.linearVelocity = def->linearVelocity; + joint->motorJoint.maxVelocityForce = def->maxVelocityForce; + joint->motorJoint.angularVelocity = def->angularVelocity; + joint->motorJoint.maxVelocityTorque = def->maxVelocityTorque; + joint->motorJoint.linearHertz = def->linearHertz; + joint->motorJoint.linearDampingRatio = def->linearDampingRatio; + joint->motorJoint.maxSpringForce = def->maxSpringForce; + joint->motorJoint.angularHertz = def->angularHertz; + joint->motorJoint.angularDampingRatio = def->angularDampingRatio; + joint->motorJoint.maxSpringTorque = def->maxSpringTorque; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + return jointId; +} + +b3JointId b3CreateFilterJoint( b3WorldId worldId, const b3FilterJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_filterJoint ); + + b3JointSim* joint = pair.jointSim; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + return jointId; +} + +b3JointId b3CreateParallelJoint( b3WorldId worldId, const b3ParallelJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + B3_ASSERT( b3IsValidFloat( def->hertz ) && def->hertz >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->dampingRatio ) && def->dampingRatio >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->maxTorque ) && def->maxTorque >= 0.0f ); + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_parallelJoint ); + + b3JointSim* joint = pair.jointSim; + + joint->parallelJoint = (b3ParallelJoint){ 0 }; + joint->parallelJoint.hertz = def->hertz; + joint->parallelJoint.dampingRatio = def->dampingRatio; + joint->parallelJoint.maxTorque = def->maxTorque; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + return jointId; +} + +b3JointId b3CreatePrismaticJoint( b3WorldId worldId, const b3PrismaticJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + B3_ASSERT( def->lowerTranslation <= def->upperTranslation ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_prismaticJoint ); + + b3JointSim* joint = pair.jointSim; + + joint->prismaticJoint = (b3PrismaticJoint){ 0 }; + joint->prismaticJoint.hertz = def->hertz; + joint->prismaticJoint.dampingRatio = def->dampingRatio; + joint->prismaticJoint.targetTranslation = def->targetTranslation; + joint->prismaticJoint.lowerTranslation = def->lowerTranslation; + joint->prismaticJoint.upperTranslation = def->upperTranslation; + joint->prismaticJoint.maxMotorForce = def->maxMotorForce; + joint->prismaticJoint.motorSpeed = def->motorSpeed; + joint->prismaticJoint.enableSpring = def->enableSpring; + joint->prismaticJoint.enableLimit = def->enableLimit; + joint->prismaticJoint.enableMotor = def->enableMotor; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + return jointId; +} + +b3JointId b3CreateRevoluteJoint( b3WorldId worldId, const b3RevoluteJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + B3_ASSERT( def->lowerAngle <= def->upperAngle ); + B3_ASSERT( def->lowerAngle >= -0.99f * B3_PI ); + B3_ASSERT( def->upperAngle <= 0.99f * B3_PI ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_revoluteJoint ); + + b3JointSim* joint = pair.jointSim; + + joint->revoluteJoint = (b3RevoluteJoint){ 0 }; + joint->revoluteJoint.hertz = def->hertz; + joint->revoluteJoint.dampingRatio = def->dampingRatio; + joint->revoluteJoint.targetAngle = b3ClampFloat( def->targetAngle, -B3_PI, B3_PI ); + joint->revoluteJoint.lowerAngle = b3MinFloat( def->lowerAngle, def->upperAngle ); + joint->revoluteJoint.upperAngle = b3MaxFloat( def->lowerAngle, def->upperAngle ); + joint->revoluteJoint.lowerAngle = b3ClampFloat( joint->revoluteJoint.lowerAngle, -B3_PI, B3_PI ); + joint->revoluteJoint.upperAngle = b3ClampFloat( joint->revoluteJoint.upperAngle, -B3_PI, B3_PI ); + joint->revoluteJoint.maxMotorTorque = def->maxMotorTorque; + joint->revoluteJoint.motorSpeed = def->motorSpeed; + joint->revoluteJoint.enableSpring = def->enableSpring; + joint->revoluteJoint.enableLimit = def->enableLimit; + joint->revoluteJoint.enableMotor = def->enableMotor; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + return jointId; +} + +b3JointId b3CreateSphericalJoint( b3WorldId worldId, const b3SphericalJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + B3_ASSERT( def->lowerTwistAngle <= def->upperTwistAngle ); + B3_ASSERT( def->lowerTwistAngle >= -0.99f * B3_PI ); + B3_ASSERT( def->upperTwistAngle <= 0.99f * B3_PI ); + B3_ASSERT( 0.0f <= def->coneAngle && def->coneAngle <= 0.99f * B3_PI ); + B3_ASSERT( b3IsValidQuat( def->targetRotation ) ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_sphericalJoint ); + + b3JointSim* joint = pair.jointSim; + + joint->sphericalJoint = (b3SphericalJoint){ 0 }; + joint->sphericalJoint.hertz = def->hertz; + joint->sphericalJoint.dampingRatio = def->dampingRatio; + joint->sphericalJoint.targetRotation = def->targetRotation; + joint->sphericalJoint.coneAngle = b3ClampFloat( def->coneAngle, 0.0f, 0.5f * B3_PI ); + joint->sphericalJoint.lowerTwistAngle = b3MinFloat( def->lowerTwistAngle, def->upperTwistAngle ); + joint->sphericalJoint.upperTwistAngle = b3MaxFloat( def->lowerTwistAngle, def->upperTwistAngle ); + joint->sphericalJoint.lowerTwistAngle = b3ClampFloat( joint->sphericalJoint.lowerTwistAngle, -B3_PI, B3_PI ); + joint->sphericalJoint.upperTwistAngle = b3ClampFloat( joint->sphericalJoint.upperTwistAngle, -B3_PI, B3_PI ); + joint->sphericalJoint.maxMotorTorque = def->maxMotorTorque; + joint->sphericalJoint.motorVelocity = def->motorVelocity; + joint->sphericalJoint.enableSpring = def->enableSpring; + joint->sphericalJoint.enableConeLimit = def->enableConeLimit; + joint->sphericalJoint.enableTwistLimit = def->enableTwistLimit; + joint->sphericalJoint.enableMotor = def->enableMotor; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + return jointId; +} + +b3JointId b3CreateWeldJoint( b3WorldId worldId, const b3WeldJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + B3_ASSERT( 0.0f <= def->angularHertz ); + B3_ASSERT( 0.0f <= def->angularDampingRatio ); + B3_ASSERT( 0.0f <= def->linearHertz ); + B3_ASSERT( 0.0f <= def->linearDampingRatio ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_weldJoint ); + + b3JointSim* joint = pair.jointSim; + + joint->weldJoint = (b3WeldJoint){ 0 }; + joint->weldJoint.linearHertz = def->linearHertz; + joint->weldJoint.linearDampingRatio = def->linearDampingRatio; + joint->weldJoint.angularHertz = def->angularHertz; + joint->weldJoint.angularDampingRatio = def->angularDampingRatio; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + return jointId; +} + +b3JointId b3CreateWheelJoint( b3WorldId worldId, const b3WheelJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + B3_ASSERT( def->lowerSuspensionLimit <= def->upperSuspensionLimit ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_wheelJoint ); + + b3JointSim* joint = pair.jointSim; + + joint->wheelJoint = (b3WheelJoint){ 0 }; + joint->wheelJoint.enableSuspensionSpring = def->enableSuspensionSpring; + joint->wheelJoint.suspensionHertz = def->suspensionHertz; + joint->wheelJoint.suspensionDampingRatio = def->suspensionDampingRatio; + joint->wheelJoint.enableSuspensionLimit = def->enableSuspensionLimit; + joint->wheelJoint.lowerSuspensionLimit = def->lowerSuspensionLimit; + joint->wheelJoint.upperSuspensionLimit = def->upperSuspensionLimit; + joint->wheelJoint.enableSpinMotor = def->enableSpinMotor; + joint->wheelJoint.maxSpinTorque = def->maxSpinTorque; + joint->wheelJoint.spinSpeed = def->spinSpeed; + + joint->wheelJoint.enableSteering = def->enableSteering; + joint->wheelJoint.steeringHertz = def->steeringHertz; + joint->wheelJoint.steeringDampingRatio = def->steeringDampingRatio; + joint->wheelJoint.targetSteeringAngle = def->targetSteeringAngle; + joint->wheelJoint.maxSteeringTorque = def->maxSteeringTorque; + joint->wheelJoint.enableSteeringLimit = def->enableSteeringLimit; + joint->wheelJoint.lowerSteeringLimit = def->lowerSteeringLimit; + joint->wheelJoint.upperSteeringLimit = def->upperSteeringLimit; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + return jointId; +} + +void b3DestroyJointInternal( b3World* world, b3Joint* joint, bool wakeBodies ) +{ + int jointId = joint->jointId; + + b3JointEdge* edgeA = joint->edges + 0; + b3JointEdge* edgeB = joint->edges + 1; + + int idA = edgeA->bodyId; + int idB = edgeB->bodyId; + b3Body* bodyA = b3Array_Get( world->bodies, idA ); + b3Body* bodyB = b3Array_Get( world->bodies, idB ); + + // Remove from body A + if ( edgeA->prevKey != B3_NULL_INDEX ) + { + b3Joint* prevJoint = b3Array_Get( world->joints, edgeA->prevKey >> 1 ); + b3JointEdge* prevEdge = prevJoint->edges + ( edgeA->prevKey & 1 ); + prevEdge->nextKey = edgeA->nextKey; + } + + if ( edgeA->nextKey != B3_NULL_INDEX ) + { + b3Joint* nextJoint = b3Array_Get( world->joints, edgeA->nextKey >> 1 ); + b3JointEdge* nextEdge = nextJoint->edges + ( edgeA->nextKey & 1 ); + nextEdge->prevKey = edgeA->prevKey; + } + + int edgeKeyA = ( jointId << 1 ) | 0; + if ( bodyA->headJointKey == edgeKeyA ) + { + bodyA->headJointKey = edgeA->nextKey; + } + + bodyA->jointCount -= 1; + + // Remove from body B + if ( edgeB->prevKey != B3_NULL_INDEX ) + { + b3Joint* prevJoint = b3Array_Get( world->joints, edgeB->prevKey >> 1 ); + b3JointEdge* prevEdge = prevJoint->edges + ( edgeB->prevKey & 1 ); + prevEdge->nextKey = edgeB->nextKey; + } + + if ( edgeB->nextKey != B3_NULL_INDEX ) + { + b3Joint* nextJoint = b3Array_Get( world->joints, edgeB->nextKey >> 1 ); + b3JointEdge* nextEdge = nextJoint->edges + ( edgeB->nextKey & 1 ); + nextEdge->prevKey = edgeB->prevKey; + } + + int edgeKeyB = ( jointId << 1 ) | 1; + if ( bodyB->headJointKey == edgeKeyB ) + { + bodyB->headJointKey = edgeB->nextKey; + } + + bodyB->jointCount -= 1; + + if ( joint->islandId != B3_NULL_INDEX ) + { + B3_ASSERT( joint->setIndex > b3_disabledSet ); + b3UnlinkJoint( world, joint ); + } + else + { + B3_ASSERT( joint->setIndex <= b3_disabledSet ); + } + + // Remove joint from solver set that owns it + int setIndex = joint->setIndex; + int localIndex = joint->localIndex; + + if ( setIndex == b3_awakeSet ) + { + b3RemoveJointFromGraph( world, joint->edges[0].bodyId, joint->edges[1].bodyId, joint->colorIndex, localIndex ); + } + else + { + b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + int movedIndex = b3Array_RemoveSwap( set->jointSims, localIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fix moved joint + b3JointSim* movedJointSim = set->jointSims.data + localIndex; + int movedId = movedJointSim->jointId; + b3Joint* movedJoint = b3Array_Get( world->joints, movedId ); + B3_ASSERT( movedJoint->localIndex == movedIndex ); + movedJoint->localIndex = localIndex; + } + } + + // Free joint and id (preserve joint revision) + joint->setIndex = B3_NULL_INDEX; + joint->localIndex = B3_NULL_INDEX; + joint->colorIndex = B3_NULL_INDEX; + joint->jointId = B3_NULL_INDEX; + b3FreeId( &world->jointIdPool, jointId ); + + if ( wakeBodies ) + { + b3WakeBody( world, bodyA ); + b3WakeBody( world, bodyB ); + } + + b3ValidateSolverSets( world ); +} + +void b3DestroyJoint( b3JointId jointId, bool wakeAttached ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + if ( world == NULL ) + { + return; + } + + b3Joint* joint = b3GetJointFullId( world, jointId ); + + b3DestroyJointInternal( world, joint, wakeAttached ); +} + +b3JointType b3Joint_GetType( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + return joint->type; +} + +b3BodyId b3Joint_GetBodyA( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + return b3MakeBodyId( world, joint->edges[0].bodyId ); +} + +b3BodyId b3Joint_GetBodyB( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + return b3MakeBodyId( world, joint->edges[1].bodyId ); +} + +b3WorldId b3Joint_GetWorld( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + return (b3WorldId){ (uint16_t)( jointId.world0 + 1 ), world->generation }; +} + +void b3Joint_SetLocalFrameA( b3JointId jointId, b3Transform localFrame ) +{ + B3_ASSERT( b3IsValidTransform( localFrame ) ); + + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* jointSim = b3GetJointSim( world, joint ); + jointSim->localFrameA = localFrame; +} + +b3Transform b3Joint_GetLocalFrameA( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* jointSim = b3GetJointSim( world, joint ); + return jointSim->localFrameA; +} + +void b3Joint_SetLocalFrameB( b3JointId jointId, b3Transform localFrame ) +{ + B3_ASSERT( b3IsValidTransform( localFrame ) ); + + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* jointSim = b3GetJointSim( world, joint ); + jointSim->localFrameB = localFrame; +} + +b3Transform b3Joint_GetLocalFrameB( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* jointSim = b3GetJointSim( world, joint ); + return jointSim->localFrameB; +} + +void b3Joint_SetCollideConnected( b3JointId jointId, bool shouldCollide ) +{ + b3World* world = b3GetUnlockedWorld( jointId.world0 ); + if ( world == NULL ) + { + return; + } + + b3Joint* joint = b3GetJointFullId( world, jointId ); + if ( joint->collideConnected == shouldCollide ) + { + return; + } + + joint->collideConnected = shouldCollide; + + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + + if ( shouldCollide ) + { + // need to tell the broad-phase to look for new pairs for one of the + // two bodies. Pick the one with the fewest shapes. + int shapeCountA = bodyA->shapeCount; + int shapeCountB = bodyB->shapeCount; + + int shapeId = shapeCountA < shapeCountB ? bodyA->headShapeId : bodyB->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + if ( shape->proxyKey != B3_NULL_INDEX ) + { + b3BufferMove( &world->broadPhase, shape->proxyKey ); + } + + shapeId = shape->nextShapeId; + } + } + else + { + b3DestroyContactsBetweenBodies( world, bodyA, bodyB ); + } +} + +bool b3Joint_GetCollideConnected( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + return joint->collideConnected; +} + +void b3Joint_SetUserData( b3JointId jointId, void* userData ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + joint->userData = userData; +} + +void* b3Joint_GetUserData( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + return joint->userData; +} + +void b3Joint_WakeBodies( b3JointId jointId ) +{ + b3World* world = b3GetUnlockedWorld( jointId.world0 ); + if ( world == NULL ) + { + return; + } + + world->locked = true; + + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + + b3WakeBody( world, bodyA ); + b3WakeBody( world, bodyB ); + + world->locked = false; +} + +void b3GetJointReaction( b3World* world, b3JointSim* sim, float invTimeStep, float* force, float* torque ) +{ + float linearImpulse = 0.0f; + float angularImpulse = 0.0f; + + switch ( sim->type ) + { + case b3_parallelJoint: + { + b3ParallelJoint* joint = &sim->parallelJoint; + b3Vec3 impulse = { + .x = joint->perpImpulse.x, + .y = joint->perpImpulse.y, + .z = 0.0f, + }; + angularImpulse = b3Length( impulse ); + } + break; + + case b3_distanceJoint: + { + b3DistanceJoint* joint = &sim->distanceJoint; + linearImpulse = b3AbsFloat( joint->impulse + joint->lowerImpulse - joint->upperImpulse + joint->motorImpulse ); + } + break; + + case b3_motorJoint: + { + b3MotorJoint* joint = &sim->motorJoint; + linearImpulse = b3Length( b3Add( joint->linearVelocityImpulse, joint->linearSpringImpulse ) ); + angularImpulse = b3Length( b3Add( joint->angularVelocityImpulse, joint->angularSpringImpulse ) ); + } + break; + + case b3_prismaticJoint: + { + b3PrismaticJoint* joint = &sim->prismaticJoint; + b3Vec3 impulse = { + .x = joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse, + .y = joint->perpImpulse.x, + .z = joint->perpImpulse.y, + }; + linearImpulse = b3Length( impulse ); + angularImpulse = b3Length( joint->angularImpulse ); + } + break; + + case b3_revoluteJoint: + { + b3RevoluteJoint* joint = &sim->revoluteJoint; + linearImpulse = b3Length( joint->linearImpulse ); + b3Vec3 impulse = { + .x = joint->perpImpulse.x, + .y = joint->perpImpulse.y, + .z = joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse, + }; + angularImpulse = b3Length( impulse ); + } + break; + + case b3_sphericalJoint: + { + // todo improve performance + b3SphericalJoint* joint = &sim->sphericalJoint; + linearImpulse = b3Length( joint->linearImpulse ); + + b3Transform xfA = b3GetBodyTransform( world, sim->bodyIdA ); + b3Transform xfB = b3GetBodyTransform( world, sim->bodyIdB ); + b3Quat qA = b3MulQuat( xfA.q, sim->localFrameA.q ); + b3Quat qB = b3MulQuat( xfB.q, sim->localFrameB.q ); + + // Cone axis is the z-axis of body A. + b3Vec3 coneAxis = b3RotateVector( qA, b3Vec3_axisZ ); + b3Vec3 twistAxis = b3RotateVector( qB, b3Vec3_axisZ ); + b3Vec3 swingAxis = b3Normalize( b3Cross( coneAxis, twistAxis ) ); + + b3Vec3 impulse = b3Add( joint->springImpulse, joint->motorImpulse ); + impulse = b3MulAdd( impulse, joint->lowerTwistImpulse - joint->upperTwistImpulse, twistAxis ); + impulse = b3MulAdd( impulse, joint->swingImpulse, swingAxis ); + + angularImpulse = b3Length( impulse ); + } + break; + + case b3_weldJoint: + { + b3WeldJoint* joint = &sim->weldJoint; + linearImpulse = b3Length( joint->linearImpulse ); + angularImpulse = b3Length( joint->angularImpulse ); + } + break; + + case b3_wheelJoint: + { + // todo probably wrong + b3WheelJoint* joint = &sim->wheelJoint; + b3Vec2 perpImpulse = joint->linearImpulse; + float axialImpulse = joint->suspensionSpringImpulse + joint->lowerSuspensionImpulse - joint->upperSuspensionImpulse; + linearImpulse = sqrtf( perpImpulse.x * perpImpulse.x + perpImpulse.y * perpImpulse.y + axialImpulse * axialImpulse ); + angularImpulse = b3AbsFloat( joint->spinImpulse ); + } + break; + + default: + break; + } + + *force = linearImpulse * invTimeStep; + *torque = angularImpulse * invTimeStep; +} + +static b3Vec3 b3GetJointConstraintForce( b3World* world, b3Joint* joint ) +{ + b3JointSim* base = b3GetJointSim( world, joint ); + + switch ( joint->type ) + { + case b3_parallelJoint: + return b3Vec3_zero; + + case b3_distanceJoint: + return b3GetDistanceJointForce( world, base ); + + case b3_filterJoint: + return b3Vec3_zero; + + case b3_motorJoint: + return b3GetMotorJointForce( world, base ); + + case b3_prismaticJoint: + return b3GetPrismaticJointForce( world, base ); + + case b3_revoluteJoint: + return b3GetRevoluteJointForce( world, base ); + + case b3_sphericalJoint: + return b3GetSphericalJointForce( world, base ); + + case b3_weldJoint: + return b3GetWeldJointForce( world, base ); + + case b3_wheelJoint: + return b3GetWheelJointForce( world, base ); + + default: + B3_ASSERT( false ); + return b3Vec3_zero; + } +} + +static b3Vec3 b3GetJointConstraintTorque( b3World* world, b3Joint* joint ) +{ + b3JointSim* base = b3GetJointSim( world, joint ); + + switch ( joint->type ) + { + case b3_distanceJoint: + return b3Vec3_zero; + + case b3_filterJoint: + return b3Vec3_zero; + + case b3_motorJoint: + return b3GetMotorJointTorque( world, base ); + + case b3_prismaticJoint: + return b3GetPrismaticJointTorque( world, base ); + + case b3_revoluteJoint: + return b3GetRevoluteJointTorque( world, base ); + + case b3_sphericalJoint: + return b3GetSphericalJointTorque( world, base ); + + case b3_weldJoint: + return b3GetWeldJointTorque( world, base ); + + case b3_wheelJoint: + return b3GetWheelJointTorque( world, base ); + + default: + B3_ASSERT( false ); + return b3Vec3_zero; + } +} + +b3Vec3 b3Joint_GetConstraintForce( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + return b3GetJointConstraintForce( world, joint ); +} + +b3Vec3 b3Joint_GetConstraintTorque( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + return b3GetJointConstraintTorque( world, joint ); +} + +float b3Joint_GetLinearSeparation( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + + b3Transform xfA = b3GetBodyTransform( world, joint->edges[0].bodyId ); + b3Transform xfB = b3GetBodyTransform( world, joint->edges[1].bodyId ); + + b3Vec3 pA = b3TransformPoint( xfA, base->localFrameA.p ); + b3Vec3 pB = b3TransformPoint( xfB, base->localFrameB.p ); + b3Vec3 dp = b3Sub( pB, pA ); + + switch ( joint->type ) + { + case b3_parallelJoint: + return 0.0f; + + case b3_distanceJoint: + { + b3DistanceJoint* distanceJoint = &base->distanceJoint; + float length = b3Length( dp ); + if ( distanceJoint->enableSpring ) + { + if ( distanceJoint->enableLimit ) + { + if ( length < distanceJoint->minLength ) + { + return distanceJoint->minLength - length; + } + + if ( length > distanceJoint->maxLength ) + { + return length - distanceJoint->maxLength; + } + + return 0.0f; + } + + return 0.0f; + } + + return b3AbsFloat( length - distanceJoint->length ); + } + + case b3_motorJoint: + return 0.0f; + + case b3_filterJoint: + return 0.0f; + + case b3_prismaticJoint: + { + b3PrismaticJoint* prismaticJoint = &base->prismaticJoint; + b3Vec3 axisA = b3RotateVector( xfA.q, b3Vec3_axisX ); + b3Vec3 perpA = b3Perp( axisA ); + float perpendicularSeparation = b3AbsFloat( b3Dot( perpA, dp ) ); + float limitSeparation = 0.0f; + + if ( prismaticJoint->enableLimit ) + { + float translation = b3Dot( axisA, dp ); + if ( translation < prismaticJoint->lowerTranslation ) + { + limitSeparation = prismaticJoint->lowerTranslation - translation; + } + + if ( prismaticJoint->upperTranslation < translation ) + { + limitSeparation = translation - prismaticJoint->upperTranslation; + } + } + + return sqrtf( perpendicularSeparation * perpendicularSeparation + limitSeparation * limitSeparation ); + } + + case b3_revoluteJoint: + return b3Length( dp ); + + case b3_sphericalJoint: + return b3Length( dp ); + + case b3_weldJoint: + { + b3WeldJoint* weldJoint = &base->weldJoint; + if ( weldJoint->linearHertz == 0.0f ) + { + return b3Length( dp ); + } + + return 0.0f; + } + + case b3_wheelJoint: + { + b3WheelJoint* wheelJoint = &base->wheelJoint; + b3Vec3 axisA = b3RotateVector( xfA.q, b3Vec3_axisX ); + b3Vec3 perpA = b3Perp( axisA ); + float perpendicularSeparation = b3AbsFloat( b3Dot( perpA, dp ) ); + float limitSeparation = 0.0f; + + if ( wheelJoint->enableSuspensionLimit ) + { + float translation = b3Dot( axisA, dp ); + if ( translation < wheelJoint->lowerSuspensionLimit ) + { + limitSeparation = wheelJoint->lowerSuspensionLimit - translation; + } + + if ( wheelJoint->upperSuspensionLimit < translation ) + { + limitSeparation = translation - wheelJoint->upperSuspensionLimit; + } + } + + return sqrtf( perpendicularSeparation * perpendicularSeparation + limitSeparation * limitSeparation ); + } + + default: + B3_ASSERT( false ); + return 0.0f; + } +} + +float b3Joint_GetAngularSeparation( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + + b3Transform xfA = b3GetBodyTransform( world, joint->edges[0].bodyId ); + b3Transform xfB = b3GetBodyTransform( world, joint->edges[1].bodyId ); + + b3Quat relQ = b3InvMulQuat( xfA.q, xfB.q ); + + switch ( joint->type ) + { + case b3_parallelJoint: + { + // Remove hinge angle + relQ.v.z = 0.0f; + return b3GetQuatAngle( relQ ); + } + + case b3_distanceJoint: + return 0.0f; + + case b3_motorJoint: + return 0.0f; + + case b3_filterJoint: + return 0.0f; + + case b3_prismaticJoint: + return b3GetQuatAngle( relQ ); + + case b3_revoluteJoint: + { + b3RevoluteJoint* revoluteJoint = &base->revoluteJoint; + if ( revoluteJoint->enableLimit ) + { + float angle = b3GetTwistAngle( relQ ); + if ( angle < revoluteJoint->lowerAngle ) + { + return b3GetQuatAngle( relQ ); + } + + if ( revoluteJoint->upperAngle < angle ) + { + return b3GetQuatAngle( relQ ); + } + } + + // Remove hinge angle + relQ.v.z = 0.0f; + return b3GetQuatAngle( relQ ); + } + + case b3_sphericalJoint: + { + b3SphericalJoint* sphericalJoint = &base->sphericalJoint; + float sum = 0.0f; + if ( sphericalJoint->enableConeLimit ) + { + float swingAngle = b3GetSwingAngle( relQ ); + sum += b3MaxFloat( 0.0f, swingAngle - sphericalJoint->coneAngle ); + } + + if ( sphericalJoint->enableTwistLimit ) + { + float twistAngle = b3GetTwistAngle( relQ ); + sum += b3MaxFloat( 0.0f, sphericalJoint->lowerTwistAngle - twistAngle ); + sum += b3MaxFloat( 0.0f, twistAngle - sphericalJoint->upperTwistAngle ); + } + + return sum; + } + + case b3_weldJoint: + { + b3WeldJoint* weldJoint = &base->weldJoint; + if ( weldJoint->angularHertz == 0.0f ) + { + return b3GetQuatAngle( relQ ); + } + + return 0.0f; + } + + case b3_wheelJoint: + // todo + B3_ASSERT( false ); + return 0.0f; + + default: + B3_ASSERT( false ); + return 0.0f; + } +} + +#if 0 +void b3Joint_SetSpringRotationTarget( b3JointId jointId, b3Quat relativeBodyRotation, float hertz ) +{ + B3_ASSERT( b3IsValidQuat( relativeBodyRotation ) ); + B3_ASSERT( b3IsValidFloat( hertz ) && hertz > 0.0f ); + + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + + b3Quat qA = base->localFrameA.q; + b3Quat qB = b3MulQuat( relativeBodyRotation, base->localFrameB.q ); + + // This keeps the twist angle in the range [-pi, pi] + if ( b3DotQuat( qA, qB ) < 0.0f ) + { + qA = -qA; + } + + b3Quat relQ = b3InvMulQuat( qA, qB ); + + switch ( joint->type ) + { + case b3_revoluteJoint: + base->revoluteJoint.targetAngle = b3GetTwistAngle( relQ ); + base->revoluteJoint.hertz = hertz; + break; + + case b3_sphericalJoint: + base->sphericalJoint.targetRotation = relQ; + base->sphericalJoint.hertz = hertz; + break; + + default: + break; + } +} +#endif + +void b3PrepareJoint( b3JointSim* joint, b3StepContext* context ) +{ + // Clamp joint hertz based on the time step to reduce jitter. + float hertz = b3MinFloat( joint->constraintHertz, 0.25f * context->inv_h ); + joint->constraintSoftness = b3MakeSoft( hertz, joint->constraintDampingRatio, context->h ); + + switch ( joint->type ) + { + case b3_parallelJoint: + b3PrepareParallelJoint( joint, context ); + break; + + case b3_distanceJoint: + b3PrepareDistanceJoint( joint, context ); + break; + + case b3_filterJoint: + break; + + case b3_motorJoint: + b3PrepareMotorJoint( joint, context ); + break; + + case b3_prismaticJoint: + b3PreparePrismaticJoint( joint, context ); + break; + + case b3_revoluteJoint: + b3PrepareRevoluteJoint( joint, context ); + break; + + case b3_sphericalJoint: + b3PrepareSphericalJoint( joint, context ); + break; + + case b3_weldJoint: + b3PrepareWeldJoint( joint, context ); + break; + + case b3_wheelJoint: + b3PrepareWheelJoint( joint, context ); + break; + + default: + B3_ASSERT( false ); + } +} + +void b3WarmStartJoint( b3JointSim* joint, b3StepContext* context ) +{ + switch ( joint->type ) + { + case b3_parallelJoint: + b3WarmStartParallelJoint( joint, context ); + break; + + case b3_distanceJoint: + b3WarmStartDistanceJoint( joint, context ); + break; + + case b3_filterJoint: + break; + + case b3_motorJoint: + b3WarmStartMotorJoint( joint, context ); + break; + + case b3_prismaticJoint: + b3WarmStartPrismaticJoint( joint, context ); + break; + + case b3_revoluteJoint: + b3WarmStartRevoluteJoint( joint, context ); + break; + + case b3_sphericalJoint: + b3WarmStartSphericalJoint( joint, context ); + break; + + case b3_weldJoint: + b3WarmStartWeldJoint( joint, context ); + break; + + case b3_wheelJoint: + b3WarmStartWheelJoint( joint, context ); + break; + + default: + B3_ASSERT( false ); + } +} + +void b3SolveJoint( b3JointSim* joint, b3StepContext* context, bool useBias ) +{ + B3_UNUSED( useBias ); + + switch ( joint->type ) + { + case b3_parallelJoint: + b3SolveParallelJoint( joint, context ); + break; + + case b3_distanceJoint: + b3SolveDistanceJoint( joint, context, useBias ); + break; + + case b3_filterJoint: + break; + + case b3_motorJoint: + b3SolveMotorJoint( joint, context ); + break; + + case b3_prismaticJoint: + b3SolvePrismaticJoint( joint, context, useBias ); + break; + + case b3_revoluteJoint: + b3SolveRevoluteJoint( joint, context, useBias ); + break; + + case b3_sphericalJoint: + b3SolveSphericalJoint( joint, context, useBias ); + break; + + case b3_weldJoint: + b3SolveWeldJoint( joint, context, useBias ); + break; + + case b3_wheelJoint: + b3SolveWheelJoint( joint, context, useBias ); + break; + + default: + B3_ASSERT( false ); + } +} + +void b3PrepareJoints_Overflow( b3StepContext* context ) +{ + b3TracyCZoneNC( prepare_joints, "PrepJoints", b3_colorOldLace, true ); + + b3ConstraintGraph* graph = context->graph; + b3JointSim* joints = graph->colors[B3_OVERFLOW_INDEX].jointSims.data; + int jointCount = graph->colors[B3_OVERFLOW_INDEX].jointSims.count; + + for ( int i = 0; i < jointCount; ++i ) + { + b3JointSim* joint = joints + i; + b3PrepareJoint( joint, context ); + } + + b3TracyCZoneEnd( prepare_joints ); +} + +void b3WarmStartJoints_Overflow( b3StepContext* context ) +{ + b3TracyCZoneNC( prepare_joints, "PrepJoints", b3_colorOldLace, true ); + + b3ConstraintGraph* graph = context->graph; + b3JointSim* joints = graph->colors[B3_OVERFLOW_INDEX].jointSims.data; + int jointCount = graph->colors[B3_OVERFLOW_INDEX].jointSims.count; + + for ( int i = 0; i < jointCount; ++i ) + { + b3JointSim* joint = joints + i; + b3WarmStartJoint( joint, context ); + } + + b3TracyCZoneEnd( prepare_joints ); +} + +void b3SolveJoints_Overflow( b3StepContext* context, bool useBias ) +{ + b3TracyCZoneNC( solve_joints, "SolveJoints", b3_colorLemonChiffon, true ); + + b3ConstraintGraph* graph = context->graph; + b3JointSim* joints = graph->colors[B3_OVERFLOW_INDEX].jointSims.data; + int jointCount = graph->colors[B3_OVERFLOW_INDEX].jointSims.count; + + for ( int i = 0; i < jointCount; ++i ) + { + b3JointSim* joint = joints + i; + b3SolveJoint( joint, context, useBias ); + } + + b3TracyCZoneEnd( solve_joints ); +} + +void b3DrawJoint( b3DebugDraw* draw, b3World* world, b3Joint* joint ) +{ + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + if ( bodyA->setIndex == b3_disabledSet || bodyB->setIndex == b3_disabledSet ) + { + return; + } + + b3JointSim* jointSim = b3GetJointSim( world, joint ); + + b3Transform transformA = b3GetBodyTransformQuick( world, bodyA ); + b3Transform transformB = b3GetBodyTransformQuick( world, bodyB ); + b3Vec3 pA = b3TransformPoint( transformA, jointSim->localFrameA.p ); + b3Vec3 pB = b3TransformPoint( transformB, jointSim->localFrameB.p ); + + b3HexColor color = b3_colorDarkSeaGreen; + + float scale = b3MaxFloat( 0.0001f, draw->jointScale * joint->drawScale ); + + switch ( joint->type ) + { + case b3_parallelJoint: + b3DrawParallelJoint( draw, jointSim, transformA, transformB, scale ); + break; + + case b3_distanceJoint: + b3DrawDistanceJoint( draw, jointSim, transformA, transformB ); + break; + + case b3_filterJoint: + draw->DrawSegmentFcn( pA, pB, b3_colorGold, draw->context ); + break; + + case b3_motorJoint: + draw->DrawPointFcn( pA, 8.0f, b3_colorYellowGreen, draw->context ); + draw->DrawPointFcn( pB, 8.0f, b3_colorPlum, draw->context ); + break; + + case b3_prismaticJoint: + b3DrawPrismaticJoint( draw, jointSim, transformA, transformB, scale ); + break; + + case b3_revoluteJoint: + b3DrawRevoluteJoint( draw, jointSim, transformA, transformB, scale ); + break; + + case b3_sphericalJoint: + b3DrawSphericalJoint( draw, jointSim, transformA, transformB, scale ); + break; + + case b3_weldJoint: + b3DrawWeldJoint( draw, jointSim, transformA, transformB, scale ); + break; + + case b3_wheelJoint: + b3DrawWheelJoint( draw, jointSim, transformA, transformB, scale ); + break; + + default: + draw->DrawSegmentFcn( transformA.p, pA, color, draw->context ); + draw->DrawSegmentFcn( pA, pB, color, draw->context ); + draw->DrawSegmentFcn( transformB.p, pB, color, draw->context ); + break; + } + + if ( draw->drawGraphColors ) + { + b3HexColor graphColors[B3_GRAPH_COLOR_COUNT] = { + b3_colorRed, b3_colorOrange, b3_colorYellow, b3_colorGreen, b3_colorCyan, b3_colorBlue, + b3_colorViolet, b3_colorPink, b3_colorChocolate, b3_colorGoldenRod, b3_colorCoral, b3_colorRosyBrown, + b3_colorAqua, b3_colorPeru, b3_colorLime, b3_colorGold, b3_colorPlum, b3_colorSnow, + b3_colorTeal, b3_colorKhaki, b3_colorSalmon, b3_colorPeachPuff, b3_colorHoneyDew, b3_colorBlack, + }; + + int colorIndex = joint->colorIndex; + if ( colorIndex != B3_NULL_INDEX ) + { + b3Vec3 p = b3Lerp( pA, pB, 0.5f ); + draw->DrawPointFcn( p, 5.0f, graphColors[colorIndex], draw->context ); + } + } + + if ( draw->drawJointExtras ) + { + b3Vec3 force = b3GetJointConstraintForce( world, joint ); + b3Vec3 torque = b3GetJointConstraintTorque( world, joint ); + b3Vec3 p = b3Lerp( pA, pB, 0.5f ); + + draw->DrawSegmentFcn( p, b3MulAdd( p, 0.001f, force ), b3_colorAzure, draw->context ); + + char buffer[64]; + snprintf( buffer, 64, "f = %g, t = %g", b3Length( force ), b3Length( torque ) ); + draw->DrawStringFcn( p, buffer, b3_colorAzure, draw->context ); + } +} diff --git a/src/joint.h b/src/joint.h new file mode 100644 index 0000000..49af3f4 --- /dev/null +++ b/src/joint.h @@ -0,0 +1,424 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "math_internal.h" +#include "solver.h" + +#include "box3d/types.h" + +typedef struct b3DebugDraw b3DebugDraw; +typedef struct b3StepContext b3StepContext; +typedef struct b3World b3World; + +/// A joint edge is used to connect bodies and joints together +/// in a joint graph where each body is a node and each joint +/// is an edge. A joint edge belongs to a doubly linked list +/// maintained in each attached body. Each joint has two joint +/// nodes, one for each attached body. +typedef struct b3JointEdge +{ + int bodyId; + int prevKey; + int nextKey; +} b3JointEdge; + +// Map from b3JointId to b3Joint in the solver sets +typedef struct b3Joint +{ + void* userData; + + // index of simulation set stored in b3World + // B3_NULL_INDEX when slot is free + int setIndex; + + // index into the constraint graph color array, may be B3_NULL_INDEX for sleeping/disabled joints + // B3_NULL_INDEX when slot is free + int colorIndex; + + // joint index within set or graph color + // B3_NULL_INDEX when slot is free + int localIndex; + + b3JointEdge edges[2]; + + int jointId; + int islandId; + + // Index into the island's joints array for O(1) swap-removal. + // B3_NULL_INDEX when not in an island. + int islandIndex; + + float drawScale; + + b3JointType type; + + // This is monotonically advanced when a body is allocated in this slot + // Used to check for invalid b3JointId + uint16_t generation; + + bool collideConnected; +} b3Joint; + +typedef struct b3DistanceJoint +{ + float length; + float hertz; + float dampingRatio; + float lowerSpringForce; + float upperSpringForce; + float minLength; + float maxLength; + + float maxMotorForce; + float motorSpeed; + + float impulse; + float lowerImpulse; + float upperImpulse; + float motorImpulse; + + int indexA; + int indexB; + b3Vec3 anchorA; + b3Vec3 anchorB; + b3Vec3 deltaCenter; + b3Softness distanceSoftness; + float axialMass; + + bool enableSpring; + bool enableLimit; + bool enableMotor; +} b3DistanceJoint; + +typedef struct b3MotorJoint +{ + b3Vec3 linearVelocity; + b3Vec3 angularVelocity; + float maxVelocityForce; + float maxVelocityTorque; + float linearHertz; + float linearDampingRatio; + float maxSpringForce; + float angularHertz; + float angularDampingRatio; + float maxSpringTorque; + + b3Vec3 linearVelocityImpulse; + b3Vec3 angularVelocityImpulse; + b3Vec3 linearSpringImpulse; + b3Vec3 angularSpringImpulse; + + b3Softness linearSpring; + b3Softness angularSpring; + + int indexA; + int indexB; + b3Transform frameA; + b3Transform frameB; + b3Vec3 deltaCenter; + b3Matrix3 angularMass; +} b3MotorJoint; + +typedef struct b3ParallelJoint +{ + float hertz; + float dampingRatio; + float maxTorque; + + b3Vec2 perpImpulse; + b3Vec3 perpAxisX; + b3Vec3 perpAxisY; + + b3Quat quatA; + b3Quat quatB; + int indexA; + int indexB; + b3Softness softness; +} b3ParallelJoint; + +typedef struct b3PrismaticJoint +{ + b3Vec2 perpImpulse; + b3Vec3 angularImpulse; + float springImpulse; + float motorImpulse; + float lowerImpulse; + float upperImpulse; + float hertz; + float dampingRatio; + float maxMotorForce; + float motorSpeed; + float targetTranslation; + float lowerTranslation; + float upperTranslation; + + int indexA; + int indexB; + b3Transform frameA; + b3Transform frameB; + b3Vec3 jointAxis; + b3Vec3 perpAxisY; + b3Vec3 perpAxisZ; + b3Vec3 deltaCenter; + float deltaAngle; + b3Matrix3 rotationMass; + b3Softness springSoftness; + + bool enableSpring; + bool enableLimit; + bool enableMotor; +} b3PrismaticJoint; + +typedef struct b3RevoluteJoint +{ + b3Vec3 linearImpulse; + b3Vec2 perpImpulse; + float springImpulse; + float motorImpulse; + float lowerImpulse; + float upperImpulse; + float hertz; + float dampingRatio; + float maxMotorTorque; + float motorSpeed; + float targetAngle; + float lowerAngle; + float upperAngle; + + int indexA; + int indexB; + b3Transform frameA; + b3Transform frameB; + b3Vec3 rotationAxisZ; + b3Vec3 perpAxisX; + b3Vec3 perpAxisY; + b3Vec3 deltaCenter; + float deltaAngle; + float axialMass; + b3Softness springSoftness; + + bool enableSpring; + bool enableMotor; + bool enableLimit; +} b3RevoluteJoint; + +typedef struct b3SphericalJoint +{ + b3Vec3 linearImpulse; + b3Vec3 springImpulse; + b3Vec3 motorImpulse; + float lowerTwistImpulse; + float upperTwistImpulse; + float swingImpulse; + float hertz; + float dampingRatio; + float maxMotorTorque; + b3Vec3 motorVelocity; + float lowerTwistAngle; + float upperTwistAngle; + float coneAngle; + b3Quat targetRotation; + + int indexA; + int indexB; + b3Transform frameA; + b3Transform frameB; + b3Vec3 deltaCenter; + b3Vec3 swingAxis; + b3Vec3 twistJacobian; + + b3Matrix3 rotationMass; + float swingMass; + float twistMass; + b3Softness springSoftness; + + bool enableSpring; + bool enableMotor; + bool enableConeLimit; + bool enableTwistLimit; +} b3SphericalJoint; + +typedef struct b3WeldJoint +{ + float linearHertz; + float linearDampingRatio; + float angularHertz; + float angularDampingRatio; + + b3Softness linearSpring; + b3Softness angularSpring; + b3Vec3 linearImpulse; + b3Vec3 angularImpulse; + + int indexA; + int indexB; + b3Transform frameA; + b3Transform frameB; + b3Vec3 deltaCenter; + + b3Matrix3 angularMass; +} b3WeldJoint; + +typedef struct b3WheelJoint +{ + b3Vec2 linearImpulse; + b3Vec2 angularImpulse; + float spinImpulse; + float maxSpinTorque; + float spinSpeed; + float suspensionSpringImpulse; + float lowerSuspensionImpulse; + float upperSuspensionImpulse; + float lowerSuspensionLimit; + float upperSuspensionLimit; + float suspensionHertz; + float suspensionDampingRatio; + float steeringSpringImpulse; + float lowerSteeringImpulse; + float upperSteeringImpulse; + float lowerSteeringLimit; + float upperSteeringLimit; + float targetSteeringAngle; + float maxSteeringTorque; + float steeringHertz; + float steeringDampingRatio; + + int indexA; + int indexB; + b3Transform frameA; + b3Transform frameB; + b3Vec3 deltaCenter; + float spinMass; + float suspensionMass; + float steeringMass; + b3Softness suspensionSoftness; + b3Softness steeringSoftness; + + bool enableSpinMotor; + bool enableSuspensionSpring; + bool enableSuspensionLimit; + bool enableSteering; + bool enableSteeringLimit; + bool enableSteeringMotor; +} b3WheelJoint; + +/// The base joint class. Joints are used to constraint two bodies together in +/// various fashions. Some joints also feature limits and motors. +typedef struct b3JointSim +{ + int jointId; + + int bodyIdA; + int bodyIdB; + + b3JointType type; + + // Joint frames local to body origin + b3Transform localFrameA; + b3Transform localFrameB; + + float invMassA, invMassB; + b3Matrix3 invIA, invIB; + + float constraintHertz; + float constraintDampingRatio; + + b3Softness constraintSoftness; + + float forceThreshold; + float torqueThreshold; + + bool fixedRotation; + + union + { + b3DistanceJoint distanceJoint; + b3MotorJoint motorJoint; + b3ParallelJoint parallelJoint; + b3RevoluteJoint revoluteJoint; + b3SphericalJoint sphericalJoint; + b3PrismaticJoint prismaticJoint; + b3WeldJoint weldJoint; + b3WheelJoint wheelJoint; + }; +} b3JointSim; + +#ifdef __cplusplus +extern "C" +{ +#endif + +void b3DestroyJointInternal( b3World* world, b3Joint* joint, bool wakeBodies ); + +b3Joint* b3GetJointFullId( b3World* world, b3JointId jointId ); +b3JointSim* b3GetJointSim( b3World* world, b3Joint* joint ); +b3JointSim* b3GetJointSimCheckType( b3JointId jointId, b3JointType type ); + +void b3PrepareJoint( b3JointSim* joint, b3StepContext* context ); +void b3WarmStartJoint( b3JointSim* joint, b3StepContext* context ); +void b3SolveJoint( b3JointSim* joint, b3StepContext* context, bool useBias ); + +void b3PrepareJoints_Overflow( b3StepContext* context ); +void b3WarmStartJoints_Overflow( b3StepContext* context ); +void b3SolveJoints_Overflow( b3StepContext* context, bool useBias ); + +void b3GetJointReaction( b3World* world, b3JointSim* sim, float invTimeStep, float* force, float* torque ); + +void b3DrawJoint( b3DebugDraw* draw, b3World* world, b3Joint* joint ); + +b3Vec3 b3GetDistanceJointForce( b3World* world, b3JointSim* base ); +b3Vec3 b3GetMotorJointForce( b3World* world, b3JointSim* base ); +b3Vec3 b3GetPrismaticJointForce( b3World* world, b3JointSim* base ); +b3Vec3 b3GetRevoluteJointForce( b3World* world, b3JointSim* base ); +b3Vec3 b3GetSphericalJointForce( b3World* world, b3JointSim* base ); +b3Vec3 b3GetWeldJointForce( b3World* world, b3JointSim* base ); +b3Vec3 b3GetWheelJointForce( b3World* world, b3JointSim* base ); + +b3Vec3 b3GetMotorJointTorque( b3World* world, b3JointSim* base ); +b3Vec3 b3GetParallelJointTorque( b3World* world, b3JointSim* base ); +b3Vec3 b3GetPrismaticJointTorque( b3World* world, b3JointSim* base ); +b3Vec3 b3GetRevoluteJointTorque( b3World* world, b3JointSim* base ); +b3Vec3 b3GetSphericalJointTorque( b3World* world, b3JointSim* base ); +b3Vec3 b3GetWeldJointTorque( b3World* world, b3JointSim* base ); +b3Vec3 b3GetWheelJointTorque( b3World* world, b3JointSim* base ); + +void b3PrepareDistanceJoint( b3JointSim* base, b3StepContext* context ); +void b3PrepareMotorJoint( b3JointSim* base, b3StepContext* context ); +void b3PrepareParallelJoint( b3JointSim* base, b3StepContext* context ); +void b3PreparePrismaticJoint( b3JointSim* base, b3StepContext* context ); +void b3PrepareRevoluteJoint( b3JointSim* base, b3StepContext* context ); +void b3PrepareSphericalJoint( b3JointSim* base, b3StepContext* context ); +void b3PrepareWeldJoint( b3JointSim* base, b3StepContext* context ); +void b3PrepareWheelJoint( b3JointSim* base, b3StepContext* context ); + +void b3WarmStartDistanceJoint( b3JointSim* base, b3StepContext* context ); +void b3WarmStartMotorJoint( b3JointSim* base, b3StepContext* context ); +void b3WarmStartParallelJoint( b3JointSim* base, b3StepContext* context ); +void b3WarmStartPrismaticJoint( b3JointSim* base, b3StepContext* context ); +void b3WarmStartRevoluteJoint( b3JointSim* base, b3StepContext* context ); +void b3WarmStartSphericalJoint( b3JointSim* base, b3StepContext* context ); +void b3WarmStartWeldJoint( b3JointSim* base, b3StepContext* context ); +void b3WarmStartWheelJoint( b3JointSim* base, b3StepContext* context ); + +void b3SolveDistanceJoint( b3JointSim* base, b3StepContext* context, bool useBias ); +void b3SolveMotorJoint( b3JointSim* base, b3StepContext* context ); +void b3SolveParallelJoint( b3JointSim* base, b3StepContext* context ); +void b3SolvePrismaticJoint( b3JointSim* base, b3StepContext* context, bool useBias ); +void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBias ); +void b3SolveSphericalJoint( b3JointSim* base, b3StepContext* context, bool useBias ); +void b3SolveWeldJoint( b3JointSim* base, b3StepContext* context, bool useBias ); +void b3SolveWheelJoint( b3JointSim* base, b3StepContext* context, bool useBias ); + +void b3DrawDistanceJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB ); +void b3DrawParallelJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale ); +void b3DrawPrismaticJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale ); +void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale ); +void b3DrawSphericalJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale ); +void b3DrawWeldJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale ); +void b3DrawWheelJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale ); + +#ifdef __cplusplus +} +#endif diff --git a/src/manifold.c b/src/manifold.c new file mode 100644 index 0000000..6d4a6b2 --- /dev/null +++ b/src/manifold.c @@ -0,0 +1,246 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +// Dirk Gregorius contributed portions of this code + +#include "manifold.h" + +#include "algorithm.h" +#include "shape.h" + +#include "box3d/math_functions.h" + +// p1 : origin on edge 1 +// e1 : edge 1 +// c1 : shape 1 centroid +// p2 : origin on edge 2 +// e2 : edge 2 +// c2 : shape 2 centroid +float b3EdgeEdgeSeparation( b3Vec3 p1, b3Vec3 e1, b3Vec3 c1, b3Vec3 p2, b3Vec3 e2, b3Vec3 c2 ) +{ + // Build search direction + b3Vec3 u = b3Cross( e1, e2 ); + float length = b3Length( u ); + + // Skip near parallel edges: |e1 x e1| = sin(alpha) * |e1| * |e2| + const float kTolerance = 0.005f; + if ( length < kTolerance * sqrtf( b3LengthSquared( e1 ) * b3LengthSquared( e2 ) ) ) + { + return -FLT_MAX; + } + + if ( length * length < 1000.0f * FLT_MIN ) + { + return -FLT_MAX; + } + + b3Vec3 n = b3MulSV( 1.0f / length, u ); + + // Make sure normal points away from the first shape + // For a triangle, it is possible that N is aligned with the triangle normal and the sign + // value can be close to zero and flicker between small negative and positive values, leading to + // an incorrect separation value. So we assume the other hull has some volume and pick the most + // significant sign value to orient N. + float sign1 = b3Dot( n, b3Sub( p1, c1 ) ); + float sign2 = b3Dot( n, b3Sub( p2, c2 ) ); + if ( b3AbsFloat( sign1 ) > b3AbsFloat( sign2 ) ) + { + if ( sign1 < 0.0f ) + { + n = b3Neg( n ); + } + } + else + { + if ( sign2 > 0.0f ) + { + n = b3Neg( n ); + } + } + + // s = Dot(n, p2) - d = Dot(n, p2) - Dot(n, p1) = Dot(n, p2 - p1) + return b3Dot( n, b3Sub( p2, p1 ) ); +} + +// This was extended to make the wedge shape get the correct incident face. +// Instead of looking directly for the most anti-parallel face, we first find the closest vertex (passed in). +// Then we look for all edges coming out of that vertex and look for the edge that is +// most perpendicular to the reference normal. +// Then from that edge, we select the adjacent face that is most anti-parallel to the reference normal. +int b3FindIncidentFace( const b3Hull* hull, b3Vec3 refNormal, int vertexIndex ) +{ + const b3HullVertex* vertices = b3GetHullVertices( hull ); + const b3HullHalfEdge* edges = b3GetHullEdges( hull ); + const b3Plane* planes = b3GetHullPlanes( hull ); + const b3Vec3* points = b3GetHullPoints( hull ); + + int minEdgeIndex = -1; + float minEdgeProjection = FLT_MAX; + + const b3HullVertex* vertex = vertices + vertexIndex; + B3_ASSERT( vertex ); + + int edgeIndex = vertex->edge; + const b3HullHalfEdge* edge = edges + edgeIndex; + b3Vec3 edgeOrigin = points[edge->origin]; + B3_ASSERT( edge->origin == vertexIndex ); + + do + { + const b3HullHalfEdge* twin = edges + edge->twin; + b3Vec3 twinOrigin = points[twin->origin]; + + b3Vec3 axis = b3Normalize( b3Sub( twinOrigin, edgeOrigin ) ); + float edgeProjection = b3AbsFloat( b3Dot( axis, refNormal ) ); + if ( edgeProjection < minEdgeProjection ) + { + minEdgeIndex = edgeIndex; + minEdgeProjection = edgeProjection; + } + + edgeIndex = twin->next; + edge = edges + edgeIndex; + B3_ASSERT( edge->origin == vertexIndex ); + } + while ( edge != edges + vertex->edge ); + B3_ASSERT( minEdgeIndex >= 0 ); + + const b3HullHalfEdge* minEdge = edges + minEdgeIndex; + int minFaceIndex1 = minEdge->face; + b3Plane minPlane1 = planes[minFaceIndex1]; + + const b3HullHalfEdge* minTwin = edges + minEdge->twin; + int minFaceIndex2 = minTwin->face; + b3Plane minPlane2 = planes[minFaceIndex2]; + + return b3Dot( minPlane1.normal, refNormal ) < b3Dot( minPlane2.normal, refNormal ) ? minFaceIndex1 : minFaceIndex2; +} + +b3FeaturePair b3MakeFeaturePair( b3FeatureOwner owner1, int index1, b3FeatureOwner owner2, int index2 ) +{ + B3_ASSERT( 0 <= index1 && index1 <= UINT8_MAX ); + B3_ASSERT( 0 <= index2 && index2 <= UINT8_MAX ); + + b3FeaturePair pair; + pair.index1 = (uint8_t)index1; + pair.owner1 = (uint8_t)owner1; + pair.index2 = (uint8_t)index2; + pair.owner2 = (uint8_t)owner2; + return pair; +} + +// This logic seems wrong but it is designed so that choosing +// face A or B as the reference face does not change the resulting +// feature pair. This way the contact impulses are persisted even +// if there is reference face flip-flop. This is verified in the +// HullAndHull sample using the b3SATCache with a manual feature +// specified such as b3_manualFaceAxisA. +b3FeaturePair b3FlipPair( b3FeaturePair pair ) +{ + B3_ASSERT( pair.owner1 == 0 || pair.owner1 == 1 ); + B3_ASSERT( pair.owner2 == 0 || pair.owner2 == 1 ); + B3_SWAP( pair.owner1, pair.owner2 ); + pair.owner1 = 1 - pair.owner1; + pair.owner2 = 1 - pair.owner2; + B3_SWAP( pair.index1, pair.index2 ); + return pair; +} + +#if B3_ENABLE_VALIDATION +bool b3ValidatePolygon( b3ClipVertex* polygon, int count ) +{ + // Empty polygons are valid (we can clip away all points when re-constructing manifolds from cache) + if ( count == 0 ) + { + return true; + } + + // Validate that incoming and outgoing edges match + b3ClipVertex vertex1 = polygon[count - 1]; + for ( int i = 0; i < count; ++i ) + { + b3ClipVertex vertex2 = polygon[i]; + + if ( vertex1.pair.owner2 != vertex2.pair.owner1 ) + { + return false; + } + + if ( vertex1.pair.index2 != vertex2.pair.index1 ) + { + return false; + } + + vertex1 = vertex2; + } + + return true; +} +#endif + +int b3ClipPolygon( b3ClipVertex* out, b3ClipVertex* polygon, int count, b3Plane clipPlane, int edge, b3Plane refPlane ) +{ + B3_ASSERT( count >= 3 ); + + b3ClipVertex vertex1 = polygon[count - 1]; + float distance1 = b3PlaneSeparation( clipPlane, vertex1.position ); + int outCount = 0; + + for ( int index = 0; index < count; ++index ) + { + b3ClipVertex vertex2 = polygon[index]; + float distance2 = b3PlaneSeparation( clipPlane, vertex2.position ); + + // Clip edge against plane (Sutherland-Hodgman clipping) + if ( distance1 <= 0.0f && distance2 <= 0.0f ) + { + // Both vertices are behind the plane - keep vertex2 + out[outCount] = vertex2; + outCount += 1; + } + else if ( distance1 <= 0.0f && distance2 > 0.0f ) + { + // Vertex1 is behind of the plane, vertex2 is in front -> intersection point + float fraction = distance1 / ( distance1 - distance2 ); + b3Vec3 position = b3MulAdd( vertex1.position, fraction, b3Sub( vertex2.position, vertex1.position ) ); + + // Keep intersection point and adjust outgoing edge + b3ClipVertex vertex; + vertex.position = position; + vertex.separation = b3PlaneSeparation( refPlane, position ); + vertex.pair = vertex2.pair; + vertex.pair.owner2 = b3_featureShapeA; + vertex.pair.index2 = (uint8_t)edge; + out[outCount] = vertex; + outCount += 1; + } + else if ( distance2 <= 0.0f && distance1 > 0.0f ) + { + // Vertex1 is in front, vertex2 is behind of the plane, -> intersection point + float fraction = distance1 / ( distance1 - distance2 ); + b3Vec3 position = b3MulAdd( vertex1.position, fraction, b3Sub( vertex2.position, vertex1.position ) ); + + // Keep intersection point and adjust incoming edge + b3ClipVertex vertex; + vertex.position = position; + vertex.separation = b3PlaneSeparation( refPlane, position ); + vertex.pair = vertex1.pair; + vertex.pair.owner1 = b3_featureShapeA; + vertex.pair.index1 = (uint8_t)edge; + out[outCount] = vertex; + outCount += 1; + + // And also keep vertex2 + out[outCount] = vertex2; + outCount += 1; + } + + // Keep vertex2 as starting vertex for next edge + vertex1 = vertex2; + distance1 = distance2; + } + + B3_VALIDATE( b3ValidatePolygon( out, outCount ) ); + + return outCount; +} diff --git a/src/manifold.h b/src/manifold.h new file mode 100644 index 0000000..1ff93ed --- /dev/null +++ b/src/manifold.h @@ -0,0 +1,66 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box3d/collision.h" +#include "box3d/math_functions.h" + +#define B3_MAX_CLIP_POINTS 64 + +typedef struct b3FaceQuery +{ + float separation; + int faceIndex; + int vertexIndex; +} b3FaceQuery; + +typedef struct b3EdgeQuery +{ + float separation; + int indexA; + int indexB; +} b3EdgeQuery; + +typedef struct b3ClipVertex +{ + b3Vec3 position; + float separation; + b3FeaturePair pair; +} b3ClipVertex; + +typedef enum b3FeatureOwner +{ + b3_featureShapeA = 0, + b3_featureShapeB = 1 +} b3FeatureOwner; + +#ifdef __cplusplus +extern "C" +{ +#endif + +float b3EdgeEdgeSeparation( b3Vec3 p1, b3Vec3 e1, b3Vec3 c1, b3Vec3 p2, b3Vec3 e2, b3Vec3 c2 ); +int b3FindIncidentFace( const b3Hull* hull, b3Vec3 refNormal, int vertexIndex ); +b3FeaturePair b3MakeFeaturePair( b3FeatureOwner owner1, int index1, b3FeatureOwner owner2, int index2 ); + +b3FeaturePair b3FlipPair( b3FeaturePair pair ); + +int b3ClipPolygon( b3ClipVertex* out, b3ClipVertex* polygon, int count, b3Plane clipPlane, int edge, b3Plane refPlane ); + +#if B3_ENABLE_VALIDATION +bool b3ValidatePolygon( b3ClipVertex* polygon, int count ); +#endif + +#ifdef __cplusplus +} +#endif + +// For single point contact, such as sphere-sphere, sphere-capsule, sphere-triangle +static const b3FeaturePair b3FeaturePair_single = { 0 }; + +B3_INLINE uint32_t b3MakeFeatureId( b3FeaturePair pair ) +{ + return ( (uint32_t)pair.owner1 << 24 ) | ( (uint32_t)pair.index1 << 16 ) | ( (uint32_t)pair.owner2 << 8 ) | + (uint32_t)pair.index2; +} diff --git a/src/math_functions.c b/src/math_functions.c new file mode 100644 index 0000000..a458767 --- /dev/null +++ b/src/math_functions.c @@ -0,0 +1,587 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "box3d/math_functions.h" + +#include "math_internal.h" + +#include "box3d/collision.h" +#include "box3d/constants.h" + +#include +#include + +bool b3IsValidFloat( float a ) +{ + if ( isnan( a ) ) + { + return false; + } + + if ( isinf( a ) ) + { + return false; + } + + return true; +} + +bool b3IsValidVec3( b3Vec3 a ) +{ + if ( isnan( a.x ) || isnan( a.y ) || isnan( a.z ) ) + { + return false; + } + + if ( isinf( a.x ) || isinf( a.y ) || isinf( a.z ) ) + { + return false; + } + + return true; +} + +bool b3IsValidQuat( b3Quat a ) +{ + if ( isnan( a.v.x ) || isnan( a.v.y ) || isnan( a.v.z ) || isnan( a.s ) ) + { + return false; + } + + if ( isinf( a.v.x ) || isinf( a.v.y ) || isinf( a.v.z ) || isinf( a.s ) ) + { + return false; + } + + return b3IsNormalizedQuat( a ); +} + +bool b3IsValidTransform( b3Transform a ) +{ + return b3IsValidVec3( a.p ) && b3IsValidQuat( a.q ); +} + +bool b3IsValidMatrix3( b3Matrix3 a ) +{ + return b3IsValidVec3( a.cx ) && b3IsValidVec3( a.cy ) && b3IsValidVec3( a.cz ); +} + +bool b3IsValidAABB( b3AABB a ) +{ + if ( b3IsValidVec3( a.lowerBound ) == false ) + { + return false; + } + + if ( b3IsValidVec3( a.upperBound ) == false ) + { + return false; + } + + if ( a.lowerBound.x > a.upperBound.x ) + { + return false; + } + + if ( a.lowerBound.y > a.upperBound.y ) + { + return false; + } + + if ( a.lowerBound.z > a.upperBound.z ) + { + return false; + } + + return true; +} + +bool b3IsSaneAABB( b3AABB a ) +{ + if ( b3IsValidAABB( a ) == false ) + { + return false; + } + + if ( a.lowerBound.x < -B3_HUGE || a.lowerBound.y < -B3_HUGE || a.lowerBound.z < -B3_HUGE ) + { + return false; + } + + if ( a.upperBound.x > B3_HUGE || a.upperBound.y > B3_HUGE || a.upperBound.z > B3_HUGE ) + { + return false; + } + + return true; +} + +bool b3IsValidPlane( b3Plane a ) +{ + if ( b3IsValidVec3( a.normal ) == false ) + { + return false; + } + + if ( b3IsNormalized( a.normal ) == false ) + { + return false; + } + + return b3IsValidFloat( a.offset ); +} + +// https://stackoverflow.com/questions/46210708/atan2-approximation-with-11bits-in-mantissa-on-x86with-sse2-and-armwith-vfpv4 +float b3Atan2( float y, float x ) +{ + // Added check for (0,0) to match atan2f and avoid NaN + if ( x == 0.0f && y == 0.0f ) + { + return 0.0f; + } + + float ax = b3AbsFloat( x ); + float ay = b3AbsFloat( y ); + float mx = b3MaxFloat( ay, ax ); + float mn = b3MinFloat( ay, ax ); + float a = mn / mx; + + // Minimax polynomial approximation to atan(a) on [0,1] + float s = a * a; + float c = s * a; + float q = s * s; + float r = 0.024840285f * q + 0.18681418f; + float t = -0.094097948f * q - 0.33213072f; + r = r * s + t; + r = r * c + a; + + // Map to full circle + if ( ay > ax ) + { + r = 1.57079637f - r; + } + + if ( x < 0 ) + { + r = 3.14159274f - r; + } + + if ( y < 0 ) + { + r = -r; + } + + return r; +} + +// Approximate cosine and sine for determinism. In my testing cosf and sinf produced +// the same results on x64 and ARM using MSVC, GCC, and Clang. However, I don't trust +// this result. +// https://en.wikipedia.org/wiki/Bh%C4%81skara_I%27s_sine_approximation_formula +b3CosSin b3ComputeCosSin( float radians ) +{ +#if 0 + return { + cosf( radians ), + sinf( radians ), + }; +#else + float x = b3UnwindAngle( radians ); + float pi2 = B3_PI * B3_PI; + + // cosine needs angle in [-pi/2, pi/2] + float c; + if ( x < -0.5f * B3_PI ) + { + float y = x + B3_PI; + float y2 = y * y; + c = -( pi2 - 4.0f * y2 ) / ( pi2 + y2 ); + } + else if ( x > 0.5f * B3_PI ) + { + float y = x - B3_PI; + float y2 = y * y; + c = -( pi2 - 4.0f * y2 ) / ( pi2 + y2 ); + } + else + { + float y2 = x * x; + c = ( pi2 - 4.0f * y2 ) / ( pi2 + y2 ); + } + + // sine needs angle in [0, pi] + float s; + if ( x < 0.0f ) + { + float y = x + B3_PI; + s = -16.0f * y * ( B3_PI - y ) / ( 5.0f * pi2 - 4.0f * y * ( B3_PI - y ) ); + } + else + { + s = 16.0f * x * ( B3_PI - x ) / ( 5.0f * pi2 - 4.0f * x * ( B3_PI - x ) ); + } + + float mag = sqrtf( s * s + c * c ); + float invMag = mag > 0.0f ? 1.0f / mag : 0.0f; + b3CosSin cs = { c * invMag, s * invMag }; + return cs; +#endif +} + +b3Quat b3MakeQuatFromMatrix( const b3Matrix3* m ) +{ + b3Vec3 c1 = m->cx; + b3Vec3 c2 = m->cy; + b3Vec3 c3 = m->cz; + + b3Quat q; + + float trace = b3Trace( *m ); + if ( trace >= 0.0f ) + { + q.v.x = c2.z - c3.y; + q.v.y = c3.x - c1.z; + q.v.z = c1.y - c2.x; + q.s = trace + 1.0f; + } + else + { + if ( c1.x > c2.y && c1.x > c3.z ) + { + q.v.x = c1.x - c2.y - c3.z + 1.0f; + q.v.y = c2.x + c1.y; + q.v.z = c3.x + c1.z; + q.s = c2.z - c3.y; + } + else if ( c2.y > c3.z ) + { + q.v.x = c1.y + c2.x; + q.v.y = c2.y - c3.z - c1.x + 1.0f; + q.v.z = c3.y + c2.z; + q.s = c3.x - c1.z; + } + else + { + q.v.x = c1.z + c3.x; + q.v.y = c2.z + c3.y; + q.v.z = c3.z - c1.x - c2.y + 1.0f; + q.s = c1.y - c2.x; + } + } + + // The algorithm is simplified and made more accurate by normalizing at the end + return b3NormalizeQuat( q ); +} + +b3Quat b3ComputeQuatBetweenUnitVectors( b3Vec3 v1, b3Vec3 v2 ) +{ + B3_ASSERT( b3IsNormalized( v1 ) ); + B3_ASSERT( b3IsNormalized( v2 ) ); + + b3Quat out; + + b3Vec3 m = b3Lerp( v1, v2, 0.5f ); + float tolerance = 100.0f * FLT_EPSILON; + if ( b3LengthSquared( m ) > tolerance * tolerance ) + { + out.v = b3Cross( v1, m ); + out.s = b3Dot( v1, m ); + } + else + { + // Anti-parallel: Use a perpendicular vector + if ( b3AbsFloat( v1.x ) > 0.5f ) + { + out.v.x = v1.y; + out.v.y = -v1.x; + out.v.z = 0.0f; + } + else + { + out.v.x = 0.0f; + out.v.y = v1.z; + out.v.z = -v1.y; + } + + out.s = 0.0f; + } + + // The algorithm is simplified and made more accurate by normalizing at the end + return b3NormalizeQuat( out ); +} + +//-------------------------------------------------------------------------------------------------- +// Collision utilities +//-------------------------------------------------------------------------------------------------- + +//-------------------------------------------------------------------------------------------------- +b3ClosestApproachResult b3ClosestApproachLines( b3Vec3 p1, b3Vec3 d1, b3Vec3 p2, b3Vec3 d2 ) +{ + b3ClosestApproachResult result; + + // Solve A*x = b + float a11 = b3Dot( d1, d1 ); + float a12 = -b3Dot( d1, d2 ); + float a21 = b3Dot( d2, d1 ); + float a22 = -b3Dot( d2, d2 ); + + b3Vec3 w = b3Sub( p1, p2 ); + float b1 = -b3Dot( d1, w ); + float b2 = -b3Dot( d2, w ); + + float det = a11 * a22 - a12 * a21; + if ( det * det < 1000.0f * FLT_MIN ) + { + // Lines are parallel - project p2 onto line L1: x1 = p1 + s1 * d1 + float s1 = b3Dot( b3Sub( p2, p1 ), d1 ) / b3Dot( d1, d1 ); + float s2 = 0.0f; + + result.point1 = b3MulAdd( p1, s1, d1 ); + result.lambda1 = s1; + result.point2 = b3MulAdd( p2, s2, d2 ); + result.lambda2 = s2; + + return result; + } + + float s1 = ( a22 * b1 - a12 * b2 ) / det; + float s2 = ( a11 * b2 - a21 * b1 ) / det; + + result.point1 = b3MulAdd( p1, s1, d1 ); + result.lambda1 = s1; + result.point2 = b3MulAdd( p2, s2, d2 ); + result.lambda2 = s2; + return result; +} + +//-------------------------------------------------------------------------------------------------- +b3ClosestApproachResult b3ClosestApproachSegments( b3Vec3 p1, b3Vec3 q1, b3Vec3 p2, b3Vec3 q2 ) +{ + b3ClosestApproachResult result; + + b3Vec3 d1 = b3Sub( q1, p1 ); + b3Vec3 d2 = b3Sub( q2, p2 ); + b3Vec3 r = b3Sub( p1, p2 ); + + float a = b3Dot( d1, d1 ); + float b = b3Dot( d1, d2 ); + float c = b3Dot( d1, r ); + float e = b3Dot( d2, d2 ); + float f = b3Dot( d2, r ); + + // Check if one of the segments degenerates into a point + if ( a < 100.0f * FLT_EPSILON && e < 100.0f * FLT_EPSILON ) + { + // Both segments degenerate into points + result.point1 = p1; + result.lambda1 = 0.0f; + result.point2 = p2; + result.lambda2 = 0.0f; + + return result; + } + + if ( a < 100.0f * FLT_EPSILON ) + { + // First segment degenerates into a point + float s2 = b3ClampFloat( f / e, 0.0f, 1.0f ); + + result.point1 = p1; + result.lambda1 = 0.0f; + result.point2 = b3MulAdd( p2, s2, d2 ); + result.lambda2 = s2; + + return result; + } + + if ( e < 100.0f * FLT_EPSILON ) + { + // Second segment degenerates into a point + float s1 = b3ClampFloat( -c / a, 0.0f, 1.0f ); + + result.point1 = b3MulAdd( p1, s1, d1 ); + result.lambda1 = s1; + result.point2 = p2; + result.lambda2 = 0.0f; + + return result; + } + + // Non-degenerate case + float denom = a * e - b * b; + float s1 = denom > 1000.0f * FLT_MIN ? b3ClampFloat( ( b * f - c * e ) / denom, 0.0f, 1.0f ) : 0.0f; + float s2 = ( b * s1 + f ) / e; + + // Clamp lambda2 and recompute lambda1 if necessary + if ( s2 < 0.0f ) + { + s1 = b3ClampFloat( -c / a, 0.0f, 1.0f ); + s2 = 0.0f; + } + else if ( s2 > 1.0f ) + { + s1 = b3ClampFloat( ( b - c ) / a, 0.0f, 1.0f ); + s2 = 1.0f; + } + + result.point1 = b3MulAdd( p1, s1, d1 ); + result.lambda1 = s1; + result.point2 = b3MulAdd( p2, s2, d2 ); + result.lambda2 = s2; + + return result; +} + +b3Vec3 b3ClosestPointOnSegment( b3Vec3 a, b3Vec3 b, b3Vec3 q ) +{ + b3Vec3 ab = b3Sub( b, a ); + b3Vec3 aq = b3Sub( q, a ); + + float alpha = b3Dot( ab, aq ); + + if ( alpha <= 0.0f ) + { + // q projects outside interval [a, b] on the side of a + return a; + } + else + { + float denominator = b3Dot( ab, ab ); + if ( alpha > denominator ) + { + // q projects outside interval [a, b] on the side of b + return b; + } + else + { + // q projects inside interval [a, b] + alpha /= denominator; + return b3MulAdd( a, alpha, ab ); + } + } +} + +b3TrianglePoint b3ClosestPointOnTriangle( b3Vec3 a, b3Vec3 b, b3Vec3 c, b3Vec3 q ) +{ + // Check if P lies in vertex region of A + b3Vec3 ab = b3Sub( b, a ); + b3Vec3 ac = b3Sub( c, a ); + b3Vec3 aq = b3Sub( q, a ); + + float d1 = b3Dot( ab, aq ); + float d2 = b3Dot( ac, aq ); + if ( d1 <= 0.0f && d2 <= 0.0f ) + { + return (b3TrianglePoint){ a, b3_featureVertex1 }; + } + + // Check if P lies in vertex region of B + b3Vec3 bq = b3Sub( q, b ); + + float d3 = b3Dot( ab, bq ); + float d4 = b3Dot( ac, bq ); + if ( d3 > 0.0f && d4 <= d3 ) + { + return (b3TrianglePoint){ b, b3_featureVertex2 }; + } + + // Check if P lies in edge region AB + float vc = d1 * d4 - d3 * d2; + if ( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f ) + { + float t = d1 / ( d1 - d3 ); + return (b3TrianglePoint){ b3MulAdd( a, t, ab ), b3_featureEdge1 }; + } + + // Check if P lies in vertex region of C + b3Vec3 cq = b3Sub( q, c ); + + float d5 = b3Dot( ab, cq ); + float d6 = b3Dot( ac, cq ); + if ( d6 >= 0.0f && d5 <= d6 ) + { + return (b3TrianglePoint){ c, b3_featureVertex3 }; + } + + // Check if P lies in edge region AC + float vb = d5 * d2 - d1 * d6; + if ( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f ) + { + float t = d2 / ( d2 - d6 ); + return (b3TrianglePoint){ b3MulAdd( a, t, ac ), b3_featureEdge3 }; + } + + // Check if P lies in edge region of BC + float va = d3 * d6 - d5 * d4; + if ( va <= 0.0f && d4 >= d3 && d5 >= d6 ) + { + b3Vec3 bc = b3Sub( c, b ); + + float t = ( d4 - d3 ) / ( ( d4 - d3 ) + ( d5 - d6 ) ); + return (b3TrianglePoint){ b3MulAdd( b, t, bc ), b3_featureEdge2 }; + } + + // P inside face region ABC + float t1 = vb / ( va + vb + vc ); + float t2 = vc / ( va + vb + vc ); + + b3Vec3 p = b3MulAdd( a, t1, ab ); + p = b3MulAdd( p, t2, ac ); + return (b3TrianglePoint){ p, b3_featureTriangleFace }; +} + +b3Matrix3 b3SphereInertia( float mass, float radius ) +{ + float i = 0.4f * mass * radius * radius; + return b3MakeDiagonalMatrix( i, i, i ); +} + +b3Matrix3 b3CylinderInertia( float mass, float radius, float height ) +{ + float ixx = mass * ( 3 * radius * radius + height * height ) / 12.0f; + float iyy = 0.5f * mass * radius * radius; + return b3MakeDiagonalMatrix( ixx, iyy, ixx ); +} + +b3Matrix3 b3BoxInertia( float mass, b3Vec3 min, b3Vec3 max ) +{ + b3Vec3 delta = b3Sub( max, min ); + float ixx = mass * ( delta.y * delta.y + delta.z * delta.z ) / 12.0f; + float iyy = mass * ( delta.x * delta.x + delta.z * delta.z ) / 12.0f; + float izz = mass * ( delta.x * delta.x + delta.y * delta.y ) / 12.0f; + + return b3MakeDiagonalMatrix( ixx, iyy, izz ); +} + +// https://en.wikipedia.org/wiki/Parallel_axis_theorem +b3Matrix3 b3Steiner( float mass, b3Vec3 origin ) +{ + // Usage: Io = Ic + Is and Ic = Io - Is + float ixx = mass * ( origin.y * origin.y + origin.z * origin.z ); + float iyy = mass * ( origin.x * origin.x + origin.z * origin.z ); + float izz = mass * ( origin.x * origin.x + origin.y * origin.y ); + float ixy = -mass * origin.x * origin.y; + float ixz = -mass * origin.x * origin.z; + float iyz = -mass * origin.y * origin.z; + + // Write + b3Matrix3 out; + out.cx.x = ixx; + out.cy.x = ixy; + out.cz.x = ixz; + out.cx.y = ixy; + out.cy.y = iyy; + out.cz.y = iyz; + out.cx.z = ixz; + out.cy.z = iyz; + out.cz.z = izz; + + return out; +} + +bool b3IsValidRay( const b3RayCastInput* input ) +{ + bool isValid = b3IsValidVec3( input->origin ) && b3IsValidVec3( input->translation ) && + b3IsValidFloat( input->maxFraction ) && 0.0f <= input->maxFraction && input->maxFraction < B3_HUGE; + return isValid; +} diff --git a/src/math_internal.h b/src/math_internal.h new file mode 100644 index 0000000..40b8d54 --- /dev/null +++ b/src/math_internal.h @@ -0,0 +1,513 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "core.h" + +#include "box3d/collision.h" +#include "box3d/math_functions.h" + +struct b3Sweep; +struct b3Plane; + +#define B3_TWO_PI 6.283185307f +#define B3_PI_OVER_TWO 1.570796327f +#define B3_PI_OVER_FOUR 0.785398163f +#define B3_SQRT3 1.732050808f + +// todo eliminate this +static const b3AABB B3_BOUNDS3_EMPTY = { { FLT_MAX, FLT_MAX, FLT_MAX }, { -FLT_MAX, -FLT_MAX, -FLT_MAX } }; + +typedef struct b3Matrix2 +{ + b3Vec2 cx, cy; +} b3Matrix2; + +typedef struct b3Triangle +{ + b3Vec3 vertices[3]; + int i1, i2, i3; + int flags; +} b3Triangle; + +typedef struct +{ + b3Vec3 point; + b3TriangleFeature feature; +} b3TrianglePoint; + +typedef struct b3ShapeExtent +{ + float minExtent; + b3Vec3 maxExtent; +} b3ShapeExtent; + +#ifdef __cplusplus +extern "C" +{ +#endif + +b3TrianglePoint b3ClosestPointOnTriangle( b3Vec3 a, b3Vec3 b, b3Vec3 c, b3Vec3 q ); + +float b3IntersectSegmentTriangle( b3Vec3 p, b3Vec3 q, b3Vec3 a, b3Vec3 b, b3Vec3 c ); +float b3IntersectSegmentSphere( b3Vec3 p, b3Vec3 q, b3Vec3 c, float r ); + +b3MassData b3ComputeMassProperties( int triangleCount, const int* triangles, int vertexCount, const b3Vec3* vertices, + float density ); + +bool b3IsValidMassData( const b3MassData* massData ); + +b3Matrix3 b3SphereInertia( float mass, float radius ); +b3Matrix3 b3CylinderInertia( float mass, float radius, float height ); +b3Matrix3 b3BoxInertia( float mass, b3Vec3 min, b3Vec3 max ); + +// Inertia helper (Io = Ic + Is and Ic = Io - Is) +b3Matrix3 b3Steiner( float mass, b3Vec3 origin ); +int b3GetProxySupport( const b3ShapeProxy* proxy, b3Vec3 axis ); +int b3GetPointSupport( const b3Vec3* points, int count, b3Vec3 axis ); + +bool b3IntersectRayAndAABB( b3Vec3 lowerBound, b3Vec3 upperBound, b3Vec3 p1, b3Vec3 p2, float* minFraction, float* maxFraction ); + +#ifdef __cplusplus +} +#endif + +// https://en.wikipedia.org/wiki/Floor_and_ceiling_functions +B3_INLINE int b3CeilingInt( int numerator, int denominator ) +{ + B3_VALIDATE( denominator > 0 ); + return ( numerator + denominator - 1 ) / denominator; +} + +// Assumes denominator == 2^exponent +B3_INLINE int b3CeilingPow2( int numerator, int denominator, int exponent ) +{ + B3_VALIDATE( exponent > 0 && ( denominator == 1 << exponent ) ); + return ( numerator + denominator - 1 ) >> exponent; +} + +bool b3IsSweepNormalized( b3Sweep* sweep ); + +B3_INLINE float b3Dot2( b3Vec2 v1, b3Vec2 v2 ) +{ + return v1.x * v2.x + v1.y * v2.y; +} + +B3_INLINE float b3Length2( b3Vec2 v ) +{ + return sqrtf( b3Dot2( v, v ) ); +} + +B3_INLINE float b3LengthSquared2( b3Vec2 v ) +{ + return b3Dot2( v, v ); +} + +B3_INLINE b3Vec2 b3MinVec2( b3Vec2 v1, b3Vec2 v2 ) +{ + b3Vec2 v; + v.x = b3MinFloat( v1.x, v2.x ); + v.y = b3MinFloat( v1.y, v2.y ); + return v; +} + +B3_INLINE b3Vec2 b3MaxVec2( b3Vec2 v1, b3Vec2 v2 ) +{ + b3Vec2 v; + v.x = b3MaxFloat( v1.x, v2.x ); + v.y = b3MaxFloat( v1.y, v2.y ); + return v; +} + +B3_INLINE void b3Store( float* dst, b3Vec3 src ) +{ + dst[0] = src.x; + dst[1] = src.y; + dst[2] = src.z; +} + +B3_INLINE b3Vec3 b3ClampLength( b3Vec3 v, float maxLength ) +{ + float lengthSq = b3LengthSquared( v ); + if ( lengthSq <= maxLength * maxLength ) + { + return v; + } + + float length = sqrtf( lengthSq ); + return b3MulSV( maxLength / length, v ); +} + +// Assume v is a unit vector +B3_INLINE b3Vec3 b3ArbitraryPerp( b3Vec3 v ) +{ + // Suppose vector a has all equal components and is a unit vector: a = (s, s, s) + // Then 3*s*s = 1, s = sqrt(1/3) = 0.57735. This means that at least one component + // of a unit vector must be greater or equal to 0.57735. + b3Vec3 p; + if ( v.x < -0.5f || 0.5f < v.x ) + { + // x is non-zero and it should not go into the x component + // dot([ay + bz, cx, dx], [x, y, z]) = ayx + bzx + cxy + dzx + // for the dot product to be zero need: c = -a, d = -b + float a = 0.67f; + float b = -0.42f; + p = B3_LITERAL( b3Vec3 ){ a * v.y + b * v.z, -a * v.x, -b * v.x }; + } + else if ( v.y < -0.5f || 0.5f < v.y ) + { + // y is non-zero and it should not go into the y component + // p = [ay, bx + cz, dy] + // axy + bxy + cyz + dyz = 0 + // b = -a, d = -c + float a = 0.67f; + float c = -0.42f; + p = B3_LITERAL( b3Vec3 ){ a * v.y, -a * v.x + c * v.z, -c * v.y }; + } + else + { + // This would trip if the input is not a unit vector + B3_VALIDATE( v.z < -0.5f || 0.5f < v.z ); + + // z is non-zero and it should not go into the z component + // p = [az, bz, cx + dy] + // axz + byz + cxz + dyz = 0 + // c = -a, d = -b + float a = 0.67f; + float b = -0.42f; + p = B3_LITERAL( b3Vec3 ){ a * v.z, b * v.z, -a * v.x - b * v.y }; + } + + B3_VALIDATE( b3LengthSquared( p ) > 0.1f ); + B3_VALIDATE( b3AbsFloat( b3Dot( p, v ) ) < 100.0f * FLT_EPSILON ); + + return b3Normalize( p ); +} + +B3_INLINE b3Quat b3QuatFromExponentialMap( b3Vec3 v ) +{ + // Exponential map (Grassia) + float threshold = 0.018581361f; + + float angle = b3Length( v ); + if ( angle < threshold ) + { + // Taylor expansion + b3Quat out; + out.v = b3MulSV( 0.5f + angle * angle / 48.0f, v ); + out.s = b3Cos( 0.5f * angle ); + + return out; + } + + return b3MakeQuatFromAxisAngle( b3MulSV( 1.0f / angle, v ), angle ); +} + +/// Integrate rotation from angular velocity +/// @param q1 initial rotation +/// @param deltaRotation the angular displacement vector in radians (angular velocity multiplied by the time step) +/// q2 = q1 + 0.5 * omega * q1 +B3_INLINE b3Quat b3IntegrateRotation( b3Quat q1, b3Vec3 deltaRotation ) +{ + // https://fgiesen.wordpress.com/2012/08/24/quaternion-differentiation/ + b3Quat qd = { b3MulSV( 0.5f, deltaRotation ), 0.0f }; + qd = b3MulQuat( qd, q1 ); + b3Quat q2 = { b3Add( q1.v, qd.v ), qd.s + q1.s }; + q2 = b3NormalizeQuat( q2 ); + return q2; + + // return b3NormalizeQuat( b3MulQuat(b3QuatFromExponentialMap( deltaRotation ), q1) ); +} + +// Pseudo angular velocity from a quaternion target +// w = 2 * (target - q) * conj(q) +B3_INLINE b3Vec3 b3DeltaQuatToRotation(b3Quat q, b3Quat target) +{ + b3Quat s = q; + if (b3DotQuat(q, target) < 0.0f) + { + // Correct polarity + s = b3NegateQuat( q ); + } + + b3Quat diff = { b3Sub( target.v, s.v ), target.s - s.s }; + b3Quat product = b3MulQuat( diff, b3Conjugate( s ) ); + return b3MulSV( 2.0f, product.v ); +} + +B3_INLINE float b3ScalarTripleProduct( b3Vec3 a, b3Vec3 b, b3Vec3 c ) +{ + b3Vec3 d; + d.x = b.y * c.z - b.z * c.y; + d.y = b.z * c.x - b.x * c.z; + d.z = b.x * c.y - b.y * c.x; + return a.x * d.x + a.y * d.y + a.z * d.z; +} + +// Get a value by index. Avoid undefined behavior of code like (&v.x)[2]. +B3_INLINE float b3GetByIndex( b3Vec3 v, int index ) +{ + B3_ASSERT( 0 <= index && index < 3 ); + float temp[3] = { v.x, v.y, v.z }; + return temp[index]; +} + +B3_INLINE int b3MajorAxis( b3Vec3 v ) +{ + return v.x < v.y ? ( v.y < v.z ? 2 : 1 ) : ( v.x < v.z ? 2 : 0 ); +} + +B3_INLINE float b3MinElement( b3Vec3 v ) +{ + return b3MinFloat( v.x, b3MinFloat( v.y, v.z ) ); +} + +B3_INLINE float b3MaxElement( b3Vec3 v ) +{ + return b3MaxFloat( v.x, b3MaxFloat( v.y, v.z ) ); +} + +B3_INLINE int b3MaxElementIndex( b3Vec3 v ) +{ + return v.x < v.y ? ( v.y < v.z ? 2 : 1 ) : ( v.x < v.z ? 2 : 0 ); +} + +B3_INLINE b3Vec2 b3Add2( b3Vec2 a, b3Vec2 b ) +{ + b3Vec2 c = { a.x + b.x, a.y + b.y }; + return c; +} + +B3_INLINE b3Vec2 b3Sub2( b3Vec2 a, b3Vec2 b ) +{ + b3Vec2 c = { a.x - b.x, a.y - b.y }; + return c; +} + +B3_INLINE b3Vec2 b3Neg2( b3Vec2 v ) +{ + b3Vec2 c = { -v.x, -v.y }; + return c; +} + +B3_INLINE b3Vec2 b3MulSV2( float s, b3Vec2 v ) +{ + b3Vec2 c = { s * v.x, s * v.y }; + return c; +} + +// a + s * b +B3_INLINE b3Vec2 b3MulAdd2( b3Vec2 a, float s, b3Vec2 b ) +{ + b3Vec2 c = { a.x + s * b.x, a.y + s * b.y }; + return c; +} + +// a - s * b +B3_INLINE b3Vec2 b3MulSub2( b3Vec2 a, float s, b3Vec2 b ) +{ + b3Vec2 c = { a.x - s * b.x, a.y - s * b.y }; + return c; +} + +B3_INLINE float b3Cross2( b3Vec2 a, b3Vec2 b ) +{ + return a.x * b.y - a.y * b.x; +} + +B3_INLINE float b3DistanceSquared2( b3Vec2 a, b3Vec2 b ) +{ + float dx = b.x - a.x; + float dy = b.y - a.y; + return dx * dx + dy * dy; +} + +B3_INLINE b3Vec2 b3MulMV2( b3Matrix2 m, b3Vec2 a ) +{ + b3Vec2 b = { m.cx.x * a.x + m.cy.x * a.y, m.cx.y * a.x + m.cy.y * a.y }; + return b; +} + +B3_INLINE b3Matrix2 b3MulMM2( b3Matrix2 m1, b3Matrix2 m2 ) +{ + b3Matrix2 out; + out.cx = b3MulMV2( m1, m2.cx ); + out.cy = b3MulMV2( m1, m2.cy ); + return out; +} + +B3_INLINE float b3Det2( b3Matrix2 m ) +{ + return m.cx.x * m.cy.y - m.cx.y * m.cy.x; +} + +B3_INLINE b3Matrix2 b3Invert2( b3Matrix2 m ) +{ + float det = b3Det2( m ); + if ( b3AbsFloat( det ) > 1000.0f * FLT_MIN ) + { + float invDet = 1.0f / det; + return B3_LITERAL( b3Matrix2 ){ + { invDet * m.cy.y, -invDet * m.cx.y }, + { -invDet * m.cy.x, invDet * m.cx.x }, + }; + } + + return B3_LITERAL( b3Matrix2 ){ { 0.0f, 0.0f }, { 0.0f, 0.0f } }; +} + +// Assumes positive semi-definite +B3_INLINE b3Vec2 b3Solve2( b3Matrix2 m, b3Vec2 b ) +{ + float det = b3Det2( m ); + if ( det > 1000.0f * FLT_MIN ) + { + float invDet = 1.0f / det; + return B3_LITERAL( b3Vec2 ){ + invDet * m.cy.y * b.x - invDet * m.cy.x * b.y, + -invDet * m.cx.y * b.x + invDet * m.cx.x * b.y, + }; + } + + return B3_LITERAL( b3Vec2 ){ 0.0f, 0.0f }; +} + +B3_INLINE b3Vec3 b3ModifiedCross(b3Vec3 a, b3Vec3 b) +{ + b3Vec3 c; + c.x = a.y * b.z + a.z * b.y; + c.y = a.z * b.x + a.x * b.z; + c.z = a.x * b.y + a.y * b.x; + return c; +} + +B3_INLINE b3Plane b3NormalizePlane( b3Plane plane ) +{ + float invLength = 1.0f / b3Length( plane.normal ); + return B3_LITERAL( b3Plane ){ b3MulSV( invLength, plane.normal ), invLength * plane.offset }; +} + +// Negative if p is below the triangle v1-v2-v3 +B3_INLINE float b3SignedVolume( b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, b3Vec3 p ) +{ + b3Vec3 e1 = b3Sub( v2, v1 ); + b3Vec3 e2 = b3Sub( v3, v1 ); + b3Vec3 n = b3Cross( e1, e2 ); + return b3Dot( n, b3Sub( p, v1 ) ); +} + +// todo eliminate this +B3_INLINE bool b3IsWithinSegments( const b3ClosestApproachResult* result ) +{ + return ( 0.0f <= result->lambda1 && result->lambda1 <= 1.0f ) && ( 0.0f <= result->lambda2 && result->lambda2 <= 1.0f ); +} + +B3_INLINE b3Matrix3 b3RotateInertia( b3Quat q, b3Matrix3 centralInertia ) +{ + b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( q ); + b3Matrix3 inertia = b3MulMM( rotationMatrix, b3MulMM( centralInertia, b3Transpose( rotationMatrix ) ) ); + return inertia; +} + +B3_INLINE b3Matrix3 b3TransformInertia( b3Transform transform, b3Matrix3 centralInertia, float mass ) +{ + b3Matrix3 inertia = b3RotateInertia( transform.q, centralInertia ); + inertia = b3AddMM( inertia, b3Steiner( mass, transform.p ) ); + return inertia; +} + +#ifdef __cplusplus + +B3_INLINE b3Vec2& operator+=( b3Vec2& a, b3Vec2 b ) +{ + a.x += b.x; + a.y += b.y; + return a; +} + +B3_INLINE b3Vec2& operator*=( b3Vec2& a, float s ) +{ + a.x *= s; + a.y *= s; + return a; +} + +B3_INLINE b3Vec2 operator-( b3Vec2 v ) +{ + return { -v.x, -v.y }; +} + +B3_INLINE b3Vec2 operator+( b3Vec2 v1, b3Vec2 v2 ) +{ + b3Vec2 out; + out.x = v1.x + v2.x; + out.y = v1.y + v2.y; + + return out; +} + +B3_INLINE b3Vec2 operator-( b3Vec2 v1, b3Vec2 v2 ) +{ + b3Vec2 out; + out.x = v1.x - v2.x; + out.y = v1.y - v2.y; + + return out; +} + +B3_INLINE b3Vec2 operator*( float f, b3Vec2 v ) +{ + b3Vec2 out; + out.x = f * v.x; + out.y = f * v.y; + + return out; +} + +B3_INLINE b3Vec2 operator*( b3Vec2 v, float f ) +{ + b3Vec2 out; + out.x = v.x * f; + out.y = v.y * f; + + return out; +} + +B3_INLINE b3Matrix2 operator+( b3Matrix2 m1, b3Matrix2 m2 ) +{ + b3Matrix2 out; + out.cx = m1.cx + m2.cx; + out.cy = m1.cy + m2.cy; + + return out; +} + +B3_INLINE b3Matrix2 operator-( b3Matrix2 m1, b3Matrix2 m2 ) +{ + b3Matrix2 out; + out.cx = m1.cx - m2.cx; + out.cy = m1.cy - m2.cy; + + return out; +} + +B3_INLINE b3Matrix2 operator*( float f, b3Matrix2 m ) +{ + b3Matrix2 out; + out.cx = f * m.cx; + out.cy = f * m.cy; + + return out; +} + +B3_INLINE b3Matrix2 operator*( b3Matrix2 m, float f ) +{ + b3Matrix2 out; + out.cx = m.cx * f; + out.cy = m.cy * f; + + return out; +} + +#endif diff --git a/src/mesh.c b/src/mesh.c new file mode 100644 index 0000000..80e7b41 --- /dev/null +++ b/src/mesh.c @@ -0,0 +1,2342 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +// Dirk Gregorius contributed portions of this code + +#include "container.h" +#include "math_internal.h" +#include "shape.h" +#include "simd.h" + +#include "box3d/collision.h" +#include "box3d/constants.h" + +#include + +b3DeclareArray( b3VertexNode ); +b3DeclareArray( b3MeshNode ); +b3DeclareArray( b3MeshTriangle ); +b3DeclareArray( b3Vec3 ); +b3DeclareArray( b3Primitive ); +b3DeclareArrayNative( uint8_t ); + +#define B3_BIN_COUNT 8 +#define B3_DESIRED_TRIANGLES_PER_LEAF 4 +#define B3_LEAF_NODE 3 +#define B3_MAXIMUM_TRIANGLES_PER_LEAF 8 +#define B3_MESH_STACK_SIZE 256 + +static bool b3IsLeaf( const b3MeshNode* node ) +{ + return node->data.asLeaf.type == B3_LEAF_NODE; +} + +static b3MeshNode* b3GetMeshNodesWrite( b3MeshData* mesh ) +{ + if ( mesh->nodeOffset == 0 ) + { + return NULL; + } + + return (b3MeshNode*)( (intptr_t)mesh + mesh->nodeOffset ); +} + +static b3MeshNode* b3GetLeftChildWrite( b3MeshNode* node ) +{ + // The left child follows its parent. + B3_ASSERT( !b3IsLeaf( node ) ); + return node + 1; +} + +static const b3MeshNode* b3GetLeftChild( const b3MeshNode* node ) +{ + // The left child follows its parent. + B3_ASSERT( !b3IsLeaf( node ) ); + return node + 1; +} + +static b3MeshNode* b3GetRightChildWrite( b3MeshNode* node ) +{ + // We store the offset of the right child relative to its parent + B3_ASSERT( !b3IsLeaf( node ) ); + return node + node->data.asNode.childOffset; +} + +static const b3MeshNode* b3GetRightChild( const b3MeshNode* node ) +{ + // We store the offset of the right child relative to its parent + B3_ASSERT( !b3IsLeaf( node ) ); + return node + node->data.asNode.childOffset; +} + +static const b3MeshNode* b3GetRoot( const b3MeshData* mesh ) +{ + // The first node is the root + return b3GetMeshNodes( mesh ); +} + +static b3MeshNode* b3GetRootWrite( b3MeshData* mesh ) +{ + // The first node is the root + return b3GetMeshNodesWrite( mesh ); +} + +static b3MeshTriangle* b3GetMeshTrianglesWrite( b3MeshData* mesh ) +{ + if ( mesh->triangleOffset == 0 ) + { + return NULL; + } + + return (b3MeshTriangle*)( (intptr_t)mesh + mesh->triangleOffset ); +} + +static b3Vec3* b3GetMeshVerticesWrite( b3MeshData* mesh ) +{ + if ( mesh->vertexOffset == 0 ) + { + return NULL; + } + + return (b3Vec3*)( (intptr_t)mesh + mesh->vertexOffset ); +} + +// static b3Vec3 b3GetVertex( b3MeshData& mesh, int vertexIndex ) +//{ +// B3_ASSERT( 0 <= vertexIndex && vertexIndex < mesh.vertexCount ); +// b3Vec3* vertices = b3GetMeshVertices( &mesh ); +// return vertices[vertexIndex]; +// } + +static uint8_t* b3GetMeshMaterialIndicesWrite( b3MeshData* mesh ) +{ + if ( mesh->materialOffset == 0 ) + { + return NULL; + } + + return (uint8_t*)( (intptr_t)mesh + mesh->materialOffset ); +} + +static uint8_t* b3GetMeshFlagsWrite( b3MeshData* mesh ) +{ + if ( mesh->flagsOffset == 0 ) + { + return NULL; + } + + return (uint8_t*)( (intptr_t)mesh + mesh->flagsOffset ); +} + +static int b3GetNodeHeight( const b3MeshNode* node ) +{ + if ( b3IsLeaf( node ) ) + { + return 0; + } + + const b3MeshNode* leftChild = b3GetLeftChild( node ); + int leftHeight = b3GetNodeHeight( leftChild ); + const b3MeshNode* rightChild = b3GetRightChild( node ); + int rightHeight = b3GetNodeHeight( rightChild ); + + return 1 + b3MaxInt( leftHeight, rightHeight ); +} + +int b3GetHeight( const b3MeshData* mesh ) +{ + const b3MeshNode* root = b3GetRoot( mesh ); + if ( root == NULL ) + { + return 0; + } + + return b3GetNodeHeight( root ); +} + +#if B3_ENABLE_VALIDATION == 1 +static bool b3IsDegenerate( b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, float minArea ) +{ + b3Vec3 normal = b3Cross( b3Sub( v2, v1 ), b3Sub( v3, v1 ) ); + float lengthSq = b3LengthSquared( normal ); + return lengthSq < minArea * minArea; +} + +static bool b3IsNonDegenerate( const b3MeshData* mesh, float minArea ) +{ + const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh ); + const b3Vec3* vertices = b3GetMeshVertices( mesh ); + + // Check triangles + for ( int index = 0; index < mesh->triangleCount; ++index ) + { + // Index range + b3MeshTriangle triangle = triangles[index]; + if ( triangle.index1 >= mesh->vertexCount ) + { + return false; + } + + if ( triangle.index2 >= mesh->vertexCount ) + { + return false; + } + + if ( triangle.index3 >= mesh->vertexCount ) + { + return false; + } + + // Degenerate topology + if ( triangle.index1 == triangle.index2 ) + { + return false; + } + if ( triangle.index1 == triangle.index3 ) + { + return false; + } + if ( triangle.index2 == triangle.index3 ) + { + return false; + } + + // Degenerate geometry + b3Vec3 vertex1 = vertices[triangle.index1]; + b3Vec3 vertex2 = vertices[triangle.index2]; + b3Vec3 vertex3 = vertices[triangle.index3]; + if ( b3IsDegenerate( vertex1, vertex2, vertex3, minArea ) ) + { + return false; + } + } + + return true; +} + +static inline b3AABB b3GetNodeAABB( const b3MeshNode* node ) +{ + return (b3AABB){ + node->lowerBound, + node->upperBound, + }; +} + +static bool b3IsConsistent( const b3MeshData* mesh ) +{ + const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh ); + const b3Vec3* vertices = b3GetMeshVertices( mesh ); + + // Check nodes + int count = 0; + const b3MeshNode* stack[64]; + stack[count++] = b3GetRoot( mesh ); + + while ( count > 0 ) + { + const b3MeshNode* node = stack[--count]; + b3AABB nodeBounds = b3GetNodeAABB( node ); + + if ( b3IsLeaf( node ) == false ) + { + const b3MeshNode* child1 = b3GetLeftChild( node ); + b3AABB bounds1 = b3GetNodeAABB( child1 ); + const b3MeshNode* child2 = b3GetRightChild( node ); + b3AABB bounds2 = b3GetNodeAABB( child2 ); + + if ( !b3AABB_Contains( nodeBounds, bounds1 ) ) + { + return false; + } + + if ( !b3AABB_Contains( nodeBounds, bounds2 ) ) + { + return false; + } + + stack[count++] = child2; + stack[count++] = child1; + } + else + { + b3AABB triangleBounds = B3_BOUNDS3_EMPTY; + for ( uint32_t index = 0; index < node->data.asLeaf.triangleCount; ++index ) + { + int triangleIndex = node->triangleOffset + index; + B3_ASSERT( 0 <= triangleIndex && triangleIndex < mesh->triangleCount ); + + b3MeshTriangle triangle = triangles[triangleIndex]; + + b3AABB vertexBounds = B3_BOUNDS3_EMPTY; + vertexBounds = b3AABB_AddPoint( vertexBounds, vertices[triangle.index1] ); + vertexBounds = b3AABB_AddPoint( vertexBounds, vertices[triangle.index2] ); + vertexBounds = b3AABB_AddPoint( vertexBounds, vertices[triangle.index3] ); + + triangleBounds = b3AABB_Union( triangleBounds, vertexBounds ); + } + + if ( !b3AABB_Contains( nodeBounds, triangleBounds ) ) + { + return false; + } + } + } + + return true; +} + +bool b3IsValidMesh( const b3MeshData* meshData ) +{ + if ( meshData == NULL ) + { + return false; + } + + if ( meshData->version != B3_MESH_VERSION ) + { + return false; + } + + if ( meshData->byteCount < (int)sizeof( b3MeshData ) ) + { + return false; + } + + return b3IsConsistent( meshData ); +} + +#else + +bool b3IsValidMesh( const b3MeshData* meshData ) +{ + if ( meshData == NULL ) + { + return false; + } + + if ( meshData->version != B3_MESH_VERSION ) + { + return false; + } + + if ( meshData->byteCount < (int)sizeof( b3MeshData ) ) + { + return false; + } + + return true; +} + +#endif + +// Node for a vertex linked list +typedef struct b3VertexNode +{ + int32_t vertexIndex; + int nextNodeIndex; +} b3VertexNode; + +#define NAME b3VertexMap +#define KEY_TY uint64_t +#define VAL_TY int +#define HASH_FN vt_hash_integer +#define CMPR_FN vt_cmpr_integer +#define MALLOC_FN b3Alloc +#define FREE_FN b3Free +#include "verstable.h" + +typedef struct b3SpatialHash +{ + b3ArrayC( b3VertexNode ) nodes; + const b3Vec3* vertices; + int vertexCount; + b3VertexMap vertexMap; + float cellSize; + float tolerance; +} b3SpatialHash; + +static void b3SpatialHash_Create( b3SpatialHash* h, const b3Vec3* vertices, int vertexCount, float tolerance ) +{ + h->vertices = vertices; + h->vertexCount = vertexCount; + h->tolerance = tolerance; + h->cellSize = 2.0f * tolerance; + b3Array_CreateN( h->nodes, vertexCount ); + + b3VertexMap_init( &h->vertexMap ); + b3VertexMap_reserve( &h->vertexMap, vertexCount ); + + B3_ASSERT( h->cellSize > 0.0f ); +} + +static void b3SpatialHash_Destroy( b3SpatialHash* h ) +{ + b3VertexMap_cleanup( &h->vertexMap ); + b3Array_Destroy( h->nodes ); +} + +// Welding works by bucketing nearby vertices into identical keys in a hash table. +// Bucketing is done manually with an array. +static int32_t b3SpatialHash_FindDuplicate( b3SpatialHash* h, int32_t currentIndex ) +{ + B3_ASSERT( currentIndex < h->vertexCount ); + b3Vec3 vertex = h->vertices[currentIndex]; + float cellSize = h->cellSize; + float tolerance = h->tolerance; + + // Get the grid coordinates for the current vertex + int32_t baseX = (int32_t)( floorf( vertex.x / cellSize ) ); + int32_t baseY = (int32_t)( floorf( vertex.y / cellSize ) ); + int32_t baseZ = (int32_t)( floorf( vertex.z / cellSize ) ); + + // Check the current cell and all 26 neighboring cells (3x3x3 - 1) + for ( int dx = -1; dx <= 1; ++dx ) + { + for ( int dy = -1; dy <= 1; ++dy ) + { + for ( int dz = -1; dz <= 1; ++dz ) + { + int32_t x = baseX + dx; + int32_t y = baseY + dy; + int32_t z = baseZ + dz; + + // Compute hash for this neighboring cell (this is the key in the map) + uint64_t key = 0; + key ^= (uint64_t)( x ) + 0x9e3779b9 + ( key << 6 ) + ( key >> 2 ); + key ^= (uint64_t)( y ) + 0x9e3779b9 + ( key << 6 ) + ( key >> 2 ); + key ^= (uint64_t)( z ) + 0x9e3779b9 + ( key << 6 ) + ( key >> 2 ); + + b3VertexMap_itr it = b3VertexMap_get( &h->vertexMap, key ); + if ( b3VertexMap_is_end( it ) == false ) + { + // Check all vertices in this key + int nodeIndex = it.data->val; + + while ( nodeIndex != B3_NULL_INDEX ) + { + b3VertexNode node = h->nodes.data[nodeIndex]; + + int32_t existingIndex = node.vertexIndex; + B3_ASSERT( existingIndex < currentIndex ); + B3_ASSERT( existingIndex < h->vertexCount ); + + b3Vec3 other = h->vertices[existingIndex]; + + // IsEqual inlined: check if vertices are within tolerance + if ( fabsf( vertex.x - other.x ) <= tolerance && + fabsf( vertex.y - other.y ) <= tolerance && + fabsf( vertex.z - other.z ) <= tolerance ) + { + // Found duplicate + return existingIndex; + } + + nodeIndex = node.nextNodeIndex; + } + } + } + } + } + + // No duplicate found, add to hash table + uint64_t currentKey = 0; + currentKey ^= (uint64_t)( baseX ) + 0x9e3779b9 + ( currentKey << 6 ) + ( currentKey >> 2 ); + currentKey ^= (uint64_t)( baseY ) + 0x9e3779b9 + ( currentKey << 6 ) + ( currentKey >> 2 ); + currentKey ^= (uint64_t)( baseZ ) + 0x9e3779b9 + ( currentKey << 6 ) + ( currentKey >> 2 ); + + b3VertexMap_itr it = b3VertexMap_get( &h->vertexMap, currentKey ); + if ( b3VertexMap_is_end( it ) == false ) + { + int nodeIndex = it.data->val; + + b3VertexNode node = { + .vertexIndex = currentIndex, + .nextNodeIndex = nodeIndex, + }; + + it.data->val = h->nodes.count; + b3Array_Push( h->nodes, node ); + } + else + { + b3VertexNode node = { + .vertexIndex = currentIndex, + .nextNodeIndex = B3_NULL_INDEX, + }; + + b3VertexMap_insert( &h->vertexMap, currentKey, h->nodes.count ); + b3Array_Push( h->nodes, node ); + } + + // Not welded + return B3_NULL_INDEX; +} + +typedef struct b3WeldData +{ + const b3Vec3* srcVertices; + const int32_t* srcIndices; + + b3Vec3* dstVertices; + int32_t* dstIndices; + + int vertexCount; + int indexCount; +} b3WeldData; + +static int b3WeldVertices( b3WeldData* data, float tolerance ) +{ + int vertexCount = data->vertexCount; + int uniqueCount = 0; + + // Create spatial hash and find duplicates + b3SpatialHash spatialHash; + b3SpatialHash_Create( &spatialHash, data->srcVertices, vertexCount, tolerance ); + b3ArrayC( int ) vertexMapping = { 0 }; + b3Array_Resize( vertexMapping, vertexCount ); + + for ( int i = 0; i < vertexCount; ++i ) + { + int32_t duplicateIndex = b3SpatialHash_FindDuplicate( &spatialHash, i ); + + if ( duplicateIndex == B3_NULL_INDEX ) + { + // New unique vertex + vertexMapping.data[i] = uniqueCount; + data->dstVertices[uniqueCount] = data->srcVertices[i]; + uniqueCount += 1; + } + else + { + // Found duplicate, map to existing vertex + vertexMapping.data[i] = vertexMapping.data[duplicateIndex]; + } + } + + // Update indices to reference the new vertex array + int indexCount = data->indexCount; + for ( int i = 0; i < indexCount; ++i ) + { + int srcIndex = data->srcIndices[i]; + B3_ASSERT( srcIndex < vertexCount ); + data->dstIndices[i] = vertexMapping.data[srcIndex]; + } + + b3SpatialHash_Destroy( &spatialHash ); + b3Array_Destroy( vertexMapping ); + + return uniqueCount; +} + +static inline void b3StoreLeaf( b3MeshNode* node, const b3AABB* aabb, int triangleCount, int triangleOffset ) +{ + node->data.asLeaf.type = B3_LEAF_NODE; + node->data.asLeaf.triangleCount = triangleCount; + node->triangleOffset = triangleOffset; + node->lowerBound = aabb->lowerBound; + node->upperBound = aabb->upperBound; +} + +typedef struct b3Primitive +{ + b3AABB aabb; + b3Vec3 center; + int triangleIndex; +} b3Primitive; + +typedef struct b3Bucket +{ + int count; + b3AABB bounds; +} b3Bucket; + +typedef struct b3Split +{ + b3AABB leftBounds; + b3AABB rightBounds; + int axis; + int index; +} b3Split; + +static b3Split b3SplitBinnedSah( int count, b3Primitive* primitives ) +{ + b3Split split; + split.axis = -1; + split.index = -1; + + // Compute bounds of primitive centroids and choose split axis + b3AABB bounds = { primitives[0].center, primitives[0].center }; + for ( int i = 1; i < count; ++i ) + { + bounds = b3AABB_AddPoint( bounds, primitives[i].center ); + } + + // Compute costs for splitting after each bucket and keep track of best split + // This is a small O(n^2) loop. This can be further optimized, but it is already + // very fast and is kept for simplicity right now. + int bestBucket = -1; + float bestCost = FLT_MAX; + + for ( int axis = 0; axis < 3; ++axis ) + { + b3Vec3 extent = b3AABB_Extents( bounds ); + if ( b3GetByIndex( extent, axis ) < B3_LINEAR_SLOP ) + { + continue; + } + + // Initialize buckets + b3Bucket buckets[B3_BIN_COUNT]; + for ( int i = 0; i < B3_BIN_COUNT; ++i ) + { + buckets[i].count = 0; + buckets[i].bounds = B3_BOUNDS3_EMPTY; + } + + // Fill buckets + float factor = B3_BIN_COUNT * ( 1.0f - FLT_EPSILON ) / + ( b3GetByIndex( bounds.upperBound, axis ) - b3GetByIndex( bounds.lowerBound, axis ) ); + for ( int i = 0; i < count; ++i ) + { + b3Vec3 center = primitives[i].center; + int index = (int)( factor * ( b3GetByIndex( center, axis ) - b3GetByIndex( bounds.lowerBound, axis ) ) ); + B3_ASSERT( 0 <= index && index < B3_BIN_COUNT ); + + buckets[index].count++; + buckets[index].bounds = b3AABB_Union( buckets[index].bounds, primitives[i].aabb ); + } + + // Evaluate splits + for ( int i = 0; i < B3_BIN_COUNT - 1; ++i ) + { + int leftCount = 0; + b3AABB leftBounds = B3_BOUNDS3_EMPTY; + for ( int k = 0; k <= i; ++k ) + { + leftCount += buckets[k].count; + leftBounds = b3AABB_Union( leftBounds, buckets[k].bounds ); + } + + int rightCount = 0; + b3AABB rightBounds = B3_BOUNDS3_EMPTY; + for ( int k = i + 1; k < B3_BIN_COUNT; ++k ) + { + rightCount += buckets[k].count; + rightBounds = b3AABB_Union( rightBounds, buckets[k].bounds ); + } + + B3_ASSERT( leftCount + rightCount == count ); + if ( leftCount > 0 && rightCount > 0 ) + { + float cost = leftCount * b3AABB_Area( leftBounds ) + rightCount * b3AABB_Area( rightBounds ); + + if ( cost < bestCost ) + { + bestBucket = i; + bestCost = cost; + + split.axis = axis; + split.index = leftCount; + split.leftBounds = leftBounds; + split.rightBounds = rightBounds; + } + } + } + } + + // Partition + if ( bestBucket >= 0 ) + { + int axis = split.axis; + float factor = B3_BIN_COUNT * ( 1.0f - FLT_EPSILON ) / + ( b3GetByIndex( bounds.upperBound, axis ) - b3GetByIndex( bounds.lowerBound, axis ) ); + + int splitIndex = 0; + for ( int i = 0; i < count; ++i ) + { + b3Vec3 center = primitives[i].center; + int index = (int)( factor * ( b3GetByIndex( center, axis ) - b3GetByIndex( bounds.lowerBound, axis ) ) ); + + if ( index <= bestBucket ) + { + b3Primitive temp = primitives[i]; + primitives[i] = primitives[splitIndex]; + primitives[splitIndex] = temp; + splitIndex++; + } + } + B3_ASSERT( splitIndex == split.index ); + } + + return split; +} + +static b3Split b3SplitHalf( int count, b3Primitive* primitives ) +{ + // Split in the middle + int splitIndex = count / 2; + + b3AABB leftBounds = B3_BOUNDS3_EMPTY; + for ( int i = 0; i < splitIndex; ++i ) + { + leftBounds = b3AABB_Union( leftBounds, primitives[i].aabb ); + } + + b3AABB rightBounds = B3_BOUNDS3_EMPTY; + for ( int i = splitIndex; i < count; ++i ) + { + + rightBounds = b3AABB_Union( rightBounds, primitives[i].aabb ); + } + + b3AABB bounds = b3AABB_Union( leftBounds, rightBounds ); + int axis = b3MajorAxis( b3AABB_Extents( bounds ) ); + + b3Split split; + split.axis = axis; + split.index = splitIndex; + split.leftBounds = leftBounds; + split.rightBounds = rightBounds; + + return split; +} + +static b3Split b3SplitMedian( int count, b3Primitive* primitives ) +{ + B3_ASSERT( count > 2 ); + + b3Vec3 lowerBound = primitives[0].center; + b3Vec3 upperBound = primitives[0].center; + + for ( int i = 1; i < count; ++i ) + { + lowerBound = b3Min( lowerBound, primitives[i].center ); + upperBound = b3Max( upperBound, primitives[i].center ); + } + + b3Vec3 d = b3Sub( upperBound, lowerBound ); + b3Vec3 c = b3MulSV( 0.5f, b3Add( lowerBound, upperBound ) ); + + b3Split split = { 0 }; + split.index = -1; + + // Partition longest axis using the Hoare partition scheme + // https://en.wikipedia.org/wiki/Quicksort + // https://nicholasvadivelu.com/2021/01/11/array-partition/ + int i1 = 0, i2 = count; + if ( d.x >= d.y && d.x >= d.z ) + { + split.axis = 0; + + float pivot = c.x; + + while ( i1 < i2 ) + { + while ( i1 < i2 && primitives[i1].center.x < pivot ) + { + i1 += 1; + }; + + while ( i1 < i2 && primitives[i2 - 1].center.x >= pivot ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap primitives + b3Primitive temp = primitives[i1]; + primitives[i1] = primitives[i2 - 1]; + primitives[i2 - 1] = temp; + + i1 += 1; + i2 -= 1; + } + } + } + else if ( d.y >= d.z ) + { + split.axis = 1; + + float pivot = c.y; + + while ( i1 < i2 ) + { + while ( i1 < i2 && primitives[i1].center.y < pivot ) + { + i1 += 1; + }; + + while ( i1 < i2 && primitives[i2 - 1].center.y >= pivot ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap primitives + b3Primitive temp = primitives[i1]; + primitives[i1] = primitives[i2 - 1]; + primitives[i2 - 1] = temp; + + i1 += 1; + i2 -= 1; + } + } + } + else + { + split.axis = 2; + + float pivot = c.z; + + while ( i1 < i2 ) + { + while ( i1 < i2 && primitives[i1].center.z < pivot ) + { + i1 += 1; + }; + + while ( i1 < i2 && primitives[i2 - 1].center.z >= pivot ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap primitives + b3Primitive temp = primitives[i1]; + primitives[i1] = primitives[i2 - 1]; + primitives[i2 - 1] = temp; + + i1 += 1; + i2 -= 1; + } + } + } + B3_ASSERT( i1 == i2 ); + B3_ASSERT( 0 <= i1 && i1 < count ); + + if ( i1 == 0 || i1 == count - 1 ) + { + // failed to split + i1 = count / 2; + } + + b3AABB leftBounds = B3_BOUNDS3_EMPTY; + for ( int i = 0; i < i1; ++i ) + { + leftBounds = b3AABB_Union( leftBounds, primitives[i].aabb ); + } + + b3AABB rightBounds = B3_BOUNDS3_EMPTY; + for ( int i = i1; i < count; ++i ) + { + rightBounds = b3AABB_Union( rightBounds, primitives[i].aabb ); + } + + split.index = i1; + split.leftBounds = leftBounds; + split.rightBounds = rightBounds; + return split; +} + +#if B3_ENABLE_VALIDATION == 1 + +static bool b3ValidateSplit( int count, b3Primitive* primitives, const b3Split* split ) +{ + if ( split->axis < 0 ) + { + return false; + } + + for ( int i = 0; i < split->index; ++i ) + { + if ( !b3AABB_Contains( split->leftBounds, primitives[i].aabb ) ) + { + return false; + } + } + + for ( int i = split->index; i < count; ++i ) + { + if ( !b3AABB_Contains( split->rightBounds, primitives[i].aabb ) ) + { + return false; + } + } + + return true; +} + +#endif + +static int b3BuildRecursive( b3ArrayC( b3MeshNode ) * nodes, int count, b3Primitive* primitives, b3Primitive* base, + bool useMedianSplit, int* height ) +{ + if ( count > B3_DESIRED_TRIANGLES_PER_LEAF ) + { + // Try to split the input set using the SAH + b3Split split; + if ( useMedianSplit ) + { + split = b3SplitMedian( count, primitives ); + } + else + { + split = b3SplitBinnedSah( count, primitives ); + } + + if ( split.axis < 0 ) + { + if ( count > B3_MAXIMUM_TRIANGLES_PER_LEAF ) + { + // Re-split. This is a less optimal split and can create more false positives! + split = b3SplitHalf( count, primitives ); + } + else + { + b3AABB bounds = B3_BOUNDS3_EMPTY; + for ( int i = 0; i < count; ++i ) + { + bounds = b3AABB_Union( bounds, primitives[i].aabb ); + } + + // We have only a few triangles left. Create a leaf. + int index = b3Array_AddIndex( *nodes ); + b3StoreLeaf( &nodes->data[index], &bounds, count, (int)( primitives - base ) ); + + return index; + } + } + B3_VALIDATE( b3ValidateSplit( count, primitives, &split ) ); + + // Allocate node and recurse + int index = b3Array_AddIndex( *nodes ); + int heightLeft = 0, heightRight = 0; + int leftIndex = b3BuildRecursive( nodes, split.index, primitives, base, useMedianSplit, &heightLeft ); + int rightIndex = + b3BuildRecursive( nodes, count - split.index, primitives + split.index, base, useMedianSplit, &heightRight ); + + *height = b3MaxInt( heightLeft, heightRight ) + 1; + + B3_UNUSED( leftIndex ); + B3_ASSERT( leftIndex - index == 1 && rightIndex - index > 1 ); + + b3AABB aabb = b3AABB_Union( split.leftBounds, split.rightBounds ); + b3MeshNode* node = b3Array_Get( *nodes, index ); + node->data.asNode.axis = split.axis; + node->data.asNode.childOffset = rightIndex - index; + node->lowerBound = aabb.lowerBound; + node->upperBound = aabb.upperBound; + + return index; + } + + b3AABB aabb = B3_BOUNDS3_EMPTY; + for ( int i = 0; i < count; ++i ) + { + aabb = b3AABB_Union( aabb, primitives[i].aabb ); + } + + int index = b3Array_AddIndex( *nodes ); + b3StoreLeaf( &nodes->data[index], &aabb, count, (int)( primitives - base ) ); + + *height = 1; + + return index; +} + +static bool b3SortMeshTriangles( b3MeshData* mesh ) +{ + b3MeshTriangle* triangles = b3GetMeshTrianglesWrite( mesh ); + uint8_t* materialIndices = b3GetMeshMaterialIndicesWrite( mesh ); + + // Sort triangles in depth-first-order + int offset = 0; + b3ArrayC( b3MeshTriangle ) tempTriangles; + b3Array_CreateN( tempTriangles, mesh->triangleCount ); + + b3ArrayC( uint8_t ) tempMaterialIndices; + b3Array_CreateN( tempMaterialIndices, mesh->triangleCount ); + + int count = 0; + b3MeshNode* stack[B3_MESH_STACK_SIZE]; + stack[count++] = b3GetRootWrite( mesh ); + + while ( count > 0 ) + { + b3MeshNode* node = stack[--count]; + + if ( b3IsLeaf( node ) == false ) + { + if ( count >= B3_MESH_STACK_SIZE - 2 ) + { + return false; + } + + stack[count++] = b3GetRightChildWrite( node ); + stack[count++] = b3GetLeftChildWrite( node ); + } + else + { + int triangleCount = node->data.asLeaf.triangleCount; + int triangleOffset = node->triangleOffset; + + for ( int triangle = 0; triangle < triangleCount; ++triangle ) + { + int index = triangleOffset + triangle; + b3Array_Push( tempTriangles, triangles[index] ); + b3Array_Push( tempMaterialIndices, materialIndices[index] ); + } + + node->triangleOffset = offset; + offset += triangleCount; + } + } + + B3_ASSERT( offset == tempTriangles.count ); + B3_ASSERT( tempTriangles.count == mesh->triangleCount ); + B3_ASSERT( tempMaterialIndices.count == mesh->triangleCount ); + + // Copy sorted triangle array back to tree + memcpy( triangles, tempTriangles.data, mesh->triangleCount * sizeof( b3MeshTriangle ) ); + memcpy( materialIndices, tempMaterialIndices.data, mesh->triangleCount * sizeof( uint8_t ) ); + + b3Array_Destroy( tempTriangles ); + b3Array_Destroy( tempMaterialIndices ); + + return true; +} + +typedef struct +{ + int vertex1; + int vertex2; + int triangle1; + int triangle2; + uint16_t triangleCount; + + // The index of an edge within the parent triangle: 0, 1, or 2. 0xFF is unset + uint8_t triangleEdgeIndex1; + uint8_t triangleEdgeIndex2; +} b3MeshEdge; + +#define NAME b3EdgeMap +#define KEY_TY uint64_t +#define VAL_TY int +#define HASH_FN vt_hash_integer +#define CMPR_FN vt_cmpr_integer +#define MALLOC_FN b3Alloc +#define FREE_FN b3Free +#include "verstable.h" + +#if 0 +// todo this is for testing other hash tables +struct b3TestHash +{ + size_t operator()( uint64_t key ) const + { + key ^= key >> 23; + key *= 0x2127599bf4325c37ull; + key ^= key >> 47; + return (size_t)key; + } +}; +#endif + +// Results for MeshCreationBenchmark +// eastl::hash_map : 4.9703 ms and 5.1445ms with FastHash function +// std::unordered_map : 4.8755 ms and 4.4780ms with FastHash function +// verstable : 3.3968 ms with default FastHash +// no edge identification : 1.7396 ms +static void b3IdentifyEdges( b3MeshData* mesh ) +{ + b3MeshTriangle* triangles = b3GetMeshTrianglesWrite( mesh ); + const b3Vec3* vertices = b3GetMeshVertices( mesh ); + uint8_t* flags = b3GetMeshFlagsWrite( mesh ); + + int triangleCount = mesh->triangleCount; + int edgeCount = 3 * triangleCount; + b3MeshEdge* edges = B3_ALLOC( b3MeshEdge, edgeCount ); + b3Vec3* normals = B3_ALLOC( b3Vec3, triangleCount ); + + for ( int i = 0; i < triangleCount; ++i ) + { + b3MeshTriangle* triangle = triangles + i; + int i1 = triangle->index1; + int i2 = triangle->index2; + int i3 = triangle->index3; + + edges[3 * i + 0].vertex1 = b3MinInt( i1, i2 ); + edges[3 * i + 0].vertex2 = b3MaxInt( i1, i2 ); + edges[3 * i + 0].triangle1 = i; + edges[3 * i + 0].triangle2 = B3_NULL_INDEX; + edges[3 * i + 0].triangleEdgeIndex1 = 0; + edges[3 * i + 0].triangleEdgeIndex2 = 0xFF; + edges[3 * i + 0].triangleCount = 1; + + edges[3 * i + 1].vertex1 = b3MinInt( i2, i3 ); + edges[3 * i + 1].vertex2 = b3MaxInt( i2, i3 ); + edges[3 * i + 1].triangle1 = i; + edges[3 * i + 1].triangle2 = B3_NULL_INDEX; + edges[3 * i + 1].triangleEdgeIndex1 = 1; + edges[3 * i + 1].triangleEdgeIndex2 = 0xFF; + edges[3 * i + 1].triangleCount = 1; + + edges[3 * i + 2].vertex1 = b3MinInt( i3, i1 ); + edges[3 * i + 2].vertex2 = b3MaxInt( i3, i1 ); + edges[3 * i + 2].triangle1 = i; + edges[3 * i + 2].triangle2 = B3_NULL_INDEX; + edges[3 * i + 2].triangleEdgeIndex1 = 2; + edges[3 * i + 2].triangleEdgeIndex2 = 0xFF; + edges[3 * i + 2].triangleCount = 1; + + b3Vec3 v1 = vertices[i1]; + b3Vec3 v2 = vertices[i2]; + b3Vec3 v3 = vertices[i3]; + + b3Vec3 e1 = b3Sub( v2, v1 ); + b3Vec3 e2 = b3Sub( v3, v1 ); + b3Vec3 n = b3Cross( e1, e2 ); + + normals[i] = b3Normalize( n ); + } + + b3EdgeMap map; + b3EdgeMap_init( &map ); + b3EdgeMap_reserve( &map, edgeCount ); + + uint64_t key = (uint64_t)edges[0].vertex1 << 32 | (uint64_t)edges[0].vertex2; + b3EdgeMap_insert( &map, key, 0 ); + + // Find unique edges and assign adjacency + for ( int i = 1; i < edgeCount; ++i ) + { + b3MeshEdge* edge = edges + i; + key = (uint64_t)edge->vertex1 << 32 | (uint64_t)edge->vertex2; + b3EdgeMap_itr itr = b3EdgeMap_get( &map, key ); + + if ( b3EdgeMap_is_end( itr ) ) + { + b3EdgeMap_insert( &map, key, i ); + } + else + { + int otherIndex = itr.data->val; + B3_ASSERT( otherIndex < i ); + + b3MeshEdge* base = edges + otherIndex; + if ( base->triangleCount == 1 ) + { + base->triangle2 = edge->triangle1; + base->triangleEdgeIndex2 = edge->triangleEdgeIndex1; + } + + base->triangleCount += 1; + } + } + + b3EdgeMap_cleanup( &map ); + + for ( int i = 0; i < edgeCount; ++i ) + { + b3MeshEdge* edge = edges + i; + if ( edge->triangleCount != 2 ) + { + continue; + } + + B3_ASSERT( edge->triangleEdgeIndex1 < 3 ); + B3_ASSERT( edge->triangleEdgeIndex2 < 3 ); + + b3MeshTriangle* triangle1 = triangles + edge->triangle1; + b3MeshTriangle* triangle2 = triangles + edge->triangle2; + uint8_t* flag1 = flags + edge->triangle1; + uint8_t* flag2 = flags + edge->triangle2; + + int j1 = triangle2->index1; + int j2 = triangle2->index2; + int j3 = triangle2->index3; + + int opposite = B3_NULL_INDEX; + + switch ( edge->triangleEdgeIndex2 ) + { + case 0: + opposite = j3; + break; + + case 1: + opposite = j1; + break; + + case 2: + opposite = j2; + break; + + default: + B3_ASSERT( false ); + } + + int i1 = triangle1->index1; + int i2 = triangle1->index2; + int i3 = triangle1->index3; + + b3Vec3 v1 = vertices[i1]; + b3Vec3 v2 = vertices[i2]; + b3Vec3 v3 = vertices[i3]; + b3Vec3 p = vertices[opposite]; + + float cos5Deg = 0.9962f; + float signedVolume = b3SignedVolume( v1, v2, v3, p ); + b3Vec3 n1 = normals[edge->triangle1]; + b3Vec3 n2 = normals[edge->triangle2]; + float cosAngle = b3Dot( n1, n2 ); + if ( signedVolume > 0.0f || cosAngle > cos5Deg ) + { + int edgeFlags[3] = { b3_concaveEdge1, b3_concaveEdge2, b3_concaveEdge3 }; + *flag1 |= edgeFlags[edge->triangleEdgeIndex1]; + *flag2 |= edgeFlags[edge->triangleEdgeIndex2]; + } + + if ( signedVolume < 0.0f || cosAngle > cos5Deg ) + { + int edgeFlags[3] = { b3_inverseConcaveEdge1, b3_inverseConcaveEdge2, b3_inverseConcaveEdge3 }; + *flag1 |= edgeFlags[edge->triangleEdgeIndex1]; + *flag2 |= edgeFlags[edge->triangleEdgeIndex2]; + } + } + + B3_FREE( normals, b3Vec3, triangleCount ); + B3_FREE( edges, b3MeshEdge, edgeCount ); +} + +b3MeshData* b3CreateGridMesh( int xCount, int zCount, float cellWidth, int materialCount, bool identifyEdges ) +{ + B3_ASSERT( 0 <= materialCount && materialCount <= UINT8_MAX ); + + // Create vertices + int vertexCount = ( xCount + 1 ) * ( zCount + 1 ); + + b3ArrayC( b3Vec3 ) vertices = { 0 }; + b3Array_Resize( vertices, vertexCount ); + int index = 0; + + float xWidth = cellWidth * xCount; + float zWidth = cellWidth * zCount; + + float x = -0.5f * xWidth; + for ( int ix = 0; ix <= xCount; ++ix ) + { + float z = -0.5f * zWidth; + for ( int iz = 0; iz <= zCount; ++iz ) + { + vertices.data[index] = (b3Vec3){ x, 0.0f, z }; + z += cellWidth; + index += 1; + } + x += cellWidth; + } + B3_ASSERT( index == vertexCount ); + + // Triangles + int triangleCount = 2 * xCount * zCount; + + b3ArrayC( int ) indices = { 0 }; + b3Array_Resize( indices, 3 * triangleCount ); + + b3ArrayC( uint8_t ) materialIndices = { 0 }; + b3Array_Resize( materialIndices, triangleCount ); + + int materialIndex = 0; + index = 0; + for ( int ix = 0; ix < xCount; ++ix ) + { + for ( int iz = 0; iz < zCount; ++iz ) + { + int index1 = iz + ( zCount + 1 ) * ix; + int index2 = index1 + 1; + int index3 = index2 + ( zCount + 1 ); + int index4 = index3 - 1; + + B3_ASSERT( index1 < vertexCount ); + B3_ASSERT( index2 < vertexCount ); + B3_ASSERT( index3 < vertexCount ); + B3_ASSERT( index4 < vertexCount ); + + indices.data[index + 0] = index1; + indices.data[index + 1] = index2; + indices.data[index + 2] = index3; + + indices.data[index + 3] = index3; + indices.data[index + 4] = index4; + indices.data[index + 5] = index1; + + if ( materialCount > 0 ) + { + materialIndices.data[2 * materialIndex + 0] = (uint8_t)( materialIndex % materialCount ); + materialIndices.data[2 * materialIndex + 1] = (uint8_t)( materialIndex % materialCount ); + } + + materialIndex += 1; + index += 6; + } + } + B3_ASSERT( index == 3 * triangleCount ); + + b3MeshDef def = { 0 }; + def.vertexCount = vertices.count; + def.vertices = vertices.data; + def.triangleCount = indices.count / 3; + def.indices = indices.data; + def.materialIndices = materialCount > 0 ? materialIndices.data : NULL; + def.useMedianSplit = true; + def.identifyEdges = identifyEdges; + + b3MeshData* meshData = b3CreateMesh( &def, NULL, 0 ); + + b3Array_Destroy( indices ); + b3Array_Destroy( vertices ); + b3Array_Destroy( materialIndices ); + + return meshData; +} + +b3MeshData* b3CreateWaveMesh( int xCount, int zCount, float cellWidth, float amplitude, float rowFrequency, + float columnFrequency ) +{ + // Create vertices + int vertexCount = ( xCount + 1 ) * ( zCount + 1 ); + + b3ArrayC( b3Vec3 ) vertices = { 0 }; + b3Array_Resize( vertices, vertexCount ); + int index = 0; + + float xWidth = cellWidth * xCount; + float zWidth = cellWidth * zCount; + + float omegaZ = 2.0f * B3_PI * rowFrequency; + float omegaX = 2.0f * B3_PI * columnFrequency; + + float x = -0.5f * xWidth; + for ( int ix = 0; ix <= xCount; ++ix ) + { + float rowHeight = sinf( omegaX * ix ); + + float z = -0.5f * zWidth; + for ( int iz = 0; iz <= zCount; ++iz ) + { + float columnHeight = sinf( omegaZ * iz ); + + float y = amplitude * rowHeight * columnHeight; + vertices.data[index] = (b3Vec3){ x, y, z }; + z += cellWidth; + index += 1; + } + x += cellWidth; + } + B3_ASSERT( index == vertexCount ); + + // Triangles + int triangleCount = 2 * xCount * zCount; + + b3ArrayC( int ) indices = { 0 }; + b3Array_Resize( indices, 3 * triangleCount ); + + index = 0; + for ( int ix = 0; ix < xCount; ++ix ) + { + for ( int iz = 0; iz < zCount; ++iz ) + { + int index1 = iz + ( zCount + 1 ) * ix; + int index2 = index1 + 1; + int index3 = index2 + ( zCount + 1 ); + int index4 = index3 - 1; + + B3_ASSERT( index1 < vertexCount ); + B3_ASSERT( index2 < vertexCount ); + B3_ASSERT( index3 < vertexCount ); + B3_ASSERT( index4 < vertexCount ); + + indices.data[index + 0] = index1; + indices.data[index + 1] = index2; + indices.data[index + 2] = index3; + + indices.data[index + 3] = index3; + indices.data[index + 4] = index4; + indices.data[index + 5] = index1; + + index += 6; + } + } + B3_ASSERT( index == 3 * triangleCount ); + + b3MeshDef def = { 0 }; + def.vertexCount = vertices.count; + def.vertices = vertices.data; + def.triangleCount = indices.count / 3; + def.indices = indices.data; + def.useMedianSplit = true; + def.identifyEdges = true; + + b3MeshData* meshData = b3CreateMesh( &def, NULL, 0 ); + + b3Array_Destroy( indices ); + b3Array_Destroy( vertices ); + + return meshData; +} + +b3MeshData* b3CreateTorusMesh( int radialResolution, int tubularResolution, float radius, float thickness ) +{ + // Create vertices + b3ArrayC( b3Vec3 ) vertices = { 0 }; + + for ( int radialIndex = 0; radialIndex < radialResolution; radialIndex++ ) + { + for ( int tubularIndex = 0; tubularIndex < tubularResolution; tubularIndex++ ) + { + float u = (float)tubularIndex / tubularResolution * B3_TWO_PI; + float v = (float)radialIndex / radialResolution * B3_TWO_PI; + + float x = ( radius + thickness * b3Cos( v ) ) * b3Cos( u ); + float y = ( radius + thickness * b3Cos( v ) ) * b3Sin( u ); + float z = thickness * b3Sin( v ); + + b3Vec3 vertex = { x, y, z }; + b3Array_Push( vertices, vertex ); + } + } + + // Triangles + b3ArrayC( int ) indices = { 0 }; + for ( int radialIndex1 = 0; radialIndex1 < radialResolution; radialIndex1++ ) + { + int radialIndex2 = ( radialIndex1 + 1 ) % radialResolution; + for ( int tubularIndex1 = 0; tubularIndex1 < tubularResolution; tubularIndex1++ ) + { + int tubularIndex2 = ( tubularIndex1 + 1 ) % tubularResolution; + int index1 = radialIndex1 * tubularResolution + tubularIndex1; + int index2 = radialIndex1 * tubularResolution + tubularIndex2; + int index3 = radialIndex2 * tubularResolution + tubularIndex2; + int index4 = radialIndex2 * tubularResolution + tubularIndex1; + + b3Array_Push( indices, index1 ); + b3Array_Push( indices, index2 ); + b3Array_Push( indices, index3 ); + + b3Array_Push( indices, index3 ); + b3Array_Push( indices, index4 ); + b3Array_Push( indices, index1 ); + } + } + + b3MeshDef def = { 0 }; + def.vertexCount = vertices.count; + def.vertices = vertices.data; + def.triangleCount = indices.count / 3; + def.indices = indices.data; + def.useMedianSplit = false; + def.identifyEdges = true; + + b3MeshData* meshData = b3CreateMesh( &def, NULL, 0 ); + + b3Array_Destroy( vertices ); + b3Array_Destroy( indices ); + return meshData; +} + +b3MeshData* b3CreateBoxMesh( b3Vec3 center, b3Vec3 extent, bool identifyEdges ) +{ + float x = extent.x; + float y = extent.y; + float z = extent.z; + b3Vec3 vertices[] = { + { x, y, z }, { -x, y, z }, { -x, -y, z }, { x, -y, z }, { x, y, -z }, { -x, y, -z }, { -x, -y, -z }, { x, -y, -z }, + }; + + for ( int i = 0; i < 8; ++i ) + { + vertices[i] = b3Add( vertices[i], center ); + } + + int indices[] = { + 0, 1, 3, 1, 2, 3, // front + 0, 4, 1, 1, 4, 5, // top + 0, 3, 7, 4, 0, 7, // right + 4, 7, 5, 6, 5, 7, // back + 1, 5, 2, 6, 2, 5, // left + 3, 2, 7, 6, 7, 2, // bottom + }; + + b3MeshDef def = { 0 }; + def.vertexCount = 8; + def.vertices = vertices; + def.triangleCount = 12; + def.indices = indices; + def.useMedianSplit = false; + def.identifyEdges = identifyEdges; + + return b3CreateMesh( &def, NULL, 0 ); +} + +// todo this should fail if the mesh has a height greater than B3_MESH_STACK_SIZE +b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, int degenerateCapacity ) +{ + if ( def->vertexCount < 3 || def->vertices == NULL || def->triangleCount <= 0 || def->indices == NULL ) + { + return NULL; + } + + int triangleCount = def->triangleCount; + if ( triangleCount == 0 ) + { + return NULL; + } + + int vertexCount = def->vertexCount; + + b3AABB meshBounds = B3_BOUNDS3_EMPTY; + + // Clone indices and vertices to support welding + b3ArrayC( int ) indices; + b3Array_CreateN( indices, 3 * triangleCount ); + + b3ArrayC( b3Vec3 ) vertices; + b3Array_CreateN( vertices, vertexCount ); + + if ( def->weldVertices && def->weldTolerance > 0.0f ) + { + b3Array_Resize( vertices, vertexCount ); + b3Array_Resize( indices, 3 * triangleCount ); + b3WeldData data = { + .srcVertices = def->vertices, + .srcIndices = def->indices, + .dstVertices = vertices.data, + .dstIndices = indices.data, + .vertexCount = vertexCount, + .indexCount = 3 * triangleCount, + }; + vertices.count = b3WeldVertices( &data, def->weldTolerance ); + vertexCount = vertices.count; + B3_ASSERT( vertexCount <= def->vertexCount ); + } + else + { + b3Array_Append( vertices, def->vertices, vertexCount ); + b3Array_Append( indices, def->indices, 3 * triangleCount ); + } + + b3ArrayC( b3Primitive ) primitives; + b3Array_CreateN( primitives, triangleCount ); + int degenerateCount = 0; + float minArea = 0.01f * B3_LINEAR_SLOP * B3_LINEAR_SLOP; + float surfaceArea = 0.0f; + int materialCount = 1; + + for ( int index = 0; index < triangleCount; ++index ) + { + int index1 = indices.data[3 * index + 0]; + int index2 = indices.data[3 * index + 1]; + int index3 = indices.data[3 * index + 2]; + + b3Vec3 vertex1 = vertices.data[index1]; + b3Vec3 vertex2 = vertices.data[index2]; + b3Vec3 vertex3 = vertices.data[index3]; + + b3Vec3 normal = b3Cross( b3Sub( vertex2, vertex1 ), b3Sub( vertex3, vertex1 ) ); + float area = 0.5f * b3Length( normal ); + + if ( area < minArea ) + { + // b3Log( "degenerate: %d %d %d\n", index1, index2, index3 ); + + if ( index1 != index2 && index1 != index3 && index2 != index3 ) + { + degenerateCount += 1; + if ( degenerateTriangleIndices != NULL && degenerateCount < degenerateCapacity ) + { + degenerateTriangleIndices[degenerateCount - 1] = index; + } + } + + continue; + } + + surfaceArea += area; + + b3AABB box = { + b3Min( vertex1, b3Min( vertex2, vertex3 ) ), + b3Max( vertex1, b3Max( vertex2, vertex3 ) ), + }; + + b3Vec3 center = b3AABB_Center( box ); + + b3Primitive primitive = { + .aabb = box, + .center = center, + .triangleIndex = index, + }; + b3Array_Push( primitives, primitive ); + + if ( def->materialIndices != NULL ) + { + materialCount = b3MaxInt( materialCount, def->materialIndices[index] + 1 ); + } + + meshBounds = b3AABB_Union( meshBounds, box ); + } + + // Update triangle count due to degenerates being skipped + triangleCount = primitives.count; + + if ( b3IsSaneAABB( meshBounds ) == false ) + { + b3Array_Destroy( primitives ); + return NULL; + } + + // Build the tree (this reorders the builder triangles) + b3ArrayC( b3MeshNode ) tempNodes; + b3Array_CreateN( tempNodes, 2 * triangleCount - 1 ); + + int treeHeight = 0; + b3BuildRecursive( &tempNodes, triangleCount, primitives.data, primitives.data, def->useMedianSplit, &treeHeight ); + + // Allocate the mesh + int byteCount = sizeof( b3MeshData ); + int nodeOffset = byteCount; + byteCount += tempNodes.count * sizeof( b3MeshNode ); + int vertexOffset = byteCount; + byteCount += vertexCount * sizeof( b3Vec3 ); + int triangleOffset = byteCount; + byteCount += triangleCount * sizeof( b3MeshTriangle ); + int materialIndicesOffset = byteCount; + byteCount += triangleCount * sizeof( uint8_t ); + int flagsOffset = byteCount; + byteCount += triangleCount * sizeof( uint8_t ); + + b3MeshData* mesh = (b3MeshData*)b3Alloc( byteCount ); + + // zero initialize for determinism + memset( mesh, 0, byteCount ); + + mesh->version = B3_MESH_VERSION; + mesh->byteCount = byteCount; + mesh->bounds = meshBounds; + mesh->surfaceArea = surfaceArea; + mesh->nodeCount = tempNodes.count; + mesh->treeHeight = treeHeight; + mesh->vertexCount = vertexCount; + mesh->triangleCount = triangleCount; + mesh->degenerateCount = degenerateCount; + mesh->nodeOffset = nodeOffset; + mesh->vertexOffset = vertexOffset; + mesh->triangleOffset = triangleOffset; + mesh->materialOffset = materialIndicesOffset; + mesh->materialCount = materialCount; + mesh->flagsOffset = flagsOffset; + + b3MeshNode* nodes = b3GetMeshNodesWrite( mesh ); + b3MeshTriangle* triangles = b3GetMeshTrianglesWrite( mesh ); + b3Vec3* meshVertices = b3GetMeshVerticesWrite( mesh ); + uint8_t* materialIndices = b3GetMeshMaterialIndicesWrite( mesh ); + uint8_t* flags = b3GetMeshFlagsWrite( mesh ); + + memcpy( nodes, tempNodes.data, tempNodes.count * sizeof( b3MeshNode ) ); + memcpy( meshVertices, vertices.data, vertexCount * sizeof( b3Vec3 ) ); + + for ( int index = 0; index < triangleCount; ++index ) + { + b3Primitive primitive = primitives.data[index]; + triangles[index].index1 = indices.data[3 * primitive.triangleIndex + 0]; + triangles[index].index2 = indices.data[3 * primitive.triangleIndex + 1]; + triangles[index].index3 = indices.data[3 * primitive.triangleIndex + 2]; + flags[index] = 0; + + // Copy material indices if they exist. Otherwise the material indices are all zeroes. + if ( def->materialIndices != NULL ) + { + uint8_t materialIndex = def->materialIndices[primitive.triangleIndex]; + materialIndices[index] = materialIndex; + } + } + + // Sort triangle in DFS order. Casts and volume queries will return sorted arrays. + // This also sorts material indices, but not the materials. + // This can fail if the BVH height is too large. + bool success = b3SortMeshTriangles( mesh ); + if ( success == false ) + { + b3Array_Destroy( tempNodes ); + b3Array_Destroy( primitives ); + return NULL; + } + + if ( def->identifyEdges ) + { + b3IdentifyEdges( mesh ); + } + + B3_VALIDATE( b3IsNonDegenerate( mesh, minArea ) ); + B3_VALIDATE( b3IsConsistent( mesh ) ); + + b3Array_Destroy( tempNodes ); + b3Array_Destroy( primitives ); + b3Array_Destroy( indices ); + b3Array_Destroy( vertices ); + + mesh->hash = 0; + mesh->hash = b3Hash( B3_HASH_INIT, (uint8_t*)mesh, mesh->byteCount ); + + return mesh; +} + +void b3DestroyMesh( b3MeshData* mesh ) +{ + b3Free( mesh, mesh->byteCount ); +} + +bool b3OverlapMesh( const b3Mesh* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ) +{ + B3_ASSERT( proxy->count > 0 ); + b3SimplexCache cache = { 0 }; + + b3Vec3 buffer[B3_MAX_SHAPE_CAST_POINTS]; + b3ShapeProxy localProxy = b3MakeLocalProxy( proxy, shapeTransform, buffer ); + b3AABB aabb = b3ComputeProxyAABB( &localProxy ); + + b3Vec3 meshScale = shape->scale; + + // Scale may have reflection so min/max may become invalid when unscaled + b3V32 scale = b3LoadV( &meshScale.x ); + b3V32 invScale = b3DivV( b3_oneV, scale ); + b3V32 temp1 = b3MulV( invScale, b3LoadV( &aabb.lowerBound.x ) ); + b3V32 temp2 = b3MulV( invScale, b3LoadV( &aabb.upperBound.x ) ); + b3V32 invScaledBoundsMin = b3MinV( temp1, temp2 ); + b3V32 invScaledBoundsMax = b3MaxV( temp1, temp2 ); + b3V32 invScaledBoundsCenter = b3MulV( b3_halfV, b3AddV( invScaledBoundsMin, invScaledBoundsMax ) ); + b3V32 invScaledBoundsExtent = b3SubV( invScaledBoundsMax, invScaledBoundsCenter ); + + b3DistanceInput input; + input.proxyB = localProxy; + input.transformA = b3Transform_identity; + input.transformB = b3Transform_identity; + input.useRadii = true; + + int count = 0; + const b3MeshNode* stack[B3_MESH_STACK_SIZE]; + const b3MeshNode* node = b3GetRoot( shape->data ); + const b3MeshTriangle* triangles = b3GetMeshTriangles( shape->data ); + const b3Vec3* vertices = b3GetMeshVertices( shape->data ); + + while ( true ) + { + // Test node overlap in unscaled space + b3V32 nodeMin = b3LoadV( &node->lowerBound.x ); + b3V32 nodeMax = b3LoadV( &node->upperBound.x ); + if ( b3TestBoundsOverlap( nodeMin, nodeMax, invScaledBoundsMin, invScaledBoundsMax ) ) + { + if ( b3IsLeaf( node ) ) + { + int triangleCount = node->data.asLeaf.triangleCount; + int triangleOffset = node->triangleOffset; + + for ( int index = 0; index < triangleCount; ++index ) + { + int triangleIndex = triangleOffset + index; + b3MeshTriangle triangle = triangles[triangleIndex]; + + b3Vec3 vertex1 = vertices[triangle.index1]; + b3Vec3 vertex2 = vertices[triangle.index2]; + b3Vec3 vertex3 = vertices[triangle.index3]; + b3V32 v1 = b3LoadV( &vertex1.x ); + b3V32 v2 = b3LoadV( &vertex2.x ); + b3V32 v3 = b3LoadV( &vertex3.x ); + + // Bounding box overlap test in unscaled space + if ( b3TestBoundsTriangleOverlap( invScaledBoundsCenter, invScaledBoundsExtent, v1, v2, v3 ) ) + { + // Shape-triangle overlap test in scaled space. Winding order doesn't matter. + b3Vec3 triangleVertices[] = { b3Mul( meshScale, vertex1 ), b3Mul( meshScale, vertex2 ), b3Mul( meshScale, vertex3 ) }; + input.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + + // reset the cache + cache.count = 0; + + // get distance between triangle and query shape + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + + float tolerance = 0.1f * B3_LINEAR_SLOP; + if ( output.distance < tolerance ) + { + // overlap detected + return true; + } + } + } + } + else + { + // Recurse + B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 ); + stack[count++] = b3GetRightChild( node ); + node = b3GetLeftChild( node ); + + continue; + } + } + + if ( count == 0 ) + { + break; + } + node = stack[--count]; + } + + return false; +} + +b3AABB b3ComputeMeshAABB( const b3MeshData* shape, b3Transform transform, b3Vec3 scale ) +{ + b3Vec3 scaledLower = b3Mul( scale, shape->bounds.lowerBound ); + b3Vec3 scaledUpper = b3Mul( scale, shape->bounds.upperBound ); + b3AABB bounds = { b3Min( scaledLower, scaledUpper ), b3Max( scaledLower, scaledUpper ) }; + return b3AABB_Transform( transform, bounds ); +} + +b3CastOutput b3RayCastMesh( const b3Mesh* mesh, const b3RayCastInput* input ) +{ + const b3MeshData* data = mesh->data; + b3Vec3 meshScale = mesh->scale; + + b3CastOutput bestOutput = { 0 }; + bestOutput.fraction = input->maxFraction; + bestOutput.triangleIndex = B3_NULL_INDEX; + + b3V32 lambda = b3SplatV( input->maxFraction ); + + b3V32 rayStart = b3LoadV( &input->origin.x ); + b3V32 rayDelta = b3LoadV( &input->translation.x ); + + b3V32 scale = b3LoadV( &meshScale.x ); + b3V32 invScale = b3DivV( b3_oneV, scale ); + bool clockwise = meshScale.x * meshScale.y * meshScale.z < 0.0f; + + // Use the inverse scaled ray for traversal of the BVH + b3V32 invScaledRayStart = b3MulV( invScale, rayStart ); + b3V32 invScaledRayDelta = b3MulV( invScale, rayDelta ); + b3V32 invScaledRayEnd = b3AddV( invScaledRayStart, b3MulV( lambda, invScaledRayDelta ) ); + b3V32 invScaledRayMin = b3MinV( invScaledRayStart, invScaledRayEnd ); + b3V32 invScaledRayMax = b3MaxV( invScaledRayStart, invScaledRayEnd ); + + int count = 0; + const b3MeshNode* stack[B3_MESH_STACK_SIZE]; + const b3MeshNode* node = b3GetRoot( data ); + const b3MeshTriangle* triangles = b3GetMeshTriangles( data ); + const b3Vec3* vertices = b3GetMeshVertices( data ); + const uint8_t* materialIndices = b3GetMeshMaterialIndices( data ); + + while ( true ) + { + // Test node/ray overlap using SAT + b3V32 nodeMin = b3LoadV( &node->lowerBound.x ); + b3V32 nodeMax = b3LoadV( &node->upperBound.x ); + if ( b3TestBoundsOverlap( nodeMin, nodeMax, invScaledRayMin, invScaledRayMax ) && + b3TestBoundsRayOverlap( nodeMin, nodeMax, invScaledRayStart, invScaledRayDelta ) ) + { + // SAT: The node and ray overlap - process leaf node or recurse + if ( b3IsLeaf( node ) ) + { + int triangleCount = node->data.asLeaf.triangleCount; + int triangleOffset = node->triangleOffset; + + for ( int index = 0; index < triangleCount; ++index ) + { + int triangleIndex = triangleOffset + index; + b3MeshTriangle triangle = triangles[triangleIndex]; + + // Collide ray with triangle in scaled space + b3Vec3 vertex1 = b3Mul( meshScale, vertices[triangle.index1] ); + b3Vec3 vertex2, vertex3; + + // The CPU should predict this branch + if ( clockwise ) + { + vertex2 = b3Mul( meshScale, vertices[triangle.index3] ); + vertex3 = b3Mul( meshScale, vertices[triangle.index2] ); + } + else + { + vertex2 = b3Mul( meshScale, vertices[triangle.index2] ); + vertex3 = b3Mul( meshScale, vertices[triangle.index3] ); + } + + // Collide ray with triangle in scaled space + b3V32 v1 = b3LoadV( &vertex1.x ); + b3V32 v2 = b3LoadV( &vertex2.x ); + b3V32 v3 = b3LoadV( &vertex3.x ); + + float alpha = b3IntersectRayTriangle( rayStart, rayDelta, v1, v2, v3 ); + B3_ASSERT( 0 <= alpha && alpha <= 1.0f ); + + if ( alpha < bestOutput.fraction ) + { + b3Vec3 edge1 = b3Sub( vertex2, vertex1 ); + b3Vec3 edge2 = b3Sub( vertex3, vertex1 ); + bestOutput.normal = b3Normalize( b3Cross( edge1, edge2 ) ); + bestOutput.point = b3Add( input->origin, b3MulSV( alpha, input->translation ) ); + bestOutput.fraction = alpha; + bestOutput.triangleIndex = triangleIndex; + bestOutput.materialIndex = materialIndices[triangleIndex]; + bestOutput.hit = true; + + // Update ray bounds in unscaled space + lambda = b3SplatV( alpha ); + invScaledRayEnd = b3AddV( invScaledRayStart, b3MulV( lambda, invScaledRayDelta ) ); + invScaledRayMin = b3MinV( invScaledRayStart, invScaledRayEnd ); + invScaledRayMax = b3MaxV( invScaledRayStart, invScaledRayEnd ); + } + } + } + else + { + // Determine traversal order (front -> back) and recurse + int axis = node->data.asNode.axis; + if ( b3GetV( invScaledRayDelta, axis ) > 0.0f ) + { + B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 ); + stack[count++] = b3GetRightChild( node ); + node = b3GetLeftChild( node ); + } + else + { + B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 ); + stack[count++] = b3GetLeftChild( node ); + node = b3GetRightChild( node ); + } + + continue; + } + } + + if ( count == 0 ) + { + break; + } + node = stack[--count]; + } + + return bestOutput; +} + +b3CastOutput b3ShapeCastMesh( const b3Mesh* mesh, const b3ShapeCastInput* input ) +{ + const b3MeshData* data = mesh->data; + b3Vec3 meshScale = mesh->scale; + + b3CastOutput bestOutput = { 0 }; + bestOutput.fraction = input->maxFraction; + bestOutput.triangleIndex = B3_NULL_INDEX; + + b3V32 lambda = b3SplatV( input->maxFraction ); + + b3AABB shapeBounds = b3MakeAABB( input->proxy.points, input->proxy.count, input->proxy.radius ); + b3Vec3 center = b3AABB_Center( shapeBounds ); + b3Vec3 extents = b3AABB_Extents( shapeBounds ); + b3V32 shapeExtent = b3LoadV( &extents.x ); + + b3V32 rayStart = b3LoadV( ¢er.x ); + b3V32 rayDelta = b3LoadV( &input->translation.x ); + b3V32 rayEnd = b3AddV( rayStart, b3MulV( lambda, rayDelta ) ); + b3V32 rayMin = b3MinV( rayStart, rayEnd ); + b3V32 rayMax = b3MaxV( rayStart, rayEnd ); + + b3V32 scale = b3LoadV( &meshScale.x ); + b3V32 invScale = b3DivV( b3_oneV, scale ); + b3V32 absInvScale = b3AbsV( invScale ); + bool clockwise = meshScale.x * meshScale.y * meshScale.z < 0.0f; + + // Use the inverse scaled shape cast for traversal of the BVH + b3V32 invScaledRayStart = b3MulV( invScale, rayStart ); + b3V32 invScaledRayDelta = b3MulV( invScale, rayDelta ); + b3V32 invScaledRayEnd = b3AddV( invScaledRayStart, b3MulV( lambda, invScaledRayDelta ) ); + b3V32 invScaledRayMin = b3MinV( invScaledRayStart, invScaledRayEnd ); + b3V32 invScaledRayMax = b3MaxV( invScaledRayStart, invScaledRayEnd ); + b3V32 invScaledShapeExtent = b3MulV( absInvScale, shapeExtent ); + + int count = 0; + const b3MeshNode* stack[B3_MESH_STACK_SIZE]; + const b3MeshNode* node = b3GetRoot( data ); + const b3MeshTriangle* triangles = b3GetMeshTriangles( data ); + const b3Vec3* vertices = b3GetMeshVertices( data ); + const uint8_t* materialIndices = b3GetMeshMaterialIndices( data ); + + while ( true ) + { + // Test node/ray overlap using SAT in unscaled space + b3V32 nodeMin = b3SubV( b3LoadV( &node->lowerBound.x ), invScaledShapeExtent ); + b3V32 nodeMax = b3AddV( b3LoadV( &node->upperBound.x ), invScaledShapeExtent ); + + if ( b3TestBoundsOverlap( nodeMin, nodeMax, invScaledRayMin, invScaledRayMax ) && + b3TestBoundsRayOverlap( nodeMin, nodeMax, invScaledRayStart, invScaledRayDelta ) ) + { + // SAT: The node and ray overlap - process leaf node or recurse + if ( b3IsLeaf( node ) ) + { + int triangleCount = node->data.asLeaf.triangleCount; + int triangleOffset = node->triangleOffset; + + for ( int index = 0; index < triangleCount; ++index ) + { + int triangleIndex = triangleOffset + index; + b3MeshTriangle triangle = triangles[triangleIndex]; + + // Collide ray with triangle in scaled space + b3Vec3 vertex1 = b3Mul( meshScale, vertices[triangle.index1] ); + b3Vec3 vertex2, vertex3; + + // The CPU should predict this branch + if ( clockwise ) + { + vertex2 = b3Mul( meshScale, vertices[triangle.index3] ); + vertex3 = b3Mul( meshScale, vertices[triangle.index2] ); + } + else + { + vertex2 = b3Mul( meshScale, vertices[triangle.index2] ); + vertex3 = b3Mul( meshScale, vertices[triangle.index3] ); + } + + b3V32 v1 = b3LoadV( &vertex1.x ); + b3V32 v2 = b3LoadV( &vertex2.x ); + b3V32 v3 = b3LoadV( &vertex3.x ); + + b3V32 triangleMin = b3SubV( b3MinV( v1, b3MinV( v2, v3 ) ), shapeExtent ); + b3V32 triangleMax = b3AddV( b3MaxV( v1, b3MaxV( v2, v3 ) ), shapeExtent ); + + // Test triangle-ray overlap in scaled space + if ( b3TestBoundsOverlap( triangleMin, triangleMax, rayMin, rayMax ) ) + { + // Collide shape with triangle in scaled space + b3Vec3 origin = vertex1; + b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( vertex2, origin ), b3Sub( vertex3, origin ) }; + b3Transform shiftedOrigin = { b3Neg( origin ), b3Quat_identity }; + + b3ShapeCastPairInput pairInput; + pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + pairInput.proxyB = input->proxy; + pairInput.transformA = b3Transform_identity; + pairInput.transformB = shiftedOrigin; + pairInput.maxFraction = bestOutput.fraction; + pairInput.translationB = input->translation; + pairInput.canEncroach = input->canEncroach; + + b3CastOutput pairOutput = b3ShapeCast( &pairInput ); + + if ( pairOutput.hit ) + { + pairOutput.point = b3Add( pairOutput.point, origin ); + + bestOutput = pairOutput; + bestOutput.triangleIndex = triangleIndex; + bestOutput.materialIndex = materialIndices[triangleIndex]; + + // Update ray bounds in scaled space + lambda = b3SplatV( pairOutput.fraction ); + rayEnd = b3AddV( rayStart, b3MulV( lambda, rayDelta ) ); + rayMin = b3MinV( rayStart, rayEnd ); + rayMax = b3MaxV( rayStart, rayEnd ); + + // Ray bounds in unscaled space + invScaledRayEnd = b3AddV( invScaledRayStart, b3MulV( lambda, invScaledRayDelta ) ); + invScaledRayMin = b3MinV( invScaledRayStart, invScaledRayEnd ); + invScaledRayMax = b3MaxV( invScaledRayStart, invScaledRayEnd ); + } + } + } + } + else + { + // Determine traversal order (front -> back) and recurse + int axis = node->data.asNode.axis; + if ( b3GetV( invScaledRayDelta, axis ) > 0.0f ) + { + B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 ); + stack[count++] = b3GetRightChild( node ); + node = b3GetLeftChild( node ); + } + else + { + B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 ); + stack[count++] = b3GetLeftChild( node ); + node = b3GetRightChild( node ); + } + + continue; + } + } + + if ( count == 0 ) + { + break; + } + node = stack[--count]; + } + + return bestOutput; +} + +b3Triangle b3GetMeshTriangle( const b3Mesh* mesh, int triangleIndex ) +{ + B3_ASSERT( 0 <= triangleIndex && triangleIndex < mesh->data->triangleCount ); + + const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh->data ); + const uint8_t* flags = b3GetMeshFlags( mesh->data ); + const b3Vec3* vertices = b3GetMeshVertices( mesh->data ); + + b3Triangle result; + b3MeshTriangle triangle = triangles[triangleIndex]; + uint8_t triangleFlags = flags[triangleIndex]; + + b3Vec3 scale = mesh->scale; + + result.vertices[0] = b3Mul( scale, vertices[triangle.index1] ); + result.i1 = triangle.index1; + + if ( scale.x * scale.y * scale.z < 0.0f ) + { + result.vertices[1] = b3Mul( scale, vertices[triangle.index3] ); + result.vertices[2] = b3Mul( scale, vertices[triangle.index2] ); + + result.i2 = triangle.index3; + result.i3 = triangle.index2; + + // mesh is inverted, so concave edges are now convex + result.flags = 0; + result.flags |= ( triangleFlags & b3_inverseConcaveEdge1 ) ? b3_concaveEdge1 : 0; + result.flags |= ( triangleFlags & b3_inverseConcaveEdge2 ) ? b3_concaveEdge2 : 0; + result.flags |= ( triangleFlags & b3_inverseConcaveEdge3 ) ? b3_concaveEdge3 : 0; + } + else + { + result.vertices[1] = b3Mul( scale, vertices[triangle.index2] ); + result.vertices[2] = b3Mul( scale, vertices[triangle.index3] ); + + result.i2 = triangle.index2; + result.i3 = triangle.index3; + result.flags = triangleFlags; + } + + return result; +} + +int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* shape, const b3Capsule* mover ) +{ + if ( capacity == 0 ) + { + return 0; + } + + b3DistanceInput distanceInput = { 0 }; + distanceInput.proxyB = (b3ShapeProxy){ &mover->center1, 2, 0.0f }; + distanceInput.transformA = b3Transform_identity; + distanceInput.transformB = b3Transform_identity; + distanceInput.useRadii = false; + + b3SimplexCache cache = { 0 }; + float radius = mover->radius; + + b3V32 center1 = b3LoadV( &mover->center1.x ); + b3V32 center2 = b3LoadV( &mover->center2.x ); + b3V32 r = b3SplatV( radius ); + b3V32 boundsMin = b3SubV( b3MinV( center1, center2 ), r ); + b3V32 boundsMax = b3AddV( b3MaxV( center1, center2 ), r ); + + // Scale may have reflection so min/max may become invalid when unscaled + b3Vec3 meshScale = shape->scale; + b3V32 scale = b3LoadV( &meshScale.x ); + b3V32 invScale = b3DivV( b3_oneV, scale ); + b3V32 temp1 = b3MulV( invScale, boundsMin ); + b3V32 temp2 = b3MulV( invScale, boundsMax ); + b3V32 invScaledBoundsMin = b3MinV( temp1, temp2 ); + b3V32 invScaledBoundsMax = b3MaxV( temp1, temp2 ); + b3V32 invScaledBoundsCenter = b3MulV( b3_halfV, b3AddV( invScaledBoundsMin, invScaledBoundsMax ) ); + b3V32 invScaledBoundsExtent = b3SubV( invScaledBoundsMax, invScaledBoundsCenter ); + + int count = 0; + const b3MeshNode* stack[B3_MESH_STACK_SIZE]; + const b3MeshNode* node = b3GetRoot( shape->data ); + const b3MeshTriangle* triangles = b3GetMeshTriangles( shape->data ); + const b3Vec3* vertices = b3GetMeshVertices( shape->data ); + + int planeCount = 0; + while ( planeCount < capacity ) + { + // Test node overlap in unscaled space + b3V32 nodeMin = b3LoadV( &node->lowerBound.x ); + b3V32 nodeMax = b3LoadV( &node->upperBound.x ); + if ( b3TestBoundsOverlap( nodeMin, nodeMax, invScaledBoundsMin, invScaledBoundsMax ) ) + { + if ( b3IsLeaf( node ) ) + { + int triangleCount = node->data.asLeaf.triangleCount; + int triangleOffset = node->triangleOffset; + + for ( int index = 0; index < triangleCount; ++index ) + { + int triangleIndex = triangleOffset + index; + b3MeshTriangle triangle = triangles[triangleIndex]; + + b3Vec3 vertex1 = vertices[triangle.index1]; + b3Vec3 vertex2 = vertices[triangle.index2]; + b3Vec3 vertex3 = vertices[triangle.index3]; + b3V32 v1 = b3LoadV( &vertex1.x ); + b3V32 v2 = b3LoadV( &vertex2.x ); + b3V32 v3 = b3LoadV( &vertex3.x ); + + // Test triangle bounds overlap in unscaled space + if ( b3TestBoundsTriangleOverlap( invScaledBoundsCenter, invScaledBoundsExtent, v1, v2, v3 ) ) + { + // Compute shape distance in scaled space. Winding order doesn't matter. + // todo implement one-sided collision? + b3Vec3 triangleVertices[] = { b3Mul( meshScale, vertex1 ), b3Mul( meshScale, vertex2 ), b3Mul( meshScale, vertex3 ) }; + distanceInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + + // reset the cache + cache.count = 0; + + // get distance between triangle and query shape + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance == 0.0f ) + { + // todo SAT + } + else if ( distanceOutput.distance <= mover->radius ) + { + b3Plane plane = { distanceOutput.normal, mover->radius - distanceOutput.distance }; + planes[planeCount] = (b3PlaneResult){ plane, distanceOutput.pointA }; + planeCount += 1; + + if ( planeCount == capacity ) + { + return planeCount; + } + } + } + } + } + else + { + // Recurse + B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 ); + stack[count++] = b3GetRightChild( node ); + node = b3GetLeftChild( node ); + + continue; + } + } + + if ( count == 0 ) + { + break; + } + node = stack[--count]; + } + + return planeCount; +} + +void b3QueryMesh( const b3Mesh* mesh, b3AABB bounds, b3MeshQueryFcn* fcn, void* context ) +{ + b3Vec3 meshScale = mesh->scale; + bool clockwise = meshScale.x * meshScale.y * meshScale.z > 0.0f; + + // Scale may have reflection so min/max may become invalid when unscaled + b3V32 scale = b3LoadV( &meshScale.x ); + b3V32 invScale = b3DivV( b3_oneV, scale ); + b3V32 temp1 = b3MulV( invScale, b3LoadV( &bounds.lowerBound.x ) ); + b3V32 temp2 = b3MulV( invScale, b3LoadV( &bounds.upperBound.x ) ); + b3V32 invScaledBoundsMin = b3MinV( temp1, temp2 ); + b3V32 invScaledBoundsMax = b3MaxV( temp1, temp2 ); + b3V32 invScaledBoundsCenter = b3MulV( b3_halfV, b3AddV( invScaledBoundsMin, invScaledBoundsMax ) ); + b3V32 invScaledBoundsExtent = b3SubV( invScaledBoundsMax, invScaledBoundsCenter ); + + const b3MeshData* data = mesh->data; + + int count = 0; + const b3MeshNode* stack[B3_MESH_STACK_SIZE]; + const b3MeshNode* node = b3GetRoot( data ); + const b3MeshTriangle* triangles = b3GetMeshTriangles( data ); + const b3Vec3* vertices = b3GetMeshVertices( data ); + + while ( true ) + { + // Test node overlap in unscaled space + b3V32 nodeMin = b3LoadV( &node->lowerBound.x ); + b3V32 nodeMax = b3LoadV( &node->upperBound.x ); + + if ( b3TestBoundsOverlap( nodeMin, nodeMax, invScaledBoundsMin, invScaledBoundsMax ) ) + { + if ( b3IsLeaf( node ) ) + { + int triangleCount = node->data.asLeaf.triangleCount; + int triangleOffset = node->triangleOffset; + + for ( int index = 0; index < triangleCount; ++index ) + { + int triangleIndex = triangleOffset + index; + b3MeshTriangle triangle = triangles[triangleIndex]; + + b3Vec3 vertex1 = vertices[triangle.index1]; + b3Vec3 vertex2 = vertices[triangle.index2]; + b3Vec3 vertex3 = vertices[triangle.index3]; + b3V32 v1 = b3LoadV( &vertex1.x ); + b3V32 v2 = b3LoadV( &vertex2.x ); + b3V32 v3 = b3LoadV( &vertex3.x ); + + // Perform triangle overlap test in unscaled space. Winding order doesn't matter. + // todo it is possible that some margins are getting scaled + if ( b3TestBoundsTriangleOverlap( invScaledBoundsCenter, invScaledBoundsExtent, v1, v2, v3 ) ) + { + b3Vec3 a = b3Mul( meshScale, vertex1 ); + b3Vec3 b, c; + if ( clockwise ) + { + b = b3Mul( meshScale, vertex2 ); + c = b3Mul( meshScale, vertex3 ); + } + else + { + b = b3Mul( meshScale, vertex3 ); + c = b3Mul( meshScale, vertex2 ); + } + + bool result = fcn( a, b, c, triangleIndex, context ); + if ( result == false ) + { + return; + } + } + } + } + else + { + // Recurse + B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 ); + stack[count++] = b3GetRightChild( node ); + node = b3GetLeftChild( node ); + + continue; + } + } + + if ( count == 0 ) + { + break; + } + node = stack[--count]; + } +} diff --git a/src/mesh_contact.c b/src/mesh_contact.c new file mode 100644 index 0000000..bcb74b4 --- /dev/null +++ b/src/mesh_contact.c @@ -0,0 +1,1130 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "contact.h" +#include "manifold.h" +#include "physics_world.h" +#include "qsort.h" +#include "shape.h" + +#include "box3d/types.h" + +#include + +#if B3_ENABLE_VALIDATION +static bool b3IsSorted( const int* array, int count ) +{ + for ( int i = 0; i < count - 1; ++i ) + { + if ( array[i] >= array[i + 1] ) + { + return false; + } + } + + return true; +} +#endif + +typedef struct b3TriangleQueryContext +{ + int* indices; + int capacity; + int count; +} b3TriangleQueryContext; + +static bool b3CollectTriangleIndicesCallback( b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleIndex, void* context ) +{ + B3_UNUSED( a, b, c ); + b3TriangleQueryContext* triangleContext = (b3TriangleQueryContext*)context; + if ( triangleContext->count == triangleContext->capacity ) + { + return false; + } + triangleContext->indices[triangleContext->count] = triangleIndex; + triangleContext->count += 1; + return triangleContext->count < triangleContext->capacity; +} + +static int b3QueryMeshTriangles( int* indices, int capacity, const b3Mesh* mesh, b3AABB bounds ) +{ + b3TriangleQueryContext context = { + .indices = indices, + .capacity = capacity, + .count = 0, + }; + + b3QueryMesh( mesh, bounds, b3CollectTriangleIndicesCallback, &context ); + return context.count; +} + +static int b3QueryHeightFieldTriangles( int* indices, int capacity, const b3HeightField* heightField, b3AABB bounds ) +{ + b3TriangleQueryContext context = { + .indices = indices, + .capacity = capacity, + .count = 0, + }; + + b3QueryHeightField( heightField, bounds, b3CollectTriangleIndicesCallback, &context ); + return context.count; +} + +static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3Transform xfA, const b3AABB* bounds, b3Arena arena ) +{ + B3_ASSERT( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape ); + + b3MeshContact* meshContact = &contact->meshContact; + + // If the dynamic body didn't move out of the cached query bounds we are done! + if ( b3AABB_Contains( meshContact->queryBounds, *bounds ) ) + { + if ( shapeA->type == b3_meshShape ) + { + for ( int i = 0; i < contact->meshContact.triangleCache.count; ++i ) + { + B3_ASSERT( 0 <= contact->meshContact.triangleCache.data[i].triangleIndex && + contact->meshContact.triangleCache.data[i].triangleIndex < shapeA->mesh.data->triangleCount ); + } + } + + return; + } + + // Enlarge to the query bounds to absorb small movement + float radius = B3_AABB_MARGIN + B3_SPECULATIVE_DISTANCE; + b3Vec3 extension = { radius, radius, radius }; + meshContact->queryBounds.lowerBound = b3Sub( bounds->lowerBound, extension ); + meshContact->queryBounds.upperBound = b3Add( bounds->upperBound, extension ); + + // Query triangles + int triangleCapacity = 256; + int* triangleIndices = (int*)b3Bump( &arena, triangleCapacity * sizeof( int ) ); + + // Bounds are in world space. Convert to the local mesh frame. + b3AABB localBounds = b3AABB_Transform( b3InvertTransform( xfA ), meshContact->queryBounds ); + int triangleCount; + if ( shapeA->type == b3_meshShape ) + { + triangleCount = b3QueryMeshTriangles( triangleIndices, triangleCapacity, &shapeA->mesh, localBounds ); + } + else + { + B3_ASSERT( shapeA->type == b3_heightShape ); + triangleCount = b3QueryHeightFieldTriangles( triangleIndices, triangleCapacity, shapeA->heightField, localBounds ); + } + + if ( triangleCount == triangleCapacity ) + { + b3Log( "WARNING: complex mesh detected, triangle buffer capacity of %d reached", triangleCapacity ); + B3_VALIDATE( false ); + } + + // Triangle indices must be sorted to match caches. + B3_VALIDATE( b3IsSorted( triangleIndices, triangleCount ) ); + + // Create new contact cache and match with old one + b3ContactCache* contactCache = (b3ContactCache*)b3Bump( &arena, triangleCount * sizeof( b3ContactCache ) ); + + int index2 = 0; + for ( int index1 = 0; index1 < triangleCount; ++index1 ) + { + contactCache[index1] = (b3ContactCache){ 0 }; + + while ( index2 < contact->meshContact.triangleCache.count && + contact->meshContact.triangleCache.data[index2].triangleIndex < triangleIndices[index1] ) + { + index2 += 1; + } + + if ( index2 < contact->meshContact.triangleCache.count && + contact->meshContact.triangleCache.data[index2].triangleIndex == triangleIndices[index1] ) + { + contactCache[index1] = contact->meshContact.triangleCache.data[index2].cache; + } + } + + // Save new cache + b3Array_Resize( contact->meshContact.triangleCache, triangleCount ); + for ( int i = 0; i < triangleCount; ++i ) + { + contact->meshContact.triangleCache.data[i] = (b3TriangleCache){ triangleIndices[i], contactCache[i] }; + + if ( shapeA->type == b3_meshShape ) + { + B3_ASSERT( 0 <= contact->meshContact.triangleCache.data[i].triangleIndex && + contact->meshContact.triangleCache.data[i].triangleIndex < shapeA->mesh.data->triangleCount ); + } + } +} + +typedef struct b3TentativeTriangle +{ + float squaredDistance; + int index; +} b3TentativeTriangle; + +#define B3_MAX_EDGE_COUNT 64 + +typedef struct b3FoundEdges +{ + uint64_t keys[B3_MAX_EDGE_COUNT]; + int count; +} b3FoundEdges; + +static inline bool b3AddEdge( b3FoundEdges* edges, int vertex1, int vertex2 ) +{ + uint64_t i1 = (uint64_t)b3MinInt( vertex1, vertex2 ); + uint64_t i2 = (uint64_t)b3MaxInt( vertex1, vertex2 ); + uint64_t key = i1 << 32 | i2; + + int count = edges->count; + for ( int i = 0; i < count; ++i ) + { + if ( edges->keys[i] == key ) + { + return false; + } + } + + if ( count == B3_MAX_EDGE_COUNT ) + { + // This will lead to a potential ghost collision + return true; + } + + edges->keys[count] = key; + edges->count += 1; + + return true; +} + +static inline bool b3FindEdge( b3FoundEdges* edges, int vertex1, int vertex2 ) +{ + uint64_t i1 = (uint64_t)b3MinInt( vertex1, vertex2 ); + uint64_t i2 = (uint64_t)b3MaxInt( vertex1, vertex2 ); + uint64_t key = i1 << 32 | i2; + + int count = edges->count; + for ( int i = 0; i < count; ++i ) + { + if ( edges->keys[i] == key ) + { + return true; + } + } + + return false; +} + +#if 0 +// Two triangles share an edge iff they share at least two vertex indices. +static inline bool b3TrianglesShareEdge( int a1, int a2, int a3, int b1, int b2, int b3 ) +{ + int matches = 0; + matches += ( a1 == b1 || a1 == b2 || a1 == b3 ); + matches += ( a2 == b1 || a2 == b2 || a2 == b3 ); + matches += ( a3 == b1 || a3 == b2 || a3 == b3 ); + return matches >= 2; +} +#endif + +#define B3_MAX_VERTEX_COUNT 64 + +typedef struct b3FoundVertices +{ + int keys[B3_MAX_VERTEX_COUNT]; + int count; +} b3FoundVertices; + +static inline bool b3AddVertex( b3FoundVertices* vertices, int vertex ) +{ + int key = vertex; + + int count = vertices->count; + for ( int i = 0; i < count; ++i ) + { + if ( vertices->keys[i] == key ) + { + return false; + } + } + + if ( count == B3_MAX_VERTEX_COUNT ) + { + // This will lead to a potential ghost collision + return true; + } + + vertices->keys[count] = key; + vertices->count += 1; + + return true; +} + +int b3CullPoints( b3Point2D* points, int count, int target ) +{ + if ( count <= 1 ) + { + return count; + } + + float tol = 0.25f * B3_LINEAR_SLOP; + float tolSqr = tol * tol; + + b3Point2D finalPoints[4]; + int count1 = count; + + // Step 1: the two points with the largest distance + float bestScore = 0.0f; + int bestIndex1 = B3_NULL_INDEX; + int bestIndex2 = B3_NULL_INDEX; + + for ( int i = 0; i < count1; ++i ) + { + b3Vec2 p1 = points[i].p; + for ( int j = i + 1; j < count1; ++j ) + { + float score = b3DistanceSquared2( p1, points[j].p ); + + // Use tolerance for welding and hysteresis + if ( score > bestScore + tolSqr ) + { + bestIndex1 = i; + bestIndex2 = j; + bestScore = score; + } + } + } + + if ( bestScore < tolSqr || target == 1 ) + { + // All points coincident. Just use first point. + return 1; + } + + finalPoints[0] = points[bestIndex1]; + finalPoints[1] = points[bestIndex2]; + + // Cull + points[bestIndex2] = points[count1 - 1]; + points[bestIndex1] = points[count1 - 2]; + count1 -= 2; + + if ( count1 == 0 || target == 2 ) + { + points[0] = finalPoints[0]; + points[1] = finalPoints[1]; + return 2; + } + + // First anchor point + b3Vec2 a = finalPoints[0].p; + + // Second anchor point + b3Vec2 b = finalPoints[1].p; + b3Vec2 ba = b3Sub2( b, a ); + // float length = b3Length2( ba ); + // float areaTol = tol * length; + + // Step 2: find the point with the maximum triangular area + bestScore = 0.0f; + int bestIndex = B3_NULL_INDEX; + float bestSignedArea = 0.0f; + for ( int i = 0; i < count1; ++i ) + { + b3Vec2 p = points[i].p; + float signedArea = b3Cross2( ba, b3Sub2( p, a ) ); + float score = b3AbsFloat( signedArea ); + + // Use the area tolerance for collinear points and hysteresis + if ( score > bestScore + tolSqr ) + { + bestSignedArea = signedArea; + bestScore = score; + bestIndex = i; + } + } + + if ( bestIndex == B3_NULL_INDEX ) + { + // All points collinear + points[0] = finalPoints[0]; + points[1] = finalPoints[1]; + return 2; + } + + // Store best point + finalPoints[2] = points[bestIndex]; + + if ( count1 == 1 || target == 3 ) + { + points[0] = finalPoints[0]; + points[1] = finalPoints[1]; + points[2] = finalPoints[2]; + return 3; + } + + // Cull + points[bestIndex] = points[count1 - 1]; + count1 -= 1; + + // Step 4: get the point that adds the most area outside the current triangle + + // Third anchor + b3Vec2 c = finalPoints[2].p; + + // Ensure CCW ordering + if ( bestSignedArea < 0.0f ) + { + B3_SWAP( b, c ); + ba = b3Sub2( b, a ); + } + + b3Vec2 cb = b3Sub2( c, b ); + b3Vec2 ac = b3Sub2( a, c ); + + bestScore = 0.0f; + bestIndex = B3_NULL_INDEX; + for ( int i = 0; i < count1; ++i ) + { + b3Vec2 p = points[i].p; + float u1 = b3Cross2( b3Sub2( p, a ), ba ); + float u2 = b3Cross2( b3Sub2( p, b ), cb ); + float u3 = b3Cross2( b3Sub2( p, c ), ac ); + float score = b3MaxFloat( u1, b3MaxFloat( u2, u3 ) ); + + // Use the area tolerance for collinear points and hysteresis + if ( score > bestScore + tolSqr ) + { + bestScore = score; + bestIndex = i; + } + } + + if ( bestIndex == B3_NULL_INDEX ) + { + // No additional area + points[0] = finalPoints[0]; + points[1] = finalPoints[1]; + points[2] = finalPoints[2]; + return 3; + } + + // Store best point + finalPoints[3] = points[bestIndex]; + + // Full quad + points[0] = finalPoints[0]; + points[1] = finalPoints[1]; + points[2] = finalPoints[2]; + points[3] = finalPoints[3]; + return 4; +} + +static int b3ReduceCluster( b3LocalManifoldPoint* points, int count1, b3Vec3 normal, b3Arena arena ) +{ + int targetCount = 1; + if ( count1 <= targetCount ) + { + return count1; + } + + b3Point2D* pts = (b3Point2D*)b3Bump( &arena, count1 * sizeof( b3Point2D ) ); + b3Vec3 u = b3Perp( normal ); + b3Vec3 v = b3Cross( normal, u ); + b3Vec3 origin = points[0].point; + + for ( int i = 0; i < count1; ++i ) + { + pts[i].originalIndex = (int16_t)i; + pts[i].separation = 0.0f; + + b3Vec3 d = b3Sub( points[i].point, origin ); + pts[i].p = (b3Vec2){ b3Dot( d, u ), b3Dot( d, v ) }; + pts[i].persisted = false; + } + + int count3 = b3CullPoints( pts, count1, 4 ); + + b3LocalManifoldPoint* finalPoints = (b3LocalManifoldPoint*)b3Bump( &arena, count3 * sizeof( b3LocalManifoldPoint ) ); + for ( int i = 0; i < count3; ++i ) + { + int index = pts[i].originalIndex; + B3_ASSERT( 0 <= index && index < count1 ); + finalPoints[i] = points[index]; + } + + memcpy( points, finalPoints, count3 * sizeof( b3LocalManifoldPoint ) ); + return count3; +} + +typedef struct b3Cluster +{ + b3Vec3 manifoldNormal; + b3Vec3 triangleNormal; + b3LocalManifoldPoint* points; + int pointCapacity; + int pointCount; +} b3Cluster; + +bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact, const b3Shape* shapeA, const int* materialMap, + b3Transform xfA, const b3Shape* shapeB, b3Transform xfB, bool isFast, b3Arena arena ) +{ + B3_ASSERT( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape ); + B3_UNUSED( workerIndex ); + B3_UNUSED( isFast ); + B3_UNUSED( materialMap ); + + b3TaskContext* context = b3Array_Get( world->taskContexts, workerIndex ); + + // todo test using the shape local aabb + b3RefreshCache( contact, shapeA, xfA, &shapeB->aabb, arena ); + + // Collide with triangles and build manifolds + b3MeshContact* meshContact = &contact->meshContact; + int triangleCount = meshContact->triangleCache.count; + + b3LocalManifold** acceptedManifolds = (b3LocalManifold**)b3Bump( &arena, triangleCount * sizeof( b3LocalManifold* ) ); + int acceptedManifoldCount = 0; + b3LocalManifold** tentativeManifolds = (b3LocalManifold**)b3Bump( &arena, triangleCount * sizeof( b3LocalManifold* ) ); + int tentativeManifoldCount = 0; + b3TentativeTriangle* tentativeTriangles = + (b3TentativeTriangle*)b3Bump( &arena, triangleCount * sizeof( b3TentativeTriangle ) ); + int tentativeTriangleCount = 0; + + b3FoundEdges foundEdges; + b3FoundVertices foundVertices; + foundEdges.count = 0; + foundVertices.count = 0; + + // This transform converts from mesh frame into the shapeB frame + b3Transform transformAtoB = b3InvMulTransforms( xfB, xfA ); + b3Matrix3 relativeMatrix = b3MakeMatrixFromQuat( transformAtoB.q ); + float linearSlop = B3_LINEAR_SLOP; + + int pointBufferCapacity = 256; + b3LocalManifoldPoint* pointBuffer = + (b3LocalManifoldPoint*)b3Bump( &arena, pointBufferCapacity * sizeof( b3LocalManifoldPoint ) ); + int totalPointCount = 0; + + b3LocalManifold* manifoldBuffer = (b3LocalManifold*)b3Bump( &arena, triangleCount * sizeof( b3LocalManifold ) ); + int manifoldCount = 0; + + b3TriangleCache* triangleCaches = meshContact->triangleCache.data; + + for ( int index = 0; index < triangleCount && totalPointCount + 3 < pointBufferCapacity; ++index ) + { + int triangleIndex = triangleCaches[index].triangleIndex; + + b3Triangle triangle; + if ( shapeA->type == b3_meshShape ) + { + triangle = b3GetMeshTriangle( &shapeA->mesh, triangleIndex ); + } + else + { + B3_ASSERT( shapeA->type == b3_heightShape ); + triangle = b3GetHeightFieldTriangle( shapeA->heightField, triangleIndex ); + } + + // Transform triangle into the shape frame + b3Vec3 vertices[3]; + vertices[0] = b3Add( b3MulMV( relativeMatrix, triangle.vertices[0] ), transformAtoB.p ); + vertices[1] = b3Add( b3MulMV( relativeMatrix, triangle.vertices[1] ), transformAtoB.p ); + vertices[2] = b3Add( b3MulMV( relativeMatrix, triangle.vertices[2] ), transformAtoB.p ); + + b3ContactCache* cache = &triangleCaches[index].cache; + int pointCapacity = pointBufferCapacity - totalPointCount; + b3LocalManifold* manifold = manifoldBuffer + manifoldCount; + manifold->points = pointBuffer + totalPointCount; + manifold->pointCount = 0; + manifold->triangleFlags = triangle.flags; + manifold->feature = b3_featureNone; + + switch ( shapeB->type ) + { + case b3_capsuleShape: + b3CollideCapsuleAndTriangle( manifold, pointCapacity, &shapeB->capsule, vertices, &cache->simplexCache ); + break; + + case b3_hullShape: + // Cached edge contact is dangerous at high speed because the hull can rotate around the edge and tunnel + // through the triangle. + if ( isFast && cache->satCache.type == b3_edgePairAxis ) + { + cache->satCache = (b3SATCache){ 0 }; + } + + b3CollideHullAndTriangle( manifold, pointCapacity, shapeB->hull, vertices, triangle.flags, &cache->satCache ); + context->satCallCount += 1; + context->satCacheHitCount += cache->satCache.hit; + break; + + case b3_sphereShape: + b3CollideSphereAndTriangle( manifold, pointCapacity, &shapeB->sphere, vertices ); + break; + + default: + B3_ASSERT( false ); + return false; + } + + int manifoldPointCount = manifold->pointCount; + + if ( manifoldPointCount > 0 ) + { + B3_ASSERT( manifold->feature != b3_featureNone ); + + manifoldCount += 1; + totalPointCount += manifoldPointCount; + manifold->triangleIndex = triangleIndex; + manifold->trianglePlane = b3MakePlaneFromPoints( vertices[0], vertices[1], vertices[2] ); + manifold->i1 = triangle.i1; + manifold->i2 = triangle.i2; + manifold->i3 = triangle.i3; + + if ( manifold->feature == b3_featureTriangleFace || B3_FORCE_GHOST_COLLISIONS ) + { + (void)b3AddEdge( &foundEdges, triangle.i1, triangle.i2 ); + (void)b3AddEdge( &foundEdges, triangle.i2, triangle.i3 ); + (void)b3AddEdge( &foundEdges, triangle.i3, triangle.i1 ); + (void)b3AddVertex( &foundVertices, triangle.i1 ); + (void)b3AddVertex( &foundVertices, triangle.i2 ); + (void)b3AddVertex( &foundVertices, triangle.i3 ); + + acceptedManifolds[acceptedManifoldCount++] = manifold; + } + else if ( manifold->feature == b3_featureHullFace ) + { + float cosNormalAngle = b3Dot( manifold->trianglePlane.normal, manifold->normal ); + if ( cosNormalAngle > 0.5f ) + { + (void)b3AddEdge( &foundEdges, triangle.i1, triangle.i2 ); + (void)b3AddEdge( &foundEdges, triangle.i2, triangle.i3 ); + (void)b3AddEdge( &foundEdges, triangle.i3, triangle.i1 ); + (void)b3AddVertex( &foundVertices, triangle.i1 ); + (void)b3AddVertex( &foundVertices, triangle.i2 ); + (void)b3AddVertex( &foundVertices, triangle.i3 ); + + acceptedManifolds[acceptedManifoldCount++] = manifold; + } + else + { + float minSeparation = manifold->points[0].separation; + for ( int i = 1; i < manifoldPointCount; ++i ) + { + minSeparation = b3MinFloat( minSeparation, manifold->points[i].separation ); + } + + if ( minSeparation < -2.0f * linearSlop ) + { + // Deep overlap + (void)b3AddEdge( &foundEdges, triangle.i1, triangle.i2 ); + (void)b3AddEdge( &foundEdges, triangle.i2, triangle.i3 ); + (void)b3AddEdge( &foundEdges, triangle.i3, triangle.i1 ); + (void)b3AddVertex( &foundVertices, triangle.i1 ); + (void)b3AddVertex( &foundVertices, triangle.i2 ); + (void)b3AddVertex( &foundVertices, triangle.i3 ); + acceptedManifolds[acceptedManifoldCount++] = manifold; + } + else + { + b3TentativeTriangle tentativeTriangle = { .squaredDistance = manifold->squaredDistance, + .index = tentativeManifoldCount }; + tentativeTriangles[tentativeTriangleCount++] = tentativeTriangle; + tentativeManifolds[tentativeManifoldCount++] = manifold; + } + } + } + else + { + b3TentativeTriangle tentativeTriangle = { .squaredDistance = manifold->squaredDistance, + .index = tentativeManifoldCount }; + tentativeTriangles[tentativeTriangleCount++] = tentativeTriangle; + tentativeManifolds[tentativeManifoldCount++] = manifold; + } + } + } + + B3_ASSERT( acceptedManifoldCount <= triangleCount ); + B3_ASSERT( tentativeManifoldCount <= triangleCount ); + B3_ASSERT( tentativeTriangleCount <= triangleCount ); + + if ( shapeB->type == b3_sphereShape ) + { + // Sort triangles so the closest triangles are processed first + { +#define LESS( i, j ) tentativeTriangles[(int)i].squaredDistance < tentativeTriangles[(int)j].squaredDistance +#define SWAP( i, j ) \ + do \ + { \ + b3TentativeTriangle tmp = tentativeTriangles[(int)i]; \ + tentativeTriangles[(int)i] = tentativeTriangles[(int)j]; \ + tentativeTriangles[(int)j] = tmp; \ + } \ + while ( 0 ) + QSORT( tentativeTriangleCount, LESS, SWAP ); +#undef LESS +#undef SWAP + } + + // Add tentative manifolds in sorted order. Avoid adding manifolds that generate ghost collisions. + for ( int i = 0; i < tentativeTriangleCount; ++i ) + { + b3LocalManifold* m = tentativeManifolds[tentativeTriangles[i].index]; + + bool addedEdge1 = b3AddEdge( &foundEdges, m->i1, m->i2 ); + bool addedEdge2 = b3AddEdge( &foundEdges, m->i2, m->i3 ); + bool addedEdge3 = b3AddEdge( &foundEdges, m->i3, m->i1 ); + bool addedVertex1 = b3AddVertex( &foundVertices, m->i1 ); + bool addedVertex2 = b3AddVertex( &foundVertices, m->i2 ); + bool addedVertex3 = b3AddVertex( &foundVertices, m->i3 ); + + b3TriangleFeature feature = m->feature; + bool shouldCollide = false; + switch ( feature ) + { + case b3_featureNone: + case b3_featureTriangleFace: + B3_ASSERT( false ); + break; + + case b3_featureEdge1: + shouldCollide = addedEdge1; + break; + + case b3_featureEdge2: + shouldCollide = addedEdge2; + break; + + case b3_featureEdge3: + shouldCollide = addedEdge3; + break; + + case b3_featureVertex1: + shouldCollide = addedVertex1; + break; + + case b3_featureVertex2: + shouldCollide = addedVertex2; + break; + + case b3_featureVertex3: + shouldCollide = addedVertex3; + break; + + default: + B3_ASSERT( false ); + break; + } + + if ( shouldCollide == true ) + { + acceptedManifolds[acceptedManifoldCount++] = m; + } + } + } + else + { + // Problem: hull can tunnel if time of impact is at concave edge + // Example: flat box sliding down a ramp to a flat bottom + // Solution: only ignore flat edges + for ( int i = 0; i < tentativeManifoldCount; ++i ) + { + b3LocalManifold* m = tentativeManifolds[i]; + int triangleFlags = m->triangleFlags; + + if ( ( triangleFlags & b3_allFlatEdges ) == b3_allFlatEdges ) + { + continue; + } + + if ( ( triangleFlags & b3_flatEdge1 ) == b3_flatEdge1 ) + { + if ( b3FindEdge( &foundEdges, m->i1, m->i2 ) ) + { + continue; + } + } + + if ( ( triangleFlags & b3_flatEdge2 ) == b3_flatEdge2 ) + { + if ( b3FindEdge( &foundEdges, m->i2, m->i3 ) ) + { + continue; + } + } + + if ( ( triangleFlags & b3_flatEdge3 ) == b3_flatEdge3 ) + { + if ( b3FindEdge( &foundEdges, m->i3, m->i1 ) ) + { + continue; + } + } + + acceptedManifolds[acceptedManifoldCount++] = m; + } + } + + B3_ASSERT( acceptedManifoldCount <= triangleCount ); + + if ( acceptedManifoldCount == 0 ) + { + if ( contact->manifoldCount > 0 ) + { + b3FreeManifolds( world, contact->manifolds, contact->manifoldCount ); + contact->manifolds = NULL; + contact->manifoldCount = 0; + } + return false; + } + + // todo this should push apart shapes after a time of impact event + // todo consider doing this based on speed + // todo this must be constant or varied smoothly or there will be jitter + // In the past I've called this `polygon skin`, but PhysX and Unreal call it `rest offset` which + // seems appropriate in this case. + float restOffset = linearSlop; + + b3Cluster* clusters = (b3Cluster*)b3Bump( &arena, acceptedManifoldCount * sizeof( b3Cluster ) ); + int* clusterMemberships = (int*)b3Bump( &arena, acceptedManifoldCount * sizeof( int ) ); + + int clusterCount = 0; + // Cluster tolerance is tighter than the warm starting manifold matching tolerance. These + // serve different purposes. + const float clusterThreshold = 0.996f; + int pointCount2 = 0; + for ( int i = 0; i < acceptedManifoldCount; ++i ) + { + clusterMemberships[i] = B3_NULL_INDEX; + + const b3LocalManifold* manifold = acceptedManifolds[i]; + pointCount2 += manifold->pointCount; + + // Cluster based on the triangle normal and contact normal. + // The first cluster found is accepted because the tolerance is tight. + // todo consider requiring the triangles to be connect by an edge. + // todo consider looking for the best cluster instead of the first one within tolerance + // This bool is here to allow quick testing with and without clustering. + bool allowClustering = true; + b3Vec3 manifoldNormal = manifold->normal; + b3Vec3 triangleNormal = manifold->trianglePlane.normal; + int clusterIndex = B3_NULL_INDEX; + for ( int j = 0; j < clusterCount && allowClustering; ++j ) + { + float cosManifoldAngle = b3Dot( clusters[j].manifoldNormal, manifoldNormal ); + float cosTriangleAngle = b3Dot( clusters[j].triangleNormal, triangleNormal ); + if ( cosManifoldAngle <= clusterThreshold || cosTriangleAngle <= clusterThreshold ) + { + continue; + } + +#if 0 + // todo there could be later triangles that create the connection + // then failure to cluster breaks greedy impulse warm starting + bool edgeConnected = false; + + for ( int k = 0; k < i; ++k ) + { + if ( clusterMemberships[k] != j ) + { + continue; + } + + const b3LocalManifold* other = acceptedManifolds[k]; + if ( b3TrianglesShareEdge( manifold->i1, manifold->i2, manifold->i3, other->i1, other->i2, other->i3 ) ) + { + edgeConnected = true; + break; + } + } + + if ( edgeConnected ) + { + clusterIndex = j; + break; + } +#else + + // Found a cluster + clusterIndex = j; + break; +#endif + } + + if ( clusterIndex != B3_NULL_INDEX ) + { + clusterMemberships[i] = clusterIndex; + clusters[clusterIndex].pointCapacity += manifold->pointCount; + } + else + { + clusters[clusterCount].manifoldNormal = manifoldNormal; + clusters[clusterCount].triangleNormal = triangleNormal; + clusters[clusterCount].pointCapacity = manifold->pointCount; + clusterMemberships[i] = clusterCount; + clusterCount += 1; + } + } + + if ( pointCount2 == 0 ) + { + return false; + } + + // Setup clusters + b3LocalManifoldPoint* clusterPoints = (b3LocalManifoldPoint*)b3Bump( &arena, pointCount2 * sizeof( b3LocalManifoldPoint ) ); + int pointOffset = 0; + + for ( int i = 0; i < clusterCount; ++i ) + { + b3Cluster* cluster = clusters + i; + cluster->points = clusterPoints + pointOffset; + cluster->pointCount = 0; + pointOffset += cluster->pointCapacity; + } + + // Populate clusters + for ( int i = 0; i < acceptedManifoldCount; ++i ) + { + int clusterIndex = clusterMemberships[i]; + if ( clusterIndex == B3_NULL_INDEX ) + { + continue; + } + + B3_ASSERT( 0 <= clusterIndex && clusterIndex < clusterCount ); + + b3LocalManifold* am = acceptedManifolds[i]; + b3Cluster* cm = clusters + clusterIndex; + for ( int j = 0; j < am->pointCount; ++j ) + { + B3_ASSERT( cm->pointCount < cm->pointCapacity ); + b3LocalManifoldPoint* ap = am->points + j; + b3LocalManifoldPoint* cp = cm->points + cm->pointCount; + + // The contact normal may be different than the cluster normal (triangle normal) + cp->normal = am->normal; + cp->triangleIndex = am->triangleIndex; + cp->point = ap->point; + cp->separation = ap->separation; + cp->pair = ap->pair; + cm->pointCount += 1; + } + } + + // Simplify clusters + for ( int i = 0; i < clusterCount; ++i ) + { + b3Cluster* cm = clusters + i; + B3_ASSERT( cm->pointCount == cm->pointCapacity ); + int reducedCount = b3ReduceCluster( cm->points, cm->pointCount, cm->triangleNormal, arena ); + cm->pointCount = reducedCount; + } + + // Make a temporary copy of previous manifolds + int oldManifoldCount = contact->manifoldCount; + b3Manifold* oldManifolds = NULL; + if ( oldManifoldCount > 0 ) + { + oldManifolds = (b3Manifold*)b3Bump( &arena, oldManifoldCount * sizeof( b3Manifold ) ); + memcpy( oldManifolds, contact->manifolds, oldManifoldCount * sizeof( b3Manifold ) ); + } + + // Resize manifolds if needed + if ( oldManifoldCount != clusterCount ) + { + b3FreeManifolds( world, contact->manifolds, contact->manifoldCount ); + contact->manifolds = b3AllocateManifolds( world, clusterCount ); + contact->manifoldCount = (uint16_t)clusterCount; + } + else + { + // Mem zero manifolds + memset( contact->manifolds, 0, contact->manifoldCount * sizeof( b3Manifold ) ); + } + + bool* consumed = NULL; + if ( oldManifoldCount > 0 ) + { + consumed = (bool*)b3Bump( &arena, oldManifoldCount * sizeof( bool ) ); + memset( consumed, 0, oldManifoldCount * sizeof( bool ) ); + } + + b3Matrix3 matrixB = b3MakeMatrixFromQuat( xfB.q ); + b3Vec3 offsetA = b3Sub( xfB.p, xfA.p ); + + const float normalMatchTolerance = 0.995f; + for ( int i = 0; i < clusterCount; ++i ) + { + b3Cluster* cm = clusters + i; + int pointCount = cm->pointCount; + B3_ASSERT( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); + + b3Manifold* manifold = contact->manifolds + i; + manifold->pointCount = pointCount; + manifold->normal = b3MulMV( matrixB, cm->manifoldNormal ); + + b3Vec3 clusterNormal = b3MulMV( matrixB, cm->manifoldNormal ); + float bestDot = normalMatchTolerance; + int bestIndex = B3_NULL_INDEX; + + for ( int j = 0; j < oldManifoldCount; ++j ) + { + if ( consumed[j] == true ) + { + continue; + } + + float dot = b3Dot( oldManifolds[j].normal, clusterNormal ); + if ( dot > bestDot ) + { + bestIndex = j; + bestDot = dot; + } + } + + b3Manifold* matchedManifold = NULL; + if ( bestIndex != B3_NULL_INDEX ) + { + matchedManifold = oldManifolds + bestIndex; + manifold->frictionImpulse = matchedManifold->frictionImpulse; + manifold->rollingImpulse = matchedManifold->rollingImpulse; + manifold->twistImpulse = matchedManifold->twistImpulse; + consumed[bestIndex] = true; + } + + for ( int j = 0; j < pointCount; ++j ) + { + const b3LocalManifoldPoint* source = cm->points + j; + b3ManifoldPoint* target = manifold->points + j; + + // Contact points are computed in frame B + target->anchorB = b3MulMV( matrixB, source->point ); + target->anchorA = b3Add( target->anchorB, offsetA ); + target->separation = source->separation - restOffset; + target->featureId = b3MakeFeatureId( source->pair ); + target->triangleIndex = source->triangleIndex; + + // Preserve normal impulse if possible + if ( matchedManifold != NULL ) + { + int oldPointCount = matchedManifold->pointCount; + for ( int k = 0; k < oldPointCount; ++k ) + { + b3ManifoldPoint* oldPt = matchedManifold->points + k; + + if ( target->featureId == oldPt->featureId && target->triangleIndex == oldPt->triangleIndex ) + { + target->normalImpulse = oldPt->normalImpulse; + target->persisted = true; + + // claimed + oldPt->triangleIndex = B3_NULL_INDEX; + break; + } + } + } + } + } + + const b3SurfaceMaterial* materialB = shapeB->materials + 0; + b3Vec3 tangentVelocityA = b3Vec3_zero; + + // Update friction and restitution if the mesh has per triangle material + if ( shapeA->materialCount > 0 ) + { + float friction = 0.0f; + float restitution = 0.0f; + float sampleCount = 0.0f; + + const uint8_t* materialIndices; + if ( shapeA->type == b3_meshShape ) + { + materialIndices = b3GetMeshMaterialIndices( shapeA->mesh.data ); + } + else + { + materialIndices = shapeA->heightField->materialIndices; + } + + for ( int i = 0; i < clusterCount; ++i ) + { + b3Manifold* manifold = contact->manifolds + i; + int pointCount = manifold->pointCount; + for ( int j = 0; j < pointCount; ++j ) + { + int triangleIndex = manifold->points[j].triangleIndex; + int materialIndex; + if ( shapeA->type == b3_meshShape ) + { + materialIndex = materialIndices[triangleIndex]; + + if ( materialMap != NULL ) + { + materialIndex = materialMap[materialIndex]; + } + } + else + { + materialIndex = materialIndices[triangleIndex >> 1]; + } + + materialIndex = b3ClampInt( materialIndex, 0, shapeA->materialCount - 1 ); + b3SurfaceMaterial material = shapeA->materials[materialIndex]; + friction += world->frictionCallback( material.friction, material.userMaterialId, materialB->friction, + materialB->userMaterialId ); + restitution += world->restitutionCallback( material.restitution, material.userMaterialId, materialB->restitution, + materialB->userMaterialId ); + + tangentVelocityA = b3Add( tangentVelocityA, material.tangentVelocity ); + + sampleCount += 1.0f; + } + } + + if ( sampleCount > 0.0f ) + { + float invCount = 1.0f / sampleCount; + contact->friction = invCount * friction; + contact->restitution = invCount * restitution; + tangentVelocityA = b3MulSV( invCount, tangentVelocityA ); + } + + B3_ASSERT( b3IsValidFloat( contact->friction ) && contact->friction >= 0.0f ); + B3_ASSERT( b3IsValidFloat( contact->restitution ) && contact->restitution >= 0.0f ); + } + else + { + // Keep these updated in case the values on the shapes are modified + contact->friction = world->frictionCallback( shapeA->materials[0].friction, shapeA->materials[0].userMaterialId, + materialB->friction, materialB->userMaterialId ); + contact->restitution = world->restitutionCallback( shapeA->materials[0].restitution, shapeA->materials[0].userMaterialId, + materialB->restitution, materialB->userMaterialId ); + tangentVelocityA = shapeA->materials[0].tangentVelocity; + } + + tangentVelocityA = b3RotateVector( xfA.q, tangentVelocityA ); + + float radiusB = 0.0f; + if ( shapeB->type == b3_sphereShape ) + { + radiusB = shapeB->sphere.radius; + } + else if ( shapeB->type == b3_capsuleShape ) + { + radiusB = shapeB->capsule.radius; + } + else if ( shapeB->type == b3_hullShape ) + { + radiusB = shapeB->hull->innerRadius; + } + + contact->rollingResistance = shapeB->materials[0].rollingResistance * radiusB; + + b3Vec3 tangentVelocityB = b3RotateVector( xfB.q, materialB->tangentVelocity ); + contact->tangentVelocity = b3Sub( tangentVelocityA, tangentVelocityB ); + return true; +} diff --git a/src/motor_joint.c b/src/motor_joint.c new file mode 100644 index 0000000..1b4b584 --- /dev/null +++ b/src/motor_joint.c @@ -0,0 +1,437 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "math_internal.h" +#include "physics_world.h" +#include "solver.h" +#include "solver_set.h" + +// needed for dll export +#include "box3d/box3d.h" + +void b3MotorJoint_SetLinearVelocity( b3JointId jointId, b3Vec3 velocity ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + joint->motorJoint.linearVelocity = velocity; +} + +b3Vec3 b3MotorJoint_GetLinearVelocity( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + return joint->motorJoint.linearVelocity; +} + +void b3MotorJoint_SetAngularVelocity( b3JointId jointId, b3Vec3 velocity ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + joint->motorJoint.angularVelocity = velocity; +} + +b3Vec3 b3MotorJoint_GetAngularVelocity( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + return joint->motorJoint.angularVelocity; +} + +void b3MotorJoint_SetMaxVelocityTorque( b3JointId jointId, float maxTorque ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + joint->motorJoint.maxVelocityTorque = maxTorque; +} + +float b3MotorJoint_GetMaxVelocityTorque( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + return joint->motorJoint.maxVelocityTorque; +} + +void b3MotorJoint_SetMaxVelocityForce( b3JointId jointId, float maxForce ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + joint->motorJoint.maxVelocityForce = maxForce; +} + +float b3MotorJoint_GetMaxVelocityForce( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + return joint->motorJoint.maxVelocityForce; +} + +void b3MotorJoint_SetLinearHertz( b3JointId jointId, float hertz ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + joint->motorJoint.linearHertz = hertz; +} + +float b3MotorJoint_GetLinearHertz( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + return joint->motorJoint.linearHertz; +} + +void b3MotorJoint_SetLinearDampingRatio( b3JointId jointId, float damping ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + joint->motorJoint.linearDampingRatio = damping; +} + +float b3MotorJoint_GetLinearDampingRatio( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + return joint->motorJoint.linearDampingRatio; +} + +void b3MotorJoint_SetAngularHertz( b3JointId jointId, float hertz ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + joint->motorJoint.angularHertz = hertz; +} + +float b3MotorJoint_GetAngularHertz( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + return joint->motorJoint.angularHertz; +} + +void b3MotorJoint_SetAngularDampingRatio( b3JointId jointId, float damping ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + joint->motorJoint.angularDampingRatio = damping; +} + +float b3MotorJoint_GetAngularDampingRatio( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + return joint->motorJoint.angularDampingRatio; +} + +void b3MotorJoint_SetMaxSpringForce( b3JointId jointId, float maxForce ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + joint->motorJoint.maxSpringForce = b3MaxFloat( 0.0f, maxForce ); +} + +float b3MotorJoint_GetMaxSpringForce( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + return joint->motorJoint.maxSpringForce; +} + +void b3MotorJoint_SetMaxSpringTorque( b3JointId jointId, float maxTorque ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + joint->motorJoint.maxSpringTorque = b3MaxFloat( 0.0f, maxTorque ); +} + +float b3MotorJoint_GetMaxSpringTorque( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + return joint->motorJoint.maxSpringTorque; +} + +b3Vec3 b3GetMotorJointForce( b3World* world, b3JointSim* base ) +{ + b3Vec3 force = b3MulSV( world->inv_h, b3Add( base->motorJoint.linearVelocityImpulse, base->motorJoint.linearSpringImpulse ) ); + return force; +} + +b3Vec3 b3GetMotorJointTorque( b3World* world, b3JointSim* base ) +{ + return b3MulSV( world->inv_h, b3Add( base->motorJoint.angularVelocityImpulse, base->motorJoint.angularSpringImpulse ) ); +} + +// Point-to-point constraint +// C = p2 - p1 +// Cdot = v2 - v1 +// = v2 + cross(w2, r2) - v1 - cross(w1, r1) +// J = [-I -r1_skew I r2_skew ] +// Identity used: +// w k % (rx i + ry j) = w * (-ry i + rx j) + +// Angle constraint +// C = angle2 - angle1 - referenceAngle +// Cdot = w2 - w1 +// J = [0 0 -1 0 0 1] +// K = invI1 + invI2 + +void b3PrepareMotorJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_motorJoint ); + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b3World* world = context->world; + + b3Body* bodyA = b3Array_Get( world->bodies, idA ); + b3Body* bodyB = b3Array_Get( world->bodies, idB ); + + B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); + + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + base->invMassA = bodySimA->invMass; + base->invMassB = bodySimB->invMass; + base->invIA = bodySimA->invInertiaWorld; + base->invIB = bodySimB->invInertiaWorld; + + b3Matrix3 invInertiaSum = b3AddMM( base->invIA, base->invIB ); + base->fixedRotation = b3Det( invInertiaSum ) < 1000.0f * FLT_MIN; + + b3MotorJoint* joint = &base->motorJoint; + joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX; + joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX; + + // Compute joint anchor frames with world space rotation, relative to center of mass + joint->frameA.q = b3MulQuat( bodySimA->transform.q, base->localFrameA.q ); + joint->frameA.p = b3RotateVector( bodySimA->transform.q, b3Sub( base->localFrameA.p, bodySimA->localCenter ) ); + joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + joint->frameB.p = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) ); + + // Compute the initial center delta. Incremental position updates are relative to this. + joint->deltaCenter = b3Sub( bodySimB->center, bodySimA->center ); + + joint->linearSpring = b3MakeSoft( joint->linearHertz, joint->linearDampingRatio, context->h ); + joint->angularSpring = b3MakeSoft( joint->angularHertz, joint->angularDampingRatio, context->h ); + + joint->angularMass = b3InvertMatrix( invInertiaSum ); + + if ( context->enableWarmStarting == false ) + { + joint->linearVelocityImpulse = b3Vec3_zero; + joint->angularVelocityImpulse = b3Vec3_zero; + joint->linearSpringImpulse = b3Vec3_zero; + joint->angularSpringImpulse = b3Vec3_zero; + } +} + +void b3WarmStartMotorJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_motorJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + b3MotorJoint* joint = &base->motorJoint; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + b3Vec3 linearImpulse = b3Add( joint->linearVelocityImpulse, joint->linearSpringImpulse ); + b3Vec3 angularImpulse = b3Add( joint->angularVelocityImpulse, joint->angularSpringImpulse ); + + stateA->linearVelocity = b3MulSub( stateA->linearVelocity, mA, linearImpulse ); + stateA->angularVelocity = b3Sub( stateA->angularVelocity, b3MulMV( iA, b3Add( b3Cross( rA, linearImpulse ), angularImpulse ) ) ); + stateB->linearVelocity = b3MulAdd( stateB->linearVelocity, mB, linearImpulse ); + stateB->angularVelocity = b3Add( stateB->angularVelocity, b3MulMV( iB, b3Add( b3Cross( rB, linearImpulse ), angularImpulse ) ) ); +} + +void b3SolveMotorJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_motorJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3MotorJoint* joint = &base->motorJoint; + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + b3Quat quatA = b3MulQuat( stateA->deltaRotation, joint->frameA.q ); + b3Quat quatB = b3MulQuat( stateB->deltaRotation, joint->frameB.q ); + + if ( b3DotQuat( quatA, quatB ) < 0.0f ) + { + // this keeps the rotation angle in the range [-pi, pi] + quatB = b3NegateQuat( quatB ); + } + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + // angular spring + if ( joint->maxSpringTorque > 0.0f && joint->angularHertz > 0.0f ) + { + b3Quat targetQuat = b3Quat_identity; + b3Vec3 deltaRotation = b3DeltaQuatToRotation( relQ, targetQuat ); + b3Vec3 c = b3Neg( b3RotateVector( quatA, deltaRotation ) ); + + b3Vec3 bias = b3MulSV( joint->angularSpring.biasRate, c ); + float massScale = joint->angularSpring.massScale; + float impulseScale = joint->angularSpring.impulseScale; + + b3Vec3 cdot = b3Sub( wB, wA ); + + float maxImpulse = context->h * joint->maxSpringTorque; + b3Vec3 oldImpulse = joint->angularSpringImpulse; + b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b3MulMV( joint->angularMass, b3Add( cdot, bias ) ) ), impulseScale, oldImpulse ); + joint->angularSpringImpulse = b3Add( oldImpulse, impulse ); + if ( b3LengthSquared( joint->angularSpringImpulse ) > maxImpulse * maxImpulse ) + { + joint->angularSpringImpulse = b3MulSV( maxImpulse, b3Normalize( joint->angularSpringImpulse ) ); + } + impulse = b3Sub( joint->angularSpringImpulse, oldImpulse ); + + wA = b3Sub( wA, b3MulMV( iA, impulse ) ); + wB = b3Add( wB, b3MulMV( iB, impulse ) ); + } + + // angular velocity + if ( joint->maxVelocityTorque > 0.0 ) + { + b3Vec3 cdot = b3Sub( b3Sub( wB, wA ), joint->angularVelocity ); + b3Vec3 impulse = b3Neg( b3MulMV( joint->angularMass, cdot ) ); + + float maxImpulse = context->h * joint->maxVelocityTorque; + b3Vec3 oldImpulse = joint->angularVelocityImpulse; + joint->angularVelocityImpulse = b3Add( oldImpulse, impulse ); + if ( b3LengthSquared( joint->angularVelocityImpulse ) > maxImpulse * maxImpulse ) + { + joint->angularVelocityImpulse = b3MulSV( maxImpulse, b3Normalize( joint->angularVelocityImpulse ) ); + } + impulse = b3Sub( joint->angularVelocityImpulse, oldImpulse ); + + wA = b3Sub( wA, b3MulMV( iA, impulse ) ); + wB = b3Add( wB, b3MulMV( iB, impulse ) ); + } + + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + // linear spring + if ( joint->maxSpringForce > 0.0f && joint->linearHertz > 0.0f ) + { + b3Vec3 dcA = stateA->deltaPosition; + b3Vec3 dcB = stateB->deltaPosition; + b3Vec3 c = b3Add( b3Add( b3Sub( dcB, dcA ), b3Sub( rB, rA ) ), joint->deltaCenter ); + + b3Vec3 bias = b3MulSV( joint->linearSpring.biasRate, c ); + float massScale = joint->linearSpring.massScale; + float impulseScale = joint->linearSpring.impulseScale; + + b3Vec3 cdot = b3Sub( b3Add( vB, b3Cross( wB, rB ) ), b3Add( vA, b3Cross( wA, rA ) ) ); + + //// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] + b3Matrix3 sA = b3Skew( rA ); + b3Matrix3 sB = b3Skew( rB ); + b3Matrix3 kA = b3MulMM( sA, b3MulMM( base->invIA, sA ) ); + b3Matrix3 kB = b3MulMM( sB, b3MulMM( base->invIB, sB ) ); + b3Matrix3 k = b3NegateMat3( b3AddMM( kA, kB ) ); + k.cx.x += mA + mB; + k.cy.y += mA + mB; + k.cz.z += mA + mB; + + b3Vec3 b = b3Solve3( k, b3Add( cdot, bias ) ); + + b3Vec3 oldImpulse = joint->linearSpringImpulse; + b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b ), impulseScale, oldImpulse ); + float maxImpulse = context->h * joint->maxSpringForce; + joint->linearSpringImpulse = b3Add( joint->linearSpringImpulse, impulse ); + + if ( b3LengthSquared( joint->linearSpringImpulse ) > maxImpulse * maxImpulse ) + { + joint->linearSpringImpulse = b3MulSV( maxImpulse, b3Normalize( joint->linearSpringImpulse ) ); + } + + impulse = b3Sub( joint->linearSpringImpulse, oldImpulse ); + + vA = b3MulSub( vA, mA, impulse ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) ); + vB = b3MulAdd( vB, mB, impulse ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) ); + } + + // linear velocity + if ( joint->maxVelocityForce > 0.0f ) + { + b3Vec3 cdot = b3Sub( b3Add( vB, b3Cross( wB, rB ) ), b3Add( vA, b3Cross( wA, rA ) ) ); + cdot = b3Sub( cdot, joint->linearVelocity ); + //// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] + b3Matrix3 sA = b3Skew( rA ); + b3Matrix3 sB = b3Skew( rB ); + b3Matrix3 kA = b3MulMM( sA, b3MulMM( base->invIA, sA ) ); + b3Matrix3 kB = b3MulMM( sB, b3MulMM( base->invIB, sB ) ); + b3Matrix3 k = b3NegateMat3( b3AddMM( kA, kB ) ); + k.cx.x += mA + mB; + k.cy.y += mA + mB; + k.cz.z += mA + mB; + + b3Vec3 b = b3Solve3( k, cdot ); + b3Vec3 impulse = b3Neg( b ); + + b3Vec3 oldImpulse = joint->linearVelocityImpulse; + float maxImpulse = context->h * joint->maxVelocityForce; + joint->linearVelocityImpulse = b3Add( joint->linearVelocityImpulse, impulse ); + + if ( b3LengthSquared( joint->linearVelocityImpulse ) > maxImpulse * maxImpulse ) + { + joint->linearVelocityImpulse = b3MulSV( maxImpulse, b3Normalize( joint->linearVelocityImpulse ) ); + } + + impulse = b3Sub( joint->linearVelocityImpulse, oldImpulse ); + + vA = b3MulSub( vA, mA, impulse ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) ); + vB = b3MulAdd( vB, mB, impulse ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) ); + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +#if 0 +void b3DumpMotorJoint() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b3Dump(" b3MotorJointDef jd;\n"); + b3Dump(" jd.bodyA = sims[%d];\n", indexA); + b3Dump(" jd.bodyB = sims[%d];\n", indexB); + b3Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b3Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y); + b3Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y); + b3Dump(" jd.referenceAngle = %.9g;\n", m_referenceAngle); + b3Dump(" jd.stiffness = %.9g;\n", m_stiffness); + b3Dump(" jd.damping = %.9g;\n", m_damping); + b3Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} +#endif diff --git a/src/mover.c b/src/mover.c new file mode 100644 index 0000000..209426e --- /dev/null +++ b/src/mover.c @@ -0,0 +1,70 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "math_internal.h" + +#include "box3d/collision.h" +#include "box3d/constants.h" + +b3PlaneSolverResult b3SolvePlanes( b3Vec3 targetDelta, b3CollisionPlane* planes, int count ) +{ + for ( int i = 0; i < count; ++i ) + { + planes[i].push = 0.0f; + } + + b3Vec3 delta = targetDelta; + float tolerance = B3_LINEAR_SLOP; + + int iteration; + for ( iteration = 0; iteration < 20; ++iteration ) + { + float totalPush = 0.0f; + for ( int planeIndex = 0; planeIndex < count; ++planeIndex ) + { + b3CollisionPlane* plane = planes + planeIndex; + + // Add slop to prevent jitter + float separation = b3PlaneSeparation( plane->plane, delta ) + B3_LINEAR_SLOP; + + float push = -separation; + + // Clamp accumulated push + float accumulatedPush = plane->push; + plane->push = b3ClampFloat( plane->push + push, 0.0f, plane->pushLimit ); + push = plane->push - accumulatedPush; + delta = b3MulAdd( delta, push, plane->plane.normal ); + + // Track maximum push for convergence + totalPush += b3AbsFloat( push ); + } + + if ( totalPush < tolerance ) + { + break; + } + } + + return (b3PlaneSolverResult){ + .delta = delta, + .iterationCount = iteration, + }; +} + +b3Vec3 b3ClipVector( b3Vec3 vector, const b3CollisionPlane* planes, int count ) +{ + b3Vec3 v = vector; + + for ( int planeIndex = 0; planeIndex < count; ++planeIndex ) + { + const b3CollisionPlane* plane = planes + planeIndex; + if ( plane->push == 0.0f || plane->clipVelocity == false ) + { + continue; + } + + v = b3MulSub( v, b3MinFloat( 0.0f, b3Dot( v, plane->plane.normal ) ), plane->plane.normal ); + } + + return v; +} diff --git a/src/parallel_for.c b/src/parallel_for.c new file mode 100644 index 0000000..07d1c3f --- /dev/null +++ b/src/parallel_for.c @@ -0,0 +1,135 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "parallel_for.h" + +#include "core.h" +#include "physics_world.h" +#include "platform.h" + +#include "box3d/base.h" +#include "box3d/constants.h" + +#include + +// Shared state for one b3ParallelFor invocation. Workers race on nextBlock to +// claim work, so a slow chunk can't strand the other threads. +typedef struct b3ParallelForShared +{ + b3AtomicInt nextBlock; + int blockCount; + int blockSize; + int itemCount; + b3ParallelForCallback* callback; + void* context; +} b3ParallelForShared; + +typedef struct b3ParallelForTask +{ + b3ParallelForShared* shared; + int workerIndex; +} b3ParallelForTask; + +static void b3ParallelForTrampoline( void* taskContext ) +{ + b3ParallelForTask* task = (b3ParallelForTask*)taskContext; + b3ParallelForShared* shared = task->shared; + int workerIndex = task->workerIndex; + void* context = shared->context; + b3ParallelForCallback* callback = shared->callback; + + int blockCount = shared->blockCount; + int blockSize = shared->blockSize; + int itemCount = shared->itemCount; + + for ( ;; ) + { + int blockIndex = b3AtomicFetchAddInt( &shared->nextBlock, 1 ); + if ( blockIndex >= blockCount ) + { + break; + } + + int start = blockIndex * blockSize; + int end = start + blockSize; + if ( end > itemCount ) + { + end = itemCount; + } + + callback( start, end, workerIndex, context ); + } +} + +void b3ParallelFor( b3World* world, b3ParallelForCallback* callback, int itemCount, int minRange, void* context, + const char* name ) +{ + if ( itemCount <= 0 ) + { + return; + } + + B3_ASSERT( minRange > 0 ); + + int workerCount = world->workerCount; + B3_ASSERT( 0 < workerCount && workerCount <= B3_MAX_WORKERS ); + + // Target multiple blocks per worker to reduce thread stalls. + // block size grows once items exceed maxBlockCount * minRange + // so the block count stays bounded and per-block sync overhead stays low. + int blocksPerWorker = 4; + int maxBlockCount = blocksPerWorker * workerCount; + + int blockSize; + int blockCount; + if ( itemCount <= minRange * maxBlockCount ) + { + blockSize = minRange; + blockCount = ( itemCount + blockSize - 1 ) / blockSize; + } + else + { + blockSize = ( itemCount + maxBlockCount - 1 ) / maxBlockCount; + blockCount = ( itemCount + blockSize - 1 ) / blockSize; + } + B3_ASSERT( blockCount >= 1 ); + B3_ASSERT( blockSize * blockCount >= itemCount ); + + // No point enqueueing more tasks than blocks. + int taskCount = workerCount < blockCount ? workerCount : blockCount; + + b3ParallelForShared shared; + shared.blockCount = blockCount; + shared.blockSize = blockSize; + shared.itemCount = itemCount; + shared.callback = callback; + shared.context = context; + b3AtomicStoreInt( &shared.nextBlock, 0 ); + + b3ParallelForTask tasks[B3_MAX_WORKERS]; + void* handles[B3_MAX_WORKERS]; + for ( int i = 0; i < taskCount; ++i ) + { + tasks[i].shared = &shared; + tasks[i].workerIndex = i; + + if (world->taskCount < B3_MAX_TASKS) + { + handles[i] = world->enqueueTaskFcn( &b3ParallelForTrampoline, tasks + i, world->userTaskContext, name ); + world->taskCount += 1; + } + else + { + handles[i] = NULL; + b3ParallelForTrampoline( tasks + i ); + } + } + + for ( int i = 0; i < taskCount; ++i ) + { + if ( handles[i] != NULL ) + { + world->finishTaskFcn( handles[i], world->userTaskContext ); + } + } +} diff --git a/src/parallel_for.h b/src/parallel_for.h new file mode 100644 index 0000000..237621a --- /dev/null +++ b/src/parallel_for.h @@ -0,0 +1,32 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +typedef struct b3World b3World; + +// Callback invoked by b2ParallelFor to process a range of items. May be called +// multiple times per worker: work is divided into blocks that workers claim +// atomically, so a worker that finishes early picks up the next unclaimed +// block instead of sitting idle. workerIndex is the worker identity and is +// stable across all invocations from the same worker, so it is safe to use as +// an index into per-worker state (e.g. world->taskContexts.data + workerIndex). +typedef void b3ParallelForCallback( int startIndex, int endIndex, int workerIndex, void* context ); + +#ifdef __cplusplus +extern "C" +{ +#endif + +// Divide [0, itemCount) into blocks and process them with cooperative claiming: +// up to world->workerCount tasks are enqueued, and each task loops, atomically +// claiming the next unclaimed block until the range is drained. Blocks the +// caller until all work is complete. minRange is the minimum block size; block +// size grows once itemCount exceeds 4 * workerCount * minRange so block count +// stays bounded. +void b3ParallelFor( b3World* world, b3ParallelForCallback* callback, int itemCount, int minRange, void* context, + const char* name ); + +#ifdef __cplusplus +} +#endif diff --git a/src/parallel_joint.c b/src/parallel_joint.c new file mode 100644 index 0000000..b2123d2 --- /dev/null +++ b/src/parallel_joint.c @@ -0,0 +1,244 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "math_internal.h" +#include "physics_world.h" +#include "solver.h" +#include "solver_set.h" + +// needed for dll export +#include "box3d/box3d.h" + +void b3ParallelJoint_SetSpringHertz( b3JointId jointId, float hertz ) +{ + B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_parallelJoint ); + base->parallelJoint.hertz = hertz; +} + +float b3ParallelJoint_GetSpringHertz( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_parallelJoint ); + return base->parallelJoint.hertz; +} + +void b3ParallelJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio ) +{ + B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_parallelJoint ); + base->parallelJoint.dampingRatio = dampingRatio; +} + +float b3ParallelJoint_GetSpringDampingRatio( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_parallelJoint ); + return base->parallelJoint.dampingRatio; +} + +void b3ParallelJoint_SetMaxTorque( b3JointId jointId, float maxForce ) +{ + B3_ASSERT( b3IsValidFloat( maxForce ) && maxForce >= 0.0f ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_parallelJoint ); + base->parallelJoint.maxTorque = maxForce; +} + +float b3ParallelJoint_GetMaxTorque( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_parallelJoint ); + return base->parallelJoint.maxTorque; +} + +b3Vec3 b3GetParallelJointTorque( b3World* world, b3JointSim* base ) +{ + b3ParallelJoint* joint = &base->parallelJoint; + + b3Quat relQ = b3InvMulQuat( joint->quatA, joint->quatB ); + joint->perpAxisX = b3MulSV( 0.5f, b3RotateVector( joint->quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) ); + joint->perpAxisY = b3MulSV( 0.5f, b3RotateVector( joint->quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) ); + + b3Vec3 angularImpulse = b3Blend2( joint->perpImpulse.x, joint->perpAxisX, joint->perpImpulse.y, joint->perpAxisY ); + b3Vec3 torque = b3MulSV( world->inv_h, angularImpulse ); + return torque; +} + +void b3PrepareParallelJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_parallelJoint ); + + b3World* world = context->world; + + b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB ); + + B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + base->invMassA = bodySimA->invMass; + base->invMassB = bodySimB->invMass; + base->invIA = bodySimA->invInertiaWorld; + base->invIB = bodySimB->invInertiaWorld; + + b3Matrix3 invInertiaSum = b3AddMM( base->invIA, base->invIB ); + base->fixedRotation = b3Det( invInertiaSum ) < 1000.0f * FLT_MIN; + + b3ParallelJoint* joint = &base->parallelJoint; + joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX; + joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX; + + // Compute joint anchor frames with world space rotation, relative to center of mass + joint->quatA = b3MulQuat( bodySimA->transform.q, base->localFrameA.q ); + joint->quatB = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + + b3Quat relQ = b3InvMulQuat( joint->quatA, joint->quatB ); + + { + // These are needed for warm starting + joint->perpAxisX = b3MulSV( 0.5f, b3RotateVector( joint->quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) ); + joint->perpAxisY = b3MulSV( 0.5f, b3RotateVector( joint->quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) ); + } + + joint->softness = b3MakeSoft( joint->hertz, joint->dampingRatio, context->h ); + + if ( context->enableWarmStarting == false ) + { + joint->perpImpulse = (b3Vec2){ 0.0f, 0.0f }; + } +} + +void b3WarmStartParallelJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_parallelJoint ); + + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3ParallelJoint* joint = &base->parallelJoint; + + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 wB = stateB->angularVelocity; + + b3Vec3 angularImpulse = b3Blend2( joint->perpImpulse.x, joint->perpAxisX, joint->perpImpulse.y, joint->perpAxisY ); + + wA = b3Sub( wA, b3MulMV( iA, angularImpulse ) ); + wB = b3Add( wB, b3MulMV( iB, angularImpulse ) ); + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->angularVelocity = wB; + } +} + +void b3SolveParallelJoint( b3JointSim* base, b3StepContext* context ) +{ + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3ParallelJoint* joint = &base->parallelJoint; + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 wB = stateB->angularVelocity; + + bool fixedRotation = base->fixedRotation; + b3Quat quatA = b3MulQuat( stateA->deltaRotation, joint->quatA ); + b3Quat quatB = b3MulQuat( stateB->deltaRotation, joint->quatB ); + + if ( b3DotQuat( quatA, quatB ) < 0.0f ) + { + // this keeps the rotation angle in the range [-pi, pi] + quatB = b3NegateQuat( quatB ); + } + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + if ( fixedRotation == false && joint->maxTorque > 0.0f ) + { + b3Vec2 c = { relQ.v.x, relQ.v.y }; + b3Vec2 bias = { joint->softness.biasRate * c.x, joint->softness.biasRate * c.y }; + float massScale = joint->softness.massScale; + float impulseScale = joint->softness.impulseScale; + + // Collinearity constraint as 2-by-2 + b3Vec3 perpAxisX = b3MulSV( 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) ); + b3Vec3 perpAxisY = b3MulSV( 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) ); + joint->perpAxisX = perpAxisX; + joint->perpAxisY = perpAxisY; + + b3Matrix3 invInertiaSum = b3AddMM( iA, iB ); + float kxx = b3Dot( perpAxisX, b3MulMV( invInertiaSum, perpAxisX ) ); + float kyy = b3Dot( perpAxisY, b3MulMV( invInertiaSum, perpAxisY ) ); + float kxy = b3Dot( perpAxisX, b3MulMV( invInertiaSum, perpAxisY ) ); + + b3Matrix2 k = { { kxx, kxy }, { kxy, kyy } }; + + b3Vec3 wRel = b3Sub( wB, wA ); + b3Vec2 cdot = { b3Dot( wRel, perpAxisX ), b3Dot( wRel, perpAxisY ) }; + + float maxImpulse = context->h * joint->maxTorque; + b3Vec2 oldImpulse = joint->perpImpulse; + b3Vec2 cdotPlusBias = { cdot.x + bias.x, cdot.y + bias.y }; + b3Vec2 sol = b3Solve2( k, cdotPlusBias ); + b3Vec2 deltaImpulse = { + -massScale * sol.x - impulseScale * oldImpulse.x, + -massScale * sol.y - impulseScale * oldImpulse.y, + }; + joint->perpImpulse = (b3Vec2){ oldImpulse.x + deltaImpulse.x, oldImpulse.y + deltaImpulse.y }; + if ( b3LengthSquared2( joint->perpImpulse ) > maxImpulse * maxImpulse ) + { + float s = maxImpulse / b3Length2( joint->perpImpulse ); + joint->perpImpulse = (b3Vec2){ s * joint->perpImpulse.x, s * joint->perpImpulse.y }; + } + + deltaImpulse = b3Sub2( joint->perpImpulse, oldImpulse ); + + b3Vec3 angularImpulse = b3Blend2( deltaImpulse.x, perpAxisX, deltaImpulse.y, perpAxisY ); + wA = b3Sub( wA, b3MulMV( iA, angularImpulse ) ); + wB = b3Add( wB, b3MulMV( iB, angularImpulse ) ); + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->angularVelocity = wB; + } +} + +void b3DrawParallelJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale ) +{ + float length = 0.1f * scale; + + b3Transform frameA = b3MulTransforms( transformA, base->localFrameA ); + draw->DrawSegmentFcn( frameA.p, b3MulAdd( frameA.p, length, b3RotateVector( frameA.q, b3Vec3_axisZ ) ), b3_colorGreen, draw->context ); + + b3Transform frameB = b3MulTransforms( transformB, base->localFrameB ); + draw->DrawSegmentFcn( frameB.p, b3MulAdd( frameB.p, length, b3RotateVector( frameB.q, b3Vec3_axisZ ) ), b3_colorBlue, draw->context ); +} diff --git a/src/physics_world.c b/src/physics_world.c new file mode 100644 index 0000000..ebe53c5 --- /dev/null +++ b/src/physics_world.c @@ -0,0 +1,3627 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "physics_world.h" + +#include "arena_allocator.h" +#include "bitset.h" +#include "body.h" +#include "broad_phase.h" +#include "compound.h" +#include "constraint_graph.h" +#include "contact.h" +#include "core.h" +#include "ctz.h" +#include "island.h" +#include "joint.h" +#include "parallel_for.h" +#include "platform.h" +#include "scheduler.h" +#include "sensor.h" +#include "shape.h" +#include "solver.h" +#include "solver_set.h" + +#include "box3d/box3d.h" +#include "box3d/constants.h" + +#include +#include +#include + +_Static_assert( B3_MAX_WORLDS > 0, "must be 1 or more" ); +_Static_assert( B3_MAX_WORLDS < UINT16_MAX, "B3_MAX_WORLDS limit exceeded" ); +b3World b3_worlds[B3_MAX_WORLDS]; +b3AtomicInt b3_worldCount; +int b3_maxWorldCount; + +b3World* b3GetUnlockedWorldFromId( b3WorldId id ) +{ + B3_ASSERT( 1 <= id.index1 && id.index1 <= B3_MAX_WORLDS ); + b3World* world = b3_worlds + ( id.index1 - 1 ); + B3_ASSERT( id.index1 == world->worldId + 1 ); + B3_ASSERT( id.generation == world->generation ); + + // A world accessed from an id should not be locked + if ( world->locked ) + { + B3_ASSERT( false ); + return NULL; + } + return world; +} + +b3World* b3GetWorldFromId( b3WorldId id ) +{ + B3_ASSERT( 1 <= id.index1 && id.index1 <= B3_MAX_WORLDS ); + b3World* world = b3_worlds + ( id.index1 - 1 ); + B3_ASSERT( id.index1 == world->worldId + 1 ); + B3_ASSERT( id.generation == world->generation ); + return world; +} + +b3World* b3GetWorld( int index ) +{ + B3_ASSERT( 0 <= index && index < B3_MAX_WORLDS ); + b3World* world = b3_worlds + index; + B3_ASSERT( world->worldId == index ); + return world; +} + +b3World* b3GetUnlockedWorld( int index ) +{ + B3_ASSERT( 0 <= index && index < B3_MAX_WORLDS ); + b3World* world = b3_worlds + index; + B3_ASSERT( world->worldId == index ); + if ( world->locked ) + { + B3_ASSERT( false ); + return NULL; + } + + return world; +} + +static void* b3DefaultAddTaskFcn( b3TaskCallback* task, void* taskContext, void* userContext, const char* name ) +{ + B3_UNUSED( userContext, name ); + task( taskContext ); + return NULL; +} + +static void b3DefaultFinishTaskFcn( void* userTask, void* userContext ) +{ + B3_UNUSED( userTask ); + B3_UNUSED( userContext ); +} + +static float b3DefaultFrictionCallback( float frictionA, uint64_t materialA, float frictionB, uint64_t materialB ) +{ + B3_UNUSED( materialA, materialB ); + return sqrtf( frictionA * frictionB ); +} + +static float b3DefaultRestitutionCallback( float restitutionA, uint64_t materialA, float restitutionB, uint64_t materialB ) +{ + B3_UNUSED( materialA, materialB ); + return b3MaxFloat( restitutionA, restitutionB ); +} + +static void b3CreateWorkerContexts( b3World* world ) +{ + b3Array_Resize( world->taskContexts, world->workerCount ); + b3Array_MemZero( world->taskContexts ); + + b3Array_Resize( world->sensorTaskContexts, world->workerCount ); + b3Array_MemZero( world->sensorTaskContexts ); + + for ( int i = 0; i < world->workerCount; ++i ) + { + world->taskContexts.data[i].arena = b3CreateArena( 32 * 1024 ); + b3Array_Reserve( world->taskContexts.data[i].sensorHits, 8 ); + world->taskContexts.data[i].contactStateBitSet = b3CreateBitSet( 1024 ); + world->taskContexts.data[i].hitEventBitSet = b3CreateBitSet( 1024 ); + world->taskContexts.data[i].hasHitEvents = false; + world->taskContexts.data[i].jointStateBitSet = b3CreateBitSet( 1024 ); + world->taskContexts.data[i].enlargedSimBitSet = b3CreateBitSet( 256 ); + world->taskContexts.data[i].awakeIslandBitSet = b3CreateBitSet( 256 ); + world->taskContexts.data[i].splitIslandId = B3_NULL_INDEX; + + world->sensorTaskContexts.data[i].eventBits = b3CreateBitSet( 128 ); + } +} + +static void b3DestroyWorkerContexts( b3World* world ) +{ + for ( int i = 0; i < world->workerCount; ++i ) + { + b3DestroyArena( &world->taskContexts.data[i].arena ); + b3Array_Destroy( world->taskContexts.data[i].sensorHits ); + b3DestroyBitSet( &world->taskContexts.data[i].contactStateBitSet ); + b3DestroyBitSet( &world->taskContexts.data[i].hitEventBitSet ); + b3DestroyBitSet( &world->taskContexts.data[i].jointStateBitSet ); + b3DestroyBitSet( &world->taskContexts.data[i].enlargedSimBitSet ); + b3DestroyBitSet( &world->taskContexts.data[i].awakeIslandBitSet ); + + b3DestroyBitSet( &world->sensorTaskContexts.data[i].eventBits ); + } + + b3Array_Destroy( world->taskContexts ); + b3Array_Destroy( world->sensorTaskContexts ); +} + +b3WorldId b3CreateWorld( const b3WorldDef* def ) +{ + B3_CHECK_DEF( def ); + + int worldId = B3_NULL_INDEX; + for ( int i = 0; i < B3_MAX_WORLDS; ++i ) + { + if ( b3_worlds[i].inUse == false ) + { + worldId = i; + break; + } + } + + if ( worldId == B3_NULL_INDEX ) + { + b3Log( "B3_MAX_WORLDS of %d exceeded!!!", B3_MAX_WORLDS ); + B3_ASSERT( worldId != B3_NULL_INDEX ); + return (b3WorldId){ 0 }; + } + + // b3Log( "b3_lengthUnitsPerMeter = %g", b3_lengthUnitsPerMeter ); + + int oldCount = b3AtomicFetchAddInt( &b3_worldCount, 1 ); + b3_maxWorldCount = b3MaxInt( b3_maxWorldCount, oldCount + 1 ); + + // b3Log( "Created world %d", worldId ); + + b3InitializeContactRegisters(); + + b3World* world = b3_worlds + worldId; + uint16_t revision = world->generation; + + memset( world, 0, sizeof( b3World ) ); + + world->worldId = (uint16_t)worldId; + world->generation = revision; + world->inUse = true; + + world->stack = b3CreateStack( 2048 ); + + b3Array_Reserve( world->manifoldAllocators, 16 ); + world->manifoldAllocatorMutex = b3CreateMutex(); + + b3CreateBroadPhase( &world->broadPhase, &def->capacity ); + b3CreateGraph( &world->constraintGraph, 16 ); + + // pools + world->bodyIdPool = b3CreateIdPool(); + + int bodyCapacity = b3MaxInt( 16, def->capacity.staticBodyCount + def->capacity.dynamicBodyCount ); + b3Array_Reserve( world->bodies, bodyCapacity ); + b3Array_Reserve( world->solverSets, 8 ); + + // add empty static, active, and disabled body sets + world->solverSetIdPool = b3CreateIdPool(); + b3SolverSet set = { 0 }; + + // static set + set.setIndex = b3AllocId( &world->solverSetIdPool ); + b3Array_Push( world->solverSets, set ); + b3Array_Reserve( world->solverSets.data[b3_staticSet].bodySims, b3MaxInt( 16, def->capacity.staticBodyCount ) ); + B3_ASSERT( world->solverSets.data[b3_staticSet].setIndex == b3_staticSet ); + + // disabled set + set.setIndex = b3AllocId( &world->solverSetIdPool ); + b3Array_Push( world->solverSets, set ); + B3_ASSERT( world->solverSets.data[b3_disabledSet].setIndex == b3_disabledSet ); + + // awake set + set.setIndex = b3AllocId( &world->solverSetIdPool ); + b3Array_Push( world->solverSets, set ); + b3Array_Reserve( world->solverSets.data[b3_awakeSet].bodySims, b3MaxInt( 16, def->capacity.dynamicBodyCount ) ); + b3Array_Reserve( world->solverSets.data[b3_awakeSet].bodyStates, b3MaxInt( 16, def->capacity.dynamicBodyCount ) ); + b3Array_Reserve( world->solverSets.data[b3_awakeSet].contactIndices, b3MaxInt( 16, def->capacity.contactCount ) ); + B3_ASSERT( world->solverSets.data[b3_awakeSet].setIndex == b3_awakeSet ); + + world->shapeIdPool = b3CreateIdPool(); + + int shapeCapacity = b3MaxInt( 16, def->capacity.staticShapeCount + def->capacity.dynamicShapeCount ); + b3Array_Reserve( world->shapes, shapeCapacity ); + + world->contactIdPool = b3CreateIdPool(); + b3Array_Reserve( world->contacts, b3MaxInt( 16, def->capacity.contactCount ) ); + + world->jointIdPool = b3CreateIdPool(); + b3Array_Reserve( world->joints, 16 ); + + world->islandIdPool = b3CreateIdPool(); + b3Array_Reserve( world->islands, b3MaxInt( 16, def->capacity.dynamicBodyCount ) ); + + b3Array_Reserve( world->sensors, 4 ); + + b3Array_Reserve( world->bodyMoveEvents, 4 ); + b3Array_Reserve( world->sensorBeginEvents, 4 ); + b3Array_Reserve( world->sensorEndEvents[0], 4 ); + b3Array_Reserve( world->sensorEndEvents[1], 4 ); + b3Array_Reserve( world->contactBeginEvents, 4 ); + b3Array_Reserve( world->contactEndEvents[0], 4 ); + b3Array_Reserve( world->contactEndEvents[1], 4 ); + b3Array_Reserve( world->contactHitEvents, 4 ); + b3Array_Reserve( world->jointEvents, 4 ); + world->endEventArrayIndex = 0; + + world->stepIndex = 0; + world->splitIslandId = B3_NULL_INDEX; + world->activeTaskCount = 0; + world->taskCount = 0; + world->gravity = def->gravity; + world->hitEventThreshold = def->hitEventThreshold; + world->restitutionThreshold = def->restitutionThreshold; + world->maxLinearSpeed = def->maximumLinearSpeed; + world->contactSpeed = def->contactSpeed; + world->contactHertz = def->contactHertz; + world->contactDampingRatio = def->contactDampingRatio; + world->contactRecycleDistance = B3_CONTACT_RECYCLE_DISTANCE; + + if ( def->frictionCallback == NULL ) + { + world->frictionCallback = b3DefaultFrictionCallback; + } + else + { + world->frictionCallback = def->frictionCallback; + } + + if ( def->restitutionCallback == NULL ) + { + world->restitutionCallback = b3DefaultRestitutionCallback; + } + else + { + world->restitutionCallback = def->restitutionCallback; + } + + world->enableSleep = def->enableSleep; + world->locked = false; + world->enableWarmStarting = true; + world->enableContinuous = def->enableContinuous; + world->enableSpeculative = true; + world->userTreeTask = NULL; + world->userData = def->userData; + + if ( def->workerCount > 0 && def->enqueueTask != NULL && def->finishTask != NULL ) + { + // External task system + world->workerCount = b3MinInt( def->workerCount, B3_MAX_WORKERS ); + world->enqueueTaskFcn = def->enqueueTask; + world->finishTaskFcn = def->finishTask; + world->userTaskContext = def->userTaskContext; + world->scheduler = NULL; + } + else if ( def->workerCount > 1 ) + { + // Built-in scheduler + world->workerCount = b3MinInt( def->workerCount, B3_MAX_WORKERS ); + world->scheduler = b3CreateScheduler( world->workerCount ); + world->enqueueTaskFcn = b3SchedulerEnqueueTask; + world->finishTaskFcn = b3SchedulerFinishTask; + world->userTaskContext = world->scheduler; + } + else + { + // Serial fallback + world->workerCount = 1; + world->enqueueTaskFcn = b3DefaultAddTaskFcn; + world->finishTaskFcn = b3DefaultFinishTaskFcn; + world->userTaskContext = NULL; + world->scheduler = NULL; + } + + b3CreateWorkerContexts( world ); + + world->debugBodySet = b3CreateBitSet( 256 ); + world->debugJointSet = b3CreateBitSet( 256 ); + world->debugContactSet = b3CreateBitSet( 256 ); + world->debugIslandSet = b3CreateBitSet( 256 ); + world->createDebugShape = def->createDebugShape; + world->destroyDebugShape = def->destroyDebugShape; + world->userDebugShapeContext = def->userDebugShapeContext; + + // add one to worldId so that 0 represents a null b3WorldId + return (b3WorldId){ (uint16_t)( worldId + 1 ), world->generation }; +} + +void b3DestroyWorld( b3WorldId worldId ) +{ + b3AtomicFetchAddInt( &b3_worldCount, -1 ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + world->locked = true; + + if ( world->scheduler != NULL ) + { + b3DestroyScheduler( world->scheduler ); + world->scheduler = NULL; + } + + b3DestroyBitSet( &world->debugBodySet ); + b3DestroyBitSet( &world->debugJointSet ); + b3DestroyBitSet( &world->debugContactSet ); + b3DestroyBitSet( &world->debugIslandSet ); + + b3DestroyWorkerContexts( world ); + + b3Array_Destroy( world->bodyMoveEvents ); + b3Array_Destroy( world->sensorBeginEvents ); + b3Array_Destroy( world->sensorEndEvents[0] ); + b3Array_Destroy( world->sensorEndEvents[1] ); + b3Array_Destroy( world->contactBeginEvents ); + b3Array_Destroy( world->contactEndEvents[0] ); + b3Array_Destroy( world->contactEndEvents[1] ); + b3Array_Destroy( world->contactHitEvents ); + b3Array_Destroy( world->jointEvents ); + + int sensorCount = world->sensors.count; + for ( int i = 0; i < sensorCount; ++i ) + { + b3Array_Destroy( world->sensors.data[i].hits ); + b3Array_Destroy( world->sensors.data[i].overlaps1 ); + b3Array_Destroy( world->sensors.data[i].overlaps2 ); + } + + b3Array_Destroy( world->sensors ); + b3Array_Destroy( world->bodies ); + + int shapeCapacity = world->shapes.count; + b3Shape* shapes = world->shapes.data; + for ( int i = 0; i < shapeCapacity; ++i ) + { + b3Shape* shape = shapes + i; + if ( shape->id != B3_NULL_INDEX ) + { + b3DestroyShapeAllocations( world, shapes + i ); + } + } + + int contactCapacity = world->contacts.count; + b3Contact* contacts = world->contacts.data; + for ( int i = 0; i < contactCapacity; ++i ) + { + b3Contact* contact = contacts + i; + if ( contact->contactId != B3_NULL_INDEX ) + { + if ( contact->flags & b3_simMeshContact ) + { + b3Array_Destroy( contact->meshContact.triangleCache ); + } + } + } + + b3Array_Destroy( world->shapes ); + b3Array_Destroy( world->contacts ); + b3Array_Destroy( world->joints ); + + for ( int i = 0; i < world->islands.count; ++i ) + { + b3Array_Destroy( world->islands.data[i].bodies ); + b3Array_Destroy( world->islands.data[i].contacts ); + b3Array_Destroy( world->islands.data[i].joints ); + } + b3Array_Destroy( world->islands ); + + // Destroy solver sets + int setCapacity = world->solverSets.count; + for ( int i = 0; i < setCapacity; ++i ) + { + b3SolverSet* set = world->solverSets.data + i; + if ( set->setIndex != B3_NULL_INDEX ) + { + b3DestroySolverSet( world, i ); + } + } + + b3Array_Destroy( world->solverSets ); + + b3DestroyGraph( &world->constraintGraph ); + b3DestroyBroadPhase( &world->broadPhase ); + + b3DestroyIdPool( &world->bodyIdPool ); + b3DestroyIdPool( &world->shapeIdPool ); + b3DestroyIdPool( &world->contactIdPool ); + b3DestroyIdPool( &world->jointIdPool ); + b3DestroyIdPool( &world->islandIdPool ); + b3DestroyIdPool( &world->solverSetIdPool ); + + for ( int i = 0; i < world->manifoldAllocators.count; ++i ) + { + b3DestroyBlockAllocator( world->manifoldAllocators.data + i ); + } + b3Array_Destroy( world->manifoldAllocators ); + b3DestroyMutex( world->manifoldAllocatorMutex ); + + b3DestroyStack( &world->stack ); + + // Wipe world but preserve generation + uint16_t generation = world->generation; + memset( world, 0, sizeof( b3World ) ); + world->generation = generation + 1; + + // b3Log( "Destroyed world %d", worldId.index1 - 1 ); +} + +int b3GetWorldCount( void ) +{ + return b3AtomicLoadInt( &b3_worldCount ); +} + +int b3GetMaxWorldCount( void ) +{ + return b3_maxWorldCount; +} + +// Issues T0 prefetches across the cache lines of a b3Contact (216 B / 4 lines). +// Used to hide the random-access latency of contact lookups while we work on an +// earlier index. +static inline void b3PrefetchContact( const b3Contact* contact ) +{ + const char* p = (const char*)contact; + b3Prefetch( p ); + b3Prefetch( p + 64 ); + b3Prefetch( p + 128 ); + b3Prefetch( p + 192 ); +} + +static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* context ) +{ + b3TracyCZoneNC( collide_task, "Collide Task", b3_colorDodgerBlue, true ); + + b3StepContext* stepContext = (b3StepContext*)context; + b3World* world = stepContext->world; + b3ConstraintGraph* graph = &world->constraintGraph; + b3TaskContext* taskContext = world->taskContexts.data + workerIndex; + int* contactIndices = stepContext->awakeContactIndices; + b3Contact* contacts = world->contacts.data; + b3Shape* shapes = world->shapes.data; + b3Body* bodies = world->bodies.data; + b3BodySim* awakeSims = world->solverSets.data[b3_awakeSet].bodySims.data; + b3BodySim* staticSims = world->solverSets.data[b3_staticSet].bodySims.data; + + B3_ASSERT( startIndex < endIndex ); + + float recycleDistance = world->contactRecycleDistance; + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + float recycleDistanceNonTouching = b3MinFloat( recycleDistance, speculativeDistance ); + + // Prefetch contact[i + contactPrefetchDistance] each iteration so the random + // 216 B contact load lands in L1 by the time we reach it. Distance picked to + // cover ~200 cycles of memory latency without overshooting the L1 working set. + const int contactPrefetchDistance = 4; + int prefetchEnd = endIndex - contactPrefetchDistance; + + for ( int i = startIndex; i < endIndex; ++i ) + { + if ( i < prefetchEnd ) + { + b3PrefetchContact( contacts + contactIndices[i + contactPrefetchDistance] ); + } + + int contactIndex = contactIndices[i]; + B3_ASSERT( contactIndex < world->contacts.count ); + + b3Contact* contact = contacts + contactIndex; + B3_VALIDATE( contact->contactId == contactIndex ); + + b3Shape* shapeA = shapes + contact->shapeIdA; + b3Shape* shapeB = shapes + contact->shapeIdB; + + // Do proxies still overlap? + bool overlap = b3AABB_Overlaps( shapeA->fatAABB, shapeB->fatAABB ); + if ( overlap == false ) + { + // This contact will be destroyed + contact->flags |= b3_simDisjoint; + contact->flags &= ~b3_simTouchingFlag; + b3SetBit( &taskContext->contactStateBitSet, contactIndex ); + continue; + } + + // Update contact respecting shape/body order (A,B). Bodies behind awake-set + // contacts are always either awake or static - inline b3GetBodySim with that + // invariant to skip the cross-TU call and per-call solverSets indirection. + b3Body* bodyA = bodies + shapeA->bodyId; + b3Body* bodyB = bodies + shapeB->bodyId; + bool isStaticA = bodyA->type == b3_staticBody; + bool isStaticB = bodyB->type == b3_staticBody; + bool wasTouching = ( contact->flags & b3_simTouchingFlag ); + bool isMeshContact = ( contact->flags & b3_simMeshContact ); + b3BodySim* bodySimA; + b3BodySim* bodySimB; + if ( wasTouching ) + { + B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyA->setIndex == b3_staticSet ); + B3_ASSERT( bodyB->setIndex == b3_awakeSet || bodyB->setIndex == b3_staticSet ); + bodySimA = ( isStaticA ? staticSims : awakeSims ) + bodyA->localIndex; + bodySimB = ( isStaticB ? staticSims : awakeSims ) + bodyB->localIndex; + } + else + { + // There can be non-touching contacts between awake bodies and sleeping bodies. + { + b3SolverSet* set = b3Array_Get( world->solverSets, bodyA->setIndex ); + bodySimA = b3Array_Get( set->bodySims, bodyA->localIndex ); + } + { + b3SolverSet* set = b3Array_Get( world->solverSets, bodyB->setIndex ); + bodySimB = b3Array_Get( set->bodySims, bodyB->localIndex ); + } + } + + b3Transform transformA = bodySimA->transform; + b3Transform transformB = bodySimB->transform; + + bool isFast = ( bodySimA->flags & b3_isFast ) || ( bodySimB->flags & b3_isFast ); + + // These are used by the contact solver. If the contact is between an awake body + // and a sleeping body and the contact begins to touch, the these will be invalid + // but fixed when linked in the constraint graph. + contact->bodySimIndexA = isStaticA ? B3_NULL_INDEX : bodyA->localIndex; + contact->bodySimIndexB = isStaticB ? B3_NULL_INDEX : bodyB->localIndex; + float recycleTolerance = wasTouching ? recycleDistance : recycleDistanceNonTouching; + + // Contact recycling optimization. Please cite this library if you use this optimization. + // This is inspired by persistent contact manifolds used in some physics engines, such as PhysX. + // However, this allows larger relative motion and has fewer tuning parameters (just one). + if ( ( isFast == false || isMeshContact == false ) && recycleDistance > 0.0f && + contact->flags & b3_relativeTransformValid ) + { + b3Transform xf = b3InvMulTransforms( transformA, transformB ); + b3Transform xfc = b3InvMulTransforms( contact->cachedTransformA, contact->cachedTransformB ); + b3Vec3 maxExtentA = isStaticA ? b3Vec3_zero : bodySimA->maxExtent; + b3Vec3 maxExtentB = isStaticB ? b3Vec3_zero : bodySimB->maxExtent; + b3Vec3 maxExtent = b3Max( maxExtentA, maxExtentB ); + + // Variation of Conservative Advancement + // distance + 2 * length(modified_cross(|qr.v|, maxExtent)) < recycleTolerance. + // 2*|qr.v| == 2*|sin(theta/2)| ~= theta for small angles. + float distSquared = b3DistanceSquared( xf.p, xfc.p ); + + if ( distSquared < recycleTolerance * recycleTolerance ) + { + float distance = sqrtf( distSquared ); + float slack = recycleTolerance - distance; + + // qr = inv( inv(qA0) * qB0 ) * inv(qA) * qB + // = inv(qB0) * qA0 * inv(qA) * qB + // Suppose A is static + // qr = inv(qB0) * qA0 * inv(qA0) * qB + // = inv(qB0) * qB + // qB = qB0 * qr + // Therefore qr is associated with the local angular velocity of body B when A is static. + b3Quat qr = b3InvMulQuat( xfc.q, xf.q ); + b3Vec3 arc = b3ModifiedCross( b3Abs( qr.v ), maxExtent ); + + float arcSq = 4.0f * b3LengthSquared( arc ); + if ( arcSq < slack * slack ) + { + b3Quat dqA = b3MulQuat( transformA.q, b3Conjugate( contact->cachedTransformA.q ) ); + b3Quat dqB = b3MulQuat( transformB.q, b3Conjugate( contact->cachedTransformB.q ) ); + b3Matrix3 matrixA = b3MakeMatrixFromQuat( dqA ); + b3Matrix3 matrixB = b3MakeMatrixFromQuat( dqB ); + + // Minimize round-off + b3Vec3 dc = b3Sub( bodySimB->center, bodySimA->center ); + + int manifoldCount = contact->manifoldCount; + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) + { + b3Manifold* manifold = contact->manifolds + manifoldIndex; + b3Vec3 normal = manifold->normal; + + int pointCount = manifold->pointCount; + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + // Keep anchors but update separation, same as sub-stepping. This eliminates jitter. + b3ManifoldPoint* mp = manifold->points + pointIndex; + b3Vec3 rA = b3MulMV( matrixA, mp->anchorA ); + b3Vec3 rB = b3MulMV( matrixB, mp->anchorB ); + b3Vec3 dp = b3Add( dc, b3Sub( rB, rA ) ); + mp->separation = mp->baseSeparation + b3Dot( dp, normal ); + mp->persisted = true; + } + } + + // Diagnostics + taskContext->recycledContactCount += 1; + int bucketIndex = b3MinInt( manifoldCount, B3_CONTACT_MANIFOLD_COUNT_BUCKETS - 1 ); + if ( bucketIndex > 0 ) + { + taskContext->manifoldCounts[bucketIndex - 1] += 1; + } + + // Contact is recycled. This also skips updating other aspects of the contact + // such as material parameters. + continue; + } + } + } + + // Caching for contact recycling. + contact->cachedTransformA = transformA; + contact->cachedTransformB = transformB; + contact->flags |= b3_relativeTransformValid; + + // This updates solid contacts + bool touching = b3UpdateContact( world, workerIndex, contact, shapeA, bodySimA->localCenter, transformA, shapeB, + bodySimB->localCenter, transformB, isFast, taskContext->arena ); + + int bucketIndex = b3MinInt( contact->manifoldCount, B3_CONTACT_MANIFOLD_COUNT_BUCKETS - 1 ); + if ( bucketIndex > 0 ) + { + taskContext->manifoldCounts[bucketIndex - 1] += 1; + } + + // Update the mesh contact spec + if ( touching == true && wasTouching == true && ( contact->flags & b3_simMeshContact ) ) + { + B3_ASSERT( contact->colorIndex != B3_NULL_INDEX ); + B3_ASSERT( 0 <= contact->colorIndex && contact->colorIndex < B3_GRAPH_COLOR_COUNT ); + b3GraphColor* color = graph->colors + contact->colorIndex; + b3ContactSpec* spec = b3Array_Get( color->contacts, contact->localIndex ); + spec->manifoldCount = (uint16_t)contact->manifoldCount; + } + + // State changes that affect island connectivity. Also affects contact events. + if ( touching == true && wasTouching == false ) + { + contact->flags |= b3_simStartedTouching; + b3SetBit( &taskContext->contactStateBitSet, contactIndex ); + } + else if ( touching == false && wasTouching == true ) + { + contact->flags |= b3_simStoppedTouching; + b3SetBit( &taskContext->contactStateBitSet, contactIndex ); + } + + for ( int manifoldIndex = 0; manifoldIndex < contact->manifoldCount; ++manifoldIndex ) + { + b3Manifold* manifold = contact->manifolds + manifoldIndex; + for ( int pointIndex = 0; pointIndex < manifold->pointCount; ++pointIndex ) + { + // Cache separation + b3ManifoldPoint* mp = manifold->points + pointIndex; + mp->baseSeparation = mp->separation; + } + } + } + + b3TracyCZoneEnd( collide_task ); +} + +static void b3AddNonTouchingContact( b3World* world, b3Contact* contact ) +{ + B3_ASSERT( contact->setIndex == b3_awakeSet ); + b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + contact->colorIndex = B3_NULL_INDEX; + contact->localIndex = set->contactIndices.count; + contact->bodySimIndexA = B3_NULL_INDEX; + contact->bodySimIndexB = B3_NULL_INDEX; + b3Array_Push( set->contactIndices, contact->contactId ); +} + +static void b3RemoveNonTouchingContact( b3World* world, int setIndex, int localIndex ) +{ + b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + int movedIndex = b3Array_RemoveSwap( set->contactIndices, localIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + int movedContactIndex = set->contactIndices.data[localIndex]; + b3Contact* movedContact = b3Array_Get( world->contacts, movedContactIndex ); + B3_ASSERT( movedContact->setIndex == setIndex ); + B3_ASSERT( movedContact->colorIndex == B3_NULL_INDEX ); + B3_ASSERT( movedContact->localIndex == movedIndex ); + movedContact->localIndex = localIndex; + } +} + +// Narrow-phase collision +static void b3Collide( b3StepContext* context ) +{ + b3World* world = context->world; + + B3_ASSERT( world->workerCount > 0 ); + + b3TracyCZoneNC( collide, "Collide", b3_colorDarkOrchid, true ); + + // Gather contacts from all the graph colors into a single array for easier parallel-for + int touchingCount = 0; + + b3GraphColor* graphColors = world->constraintGraph.colors; + for ( int i = 0; i < B3_GRAPH_COLOR_COUNT; ++i ) + { + touchingCount += graphColors[i].convexContacts.count + graphColors[i].contacts.count; + } + + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + int nonTouchingCount = awakeSet->contactIndices.count; + + int contactCount = touchingCount + nonTouchingCount; + + if ( contactCount == 0 ) + { + b3TracyCZoneEnd( collide ); + return; + } + + int* contactIndices = (int*)b3StackAlloc( &world->stack, contactCount * sizeof( int ), "contact indices" ); + + int contactIndex = 0; + for ( int i = 0; i < B3_GRAPH_COLOR_COUNT; ++i ) + { + b3GraphColor* color = graphColors + i; + int count = color->convexContacts.count; + for ( int j = 0; j < count; ++j ) + { + contactIndices[contactIndex] = color->convexContacts.data[j]; + contactIndex += 1; + } + + count = color->contacts.count; + for ( int j = 0; j < count; ++j ) + { + contactIndices[contactIndex] = color->contacts.data[j].contactId; + contactIndex += 1; + } + } + + B3_ASSERT( contactIndex == touchingCount ); + + if ( nonTouchingCount > 0 ) + { + int* nonTouchingIndices = awakeSet->contactIndices.data; + memcpy( contactIndices + touchingCount, nonTouchingIndices, nonTouchingCount * sizeof( int ) ); + } + + context->awakeContactIndices = contactIndices; + + // Contact bit set on ids because contact pointers are unstable as they move between touching and not touching. + int contactIdCapacity = b3GetIdCapacity( &world->contactIdPool ); + for ( int i = 0; i < world->workerCount; ++i ) + { + b3TaskContext* taskContext = world->taskContexts.data + i; + b3SetBitCountAndClear( &taskContext->contactStateBitSet, contactIdCapacity ); + taskContext->satCallCount = 0; + taskContext->satCacheHitCount = 0; + taskContext->recycledContactCount = 0; + memset( taskContext->manifoldCounts, 0, sizeof( taskContext->manifoldCounts ) ); + } + + // Task should take at least 40us on a 4GHz CPU (10K cycles) + int minRange = 20; + b3ParallelFor( world, b3CollideTask, contactCount, minRange, context, "collide" ); + + b3StackFree( &world->stack, contactIndices ); + context->awakeContactIndices = NULL; + contactIndices = NULL; + + // Serially update contact state + // todo bring this zone together with island merge + b3TracyCZoneNC( contact_state, "Contact State", b3_colorLightSlateGray, true ); + + int satMultiplier = context->dt > 0.0f ? 1 : 0; + + // Bitwise OR all contact bits + b3BitSet* bitSet = &world->taskContexts.data[0].contactStateBitSet; + world->satCallCount = satMultiplier * world->taskContexts.data[0].satCallCount; + world->satCacheHitCount = satMultiplier * world->taskContexts.data[0].satCacheHitCount; + memcpy( world->manifoldCounts, world->taskContexts.data[0].manifoldCounts, + B3_CONTACT_MANIFOLD_COUNT_BUCKETS * sizeof( int ) ); + + for ( int i = 1; i < world->workerCount; ++i ) + { + b3InPlaceUnion( bitSet, &world->taskContexts.data[i].contactStateBitSet ); + world->satCallCount += world->taskContexts.data[i].satCallCount; + world->satCacheHitCount += world->taskContexts.data[i].satCacheHitCount; + for ( int j = 0; j < B3_CONTACT_MANIFOLD_COUNT_BUCKETS; ++j ) + { + world->manifoldCounts[j] += world->taskContexts.data[i].manifoldCounts[j]; + } + } + + int endEventArrayIndex = world->endEventArrayIndex; + + const b3Shape* shapes = world->shapes.data; + uint16_t worldId = world->worldId; + + // Process contact state changes. Iterate over set bits + for ( uint32_t k = 0; k < bitSet->blockCount; ++k ) + { + uint64_t bits = bitSet->bits[k]; + while ( bits != 0 ) + { + uint32_t ctz = b3CTZ64( bits ); + int contactId = (int)( 64 * k + ctz ); + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + B3_ASSERT( contact->setIndex == b3_awakeSet ); + + const b3Shape* shapeA = shapes + contact->shapeIdA; + const b3Shape* shapeB = shapes + contact->shapeIdB; + b3ShapeId shapeIdA = { shapeA->id + 1, worldId, shapeA->generation }; + b3ShapeId shapeIdB = { shapeB->id + 1, worldId, shapeB->generation }; + b3ContactId contactFullId = { + .index1 = contactId + 1, + .world0 = worldId, + .padding = 0, + .generation = contact->generation, + }; + uint32_t flags = contact->flags; + + if ( flags & b3_simDisjoint ) + { + // Bounding boxes no longer overlap + b3DestroyContact( world, contact, false ); + contact = NULL; + } + else if ( flags & b3_simStartedTouching ) + { + B3_ASSERT( contact->islandId == B3_NULL_INDEX ); + + if ( flags & b3_contactEnableContactEvents ) + { + b3ContactBeginTouchEvent event = { shapeIdA, shapeIdB, contactFullId }; + b3Array_Push( world->contactBeginEvents, event ); + } + + B3_ASSERT( contact->manifoldCount > 0 ); + B3_ASSERT( contact->setIndex == b3_awakeSet ); + + // Link first because this wakes colliding bodies and ensures the body sims + // are in the correct place. + contact->flags &= ~b3_simStartedTouching; + contact->flags |= b3_contactTouchingFlag; + b3LinkContact( world, contact ); + + // Make sure these didn't change + B3_ASSERT( contact->colorIndex == B3_NULL_INDEX ); + + // Contact sim pointer may have become orphaned due to awake set growth, + // so I just need to refresh it. + + int oldLocalIndex = contact->localIndex; + + b3AddContactToGraph( world, contact ); + b3RemoveNonTouchingContact( world, b3_awakeSet, oldLocalIndex ); + } + else if ( flags & b3_simStoppedTouching ) + { + contact->flags &= ~b3_simStoppedTouching; + contact->flags &= ~b3_contactTouchingFlag; + + if ( contact->flags & b3_contactEnableContactEvents ) + { + b3ContactEndTouchEvent event = { shapeIdA, shapeIdB, contactFullId }; + b3Array_Push( world->contactEndEvents[endEventArrayIndex], event ); + } + + B3_ASSERT( contact->manifoldCount == 0 ); + + // Cache these here for the remove below + int colorIndex = contact->colorIndex; + int localIndex = contact->localIndex; + + b3UnlinkContact( world, contact ); + int bodyIdA = contact->edges[0].bodyId; + int bodyIdB = contact->edges[1].bodyId; + + b3AddNonTouchingContact( world, contact ); + + bool isMeshContact = contact->flags & b3_simMeshContact; + b3RemoveContactFromGraph( world, bodyIdA, bodyIdB, colorIndex, localIndex, isMeshContact ); + contact = NULL; + } + + // Clear the smallest set bit + bits = bits & ( bits - 1 ); + } + } + + b3ValidateSolverSets( world ); + b3ValidateContacts( world ); + + b3TracyCZoneEnd( contact_state ); + b3TracyCZoneEnd( collide ); +} + +void b3World_Step( b3WorldId worldId, float timeStep, int subStepCount ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + world->locked = true; + + b3TracyCZoneNC( world_step, "Step", b3_colorBox2DGreen, true ); + + // Clear debug buffers + for ( int i = 0; i < world->workerCount; ++i ) + { + world->taskContexts.data[i].pointCount = 0; + world->taskContexts.data[i].lineCount = 0; + } + + // Prepare to capture events + // Ensure user does not access stale data if there is an early return + b3Array_Clear( world->bodyMoveEvents ); + b3Array_Clear( world->sensorBeginEvents ); + b3Array_Clear( world->contactBeginEvents ); + b3Array_Clear( world->contactHitEvents ); + b3Array_Clear( world->jointEvents ); + + world->profile = (b3Profile){ 0 }; + + + world->activeTaskCount = 0; + world->taskCount = 0; + + if ( world->scheduler != NULL ) + { + b3ResetScheduler( world->scheduler ); + } + + uint64_t stepTicks = b3GetTicks(); + + { + b3Capacity* c = &world->maxCapacity; + c->staticShapeCount = b3MaxInt( c->staticShapeCount, world->broadPhase.trees[b3_staticBody].proxyCount ); + c->dynamicShapeCount = b3MaxInt( c->dynamicShapeCount, world->broadPhase.trees[b3_dynamicBody].proxyCount ); + + int staticBodyCount = world->solverSets.data[b3_staticSet].bodySims.count; + c->staticBodyCount = b3MaxInt( c->staticBodyCount, staticBodyCount ); + + // this includes kinematic bodies + int totalBodyCount = b3GetIdCount( &world->bodyIdPool ); + c->dynamicBodyCount = b3MaxInt( c->dynamicBodyCount, totalBodyCount - staticBodyCount ); + + int totalContactCount = b3GetIdCount( &world->contactIdPool ); + c->contactCount = b3MaxInt( c->contactCount, totalContactCount ); + } + + // Update collision pairs and create contacts + { + uint64_t pairTicks = b3GetTicks(); + b3UpdateBroadPhasePairs( world ); + world->profile.pairs = b3GetMilliseconds( pairTicks ); + } + + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + + b3StepContext context = { 0 }; + context.world = world; + context.states = awakeSet->bodyStates.data; + context.dt = timeStep; + context.subStepCount = b3MaxInt( 1, subStepCount ); + + if ( timeStep > 0.0f ) + { + context.inv_dt = 1.0f / timeStep; + context.h = timeStep / context.subStepCount; + context.inv_h = context.subStepCount * context.inv_dt; + } + else + { + context.inv_dt = 0.0f; + context.h = 0.0f; + context.inv_h = 0.0f; + } + + world->inv_h = context.inv_h; + world->inv_dt = context.inv_dt; + + // Hertz values get reduced for large time steps + float contactHertz = b3MinFloat( world->contactHertz, 0.125f * context.inv_h ); + context.contactSoftness = b3MakeSoft( contactHertz, world->contactDampingRatio, context.h ); + context.staticSoftness = b3MakeSoft( 2.0f * contactHertz, 0.5f * world->contactDampingRatio, context.h ); + + context.restitutionThreshold = world->restitutionThreshold; + context.maxLinearVelocity = world->maxLinearSpeed; + context.enableWarmStarting = world->enableWarmStarting; + + // Narrow phase : update contacts + { + uint64_t collideTicks = b3GetTicks(); + b3Collide( &context ); + world->profile.collide = b3GetMilliseconds( collideTicks ); + } + + // Integrate velocities, solve velocity constraints, and integrate positions. + if ( timeStep > 0.0f ) + { + uint64_t solveTicks = b3GetTicks(); + b3Solve( world, &context ); + world->profile.solve = b3GetMilliseconds( solveTicks ); + } + + // Finish the tree task in case b3Solve didn't finish it + if ( world->userTreeTask ) + { + world->finishTaskFcn( world->userTreeTask, world->userTaskContext ); + world->userTreeTask = NULL; + world->activeTaskCount -= 1; + } + + // Update sensors + { + uint64_t sensorTicks = b3GetTicks(); + b3OverlapSensors( world ); + world->profile.sensors = b3GetMilliseconds( sensorTicks ); + } + + world->profile.step = b3GetMilliseconds( stepTicks ); + + B3_ASSERT( world->stack.allocation == 0 ); + + // Ensure stack is large enough + b3GrowStack( &world->stack ); + + // Make sure all tasks that were started were also finished + B3_ASSERT( world->activeTaskCount == 0 ); + + + // Swap end event array buffers + world->endEventArrayIndex = 1 - world->endEventArrayIndex; + b3Array_Clear( world->sensorEndEvents[world->endEventArrayIndex] ); + b3Array_Clear( world->contactEndEvents[world->endEventArrayIndex] ); + world->locked = false; + + b3TracyCZoneEnd( world_step ); + b3TracyCFrame; +} + +typedef struct DrawContext +{ + b3World* world; + b3DebugDraw* draw; +} DrawContext; + +static bool DrawQueryCallback( int proxyId, uint64_t userData, void* context ) +{ + int shapeId = (int)userData; + + B3_UNUSED( proxyId ); + + struct DrawContext* drawContext = (DrawContext*)context; + b3World* world = drawContext->world; + b3DebugDraw* draw = drawContext->draw; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + B3_ASSERT( shape->id == shapeId ); + + b3SetBit( &world->debugBodySet, shape->bodyId ); + + if ( draw->drawShapes ) + { + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + + b3HexColor color; + + if ( shape->materials[0].customColor != 0 ) + { + color = (b3HexColor)shape->materials[0].customColor; + } + else if ( body->type == b3_dynamicBody && body->mass == 0.0f ) + { + // Bad body + color = b3_colorRed; + } + else if ( body->setIndex == b3_disabledSet ) + { + color = b3_colorSlateGray; + } + else if ( shape->sensorIndex != B3_NULL_INDEX ) + { + color = b3_colorWheat; + } + else if ( body->flags & b3_hadTimeOfImpact ) + { + color = b3_colorLime; + } + else if ( ( bodySim->flags & b3_isBullet ) && body->setIndex == b3_awakeSet ) + { + color = b3_colorTurquoise; + } + else if ( body->flags & b3_isSpeedCapped ) + { + color = b3_colorYellow; + } + else if ( bodySim->flags & b3_isFast ) + { + color = b3_colorSalmon; + } + else if ( body->type == b3_staticBody ) + { + color = b3_colorPaleGreen; + } + else if ( body->type == b3_kinematicBody ) + { + if ( body->setIndex == b3_awakeSet ) + { + color = b3_colorRoyalBlue; + } + else + { + color = b3_colorDarkBlue; + } + } + else if ( body->setIndex == b3_awakeSet ) + { + color = b3_colorPink; + } + else + { + color = b3_colorGray; + } + + if ( shape->userShape == NULL && world->createDebugShape != NULL ) + { + b3DebugShape debugShape = { 0 }; + debugShape.shapeId = (b3ShapeId){ + .world0 = world->worldId, + .index1 = shapeId + 1, + .generation = shape->generation, + }; + debugShape.type = shape->type; + + switch ( shape->type ) + { + case b3_capsuleShape: + debugShape.capsule = &shape->capsule; + shape->userShape = world->createDebugShape( &debugShape, world->userDebugShapeContext ); + break; + case b3_compoundShape: + debugShape.compound = shape->compound; + shape->userShape = world->createDebugShape( &debugShape, world->userDebugShapeContext ); + break; + case b3_heightShape: + debugShape.heightField = shape->heightField; + shape->userShape = world->createDebugShape( &debugShape, world->userDebugShapeContext ); + break; + case b3_hullShape: + debugShape.hull = shape->hull; + shape->userShape = world->createDebugShape( &debugShape, world->userDebugShapeContext ); + break; + case b3_meshShape: + debugShape.mesh = &shape->mesh; + shape->userShape = world->createDebugShape( &debugShape, world->userDebugShapeContext ); + break; + case b3_sphereShape: + debugShape.sphere = &shape->sphere; + shape->userShape = world->createDebugShape( &debugShape, world->userDebugShapeContext ); + break; + default: + B3_ASSERT( false ); + break; + } + } + + if ( shape->userShape != NULL ) + { + draw->DrawShapeFcn( shape->userShape, bodySim->transform, color, draw->context ); + } + } + + if ( draw->drawBounds ) + { + b3AABB aabb = shape->fatAABB; + draw->DrawBoundsFcn( aabb, b3_colorGold, draw->context ); + } + + return true; +} + +void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_ASSERT( b3IsValidAABB( draw->drawingBounds ) ); + + const float axisScale = 0.3f * b3GetLengthUnitsPerMeter(); + b3HexColor farColor = b3_colorDarkBlue; + b3HexColor speculativeColor = b3_colorBlue; + b3HexColor addColor = b3_colorLimeGreen; + b3HexColor persistColor = b3_colorLightBlue; + // b3HexColor normalColors[3] = { b3_colorLightGray, b3_colorLightSalmon, b3_colorLightSeaGreen }; + b3HexColor impulseColor = b3_colorMagenta; + b3HexColor frictionColor = b3_colorYellow; + + b3HexColor graphColors[B3_GRAPH_COLOR_COUNT] = { + b3_colorRed, b3_colorOrange, b3_colorYellow, b3_colorGreen, b3_colorCyan, b3_colorBlue, + b3_colorViolet, b3_colorPink, b3_colorChocolate, b3_colorGoldenRod, b3_colorCoral, b3_colorRosyBrown, + b3_colorAqua, b3_colorPeru, b3_colorLime, b3_colorGold, b3_colorPlum, b3_colorSnow, + b3_colorTeal, b3_colorKhaki, b3_colorSalmon, b3_colorPeachPuff, b3_colorHoneyDew, b3_colorBlack, + }; + + int bodyCapacity = b3GetIdCapacity( &world->bodyIdPool ); + b3SetBitCountAndClear( &world->debugBodySet, bodyCapacity ); + + int jointCapacity = b3GetIdCapacity( &world->jointIdPool ); + b3SetBitCountAndClear( &world->debugJointSet, jointCapacity ); + + int contactCapacity = b3GetIdCapacity( &world->contactIdPool ); + b3SetBitCountAndClear( &world->debugContactSet, contactCapacity ); + + int islandCapacity = b3GetIdCapacity( &world->islandIdPool ); + b3SetBitCountAndClear( &world->debugIslandSet, islandCapacity ); + + struct DrawContext drawContext = { world, draw }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3DynamicTree_Query( world->broadPhase.trees + i, draw->drawingBounds, maskBits, false, DrawQueryCallback, &drawContext ); + } + + uint32_t wordCount = world->debugBodySet.blockCount; + uint64_t* bits = world->debugBodySet.bits; + for ( uint32_t wordIndex = 0; wordIndex < wordCount; ++wordIndex ) + { + uint64_t word = bits[wordIndex]; + while ( word != 0 ) + { + uint32_t ctz = b3CTZ64( word ); + int bodyId = (int)( 64 * wordIndex + ctz ); + + b3Body* body = b3Array_Get( world->bodies, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + + if ( draw->drawBodyNames && body->name[0] != 0 ) + { + b3Vec3 offset = { 0.05f, 0.05f, 0.05f }; + b3Transform transform = { bodySim->center, bodySim->transform.q }; + b3Vec3 p = b3TransformPoint( transform, offset ); + draw->DrawStringFcn( p, body->name, b3_colorOrange, draw->context ); + } + + if ( draw->drawMass && body->type == b3_dynamicBody ) + { + b3Vec3 offset = { 0.1f, 0.1f, 0.1f }; + + b3Transform transform = { bodySim->center, bodySim->transform.q }; + draw->DrawTransformFcn( transform, draw->context ); + b3Vec3 p = b3TransformPoint( transform, offset ); + + char buffer[32]; + snprintf( buffer, 32, " %.2f", body->mass ); + draw->DrawStringFcn( p, buffer, b3_colorWhite, draw->context ); + } + + if ( draw->drawJoints ) + { + int jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + b3Joint* joint = b3Array_Get( world->joints, jointId ); + + // avoid double draw + if ( b3GetBit( &world->debugJointSet, jointId ) == false ) + { + b3DrawJoint( draw, world, joint ); + b3SetBit( &world->debugJointSet, jointId ); + } + + jointKey = joint->edges[edgeIndex].nextKey; + } + } + + const float speculativeDistance = B3_SPECULATIVE_DISTANCE; + if ( draw->drawContacts && body->type == b3_dynamicBody && body->setIndex == b3_awakeSet ) + { + int contactKey = body->headContactKey; + while ( contactKey != B3_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->setIndex != b3_awakeSet || contact->colorIndex == B3_NULL_INDEX ) + { + continue; + } + + // avoid double draw + if ( b3GetBit( &world->debugContactSet, contactId ) == false ) + { + b3Body* bodyA = b3Array_Get( world->bodies, contact->edges[0].bodyId ); + b3BodySim* bodySimA = b3GetBodySim( world, bodyA ); + b3Body* bodyB = b3Array_Get( world->bodies, contact->edges[1].bodyId ); + b3BodySim* bodySimB = b3GetBodySim( world, bodyB ); + + for ( int manifoldIndex = 0; manifoldIndex < contact->manifoldCount; ++manifoldIndex ) + { + const b3Manifold* manifold = contact->manifolds + manifoldIndex; + B3_ASSERT( manifold->pointCount > 0 ); + + b3Vec3 normal = manifold->normal; + b3Vec3 frictionCenter = b3Vec3_zero; + + const b3ManifoldPoint* points = manifold->points; + for ( int pointIndex = 0; pointIndex < manifold->pointCount; ++pointIndex ) + { + const b3ManifoldPoint* mp = points + pointIndex; + + char buffer[32]; + + b3Vec3 p; + if ( draw->drawAnchorA == 1 ) + { + p = b3Add( bodySimA->center, mp->anchorA ); + } + else + { + p = b3Add( bodySimB->center, mp->anchorB ); + } + + frictionCenter = b3Add( frictionCenter, p ); + + if ( draw->drawContactNormals ) + { + b3Vec3 p1 = p; + b3Vec3 p2 = b3MulAdd( p1, axisScale, normal ); + draw->DrawSegmentFcn( p1, p2, b3_colorLightGray, draw->context ); + + snprintf( buffer, B3_ARRAY_COUNT( buffer ), " %.2f", 100.0f * mp->separation ); + draw->DrawStringFcn( p, buffer, b3_colorWhite, draw->context ); + } + else if ( draw->drawContactForces ) + { + // Hack inv_dt for single step debugging + float inv_dt = world->inv_dt > 0.0f ? world->inv_dt : 60.0f; + // todo validate + // multiply by one-half due to relax iteration + float force = 0.5f * mp->totalNormalImpulse * inv_dt; + b3Vec3 p1 = p; + b3Vec3 p2 = b3MulAdd( p1, draw->forceScale * force, normal ); + draw->DrawSegmentFcn( p1, p2, impulseColor, draw->context ); + snprintf( buffer, B3_ARRAY_COUNT( buffer ), " %.1f", force ); + draw->DrawStringFcn( p1, buffer, b3_colorWhite, draw->context ); + } + else if ( draw->drawContactFeatures ) + { + snprintf( buffer, B3_ARRAY_COUNT( buffer ), " %#X", mp->featureId ); + draw->DrawStringFcn( p, buffer, b3_colorWhite, draw->context ); + } + + if ( draw->drawGraphColors ) + { + // graph color + float pointSize = contact->colorIndex == B3_OVERFLOW_INDEX ? 15.0f : 10.0f; + draw->DrawPointFcn( p, pointSize, graphColors[contact->colorIndex], draw->context ); + // g_draw.DrawString(point->position, "%d", point->color); + } + else if ( mp->persisted == false ) + { + // Add + draw->DrawPointFcn( p, 20.0f, addColor, draw->context ); + } + else if ( mp->separation > speculativeDistance ) + { + // Speculative + draw->DrawPointFcn( p, 10.0f, farColor, draw->context ); + } + else if ( mp->separation > 0.0f ) + { + // Speculative + draw->DrawPointFcn( p, 10.0f, speculativeColor, draw->context ); + } + else + { + // Persist + draw->DrawPointFcn( p, 10.0f, persistColor, draw->context ); + } + } + + if ( draw->drawContactForces ) + { + // Hack inv_dt for single step debugging + float inv_dt = world->inv_dt > 0.0f ? world->inv_dt : 60.0f; + + b3Vec3 p1 = b3MulSV( 1.0f / manifold->pointCount, frictionCenter ); + b3Vec3 frictionForce = b3MulSV( 0.5f * inv_dt, manifold->frictionImpulse ); + b3Vec3 p2 = b3MulAdd( p1, draw->forceScale, frictionForce ); + draw->DrawSegmentFcn( p1, p2, frictionColor, draw->context ); + draw->DrawPointFcn( p1, 5.0f, frictionColor, draw->context ); + + char buffer[32]; + snprintf( buffer, B3_ARRAY_COUNT( buffer ), " %.2f", b3Length( frictionForce ) ); + draw->DrawStringFcn( p1, buffer, b3_colorWhite, draw->context ); + } + } + + b3SetBit( &world->debugContactSet, contactId ); + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + } + + if ( draw->drawIslands ) + { + int islandId = body->islandId; + if ( islandId != B3_NULL_INDEX && b3GetBit( &world->debugIslandSet, islandId ) == false ) + { + b3Island* island = world->islands.data + islandId; + if ( island->setIndex == B3_NULL_INDEX ) + { + continue; + } + + int shapeCount = 0; + b3AABB aabb = { + .lowerBound = { FLT_MAX, FLT_MAX, FLT_MAX }, + .upperBound = { -FLT_MAX, -FLT_MAX, -FLT_MAX }, + }; + + for ( int b = 0; b < island->bodies.count; ++b ) + { + int islandBodyId = island->bodies.data[b]; + b3Body* islandBody = b3Array_Get( world->bodies, islandBodyId ); + int shapeId = islandBody->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + aabb = b3AABB_Union( aabb, shape->fatAABB ); + shapeCount += 1; + shapeId = shape->nextShapeId; + } + } + + if ( shapeCount > 0 ) + { + draw->DrawBoundsFcn( aabb, b3_colorOrangeRed, draw->context ); + } + + b3SetBit( &world->debugIslandSet, islandId ); + } + } + + // Clear the smallest set bit + word = word & ( word - 1 ); + } + } + + char buffer[32] = { 0 }; + float lengthUnits = b3GetLengthUnitsPerMeter(); + b3Vec3 offset = { 0.002f * lengthUnits, 0.002f * lengthUnits, 0.002f * lengthUnits }; + for ( int i = 0; i < world->workerCount; ++i ) + { + int pointCount = world->taskContexts.data[i].pointCount; + b3DebugPoint* points = world->taskContexts.data[i].points; + for ( int j = 0; j < pointCount; ++j ) + { + b3DebugPoint* point = points + j; + draw->DrawPointFcn( point->p, 5.0f, point->color, draw->context ); + snprintf( buffer, 32, " %d, %.2f", point->label, point->value ); + b3Vec3 ps = b3Add( point->p, offset ); + draw->DrawStringFcn( ps, buffer, point->color, draw->context ); + } + + int lineCount = world->taskContexts.data[i].lineCount; + b3DebugLine* lines = world->taskContexts.data[i].lines; + for ( int j = 0; j < lineCount; ++j ) + { + b3DebugLine* line = lines + j; + draw->DrawSegmentFcn( line->p1, line->p2, line->color, draw->context ); + draw->DrawPointFcn( line->p1, 10.0f, line->color, draw->context ); + snprintf( buffer, 32, "%d", line->label ); + b3Vec3 ps = b3Add( line->p1, offset ); + draw->DrawStringFcn( ps, buffer, line->color, draw->context ); + } + } +} + +b3BodyEvents b3World_GetBodyEvents( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3BodyEvents){ 0 }; + } + + int count = world->bodyMoveEvents.count; + b3BodyEvents events = { world->bodyMoveEvents.data, count }; + return events; +} + +b3SensorEvents b3World_GetSensorEvents( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3SensorEvents){ 0 }; + } + + // Careful to use previous buffer + int endEventArrayIndex = 1 - world->endEventArrayIndex; + + int beginCount = world->sensorBeginEvents.count; + int endCount = world->sensorEndEvents[endEventArrayIndex].count; + + b3SensorEvents events = { + .beginEvents = world->sensorBeginEvents.data, + .endEvents = world->sensorEndEvents[endEventArrayIndex].data, + .beginCount = beginCount, + .endCount = endCount, + }; + return events; +} + +b3ContactEvents b3World_GetContactEvents( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3ContactEvents){ 0 }; + } + + // Careful to use previous buffer + int endEventArrayIndex = 1 - world->endEventArrayIndex; + + int beginCount = world->contactBeginEvents.count; + int endCount = world->contactEndEvents[endEventArrayIndex].count; + int hitCount = world->contactHitEvents.count; + + b3ContactEvents events = { + .beginEvents = world->contactBeginEvents.data, + .endEvents = world->contactEndEvents[endEventArrayIndex].data, + .hitEvents = world->contactHitEvents.data, + .beginCount = beginCount, + .endCount = endCount, + .hitCount = hitCount, + }; + + return events; +} + +b3JointEvents b3World_GetJointEvents( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3JointEvents){ 0 }; + } + + int count = world->jointEvents.count; + b3JointEvents events = { world->jointEvents.data, count }; + return events; +} + +bool b3World_IsValid( b3WorldId id ) +{ + if ( id.index1 < 1 || B3_MAX_WORLDS < id.index1 ) + { + return false; + } + + b3World* world = b3_worlds + ( id.index1 - 1 ); + + if ( world->worldId != id.index1 - 1 ) + { + // world is not allocated + return false; + } + + return id.generation == world->generation; +} + +bool b3Body_IsValid( b3BodyId id ) +{ + if ( B3_MAX_WORLDS <= id.world0 ) + { + // invalid world + return false; + } + + b3World* world = b3_worlds + id.world0; + if ( world->worldId != id.world0 ) + { + // world is free + return false; + } + + if ( id.index1 < 1 || world->bodies.count < id.index1 ) + { + // invalid index + return false; + } + + b3Body* body = world->bodies.data + ( id.index1 - 1 ); + if ( body->setIndex == B3_NULL_INDEX ) + { + // this was freed + return false; + } + + B3_ASSERT( body->localIndex != B3_NULL_INDEX ); + + if ( body->generation != id.generation ) + { + // this id is orphaned + return false; + } + + return true; +} + +bool b3Shape_IsValid( b3ShapeId id ) +{ + if ( B3_MAX_WORLDS <= id.world0 ) + { + return false; + } + + b3World* world = b3_worlds + id.world0; + if ( world->worldId != id.world0 ) + { + // world is free + return false; + } + + int shapeId = id.index1 - 1; + if ( shapeId < 0 || world->shapes.count <= shapeId ) + { + return false; + } + + b3Shape* shape = world->shapes.data + shapeId; + if ( shape->id == B3_NULL_INDEX ) + { + // shape is free + return false; + } + + B3_ASSERT( shape->id == shapeId ); + + return id.generation == shape->generation; +} + +bool b3Joint_IsValid( b3JointId id ) +{ + if ( B3_MAX_WORLDS <= id.world0 ) + { + return false; + } + + b3World* world = b3_worlds + id.world0; + if ( world->worldId != id.world0 ) + { + // world is free + return false; + } + + int jointId = id.index1 - 1; + if ( jointId < 0 || world->joints.count <= jointId ) + { + return false; + } + + b3Joint* joint = world->joints.data + jointId; + if ( joint->jointId == B3_NULL_INDEX ) + { + // joint is free + return false; + } + + B3_ASSERT( joint->jointId == jointId ); + + return id.generation == joint->generation; +} + +bool b3Contact_IsValid( b3ContactId id ) +{ + if ( B3_MAX_WORLDS <= id.world0 ) + { + return false; + } + + b3World* world = b3_worlds + id.world0; + if ( world->worldId != id.world0 ) + { + // world is free + return false; + } + + int contactId = id.index1 - 1; + if ( contactId < 0 || world->contacts.count <= contactId ) + { + return false; + } + + b3Contact* contact = world->contacts.data + contactId; + if ( contact->contactId == B3_NULL_INDEX ) + { + // contact is free + return false; + } + + B3_ASSERT( contact->contactId == contactId ); + + return id.generation == contact->generation; +} + +void b3World_EnableSleeping( b3WorldId worldId, bool flag ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + if ( flag == world->enableSleep ) + { + return; + } + + world->enableSleep = flag; + + if ( flag == false ) + { + int setCount = world->solverSets.count; + for ( int i = b3_firstSleepingSet; i < setCount; ++i ) + { + b3SolverSet* set = b3Array_Get( world->solverSets, i ); + if ( set->bodySims.count > 0 ) + { + b3WakeSolverSet( world, i ); + } + } + } +} + +bool b3World_IsSleepingEnabled( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->enableSleep; +} + +void b3World_EnableWarmStarting( b3WorldId worldId, bool flag ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + world->enableWarmStarting = flag; +} + +bool b3World_IsWarmStartingEnabled( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->enableWarmStarting; +} + +int b3World_GetAwakeBodyCount( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return 0; + } + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + return awakeSet->bodySims.count; +} + +void b3World_EnableContinuous( b3WorldId worldId, bool flag ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + world->enableContinuous = flag; +} + +bool b3World_IsContinuousEnabled( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->enableContinuous; +} + +void b3World_SetRestitutionThreshold( b3WorldId worldId, float value ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + world->restitutionThreshold = b3ClampFloat( value, 0.0f, FLT_MAX ); +} + +float b3World_GetRestitutionThreshold( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->restitutionThreshold; +} + +void b3World_SetHitEventThreshold( b3WorldId worldId, float value ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + world->hitEventThreshold = b3ClampFloat( value, 0.0f, FLT_MAX ); +} + +float b3World_GetHitEventThreshold( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->hitEventThreshold; +} + +void b3World_SetContactTuning( b3WorldId worldId, float hertz, float dampingRatio, float contactSpeed ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + world->contactHertz = b3ClampFloat( hertz, 0.0f, FLT_MAX ); + world->contactDampingRatio = b3ClampFloat( dampingRatio, 0.0f, FLT_MAX ); + world->contactSpeed = b3ClampFloat( contactSpeed, 0.0f, FLT_MAX ); +} + +void b3World_SetContactRecycleDistance( b3WorldId worldId, float recycleDistance ) +{ + b3World* world = b3GetWorldFromId( worldId ); + B3_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + world->contactRecycleDistance = b3ClampFloat( recycleDistance, 0.0f, FLT_MAX ); +} + +float b3World_GetContactRecycleDistance( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->contactRecycleDistance; +} + +void b3World_SetMaximumLinearSpeed( b3WorldId worldId, float maximumLinearSpeed ) +{ + B3_ASSERT( b3IsValidFloat( maximumLinearSpeed ) && maximumLinearSpeed > 0.0f ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + world->maxLinearSpeed = maximumLinearSpeed; +} + +float b3World_GetMaximumLinearSpeed( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->maxLinearSpeed; +} + +b3Profile b3World_GetProfile( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3Profile){ 0 }; + } + return world->profile; +} + +b3Counters b3World_GetCounters( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3Counters){ 0 }; + } + + b3Counters s = { 0 }; + s.bodyCount = b3GetIdCount( &world->bodyIdPool ); + s.shapeCount = b3GetIdCount( &world->shapeIdPool ); + s.contactCount = b3GetIdCount( &world->contactIdPool ); + s.jointCount = b3GetIdCount( &world->jointIdPool ); + s.islandCount = b3GetIdCount( &world->islandIdPool ); + + b3DynamicTree* staticTree = world->broadPhase.trees + b3_staticBody; + s.staticTreeHeight = b3DynamicTree_GetHeight( staticTree ); + + b3DynamicTree* dynamicTree = world->broadPhase.trees + b3_dynamicBody; + b3DynamicTree* kinematicTree = world->broadPhase.trees + b3_kinematicBody; + s.treeHeight = b3MaxInt( b3DynamicTree_GetHeight( dynamicTree ), b3DynamicTree_GetHeight( kinematicTree ) ); + + s.satCallCount = world->satCallCount; + s.satCacheHitCount = world->satCacheHitCount; + memcpy( s.manifoldCounts, world->manifoldCounts, B3_CONTACT_MANIFOLD_COUNT_BUCKETS * sizeof( int ) ); + s.stackUsed = world->stack.maxAllocation; + s.byteCount = b3GetByteCount(); + s.taskCount = world->taskCount; + + _Static_assert( B3_GRAPH_COLOR_COUNT == sizeof( s.colorCounts ) / sizeof( s.colorCounts[0] ), "colorCounts size mismatch" ); + + s.awakeContactCount = 0; + for ( int i = 0; i < B3_GRAPH_COLOR_COUNT; ++i ) + { + b3GraphColor* color = world->constraintGraph.colors + i; + int colorContactCount = color->convexContacts.count + color->contacts.count; + s.colorCounts[i] = colorContactCount + color->jointSims.count; + s.awakeContactCount += colorContactCount; + } + s.awakeContactCount += world->solverSets.data[b3_awakeSet].contactIndices.count; + + s.recycledContactCount = 0; + for ( int i = 0; i < world->workerCount; ++i ) + { + s.recycledContactCount += world->taskContexts.data[i].recycledContactCount; + } + + return s; +} + +b3Capacity b3World_GetMaxCapacity( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3Capacity){ 0 }; + } + return world->maxCapacity; +} + +void b3World_SetUserData( b3WorldId worldId, void* userData ) +{ + b3World* world = b3GetWorldFromId( worldId ); + world->userData = userData; +} + +void* b3World_GetUserData( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->userData; +} + +void b3World_SetWorkerCount( b3WorldId worldId, int count ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + if ( count == world->workerCount ) + { + return; + } + + b3DestroyWorkerContexts( world ); + world->workerCount = b3ClampInt( count, 1, B3_MAX_WORKERS ); + b3CreateWorkerContexts( world ); +} + +int b3World_GetWorkerCount( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return 0; + } + + return world->workerCount; +} + +static FILE* b3OpenFile( const char* fileName ) +{ + FILE* file = NULL; + +#if defined( _MSC_VER ) + errno_t e = fopen_s( &file, fileName, "w" ); + if ( e != 0 ) + { + return NULL; + } +#else + file = fopen( fileName, "w" ); +#endif + + return file; +} + +void b3World_DumpMemoryStats( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + FILE* file = b3OpenFile( "box3d_memory.txt" ); + if ( file == NULL ) + { + return; + } + + // id pools + fprintf( file, "id pools\n" ); + fprintf( file, "body ids: %d\n", b3GetIdBytes( &world->bodyIdPool ) ); + fprintf( file, "solver set ids: %d\n", b3GetIdBytes( &world->solverSetIdPool ) ); + fprintf( file, "joint ids: %d\n", b3GetIdBytes( &world->jointIdPool ) ); + fprintf( file, "contact ids: %d\n", b3GetIdBytes( &world->contactIdPool ) ); + fprintf( file, "island ids: %d\n", b3GetIdBytes( &world->islandIdPool ) ); + fprintf( file, "shape ids: %d\n", b3GetIdBytes( &world->shapeIdPool ) ); + fprintf( file, "\n" ); + + // world arrays + fprintf( file, "world arrays\n" ); + fprintf( file, "bodies: %d\n", b3Array_ByteCount( world->bodies ) ); + fprintf( file, "solver sets: %d\n", b3Array_ByteCount( world->solverSets ) ); + fprintf( file, "joints: %d\n", b3Array_ByteCount( world->joints ) ); + fprintf( file, "contacts: %d\n", b3Array_ByteCount( world->contacts ) ); + fprintf( file, "islands: %d\n", b3Array_ByteCount( world->islands ) ); + fprintf( file, "shapes: %d\n", b3Array_ByteCount( world->shapes ) ); + fprintf( file, "\n" ); + + // broad-phase + fprintf( file, "broad-phase\n" ); + fprintf( file, "static tree: %d\n", b3DynamicTree_GetByteCount( world->broadPhase.trees + b3_staticBody ) ); + fprintf( file, "kinematic tree: %d\n", b3DynamicTree_GetByteCount( world->broadPhase.trees + b3_kinematicBody ) ); + fprintf( file, "dynamic tree: %d\n", b3DynamicTree_GetByteCount( world->broadPhase.trees + b3_dynamicBody ) ); + b3HashSet* moveSet = &world->broadPhase.moveSet; + fprintf( file, "moveSet: %d (%d, %d)\n", b3GetHashSetBytes( moveSet ), moveSet->count, moveSet->capacity ); + fprintf( file, "moveArray: %d\n", b3Array_ByteCount( world->broadPhase.moveArray ) ); + b3HashSet* pairSet = &world->broadPhase.pairSet; + fprintf( file, "pairSet: %d (%d, %d)\n", b3GetHashSetBytes( pairSet ), pairSet->count, pairSet->capacity ); + fprintf( file, "\n" ); + + // solver sets + int bodySimCapacity = 0; + int bodyStateCapacity = 0; + int jointSimCapacity = 0; + int contactIndexCapacity = 0; + int islandSimCapacity = 0; + int solverSetCapacity = world->solverSets.count; + for ( int i = 0; i < solverSetCapacity; ++i ) + { + b3SolverSet* set = world->solverSets.data + i; + if ( set->setIndex == B3_NULL_INDEX ) + { + continue; + } + + bodySimCapacity += set->bodySims.capacity; + bodyStateCapacity += set->bodyStates.capacity; + jointSimCapacity += set->jointSims.capacity; + contactIndexCapacity += set->contactIndices.capacity; + islandSimCapacity += set->islandSims.capacity; + } + + fprintf( file, "solver sets\n" ); + fprintf( file, "body sim: %d\n", bodySimCapacity * (int)sizeof( b3BodySim ) ); + fprintf( file, "body state: %d\n", bodyStateCapacity * (int)sizeof( b3BodyState ) ); + fprintf( file, "joint sim: %d\n", jointSimCapacity * (int)sizeof( b3JointSim ) ); + fprintf( file, "contact sim: %d\n", contactIndexCapacity * (int)sizeof( int ) ); + fprintf( file, "island sim: %d\n", islandSimCapacity * (int)sizeof( islandSimCapacity ) ); + fprintf( file, "\n" ); + + // constraint graph + int bodyBitSetBytes = 0; + int contactSpecCapacity = 0; + jointSimCapacity = 0; + for ( int i = 0; i < B3_GRAPH_COLOR_COUNT; ++i ) + { + b3GraphColor* c = world->constraintGraph.colors + i; + bodyBitSetBytes += b3GetBitSetBytes( &c->bodySet ); + contactSpecCapacity += c->convexContacts.capacity + c->contacts.capacity; + jointSimCapacity += c->jointSims.capacity; + } + + fprintf( file, "constraint graph\n" ); + fprintf( file, "body bit sets: %d\n", bodyBitSetBytes ); + fprintf( file, "joint sim: %d\n", jointSimCapacity * (int)sizeof( b3JointSim ) ); + fprintf( file, "contact sim: %d\n", contactSpecCapacity * (int)sizeof( int ) ); + fprintf( file, "\n" ); + + // stack allocator + fprintf( file, "stack allocator: %d\n\n", world->stack.capacity ); + + fclose( file ); +} + +void b3World_DumpShapeBounds( b3WorldId worldId, b3BodyType type ) +{ + B3_ASSERT( b3_staticBody <= type && type <= b3_dynamicBody ); + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + FILE* file = b3OpenFile( "box3d_bounds.txt" ); + if ( file == NULL ) + { + return; + } + + b3DynamicTree* tree = world->broadPhase.trees + type; + b3TreeNode* nodes = tree->nodes; + + uint16_t requiredFlags = b3_allocatedNode | b3_leafNode; + int capacity = tree->nodeCapacity; + for ( int i = 0; i < capacity; ++i ) + { + b3TreeNode* node = nodes + i; + if ( ( node->flags & requiredFlags ) != requiredFlags ) + { + // skip internal and free nodes + continue; + } + + b3Vec3 a = node->aabb.lowerBound; + b3Vec3 b = node->aabb.upperBound; + fprintf( file, "%.9f %.9f %.9f %.9f %.9f %.9f\n", a.x, a.y, a.z, b.x, b.y, b.z ); + } + + fclose( file ); +} + +typedef struct WorldQueryContext +{ + b3World* world; + b3OverlapResultFcn* fcn; + b3QueryFilter filter; + void* userContext; +} WorldQueryContext; + +static bool TreeQueryCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + WorldQueryContext* worldContext = (WorldQueryContext*)context; + b3World* world = worldContext->world; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + b3Filter shapeFilter = shape->filter; + b3QueryFilter queryFilter = worldContext->filter; + + if ( b3ShouldQueryCollide( &shapeFilter, &queryFilter ) == false ) + { + return true; + } + + b3ShapeId id = { shapeId + 1, world->worldId, shape->generation }; + bool result = worldContext->fcn( id, worldContext->userContext ); + return result; +} + +b3TreeStats b3World_OverlapAABB( b3WorldId worldId, b3AABB aabb, b3QueryFilter filter, b3OverlapResultFcn* fcn, void* context ) +{ + b3TreeStats treeStats = { 0 }; + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return treeStats; + } + + B3_ASSERT( b3IsValidAABB( aabb ) ); + + WorldQueryContext worldContext = { world, fcn, filter, context }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3TreeStats treeResult = + b3DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, false, TreeQueryCallback, &worldContext ); + + treeStats.nodeVisits += treeResult.nodeVisits; + treeStats.leafVisits += treeResult.leafVisits; + } + + return treeStats; +} + +typedef struct WorldOverlapContext +{ + b3World* world; + b3OverlapResultFcn* fcn; + b3QueryFilter filter; + b3ShapeProxy proxy; + void* userContext; +} WorldOverlapContext; + +static bool b3TreeOverlapCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + WorldOverlapContext* worldContext = (WorldOverlapContext*)context; + b3World* world = worldContext->world; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + b3Filter shapeFilter = shape->filter; + b3QueryFilter queryFilter = worldContext->filter; + + if ( b3ShouldQueryCollide( &shapeFilter, &queryFilter ) == false ) + { + return true; + } + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3Transform transform = b3GetBodyTransformQuick( world, body ); + + bool overlapping = b3OverlapShape( shape, transform, &worldContext->proxy ); + if ( overlapping == false ) + { + return true; + } + + b3ShapeId id = { shape->id + 1, world->worldId, shape->generation }; + bool result = worldContext->fcn( id, worldContext->userContext ); + return result; +} + +b3TreeStats b3World_OverlapShape( b3WorldId worldId, const b3ShapeProxy* proxy, b3QueryFilter filter, b3OverlapResultFcn* fcn, + void* context ) +{ + b3TreeStats treeStats = { 0 }; + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return treeStats; + } + + b3AABB aabb = b3MakeAABB( proxy->points, proxy->count, proxy->radius ); + WorldOverlapContext worldContext = { + world, fcn, filter, *proxy, context, + }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3TreeStats treeResult = b3DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, false, + b3TreeOverlapCallback, &worldContext ); + + treeStats.nodeVisits += treeResult.nodeVisits; + treeStats.leafVisits += treeResult.leafVisits; + } + + return treeStats; +} + +typedef struct WorldMoverContext +{ + b3World* world; + b3PlaneResultFcn* fcn; + b3QueryFilter filter; + b3Capsule mover; + void* userContext; +} WorldMoverContext; + +static bool TreeCollideCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + WorldMoverContext* worldContext = (WorldMoverContext*)context; + b3World* world = worldContext->world; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + b3Filter shapeFilter = shape->filter; + b3QueryFilter queryFilter = worldContext->filter; + + if ( b3ShouldQueryCollide( &shapeFilter, &queryFilter ) == false ) + { + return true; + } + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3Transform transform = b3GetBodyTransformQuick( world, body ); + + b3PlaneResult buffer[64]; + int count = b3CollideMover( buffer, 64, shape, transform, &worldContext->mover ); + + if ( count > 0 ) + { + b3ShapeId id = { shape->id + 1, world->worldId, shape->generation }; + return worldContext->fcn( id, buffer, count, worldContext->userContext ); + } + + return true; +} + +// 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 b3World_CollideMover( b3WorldId worldId, const b3Capsule* mover, b3QueryFilter filter, b3PlaneResultFcn* fcn, void* context ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + b3Vec3 r = { mover->radius, mover->radius, mover->radius }; + + b3AABB aabb; + aabb.lowerBound = b3Sub( b3Min( mover->center1, mover->center2 ), r ); + aabb.upperBound = b3Add( b3Max( mover->center1, mover->center2 ), r ); + + WorldMoverContext worldContext = { + world, fcn, filter, *mover, context, + }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, false, TreeCollideCallback, &worldContext ); + } +} + +typedef struct WorldRayCastContext +{ + b3World* world; + b3CastResultFcn* fcn; + b3QueryFilter filter; + float fraction; + void* userContext; +} WorldRayCastContext; + +static float RayCastCallback( const b3RayCastInput* input, int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + WorldRayCastContext* worldContext = (WorldRayCastContext*)context; + b3World* world = worldContext->world; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + b3Filter shapeFilter = shape->filter; + b3QueryFilter queryFilter = worldContext->filter; + + if ( b3ShouldQueryCollide( &shapeFilter, &queryFilter ) == false ) + { + return input->maxFraction; + } + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3Transform transform = b3GetBodyTransformQuick( world, body ); + b3CastOutput output = b3RayCastShape( shape, transform, input ); + + if ( output.hit ) + { + B3_ASSERT( output.fraction <= input->maxFraction ); + + b3ShapeId id = { shapeId + 1, world->worldId, shape->generation }; + + int materialIndex = b3ClampInt( output.materialIndex, 0, shape->materialCount - 1 ); + uint64_t userMaterialId = shape->materials[materialIndex].userMaterialId; + + int triangleIndex = output.triangleIndex; + int childIndex = output.childIndex; + float fraction = worldContext->fcn( id, output.point, output.normal, output.fraction, userMaterialId, triangleIndex, + childIndex, worldContext->userContext ); + + // The user may return -1 to skip this shape + if ( 0.0f <= fraction && fraction <= 1.0f ) + { + worldContext->fraction = fraction; + } + + return fraction; + } + + return input->maxFraction; +} + +b3TreeStats b3World_CastRay( b3WorldId worldId, b3Vec3 origin, b3Vec3 translation, b3QueryFilter filter, b3CastResultFcn* fcn, + void* context ) +{ + b3TreeStats treeStats = { 0 }; + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return treeStats; + } + + B3_ASSERT( b3IsValidVec3( origin ) ); + B3_ASSERT( b3IsValidVec3( translation ) ); + + b3RayCastInput input = { origin, translation, 1.0f }; + + WorldRayCastContext worldContext = { world, fcn, filter, 1.0f, context }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3TreeStats treeResult = + b3DynamicTree_RayCast( world->broadPhase.trees + i, &input, filter.maskBits, false, RayCastCallback, &worldContext ); + treeStats.nodeVisits += treeResult.nodeVisits; + treeStats.leafVisits += treeResult.leafVisits; + + if ( worldContext.fraction == 0.0f ) + { + return treeStats; + } + + input.maxFraction = worldContext.fraction; + } + + return treeStats; +} + +// This callback finds the closest hit. This is the most common callback used in games. +static float b3RayCastClosestFcn( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t userMaterialId, + int triangleIndex, int childIndex, void* context ) +{ + // Ignore initial overlap + if ( fraction == 0.0f ) + { + return -1.0f; + } + + b3RayResult* rayResult = (b3RayResult*)context; + rayResult->shapeId = shapeId; + rayResult->point = point; + rayResult->normal = normal; + rayResult->fraction = fraction; + rayResult->userMaterialId = userMaterialId; + rayResult->triangleIndex = triangleIndex; + rayResult->childIndex = childIndex; + rayResult->hit = true; + return fraction; +} + +b3RayResult b3World_CastRayClosest( b3WorldId worldId, b3Vec3 origin, b3Vec3 translation, b3QueryFilter filter ) +{ + b3RayResult result = { 0 }; + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return result; + } + + B3_ASSERT( b3IsValidVec3( origin ) ); + B3_ASSERT( b3IsValidVec3( translation ) ); + + b3RayCastInput input = { origin, translation, 1.0f }; + WorldRayCastContext worldContext = { + .world = world, + .fcn = b3RayCastClosestFcn, + .filter = filter, + .fraction = 1.0f, + .userContext = &result, + }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3TreeStats treeResult = + b3DynamicTree_RayCast( world->broadPhase.trees + i, &input, filter.maskBits, false, RayCastCallback, &worldContext ); + result.nodeVisits += treeResult.nodeVisits; + result.leafVisits += treeResult.leafVisits; + + if ( worldContext.fraction == 0.0f ) + { + return result; + } + + input.maxFraction = worldContext.fraction; + } + + return result; +} + +static float b3ShapeCastCallback( const b3ShapeCastInput* input, int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + WorldRayCastContext* worldContext = (WorldRayCastContext*)context; + b3World* world = worldContext->world; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + b3Filter shapeFilter = shape->filter; + b3QueryFilter queryFilter = worldContext->filter; + + if ( b3ShouldQueryCollide( &shapeFilter, &queryFilter ) == false ) + { + return input->maxFraction; + } + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3Transform transform = b3GetBodyTransformQuick( world, body ); + + b3CastOutput output = b3ShapeCastShape( shape, transform, input ); + + if ( output.hit ) + { + b3ShapeId id = { shapeId + 1, world->worldId, shape->generation }; + int materialIndex = b3ClampInt( output.materialIndex, 0, shape->materialCount - 1 ); + uint64_t userMaterialId = shape->materials[materialIndex].userMaterialId; + + int triangleIndex = output.triangleIndex; + int childIndex = output.childIndex; + float fraction = worldContext->fcn( id, output.point, output.normal, output.fraction, userMaterialId, triangleIndex, + childIndex, worldContext->userContext ); + + // The user may return -1 to skip this shape + if ( 0.0f <= fraction && fraction <= 1.0f ) + { + worldContext->fraction = fraction; + } + + return fraction; + } + + return input->maxFraction; +} + +b3TreeStats b3World_CastShape( b3WorldId worldId, const b3ShapeProxy* proxy, b3Vec3 translation, b3QueryFilter filter, + b3CastResultFcn* fcn, void* context ) +{ + b3TreeStats treeStats = { 0 }; + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return treeStats; + } + + B3_ASSERT( b3IsValidVec3( translation ) ); + + b3ShapeCastInput input; + input.proxy = *proxy; + input.translation = translation; + input.maxFraction = 1.0f; + input.canEncroach = false; + + WorldRayCastContext worldContext = { world, fcn, filter, 1.0f, context }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3TreeStats treeResult = b3DynamicTree_ShapeCast( world->broadPhase.trees + i, &input, filter.maskBits, false, + b3ShapeCastCallback, &worldContext ); + treeStats.nodeVisits += treeResult.nodeVisits; + treeStats.leafVisits += treeResult.leafVisits; + + if ( worldContext.fraction == 0.0f ) + { + return treeStats; + } + + input.maxFraction = worldContext.fraction; + } + + return treeStats; +} + +typedef struct WorldMoverCastContext +{ + b3World* world; + b3MoverFilterFcn* fcn; + b3QueryFilter filter; + float fraction; + void* userContext; +} WorldMoverCastContext; + +static float MoverCastCallback( const b3ShapeCastInput* input, int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + WorldMoverCastContext* worldContext = (WorldMoverCastContext*)context; + b3World* world = worldContext->world; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + b3Filter shapeFilter = shape->filter; + b3QueryFilter queryFilter = worldContext->filter; + + if ( b3ShouldQueryCollide( &shapeFilter, &queryFilter ) == false ) + { + return worldContext->fraction; + } + + if ( worldContext->fcn != NULL ) + { + b3ShapeId id = { shapeId + 1, world->worldId, shape->generation }; + bool shouldCollide = worldContext->fcn( id, worldContext->userContext ); + if ( shouldCollide == false ) + { + return worldContext->fraction; + } + } + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3Transform transform = b3GetBodyTransformQuick( world, body ); + + b3CastOutput output = b3ShapeCastShape( shape, transform, input ); + if ( output.fraction == 0.0f ) + { + // Ignore overlapping shapes + return worldContext->fraction; + } + + worldContext->fraction = output.fraction; + return output.fraction; +} + +float b3World_CastMover( b3WorldId worldId, const b3Capsule* mover, b3Vec3 translation, b3QueryFilter filter, + b3MoverFilterFcn* fcn, void* context ) +{ + B3_ASSERT( b3IsValidVec3( translation ) ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return 1.0f; + } + + b3ShapeCastInput input; + input.proxy = (b3ShapeProxy){ &mover->center1, 2, mover->radius }; + input.translation = translation; + input.maxFraction = 1.0f; + input.canEncroach = mover->radius > 0.0f; + + WorldMoverCastContext worldContext = { + .world = world, + .fcn = fcn, + .filter = filter, + .fraction = 1.0f, + .userContext = context, + }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3DynamicTree_ShapeCast( world->broadPhase.trees + i, &input, filter.maskBits, false, MoverCastCallback, &worldContext ); + + if ( worldContext.fraction == 0.0f ) + { + return 0.0f; + } + + input.maxFraction = worldContext.fraction; + } + + return worldContext.fraction; +} + +void b3World_SetCustomFilterCallback( b3WorldId worldId, b3CustomFilterFcn* fcn, void* context ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + world->customFilterFcn = fcn; + world->customFilterContext = context; +} + +void b3World_SetPreSolveCallback( b3WorldId worldId, b3PreSolveFcn* fcn, void* context ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + world->preSolveFcn = fcn; + world->preSolveContext = context; +} + +void b3World_SetGravity( b3WorldId worldId, b3Vec3 gravity ) +{ + b3World* world = b3GetWorldFromId( worldId ); + world->gravity = gravity; +} + +b3Vec3 b3World_GetGravity( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->gravity; +} + +typedef struct ExplosionContext +{ + b3World* world; + b3Vec3 position; + float radius; + float falloff; + float impulsePerArea; +} ExplosionContext; + +static bool ExplosionCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + ExplosionContext* explosionContext = (ExplosionContext*)context; + b3World* world = explosionContext->world; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + if ( shape->explosionScale == 0.0f ) + { + return true; + } + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + B3_ASSERT( body->type == b3_dynamicBody ); + + b3Transform transform = b3GetBodyTransformQuick( world, body ); + + b3DistanceInput input; + input.proxyA = b3MakeShapeProxy( shape ); + input.proxyB = (b3ShapeProxy){ &explosionContext->position, 1, 0.0f }; + input.transformA = transform; + input.transformB = b3Transform_identity; + input.useRadii = true; + + b3SimplexCache cache = { 0 }; + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + + float radius = explosionContext->radius; + float falloff = explosionContext->falloff; + if ( output.distance > radius + falloff ) + { + return true; + } + + b3WakeBody( world, body ); + + if ( body->setIndex != b3_awakeSet ) + { + return true; + } + + b3Vec3 closestPoint = output.pointA; + if ( output.distance == 0.0f ) + { + b3Vec3 localCentroid = b3GetShapeCentroid( shape ); + closestPoint = b3TransformPoint( transform, localCentroid ); + } + + b3Vec3 direction = b3Sub( closestPoint, explosionContext->position ); + if ( b3LengthSquared( direction ) > 100.0f * FLT_EPSILON * FLT_EPSILON ) + { + direction = b3Normalize( direction ); + } + else + { + direction = (b3Vec3){ 1.0f, 0.0f, 0.0f }; + } + + float area = b3GetShapeProjectedArea( shape, direction ); + float scale = 1.0f; + if ( output.distance > radius && falloff > 0.0f ) + { + scale = b3ClampFloat( ( radius + falloff - output.distance ) / falloff, 0.0f, 1.0f ); + } + + float magnitude = explosionContext->impulsePerArea * area * scale * shape->explosionScale; + b3Vec3 impulse = b3MulSV( magnitude, direction ); + + int localIndex = body->localIndex; + b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + b3BodyState* state = b3Array_Get( set->bodyStates, localIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, localIndex ); + state->linearVelocity = b3MulAdd( state->linearVelocity, bodySim->invMass, impulse ); + state->angularVelocity = b3Add( + state->angularVelocity, b3MulMV( bodySim->invInertiaWorld, b3Cross( b3Sub( closestPoint, bodySim->center ), impulse ) ) ); + + return true; +} + +void b3World_Explode( b3WorldId worldId, const b3ExplosionDef* explosionDef ) +{ + uint64_t maskBits = explosionDef->maskBits; + b3Vec3 position = explosionDef->position; + float radius = explosionDef->radius; + float falloff = explosionDef->falloff; + float impulsePerArea = explosionDef->impulsePerArea; + + B3_ASSERT( b3IsValidVec3( position ) ); + B3_ASSERT( b3IsValidFloat( radius ) && radius >= 0.0f ); + B3_ASSERT( b3IsValidFloat( falloff ) && falloff >= 0.0f ); + B3_ASSERT( b3IsValidFloat( impulsePerArea ) ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + // Locked due to waking + world->locked = true; + + struct ExplosionContext explosionContext = { world, position, radius, falloff, impulsePerArea }; + + float totalRadius = radius + falloff; + b3Vec3 r = { totalRadius, totalRadius, totalRadius }; + b3AABB aabb = { b3Sub( position, r ), b3Add( position, r ) }; + + b3DynamicTree_Query( world->broadPhase.trees + b3_dynamicBody, aabb, maskBits, false, ExplosionCallback, &explosionContext ); + + world->locked = false; +} + +void b3World_RebuildStaticTree( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + b3DynamicTree* staticTree = world->broadPhase.trees + b3_staticBody; + b3DynamicTree_Rebuild( staticTree, true ); +} + +void b3World_EnableSpeculative( b3WorldId worldId, bool flag ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + world->enableSpeculative = flag; +} + +#if B3_ENABLE_VALIDATION +// This validates island graph connectivity for each body +void b3ValidateConnectivity( b3World* world ) +{ + b3Body* bodies = world->bodies.data; + int bodyCapacity = world->bodies.count; + + for ( int bodyIndex = 0; bodyIndex < bodyCapacity; ++bodyIndex ) + { + b3Body* body = bodies + bodyIndex; + if ( body->id == B3_NULL_INDEX ) + { + b3ValidateFreeId( &world->bodyIdPool, bodyIndex ); + continue; + } + + B3_ASSERT( bodyIndex == body->id ); + + // Need to get the root island because islands are not merged until the next time step + int bodyIslandId = body->islandId; + int bodySetIndex = body->setIndex; + + int contactKey = body->headContactKey; + while ( contactKey != B3_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + + bool touching = ( contact->flags & b3_contactTouchingFlag ) != 0; + if ( touching ) + { + if ( bodySetIndex != b3_staticSet ) + { + int contactIslandId = contact->islandId; + B3_ASSERT( contactIslandId == bodyIslandId ); + } + } + else + { + B3_ASSERT( contact->islandId == B3_NULL_INDEX ); + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + + int jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + + int otherEdgeIndex = edgeIndex ^ 1; + + b3Body* otherBody = b3Array_Get( world->bodies, joint->edges[otherEdgeIndex].bodyId ); + + if ( bodySetIndex == b3_disabledSet || otherBody->setIndex == b3_disabledSet ) + { + B3_ASSERT( joint->islandId == B3_NULL_INDEX ); + } + else if ( bodySetIndex == b3_staticSet ) + { + // Intentional nesting + if ( otherBody->setIndex == b3_staticSet ) + { + B3_ASSERT( joint->islandId == B3_NULL_INDEX ); + } + } + else if ( body->type != b3_dynamicBody && otherBody->type != b3_dynamicBody ) + { + B3_ASSERT( joint->islandId == B3_NULL_INDEX ); + } + else + { + int jointIslandId = joint->islandId; + B3_ASSERT( jointIslandId == bodyIslandId ); + } + + jointKey = joint->edges[edgeIndex].nextKey; + } + } +} + +// Validates solver sets, but not island connectivity +void b3ValidateSolverSets( b3World* world ) +{ + B3_ASSERT( b3GetIdCapacity( &world->bodyIdPool ) == world->bodies.count ); + B3_ASSERT( b3GetIdCapacity( &world->contactIdPool ) == world->contacts.count ); + B3_ASSERT( b3GetIdCapacity( &world->jointIdPool ) == world->joints.count ); + B3_ASSERT( b3GetIdCapacity( &world->islandIdPool ) == world->islands.count ); + B3_ASSERT( b3GetIdCapacity( &world->solverSetIdPool ) == world->solverSets.count ); + + int activeSetCount = 0; + int totalBodyCount = 0; + int totalJointCount = 0; + int totalContactCount = 0; + int totalIslandCount = 0; + + // Validate all solver sets + int setCount = world->solverSets.count; + for ( int setIndex = 0; setIndex < setCount; ++setIndex ) + { + b3SolverSet* set = world->solverSets.data + setIndex; + if ( set->setIndex != B3_NULL_INDEX ) + { + activeSetCount += 1; + + if ( setIndex == b3_staticSet ) + { + B3_ASSERT( set->contactIndices.count == 0 ); + B3_ASSERT( set->islandSims.count == 0 ); + B3_ASSERT( set->bodyStates.count == 0 ); + } + else if ( setIndex == b3_disabledSet ) + { + B3_ASSERT( set->islandSims.count == 0 ); + B3_ASSERT( set->bodyStates.count == 0 ); + } + else if ( setIndex == b3_awakeSet ) + { + B3_ASSERT( set->bodySims.count == set->bodyStates.count ); + B3_ASSERT( set->jointSims.count == 0 ); + } + else + { + B3_ASSERT( set->bodyStates.count == 0 ); + } + + // Validate bodies + { + b3Body* bodies = world->bodies.data; + B3_ASSERT( set->bodySims.count >= 0 ); + totalBodyCount += set->bodySims.count; + for ( int i = 0; i < set->bodySims.count; ++i ) + { + b3BodySim* bodySim = set->bodySims.data + i; + + int bodyId = bodySim->bodyId; + B3_ASSERT( 0 <= bodyId && bodyId < world->bodies.count ); + b3Body* body = bodies + bodyId; + B3_ASSERT( body->setIndex == setIndex ); + B3_ASSERT( body->localIndex == i ); + B3_ASSERT( body->generation == body->generation ); + + if ( body->type == b3_dynamicBody ) + { + B3_ASSERT( body->flags & b3_dynamicFlag ); + } + + if ( setIndex == b3_disabledSet ) + { + B3_ASSERT( body->headContactKey == B3_NULL_INDEX ); + } + + // Validate body shapes + int prevShapeId = B3_NULL_INDEX; + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + B3_ASSERT( shape->id == shapeId ); + B3_ASSERT( shape->prevShapeId == prevShapeId ); + + if ( setIndex == b3_disabledSet ) + { + B3_ASSERT( shape->proxyKey == B3_NULL_INDEX ); + } + else if ( setIndex == b3_staticSet ) + { + B3_ASSERT( B3_PROXY_TYPE( shape->proxyKey ) == b3_staticBody ); + } + else + { + b3BodyType proxyType = B3_PROXY_TYPE( shape->proxyKey ); + B3_ASSERT( proxyType == b3_kinematicBody || proxyType == b3_dynamicBody ); + } + + prevShapeId = shapeId; + shapeId = shape->nextShapeId; + } + + // Validate body contacts + int contactKey = body->headContactKey; + while ( contactKey != B3_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + B3_ASSERT( contact->setIndex != b3_staticSet ); + B3_ASSERT( contact->edges[0].bodyId == bodyId || contact->edges[1].bodyId == bodyId ); + contactKey = contact->edges[edgeIndex].nextKey; + } + + // Validate body joints + int jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + + int otherEdgeIndex = edgeIndex ^ 1; + + b3Body* otherBody = b3Array_Get( world->bodies, joint->edges[otherEdgeIndex].bodyId ); + + if ( setIndex == b3_disabledSet || otherBody->setIndex == b3_disabledSet ) + { + B3_ASSERT( joint->setIndex == b3_disabledSet ); + } + else if ( setIndex == b3_staticSet && otherBody->setIndex == b3_staticSet ) + { + B3_ASSERT( joint->setIndex == b3_staticSet ); + } + else if ( body->type != b3_dynamicBody && otherBody->type != b3_dynamicBody ) + { + B3_ASSERT( joint->setIndex == b3_staticSet ); + } + else if ( setIndex == b3_awakeSet ) + { + B3_ASSERT( joint->setIndex == b3_awakeSet ); + } + else if ( setIndex >= b3_firstSleepingSet ) + { + B3_ASSERT( joint->setIndex == setIndex ); + } + + b3JointSim* jointSim = b3GetJointSim( world, joint ); + B3_ASSERT( jointSim->jointId == jointId ); + B3_ASSERT( jointSim->bodyIdA == joint->edges[0].bodyId ); + B3_ASSERT( jointSim->bodyIdB == joint->edges[1].bodyId ); + + jointKey = joint->edges[edgeIndex].nextKey; + } + } + } + + // Validate contacts + { + B3_ASSERT( set->contactIndices.count >= 0 ); + totalContactCount += set->contactIndices.count; + for ( int i = 0; i < set->contactIndices.count; ++i ) + { + int contactIndex = set->contactIndices.data[i]; + b3Contact* contact = b3Array_Get( world->contacts, contactIndex ); + if ( setIndex == b3_awakeSet ) + { + // contact should be non-touching if awake + // or it could be this contact hasn't been transferred yet + B3_ASSERT( contact->manifoldCount == 0 || ( contact->flags & b3_simStartedTouching ) != 0 ); + } + B3_ASSERT( contact->setIndex == setIndex ); + B3_ASSERT( contact->colorIndex == B3_NULL_INDEX ); + B3_ASSERT( contact->localIndex == i ); + } + } + + // Validate joints + { + B3_ASSERT( set->jointSims.count >= 0 ); + totalJointCount += set->jointSims.count; + for ( int i = 0; i < set->jointSims.count; ++i ) + { + b3JointSim* jointSim = set->jointSims.data + i; + b3Joint* joint = b3Array_Get( world->joints, jointSim->jointId ); + B3_ASSERT( joint->setIndex == setIndex ); + B3_ASSERT( joint->colorIndex == B3_NULL_INDEX ); + B3_ASSERT( joint->localIndex == i ); + } + } + + // Validate islands + { + B3_ASSERT( set->islandSims.count >= 0 ); + totalIslandCount += set->islandSims.count; + for ( int i = 0; i < set->islandSims.count; ++i ) + { + b3IslandSim* islandSim = set->islandSims.data + i; + b3Island* island = b3Array_Get( world->islands, islandSim->islandId ); + B3_ASSERT( island->setIndex == setIndex ); + B3_ASSERT( island->localIndex == i ); + } + } + } + else + { + B3_ASSERT( set->bodySims.count == 0 ); + B3_ASSERT( set->contactIndices.count == 0 ); + B3_ASSERT( set->jointSims.count == 0 ); + B3_ASSERT( set->islandSims.count == 0 ); + B3_ASSERT( set->bodyStates.count == 0 ); + } + } + + int setIdCount = b3GetIdCount( &world->solverSetIdPool ); + B3_ASSERT( activeSetCount == setIdCount ); + + int bodyIdCount = b3GetIdCount( &world->bodyIdPool ); + B3_ASSERT( totalBodyCount == bodyIdCount ); + + int islandIdCount = b3GetIdCount( &world->islandIdPool ); + B3_ASSERT( totalIslandCount == islandIdCount ); + + // Validate constraint graph + for ( int colorIndex = 0; colorIndex < B3_GRAPH_COLOR_COUNT; ++colorIndex ) + { + b3GraphColor* color = world->constraintGraph.colors + colorIndex; + int bitCount = 0; + + B3_ASSERT( color->convexContacts.count >= 0 ); + totalContactCount += color->convexContacts.count; + for ( int i = 0; i < color->convexContacts.count; ++i ) + { + int contactId = color->convexContacts.data[i]; + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + // contact should be touching in the constraint graph or awaiting transfer to non-touching + B3_ASSERT( contact->manifoldCount > 0 || ( contact->flags & ( b3_simStoppedTouching | b3_simDisjoint ) ) != 0 ); + B3_ASSERT( contact->setIndex == b3_awakeSet ); + B3_ASSERT( contact->colorIndex == colorIndex ); + B3_ASSERT( contact->localIndex == i ); + + int bodyIdA = contact->edges[0].bodyId; + int bodyIdB = contact->edges[1].bodyId; + + if ( colorIndex < B3_OVERFLOW_INDEX ) + { + b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB ); + B3_ASSERT( b3GetBit( &color->bodySet, bodyIdA ) == ( bodyA->type == b3_dynamicBody ) ); + B3_ASSERT( b3GetBit( &color->bodySet, bodyIdB ) == ( bodyB->type == b3_dynamicBody ) ); + + bitCount += bodyA->type == b3_dynamicBody ? 1 : 0; + bitCount += bodyB->type == b3_dynamicBody ? 1 : 0; + } + } + + totalContactCount += color->contacts.count; + for ( int i = 0; i < color->contacts.count; ++i ) + { + int contactId = color->contacts.data[i].contactId; + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + // contact should be touching in the constraint graph or awaiting transfer to non-touching + B3_ASSERT( contact->manifoldCount > 0 || ( contact->flags & ( b3_simStoppedTouching | b3_simDisjoint ) ) != 0 ); + B3_ASSERT( contact->setIndex == b3_awakeSet ); + B3_ASSERT( contact->colorIndex == colorIndex ); + B3_ASSERT( contact->localIndex == i ); + + int bodyIdA = contact->edges[0].bodyId; + int bodyIdB = contact->edges[1].bodyId; + + if ( colorIndex < B3_OVERFLOW_INDEX ) + { + b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB ); + B3_ASSERT( b3GetBit( &color->bodySet, bodyIdA ) == ( bodyA->type == b3_dynamicBody ) ); + B3_ASSERT( b3GetBit( &color->bodySet, bodyIdB ) == ( bodyB->type == b3_dynamicBody ) ); + + bitCount += bodyA->type == b3_dynamicBody ? 1 : 0; + bitCount += bodyB->type == b3_dynamicBody ? 1 : 0; + } + } + + B3_ASSERT( color->jointSims.count >= 0 ); + totalJointCount += color->jointSims.count; + for ( int i = 0; i < color->jointSims.count; ++i ) + { + b3JointSim* jointSim = color->jointSims.data + i; + b3Joint* joint = b3Array_Get( world->joints, jointSim->jointId ); + B3_ASSERT( joint->setIndex == b3_awakeSet ); + B3_ASSERT( joint->colorIndex == colorIndex ); + B3_ASSERT( joint->localIndex == i ); + + int bodyIdA = joint->edges[0].bodyId; + int bodyIdB = joint->edges[1].bodyId; + + if ( colorIndex < B3_OVERFLOW_INDEX ) + { + b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB ); + B3_ASSERT( b3GetBit( &color->bodySet, bodyIdA ) == ( bodyA->type == b3_dynamicBody ) ); + B3_ASSERT( b3GetBit( &color->bodySet, bodyIdB ) == ( bodyB->type == b3_dynamicBody ) ); + + bitCount += bodyA->type == b3_dynamicBody ? 1 : 0; + bitCount += bodyB->type == b3_dynamicBody ? 1 : 0; + } + } + + // Validate the bit population for this graph color + B3_ASSERT( bitCount == b3CountSetBits( &color->bodySet ) ); + } + + int contactIdCount = b3GetIdCount( &world->contactIdPool ); + B3_ASSERT( totalContactCount == contactIdCount ); + B3_ASSERT( totalContactCount == (int)world->broadPhase.pairSet.count ); + + int jointIdCount = b3GetIdCount( &world->jointIdPool ); + B3_ASSERT( totalJointCount == jointIdCount ); + +// Validate shapes +// This is very slow on compounds +#if 0 + int shapeCapacity = b3Array(world->shapeArray).count; + for (int shapeIndex = 0; shapeIndex < shapeCapacity; shapeIndex += 1) + { + b3Shape* shape = world->shapeArray + shapeIndex; + if (shape->id != shapeIndex) + { + continue; + } + + B3_ASSERT(0 <= shape->bodyId && shape->bodyId < b3Array(world->bodyArray).count); + + b3Body* body = world->bodyArray + shape->bodyId; + B3_ASSERT(0 <= body->setIndex && body->setIndex < b3Array(world->solverSetArray).count); + + b3SolverSet* set = world->solverSetArray + body->setIndex; + B3_ASSERT(0 <= body->localIndex && body->localIndex < set->sims.count); + + b3BodySim* bodySim = set->sims.mData + body->localIndex; + B3_ASSERT(bodySim->bodyId == shape->bodyId); + + bool found = false; + int shapeCount = 0; + int index = body->headShapeId; + while (index != B3_NULL_INDEX) + { + b3CheckId(world->shapeArray, index); + b3Shape* s = world->shapeArray + index; + if (index == shapeIndex) + { + found = true; + } + + index = s->nextShapeId; + shapeCount += 1; + } + + B3_ASSERT(found); + B3_ASSERT(shapeCount == body->shapeCount); + } +#endif +} + +// Validate contact touching status. +void b3ValidateContacts( b3World* world ) +{ + b3ConstraintGraph* graph = &world->constraintGraph; + int contactCount = world->contacts.count; + B3_ASSERT( contactCount == b3GetIdCapacity( &world->contactIdPool ) ); + int allocatedContactCount = 0; + + for ( int contactIndex = 0; contactIndex < contactCount; ++contactIndex ) + { + b3Contact* contact = b3Array_Get( world->contacts, contactIndex ); + if ( contact->contactId == B3_NULL_INDEX ) + { + continue; + } + + B3_ASSERT( contact->contactId == contactIndex ); + + allocatedContactCount += 1; + + bool touching = ( contact->flags & b3_contactTouchingFlag ) != 0; + + int setId = contact->setIndex; + b3SolverSet* set = b3Array_Get( world->solverSets, setId ); + + if ( setId == b3_awakeSet ) + { + if ( touching ) + { + B3_ASSERT( 0 <= contact->colorIndex && contact->colorIndex < B3_GRAPH_COLOR_COUNT ); + // Validate body sim indices + b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); + b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB ); + + b3Body* bodyA = b3Array_Get( world->bodies, shapeA->bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, shapeB->bodyId ); + + if ( bodyA->type == b3_staticBody ) + { + B3_ASSERT( contact->bodySimIndexA == B3_NULL_INDEX ); + } + else + { + B3_ASSERT( contact->bodySimIndexA == bodyA->localIndex ); + } + + if ( bodyB->type == b3_staticBody ) + { + B3_ASSERT( contact->bodySimIndexB == B3_NULL_INDEX ); + } + else + { + B3_ASSERT( contact->bodySimIndexB == bodyB->localIndex ); + } + + if ( ( contact->flags & b3_simMeshContact ) != 0 || contact->colorIndex == B3_OVERFLOW_INDEX ) + { + b3GraphColor* color = graph->colors + contact->colorIndex; + int contactId = b3Array_Get( color->contacts, contact->localIndex )->contactId; + B3_ASSERT( contactId == contactIndex ); + } + else + { + b3GraphColor* color = graph->colors + contact->colorIndex; + int contactId = *b3Array_Get( color->convexContacts, contact->localIndex ); + B3_ASSERT( contactId == contactIndex ); + } + } + else + { + B3_ASSERT( contact->colorIndex == B3_NULL_INDEX ); + B3_ASSERT( contact->manifolds == NULL ); + B3_ASSERT( contact->manifoldCount == 0 ); + + int* index = b3Array_Get( set->contactIndices, contact->localIndex ); + B3_ASSERT( *index == contactIndex ); + } + } + else if ( setId >= b3_firstSleepingSet ) + { + // Only touching contacts allowed in a sleeping set + B3_ASSERT( touching == true ); + B3_ASSERT( contact->manifolds != NULL ); + B3_ASSERT( contact->manifoldCount > 0 ); + int* index = b3Array_Get( set->contactIndices, contact->localIndex ); + B3_ASSERT( *index == contactIndex ); + } + else + { + // Sleeping and non-touching contacts belong in the disabled set + B3_ASSERT( touching == false && setId == b3_disabledSet ); + B3_ASSERT( contact->manifolds == NULL ); + B3_ASSERT( contact->manifoldCount == 0 ); + int* index = b3Array_Get( set->contactIndices, contact->localIndex ); + B3_ASSERT( *index == contactIndex ); + } + + if ( contact->flags & b3_simMeshContact ) + { + int cacheCount = contact->meshContact.triangleCache.count; + if ( cacheCount > 0 ) + { + B3_ASSERT( contact->meshContact.triangleCache.data != NULL ); + B3_ASSERT( contact->meshContact.triangleCache.capacity >= cacheCount ); + + b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); + if ( shapeA->type == b3_meshShape ) + { + int triangleCount = shapeA->mesh.data->triangleCount; + for ( int i = 0; i < cacheCount; ++i ) + { + int triangleIndex = contact->meshContact.triangleCache.data[i].triangleIndex; + B3_ASSERT( 0 <= triangleIndex && triangleIndex < triangleCount ); + } + } + else if ( shapeA->type == b3_heightShape ) + { + int triangleCount = b3GetHeightFieldTriangleCount( shapeA->heightField ); + for ( int i = 0; i < cacheCount; ++i ) + { + int triangleIndex = contact->meshContact.triangleCache.data[i].triangleIndex; + B3_ASSERT( 0 <= triangleIndex && triangleIndex < triangleCount ); + } + } + else + { + B3_ASSERT( shapeA->type == b3_compoundShape ); + b3ChildShape child = b3GetCompoundChild( shapeA->compound, contact->childIndex ); + B3_ASSERT( child.type == b3_meshShape ); + + int triangleCount = child.mesh.data->triangleCount; + for ( int i = 0; i < cacheCount; ++i ) + { + int triangleIndex = contact->meshContact.triangleCache.data[i].triangleIndex; + B3_ASSERT( 0 <= triangleIndex && triangleIndex < triangleCount ); + } + } + } + } + } + + int contactIdCount = b3GetIdCount( &world->contactIdPool ); + B3_ASSERT( allocatedContactCount == contactIdCount ); +} + +#else + +void b3ValidateConnectivity( b3World* world ) +{ + B3_UNUSED( world ); +} + +void b3ValidateSolverSets( b3World* world ) +{ + B3_UNUSED( world ); +} + +void b3ValidateContacts( b3World* world ) +{ + B3_UNUSED( world ); +} + +#endif + +#define NAME int_set +#define KEY_TY int +#define MALLOC_FN b3Alloc +#define FREE_FN b3Free +#define HASH_FN vt_hash_integer +#define CMPR_FN vt_cmpr_integer +#include "verstable.h" + +void b3World_DumpAwake( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + if ( world->locked == true ) + { + return; + } + + b3OpenDump( "box3d_dump.inl" ); + + b3Vec3 g = world->gravity; + b3Dump( "b3Vec3 gravity = {%.9g, %.9g, %.9g};\n", g.x, g.y, g.z ); + b3Dump( "b3World_SetGravity(m_worldId, gravity);\n" ); + + b3Dump( "std::vector bodies;\n" ); + b3Dump( "std::vector joints;\n\n" ); + + int_set staticBodies; + int_set_init( &staticBodies ); + + b3Body* bodies = world->bodies.data; + for ( int i = 0; i < world->bodies.count; ++i ) + { + b3Body* body = bodies + i; + + if ( body->id == B3_NULL_INDEX ) + { + continue; + } + + if ( body->setIndex != b3_awakeSet ) + { + continue; + } + + b3DumpBody( world, body ); + + // Gather + int edgeKey = body->headContactKey; + while ( edgeKey != B3_NULL_INDEX ) + { + int contactId = edgeKey >> 1; + int edgeIndex = edgeKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + edgeKey = contact->edges[edgeIndex].nextKey; + + if ( ( contact->flags & b3_contactTouchingFlag ) == 0 ) + { + continue; + } + + int otherIndex = 1 - edgeIndex; + b3Body* otherBody = b3Array_Get( world->bodies, contact->edges[otherIndex].bodyId ); + if ( otherBody->setIndex == b3_staticSet ) + { + (void)int_set_insert( &staticBodies, otherBody->id ); + } + } + } + + // Dump the bodies that are touching + for ( int_set_itr itr = int_set_first( &staticBodies ); int_set_is_end( itr ) == false; itr = int_set_next( itr ) ) + { + b3Body* body = b3Array_Get( world->bodies, itr.data->key ); + b3DumpBody( world, body ); + } + + int_set_cleanup( &staticBodies ); + + b3CloseDump(); +} + +void b3World_Dump( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + + if ( world->locked == true ) + { + return; + } + + b3OpenDump( "box3d_dump.inl" ); + + b3Vec3 g = world->gravity; + b3Dump( "b3Vec3 gravity = {%.9g, %.9g, %.9g};\n", g.x, g.y, g.z ); + b3Dump( "b3World_SetGravity(m_worldId, gravity);\n" ); + + b3Dump( "std::vector bodies;\n" ); + b3Dump( "std::vector joints;\n\n" ); + + b3Body* bodies = world->bodies.data; + for ( int i = 0; i < world->bodies.count; ++i ) + { + b3Body* body = bodies + i; + + if ( body->id == B3_NULL_INDEX ) + { + continue; + } + + if ( body->setIndex != b3_awakeSet ) + { + continue; + } + + b3DumpBody( world, body ); + } + + b3CloseDump(); +} diff --git a/src/physics_world.h b/src/physics_world.h new file mode 100644 index 0000000..c964d2f --- /dev/null +++ b/src/physics_world.h @@ -0,0 +1,333 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "arena_allocator.h" +#include "bitset.h" +#include "block_allocator.h" +#include "broad_phase.h" +#include "constraint_graph.h" +#include "id_pool.h" + +#include "box3d/types.h" + +#define B3_DEBUG_POINT_CAPACITY 64 +#define B3_DEBUG_LINE_CAPACITY 64 + +typedef struct b3Body b3Body; +typedef struct b3Contact b3Contact; +typedef struct b3Island b3Island; +typedef struct b3Joint b3Joint; +typedef struct b3Sensor b3Sensor; +typedef struct b3SensorTaskContext b3SensorTaskContext; +typedef struct b3SensorHit b3SensorHit; +typedef struct b3Shape b3Shape; +typedef struct b3SolverSet b3SolverSet; + +b3DeclareArray( b3BlockAllocator ); +b3DeclareArray( b3Body ); +b3DeclareArray( b3SolverSet ); +b3DeclareArray( b3Joint ); +b3DeclareArray( b3Contact ); +b3DeclareArray( b3Island ); +b3DeclareArray( b3Shape ); +b3DeclareArray( b3Sensor ); +b3DeclareArray( b3SensorTaskContext ); +b3DeclareArray( b3SensorHit ); +b3DeclareArray( b3BodyMoveEvent ); +b3DeclareArray( b3SensorBeginTouchEvent ); +b3DeclareArray( b3ContactBeginTouchEvent ); +b3DeclareArray( b3SensorEndTouchEvent ); +b3DeclareArray( b3ContactEndTouchEvent ); +b3DeclareArray( b3ContactHitEvent ); +b3DeclareArray( b3JointEvent ); + +enum b3SetType +{ + b3_staticSet = 0, + b3_disabledSet = 1, + b3_awakeSet = 2, + b3_firstSleepingSet = 3, +}; + +typedef struct b3DebugPoint +{ + b3Vec3 p; + int label; + float value; + b3HexColor color; +} b3DebugPoint; + +typedef struct b3DebugLine +{ + b3Vec3 p1, p2; + int label; + b3HexColor color; +} b3DebugLine; + +// Per thread task storage +typedef struct b3TaskContext +{ + b3Arena arena; + + // Collect per thread sensor continuous hit events. + b3ArrayC( b3SensorHit ) sensorHits; + + // These bits align with the b3ConstraintGraph::contactBlocks and signal a change in contact status + b3BitSet contactStateBitSet; + + // These bits align with the joint id capacity and signal a change in contact status + b3BitSet jointStateBitSet; + + // These bits align with the contact id capacity and signal a hit event. + b3BitSet hitEventBitSet; + + // Fast-path flag: true when this worker set at least one bit in hitEventBitSet this step. + bool hasHitEvents; + + // Used to track bodies with shapes that have enlarged AABBs. This avoids having a bit array + // that is very large when there are many static shapes. + b3BitSet enlargedSimBitSet; + + // Used to put islands to sleep + b3BitSet awakeIslandBitSet; + + // Per worker split island candidate + float splitSleepTime; + int splitIslandId; + + // Profiling + int satCallCount; + int satCacheHitCount; + + b3DebugPoint points[B3_DEBUG_POINT_CAPACITY]; + int pointCount; + + b3DebugLine lines[B3_DEBUG_LINE_CAPACITY]; + int lineCount; + + int manifoldCounts[B3_CONTACT_MANIFOLD_COUNT_BUCKETS]; + + // Number of contacts recycled this step (collide pass). + int recycledContactCount; + + // Prevent false sharing + char cacheLine[64]; +} b3TaskContext; + +b3DeclareArray( b3TaskContext ); + +// The world struct manages all physics entities, dynamic simulation, and asynchronous queries. +// The world also contains efficient memory management facilities. +typedef struct b3World +{ + b3Stack stack; + b3BroadPhase broadPhase; + b3ConstraintGraph constraintGraph; + + // Manifold allocators have one allocator for each manifold count. + b3ArrayC( b3BlockAllocator ) manifoldAllocators; + b3Mutex* manifoldAllocatorMutex; + + // The body id pool is used to allocate and recycle body ids. Body ids + // provide a stable identifier for users, but incur caches misses when used + // to access body data. Aligns with b3Body. + b3IdPool bodyIdPool; + + // This is a sparse array that maps body ids to the body data + // stored in solver sets. As sims move within a set or across set. + // Indices come from id pool. + b3ArrayC( b3Body ) bodies; + + // Provides free list for solver sets. + b3IdPool solverSetIdPool; + + // Solvers sets allow sims to be stored in contiguous arrays. The first + // set is all static sims. The second set is active sims. The third set is disabled + // sims. The remaining sets are sleeping islands. + b3ArrayC( b3SolverSet ) solverSets; + + // Used to create stable ids for joints + b3IdPool jointIdPool; + + // This is a sparse array that maps joint ids to the joint data stored in the constraint graph + // or in the solver sets. + b3ArrayC( b3Joint ) joints; + + // Used to create stable ids for contacts + b3IdPool contactIdPool; + + // This is a sparse array that maps contact ids to the contact data stored in the constraint graph + // or in the solver sets. + b3ArrayC( b3Contact ) contacts; + + // Used to create stable ids for islands + b3IdPool islandIdPool; + + // This is a sparse array that maps island ids to the island data stored in the solver sets. + b3ArrayC( b3Island ) islands; + + b3IdPool shapeIdPool; + + // These are sparse arrays that point into the pools above + b3ArrayC( b3Shape ) shapes; + + // This is a dense array of sensor data. + b3ArrayC( b3Sensor ) sensors; + + // Per thread storage + b3ArrayC( b3TaskContext ) taskContexts; + b3ArrayC( b3SensorTaskContext ) sensorTaskContexts; + + b3ArrayC( b3BodyMoveEvent ) bodyMoveEvents; + b3ArrayC( b3SensorBeginTouchEvent ) sensorBeginEvents; + b3ArrayC( b3ContactBeginTouchEvent ) contactBeginEvents; + + // End events are double buffered so that the user doesn't need to flush events + b3ArrayC( b3SensorEndTouchEvent ) sensorEndEvents[2]; + b3ArrayC( b3ContactEndTouchEvent ) contactEndEvents[2]; + int endEventArrayIndex; + + b3ArrayC( b3ContactHitEvent ) contactHitEvents; + b3ArrayC( b3JointEvent ) jointEvents; + + // Used to track debug draw + b3BitSet debugBodySet; + b3BitSet debugJointSet; + b3BitSet debugContactSet; + b3BitSet debugIslandSet; + b3CreateDebugShapeCallback* createDebugShape; + b3DestroyDebugShapeCallback* destroyDebugShape; + void* userDebugShapeContext; + + // Id that is incremented every time step + uint64_t stepIndex; + + // Identify islands for splitting as follows: + // - I want to split islands so smaller islands can sleep + // - when a body comes to rest and its sleep timer trips, I can look at the island and flag it for splitting + // if it has removed constraints + // - islands that have removed constraints must be put split first because I don't want to wake bodies incorrectly + // - otherwise I can use the awake islands that have bodies wanting to sleep as the splitting candidates + // - if no bodies want to sleep then there is no reason to perform island splitting + int splitIslandId; + + b3Vec3 gravity; + float hitEventThreshold; + float restitutionThreshold; + float maxLinearSpeed; + float contactSpeed; + float contactHertz; + float contactDampingRatio; + float contactRecycleDistance; + + b3FrictionCallback* frictionCallback; + b3RestitutionCallback* restitutionCallback; + + uint16_t generation; + + b3Profile profile; + int satCallCount; + int satCacheHitCount; + int manifoldCounts[B3_CONTACT_MANIFOLD_COUNT_BUCKETS]; + + b3Capacity maxCapacity; + + b3PreSolveFcn* preSolveFcn; + void* preSolveContext; + + b3CustomFilterFcn* customFilterFcn; + void* customFilterContext; + + int workerCount; + b3EnqueueTaskCallback* enqueueTaskFcn; + b3FinishTaskCallback* finishTaskFcn; + void* userTaskContext; + void* userTreeTask; + + struct b3Scheduler* scheduler; + + void* userData; + + // latest inverse sub-step + float inv_h; + + // latest inverse full-step + float inv_dt; + + int activeTaskCount; + int taskCount; + + uint16_t worldId; + + bool enableSleep; + + // This indicates there is a world write operation in progress. This is for debugging and + // not a real mutex. This should have minimal performance impact. + bool locked; + + bool enableWarmStarting; + bool enableContinuous; + bool enableSpeculative; + bool inUse; +} b3World; + +#ifdef __cplusplus +extern "C" +{ +#endif + +b3World* b3GetUnlockedWorldFromId( b3WorldId id ); +b3World* b3GetWorldFromId( b3WorldId id ); + +b3World* b3GetUnlockedWorld( int index ); +b3World* b3GetWorld( int index ); + +void b3ValidateConnectivity( b3World* world ); +void b3ValidateSolverSets( b3World* world ); +void b3ValidateContacts( b3World* world ); + +#ifdef __cplusplus +} +#endif + +static inline b3Manifold* b3AllocateManifolds( b3World* world, int count ) +{ + if ( count == 0 ) + { + return NULL; + } + + int index = count - 1; + + // Need lock because this is called from the parallel narrow phase + b3LockMutex( world->manifoldAllocatorMutex ); + int currentCount = world->manifoldAllocators.count; + for ( int i = currentCount; i < count; ++i ) + { + b3BlockAllocator allocator = b3CreateBlockAllocator( ( i + 1 ) * sizeof( b3Manifold ), 2 * B3_BLOCK_SIZE ); + b3Array_Push( world->manifoldAllocators, allocator ); + } + + b3BlockAllocator* allocator = b3Array_Get( world->manifoldAllocators, index ); + b3Manifold* manifolds = (b3Manifold*)b3AllocateElement( allocator ); + b3UnlockMutex( world->manifoldAllocatorMutex ); + memset( manifolds, 0, count * sizeof( b3Manifold ) ); + return manifolds; +} + +static inline void b3FreeManifolds( b3World* world, b3Manifold* manifolds, int count ) +{ + if ( count == 0 ) + { + return; + } + + int index = count - 1; + b3LockMutex( world->manifoldAllocatorMutex ); + b3BlockAllocator* allocator = b3Array_Get( world->manifoldAllocators, index ); + b3FreeElement( allocator, manifolds ); + b3UnlockMutex( world->manifoldAllocatorMutex ); +} + diff --git a/src/platform.h b/src/platform.h new file mode 100644 index 0000000..cc5fd5e --- /dev/null +++ b/src/platform.h @@ -0,0 +1,88 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "core.h" + +#include +#include + +// Software prefetch hint. _MM_HINT_T0 brings the line into all cache levels. +// MSVC exposes _mm_prefetch via ; clang/gcc accept it via +// on x86 and provide __builtin_prefetch elsewhere. +#if defined( B3_COMPILER_MSVC ) +#include +#define b3Prefetch( addr ) _mm_prefetch( (const char*)( addr ), _MM_HINT_T0 ) +#elif defined( B3_COMPILER_CLANG ) || defined( B3_COMPILER_GCC ) +#define b3Prefetch( addr ) __builtin_prefetch( (const void*)( addr ), 0, 3 ) +#else +#define b3Prefetch( addr ) ( (void)( addr ) ) +#endif + +static inline void b3AtomicStoreInt( b3AtomicInt* a, int value ) +{ +#if defined( _MSC_VER ) + (void)_InterlockedExchange( (long*)&a->value, value ); +#elif defined( __GNUC__ ) || defined( __clang__ ) + __atomic_store_n( &a->value, value, __ATOMIC_SEQ_CST ); +#else + #error "Unsupported platform" +#endif +} + +static inline int b3AtomicLoadInt( b3AtomicInt* a ) +{ +#if defined( _MSC_VER ) + return _InterlockedOr( (long*)&a->value, 0 ); +#elif defined( __GNUC__ ) || defined( __clang__ ) + return __atomic_load_n( &a->value, __ATOMIC_SEQ_CST ); +#else + #error "Unsupported platform" +#endif +} + +static inline int b3AtomicFetchAddInt( b3AtomicInt* a, int increment ) +{ +#if defined( _MSC_VER ) + return _InterlockedExchangeAdd( (long*)&a->value, (long)increment ); +#elif defined( __GNUC__ ) || defined( __clang__ ) + return __atomic_fetch_add( &a->value, increment, __ATOMIC_SEQ_CST ); +#else + #error "Unsupported platform" +#endif +} + +static inline bool b3AtomicCompareExchangeInt( b3AtomicInt* a, int expected, int desired ) +{ +#if defined( _MSC_VER ) + return _InterlockedCompareExchange( (long*)&a->value, (long)desired, (long)expected ) == expected; +#elif defined( __GNUC__ ) || defined( __clang__ ) + // The value written to expected is ignored + return __atomic_compare_exchange_n( &a->value, &expected, desired, false, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST ); +#else + #error "Unsupported platform" +#endif +} + +static inline void b3AtomicStoreU32( b3AtomicU32* a, uint32_t value ) +{ +#if defined( _MSC_VER ) + (void)_InterlockedExchange( (long*)&a->value, value ); +#elif defined( __GNUC__ ) || defined( __clang__ ) + __atomic_store_n( &a->value, value, __ATOMIC_SEQ_CST ); +#else + #error "Unsupported platform" +#endif +} + +static inline uint32_t b3AtomicLoadU32( b3AtomicU32* a ) +{ +#if defined( _MSC_VER ) + return (uint32_t)_InterlockedOr( (long*)&a->value, 0 ); +#elif defined( __GNUC__ ) || defined( __clang__ ) + return __atomic_load_n( &a->value, __ATOMIC_SEQ_CST ); +#else + #error "Unsupported platform" +#endif +} diff --git a/src/prismatic_joint.c b/src/prismatic_joint.c new file mode 100644 index 0000000..640d22d --- /dev/null +++ b/src/prismatic_joint.c @@ -0,0 +1,665 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "math_internal.h" +#include "physics_world.h" +#include "solver.h" +#include "solver_set.h" + +// needed for dll export +#include "box3d/box3d.h" + +// Linear constraint (point-to-line) +// joint axis is along joint frame A local z-axis +// perpX and perpY are world vectors fixed in A +// +// d = pB - pA = xB + rB - xA - rA +// Cx = dot(perpX, d) +// Cy = dot(perpY, d) + +// CdotX = dot(d, cross(wA, perpX)) + dot(perpX, vB + cross(wB, rB) - vA - cross(wA, rA)) +// = -dot(perpX, vA) - dot(cross(d + rA, perpX), wA) + dot(perpX, vB) + dot(cross(rB, perpX), vB) +// Jx = [-perpX, -cross(d + rA, perpX), perpX, cross(rB, perpX)] +// similar for perpY +// +// Simplification dropping dot(d, cross(wA, perpX)) (todo needs testing) +// CdotXs = dot(perpX, vB + cross(wB, rB) - vA - cross(wA, rA)) +// Jxs = [-perpX, -cross(rA, perpX), perpX, cross(rB, perpX)] + +// Motor/limit/spring linear constraint +// axis is the world joint axis fixed in A + +// C = dot(axis, d) +// Cdot = dot(d, cross(wA, axis)) + dot(axis, vB + cross(wB, rB) - vA - cross(wA, rA)) +// Cdot = -dot(axis, vA) - dot(cross(d + rA, axis), wA) + dot(axis, vB) + dot(cross(rB, axis), vB) +// J = [-axis -cross(d + rA, axis) axis cross(rB, axis)] +// +// Simplified (todo needs testing) +// Cdot = -dot(axis, vA) - dot(cross(rA, axis), wA) + dot(axis, vB) + dot(cross(rB, axis), vB) +// J = [-axis -cross(rA, axis) axis cross(rB, axis)] + +// Predictive limit is applied even when the limit is not active. +// Prevents a constraint speed that can lead to a constraint error in one time step. +// Want C2 = C1 + h * Cdot >= 0 +// Or: +// Cdot + C1/h >= 0 +// I do not apply a negative constraint error because that is handled in position correction. +// So: +// Cdot + max(C1, 0)/h >= 0 + +void b3PrismaticJoint_EnableLimit( b3JointId jointId, bool enableLimit ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + if ( enableLimit != base->prismaticJoint.enableLimit ) + { + base->prismaticJoint.lowerImpulse = 0.0f; + base->prismaticJoint.upperImpulse = 0.0f; + } + base->prismaticJoint.enableLimit = enableLimit; +} + +bool b3PrismaticJoint_IsLimitEnabled( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + return base->prismaticJoint.enableLimit; +} + +float b3PrismaticJoint_GetLowerLimit( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + return base->prismaticJoint.lowerTranslation; +} + +float b3PrismaticJoint_GetUpperLimit( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + return base->prismaticJoint.upperTranslation; +} + +void b3PrismaticJoint_SetLimits( b3JointId jointId, float lower, float upper ) +{ + B3_ASSERT( b3IsValidFloat( lower ) && b3IsValidFloat( upper ) ); + float lowerAngle = b3MinFloat( lower, upper ); + float upperAngle = b3MaxFloat( lower, upper ); + + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + base->prismaticJoint.lowerTranslation = lowerAngle; + base->prismaticJoint.upperTranslation = upperAngle; +} + +float b3PrismaticJoint_GetTranslation( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + b3Transform transformA = b3GetBodyTransform( world, base->bodyIdA ); + b3Transform transformB = b3GetBodyTransform( world, base->bodyIdB ); + + b3Vec3 jointAxis = b3RotateVector( base->localFrameA.q, b3Vec3_axisX ); + jointAxis = b3RotateVector( transformA.q, jointAxis ); + + b3Vec3 anchorA = b3RotateVector( transformA.q, base->localFrameA.p ); + b3Vec3 anchorB = b3RotateVector( transformB.q, base->localFrameB.p ); + b3Vec3 d = b3Add( b3Sub( transformB.p, transformA.p ), b3Sub( anchorB, anchorA ) ); + float translation = b3Dot( d, jointAxis ); + return translation; +} + +void b3PrismaticJoint_EnableSpring( b3JointId jointId, bool enableSpring ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + if ( enableSpring != base->prismaticJoint.enableSpring ) + { + base->prismaticJoint.springImpulse = 0.0f; + } + base->prismaticJoint.enableSpring = enableSpring; +} + +bool b3PrismaticJoint_IsSpringEnabled( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + return base->prismaticJoint.enableSpring; +} + +void b3PrismaticJoint_SetTargetTranslation( b3JointId jointId, float targetTranslation ) +{ + B3_ASSERT( b3IsValidFloat( targetTranslation ) ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + base->prismaticJoint.targetTranslation = targetTranslation; +} + +float b3PrismaticJoint_GetTargetTranslation( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + return base->prismaticJoint.targetTranslation; +} + +void b3PrismaticJoint_SetSpringHertz( b3JointId jointId, float hertz ) +{ + B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + base->prismaticJoint.hertz = hertz; +} + +float b3PrismaticJoint_GetSpringHertz( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + return base->prismaticJoint.hertz; +} + +void b3PrismaticJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio ) +{ + B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + base->prismaticJoint.dampingRatio = dampingRatio; +} + +float b3PrismaticJoint_GetSpringDampingRatio( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + return base->prismaticJoint.dampingRatio; +} + +void b3PrismaticJoint_EnableMotor( b3JointId jointId, bool enableMotor ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + if ( enableMotor != base->prismaticJoint.enableMotor ) + { + base->prismaticJoint.motorImpulse = 0.0f; + } + base->prismaticJoint.enableMotor = enableMotor; +} + +bool b3PrismaticJoint_IsMotorEnabled( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + return base->prismaticJoint.enableMotor; +} + +void b3PrismaticJoint_SetMotorSpeed( b3JointId jointId, float motorSpeed ) +{ + B3_ASSERT( b3IsValidFloat( motorSpeed ) ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + base->prismaticJoint.motorSpeed = motorSpeed; +} + +float b3PrismaticJoint_GetMotorSpeed( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + return base->prismaticJoint.motorSpeed; +} + +void b3PrismaticJoint_SetMaxMotorForce( b3JointId jointId, float maxForce ) +{ + B3_ASSERT( b3IsValidFloat( maxForce ) && maxForce >= 0.0f ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + base->prismaticJoint.maxMotorForce = maxForce; +} + +float b3PrismaticJoint_GetMaxMotorForce( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + return base->prismaticJoint.maxMotorForce; +} + +b3Vec3 b3GetPrismaticJointForce( b3World* world, b3JointSim* base ) +{ + b3Transform transformA = b3GetBodyTransform( world, base->bodyIdA ); + b3PrismaticJoint* joint = &base->prismaticJoint; + + // impulse in joint space + b3Vec3 impulse = { + joint->perpImpulse.x, + joint->perpImpulse.y, + joint->motorImpulse + joint->lowerImpulse + joint->upperImpulse + joint->springImpulse, + }; + + // convert impulse to force + b3Vec3 force = b3MulSV( world->inv_h, impulse ); + + // convert to body space + force = b3RotateVector( base->localFrameA.q, force ); + + // convert to world space + force = b3RotateVector( transformA.q, force ); + return force; +} + +b3Vec3 b3GetPrismaticJointTorque( b3World* world, b3JointSim* base ) +{ + b3Transform transformA = b3GetBodyTransform( world, base->bodyIdA ); + b3PrismaticJoint* joint = &base->prismaticJoint; + + b3Vec3 torque = b3MulSV( world->inv_h, joint->angularImpulse ); + torque = b3RotateVector( base->localFrameA.q, torque ); + torque = b3RotateVector( transformA.q, torque ); + return torque; +} + +void b3PreparePrismaticJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_prismaticJoint ); + + b3World* world = context->world; + + b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB ); + + B3_ASSERT( bodyB->setIndex == b3_awakeSet ); + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + base->invMassA = bodySimA->invMass; + base->invMassB = bodySimB->invMass; + base->invIA = bodySimA->invInertiaWorld; + base->invIB = bodySimB->invInertiaWorld; + + b3Matrix3 invInertiaSum = b3AddMM( base->invIA, base->invIB ); + base->fixedRotation = b3Det( invInertiaSum ) < 1000.0f * FLT_MIN; + + b3PrismaticJoint* joint = &base->prismaticJoint; + joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX; + joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX; + + // Compute joint anchor frames with world space rotation, relative to center of mass + joint->frameA.q = b3MulQuat( bodySimA->transform.q, base->localFrameA.q ); + joint->frameA.p = b3RotateVector( bodySimA->transform.q, b3Sub( base->localFrameA.p, bodySimA->localCenter ) ); + joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + joint->frameB.p = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) ); + + joint->deltaCenter = b3Sub( bodySimB->center, bodySimA->center ); + joint->rotationMass = b3InvertMatrix( invInertiaSum ); + + // Initial joint axes in world space + b3Matrix3 matrixA = b3MakeMatrixFromQuat( joint->frameA.q ); + joint->jointAxis = matrixA.cx; + joint->perpAxisY = matrixA.cy; + joint->perpAxisZ = matrixA.cz; + + joint->springSoftness = b3MakeSoft( joint->hertz, joint->dampingRatio, context->h ); + + if ( context->enableWarmStarting == false ) + { + joint->perpImpulse = (b3Vec2){ 0.0f, 0.0f }; + joint->angularImpulse = (b3Vec3){ 0.0f, 0.0f, 0.0f }; + joint->motorImpulse = 0.0f; + joint->springImpulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; + } +} + +void b3WarmStartPrismaticJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_prismaticJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3PrismaticJoint* joint = &base->prismaticJoint; + + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + // todo make this code and the wheel joint more similar + + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + b3Vec3 d = b3Add( b3Add( b3Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b3Sub( rB, rA ) ); + b3Vec3 jointAxis = b3RotateVector( stateA->deltaRotation, joint->jointAxis ); + b3Vec3 sAx = b3Cross( b3Add( rA, d ), jointAxis ); + b3Vec3 sBx = b3Cross( rB, jointAxis ); + + b3Vec3 perpY = b3RotateVector( stateA->deltaRotation, joint->perpAxisY ); + b3Vec3 perpZ = b3RotateVector( stateA->deltaRotation, joint->perpAxisZ ); + b3Vec3 sAy = b3Cross( b3Add( rA, d ), perpY ); + b3Vec3 sBy = b3Cross( rB, perpY ); + b3Vec3 sAz = b3Cross( b3Add( rA, d ), perpZ ); + b3Vec3 sBz = b3Cross( rB, perpZ ); + + float axialImpulse = joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse; + b3Vec2 perpImpulse = joint->perpImpulse; + + b3Vec3 P = b3Blend3( axialImpulse, jointAxis, perpImpulse.x, perpY, perpImpulse.y, perpZ ); + b3Vec3 LA = b3Add( b3Blend3( axialImpulse, sAx, perpImpulse.x, sAy, perpImpulse.y, sAz ), joint->angularImpulse ); + b3Vec3 LB = b3Add( b3Blend3( axialImpulse, sBx, perpImpulse.x, sBy, perpImpulse.y, sBz ), joint->angularImpulse ); + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, LA ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, LB ) ); + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +void b3SolvePrismaticJoint( b3JointSim* base, b3StepContext* context, bool useBias ) +{ + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3PrismaticJoint* joint = &base->prismaticJoint; + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + bool fixedRotation = base->fixedRotation; + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + b3Vec3 dcA = stateA->deltaPosition; + b3Vec3 dcB = stateB->deltaPosition; + b3Vec3 d = b3Add( b3Add( b3Sub( dcB, dcA ), joint->deltaCenter ), b3Sub( rB, rA ) ); + + b3Vec3 jointAxis = b3RotateVector( stateA->deltaRotation, joint->jointAxis ); + b3Vec3 sAx = b3Cross( b3Add( rA, d ), jointAxis ); + b3Vec3 sBx = b3Cross( rB, jointAxis ); + float jointTranslation = b3Dot( d, jointAxis ); + float targetTranslation = joint->targetTranslation; + + // The axial effective mass must be fresh to avoid divergence when the joint is stressed + float ka = mA + mB + b3Dot( sAx, b3MulMV( iA, sAx ) ) + b3Dot( sBx, b3MulMV( iB, sBx ) ); + float axialMass = ka > 0.0f ? 1.0f / ka : 0.0f; + + // Solve spring + if ( joint->enableSpring && fixedRotation == false ) + { + // Get the substep relative rotation + float c = jointTranslation - targetTranslation; + + float bias = joint->springSoftness.biasRate * c; + float massScale = joint->springSoftness.massScale; + float impulseScale = joint->springSoftness.impulseScale; + + b3Vec3 vRel = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, b3Add( rA, d ) ) ); + float cdot = b3Dot( vRel, jointAxis ); + float deltaImpulse = -massScale * axialMass * ( cdot + bias ) - impulseScale * joint->springImpulse; + joint->springImpulse += deltaImpulse; + + b3Vec3 P = b3MulSV( deltaImpulse, jointAxis ); + b3Vec3 LA = b3MulSV( deltaImpulse, sAx ); + b3Vec3 LB = b3MulSV( deltaImpulse, sBx ); + + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, LA ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, LB ) ); + } + + if ( joint->enableMotor && fixedRotation == false ) + { + b3Vec3 vRel = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, b3Add( rA, d ) ) ); + float cdot = b3Dot( vRel, jointAxis ) - joint->motorSpeed; + + float deltaImpulse = -axialMass * cdot; + float newImpulse = joint->motorImpulse + deltaImpulse; + float maxImpulse = joint->maxMotorForce * context->h; + newImpulse = b3ClampFloat( newImpulse, -maxImpulse, maxImpulse ); + deltaImpulse = newImpulse - joint->motorImpulse; + joint->motorImpulse = newImpulse; + + b3Vec3 P = b3MulSV( deltaImpulse, jointAxis ); + b3Vec3 LA = b3MulSV( deltaImpulse, sAx ); + b3Vec3 LB = b3MulSV( deltaImpulse, sBx ); + + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, LA ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, LB ) ); + } + + if ( joint->enableLimit && fixedRotation == false ) + { + float speculativeDistance = 0.25f * ( joint->upperTranslation - joint->lowerTranslation ); + + // Lower limit + { + float C = jointTranslation - joint->lowerTranslation; + + if ( C < speculativeDistance ) + { + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( C > 0.0f ) + { + // speculation + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * C; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + b3Vec3 vRel = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, b3Add( rA, d ) ) ); + float cdot = b3Dot( vRel, jointAxis ); + float oldImpulse = joint->lowerImpulse; + float deltaImpulse = -massScale * axialMass * ( cdot + bias ) - impulseScale * oldImpulse; + joint->lowerImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f ); + deltaImpulse = joint->lowerImpulse - oldImpulse; + + b3Vec3 P = b3MulSV( deltaImpulse, jointAxis ); + b3Vec3 LA = b3MulSV( deltaImpulse, sAx ); + b3Vec3 LB = b3MulSV( deltaImpulse, sBx ); + + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, LA ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, LB ) ); + } + else + { + joint->lowerImpulse = 0.0f; + } + } + + // Upper limit + { + float C = joint->upperTranslation - jointTranslation; + + if ( C < speculativeDistance ) + { + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( C > 0.0f ) + { + // speculation + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * C; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + // sign flipped on Cdot + b3Vec3 vRel = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, b3Add( rA, d ) ) ); + float cdot = -b3Dot( vRel, jointAxis ); + float oldImpulse = joint->upperImpulse; + float deltaImpulse = -massScale * axialMass * ( cdot + bias ) - impulseScale * oldImpulse; + joint->upperImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f ); + + // sign flipped on applied impulse + float negDeltaImpulse = oldImpulse - joint->upperImpulse; + b3Vec3 P = b3MulSV( negDeltaImpulse, jointAxis ); + b3Vec3 LA = b3MulSV( negDeltaImpulse, sAx ); + b3Vec3 LB = b3MulSV( negDeltaImpulse, sBx ); + + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, LA ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, LB ) ); + } + else + { + joint->upperImpulse = 0.0f; + } + } + } + + // Rotation constraint + if ( fixedRotation == false ) + { + b3Vec3 bias = { 0.0f, 0.0f, 0.0f }; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( useBias ) + { + b3Quat quatA = b3MulQuat( stateA->deltaRotation, joint->frameA.q ); + b3Quat quatB = b3MulQuat( stateB->deltaRotation, joint->frameB.q ); + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + b3Quat targetQuat = b3Quat_identity; + b3Vec3 deltaRotation = b3DeltaQuatToRotation( relQ, targetQuat ); + b3Vec3 c = b3Neg( b3RotateVector( quatA, deltaRotation ) ); + + bias = b3MulSV( base->constraintSoftness.biasRate, c ); + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + b3Vec3 cdot = b3Sub( wB, wA ); + b3Vec3 impulse = b3Sub( + b3MulSV( -massScale, b3MulMV( joint->rotationMass, b3Add( cdot, bias ) ) ), + b3MulSV( impulseScale, joint->angularImpulse ) ); + joint->angularImpulse = b3Add( joint->angularImpulse, impulse ); + + wA = b3Sub( wA, b3MulMV( iA, impulse ) ); + wB = b3Add( wB, b3MulMV( iB, impulse ) ); + } + + // Solve point-to-line constraint + { + b3Vec3 perpY = b3RotateVector( stateA->deltaRotation, joint->perpAxisY ); + b3Vec3 perpZ = b3RotateVector( stateA->deltaRotation, joint->perpAxisZ ); + + b3Vec2 bias = { 0.0f, 0.0f }; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias ) + { + b3Vec2 c = { b3Dot( perpY, d ), b3Dot( perpZ, d ) }; + bias = b3MulSV2( base->constraintSoftness.biasRate, c ); + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + b3Vec3 vRel = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, b3Add( rA, d ) ) ); + b3Vec2 cdot = { b3Dot( perpY, vRel ), b3Dot( perpZ, vRel ) }; + + // K = [(1/mA + 1/mB) * eye(2) - skew(rA) * invIA * skew(rA) - skew(rB) * invIB * skew(rB)] + // Jx = [-perpX, -cross(d + rA, perpX), perpX, cross(rB, perpX)] + b3Vec3 sAy = b3Cross( b3Add( rA, d ), perpY ); + b3Vec3 sBy = b3Cross( rB, perpY ); + b3Vec3 sAz = b3Cross( b3Add( rA, d ), perpZ ); + b3Vec3 sBz = b3Cross( rB, perpZ ); + + float kyy = mA + mB + b3Dot( sAy, b3MulMV( iA, sAy ) ) + b3Dot( sBy, b3MulMV( iB, sBy ) ); + float kyz = b3Dot( sAy, b3MulMV( iA, sAz ) ) + b3Dot( sBy, b3MulMV( iB, sBz ) ); + float kzz = mA + mB + b3Dot( sAz, b3MulMV( iA, sAz ) ) + b3Dot( sBz, b3MulMV( iB, sBz ) ); + + b3Matrix2 K = { { kyy, kyz }, { kyz, kzz } }; + + b3Vec2 oldImpulse = joint->perpImpulse; + b3Vec2 sol = b3Solve2( K, b3Add2( cdot, bias ) ); + b3Vec2 deltaImpulse = b3Sub2( b3MulSV2( -massScale, sol ), b3MulSV2( impulseScale, oldImpulse ) ); + joint->perpImpulse = b3Add2( oldImpulse, deltaImpulse ); + + b3Vec3 P = b3Blend2( deltaImpulse.x, perpY, deltaImpulse.y, perpZ ); + + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, b3Blend2( deltaImpulse.x, sAy, deltaImpulse.y, sAz ) ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, b3Blend2( deltaImpulse.x, sBy, deltaImpulse.y, sBz ) ) ); + } + + B3_ASSERT( b3IsValidVec3( vA ) ); + B3_ASSERT( b3IsValidVec3( wA ) ); + B3_ASSERT( b3IsValidVec3( vB ) ); + B3_ASSERT( b3IsValidVec3( wB ) ); + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +void b3DrawPrismaticJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale ) +{ + b3Transform frameA = b3MulTransforms( transformA, base->localFrameA ); + b3Transform frameB = b3MulTransforms( transformB, base->localFrameB ); + + b3Matrix3 R = b3MakeMatrixFromQuat( frameA.q ); + b3Vec3 axis = R.cx; + b3Vec3 perpY = R.cy; + b3Vec3 perpZ = R.cz; + + float s = 0.2f * scale; + draw->DrawSegmentFcn( frameA.p, b3MulAdd( frameA.p, s, perpY ), b3_colorGreen, draw->context ); + draw->DrawSegmentFcn( frameA.p, b3MulAdd( frameA.p, s, perpZ ), b3_colorBlue, draw->context ); + + b3PrismaticJoint* joint = &base->prismaticJoint; + if ( joint->enableLimit ) + { + b3Vec3 p1 = b3MulAdd( frameA.p, joint->lowerTranslation, axis ); + b3Vec3 p2 = b3MulAdd( frameA.p, joint->upperTranslation, axis ); + draw->DrawSegmentFcn( p1, p2, b3_colorOrange, draw->context ); + draw->DrawPointFcn( p1, 10.0f, b3_colorGreen, draw->context ); + draw->DrawPointFcn( p2, 10.0f, b3_colorRed, draw->context ); + } + else + { + b3Vec3 p1 = b3MulSub( frameA.p, 0.5f * scale, axis ); + b3Vec3 p2 = b3MulAdd( frameA.p, 0.5f * scale, axis ); + draw->DrawSegmentFcn( p1, p2, b3_colorOrange, draw->context ); + } + + draw->DrawPointFcn( frameB.p, 8.0f, b3_colorViolet, draw->context ); +} diff --git a/src/qsort.h b/src/qsort.h new file mode 100644 index 0000000..9ab426b --- /dev/null +++ b/src/qsort.h @@ -0,0 +1,186 @@ +/* + * Copyright (c) 2013, 2017 Alexey Tourbin + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/* + * This is a traditional Quicksort implementation which mostly follows + * [Sedgewick 1978]. Sorting is performed entirely on array indices, + * while actual access to the array elements is abstracted out with the + * user-defined `LESS` and `SWAP` primitives. + * + * Synopsis: + * QSORT(N, LESS, SWAP); + * where + * N - the number of elements in A[]; + * LESS(i, j) - compares A[i] to A[j]; + * SWAP(i, j) - exchanges A[i] with A[j]. + */ + +#ifndef QSORT_H +#define QSORT_H + +/* Sort 3 elements. */ +#define Q_SORT3(q_a1, q_a2, q_a3, Q_LESS, Q_SWAP) \ +do { \ + if (Q_LESS(q_a2, q_a1)) { \ + if (Q_LESS(q_a3, q_a2)) \ + Q_SWAP(q_a1, q_a3); \ + else { \ + Q_SWAP(q_a1, q_a2); \ + if (Q_LESS(q_a3, q_a2)) \ + Q_SWAP(q_a2, q_a3); \ + } \ + } \ + else if (Q_LESS(q_a3, q_a2)) { \ + Q_SWAP(q_a2, q_a3); \ + if (Q_LESS(q_a2, q_a1)) \ + Q_SWAP(q_a1, q_a2); \ + } \ +} while (0) + +/* Partition [q_l,q_r] around a pivot. After partitioning, + * [q_l,q_j] are the elements that are less than or equal to the pivot, + * while [q_i,q_r] are the elements greater than or equal to the pivot. */ +#define Q_PARTITION(q_l, q_r, q_i, q_j, Q_UINT, Q_LESS, Q_SWAP) \ +do { \ + /* The middle element, not to be confused with the median. */ \ + Q_UINT q_m = q_l + ((q_r - q_l) >> 1); \ + /* Reorder the second, the middle, and the last items. \ + * As [Edelkamp Weiss 2016] explain, using the second element \ + * instead of the first one helps avoid bad behaviour for \ + * decreasingly sorted arrays. This method is used in recent \ + * versions of gcc's std::sort, see gcc bug 58437#c13, although \ + * the details are somewhat different (cf. #c14). */ \ + Q_SORT3(q_l + 1, q_m, q_r, Q_LESS, Q_SWAP); \ + /* Place the median at the beginning. */ \ + Q_SWAP(q_l, q_m); \ + /* Partition [q_l+2, q_r-1] around the median which is in q_l. \ + * q_i and q_j are initially off by one, they get decremented \ + * in the do-while loops. */ \ + q_i = q_l + 1; q_j = q_r; \ + while (1) { \ + do q_i++; while (Q_LESS(q_i, q_l)); \ + do q_j--; while (Q_LESS(q_l, q_j)); \ + if (q_i >= q_j) break; /* Sedgewick says "until j < i" */ \ + Q_SWAP(q_i, q_j); \ + } \ + /* Compensate for the i==j case. */ \ + q_i = q_j + 1; \ + /* Put the median to its final place. */ \ + Q_SWAP(q_l, q_j); \ + /* The median is not part of the left subfile. */ \ + q_j--; \ +} while (0) + +/* Insertion sort is applied to small subfiles - this is contrary to + * Sedgewick's suggestion to run a separate insertion sort pass after + * the partitioning is done. The reason I don't like a separate pass + * is that it triggers extra comparisons, because it can't see that the + * medians are already in their final positions and need not be rechecked. + * Since I do not assume that comparisons are cheap, I also do not try + * to eliminate the (q_j > q_l) boundary check. */ +#define Q_INSERTION_SORT(q_l, q_r, Q_UINT, Q_LESS, Q_SWAP) \ +do { \ + Q_UINT q_i, q_j; \ + /* For each item starting with the second... */ \ + for (q_i = q_l + 1; q_i <= q_r; q_i++) \ + /* move it down the array so that the first part is sorted. */ \ + for (q_j = q_i; q_j > q_l && (Q_LESS(q_j, q_j - 1)); q_j--) \ + Q_SWAP(q_j, q_j - 1); \ +} while (0) + +/* When the size of [q_l,q_r], i.e. q_r-q_l+1, is greater than or equal to + * Q_THRESH, the algorithm performs recursive partitioning. When the size + * drops below Q_THRESH, the algorithm switches to insertion sort. + * The minimum valid value is probably 5 (with 5 items, the second and + * the middle items, the middle itself being rounded down, are distinct). */ +#define Q_THRESH 16 + +/* The main loop. */ +#define Q_LOOP(Q_UINT, Q_N, Q_LESS, Q_SWAP) \ +do { \ + Q_UINT q_l = 0; \ + Q_UINT q_r = (Q_N) - 1; \ + Q_UINT q_sp = 0; /* the number of frames pushed to the stack */ \ + struct { Q_UINT q_l, q_r; } \ + /* On 32-bit platforms, to sort a "char[3GB+]" array, \ + * it may take full 32 stack frames. On 64-bit CPUs, \ + * though, the address space is limited to 48 bits. \ + * The usage is further reduced if Q_N has a 32-bit type. */ \ + q_st[sizeof(Q_UINT) > 4 && sizeof(Q_N) > 4 ? 48 : 32]; \ + while (1) { \ + if (q_r - q_l + 1 >= Q_THRESH) { \ + Q_UINT q_i, q_j; \ + Q_PARTITION(q_l, q_r, q_i, q_j, Q_UINT, Q_LESS, Q_SWAP); \ + /* Now have two subfiles: [q_l,q_j] and [q_i,q_r]. \ + * Dealing with them depends on which one is bigger. */ \ + if (q_j - q_l >= q_r - q_i) \ + Q_SUBFILES(q_l, q_j, q_i, q_r); \ + else \ + Q_SUBFILES(q_i, q_r, q_l, q_j); \ + } \ + else { \ + Q_INSERTION_SORT(q_l, q_r, Q_UINT, Q_LESS, Q_SWAP); \ + /* Pop subfiles from the stack, until it gets empty. */ \ + if (q_sp == 0) break; \ + q_sp--; \ + q_l = q_st[q_sp].q_l; \ + q_r = q_st[q_sp].q_r; \ + } \ + } \ +} while (0) + +/* The missing part: dealing with subfiles. + * Assumes that the first subfile is not smaller than the second. */ +#define Q_SUBFILES(q_l1, q_r1, q_l2, q_r2) \ +do { \ + /* If the second subfile is only a single element, it needs \ + * no further processing. The first subfile will be processed \ + * on the next iteration (both subfiles cannot be only a single \ + * element, due to Q_THRESH). */ \ + if (q_l2 == q_r2) { \ + q_l = q_l1; \ + q_r = q_r1; \ + } \ + else { \ + /* Otherwise, both subfiles need processing. \ + * Push the larger subfile onto the stack. */ \ + q_st[q_sp].q_l = q_l1; \ + q_st[q_sp].q_r = q_r1; \ + q_sp++; \ + /* Process the smaller subfile on the next iteration. */ \ + q_l = q_l2; \ + q_r = q_r2; \ + } \ +} while (0) + +/* And now, ladies and gentlemen, may I proudly present to you... */ +#define QSORT(Q_N, Q_LESS, Q_SWAP) \ +do { \ + if ((Q_N) > 1) \ + /* We could check sizeof(Q_N) and use "unsigned", but at least \ + * on x86_64, this has the performance penalty of up to 5%. */ \ + Q_LOOP(unsigned long, Q_N, Q_LESS, Q_SWAP); \ +} while (0) + +#endif + +/* ex:set ts=8 sts=4 sw=4 noet: */ diff --git a/src/revolute_joint.c b/src/revolute_joint.c new file mode 100644 index 0000000..afe5958 --- /dev/null +++ b/src/revolute_joint.c @@ -0,0 +1,649 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "joint.h" +#include "math_internal.h" +#include "physics_world.h" +#include "solver.h" +#include "solver_set.h" + +// needed for dll export +#include "box3d/box3d.h" + +// Point-to-point linear constraint +// C = pB - pA +// Cdot = vB - vA +// = vB + cross(wB, rB) - vA - cross(wA, rA) +// Cdot = J * v +// J = [-E -skew(rA) E skew(rB) ] +// +// K = J * invM * JT +// = [(1/mA + 1/mB) * E - skew(rA) * invIA * skew(rA) - skew(rB) * invIB * skew(rB)] + +// Perpendicularity constraint +// frameA = qA * localFrameA +// frameB = qB * localFrameB +// qRel = conj(frameA) * frameB +// C = [qRel.x; qRel.y] +// qRelDot = 0.5 * conj(frameA) * (wB - wA) * frameB +// Cdot = [qRelDot.x, qRelDot.y] +// Pulling out wB and wA +// sr = qRel.s +// vr = qRel.v +// Jx = 0.5 * rotate(frameA, sr * ex + cross(vr, ex)) +// Jy = 0.5 * rotate(frameA, sr * ey + cross(vr, ey)) + +// Motor constraint +// Cdot = wB - wA +// J = [0 0 -E 0 0 E] +// K = invIA + invIB + +void b3RevoluteJoint_EnableLimit( b3JointId jointId, bool enableLimit ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + if ( enableLimit != base->revoluteJoint.enableLimit ) + { + base->revoluteJoint.lowerImpulse = 0.0f; + base->revoluteJoint.upperImpulse = 0.0f; + } + base->revoluteJoint.enableLimit = enableLimit; +} + +bool b3RevoluteJoint_IsLimitEnabled( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + return base->revoluteJoint.enableLimit; +} + +float b3RevoluteJoint_GetLowerLimit( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + return base->revoluteJoint.lowerAngle; +} + +float b3RevoluteJoint_GetUpperLimit( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + return base->revoluteJoint.upperAngle; +} + +void b3RevoluteJoint_SetLimits( b3JointId jointId, float lowerLimitRadians, float upperLimitRadians ) +{ + B3_ASSERT( lowerLimitRadians <= upperLimitRadians ); + B3_ASSERT( b3IsValidFloat( lowerLimitRadians ) && b3IsValidFloat( upperLimitRadians ) ); + float lowerAngle = b3ClampFloat( lowerLimitRadians, -B3_PI, B3_PI ); + float upperAngle = b3ClampFloat( upperLimitRadians, -B3_PI, B3_PI ); + + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + base->revoluteJoint.lowerAngle = lowerAngle; + base->revoluteJoint.upperAngle = upperAngle; + base->revoluteJoint.lowerImpulse = 0.0f; + base->revoluteJoint.upperImpulse = 0.0f; +} + +float b3RevoluteJoint_GetAngle( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + b3Transform transformA = b3GetBodyTransform( world, base->bodyIdA ); + b3Transform transformB = b3GetBodyTransform( world, base->bodyIdB ); + + b3Quat quatA = b3MulQuat( transformA.q, base->localFrameA.q ); + b3Quat quatB = b3MulQuat( transformB.q, base->localFrameB.q ); + + if ( b3DotQuat( quatA, quatB ) < 0.0f ) + { + // this keeps the twist angle in the range [-pi, pi] + quatB = b3NegateQuat( quatB ); + } + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + return b3GetTwistAngle( relQ ); +} + +void b3RevoluteJoint_EnableSpring( b3JointId jointId, bool enableSpring ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + if ( enableSpring != base->revoluteJoint.enableSpring ) + { + base->revoluteJoint.springImpulse = 0.0f; + } + base->revoluteJoint.enableSpring = enableSpring; +} + +bool b3RevoluteJoint_IsSpringEnabled( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + return base->revoluteJoint.enableSpring; +} + +void b3RevoluteJoint_SetTargetAngle( b3JointId jointId, float targetRadians ) +{ + B3_ASSERT( b3IsValidFloat( targetRadians ) && -B3_PI <= targetRadians && targetRadians <= B3_PI ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + base->revoluteJoint.targetAngle = targetRadians; +} + +float b3RevoluteJoint_GetTargetAngle( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + return base->revoluteJoint.targetAngle; +} + +void b3RevoluteJoint_SetSpringHertz( b3JointId jointId, float hertz ) +{ + B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + base->revoluteJoint.hertz = hertz; +} + +float b3RevoluteJoint_GetSpringHertz( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + return base->revoluteJoint.hertz; +} + +void b3RevoluteJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio ) +{ + B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + base->revoluteJoint.dampingRatio = dampingRatio; +} + +float b3RevoluteJoint_GetSpringDampingRatio( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + return base->revoluteJoint.dampingRatio; +} + +void b3RevoluteJoint_EnableMotor( b3JointId jointId, bool enableMotor ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + if ( enableMotor != base->revoluteJoint.enableMotor ) + { + base->revoluteJoint.motorImpulse = 0.0f; + } + base->revoluteJoint.enableMotor = enableMotor; +} + +bool b3RevoluteJoint_IsMotorEnabled( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + return base->revoluteJoint.enableMotor; +} + +void b3RevoluteJoint_SetMotorSpeed( b3JointId jointId, float motorSpeed ) +{ + B3_ASSERT( b3IsValidFloat( motorSpeed ) ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + base->revoluteJoint.motorSpeed = motorSpeed; +} + +float b3RevoluteJoint_GetMotorSpeed( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + return base->revoluteJoint.motorSpeed; +} + +void b3RevoluteJoint_SetMaxMotorTorque( b3JointId jointId, float maxForce ) +{ + B3_ASSERT( b3IsValidFloat( maxForce ) && maxForce >= 0.0f ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + base->revoluteJoint.maxMotorTorque = maxForce; +} + +float b3RevoluteJoint_GetMaxMotorTorque( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + return base->revoluteJoint.maxMotorTorque; +} + +b3Vec3 b3GetRevoluteJointForce( b3World* world, b3JointSim* base ) +{ + b3Vec3 force = b3MulSV( world->inv_h, base->revoluteJoint.linearImpulse ); + return force; +} + +b3Vec3 b3GetRevoluteJointTorque( b3World* world, b3JointSim* base ) +{ + b3Transform transformA = b3GetBodyTransform( world, base->bodyIdA ); + b3RevoluteJoint* joint = &base->revoluteJoint; + b3Vec3 axis = b3RotateVector( base->localFrameA.q, b3Vec3_axisZ ); + axis = b3RotateVector( transformA.q, axis ); + + b3Quat relQ = b3InvMulQuat( joint->frameA.q, joint->frameB.q ); + + // These are needed for warm starting + joint->perpAxisX = b3MulSV( + 0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) ); + joint->perpAxisY = b3MulSV( + 0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) ); + + float axialImpulse = joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse; + b3Vec3 angularImpulse = + b3Add( b3MulSV( joint->perpImpulse.x, joint->perpAxisX ), b3MulSV( joint->perpImpulse.y, joint->perpAxisY ) ); + angularImpulse = b3MulAdd( angularImpulse, axialImpulse, joint->rotationAxisZ ); + + // todo add pivot torque + b3Vec3 impulse = b3MulAdd( angularImpulse, + joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse, axis ); + b3Vec3 torque = b3MulSV( world->inv_h, impulse ); + return torque; +} + +void b3PrepareRevoluteJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_revoluteJoint ); + + b3World* world = context->world; + + b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB ); + + B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + base->invMassA = bodySimA->invMass; + base->invMassB = bodySimB->invMass; + base->invIA = bodySimA->invInertiaWorld; + base->invIB = bodySimB->invInertiaWorld; + + b3Matrix3 invInertiaSum = b3AddMM( base->invIA, base->invIB ); + base->fixedRotation = b3Det( invInertiaSum ) < 1000.0f * FLT_MIN; + + b3RevoluteJoint* joint = &base->revoluteJoint; + joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX; + joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX; + + // Compute joint anchor frames with world space rotation, relative to center of mass + // Avoid round-off here as much as possible. + // b3Vec3 pf = (xf.p - c) + rot(xf.q, f.p) + // pf = xf.p - (xf.p + rot(xf.q, lc)) + rot(xf.q, f.p) + // pf = rot(xf.q, f.p - lc) + joint->frameA.q = b3MulQuat( bodySimA->transform.q, base->localFrameA.q ); + joint->frameA.p = b3RotateVector( bodySimA->transform.q, b3Sub( base->localFrameA.p, bodySimA->localCenter ) ); + joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + joint->frameB.p = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) ); + + joint->deltaCenter = b3Sub( bodySimB->center, bodySimA->center ); + + { + // Rotation axis is the z-axis of body A. + b3Vec3 rotationAxisZ = b3RotateVector( joint->frameA.q, b3Vec3_axisZ ); + float k = b3Dot( rotationAxisZ, b3MulMV( invInertiaSum, rotationAxisZ ) ); + joint->axialMass = k > 0.0f ? 1.0f / k : 0.0f; + joint->rotationAxisZ = rotationAxisZ; + } + + b3Quat relQ = b3InvMulQuat( joint->frameA.q, joint->frameB.q ); + + { + // These are needed for warm starting + joint->perpAxisX = b3MulSV( + 0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) ); + joint->perpAxisY = b3MulSV( + 0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) ); + } + + joint->springSoftness = b3MakeSoft( joint->hertz, joint->dampingRatio, context->h ); + + if ( context->enableWarmStarting == false ) + { + joint->linearImpulse = b3Vec3_zero; + joint->perpImpulse = (b3Vec2){ 0.0f, 0.0f }; + joint->motorImpulse = 0.0f; + joint->springImpulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; + } +} + +void b3WarmStartRevoluteJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_revoluteJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3RevoluteJoint* joint = &base->revoluteJoint; + + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + float axialImpulse = joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse; + b3Vec3 angularImpulse = + b3Add( b3MulSV( joint->perpImpulse.x, joint->perpAxisX ), b3MulSV( joint->perpImpulse.y, joint->perpAxisY ) ); + angularImpulse = b3MulAdd( angularImpulse, axialImpulse, joint->rotationAxisZ ); + + vA = b3MulSub( vA, mA, joint->linearImpulse ); + wA = b3Sub( wA, b3MulMV( iA, b3Add( b3Cross( rA, joint->linearImpulse ), angularImpulse ) ) ); + + vB = b3MulAdd( vB, mB, joint->linearImpulse ); + wB = b3Add( wB, b3MulMV( iB, b3Add( b3Cross( rB, joint->linearImpulse ), angularImpulse ) ) ); + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBias ) +{ + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3RevoluteJoint* joint = &base->revoluteJoint; + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + bool fixedRotation = base->fixedRotation; + b3Quat quatA = b3MulQuat( stateA->deltaRotation, joint->frameA.q ); + b3Quat quatB = b3MulQuat( stateB->deltaRotation, joint->frameB.q ); + + if ( b3DotQuat( quatA, quatB ) < 0.0f ) + { + // this keeps the rotation angle in the range [-pi, pi] + quatB = b3NegateQuat( quatB ); + } + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + // Solve spring + if ( joint->enableSpring && fixedRotation == false ) + { + // Get the substep relative rotation + float targetAngle = joint->targetAngle; + float angle = b3GetTwistAngle( relQ ); + float c = angle - targetAngle; + + float bias = joint->springSoftness.biasRate * c; + float massScale = joint->springSoftness.massScale; + float impulseScale = joint->springSoftness.impulseScale; + float cdot = b3Dot( b3Sub( wB, wA ), joint->rotationAxisZ ); + + float deltaImpulse = -massScale * joint->axialMass * ( cdot + bias ) - impulseScale * joint->springImpulse; + joint->springImpulse += deltaImpulse; + + wA = b3MulSub( wA, deltaImpulse, b3MulMV( iA, joint->rotationAxisZ ) ); + wB = b3MulAdd( wB, deltaImpulse, b3MulMV( iB, joint->rotationAxisZ ) ); + } + + if ( joint->enableMotor && fixedRotation == false ) + { + float cdot = b3Dot( b3Sub( wB, wA ), joint->rotationAxisZ ) - joint->motorSpeed; + + float deltaImpulse = -joint->axialMass * cdot; + float newImpulse = joint->motorImpulse + deltaImpulse; + float maxImpulse = joint->maxMotorTorque * context->h; + newImpulse = b3ClampFloat( newImpulse, -maxImpulse, maxImpulse ); + deltaImpulse = newImpulse - joint->motorImpulse; + joint->motorImpulse = newImpulse; + + wA = b3MulSub( wA, deltaImpulse, b3MulMV( iA, joint->rotationAxisZ ) ); + wB = b3MulAdd( wB, deltaImpulse, b3MulMV( iB, joint->rotationAxisZ ) ); + } + + if ( joint->enableLimit && fixedRotation == false ) + { + float angle = b3GetTwistAngle( relQ ); + + // todo does an updated twist axis help? + + b3Vec3 axis = joint->rotationAxisZ; + + // Lower limit + { + float c = angle - joint->lowerAngle; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + float cdot = b3Dot( b3Sub( wB, wA ), axis ); + float oldImpulse = joint->lowerImpulse; + float deltaImpulse = -massScale * joint->axialMass * ( cdot + bias ) - impulseScale * oldImpulse; + joint->lowerImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f ); + deltaImpulse = joint->lowerImpulse - oldImpulse; + + wA = b3MulSub( wA, deltaImpulse, b3MulMV( iA, axis ) ); + wB = b3MulAdd( wB, deltaImpulse, b3MulMV( iB, axis ) ); + } + + // Upper limit + { + float c = joint->upperAngle - angle; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + // sign flipped on Cdot + float cdot = b3Dot( b3Sub( wA, wB ), axis ); + float oldImpulse = joint->upperImpulse; + float deltaImpulse = -massScale * joint->axialMass * ( cdot + bias ) - impulseScale * oldImpulse; + joint->upperImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f ); + deltaImpulse = joint->upperImpulse - oldImpulse; + + // sign flipped on applied impulse + wA = b3MulAdd( wA, deltaImpulse, b3MulMV( iA, axis ) ); + wB = b3MulSub( wB, deltaImpulse, b3MulMV( iB, axis ) ); + } + } + + // Collinearity constraint + if ( fixedRotation == false ) + { + b3Vec2 bias = { 0.0f, 0.0f }; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( useBias ) + { + b3Vec2 c = { relQ.v.x, relQ.v.y }; + bias = b3MulSV2( base->constraintSoftness.biasRate, c ); + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + // Collinearity constraint as 2-by-2 + b3Vec3 perpAxisX = + b3MulSV( 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) ); + b3Vec3 perpAxisY = + b3MulSV( 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) ); + joint->perpAxisX = perpAxisX; + joint->perpAxisY = perpAxisY; + + b3Matrix3 invInertiaSum = b3AddMM( iA, iB ); + float kxx = b3Dot( perpAxisX, b3MulMV( invInertiaSum, perpAxisX ) ); + float kyy = b3Dot( perpAxisY, b3MulMV( invInertiaSum, perpAxisY ) ); + float kxy = b3Dot( perpAxisX, b3MulMV( invInertiaSum, perpAxisY ) ); + + b3Matrix2 k = { { kxx, kxy }, { kxy, kyy } }; + + b3Vec3 wRel = b3Sub( wB, wA ); + b3Vec2 cdot = { b3Dot( wRel, perpAxisX ), b3Dot( wRel, perpAxisY ) }; + b3Vec2 oldImpulse = joint->perpImpulse; + b3Vec2 sol = b3Solve2( k, b3Add2( cdot, bias ) ); + b3Vec2 deltaImpulse = b3Sub2( b3MulSV2( -massScale, sol ), b3MulSV2( impulseScale, oldImpulse ) ); + joint->perpImpulse = b3Add2( joint->perpImpulse, deltaImpulse ); + + b3Vec3 angularImpulse = b3Add( b3MulSV( deltaImpulse.x, perpAxisX ), b3MulSV( deltaImpulse.y, perpAxisY ) ); + wA = b3Sub( wA, b3MulMV( iA, angularImpulse ) ); + wB = b3Add( wB, b3MulMV( iB, angularImpulse ) ); + } + + // Solve point-to-point constraint + { + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + b3Vec3 cdot = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, rA ) ); + + b3Vec3 bias = b3Vec3_zero; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias ) + { + b3Vec3 dcA = stateA->deltaPosition; + b3Vec3 dcB = stateB->deltaPosition; + + b3Vec3 separation = b3Add( b3Add( b3Sub( dcB, dcA ), b3Sub( rB, rA ) ), joint->deltaCenter ); + + bias = b3MulSV( base->constraintSoftness.biasRate, separation ); + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + //// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] + b3Matrix3 sA = b3Skew( rA ); + b3Matrix3 sB = b3Skew( rB ); + b3Matrix3 kA = b3MulMM( sA, b3MulMM( base->invIA, sA ) ); + b3Matrix3 kB = b3MulMM( sB, b3MulMM( base->invIB, sB ) ); + b3Matrix3 k = b3NegateMat3( b3AddMM( kA, kB ) ); + k.cx.x += mA + mB; + k.cy.y += mA + mB; + k.cz.z += mA + mB; + + b3Vec3 b = b3Solve3( k, b3Add( cdot, bias ) ); + + b3Vec3 impulse = b3Sub( b3MulSV( -massScale, b ), b3MulSV( impulseScale, joint->linearImpulse ) ); + joint->linearImpulse = b3Add( joint->linearImpulse, impulse ); + + vA = b3MulSub( vA, mA, impulse ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) ); + vB = b3MulAdd( vB, mB, impulse ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) ); + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale ) +{ + b3Transform frameA = b3MulTransforms( transformA, base->localFrameA ); + + float length1 = 0.1f * scale; + draw->DrawSegmentFcn( frameA.p, b3MulAdd( frameA.p, length1, b3RotateVector( frameA.q, b3Vec3_axisX ) ), b3_colorRed, + draw->context ); + draw->DrawSegmentFcn( frameA.p, b3MulAdd( frameA.p, length1, b3RotateVector( frameA.q, b3Vec3_axisY ) ), b3_colorGreen, + draw->context ); + draw->DrawSegmentFcn( frameA.p, b3MulAdd( frameA.p, length1, b3RotateVector( frameA.q, b3Vec3_axisZ ) ), b3_colorBlue, + draw->context ); + + b3Transform frameB = b3MulTransforms( transformB, base->localFrameB ); + + b3RevoluteJoint* joint = &base->revoluteJoint; + enum { kSliceCount = 16 }; + + // Twist limit + if ( joint->enableLimit ) + { + b3Quat quatA = frameA.q; + b3Quat quatB = frameB.q; + + if ( b3DotQuat( quatA, quatB ) < 0.0f ) + { + // this keeps the twist angle in the range [-pi, pi] + quatB = b3NegateQuat( quatB ); + } + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + const float wedgeRadius = 0.2f * scale; + for ( int index = 0; index < kSliceCount; ++index ) + { + float t1 = (float)( index + 0 ) / kSliceCount; + float alpha1 = ( 1.0f - t1 ) * joint->lowerAngle + t1 * joint->upperAngle; + float t2 = (float)( index + 1 ) / kSliceCount; + float alpha2 = ( 1.0f - t2 ) * joint->lowerAngle + t2 * joint->upperAngle; + + b3Vec3 vertex1 = { wedgeRadius * b3Cos( alpha1 ), wedgeRadius * b3Sin( alpha1 ), 0.0f }; + b3Vec3 vertex2 = { wedgeRadius * b3Cos( alpha2 ), wedgeRadius * b3Sin( alpha2 ), 0.0f }; + + if ( index == 0 ) + { + draw->DrawSegmentFcn( frameA.p, b3TransformPoint( frameA, vertex1 ), b3_colorCyan, draw->context ); + } + + if ( index == kSliceCount - 1 ) + { + draw->DrawSegmentFcn( b3TransformPoint( frameA, vertex2 ), frameA.p, b3_colorCyan, draw->context ); + } + draw->DrawSegmentFcn( b3TransformPoint( frameA, vertex1 ), b3TransformPoint( frameA, vertex2 ), b3_colorCyan, + draw->context ); + } + + float twistAngle = b3GetTwistAngle( relQ ); + b3Vec3 p2 = { wedgeRadius * b3Cos( twistAngle ), wedgeRadius * b3Sin( twistAngle ), 0.0f }; + draw->DrawSegmentFcn( frameA.p, b3TransformPoint( frameA, p2 ), b3_colorYellow, draw->context ); + } +} diff --git a/src/scheduler.c b/src/scheduler.c new file mode 100644 index 0000000..7bcde42 --- /dev/null +++ b/src/scheduler.c @@ -0,0 +1,198 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "platform.h" +#include "core.h" +#include "scheduler.h" + +#include "box3d/base.h" +#include "box3d/constants.h" + +#include +#include +#include + +enum b3SchedulerTaskStatus +{ + b3_schedulerFree = 0, + b3_schedulerPending = 1, + b3_schedulerClaimed = 2, + b3_schedulerComplete = 3, +}; + +typedef struct b3SchedulerTask +{ + b3TaskCallback* callback; + void* taskContext; + b3AtomicInt status; +} b3SchedulerTask; + +typedef struct b3SchedulerWorkerContext +{ + struct b3Scheduler* scheduler; + int threadIndex; +} b3SchedulerWorkerContext; + +typedef struct b3Scheduler +{ + b3Thread* threads[B3_MAX_WORKERS]; + b3SchedulerWorkerContext workerContexts[B3_MAX_WORKERS]; + + // total workers including main thread + int workerCount; + + // threads created = workerCount - 1 + int threadCount; + + b3SchedulerTask tasks[B3_MAX_TASKS]; + b3AtomicInt nextSlot; + + b3Semaphore* taskSemaphore; + b3AtomicInt shutdown; +} b3Scheduler; + +// Try to claim and execute one pending task. +// Returns true if work was performed, false otherwise. +static bool b3SchedulerExecuteOne( b3Scheduler* scheduler ) +{ + int taskCount = b3AtomicLoadInt( &scheduler->nextSlot ); + for ( int t = 0; t < taskCount; ++t ) + { + b3SchedulerTask* task = scheduler->tasks + t; + if ( b3AtomicLoadInt( &task->status ) != b3_schedulerPending ) + { + continue; + } + + if ( b3AtomicCompareExchangeInt( &task->status, b3_schedulerPending, b3_schedulerClaimed ) == false ) + { + continue; + } + + task->callback( task->taskContext ); + + b3AtomicStoreInt( &task->status, b3_schedulerComplete ); + return true; + } + + return false; +} + +// Background worker thread entry point. +static void b3SchedulerWorkerMain( void* context ) +{ + b3SchedulerWorkerContext* workerContext = context; + b3Scheduler* scheduler = workerContext->scheduler; + + while ( true ) + { + b3WaitSemaphore( scheduler->taskSemaphore ); + + if ( b3AtomicLoadInt( &scheduler->shutdown ) != 0 ) + { + break; + } + + // Claim and execute all available work + while ( b3SchedulerExecuteOne( scheduler ) ) + { + } + } +} + +b3Scheduler* b3CreateScheduler( int workerCount ) +{ + B3_ASSERT( 0 < workerCount && workerCount <= B3_MAX_WORKERS ); + + b3Scheduler* scheduler = b3Alloc( sizeof( b3Scheduler ) ); + memset( scheduler, 0, sizeof( b3Scheduler ) ); + + scheduler->workerCount = workerCount; + int threadCount = workerCount - 1; + scheduler->threadCount = threadCount; + scheduler->taskSemaphore = b3CreateSemaphore( 0 ); + b3AtomicStoreInt( &scheduler->shutdown, 0 ); + b3AtomicStoreInt( &scheduler->nextSlot, 0 ); + + // Background threads use indices 1..workerCount-1. + // Main thread uses index 0. + for ( int i = 0; i < threadCount; ++i ) + { + scheduler->workerContexts[i].scheduler = scheduler; + scheduler->workerContexts[i].threadIndex = i + 1; + + char name[16]; + snprintf( name, sizeof( name ), "box2d_worker_%02d", i + 1 ); + scheduler->threads[i] = b3CreateThread( b3SchedulerWorkerMain, scheduler->workerContexts + i, name ); + } + + return scheduler; +} + +void b3DestroyScheduler( b3Scheduler* scheduler ) +{ + b3AtomicStoreInt( &scheduler->shutdown, 1 ); + + // Wake all background threads so they see the shutdown flag + for ( int i = 0; i < scheduler->threadCount; ++i ) + { + b3SignalSemaphore( scheduler->taskSemaphore ); + } + + for ( int i = 0; i < scheduler->threadCount; ++i ) + { + b3JoinThread( scheduler->threads[i] ); + scheduler->threads[i] = NULL; + } + + b3DestroySemaphore( scheduler->taskSemaphore ); + b3Free( scheduler, sizeof( b3Scheduler ) ); +} + +void b3ResetScheduler( b3Scheduler* scheduler ) +{ + b3AtomicStoreInt( &scheduler->nextSlot, 0 ); +} + +void* b3SchedulerEnqueueTask( b3TaskCallback* task, void* taskContext, void* userContext, const char* name ) +{ + B3_UNUSED( name ); + b3Scheduler* scheduler = userContext; + + int slot = b3AtomicFetchAddInt( &scheduler->nextSlot, 1 ); + B3_ASSERT( slot < B3_MAX_TASKS ); + + b3SchedulerTask* schedulerTask = scheduler->tasks + slot; + schedulerTask->callback = task; + schedulerTask->taskContext = taskContext; + + // Memory fence: status must be published after callback and context are written + b3AtomicStoreInt( &schedulerTask->status, b3_schedulerPending ); + + // One wake per enqueue is enough: at most one worker picks up each task. + b3SignalSemaphore( scheduler->taskSemaphore ); + + return schedulerTask; +} + +void b3SchedulerFinishTask( void* userTask, void* userContext ) +{ + if ( userTask == NULL ) + { + return; + } + + b3Scheduler* scheduler = userContext; + b3SchedulerTask* waitTask = userTask; + + // Main thread helps execute any available work while waiting for the + // target task to complete. This keeps the main thread from idling when + // background threads are busy on other tasks from the same phase. + while ( b3AtomicLoadInt( &waitTask->status ) != b3_schedulerComplete ) + { + if ( b3SchedulerExecuteOne( scheduler ) == false ) + { + b3Yield(); + } + } +} diff --git a/src/scheduler.h b/src/scheduler.h new file mode 100644 index 0000000..7a08e6e --- /dev/null +++ b/src/scheduler.h @@ -0,0 +1,24 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +typedef void b3TaskCallback( void* taskContext ); +typedef struct b3Scheduler b3Scheduler; + +#ifdef __cplusplus +extern "C" +{ +#endif + +b3Scheduler* b3CreateScheduler( int workerCount ); +void b3DestroyScheduler( b3Scheduler* scheduler ); +void b3ResetScheduler( b3Scheduler* scheduler ); + +// See b3EnqueueTaskCallback and b3FinishTaskCallback +void* b3SchedulerEnqueueTask( b3TaskCallback* task, void* taskContext, void* userContext, const char* name ); +void b3SchedulerFinishTask( void* userTask, void* userContext ); + +#ifdef __cplusplus +} +#endif diff --git a/src/sensor.c b/src/sensor.c new file mode 100644 index 0000000..c28f2ca --- /dev/null +++ b/src/sensor.c @@ -0,0 +1,452 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "sensor.h" + +#include "body.h" +#include "contact.h" +#include "ctz.h" +#include "physics_world.h" +#include "shape.h" + +#include "box3d/collision.h" + +// for qsort +#include "parallel_for.h" + +#include + +typedef struct b3SensorQueryContext +{ + b3World* world; + b3SensorTaskContext* taskContext; + b3Sensor* sensor; + b3Shape* sensorShape; + b3Transform transform; +} b3SensorQueryContext; + +static bool b3OverlapSensor( b3Shape* sensorShape, b3Transform sensorTransform, b3Shape* visitorShape, + b3Transform visitorTransform ) +{ + b3ShapeType type = sensorShape->type; + + b3ShapeProxy proxy = b3MakeShapeProxy( visitorShape ); + + // Get the visitor shape in the frame of the sensor + b3Transform relativeTransform = b3InvMulTransforms( sensorTransform, visitorTransform ); + + b3Vec3 localPoints[B3_MAX_SHAPE_CAST_POINTS]; + b3ShapeProxy localProxy; + + localProxy.count = b3MinInt( proxy.count, B3_MAX_SHAPE_CAST_POINTS ); + for ( int i = 0; i < localProxy.count; ++i ) + { + localPoints[i] = b3TransformPoint( relativeTransform, proxy.points[i] ); + } + + localProxy.points = localPoints; + localProxy.radius = proxy.radius; + + switch ( type ) + { + case b3_capsuleShape: + return b3OverlapCapsule( &sensorShape->capsule, b3Transform_identity, &localProxy ); + + case b3_compoundShape: + return b3OverlapCompound( sensorShape->compound , b3Transform_identity, &localProxy ); + + case b3_heightShape: + return b3OverlapHeightField( sensorShape->heightField, b3Transform_identity, &localProxy ); + + case b3_hullShape: + return b3OverlapHull( sensorShape->hull, b3Transform_identity, &localProxy ); + + case b3_meshShape: + return b3OverlapMesh( &sensorShape->mesh, b3Transform_identity, &localProxy ); + + case b3_sphereShape: + return b3OverlapSphere( &sensorShape->sphere, b3Transform_identity, &localProxy ); + + default: + B3_ASSERT( false ); + return false; + } +} + +// Sensor shapes need to +// - detect begin and end overlap events +// - events must be reported in deterministic order +// - maintain an active list of overlaps for query + +// Assumption +// - sensors don't detect shapes on the same body + +// Algorithm +// Query all sensors for overlaps +// Check against previous overlaps + +// Data structures +// Each sensor has an double buffered array of overlaps +// These overlaps use a shape reference with index and generation + +static bool b3SensorQueryCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + + b3SensorQueryContext* queryContext = (b3SensorQueryContext*)context; + b3Shape* sensorShape = queryContext->sensorShape; + int sensorShapeId = sensorShape->id; + + if ( shapeId == sensorShapeId ) + { + return true; + } + + b3World* world = queryContext->world; + b3Shape* otherShape = b3Array_Get( world->shapes, shapeId ); + + // Mesh vs mesh is not supported + if ( ( otherShape->type == b3_meshShape || otherShape->type == b3_heightShape ) && + ( sensorShape->type == b3_meshShape || sensorShape->type == b3_heightShape ) ) + { + return true; + } + + // Are sensor events enabled on the other shape? + if ( otherShape->enableSensorEvents == false ) + { + return true; + } + + // Skip shapes on the same body + if ( otherShape->bodyId == sensorShape->bodyId ) + { + return true; + } + + // Check filter + if ( b3ShouldShapesCollide( sensorShape->filter, otherShape->filter ) == false ) + { + return true; + } + + // Custom user filter + if ( sensorShape->enableCustomFiltering || otherShape->enableCustomFiltering ) + { + b3CustomFilterFcn* customFilterFcn = queryContext->world->customFilterFcn; + if ( customFilterFcn != NULL ) + { + b3ShapeId idA = { sensorShapeId + 1, world->worldId, sensorShape->generation }; + b3ShapeId idB = { shapeId + 1, world->worldId, otherShape->generation }; + bool shouldCollide = customFilterFcn( idA, idB, queryContext->world->customFilterContext ); + if ( shouldCollide == false ) + { + return true; + } + } + } + + b3Transform otherTransform = b3GetBodyTransform( world, otherShape->bodyId ); + + bool overlap = b3OverlapSensor( sensorShape, queryContext->transform, otherShape, otherTransform ); + if ( overlap == false ) + { + return true; + } + + // Record the overlap + b3Sensor* sensor = queryContext->sensor; + b3Visitor* shapeRef = b3Array_Emplace( sensor->overlaps2 ); + shapeRef->shapeId = shapeId; + shapeRef->generation = otherShape->generation; + + return true; +} + +static int b3CompareVisitors( const void* a, const void* b ) +{ + const b3Visitor* sa = (const b3Visitor*)a; + const b3Visitor* sb = (const b3Visitor*)b; + + if ( sa->shapeId < sb->shapeId ) + { + return -1; + } + + return 1; +} + +static void b3SensorTask( int startIndex, int endIndex, int workerIndex, void* context ) +{ + b3TracyCZoneNC( sensor_task, "Overlap", b3_colorBrown, true ); + + b3World* world = (b3World*)context; + B3_ASSERT( (int)workerIndex < world->workerCount ); + b3SensorTaskContext* taskContext = world->sensorTaskContexts.data + workerIndex; + + B3_ASSERT( startIndex < endIndex ); + + b3DynamicTree* trees = world->broadPhase.trees; + for ( int sensorIndex = startIndex; sensorIndex < endIndex; ++sensorIndex ) + { + b3Sensor* sensor = b3Array_Get( world->sensors, sensorIndex ); + b3Shape* sensorShape = b3Array_Get( world->shapes, sensor->shapeId ); + + // Swap overlap arrays + b3ArrayC( b3Visitor ) temp = sensor->overlaps1; + sensor->overlaps1 = sensor->overlaps2; + sensor->overlaps2 = temp; + b3Array_Clear( sensor->overlaps2 ); + + // Append sensor hits + b3Array_Append( sensor->overlaps2, sensor->hits.data, sensor->hits.count ); + + // Clear the hits + b3Array_Clear( sensor->hits ); + + b3Body* body = b3Array_Get( world->bodies, sensorShape->bodyId ); + if ( body->setIndex == b3_disabledSet || sensorShape->enableSensorEvents == false ) + { + if ( sensor->overlaps1.count != 0 ) + { + // This sensor is dropping all overlaps because it has been disabled. + b3SetBit( &taskContext->eventBits, sensorIndex ); + } + continue; + } + + b3Transform transform = b3GetBodyTransformQuick( world, body ); + + b3SensorQueryContext queryContext = { + .world = world, + .taskContext = taskContext, + .sensor = sensor, + .sensorShape = sensorShape, + .transform = transform, + }; + + B3_ASSERT( sensorShape->sensorIndex == sensorIndex ); + b3AABB queryBounds = sensorShape->aabb; + + // Query all trees + b3DynamicTree_Query( trees + 0, queryBounds, sensorShape->filter.maskBits, false, b3SensorQueryCallback, &queryContext ); + b3DynamicTree_Query( trees + 1, queryBounds, sensorShape->filter.maskBits, false, b3SensorQueryCallback, &queryContext ); + b3DynamicTree_Query( trees + 2, queryBounds, sensorShape->filter.maskBits, false, b3SensorQueryCallback, &queryContext ); + + // Sort the overlaps to enable finding begin and end events. + qsort( sensor->overlaps2.data, sensor->overlaps2.count, sizeof( b3Visitor ), b3CompareVisitors ); + + // Remove duplicates from overlaps2 (sorted). Duplicates are possible due to the hit events appended earlier. + int uniqueCount = 0; + int overlapCount = sensor->overlaps2.count; + b3Visitor* overlapData = sensor->overlaps2.data; + for ( int i = 0; i < overlapCount; ++i ) + { + if ( uniqueCount == 0 || overlapData[i].shapeId != overlapData[uniqueCount - 1].shapeId ) + { + overlapData[uniqueCount] = overlapData[i]; + uniqueCount += 1; + } + } + sensor->overlaps2.count = uniqueCount; + + int count1 = sensor->overlaps1.count; + int count2 = sensor->overlaps2.count; + if ( count1 != count2 ) + { + // something changed + b3SetBit( &taskContext->eventBits, sensorIndex ); + } + else + { + for ( int i = 0; i < count1; ++i ) + { + b3Visitor* s1 = sensor->overlaps1.data + i; + b3Visitor* s2 = sensor->overlaps2.data + i; + + if ( s1->shapeId != s2->shapeId || s1->generation != s2->generation ) + { + // something changed + b3SetBit( &taskContext->eventBits, sensorIndex ); + break; + } + } + } + } + + b3TracyCZoneEnd( sensor_task ); +} + +void b3OverlapSensors( b3World* world ) +{ + int sensorCount = world->sensors.count; + if ( sensorCount == 0 ) + { + return; + } + + B3_ASSERT( world->workerCount > 0 ); + + b3TracyCZoneNC( overlap_sensors, "Sensors", b3_colorMediumPurple, true ); + + for ( int i = 0; i < world->workerCount; ++i ) + { + b3SetBitCountAndClear( &world->sensorTaskContexts.data[i].eventBits, sensorCount ); + } + + // Parallel-for sensors overlaps + int minRange = 16; + b3ParallelFor( world, b3SensorTask, sensorCount, minRange, world, "sensors" ); + + b3TracyCZoneNC( sensor_state, "Events", b3_colorLightSlateGray, true ); + + b3BitSet* bitSet = &world->sensorTaskContexts.data[0].eventBits; + for ( int i = 1; i < world->workerCount; ++i ) + { + b3InPlaceUnion( bitSet, &world->sensorTaskContexts.data[i].eventBits ); + } + + // Iterate sensors bits and publish events + // Process sensor state changes. Iterate over set bits + uint64_t* bits = bitSet->bits; + uint32_t blockCount = bitSet->blockCount; + + for ( uint32_t k = 0; k < blockCount; ++k ) + { + uint64_t word = bits[k]; + while ( word != 0 ) + { + uint32_t ctz = b3CTZ64( word ); + int sensorIndex = (int)( 64 * k + ctz ); + + b3Sensor* sensor = b3Array_Get( world->sensors, sensorIndex ); + b3Shape* sensorShape = b3Array_Get( world->shapes, sensor->shapeId ); + b3ShapeId sensorId = { sensor->shapeId + 1, world->worldId, sensorShape->generation }; + + int count1 = sensor->overlaps1.count; + int count2 = sensor->overlaps2.count; + const b3Visitor* refs1 = sensor->overlaps1.data; + const b3Visitor* refs2 = sensor->overlaps2.data; + + // overlaps1 can have overlaps that end + // overlaps2 can have overlaps that begin + int index1 = 0, index2 = 0; + while ( index1 < count1 && index2 < count2 ) + { + const b3Visitor* r1 = refs1 + index1; + const b3Visitor* r2 = refs2 + index2; + if ( r1->shapeId == r2->shapeId ) + { + if ( r1->generation < r2->generation ) + { + // end + b3ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation }; + b3SensorEndTouchEvent event = { + .sensorShapeId = sensorId, + .visitorShapeId = visitorId, + }; + b3Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event ); + index1 += 1; + } + else if ( r1->generation > r2->generation ) + { + // begin + b3ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation }; + b3SensorBeginTouchEvent event = { sensorId, visitorId }; + b3Array_Push( world->sensorBeginEvents, event ); + index2 += 1; + } + else + { + // persisted + index1 += 1; + index2 += 1; + } + } + else if ( r1->shapeId < r2->shapeId ) + { + // end + b3ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation }; + b3SensorEndTouchEvent event = { sensorId, visitorId }; + b3Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event ); + index1 += 1; + } + else + { + // begin + b3ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation }; + b3SensorBeginTouchEvent event = { sensorId, visitorId }; + b3Array_Push( world->sensorBeginEvents, event ); + index2 += 1; + } + } + + while ( index1 < count1 ) + { + // end + const b3Visitor* r1 = refs1 + index1; + b3ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation }; + b3SensorEndTouchEvent event = { sensorId, visitorId }; + b3Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event ); + index1 += 1; + } + + while ( index2 < count2 ) + { + // begin + const b3Visitor* r2 = refs2 + index2; + b3ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation }; + b3SensorBeginTouchEvent event = { sensorId, visitorId }; + b3Array_Push( world->sensorBeginEvents, event ); + index2 += 1; + } + + // Clear the smallest set bit + word = word & ( word - 1 ); + } + } + + b3TracyCZoneEnd( sensor_state ); + b3TracyCZoneEnd( overlap_sensors ); +} + +void b3DestroySensor( b3World* world, b3Shape* sensorShape ) +{ + b3Sensor* sensor = b3Array_Get( world->sensors, sensorShape->sensorIndex ); + for ( int i = 0; i < sensor->overlaps2.count; ++i ) + { + b3Visitor* ref = sensor->overlaps2.data + i; + b3SensorEndTouchEvent event = { + .sensorShapeId = + { + .index1 = sensorShape->id + 1, + .world0 = world->worldId, + .generation = sensorShape->generation, + }, + .visitorShapeId = + { + .index1 = ref->shapeId + 1, + .world0 = world->worldId, + .generation = ref->generation, + }, + }; + + b3Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event ); + } + + // Destroy sensor + b3Array_Destroy( sensor->hits ); + b3Array_Destroy( sensor->overlaps1 ); + b3Array_Destroy( sensor->overlaps2 ); + + int movedIndex = b3Array_RemoveSwap( world->sensors, sensorShape->sensorIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fixup moved sensor + b3Sensor* movedSensor = b3Array_Get( world->sensors, sensorShape->sensorIndex ); + b3Shape* otherSensorShape = b3Array_Get( world->shapes, movedSensor->shapeId ); + otherSensorShape->sensorIndex = sensorShape->sensorIndex; + } +} diff --git a/src/sensor.h b/src/sensor.h new file mode 100644 index 0000000..038d076 --- /dev/null +++ b/src/sensor.h @@ -0,0 +1,51 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "bitset.h" +#include "container.h" + +typedef struct b3Shape b3Shape; +typedef struct b3World b3World; + +// Used to track shapes that hit sensors using time of impact +typedef struct b3SensorHit +{ + int sensorId; + int visitorId; +} b3SensorHit; + +typedef struct b3Visitor +{ + int shapeId; + uint16_t generation; +} b3Visitor; + +b3DeclareArray( b3Visitor ); + +typedef struct b3Sensor +{ + b3ArrayC( b3Visitor ) hits; + b3ArrayC( b3Visitor ) overlaps1; + b3ArrayC( b3Visitor ) overlaps2; + int shapeId; +} b3Sensor; + +typedef struct b3SensorTaskContext +{ + b3BitSet eventBits; +} b3SensorTaskContext; + +#ifdef __cplusplus +extern "C" +{ +#endif + +void b3OverlapSensors( b3World* world ); + +void b3DestroySensor( b3World* world, b3Shape* sensorShape ); + +#ifdef __cplusplus +} +#endif diff --git a/src/shape.c b/src/shape.c new file mode 100644 index 0000000..ba9b923 --- /dev/null +++ b/src/shape.c @@ -0,0 +1,2340 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "shape.h" + +#include "body.h" +#include "broad_phase.h" +#include "contact.h" +#include "physics_world.h" +#include "sensor.h" + +// needed for dll export +#include "aabb.h" +#include "compound.h" + +#include "box3d/box3d.h" + +#include +#include + +static b3Shape* b3GetShape( b3World* world, b3ShapeId shapeId ) +{ + int id = shapeId.index1 - 1; + b3Shape* shape = b3Array_Get( world->shapes, id ); + B3_ASSERT( shape->id == id && shape->generation == shapeId.generation ); + return shape; +} + +static void b3UpdateShapeAABBs( b3Shape* shape, b3Transform transform, b3BodyType proxyType ) +{ + // Compute a bounding box with a speculative margin + const float speculativeDistance = B3_SPECULATIVE_DISTANCE; + const float aabbMargin = B3_AABB_MARGIN; + + b3AABB aabb = b3ComputeShapeAABB( shape, transform ); + aabb.lowerBound.x -= speculativeDistance; + aabb.lowerBound.y -= speculativeDistance; + aabb.lowerBound.z -= speculativeDistance; + aabb.upperBound.x += speculativeDistance; + aabb.upperBound.y += speculativeDistance; + aabb.upperBound.z += speculativeDistance; + shape->aabb = aabb; + + // Smaller margin for static bodies. Cannot be zero due to TOI tolerance. + float margin = proxyType == b3_staticBody ? speculativeDistance : aabbMargin; + b3AABB fatAABB; + fatAABB.lowerBound.x = aabb.lowerBound.x - margin; + fatAABB.lowerBound.y = aabb.lowerBound.y - margin; + fatAABB.lowerBound.z = aabb.lowerBound.z - margin; + fatAABB.upperBound.x = aabb.upperBound.x + margin; + fatAABB.upperBound.y = aabb.upperBound.y + margin; + fatAABB.upperBound.z = aabb.upperBound.z + margin; + shape->fatAABB = fatAABB; + + B3_VALIDATE( b3IsSaneAABB( fatAABB ) ); +} + +static b3Shape* b3CreateShapeInternal( b3World* world, b3Body* body, b3Transform bodyTransform, const b3ShapeDef* def, + const void* geometry, b3ShapeType shapeType, b3Transform shapeTransform, b3Vec3 scale, + bool haveShapeTransform ) +{ + int shapeId = b3AllocId( &world->shapeIdPool ); + + if ( shapeId == world->shapes.count ) + { + b3Array_Push( world->shapes, (b3Shape){ 0 } ); + } + else + { + B3_ASSERT( world->shapes.data[shapeId].id == B3_NULL_INDEX ); + } + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + switch ( shapeType ) + { + case b3_capsuleShape: + shape->capsule = *(b3Capsule*)geometry; + break; + + case b3_compoundShape: + // Compounds must be a static and not a sensor + B3_ASSERT( body->type == b3_staticBody ); + B3_ASSERT( def->isSensor == false ); + shape->compound = (b3Compound*)geometry; + break; + + case b3_sphereShape: + shape->sphere = *(b3Sphere*)geometry; + break; + + case b3_hullShape: + if ( haveShapeTransform ) + { + shape->hull = b3CloneAndTransformHull( (b3Hull*)geometry, shapeTransform, scale ); + if ( shape->hull == NULL ) + { + // This can fail to produce a valid hull in extreme cases + b3FreeId( &world->shapeIdPool, shapeId ); + shape->id = B3_NULL_INDEX; + return NULL; + } + } + else + { + shape->hull = b3CloneHull( (b3Hull*)geometry ); + } + break; + + case b3_meshShape: + { + shape->mesh.data = (b3MeshData*)geometry; + shape->mesh.scale = b3SafeScale( scale ); + } + break; + + case b3_heightShape: + shape->heightField = (b3HeightField*)geometry; + break; + + default: + B3_ASSERT( false ); + break; + } + + shape->id = shapeId; + shape->bodyId = body->id; + shape->type = shapeType; + shape->density = def->density; + shape->explosionScale = def->explosionScale; + shape->filter = def->filter; + shape->userData = def->userData; + shape->userShape = NULL; + shape->enlargedAABB = false; + shape->enableSensorEvents = def->enableSensorEvents; + shape->enableContactEvents = def->enableContactEvents; + shape->enableCustomFiltering = def->enableCustomFiltering; + shape->enableHitEvents = def->enableHitEvents; + shape->enablePreSolveEvents = def->enablePreSolveEvents; + shape->proxyKey = B3_NULL_INDEX; + shape->localCentroid = b3GetShapeCentroid( shape ); + shape->aabb = (b3AABB){ b3Vec3_zero, b3Vec3_zero }; + shape->fatAABB = (b3AABB){ b3Vec3_zero, b3Vec3_zero }; + shape->generation += 1; + + if ( def->name ) + { + size_t length = strlen( def->name ) + 1; + shape->name = (char*)b3Alloc( length * sizeof( char ) ); + memcpy( shape->name, def->name, length ); + } + else + { + shape->name = NULL; + } + + if ( shape->type == b3_compoundShape ) + { + // Unfortunate const removal + shape->materials = (b3SurfaceMaterial*)b3GetCompoundMaterials( shape->compound ); + shape->materialCount = shape->compound->materialCount; + } + else if ( def->materialCount > 0 && def->materials != NULL ) + { + b3SurfaceMaterial* materials = (b3SurfaceMaterial*)b3Alloc( def->materialCount * sizeof( b3SurfaceMaterial ) ); + memcpy( materials, def->materials, def->materialCount * sizeof( b3SurfaceMaterial ) ); + shape->materials = materials; + shape->materialCount = def->materialCount; + } + else + { + b3SurfaceMaterial* materials = (b3SurfaceMaterial*)b3Alloc( sizeof( b3SurfaceMaterial ) ); + memcpy( materials, &def->baseMaterial, sizeof( b3SurfaceMaterial ) ); + shape->materials = materials; + shape->materialCount = 1; + } + + if ( body->setIndex != b3_disabledSet ) + { + b3BodyType proxyType = body->type; + bool forcePairCreation = def->invokeContactCreation && shape->type != b3_compoundShape; + b3CreateShapeProxy( shape, &world->broadPhase, proxyType, bodyTransform, forcePairCreation ); + } + + // Add to shape doubly linked list + if ( body->headShapeId != B3_NULL_INDEX ) + { + b3Shape* headShape = b3Array_Get( world->shapes, body->headShapeId ); + headShape->prevShapeId = shapeId; + } + + shape->prevShapeId = B3_NULL_INDEX; + shape->nextShapeId = body->headShapeId; + body->headShapeId = shapeId; + body->shapeCount += 1; + + if ( def->isSensor ) + { + shape->sensorIndex = world->sensors.count; + b3Sensor* sensor = b3Array_Emplace( world->sensors ); + b3Array_CreateN( sensor->hits, 4 ); + b3Array_CreateN( sensor->overlaps1, 16 ); + b3Array_CreateN( sensor->overlaps2, 16 ); + sensor->shapeId = shapeId; + } + else + { + shape->sensorIndex = B3_NULL_INDEX; + } + + b3ValidateSolverSets( world ); + + return shape; +} + +static b3ShapeId b3CreateShape( b3BodyId bodyId, const b3ShapeDef* def, const void* geometry, b3ShapeType shapeType, + b3Transform transform, b3Vec3 scale, bool haveTransform ) +{ + B3_CHECK_DEF( def ); + B3_ASSERT( b3IsValidFloat( def->density ) && def->density >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->baseMaterial.friction ) && def->baseMaterial.friction >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->baseMaterial.restitution ) && def->baseMaterial.restitution >= 0.0f ); + + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return (b3ShapeId){ 0 }; + } + + if ( world->shapes.count == B3_MAX_SHAPES && world->shapeIdPool.freeArray.count == 0 ) + { + B3_ASSERT( false ); + return b3_nullShapeId; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + if ( body->type != b3_staticBody && ( shapeType == b3_compoundShape || shapeType == b3_heightShape ) ) + { + // Compound and height shapes must be on static bodies. + return b3_nullShapeId; + } + + world->locked = true; + + b3Transform bodyTransform = b3GetBodyTransformQuick( world, body ); + + b3Shape* shape = + b3CreateShapeInternal( world, body, bodyTransform, def, geometry, shapeType, transform, scale, haveTransform ); + + if ( shape == NULL ) + { + world->locked = false; + return b3_nullShapeId; + } + + if ( def->updateBodyMass == true ) + { + b3UpdateBodyMassData( world, body ); + } + + b3ValidateSolverSets( world ); + + b3ShapeId id = { shape->id + 1, bodyId.world0, shape->generation }; + + world->locked = false; + + return id; +} + +b3ShapeId b3CreateSphereShape( b3BodyId bodyId, const b3ShapeDef* def, const b3Sphere* sphere ) +{ + return b3CreateShape( bodyId, def, sphere, b3_sphereShape, b3Transform_identity, b3Vec3_one, false ); +} + +b3ShapeId b3CreateCapsuleShape( b3BodyId bodyId, const b3ShapeDef* def, const b3Capsule* capsule ) +{ + float lengthSqr = b3DistanceSquared( capsule->center1, capsule->center2 ); + if ( lengthSqr <= B3_LINEAR_SLOP * B3_LINEAR_SLOP ) + { + b3Sphere sphere = { b3Lerp( capsule->center1, capsule->center2, 0.5f ), capsule->radius }; + return b3CreateShape( bodyId, def, &sphere, b3_sphereShape, b3Transform_identity, b3Vec3_one, false ); + } + + return b3CreateShape( bodyId, def, capsule, b3_capsuleShape, b3Transform_identity, b3Vec3_one, false ); +} + +b3ShapeId b3CreateHullShape( b3BodyId bodyId, const b3ShapeDef* def, const b3Hull* hull ) +{ + B3_VALIDATE( b3IsValidHull( hull ) ); + B3_VALIDATE( hull->hash != 0 ); + return b3CreateShape( bodyId, def, hull, b3_hullShape, b3Transform_identity, b3Vec3_one, false ); +} + +b3ShapeId b3CreateTransformedHullShape( b3BodyId bodyId, const b3ShapeDef* def, const b3Hull* hull, b3Transform transform, + b3Vec3 scale ) +{ + B3_VALIDATE( b3IsValidHull( hull ) ); + return b3CreateShape( bodyId, def, hull, b3_hullShape, transform, scale, true ); +} + +b3ShapeId b3CreateMeshShape( b3BodyId bodyId, const b3ShapeDef* def, const b3MeshData* mesh, b3Vec3 scale ) +{ + B3_VALIDATE( b3IsValidMesh( mesh ) ); + B3_VALIDATE( mesh->hash != 0 ); + return b3CreateShape( bodyId, def, mesh, b3_meshShape, b3Transform_identity, scale, true ); +} + +b3ShapeId b3CreateHeightFieldShape( b3BodyId bodyId, const b3ShapeDef* def, const b3HeightField* heightField ) +{ + B3_VALIDATE( heightField->hash != 0 ); + return b3CreateShape( bodyId, def, heightField, b3_heightShape, b3Transform_identity, b3Vec3_one, false ); +} + +b3ShapeId b3CreateCompoundShape( b3BodyId bodyId, b3ShapeDef* def, const b3Compound* compound ) +{ + return b3CreateShape( bodyId, def, compound, b3_compoundShape, b3Transform_identity, b3Vec3_one, false ); +} + +// Destroy a shape on a body. This doesn't need to be called when destroying a body. +static void b3DestroyShapeInternal( b3World* world, b3Shape* shape, b3Body* body, bool wakeBodies ) +{ + int shapeId = shape->id; + + // Remove the shape from the body's doubly linked list. + if ( shape->prevShapeId != B3_NULL_INDEX ) + { + b3Shape* prevShape = b3Array_Get( world->shapes, shape->prevShapeId ); + prevShape->nextShapeId = shape->nextShapeId; + } + + if ( shape->nextShapeId != B3_NULL_INDEX ) + { + b3Shape* nextShape = b3Array_Get( world->shapes, shape->nextShapeId ); + nextShape->prevShapeId = shape->prevShapeId; + } + + if ( shapeId == body->headShapeId ) + { + body->headShapeId = shape->nextShapeId; + } + + body->shapeCount -= 1; + + // Remove from broad-phase. + b3DestroyShapeProxy( shape, &world->broadPhase ); + + // Destroy any contacts associated with the shape. + int contactKey = body->headContactKey; + while ( contactKey != B3_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->shapeIdA == shapeId || contact->shapeIdB == shapeId ) + { + b3DestroyContact( world, contact, wakeBodies ); + } + } + + if ( shape->sensorIndex != B3_NULL_INDEX ) + { + b3Sensor* sensor = b3Array_Get( world->sensors, shape->sensorIndex ); + for ( int i = 0; i < sensor->overlaps2.count; ++i ) + { + b3Visitor* ref = sensor->overlaps2.data + i; + b3SensorEndTouchEvent event = { + .sensorShapeId = + { + .index1 = shapeId + 1, + .world0 = world->worldId, + .generation = shape->generation, + }, + .visitorShapeId = + { + .index1 = ref->shapeId + 1, + .world0 = world->worldId, + .generation = ref->generation, + }, + }; + + b3Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event ); + } + + // Destroy sensor + b3Array_Destroy( sensor->hits ); + b3Array_Destroy( sensor->overlaps1 ); + b3Array_Destroy( sensor->overlaps2 ); + + int movedIndex = b3Array_RemoveSwap( world->sensors, shape->sensorIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fixup moved sensor + b3Sensor* movedSensor = b3Array_Get( world->sensors, shape->sensorIndex ); + b3Shape* otherSensorShape = b3Array_Get( world->shapes, movedSensor->shapeId ); + otherSensorShape->sensorIndex = shape->sensorIndex; + } + } + + // Destroy every shape member from b3Alloc + b3DestroyShapeAllocations( world, shape ); + + // Return shape to free list. + b3FreeId( &world->shapeIdPool, shapeId ); + shape->id = B3_NULL_INDEX; + + b3ValidateSolverSets( world ); +} + +void b3DestroyShape( b3ShapeId shapeId, bool updateBodyMass ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + world->locked = true; + + b3Shape* shape = b3GetShape( world, shapeId ); + + // need to wake bodies because this might be a static body + bool wakeBodies = true; + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3DestroyShapeInternal( world, shape, body, wakeBodies ); + + if ( updateBodyMass == true ) + { + b3UpdateBodyMassData( world, body ); + } + + world->locked = false; +} + +b3AABB b3ComputeShapeAABB( const b3Shape* shape, b3Transform transform ) +{ + switch ( shape->type ) + { + case b3_capsuleShape: + return b3ComputeCapsuleAABB( &shape->capsule, transform ); + + case b3_compoundShape: + return b3ComputeCompoundAABB( shape->compound, transform ); + + case b3_heightShape: + return b3ComputeHeightFieldAABB( shape->heightField, transform ); + + case b3_hullShape: + return b3ComputeHullAABB( shape->hull, transform ); + + case b3_meshShape: + return b3ComputeMeshAABB( shape->mesh.data, transform, shape->mesh.scale ); + + case b3_sphereShape: + return b3ComputeSphereAABB( &shape->sphere, transform ); + + default: + { + B3_ASSERT( false ); + b3AABB empty = { transform.p, transform.p }; + return empty; + } + } +} + +b3AABB b3ComputeSweptShapeAABB( const b3Shape* shape, const b3Sweep* sweep, float time ) +{ + B3_ASSERT( 0.0f <= time && time <= 1.0f ); + b3Transform xf1 = { b3Sub( sweep->c1, b3RotateVector( sweep->q1, sweep->localCenter ) ), sweep->q1 }; + b3Transform xf2 = b3GetSweepTransform( sweep, time ); + + switch ( shape->type ) + { + case b3_capsuleShape: + return b3ComputeSweptCapsuleAABB( &shape->capsule, xf1, xf2 ); + + case b3_hullShape: + return b3ComputeSweptHullAABB( shape->hull, xf1, xf2 ); + + case b3_sphereShape: + return b3ComputeSweptSphereAABB( &shape->sphere, xf1, xf2 ); + + default: + B3_ASSERT( false ); + return (b3AABB){ xf1.p, xf1.p }; + } +} + +b3Vec3 b3GetShapeCentroid( const b3Shape* shape ) +{ + switch ( shape->type ) + { + case b3_capsuleShape: + return b3Lerp( shape->capsule.center1, shape->capsule.center2, 0.5f ); + case b3_compoundShape: + { + b3AABB aabb = b3ComputeCompoundAABB( shape->compound, b3Transform_identity ); + return b3AABB_Center( aabb ); + } + case b3_sphereShape: + return shape->sphere.center; + case b3_hullShape: + return shape->hull->center; + case b3_meshShape: + { + b3AABB aabb = b3ComputeMeshAABB( shape->mesh.data, b3Transform_identity, shape->mesh.scale ); + return b3AABB_Center( aabb ); + } + case b3_heightShape: + { + b3AABB aabb = b3ComputeHeightFieldAABB( shape->heightField, b3Transform_identity ); + return b3AABB_Center( aabb ); + } + default: + return b3Vec3_zero; + } +} + +float b3GetShapeArea( const b3Shape* shape ) +{ + // todo_erin fix these + switch ( shape->type ) + { + case b3_capsuleShape: + return 2.0f * b3Length( b3Sub( shape->capsule.center1, shape->capsule.center2 ) ) + + 2.0f * B3_PI * shape->capsule.radius; + + case b3_hullShape: + return shape->hull->surfaceArea; + + case b3_sphereShape: + return 2.0f * B3_PI * shape->sphere.radius; + + default: + return 0.0f; + } +} + +// This projects the shape surface area onto a plane +float b3GetShapeProjectedArea( const b3Shape* shape, b3Vec3 planeNormal ) +{ + switch ( shape->type ) + { + case b3_capsuleShape: + { + float radius = shape->capsule.radius; + b3Vec3 axis = b3Sub( shape->capsule.center2, shape->capsule.center1 ); + float projectedLength = b3Length( b3Cross( axis, planeNormal ) ); + float cylinderArea = 2.0f * radius * projectedLength; + float sphereArea = B3_PI * radius * radius; + return sphereArea + cylinderArea; + } + + case b3_hullShape: + return b3ComputeHullProjectedArea( shape->hull, planeNormal ); + + case b3_sphereShape: + return B3_PI * shape->sphere.radius * shape->sphere.radius; + + default: + return 0.0f; + } +} + +b3MassData b3ComputeShapeMass( const b3Shape* shape ) +{ + switch ( shape->type ) + { + case b3_capsuleShape: + return b3ComputeCapsuleMass( &shape->capsule, shape->density ); + + case b3_hullShape: + return b3ComputeHullMass( shape->hull, shape->density ); + + case b3_sphereShape: + return b3ComputeSphereMass( &shape->sphere, shape->density ); + + default: + return (b3MassData){ 0 }; + } +} + +b3ShapeExtent b3ComputeShapeExtent( const b3Shape* shape, b3Vec3 localCenter ) +{ + b3ShapeExtent extent = { 0 }; + + switch ( shape->type ) + { + case b3_capsuleShape: + { + float radius = shape->capsule.radius; + extent.minExtent = radius; + b3Vec3 c1 = b3Sub( shape->capsule.center1, localCenter ); + b3Vec3 c2 = b3Sub( shape->capsule.center2, localCenter ); + b3Vec3 r = { radius, radius, radius }; + extent.maxExtent = b3Add( b3Max( c1, c2 ), r ); + } + break; + + case b3_compoundShape: + { + // This is shouldn't be needed but here for completeness + b3AABB aabb = b3ComputeCompoundAABB( shape->compound, b3Transform_identity ); + float r1 = b3Length( b3Sub( aabb.lowerBound, localCenter ) ); + float r2 = b3Length( b3Sub( aabb.upperBound, localCenter ) ); + extent.minExtent = b3MinFloat( r1, r2 ); + b3Vec3 p = b3FarthestPointOnAABB( aabb, localCenter ); + extent.maxExtent = b3Abs( b3Sub( p, localCenter ) ); + } + break; + + case b3_sphereShape: + { + float radius = shape->sphere.radius; + extent.minExtent = radius; + b3Vec3 r = { radius, radius, radius }; + b3Vec3 p = b3Add( b3Sub( shape->sphere.center, localCenter ), r ); + extent.maxExtent = b3Abs( b3Sub( p, localCenter ) ); + } + break; + + case b3_hullShape: + extent = b3ComputeHullExtent( shape->hull, localCenter ); + break; + + case b3_meshShape: + { + // This is needed for kinematic mesh sleeping + b3AABB aabb = b3ComputeMeshAABB( shape->mesh.data, b3Transform_identity, shape->mesh.scale ); + float r1 = b3Length( b3Sub( aabb.lowerBound, localCenter ) ); + float r2 = b3Length( b3Sub( aabb.upperBound, localCenter ) ); + extent.minExtent = b3MinFloat( r1, r2 ); + b3Vec3 p = b3FarthestPointOnAABB( aabb, localCenter ); + extent.maxExtent = b3Abs( p ); + } + break; + + default: + break; + } + + return extent; +} + +b3CastOutput b3RayCastShape( const b3Shape* shape, b3Transform transform, const b3RayCastInput* input ) +{ + b3RayCastInput localInput = *input; + localInput.origin = b3InvTransformPoint( transform, input->origin ); + localInput.translation = b3InvRotateVector( transform.q, input->translation ); + + b3CastOutput output = { 0 }; + switch ( shape->type ) + { + case b3_capsuleShape: + output = b3RayCastCapsule( &shape->capsule, &localInput ); + break; + case b3_compoundShape: + output = b3RayCastCompound( shape->compound, &localInput ); + break; + case b3_sphereShape: + output = b3RayCastSphere( &shape->sphere, &localInput ); + break; + case b3_hullShape: + output = b3RayCastHull( shape->hull, &localInput ); + break; + case b3_meshShape: + output = b3RayCastMesh( &shape->mesh, &localInput ); + break; + case b3_heightShape: + output = b3RayCastHeightField( shape->heightField, &localInput ); + break; + default: + return output; + } + + output.point = b3TransformPoint( transform, output.point ); + output.normal = b3RotateVector( transform.q, output.normal ); + return output; +} + +b3CastOutput b3ShapeCastShape( const b3Shape* shape, b3Transform transform, const b3ShapeCastInput* input ) +{ + b3ShapeCastInput localInput = *input; + b3Vec3 localPoints[B3_MAX_SHAPE_CAST_POINTS]; + + localInput.proxy.count = b3MinInt( input->proxy.count, B3_MAX_SHAPE_CAST_POINTS ); + for ( int i = 0; i < localInput.proxy.count; ++i ) + { + localPoints[i] = b3InvTransformPoint( transform, input->proxy.points[i] ); + } + + localInput.proxy.points = localPoints; + localInput.translation = b3InvRotateVector( transform.q, input->translation ); + + b3CastOutput output = { 0 }; + switch ( shape->type ) + { + case b3_capsuleShape: + output = b3ShapeCastCapsule( &shape->capsule, &localInput ); + break; + + case b3_compoundShape: + output = b3ShapeCastCompound( shape->compound, &localInput ); + break; + + case b3_heightShape: + output = b3ShapeCastHeightField( shape->heightField, &localInput ); + break; + + case b3_hullShape: + output = b3ShapeCastHull( shape->hull, &localInput ); + break; + + case b3_meshShape: + output = b3ShapeCastMesh( &shape->mesh, &localInput ); + break; + + case b3_sphereShape: + output = b3ShapeCastSphere( &shape->sphere, &localInput ); + break; + default: + return output; + } + + output.point = b3TransformPoint( transform, output.point ); + output.normal = b3RotateVector( transform.q, output.normal ); + return output; +} + +bool b3OverlapShape( const b3Shape* shape, b3Transform transform, const b3ShapeProxy* proxy ) +{ + b3ShapeType type = shape->type; + switch ( type ) + { + case b3_capsuleShape: + return b3OverlapCapsule( &shape->capsule, transform, proxy ); + + case b3_compoundShape: + return b3OverlapCompound( shape->compound, transform, proxy ); + + case b3_heightShape: + return b3OverlapHeightField( shape->heightField, transform, proxy ); + + case b3_hullShape: + return b3OverlapHull( shape->hull, transform, proxy ); + + case b3_meshShape: + return b3OverlapMesh( &shape->mesh, transform, proxy ); + + case b3_sphereShape: + return b3OverlapSphere( &shape->sphere, transform, proxy ); + + default: + B3_ASSERT( false ); + return false; + } + +#if 0 + b3Vec3 localPoints[B3_MAX_SHAPE_CAST_POINTS]; + b3ShapeProxy localProxy; + + b3Transform invTransform = b3InvertTransform( transform ); + b3Matrix3 R = b3MakeMatrixFromQuat( invTransform.q ); + + localProxy.count = b3MinInt( proxy->count, B3_MAX_SHAPE_CAST_POINTS ); + for ( int i = 0; i < localProxy.count; ++i ) + { + localPoints[i] = b3Add( b3MulMV( R, proxy->points[i] ), invTransform.p ); + } + + localProxy.points = localPoints; + localProxy.radius = proxy->radius; + + if ( type == b3_meshShape ) + { + return b3OverlapMesh( &localProxy, shape->mesh.data, shape->mesh.scale ); + } + + B3_ASSERT( type == b3_heightShape ); + + return b3OverlapHeightField( &localProxy, shape->heightField ); +#endif +} + +int b3CollideMover( b3PlaneResult* planes, int planeCapacity, const b3Shape* shape, b3Transform transform, + const b3Capsule* mover ) +{ + if ( planeCapacity == 0 ) + { + return 0; + } + + b3Capsule localMover; + localMover.center1 = b3InvTransformPoint( transform, mover->center1 ); + localMover.center2 = b3InvTransformPoint( transform, mover->center2 ); + localMover.radius = mover->radius; + + int planeCount = 0; + switch ( shape->type ) + { + case b3_capsuleShape: + planeCount = b3CollideMoverAndCapsule( planes, &shape->capsule, &localMover ); + break; + + case b3_compoundShape: + planeCount = b3CollideMoverAndCompound( planes, planeCapacity, shape->compound, &localMover ); + break; + + case b3_sphereShape: + planeCount = b3CollideMoverAndSphere( planes, &shape->sphere, &localMover ); + break; + + case b3_hullShape: + planeCount = b3CollideMoverAndHull( planes, shape->hull, &localMover ); + break; + + case b3_meshShape: + planeCount = b3CollideMoverAndMesh( planes, planeCapacity, &shape->mesh, &localMover ); + break; + + case b3_heightShape: + planeCount = b3CollideMoverAndHeightField( planes, planeCapacity, shape->heightField, &localMover ); + break; + + default: + B3_ASSERT( false ); + break; + } + + for ( int i = 0; i < planeCount; ++i ) + { + planes[i].plane.normal = b3RotateVector( transform.q, planes[i].plane.normal ); + planes[i].point = b3TransformPoint( transform, planes[i].point ); + } + + return planeCount; +} + +void b3CreateShapeProxy( b3Shape* shape, b3BroadPhase* bp, b3BodyType type, b3Transform transform, bool forcePairCreation ) +{ + B3_ASSERT( shape->proxyKey == B3_NULL_INDEX ); + + b3UpdateShapeAABBs( shape, transform, type ); + + // Create proxies in the broad-phase. + shape->proxyKey = + b3BroadPhase_CreateProxy( bp, type, shape->fatAABB, shape->filter.categoryBits, shape->id, forcePairCreation ); + B3_ASSERT( B3_PROXY_TYPE( shape->proxyKey ) < b3_bodyTypeCount ); +} + +void b3DestroyShapeProxy( b3Shape* shape, b3BroadPhase* bp ) +{ + if ( shape->proxyKey != B3_NULL_INDEX ) + { + b3BroadPhase_DestroyProxy( bp, shape->proxyKey ); + shape->proxyKey = B3_NULL_INDEX; + } +} + +static void b3DestroyShapeAllocationForShapeChange( b3World* world, b3Shape* shape ) +{ + b3ShapeType type = shape->type; + switch ( type ) + { + case b3_hullShape: + b3DestroyHull( (b3Hull*)shape->hull ); + shape->hull = NULL; + break; + + default: + break; + } + + if ( shape->userShape != NULL ) + { + world->destroyDebugShape( shape->userShape, world->userDebugShapeContext ); + shape->userShape = NULL; + } +} + +void b3DestroyShapeAllocations( b3World* world, b3Shape* shape ) +{ + b3DestroyShapeAllocationForShapeChange( world, shape ); + + if ( shape->materials != NULL && shape->type != b3_compoundShape ) + { + B3_ASSERT( shape->materialCount > 0 ); + b3Free( shape->materials, shape->materialCount * sizeof( b3SurfaceMaterial ) ); + shape->materials = NULL; + shape->materialCount = 0; + } + + if ( shape->name != NULL ) + { + b3Free( shape->name, ( strlen( shape->name ) + 1 ) * sizeof( char ) ); + shape->name = NULL; + } + + // Sensor data is destroyed elsewhere +} + +b3ShapeProxy b3MakeShapeProxy( const b3Shape* shape ) +{ + switch ( shape->type ) + { + case b3_capsuleShape: + return (b3ShapeProxy){ &shape->capsule.center1, 2, shape->capsule.radius }; + + case b3_sphereShape: + return (b3ShapeProxy){ &shape->sphere.center, 1, shape->sphere.radius }; + + case b3_hullShape: + { + const b3Vec3* points = b3GetHullPoints( shape->hull ); + return (b3ShapeProxy){ points, shape->hull->vertexCount, 0.0f }; + } + + default: + { + B3_ASSERT( false ); + return (b3ShapeProxy){ 0 }; + } + } +} + +b3ShapeProxy b3MakeLocalProxy( const b3ShapeProxy* proxy, b3Transform transform, b3Vec3* buffer ) +{ + b3Transform invTransform = b3InvertTransform( transform ); + b3Matrix3 R = b3MakeMatrixFromQuat( invTransform.q ); + + int count = b3MinInt( proxy->count, B3_MAX_SHAPE_CAST_POINTS ); + for ( int i = 0; i < count; ++i ) + { + buffer[i] = b3Add( b3MulMV( R, proxy->points[i] ), invTransform.p ); + } + + return (b3ShapeProxy){ + .points = buffer, + .count = count, + .radius = proxy->radius, + }; +} + +b3AABB b3ComputeProxyAABB( const b3ShapeProxy* proxy ) +{ + const b3Vec3* points = proxy->points; + b3AABB aabb = { + .lowerBound = points[0], + .upperBound = points[0], + }; + + for ( int i = 1; i < proxy->count; ++i ) + { + aabb.lowerBound = b3Min( aabb.lowerBound, points[i] ); + aabb.upperBound = b3Max( aabb.upperBound, points[i] ); + } + + b3Vec3 r = { proxy->radius, proxy->radius, proxy->radius }; + aabb.lowerBound = b3Sub( aabb.lowerBound, r ); + aabb.upperBound = b3Add( aabb.upperBound, r ); + return aabb; +} + +b3BodyId b3Shape_GetBody( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return b3MakeBodyId( world, shape->bodyId ); +} + +b3WorldId b3Shape_GetWorld( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + return (b3WorldId){ (uint16_t)( shapeId.world0 + 1 ), world->generation }; +} + +void b3Shape_SetUserData( b3ShapeId shapeId, void* userData ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + shape->userData = userData; +} + +void* b3Shape_GetUserData( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->userData; +} + +bool b3Shape_IsSensor( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->sensorIndex != B3_NULL_INDEX; +} + +void b3Shape_SetName( b3ShapeId shapeId, const char* name ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + + if ( shape->name != NULL ) + { + b3Free( shape->name, ( strlen( shape->name ) + 1 ) * sizeof( char ) ); + shape->name = NULL; + } + + if ( name != NULL ) + { + size_t length = strlen( name ) + 1; + shape->name = (char*)b3Alloc( length * sizeof( char ) ); + memcpy( shape->name, name, length ); + } +} + +const char* b3Shape_GetName( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->name; +} + +// todo no tests +b3CastOutput b3Shape_RayCast( b3ShapeId shapeId, const b3RayCastInput* input ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + + b3Transform transform = b3GetBodyTransform( world, shape->bodyId ); + + // input in local coordinates + b3RayCastInput localInput; + localInput.origin = b3InvTransformPoint( transform, input->origin ); + localInput.translation = b3InvRotateVector( transform.q, input->translation ); + localInput.maxFraction = input->maxFraction; + + b3CastOutput output = { 0 }; + switch ( shape->type ) + { + case b3_capsuleShape: + output = b3RayCastCapsule( &shape->capsule, &localInput ); + break; + + case b3_compoundShape: + output = b3RayCastCompound( shape->compound, &localInput ); + break; + + case b3_sphereShape: + output = b3RayCastSphere( &shape->sphere, &localInput ); + break; + + case b3_meshShape: + output = b3RayCastMesh( &shape->mesh, &localInput ); + break; + + case b3_hullShape: + output = b3RayCastHull( shape->hull, &localInput ); + break; + + case b3_heightShape: + output = b3RayCastHeightField( shape->heightField, &localInput ); + break; + + default: + B3_ASSERT( false ); + return output; + } + + if ( output.hit ) + { + // convert to world coordinates + output.normal = b3RotateVector( transform.q, output.normal ); + output.point = b3TransformPoint( transform, output.point ); + } + + return output; +} + +void b3Shape_SetDensity( b3ShapeId shapeId, float density, bool updateBodyMass ) +{ + B3_ASSERT( b3IsValidFloat( density ) && density >= 0.0f ); + + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + if ( density == shape->density ) + { + // early return to avoid expensive function + return; + } + + shape->density = density; + + if ( updateBodyMass == true ) + { + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3UpdateBodyMassData( world, body ); + } +} + +float b3Shape_GetDensity( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->density; +} + +void b3Shape_SetFriction( b3ShapeId shapeId, float friction ) +{ + B3_ASSERT( b3IsValidFloat( friction ) && friction >= 0.0f ); + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type != b3_compoundShape ); + shape->materials[0].friction = friction; +} + +float b3Shape_GetFriction( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->materials[0].friction; +} + +void b3Shape_SetRestitution( b3ShapeId shapeId, float restitution ) +{ + B3_ASSERT( b3IsValidFloat( restitution ) && restitution >= 0.0f ); + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type != b3_compoundShape ); + shape->materials[0].restitution = restitution; +} + +float b3Shape_GetRestitution( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->materials[0].restitution; +} + +void b3Shape_SetSurfaceMaterial( b3ShapeId shapeId, b3SurfaceMaterial surfaceMaterial ) +{ + B3_ASSERT( b3IsValidFloat( surfaceMaterial.friction ) && surfaceMaterial.friction >= 0.0f ); + B3_ASSERT( b3IsValidFloat( surfaceMaterial.restitution ) && surfaceMaterial.restitution >= 0.0f ); + B3_ASSERT( b3IsValidFloat( surfaceMaterial.rollingResistance ) && surfaceMaterial.rollingResistance >= 0.0f ); + B3_ASSERT( b3IsValidVec3( surfaceMaterial.tangentVelocity ) ); + + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type != b3_compoundShape ); + shape->materials[0] = surfaceMaterial; +} + +b3SurfaceMaterial b3Shape_GetSurfaceMaterial( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->materials[0]; +} + +int b3Shape_GetMeshMaterialCount( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->materialCount; +} + +void b3Shape_SetMeshMaterial( b3ShapeId shapeId, b3SurfaceMaterial surfaceMaterial, int index ) +{ + B3_ASSERT( b3IsValidFloat( surfaceMaterial.friction ) && surfaceMaterial.friction >= 0.0f ); + B3_ASSERT( b3IsValidFloat( surfaceMaterial.restitution ) && surfaceMaterial.restitution >= 0.0f ); + B3_ASSERT( b3IsValidFloat( surfaceMaterial.rollingResistance ) && surfaceMaterial.rollingResistance >= 0.0f ); + B3_ASSERT( b3IsValidVec3( surfaceMaterial.tangentVelocity ) ); + + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + + B3_ASSERT( 0 <= index && index < shape->materialCount ); + B3_ASSERT( shape->type != b3_compoundShape ); + shape->materials[index] = surfaceMaterial; +} + +b3SurfaceMaterial b3Shape_GetMeshSurfaceMaterial( b3ShapeId shapeId, int index ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( 0 <= index && index < shape->materialCount ); + return shape->materials[index]; +} + +b3Filter b3Shape_GetFilter( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->filter; +} + +static void b3ResetProxy( b3World* world, b3Shape* shape, bool wakeBodies, bool destroyProxy ) +{ + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + + int shapeId = shape->id; + + // destroy all contacts associated with this shape + int contactKey = body->headContactKey; + while ( contactKey != B3_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->shapeIdA == shapeId || contact->shapeIdB == shapeId ) + { + b3DestroyContact( world, contact, wakeBodies ); + } + } + + b3Transform transform = b3GetBodyTransformQuick( world, body ); + if ( shape->proxyKey != B3_NULL_INDEX ) + { + b3BodyType proxyType = B3_PROXY_TYPE( shape->proxyKey ); + b3UpdateShapeAABBs( shape, transform, proxyType ); + + if ( destroyProxy ) + { + b3BroadPhase_DestroyProxy( &world->broadPhase, shape->proxyKey ); + + bool forcePairCreation = true; + shape->proxyKey = b3BroadPhase_CreateProxy( &world->broadPhase, proxyType, shape->fatAABB, shape->filter.categoryBits, + shapeId, forcePairCreation ); + } + else + { + b3BroadPhase_MoveProxy( &world->broadPhase, shape->proxyKey, shape->fatAABB ); + } + } + else + { + b3BodyType proxyType = body->type; + b3UpdateShapeAABBs( shape, transform, proxyType ); + } + + b3ValidateSolverSets( world ); +} + +void b3Shape_SetFilter( b3ShapeId shapeId, b3Filter filter, bool invokeContacts ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + if ( filter.maskBits == shape->filter.maskBits && filter.categoryBits == shape->filter.categoryBits && + filter.groupIndex == shape->filter.groupIndex ) + { + return; + } + + shape->filter = filter; + + if ( invokeContacts ) + { + world->locked = true; + bool wakeBodies = true; + + // If the category bits change, I need to destroy the proxy because it affects the tree sorting. + bool destroyProxy = filter.categoryBits == shape->filter.categoryBits; + + // need to wake bodies because a filter change may destroy contacts + b3ResetProxy( world, shape, wakeBodies, destroyProxy ); + world->locked = false; + } + + // note: this does not immediately update sensor overlaps. Instead sensor + // overlaps are updated the next time step +} + +void b3Shape_EnableSensorEvents( b3ShapeId shapeId, bool flag ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + shape->enableSensorEvents = flag; +} + +bool b3Shape_AreSensorEventsEnabled( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->enableSensorEvents; +} + +void b3Shape_EnableContactEvents( b3ShapeId shapeId, bool flag ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + shape->enableContactEvents = flag; +} + +bool b3Shape_AreContactEventsEnabled( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->enableContactEvents; +} + +void b3Shape_EnablePreSolveEvents( b3ShapeId shapeId, bool flag ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + shape->enablePreSolveEvents = flag; +} + +bool b3Shape_ArePreSolveEventsEnabled( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->enablePreSolveEvents; +} + +void b3Shape_EnableHitEvents( b3ShapeId shapeId, bool flag ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + shape->enableHitEvents = flag; +} + +bool b3Shape_AreHitEventsEnabled( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->enableHitEvents; +} + +b3ShapeType b3Shape_GetType( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->type; +} + +b3Sphere b3Shape_GetSphere( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type == b3_sphereShape ); + return shape->sphere; +} + +b3Capsule b3Shape_GetCapsule( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type == b3_capsuleShape ); + return shape->capsule; +} + +const b3Hull* b3Shape_GetHull( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type == b3_hullShape ); + return shape->hull; +} + +b3Mesh b3Shape_GetMesh( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type == b3_meshShape ); + return shape->mesh; +} + +const b3HeightField* b3Shape_GetHeightField( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type == b3_heightShape ); + return shape->heightField; +} + +void b3Shape_SetSphere( b3ShapeId shapeId, const b3Sphere* sphere ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + world->locked = true; + + b3Shape* shape = b3GetShape( world, shapeId ); + + b3DestroyShapeAllocationForShapeChange( world, shape ); + + shape->sphere = *sphere; + shape->type = b3_sphereShape; + + // need to wake bodies so they can react to the shape change + bool wakeBodies = true; + bool destroyProxy = true; + b3ResetProxy( world, shape, wakeBodies, destroyProxy ); + + world->locked = false; +} + +void b3Shape_SetCapsule( b3ShapeId shapeId, const b3Capsule* capsule ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + world->locked = true; + + b3Shape* shape = b3GetShape( world, shapeId ); + + b3DestroyShapeAllocationForShapeChange( world, shape ); + + shape->capsule = *capsule; + shape->type = b3_capsuleShape; + + // need to wake bodies so they can react to the shape change + bool wakeBodies = true; + bool destroyProxy = true; + b3ResetProxy( world, shape, wakeBodies, destroyProxy ); + + world->locked = false; +} + +void b3Shape_SetHull( b3ShapeId shapeId, const b3Hull* hull ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + world->locked = true; + + b3Shape* shape = b3GetShape( world, shapeId ); + + b3DestroyShapeAllocationForShapeChange( world, shape ); + + shape->hull = b3CloneHull( hull ); + shape->type = b3_hullShape; + + // need to wake bodies so they can react to the shape change + bool wakeBodies = true; + bool destroyProxy = true; + b3ResetProxy( world, shape, wakeBodies, destroyProxy ); + + world->locked = false; +} + +void b3Shape_SetMesh( b3ShapeId shapeId, const b3MeshData* meshData, b3Vec3 scale ) +{ + B3_ASSERT( b3IsValidVec3( scale ) ); + B3_ASSERT( meshData != NULL && b3IsValidMesh( meshData ) ); + + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + world->locked = true; + + b3Shape* shape = b3GetShape( world, shapeId ); + + b3DestroyShapeAllocationForShapeChange( world, shape ); + + shape->mesh.data = meshData; + shape->mesh.scale = b3SafeScale( scale ); + shape->type = b3_meshShape; + + // need to wake bodies so they can react to the shape change + bool wakeBodies = true; + bool destroyProxy = true; + b3ResetProxy( world, shape, wakeBodies, destroyProxy ); + + world->locked = false; +} + +int b3Shape_GetContactCapacity( b3ShapeId shapeId ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + if ( shape->sensorIndex != B3_NULL_INDEX ) + { + return 0; + } + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + + // Conservative and fast + return body->contactCount; +} + +int b3Shape_GetContactData( b3ShapeId shapeId, b3ContactData* contactData, int capacity ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + if ( shape->sensorIndex != B3_NULL_INDEX ) + { + return 0; + } + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + int contactKey = body->headContactKey; + int index = 0; + while ( contactKey != B3_NULL_INDEX && index < capacity ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + + // Does contact involve this shape and is it touching? + if ( ( contact->shapeIdA == shapeId.index1 - 1 || contact->shapeIdB == shapeId.index1 - 1 ) && + ( contact->flags & b3_contactTouchingFlag ) != 0 ) + { + b3Shape* shapeA = world->shapes.data + contact->shapeIdA; + b3Shape* shapeB = world->shapes.data + contact->shapeIdB; + + contactData[index].contactId = (b3ContactId){ contact->contactId + 1, shapeId.world0, 0, contact->generation }; + contactData[index].shapeIdA = (b3ShapeId){ shapeA->id + 1, shapeId.world0, shapeA->generation }; + contactData[index].shapeIdB = (b3ShapeId){ shapeB->id + 1, shapeId.world0, shapeB->generation }; + contactData[index].manifolds = contact->manifolds; + contactData[index].manifoldCount = contact->manifoldCount; + index += 1; + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + + B3_ASSERT( index <= capacity ); + + return index; +} + +int b3Shape_GetSensorCapacity( b3ShapeId shapeId ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + if ( shape->sensorIndex == B3_NULL_INDEX ) + { + return 0; + } + + b3Sensor* sensor = b3Array_Get( world->sensors, shape->sensorIndex ); + return sensor->overlaps2.count; +} + +int b3Shape_GetSensorData( b3ShapeId shapeId, b3ShapeId* visitorIds, int capacity ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + if ( shape->sensorIndex == B3_NULL_INDEX ) + { + return 0; + } + + b3Sensor* sensor = b3Array_Get( world->sensors, shape->sensorIndex ); + + int count = b3MinInt( sensor->overlaps2.count, capacity ); + b3Visitor* refs = sensor->overlaps2.data; + for ( int i = 0; i < count; ++i ) + { + b3ShapeId visitorId = { + .index1 = refs[i].shapeId + 1, + .world0 = shapeId.world0, + .generation = refs[i].generation, + }; + + visitorIds[i] = visitorId; + } + + return count; +} + +b3AABB b3Shape_GetAABB( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + if ( world == NULL ) + { + return (b3AABB){ 0 }; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->aabb; +} + +b3MassData b3Shape_ComputeMassData( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + if ( world == NULL ) + { + return (b3MassData){ 0 }; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + return b3ComputeShapeMass( shape ); +} + +b3Vec3 b3Shape_GetClosestPoint( b3ShapeId shapeId, b3Vec3 target ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + if ( world == NULL ) + { + return b3Vec3_zero; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3Transform transform = b3GetBodyTransformQuick( world, body ); + + b3DistanceInput input; + input.proxyA = b3MakeShapeProxy( shape ); + input.proxyB = (b3ShapeProxy){ &target, 1, 0.0f }; + input.transformA = transform; + input.transformB = b3Transform_identity; + input.useRadii = true; + + b3SimplexCache cache = { 0 }; + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + + return output.pointA; +} + +#define B3_DEBUG_WIND 0 + +// https://en.wikipedia.org/wiki/Density_of_air +// https://www.engineeringtoolbox.com/wind-load-d_1775.html +// force = 0.5 * air_density * velocity^2 * area +// https://en.wikipedia.org/wiki/Lift_(force) +void b3Shape_ApplyWind( b3ShapeId shapeId, b3Vec3 wind, float drag, float lift, float maxSpeed, bool wake ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + + b3ShapeType shapeType = shape->type; + if ( shapeType != b3_sphereShape && shapeType != b3_capsuleShape && shapeType != b3_hullShape ) + { + return; + } + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + + if ( body->type != b3_dynamicBody ) + { + return; + } + + if ( body->setIndex == b3_disabledSet ) + { + return; + } + + if ( body->setIndex >= b3_firstSleepingSet && wake == false ) + { + return; + } + + b3BodySim* sim = b3GetBodySim( world, body ); + + if ( body->setIndex != b3_awakeSet ) + { + // Must wake for state to exist + b3WakeBodyWithLock( world, body ); + } + + B3_ASSERT( body->setIndex == b3_awakeSet ); + + b3BodyState* state = b3GetBodyState( world, body ); + b3Transform transform = sim->transform; + + float lengthUnits = b3GetLengthUnitsPerMeter(); + float volumeUnits = lengthUnits * lengthUnits * lengthUnits; + + float airDensity = 1.2250f / ( volumeUnits ); + + b3Vec3 force = { 0 }; + b3Vec3 torque = { 0 }; + + switch ( shape->type ) + { + case b3_sphereShape: + { + float radius = shape->sphere.radius; + b3Vec3 centroid = shape->localCentroid; + b3Vec3 lever = b3RotateVector( transform.q, b3Sub( centroid, sim->localCenter ) ); + b3Vec3 shapeVelocity = b3Add( state->linearVelocity, b3Cross( state->angularVelocity, lever ) ); + b3Vec3 relativeVelocity = b3MulSub( wind, drag, shapeVelocity ); + float speed; + b3Vec3 direction = b3GetLengthAndNormalize( &speed, relativeVelocity ); + speed = b3MinFloat( speed, maxSpeed ); + float projectedArea = B3_PI * radius * radius; + force = b3MulSV( 0.5f * airDensity * projectedArea * speed * speed, direction ); + torque = b3Cross( lever, force ); + } + break; + + case b3_capsuleShape: + { + b3Vec3 centroid = shape->localCentroid; + b3Vec3 lever = b3RotateVector( transform.q, b3Sub( centroid, sim->localCenter ) ); + b3Vec3 shapeVelocity = b3Add( state->linearVelocity, b3Cross( state->angularVelocity, lever ) ); + b3Vec3 relativeVelocity = b3MulSub( wind, drag, shapeVelocity ); + float speed; + b3Vec3 direction = b3GetLengthAndNormalize( &speed, relativeVelocity ); + speed = b3MinFloat( speed, maxSpeed ); + + b3Vec3 d = b3Sub( shape->capsule.center2, shape->capsule.center1 ); + d = b3RotateVector( transform.q, d ); + + float radius = shape->capsule.radius; + float projectedArea = B3_PI * radius * radius + 2.0f * radius * b3Length( b3Cross( d, direction ) ); + + // Normal that opposes the wind + b3Vec3 e = b3Normalize( d ); + b3Vec3 normal = b3Sub( b3MulSV( b3Dot( direction, e ), e ), direction ); + + // portion of wind that is perpendicular to surface + b3Vec3 liftDirection = b3Cross( b3Cross( normal, direction ), direction ); + + float forceMagnitude = 0.5f * airDensity * projectedArea * speed * speed; + force = b3MulSV( forceMagnitude, b3MulAdd( direction, lift, liftDirection ) ); + + b3Vec3 edgeLever = b3MulAdd( lever, radius, normal ); + torque = b3Cross( edgeLever, force ); + } + break; + + case b3_hullShape: + { + b3Matrix3 matrix = b3MakeMatrixFromQuat( transform.q ); + + int faceCount = shape->hull->faceCount; + const b3Vec3* points = b3GetHullPoints( shape->hull ); + const b3HullFace* faces = b3GetHullFaces( shape->hull ); + const b3HullHalfEdge* edges = b3GetHullEdges( shape->hull ); + const b3Plane* planes = b3GetHullPlanes( shape->hull ); + + b3Vec3 linearVelocity = state->linearVelocity; + b3Vec3 angularVelocity = state->angularVelocity; + b3Vec3 localCenterOfMass = sim->localCenter; + + for ( int i = 0; i < faceCount; ++i ) + { + const b3HullFace* face = faces + i; + const b3HullHalfEdge* edge1 = edges + face->edge; + const b3HullHalfEdge* edge2 = edges + edge1->next; + const b3HullHalfEdge* edge3 = edges + edge2->next; + + B3_ASSERT( edge1 != edge3 ); + B3_ASSERT( edge1->origin < shape->hull->vertexCount ); + B3_ASSERT( edge2->origin < shape->hull->vertexCount ); + + b3Vec3 localPoint1 = points[edge1->origin]; + b3Vec3 localPoint2 = points[edge2->origin]; + b3Vec3 v1 = b3MulMV( matrix, localPoint1 ); + b3Vec3 v2 = b3MulMV( matrix, localPoint2 ); + b3Vec3 normal = b3MulMV( matrix, planes[i].normal ); + + do + { + B3_ASSERT( edge3->origin < shape->hull->vertexCount ); + b3Vec3 localPoint3 = points[edge3->origin]; + b3Vec3 v3 = b3MulMV( matrix, localPoint3 ); + + // Triangle center + b3Vec3 localCenter = b3MulSV( 0.333333f, b3Add( localPoint1, b3Add( localPoint2, localPoint3 ) ) ); + + // Lever arm from center of mass to triangle center in world space + b3Vec3 lever = b3MulMV( matrix, b3Sub( localCenter, localCenterOfMass ) ); + + // Velocity of the triangle center in world space + b3Vec3 centerVelocity = b3Add( linearVelocity, b3Cross( angularVelocity, lever ) ); + + b3Vec3 relativeVelocity = b3MulSub( wind, drag, centerVelocity ); + float speed; + b3Vec3 direction = b3GetLengthAndNormalize( &speed, relativeVelocity ); + + // Check for back-side + if ( b3Dot( normal, direction ) < -FLT_EPSILON ) + { + float projectedArea = -0.5f * b3Dot( b3Cross( b3Sub( v2, v1 ), b3Sub( v3, v1 ) ), direction ); + B3_VALIDATE( projectedArea >= -FLT_EPSILON ); + + b3Vec3 liftDirection = b3Cross( b3Cross( normal, direction ), direction ); + + speed = b3MinFloat( speed, maxSpeed ); + + float forceMagnitude = 0.5f * airDensity * projectedArea * speed * speed; + b3Vec3 deltaForce = b3MulSV( forceMagnitude, b3MulAdd( direction, lift, liftDirection ) ); + b3Vec3 deltaTorque = b3Cross( lever, deltaForce ); + + force = b3Add( force, deltaForce ); + torque = b3Add( torque, deltaTorque ); + +#if B3_DEBUG_WIND + int lineIndex = world->taskContexts.data[0].lineCount; + if ( lineIndex < B3_DEBUG_LINE_CAPACITY ) + { + b3DebugLine* line = world->taskContexts.data[0].lines + lineIndex; + line->p1 = b3Add( b3MulMV( matrix, localCenter ), transform.p ); + line->p2 = b3Add( line->p1, deltaForce ); + line->label = i; + line->color = b3_colorBlanchedAlmond; + world->taskContexts.data[0].lineCount += 1; + } +#endif + } + + edge2 = edge3; + edge3 = edges + edge3->next; + v2 = v3; + localPoint2 = localPoint3; + } + while ( edge1 != edge3 ); + } + } + break; + + default: + break; + } + + sim->force = b3Add( sim->force, force ); + sim->torque = b3Add( sim->torque, torque ); +} + +typedef struct b3MeshImpactContext +{ + b3TOIInput toiInput; + b3TOIOutput toiOutput; + // Centroid of shape in body B local space + b3Vec3 localCentroidB; + // Centroid of shape at beginning and end of sweep in mesh local space. Used for early out. + b3Vec3 meshLocalCentroidB1, meshLocalCentroidB2; + float fallbackRadius; + bool isSensor; +} b3MeshImpactContext; + +static bool b3MeshTimeOfImpactFcn( b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleIndex, void* context ) +{ + B3_UNUSED( triangleIndex ); + + b3MeshImpactContext* toiContext = (b3MeshImpactContext*)context; + + // Early out for parallel movement + b3Vec3 c1 = toiContext->meshLocalCentroidB1; + b3Vec3 c2 = toiContext->meshLocalCentroidB2; + + b3Vec3 n = b3Normalize( b3Cross( b3Sub( b, a ), b3Sub( c, a ) ) ); + float offset1 = b3Dot( n, b3Sub( c1, a ) ); + float offset2 = b3Dot( n, b3Sub( c2, a ) ); + + if ( offset1 < 0.0f ) + { + // Started behind or finished in front + return true; + } + + if ( toiContext->isSensor == false && offset1 - offset2 < toiContext->fallbackRadius && offset2 > toiContext->fallbackRadius ) + { + // Finished in front + return true; + } + + b3Vec3 triangle[3] = { a, b, c }; + toiContext->toiInput.proxyA.points = triangle; + toiContext->toiInput.proxyA.count = 3; + + b3TOIOutput output = b3TimeOfImpact( &toiContext->toiInput ); + + // It is possible for a hit at fraction == 0 + + if ( 0.0f < output.fraction && output.fraction < toiContext->toiInput.maxFraction ) + { + toiContext->toiOutput = output; + toiContext->toiInput.maxFraction = output.fraction; + } + else if ( 0.0f == output.fraction ) + { + // fallback to TOI of a small circle around the fast shape centroid + b3TOIInput fallbackInput = toiContext->toiInput; + fallbackInput.proxyB = (b3ShapeProxy){ &toiContext->localCentroidB, 1, toiContext->fallbackRadius + B3_LINEAR_SLOP }; + output = b3TimeOfImpact( &fallbackInput ); + + if ( 0.0f < output.fraction && output.fraction < toiContext->toiInput.maxFraction ) + { + toiContext->toiOutput = output; + toiContext->toiInput.maxFraction = output.fraction; + toiContext->toiOutput.usedFallback = true; + } + } + + // Continue the query + return true; +} + +typedef struct b3CompoundImpactContext +{ + b3TOIInput toiInput; + b3TOIOutput toiOutput; + b3Transform compoundTransform; + + // Bounds local to compound + b3AABB localSweepBoundsB; + + // Centroid of shape in body B local space + b3Vec3 localCentroidB; + float fallbackRadius; +} b3CompoundImpactContext; + +// Implements b3CompoundQueryFcn +static bool b3CompoundTimeOfImpactFcn( const b3Compound* compound, int childIndex, void* context ) +{ + b3CompoundImpactContext* toiContext = (b3CompoundImpactContext*)context; + + b3ChildShape child = b3GetCompoundChild( compound, childIndex ); + + b3TOIOutput output = { 0 }; + toiContext->toiInput.sweepA = b3MakeCompoundChildSweep( toiContext->compoundTransform, child.transform ); + + switch ( child.type ) + { + case b3_capsuleShape: + { + toiContext->toiInput.proxyA.points = &child.capsule.center1; + toiContext->toiInput.proxyA.count = 2; + toiContext->toiInput.proxyA.radius = child.capsule.radius; + output = b3TimeOfImpact( &toiContext->toiInput ); + } + break; + + case b3_hullShape: + { + toiContext->toiInput.proxyA.points = b3GetHullPoints( child.hull ); + toiContext->toiInput.proxyA.count = child.hull->vertexCount; + toiContext->toiInput.proxyA.radius = 0.0f; + output = b3TimeOfImpact( &toiContext->toiInput ); + } + break; + + case b3_meshShape: + { + b3MeshImpactContext meshContext = { 0 }; + meshContext.toiInput = toiContext->toiInput; + meshContext.isSensor = false; + meshContext.localCentroidB = toiContext->localCentroidB; + meshContext.fallbackRadius = toiContext->fallbackRadius; + + b3Transform meshWorldTransform = b3MulTransforms( toiContext->compoundTransform, child.transform ); + + const b3Sweep* sweepB = &toiContext->toiInput.sweepB; + b3Transform xfB1 = { + .p = b3Sub( sweepB->c1, b3RotateVector( sweepB->q1, sweepB->localCenter ) ), + .q = sweepB->q1, + }; + + b3Transform xfB2 = { + .p = b3Sub( sweepB->c2, b3RotateVector( sweepB->q2, sweepB->localCenter ) ), + .q = sweepB->q2, + }; + + meshContext.meshLocalCentroidB1 = + b3InvTransformPoint( meshWorldTransform, b3TransformPoint( xfB1, meshContext.localCentroidB ) ); + meshContext.meshLocalCentroidB2 = + b3InvTransformPoint( meshWorldTransform, b3TransformPoint( xfB2, meshContext.localCentroidB ) ); + + // Bounds local to mesh + b3AABB localBounds = b3AABB_Transform( b3InvertTransform( child.transform ), toiContext->localSweepBoundsB ); + + b3QueryMesh( &child.mesh, localBounds, b3MeshTimeOfImpactFcn, &meshContext ); + + output = meshContext.toiOutput; + } + break; + + case b3_sphereShape: + { + toiContext->toiInput.proxyA.points = &child.sphere.center; + toiContext->toiInput.proxyA.count = 1; + toiContext->toiInput.proxyA.radius = child.sphere.radius; + output = b3TimeOfImpact( &toiContext->toiInput ); + } + break; + + default: + B3_ASSERT( false ); + break; + } + + if ( 0.0f < output.fraction && output.fraction < toiContext->toiInput.maxFraction ) + { + toiContext->toiOutput = output; + toiContext->toiInput.maxFraction = output.fraction; + } + + // Clear this to be safe + toiContext->toiInput.proxyA = (b3ShapeProxy){ 0 }; + + // Continue the query + return true; +} + +b3TOIOutput b3ShapeTimeOfImpact( b3Shape* shapeA, b3Shape* shapeB, b3Sweep* sweepA, b3Sweep* sweepB, float maxFraction ) +{ + bool isSensor = shapeA->sensorIndex != B3_NULL_INDEX; + + b3ShapeType typeA = shapeA->type; + if ( typeA == b3_compoundShape ) + { + // todo implement b3CompoundTimeOfImpact + b3CompoundImpactContext context = { 0 }; + context.toiInput.proxyB = b3MakeShapeProxy( shapeB ); + context.toiInput.sweepB = *sweepB; + context.toiInput.maxFraction = maxFraction; + + context.compoundTransform = (b3Transform){ + .p = sweepA->c1, + .q = sweepA->q1, + }; + + b3Vec3 localCentroidB = b3GetShapeCentroid( shapeB ); + context.localCentroidB = localCentroidB; + + b3ShapeExtent extents = b3ComputeShapeExtent( shapeB, context.localCentroidB ); + context.fallbackRadius = b3MaxFloat( 0.75f * extents.minExtent, B3_SPECULATIVE_DISTANCE ); + + // Swept bounds of shapeB + b3AABB bounds = b3ComputeSweptShapeAABB( shapeB, sweepB, maxFraction ); + + // Bounds local to mesh + b3AABB localBounds = b3AABB_Transform( b3InvertTransform( context.compoundTransform ), bounds ); + context.localSweepBoundsB = localBounds; + + b3QueryCompound( shapeA->compound, localBounds, b3CompoundTimeOfImpactFcn, &context ); + + return context.toiOutput; + } + + if ( typeA == b3_heightShape || typeA == b3_meshShape ) + { + // todo implement b3MeshTimeOfImpact and b3HeightFieldTimeOfImpact + // Note: assuming mesh is static + + b3MeshImpactContext context = { 0 }; + context.toiInput.sweepA = *sweepA; + context.toiInput.proxyA.count = 3; + context.toiInput.proxyB = b3MakeShapeProxy( shapeB ); + context.toiInput.sweepB = *sweepB; + context.toiInput.maxFraction = maxFraction; + context.isSensor = isSensor; + + b3Vec3 localCentroidB = b3GetShapeCentroid( shapeB ); + context.localCentroidB = localCentroidB; + + // Assume mesh is static + b3Transform xfA = { + .p = b3Sub( sweepA->c1, b3RotateVector( sweepA->q1, sweepA->localCenter ) ), + .q = sweepA->q1, + }; + + b3Transform xfB1 = { + .p = b3Sub( sweepB->c1, b3RotateVector( sweepB->q1, sweepB->localCenter ) ), + .q = sweepB->q1, + }; + + b3Transform xfB2 = { + .p = b3Sub( sweepB->c2, b3RotateVector( sweepB->q2, sweepB->localCenter ) ), + .q = sweepB->q2, + }; + + // b3Vec3 centroidB1 = b3InvRotateVector( xfA.q, b3RotateVector( sweepB->q1, localCentroidB - sweepB->localCenter ) + + // ( sweepB->c1 - xfA.p ) ); + // b3Vec3 centroidB2 = b3InvRotateVector( xfA.q, b3RotateVector( sweepB->q2, localCentroidB - sweepB->localCenter ) + + // ( sweepB->c2 - xfA.p ) ); + + context.meshLocalCentroidB1 = b3InvTransformPoint( xfA, b3TransformPoint( xfB1, localCentroidB ) ); + context.meshLocalCentroidB2 = b3InvTransformPoint( xfA, b3TransformPoint( xfB2, localCentroidB ) ); + + b3ShapeExtent extents = b3ComputeShapeExtent( shapeB, context.localCentroidB ); + context.fallbackRadius = b3MaxFloat( 0.5f * extents.minExtent, B3_LINEAR_SLOP ); + + // Swept bounds of shapeB + // todo pass in xfA to get local bounds directly + b3AABB bounds = b3ComputeSweptShapeAABB( shapeB, sweepB, maxFraction ); + + // Bounds local to mesh + b3AABB localBounds = b3AABB_Transform( b3InvertTransform( xfA ), bounds ); + + if ( typeA == b3_meshShape ) + { + b3QueryMesh( &shapeA->mesh, localBounds, b3MeshTimeOfImpactFcn, &context ); + } + else if ( typeA == b3_heightShape ) + { + b3QueryHeightField( shapeA->heightField, localBounds, b3MeshTimeOfImpactFcn, &context ); + } + + return context.toiOutput; + } + + B3_ASSERT( shapeB->type != b3_compoundShape && shapeB->type != b3_meshShape && shapeB->type != b3_heightShape ); + + b3TOIInput input; + input.proxyA = b3MakeShapeProxy( shapeA ); + input.proxyB = b3MakeShapeProxy( shapeB ); + input.sweepA = *sweepA; + input.sweepB = *sweepB; + input.maxFraction = maxFraction; + + b3TOIOutput output = b3TimeOfImpact( &input ); + +#if 0 + // todo I'm not sure this is worth it for convex vs convex. + if (0.0f < output.fraction && output.fraction < maxFraction) + { + return output; + } + + if (0.0f == output.fraction) + { + // fallback to TOI of a small circle around the fast shape centroid + b3Vec3 centroid = b3GetShapeCentroid( shapeB ); + input.proxyB = ( b3ShapeProxy ){ ¢roid, 1, B3_SPECULATIVE_DISTANCE }; + output = b3TimeOfImpact( &input ); + return output; + } +#endif + + return output; +} + +// Resolve the user material id for a hit point on the given shape. Mesh/heightfield shapes +// use the manifold-point triangleIndex to pick a per-triangle material. Compound shapes use +// the contact's childIndex to find the participating child, then for a mesh child apply the +// child's materialIndices indirection on top of the per-triangle index. Convex shapes fall +// back to materials[0]. childIndex is unused for non-compound shapes. +uint64_t b3GetShapeUserMaterialId( const b3Shape* shape, int childIndex, int triangleIndex ) +{ + if ( shape->materialCount == 0 ) + { + return 0; + } + + int materialIndex = 0; + if ( shape->type == b3_meshShape ) + { + const uint8_t* indices = b3GetMeshMaterialIndices( shape->mesh.data ); + if ( indices != NULL ) + { + materialIndex = indices[triangleIndex]; + } + } + else if ( shape->type == b3_heightShape ) + { + materialIndex = b3GetHeightFieldMaterial( shape->heightField, triangleIndex ); + } + else if ( shape->type == b3_compoundShape ) + { + b3ChildShape child = b3GetCompoundChild( shape->compound, childIndex ); + if ( child.type == b3_meshShape ) + { + const uint8_t* indices = b3GetMeshMaterialIndices( child.mesh.data ); + int meshMaterialIndex = indices != NULL ? indices[triangleIndex] : 0; + meshMaterialIndex = b3ClampInt( meshMaterialIndex, 0, B3_MAX_COMPOUND_MESH_MATERIALS - 1 ); + materialIndex = child.materialIndices[meshMaterialIndex]; + } + else + { + materialIndex = child.materialIndices[0]; + } + } + + materialIndex = b3ClampInt( materialIndex, 0, shape->materialCount - 1 ); + return shape->materials[materialIndex].userMaterialId; +} + +void b3DumpShape( b3World* world, int shapeIndex ) +{ + b3Shape* shape = b3Array_Get( world->shapes, shapeIndex ); + + b3Dump( " b3ShapeDef sd = b3DefaultShapeDef();\n" ); + + if ( shape->name != NULL ) + { + b3Dump( " sd.name = \"%s\";\n", shape->name ); + } + + // printf("%" PRIx64 ";\n", t); + b3Dump( " sd.density = %.9g;\n", shape->density ); + b3Dump( " sd.isSensor = bool(%d);\n", shape->sensorIndex != B3_NULL_INDEX ); + b3Dump( " sd.filter.categoryBits = 0x%" PRIx64 ";\n", shape->filter.categoryBits ); + b3Dump( " sd.filter.maskBits = 0x%" PRIx64 ";\n", shape->filter.maskBits ); + b3Dump( " sd.filter.groupIndex = %d;\n", shape->filter.groupIndex ); + + B3_ASSERT( shape->materialCount >= 1 ); + b3SurfaceMaterial* m = shape->materials + 0; + b3Dump( " sd.baseMaterial.friction = %.9g;\n", m->friction ); + b3Dump( " sd.baseMaterial.restitution = %.9g;\n", m->restitution ); + b3Dump( " sd.baseMaterial.rollingResistance = %.9g;\n", m->rollingResistance ); + + switch ( shape->type ) + { + case b3_capsuleShape: + { + b3Capsule* s = &shape->capsule; + b3Dump( " b3CapsuleShape shape;\n" ); + b3Dump( " shape.center1 = {%.9g, %.9g, %.9g};\n", s->center1.x, s->center1.y, s->center1.z ); + b3Dump( " shape.center2 = {%.9g, %.9g, %.9g};\n", s->center2.x, s->center2.y, s->center2.z ); + b3Dump( " shape.radius = %.9g;\n", s->radius ); + b3Dump( " b3CreateCapsuleShape(bodyId, &sd, &shape);\n" ); + } + break; + + case b3_compoundShape: + { + // todo + } + break; + + case b3_heightShape: + { + // todo + } + break; + + case b3_hullShape: + { + const b3Hull* s = shape->hull; + int vertexCount = s->vertexCount; + const b3Vec3* vs = b3GetHullPoints( s ); + b3Dump( " b3Vec3 vs[%d];\n", vertexCount ); + for ( int i = 0; i < vertexCount; ++i ) + { + b3Dump( " vs[%d] = {%.9g, %.9g, %.9g};\n", i, vs[i].x, vs[i].y, vs[i].z ); + } + b3Dump( " b3Hull* hull = b3CreateHull(vs, %d);\n", vertexCount ); + b3Dump( " b3CreateHullShape(bodyId, &sd, hull);\n" ); + b3Dump( " b3DestroyHull(hull);\n" ); + } + break; + + case b3_meshShape: + { + const b3MeshData* s = shape->mesh.data; + b3Vec3 scale = shape->mesh.scale; + int meshIndex = b3FetchAddMeshDumpIndex(); + char buffer[64]; + snprintf( buffer, 64, "mesh_dump_%d.b3m", meshIndex ); + b3WriteBinaryFile( (void*)s, s->byteCount, buffer ); + + b3Dump( " int dataSize = 0;\n" ); + b3Dump( " b3MeshData* data = (b3MeshData*)b3ReadBinaryFile(dumpPrefix, \"%s\", &dataSize);\n", buffer ); + b3Dump( " if (data == nullptr || data->version != B3_MESH_VERSION || data->byteCount != dataSize)\n" ); + b3Dump( " {\n" ); + b3Dump( " return;\n" ); + b3Dump( " }\n" ); + b3Dump( " b3Vec3 scale = { %.9g, %.9g, %.9g };\n", scale.x, scale.y, scale.z ); + b3Dump( " b3CreateMeshShape(bodyId, &sd, data, scale);\n" ); + // Host code must have this container and destroy the mesh data when the world is destroyed. + b3Dump( " m_meshes.push_back(data);\n" ); + } + break; + + case b3_sphereShape: + { + b3Sphere* s = &shape->sphere; + b3Dump( " b3SphereShape shape;\n" ); + b3Dump( " shape.center = {%.9g, %.9g, %.9g};\n", s->center.x, s->center.y, s->center.z ); + b3Dump( " shape.radius = %.9g;\n", s->radius ); + b3Dump( " b3CreateSphereShape(bodyId, &sd, &shape);\n" ); + } + break; + + default: + return; + } +} diff --git a/src/shape.h b/src/shape.h new file mode 100644 index 0000000..57b6301 --- /dev/null +++ b/src/shape.h @@ -0,0 +1,143 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "math_internal.h" + +#include "box3d/types.h" + +#include + +typedef struct b3BroadPhase b3BroadPhase; +typedef struct b3World b3World; + +typedef struct b3Shape +{ + int id; + int bodyId; + int prevShapeId; + int nextShapeId; + int sensorIndex; + int proxyKey; + b3ShapeType type; + float density; + float explosionScale; + + b3AABB aabb; + b3AABB fatAABB; + b3Vec3 localCentroid; + + int materialCount; + b3SurfaceMaterial* materials; + + b3Filter filter; + char* name; + void* userData; + void* userShape; + + uint16_t generation; + bool enableSensorEvents; + bool enableContactEvents; + bool enableCustomFiltering; + bool enableHitEvents; + bool enablePreSolveEvents; + bool enlargedAABB; + + union + { + b3Capsule capsule; + b3Sphere sphere; + const b3Hull* hull; + b3Mesh mesh; + const b3HeightField* heightField; + const b3Compound* compound; + }; + +} b3Shape; + +#ifdef __cplusplus +extern "C" +{ +#endif + +void b3CreateShapeProxy( b3Shape* shape, b3BroadPhase* bp, b3BodyType type, b3Transform transform, bool forcePairCreation ); +void b3DestroyShapeProxy( b3Shape* shape, b3BroadPhase* bp ); + +void b3DestroyShapeAllocations( b3World* world, b3Shape* shape ); + +b3MassData b3ComputeShapeMass( const b3Shape* shape ); +b3ShapeExtent b3ComputeShapeExtent( const b3Shape* shape, b3Vec3 localCenter ); + +b3AABB b3ComputeSweptSphereAABB( const b3Sphere* shape, b3Transform xf1, b3Transform xf2 ); +b3AABB b3ComputeSweptCapsuleAABB( const b3Capsule* shape, b3Transform xf1, b3Transform xf2 ); + +b3AABB b3ComputeShapeAABB( const b3Shape* shape, b3Transform transform ); +b3AABB b3ComputeSweptShapeAABB( const b3Shape* shape, const b3Sweep* sweep, float time ); +b3Vec3 b3GetShapeCentroid( const b3Shape* shape ); +float b3GetShapeArea( const b3Shape* shape ); +float b3GetShapeProjectedArea( const b3Shape* shape, b3Vec3 planeNormal ); +uint64_t b3GetShapeUserMaterialId( const b3Shape* shape, int childIndex, int triangleIndex ); + +b3ShapeProxy b3MakeShapeProxy( const b3Shape* shape ); +b3ShapeProxy b3MakeLocalProxy( const b3ShapeProxy* proxy, b3Transform transform, b3Vec3* buffer ); +b3AABB b3ComputeProxyAABB( const b3ShapeProxy* proxy ); + +b3CastOutput b3RayCastShape( const b3Shape* shape, b3Transform transform, const b3RayCastInput* input ); +b3CastOutput b3ShapeCastShape( const b3Shape* shape, b3Transform transform, const b3ShapeCastInput* input ); +bool b3OverlapShape( const b3Shape* shape, b3Transform transform, const b3ShapeProxy* proxy ); + +float b3GetShapeArea( const b3Shape* shape ); +float b3GetShapeProjectedArea( const b3Shape* shape, b3Vec3 planeNormal ); +b3TOIOutput b3ShapeTimeOfImpact( b3Shape* shapeA, b3Shape* shapeB, b3Sweep* sweepA, b3Sweep* sweepB, float maxFraction ); + +int b3CollideMoverAndSphere( b3PlaneResult* result, const b3Sphere* shape, const b3Capsule* mover ); +int b3CollideMoverAndCapsule( b3PlaneResult* result, const b3Capsule* shape, const b3Capsule* mover ); +int b3CollideMoverAndHull( b3PlaneResult* result, const b3Hull* shape, const b3Capsule* mover ); +int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* shape, const b3Capsule* mover ); +int b3CollideMoverAndHeightField( b3PlaneResult* results, int capacity, const b3HeightField* shape, const b3Capsule* mover ); +int b3CollideMover( b3PlaneResult* planes, int planeCapacity, const b3Shape* shape, b3Transform transform, + const b3Capsule* mover ); + +// Hull +int b3FindHullSupportVertex( const b3Hull* hull, b3Vec3 direction ); +int b3FindHullSupportFace( const b3Hull* hull, b3Vec3 direction ); +bool b3IsValidHull( const b3Hull* hull ); +b3AABB b3ComputeSweptHullAABB( const b3Hull* shape, b3Transform xf1, b3Transform xf2 ); +b3ShapeExtent b3ComputeHullExtent( const b3Hull* hull, b3Vec3 origin ); +float b3ComputeHullProjectedArea( const b3Hull* hull, b3Vec3 direction ); + +// Height field +b3Triangle b3GetHeightFieldTriangle( const b3HeightField* heightField, int triangleIndex ); +int b3GetHeightFieldMaterial( const b3HeightField* heightField, int triangleIndex ); + +B3_INLINE int b3GetHeightFieldTriangleCount( const b3HeightField* heightField ) +{ + int cellCount = ( heightField->rowCount - 1 ) * ( heightField->columnCount - 1 ); + return 2 * cellCount; +} + +// Mesh +b3Triangle b3GetMeshTriangle( const b3Mesh* mesh, int triangleIndex ); +bool b3IsValidMesh( const b3MeshData* meshData ); +void b3DumpShape( b3World* world, int shapeIndex ); + +#ifdef __cplusplus +} +#endif + +B3_INLINE bool b3ShouldShapesCollide( b3Filter filterA, b3Filter filterB ) +{ + if ( filterA.groupIndex == filterB.groupIndex && filterA.groupIndex != 0 ) + { + return filterA.groupIndex > 0; + } + + return ( filterA.maskBits & filterB.categoryBits ) != 0 && ( filterA.categoryBits & filterB.maskBits ) != 0; +} + +B3_INLINE bool b3ShouldQueryCollide( const b3Filter* shapeFilter, const b3QueryFilter* queryFilter ) +{ + return ( shapeFilter->categoryBits & queryFilter->maskBits ) != 0 && + ( shapeFilter->maskBits & queryFilter->categoryBits ) != 0; +} diff --git a/src/simd.c b/src/simd.c new file mode 100644 index 0000000..6a9faac --- /dev/null +++ b/src/simd.c @@ -0,0 +1,202 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +// Dirk Gregorius contributed portions of this code + +#include "simd.h" + +#if defined( B3_SIMD_SSE2 ) + +#define B3_TRANSPOSE3( C1, C2, C3 ) \ + { \ + b3V32 T1 = _mm_unpacklo_ps( ( C1 ), ( C2 ) ); \ + b3V32 T2 = _mm_unpackhi_ps( ( C1 ), ( C2 ) ); \ + ( C1 ) = _mm_shuffle_ps( ( T1 ), ( C3 ), _MM_SHUFFLE( 0, 0, 1, 0 ) ); \ + ( C2 ) = _mm_shuffle_ps( ( T1 ), ( C3 ), _MM_SHUFFLE( 1, 1, 3, 2 ) ); \ + ( C3 ) = _mm_shuffle_ps( ( T2 ), ( C3 ), _MM_SHUFFLE( 2, 2, 1, 0 ) ); \ + } + +static inline b3V32 b3SplatXV( b3V32 v ) +{ + return _mm_shuffle_ps( v, v, _MM_SHUFFLE( 0, 0, 0, 0 ) ); +} + +static inline b3V32 b3SplatYV( b3V32 v ) +{ + return _mm_shuffle_ps( v, v, _MM_SHUFFLE( 1, 1, 1, 1 ) ); +} + +static inline b3V32 b3SplatZV( b3V32 v ) +{ + return _mm_shuffle_ps( v, v, _MM_SHUFFLE( 2, 2, 2, 2 ) ); +} + +static inline bool b3AnyGreaterEq3V( b3V32 a, b3V32 b ) +{ + b3V32 v = _mm_cmpge_ps( a, b ); + return ( _mm_movemask_ps( v ) & 0x07 ) != 0; +} + +static inline b3V32 b3Dot3V( b3V32 a, b3V32 b ) +{ + b3V32 m = _mm_mul_ps( a, b ); + b3V32 x = _mm_shuffle_ps( m, m, _MM_SHUFFLE( 0, 0, 0, 0 ) ); + b3V32 y = _mm_shuffle_ps( m, m, _MM_SHUFFLE( 1, 1, 1, 1 ) ); + b3V32 z = _mm_shuffle_ps( m, m, _MM_SHUFFLE( 2, 2, 2, 2 ) ); + + return _mm_add_ps( _mm_add_ps( x, y ), z ); +} + +#else + +#define B3_TRANSPOSE3( C1, C2, C3 ) \ + { \ + float temp1 = C1.y; \ + float temp2 = C1.z; \ + float temp3 = C2.z; \ + \ + C1.y = C2.x; \ + C1.z = C3.x; \ + C2.z = C3.y; \ + \ + C2.x = temp1; \ + C3.x = temp2; \ + C3.y = temp3; \ + } + +static inline b3V32 b3SplatXV( b3V32 a ) +{ + return B3_LITERAL( b3V32 ){ a.x, a.x, a.x }; +} + +static inline b3V32 b3SplatYV( b3V32 a ) +{ + return B3_LITERAL( b3V32 ){ a.y, a.y, a.y }; +} + +static inline b3V32 b3SplatZV( b3V32 a ) +{ + return B3_LITERAL( b3V32 ){ a.z, a.z, a.z }; +} + +static inline bool b3AnyGreaterEq3V( b3V32 a, b3V32 b ) +{ + return a.x >= b.x || a.y >= b.y || a.z >= b.z; +} + +static inline b3V32 b3Dot3V( b3V32 a, b3V32 b ) +{ + float d = a.x * b.x + a.y * b.y + a.z * b.z; + return B3_LITERAL( b3V32 ){ d, d, d }; +} + +#endif + +bool b3TestBoundsTriangleOverlap( b3V32 nodeCenter, b3V32 nodeExtent, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 ) +{ + b3V32 two = b3SplatV( 2.0f ); + + // Setup triangle + vertex1 = b3SubV( vertex1, nodeCenter ); + vertex2 = b3SubV( vertex2, nodeCenter ); + vertex3 = b3SubV( vertex3, nodeCenter ); + + // Face separation + b3V32 triangleMin = b3MinV( vertex1, b3MinV( vertex2, vertex3 ) ); + b3V32 triangleMax = b3MaxV( vertex1, b3MaxV( vertex2, vertex3 ) ); + + b3V32 separation1 = b3SubV( triangleMin, nodeExtent ); + b3V32 separation2 = b3AddV( triangleMax, nodeExtent ); + + b3V32 faceSeparation = b3MaxV( separation1, b3NegV( separation2 ) ); + if ( b3AnyGreater3V( faceSeparation, b3_zeroV ) ) + { + return false; + } + + // SAT: Face separation + b3V32 edge1 = b3SubV( vertex2, vertex1 ); + b3V32 edge2 = b3SubV( vertex3, vertex2 ); + b3V32 edge3 = b3SubV( vertex1, vertex3 ); + + b3V32 normal = b3CrossV( edge1, edge2 ); + + b3V32 triangleSeparation = b3SubV( b3AbsV( b3Dot3V( normal, vertex1 ) ), b3Dot3V( b3AbsV( normal ), nodeExtent ) ); + if ( b3AnyGreater3V( triangleSeparation, b3_zeroV ) ) + { + return false; + } + + // SAT: Edge separation + b3V32 edgeSeparation1 = b3SubV( b3SubV( b3AbsV( b3CrossV( edge1, b3AddV( vertex1, vertex3 ) ) ), b3AbsV( b3CrossV( edge1, edge3 ) ) ), + b3MulV( two, b3ModifiedCrossV( b3AbsV( edge1 ), nodeExtent ) ) ); + if ( b3AnyGreater3V( edgeSeparation1, b3_zeroV ) ) + { + return false; + } + + b3V32 edgeSeparation2 = b3SubV( b3SubV( b3AbsV( b3CrossV( edge2, b3AddV( vertex1, vertex2 ) ) ), b3AbsV( b3CrossV( edge2, edge1 ) ) ), + b3MulV( two, b3ModifiedCrossV( b3AbsV( edge2 ), nodeExtent ) ) ); + if ( b3AnyGreater3V( edgeSeparation2, b3_zeroV ) ) + { + return false; + } + + b3V32 edgeSeparation3 = b3SubV( b3SubV( b3AbsV( b3CrossV( edge3, b3AddV( vertex2, vertex3 ) ) ), b3AbsV( b3CrossV( edge3, edge2 ) ) ), + b3MulV( two, b3ModifiedCrossV( b3AbsV( edge3 ), nodeExtent ) ) ); + if ( b3AnyGreater3V( edgeSeparation3, b3_zeroV ) ) + { + return false; + } + + return true; +} + +float b3IntersectRayTriangle( b3V32 rayStart, b3V32 rayDelta, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 ) +{ + // Test if ray intersects this triangle sharing same calculations for each triangle + { + b3V32 edge1 = b3SubV( vertex3, vertex2 ); + b3V32 edge2 = b3SubV( vertex1, vertex3 ); + b3V32 edge3 = b3SubV( vertex2, vertex1 ); + + b3V32 midPoint1 = b3MulV( b3_halfV, b3AddV( vertex2, vertex3 ) ); + b3V32 midPoint2 = b3MulV( b3_halfV, b3AddV( vertex3, vertex1 ) ); + b3V32 midPoint3 = b3MulV( b3_halfV, b3AddV( vertex1, vertex2 ) ); + + b3V32 normal1 = b3CrossV( edge1, b3SubV( midPoint1, rayStart ) ); + b3V32 normal2 = b3CrossV( edge2, b3SubV( midPoint2, rayStart ) ); + b3V32 normal3 = b3CrossV( edge3, b3SubV( midPoint3, rayStart ) ); + B3_TRANSPOSE3( normal1, normal2, normal3 ); + + b3V32 rayDeltaX = b3SplatXV( rayDelta ); + b3V32 rayDeltaY = b3SplatYV( rayDelta ); + b3V32 rayDeltaZ = b3SplatZV( rayDelta ); + + b3V32 volumes = b3AddV( b3AddV( b3MulV( normal1, rayDeltaX ), b3MulV( normal2, rayDeltaY ) ), b3MulV( normal3, rayDeltaZ ) ); + if ( b3AnyLess3V( volumes, b3_zeroV ) ) + { + return 1.0f; + } + } + + // Compute intersection with triangle plane + b3V32 edge1 = b3SubV( vertex2, vertex1 ); + b3V32 edge2 = b3SubV( vertex3, vertex1 ); + b3V32 normal = b3CrossV( edge1, edge2 ); + + b3V32 denominator = b3Dot3V( normal, rayDelta ); + if ( b3AnyGreaterEq3V( denominator, b3_zeroV ) ) + { + return 1.0f; + } + + b3V32 lambda = b3DivV( b3Dot3V( normal, b3SubV( vertex1, rayStart ) ), denominator ); + if ( b3AnyLessEq3V( lambda, b3_zeroV ) ) + { + return 1.0f; + } + + lambda = b3MinV( lambda, b3_oneV ); + return b3GetXV( lambda ); +} diff --git a/src/simd.h b/src/simd.h new file mode 100644 index 0000000..ec9399c --- /dev/null +++ b/src/simd.h @@ -0,0 +1,374 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "core.h" + +#include + +#if defined( B3_SIMD_SSE2 ) + +#include + +// wide float holds 4 numbers +typedef __m128 b3V32; + +typedef union b3128 +{ + b3V32 v; + float f[4]; +} b3128; + +#if defined( _MSC_VER ) && !defined( __clang__ ) + +static const b3V32 b3_zeroV = { { 0.0f, 0.0f, 0.0f, 0.0f } }; +static const b3V32 b3_halfV = { { 0.5f, 0.5f, 0.5f, 0.5f } }; +static const b3V32 b3_oneV = { { 1.0f, 1.0f, 1.0f, 1.0f } }; + +#else + +static const b3V32 b3_zeroV = { 0.0f, 0.0f, 0.0f, 0.0f }; +static const b3V32 b3_halfV = { 0.5f, 0.5f, 0.5f, 0.5f }; +static const b3V32 b3_oneV = { 1.0f, 1.0f, 1.0f, 1.0f }; + +#endif + +static inline b3V32 b3AddV( b3V32 a, b3V32 b ) +{ + return _mm_add_ps( a, b ); +} + +static inline b3V32 b3SubV( b3V32 a, b3V32 b ) +{ + return _mm_sub_ps( a, b ); +} + +static inline b3V32 b3MulV( b3V32 a, b3V32 b ) +{ + return _mm_mul_ps( a, b ); +} + +static inline b3V32 b3DivV( b3V32 a, b3V32 b ) +{ + return _mm_div_ps( a, b ); +} + +static inline b3V32 b3NegV( b3V32 a ) +{ + return _mm_sub_ps( _mm_setzero_ps(), a ); +} + +// Unaligned loads are much faster on recent hardware with little to no penalty +// Note: This may read one extra float beyond a b3Vec3 for performance. Address sanitizer +// is disabled for this function as the extra read is intentional and safe. +#if defined( __linux__ ) || defined( __APPLE__ ) || defined( __clang__ ) +__attribute__( ( no_sanitize_address ) ) +#elif defined( _MSC_VER ) +__declspec( no_sanitize_address ) +#endif +static inline b3V32 +b3LoadV( const float* src ) +{ + const __m128i masK3 = _mm_setr_epi32( 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0 ); + + // Read an extra float which gets zeroed (to avoid NaN or Inf) + b3V32 a = _mm_loadu_ps( src ); + return _mm_and_ps( a, _mm_castsi128_ps( masK3 ) ); +} + +static inline b3V32 b3ZeroV( void ) +{ + return _mm_setzero_ps(); +} + +static inline float b3GetXV( b3V32 a ) +{ + return _mm_cvtss_f32( a ); +} + +static inline float b3GetYV( b3V32 a ) +{ + return _mm_cvtss_f32( _mm_shuffle_ps( a, a, _MM_SHUFFLE( 1, 1, 1, 1 ) ) ); +} + +static inline float b3GetZV( b3V32 a ) +{ + return _mm_cvtss_f32( _mm_shuffle_ps( a, a, _MM_SHUFFLE( 2, 2, 2, 2 ) ) ); +} + +static inline float b3GetV( b3V32 a, int index ) +{ + b3128 b; + b.v = a; + return b.f[index]; +} + +static inline b3V32 b3SplatV( float x ) +{ + return _mm_set_ps1( x ); +} + +static inline b3V32 b3AbsV( b3V32 a ) +{ + // Abs( V ) = Max( -V, V ) + b3V32 zero = _mm_setzero_ps(); + return _mm_max_ps( _mm_sub_ps( zero, a ), a ); +} + +static inline b3V32 b3MinV( b3V32 a, b3V32 b ) +{ + return _mm_min_ps( a, b ); +} + +static inline b3V32 b3MaxV( b3V32 a, b3V32 b ) +{ + return _mm_max_ps( a, b ); +} + +static inline b3V32 b3CrossV( b3V32 a, b3V32 b ) +{ + b3V32 yzX1 = _mm_shuffle_ps( a, a, _MM_SHUFFLE( 3, 0, 2, 1 ) ); + b3V32 zxY1 = _mm_shuffle_ps( a, a, _MM_SHUFFLE( 3, 1, 0, 2 ) ); + b3V32 yzX2 = _mm_shuffle_ps( b, b, _MM_SHUFFLE( 3, 0, 2, 1 ) ); + b3V32 zxY2 = _mm_shuffle_ps( b, b, _MM_SHUFFLE( 3, 1, 0, 2 ) ); + + return _mm_sub_ps( _mm_mul_ps( yzX1, zxY2 ), _mm_mul_ps( zxY1, yzX2 ) ); +} + +static inline b3V32 b3ModifiedCrossV( b3V32 a, b3V32 b ) +{ + b3V32 yzX1 = _mm_shuffle_ps( a, a, _MM_SHUFFLE( 3, 0, 2, 1 ) ); + b3V32 zxY1 = _mm_shuffle_ps( a, a, _MM_SHUFFLE( 3, 1, 0, 2 ) ); + b3V32 yzX2 = _mm_shuffle_ps( b, b, _MM_SHUFFLE( 3, 0, 2, 1 ) ); + b3V32 zxY2 = _mm_shuffle_ps( b, b, _MM_SHUFFLE( 3, 1, 0, 2 ) ); + + return _mm_add_ps( _mm_mul_ps( yzX1, zxY2 ), _mm_mul_ps( zxY1, yzX2 ) ); +} + +static inline bool b3AnyLess3V( b3V32 a, b3V32 b ) +{ + b3V32 v = _mm_cmplt_ps( a, b ); + return ( _mm_movemask_ps( v ) & 0x07 ) != 0; +} + +static inline bool b3AnyLessEq3V( b3V32 a, b3V32 b ) +{ + b3V32 v = _mm_cmple_ps( a, b ); + return ( _mm_movemask_ps( v ) & 0x07 ) != 0; +} + +static inline bool b3AnyGreater3V( b3V32 a, b3V32 b ) +{ + b3V32 v = _mm_cmpgt_ps( a, b ); + return ( _mm_movemask_ps( v ) & 0x07 ) != 0; +} + +static inline bool b3AllLessEq3V( b3V32 a, b3V32 b ) +{ + b3V32 v = _mm_cmple_ps( a, b ); + return ( _mm_movemask_ps( v ) & 0x07 ) == 0x07; +} + +#else + +// I don't expect the use case of b3V32 to benefit from Neon code. +// In particular the cross product is very complex in Neon. + +// scalar math +typedef struct b3V32 +{ + float x, y, z; +} b3V32; + +typedef union b3128 +{ + b3V32 v; + float f[3]; +} b3128; + +static const b3V32 b3_zeroV = { 0.0f, 0.0f, 0.0f }; +static const b3V32 b3_halfV = { 0.5f, 0.5f, 0.5f }; +static const b3V32 b3_oneV = { 1.0f, 1.0f, 1.0f }; + +static inline b3V32 b3AddV( b3V32 a, b3V32 b ) +{ + return B3_LITERAL( b3V32 ){ + a.x + b.x, + a.y + b.y, + a.z + b.z, + }; +} + +static inline b3V32 b3SubV( b3V32 a, b3V32 b ) +{ + return B3_LITERAL( b3V32 ){ + a.x - b.x, + a.y - b.y, + a.z - b.z, + }; +} + +static inline b3V32 b3MulV( b3V32 a, b3V32 b ) +{ + return B3_LITERAL( b3V32 ){ + a.x * b.x, + a.y * b.y, + a.z * b.z, + }; +} + +static inline b3V32 b3DivV( b3V32 a, b3V32 b ) +{ + return B3_LITERAL( b3V32 ){ + a.x / b.x, + a.y / b.y, + a.z / b.z, + }; +} + +static inline b3V32 b3NegV( b3V32 a ) +{ + return B3_LITERAL( b3V32 ){ + -a.x, + -a.y, + -a.z, + }; +} + +// Unaligned loads are much faster on recent hardware with little to no penalty +static inline b3V32 b3LoadV( const float* src ) +{ + return B3_LITERAL( b3V32 ){ src[0], src[1], src[2] }; +} + +static inline b3V32 b3ZeroV( void ) +{ + return B3_LITERAL( b3V32 ){ 0.0f, 0.0f, 0.0f }; +} + +static inline float b3GetXV( b3V32 a ) +{ + return a.x; +} + +static inline float b3GetYV( b3V32 a ) +{ + return a.y; +} + +static inline float b3GetZV( b3V32 a ) +{ + return a.z; +} + +static inline float b3GetV( b3V32 a, int index ) +{ + b3128 b; + b.v = a; + return b.f[index]; +} + +static inline b3V32 b3SplatV( float x ) +{ + return B3_LITERAL( b3V32 ){ x, x, x }; +} + +static inline b3V32 b3AbsV( b3V32 a ) +{ + return B3_LITERAL( b3V32 ){ + a.x < 0.0f ? -a.x : a.x, + a.y < 0.0f ? -a.y : a.y, + a.z < 0.0f ? -a.z : a.z, + }; +} + +static inline b3V32 b3MinV( b3V32 a, b3V32 b ) +{ + return B3_LITERAL( b3V32 ){ + a.x < b.x ? a.x : b.x, + a.y < b.y ? a.y : b.y, + a.z < b.z ? a.z : b.z, + }; +} + +static inline b3V32 b3MaxV( b3V32 a, b3V32 b ) +{ + return B3_LITERAL( b3V32 ){ + a.x > b.x ? a.x : b.x, + a.y > b.y ? a.y : b.y, + a.z > b.z ? a.z : b.z, + }; +} + +static inline b3V32 b3CrossV( b3V32 a, b3V32 b ) +{ + b3V32 c; + c.x = a.y * b.z - a.z * b.y; + c.y = a.z * b.x - a.x * b.z; + c.z = a.x * b.y - a.y * b.x; + return c; +} + +static inline b3V32 b3ModifiedCrossV( b3V32 a, b3V32 b ) +{ + b3V32 c; + c.x = a.y * b.z + a.z * b.y; + c.y = a.z * b.x + a.x * b.z; + c.z = a.x * b.y + a.y * b.x; + return c; +} + +static inline bool b3AnyLess3V( b3V32 a, b3V32 b ) +{ + return a.x < b.x || a.y < b.y || a.z < b.z; +} + +static inline bool b3AnyLessEq3V( b3V32 a, b3V32 b ) +{ + return a.x <= b.x || a.y <= b.y || a.z <= b.z; +} + +static inline bool b3AnyGreater3V( b3V32 a, b3V32 b ) +{ + return a.x > b.x || a.y > b.y || a.z > b.z; +} + +static inline bool b3AllLessEq3V( b3V32 a, b3V32 b ) +{ + return a.x <= b.x && a.y <= b.y && a.z <= b.z; +} + +#endif + +static inline bool b3TestBoundsOverlap( b3V32 nodeMin1, b3V32 nodeMax1, b3V32 nodeMin2, b3V32 nodeMax2 ) +{ + b3V32 separation = b3MaxV( b3SubV( nodeMin2, nodeMax1 ), b3SubV( nodeMin1, nodeMax2 ) ); + return b3AllLessEq3V( separation, b3_zeroV ); +} + +// Test a ray for edge separation with an AABB (Gino, p80). +static inline bool b3TestBoundsRayOverlap( b3V32 nodeMin, b3V32 nodeMax, b3V32 rayStart, b3V32 rayDelta ) +{ + // Setup node + b3V32 nodeCenter = b3MulV( b3_halfV, b3AddV( nodeMin, nodeMax ) ); + b3V32 nodeExtent = b3SubV( nodeMax, nodeCenter ); + + // Setup ray + rayStart = b3SubV( rayStart, nodeCenter ); + + // SAT: Edge separation + b3V32 edgeSeparation = b3SubV( b3AbsV( b3CrossV( rayDelta, rayStart ) ), b3ModifiedCrossV( b3AbsV( rayDelta ), nodeExtent ) ); + return b3AllLessEq3V( edgeSeparation, b3_zeroV ); +} + +#ifdef __cplusplus +extern "C" { +#endif + +bool b3TestBoundsTriangleOverlap( b3V32 nodeCenter, b3V32 nodeExtent, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 ); +float b3IntersectRayTriangle( b3V32 rayStart, b3V32 rayDelta, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 ); + +#ifdef __cplusplus +} +#endif diff --git a/src/solver.c b/src/solver.c new file mode 100644 index 0000000..2edddf3 --- /dev/null +++ b/src/solver.c @@ -0,0 +1,2292 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "solver.h" + +#include "arena_allocator.h" +#include "bitset.h" +#include "body.h" +#include "compound.h" +#include "contact.h" +#include "contact_solver.h" +#include "core.h" +#include "ctz.h" +#include "island.h" +#include "joint.h" +#include "parallel_for.h" +#include "physics_world.h" +#include "platform.h" +#include "sensor.h" +#include "shape.h" +#include "solver_set.h" + +#include +#include +#include + +// these are useful for solver testing +#define ITERATIONS 1 +#define RELAX_ITERATIONS 1 + +#if ( defined( __GNUC__ ) || defined( __clang__ ) ) && ( defined( __i386__ ) || defined( __x86_64__ ) ) +static void b3Pause( void ) +{ + __asm__ __volatile__( "pause\n" ); +} +#elif ( defined( __arm__ ) && defined( __ARM_ARCH ) && __ARM_ARCH >= 7 ) || defined( __aarch64__ ) +static void b3Pause( void ) +{ + __asm__ __volatile__( "yield" ::: "memory" ); +} +#elif defined( _MSC_VER ) && ( defined( _M_IX86 ) || defined( _M_X64 ) ) +static void b3Pause( void ) +{ + _mm_pause(); +} +#elif defined( _MSC_VER ) && ( defined( _M_ARM ) || defined( _M_ARM64 ) ) +static void b3Pause( void ) +{ + __yield(); +} +#else +static void b3Pause( void ) +{ +} +#endif + +typedef struct b3WorkerContext +{ + b3StepContext* context; + int workerIndex; + void* userTask; +} b3WorkerContext; + +// Integrate velocities, apply damping, and gyroscopic torque +static void b3IntegrateVelocitiesTask( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( integrate_velocity, "IntVel", b3_colorDeepPink, true ); + + B3_VALIDATE( block.startIndex + block.count <= context->world->solverSets.data[b3_awakeSet].bodyStates.count ); + + b3BodyState* states = context->states; + b3BodySim* sims = context->sims; + + b3Vec3 gravity = context->world->gravity; + float h = context->h; + + for ( int i = block.startIndex; i < block.startIndex + block.count; ++i ) + { + b3BodySim* sim = sims + i; + b3BodyState* state = states + i; + + b3Vec3 v = state->linearVelocity; + b3Vec3 w = state->angularVelocity; + + // Damping math + // Differential equation: dv/dt + c * v = 0 + // Solution: v(t) = v0 * exp(-c * t) + // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v(t) * exp(-c * dt) + // v2 = exp(-c * dt) * v1 + // Pade approximation: + // v2 = v1 * 1 / (1 + c * dt) + float linearDamping = 1.0f / ( 1.0f + h * sim->linearDamping ); + float angularDamping = 1.0f / ( 1.0f + h * sim->angularDamping ); + + // Gravity scale will be zero for kinematic bodies + float gravityScale = sim->invMass > 0.0f ? sim->gravityScale : 0.0f; + + b3Vec3 linearVelocityDelta = b3Blend2( h * sim->invMass, sim->force, h * gravityScale, gravity ); + v = b3MulAdd( linearVelocityDelta, linearDamping, v ); + + b3Vec3 angularVelocityDelta = b3MulSV( h, b3MulMV( sim->invInertiaWorld, sim->torque ) ); + w = b3MulAdd( angularVelocityDelta, angularDamping, w ); + + // Gyroscopic torque by solving this nonlinear equation using Newton-Raphson. + // I * (w2 - w1) + h * cross(w2, I * w2) = 0 + // This is all done in local coordinates where the Jacobian is easier to compute. + // This improves the simulation of long skinny bodies. + { + // Get current rotation. + b3Quat q0 = sim->transform.q; + b3Quat q = b3MulQuat( state->deltaRotation, q0 ); + + // todo wasteful computation + b3Matrix3 inertiaLocal = b3InvertMatrix( sim->invInertiaLocal ); + + // Compute local angular velocity + b3Vec3 omega1 = b3InvRotateVector( q, w ); + b3Vec3 omega2 = omega1; + + // Symmetric inertia tensor: 6 unique entries (column-major) + const float i00 = inertiaLocal.cx.x; + const float i01 = inertiaLocal.cy.x; + const float i02 = inertiaLocal.cz.x; + const float i11 = inertiaLocal.cy.y; + const float i12 = inertiaLocal.cz.y; + const float i22 = inertiaLocal.cz.z; + + for ( int gyroIteration = 0; gyroIteration < 1; ++gyroIteration ) + { + const float w1 = omega2.x; + const float w2 = omega2.y; + const float w3 = omega2.z; + + // Iw = I * omega2 (shared between residual and Jacobian) + const float Iw1 = i00 * w1 + i01 * w2 + i02 * w3; + const float Iw2 = i01 * w1 + i11 * w2 + i12 * w3; + const float Iw3 = i02 * w1 + i12 * w2 + i22 * w3; + + // Residual: b = I*(omega2 - omega1) + h * (omega2 × I*omega2) + const b3Vec3 dw = b3Sub( omega2, omega1 ); + b3Vec3 b = { + i00 * dw.x + i01 * dw.y + i02 * dw.z + h * ( w2 * Iw3 - w3 * Iw2 ), + i01 * dw.x + i11 * dw.y + i12 * dw.z + h * ( w3 * Iw1 - w1 * Iw3 ), + i02 * dw.x + i12 * dw.y + i22 * dw.z + h * ( w1 * Iw2 - w2 * Iw1 ), + }; + + // Jacobian J = I + h * (skew(omega2) * I - skew(I*omega2)) + // Jacobian derived by Erin Catto, Ph.D. Do not attempt to do this without a Ph.D. + // Doubled inertia terms above fold into Iw, e.g. row 2 col 1: i00*w3 - i02*w1 - Iw3. + b3Matrix3 J = { + { i00 + h * ( w2 * i02 - w3 * i01 ), + i01 + h * ( w3 * i00 - w1 * i02 - Iw3 ), + i02 + h * ( w1 * i01 - w2 * i00 + Iw2 ) }, + { i01 + h * ( w2 * i12 - w3 * i11 + Iw3 ), + i11 + h * ( w3 * i01 - w1 * i12 ), + i12 + h * ( w1 * i11 - w2 * i01 - Iw1 ) }, + { i02 + h * ( w2 * i22 - w3 * i12 - Iw2 ), + i12 + h * ( w3 * i02 - w1 * i22 + Iw1 ), + i22 + h * ( w1 * i12 - w2 * i02 ) }, + }; + + omega2 = b3Sub( omega2, b3Solve3( J, b ) ); + } + + w = b3RotateVector( q, omega2 ); + } + + state->linearVelocity = v; + state->angularVelocity = w; + } + + b3TracyCZoneEnd( integrate_velocity ); +} + +static void b3IntegratePositionsTask( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( integrate_positions, "IntPos", b3_colorDarkSeaGreen, true ); + + B3_VALIDATE( block.startIndex + block.count <= context->world->solverSets.data[b3_awakeSet].bodyStates.count ); + + b3BodyState* states = context->states; + float h = context->h; + float maxLinearSpeed = context->maxLinearVelocity; + float maxAngularSpeed = B3_MAX_ROTATION * context->inv_dt; + float maxLinearSpeedSquared = maxLinearSpeed * maxLinearSpeed; + float maxAngularSpeedSquared = maxAngularSpeed * maxAngularSpeed; + + for ( int i = block.startIndex; i < block.startIndex + block.count; ++i ) + { + b3BodyState* state = states + i; + + b3Vec3 v = state->linearVelocity; + b3Vec3 w = state->angularVelocity; + + // Motion locks - these can be viewed as a constraint that come last + v.x = ( state->flags & b3_lockLinearX ) ? 0.0f : v.x; + v.y = ( state->flags & b3_lockLinearY ) ? 0.0f : v.y; + v.z = ( state->flags & b3_lockLinearZ ) ? 0.0f : v.z; + w.x = ( state->flags & b3_lockAngularX ) ? 0.0f : w.x; + w.y = ( state->flags & b3_lockAngularY ) ? 0.0f : w.y; + w.z = ( state->flags & b3_lockAngularZ ) ? 0.0f : w.z; + + // Clamp to max linear speed + if ( b3Dot( v, v ) > maxLinearSpeedSquared ) + { + float ratio = maxLinearSpeed / b3Length( v ); + v = b3MulSV( ratio, v ); + state->flags |= b3_isSpeedCapped; + } + + // Clamp to max angular speed + if ( b3Dot( w, w ) > maxAngularSpeedSquared && ( state->flags & b3_allowFastRotation ) == 0 ) + { + float ratio = maxAngularSpeed / b3Length( w ); + w = b3MulSV( ratio, w ); + state->flags |= b3_isSpeedCapped; + } + + state->linearVelocity = v; + state->angularVelocity = w; + state->deltaPosition = b3MulAdd( state->deltaPosition, h, v ); + state->deltaRotation = b3IntegrateRotation( state->deltaRotation, b3MulSV( h, w ) ); + } + + b3TracyCZoneEnd( integrate_positions ); +} + +static void b3PrepareJointsTask( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( prepare_joints, "PrepJoints", b3_colorOldLace, true ); + + b3JointPrepareSpan* spans = context->jointPrepareSpans; + + int index = block.startIndex; + int endIndex = block.startIndex + block.count; + + // Find color for start index. Linear search but fast. + int colorIndex = 0; + while ( spans[colorIndex + 1].start <= index ) + { + colorIndex += 1; + } + + // Loop over block + while ( index < endIndex ) + { + int colorStart = spans[colorIndex].start; + int colorEndIndex = b3MinInt( spans[colorIndex + 1].start, endIndex ); + b3JointSim* joints = spans[colorIndex].joints; + + // Loop over color + for ( ; index < colorEndIndex; ++index ) + { + B3_ASSERT( 0 <= index - colorStart && index - colorStart < spans[colorIndex].count ); + b3JointSim* joint = joints + ( index - colorStart ); + b3PrepareJoint( joint, context ); + } + + // Advance to next color + colorIndex += 1; + } + + b3TracyCZoneEnd( prepare_joints ); +} + +static void b3WarmStartJointsTask( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( warm_joints, "WarmJoints", b3_colorGold, true ); + + b3GraphColor* color = context->graph->colors + block.colorIndex; + b3JointSim* joints = color->jointSims.data; + + for ( int i = block.startIndex; i < block.startIndex + block.count; ++i ) + { + b3JointSim* joint = joints + i; + b3WarmStartJoint( joint, context ); + } + + b3TracyCZoneEnd( warm_joints ); +} + +static void b3SolveJointsTask( b3SolverBlock block, b3StepContext* context, bool useBias, int workerIndex ) +{ + b3TracyCZoneNC( solve_joints, "SolveJoints", b3_colorLemonChiffon, true ); + + b3GraphColor* color = context->graph->colors + block.colorIndex; + b3JointSim* joints = color->jointSims.data; + + B3_ASSERT( 0 <= block.startIndex && block.startIndex + block.count <= color->jointSims.count ); + + b3BitSet* jointStateBitSet = &context->world->taskContexts.data[workerIndex].jointStateBitSet; + + for ( int i = block.startIndex; i < block.startIndex + block.count; ++i ) + { + b3JointSim* joint = joints + i; + b3SolveJoint( joint, context, useBias ); + + if ( useBias && ( joint->forceThreshold < FLT_MAX || joint->torqueThreshold < FLT_MAX ) && + b3GetBit( jointStateBitSet, joint->jointId ) == false ) + { + float force, torque; + b3GetJointReaction( context->world, joint, context->inv_h, &force, &torque ); + + // Check thresholds. A zero threshold means all awake joints get reported. + if ( force >= joint->forceThreshold || torque >= joint->torqueThreshold ) + { + // Flag this joint for processing. + b3SetBit( jointStateBitSet, joint->jointId ); + } + } + } + + b3TracyCZoneEnd( solve_joints ); +} + +#define B2_MAX_CONTINUOUS_SENSOR_HITS 8 + +typedef struct b3ContinuousContext +{ + b3World* world; + b3BodySim* fastBodySim; + b3Shape* fastShape; + b3Vec3 centroid1, centroid2; + b3Sweep sweep; + float fraction; + b3SensorHit sensorHits[B2_MAX_CONTINUOUS_SENSOR_HITS]; + float sensorFractions[B2_MAX_CONTINUOUS_SENSOR_HITS]; + int sensorCount; +} b3ContinuousContext; + +// This is called from b3DynamicTree_Query for continuous collision +static bool b3ContinuousQueryCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + b3ContinuousContext* continuousContext = (b3ContinuousContext*)context; + b3Shape* fastShape = continuousContext->fastShape; + b3BodySim* fastBodySim = continuousContext->fastBodySim; + + B3_ASSERT( fastShape->sensorIndex == B3_NULL_INDEX ); + + // Skip same shape + if ( shapeId == fastShape->id ) + { + return true; + } + + b3World* world = continuousContext->world; + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + // Skip same body + if ( shape->bodyId == fastShape->bodyId ) + { + return true; + } + + // Skip sensors unless the shapes want sensor events + bool isSensor = shape->sensorIndex != B3_NULL_INDEX; + if ( isSensor && ( shape->enableSensorEvents == false || fastShape->enableSensorEvents == false ) ) + { + return true; + } + + // Skip filtered shapes + bool canCollide = b3ShouldShapesCollide( fastShape->filter, shape->filter ); + if ( canCollide == false ) + { + return true; + } + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + + b3BodySim* bodySim = b3GetBodySim( world, body ); + B3_ASSERT( body->type == b3_staticBody || ( fastBodySim->flags & b3_isBullet ) ); + + // Skip bullets + if ( bodySim->flags & b3_isBullet ) + { + return true; + } + + // Skip filtered bodies + b3Body* fastBody = b3Array_Get( world->bodies, fastBodySim->bodyId ); + canCollide = b3ShouldBodiesCollide( world, fastBody, body ); + if ( canCollide == false ) + { + return true; + } + + // Custom user filtering + if ( shape->enableCustomFiltering || fastShape->enableCustomFiltering ) + { + b3CustomFilterFcn* customFilterFcn = world->customFilterFcn; + if ( customFilterFcn != NULL ) + { + b3ShapeId idA = { shape->id + 1, world->worldId, shape->generation }; + b3ShapeId idB = { fastShape->id + 1, world->worldId, fastShape->generation }; + canCollide = customFilterFcn( idA, idB, world->customFilterContext ); + if ( canCollide == false ) + { + return true; + } + } + } + + // todo does having a sweep on shapeA help with bullets? + b3Sweep sweepA = b3MakeSweep( bodySim ); + + // Time of impact versus shape. Supports all shape types + b3TOIOutput output = b3ShapeTimeOfImpact( shape, fastShape, &sweepA, &continuousContext->sweep, continuousContext->fraction ); + if ( isSensor ) + { + // Only accept a sensor hit that is sooner than the current solid hit. + if ( output.fraction <= continuousContext->fraction && continuousContext->sensorCount < B2_MAX_CONTINUOUS_SENSOR_HITS ) + { + int index = continuousContext->sensorCount; + + // The hit shape is a sensor + b3SensorHit sensorHit = { + .sensorId = shape->id, + .visitorId = fastShape->id, + }; + + continuousContext->sensorHits[index] = sensorHit; + continuousContext->sensorFractions[index] = output.fraction; + continuousContext->sensorCount += 1; + } + } + else if ( 0.0f < output.fraction && output.fraction < continuousContext->fraction ) + { + bool didHit = true; + + if ( didHit && ( shape->enablePreSolveEvents || fastShape->enablePreSolveEvents ) ) + { + b3ShapeId shapeIdA = { shape->id + 1, world->worldId, shape->generation }; + b3ShapeId shapeIdB = { fastShape->id + 1, world->worldId, fastShape->generation }; + didHit = world->preSolveFcn( shapeIdA, shapeIdB, output.point, output.normal, world->preSolveContext ); + } + + if ( didHit ) + { + fastBodySim->flags |= b3_hadTimeOfImpact; + continuousContext->fraction = output.fraction; + + // if (output.usedFallback == true) + //{ + // // help out the solver + // b3BodyState* state = b3GetBodyState( world, fastBody ); + // B3_ASSERT( state != NULL ); + // state->angularVelocity = b3Vec3_zero; + // } + } + } + + // Continue query + return true; +} + +// Continuous collision of dynamic versus static +static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext* taskContext ) +{ + b3TracyCZoneNC( ccd, "CCD", b3_colorDarkGoldenRod, true ); + + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + b3BodySim* fastBodySim = b3Array_Get( awakeSet->bodySims, bodySimIndex ); + B3_ASSERT( fastBodySim->flags & b3_isFast ); + + b3Sweep sweep = b3MakeSweep( fastBodySim ); + + b3Transform xf1; + xf1.q = sweep.q1; + xf1.p = b3Sub( sweep.c1, b3RotateVector( sweep.q1, sweep.localCenter ) ); + + b3Transform xf2; + xf2.q = sweep.q2; + xf2.p = b3Sub( sweep.c2, b3RotateVector( sweep.q2, sweep.localCenter ) ); + + b3DynamicTree* staticTree = world->broadPhase.trees + b3_staticBody; + b3DynamicTree* kinematicTree = world->broadPhase.trees + b3_kinematicBody; + b3DynamicTree* dynamicTree = world->broadPhase.trees + b3_dynamicBody; + b3Body* fastBody = b3Array_Get( world->bodies, fastBodySim->bodyId ); + + b3ContinuousContext context = { 0 }; + context.world = world; + context.sweep = sweep; + context.fastBodySim = fastBodySim; + context.fraction = 1.0f; + + bool isBullet = ( fastBodySim->flags & b3_isBullet ) != 0; + + int shapeId = fastBody->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* fastShape = b3Array_Get( world->shapes, shapeId ); + shapeId = fastShape->nextShapeId; + + context.fastShape = fastShape; + context.centroid1 = b3TransformPoint( xf1, fastShape->localCentroid ); + context.centroid2 = b3TransformPoint( xf2, fastShape->localCentroid ); + + b3AABB box1 = fastShape->aabb; + b3AABB box2 = b3ComputeShapeAABB( fastShape, xf2 ); + + // Store this to avoid double computation in the case there is no impact event + fastShape->aabb = box2; + + // No continuous collision for meshes + if ( fastShape->type == b3_meshShape || fastShape->type == b3_heightShape ) + { + continue; + } + + // No continuous collision for sensors + if ( fastShape->sensorIndex != B3_NULL_INDEX ) + { + continue; + } + + b3AABB sweptBox = b3AABB_Union( box1, box2 ); + b3DynamicTree_Query( staticTree, sweptBox, B3_DEFAULT_MASK_BITS, false, b3ContinuousQueryCallback, &context ); + + if ( isBullet ) + { + b3DynamicTree_Query( kinematicTree, sweptBox, B3_DEFAULT_MASK_BITS, false, b3ContinuousQueryCallback, &context ); + b3DynamicTree_Query( dynamicTree, sweptBox, B3_DEFAULT_MASK_BITS, false, b3ContinuousQueryCallback, &context ); + } + } + + const float speculativeScalar = B3_SPECULATIVE_DISTANCE; + const b3Vec3 speculativeMargin = { speculativeScalar, speculativeScalar, speculativeScalar }; + + // todo implement per shape margin as in Box2D + const float marginScalar = B3_AABB_MARGIN; + const b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar }; + + if ( context.fraction < 1.0f ) + { + // Handle time of impact event + b3Quat q = b3NLerp( sweep.q1, sweep.q2, context.fraction ); + b3Vec3 c = b3Lerp( sweep.c1, sweep.c2, context.fraction ); + b3Vec3 origin = b3Sub( c, b3RotateVector( q, sweep.localCenter ) ); + + // Advance body + b3Transform transform = { origin, q }; + fastBodySim->transform = transform; + fastBodySim->center = c; + fastBodySim->rotation0 = q; + fastBodySim->center0 = c; + + // Prepare AABBs for broad-phase. + // Even though a body is fast, it may not move much. So the AABB may not need enlargement. + + shapeId = fastBody->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + // Must recompute aabb at the interpolated transform + b3AABB aabb = b3ComputeShapeAABB( shape, transform ); + aabb.lowerBound = b3Sub( aabb.lowerBound, speculativeMargin ); + aabb.upperBound = b3Add( aabb.upperBound, speculativeMargin ); + shape->aabb = aabb; + + if ( b3AABB_Contains( shape->fatAABB, aabb ) == false ) + { + shape->fatAABB = (b3AABB){ b3Sub( aabb.lowerBound, aabbMargin ), b3Add( aabb.upperBound, aabbMargin ) }; + + shape->enlargedAABB = true; + fastBodySim->flags |= b3_enlargeBounds; + } + + shapeId = shape->nextShapeId; + } + } + else + { + // No time of impact event + + // Advance body + fastBodySim->rotation0 = fastBodySim->transform.q; + fastBodySim->center0 = fastBodySim->center; + + // Prepare AABBs for broad-phase + shapeId = fastBody->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + // shape->aabb is still valid from above + + if ( b3AABB_Contains( shape->fatAABB, shape->aabb ) == false ) + { + shape->fatAABB = (b3AABB){ + .lowerBound = b3Sub( shape->aabb.lowerBound, aabbMargin ), + .upperBound = b3Add( shape->aabb.upperBound, aabbMargin ), + }; + + if ( b3IsSaneAABB( shape->fatAABB ) == false ) + { + b3Vec3 c = fastBodySim->center; + b3Log( "body %s out of bounds at %g %g %g", fastBody->name, c.x, c.y, c.z ); + } + + shape->enlargedAABB = true; + fastBodySim->flags |= b3_enlargeBounds; + } + + shapeId = shape->nextShapeId; + } + } + + // Push sensor hits on the the task context for serial processing. + for ( int i = 0; i < context.sensorCount; ++i ) + { + // Skip any sensor hits that occurred after a solid hit + if ( context.sensorFractions[i] < context.fraction ) + { + b3Array_Push( taskContext->sensorHits, context.sensorHits[i] ); + } + } + + b3TracyCZoneEnd( ccd ); +} + +// Implements b3ParallelForCallback +static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex, void* context ) +{ + b3TracyCZoneNC( finalize_bodies, "Finalize", b3_colorMediumSeaGreen, true ); + + b3StepContext* stepContext = (b3StepContext*)context; + b3World* world = stepContext->world; + b3Body* bodies = world->bodies.data; + b3BodySim* sims = stepContext->sims; + b3BodyState* states = stepContext->states; + + B3_ASSERT( endIndex <= world->bodyMoveEvents.count ); + + bool enableSleep = world->enableSleep; + bool enableContinuous = world->enableContinuous; + float timeStep = stepContext->dt; + float invTimeStep = stepContext->inv_dt; + uint16_t worldId = world->worldId; + + // The body move event array has should already have the correct size + b3BodyMoveEvent* moveEvents = world->bodyMoveEvents.data; + + b3TaskContext* taskContext = world->taskContexts.data + workerIndex; + b3BitSet* enlargedSimBitSet = &taskContext->enlargedSimBitSet; + b3BitSet* awakeIslandBitSet = &taskContext->awakeIslandBitSet; + + const float speculativeScalar = B3_SPECULATIVE_DISTANCE; + const b3Vec3 speculativeMargin = { speculativeScalar, speculativeScalar, speculativeScalar }; + + // todo per shape margin + const float marginScalar = B3_AABB_MARGIN; + const b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar }; + + for ( int simIndex = startIndex; simIndex < endIndex; ++simIndex ) + { + b3BodyState* state = states + simIndex; + b3BodySim* sim = sims + simIndex; + + b3Vec3 v = state->linearVelocity; + b3Vec3 w = state->angularVelocity; + b3Vec3 localOmega = b3InvRotateVector( sim->transform.q, w ); + b3Vec3 localDeltaRotation = b3InvRotateVector( sim->transform.q, state->deltaRotation.v ); + + if ( b3IsValidVec3( v ) == false || b3IsValidVec3( w ) == false ) + { + b3Log( "unstable: %s", bodies[sim->bodyId].name ); + } + + B3_ASSERT( b3IsValidVec3( v ) ); + B3_ASSERT( b3IsValidVec3( w ) ); + + sim->center = b3Add( sim->center, state->deltaPosition ); + sim->transform.q = b3NormalizeQuat( b3MulQuat( state->deltaRotation, sim->transform.q ) ); + + // Use the velocity of the farthest point on the body to account for rotation. + b3Vec3 velocityArc = b3ModifiedCross( b3Abs( localOmega ), sim->maxExtent ); + float maxVelocity = b3Length( v ) + b3Length( velocityArc ); + + // Sleep needs to observe position correction as well as true velocity. + // q = [sin(theta/2) * v, cos(theta/2)] + // for small angles abs(theta) ~= 2 * length(sin(theta/2) * v) + b3Vec3 rotationArc = b3ModifiedCross( b3Abs( localDeltaRotation ), sim->maxExtent ); + float maxDeltaPosition = b3Length( state->deltaPosition ) + 2.0f * b3Length(rotationArc); + + // Position correction is not as important for sleep as true velocity. + float positionSleepFactor = 0.5f; + float sleepVelocity = b3MaxFloat( maxVelocity, positionSleepFactor * invTimeStep * maxDeltaPosition ); + + // reset state deltas + state->deltaPosition = b3Vec3_zero; + state->deltaRotation = b3Quat_identity; + + sim->transform.p = b3Sub( sim->center, b3RotateVector( sim->transform.q, sim->localCenter ) ); + + // cache miss here, however I need the shape list below + b3Body* body = bodies + sim->bodyId; + body->bodyMoveIndex = simIndex; + moveEvents[simIndex].userData = body->userData; + moveEvents[simIndex].transform = sim->transform; + moveEvents[simIndex].bodyId = (b3BodyId){ sim->bodyId + 1, worldId, body->generation }; + moveEvents[simIndex].fellAsleep = false; + + // reset applied force and torque + sim->force = b3Vec3_zero; + sim->torque = b3Vec3_zero; + + body->flags &= ~( b3_isFast | b3_isSpeedCapped | b3_hadTimeOfImpact ); + body->flags |= ( sim->flags & ( b3_isSpeedCapped | b3_hadTimeOfImpact ) ); + body->flags |= ( state->flags & ( b3_isSpeedCapped | b3_hadTimeOfImpact ) ); + sim->flags &= ~( b3_isFast | b3_isSpeedCapped | b3_hadTimeOfImpact ); + state->flags &= ~( b3_isFast | b3_isSpeedCapped | b3_hadTimeOfImpact ); + + if ( enableSleep == false || body->enableSleep == false || sleepVelocity > body->sleepThreshold ) + { + // Body is not sleepy + body->sleepTime = 0.0f; + + const float safetyFactor = 0.5f; + float maxMotion = b3MaxFloat( maxDeltaPosition, maxVelocity * timeStep ); + if ( body->type == b3_dynamicBody && enableContinuous && maxMotion > safetyFactor * sim->minExtent ) + { + // This flag is only retained for debug draw + sim->flags |= b3_isFast; + + // Store in fast array for the continuous collision stage + // This is deterministic because the order of TOI sweeps doesn't matter + if ( sim->flags & b3_isBullet ) + { + int bulletIndex = b3AtomicFetchAddInt( &stepContext->bulletBodyCount, 1 ); + stepContext->bulletBodies[bulletIndex] = simIndex; + } + else + { + b3SolveContinuous( world, simIndex, taskContext ); + } + } + else + { + // Body is safe to advance + sim->center0 = sim->center; + sim->rotation0 = sim->transform.q; + } + } + else + { + // Body is safe to advance and is falling asleep + sim->center0 = sim->center; + sim->rotation0 = sim->transform.q; + body->sleepTime += timeStep; + } + + // Update world space inverse inertia tensor. + b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( sim->transform.q ); + sim->invInertiaWorld = b3MulMM( b3MulMM( rotationMatrix, sim->invInertiaLocal ), b3Transpose( rotationMatrix ) ); + + // Any single body in an island can keep it awake + b3Island* island = b3Array_Get( world->islands, body->islandId ); + if ( body->sleepTime < B3_TIME_TO_SLEEP ) + { + // keep island awake + int islandIndex = island->localIndex; + b3SetBit( awakeIslandBitSet, islandIndex ); + } + else if ( island->constraintRemoveCount > 0 ) + { + // body wants to sleep but its island needs splitting first + if ( body->sleepTime > taskContext->splitSleepTime ) + { + // pick the sleepiest candidate + taskContext->splitIslandId = body->islandId; + taskContext->splitSleepTime = body->sleepTime; + } + } + + // Update shapes AABBs + b3Transform transform = sim->transform; + bool isFast = ( sim->flags & b3_isFast ) != 0; + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + if ( isFast ) + { + // For fast non-bullet bodies the AABB has already been updated in b3SolveContinuous + // For fast bullet bodies the AABB will be updated at a later stage + + // Add to enlarged shapes regardless of AABB changes. + // Bit-set to keep the move array sorted + b3SetBit( enlargedSimBitSet, simIndex ); + } + else + { + b3AABB aabb = b3ComputeShapeAABB( shape, transform ); + aabb.lowerBound = b3Sub( aabb.lowerBound, speculativeMargin ); + aabb.upperBound = b3Add( aabb.upperBound, speculativeMargin ); + shape->aabb = aabb; + + B3_ASSERT( shape->enlargedAABB == false ); + + if ( b3AABB_Contains( shape->fatAABB, aabb ) == false ) + { + shape->fatAABB = (b3AABB){ b3Sub( aabb.lowerBound, aabbMargin ), b3Add( aabb.upperBound, aabbMargin ) }; + shape->enlargedAABB = true; + + // Bit-set to keep the move array sorted + b3SetBit( enlargedSimBitSet, simIndex ); + } + } + + shapeId = shape->nextShapeId; + } + } + + b3TracyCZoneEnd( finalize_bodies ); +} + +typedef struct b3BlockDim +{ + // number of items per block (except last block) + int size; + + // total number of blocks + int count; +} b3BlockDim; + +// A block is a range of tasks, a start index and count as a sub-array. Each worker receives at +// most M blocks of work. The workers may receive less blocks if there is not sufficient work. +// Each block of work has a minimum number of elements (block size). This in turn may limit the +// number of blocks. If there are many elements then the block size is increased so there are +// still at most M blocks of work per worker. M is a tunable number that has two goals: +// 1. keep M small to reduce overhead +// 2. keep M large enough for other workers to be able to steal work +// The block size is a power of two to make math efficient. +static inline b3BlockDim b3ComputeBlockCount( int itemCount, int minSize, int maxBlockCount ) +{ + b3BlockDim dim = { 0 }; + if ( itemCount == 0 ) + { + return dim; + } + + if ( itemCount <= minSize * maxBlockCount ) + { + dim.size = minSize; + } + else + { + dim.size = ( itemCount + maxBlockCount - 1 ) / maxBlockCount; + } + + dim.count = ( itemCount + dim.size - 1 ) / dim.size; + + B3_ASSERT( dim.count >= 1 ); + B3_ASSERT( dim.size * dim.count >= itemCount ); + + return dim; +} + +// Initialize solver blocks for a contiguous range of items. Computes block size internally +// from the same parameters used by b3ComputeBlockCount. The atomic claim counter is zeroed +// so workers can CAS (0, 1) on the first stage that owns these blocks. +static void b3InitBlocks( b3SyncBlock* blocks, b3BlockDim dim, int itemCount, uint8_t blockType, uint8_t colorIndex ) +{ + if ( dim.count == 0 ) + { + return; + } + + B3_ASSERT( itemCount >= dim.count ); + + // Compute the number of elements per block + int blockSize = dim.size; + + // Simulation too big + B3_ASSERT( blockSize <= UINT16_MAX ); + + for ( int i = 0; i < dim.count; ++i ) + { + blocks[i].block.startIndex = i * blockSize; + blocks[i].block.count = (uint16_t)blockSize; + blocks[i].block.blockType = blockType; + blocks[i].block.colorIndex = colorIndex; + b3AtomicStoreInt( &blocks[i].syncIndex, 0 ); + } + + // The last block may not be full + blocks[dim.count - 1].block.count = (uint16_t)( itemCount - ( dim.count - 1 ) * blockSize ); + + B3_VALIDATE( blocks[dim.count - 1].block.count <= blockSize ); + B3_VALIDATE( ( dim.count - 1 ) * dim.size + blocks[dim.count - 1].block.count == itemCount ); +} + +static inline b3SolverStage* b3InitStage( b3SolverStage* stage, b3SolverStageType type, b3SyncBlock* blocks, int blockCount, + uint8_t colorIndex ) +{ + stage->type = type; + stage->blocks = blocks; + stage->blockCount = blockCount; + stage->colorIndex = colorIndex; + b3AtomicStoreInt( &stage->completionCount, 0 ); + return stage + 1; +} + +// Initialize one stage per color for each iteration. Used for warm start, solve, relax, and restitution. +// All iterations of a given color share the same b3SyncBlock array so the per-block syncIndex +// grows monotonically across stages within that color. +static b3SolverStage* b3InitColorStages( b3SolverStage* stage, b3SolverStageType type, int iterations, int activeColorCount, + b3SyncBlock** colorBlocks, int* colorBlockCounts, int* activeColorIndices ) +{ + for ( int j = 0; j < iterations; ++j ) + { + for ( int i = 0; i < activeColorCount; ++i ) + { + stage = b3InitStage( stage, type, colorBlocks[i], colorBlockCounts[i], (uint8_t)activeColorIndices[i] ); + } + } + return stage; +} + +static void b3ExecuteBlock( b3SolverStage* stage, b3StepContext* context, b3SolverBlock block, int workerIndex ) +{ + b3SolverStageType stageType = stage->type; + b3SolverBlockType blockType = (b3SolverBlockType)block.blockType; + + switch ( stageType ) + { + case b3_stagePrepareJoints: + b3PrepareJointsTask( block, context ); + break; + + case b3_stagePrepareWideContacts: + b3PrepareContacts_Convex( block, context ); + break; + + case b3_stagePrepareContacts: + b3PrepareContacts_Mesh( block, context ); + break; + + case b3_stageIntegrateVelocities: + b3IntegrateVelocitiesTask( block, context ); + break; + + case b3_stageWarmStart: + if ( blockType == b3_graphJointBlock ) + { + b3WarmStartJointsTask( block, context ); + } + else if ( blockType == b3_graphWideContactBlock ) + { + b3WarmStartContacts_Convex( block, context ); + } + else + { + b3WarmStartContacts_Mesh( block, context ); + } + break; + + case b3_stageSolve: + if ( blockType == b3_graphJointBlock ) + { + bool useBias = true; + b3SolveJointsTask( block, context, useBias, workerIndex ); + } + else if ( blockType == b3_graphWideContactBlock ) + { + bool useBias = true; + b3SolveContacts_Convex( block, context, useBias ); + } + else + { + bool useBias = true; + b3SolveContacts_Mesh( block, context, useBias ); + } + break; + + case b3_stageIntegratePositions: + b3IntegratePositionsTask( block, context ); + break; + + case b3_stageRelax: + if ( blockType == b3_graphJointBlock ) + { + bool useBias = false; + b3SolveJointsTask( block, context, useBias, workerIndex ); + } + else if ( blockType == b3_graphWideContactBlock ) + { + bool useBias = false; + b3SolveContacts_Convex( block, context, useBias ); + } + else + { + bool useBias = false; + b3SolveContacts_Mesh( block, context, useBias ); + } + break; + + case b3_stageRestitution: + if ( blockType == b3_graphWideContactBlock ) + { + b3ApplyRestitution_Convex( block, context ); + } + else if ( blockType == b3_graphContactBlock ) + { + b3ApplyRestitution_Mesh( block, context ); + } + break; + + case b3_stageStoreWideImpulses: + b3StoreImpulses_Convex( block, context, workerIndex ); + break; + + case b3_stageStoreImpulses: + b3StoreImpulses_Mesh( block, context, workerIndex ); + break; + } +} + +// This staggers the worker start indices so they avoid touching the same solver blocks +static inline int GetWorkerStartIndex( int workerIndex, int blockCount, int workerCount ) +{ + if ( blockCount <= workerCount ) + { + return workerIndex < blockCount ? workerIndex : B3_NULL_INDEX; + } + + int blocksPerWorker = blockCount / workerCount; + int remainder = blockCount - blocksPerWorker * workerCount; + return blocksPerWorker * workerIndex + b3MinInt( remainder, workerIndex ); +} + +// Execute a stage, which is an array of solver blocks, each controlled with an atomic sync index. +// Each worker starts at its home index and sweeps the ring, CAS-claiming any unclaimed blocks. +static void b3ExecuteStage( b3SolverStage* stage, b3StepContext* context, int previousSyncIndex, int syncIndex, int workerIndex ) +{ + int completedCount = 0; + b3SyncBlock* blocks = stage->blocks; + int blockCount = stage->blockCount; + + int startIndex = GetWorkerStartIndex( workerIndex, blockCount, context->workerCount ); + if ( startIndex == B3_NULL_INDEX ) + { + return; + } + + B3_ASSERT( 0 <= startIndex && startIndex < blockCount ); + + int blockIndex = startIndex; + for ( int i = 0; i < blockCount; ++i ) + { + if ( b3AtomicCompareExchangeInt( &blocks[blockIndex].syncIndex, previousSyncIndex, syncIndex ) ) + { + B3_ASSERT( completedCount < blockCount ); + + // Pass the descriptor by value -- the wrapping b3SyncBlock holds the atomic + // syncIndex but we only copy .block, so the struct copy never aliases the CAS target. + b3ExecuteBlock( stage, context, blocks[blockIndex].block, workerIndex ); + completedCount += 1; + } + + blockIndex += 1; + if ( blockIndex >= blockCount ) + { + blockIndex = 0; + } + } + + (void)b3AtomicFetchAddInt( &stage->completionCount, completedCount ); +} + +// Execute a stage on worker 0 (main thread). +static void b3ExecuteMainStage( b3SolverStage* stage, b3StepContext* context, uint32_t syncBits ) +{ + int blockCount = stage->blockCount; + if ( blockCount == 0 ) + { + return; + } + + const int workerIndex = 0; + + if ( blockCount == 1 ) + { + b3ExecuteBlock( stage, context, stage->blocks[0].block, workerIndex ); + } + else + { + b3AtomicStoreU32( &context->atomicSyncBits, syncBits ); + + int syncIndex = ( syncBits >> 16 ) & 0xFFFF; + B3_ASSERT( syncIndex > 0 ); + int previousSyncIndex = syncIndex - 1; + + b3ExecuteStage( stage, context, previousSyncIndex, syncIndex, workerIndex ); + + // Spin waiting for thieves to finish + while ( b3AtomicLoadInt( &stage->completionCount ) != blockCount ) + { + b3Pause(); + } + + b3AtomicStoreInt( &stage->completionCount, 0 ); + } +} + +// Parallel solver task +static void b3SolverTask( void* taskContext ) +{ + b3WorkerContext* workerContext = (b3WorkerContext*)taskContext; + int workerIndex = workerContext->workerIndex; + b3StepContext* context = workerContext->context; + int activeColorCount = context->activeColorCount; + b3SolverStage* stages = context->stages; + b3Profile* profile = &context->world->profile; + + if ( workerIndex == 0 ) + { + // The orchestrator slot is a race. The calling thread of b3World_Step also enters here + // as worker 0, so progress is guaranteed even if the user's task system schedules tasks + // out of order, has fewer threads than workerCount, or runs the task synchronously + // inside enqueueTaskFcn. Whoever wins the CAS becomes the orchestrator; the loser + // returns and lets the spinner-only path handle workers >0. + if ( b3AtomicCompareExchangeInt( &context->mainClaimed, 0, 1 ) == false ) + { + return; + } + + // Main thread synchronizes the workers and does work itself. + // + // This needs to be a task for the main thread because the user's task system may execute + // the tasks serially and this is the first task. This single task is able to fully + // complete all work even if all other workers are blocked. + + // Stages are re-used by loops so that I don't need more stages for large substep counts. + // The sync indices grow monotonically for the body/graph/constraint groupings because they share solver blocks. + // The stage index and sync indices are combined in to sync bits for atomic synchronization. + // The workers need to compute the previous sync index for a given stage so that CAS works correctly. This + // setup makes this easy to do. + + /* + b3_stagePrepareJoints, + b3_stagePrepareContacts, + b3_stageIntegrateVelocities, + b3_stageWarmStart, + b3_stageSolve, + b3_stageIntegratePositions, + b3_stageRelax, + b3_stageRestitution, + b3_stageStoreImpulses + */ + + uint64_t ticks = b3GetTicks(); + + int bodySyncIndex = 1; + int stageIndex = 0; + + // Prepare joint constraints + uint32_t jointSyncIndex = 1; + uint32_t syncBits = ( jointSyncIndex << 16 ) | stageIndex; + B3_ASSERT( stages[stageIndex].type == b3_stagePrepareJoints ); + b3ExecuteMainStage( stages + stageIndex, context, syncBits ); + stageIndex += 1; + jointSyncIndex += 1; + + // Prepare convex contact constraints + uint32_t convexSyncIndex = 1; + syncBits = ( convexSyncIndex << 16 ) | stageIndex; + B3_ASSERT( stages[stageIndex].type == b3_stagePrepareWideContacts ); + b3ExecuteMainStage( stages + stageIndex, context, syncBits ); + stageIndex += 1; + convexSyncIndex += 1; + + // Prepare mesh contact constraints + uint32_t meshSyncIndex = 1; + syncBits = ( meshSyncIndex << 16 ) | stageIndex; + B3_ASSERT( stages[stageIndex].type == b3_stagePrepareContacts ); + b3ExecuteMainStage( stages + stageIndex, context, syncBits ); + stageIndex += 1; + meshSyncIndex += 1; + + // Single-threaded overflow work. These constraints don't fit in the graph coloring. + b3PrepareJoints_Overflow( context ); + b3PrepareContacts_Overflow( context ); + + profile->prepareConstraints += b3GetMillisecondsAndReset( &ticks ); + + int graphSyncIndex = 1; + int subStepCount = context->subStepCount; + for ( int subStepIndex = 0; subStepIndex < subStepCount; ++subStepIndex ) + { + // stageIndex restarted each iteration + // syncBits still increases monotonically because the upper bits increase each iteration + int iterationStageIndex = stageIndex; + + // Integrate velocities + syncBits = ( bodySyncIndex << 16 ) | iterationStageIndex; + B3_ASSERT( stages[iterationStageIndex].type == b3_stageIntegrateVelocities ); + b3ExecuteMainStage( stages + iterationStageIndex, context, syncBits ); + iterationStageIndex += 1; + bodySyncIndex += 1; + + profile->integrateVelocities += b3GetMillisecondsAndReset( &ticks ); + + // Warm start constraints + b3WarmStartJoints_Overflow( context ); + b3WarmStartContacts_Overflow( context ); + + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterationStageIndex; + B3_ASSERT( stages[iterationStageIndex].type == b3_stageWarmStart ); + b3ExecuteMainStage( stages + iterationStageIndex, context, syncBits ); + iterationStageIndex += 1; + } + graphSyncIndex += 1; + + profile->warmStart += b3GetMillisecondsAndReset( &ticks ); + + // Solve constraints + bool useBias = true; + for ( int j = 0; j < ITERATIONS; ++j ) + { + // Overflow constraints have lower priority. Typically these are dynamic-vs-dynamic. + b3SolveJoints_Overflow( context, useBias ); + b3SolveContacts_Overflow( context, useBias ); + + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterationStageIndex; + B3_ASSERT( stages[iterationStageIndex].type == b3_stageSolve ); + b3ExecuteMainStage( stages + iterationStageIndex, context, syncBits ); + iterationStageIndex += 1; + } + graphSyncIndex += 1; + } + + profile->solveImpulses += b3GetMillisecondsAndReset( &ticks ); + + // Integrate positions + B3_ASSERT( stages[iterationStageIndex].type == b3_stageIntegratePositions ); + syncBits = ( bodySyncIndex << 16 ) | iterationStageIndex; + b3ExecuteMainStage( stages + iterationStageIndex, context, syncBits ); + iterationStageIndex += 1; + bodySyncIndex += 1; + + profile->integratePositions += b3GetMillisecondsAndReset( &ticks ); + + // Relax constraints + useBias = false; + for ( int j = 0; j < RELAX_ITERATIONS; ++j ) + { + b3SolveJoints_Overflow( context, useBias ); + b3SolveContacts_Overflow( context, useBias ); + + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterationStageIndex; + B3_ASSERT( stages[iterationStageIndex].type == b3_stageRelax ); + b3ExecuteMainStage( stages + iterationStageIndex, context, syncBits ); + iterationStageIndex += 1; + } + graphSyncIndex += 1; + } + + profile->relaxImpulses += b3GetMillisecondsAndReset( &ticks ); + } + + // Advance the stage according to the sub-stepping tasks just completed + // integrate velocities / warm start / solve / integrate positions / relax + stageIndex += 1 + activeColorCount + ITERATIONS * activeColorCount + 1 + RELAX_ITERATIONS * activeColorCount; + + // Restitution + { + b3ApplyRestitution_Overflow( context ); + + int iterStageIndex = stageIndex; + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterStageIndex; + B3_ASSERT( stages[iterStageIndex].type == b3_stageRestitution ); + b3ExecuteMainStage( stages + iterStageIndex, context, syncBits ); + iterStageIndex += 1; + } + // graphSyncIndex += 1; + stageIndex += activeColorCount; + } + + profile->applyRestitution += b3GetMillisecondsAndReset( &ticks ); + + // Store impulses + b3StoreImpulses_Overflow( context ); + + syncBits = ( convexSyncIndex << 16 ) | stageIndex; + B3_ASSERT( stages[stageIndex].type == b3_stageStoreWideImpulses ); + b3ExecuteMainStage( stages + stageIndex, context, syncBits ); + stageIndex += 1; + + syncBits = ( meshSyncIndex << 16 ) | stageIndex; + B3_ASSERT( stages[stageIndex].type == b3_stageStoreImpulses ); + b3ExecuteMainStage( stages + stageIndex, context, syncBits ); + stageIndex += 1; + + profile->storeImpulses += b3GetMillisecondsAndReset( &ticks ); + + // Signal workers to finish + b3AtomicStoreU32( &context->atomicSyncBits, UINT_MAX ); + + B3_ASSERT( stageIndex == context->stageCount ); + return; + } + + // Worker spins and waits for work + uint32_t lastSyncBits = 0; + // uint64_t maxSpinTime = 10; + while ( true ) + { + // Spin until main thread bumps changes the sync bits. This can waste significant time overall, but it is necessary for + // parallel simulation with graph coloring. + // todo improve this spinner + uint32_t syncBits; + int spinCount = 0; + while ( ( syncBits = b3AtomicLoadU32( &context->atomicSyncBits ) ) == lastSyncBits ) + { + if ( spinCount > 5 ) + { + b3Yield(); + spinCount = 0; + } + else + { + // Using the cycle counter helps to account for variation in mm_pause timing across different + // CPUs. However, this is X64 only. + // uint64_t prev = __rdtsc(); + // do + //{ + // b3Pause(); + //} + // while ((__rdtsc() - prev) < maxSpinTime); + // maxSpinTime += 10; + b3Pause(); + b3Pause(); + spinCount += 1; + } + } + + if ( syncBits == UINT_MAX ) + { + // sentinel hit + break; + } + + int stageIndex = syncBits & 0xFFFF; + B3_ASSERT( stageIndex < context->stageCount ); + + int syncIndex = ( syncBits >> 16 ) & 0xFFFF; + B3_ASSERT( syncIndex > 0 ); + + int previousSyncIndex = syncIndex - 1; + + b3SolverStage* stage = stages + stageIndex; + b3ExecuteStage( stage, context, previousSyncIndex, syncIndex, workerIndex ); + + lastSyncBits = syncBits; + } +} + +static void b3BulletBodyTask( int startIndex, int endIndex, int workerIndex, void* context ) +{ + b3TracyCZoneNC( bullet_body_task, "Bullet Body Task", b3_colorLightSkyBlue, true ); + + b3StepContext* stepContext = (b3StepContext*)context; + b3TaskContext* taskContext = b3Array_Get( stepContext->world->taskContexts, workerIndex ); + + B3_ASSERT( startIndex <= endIndex ); + + for ( int i = startIndex; i < endIndex; ++i ) + { + int simIndex = stepContext->bulletBodies[i]; + b3SolveContinuous( stepContext->world, simIndex, taskContext ); + } + + b3TracyCZoneEnd( bullet_body_task ); +} + +#if B3_SIMD_WIDTH == 4 +#define B3_SIMD_SHIFT 2 +#else +#define B3_SIMD_SHIFT 0 +#endif + +// Solve with graph coloring +void b3Solve( b3World* world, b3StepContext* stepContext ) +{ + // Only count steps that advance the simulation + world->stepIndex += 1; + + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + int awakeBodyCount = awakeSet->bodySims.count; + if ( awakeBodyCount == 0 ) + { + b3ValidateNoEnlarged( &world->broadPhase ); + return; + } + + // Solve constraints using graph coloring + { + b3TracyCZoneNC( solver_setup, "Solver Setup", b3_colorDarkOrange, true ); + uint64_t setupTicks = b3GetTicks(); + + // Prepare buffers for continuous collision (fast bodies) + b3AtomicStoreInt( &stepContext->bulletBodyCount, 0 ); + stepContext->bulletBodies = (int*)b3StackAlloc( &world->stack, awakeBodyCount * sizeof( int ), "bullet bodies" ); + + b3ConstraintGraph* graph = &world->constraintGraph; + b3GraphColor* colors = graph->colors; + + stepContext->sims = awakeSet->bodySims.data; + stepContext->states = awakeSet->bodyStates.data; + + // count contacts, joints, and colors + int activeColorCount = 0; + for ( int i = 0; i < B3_GRAPH_COLOR_COUNT - 1; ++i ) + { + int perColorContactCount = colors[i].convexContacts.count + colors[i].contacts.count; + int perColorJointCount = colors[i].jointSims.count; + int occupancyCount = perColorContactCount + perColorJointCount; + activeColorCount += occupancyCount > 0 ? 1 : 0; + } + + // prepare for move events + b3Array_Resize( world->bodyMoveEvents, awakeBodyCount ); + + int workerCount = world->workerCount; + + // Target 4 blocks per worker to allow work stealing + const int maxBlockCount = 4 * workerCount; + + // Body blocks are for parallel iteration over bodies directly (integration, update transforms) + int minBodiesPerBlock = 32; + b3BlockDim bodyDim = b3ComputeBlockCount( awakeBodyCount, minBodiesPerBlock, maxBlockCount ); + + const int minContactsPerBlock = 4; + const int minJointsPerBlock = 4; + + // Configure blocks for tasks parallel-for each active graph color + // The blocks are a mix of convex contact, mesh contact, and joint blocks + int activeColorIndices[B3_GRAPH_COLOR_COUNT]; + int colorWideContactCounts[B3_GRAPH_COLOR_COUNT]; + int colorContactCounts[B3_GRAPH_COLOR_COUNT]; + // int colorManifoldCounts[B3_GRAPH_COLOR_COUNT]; + int colorJointCounts[B3_GRAPH_COLOR_COUNT]; + b3BlockDim graphWideContactDims[B3_GRAPH_COLOR_COUNT]; + b3BlockDim graphContactDims[B3_GRAPH_COLOR_COUNT]; + b3BlockDim graphJointDims[B3_GRAPH_COLOR_COUNT]; + int graphBlockCount = 0; + + // c is the active color index + int wideContactCount = 0; + int contactCount = 0; + int manifoldCount = 0; + int jointCount = 0; + int c = 0; + for ( int i = 0; i < B3_GRAPH_COLOR_COUNT - 1; ++i ) + { + b3GraphColor* color = colors + i; + int colorConvexContactCount = color->convexContacts.count; + int colorContactCount = color->contacts.count; + int colorJointCount = color->jointSims.count; + + if ( colorConvexContactCount + colorContactCount + colorJointCount == 0 ) + { + continue; + } + + activeColorIndices[c] = i; + + // Ceiling for wide constraint count + int colorWideConstraintCount = + colorConvexContactCount > 0 ? ( ( colorConvexContactCount - 1 ) >> B3_SIMD_SHIFT ) + 1 : 0; + wideContactCount += colorWideConstraintCount; + colorWideContactCounts[c] = colorWideConstraintCount; + + colorContactCounts[c] = colorContactCount; + contactCount += colorContactCount; + + // Compute manifold starts and accumulate manifold count + for ( int j = 0; j < colorContactCount; ++j ) + { + color->contacts.data[j].manifoldStart = manifoldCount; + manifoldCount += color->contacts.data[j].manifoldCount; + } + + colorJointCounts[c] = colorJointCount; + jointCount += colorJointCount; + + // Solver block dimensions + graphWideContactDims[c] = b3ComputeBlockCount( colorWideConstraintCount, minContactsPerBlock, maxBlockCount ); + graphContactDims[c] = b3ComputeBlockCount( colorContactCount, minContactsPerBlock, maxBlockCount ); + graphJointDims[c] = b3ComputeBlockCount( colorJointCount, minJointsPerBlock, maxBlockCount ); + graphBlockCount += graphWideContactDims[c].count + graphContactDims[c].count + graphJointDims[c].count; + + c += 1; + } + activeColorCount = c; + + // Prepare and store run as one flat parallel-for over the entire wide constraint range, + // partitioned into uniformly sized blocks. Color info is consulted inside the task via + // a small span array, so blocks do not need to honor color boundaries here. + b3BlockDim convexPrepareDim = b3ComputeBlockCount( wideContactCount, minContactsPerBlock, maxBlockCount ); + b3BlockDim meshPrepareDim = b3ComputeBlockCount( contactCount, minContactsPerBlock, maxBlockCount ); + b3BlockDim jointPrepareDim = b3ComputeBlockCount( jointCount, minJointsPerBlock, maxBlockCount ); + + int wideContactByteCount = b3GetWideContactConstraintByteCount(); + b3ContactConstraintWide* wideConstraints = + (b3ContactConstraintWide*)b3StackAlloc( &world->stack, wideContactCount * wideContactByteCount, "wide contacts" ); + b3ContactConstraint* contactConstraints = + (b3ContactConstraint*)b3StackAlloc( &world->stack, contactCount * sizeof( b3ContactConstraint ), "contacts" ); + b3ManifoldConstraint* manifoldConstraints = (b3ManifoldConstraint*)b3StackAlloc( + &world->stack, manifoldCount * sizeof( b3ManifoldConstraint ), "manifold constraints" ); + + b3GraphColor* overflow = colors + B3_OVERFLOW_INDEX; + int overflowCount = overflow->contacts.count; + int overflowManifoldCount = 0; + for ( int i = 0; i < overflowCount; ++i ) + { + overflow->contacts.data[i].manifoldStart = overflowManifoldCount; + overflowManifoldCount += overflow->contacts.data[i].manifoldCount; + } + + overflow->contactConstraints = (b3ContactConstraint*)b3StackAlloc( + &world->stack, overflowCount * sizeof( b3ContactConstraint ), "overflow contacts" ); + overflow->manifoldConstraints = (b3ManifoldConstraint*)b3StackAlloc( + &world->stack, overflowManifoldCount * sizeof( b3ManifoldConstraint ), "overflow manifolds" ); + + // Build the span table for the flat prepare/store parallel-for while I slice the + // wide constraint buffer across colors. One entry per active color plus a sentinel + // at wideContactCount. + b3WidePrepareSpan widePrepareSpans[B3_GRAPH_COLOR_COUNT + 1]; + b3ContactPrepareSpan contactPrepareSpans[B3_GRAPH_COLOR_COUNT + 1]; + b3JointPrepareSpan jointPrepareSpans[B3_GRAPH_COLOR_COUNT + 1]; + + // Distribute transient constraints to each graph color and prepare spans + // todo it might be simpler for solver blocks to index into the global arrays + { + int wideBase = 0; + int contactBase = 0; + int jointBase = 0; + for ( int i = 0; i < activeColorCount; ++i ) + { + int j = activeColorIndices[i]; + b3GraphColor* color = colors + j; + + int colorConvexContactCount = color->convexContacts.count; + widePrepareSpans[i].start = wideBase; + widePrepareSpans[i].count = colorConvexContactCount; + widePrepareSpans[i].contacts = color->convexContacts.data; + + if ( colorConvexContactCount == 0 ) + { + color->wideConstraints = NULL; + color->wideConstraintCount = 0; + } + else + { + color->wideConstraints = + (b3ContactConstraintWide*)( (uint8_t*)wideConstraints + wideBase * wideContactByteCount ); + + int colorContactCountW = ( ( colorConvexContactCount - 1 ) >> B3_SIMD_SHIFT ) + 1; + color->wideConstraintCount = colorContactCountW; + + // Zero remainder lanes in the tail wide slot so prepare workers don't need to + // initialize them. + if ( ( colorConvexContactCount & ( B3_SIMD_WIDTH - 1 ) ) != 0 ) + { + memset( (uint8_t*)color->wideConstraints + ( colorContactCountW - 1 ) * wideContactByteCount, 0, + wideContactByteCount ); + } + + wideBase += colorContactCountW; + } + + int colorContactCount = color->contacts.count; + contactPrepareSpans[i].start = contactBase; + contactPrepareSpans[i].count = colorContactCount; + contactPrepareSpans[i].contacts = color->contacts.data; + + if ( colorContactCount == 0 ) + { + color->contactConstraints = NULL; + color->contactConstraintCount = 0; + } + else + { + color->contactConstraints = contactConstraints + contactBase; + color->contactConstraintCount = colorContactCount; + contactBase += colorContactCount; + } + + jointPrepareSpans[i].start = jointBase; + jointPrepareSpans[i].count = color->jointSims.count; + jointPrepareSpans[i].joints = color->jointSims.data; + jointBase += color->jointSims.count; + } + + // Sentinels + widePrepareSpans[activeColorCount].start = wideContactCount; + widePrepareSpans[activeColorCount].count = 0; + widePrepareSpans[activeColorCount].contacts = NULL; + B3_ASSERT( wideBase == wideContactCount ); + + contactPrepareSpans[activeColorCount].start = contactCount; + contactPrepareSpans[activeColorCount].count = 0; + contactPrepareSpans[activeColorCount].contacts = NULL; + B3_ASSERT( contactBase == contactCount ); + + jointPrepareSpans[activeColorCount].start = jointCount; + jointPrepareSpans[activeColorCount].count = 0; + jointPrepareSpans[activeColorCount].joints = NULL; + B3_ASSERT( jointBase == jointCount ); + } + + //// Special span for overflow to allow for function re-use + b3ContactPrepareSpan overflowSpans[2] = { 0 }; + overflowSpans[0].start = 0; + overflowSpans[0].count = overflow->contacts.count; + overflowSpans[0].contacts = overflow->contacts.data; + overflowSpans[1].start = overflow->contacts.count; + overflowSpans[1].count = 0; + overflowSpans[1].contacts = NULL; + + int stageCount = 0; + + // b3_stagePrepareJoints + stageCount += 1; + // b3_stagePrepareWideContacts + stageCount += 1; + // b3_stagePrepareContacts + stageCount += 1; + // b3_stageIntegrateVelocities + stageCount += 1; + // b3_stageWarmStart + stageCount += activeColorCount; + // b3_stageSolve + stageCount += ITERATIONS * activeColorCount; + // b3_stageIntegratePositions + stageCount += 1; + // b3_stageRelax + stageCount += RELAX_ITERATIONS * activeColorCount; + // b3_stageRestitution + stageCount += activeColorCount; + // b3_stageStoreWideImpulses + stageCount += 1; + // b3_stageStoreImpulses + stageCount += 1; + + b3SolverStage* stages = (b3SolverStage*)b3StackAlloc( &world->stack, stageCount * sizeof( b3SolverStage ), "stages" ); + b3SyncBlock* bodyBlocks = + (b3SyncBlock*)b3StackAlloc( &world->stack, bodyDim.count * sizeof( b3SyncBlock ), "body blocks" ); + b3SyncBlock* convexBlocks = + (b3SyncBlock*)b3StackAlloc( &world->stack, convexPrepareDim.count * sizeof( b3SyncBlock ), "convex blocks" ); + b3SyncBlock* meshBlocks = + (b3SyncBlock*)b3StackAlloc( &world->stack, meshPrepareDim.count * sizeof( b3SyncBlock ), "mesh blocks" ); + b3SyncBlock* jointBlocks = + (b3SyncBlock*)b3StackAlloc( &world->stack, jointPrepareDim.count * sizeof( b3SyncBlock ), "joint blocks" ); + b3SyncBlock* graphBlocks = + (b3SyncBlock*)b3StackAlloc( &world->stack, graphBlockCount * sizeof( b3SyncBlock ), "graph blocks" ); + + // Split an awake island. This modifies: + // - stack allocator + // - world island array and solver set + // - island indices on bodies, contacts, and joints + // I'm squeezing this task in here because it may be expensive and this is a safe place to put it. + // Note: cannot split islands in parallel with FinalizeBodies + void* splitIslandTask = NULL; + if ( world->splitIslandId != B3_NULL_INDEX ) + { + if ( world->taskCount < B3_MAX_TASKS ) + { + splitIslandTask = world->enqueueTaskFcn( &b3SplitIslandTask, world, world->userTaskContext, "split" ); + world->taskCount += 1; + world->activeTaskCount += splitIslandTask == NULL ? 0 : 1; + } + else + { + b3SplitIslandTask( world ); + } + } + + // Prepare body blocks + b3InitBlocks( bodyBlocks, bodyDim, awakeBodyCount, b3_bodyBlock, UINT8_MAX ); + + // Prepare blocks as a single flat parallel-for over the whole constraint range. + // The task walks spans to decode flat slot indices back to per-color arrays. + b3InitBlocks( convexBlocks, convexPrepareDim, wideContactCount, b3_wideContactBlock, UINT8_MAX ); + b3InitBlocks( meshBlocks, meshPrepareDim, contactCount, b3_contactBlock, UINT8_MAX ); + b3InitBlocks( jointBlocks, jointPrepareDim, jointCount, b3_jointBlock, UINT8_MAX ); + + // Prepare graph work blocks. Each color gets joint blocks followed by contact blocks. + b3SyncBlock* graphColorBlocks[B3_GRAPH_COLOR_COUNT] = { 0 }; + b3SyncBlock* baseGraphBlock = graphBlocks; + int graphBlockCounts[B3_GRAPH_COLOR_COUNT] = { 0 }; + for ( int i = 0; i < activeColorCount; ++i ) + { + graphColorBlocks[i] = baseGraphBlock; + + uint8_t colorIndex = (uint8_t)activeColorIndices[i]; + b3InitBlocks( baseGraphBlock, graphJointDims[i], colorJointCounts[i], b3_graphJointBlock, colorIndex ); + baseGraphBlock += graphJointDims[i].count; + + b3InitBlocks( baseGraphBlock, graphWideContactDims[i], colorWideContactCounts[i], b3_graphWideContactBlock, + colorIndex ); + baseGraphBlock += graphWideContactDims[i].count; + + b3InitBlocks( baseGraphBlock, graphContactDims[i], colorContactCounts[i], b3_graphContactBlock, colorIndex ); + baseGraphBlock += graphContactDims[i].count; + + graphBlockCounts[i] = graphJointDims[i].count + graphWideContactDims[i].count + graphContactDims[i].count; + } + + B3_ASSERT( (ptrdiff_t)( baseGraphBlock - graphBlocks ) == graphBlockCount ); + + b3SolverStage* stage = stages; + stage = b3InitStage( stage, b3_stagePrepareJoints, jointBlocks, jointPrepareDim.count, UINT8_MAX ); + stage = b3InitStage( stage, b3_stagePrepareWideContacts, convexBlocks, convexPrepareDim.count, UINT8_MAX ); + stage = b3InitStage( stage, b3_stagePrepareContacts, meshBlocks, meshPrepareDim.count, UINT8_MAX ); + stage = b3InitStage( stage, b3_stageIntegrateVelocities, bodyBlocks, bodyDim.count, UINT8_MAX ); + stage = b3InitColorStages( stage, b3_stageWarmStart, 1, activeColorCount, graphColorBlocks, graphBlockCounts, + activeColorIndices ); + stage = b3InitColorStages( stage, b3_stageSolve, ITERATIONS, activeColorCount, graphColorBlocks, graphBlockCounts, + activeColorIndices ); + stage = b3InitStage( stage, b3_stageIntegratePositions, bodyBlocks, bodyDim.count, UINT8_MAX ); + stage = b3InitColorStages( stage, b3_stageRelax, RELAX_ITERATIONS, activeColorCount, graphColorBlocks, graphBlockCounts, + activeColorIndices ); + // Note: joint blocks mixed in, could have joint limit restitution + stage = b3InitColorStages( stage, b3_stageRestitution, 1, activeColorCount, graphColorBlocks, graphBlockCounts, + activeColorIndices ); + stage = b3InitStage( stage, b3_stageStoreWideImpulses, convexBlocks, convexPrepareDim.count, UINT8_MAX ); + stage = b3InitStage( stage, b3_stageStoreImpulses, meshBlocks, meshPrepareDim.count, UINT8_MAX ); + + B3_ASSERT( (int)( stage - stages ) == stageCount ); + + B3_ASSERT( workerCount <= B3_MAX_WORKERS ); + b3WorkerContext workerContext[B3_MAX_WORKERS]; + + stepContext->graph = graph; + stepContext->activeColorCount = activeColorCount; + stepContext->workerCount = workerCount; + stepContext->stageCount = stageCount; + stepContext->stages = stages; + stepContext->wideConstraints = wideConstraints; + stepContext->widePrepareSpans = widePrepareSpans; + stepContext->wideContactCount = wideContactCount; + stepContext->manifoldConstraints = manifoldConstraints; + stepContext->contactConstraints = contactConstraints; + stepContext->contactPrepareSpans = contactPrepareSpans; + stepContext->overflowSpans = overflowSpans; + stepContext->jointPrepareSpans = jointPrepareSpans; + b3AtomicStoreU32( &stepContext->atomicSyncBits, 0 ); + b3AtomicStoreInt( &stepContext->mainClaimed, 0 ); + + world->profile.solverSetup = b3GetMillisecondsAndReset( &setupTicks ); + b3TracyCZoneEnd( solver_setup ); + + b3TracyCZoneNC( solve_constraints, "Solve Constraints", b3_colorIndigo, true ); + uint64_t constraintTicks = b3GetTicks(); + + int jointIdCapacity = b3GetIdCapacity( &world->jointIdPool ); + int contactIdCapacity = b3GetIdCapacity( &world->contactIdPool ); + for ( int i = 0; i < workerCount; ++i ) + { + b3TaskContext* taskContext = b3Array_Get( world->taskContexts, i ); + b3SetBitCountAndClear( &taskContext->jointStateBitSet, jointIdCapacity ); + b3SetBitCountAndClear( &taskContext->hitEventBitSet, contactIdCapacity ); + taskContext->hasHitEvents = false; + + workerContext[i].context = stepContext; + workerContext[i].workerIndex = i; + + if ( world->taskCount < B3_MAX_TASKS ) + { + char buffer[16]; + snprintf( buffer, sizeof(buffer), "solve[%d]", i ); + workerContext[i].userTask = world->enqueueTaskFcn( &b3SolverTask, workerContext + i, world->userTaskContext, buffer ); + world->taskCount += 1; + world->activeTaskCount += workerContext[i].userTask == NULL ? 0 : 1; + } + else + { + workerContext[i].userTask = NULL; + b3SolverTask( workerContext + i ); + } + } + + // The calling thread of b2World_Step also enters b2SolverTask as worker 0 and races for the + // orchestrator slot via the CAS inside. This guarantees progress even when the user's task + // system can't run the queued worker 0 promptly: it might schedule out of order, have fewer + // threads than workerCount, or invert priority by parking the calling thread in finishTaskFcn. + // Whoever wins the CAS becomes the orchestrator; the loser returns and lets the spinner-only + // path handle workers >0. + b3WorkerContext callerContext = { stepContext, 0, NULL }; + b3SolverTask( &callerContext ); + + // Finish constraint solve + for ( int i = 0; i < workerCount; ++i ) + { + if ( workerContext[i].userTask != NULL ) + { + world->finishTaskFcn( workerContext[i].userTask, world->userTaskContext ); + world->activeTaskCount -= 1; + } + } + + // Finish island split + if ( splitIslandTask != NULL ) + { + world->finishTaskFcn( splitIslandTask, world->userTaskContext ); + world->activeTaskCount -= 1; + } + world->splitIslandId = B3_NULL_INDEX; + + world->profile.constraints = b3GetMillisecondsAndReset( &constraintTicks ); + b3TracyCZoneEnd( solve_constraints ); + + b3TracyCZoneNC( update_transforms, "Update Transforms", b3_colorMediumSeaGreen, true ); + uint64_t transformTicks = b3GetTicks(); + + // Prepare contact, enlarged body, and island bit sets used in body finalization. + int awakeIslandCount = awakeSet->islandSims.count; + for ( int i = 0; i < world->workerCount; ++i ) + { + b3TaskContext* taskContext = world->taskContexts.data + i; + b3Array_Clear( taskContext->sensorHits ); + b3SetBitCountAndClear( &taskContext->enlargedSimBitSet, awakeBodyCount ); + b3SetBitCountAndClear( &taskContext->awakeIslandBitSet, awakeIslandCount ); + taskContext->splitIslandId = B3_NULL_INDEX; + taskContext->splitSleepTime = 0.0f; + } + + // Finalize bodies. Must happen after the constraint solver and after island splitting. + b3ParallelFor( world, &b3FinalizeBodiesTask, awakeBodyCount, 64, stepContext, "ccd" ); + + // Free in reverse order + b3StackFree( &world->stack, graphBlocks ); + b3StackFree( &world->stack, jointBlocks ); + b3StackFree( &world->stack, meshBlocks ); + b3StackFree( &world->stack, convexBlocks ); + b3StackFree( &world->stack, bodyBlocks ); + b3StackFree( &world->stack, stages ); + b3StackFree( &world->stack, overflow->manifoldConstraints ); + b3StackFree( &world->stack, overflow->contactConstraints ); + b3StackFree( &world->stack, manifoldConstraints ); + b3StackFree( &world->stack, contactConstraints ); + b3StackFree( &world->stack, wideConstraints ); + + world->profile.transforms = b3GetMilliseconds( transformTicks ); + b3TracyCZoneEnd( update_transforms ); + } + + // Report joint events + { + b3TracyCZoneNC( joint_events, "Joint Events", b3_colorPeru, true ); + uint64_t jointEventTicks = b3GetTicks(); + + // Gather bits for all joints that have force/torque events + b3BitSet* jointStateBitSet = &world->taskContexts.data[0].jointStateBitSet; + for ( int i = 1; i < world->workerCount; ++i ) + { + b3InPlaceUnion( jointStateBitSet, &world->taskContexts.data[i].jointStateBitSet ); + } + + { + uint32_t wordCount = jointStateBitSet->blockCount; + uint64_t* bits = jointStateBitSet->bits; + + b3Joint* jointArray = world->joints.data; + uint16_t worldIndex0 = world->worldId; + + for ( uint32_t k = 0; k < wordCount; ++k ) + { + uint64_t word = bits[k]; + while ( word != 0 ) + { + uint32_t ctz = b3CTZ64( word ); + int jointId = (int)( 64 * k + ctz ); + + B3_ASSERT( jointId < world->joints.capacity ); + + b3Joint* joint = jointArray + jointId; + + B3_ASSERT( joint->setIndex == b3_awakeSet ); + + b3JointEvent event = { + .jointId = + { + .index1 = jointId + 1, + .world0 = worldIndex0, + .generation = joint->generation, + }, + .userData = joint->userData, + }; + + b3Array_Push( world->jointEvents, event ); + + // Clear the smallest set bit + word = word & ( word - 1 ); + } + } + } + + world->profile.jointEvents = b3GetMilliseconds( jointEventTicks ); + b3TracyCZoneEnd( joint_events ); + } + + // Report hit events + { + b3TracyCZoneNC( hit_events, "Hit Events", b3_colorRosyBrown, true ); + uint64_t hitTicks = b3GetTicks(); + + B3_ASSERT( world->contactHitEvents.count == 0 ); + + // Fast path: if no worker flagged any hit-event candidates during b2StoreImpulsesTask, skip entirely. + bool anyHitEvents = false; + for ( int i = 0; i < world->workerCount; ++i ) + { + if ( world->taskContexts.data[i].hasHitEvents ) + { + anyHitEvents = true; + break; + } + } + + if ( anyHitEvents ) + { + // Union per-worker bits into worker 0's bit set. + b3BitSet* hitEventBitSet = &world->taskContexts.data[0].hitEventBitSet; + for ( int i = 1; i < world->workerCount; ++i ) + { + if ( world->taskContexts.data[i].hasHitEvents ) + { + b3InPlaceUnion( hitEventBitSet, &world->taskContexts.data[i].hitEventBitSet ); + } + } + + float threshold = world->hitEventThreshold; + b3Contact* contactArray = world->contacts.data; + uint16_t worldId = world->worldId; + + uint32_t wordCount = hitEventBitSet->blockCount; + uint64_t* bits = hitEventBitSet->bits; + for ( uint32_t k = 0; k < wordCount; ++k ) + { + uint64_t word = bits[k]; + while ( word != 0 ) + { + uint32_t ctz = b3CTZ64( word ); + int contactId = (int)( 64 * k + ctz ); + + b3Contact* contact = contactArray + contactId; + B3_ASSERT( contact->setIndex == b3_awakeSet && contact->colorIndex != B3_NULL_INDEX ); + + b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); + b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB ); + b3Body* bodyA = b3Array_Get( world->bodies, shapeA->bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, shapeB->bodyId ); + b3BodySim* simA = b3GetBodySim( world, bodyA ); + b3BodySim* simB = b3GetBodySim( world, bodyB ); + b3Vec3 midCenter = b3Lerp( simA->center, simB->center, 0.5f ); + + b3ContactHitEvent event = { 0 }; + event.approachSpeed = threshold; + + bool found = false; + int triangleIndex = 0; + int manifoldCount = contact->manifoldCount; + for ( int i = 0; i < manifoldCount; ++i ) + { + b3Manifold* manifold = contact->manifolds + i; + int pointCount = manifold->pointCount; + for ( int p = 0; p < pointCount; ++p ) + { + b3ManifoldPoint* mp = manifold->points + p; + float approachSpeed = -mp->normalVelocity; + + // Need to check total impulse because the point may be speculative and not colliding + if ( approachSpeed > event.approachSpeed && mp->totalNormalImpulse > 0.0f ) + { + event.approachSpeed = approachSpeed; + event.point = b3Add( midCenter, b3Lerp( mp->anchorA, mp->anchorB, 0.5f ) ); + event.normal = manifold->normal; + triangleIndex = mp->triangleIndex; + found = true; + } + } + } + + if ( found == true ) + { + event.shapeIdA = (b3ShapeId){ shapeA->id + 1, worldId, shapeA->generation }; + event.shapeIdB = (b3ShapeId){ shapeB->id + 1, worldId, shapeB->generation }; + + event.contactId = (b3ContactId){ + .index1 = contact->contactId + 1, + .world0 = worldId, + .padding = 0, + .generation = contact->generation, + }; + + // shapeB is never a compound today (asserted in b3CreateContact), so the + // childIndex argument is irrelevant for it. shapeA carries the compound. + event.userMaterialIdA = b3GetShapeUserMaterialId( shapeA, contact->childIndex, triangleIndex ); + event.userMaterialIdB = b3GetShapeUserMaterialId( shapeB, 0, triangleIndex ); + + b3Array_Push( world->contactHitEvents, event ); + } + + // Clear the smallest set bit + word = word & ( word - 1 ); + } + } + } + + world->profile.hitEvents = b3GetMilliseconds( hitTicks ); + b3TracyCZoneEnd( hit_events ); + } + + { + b3TracyCZoneNC( refit_bvh, "Refit BVH", b3_colorFireBrick, true ); + uint64_t refitTicks = b3GetTicks(); + + // Finish the user tree task that was queued earlier in the time step. This must be complete before touching the + // broad-phase. + if ( world->userTreeTask != NULL ) + { + world->finishTaskFcn( world->userTreeTask, world->userTaskContext ); + world->userTreeTask = NULL; + world->activeTaskCount -= 1; + } + + b3ValidateNoEnlarged( &world->broadPhase ); + + // Gather bits for all sim bodies that have enlarged AABBs + b3BitSet* enlargedBodyBitSet = &world->taskContexts.data[0].enlargedSimBitSet; + for ( int i = 1; i < world->workerCount; ++i ) + { + b3InPlaceUnion( enlargedBodyBitSet, &world->taskContexts.data[i].enlargedSimBitSet ); + } + + // Enlarge broad-phase proxies and build move array + // Apply shape AABB changes to broad-phase. This also create the move array which must be + // in deterministic order. I'm tracking sim bodies because the number of shape ids can be huge. + // This has to happen before bullets are processed. + { + b3BroadPhase* broadPhase = &world->broadPhase; + uint32_t wordCount = enlargedBodyBitSet->blockCount; + uint64_t* bits = enlargedBodyBitSet->bits; + + // Fast array access is important here + b3Body* bodyArray = world->bodies.data; + b3BodySim* bodySimArray = awakeSet->bodySims.data; + b3Shape* shapeArray = world->shapes.data; + + for ( uint32_t k = 0; k < wordCount; ++k ) + { + uint64_t word = bits[k]; + while ( word != 0 ) + { + uint32_t ctz = b3CTZ64( word ); + uint32_t bodySimIndex = 64 * k + ctz; + + b3BodySim* bodySim = bodySimArray + bodySimIndex; + + b3Body* body = bodyArray + bodySim->bodyId; + + int shapeId = body->headShapeId; + if ( ( bodySim->flags & ( b3_isBullet | b3_isFast ) ) == ( b3_isBullet | b3_isFast ) ) + { + // Fast bullet bodies don't have their final AABB yet + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = shapeArray + shapeId; + + // Shape is fast. It's aabb will be enlarged in continuous collision. + // Update the move array here for determinism because bullets are processed + // below in non-deterministic order. + b3BufferMove( broadPhase, shape->proxyKey ); + + shapeId = shape->nextShapeId; + } + } + else + { + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = shapeArray + shapeId; + + // The AABB may not have been enlarged, despite the body being flagged as enlarged. + // For example, a body with multiple shapes may have not have all shapes enlarged. + // A fast body may have been flagged as enlarged despite having no shapes enlarged. + if ( shape->enlargedAABB ) + { + B3_VALIDATE( b3IsSaneAABB( shape->fatAABB ) ); + + b3BroadPhase_EnlargeProxy( broadPhase, shape->proxyKey, shape->fatAABB ); + shape->enlargedAABB = false; + } + + shapeId = shape->nextShapeId; + } + } + + // Clear the smallest set bit + word = word & ( word - 1 ); + } + } + } + + b3ValidateBroadphase( &world->broadPhase ); + + world->profile.refit = b3GetMilliseconds( refitTicks ); + b3TracyCZoneEnd( refit_bvh ); + } + + int bulletBodyCount = b3AtomicLoadInt( &stepContext->bulletBodyCount ); + if ( bulletBodyCount > 0 ) + { + b3TracyCZoneNC( bullets, "Bullets", b3_colorDarkGoldenRod, true ); + uint64_t bulletTicks = b3GetTicks(); + + // Fast bullet bodies + // Note: a bullet body may be moving slow + int minRange = 8; + b3ParallelFor( world, &b3BulletBodyTask, bulletBodyCount, minRange, stepContext, "bullets" ); + + // Serially enlarge broad-phase proxies for bullet shapes + b3BroadPhase* broadPhase = &world->broadPhase; + b3DynamicTree* dynamicTree = broadPhase->trees + b3_dynamicBody; + + // Fast array access is important here + b3Body* bodyArray = world->bodies.data; + b3BodySim* bodySimArray = awakeSet->bodySims.data; + b3Shape* shapeArray = world->shapes.data; + + // Serially enlarge broad-phase proxies for bullet shapes + int* bulletBodySimIndices = stepContext->bulletBodies; + + // This loop has non-deterministic order but it shouldn't affect the result + for ( int i = 0; i < bulletBodyCount; ++i ) + { + b3BodySim* bulletBodySim = bodySimArray + bulletBodySimIndices[i]; + if ( ( bulletBodySim->flags & b3_enlargeBounds ) == 0 ) + { + continue; + } + + // Clear flag + bulletBodySim->flags &= ~b3_enlargeBounds; + + int bodyId = bulletBodySim->bodyId; + B3_ASSERT( 0 <= bodyId && bodyId < world->bodies.count ); + b3Body* bulletBody = bodyArray + bodyId; + + int shapeId = bulletBody->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = shapeArray + shapeId; + if ( shape->enlargedAABB == false ) + { + shapeId = shape->nextShapeId; + continue; + } + + // clear flag + shape->enlargedAABB = false; + + int proxyKey = shape->proxyKey; + int proxyId = B3_PROXY_ID( proxyKey ); + B3_ASSERT( B3_PROXY_TYPE( proxyKey ) == b3_dynamicBody ); + + // all fast bullet shapes should already be in the move buffer + B3_ASSERT( b3ContainsKey( &broadPhase->moveSet, proxyKey + 1 ) ); + + b3DynamicTree_EnlargeProxy( dynamicTree, proxyId, shape->fatAABB ); + + shapeId = shape->nextShapeId; + } + } + + world->profile.bullets = b3GetMilliseconds( bulletTicks ); + b3TracyCZoneEnd( bullets ); + } + + b3StackFree( &world->stack, stepContext->bulletBodies ); + stepContext->bulletBodies = NULL; + b3AtomicStoreInt( &stepContext->bulletBodyCount, 0 ); + + // Report sensor hits. This may include bullets sensor hits. + { + b3TracyCZoneNC( sensor_hits, "Sensor Hits", b3_colorPowderBlue, true ); + uint64_t sensorHitTicks = b3GetTicks(); + + int workerCount = world->workerCount; + B3_ASSERT( workerCount == world->taskContexts.count ); + + for ( int i = 0; i < workerCount; ++i ) + { + b3TaskContext* taskContext = world->taskContexts.data + i; + int hitCount = taskContext->sensorHits.count; + b3SensorHit* hits = taskContext->sensorHits.data; + + for ( int j = 0; j < hitCount; ++j ) + { + b3SensorHit hit = hits[j]; + b3Shape* sensorShape = b3Array_Get( world->shapes, hit.sensorId ); + b3Shape* visitor = b3Array_Get( world->shapes, hit.visitorId ); + + b3Sensor* sensor = b3Array_Get( world->sensors, sensorShape->sensorIndex ); + b3Visitor shapeRef = { + .shapeId = hit.visitorId, + .generation = visitor->generation, + }; + b3Array_Push( sensor->hits, shapeRef ); + } + } + + world->profile.sensorHits = b3GetMilliseconds( sensorHitTicks ); + b3TracyCZoneEnd( sensor_hits ); + } + + // Island sleeping + // This must be done last because putting islands to sleep invalidates the enlarged body bits. + // todo_erin figure out how to do this in parallel with tree refit + if ( world->enableSleep == true ) + { + b3TracyCZoneNC( sleep_islands, "Island Sleep", b3_colorLightSlateGray, true ); + uint64_t sleepTicks = b3GetTicks(); + + // Collect split island candidate for the next time step. No need to split if sleeping is disabled. + B3_ASSERT( world->splitIslandId == B3_NULL_INDEX ); + float splitSleepTimer = 0.0f; + for ( int i = 0; i < world->workerCount; ++i ) + { + b3TaskContext* taskContext = world->taskContexts.data + i; + if ( taskContext->splitIslandId != B3_NULL_INDEX && taskContext->splitSleepTime >= splitSleepTimer ) + { + B3_ASSERT( taskContext->splitSleepTime > 0.0f ); + + // Tie breaking for determinism. Largest island id wins. Needed due to work stealing. + if ( taskContext->splitSleepTime == splitSleepTimer && taskContext->splitIslandId < world->splitIslandId ) + { + continue; + } + + world->splitIslandId = taskContext->splitIslandId; + splitSleepTimer = taskContext->splitSleepTime; + } + } + + b3BitSet* awakeIslandBitSet = &world->taskContexts.data[0].awakeIslandBitSet; + for ( int i = 1; i < world->workerCount; ++i ) + { + b3InPlaceUnion( awakeIslandBitSet, &world->taskContexts.data[i].awakeIslandBitSet ); + } + + // Need to process in reverse because this moves islands to sleeping solver sets. + b3IslandSim* islands = awakeSet->islandSims.data; + int count = awakeSet->islandSims.count; + for ( int islandIndex = count - 1; islandIndex >= 0; islandIndex -= 1 ) + { + if ( b3GetBit( awakeIslandBitSet, islandIndex ) == true ) + { + // this island is still awake + continue; + } + + b3IslandSim* island = islands + islandIndex; + int islandId = island->islandId; + + b3TrySleepIsland( world, islandId ); + } + + b3ValidateSolverSets( world ); + + world->profile.sleepIslands = b3GetMilliseconds( sleepTicks ); + b3TracyCZoneEnd( sleep_islands ); + } +} diff --git a/src/solver.h b/src/solver.h new file mode 100644 index 0000000..db4c324 --- /dev/null +++ b/src/solver.h @@ -0,0 +1,315 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +// Solver work is partitioned into fixed-size blocks that worker threads claim +// in parallel via atomic CAS on a per-block syncIndex. The descriptor (b3SolverBlock) +// and the atomic counter sit in a wrapping b3SyncBlock so the CAS-winner can +// pass the descriptor by value into stage tasks without aliasing the atomic +// memory other threads are CAS-writing. Three properties of this design +// matter for performance: +// +// 1. Distributed contention. Per-block atomic syncIndex avoids the cache line stampede +// that a single shared fetch_add counter would cause. Once a worker +// settles into a block range, its CAS targets live in its own L1. +// +// 2. Monotonic syncIndex across iterations. Iterative stages (warm start, +// solve, relax) reuse the same block array every sub-step iteration. +// syncIndex grows each iteration; workers CAS (prev, prev+1), so the +// main thread never touches any per-block state between iterations. +// Non-iterative stages simply use syncIndex 1. +// +// 3. L2 affinity across iterations. Each worker picks a start offset from +// its workerIndex, then scans forward and (after wrap) backward: +// +// blocks: [0] [1] [2] [3] [4] [5] [6] [7] +// ^ ^ ^ ^ +// W0 W1 W2 W3 <- start offsets +// +// W0 claims 0,1,2,3 (forward), W1 claims 4,5, etc. Under balanced load +// each worker re-hits the same block range every iteration, keeping that +// range's hot data resident in its L2. A failed CAS means a neighbour +// already claimed the block, so the stealing worker stops -- preserving +// locality under mild imbalance while still draining the queue. +// +// A graph color stage lays out joint blocks first, then contact blocks: +// +// stage->blocks -> +// +------+------+------+------+------+------+------+ +// | J0 | J1 | J2 | C0 | C1 | C2 | C3 | +// +------+------+------+------+------+------+------+ +// <-- graphJointBlocks --><---- graphContactBlocks ----> +// +// Each block carries its type so the dispatcher routes J-blocks to the joint +// solver and C-blocks to the SIMD contact solver; both kinds run concurrently +// within the stage -- no barrier between them. The type tag lives on the +// block (not the stage) so that mixed-type stages can keep the concurrency. +// +// The solver threading model is inspired by https://github.com/bepu/bepuphysics2 + +#pragma once + +#include "container.h" +#include "core.h" + +#include "box3d/math_functions.h" + +#include + +typedef struct b3BodySim b3BodySim; +typedef struct b3BodyState b3BodyState; +typedef struct b3ContactConstraint b3ContactConstraint; +typedef struct b3ContactConstraintWide b3ContactConstraintWide; +typedef struct b3ContactSpec b3ContactSpec; +typedef struct b3JointSim b3JointSim; +typedef struct b3Manifold b3Manifold; +typedef struct b3ManifoldConstraint b3ManifoldConstraint; +typedef struct b3World b3World; + +// Solver stages +typedef enum b3SolverStageType +{ + b3_stagePrepareJoints, + b3_stagePrepareWideContacts, + b3_stagePrepareContacts, + b3_stageIntegrateVelocities, + b3_stageWarmStart, + b3_stageSolve, + b3_stageIntegratePositions, + b3_stageRelax, + b3_stageRestitution, + b3_stageStoreWideImpulses, + b3_stageStoreImpulses, +} b3SolverStageType; + +typedef enum b3SolverBlockType +{ + // Block for iterating across bodies. + b3_bodyBlock, + + // Block for iterating across joints. For prepare. + b3_jointBlock, + + // Block for iterating across wide contacts. For prepare and store. + b3_wideContactBlock, + + // Block for iterating across contacts. For prepare and store. + b3_contactBlock, + + // Block for iterating across joints of a single graph color. + b3_graphJointBlock, + + // Block for iterating across wide contacts of a single graph color. + b3_graphWideContactBlock, + + // Block for iterating across contacts of a single graph color. + b3_graphContactBlock, + + // Block for processing overflow constraints + b3_overflowBlock, +} b3SolverBlockType; + +// Solver block describes a multithreaded unit of work. +typedef struct b3SolverBlock +{ + int startIndex; + uint16_t count; + // b3SolverBlockType + uint8_t blockType; + uint8_t colorIndex; +} b3SolverBlock; + +// A unit of multithreaded work along with atomic synchronization. The syncIndex grows +// monotonically allowing the solver block to be re-used across sub-steps. +typedef struct b3SyncBlock +{ + b3SolverBlock block; + b3AtomicInt syncIndex; +} b3SyncBlock; + +// Each stage must be completed before going to the next stage. +// Non-iterative stages use a stage instance once while iterative stages re-use the same instance each iteration. +typedef struct b3SolverStage +{ + b3SyncBlock* blocks; + b3SolverStageType type; + int blockCount; + uint8_t colorIndex; + b3AtomicInt completionCount; +} b3SolverStage; + +// Constraint softness +typedef struct b3Softness +{ + float biasRate; + float massScale; + float impulseScale; +} b3Softness; + +// Prepare/store run as a flat parallel-for over the whole wide-constraint +// range. Each span maps a slice of that range back to the owning color's +// contacts so workers can decode flat wide-slot indices without touching +// graph state. The spans array has one entry per active color plus a sentinel +// whose start == wideContactCount. +typedef struct b3WidePrepareSpan +{ + int start; + int count; + int* contacts; +} b3WidePrepareSpan; + +typedef struct b3ContactPrepareSpan +{ + int start; + int count; + b3ContactSpec* contacts; +} b3ContactPrepareSpan; + +typedef struct b3JointPrepareSpan +{ + int start; + int count; + b3JointSim* joints; +} b3JointPrepareSpan; + +// Context for a time step. Recreated each time step. +typedef struct b3StepContext +{ + // time step + float dt; + + // inverse time step (0 if dt == 0). + float inv_dt; + + // sub-step + float h; + float inv_h; + + int subStepCount; + + b3Softness contactSoftness; + b3Softness staticSoftness; + + float restitutionThreshold; + float maxLinearVelocity; + + struct b3World* world; + struct b3ConstraintGraph* graph; + + // shortcut to body states from awake set + b3BodyState* states; + + // shortcut to body sims from awake set + b3BodySim* sims; + + // array of all shape ids for shapes that have enlarged AABBs + int* enlargedShapes; + int enlargedShapeCount; + + // Array of bullet bodies that need continuous collision handling + int* bulletBodies; + b3AtomicInt bulletBodyCount; + + // Contact ids for simplified parallel-for access. Used in narrow-phase. + // These contacts may or may not be touching. They are associated with awake bodies. + int* awakeContactIndices; + + // Flat view of the wide contact constraint array used by prepare and store. + // prepareSpans has activeColorCount + 1 entries, the last being a sentinel + // at wideContactCount. wideContactConstraints is the contiguous base + // pointer; per-color slices live at colors[i].wideConstraints. + struct b3ContactConstraintWide* wideConstraints; + b3WidePrepareSpan* widePrepareSpans; + int wideContactCount; + + // Similar for mesh/overflow contact constraints + struct b3ManifoldConstraint* manifoldConstraints; + struct b3ContactConstraint* contactConstraints; + b3ContactPrepareSpan* contactPrepareSpans; + b3ContactPrepareSpan* overflowSpans; + b3JointPrepareSpan* jointPrepareSpans; + + int activeColorCount; + int workerCount; + + b3SolverStage* stages; + int stageCount; + bool enableWarmStarting; + + // padding to prevent false sharing + char padding1[64]; + + // This atomic is central to multi-threaded solver task synchronization. + // It prevents ABA problems by monotonically growing as the solver advances. + // This means a delayed worker thread will catch up without repeating already completed + // work (causing a race condition). + // sync index (16-bits) | stage type (16-bits) + b3AtomicU32 atomicSyncBits; + + // padding to prevent false sharing + char padding2[64]; + + // Race flag claimed by whichever runner reaches b3SolverTask with workerIndex 0 first. + // The calling thread of b2World_Step also races for this slot so the orchestrator can + // always make progress, regardless of how the user's task system schedules tasks (out + // of order, fewer threads than workers, or synchronously inside enqueueTaskFcn). The + // loser of the race no-ops as workerIndex 0. + b3AtomicInt mainClaimed; + + // padding to prevent false sharing + char padding3[64]; +} b3StepContext; + +#ifdef __cplusplus +extern "C" +{ +#endif + +void b3Solve( b3World* world, b3StepContext* stepContext ); + +#ifdef __cplusplus +} +#endif + +B3_INLINE b3Softness b3MakeSoft( float hertz, float zeta, float h ) +{ + if ( hertz == 0.0f ) + { + return B3_LITERAL( b3Softness ){ + .biasRate = 0.0f, + .massScale = 0.0f, + .impulseScale = 0.0f, + }; + } + + float omega = 2.0f * B3_PI * hertz; + float a1 = 2.0f * zeta + h * omega; + float a2 = h * omega * a1; + float a3 = 1.0f / ( 1.0f + a2 ); + + // bias = w / (2 * z + hw) + // massScale = hw * (2 * z + hw) / (1 + hw * (2 * z + hw)) + // impulseScale = 1 / (1 + hw * (2 * z + hw)) + + // If z == 0 + // bias = 1/h + // massScale = hw^2 / (1 + hw^2) + // impulseScale = 1 / (1 + hw^2) + + // w -> inf + // bias = 1/h + // massScale = 1 + // impulseScale = 0 + + // if w = pi / 4 * inv_h + // massScale = (pi/4)^2 / (1 + (pi/4)^2) = pi^2 / (16 + pi^2) ~= 0.38 + // impulseScale = 1 / (1 + (pi/4)^2) = 16 / (16 + pi^2) ~= 0.62 + + // In all cases: + // massScale + impulseScale == 1 + + return B3_LITERAL( b3Softness ){ + .biasRate = omega / a1, + .massScale = a2 * a3, + .impulseScale = a3, + }; +} diff --git a/src/solver_set.c b/src/solver_set.c new file mode 100644 index 0000000..f050a86 --- /dev/null +++ b/src/solver_set.c @@ -0,0 +1,633 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "solver_set.h" + +#include "body.h" +#include "constraint_graph.h" +#include "contact.h" +#include "island.h" +#include "joint.h" +#include "physics_world.h" + +#include + +void b3DestroySolverSet( b3World* world, int setIndex ) +{ + b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + + b3Array_Destroy( set->bodySims ); + b3Array_Destroy( set->bodyStates ); + b3Array_Destroy( set->contactIndices ); + b3Array_Destroy( set->jointSims ); + b3Array_Destroy( set->islandSims ); + + b3FreeId( &world->solverSetIdPool, setIndex ); + *set = (b3SolverSet){ 0 }; + set->setIndex = B3_NULL_INDEX; +} + +// Wake a solver set. Does not merge islands. +// Contacts can be in several places: +// 1. non-touching contacts in the disabled set +// 2. non-touching contacts already in the awake set +// 3. touching contacts in the sleeping set +// This handles contact types 1 and 3. Type 2 doesn't need any action. +void b3WakeSolverSet( b3World* world, int setIndex ) +{ + B3_ASSERT( setIndex >= b3_firstSleepingSet ); + b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + b3SolverSet* disabledSet = b3Array_Get( world->solverSets, b3_disabledSet ); + + b3Body* bodies = world->bodies.data; + + int bodyCount = set->bodySims.count; + for ( int i = 0; i < bodyCount; ++i ) + { + b3BodySim* simSrc = set->bodySims.data + i; + + b3Body* body = bodies + simSrc->bodyId; + B3_ASSERT( body->setIndex == setIndex ); + body->setIndex = b3_awakeSet; + body->localIndex = awakeSet->bodySims.count; + + // Reset sleep timer + body->sleepTime = 0.0f; + + b3BodySim* simDst = b3Array_Emplace( awakeSet->bodySims ); + memcpy( simDst, simSrc, sizeof( b3BodySim ) ); + + b3BodyState* state = b3Array_Emplace( awakeSet->bodyStates ); + *state = b3_identityBodyState; + state->flags = body->flags; + + // move non-touching contacts from disabled set to awake set + int contactKey = body->headContactKey; + while ( contactKey != B3_NULL_INDEX ) + { + int edgeIndex = contactKey & 1; + int contactId = contactKey >> 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->setIndex != b3_disabledSet ) + { + B3_ASSERT( contact->setIndex == b3_awakeSet || contact->setIndex == setIndex ); + continue; + } + + int localIndex = contact->localIndex; + + B3_ASSERT( 0 <= localIndex && localIndex < disabledSet->contactIndices.count ); + B3_ASSERT( disabledSet->contactIndices.data[localIndex] == contactId ); + + B3_ASSERT( ( contact->flags & b3_contactTouchingFlag ) == 0 && contact->manifoldCount == 0 ); + + contact->setIndex = b3_awakeSet; + contact->localIndex = awakeSet->contactIndices.count; + b3Array_Push( awakeSet->contactIndices, contactId ); + + int movedLocalIndex = b3Array_RemoveSwap( disabledSet->contactIndices, localIndex ); + if ( movedLocalIndex != B3_NULL_INDEX ) + { + // fix moved element + int movedContactIndex = disabledSet->contactIndices.data[localIndex]; + b3Contact* movedContact = b3Array_Get( world->contacts, movedContactIndex ); + B3_ASSERT( movedContact->localIndex == movedLocalIndex ); + movedContact->localIndex = localIndex; + } + } + } + + // Transfer touching contacts from sleeping set to constraint graph. + { + int contactCount = set->contactIndices.count; + for ( int i = 0; i < contactCount; ++i ) + { + int contactIndex = set->contactIndices.data[i]; + b3Contact* contact = b3Array_Get( world->contacts, contactIndex ); + B3_ASSERT( contact->flags & b3_contactTouchingFlag ); + B3_ASSERT( contact->flags & b3_simTouchingFlag ); + B3_ASSERT( contact->setIndex == setIndex ); + b3AddContactToGraph( world, contact ); + contact->setIndex = b3_awakeSet; + } + } + + // transfer joints from sleeping set to awake set + { + int jointCount = set->jointSims.count; + for ( int i = 0; i < jointCount; ++i ) + { + b3JointSim* jointSim = set->jointSims.data + i; + b3Joint* joint = b3Array_Get( world->joints, jointSim->jointId ); + B3_ASSERT( joint->setIndex == setIndex ); + b3AddJointToGraph( world, jointSim, joint ); + joint->setIndex = b3_awakeSet; + } + } + + // transfer island from sleeping set to awake set + // Usually a sleeping set has only one island, but it is possible + // that joints are created between sleeping islands and they + // are moved to the same sleeping set. + { + int islandCount = set->islandSims.count; + for ( int i = 0; i < islandCount; ++i ) + { + b3IslandSim* islandSrc = set->islandSims.data + i; + b3Island* island = b3Array_Get( world->islands, islandSrc->islandId ); + island->setIndex = b3_awakeSet; + island->localIndex = awakeSet->islandSims.count; + b3IslandSim* islandDst = b3Array_Emplace( awakeSet->islandSims ); + memcpy( islandDst, islandSrc, sizeof( b3IslandSim ) ); + } + } + + // destroy the sleeping set + b3DestroySolverSet( world, setIndex ); +} + +void b3TrySleepIsland( b3World* world, int islandId ) +{ + b3Island* island = b3Array_Get( world->islands, islandId ); + B3_ASSERT( island->setIndex == b3_awakeSet ); + + // Cannot put an island to sleep while it has a pending split and more than one body. + if ( island->constraintRemoveCount > 0 && island->bodies.count > 1 ) + { + return; + } + + // island is sleeping + // - create new sleeping solver set + // - move island to sleeping solver set + // - identify non-touching contacts that should move to sleeping solver set or disabled set + // - remove old island + // - fix island + int sleepSetId = b3AllocId( &world->solverSetIdPool ); + if ( sleepSetId == world->solverSets.count ) + { + b3SolverSet set = { 0 }; + set.setIndex = B3_NULL_INDEX; + b3Array_Push( world->solverSets, set ); + } + + b3SolverSet* sleepSet = b3Array_Get( world->solverSets, sleepSetId ); + *sleepSet = (b3SolverSet){ 0 }; + + // grab awake set after creating the sleep set because the solver set array may have been resized + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + b3SolverSet* disabledSet = b3Array_Get( world->solverSets, b3_disabledSet ); + B3_ASSERT( 0 <= island->localIndex && island->localIndex < awakeSet->islandSims.count ); + + sleepSet->setIndex = sleepSetId; + b3Array_Reserve( sleepSet->bodySims, island->bodies.count ); + b3Array_Reserve( sleepSet->contactIndices, island->contacts.count ); + b3Array_Reserve( sleepSet->jointSims, island->joints.count ); + + // move awake bodies to sleeping set + // this shuffles around bodies in the awake set + { + for ( int i = 0; i < island->bodies.count; ++i ) + { + int bodyId = island->bodies.data[i]; + b3Body* body = b3Array_Get( world->bodies, bodyId ); + B3_ASSERT( body->setIndex == b3_awakeSet ); + B3_ASSERT( body->islandId == islandId ); + B3_ASSERT( body->islandIndex == i ); + + // Update the body move event to indicate this body fell asleep + // It could happen the body is forced asleep before it ever moves. + if ( body->bodyMoveIndex != B3_NULL_INDEX ) + { + b3BodyMoveEvent* moveEvent = b3Array_Get( world->bodyMoveEvents, body->bodyMoveIndex ); + B3_ASSERT( moveEvent->bodyId.index1 - 1 == bodyId ); + B3_ASSERT( moveEvent->bodyId.generation == body->generation ); + moveEvent->fellAsleep = true; + body->bodyMoveIndex = B3_NULL_INDEX; + } + + int awakeBodyIndex = body->localIndex; + b3BodySim* awakeSim = b3Array_Get( awakeSet->bodySims, awakeBodyIndex ); + + // move body sim to sleep set + int sleepBodyIndex = sleepSet->bodySims.count; + b3BodySim* sleepBodySim = b3Array_Emplace( sleepSet->bodySims ); + memcpy( sleepBodySim, awakeSim, sizeof( b3BodySim ) ); + + int movedIndex = b3Array_RemoveSwap( awakeSet->bodySims, awakeBodyIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // fix local index on moved element + b3BodySim* movedSim = awakeSet->bodySims.data + awakeBodyIndex; + int movedId = movedSim->bodyId; + b3Body* movedBody = b3Array_Get( world->bodies, movedId ); + B3_ASSERT( movedBody->localIndex == movedIndex ); + movedBody->localIndex = awakeBodyIndex; + } + + // destroy state, no need to clone + b3Array_RemoveSwap( awakeSet->bodyStates, awakeBodyIndex ); + + body->setIndex = sleepSetId; + body->localIndex = sleepBodyIndex; + + // Move non-touching contacts to the disabled set. + // Non-touching contacts may exist between sleeping islands and there is no clear ownership. + int contactKey = body->headContactKey; + while ( contactKey != B3_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + + B3_ASSERT( contact->setIndex == b3_awakeSet || contact->setIndex == b3_disabledSet ); + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->setIndex == b3_disabledSet ) + { + // already moved to disabled set by another body in the island + continue; + } + + if ( contact->colorIndex != B3_NULL_INDEX ) + { + // contact is touching and will be moved separately + B3_ASSERT( ( contact->flags & b3_contactTouchingFlag ) != 0 ); + continue; + } + + // the other body may still be awake, it still may go to sleep and then it will be responsible + // for moving this contact to the disabled set. + int otherEdgeIndex = edgeIndex ^ 1; + int otherBodyId = contact->edges[otherEdgeIndex].bodyId; + b3Body* otherBody = b3Array_Get( world->bodies, otherBodyId ); + if ( otherBody->setIndex == b3_awakeSet ) + { + continue; + } + + int localIndex = contact->localIndex; + B3_ASSERT( awakeSet->contactIndices.data[localIndex] == contactId ); + + B3_ASSERT( contact->manifoldCount == 0 ); + B3_ASSERT( ( contact->flags & b3_contactTouchingFlag ) == 0 ); + + // Move the non-touching contact to the disabled set. + contact->setIndex = b3_disabledSet; + + // This is mandatory for validation to work correctly + contact->localIndex = disabledSet->contactIndices.count; + b3Array_Push( disabledSet->contactIndices, contact->contactId ); + + int movedLocalIndex = b3Array_RemoveSwap( awakeSet->contactIndices, localIndex ); + if ( movedLocalIndex != B3_NULL_INDEX ) + { + // fix moved element + int movedContactIndex = awakeSet->contactIndices.data[localIndex]; + b3Contact* movedContact = b3Array_Get( world->contacts, movedContactIndex ); + B3_ASSERT( movedContact->localIndex == movedLocalIndex ); + movedContact->localIndex = localIndex; + } + } + } + } + + // move touching contacts to sleeping set + // this shuffles contacts in the awake set + { + for ( int i = 0; i < island->contacts.count; ++i ) + { + int contactId = island->contacts.data[i].contactId; + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + B3_ASSERT( contact->setIndex == b3_awakeSet ); + B3_ASSERT( contact->islandId == islandId ); + B3_ASSERT( contact->islandIndex == i ); + int colorIndex = contact->colorIndex; + B3_ASSERT( 0 <= colorIndex && colorIndex < B3_GRAPH_COLOR_COUNT ); + + b3GraphColor* color = world->constraintGraph.colors + colorIndex; + + // Remove bodies from graph coloring associated with this constraint + if ( colorIndex != B3_OVERFLOW_INDEX ) + { + // might clear a bit for a static body, but this has no effect + b3ClearBit( &color->bodySet, contact->edges[0].bodyId ); + b3ClearBit( &color->bodySet, contact->edges[1].bodyId ); + } + + int sleepContactIndex = sleepSet->contactIndices.count; + b3Array_Push( sleepSet->contactIndices, contactId ); + + int localIndex = contact->localIndex; + if ( ( contact->flags & b3_simMeshContact ) || colorIndex == B3_OVERFLOW_INDEX ) + { + int movedLocalIndex = b3Array_RemoveSwap( color->contacts, localIndex ); + if ( movedLocalIndex != B3_NULL_INDEX ) + { + // fix moved element + int movedContactId = color->contacts.data[localIndex].contactId; + b3Contact* movedContact = b3Array_Get( world->contacts, movedContactId ); + B3_ASSERT( movedContact->localIndex == movedLocalIndex ); + movedContact->localIndex = localIndex; + } + } + else + { + int movedLocalIndex = b3Array_RemoveSwap( color->convexContacts, localIndex ); + if ( movedLocalIndex != B3_NULL_INDEX ) + { + // fix moved element + int movedContactId = color->convexContacts.data[localIndex]; + b3Contact* movedContact = b3Array_Get( world->contacts, movedContactId ); + B3_ASSERT( movedContact->localIndex == movedLocalIndex ); + movedContact->localIndex = localIndex; + } + } + + contact->setIndex = sleepSetId; + contact->colorIndex = B3_NULL_INDEX; + contact->localIndex = sleepContactIndex; + } + } + + // move joints + // this shuffles joints in the awake set + { + for ( int i = 0; i < island->joints.count; ++i ) + { + int jointId = island->joints.data[i].jointId; + b3Joint* joint = b3Array_Get( world->joints, jointId ); + B3_ASSERT( joint->setIndex == b3_awakeSet ); + B3_ASSERT( joint->islandId == islandId ); + B3_ASSERT( joint->islandIndex == i ); + int colorIndex = joint->colorIndex; + int localIndex = joint->localIndex; + + B3_ASSERT( 0 <= colorIndex && colorIndex < B3_GRAPH_COLOR_COUNT ); + + b3GraphColor* color = world->constraintGraph.colors + colorIndex; + + b3JointSim* awakeJointSim = b3Array_Get( color->jointSims, localIndex ); + + if ( colorIndex != B3_OVERFLOW_INDEX ) + { + // might clear a bit for a static body, but this has no effect + b3ClearBit( &color->bodySet, joint->edges[0].bodyId ); + b3ClearBit( &color->bodySet, joint->edges[1].bodyId ); + } + + int sleepJointIndex = sleepSet->jointSims.count; + b3JointSim* sleepJointSim = b3Array_Emplace( sleepSet->jointSims ); + memcpy( sleepJointSim, awakeJointSim, sizeof( b3JointSim ) ); + + int movedIndex = b3Array_RemoveSwap( color->jointSims, localIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // fix moved element + b3JointSim* movedJointSim = color->jointSims.data + localIndex; + int movedId = movedJointSim->jointId; + b3Joint* movedJoint = b3Array_Get( world->joints, movedId ); + B3_ASSERT( movedJoint->localIndex == movedIndex ); + movedJoint->localIndex = localIndex; + } + + joint->setIndex = sleepSetId; + joint->colorIndex = B3_NULL_INDEX; + joint->localIndex = sleepJointIndex; + } + } + + // move island struct + { + B3_ASSERT( island->setIndex == b3_awakeSet ); + + int islandIndex = island->localIndex; + b3IslandSim* sleepIsland = b3Array_Emplace( sleepSet->islandSims ); + sleepIsland->islandId = islandId; + + int movedIslandIndex = b3Array_RemoveSwap( awakeSet->islandSims, islandIndex ); + if ( movedIslandIndex != B3_NULL_INDEX ) + { + // fix index on moved element + b3IslandSim* movedIslandSim = awakeSet->islandSims.data + islandIndex; + int movedIslandId = movedIslandSim->islandId; + b3Island* movedIsland = b3Array_Get( world->islands, movedIslandId ); + B3_ASSERT( movedIsland->localIndex == movedIslandIndex ); + movedIsland->localIndex = islandIndex; + } + + island->setIndex = sleepSetId; + island->localIndex = 0; + } + + if ( world->splitIslandId == islandId ) + { + world->splitIslandId = B3_NULL_INDEX; + } + + b3ValidateSolverSets( world ); +} + +// This is called when joints are created between sets. I want to allow the sets +// to continue sleeping if both are asleep. Otherwise one set is waked. +// Islands will get merge when the set is woke. +void b3MergeSolverSets( b3World* world, int setId1, int setId2 ) +{ + B3_ASSERT( setId1 >= b3_firstSleepingSet ); + B3_ASSERT( setId2 >= b3_firstSleepingSet ); + b3SolverSet* set1 = b3Array_Get( world->solverSets, setId1 ); + b3SolverSet* set2 = b3Array_Get( world->solverSets, setId2 ); + + // Move the fewest number of bodies + if ( set1->bodySims.count < set2->bodySims.count ) + { + b3SolverSet* tempSet = set1; + set1 = set2; + set2 = tempSet; + + int tempId = setId1; + setId1 = setId2; + setId2 = tempId; + } + + // transfer bodies + { + b3Body* bodies = world->bodies.data; + int bodyCount = set2->bodySims.count; + for ( int i = 0; i < bodyCount; ++i ) + { + b3BodySim* simSrc = set2->bodySims.data + i; + + b3Body* body = bodies + simSrc->bodyId; + B3_ASSERT( body->setIndex == setId2 ); + body->setIndex = setId1; + body->localIndex = set1->bodySims.count; + + b3BodySim* simDst = b3Array_Emplace( set1->bodySims ); + memcpy( simDst, simSrc, sizeof( b3BodySim ) ); + } + } + + // transfer contacts + { + int contactCount = set2->contactIndices.count; + for ( int i = 0; i < contactCount; ++i ) + { + int contactIndex = set2->contactIndices.data[i]; + b3Contact* contact = b3Array_Get( world->contacts, contactIndex ); + B3_ASSERT( contact->setIndex == setId2 ); + contact->setIndex = setId1; + contact->localIndex = set1->contactIndices.count; + b3Array_Push( set1->contactIndices, contactIndex ); + } + } + + // transfer joints + { + int jointCount = set2->jointSims.count; + for ( int i = 0; i < jointCount; ++i ) + { + b3JointSim* jointSrc = set2->jointSims.data + i; + + b3Joint* joint = b3Array_Get( world->joints, jointSrc->jointId ); + B3_ASSERT( joint->setIndex == setId2 ); + joint->setIndex = setId1; + joint->localIndex = set1->jointSims.count; + + b3JointSim* jointDst = b3Array_Emplace( set1->jointSims ); + memcpy( jointDst, jointSrc, sizeof( b3JointSim ) ); + } + } + + // transfer islands + { + int islandCount = set2->islandSims.count; + for ( int i = 0; i < islandCount; ++i ) + { + b3IslandSim* islandSrc = set2->islandSims.data + i; + int islandId = islandSrc->islandId; + + b3Island* island = b3Array_Get( world->islands, islandId ); + island->setIndex = setId1; + island->localIndex = set1->islandSims.count; + + b3IslandSim* islandDst = b3Array_Emplace( set1->islandSims ); + memcpy( islandDst, islandSrc, sizeof( b3IslandSim ) ); + } + } + + // destroy the merged set + // Warning: need to be careful not to destroy things that got transferred, like triangle caches. + b3DestroySolverSet( world, setId2 ); + + b3ValidateSolverSets( world ); +} + +void b3TransferBody( b3World* world, b3SolverSet* targetSet, b3SolverSet* sourceSet, b3Body* body ) +{ + if ( targetSet == sourceSet ) + { + return; + } + + int sourceIndex = body->localIndex; + b3BodySim* sourceSim = b3Array_Get( sourceSet->bodySims, sourceIndex ); + + int targetIndex = targetSet->bodySims.count; + b3BodySim* targetSim = b3Array_Emplace( targetSet->bodySims ); + memcpy( targetSim, sourceSim, sizeof( b3BodySim ) ); + + // Clear transient body flags + targetSim->flags &= ~( b3_isFast | b3_isSpeedCapped | b3_hadTimeOfImpact ); + + // Remove body sim from solver set that owns it + int movedIndex = b3Array_RemoveSwap( sourceSet->bodySims, sourceIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fix moved body index + b3BodySim* movedSim = sourceSet->bodySims.data + sourceIndex; + int movedId = movedSim->bodyId; + b3Body* movedBody = b3Array_Get( world->bodies, movedId ); + B3_ASSERT( movedBody->localIndex == movedIndex ); + movedBody->localIndex = sourceIndex; + } + + if ( sourceSet->setIndex == b3_awakeSet ) + { + b3Array_RemoveSwap( sourceSet->bodyStates, sourceIndex ); + } + else if ( targetSet->setIndex == b3_awakeSet ) + { + b3BodyState* state = b3Array_Emplace( targetSet->bodyStates ); + *state = b3_identityBodyState; + state->flags = body->flags; + } + + body->setIndex = targetSet->setIndex; + body->localIndex = targetIndex; +} + +void b3TransferJoint( b3World* world, b3SolverSet* targetSet, b3SolverSet* sourceSet, b3Joint* joint ) +{ + if ( targetSet == sourceSet ) + { + return; + } + + int localIndex = joint->localIndex; + int colorIndex = joint->colorIndex; + + // Retrieve source. + b3JointSim* sourceSim; + if ( sourceSet->setIndex == b3_awakeSet ) + { + B3_ASSERT( 0 <= colorIndex && colorIndex < B3_GRAPH_COLOR_COUNT ); + b3GraphColor* color = world->constraintGraph.colors + colorIndex; + + sourceSim = b3Array_Get( color->jointSims, localIndex ); + } + else + { + B3_ASSERT( colorIndex == B3_NULL_INDEX ); + sourceSim = b3Array_Get( sourceSet->jointSims, localIndex ); + } + + // Create target and copy. Fix joint. + if ( targetSet->setIndex == b3_awakeSet ) + { + b3AddJointToGraph( world, sourceSim, joint ); + joint->setIndex = b3_awakeSet; + } + else + { + joint->setIndex = targetSet->setIndex; + joint->localIndex = targetSet->jointSims.count; + joint->colorIndex = B3_NULL_INDEX; + + b3JointSim* targetSim = b3Array_Emplace( targetSet->jointSims ); + memcpy( targetSim, sourceSim, sizeof( b3JointSim ) ); + } + + // Destroy source. + if ( sourceSet->setIndex == b3_awakeSet ) + { + b3RemoveJointFromGraph( world, joint->edges[0].bodyId, joint->edges[1].bodyId, colorIndex, localIndex ); + } + else + { + int movedIndex = b3Array_RemoveSwap( sourceSet->jointSims, localIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // fix swapped element + b3JointSim* movedJointSim = sourceSet->jointSims.data + localIndex; + int movedId = movedJointSim->jointId; + b3Joint* movedJoint = b3Array_Get( world->joints, movedId ); + movedJoint->localIndex = localIndex; + } + } +} diff --git a/src/solver_set.h b/src/solver_set.h new file mode 100644 index 0000000..b916fcc --- /dev/null +++ b/src/solver_set.h @@ -0,0 +1,76 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "container.h" +#include "contact.h" + +typedef struct b3Body b3Body; +typedef struct b3BodySim b3BodySim; +typedef struct b3BodyState b3BodyState; +typedef struct b3IslandSim b3IslandSim; +typedef struct b3Joint b3Joint; +typedef struct b3JointSim b3JointSim; +typedef struct b3World b3World; + +b3DeclareArray( b3BodySim ); +b3DeclareArray( b3BodyState ); +b3DeclareArray( b3JointSim ); +b3DeclareArray( b3IslandSim ); + +// This holds solver set data. The following sets are used: +// - static set for all static bodies and joints between static bodies +// - active set for all active bodies with body states (no +// contacts or joints) +// - disabled set for disabled bodies and their joints +// - all further sets are sleeping island sets along with their contacts and joints +// The purpose of solver sets is to achieve high memory locality. +// https://www.youtube.com/watch?v=nZNd5FjSquk +typedef struct b3SolverSet +{ + // Body array. Empty for unused set. + b3ArrayC( b3BodySim ) bodySims; + + // Body state only exists for active set + b3ArrayC( b3BodyState ) bodyStates; + + // This holds sleeping/disabled joints. Empty for static/active set. + b3ArrayC( b3JointSim ) jointSims; + + // This holds all contacts for sleeping sets. + // This holds non-touching contacts for the awake set. + // This should be empty for the static and disabled sets. + b3ArrayC( int ) contactIndices; + + // The awake set has an array of islands. Sleeping sets normally have a single islands. However, joints + // created between sleeping sets causes the sets to merge, leaving them with multiple islands. These sleeping + // islands will be naturally merged with the set is woken. + // The static and disabled sets have no islands. + // Islands live in the solver sets to limit the number of islands that need to be considered for sleeping. + b3ArrayC( b3IslandSim ) islandSims; + + // Aligns with b3World::solverSetIdPool. Used to create a stable id for body/contact/joint/islands. + int setIndex; +} b3SolverSet; + +#ifdef __cplusplus +extern "C" +{ +#endif + +void b3DestroySolverSet( b3World* world, int setIndex ); + +void b3WakeSolverSet( b3World* world, int setIndex ); +void b3TrySleepIsland( b3World* world, int islandId ); + +// Merge set 2 into set 1 then destroy set 2. +// Warning: any pointers into these sets will be orphaned. +void b3MergeSolverSets( b3World* world, int setIndex1, int setIndex2 ); + +void b3TransferBody( b3World* world, b3SolverSet* targetSet, b3SolverSet* sourceSet, b3Body* body ); +void b3TransferJoint( b3World* world, b3SolverSet* targetSet, b3SolverSet* sourceSet, b3Joint* joint ); + +#ifdef __cplusplus +} +#endif diff --git a/src/sphere.c b/src/sphere.c new file mode 100644 index 0000000..5b659ef --- /dev/null +++ b/src/sphere.c @@ -0,0 +1,241 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +// Dirk Gregorius contributed portions of this code + +#include "math_internal.h" +#include "shape.h" + +#include "box3d/base.h" +#include "box3d/collision.h" +#include "box3d/constants.h" + +b3MassData b3ComputeSphereMass( const b3Sphere* shape, float density ) +{ + b3Vec3 center = shape->center; + float radius = shape->radius; + + float volume = 4.0f / 3.0f * B3_PI * radius * radius * radius; + float mass = volume * density; + float ixx = 0.4f * mass * radius * radius; + + b3MassData out; + out.mass = mass; + out.center = center; + out.inertia = b3AddMM( b3MakeDiagonalMatrix( ixx, ixx, ixx ), b3Steiner( mass, center ) ); + return out; +} + +b3AABB b3ComputeSphereAABB( const b3Sphere* shape, b3Transform transform ) +{ + b3Vec3 center = b3TransformPoint( transform, shape->center ); + float radius = shape->radius; + b3Vec3 extent = { radius, radius, radius }; + return ( b3AABB ){ b3Sub( center, extent ), b3Add( center, extent ) }; +} + +b3AABB b3ComputeSweptSphereAABB( const b3Sphere* shape, b3Transform xf1, b3Transform xf2 ) +{ + b3Vec3 r = { shape->radius, shape->radius, shape->radius }; + b3Vec3 center1 = b3TransformPoint( xf1, shape->center ); + b3Vec3 center2 = b3TransformPoint( xf2, shape->center ); + return ( b3AABB ){ b3Sub( b3Min( center1, center2 ), r ), b3Add( b3Max( center1, center2 ), r ) }; +} + +bool b3OverlapSphere( const b3Sphere* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ) +{ + b3DistanceInput input; + input.proxyA = ( b3ShapeProxy ){ &shape->center, 1, shape->radius }; + input.proxyB = *proxy; + input.transformA = shapeTransform; + input.transformB = b3Transform_identity; + input.useRadii = true; + + b3SimplexCache cache = { 0 }; + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + return output.distance < B3_OVERLAP_SLOP; +} + +// Precision Improvements for Ray / Sphere Intersection - Ray Tracing Gems 2019 +// http://www.codercorner.com/blog/?p=321 +b3CastOutput b3RayCastSphere(const b3Sphere* shape, const b3RayCastInput* input ) +{ + B3_ASSERT( b3IsValidRay( input ) ); + b3CastOutput output = { 0 }; + + b3Vec3 p = shape->center; + + // Shift ray so sphere center is the origin + b3Vec3 s = b3Sub( input->origin, p ); + + float r = shape->radius; + float rr = r * r; + + float length; + b3Vec3 d = b3GetLengthAndNormalize( &length, input->translation ); + if ( length == 0.0f ) + { + // zero length ray + + if ( b3LengthSquared( s ) < rr ) + { + // initial overlap + output.point = input->origin; + output.hit = true; + } + + return output; + } + + // Find closest point on ray to origin + + // solve: dot(s + t * d, d) = 0 + float t = -b3Dot( s, d ); + + // c is the closest point on the line to the origin + b3Vec3 c = b3MulAdd( s, t, d ); + + float cc = b3Dot( c, c ); + + if ( cc > rr ) + { + // closest point is outside the sphere + return output; + } + + // Pythagoras + float h = sqrtf( rr - cc ); + + float fraction = t - h; + + if ( fraction < 0.0f || input->maxFraction * length < fraction ) + { + // intersection is point outside the range of the ray segment + + if ( b3LengthSquared( s ) < rr ) + { + // initial overlap + output.point = input->origin; + output.hit = true; + } + + return output; + } + + b3Vec3 hitPoint = b3MulAdd( s, fraction, d ); + + output.fraction = fraction / length; + + if ( output.fraction > input->maxFraction ) + { + b3Log( "sphere input fraction = %g, output fraction = %g", input->maxFraction, output.fraction ); + output.fraction = input->maxFraction; + } + + output.normal = b3Normalize( hitPoint ); + output.point = b3MulAdd( p, shape->radius, output.normal ); + output.hit = true; + + return output; +} + +// Precision Improvements for Ray / Sphere Intersection - Ray Tracing Gems 2019 +// http://www.codercorner.com/blog/?p=321 +// This will do interior hits. +b3CastOutput b3RayCastHollowSphere( const b3Sphere* sphere, const b3RayCastInput* input ) +{ + b3Vec3 p = sphere->center; + + b3CastOutput output = { 0 }; + + // Shift ray so sphere center is the origin + b3Vec3 s = b3Sub( input->origin, p ); + b3Vec3 d = b3Normalize( input->translation ); + + // Find closest point on ray to origin + + // solve: dot(s + t * d, d) = 0 + float t = -b3Dot( s, d ); + + // c is the closest point on the line to the origin + b3Vec3 c = b3MulAdd( s, t, d ); + + float cc = b3Dot( c, c ); + float r = sphere->radius; + float rr = r * r; + + if ( cc > rr ) + { + // closest point is outside the sphere + return output; + } + + // Pythagoras + float h = sqrtf( rr - cc ); + + float fraction = t - h; + + if ( fraction < 0.0f ) + { + fraction = t + h; + } + + if ( fraction < 0.0f ) + { + // behind the ray + return output; + } + + if (fraction > input->maxFraction) + { + return output; + } + + b3Vec3 hitPoint = b3MulAdd( s, fraction, d ); + + output.fraction = fraction; + output.normal = b3Normalize( hitPoint ); + output.point = b3MulAdd( p, sphere->radius, output.normal ); + output.hit = true; + + return output; +} + +b3CastOutput b3ShapeCastSphere( const b3Sphere* sphere, const b3ShapeCastInput* input ) +{ + b3ShapeCastPairInput pairInput; + pairInput.proxyA = ( b3ShapeProxy ){ &sphere->center, 1, sphere->radius }; + pairInput.proxyB = input->proxy; + pairInput.transformA = b3Transform_identity; + pairInput.transformB = b3Transform_identity; + pairInput.translationB = input->translation; + pairInput.maxFraction = input->maxFraction; + pairInput.canEncroach = input->canEncroach; + + b3CastOutput output = b3ShapeCast( &pairInput ); + return output; +} + +int b3CollideMoverAndSphere( b3PlaneResult* result, const b3Sphere* shape, const b3Capsule* mover ) +{ + b3DistanceInput distanceInput; + distanceInput.proxyA = ( b3ShapeProxy ){ &shape->center, 1, 0.0f }; + distanceInput.proxyB = ( b3ShapeProxy ){ &mover->center1, 2, mover->radius }; + distanceInput.transformA = b3Transform_identity; + distanceInput.transformB = b3Transform_identity; + distanceInput.useRadii = false; + + float totalRadius = mover->radius + shape->radius; + + b3SimplexCache cache = { 0 }; + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance <= totalRadius ) + { + b3Plane plane = { distanceOutput.normal, totalRadius - distanceOutput.distance }; + *result = ( b3PlaneResult ){ plane, distanceOutput.pointA }; + return 1; + } + + return 0; +} diff --git a/src/spherical_joint.c b/src/spherical_joint.c new file mode 100644 index 0000000..8442101 --- /dev/null +++ b/src/spherical_joint.c @@ -0,0 +1,717 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "math_internal.h" +#include "physics_world.h" +#include "solver.h" +#include "solver_set.h" + +// needed for dll export +#include "box3d/box3d.h" + +void b3SphericalJoint_EnableConeLimit( b3JointId jointId, bool enableLimit ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + if ( enableLimit != base->sphericalJoint.enableConeLimit ) + { + base->sphericalJoint.swingImpulse = 0.0f; + } + base->sphericalJoint.enableConeLimit = enableLimit; +} + +bool b3SphericalJoint_IsConeLimitEnabled( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.enableConeLimit; +} + +float b3SphericalJoint_GetConeLimit( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.coneAngle; +} + +void b3SphericalJoint_SetConeLimit( b3JointId jointId, float angleRadians ) +{ + B3_ASSERT( b3IsValidFloat( angleRadians ) && 0 <= angleRadians && angleRadians <= 0.5f * B3_PI ); + + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + base->sphericalJoint.coneAngle = angleRadians; +} + +float b3SphericalJoint_GetConeAngle( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + b3Transform transformA = b3GetBodyTransform( world, base->bodyIdA ); + b3Transform transformB = b3GetBodyTransform( world, base->bodyIdB ); + + b3Quat quatA = b3MulQuat( transformA.q, base->localFrameA.q ); + b3Quat quatB = b3MulQuat( transformB.q, base->localFrameB.q ); + + if ( b3DotQuat( quatA, quatB ) < 0.0f ) + { + // this keeps the swing angle in the range [0, pi] + quatB = b3NegateQuat( quatB ); + } + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + return b3GetSwingAngle( relQ ); +} + +void b3SphericalJoint_EnableTwistLimit( b3JointId jointId, bool enableLimit ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + if ( enableLimit != base->sphericalJoint.enableTwistLimit ) + { + base->sphericalJoint.lowerTwistImpulse = 0.0f; + base->sphericalJoint.upperTwistImpulse = 0.0f; + } + base->sphericalJoint.enableTwistLimit = enableLimit; +} + +bool b3SphericalJoint_IsTwistLimitEnabled( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.enableTwistLimit; +} + +float b3SphericalJoint_GetLowerTwistLimit( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.lowerTwistAngle; +} + +float b3SphericalJoint_GetUpperTwistLimit( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.upperTwistAngle; +} + +void b3SphericalJoint_SetTwistLimits( b3JointId jointId, float lowerLimitRadians, float upperLimitRadians ) +{ + B3_ASSERT( b3IsValidFloat( lowerLimitRadians ) && b3IsValidFloat( upperLimitRadians ) ); + float lowerTwistAngle = b3MinFloat( lowerLimitRadians, upperLimitRadians ); + float upperTwistAngle = b3MaxFloat( lowerLimitRadians, upperLimitRadians ); + lowerTwistAngle = b3ClampFloat( lowerTwistAngle, -B3_PI, B3_PI ); + upperTwistAngle = b3ClampFloat( upperTwistAngle, -B3_PI, B3_PI ); + + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + base->sphericalJoint.lowerTwistAngle = lowerTwistAngle; + base->sphericalJoint.upperTwistAngle = upperTwistAngle; +} + +float b3SphericalJoint_GetTwistAngle( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + b3Transform transformA = b3GetBodyTransform( world, base->bodyIdA ); + b3Transform transformB = b3GetBodyTransform( world, base->bodyIdB ); + + b3Quat quatA = b3MulQuat( transformA.q, base->localFrameA.q ); + b3Quat quatB = b3MulQuat( transformB.q, base->localFrameB.q ); + + if ( b3DotQuat( quatA, quatB ) < 0.0f ) + { + // this keeps the twist angle in the range [-pi, pi] + quatB = b3NegateQuat( quatB ); + } + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + return b3GetTwistAngle( relQ ); +} + +void b3SphericalJoint_EnableSpring( b3JointId jointId, bool enableSpring ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + if ( enableSpring != base->sphericalJoint.enableSpring ) + { + base->sphericalJoint.springImpulse = b3Vec3_zero; + } + base->sphericalJoint.enableSpring = enableSpring; +} + +bool b3SphericalJoint_IsSpringEnabled( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.enableSpring; +} + +void b3SphericalJoint_SetTargetRotation( b3JointId jointId, b3Quat targetRotation ) +{ + B3_ASSERT( b3IsValidQuat( targetRotation ) ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + base->sphericalJoint.targetRotation = targetRotation; +} + +b3Quat b3SphericalJoint_GetTargetRotation( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.targetRotation; +} + +void b3SphericalJoint_SetSpringHertz( b3JointId jointId, float hertz ) +{ + B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + base->sphericalJoint.hertz = hertz; +} + +float b3SphericalJoint_GetSpringHertz( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.hertz; +} + +void b3SphericalJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio ) +{ + B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + base->sphericalJoint.dampingRatio = dampingRatio; +} + +float b3SphericalJoint_GetSpringDampingRatio( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.dampingRatio; +} + +void b3SphericalJoint_EnableMotor( b3JointId jointId, bool enableMotor ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + if ( enableMotor != base->sphericalJoint.enableMotor ) + { + base->sphericalJoint.motorImpulse = b3Vec3_zero; + } + base->sphericalJoint.enableMotor = enableMotor; +} + +bool b3SphericalJoint_IsMotorEnabled( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.enableMotor; +} + +void b3SphericalJoint_SetMotorVelocity( b3JointId jointId, b3Vec3 motorVelocity ) +{ + B3_ASSERT( b3IsValidVec3( motorVelocity ) ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + base->sphericalJoint.motorVelocity = motorVelocity; +} + +b3Vec3 b3SphericalJoint_GetMotorVelocity( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.motorVelocity; +} + +void b3SphericalJoint_SetMaxMotorTorque( b3JointId jointId, float maxForce ) +{ + B3_ASSERT( b3IsValidFloat( maxForce ) && maxForce >= 0.0f ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + base->sphericalJoint.maxMotorTorque = maxForce; +} + +float b3SphericalJoint_GetMaxMotorTorque( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.maxMotorTorque; +} + +b3Vec3 b3GetSphericalJointForce( b3World* world, b3JointSim* base ) +{ + b3Vec3 force = b3MulSV( world->inv_h, base->sphericalJoint.linearImpulse ); + return force; +} + +b3Vec3 b3GetSphericalJointTorque( b3World* world, b3JointSim* base ) +{ + b3Transform xfA = b3GetBodyTransform( world, base->bodyIdA ); + b3Transform xfB = b3GetBodyTransform( world, base->bodyIdB ); + b3Quat qA = b3MulQuat( xfA.q, base->localFrameA.q ); + b3Quat qB = b3MulQuat( xfB.q, base->localFrameB.q ); + + // Cone axis is the z-axis of body A. + b3Vec3 coneAxis = b3RotateVector( qA, b3Vec3_axisZ ); + b3Vec3 twistAxis = b3RotateVector( qB, b3Vec3_axisZ ); + b3Vec3 swingAxis = b3Normalize( b3Cross( coneAxis, twistAxis ) ); + + b3SphericalJoint* joint = &base->sphericalJoint; + b3Vec3 impulse = b3Add( joint->springImpulse, joint->motorImpulse ); + impulse = b3MulAdd( impulse, joint->lowerTwistImpulse - joint->upperTwistImpulse, twistAxis ); + impulse = b3MulAdd( impulse, joint->swingImpulse, swingAxis ); + b3Vec3 torque = b3MulSV( world->inv_h, impulse ); + return torque; +} + +// Point-to-point constraint +// C = p2 - p1 +// Cdot = v2 - v1 +// = v2 + cross(w2, r2) - v1 - cross(w1, r1) +// J = [-I r1_skew I -r2_skew ] +// K = J * invM * transpose(J) +// transpose(skew(r)) = -skew(r) +// K = diag(1/m1 + 1/m2) - r1_skew * invI1 * r1_skew - r2_skew * invI2 * r2_skew + +// r_skew = R * skew(r_local) * RT +// invI = R * invI_local * RT +// r_skew * invI * r_skew = R * skew(r_local) * RT * R * invI_local * RT * R * r_skew * RT +// = R * ( skew(r_local) * invI_local * skew(r_local) ) * RT + +void b3PrepareSphericalJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_sphericalJoint ); + + b3World* world = context->world; + + b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB ); + + B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + base->invMassA = bodySimA->invMass; + base->invMassB = bodySimB->invMass; + base->invIA = bodySimA->invInertiaWorld; + base->invIB = bodySimB->invInertiaWorld; + + b3Matrix3 invInertiaSum = b3AddMM( base->invIA, base->invIB ); + base->fixedRotation = b3Det( invInertiaSum ) < 1000.0f * FLT_MIN; + + b3SphericalJoint* joint = &base->sphericalJoint; + joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX; + joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX; + + // Compute joint anchor frames with world space rotation, relative to center of mass + joint->frameA.q = b3MulQuat( bodySimA->transform.q, base->localFrameA.q ); + joint->frameA.p = b3RotateVector( bodySimA->transform.q, b3Sub( base->localFrameA.p, bodySimA->localCenter ) ); + joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + joint->frameB.p = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) ); + + joint->deltaCenter = b3Sub( bodySimB->center, bodySimA->center ); + + // Cone axis is the z-axis of body A. + b3Vec3 coneAxis = b3RotateVector( joint->frameA.q, b3Vec3_axisZ ); + + // Twist axis is the z-axis of body B. + b3Vec3 twistAxis = b3RotateVector( joint->frameB.q, b3Vec3_axisZ ); + + if ( joint->enableConeLimit ) + { + // Swing axis may be zero + b3Vec3 swingAxis = b3Normalize( b3Cross( coneAxis, twistAxis ) ); + float k = b3Dot( swingAxis, b3MulMV( invInertiaSum, swingAxis ) ); + joint->swingMass = k > 0.0f ? 1.0f / k : 0.0f; + joint->swingAxis = swingAxis; + } + + if ( joint->enableTwistLimit ) + { + b3Quat relQ = b3InvMulQuat( joint->frameA.q, joint->frameB.q ); + float tanThetaOver2 = sqrtf( ( relQ.v.x * relQ.v.x + relQ.v.y * relQ.v.y ) / ( relQ.v.z * relQ.v.z + relQ.s * relQ.s ) ); + + // todo verify this Jacobian using a finite difference, unit test? + b3Vec3 swingAxis = b3Normalize( b3Cross( coneAxis, twistAxis ) ); + b3Vec3 perpAxis = b3Cross( swingAxis, coneAxis ); + b3Vec3 twistJacobian = b3MulAdd( coneAxis, tanThetaOver2, perpAxis ); + float k = b3Dot( twistJacobian, b3MulMV( invInertiaSum, twistJacobian ) ); + joint->twistMass = k > 0.0f ? 1.0f / k : 0.0f; + joint->twistJacobian = twistJacobian; + } + + if ( base->fixedRotation == false ) + { + joint->rotationMass = b3InvertMatrix( invInertiaSum ); + } + else + { + joint->rotationMass = b3Mat3_zero; + } + + joint->springSoftness = b3MakeSoft( joint->hertz, joint->dampingRatio, context->h ); + + if ( context->enableWarmStarting == false ) + { + joint->linearImpulse = b3Vec3_zero; + joint->motorImpulse = b3Vec3_zero; + joint->springImpulse = b3Vec3_zero; + joint->swingImpulse = 0.0f; + joint->lowerTwistImpulse = 0.0f; + joint->upperTwistImpulse = 0.0f; + } +} + +void b3WarmStartSphericalJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_sphericalJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3SphericalJoint* joint = &base->sphericalJoint; + + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + b3Vec3 angularImpulse = b3Add( joint->springImpulse, joint->motorImpulse ); + angularImpulse = b3MulSub( angularImpulse, joint->swingImpulse, joint->swingAxis ); + angularImpulse = b3MulAdd( angularImpulse, joint->lowerTwistImpulse - joint->upperTwistImpulse, joint->twistJacobian ); + + vA = b3MulSub( vA, mA, joint->linearImpulse ); + wA = b3Sub( wA, b3MulMV( iA, b3Add( b3Cross( rA, joint->linearImpulse ), angularImpulse ) ) ); + + vB = b3MulAdd( vB, mB, joint->linearImpulse ); + wB = b3Add( wB, b3MulMV( iB, b3Add( b3Cross( rB, joint->linearImpulse ), angularImpulse ) ) ); + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +void b3SolveSphericalJoint( b3JointSim* base, b3StepContext* context, bool useBias ) +{ + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3SphericalJoint* joint = &base->sphericalJoint; + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + bool fixedRotation = base->fixedRotation; + b3Quat quatA = b3MulQuat( stateA->deltaRotation, joint->frameA.q ); + b3Quat quatB = b3MulQuat( stateB->deltaRotation, joint->frameB.q ); + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + // Solve spring + if ( joint->enableSpring && fixedRotation == false ) + { + // Rotation constraint error + b3Vec3 deltaRotation = b3DeltaQuatToRotation( relQ, joint->targetRotation ); + b3Vec3 c = b3Neg( b3RotateVector( quatA, deltaRotation ) ); + + b3Vec3 bias = b3MulSV( joint->springSoftness.biasRate, c ); + float massScale = joint->springSoftness.massScale; + float impulseScale = joint->springSoftness.impulseScale; + b3Vec3 cdot = b3Sub( wB, wA ); + + b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b3MulMV( joint->rotationMass, b3Add( cdot, bias ) ) ), + impulseScale, joint->springImpulse ); + joint->springImpulse = b3Add( joint->springImpulse, impulse ); + + wA = b3Sub( wA, b3MulMV( iA, impulse ) ); + wB = b3Add( wB, b3MulMV( iB, impulse ) ); + } + + if ( joint->enableMotor && fixedRotation == false ) + { + b3Vec3 cdot = b3Sub( wB, wA ); + + b3Vec3 lambda = b3Neg( b3MulMV( joint->rotationMass, b3Sub( cdot, joint->motorVelocity ) ) ); + b3Vec3 newImpulse = b3Add( joint->motorImpulse, lambda ); + float length = b3Length( newImpulse ); + float maxImpulse = joint->maxMotorTorque * context->h; + if ( length > maxImpulse ) + { + newImpulse = b3MulSV( maxImpulse / length, newImpulse ); + } + + lambda = b3Sub( newImpulse, joint->motorImpulse ); + joint->motorImpulse = newImpulse; + + wA = b3Sub( wA, b3MulMV( iA, lambda ) ); + wB = b3Add( wB, b3MulMV( iB, lambda ) ); + } + + if ( joint->enableTwistLimit && fixedRotation == false ) + { + float twistAngle = b3GetTwistAngle( relQ ); + + // todo does an updated twist axis help? + + b3Vec3 twistJacobian = joint->twistJacobian; + + // Lower limit + { + float c = twistAngle - joint->lowerTwistAngle; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + float cdot = b3Dot( b3Sub( wB, wA ), twistJacobian ); + float oldImpulse = joint->lowerTwistImpulse; + float deltaImpulse = -massScale * joint->twistMass * ( cdot + bias ) - impulseScale * oldImpulse; + joint->lowerTwistImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f ); + deltaImpulse = joint->lowerTwistImpulse - oldImpulse; + + wA = b3MulSub( wA, deltaImpulse, b3MulMV( iA, twistJacobian ) ); + wB = b3MulAdd( wB, deltaImpulse, b3MulMV( iB, twistJacobian ) ); + } + + // Upper limit + { + float c = joint->upperTwistAngle - twistAngle; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + // sign flipped on Cdot + float cdot = b3Dot( b3Sub( wA, wB ), twistJacobian ); + float oldImpulse = joint->upperTwistImpulse; + float deltaImpulse = -massScale * joint->twistMass * ( cdot + bias ) - impulseScale * oldImpulse; + joint->upperTwistImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f ); + deltaImpulse = joint->upperTwistImpulse - oldImpulse; + + // sign flipped on applied impulse + wA = b3MulAdd( wA, deltaImpulse, b3MulMV( iA, twistJacobian ) ); + wB = b3MulSub( wB, deltaImpulse, b3MulMV( iB, twistJacobian ) ); + } + } + + if ( joint->enableConeLimit && fixedRotation == false ) + { + float swingAngle = b3GetSwingAngle( relQ ); + + // todo does an updated swing axis help? + // b3Vec3 axisA = b3RotateVector( quatA, b3Vec3_axisZ ); + // b3Vec3 axisB = b3RotateVector( quatB, b3Vec3_axisZ ); + // b3Vec3 swingAxis = b3Normalize( b3Cross( axisA, axisB ) ); + // joint->swingAxis = swingAxis; + + b3Vec3 swingAxis = joint->swingAxis; + + float c = joint->coneAngle - swingAngle; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + // sign flipped on Cdot + float cdot = b3Dot( b3Sub( wA, wB ), swingAxis ); + float oldImpulse = joint->swingImpulse; + float deltaImpulse = -massScale * joint->swingMass * ( cdot + bias ) - impulseScale * oldImpulse; + joint->swingImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f ); + deltaImpulse = joint->swingImpulse - oldImpulse; + + // sign flipped on applied impulse + wA = b3MulAdd( wA, deltaImpulse, b3MulMV( iA, swingAxis ) ); + wB = b3MulSub( wB, deltaImpulse, b3MulMV( iB, swingAxis ) ); + } + + // Solve point-to-point constraint + { + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + b3Vec3 cdot = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, rA ) ); + + b3Vec3 bias = b3Vec3_zero; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias ) + { + b3Vec3 dcA = stateA->deltaPosition; + b3Vec3 dcB = stateB->deltaPosition; + + b3Vec3 separation = b3Add( b3Sub( dcB, dcA ), b3Sub( rB, rA ) ); + separation = b3Add( separation, joint->deltaCenter ); + + bias = b3MulSV( base->constraintSoftness.biasRate, separation ); + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + //// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] + b3Matrix3 sA = b3Skew( rA ); + b3Matrix3 sB = b3Skew( rB ); + b3Matrix3 kA = b3MulMM( sA, b3MulMM( base->invIA, sA ) ); + b3Matrix3 kB = b3MulMM( sB, b3MulMM( base->invIB, sB ) ); + b3Matrix3 k = b3NegateMat3( b3AddMM( kA, kB ) ); + k.cx.x += mA + mB; + k.cy.y += mA + mB; + k.cz.z += mA + mB; + + b3Vec3 b = b3Solve3( k, b3Add( cdot, bias ) ); + + b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b ), impulseScale, joint->linearImpulse ); + joint->linearImpulse = b3Add( joint->linearImpulse, impulse ); + + vA = b3MulSub( vA, mA, impulse ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) ); + vB = b3MulAdd( vB, mB, impulse ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) ); + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +void b3DrawSphericalJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale ) +{ + b3Transform frameA = b3MulTransforms( transformA, base->localFrameA ); + + float length1 = 0.1f * scale; + draw->DrawSegmentFcn( frameA.p, b3MulAdd( frameA.p, length1, b3RotateVector( frameA.q, b3Vec3_axisX ) ), b3_colorRed, + draw->context ); + draw->DrawSegmentFcn( frameA.p, b3MulAdd( frameA.p, length1, b3RotateVector( frameA.q, b3Vec3_axisY ) ), b3_colorGreen, + draw->context ); + draw->DrawSegmentFcn( frameA.p, b3MulAdd( frameA.p, length1, b3RotateVector( frameA.q, b3Vec3_axisZ ) ), b3_colorBlue, + draw->context ); + + b3Transform frameB = b3MulTransforms( transformB, base->localFrameB ); + + float length2 = 0.2f * scale; + draw->DrawSegmentFcn( frameB.p, b3MulAdd( frameB.p, length2, b3RotateVector( frameB.q, b3Vec3_axisZ ) ), b3_colorOrange, + draw->context ); + + b3SphericalJoint* joint = &base->sphericalJoint; + enum { kSliceCount = 16 }; + + // Twist limit + if ( joint->enableTwistLimit ) + { + b3Quat quatA = frameA.q; + b3Quat quatB = frameB.q; + + if ( b3DotQuat( quatA, quatB ) < 0.0f ) + { + // this keeps the twist angle in the range [-pi, pi] + quatB = b3NegateQuat( quatB ); + } + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + const float wedgeRadius = 0.1f * scale; + for ( int index = 0; index < kSliceCount; ++index ) + { + float t1 = (float)( index + 0 ) / kSliceCount; + float alpha1 = ( 1.0f - t1 ) * joint->lowerTwistAngle + t1 * joint->upperTwistAngle; + float t2 = (float)( index + 1 ) / kSliceCount; + float alpha2 = ( 1.0f - t2 ) * joint->lowerTwistAngle + t2 * joint->upperTwistAngle; + + b3Vec3 vertex1 = { wedgeRadius * b3Cos( alpha1 ), wedgeRadius * b3Sin( alpha1 ), 0.0f }; + b3Vec3 vertex2 = { wedgeRadius * b3Cos( alpha2 ), wedgeRadius * b3Sin( alpha2 ), 0.0f }; + + if ( index == 0 ) + { + draw->DrawSegmentFcn( frameA.p, b3TransformPoint( frameA, vertex1 ), b3_colorCyan, draw->context ); + } + + if ( index == kSliceCount - 1 ) + { + draw->DrawSegmentFcn( b3TransformPoint( frameA, vertex2 ), frameA.p, b3_colorCyan, draw->context ); + } + draw->DrawSegmentFcn( b3TransformPoint( frameA, vertex1 ), b3TransformPoint( frameA, vertex2 ), b3_colorCyan, + draw->context ); + } + + float twistAngle = b3GetTwistAngle( relQ ); + b3Vec3 p2 = { wedgeRadius * b3Cos( twistAngle ), wedgeRadius * b3Sin( twistAngle ), 0.0f }; + draw->DrawSegmentFcn( frameA.p, b3TransformPoint( frameA, p2 ), b3_colorYellow, draw->context ); + } + + // Swing limit + if ( joint->enableConeLimit ) + { + const float radius = 0.1f * scale; + float coneRadius = radius * b3Sin( joint->coneAngle ); + float coneHeight = radius * b3Cos( joint->coneAngle ); + + for ( int index = 0; index < kSliceCount; ++index ) + { + float phi1 = 2.0f * ( index + 0 ) / kSliceCount * B3_PI; + float phi2 = 2.0f * ( index + 1 ) / kSliceCount * B3_PI; + + b3Vec3 vertex1 = { coneRadius * b3Cos( phi1 ), coneRadius * b3Sin( phi1 ), coneHeight }; + b3Vec3 vertex2 = { coneRadius * b3Cos( phi2 ), coneRadius * b3Sin( phi2 ), coneHeight }; + + draw->DrawSegmentFcn( frameA.p, b3TransformPoint( frameA, vertex1 ), b3_colorCyan, draw->context ); + draw->DrawSegmentFcn( b3TransformPoint( frameA, vertex1 ), b3TransformPoint( frameA, vertex2 ), b3_colorCyan, + draw->context ); + } + } +} diff --git a/src/table.c b/src/table.c new file mode 100644 index 0000000..fb066b8 --- /dev/null +++ b/src/table.c @@ -0,0 +1,248 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "table.h" + +#include "bitset.h" +#include "core.h" +#include "ctz.h" +#include "platform.h" + +#include + +#if B3_DEBUG +b3AtomicInt b3_probeCount; +#endif + +_Static_assert( 2 * B3_SHAPE_POWER + B3_CHILD_POWER == 64, "compound power" ); +_Static_assert( B3_CHILD_POWER > 8, "compound child power" ); + +b3HashSet b3CreateSet( int32_t capacity ) +{ + b3HashSet set = { 0 }; + + // Capacity must be a power of 2 + if ( capacity > 16 ) + { + set.capacity = b3RoundUpPowerOf2( capacity ); + } + else + { + set.capacity = 16; + } + + set.count = 0; + set.items = (b3SetItem*)b3Alloc( set.capacity * sizeof( b3SetItem ) ); + memset( set.items, 0, set.capacity * sizeof( b3SetItem ) ); + + return set; +} + +void b3DestroySet( b3HashSet* set ) +{ + b3Free( set->items, set->capacity * sizeof( b3SetItem ) ); + set->items = NULL; + set->count = 0; + set->capacity = 0; +} + +void b3ClearSet( b3HashSet* set ) +{ + set->count = 0; + memset( set->items, 0, set->capacity * sizeof( b3SetItem ) ); +} + +// I need a good hash because the keys are built from pairs of increasing integers. +// A simple hash like hash = (integer1 XOR integer2) has many collisions. +// https://lemire.me/blog/2018/08/15/fast-strongly-universal-64-bit-hashing-everywhere/ +// https://preshing.com/20130107/this-hash-set-is-faster-than-a-judy-array/ +// todo try: https://www.jandrewrogers.com/2019/02/12/fast-perfect-hashing/ +// todo try: +// https://probablydance.com/2018/06/16/fibonacci-hashing-the-optimization-that-the-world-forgot-or-a-better-alternative-to-integer-modulo/ +static inline uint32_t b3KeyHash( uint64_t key ) +{ + uint64_t h = key; + h ^= h >> 33; + h *= 0xff51afd7ed558ccdL; + h ^= h >> 33; + h *= 0xc4ceb9fe1a85ec53L; + h ^= h >> 33; + + return (uint32_t)h; +} + +static int32_t b3FindSlot( const b3HashSet* set, uint64_t key, uint32_t hash ) +{ + uint32_t capacity = set->capacity; + int32_t index = hash & ( capacity - 1 ); + const b3SetItem* items = set->items; + while ( items[index].hash != 0 && items[index].key != key ) + { +#if B3_DEBUG + b3AtomicFetchAddInt( &b3_probeCount, 1 ); +#endif + index = ( index + 1 ) & ( capacity - 1 ); + } + + return index; +} + +static void b3AddKeyHaveCapacity( b3HashSet* set, uint64_t key, uint32_t hash ) +{ + int32_t index = b3FindSlot( set, key, hash ); + b3SetItem* items = set->items; + B3_ASSERT( items[index].hash == 0 ); + + items[index].key = key; + items[index].hash = hash; + set->count += 1; +} + +static void b3GrowTable( b3HashSet* set ) +{ + uint32_t oldCount = set->count; + B3_UNUSED( oldCount ); + + uint32_t oldCapacity = set->capacity; + b3SetItem* oldItems = set->items; + + set->count = 0; + // Capacity must be a power of 2 + set->capacity = 2 * oldCapacity; + set->items = (b3SetItem*)b3Alloc( set->capacity * sizeof( b3SetItem ) ); + memset( set->items, 0, set->capacity * sizeof( b3SetItem ) ); + + // Transfer items into new array + for ( uint32_t i = 0; i < oldCapacity; ++i ) + { + b3SetItem* item = oldItems + i; + if ( item->hash == 0 ) + { + // this item was empty + continue; + } + + b3AddKeyHaveCapacity( set, item->key, item->hash ); + } + + B3_ASSERT( set->count == oldCount ); + + b3Free( oldItems, oldCapacity * sizeof( b3SetItem ) ); +} + +bool b3ContainsKey( const b3HashSet* set, uint64_t key ) +{ + // key of zero is a sentinel + B3_ASSERT( key != 0 ); + uint32_t hash = b3KeyHash( key ); + int32_t index = b3FindSlot( set, key, hash ); + return set->items[index].key == key; +} + +int b3GetHashSetBytes( b3HashSet* set ) +{ + return set->capacity * (int)sizeof( b3SetItem ); +} + +bool b3AddKey( b3HashSet* set, uint64_t key ) +{ + // key of zero is a sentinel + B3_ASSERT( key != 0 ); + + uint32_t hash = b3KeyHash( key ); + B3_ASSERT( hash != 0 ); + + int32_t index = b3FindSlot( set, key, hash ); + if ( set->items[index].hash != 0 ) + { + // Already in set + B3_ASSERT( set->items[index].hash == hash && set->items[index].key == key ); + return true; + } + + if ( 2 * set->count >= set->capacity ) + { + b3GrowTable( set ); + } + + b3AddKeyHaveCapacity( set, key, hash ); + return false; +} + +// See https://en.wikipedia.org/wiki/Open_addressing +bool b3RemoveKey( b3HashSet* set, uint64_t key ) +{ + uint32_t hash = b3KeyHash( key ); + int32_t i = b3FindSlot( set, key, hash ); + b3SetItem* items = set->items; + if ( items[i].hash == 0 ) + { + // Not in set + return false; + } + + // Mark item i as unoccupied + items[i].key = 0; + items[i].hash = 0; + + B3_ASSERT( set->count > 0 ); + set->count -= 1; + + // Attempt to fill item i + int32_t j = i; + uint32_t capacity = set->capacity; + for ( ;; ) + { + j = ( j + 1 ) & ( capacity - 1 ); + if ( items[j].hash == 0 ) + { + break; + } + + // k is the first item for the hash of j + int32_t k = items[j].hash & ( capacity - 1 ); + + // determine if k lies cyclically in (i,j] + // i <= j: | i..k..j | + // i > j: |.k..j i....| or |....j i..k.| + if ( i <= j ) + { + if ( i < k && k <= j ) + { + continue; + } + } + else + { + if ( i < k || k <= j ) + { + continue; + } + } + + // Move j into i + items[i] = items[j]; + + // Mark item j as unoccupied + items[j].key = 0; + items[j].hash = 0; + + i = j; + } + + return true; +} + +// This function is here because ctz.h is included by +// this file but not in bitset.c +int b3CountSetBits( b3BitSet* bitSet ) +{ + int popCount = 0; + uint32_t blockCount = bitSet->blockCount; + for ( uint32_t i = 0; i < blockCount; ++i ) + { + popCount += b3PopCount64( bitSet->bits[i] ); + } + + return popCount; +} diff --git a/src/table.h b/src/table.h new file mode 100644 index 0000000..2e54772 --- /dev/null +++ b/src/table.h @@ -0,0 +1,62 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box3d/constants.h" + +#include +#include + +typedef struct b3SetItem +{ + uint64_t key; + uint32_t hash; +} b3SetItem; + +typedef struct b3HashSet +{ + b3SetItem* items; + uint32_t capacity; + uint32_t count; +} b3HashSet; + +#define B3_SHAPE_MASK ( B3_MAX_SHAPES - 1 ) +#define B3_CHILD_MASK ( B3_MAX_CHILD_SHAPES - 1 ) + +static inline uint64_t b3ShapePairKey( int s1, int s2, int c ) +{ + if (s1 < s2) + { + return ( (uint64_t)( B3_SHAPE_MASK & s1 ) << ( 64 - B3_SHAPE_POWER ) ) | + ( (uint64_t)( B3_SHAPE_MASK & s2 ) << ( 64 - 2 * B3_SHAPE_POWER ) ) | ( (uint64_t)( B3_CHILD_MASK & c ) ); + } + + return ( (uint64_t)( B3_SHAPE_MASK & s2 ) << ( 64 - B3_SHAPE_POWER ) ) | + ( (uint64_t)( B3_SHAPE_MASK & s1 ) << ( 64 - 2 * B3_SHAPE_POWER ) ) | + ( (uint64_t)( B3_CHILD_MASK & c ) ); +} + +#ifdef __cplusplus +extern "C" +{ +#endif + +b3HashSet b3CreateSet( int32_t capacity ); +void b3DestroySet( b3HashSet* set ); + +void b3ClearSet( b3HashSet* set ); + +// Returns true if key was already in set +bool b3AddKey( b3HashSet* set, uint64_t key ); + +// Returns true if the key was found +bool b3RemoveKey( b3HashSet* set, uint64_t key ); + +bool b3ContainsKey( const b3HashSet* set, uint64_t key ); + +int b3GetHashSetBytes( b3HashSet* set ); + +#ifdef __cplusplus +} +#endif diff --git a/src/timer.c b/src/timer.c new file mode 100644 index 0000000..3f02bf1 --- /dev/null +++ b/src/timer.c @@ -0,0 +1,671 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +// Required on Linux to expose pthread_setname_np. Must be defined before any +// system header is included. +#if defined( __linux__ ) && !defined( _GNU_SOURCE ) +#define _GNU_SOURCE +#endif + +#include "core.h" + +#include "box3d/base.h" + +#include +#include +#include + +#if defined( _WIN32 ) + +#ifndef WIN32_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN 1 +#endif + +#include +#include + +static double s_invFrequency = 0.0; + +uint64_t b3GetTicks( void ) +{ + LARGE_INTEGER counter; + QueryPerformanceCounter( &counter ); + return (uint64_t)counter.QuadPart; +} + +float b3GetMilliseconds( uint64_t ticks ) +{ + if ( s_invFrequency == 0.0 ) + { + LARGE_INTEGER frequency; + QueryPerformanceFrequency( &frequency ); + + s_invFrequency = (double)frequency.QuadPart; + if ( s_invFrequency > 0.0 ) + { + s_invFrequency = 1000.0 / s_invFrequency; + } + } + + uint64_t ticksNow = b3GetTicks(); + return (float)( s_invFrequency * ( ticksNow - ticks ) ); +} + +float b3GetMillisecondsAndReset( uint64_t* ticks ) +{ + if ( s_invFrequency == 0.0 ) + { + LARGE_INTEGER frequency; + QueryPerformanceFrequency( &frequency ); + + s_invFrequency = (double)frequency.QuadPart; + if ( s_invFrequency > 0.0 ) + { + s_invFrequency = 1000.0 / s_invFrequency; + } + } + + uint64_t ticksNow = b3GetTicks(); + float ms = (float)( s_invFrequency * ( ticksNow - *ticks ) ); + *ticks = ticksNow; + return ms; +} + +void b3Yield( void ) +{ + SwitchToThread(); +} + +typedef struct b3Mutex +{ + CRITICAL_SECTION cs; +} b3Mutex; + +b3Mutex* b3CreateMutex( void ) +{ + b3Mutex* m = b3Alloc( sizeof( b3Mutex ) ); + InitializeCriticalSection( &m->cs ); + return m; +} + +void b3DestroyMutex( b3Mutex* m ) +{ + DeleteCriticalSection( &m->cs ); + *m = (b3Mutex){ 0 }; + b3Free( m, sizeof( b3Mutex ) ); +} + +void b3LockMutex( b3Mutex* m ) +{ + EnterCriticalSection( &m->cs ); +} + +void b3UnlockMutex( b3Mutex* m ) +{ + LeaveCriticalSection( &m->cs ); +} + +typedef struct b3Semaphore +{ + HANDLE semaphore; +} b3Semaphore; + +b3Semaphore* b3CreateSemaphore( int initCount ) +{ + b3Semaphore* s = b3Alloc( sizeof( b3Semaphore ) ); + s->semaphore = CreateSemaphoreExW( NULL, initCount, INT_MAX, NULL, 0, SEMAPHORE_ALL_ACCESS ); + return s; +} + +void b3DestroySemaphore( b3Semaphore* s ) +{ + CloseHandle( s->semaphore ); + *s = (b3Semaphore){ 0 }; + b3Free( s, sizeof( b3Semaphore ) ); +} + +void b3WaitSemaphore( b3Semaphore* s ) +{ + WaitForSingleObjectEx( s->semaphore, INFINITE, FALSE ); +} + +void b3SignalSemaphore( b3Semaphore* s ) +{ + ReleaseSemaphore( s->semaphore, 1, NULL ); +} + +typedef struct b3Thread +{ + HANDLE thread; + b3ThreadFunction* function; + void* context; + char name[32]; +} b3Thread; + +typedef HRESULT( WINAPI* b3SetThreadDescriptionFn )( HANDLE, PCWSTR ); + +// SetThreadDescription exists on Windows 10 1607+. Resolve it dynamically so +// older Windows versions still link. Resolved once, cached for subsequent calls. +static void b3SetCurrentThreadName( const char* name ) +{ + if ( name == NULL || name[0] == 0 ) + { + return; + } + + static b3SetThreadDescriptionFn pfn = NULL; + static int resolved = 0; + + if ( resolved == 0 ) + { + HMODULE kernel = GetModuleHandleW( L"kernel32.dll" ); + if ( kernel != NULL ) + { + // MSVC /Wall warns C4191 on every FARPROC function-pointer cast. + // This is the intended use of GetProcAddress, so suppress locally. +#pragma warning( push ) +#pragma warning( disable : 4191 ) + pfn = (b3SetThreadDescriptionFn)GetProcAddress( kernel, "SetThreadDescription" ); +#pragma warning( pop ) + } + resolved = 1; + } + + if ( pfn == NULL ) + { + return; + } + + wchar_t wide[32]; + int n = MultiByteToWideChar( CP_UTF8, 0, name, -1, wide, (int)( sizeof( wide ) / sizeof( wide[0] ) ) ); + if ( n > 0 ) + { + pfn( GetCurrentThread(), wide ); + } +} + +static DWORD WINAPI b3ThreadStart( LPVOID param ) +{ + b3Thread* t = (b3Thread*)param; + b3SetCurrentThreadName( t->name ); + t->function( t->context ); + return 0; +} + +b3Thread* b3CreateThread( b3ThreadFunction* function, void* context, const char* name ) +{ + b3Thread* t = b3Alloc( sizeof( b3Thread ) ); + t->function = function; + t->context = context; + if ( name != NULL ) + { + snprintf( t->name, sizeof( t->name ), "%s", name ); + } + else + { + t->name[0] = 0; + } + t->thread = CreateThread( NULL, 0, b3ThreadStart, t, 0, NULL ); + return t; +} + +void b3JoinThread( b3Thread* t ) +{ + WaitForSingleObject( t->thread, INFINITE ); + CloseHandle( t->thread ); + *t = (b3Thread){ 0 }; + b3Free( t, sizeof( b3Thread ) ); +} + +#elif defined( __linux__ ) || defined( __EMSCRIPTEN__ ) + +#include +#include + +uint64_t b3GetTicks( void ) +{ + struct timespec ts; + clock_gettime( CLOCK_MONOTONIC, &ts ); + return ts.tv_sec * 1000000000LL + ts.tv_nsec; +} + +float b3GetMilliseconds( uint64_t ticks ) +{ + uint64_t ticksNow = b3GetTicks(); + return (float)( ( ticksNow - ticks ) / 1000000.0 ); +} + +float b3GetMillisecondsAndReset( uint64_t* ticks ) +{ + uint64_t ticksNow = b3GetTicks(); + float ms = (float)( ( ticksNow - *ticks ) / 1000000.0 ); + *ticks = ticksNow; + return ms; +} + +void b3Yield( void ) +{ + sched_yield(); +} + +#include +typedef struct b3Mutex +{ + pthread_mutex_t mtx; +} b3Mutex; + +b3Mutex* b3CreateMutex( void ) +{ + b3Mutex* m = b3Alloc( sizeof( b3Mutex ) ); + pthread_mutex_init( &m->mtx, NULL ); + return m; +} + +void b3DestroyMutex( b3Mutex* m ) +{ + pthread_mutex_destroy( &m->mtx ); + *m = (b3Mutex){ 0 }; + b3Free( m, sizeof( b3Mutex ) ); +} + +void b3LockMutex( b3Mutex* m ) +{ + pthread_mutex_lock( &m->mtx ); +} + +void b3UnlockMutex( b3Mutex* m ) +{ + pthread_mutex_unlock( &m->mtx ); +} + +#include + +typedef struct b3Semaphore +{ + sem_t semaphore; +} b3Semaphore; + +b3Semaphore* b3CreateSemaphore( int initCount ) +{ + b3Semaphore* s = b3Alloc( sizeof( b3Semaphore ) ); + sem_init( &s->semaphore, 0, (unsigned int)initCount ); + return s; +} + +void b3DestroySemaphore( b3Semaphore* s ) +{ + sem_destroy( &s->semaphore ); + *s = (b3Semaphore){ 0 }; + b3Free( s, sizeof( b3Semaphore ) ); +} + +void b3WaitSemaphore( b3Semaphore* s ) +{ + sem_wait( &s->semaphore ); +} + +void b3SignalSemaphore( b3Semaphore* s ) +{ + sem_post( &s->semaphore ); +} + +typedef struct b3Thread +{ + pthread_t thread; + b3ThreadFunction* function; + void* context; + char name[32]; +} b3Thread; + +static void b3SetCurrentThreadName( const char* name ) +{ + if ( name == NULL || name[0] == 0 ) + { + return; + } + +#if defined( __linux__ ) + // Linux caps thread names at 15 chars + null terminator. + char truncated[16]; + snprintf( truncated, sizeof( truncated ), "%s", name ); + pthread_setname_np( pthread_self(), truncated ); +#else + (void)name; +#endif +} + +static void* b3ThreadStart( void* param ) +{ + b3Thread* t = (b3Thread*)param; + b3SetCurrentThreadName( t->name ); + t->function( t->context ); + return NULL; +} + +b3Thread* b3CreateThread( b3ThreadFunction* function, void* context, const char* name ) +{ + b3Thread* t = b3Alloc( sizeof( b3Thread ) ); + t->function = function; + t->context = context; + if ( name != NULL ) + { + snprintf( t->name, sizeof( t->name ), "%s", name ); + } + else + { + t->name[0] = 0; + } + pthread_create( &t->thread, NULL, b3ThreadStart, t ); + return t; +} + +void b3JoinThread( b3Thread* t ) +{ + pthread_join( t->thread, NULL ); + *t = (b3Thread){ 0 }; + b3Free( t, sizeof( b3Thread ) ); +} + +#elif defined( __APPLE__ ) + +#include +#include +#include + +static double s_invFrequency = 0.0; + +uint64_t b3GetTicks( void ) +{ + return mach_absolute_time(); +} + +float b3GetMilliseconds( uint64_t ticks ) +{ + if ( s_invFrequency == 0 ) + { + mach_timebase_info_data_t timebase; + mach_timebase_info( &timebase ); + + // convert to ns then to ms + s_invFrequency = 1e-6 * (double)timebase.numer / (double)timebase.denom; + } + + uint64_t ticksNow = b3GetTicks(); + return (float)( s_invFrequency * ( ticksNow - ticks ) ); +} + +float b3GetMillisecondsAndReset( uint64_t* ticks ) +{ + if ( s_invFrequency == 0 ) + { + mach_timebase_info_data_t timebase; + mach_timebase_info( &timebase ); + + // convert to ns then to ms + s_invFrequency = 1e-6 * (double)timebase.numer / (double)timebase.denom; + } + + uint64_t ticksNow = b3GetTicks(); + float ms = (float)( s_invFrequency * ( ticksNow - *ticks ) ); + *ticks = ticksNow; + return ms; +} + +void b3Yield( void ) +{ + sched_yield(); +} + +#include +typedef struct b3Mutex +{ + pthread_mutex_t mtx; +} b3Mutex; + +b3Mutex* b3CreateMutex( void ) +{ + b3Mutex* m = b3Alloc( sizeof( b3Mutex ) ); + pthread_mutex_init( &m->mtx, NULL ); + return m; +} + +void b3DestroyMutex( b3Mutex* m ) +{ + pthread_mutex_destroy( &m->mtx ); + *m = (b3Mutex){ 0 }; + b3Free( m, sizeof( b3Mutex ) ); +} + +void b3LockMutex( b3Mutex* m ) +{ + pthread_mutex_lock( &m->mtx ); +} + +void b3UnlockMutex( b3Mutex* m ) +{ + pthread_mutex_unlock( &m->mtx ); +} + +#include + +typedef struct b3Semaphore +{ + dispatch_semaphore_t semaphore; + int initialCount; +} b3Semaphore; + +b3Semaphore* b3CreateSemaphore( int initCount ) +{ + b3Semaphore* s = b3Alloc( sizeof( b3Semaphore ) ); + s->semaphore = dispatch_semaphore_create( (long)initCount ); + s->initialCount = initCount; + return s; +} + +void b3DestroySemaphore( b3Semaphore* s ) +{ + // libdispatch aborts if the current count is less than the initial count at release time. + // Pad with signals so the invariant always holds; no one is waiting at this point. + for ( int i = 0; i < s->initialCount; ++i ) + { + dispatch_semaphore_signal( s->semaphore ); + } + dispatch_release( s->semaphore ); + *s = (b3Semaphore){ 0 }; + b3Free( s, sizeof( b3Semaphore ) ); +} + +void b3WaitSemaphore( b3Semaphore* s ) +{ + dispatch_semaphore_wait( s->semaphore, DISPATCH_TIME_FOREVER ); +} + +void b3SignalSemaphore( b3Semaphore* s ) +{ + dispatch_semaphore_signal( s->semaphore ); +} + +typedef struct b3Thread +{ + pthread_t thread; + b3ThreadFunction* function; + void* context; + char name[32]; +} b3Thread; + +// macOS pthread_setname_np takes only the name — it always names the calling thread. +static void b3SetCurrentThreadName( const char* name ) +{ + if ( name == NULL || name[0] == 0 ) + { + return; + } + pthread_setname_np( name ); +} + +static void* b3ThreadStart( void* param ) +{ + b3Thread* t = (b3Thread*)param; + b3SetCurrentThreadName( t->name ); + t->function( t->context ); + return NULL; +} + +b3Thread* b3CreateThread( b3ThreadFunction* function, void* context, const char* name ) +{ + b3Thread* t = b3Alloc( sizeof( b3Thread ) ); + t->function = function; + t->context = context; + if ( name != NULL ) + { + snprintf( t->name, sizeof( t->name ), "%s", name ); + } + else + { + t->name[0] = 0; + } + pthread_create( &t->thread, NULL, b3ThreadStart, t ); + return t; +} + +void b3JoinThread( b3Thread* t ) +{ + pthread_join( t->thread, NULL ); + *t = (b3Thread){ 0 }; + b3Free( t, sizeof( b3Thread ) ); +} + +#else + +uint64_t b3GetTicks( void ) +{ + return 0; +} + +float b3GetMilliseconds( uint64_t ticks ) +{ + ( (void)( ticks ) ); + return 0.0f; +} + +float b3GetMillisecondsAndReset( uint64_t* ticks ) +{ + ( (void)( ticks ) ); + return 0.0f; +} + +void b3Yield( void ) +{ +} + +typedef struct b3Mutex +{ + int dummy; +} b3Mutex; + +b3Mutex* b3CreateMutex( void ) +{ + b3Mutex* m = b3Alloc( sizeof( b3Mutex ) ); + m->dummy = 42; + return m; +} + +void b3DestroyMutex( b3Mutex* m ) +{ + *m = (b3Mutex){ 0 }; + b3Free( m, sizeof( b3Mutex ) ); +} + +void b3LockMutex( b3Mutex* m ) +{ + (void)m; +} + +void b3UnlockMutex( b3Mutex* m ) +{ + (void)m; +} + +typedef struct b3Semaphore +{ + int dummy; +} b3Semaphore; + +b3Semaphore* b3CreateSemaphore( int initCount ) +{ + b3Semaphore* s = b3Alloc( sizeof( b3Semaphore ) ); + (void)initCount; + s->dummy = 42; + return s; +} + +void b3DestroySemaphore( b3Semaphore* s ) +{ + *s = (b3Semaphore){ 0 }; + b3Free( s, sizeof( b3Semaphore ) ); +} + +void b3WaitSemaphore( b3Semaphore* s ) +{ + (void)s; +} + +void b3SignalSemaphore( b3Semaphore* s ) +{ + (void)s; +} + +typedef struct b3Thread +{ + int dummy; +} b3Thread; + +b3Thread* b3CreateThread( b3ThreadFunction* function, void* context, const char* name ) +{ + (void)name; + function( context ); + b3Thread* t = b3Alloc( sizeof( b3Thread ) ); + t->dummy = 42; + return t; +} + +void b3JoinThread( b3Thread* t ) +{ + *t = (b3Thread){ 0 }; + b3Free( t, sizeof( b3Thread ) ); +} + +#endif + +// djb2 hash, folded 8 bytes per iteration to shorten the dependency chain. +// memcpy lowers to a single load on most targets; on big-endian we byte-swap so +// the hash value is identical across endianness (preserving cross-platform determinism). +// Equivalent to byte-wise djb2 only in spirit; values differ from the original recurrence. +uint32_t b3Hash( uint32_t hash, const uint8_t* data, int count ) +{ + uint32_t result = hash; + int i = 0; + + while ( i + 8 <= count ) + { + uint64_t word; + memcpy( &word, data + i, sizeof( word ) ); +#if defined( __BYTE_ORDER__ ) && __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__ + word = ( ( word & 0x00000000000000FFULL ) << 56 ) | ( ( word & 0x000000000000FF00ULL ) << 40 ) | + ( ( word & 0x0000000000FF0000ULL ) << 24 ) | ( ( word & 0x00000000FF000000ULL ) << 8 ) | + ( ( word & 0x000000FF00000000ULL ) >> 8 ) | ( ( word & 0x0000FF0000000000ULL ) >> 24 ) | + ( ( word & 0x00FF000000000000ULL ) >> 40 ) | ( ( word & 0xFF00000000000000ULL ) >> 56 ); +#endif + result = ( result << 5 ) + result + (uint32_t)word; + result = ( result << 5 ) + result + (uint32_t)( word >> 32 ); + i += 8; + } + + while ( i < count ) + { + result = ( result << 5 ) + result + data[i]; + i++; + } + + return result; +} diff --git a/src/triangle_manifold.c b/src/triangle_manifold.c new file mode 100644 index 0000000..8c11b95 --- /dev/null +++ b/src/triangle_manifold.c @@ -0,0 +1,1256 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "algorithm.h" +#include "contact.h" +#include "core.h" +#include "manifold.h" +#include "shape.h" + +#include "box3d/base.h" +#include "box3d/collision.h" +#include "box3d/constants.h" + +#include +#include + +typedef struct b3TriangleData +{ + b3Vec3 v1, v2, v3; + b3Vec3 e1, e2, e3; + b3Vec3 center; + b3Plane plane; + int flags; +} b3TriangleData; + +// Indexed by the 3-bit vertex mask +static const b3TriangleFeature s_triangleFeatures[8] = { + b3_featureNone, // 000 (unreachable) + b3_featureVertex1, // 001 + b3_featureVertex2, // 010 + b3_featureEdge1, // 011 v1,v2 + b3_featureVertex3, // 100 + b3_featureEdge3, // 101 v1,v3 + b3_featureEdge2, // 110 v2,v3 + b3_featureTriangleFace, // 111 +}; + +static b3TriangleFeature b3GetTriangleFeature( const b3SimplexCache* cache ) +{ + int count = cache->count; + B3_ASSERT( 0 < count && count < 4 ); + + // Bit i set means triangle vertex i participates in the simplex. + int mask = 0; + for ( int i = 0; i < count; ++i ) + { + B3_ASSERT( cache->indexA[i] < 3 ); + mask |= 1 << cache->indexA[i]; + } + + return s_triangleFeatures[mask]; +} + +void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const b3Sphere* sphereA, const b3Vec3* triangleB ) +{ + manifold->pointCount = 0; + + if ( capacity == 0 ) + { + return; + } + + b3Vec3 center = sphereA->center; + b3Vec3 v1 = triangleB[0], v2 = triangleB[1], v3 = triangleB[2]; + b3Plane plane = b3MakePlaneFromPoints( v1, v2, v3 ); + + float offset = b3PlaneSeparation( plane, center ); + if ( offset < 0.0f ) + { + // Cull back side collision + return; + } + + // Closest point on triangle to sphere center + b3TrianglePoint closest = b3ClosestPointOnTriangle( v1, v2, v3, center ); + + // Test separating axis + float squaredDistance = b3DistanceSquared( closest.point, center ); + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + float maxDistance = sphereA->radius + speculativeDistance; + if ( squaredDistance > maxDistance * maxDistance ) + { + return; + } + + float distance = sqrtf( squaredDistance ); + b3Vec3 normal; + if ( distance * distance > 1000.0f * FLT_MIN ) + { + normal = b3MulSV( 1.0f / distance, b3Sub( center, closest.point ) ); + } + else + { + normal = b3Normalize( b3Cross( b3Sub( v2, v1 ), b3Sub( v3, v1 ) ) ); + } + + // contact point mid-way + b3Vec3 contactPoint = b3MulSV( 0.5f, b3Add( b3Sub( center, b3MulSV( sphereA->radius, normal ) ), closest.point ) ); + + manifold->normal = normal; + manifold->pointCount = 1; + manifold->feature = closest.feature; + manifold->squaredDistance = squaredDistance; + + b3LocalManifoldPoint* mp = manifold->points + 0; + mp->point = contactPoint; + mp->separation = distance - sphereA->radius; + mp->pair = b3FeaturePair_single; +} + +static bool b3ClipSegmentToTriangleFace( b3ClipVertex segment[2], const b3Vec3* points, b3Plane plane ) +{ + b3Vec3 vertex1 = points[2]; + for ( int i = 0; i < 3; ++i ) + { + b3Vec3 vertex2 = points[i]; + b3Vec3 tangent = b3Normalize( b3Sub( vertex2, vertex1 ) ); + b3Vec3 binormal = b3Cross( tangent, plane.normal ); + + b3Plane clipPlane = b3MakePlaneFromNormalAndPoint( binormal, vertex1 ); + + int vertexCount = 0; + b3ClipVertex p1 = segment[0]; + b3ClipVertex p2 = segment[1]; + + float distance1 = b3PlaneSeparation( clipPlane, p1.position ); + float distance2 = b3PlaneSeparation( clipPlane, p2.position ); + + // If the points are behind the plane + if ( distance1 <= 0.0f ) + { + segment[vertexCount++] = p1; + } + if ( distance2 <= 0.0f ) + { + segment[vertexCount++] = p2; + } + + // If the points are on different sides of the plane + if ( distance1 * distance2 < 0.0f ) + { + // Find intersection point of edge and plane + float t = distance1 / ( distance1 - distance2 ); + segment[vertexCount].position = b3Lerp( p1.position, p2.position, t ); + segment[vertexCount].pair = distance1 > 0.0f ? p1.pair : p2.pair; + vertexCount++; + } + + if ( vertexCount != 2 ) + { + return false; + } + + vertex1 = vertex2; + } + + return true; +} + +static b3FaceQuery b3QueryTriangleFaceAndCapsule( b3Plane plane, const b3Capsule* capsule ) +{ + float separation1 = b3PlaneSeparation( plane, capsule->center1 ); + float separation2 = b3PlaneSeparation( plane, capsule->center2 ); + + if ( separation1 < separation2 ) + { + return (b3FaceQuery){ + .separation = separation1, + .faceIndex = 0, + .vertexIndex = 0, + }; + } + + return (b3FaceQuery){ + .separation = separation2, + .faceIndex = 0, + .vertexIndex = 1, + }; +} + +static b3EdgeQuery b3QueryTriangleAndCapsuleEdges( const b3Vec3* vertices, const b3Capsule* capsule ) +{ + // Work in the local space of the capsule + b3Vec3 p1 = capsule->center1; + b3Vec3 p2 = capsule->center2; + b3Vec3 capsuleEdge = b3Sub( p2, p1 ); + + b3Vec3 capsuleCenter = b3Lerp( p1, p2, 0.5f ); + + b3Vec3 triangleCenter = b3MulSV( 1.0f / 3.0f, b3Add( vertices[0], b3Add( vertices[1], vertices[2] ) ) ); + + // Find axis of minimum penetration + float maxSeparation = -FLT_MAX; + int maxIndex1 = UINT8_MAX; + int maxIndex2 = UINT8_MAX; + + int edgeIndex = 2; + b3Vec3 v1 = vertices[2]; + for ( int index = 0; index < 3; ++index ) + { + b3Vec3 v2 = vertices[index]; + + b3Vec3 triangleEdge = b3Sub( v2, v1 ); + + float separation = b3EdgeEdgeSeparation( p1, capsuleEdge, capsuleCenter, v1, triangleEdge, triangleCenter ); + if ( separation > maxSeparation ) + { + // Note: We don't exit early if we find a separating axis here since we want to + // find the best one for caching and account for the convex radius later. + maxSeparation = separation; + maxIndex1 = edgeIndex; + maxIndex2 = 0; + } + + v1 = v2; + edgeIndex = index; + } + + // Save result + return (b3EdgeQuery){ + .separation = maxSeparation, + .indexA = (uint8_t)maxIndex1, + .indexB = (uint8_t)maxIndex2, + }; +} + +static void b3BuildTriangleAndCapsuleFaceContact( b3LocalManifold* manifold, const b3Vec3* triangle, b3Plane plane, + const b3Capsule* capsule ) +{ + B3_ASSERT( manifold->pointCount == 0 ); + + b3ClipVertex segment[2]; + segment[0].position = capsule->center1; + segment[0].separation = 0.0f; + segment[0].pair = b3MakeFeaturePair( b3_featureShapeA, 0, b3_featureShapeA, 0 ); + segment[1].position = capsule->center2; + segment[1].separation = 0.0f; + segment[1].pair = b3MakeFeaturePair( b3_featureShapeA, 1, b3_featureShapeA, 1 ); + + bool havePoints = b3ClipSegmentToTriangleFace( segment, triangle, plane ); + if ( havePoints == false ) + { + return; + } + + float distance1 = b3PlaneSeparation( plane, segment[0].position ); + float distance2 = b3PlaneSeparation( plane, segment[1].position ); + + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + if ( distance1 > speculativeDistance && distance2 > speculativeDistance ) + { + return; + } + + // Average points. Half-way between capsule bottom and triangle plane. + b3Vec3 point1 = b3MulSub( segment[0].position, 0.5f * ( distance1 + capsule->radius ), plane.normal ); + b3Vec3 point2 = b3MulSub( segment[1].position, 0.5f * ( distance2 + capsule->radius ), plane.normal ); + + manifold->normal = plane.normal; + manifold->feature = b3_featureTriangleFace; + manifold->pointCount = 2; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point1; + pt->separation = distance1 - capsule->radius; + pt->pair = segment[0].pair; + + pt = manifold->points + 1; + pt->point = point2; + pt->separation = distance2 - capsule->radius; + pt->pair = segment[1].pair; +} + +static void b3BuildTriangleAndCapsuleEdgeContact( b3LocalManifold* manifold, const b3Vec3* triangle, const b3Capsule* capsule, + b3EdgeQuery query ) +{ + B3_ASSERT( 0 <= query.indexA && query.indexA < 3 ); + + b3Vec3 p1 = capsule->center1; + b3Vec3 p2 = capsule->center2; + b3Vec3 capsuleEdge = b3Sub( p2, p1 ); + + const b3Vec3* vs = triangle; + + b3Vec3 triangleCenter = b3MulSV( 1.0f / 3.0f, b3Add( vs[0], b3Add( vs[1], vs[2] ) ) ); + b3Vec3 v1 = vs[query.indexA]; + b3Vec3 v2 = vs[( query.indexA + 1 ) % 3]; + b3Vec3 triangleEdge = b3Sub( v2, v1 ); + + b3Vec3 normal = b3Cross( capsuleEdge, triangleEdge ); + normal = b3Normalize( normal ); + + // Normal should point away from triangle center + if ( b3Dot( normal, b3Sub( v1, triangleCenter ) ) < 0.0f ) + { + normal = b3Neg( normal ); + } + + b3ClosestApproachResult result = b3ClosestApproachLines( v1, triangleEdge, p1, capsuleEdge ); + + if ( result.lambda1 < 0.0f || 1.0f < result.lambda1 || result.lambda2 < 0.0f || 1.0f < result.lambda2 ) + { + // closest point beyond end points + return; + } + + b3Vec3 point = b3Lerp( b3MulSub( result.point1, capsule->radius, normal ), result.point2, 0.5f ); + + float separation = b3Dot( normal, b3Sub( result.point2, result.point1 ) ); + B3_VALIDATE( b3AbsFloat( separation - query.separation ) < B3_LINEAR_SLOP ); + + manifold->normal = normal; + manifold->pointCount = 1; + + b3TriangleFeature edgesFeatures[] = { b3_featureEdge1, b3_featureEdge2, b3_featureEdge3 }; + manifold->feature = edgesFeatures[query.indexA]; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = separation - capsule->radius; + pt->pair = b3MakeFeaturePair( b3_featureShapeA, query.indexA, b3_featureShapeB, query.indexB ); +} + +void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA, const b3Vec3* triangleB, + b3SimplexCache* cache ) +{ + manifold->pointCount = 0; + + if ( capacity < 2 ) + { + return; + } + + b3Vec3 v1 = triangleB[0], v2 = triangleB[1], v3 = triangleB[2]; + b3Plane plane = b3MakePlaneFromPoints( v1, v2, v3 ); + b3Vec3 capsuleCenter = b3Lerp( capsuleA->center1, capsuleA->center2, 0.5f ); + + float offset = b3PlaneSeparation( plane, capsuleCenter ); + if ( offset < 0.0f ) + { + // Cull back side collision + return; + } + + b3DistanceInput distanceInput; + distanceInput.proxyA = (b3ShapeProxy){ triangleB, 3, 0.0f }; + distanceInput.proxyB = (b3ShapeProxy){ &capsuleA->center1, 2, 0.0f }; + distanceInput.transformA = b3Transform_identity; + distanceInput.transformB = b3Transform_identity; + distanceInput.useRadii = false; + + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, cache, NULL, 0 ); + + float radius = capsuleA->radius; + if ( distanceOutput.distance > radius + B3_SPECULATIVE_DISTANCE ) + { + // Shapes are separated, persist the cache + return; + } + + if ( distanceOutput.distance > 100.0f * FLT_EPSILON ) + { + // Shallow penetration + b3Vec3 delta = b3Normalize( b3Sub( distanceOutput.pointB, distanceOutput.pointA ) ); + + // Try to create two contact points if closest points difference is nearly parallel to face normal + const float kTolerance = 0.2f; + float cosAngle = b3AbsFloat( b3Dot( plane.normal, delta ) ); + if ( cosAngle > kTolerance ) + { + // Clip capsule segment against side planes of reference face + b3ClipVertex segment[2]; + segment[0].position = capsuleA->center1; + segment[0].separation = 0.0f; + segment[0].pair = b3MakeFeaturePair( b3_featureShapeA, 0, b3_featureShapeA, 0 ); + segment[1].position = capsuleA->center2; + segment[1].separation = 0.0f; + segment[1].pair = b3MakeFeaturePair( b3_featureShapeA, 1, b3_featureShapeA, 1 ); + + bool havePoints = b3ClipSegmentToTriangleFace( segment, triangleB, plane ); + + if ( havePoints == true ) + { + float distance1 = b3PlaneSeparation( plane, segment[0].position ); + float distance2 = b3PlaneSeparation( plane, segment[1].position ); + + b3Vec3 normal = plane.normal; + b3Vec3 point1 = b3MulSub( segment[0].position, 0.5f * ( radius + distance1 ), normal ); + b3Vec3 point2 = b3MulSub( segment[1].position, 0.5f * ( radius + distance2 ), normal ); + + manifold->normal = normal; + manifold->feature = b3_featureTriangleFace; + manifold->pointCount = 2; + + b3LocalManifoldPoint* mp = manifold->points + 0; + mp->point = point1; + mp->separation = distance1 - radius; + mp->pair = segment[0].pair; + + mp = manifold->points + 1; + mp->point = point2; + mp->separation = distance2 - radius; + mp->pair = segment[1].pair; + + return; + } + } + + // Create contact from closest points + b3Vec3 point = b3MulSV( 0.5f, b3Add( b3Sub( distanceOutput.pointA, b3MulSV( radius, delta ) ), distanceOutput.pointB ) ); + + manifold->normal = delta; + manifold->pointCount = 1; + manifold->feature = b3GetTriangleFeature( cache ); + + b3LocalManifoldPoint* mp = manifold->points + 0; + mp->point = point; + mp->separation = distanceOutput.distance - radius; + mp->pair = b3FeaturePair_single; + + return; + } + + // Deep penetration + + b3FaceQuery faceQuery = b3QueryTriangleFaceAndCapsule( plane, capsuleA ); + if ( faceQuery.separation > radius ) + { + // Shapes are separated + return; + } + + b3EdgeQuery edgeQuery = b3QueryTriangleAndCapsuleEdges( triangleB, capsuleA ); + if ( edgeQuery.separation > radius ) + { + // Shapes are separated + return; + } + + // Create face contact + float faceSeparation = faceQuery.separation - radius; + b3BuildTriangleAndCapsuleFaceContact( manifold, triangleB, plane, capsuleA ); + if ( manifold->pointCount == 2 ) + { + faceSeparation = b3MinFloat( manifold->points[0].separation, manifold->points[1].separation ); + } + B3_VALIDATE( faceSeparation <= 0.0f ); + + // Face contact can be empty if it does not realize the axis of minimum penetration. + // Create edge contact if face contact fails or edge contact is significantly better! + const float kRelEdgeTolerance = 0.50f; + const float kAbsTolerance = 1.0f * B3_LINEAR_SLOP; + float edgeSeparation = edgeQuery.separation - radius; + if ( manifold->pointCount == 0 || edgeSeparation > kRelEdgeTolerance * faceSeparation + kAbsTolerance ) + { + // Edge contact + b3BuildTriangleAndCapsuleEdgeContact( manifold, triangleB, capsuleA, edgeQuery ); + } +} + +static inline int b3GetTriangleSupport( b3Vec3* points, b3Vec3 direction ) +{ + int index = 0; + float distance = b3Dot( points[0], direction ); + + float d = b3Dot( points[1], direction ); + if ( d > distance ) + { + distance = d; + index = 1; + } + + d = b3Dot( points[2], direction ); + if ( d > distance ) + { + return 2; + } + + return index; +} + +static b3FaceQuery b3QueryTriangleFace( const b3TriangleData* triangle, const b3Hull* hull ) +{ + const b3Vec3* hullPoints = b3GetHullPoints( hull ); + b3Plane plane = triangle->plane; + int vertexIndex = b3FindHullSupportVertex( hull, b3Neg( plane.normal ) ); + b3Vec3 support = hullPoints[vertexIndex]; + float separation = b3PlaneSeparation( plane, support ); + + return (b3FaceQuery){ + .separation = separation, + .faceIndex = 0, + .vertexIndex = (uint8_t)vertexIndex, + }; +} + +static b3FaceQuery b3QueryHullFace( const b3TriangleData* triangle, const b3Hull* hull ) +{ + const b3Plane* hullPlanes = b3GetHullPlanes( hull ); + int faceCount = hull->faceCount; + + b3Vec3 trianglePoints[] = { triangle->v1, triangle->v2, triangle->v3 }; + + int maxFaceIndex = -1; + int maxVertexIndex = -1; + float maxFaceSeparation = -FLT_MAX; + + for ( int faceIndex = 0; faceIndex < faceCount; ++faceIndex ) + { + b3Plane plane = hullPlanes[faceIndex]; + + int vertexIndex = b3GetTriangleSupport( trianglePoints, b3Neg( plane.normal ) ); + b3Vec3 support = trianglePoints[vertexIndex]; + float separation = b3PlaneSeparation( plane, support ); + if ( separation > maxFaceSeparation ) + { + maxFaceIndex = faceIndex; + maxVertexIndex = vertexIndex; + maxFaceSeparation = separation; + } + } + + return (b3FaceQuery){ + .separation = maxFaceSeparation, + .faceIndex = maxFaceIndex, + .vertexIndex = maxVertexIndex, + }; +} + +static b3EdgeQuery b3TestEdgePairs( const b3TriangleData* triangle, const b3Hull* hull ) +{ + b3EdgeQuery result = { + .separation = -FLT_MAX, + .indexA = B3_NULL_INDEX, + .indexB = B3_NULL_INDEX, + }; + + b3Vec3 trianglePoints[] = { triangle->v1, triangle->v2, triangle->v3 }; + b3Vec3 triangleEdges[] = { triangle->e1, triangle->e2, triangle->e3 }; + // int edgeFlags[] = { b3_concaveEdge1, b3_concaveEdge1, b3_concaveEdge3 }; + +#if B3_FORCE_GHOST_COLLISIONS + int triangleFlags = 0xFF; +#else + int triangleFlags = triangle->flags; +#endif + (void)triangleFlags; + + b3Vec3 triNormal = triangle->plane.normal; + + const b3HullHalfEdge* hullEdges = b3GetHullEdges( hull ); + const b3Vec3* hullPoints = b3GetHullPoints( hull ); + const b3Plane* hullPlanes = b3GetHullPlanes( hull ); + int edgeCount = hull->edgeCount; + + for ( int i = 0; i < edgeCount; i += 2 ) + { + const b3HullHalfEdge* edge = hullEdges + i; + const b3HullHalfEdge* twin = hullEdges + i + 1; + B3_ASSERT( edge->twin == i + 1 && twin->twin == i ); + + b3Vec3 hullPoint = hullPoints[edge->origin]; + b3Vec3 hullEdge = b3Sub( hullPoints[twin->origin], hullPoint ); + + b3Vec3 hullNormal1 = hullPlanes[edge->face].normal; + b3Vec3 hullNormal2 = hullPlanes[twin->face].normal; + + for ( int j = 0; j < 3; ++j ) + { + b3Vec3 triEdge = triangleEdges[j]; + + float cab = b3Dot( hullNormal1, triEdge ); + float dab = b3Dot( hullNormal2, triEdge ); + float bcd = b3Dot( triNormal, hullEdge ); + if ( cab * dab >= 0.0f || cab * bcd <= 0.0f ) + { + continue; + } + + b3Vec3 triPoint = trianglePoints[j]; + float separation = b3EdgeEdgeSeparation( triPoint, triEdge, triangle->center, hullPoint, hullEdge, hull->center ); + + // if ( separation > result.separation && ( edgeFlags[j] & triangleFlags ) == 0 ) + if ( separation > result.separation ) + { + // Note: We don't exit early if we find a separating axis here since we want to + // find the best one for caching. + result.separation = separation; + result.indexA = j; + result.indexB = i; + } + } + } + + return result; +} + +static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, const b3TriangleData* triangle, const b3Hull* hull, + b3FaceQuery query, b3SATCache* cache ) +{ + manifold->pointCount = 0; + + const b3HullFace* hullFaces = b3GetHullFaces( hull ); + const b3HullHalfEdge* hullEdges = b3GetHullEdges( hull ); + const b3Plane* hullPlanes = b3GetHullPlanes( hull ); + const b3Vec3* hullPoints = b3GetHullPoints( hull ); + + // Reference hull face + int refFace = query.faceIndex; + b3Plane refPlane = hullPlanes[refFace]; + + // Build clip polygon from triangle face (the incident face) + b3ClipVertex buffer1[B3_MAX_CLIP_POINTS], buffer2[B3_MAX_CLIP_POINTS]; + + b3Vec3 v1 = triangle->v1; + b3Vec3 v2 = triangle->v2; + b3Vec3 v3 = triangle->v3; + buffer1[0].position = v1; + buffer1[0].separation = b3PlaneSeparation( refPlane, v1 ); + buffer1[0].pair = b3MakeFeaturePair( b3_featureShapeB, 2, b3_featureShapeB, 0 ); + buffer1[1].position = v2; + buffer1[1].separation = b3PlaneSeparation( refPlane, v2 ); + buffer1[1].pair = b3MakeFeaturePair( b3_featureShapeB, 0, b3_featureShapeB, 1 ); + buffer1[2].position = v3; + buffer1[2].separation = b3PlaneSeparation( refPlane, v3 ); + buffer1[2].pair = b3MakeFeaturePair( b3_featureShapeB, 1, b3_featureShapeB, 2 ); + int pointCount = 3; + + // Clip triangle face against side planes of reference face + b3ClipVertex* input = buffer1; + b3ClipVertex* output = buffer2; + + const b3HullFace* face = hullFaces + refFace; + int edgeIndex = face->edge; + + do + { + const b3HullHalfEdge* edge = hullEdges + edgeIndex; + int nextEdgeIndex = edge->next; + const b3HullHalfEdge* next = hullEdges + nextEdgeIndex; + b3Vec3 vertex1 = hullPoints[edge->origin]; + b3Vec3 vertex2 = hullPoints[next->origin]; + b3Vec3 tangent = b3Normalize( b3Sub( vertex2, vertex1 ) ); + b3Vec3 binormal = b3Cross( tangent, refPlane.normal ); + + b3Plane clipPlane = b3MakePlaneFromNormalAndPoint( binormal, vertex1 ); + + pointCount = b3ClipPolygon( output, input, pointCount, clipPlane, edgeIndex, refPlane ); + B3_ASSERT( pointCount <= B3_MAX_CLIP_POINTS ); + + if ( pointCount < 3 ) + { + // Using a stale cache + *cache = (b3SATCache){ 0 }; + return query.separation; + } + + // Swap buffers, output becomes input for the next clipping plane + B3_SWAP( output, input ); + edgeIndex = nextEdgeIndex; + } + while ( edgeIndex != face->edge ); + + pointCount = b3MinInt( pointCount, pointCapacity ); + float minSeparation = FLT_MAX; + + for ( int i = 0; i < pointCount; ++i ) + { + b3ClipVertex* clipPoint = input + i; + + // Move point onto hull face improved culling + b3Vec3 point = b3MulSub( clipPoint->position, clipPoint->separation, refPlane.normal ); + + b3LocalManifoldPoint* pt = manifold->points + i; + pt->point = point; + pt->separation = clipPoint->separation; + pt->pair = b3FlipPair( clipPoint->pair ); + + minSeparation = b3MinFloat( minSeparation, clipPoint->separation ); + } + + if ( minSeparation > B3_SPECULATIVE_DISTANCE ) + { + // This can occur with a stale SAT cache + manifold->pointCount = 0; + *cache = (b3SATCache){ 0 }; + return minSeparation; + } + + manifold->pointCount = pointCount; + manifold->normal = b3Neg( refPlane.normal ); + manifold->feature = b3_featureHullFace; + + // Save cache + cache->separation = minSeparation; + cache->type = b3_faceAxisB; + cache->indexA = (uint8_t)query.vertexIndex; + cache->indexB = (uint8_t)query.faceIndex; + return minSeparation; +} + +static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity, const b3TriangleData* triangle, + const b3Hull* hull, b3FaceQuery query, b3SATCache* cache ) +{ + B3_VALIDATE( manifold->pointCount == 0 ); + + const b3HullFace* hullFaces = b3GetHullFaces( hull ); + const b3HullHalfEdge* hullEdges = b3GetHullEdges( hull ); + const b3Vec3* hullPoints = b3GetHullPoints( hull ); + + // Find incident face + B3_ASSERT( query.faceIndex == 0 ); + b3Plane refPlane = triangle->plane; + + int incFace = b3FindIncidentFace( hull, refPlane.normal, query.vertexIndex ); + + // Build clip polygon from incident face + b3ClipVertex buffer1[2 * B3_MAX_CLIP_POINTS], buffer2[2 * B3_MAX_CLIP_POINTS]; + int pointCount = 0; + const b3HullFace* face = hullFaces + incFace; + int hullEdgeIndex = face->edge; + + do + { + const b3HullHalfEdge* edge = hullEdges + hullEdgeIndex; + + int nextEdgeIndex = edge->next; + const b3HullHalfEdge* next = hullEdges + nextEdgeIndex; + + b3Vec3 hullPoint = hullPoints[next->origin]; + buffer1[pointCount].position = hullPoint; + buffer1[pointCount].separation = b3PlaneSeparation( refPlane, hullPoint ); + buffer1[pointCount].pair = b3MakeFeaturePair( b3_featureShapeB, hullEdgeIndex, b3_featureShapeB, nextEdgeIndex ); + + pointCount += 1; + hullEdgeIndex = nextEdgeIndex; + } + while ( hullEdgeIndex != face->edge && pointCount < 2 * B3_MAX_CLIP_POINTS ); + + B3_ASSERT( pointCount >= 3 ); + + // Clip incident face against side planes of reference face (triangle) + b3ClipVertex* input = buffer1; + b3ClipVertex* output = buffer2; + + b3Vec3 trianglePoints[] = { triangle->v1, triangle->v2, triangle->v3 }; + b3Vec3 triangleEdges[] = { triangle->e1, triangle->e2, triangle->e3 }; + + for ( int i = 0; i < 3 && pointCount > 0; ++i ) + { + b3Vec3 sideNormal = b3Cross( triangleEdges[i], refPlane.normal ); + sideNormal = b3Normalize( sideNormal ); + + b3Plane clipPlane = b3MakePlaneFromNormalAndPoint( sideNormal, trianglePoints[i] ); + + pointCount = b3ClipPolygon( output, input, pointCount, clipPlane, i, refPlane ); + B3_ASSERT( pointCount <= 2 * B3_MAX_CLIP_POINTS ); + + B3_SWAP( output, input ); + } + + if ( pointCount == 0 ) + { + // Triangle face clipped away. Invalidate cache. + *cache = (b3SATCache){ 0 }; + return FLT_MAX; + } + + pointCount = b3MinInt( pointCount, pointCapacity ); + + float minSeparation = FLT_MAX; + + for ( int i = 0; i < pointCount; ++i ) + { + b3ClipVertex* clipPoint = input + i; + + // Move point onto triangle surface for improved culling + // b3Vec3 point = b3MulSub( clipPoint->position, clipPoint->separation, refPlane.normal ); + b3Vec3 point = clipPoint->position; + + b3LocalManifoldPoint* pt = manifold->points + i; + pt->point = point; + pt->separation = clipPoint->separation; + pt->pair = clipPoint->pair; + + minSeparation = b3MinFloat( minSeparation, clipPoint->separation ); + } + + if ( minSeparation >= B3_SPECULATIVE_DISTANCE ) + { + // This can happens if the objects move a part while re-using a cached axis + *cache = (b3SATCache){ 0 }; + return minSeparation; + } + + manifold->pointCount = pointCount; + manifold->normal = refPlane.normal; + manifold->feature = b3_featureTriangleFace; + + // Save cache + cache->separation = minSeparation; + cache->type = b3_faceAxisA; + cache->indexA = (uint8_t)query.faceIndex; + cache->indexB = (uint8_t)query.vertexIndex; + return minSeparation; +} + +static void b3CollideHullAndTriangleEdges( b3LocalManifold* manifold, int capacity, b3Vec3 trianglePoint, b3Vec3 triangleEdge, + b3Vec3 triangleCenter, const b3Hull* hull, b3EdgeQuery query, b3SATCache* cache ) +{ + B3_VALIDATE( query.separation <= 2.0f * B3_SPECULATIVE_DISTANCE ); + B3_ASSERT( query.indexA < 3 ); + + b3Vec3 cA = triangleCenter; + b3Vec3 pA = trianglePoint; + b3Vec3 eA = triangleEdge; + + const b3HullHalfEdge* edgesB = b3GetHullEdges( hull ); + const b3Vec3* pointsB = b3GetHullPoints( hull ); + const b3HullHalfEdge* edgeB = edgesB + query.indexB; + const b3HullHalfEdge* twinB = edgesB + edgeB->twin; + b3Vec3 pB = pointsB[edgeB->origin]; + b3Vec3 qB = pointsB[twinB->origin]; + b3Vec3 eB = b3Sub( qB, pB ); + + b3Vec3 normal = b3Cross( eA, eB ); + normal = b3Normalize( normal ); + + // Ensure normal points outward from triangle center + float outwardA = b3Dot( normal, b3Sub( pA, cA ) ); + + // Ensure normal points towards hull center + float outwardB = b3Dot( normal, b3Sub( hull->center, pB ) ); + + // Use the largest magnitude. The triangle outward value + // may be unreliable as some angles. + if ( b3AbsFloat( outwardA ) > b3AbsFloat( outwardB ) ) + { + if ( outwardA < 0.0f ) + { + normal = b3Neg( normal ); + } + } + else + { + if ( outwardB < 0.0f ) + { + normal = b3Neg( normal ); + } + } + + // Get the closest points between the infinite edge lines + b3ClosestApproachResult result = b3ClosestApproachLines( pA, eA, pB, eB ); + + // Is one of the closest points outside of the associated edge segment? + if ( capacity == 0 || result.lambda1 < 0.0f || 1.0f < result.lambda1 || result.lambda2 < 0.0f || 1.0f < result.lambda2 ) + { + // Invalid edge pair, no points generated + B3_ASSERT( manifold->pointCount == 0 ); + *cache = (b3SATCache){ 0 }; + return; + } + + // This can slide off the end from caching + float separation = b3Dot( normal, b3Sub( result.point2, result.point1 ) ); + B3_VALIDATE( b3AbsFloat( separation - query.separation ) < B3_LINEAR_SLOP ); + + b3Vec3 point = b3MulSV( 0.5f, b3Add( result.point1, result.point2 ) ); + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = separation; + pt->pair = b3MakeFeaturePair( b3_featureShapeA, query.indexA, b3_featureShapeB, query.indexB ); + + // Save cache + cache->separation = separation; + cache->type = b3_edgePairAxis; + cache->indexA = (uint8_t)query.indexA; + cache->indexB = (uint8_t)query.indexB; + + manifold->normal = normal; + manifold->pointCount = 1; + + b3TriangleFeature edgesFeatures[] = { b3_featureEdge1, b3_featureEdge2, b3_featureEdge3 }; + manifold->feature = edgesFeatures[query.indexA]; +} + +// See "Collision Detection of Convex Polyhedra Based on Duality Transformation" +// Simplified for triangle versus hull +static inline bool b3IsTriangleMinkowskiFace( b3Vec3 triNormal, b3Vec3 triEdge, b3Vec3 hullNormal1, b3Vec3 hullNormal2, + b3Vec3 hullEdge ) +{ + float cab = b3Dot( hullNormal1, triEdge ); + float dab = b3Dot( hullNormal2, triEdge ); + float bcd = b3Dot( triNormal, hullEdge ); + return cab * dab < 0.0f && cab * bcd > 0.0f; +} + +b3AtomicInt b3_triangleConvexCalls; +b3AtomicInt b3_triangleCacheHits; + +// Computes the manifold in the local space of the hull +void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3Hull* hullA, const b3Vec3* triangleB, + int triangleFlags, b3SATCache* cache ) +{ + manifold->pointCount = 0; + manifold->feature = b3_featureNone; + + if ( capacity < 4 ) + { + return; + } + + b3Vec3 v1 = triangleB[0], v2 = triangleB[1], v3 = triangleB[2]; + b3Plane trianglePlane = b3MakePlaneFromPoints( v1, v2, v3 ); + float linearSlop = B3_LINEAR_SLOP; + +#if 1 + float offset = b3PlaneSeparation( trianglePlane, hullA->center ); + if ( cache->type == b3_backsideAxis ) + { + // Use hysteresis to avoid jitter on wavy meshes + if ( b3AbsFloat( cache->separation - offset ) < linearSlop ) + { + return; + } + + cache->type = b3_invalidAxis; + } + + if ( offset < -linearSlop ) + { + // Cull back side collision. Cache offset to add hysteresis. + cache->type = b3_backsideAxis; + cache->separation = offset; + return; + } +#endif + + b3Vec3 triangleCenter = b3MulSV( 1.0f / 3.0f, b3Add( v1, b3Add( v2, v3 ) ) ); + b3Vec3 trianglePoints[] = { v1, v2, v3 }; + b3Vec3 triangleEdges[] = { b3Sub( v2, v1 ), b3Sub( v3, v2 ), b3Sub( v1, v3 ) }; + + b3TriangleData triangle = { + .v1 = v1, + .v2 = v2, + .v3 = v3, + .e1 = triangleEdges[0], + .e2 = triangleEdges[1], + .e3 = triangleEdges[2], + .center = triangleCenter, + .plane = trianglePlane, + .flags = triangleFlags, + }; + + const b3HullHalfEdge* edges = b3GetHullEdges( hullA ); + const b3Plane* hullPlanes = b3GetHullPlanes( hullA ); + const b3Vec3* hullPoints = b3GetHullPoints( hullA ); + + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + cache->hit = 1; + + // Attempt to use the cache to speed up collision + switch ( cache->type ) + { + case b3_faceAxisA: + { + B3_ASSERT( cache->indexA == 0 ); + + int vertexIndex = b3FindHullSupportVertex( hullA, b3Neg( trianglePlane.normal ) ); + b3Vec3 support = hullPoints[vertexIndex]; + float separation = b3PlaneSeparation( trianglePlane, support ); + if ( separation >= speculativeDistance ) + { + // Cache hit, shapes are separated + return; + } + + b3FaceQuery faceQuery; + faceQuery.separation = separation; + faceQuery.faceIndex = cache->indexA; + faceQuery.vertexIndex = vertexIndex; + + // Read cache but don't modify it + b3SATCache localCache = *cache; + float clippedSeparation = b3CollideTriangleFace( manifold, capacity, &triangle, hullA, faceQuery, &localCache ); + + if ( manifold->pointCount > 0 && b3AbsFloat( cache->separation - clippedSeparation ) < linearSlop ) + { + // Cache hit, contact points generated + return; + } + + // Invalidate cache and fall through + manifold->pointCount = 0; + *cache = (b3SATCache){ 0 }; + } + break; + + case b3_faceAxisB: + { + B3_ASSERT( cache->indexB < hullA->faceCount ); + + b3Plane plane = hullPlanes[cache->indexB]; + + // Get triangle support point + int vertexIndex = 0; + float distance = -b3Dot( v1, plane.normal ); + for ( int i = 1; i < 3; ++i ) + { + float d = -b3Dot( trianglePoints[i], plane.normal ); + if ( d > distance ) + { + distance = d; + vertexIndex = i; + } + } + + b3Vec3 support = trianglePoints[vertexIndex]; + + // Separation of triangle support point with hull plane + float separation = b3PlaneSeparation( plane, support ); + if ( separation >= speculativeDistance ) + { + // Cache hit, shapes are separated + return; + } + + // Deep overlap may lead to an invalid cache + // todo confirm + bool isDeep = separation < -2.0f * linearSlop; + + // Don't persist deep cache or allow separation to change too much + if ( isDeep == false ) + { + // Try to rebuild contact from last features + b3FaceQuery faceQuery; + faceQuery.separation = separation; + faceQuery.faceIndex = cache->indexB; + faceQuery.vertexIndex = vertexIndex; + + // Read cache but don't modify it + b3SATCache localCache = *cache; + float clippedSeparation = b3CollideHullFace( manifold, capacity, &triangle, hullA, faceQuery, &localCache ); + + // Cache reuse is only successful if it creates contact points and the clipped separation didn't change much. + if ( manifold->pointCount > 0 && b3AbsFloat( cache->separation - clippedSeparation ) < linearSlop ) + { + // Cache hit, contact points generated + return; + } + } + + // Invalidate cache and fall through + manifold->pointCount = 0; + *cache = (b3SATCache){ 0 }; + } + break; + + case b3_edgePairAxis: + { + B3_ASSERT( cache->indexA < 3 ); + int indexA = cache->indexA; + + b3Vec3 triPoint = trianglePoints[indexA]; + b3Vec3 triEdge = triangleEdges[indexA]; + + B3_ASSERT( cache->indexB < hullA->edgeCount - 1 ); + int indexB = cache->indexB; + + const b3HullHalfEdge* edge2 = edges + indexB; + const b3HullHalfEdge* twin2 = edges + indexB + 1; + B3_ASSERT( edge2->twin == indexB + 1 && twin2->twin == indexB ); + + b3Vec3 hullPoint = hullPoints[edge2->origin]; + b3Vec3 hullEdge = b3Sub( hullPoints[twin2->origin], hullPoint ); + b3Vec3 hullNormal1 = hullPlanes[edge2->face].normal; + b3Vec3 hullNormal2 = hullPlanes[twin2->face].normal; + + // Confirm the edge pair is still a Minkowski face + bool isMinkowski = b3IsTriangleMinkowskiFace( trianglePlane.normal, triEdge, hullNormal1, hullNormal2, hullEdge ); + if ( isMinkowski ) + { + // Transform reference center of the first hull into local space of the second hull + float separation = b3EdgeEdgeSeparation( triPoint, triEdge, triangleCenter, hullPoint, hullEdge, hullA->center ); + if ( separation > speculativeDistance ) + { + // Cache hit, shapes are separated + return; + } + + if ( b3AbsFloat( cache->separation - separation ) < linearSlop ) + { + // Try to rebuild contact from last features + b3EdgeQuery edgeQuery; + edgeQuery.indexA = indexA; + edgeQuery.indexB = indexB; + edgeQuery.separation = separation; + + // Read cache but don't modify it + b3SATCache localCache = *cache; + b3CollideHullAndTriangleEdges( manifold, capacity, triPoint, triEdge, triangleCenter, hullA, edgeQuery, + &localCache ); + + if ( manifold->pointCount > 0 ) + { + // Cache hit, contact point generated + return; + } + } + } + + // Invalidate cache and fall through + *cache = (b3SATCache){ 0 }; + } + break; + + // This case is for testing + case b3_manualFaceAxisA: + { + b3FaceQuery faceQueryA = b3QueryTriangleFace( &triangle, hullA ); + b3CollideTriangleFace( manifold, capacity, &triangle, hullA, faceQueryA, cache ); + return; + } + + // This case is for testing + case b3_manualFaceAxisB: + { + b3FaceQuery faceQueryB = b3QueryHullFace( &triangle, hullA ); + b3CollideHullFace( manifold, capacity, &triangle, hullA, faceQueryB, cache ); + return; + } + + // This case is for testing + case b3_manualEdgePairAxis: + { + b3EdgeQuery edgeQuery = b3TestEdgePairs( &triangle, hullA ); + if ( edgeQuery.indexA != B3_NULL_INDEX ) + { + b3Vec3 trianglePoint = trianglePoints[edgeQuery.indexA]; + b3Vec3 triangleEdge = triangleEdges[edgeQuery.indexA]; + b3CollideHullAndTriangleEdges( manifold, capacity, trianglePoint, triangleEdge, triangleCenter, hullA, edgeQuery, + cache ); + } + return; + } + + default: + B3_ASSERT( cache->type == b3_invalidAxis ); + break; + } + + // Cache miss + cache->hit = 0; + + // Find axis of minimum penetration + b3FaceQuery faceQueryA = b3QueryTriangleFace( &triangle, hullA ); + if ( faceQueryA.separation > speculativeDistance ) + { + // Separating axis found + cache->separation = faceQueryA.separation; + cache->type = b3_faceAxisA; + cache->indexA = 0; + cache->indexB = UINT8_MAX; + return; + } + + b3FaceQuery faceQueryB = b3QueryHullFace( &triangle, hullA ); + if ( faceQueryB.separation > speculativeDistance ) + { + // Separating axis found + cache->separation = faceQueryB.separation; + cache->type = b3_faceAxisB; + cache->indexA = UINT8_MAX; + cache->indexB = (uint8_t)faceQueryB.faceIndex; + return; + } + + b3EdgeQuery edgeQuery = b3TestEdgePairs( &triangle, hullA ); + if ( edgeQuery.separation > speculativeDistance ) + { + // Separating axis found + cache->separation = edgeQuery.separation; + cache->type = b3_edgePairAxis; + cache->indexA = (uint8_t)edgeQuery.indexA; + cache->indexB = (uint8_t)edgeQuery.indexB; + return; + } + + float clippedFaceSeparation; + + // Don't allow a hull face opposed to the triangle face. + b3Vec3 hullNormal = hullPlanes[faceQueryB.faceIndex].normal; + bool pushingUp = b3Dot( hullNormal, trianglePlane.normal ) < 0.0f; + if ( faceQueryB.separation > faceQueryA.separation + linearSlop && pushingUp ) + { + clippedFaceSeparation = b3CollideHullFace( manifold, capacity, &triangle, hullA, faceQueryB, cache ); + } + else + { + clippedFaceSeparation = b3CollideTriangleFace( manifold, capacity, &triangle, hullA, faceQueryA, cache ); + } + + // Does an edge axis exist? + if ( edgeQuery.indexA != B3_NULL_INDEX ) + { + // Idea is to ignore the jenga problem when separated and just use the best axis + // bool separated = clippedFaceSeparation > 0.0f && edgeQuery.separation > 0.0f; + // float maxFaceSeparation = b3MaxFloat( faceQueryA.separation, faceQueryB.separation ); + + // Do edge contact if face contact yield no points or only one point with significantly deeper overlap. + if ( manifold->pointCount == 0 || + ( manifold->pointCount == 1 && edgeQuery.separation > clippedFaceSeparation + linearSlop ) ) + { + B3_ASSERT( 0 <= edgeQuery.indexA && edgeQuery.indexA < 3 ); + b3Vec3 trianglePoint = trianglePoints[edgeQuery.indexA]; + b3Vec3 triangleEdge = triangleEdges[edgeQuery.indexA]; + manifold->pointCount = 0; + b3CollideHullAndTriangleEdges( manifold, capacity, trianglePoint, triangleEdge, triangleCenter, hullA, edgeQuery, + cache ); + } + } + + // Using the speculative distance means that sometimes there are no valid contact points from SAT. + // In this fall back to GJK. This is important to prevent tunneling in rare cases. + if ( manifold->pointCount == 0 ) + { + b3DistanceInput input = { 0 }; + input.proxyA = (b3ShapeProxy){ + .points = triangleB, + .count = 3, + .radius = 0.0f, + }; + input.proxyB = (b3ShapeProxy){ .points = hullPoints, .count = hullA->vertexCount, .radius = 0.0f }; + input.transformA = b3Transform_identity; + input.transformB = b3Transform_identity; + input.useRadii = false; + + b3SimplexCache simplexCache = { 0 }; + b3DistanceOutput output = b3ShapeDistance( &input, &simplexCache, NULL, 0 ); + + if ( output.distance > 0.0f ) + { + B3_ASSERT( 0 < simplexCache.count && simplexCache.count <= 3 ); + + manifold->pointCount = 1; + manifold->feature = b3GetTriangleFeature( &simplexCache ); + manifold->normal = output.normal; + manifold->points[0].point = output.pointB; + manifold->points[0].separation = output.distance; + + // This feature pair not accurate but maybe it doesn't matter + manifold->points[0].pair = b3FeaturePair_single; + } + } +} diff --git a/src/types.c b/src/types.c new file mode 100644 index 0000000..2a89696 --- /dev/null +++ b/src/types.c @@ -0,0 +1,135 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "box3d/types.h" + +#include "core.h" + +#include "box3d/constants.h" + +b3WorldDef b3DefaultWorldDef( void ) +{ + float lengthUnits = b3GetLengthUnitsPerMeter(); + + b3WorldDef def = { 0 }; + def.gravity.x = 0.0f; + def.gravity.y = -10.0f; + def.hitEventThreshold = 1.0f * lengthUnits; + def.restitutionThreshold = 1.0f * lengthUnits; + def.contactSpeed = 3.0f * lengthUnits; + def.contactHertz = 30.0f; + def.contactDampingRatio = 10.0f; + + // 400 meters per second, faster than the speed of sound + def.maximumLinearSpeed = 400.0f * lengthUnits; + + def.enableSleep = true; + def.enableContinuous = true; + def.internalValue = B3_SECRET_COOKIE; + return def; +} + +b3BodyDef b3DefaultBodyDef( void ) +{ + b3BodyDef def = { 0 }; + def.type = b3_staticBody; + def.rotation = b3Quat_identity; + def.sleepThreshold = 0.05f * b3GetLengthUnitsPerMeter(); + def.gravityScale = 1.0f; + def.enableSleep = true; + def.isAwake = true; + def.isEnabled = true; + def.enableContinuousCollision = true; + def.internalValue = B3_SECRET_COOKIE; + return def; +} + +b3Filter b3DefaultFilter( void ) +{ + b3Filter filter = { B3_DEFAULT_CATEGORY_BITS, B3_DEFAULT_MASK_BITS, 0 }; + return filter; +} + +b3QueryFilter b3DefaultQueryFilter( void ) +{ + b3QueryFilter filter = { B3_DEFAULT_CATEGORY_BITS, B3_DEFAULT_MASK_BITS }; + return filter; +} + +b3SurfaceMaterial b3DefaultSurfaceMaterial( void ) +{ + b3SurfaceMaterial surfaceMaterial = { 0 }; + surfaceMaterial.friction = 0.6f; + return surfaceMaterial; +} + +b3ShapeDef b3DefaultShapeDef( void ) +{ + float lengthUnits = b3GetLengthUnitsPerMeter(); + + b3ShapeDef def = { 0 }; + def.baseMaterial = b3DefaultSurfaceMaterial(); + // density of water + def.density = 1000.0f / ( lengthUnits * lengthUnits * lengthUnits ); + def.explosionScale = 1.0f; + def.filter = b3DefaultFilter(); + def.updateBodyMass = true; + def.invokeContactCreation = true; + def.internalValue = B3_SECRET_COOKIE; + return def; +} + +static bool b3EmptyDrawShape( void* userShape, b3Transform transform, b3HexColor color, void* context ) +{ + B3_UNUSED( userShape, transform, color, context ); + return false; +} + +static void b3EmptyDrawSegment( b3Vec3 p1, b3Vec3 p2, b3HexColor color, void* context ) +{ + B3_UNUSED( p1, p2, color, context ); +} + +static void b3EmptyDrawTransform( b3Transform transform, void* context ) +{ + B3_UNUSED( transform, context ); +} + +static void b3EmptyDrawPoint( b3Vec3 p, float size, b3HexColor color, void* context ) +{ + B3_UNUSED( p, size, color, context ); +} + +static void b3EmptyDrawBounds( b3AABB aabb, b3HexColor color, void* context ) +{ + B3_UNUSED( aabb, color, context ); +} + +static void b3EmptyDrawBox( b3Vec3 extents, b3Transform transform, b3HexColor color, void* context ) +{ + B3_UNUSED( extents, transform, color, context ); +} + +static void b3EmptyDrawString( b3Vec3 p, const char* s, b3HexColor color, void* context ) +{ + B3_UNUSED( p, s, color, context ); +} + +b3DebugDraw b3DefaultDebugDraw( void ) +{ + b3DebugDraw draw = { 0 }; + + // These allow the user to skip some implementations and not hit null exceptions. + draw.DrawShapeFcn = b3EmptyDrawShape; + draw.DrawSegmentFcn = b3EmptyDrawSegment; + draw.DrawTransformFcn = b3EmptyDrawTransform; + draw.DrawPointFcn = b3EmptyDrawPoint; + draw.DrawBoundsFcn = b3EmptyDrawBounds; + draw.DrawBoxFcn = b3EmptyDrawBox; + draw.DrawStringFcn = b3EmptyDrawString; + + draw.jointScale = 1.0f; + draw.forceScale = 1.0f; + + return draw; +} diff --git a/src/verstable.h b/src/verstable.h new file mode 100644 index 0000000..c8601e6 --- /dev/null +++ b/src/verstable.h @@ -0,0 +1,2069 @@ +/*------------------------------------------------- VERSTABLE v2.2.1 --------------------------------------------------- + +Verstable is a C99-compatible, open-addressing hash table using quadratic probing and the following additions: + +* All keys that hash (i.e. "belong") to the same bucket (their "home bucket") are linked together by an 11-bit integer + specifying the quadratic displacement, relative to that bucket, of the next key in the chain. + +* If a chain of keys exists for a given bucket, then it always begins at that bucket. To maintain this policy, a 1-bit + flag is used to mark whether the key occupying a bucket belongs there. When inserting a new key, if the bucket it + belongs to is occupied by a key that does not belong there, then the occupying key is evicted and the new key takes + the bucket. + +* A 4-bit fragment of each key's hash code is also stored. + +* The aforementioned metadata associated with each bucket (the 4-bit hash fragment, the 1-bit flag, and the 11-bit link + to the next key in the chain) are stored together in a uint16_t array rather than in the bucket alongside the key and + (optionally) the value. + +One way to conceptualize this scheme is as a chained hash table in which overflowing keys are stored not in separate +memory allocations but in otherwise unused buckets. In this regard, it shares similarities with Malte Skarupke's Bytell +hash table (https://www.youtube.com/watch?v=M2fKMP47slQ) and traditional "coalesced hashing". + +Advantages of this scheme include: + +* Fast lookups impervious to load factor: If the table contains any key belonging to the lookup key's home bucket, then + that bucket contains the first in a traversable chain of all keys belonging to it. Hence, only the home bucket and + other buckets containing keys belonging to it are ever probed. Moreover, the stored hash fragments allow skipping most + non-matching keys in the chain without accessing the actual buckets array or calling the (potentially expensive) key + comparison function. + +* Fast insertions: Insertions are faster than they are in other schemes that move keys around (e.g. Robin Hood) because + they only move, at most, one existing key. + +* Fast, tombstone-free deletions: Deletions, which usually require tombstones in quadratic-probing hash tables, are + tombstone-free and only move, at most, one existing key. + +* Fast iteration: The separate metadata array allows keys in sparsely populated tables to be found without incurring the + frequent cache misses that would result from traversing the buckets array. + +Usage example: + + +---------------------------------------------------------+----------------------------------------------------------+ + | Using the generic macro API (C11 and later): | Using the prefixed functions API (C99 and later): | + +---------------------------------------------------------+----------------------------------------------------------+ + | #include | #include | + | | | + | // Instantiating a set template. | // Instantiating a set template. | + | #define NAME int_set | #define NAME int_set | + | #define KEY_TY int | #define KEY_TY int | + | #include "verstable.h" | #define HASH_FN vt_hash_integer | + | | #define CMPR_FN vt_cmpr_integer | + | // Instantiating a map template. | #include "verstable.h" | + | #define NAME int_int_map | | + | #define KEY_TY int | // Instantiating a map template. | + | #define VAL_TY int | #define NAME int_int_map | + | #include "verstable.h" | #define KEY_TY int | + | | #define VAL_TY int | + | int main( void ) | #define HASH_FN vt_hash_integer | + | { | #define CMPR_FN vt_cmpr_integer | + | // Set. | #include "verstable.h" | + | | | + | int_set our_set; | int main( void ) | + | vt_init( &our_set ); | { | + | | // Set. | + | // Inserting keys. | | + | for( int i = 0; i < 10; ++i ) | int_set our_set; | + | { | int_set_init( &our_set ); | + | int_set_itr itr = vt_insert( &our_set, i ); | | + | if( vt_is_end( itr ) ) | // Inserting keys. | + | { | for( int i = 0; i < 10; ++i ) | + | // Out of memory, so abort. | { | + | vt_cleanup( &our_set ); | int_set_itr itr = | + | return 1; | int_set_insert( &our_set, i ); | + | } | if( int_set_is_end( itr ) ) | + | } | { | + | | // Out of memory, so abort. | + | // Erasing keys. | int_set_cleanup( &our_set ); | + | for( int i = 0; i < 10; i += 3 ) | return 1; | + | vt_erase( &our_set, i ); | } | + | | } | + | // Retrieving keys. | | + | for( int i = 0; i < 10; ++i ) | // Erasing keys. | + | { | for( int i = 0; i < 10; i += 3 ) | + | int_set_itr itr = vt_get( &our_set, i ); | int_set_erase( &our_set, i ); | + | if( !vt_is_end( itr ) ) | | + | printf( "%d ", itr.data->key ); | // Retrieving keys. | + | } | for( int i = 0; i < 10; ++i ) | + | // Printed: 1 2 4 5 7 8 | { | + | | int_set_itr itr = int_set_get( &our_set, i ); | + | // Iteration. | if( !int_set_is_end( itr ) ) | + | for( | printf( "%d ", itr.data->key ); | + | int_set_itr itr = vt_first( &our_set ); | } | + | !vt_is_end( itr ); | // Printed: 1 2 4 5 7 8 | + | itr = vt_next( itr ) | | + | ) | // Iteration. | + | printf( "%d ", itr.data->key ); | for( | + | // Printed: 2 4 7 1 5 8 | int_set_itr itr = | + | | int_set_first( &our_set ); | + | vt_cleanup( &our_set ); | !int_set_is_end( itr ); | + | | itr = int_set_next( itr ) | + | // Map. | ) | + | | printf( "%d ", itr.data->key ); | + | int_int_map our_map; | // Printed: 2 4 7 1 5 8 | + | vt_init( &our_map ); | | + | | int_set_cleanup( &our_set ); | + | // Inserting keys and values. | | + | for( int i = 0; i < 10; ++i ) | // Map. | + | { | | + | int_int_map_itr itr = | int_int_map our_map; | + | vt_insert( &our_map, i, i + 1 ); | int_int_map_init( &our_map ); | + | if( vt_is_end( itr ) ) | | + | { | // Inserting keys and values. | + | // Out of memory, so abort. | for( int i = 0; i < 10; ++i ) | + | vt_cleanup( &our_map ); | { | + | return 1; | int_int_map_itr itr = | + | } | int_int_map_insert( &our_map, i, i + 1 ); | + | } | if( int_int_map_is_end( itr ) ) | + | | { | + | // Erasing keys and values. | // Out of memory, so abort. | + | for( int i = 0; i < 10; i += 3 ) | int_int_map_cleanup( &our_map ); | + | vt_erase( &our_map, i ); | return 1; | + | | } | + | // Retrieving keys and values. | } | + | for( int i = 0; i < 10; ++i ) | | + | { | // Erasing keys and values. | + | int_int_map_itr itr = vt_get( &our_map, i ); | for( int i = 0; i < 10; i += 3 ) | + | if( !vt_is_end( itr ) ) | int_int_map_erase( &our_map, i ); | + | printf( | | + | "%d:%d ", | // Retrieving keys and values. | + | itr.data->key, | for( int i = 0; i < 10; ++i ) | + | itr.data->val | { | + | ); | int_int_map_itr itr = | + | } | int_int_map_get( &our_map, i ); | + | // Printed: 1:2 2:3 4:5 5:6 7:8 8:9 | if( !int_int_map_is_end( itr ) ) | + | | printf( | + | // Iteration. | "%d:%d ", | + | for( | itr.data->key, | + | int_int_map_itr itr = vt_first( &our_map ); | itr.data->val | + | !vt_is_end( itr ); | ); | + | itr = vt_next( itr ) | } | + | ) | // Printed: 1:2 2:3 4:5 5:6 7:8 8:9 | + | printf( | | + | "%d:%d ", | // Iteration. | + | itr.data->key, | for( | + | itr.data->val | int_int_map_itr itr = | + | ); | int_int_map_first( &our_map ); | + | // Printed: 2:3 4:5 7:8 1:2 5:6 8:9 | !int_int_map_is_end( itr ); | + | | itr = int_int_map_next( itr ) | + | vt_cleanup( &our_map ); | ) | + | } | printf( | + | | "%d:%d ", | + | | itr.data->key, | + | | itr.data->val | + | | ); | + | | // Printed: 2:3 4:5 7:8 1:2 5:6 8:9 | + | | | + | | int_int_map_cleanup( &our_map ); | + | | } | + | | | + +---------------------------------------------------------+----------------------------------------------------------+ + +API: + + Instantiating a hash table template: + + Create a new hash table type in the following manner: + + #define NAME + #define KEY_TY + #include "verstable.h" + + The NAME macro specifies the name of hash table type that the library will declare, the prefix for the functions + associated with it, and the prefix for the associated iterator type. + + The KEY_TY macro specifies the key type. + + In C99, it is also always necessary to define HASH_FN and CMPR_FN (see below) before including the header. + + The following macros may also be defined before including the header: + + #define VAL_TY + + The type of the value associated with each key. + If this macro is defined, the hash table acts as a map associating keys with values. + Otherwise, it acts as a set containing only keys. + + #define HASH_FN + + The name of the existing function used to hash each key. + The function should have the signature uint64_t ( KEY_TY key ) and return a 64-bit hash code. + For best performance, the hash function should provide a high level of entropy across all bits. + There are two default hash functions: vt_hash_integer for all integer types up to 64 bits in size, and + vt_hash_string for NULL-terminated strings (i.e. char *). + When KEY_TY is one of such types and the compiler is in C11 mode or later, HASH_FN may be left undefined, in + which case the appropriate default function is inferred from KEY_TY. + Otherwise, HASH_FN must be defined. + + #define CMPR_FN + + The name of the existing function used to compare two keys. + The function should have the signature bool ( KEY_TY key_1, KEY_TY key_2 ) and return true if the two keys are + equal. + There are two default comparison functions: vt_cmpr_integer for all integer types up to 64 bits in size, and + vt_cmpr_string for NULL-terminated strings (i.e. char *). + As with the default hash functions, in C11 or later the appropriate default comparison function is inferred if + KEY_TY is one of such types and CMPR_FN is left undefined. + Otherwise, CMPR_FN must be defined. + + #define MAX_LOAD + + The floating-point load factor at which the hash table automatically doubles the size of its internal buckets + array. + The default is 0.9, i.e. 90%. + + #define KEY_DTOR_FN + + The name of the existing destructor function, with the signature void ( KEY_TY key ), called on a key when it is + erased from the table or replaced by a newly inserted key. + The API functions that may call the key destructor are NAME_insert, NAME_erase, NAME_erase_itr, NAME_clear, + and NAME_cleanup. + + #define VAL_DTOR_FN + + The name of the existing destructor function, with the signature void ( VAL_TY val ), called on a value when it + is erased from the table or replaced by a newly inserted value. + The API functions that may call the value destructor are NAME_insert, NAME_erase, NAME_erase_itr, NAME_clear, + and NAME_cleanup. + + #define CTX_TY + + The type of the hash table type's ctx (context) member. + This member only exists if CTX_TY was defined. + It is intended to be used in conjunction with MALLOC_FN and FREE_FN (see below). + + #define MALLOC_FN + + The name of the existing function used to allocate memory. + If CTX_TY was defined, the signature should be void *( size_t size, CTX_TY *ctx ), where size is the number of + bytes to allocate and ctx points to the table's ctx member. + Otherwise, the signature should be void *( size_t size ). + The default wraps stdlib.h's malloc. + + #define FREE_FN + + The name of the existing function used to free memory. + If CTX_TY was defined, the signature should be void ( void *ptr, size_t size, CTX_TY *ctx ), where ptr points to + the memory to free, size is the number of bytes that were allocated, and ctx points to the table's ctx member. + Otherwise, the signature should be void ( void *ptr, size_t size ). + The default wraps stdlib.h's free. + + #define HEADER_MODE + #define IMPLEMENTATION_MODE + + By default, all hash table functions are defined as static inline functions, the intent being that a given hash + table template should be instantiated once per translation unit; for best performance, this is the recommended + way to use the library. + However, it is also possible to separate the struct definitions and function declarations from the function + definitions such that one implementation can be shared across all translation units (as in a traditional header + and source file pair). + In that case, instantiate a template wherever it is needed by defining HEADER_MODE, along with only NAME, + KEY_TY, and (optionally) VAL_TY, CTX_TY, and header guards, and including the library, e.g.: + + #ifndef INT_INT_MAP_H + #define INT_INT_MAP_H + #define NAME int_int_map + #define KEY_TY int + #define VAL_TY int + #define HEADER_MODE + #include "verstable.h" + #endif + + In one source file, define IMPLEMENTATION_MODE, along with NAME, KEY_TY, and any of the aforementioned optional + macros, and include the library, e.g.: + + #define NAME int_int_map + #define KEY_TY int + #define VAL_TY int + #define HASH_FN vt_hash_integer // C99. + #define CMPR_FN vt_cmpr_integer // C99. + #define MAX_LOAD 0.8 + #define IMPLEMENTATION_MODE + #include "verstable.h" + + Including the library automatically undefines all the aforementioned macros after they have been used to instantiate + the template. + + Functions: + + The functions associated with a hash table type are all prefixed with the name the user supplied via the NAME macro. + In C11 and later, the generic "vt_"-prefixed macros may be used to automatically select the correct version of the + specified function based on the arguments. + + void NAME_init( NAME *table ) + void NAME_init( NAME *table, CTX_TY ctx ) + // C11 generic macro: vt_init. + + Initializes the table for use. + If CTX_TY was defined, ctx sets the table's ctx member. + + bool NAME_init_clone( NAME *table, NAME *source ) + bool NAME_init_clone( NAME *table, NAME *source, CTX_TY ctx ) + // C11 generic macro: vt_init_clone. + + Initializes the table as a shallow copy of the specified source table. + If CTX_TY was defined, ctx sets the table's ctx member. + Returns false in the case of memory allocation failure. + + size_t NAME_size( NAME *table ) // C11 generic macro: vt_size. + + Returns the number of keys currently in the table. + + size_t NAME_bucket_count( NAME *table ) // C11 generic macro: vt_bucket_count. + + Returns the table's current bucket count. + + NAME_itr NAME_insert( NAME *table, KEY_TY key ) + NAME_itr NAME_insert( NAME *table, KEY_TY key, VAL_TY val ) + // C11 generic macro: vt_insert. + + Inserts the specified key (and value, if VAL_TY was defined) into the hash table. + If the same key already exists, then the new key (and value) replaces the existing key (and value). + Returns an iterator to the new key, or an end iterator in the case of memory allocation failure. + + NAME_itr NAME_get_or_insert( NAME *table, KEY_TY key ) + NAME_itr NAME_get_or_insert( NAME *table, KEY_TY key, VAL_TY val ) + // C11 generic macro: vt_get_or_insert. + + Inserts the specified key (and value, if VAL_TY was defined) if it does not already exist in the table. + Returns an iterator to the new key if it was inserted, or an iterator to the existing key, or an end iterator if + the key did not exist but the new key could not be inserted because of memory allocation failure. + Determine whether the key was inserted by comparing the table's size before and after the call. + + NAME_itr NAME_get( NAME *table, KEY_TY key ) // C11 generic macro: vt_get. + + Returns a iterator to the specified key, or an end iterator if no such key exists. + + bool NAME_erase( NAME *table, KEY_TY key ) // C11 generic macro: vt_erase. + + Erases the specified key (and associated value, if VAL_TY was defined), if it exists. + Returns true if a key was erased. + + NAME_itr NAME_erase_itr( NAME *table, NAME_itr itr ) // C11 generic macro: vt_erase_itr. + + Erases the key (and associated value, if VAL_TY was defined) pointed to by the specified iterator. + Returns an iterator to the next key in the table, or an end iterator if the erased key was the last one. + + bool NAME_reserve( NAME *table, size_t size ) // C11 generic macro: vt_reserve. + + Ensures that the bucket count is large enough to support the specified key count (i.e. size) without rehashing. + Returns false if unsuccessful due to memory allocation failure. + + bool NAME_shrink( NAME *table ) // C11 generic macro: vt_shrink. + + Shrinks the bucket count to best accommodate the current size. + Returns false if unsuccessful due to memory allocation failure. + + NAME_itr NAME_first( NAME *table ) // C11 generic macro: vt_first. + + Returns an iterator to the first key in the table, or an end iterator if the table is empty. + + bool NAME_is_end( NAME_itr itr ) // C11 generic macro: vt_is_end. + + Returns true if the iterator is an end iterator. + + NAME_itr NAME_next( NAME_itr itr ) // C11 generic macro: vt_next. + + Returns an iterator to the key after the one pointed to by the specified iterator, or an end iterator if the + specified iterator points to the last key in the table. + + void NAME_clear( NAME *table ) // C11 generic macro: vt_clear. + + Erases all keys (and values, if VAL_TY was defined) in the table. + + void NAME_cleanup( NAME *table ) // C11 generic macro: vt_cleanup. + + Erases all keys (and values, if VAL_TY was defined) in the table, frees all memory associated with it, and + initializes it for reuse. + + Iterators: + + Access the key (and value, if VAL_TY was defined) that an iterator points to using the NAME_itr struct's data + member: + + itr.data->key + itr.data->val + + Functions that may insert new keys (NAME_insert and NAME_get_or_insert), erase keys (NAME_erase and NAME_erase_itr), + or reallocate the internal bucket array (NAME_reserve and NAME_shrink) invalidate all existing iterators. + To delete keys during iteration and resume iterating, use the return value of NAME_erase_itr. + +Version history: + + 06/05/2025 2.2.1: Fixed incorrect signature of NAME_is_end in documentation. + 18/04/2025 2.2.0: Added const qualifier to the table parameter of NAME_size, NAME_bucket_count, NAME_get, and + NAME_first and to the source parameter of NAME_init_clone. + Added default support for const char * strings. + Added support for -Wextra. + Replaced FNV-1a with Wyhash as the default string hash function. + 18/06/2024 2.1.1: Fixed a bug affecting iteration on big-endian platforms under MSVC. + 27/05/2024 2.1.0: Replaced the Murmur3 mixer with the fast-hash mixer as the default integer hash function. + Fixed a bug that could theoretically cause a crash on rehash (triggerable in testing using + NAME_shrink with a maximum load factor significantly higher than 1.0). + 06/02/2024 2.0.0: Improved custom allocator support by introducing the CTX_TY option and allowing user-supplied free + functions to receive the allocation size. + Improved documentation. + Introduced various optimizations, including storing the buckets-array size mask instead of the + bucket count, eliminating empty-table checks, combining the buckets memory and metadata memory into + one allocation, and adding branch prediction macros. + Fixed a bug that caused a key to be used after destruction during erasure. + 12/12/2023 1.0.0: Initial release. + +License (MIT): + + Copyright (c) 2023-2025 Jackson L. Allan + + Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated + documentation files (the "Software"), to deal in the Software without restriction, including without limitation the + rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit + persons to whom the Software is furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all copies or substantial portions of the + Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +*/ + +/*--------------------------------------------------------------------------------------------------------------------*/ +/* Common header section */ +/*--------------------------------------------------------------------------------------------------------------------*/ + +#ifndef VERSTABLE_H +#define VERSTABLE_H + +#include +#include +#include +#include +#include +#include + +// Two-way concatenation macro. +#define VT_CAT_( a, b ) a##b +#define VT_CAT( a, b ) VT_CAT_( a, b ) + +// Branch optimization macros. +#ifdef __GNUC__ +#define VT_LIKELY( expression ) __builtin_expect( (bool)( expression ), true ) +#define VT_UNLIKELY( expression ) __builtin_expect( (bool)( expression ), false ) +#else +#define VT_LIKELY( expression ) ( expression ) +#define VT_UNLIKELY( expression ) ( expression ) +#endif + +// Masks for manipulating and extracting data from a bucket's uint16_t metadatum. +#define VT_EMPTY 0x0000 +#define VT_HASH_FRAG_MASK 0xF000 // 0b1111000000000000. +#define VT_IN_HOME_BUCKET_MASK 0x0800 // 0b0000100000000000. +#define VT_DISPLACEMENT_MASK 0x07FF // 0b0000011111111111, also denotes the displacement limit. Set to VT_LOAD to 1.0 + // to test proper handling of encroachment on the displacement limit during + // inserts. + +// Extracts a hash fragment from a uint64_t hash code. +// We take the highest four bits so that keys that map (via modulo) to the same bucket have distinct hash fragments. +static inline uint16_t vt_hashfrag( uint64_t hash ) +{ + return ( hash >> 48 ) & VT_HASH_FRAG_MASK; +} + +// Standard quadratic probing formula that guarantees that all buckets are visited when the bucket count is a power of +// two (at least in theory, because the displacement limit could terminate the search early when the bucket count is +// high). +static inline size_t vt_quadratic( uint16_t displacement ) +{ + return ( (size_t)displacement * displacement + displacement ) / 2; +} + +#define VT_MIN_NONZERO_BUCKET_COUNT 8 // Must be a power of two. + +// Function to find the left-most non-zero uint16_t in a uint64_t. +// This function is used when we scan four buckets at a time while iterating and relies on compiler intrinsics wherever +// possible. + +#if defined( __GNUC__ ) && ULLONG_MAX == 0xFFFFFFFFFFFFFFFF + +static inline int vt_first_nonzero_uint16( uint64_t val ) +{ + const uint16_t endian_checker = 0x0001; + if( *(const char *)&endian_checker ) // Little-endian (the compiler will optimize away the check at -O1 and above). + return __builtin_ctzll( val ) / 16; + + return __builtin_clzll( val ) / 16; +} + +#elif defined( _MSC_VER ) && ( defined( _M_X64 ) || defined( _M_ARM64 ) ) + +#include +#pragma intrinsic(_BitScanForward64) +#pragma intrinsic(_BitScanReverse64) + +static inline int vt_first_nonzero_uint16( uint64_t val ) +{ + unsigned long result; + + const uint16_t endian_checker = 0x0001; + if( *(const char *)&endian_checker ) + _BitScanForward64( &result, val ); + else + { + _BitScanReverse64( &result, val ); + result = 63 - result; + } + + return result / 16; +} + +#else + +static inline int vt_first_nonzero_uint16( uint64_t val ) +{ + int result = 0; + + uint32_t half; + memcpy( &half, &val, sizeof( uint32_t ) ); + if( !half ) + result += 2; + + uint16_t quarter; + memcpy( &quarter, (char *)&val + result * sizeof( uint16_t ), sizeof( uint16_t ) ); + if( !quarter ) + result += 1; + + return result; +} + +#endif + +// When the bucket count is zero, setting the metadata pointer to point to a VT_EMPTY placeholder, rather than NULL, +// allows us to avoid checking for a zero bucket count during insertion and lookup. +static const uint16_t vt_empty_placeholder_metadatum = VT_EMPTY; + +// Default hash and comparison functions. + +// Fast-hash, as described by https://jonkagstrom.com/bit-mixer-construction and +// https://code.google.com/archive/p/fast-hash. +// In testing, this hash function provided slightly better performance than the Murmur3 mixer. +static inline uint64_t vt_hash_integer( uint64_t key ) +{ + key ^= key >> 23; + key *= 0x2127599bf4325c37ull; + key ^= key >> 47; + return key; +} + +// For hashing strings, we use the public-domain Wyhash +// (https://github.com/wangyi-fudan/wyhash) with the following modifications: +// * We use a fixed seed and secret (the defaults suggested in the Wyhash repository). +// * We do not handle endianness, so the result will differ depending on the platform. +// * We omit the code optimized for 32-bit platforms. + +static inline void vt_wymum( uint64_t *a, uint64_t *b ) +{ +#if defined( __SIZEOF_INT128__ ) + __uint128_t r = *a; + r *= *b; + *a = (uint64_t)r; + *b = (uint64_t)( r >> 64 ); +#elif defined( _MSC_VER ) && defined( _M_X64 ) + *a = _umul128( *a, *b, b ); +#else + uint64_t ha = *a >> 32; + uint64_t hb = *b >> 32; + uint64_t la = (uint32_t)*a; + uint64_t lb = (uint32_t)*b; + uint64_t rh = ha * hb; + uint64_t rm0 = ha * lb; + uint64_t rm1 = hb * la; + uint64_t rl = la * lb; + uint64_t t = rl + ( rm0 << 32 ); + uint64_t c = t < rl; + uint64_t lo = t + ( rm1 << 32 ); + c += lo < t; + uint64_t hi = rh + ( rm0 >> 32 ) + ( rm1 >> 32 ) + c; + *a = lo; + *b = hi; +#endif +} + +static inline uint64_t vt_wymix( uint64_t a, uint64_t b ) +{ + vt_wymum( &a, &b ); + return a ^ b; +} + +static inline uint64_t vt_wyr8( const unsigned char *p ) +{ + uint64_t v; + memcpy( &v, p, 8 ); + return v; +} + +static inline uint64_t vt_wyr4( const unsigned char *p ) +{ + uint32_t v; + memcpy( &v, p, 4 ); + return v; +} + +static inline uint64_t vt_wyr3( const unsigned char *p, size_t k ) +{ + return ( ( (uint64_t)p[ 0 ] ) << 16 ) | ( ( (uint64_t)p[ k >> 1 ] ) << 8 ) | p[ k - 1 ]; +} + +static inline size_t vt_wyhash( const void *key, size_t len ) +{ + const unsigned char *p = (const unsigned char *)key; + uint64_t seed = 0xca813bf4c7abf0a9ull; + uint64_t a; + uint64_t b; + if( VT_LIKELY( len <= 16 ) ) + { + if( VT_LIKELY( len >= 4 ) ) + { + a = ( vt_wyr4( p ) << 32 ) | vt_wyr4( p + ( ( len >> 3 ) << 2 ) ); + b = ( vt_wyr4( p + len - 4 ) << 32 ) | vt_wyr4( p + len - 4 - ( ( len >> 3 ) << 2 ) ); + } + else if( VT_LIKELY( len > 0 ) ) + { + a = vt_wyr3( p, len ); + b = 0; + } + else + { + a = 0; + b = 0; + } + } + else + { + size_t i = len; + if( VT_UNLIKELY( i >= 48 ) ) + { + uint64_t see1 = seed; + uint64_t see2 = seed; + do{ + seed = vt_wymix( vt_wyr8( p ) ^ 0x8bb84b93962eacc9ull, vt_wyr8( p + 8 ) ^ seed ); + see1 = vt_wymix( vt_wyr8( p + 16 ) ^ 0x4b33a62ed433d4a3ull, vt_wyr8( p + 24 ) ^ see1 ); + see2 = vt_wymix( vt_wyr8( p + 32 ) ^ 0x4d5a2da51de1aa47ull, vt_wyr8( p + 40 ) ^ see2 ); + p += 48; + i -= 48; + } + while( VT_LIKELY( i >= 48 ) ); + seed ^= see1 ^ see2; + } + + while( VT_UNLIKELY( i > 16 ) ) + { + seed = vt_wymix( vt_wyr8( p ) ^ 0x8bb84b93962eacc9ull, vt_wyr8( p + 8 ) ^ seed ); + i -= 16; + p += 16; + } + + a = vt_wyr8( p + i - 16 ); + b = vt_wyr8( p + i - 8 ); + } + + a ^= 0x8bb84b93962eacc9ull; + b ^= seed; + vt_wymum( &a, &b ); + return (size_t)vt_wymix( a ^ 0x2d358dccaa6c78a5ull ^ len, b ^ 0x8bb84b93962eacc9ull ); +} + +static inline uint64_t vt_hash_string( const char *key ) +{ + return vt_wyhash( key, strlen( key ) ); +} + +static inline bool vt_cmpr_integer( uint64_t key_1, uint64_t key_2 ) +{ + return key_1 == key_2; +} + +static inline bool vt_cmpr_string( const char *key_1, const char *key_2 ) +{ + return strcmp( key_1, key_2 ) == 0; +} + +// Default allocation and free functions. + +static inline void *vt_malloc( size_t size ) +{ + return malloc( size ); +} + +static inline void vt_free( void *ptr, size_t size ) +{ + (void)size; + free( ptr ); +} + +static inline void *vt_malloc_with_ctx( size_t size, void *ctx ) +{ + (void)ctx; + return malloc( size ); +} + +static inline void vt_free_with_ctx( void *ptr, size_t size, void *ctx ) +{ + (void)size; + (void)ctx; + free( ptr ); +} + +// The rest of the common header section pertains to the C11 generic macro API. +// This interface is based on the extendible-_Generic mechanism documented in detail at +// https://github.com/JacksonAllan/CC/blob/main/articles/Better_C_Generics_Part_1_The_Extendible_Generic.md. +// In summary, instantiating a template also defines wrappers for the template's types and functions with names in the +// pattern of vt_table_NNNN and vt_init_NNNN, where NNNN is an automatically generated integer unique to the template +// instance in the current translation unit. +// These wrappers plug into _Generic-based API macros, which use preprocessor magic to automatically generate _Generic +// slots for every existing template instance. +#if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L && !defined( VT_NO_C11_GENERIC_API ) + +// Octal counter that supports up to 511 hash table templates. +#define VT_TEMPLATE_COUNT_D1 0 // Digit 1, i.e. least significant digit. +#define VT_TEMPLATE_COUNT_D2 0 +#define VT_TEMPLATE_COUNT_D3 0 + +// Four-way concatenation macro. +#define VT_CAT_4_( a, b, c, d ) a##b##c##d +#define VT_CAT_4( a, b, c, d ) VT_CAT_4_( a, b, c, d ) + +// Provides the current value of the counter as a three-digit octal number preceded by 0. +#define VT_TEMPLATE_COUNT VT_CAT_4( 0, VT_TEMPLATE_COUNT_D3, VT_TEMPLATE_COUNT_D2, VT_TEMPLATE_COUNT_D1 ) + +// _Generic-slot generation macros. + +#define VT_GENERIC_SLOT( ty, fn, n ) , VT_CAT( ty, n ): VT_CAT( fn, n ) +#define VT_R1_0( ty, fn, d3, d2 ) +#define VT_R1_1( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 0 ) ) +#define VT_R1_2( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 1 ) ) VT_R1_1( ty, fn, d3, d2 ) +#define VT_R1_3( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 2 ) ) VT_R1_2( ty, fn, d3, d2 ) +#define VT_R1_4( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 3 ) ) VT_R1_3( ty, fn, d3, d2 ) +#define VT_R1_5( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 4 ) ) VT_R1_4( ty, fn, d3, d2 ) +#define VT_R1_6( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 5 ) ) VT_R1_5( ty, fn, d3, d2 ) +#define VT_R1_7( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 6 ) ) VT_R1_6( ty, fn, d3, d2 ) +#define VT_R1_8( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 7 ) ) VT_R1_7( ty, fn, d3, d2 ) +#define VT_R2_0( ty, fn, d3 ) +#define VT_R2_1( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 0 ) +#define VT_R2_2( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 1 ) VT_R2_1( ty, fn, d3 ) +#define VT_R2_3( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 2 ) VT_R2_2( ty, fn, d3 ) +#define VT_R2_4( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 3 ) VT_R2_3( ty, fn, d3 ) +#define VT_R2_5( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 4 ) VT_R2_4( ty, fn, d3 ) +#define VT_R2_6( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 5 ) VT_R2_5( ty, fn, d3 ) +#define VT_R2_7( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 6 ) VT_R2_6( ty, fn, d3 ) +#define VT_R2_8( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 7 ) VT_R2_7( ty, fn, d3 ) +#define VT_R3_0( ty, fn ) +#define VT_R3_1( ty, fn ) VT_R2_8( ty, fn, 0 ) +#define VT_R3_2( ty, fn ) VT_R2_8( ty, fn, 1 ) VT_R3_1( ty, fn ) +#define VT_R3_3( ty, fn ) VT_R2_8( ty, fn, 2 ) VT_R3_2( ty, fn ) +#define VT_R3_4( ty, fn ) VT_R2_8( ty, fn, 3 ) VT_R3_3( ty, fn ) +#define VT_R3_5( ty, fn ) VT_R2_8( ty, fn, 4 ) VT_R3_4( ty, fn ) +#define VT_R3_6( ty, fn ) VT_R2_8( ty, fn, 5 ) VT_R3_5( ty, fn ) +#define VT_R3_7( ty, fn ) VT_R2_8( ty, fn, 6 ) VT_R3_6( ty, fn ) + +#define VT_GENERIC_SLOTS( ty, fn ) \ +VT_CAT( VT_R1_, VT_TEMPLATE_COUNT_D1 )( ty, fn, VT_TEMPLATE_COUNT_D3, VT_TEMPLATE_COUNT_D2 ) \ +VT_CAT( VT_R2_, VT_TEMPLATE_COUNT_D2 )( ty, fn, VT_TEMPLATE_COUNT_D3 ) \ +VT_CAT( VT_R3_, VT_TEMPLATE_COUNT_D3 )( ty, fn ) \ + +// Actual generic API macros. + +// vt_init must be handled as a special case because it could take one or two arguments, depending on whether CTX_TY +// was defined. +#define VT_ARG_3( _1, _2, _3, ... ) _3 +#define vt_init( ... ) VT_ARG_3( __VA_ARGS__, vt_init_with_ctx, vt_init_without_ctx, )( __VA_ARGS__ ) +#define vt_init_without_ctx( table ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_init_ ) )( table ) +#define vt_init_with_ctx( table, ... ) _Generic( *( table ) \ + VT_GENERIC_SLOTS( vt_table_, vt_init_ ) \ +)( table, __VA_ARGS__ ) \ + +#define vt_init_clone( table, ... ) _Generic( *( table ) \ + VT_GENERIC_SLOTS( vt_table_, vt_init_clone_ ) \ +)( table, __VA_ARGS__ ) \ + +#define vt_size( table )_Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_size_ ) )( table ) + +#define vt_bucket_count( table ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_bucket_count_ ) )( table ) + +#define vt_is_end( itr ) _Generic( itr VT_GENERIC_SLOTS( vt_table_itr_, vt_is_end_ ) )( itr ) + +#define vt_insert( table, ... ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_insert_ ) )( table, __VA_ARGS__ ) + +#define vt_get_or_insert( table, ... ) _Generic( *( table ) \ + VT_GENERIC_SLOTS( vt_table_, vt_get_or_insert_ ) \ +)( table, __VA_ARGS__ ) \ + +#define vt_get( table, ... ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_get_ ) )( table, __VA_ARGS__ ) + +#define vt_erase( table, ... ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_erase_ ) )( table, __VA_ARGS__ ) + +#define vt_next( itr ) _Generic( itr VT_GENERIC_SLOTS( vt_table_itr_, vt_next_ ) )( itr ) + +#define vt_erase_itr( table, ... ) _Generic( *( table ) \ + VT_GENERIC_SLOTS( vt_table_, vt_erase_itr_ ) \ +)( table, __VA_ARGS__ ) \ + +#define vt_reserve( table, ... ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_reserve_ ) )( table, __VA_ARGS__ ) + +#define vt_shrink( table ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_shrink_ ) )( table ) + +#define vt_first( table ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_first_ ) )( table ) + +#define vt_clear( table ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_clear_ ) )( table ) + +#define vt_cleanup( table ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_cleanup_ ) )( table ) + +#endif + +#endif + +/*--------------------------------------------------------------------------------------------------------------------*/ +/* Prefixed structs */ +/*--------------------------------------------------------------------------------------------------------------------*/ + +#ifndef IMPLEMENTATION_MODE + +typedef struct +{ + KEY_TY key; + #ifdef VAL_TY + VAL_TY val; + #endif +} VT_CAT( NAME, _bucket ); + +typedef struct +{ + VT_CAT( NAME, _bucket ) *data; + uint16_t *metadatum; + uint16_t *metadata_end; // Iterators carry an internal end pointer so that NAME_is_end does not need the table to be + // passed in as an argument. + // This also allows for the zero-bucket-count check to occur once in NAME_first, rather than + // repeatedly in NAME_is_end. + size_t home_bucket; // SIZE_MAX if home bucket is unknown. +} VT_CAT( NAME, _itr ); + +typedef struct +{ + size_t key_count; + size_t buckets_mask; // Rather than storing the bucket count directly, we store the bit mask used to reduce a hash + // code or displacement-derived bucket index to the buckets array, i.e. the bucket count minus + // one. + // Consequently, a zero bucket count (i.e. when .metadata points to the placeholder) constitutes + // a special case, represented by all bits unset (i.e. zero). + VT_CAT( NAME, _bucket ) *buckets; + uint16_t *metadata; // As described above, each metadatum consists of a 4-bit hash-code fragment (X), a 1-bit flag + // indicating whether the key in this bucket begins a chain associated with the bucket (Y), and + // an 11-bit value indicating the quadratic displacement of the next key in the chain (Z): + // XXXXYZZZZZZZZZZZ. + #ifdef CTX_TY + CTX_TY ctx; + #endif +} NAME; + +#endif + +/*--------------------------------------------------------------------------------------------------------------------*/ +/* Function prototypes */ +/*--------------------------------------------------------------------------------------------------------------------*/ + +#if defined( HEADER_MODE ) || defined( IMPLEMENTATION_MODE ) +#define VT_API_FN_QUALIFIERS +#else +#define VT_API_FN_QUALIFIERS static inline +#endif + +#ifndef IMPLEMENTATION_MODE + +VT_API_FN_QUALIFIERS void VT_CAT( NAME, _init )( + NAME * + #ifdef CTX_TY + , CTX_TY + #endif +); + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _init_clone )( + NAME *, + const NAME * + #ifdef CTX_TY + , CTX_TY + #endif +); + +VT_API_FN_QUALIFIERS size_t VT_CAT( NAME, _size )( const NAME * ); + +VT_API_FN_QUALIFIERS size_t VT_CAT( NAME, _bucket_count )( const NAME * ); + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _is_end )( VT_CAT( NAME, _itr ) ); + +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _insert )( + NAME *, + KEY_TY + #ifdef VAL_TY + , VAL_TY + #endif +); + +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _get_or_insert )( + NAME *, + KEY_TY + #ifdef VAL_TY + , VAL_TY + #endif +); + +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _get )( + const NAME *table, + KEY_TY key +); + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _erase )( NAME *, KEY_TY ); + +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _next )( VT_CAT( NAME, _itr ) ); + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _reserve )( NAME *, size_t ); + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _shrink )( NAME * ); + +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _first )( const NAME * ); + +VT_API_FN_QUALIFIERS void VT_CAT( NAME, _clear )( NAME * ); + +VT_API_FN_QUALIFIERS void VT_CAT( NAME, _cleanup )( NAME * ); + +// Not an API function, but must be prototyped anyway because it is called by the inline NAME_erase_itr below. +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _erase_itr_raw ) ( NAME *, VT_CAT( NAME, _itr ) ); + +// Erases the key pointed to by itr and returns an iterator to the next key in the table. +// This function must be inlined to ensure that the compiler optimizes away the NAME_fast_forward call if the returned +// iterator is discarded. +#ifdef __GNUC__ +static inline __attribute__((always_inline)) +#elif defined( _MSC_VER ) +static __forceinline +#else +static inline +#endif +VT_CAT( NAME, _itr ) VT_CAT( NAME, _erase_itr )( NAME *table, VT_CAT( NAME, _itr ) itr ) +{ + if( VT_CAT( NAME, _erase_itr_raw )( table, itr ) ) + return VT_CAT( NAME, _next )( itr ); + + return itr; +} + +#endif + +/*--------------------------------------------------------------------------------------------------------------------*/ +/* Function implementations */ +/*--------------------------------------------------------------------------------------------------------------------*/ + +#ifndef HEADER_MODE + +// Default settings. + +#ifndef MAX_LOAD +#define MAX_LOAD 0.9 +#endif + +#ifndef MALLOC_FN +#ifdef CTX_TY +#define MALLOC_FN vt_malloc_with_ctx +#else +#define MALLOC_FN vt_malloc +#endif +#endif + +#ifndef FREE_FN +#ifdef CTX_TY +#define FREE_FN vt_free_with_ctx +#else +#define FREE_FN vt_free +#endif +#endif + +#ifndef HASH_FN +#if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L +#ifdef _MSC_VER // In MSVC, the compound literal in the _Generic triggers a warning about unused local variables at /W4. +#define HASH_FN \ +_Pragma( "warning( push )" ) \ +_Pragma( "warning( disable: 4189 )" ) \ +_Generic( ( KEY_TY ){ 0 }, char *: vt_hash_string, const char*: vt_hash_string, default: vt_hash_integer ) \ +_Pragma( "warning( pop )" ) +#else +#define HASH_FN _Generic( ( KEY_TY ){ 0 }, \ + char *: vt_hash_string, \ + const char*: vt_hash_string, \ + default: vt_hash_integer \ +) +#endif +#else +#error Hash function inference is only available in C11 and later. In C99, you need to define HASH_FN manually to \ +vt_hash_integer, vt_hash_string, or your own custom function with the signature uint64_t ( KEY_TY ). +#endif +#endif + +#ifndef CMPR_FN +#if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L +#ifdef _MSC_VER +#define CMPR_FN \ +_Pragma( "warning( push )" ) \ +_Pragma( "warning( disable: 4189 )" ) \ +_Generic( ( KEY_TY ){ 0 }, char *: vt_cmpr_string, const char*: vt_cmpr_string, default: vt_cmpr_integer ) \ +_Pragma( "warning( pop )" ) +#else +#define CMPR_FN _Generic( ( KEY_TY ){ 0 }, \ + char *: vt_cmpr_string, \ + const char*: vt_cmpr_string, \ + default: vt_cmpr_integer \ +) +#endif +#else +#error Comparison function inference is only available in C11 and later. In C99, you need to define CMPR_FN manually \ +to vt_cmpr_integer, vt_cmpr_string, or your own custom function with the signature bool ( KEY_TY, KEY_TY ). +#endif +#endif + +VT_API_FN_QUALIFIERS void VT_CAT( NAME, _init )( + NAME *table + #ifdef CTX_TY + , CTX_TY ctx + #endif +) +{ + table->key_count = 0; + table->buckets_mask = 0x0000000000000000ull; + table->buckets = NULL; + table->metadata = (uint16_t *)&vt_empty_placeholder_metadatum; + #ifdef CTX_TY + table->ctx = ctx; + #endif +} + +// For efficiency, especially in the case of a small table, the buckets array and metadata share the same dynamic memory +// allocation: +// +-----------------------------+-----+----------------+--------+ +// | Buckets | Pad | Metadata | Excess | +// +-----------------------------+-----+----------------+--------+ +// Any allocated metadata array requires four excess elements to ensure that iteration functions, which read four +// metadata at a time, never read beyond the end of it. +// This function returns the offset of the beginning of the metadata, i.e. the size of the buckets array plus the +// (usually zero) padding. +// It assumes that the bucket count is not zero. +static inline size_t VT_CAT( NAME, _metadata_offset )( NAME *table ) +{ + // Use sizeof, rather than alignof, for C99 compatibility. + return ( ( ( table->buckets_mask + 1 ) * sizeof( VT_CAT( NAME, _bucket ) ) + sizeof( uint16_t ) - 1 ) / + sizeof( uint16_t ) ) * sizeof( uint16_t ); +} + +// Returns the total allocation size, including the buckets array, padding, metadata, and excess metadata. +// As above, this function assumes that the bucket count is not zero. +static inline size_t VT_CAT( NAME, _total_alloc_size )( NAME *table ) +{ + return VT_CAT( NAME, _metadata_offset )( table ) + ( table->buckets_mask + 1 + 4 ) * sizeof( uint16_t ); +} + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _init_clone )( + NAME *table, + const NAME *source + #ifdef CTX_TY + , CTX_TY ctx + #endif +) +{ + table->key_count = source->key_count; + table->buckets_mask = source->buckets_mask; + #ifdef CTX_TY + table->ctx = ctx; + #endif + + if( !source->buckets_mask ) + { + table->metadata = (uint16_t *)&vt_empty_placeholder_metadatum; + table->buckets = NULL; + return true; + } + + void *allocation = MALLOC_FN( + VT_CAT( NAME, _total_alloc_size )( table ) + #ifdef CTX_TY + , &table->ctx + #endif + ); + + if( VT_UNLIKELY( !allocation ) ) + return false; + + table->buckets = (VT_CAT( NAME, _bucket ) *)allocation; + table->metadata = (uint16_t *)( (unsigned char *)allocation + VT_CAT( NAME, _metadata_offset )( table ) ); + memcpy( allocation, source->buckets, VT_CAT( NAME, _total_alloc_size )( table ) ); + + return true; +} + +VT_API_FN_QUALIFIERS size_t VT_CAT( NAME, _size )( const NAME *table ) +{ + return table->key_count; +} + +VT_API_FN_QUALIFIERS size_t VT_CAT( NAME, _bucket_count )( const NAME *table ) +{ + // If the bucket count is zero, buckets_mask will be zero, not the bucket count minus one. + // We account for this special case by adding (bool)buckets_mask rather than one. + return table->buckets_mask + (bool)table->buckets_mask; +} + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _is_end )( VT_CAT( NAME, _itr ) itr ) +{ + return itr.metadatum == itr.metadata_end; +} + +// Finds the earliest empty bucket in which a key belonging to home_bucket can be placed, assuming that home_bucket +// is already occupied. +// The reason to begin the search at home_bucket, rather than the end of the existing chain, is that keys deleted from +// other chains might have freed up buckets that could fall in this chain before the final key. +// Returns true if an empty bucket within the range of the displacement limit was found, in which case the final two +// pointer arguments contain the index of the empty bucket and its quadratic displacement from home_bucket. +static inline bool VT_CAT( NAME, _find_first_empty )( + NAME *table, + size_t home_bucket, + size_t *empty, + uint16_t *displacement +) +{ + *displacement = 1; + size_t linear_dispacement = 1; + + while( true ) + { + *empty = ( home_bucket + linear_dispacement ) & table->buckets_mask; + if( table->metadata[ *empty ] == VT_EMPTY ) + return true; + + if( VT_UNLIKELY( ++*displacement == VT_DISPLACEMENT_MASK ) ) + return false; + + linear_dispacement += *displacement; + } +} + +// Finds the key in the chain beginning in home_bucket after which to link a new key with displacement_to_empty +// quadratic displacement and returns the index of the bucket containing that key. +// Although the new key could simply be linked to the end of the chain, keeping the chain ordered by displacement +// theoretically improves cache locality during lookups. +static inline size_t VT_CAT( NAME, _find_insert_location_in_chain )( + NAME *table, + size_t home_bucket, + uint16_t displacement_to_empty +) +{ + size_t candidate = home_bucket; + while( true ) + { + uint16_t displacement = table->metadata[ candidate ] & VT_DISPLACEMENT_MASK; + + if( displacement > displacement_to_empty ) + return candidate; + + candidate = ( home_bucket + vt_quadratic( displacement ) ) & table->buckets_mask; + } +} + +// Frees up a bucket occupied by a key not belonging there so that a new key belonging there can be placed there as the +// beginning of a new chain. +// This requires: +// * Finding the previous key in the chain to which the occupying key belongs by rehashing it and then traversing the +// chain. +// * Disconnecting the key from the chain. +// * Finding the appropriate empty bucket to which to move the key. +// * Moving the key (and value) data to the empty bucket. +// * Re-linking the key to the chain. +// Returns true if the eviction succeeded, or false if no empty bucket to which to evict the occupying key could be +// found within the displacement limit. +static inline bool VT_CAT( NAME, _evict )( NAME *table, size_t bucket ) +{ + // Find the previous key in chain. + size_t home_bucket = HASH_FN( table->buckets[ bucket ].key ) & table->buckets_mask; + size_t prev = home_bucket; + while( true ) + { + size_t next = ( home_bucket + vt_quadratic( table->metadata[ prev ] & VT_DISPLACEMENT_MASK ) ) & + table->buckets_mask; + + if( next == bucket ) + break; + + prev = next; + } + + // Disconnect the key from chain. + table->metadata[ prev ] = ( table->metadata[ prev ] & ~VT_DISPLACEMENT_MASK ) | ( table->metadata[ bucket ] & + VT_DISPLACEMENT_MASK ); + + // Find the empty bucket to which to move the key. + size_t empty; + uint16_t displacement; + if( VT_UNLIKELY( !VT_CAT( NAME, _find_first_empty )( table, home_bucket, &empty, &displacement ) ) ) + return false; + + // Find the key in the chain after which to link the moved key. + prev = VT_CAT( NAME, _find_insert_location_in_chain )( table, home_bucket, displacement ); + + // Move the key (and value) data. + table->buckets[ empty ] = table->buckets[ bucket ]; + + // Re-link the key to the chain from its new bucket. + table->metadata[ empty ] = ( table->metadata[ bucket ] & VT_HASH_FRAG_MASK ) | ( table->metadata[ prev ] & + VT_DISPLACEMENT_MASK ); + table->metadata[ prev ] = ( table->metadata[ prev ] & ~VT_DISPLACEMENT_MASK ) | displacement; + + return true; +} + +// Returns an end iterator, i.e. any iterator for which .metadatum == .metadata_end. +// This function just cleans up the library code in functions that return an end iterator as a failure indicator. +static inline VT_CAT( NAME, _itr ) VT_CAT( NAME, _end_itr )( void ) +{ + VT_CAT( NAME, _itr ) itr = { NULL, NULL, NULL, 0 }; + return itr; +} + +// Inserts a key, optionally replacing the existing key if it already exists. +// There are two main cases that must be handled: +// * If the key's home bucket is empty or occupied by a key that does not belong there, then the key is inserted there, +// evicting the occupying key if there is one. +// * Otherwise, the chain of keys beginning at the home bucket is (if unique is false) traversed in search of a matching +// key. +// If none is found, then the new key is inserted at the earliest available bucket, per quadratic probing from the +// home bucket, and then linked to the chain in a manner that maintains its quadratic order. +// The unique argument tells the function whether to skip searching for the key before inserting it (on rehashing, this +// step is unnecessary). +// The replace argument tells the function whether to replace an existing key. +// If replace is true, the function returns an iterator to the inserted key, or an end iterator if the key was not +// inserted because of the maximum load factor or displacement limit constraints. +// If replace is false, then the return value is as described above, except that if the key already exists, the function +// returns an iterator to the existing key. +static inline VT_CAT( NAME, _itr ) VT_CAT( NAME, _insert_raw )( + NAME *table, + KEY_TY key, + #ifdef VAL_TY + VAL_TY *val, + #endif + bool unique, + bool replace +) +{ + uint64_t hash = HASH_FN( key ); + uint16_t hashfrag = vt_hashfrag( hash ); + size_t home_bucket = hash & table->buckets_mask; + + // Case 1: The home bucket is empty or contains a key that doesn't belong there. + // This case also implicitly handles the case of a zero bucket count, since home_bucket will be zero and metadata[ 0 ] + // will be the empty placeholder. + // In that scenario, the zero buckets_mask triggers the below load-factor check. + if( !( table->metadata[ home_bucket ] & VT_IN_HOME_BUCKET_MASK ) ) + { + if( + // Load-factor check. + VT_UNLIKELY( table->key_count + 1 > VT_CAT( NAME, _bucket_count )( table ) * MAX_LOAD ) || + // Vacate the home bucket if it contains a key. + ( table->metadata[ home_bucket ] != VT_EMPTY && VT_UNLIKELY( !VT_CAT( NAME, _evict )( table, home_bucket ) ) ) + ) + return VT_CAT( NAME, _end_itr )(); + + table->buckets[ home_bucket ].key = key; + #ifdef VAL_TY + table->buckets[ home_bucket ].val = *val; + #endif + table->metadata[ home_bucket ] = hashfrag | VT_IN_HOME_BUCKET_MASK | VT_DISPLACEMENT_MASK; + + ++table->key_count; + + VT_CAT( NAME, _itr ) itr = { + table->buckets + home_bucket, + table->metadata + home_bucket, + table->metadata + table->buckets_mask + 1, // Iteration stopper (i.e. the first of the four excess metadata). + home_bucket + }; + return itr; + } + + // Case 2: The home bucket contains the beginning of a chain. + + // Optionally, check the existing chain. + if( !unique ) + { + size_t bucket = home_bucket; + while( true ) + { + if( + ( table->metadata[ bucket ] & VT_HASH_FRAG_MASK ) == hashfrag && + VT_LIKELY( CMPR_FN( table->buckets[ bucket ].key, key ) ) + ) + { + if( replace ) + { + #ifdef KEY_DTOR_FN + KEY_DTOR_FN( table->buckets[ bucket ].key ); + #endif + table->buckets[ bucket ].key = key; + + #ifdef VAL_TY + #ifdef VAL_DTOR_FN + VAL_DTOR_FN( table->buckets[ bucket ].val ); + #endif + table->buckets[ bucket ].val = *val; + #endif + } + + VT_CAT( NAME, _itr ) itr = { + table->buckets + bucket, + table->metadata + bucket, + table->metadata + table->buckets_mask + 1, + home_bucket + }; + return itr; + } + + uint16_t displacement = table->metadata[ bucket ] & VT_DISPLACEMENT_MASK; + if( displacement == VT_DISPLACEMENT_MASK ) + break; + + bucket = ( home_bucket + vt_quadratic( displacement ) ) & table->buckets_mask; + } + } + + size_t empty; + uint16_t displacement; + if( + VT_UNLIKELY( + // Load-factor check. + table->key_count + 1 > VT_CAT( NAME, _bucket_count )( table ) * MAX_LOAD || + // Find the earliest empty bucket, per quadratic probing. + !VT_CAT( NAME, _find_first_empty )( table, home_bucket, &empty, &displacement ) + ) + ) + return VT_CAT( NAME, _end_itr )(); + + // Insert the new key (and value) in the empty bucket and link it to the chain. + + size_t prev = VT_CAT( NAME, _find_insert_location_in_chain )( table, home_bucket, displacement ); + + table->buckets[ empty ].key = key; + #ifdef VAL_TY + table->buckets[ empty ].val = *val; + #endif + table->metadata[ empty ] = hashfrag | ( table->metadata[ prev ] & VT_DISPLACEMENT_MASK ); + table->metadata[ prev ] = ( table->metadata[ prev ] & ~VT_DISPLACEMENT_MASK ) | displacement; + + ++table->key_count; + + VT_CAT( NAME, _itr ) itr = { + table->buckets + empty, + table->metadata + empty, + table->metadata + table->buckets_mask + 1, + home_bucket + }; + return itr; +} + +// Resizes the bucket array. +// This function assumes that bucket_count is a power of two and large enough to accommodate all keys without violating +// the maximum load factor. +// Returns false in the case of allocation failure. +// As this function is called very rarely in _insert and _get_or_insert, ideally it should not be inlined into those +// functions. +// In testing, the no-inline approach showed a performance benefit when inserting existing keys (i.e. replacing). +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wattributes" // Silence warning about combining noinline with static inline. +__attribute__((noinline)) static inline +#elif defined( _MSC_VER ) +__declspec(noinline) static inline +#else +static inline +#endif +bool VT_CAT( NAME, _rehash )( NAME *table, size_t bucket_count ) +{ + // The attempt to resize the bucket array and rehash the keys must occur inside a loop that incrementally doubles the + // target bucket count because a failure could theoretically occur at any load factor due to the displacement limit. + while( true ) + { + NAME new_table = { + 0, + bucket_count - 1, + NULL, + NULL + #ifdef CTX_TY + , table->ctx + #endif + }; + + void *allocation = MALLOC_FN( + VT_CAT( NAME, _total_alloc_size )( &new_table ) + #ifdef CTX_TY + , &new_table.ctx + #endif + ); + + if( VT_UNLIKELY( !allocation ) ) + return false; + + new_table.buckets = (VT_CAT( NAME, _bucket ) *)allocation; + new_table.metadata = (uint16_t *)( (unsigned char *)allocation + VT_CAT( NAME, _metadata_offset )( &new_table ) ); + + memset( new_table.metadata, 0x00, ( bucket_count + 4 ) * sizeof( uint16_t ) ); + + // Iteration stopper at the end of the actual metadata array (i.e. the first of the four excess metadata). + new_table.metadata[ bucket_count ] = 0x01; + + for( size_t bucket = 0; bucket < VT_CAT( NAME, _bucket_count )( table ); ++bucket ) + if( table->metadata[ bucket ] != VT_EMPTY ) + { + VT_CAT( NAME, _itr ) itr = VT_CAT( NAME, _insert_raw )( + &new_table, + table->buckets[ bucket ].key, + #ifdef VAL_TY + &table->buckets[ bucket ].val, + #endif + true, + false + ); + + if( VT_UNLIKELY( VT_CAT( NAME, _is_end )( itr ) ) ) + break; + } + + // If a key could not be reinserted due to the displacement limit, double the bucket count and retry. + if( VT_UNLIKELY( new_table.key_count < table->key_count ) ) + { + FREE_FN( + new_table.buckets, + VT_CAT( NAME, _total_alloc_size )( &new_table ) + #ifdef CTX_TY + , &new_table.ctx + #endif + ); + + bucket_count *= 2; + continue; + } + + if( table->buckets_mask ) + FREE_FN( + table->buckets, + VT_CAT( NAME, _total_alloc_size )( table ) + #ifdef CTX_TY + , &table->ctx + #endif + ); + + *table = new_table; + return true; + } +} +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif + +// Inserts a key, replacing the existing key if it already exists. +// This function wraps insert_raw in a loop that handles growing and rehashing the table if a new key cannot be inserted +// because of the maximum load factor or displacement limit constraints. +// Returns an iterator to the inserted key, or an end iterator in the case of allocation failure. +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _insert )( + NAME *table, + KEY_TY key + #ifdef VAL_TY + , VAL_TY val + #endif +) +{ + while( true ) + { + VT_CAT( NAME, _itr ) itr = VT_CAT( NAME, _insert_raw )( + table, + key, + #ifdef VAL_TY + &val, + #endif + false, + true + ); + + if( + // Lookup succeeded, in which case itr points to the found key. + VT_LIKELY( !VT_CAT( NAME, _is_end )( itr ) ) || + // Lookup failed and rehash also fails, in which case itr is an end iterator. + VT_UNLIKELY( + !VT_CAT( NAME, _rehash )( + table, table->buckets_mask ? VT_CAT( NAME, _bucket_count )( table ) * 2 : VT_MIN_NONZERO_BUCKET_COUNT + ) + ) + ) + return itr; + } +} + +// Same as NAME_insert, except that if the key already exists, no insertion occurs and the function returns an iterator +// to the existing key. +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _get_or_insert )( + NAME *table, + KEY_TY key + #ifdef VAL_TY + , VAL_TY val + #endif +) +{ + while( true ) + { + VT_CAT( NAME, _itr ) itr = VT_CAT( NAME, _insert_raw )( + table, + key, + #ifdef VAL_TY + &val, + #endif + false, + false + ); + + if( + // Lookup succeeded, in which case itr points to the found key. + VT_LIKELY( !VT_CAT( NAME, _is_end )( itr ) ) || + // Lookup failed and rehash also fails, in which case itr is an end iterator. + VT_UNLIKELY( + !VT_CAT( NAME, _rehash )( + table, table->buckets_mask ? VT_CAT( NAME, _bucket_count )( table ) * 2 : VT_MIN_NONZERO_BUCKET_COUNT + ) + ) + ) + return itr; + } +} + +// Returns an iterator pointing to the specified key, or an end iterator if the key does not exist. +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _get )( const NAME *table, KEY_TY key ) +{ + uint64_t hash = HASH_FN( key ); + size_t home_bucket = hash & table->buckets_mask; + + // If the home bucket is empty or contains a key that does not belong there, then our key does not exist. + // This check also implicitly handles the case of a zero bucket count, since home_bucket will be zero and + // metadata[ 0 ] will be the empty placeholder. + if( !( table->metadata[ home_bucket ] & VT_IN_HOME_BUCKET_MASK ) ) + return VT_CAT( NAME, _end_itr )(); + + // Traverse the chain of keys belonging to the home bucket. + uint16_t hashfrag = vt_hashfrag( hash ); + size_t bucket = home_bucket; + while( true ) + { + if( + ( table->metadata[ bucket ] & VT_HASH_FRAG_MASK ) == hashfrag && + VT_LIKELY( CMPR_FN( table->buckets[ bucket ].key, key ) ) + ) + { + VT_CAT( NAME, _itr ) itr = { + table->buckets + bucket, + table->metadata + bucket, + table->metadata + table->buckets_mask + 1, + home_bucket + }; + return itr; + } + + uint16_t displacement = table->metadata[ bucket ] & VT_DISPLACEMENT_MASK; + if( displacement == VT_DISPLACEMENT_MASK ) + return VT_CAT( NAME, _end_itr )(); + + bucket = ( home_bucket + vt_quadratic( displacement ) ) & table->buckets_mask; + } +} + +// Erases the key pointed to by the specified iterator. +// The erasure always occurs at the end of the chain to which the key belongs. +// If the key to be erased is not the last in the chain, it is swapped with the last so that erasure occurs at the end. +// This helps keep a chain's keys close to their home bucket for the sake of cache locality. +// Returns true if, in the case of iteration from first to end, NAME_next should now be called on the iterator to find +// the next key. +// This return value is necessary because at the iterator location, the erasure could result in an empty bucket, a +// bucket containing a moved key already visited during the iteration, or a bucket containing a moved key not yet +// visited. +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _erase_itr_raw )( NAME *table, VT_CAT( NAME, _itr ) itr ) +{ + --table->key_count; + size_t itr_bucket = itr.metadatum - table->metadata; + + // For now, we only call the value's destructor because the key may need to be hashed below to determine the home + // bucket. + #ifdef VAL_DTOR_FN + VAL_DTOR_FN( table->buckets[ itr_bucket ].val ); + #endif + + // Case 1: The key is the only one in its chain, so just remove it. + if( + table->metadata[ itr_bucket ] & VT_IN_HOME_BUCKET_MASK && + ( table->metadata[ itr_bucket ] & VT_DISPLACEMENT_MASK ) == VT_DISPLACEMENT_MASK + ) + { + #ifdef KEY_DTOR_FN + KEY_DTOR_FN( table->buckets[ itr_bucket ].key ); + #endif + table->metadata[ itr_bucket ] = VT_EMPTY; + return true; + } + + // Case 2 and 3 require that we know the key's home bucket, which the iterator may not have recorded. + if( itr.home_bucket == SIZE_MAX ) + { + if( table->metadata[ itr_bucket ] & VT_IN_HOME_BUCKET_MASK ) + itr.home_bucket = itr_bucket; + else + itr.home_bucket = HASH_FN( table->buckets[ itr_bucket ].key ) & table->buckets_mask; + } + + // The key can now be safely destructed for cases 2 and 3. + #ifdef KEY_DTOR_FN + KEY_DTOR_FN( table->buckets[ itr_bucket ].key ); + #endif + + // Case 2: The key is the last in a multi-key chain. + // Traverse the chain from the beginning and find the penultimate key. + // Then disconnect the key and erase. + if( ( table->metadata[ itr_bucket ] & VT_DISPLACEMENT_MASK ) == VT_DISPLACEMENT_MASK ) + { + size_t bucket = itr.home_bucket; + while( true ) + { + uint16_t displacement = table->metadata[ bucket ] & VT_DISPLACEMENT_MASK; + size_t next = ( itr.home_bucket + vt_quadratic( displacement ) ) & table->buckets_mask; + if( next == itr_bucket ) + { + table->metadata[ bucket ] |= VT_DISPLACEMENT_MASK; + table->metadata[ itr_bucket ] = VT_EMPTY; + return true; + } + + bucket = next; + } + } + + // Case 3: The chain has multiple keys, and the key is not the last one. + // Traverse the chain from the key to be erased and find the last and penultimate keys. + // Disconnect the last key from the chain, and swap it with the key to erase. + size_t bucket = itr_bucket; + while( true ) + { + size_t prev = bucket; + bucket = ( itr.home_bucket + vt_quadratic( table->metadata[ bucket ] & VT_DISPLACEMENT_MASK ) ) & + table->buckets_mask; + + if( ( table->metadata[ bucket ] & VT_DISPLACEMENT_MASK ) == VT_DISPLACEMENT_MASK ) + { + table->buckets[ itr_bucket ] = table->buckets[ bucket ]; + + table->metadata[ itr_bucket ] = ( table->metadata[ itr_bucket ] & ~VT_HASH_FRAG_MASK ) | ( + table->metadata[ bucket ] & VT_HASH_FRAG_MASK ); + + table->metadata[ prev ] |= VT_DISPLACEMENT_MASK; + table->metadata[ bucket ] = VT_EMPTY; + + // Whether the iterator should be advanced depends on whether the key moved to the iterator bucket came from + // before or after that bucket. + // In the former case, the iteration would already have hit the moved key, so the iterator should still be + // advanced. + if( bucket > itr_bucket ) + return false; + + return true; + } + } +} + +// Erases the specified key, if it exists. +// Returns true if a key was erased. +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _erase )( NAME *table, KEY_TY key ) +{ + VT_CAT( NAME, _itr ) itr = VT_CAT( NAME, _get)( table, key ); + if( VT_CAT( NAME, _is_end )( itr ) ) + return false; + + VT_CAT( NAME, _erase_itr_raw )( table, itr ); + return true; +} + +// Finds the first occupied bucket at or after the bucket pointed to by itr. +// This function scans four buckets at a time, ideally using intrinsics. +static inline void VT_CAT( NAME, _fast_forward )( VT_CAT( NAME, _itr ) *itr ) +{ + while( true ) + { + uint64_t metadata; + memcpy( &metadata, itr->metadatum, sizeof( uint64_t ) ); + if( metadata ) + { + int offset = vt_first_nonzero_uint16( metadata ); + itr->data += offset; + itr->metadatum += offset; + itr->home_bucket = SIZE_MAX; + return; + } + + itr->data += 4; + itr->metadatum += 4; + } +} + +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _next )( VT_CAT( NAME, _itr ) itr ) +{ + ++itr.data; + ++itr.metadatum; + VT_CAT( NAME, _fast_forward )( &itr ); + return itr; +} + +// Returns the minimum bucket count required to accommodate a certain number of keys, which is governed by the maximum +// load factor. +static inline size_t VT_CAT( NAME, _min_bucket_count_for_size )( size_t size ) +{ + if( size == 0 ) + return 0; + + // Round up to a power of two. + size_t bucket_count = VT_MIN_NONZERO_BUCKET_COUNT; + while( size > bucket_count * MAX_LOAD ) + bucket_count *= 2; + + return bucket_count; +} + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _reserve )( NAME *table, size_t size ) +{ + size_t bucket_count = VT_CAT( NAME, _min_bucket_count_for_size )( size ); + + if( bucket_count <= VT_CAT( NAME, _bucket_count )( table ) ) + return true; + + return VT_CAT( NAME, _rehash )( table, bucket_count ); +} + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _shrink )( NAME *table ) +{ + size_t bucket_count = VT_CAT( NAME, _min_bucket_count_for_size )( table->key_count ); + + if( bucket_count == VT_CAT( NAME, _bucket_count )( table ) ) // Shrink unnecessary. + return true; + + if( bucket_count == 0 ) + { + FREE_FN( + table->buckets, + VT_CAT( NAME, _total_alloc_size )( table ) + #ifdef CTX_TY + , &table->ctx + #endif + ); + + table->buckets_mask = 0x0000000000000000ull; + table->metadata = (uint16_t *)&vt_empty_placeholder_metadatum; + return true; + } + + return VT_CAT( NAME, _rehash )( table, bucket_count ); +} + +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _first )( const NAME *table ) +{ + if( !table->key_count ) + return VT_CAT( NAME, _end_itr )(); + + VT_CAT( NAME, _itr ) itr = { table->buckets, table->metadata, table->metadata + table->buckets_mask + 1, SIZE_MAX }; + VT_CAT( NAME, _fast_forward )( &itr ); + return itr; +} + +VT_API_FN_QUALIFIERS void VT_CAT( NAME, _clear )( NAME *table ) +{ + if( !table->key_count ) + return; + + for( size_t i = 0; i < VT_CAT( NAME, _bucket_count )( table ); ++i ) + { + if( table->metadata[ i ] != VT_EMPTY ) + { + #ifdef KEY_DTOR_FN + KEY_DTOR_FN( table->buckets[ i ].key ); + #endif + #ifdef VAL_DTOR_FN + VAL_DTOR_FN( table->buckets[ i ].val ); + #endif + } + + table->metadata[ i ] = VT_EMPTY; + } + + table->key_count = 0; +} + +VT_API_FN_QUALIFIERS void VT_CAT( NAME, _cleanup )( NAME *table ) +{ + if( !table->buckets_mask ) + return; + + #if defined( KEY_DTOR_FN ) || defined( VAL_DTOR_FN ) + VT_CAT( NAME, _clear )( table ); + #endif + + FREE_FN( + table->buckets, + VT_CAT( NAME, _total_alloc_size )( table ) + #ifdef CTX_TY + , &table->ctx + #endif + ); + + VT_CAT( NAME, _init )( + table + #ifdef CTX_TY + , table->ctx + #endif + ); +} + +#endif + +/*--------------------------------------------------------------------------------------------------------------------*/ +/* Wrapper types and functions for the C11 generic API */ +/*--------------------------------------------------------------------------------------------------------------------*/ + +#if defined(__STDC_VERSION__) && \ + __STDC_VERSION__ >= 201112L && \ + !defined( IMPLEMENTATION_MODE ) && \ + !defined( VT_NO_C11_GENERIC_API ) \ + +typedef NAME VT_CAT( vt_table_, VT_TEMPLATE_COUNT ); +typedef VT_CAT( NAME, _itr ) VT_CAT( vt_table_itr_, VT_TEMPLATE_COUNT ); + +static inline void VT_CAT( vt_init_, VT_TEMPLATE_COUNT )( + NAME *table + #ifdef CTX_TY + , CTX_TY ctx + #endif +) +{ + VT_CAT( NAME, _init )( + table + #ifdef CTX_TY + , ctx + #endif + ); +} + +static inline bool VT_CAT( vt_init_clone_, VT_TEMPLATE_COUNT )( + NAME *table, + const NAME* source + #ifdef CTX_TY + , CTX_TY ctx + #endif +) +{ + return VT_CAT( NAME, _init_clone )( + table, + source + #ifdef CTX_TY + , ctx + #endif + ); +} + +static inline size_t VT_CAT( vt_size_, VT_TEMPLATE_COUNT )( const NAME *table ) +{ + return VT_CAT( NAME, _size )( table ); +} + +static inline size_t VT_CAT( vt_bucket_count_, VT_TEMPLATE_COUNT )( const NAME *table ) +{ + return VT_CAT( NAME, _bucket_count )( table ); +} + +static inline bool VT_CAT( vt_is_end_, VT_TEMPLATE_COUNT )( VT_CAT( NAME, _itr ) itr ) +{ + return VT_CAT( NAME, _is_end )( itr ); +} + +static inline VT_CAT( NAME, _itr ) VT_CAT( vt_insert_, VT_TEMPLATE_COUNT )( + NAME *table, + KEY_TY key + #ifdef VAL_TY + , VAL_TY val + #endif +) +{ + return VT_CAT( NAME, _insert )( + table, + key + #ifdef VAL_TY + , val + #endif + ); +} + +static inline VT_CAT( NAME, _itr ) VT_CAT( vt_get_or_insert_, VT_TEMPLATE_COUNT )( + NAME *table, + KEY_TY key + #ifdef VAL_TY + , VAL_TY val + #endif +) +{ + return VT_CAT( NAME, _get_or_insert )( + table, + key + #ifdef VAL_TY + , val + #endif + ); +} + +static inline VT_CAT( NAME, _itr ) VT_CAT( vt_get_, VT_TEMPLATE_COUNT )( const NAME *table, KEY_TY key ) +{ + return VT_CAT( NAME, _get )( table, key ); +} + +static inline bool VT_CAT( vt_erase_, VT_TEMPLATE_COUNT )( NAME *table, KEY_TY key ) +{ + return VT_CAT( NAME, _erase )( table, key ); +} + +static inline VT_CAT( NAME, _itr ) VT_CAT( vt_next_, VT_TEMPLATE_COUNT )( VT_CAT( NAME, _itr ) itr ) +{ + return VT_CAT( NAME, _next )( itr ); +} + +static inline VT_CAT( NAME, _itr ) VT_CAT( vt_erase_itr_, VT_TEMPLATE_COUNT )( NAME *table, VT_CAT( NAME, _itr ) itr ) +{ + return VT_CAT( NAME, _erase_itr )( table, itr ); +} + +static inline bool VT_CAT( vt_reserve_, VT_TEMPLATE_COUNT )( NAME *table, size_t bucket_count ) +{ + return VT_CAT( NAME, _reserve )( table, bucket_count ); +} + +static inline bool VT_CAT( vt_shrink_, VT_TEMPLATE_COUNT )( NAME *table ) +{ + return VT_CAT( NAME, _shrink )( table ); +} + +static inline VT_CAT( NAME, _itr ) VT_CAT( vt_first_, VT_TEMPLATE_COUNT )( const NAME *table ) +{ + return VT_CAT( NAME, _first )( table ); +} + +static inline void VT_CAT( vt_clear_, VT_TEMPLATE_COUNT )( NAME *table ) +{ + VT_CAT( NAME, _clear )( table ); +} + +static inline void VT_CAT( vt_cleanup_, VT_TEMPLATE_COUNT )( NAME *table ) +{ + VT_CAT( NAME, _cleanup )( table ); +} + +// Increment the template counter. +#if VT_TEMPLATE_COUNT_D1 == 0 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 1 +#elif VT_TEMPLATE_COUNT_D1 == 1 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 2 +#elif VT_TEMPLATE_COUNT_D1 == 2 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 3 +#elif VT_TEMPLATE_COUNT_D1 == 3 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 4 +#elif VT_TEMPLATE_COUNT_D1 == 4 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 5 +#elif VT_TEMPLATE_COUNT_D1 == 5 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 6 +#elif VT_TEMPLATE_COUNT_D1 == 6 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 7 +#elif VT_TEMPLATE_COUNT_D1 == 7 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 0 +#if VT_TEMPLATE_COUNT_D2 == 0 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 1 +#elif VT_TEMPLATE_COUNT_D2 == 1 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 2 +#elif VT_TEMPLATE_COUNT_D2 == 2 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 3 +#elif VT_TEMPLATE_COUNT_D2 == 3 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 4 +#elif VT_TEMPLATE_COUNT_D2 == 4 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 5 +#elif VT_TEMPLATE_COUNT_D2 == 5 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 6 +#elif VT_TEMPLATE_COUNT_D2 == 6 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 7 +#elif VT_TEMPLATE_COUNT_D2 == 7 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 0 +#if VT_TEMPLATE_COUNT_D3 == 0 +#undef VT_TEMPLATE_COUNT_D3 +#define VT_TEMPLATE_COUNT_D3 1 +#elif VT_TEMPLATE_COUNT_D3 == 1 +#undef VT_TEMPLATE_COUNT_D3 +#define VT_TEMPLATE_COUNT_D3 2 +#elif VT_TEMPLATE_COUNT_D3 == 2 +#undef VT_TEMPLATE_COUNT_D3 +#define VT_TEMPLATE_COUNT_D3 3 +#elif VT_TEMPLATE_COUNT_D3 == 3 +#undef VT_TEMPLATE_COUNT_D3 +#define VT_TEMPLATE_COUNT_D3 4 +#elif VT_TEMPLATE_COUNT_D3 == 4 +#undef VT_TEMPLATE_COUNT_D3 +#define VT_TEMPLATE_COUNT_D3 5 +#elif VT_TEMPLATE_COUNT_D3 == 5 +#undef VT_TEMPLATE_COUNT_D3 +#define VT_TEMPLATE_COUNT_D3 6 +#elif VT_TEMPLATE_COUNT_D3 == 6 +#undef VT_TEMPLATE_COUNT_D3 +#define VT_TEMPLATE_COUNT_D3 7 +#elif VT_TEMPLATE_COUNT_D3 == 7 +#error Sorry, the number of template instances is limited to 511. Define VT_NO_C11_GENERIC_API globally and use the \ +C99 prefixed function API to circumvent this restriction. +#endif +#endif +#endif + +#endif + +#undef NAME +#undef KEY_TY +#undef VAL_TY +#undef HASH_FN +#undef CMPR_FN +#undef MAX_LOAD +#undef KEY_DTOR_FN +#undef VAL_DTOR_FN +#undef CTX_TY +#undef MALLOC_FN +#undef FREE_FN +#undef HEADER_MODE +#undef IMPLEMENTATION_MODE +#undef VT_API_FN_QUALIFIERS diff --git a/src/weld_joint.c b/src/weld_joint.c new file mode 100644 index 0000000..56467b3 --- /dev/null +++ b/src/weld_joint.c @@ -0,0 +1,305 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "joint.h" +#include "physics_world.h" +#include "solver.h" +#include "solver_set.h" + +// needed for dll export +#include "box3d/box3d.h" + +void b3WeldJoint_SetLinearHertz( b3JointId jointId, float hertz ) +{ + B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint ); + base->weldJoint.linearHertz = hertz; +} + +float b3WeldJoint_GetLinearHertz( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint ); + return base->weldJoint.linearHertz; +} + +void b3WeldJoint_SetLinearDampingRatio( b3JointId jointId, float dampingRatio ) +{ + B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint ); + base->weldJoint.linearDampingRatio = dampingRatio; +} + +float b3WeldJoint_GetLinearDampingRatio( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint ); + return base->weldJoint.linearDampingRatio; +} + +void b3WeldJoint_SetAngularHertz( b3JointId jointId, float hertz ) +{ + B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint ); + base->weldJoint.angularHertz = hertz; +} + +float b3WeldJoint_GetAngularHertz( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint ); + return base->weldJoint.angularHertz; +} + +void b3WeldJoint_SetAngularDampingRatio( b3JointId jointId, float dampingRatio ) +{ + B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint ); + base->weldJoint.angularDampingRatio = dampingRatio; +} + +float b3WeldJoint_GetAngularDampingRatio( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint ); + return base->weldJoint.angularDampingRatio; +} + +b3Vec3 b3GetWeldJointForce( b3World* world, b3JointSim* base ) +{ + b3Vec3 force = b3MulSV( world->inv_h, base->weldJoint.linearImpulse ); + return force; +} + +b3Vec3 b3GetWeldJointTorque( b3World* world, b3JointSim* base ) +{ + return b3MulSV( world->inv_h, base->weldJoint.angularImpulse ); +} + +void b3PrepareWeldJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_weldJoint ); + + b3World* world = context->world; + + b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB ); + + B3_ASSERT( bodyB->setIndex == b3_awakeSet ); + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + base->invMassA = bodySimA->invMass; + base->invMassB = bodySimB->invMass; + base->invIA = bodySimA->invInertiaWorld; + base->invIB = bodySimB->invInertiaWorld; + + b3Matrix3 invInertiaSum = b3AddMM( base->invIA, base->invIB ); + base->fixedRotation = b3Det( invInertiaSum ) < 1000.0f * FLT_MIN; + + b3WeldJoint* joint = &base->weldJoint; + joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX; + joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX; + + // Compute joint anchor frames with world space rotation, relative to center of mass + joint->frameA.q = b3MulQuat( bodySimA->transform.q, base->localFrameA.q ); + joint->frameA.p = b3RotateVector( bodySimA->transform.q, b3Sub( base->localFrameA.p, bodySimA->localCenter ) ); + joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + joint->frameB.p = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) ); + + joint->deltaCenter = b3Sub( bodySimB->center, bodySimA->center ); + joint->angularMass = b3InvertMatrix( invInertiaSum ); + + if ( joint->linearHertz == 0.0f ) + { + joint->linearSpring = base->constraintSoftness; + } + else + { + joint->linearSpring = b3MakeSoft( joint->linearHertz, joint->linearDampingRatio, context->h ); + } + + if ( joint->angularHertz == 0.0f ) + { + joint->angularSpring = base->constraintSoftness; + } + else + { + joint->angularSpring = b3MakeSoft( joint->angularHertz, joint->angularDampingRatio, context->h ); + } + + if ( context->enableWarmStarting == false ) + { + joint->linearImpulse = b3Vec3_zero; + joint->angularImpulse = b3Vec3_zero; + } +} + +void b3WarmStartWeldJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_weldJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3WeldJoint* joint = &base->weldJoint; + + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + vA = b3MulSub( vA, mA, joint->linearImpulse ); + wA = b3Sub( wA, b3MulMV( iA, b3Add( b3Cross( rA, joint->linearImpulse ), joint->angularImpulse ) ) ); + + vB = b3MulAdd( vB, mB, joint->linearImpulse ); + wB = b3Add( wB, b3MulMV( iB, b3Add( b3Cross( rB, joint->linearImpulse ), joint->angularImpulse ) ) ); + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +void b3SolveWeldJoint( b3JointSim* base, b3StepContext* context, bool useBias ) +{ + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3WeldJoint* joint = &base->weldJoint; + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + bool fixedRotation = base->fixedRotation; + b3Quat quatA = b3MulQuat( stateA->deltaRotation, joint->frameA.q ); + b3Quat quatB = b3MulQuat( stateB->deltaRotation, joint->frameB.q ); + + if ( b3DotQuat( quatA, quatB ) < 0.0f ) + { + // this keeps the rotation angle in the range [-pi, pi] + quatB = b3NegateQuat( quatB ); + } + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + // angular constraint + if ( fixedRotation == false ) + { + b3Vec3 bias = b3Vec3_zero; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias || joint->angularHertz > 0.0f ) + { + b3Quat targetQuat = b3Quat_identity; + b3Vec3 deltaRotation = b3DeltaQuatToRotation( relQ, targetQuat ); + b3Vec3 c = b3Neg( b3RotateVector( quatA, deltaRotation ) ); + + bias = b3MulSV( joint->angularSpring.biasRate, c ); + massScale = joint->angularSpring.massScale; + impulseScale = joint->angularSpring.impulseScale; + } + + b3Vec3 cdot = b3Sub( wB, wA ); + b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b3MulMV( joint->angularMass, b3Add( cdot, bias ) ) ), impulseScale, joint->angularImpulse ); + joint->angularImpulse = b3Add( joint->angularImpulse, impulse ); + + wA = b3Sub( wA, b3MulMV( iA, impulse ) ); + wB = b3Add( wB, b3MulMV( iB, impulse ) ); + } + + // linear constraint + { + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + b3Vec3 cdot = b3Sub( b3Add( vB, b3Cross( wB, rB ) ), b3Add( vA, b3Cross( wA, rA ) ) ); + + b3Vec3 bias = b3Vec3_zero; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias || joint->linearHertz > 0.0f ) + { + b3Vec3 dcA = stateA->deltaPosition; + b3Vec3 dcB = stateB->deltaPosition; + + b3Vec3 separation = b3Add( b3Add( b3Sub( dcB, dcA ), b3Sub( rB, rA ) ), joint->deltaCenter ); + + bias = b3MulSV( joint->linearSpring.biasRate, separation ); + massScale = joint->linearSpring.massScale; + impulseScale = joint->linearSpring.impulseScale; + } + + //// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] + b3Matrix3 sA = b3Skew( rA ); + b3Matrix3 sB = b3Skew( rB ); + b3Matrix3 kA = b3MulMM( sA, b3MulMM( base->invIA, sA ) ); + b3Matrix3 kB = b3MulMM( sB, b3MulMM( base->invIB, sB ) ); + b3Matrix3 k = b3NegateMat3( b3AddMM( kA, kB ) ); + k.cx.x += mA + mB; + k.cy.y += mA + mB; + k.cz.z += mA + mB; + + b3Vec3 b = b3Solve3( k, b3Add( cdot, bias ) ); + + b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b ), impulseScale, joint->linearImpulse ); + joint->linearImpulse = b3Add( joint->linearImpulse, impulse ); + + vA = b3MulSub( vA, mA, impulse ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) ); + vB = b3MulAdd( vB, mB, impulse ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) ); + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +void b3DrawWeldJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale ) +{ + b3Transform frameA = b3MulTransforms( transformA, base->localFrameA ); + b3Transform frameB = b3MulTransforms( transformB, base->localFrameB ); + + b3Vec3 extents = { 0.1f * scale, 0.05f * scale, 0.025f * scale }; + draw->DrawBoxFcn( extents, frameA, b3_colorDarkOrange, draw->context ); + draw->DrawBoxFcn( extents, frameB, b3_colorDarkCyan, draw->context ); +} diff --git a/src/wheel_joint.c b/src/wheel_joint.c new file mode 100644 index 0000000..aa2e30a --- /dev/null +++ b/src/wheel_joint.c @@ -0,0 +1,1076 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "math_internal.h" +#include "physics_world.h" +#include "solver.h" +#include "solver_set.h" + +// needed for dll export +#include "box3d/box3d.h" + +void b3WheelJoint_EnableSuspension( b3JointId jointId, bool enableSpring ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + + if ( enableSpring != joint->wheelJoint.enableSuspensionSpring ) + { + joint->wheelJoint.enableSuspensionSpring = enableSpring; + joint->wheelJoint.suspensionSpringImpulse = 0.0f; + } +} + +bool b3WheelJoint_IsSuspensionEnabled( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.enableSuspensionSpring; +} + +void b3WheelJoint_SetSuspensionHertz( b3JointId jointId, float hertz ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.suspensionHertz = hertz; +} + +float b3WheelJoint_GetSuspensionHertz( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.suspensionHertz; +} + +void b3WheelJoint_SetSuspensionDampingRatio( b3JointId jointId, float dampingRatio ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.suspensionDampingRatio = dampingRatio; +} + +float b3WheelJoint_GetSuspensionDampingRatio( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.suspensionDampingRatio; +} + +void b3WheelJoint_EnableSuspensionLimit( b3JointId jointId, bool enableLimit ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + if ( joint->wheelJoint.enableSuspensionLimit != enableLimit ) + { + joint->wheelJoint.lowerSuspensionImpulse = 0.0f; + joint->wheelJoint.upperSuspensionImpulse = 0.0f; + joint->wheelJoint.enableSuspensionLimit = enableLimit; + } +} + +bool b3WheelJoint_IsSuspensionLimitEnabled( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.enableSuspensionLimit; +} + +float b3WheelJoint_GetLowerSuspensionLimit( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.lowerSuspensionLimit; +} + +float b3WheelJoint_GetUpperSuspensionLimit( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.upperSuspensionLimit; +} + +void b3WheelJoint_SetSuspensionLimits( b3JointId jointId, float lower, float upper ) +{ + B3_ASSERT( lower <= upper ); + + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + if ( lower != joint->wheelJoint.lowerSuspensionLimit || upper != joint->wheelJoint.upperSuspensionLimit ) + { + joint->wheelJoint.lowerSuspensionLimit = lower; + joint->wheelJoint.upperSuspensionLimit = upper; + joint->wheelJoint.lowerSuspensionImpulse = 0.0f; + joint->wheelJoint.upperSuspensionImpulse = 0.0f; + } +} + +void b3WheelJoint_EnableSpinMotor( b3JointId jointId, bool enableMotor ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + if ( joint->wheelJoint.enableSpinMotor != enableMotor ) + { + joint->wheelJoint.spinImpulse = 0.0f; + joint->wheelJoint.enableSpinMotor = enableMotor; + } +} + +bool b3WheelJoint_IsSpinMotorEnabled( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.enableSpinMotor; +} + +void b3WheelJoint_SetSpinMotorSpeed( b3JointId jointId, float motorSpeed ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.spinSpeed = motorSpeed; +} + +float b3WheelJoint_GetSpinMotorSpeed( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.spinSpeed; +} + +void b3WheelJoint_SetMaxSpinTorque( b3JointId jointId, float torque ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.maxSpinTorque = torque; +} + +float b3WheelJoint_GetMaxSpinTorque( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.maxSpinTorque; +} + +void b3WheelJoint_EnableSteering( b3JointId jointId, bool flag ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + if ( joint->wheelJoint.enableSteering != flag ) + { + joint->wheelJoint.angularImpulse = (b3Vec2){ 0.0f, 0.0f }; + joint->wheelJoint.enableSteering = flag; + } +} + +bool b3WheelJoint_IsSteeringEnabled( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.enableSteering; +} + +void b3WheelJoint_SetSteeringHertz( b3JointId jointId, float hertz ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.steeringHertz = hertz; +} + +float b3WheelJoint_GetSteeringHertz( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.steeringHertz; +} + +void b3WheelJoint_SetSteeringDampingRatio( b3JointId jointId, float dampingRatio ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.steeringDampingRatio = dampingRatio; +} + +float b3WheelJoint_GetSteeringDampingRatio( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.steeringDampingRatio; +} + +void b3WheelJoint_SetMaxSteeringTorque( b3JointId jointId, float maxTorque ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.maxSteeringTorque = maxTorque; +} + +float b3WheelJoint_GetMaxSteeringTorque( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.maxSteeringTorque; +} + +void b3WheelJoint_EnableSteeringLimit( b3JointId jointId, bool flag ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + if ( joint->wheelJoint.enableSteeringLimit != flag ) + { + joint->wheelJoint.lowerSteeringImpulse = 0.0f; + joint->wheelJoint.upperSteeringImpulse = 0.0f; + joint->wheelJoint.enableSteeringLimit = flag; + } +} + +bool b3WheelJoint_IsSteeringLimitEnabled( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.enableSteeringLimit; +} + +float b3WheelJoint_GetLowerSteeringLimit( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.lowerSteeringLimit; +} + +float b3WheelJoint_GetUpperSteeringLimit( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.upperSteeringLimit; +} + +void b3WheelJoint_SetSteeringLimits( b3JointId jointId, float lowerRadians, float upperRadians ) +{ + B3_ASSERT( lowerRadians <= upperRadians ); + + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.lowerSteeringLimit = lowerRadians; + joint->wheelJoint.upperSteeringLimit = upperRadians; +} + +void b3WheelJoint_SetTargetSteeringAngle( b3JointId jointId, float radians ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.targetSteeringAngle = radians; +} + +float b3WheelJoint_GetTargetSteeringAngle( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.targetSteeringAngle; +} + +float b3WheelJoint_GetSpinSpeed( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b3Body* bodyA = b3Array_Get( world->bodies, idA ); + b3Body* bodyB = b3Array_Get( world->bodies, idB ); + + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + int localIndexB = bodyB->localIndex; + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + b3Quat quatB = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + b3Vec3 spinAxis = b3RotateVector( quatB, b3Vec3_axisZ ); + + b3Vec3 wA = b3Vec3_zero; + b3BodyState* stateA = b3GetBodyState( world, bodyA ); + if ( stateA != NULL ) + { + wA = stateA->angularVelocity; + } + + b3Vec3 wB = b3Vec3_zero; + b3BodyState* stateB = b3GetBodyState( world, bodyB ); + if ( stateB != NULL ) + { + wB = stateB->angularVelocity; + } + + float speed = b3Dot( b3Sub( wB, wA ), spinAxis ); + return speed; +} + +float b3WheelJoint_GetSpinTorque( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return world->inv_h * joint->wheelJoint.spinImpulse; +} + +float b3WheelJoint_GetSteeringAngle( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b3Body* bodyA = b3Array_Get( world->bodies, idA ); + b3Body* bodyB = b3Array_Get( world->bodies, idB ); + + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + b3Quat quatA = b3MulQuat( bodySimA->transform.q, base->localFrameA.q ); + b3Quat quatB = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + + b3Matrix3 matrixA = b3MakeMatrixFromQuat( quatA ); + b3Matrix3 matrixB = b3MakeMatrixFromQuat( quatB ); + + // Twist around x-axis + float cs = b3Dot( matrixB.cz, matrixA.cz ); + float ss = -b3Dot( matrixB.cz, matrixA.cy ); + + return b3Atan2( ss, cs ); +} + +float b3WheelJoint_GetSteeringTorque( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return world->inv_h * joint->wheelJoint.steeringSpringImpulse; +} + +b3Vec3 b3GetWheelJointForce( b3World* world, b3JointSim* base ) +{ + b3Transform transformA = b3GetBodyTransform( world, base->bodyIdA ); + b3WheelJoint* joint = &base->wheelJoint; + + // impulse in joint space + b3Vec3 impulse = { + joint->linearImpulse.x, + joint->linearImpulse.y, + joint->lowerSuspensionLimit + joint->upperSuspensionImpulse + joint->suspensionSpringImpulse, + }; + + // convert impulse to force + b3Vec3 force = b3MulSV( world->inv_h, impulse ); + + // convert to body space + force = b3RotateVector( base->localFrameA.q, force ); + + // convert to world space + force = b3RotateVector( transformA.q, force ); + return force; +} + +b3Vec3 b3GetWheelJointTorque( b3World* world, b3JointSim* base ) +{ + B3_ASSERT( base->type == b3_wheelJoint ); + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + // int idB = base->bodyIdB; + + b3Body* bodyA = b3Array_Get( world->bodies, idA ); + // b3Body* bodyB = b3Array_Get( world->bodies, idB ); + + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + // b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + // int localIndexB = bodyB->localIndex; + + b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); + // b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + b3Quat qA = b3MulQuat( bodySimA->transform.q, base->localFrameA.q ); + + b3Matrix3 matrixA = b3MakeMatrixFromQuat( qA ); + + return b3MulSV( world->inv_h * base->wheelJoint.spinImpulse, matrixA.cz ); +} + +// See constraints.pdf + +void b3PrepareWheelJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_wheelJoint ); + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b3World* world = context->world; + + b3Body* bodyA = b3Array_Get( world->bodies, idA ); + b3Body* bodyB = b3Array_Get( world->bodies, idB ); + + B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + base->invMassA = bodySimA->invMass; + base->invMassB = bodySimB->invMass; + base->invIA = bodySimA->invInertiaWorld; + base->invIB = bodySimB->invInertiaWorld; + + b3Matrix3 invInertiaSum = b3AddMM( base->invIA, base->invIB ); + base->fixedRotation = b3Det( invInertiaSum ) < 1000.0f * FLT_MIN; + + b3WheelJoint* joint = &base->wheelJoint; + + joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX; + joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX; + + // Compute joint anchor frames with world space rotation, relative to center of mass + joint->frameA.q = b3MulQuat( bodySimA->transform.q, base->localFrameA.q ); + joint->frameA.p = b3RotateVector( bodySimA->transform.q, b3Sub( base->localFrameA.p, bodySimA->localCenter ) ); + joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + joint->frameB.p = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) ); + + // Compute the initial center delta. Incremental position updates are relative to this. + joint->deltaCenter = b3Sub( bodySimB->center, bodySimA->center ); + + b3Vec3 rA = joint->frameA.p; + b3Vec3 rB = joint->frameB.p; + + b3Matrix3 matrixA = b3MakeMatrixFromQuat( joint->frameA.q ); + b3Matrix3 matrixB = b3MakeMatrixFromQuat( joint->frameB.q ); + + // todo use fresh effective masses in the sub-step to avoid divergence like I saw for the prismatic joint + + { + b3Vec3 suspensionAxis = matrixA.cx; + b3Vec3 rAn = b3Cross( rA, suspensionAxis ); + b3Vec3 rBn = b3Cross( rB, suspensionAxis ); + + float k = base->invMassA + base->invMassB + b3Dot( rAn, b3MulMV( base->invIA, rAn ) ) + + b3Dot( rBn, b3MulMV( base->invIB, rBn ) ); + joint->suspensionMass = k > 0.0f ? 1.0f / k : 0.0f; + } + + joint->suspensionSoftness = b3MakeSoft( joint->suspensionHertz, joint->suspensionDampingRatio, context->h ); + joint->steeringSoftness = b3MakeSoft( joint->steeringHertz, joint->steeringDampingRatio, context->h ); + + { + // Rotation axis is the z-axis of body A. + b3Vec3 spinAxis = matrixB.cz; + float k = b3Dot( spinAxis, b3MulMV( invInertiaSum, spinAxis ) ); + joint->spinMass = k > 0.0f ? 1.0f / k : 0.0f; + } + + { + // Twist constraint around x-axis + float cs = b3Dot( matrixB.cz, matrixA.cz ); + float ss = -b3Dot( matrixB.cz, matrixA.cy ); + float den = cs * cs + ss * ss; + den = den > 0.0f ? 1.0f / den : 0.0f; + b3Vec3 steeringAxis = + b3MulSV( den, b3Cross( matrixB.cz, b3Sub( b3MulSV( -cs, matrixA.cy ), b3MulSV( ss, matrixA.cz ) ) ) ); + + float k = b3Dot( steeringAxis, b3MulMV( invInertiaSum, steeringAxis ) ); + joint->steeringMass = k > 0.0f ? 1.0f / k : 0.0f; + } + + if ( context->enableWarmStarting == false ) + { + joint->linearImpulse = (b3Vec2){ 0.0f, 0.0f }; + joint->angularImpulse = (b3Vec2){ 0.0f, 0.0f }; + joint->spinImpulse = 0.0f; + joint->suspensionSpringImpulse = 0.0f; + joint->lowerSuspensionImpulse = 0.0f; + joint->upperSuspensionImpulse = 0.0f; + joint->steeringSpringImpulse = 0.0f; + joint->lowerSteeringImpulse = 0.0f; + joint->upperSteeringImpulse = 0.0f; + } +} + +void b3WarmStartWheelJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_wheelJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3WheelJoint* joint = &base->wheelJoint; + + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + b3Vec3 d = b3Add( b3Add( b3Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b3Sub( rB, rA ) ); + + b3Quat quatA = b3MulQuat( stateA->deltaRotation, joint->frameA.q ); + b3Quat quatB = b3MulQuat( stateB->deltaRotation, joint->frameB.q ); + if ( b3DotQuat( quatA, quatB ) < 0.0f ) + { + // this keeps the rotation angle in the range [-pi, pi] + quatB = b3NegateQuat( quatB ); + } + + b3Matrix3 matrixA = b3MakeMatrixFromQuat( quatA ); + b3Matrix3 matrixB = b3MakeMatrixFromQuat( quatB ); + + b3Vec3 sAx = b3Cross( b3Add( d, rA ), matrixA.cx ); + b3Vec3 sBx = b3Cross( rB, matrixA.cx ); + b3Vec3 sAy = b3Cross( b3Add( d, rA ), matrixA.cy ); + b3Vec3 sBy = b3Cross( rB, matrixA.cy ); + b3Vec3 sAz = b3Cross( b3Add( d, rA ), matrixA.cz ); + b3Vec3 sBz = b3Cross( rB, matrixA.cz ); + + float suspensionImpulse = joint->suspensionSpringImpulse + joint->lowerSuspensionImpulse - joint->upperSuspensionImpulse; + + float linearImpulseY = joint->linearImpulse.x; + float linearImpulseZ = joint->linearImpulse.y; + float angularImpulseX = joint->angularImpulse.x; + float angularImpulseY = joint->angularImpulse.y; + + b3Vec3 linearImpulse = b3Blend3( suspensionImpulse, matrixA.cx, linearImpulseY, matrixA.cy, linearImpulseZ, matrixA.cz ); + b3Vec3 angularImpulseA = b3Blend3( suspensionImpulse, sAx, linearImpulseY, sAy, linearImpulseZ, sAz ); + b3Vec3 angularImpulseB = b3Blend3( suspensionImpulse, sBx, linearImpulseY, sBy, linearImpulseZ, sBz ); + b3Vec3 angularImpulse = b3MulSV( joint->spinImpulse, matrixA.cz ); + + b3Vec3 spinAxis = matrixB.cz; + + if ( joint->enableSteering ) + { + // Twist constraint around x-axis + float cs = b3Dot( matrixB.cz, matrixA.cz ); + float ss = -b3Dot( matrixB.cz, matrixA.cy ); + float den = cs * cs + ss * ss; + den = den > 0.0f ? 1.0f / den : 0.0f; + b3Vec3 steeringAxis = + b3MulSV( den, b3Cross( matrixB.cz, b3Sub( b3MulSV( -cs, matrixA.cy ), b3MulSV( ss, matrixA.cz ) ) ) ); + + b3Vec3 perpAxis = b3Cross( spinAxis, matrixA.cx ); + float steeringImpulse = joint->steeringSpringImpulse + joint->lowerSteeringImpulse - joint->upperSteeringImpulse; + angularImpulse = b3Blend3( angularImpulseX, perpAxis, joint->spinImpulse, spinAxis, steeringImpulse, steeringAxis ); + } + else + { + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + b3Vec3 perpAxisX = b3MulSV( + 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) ); + b3Vec3 perpAxisY = b3MulSV( + 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) ); + angularImpulse = b3Add( + angularImpulse, + b3Blend3( angularImpulseX, perpAxisX, angularImpulseY, perpAxisY, joint->spinImpulse, spinAxis ) ); + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = b3MulSub( stateA->linearVelocity, mA, linearImpulse ); + stateA->angularVelocity = b3Sub( stateA->angularVelocity, b3MulMV( iA, b3Add( angularImpulseA, angularImpulse ) ) ); + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = b3MulAdd( stateB->linearVelocity, mB, linearImpulse ); + stateB->angularVelocity = b3Add( stateB->angularVelocity, b3MulMV( iB, b3Add( angularImpulseB, angularImpulse ) ) ); + } +} + +void b3SolveWheelJoint( b3JointSim* base, b3StepContext* context, bool useBias ) +{ + B3_ASSERT( base->type == b3_wheelJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3WheelJoint* joint = &base->wheelJoint; + + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + bool fixedRotation = base->fixedRotation; + + // current anchors + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + b3Quat quatA = b3MulQuat( stateA->deltaRotation, joint->frameA.q ); + b3Quat quatB = b3MulQuat( stateB->deltaRotation, joint->frameB.q ); + + if ( b3DotQuat( quatA, quatB ) < 0.0f ) + { + // this keeps the rotation angle in the range [-pi, pi] + quatB = b3NegateQuat( quatB ); + } + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + b3Matrix3 matrixA = b3MakeMatrixFromQuat( quatA ); + b3Matrix3 matrixB = b3MakeMatrixFromQuat( quatB ); + + // b3Vec3 spinAxis = b3RotateVector( quatB, b3Vec3_axisZ ); + + b3Vec3 d = b3Add( b3Add( b3Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b3Sub( rB, rA ) ); + b3Vec3 sAx = b3Cross( b3Add( d, rA ), matrixA.cx ); + b3Vec3 sBx = b3Cross( rB, matrixA.cx ); + b3Vec3 sAy = b3Cross( b3Add( d, rA ), matrixA.cy ); + b3Vec3 sBy = b3Cross( rB, matrixA.cy ); + b3Vec3 sAz = b3Cross( b3Add( d, rA ), matrixA.cz ); + b3Vec3 sBz = b3Cross( rB, matrixA.cz ); + + float translation = b3Dot( matrixA.cx, d ); + + // Steering param ib = cz_b, ia = cz_a, ja = -cy_a + float cs = b3Dot( matrixB.cz, matrixA.cz ); + float ss = -b3Dot( matrixB.cz, matrixA.cy ); + float den = cs * cs + ss * ss; + den = den > 0.0f ? 1.0f / den : 0.0f; + b3Vec3 steeringAxis = + b3MulSV( den, b3Cross( matrixB.cz, b3Sub( b3MulSV( -cs, matrixA.cy ), b3MulSV( ss, matrixA.cz ) ) ) ); + + // motor constraint + if ( joint->enableSpinMotor && fixedRotation == false ) + { + b3Vec3 spinAxis = matrixB.cz; + float cdot = b3Dot( b3Sub( wB, wA ), spinAxis ) - joint->spinSpeed; + float impulse = -joint->spinMass * cdot; + float oldImpulse = joint->spinImpulse; + float maxImpulse = context->h * joint->maxSpinTorque; + joint->spinImpulse = b3ClampFloat( joint->spinImpulse + impulse, -maxImpulse, maxImpulse ); + impulse = joint->spinImpulse - oldImpulse; + + wA = b3Sub( wA, b3MulMV( iA, b3MulSV( impulse, spinAxis ) ) ); + wB = b3Add( wB, b3MulMV( iB, b3MulSV( impulse, spinAxis ) ) ); + } + + // suspension + if ( joint->enableSuspensionSpring ) + { + // This is a real spring and should be applied even during relax + float c = translation; + float bias = joint->suspensionSoftness.biasRate * c; + float massScale = joint->suspensionSoftness.massScale; + float impulseScale = joint->suspensionSoftness.impulseScale; + + float cdot = b3Dot( matrixA.cx, b3Sub( vB, vA ) ) + b3Dot( sBx, wB ) - b3Dot( sAx, wA ); + float impulse = -massScale * joint->suspensionMass * ( cdot + bias ) - impulseScale * joint->suspensionSpringImpulse; + joint->suspensionSpringImpulse += impulse; + + b3Vec3 linearImpulse = b3MulSV( impulse, matrixA.cx ); + b3Vec3 angularImpulseA = b3MulSV( impulse, sAx ); + b3Vec3 angularImpulseB = b3MulSV( impulse, sBx ); + + vA = b3MulSub( vA, mA, linearImpulse ); + wA = b3Sub( wA, b3MulMV( iA, angularImpulseA ) ); + vB = b3MulAdd( vB, mB, linearImpulse ); + wB = b3Add( wB, b3MulMV( iB, angularImpulseB ) ); + } + + // steering + if ( joint->enableSteering && fixedRotation == false ) + { + float steeringAngle = b3Atan2( ss, cs ); + + { + // This is a real spring and should be applied even during relax + float c = steeringAngle - joint->targetSteeringAngle; + float bias = joint->steeringSoftness.biasRate * c; + float massScale = joint->steeringSoftness.massScale; + float impulseScale = joint->steeringSoftness.impulseScale; + + float cdot = b3Dot( steeringAxis, b3Sub( wB, wA ) ); + float oldImpulse = joint->steeringSpringImpulse; + float impulse = -massScale * joint->steeringMass * ( cdot + bias ) - impulseScale * oldImpulse; + float maxImpulse = context->h * joint->maxSteeringTorque; + joint->steeringSpringImpulse = b3ClampFloat( oldImpulse + impulse, -maxImpulse, maxImpulse ); + impulse = joint->steeringSpringImpulse - oldImpulse; + + wA = b3Sub( wA, b3MulMV( iA, b3MulSV( impulse, steeringAxis ) ) ); + wB = b3Add( wB, b3MulMV( iB, b3MulSV( impulse, steeringAxis ) ) ); + } + + if ( joint->enableSteeringLimit ) + { + // Lower limit + { + float c = steeringAngle - joint->lowerSteeringLimit; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + float cdot = b3Dot( steeringAxis, b3Sub( wB, wA ) ); + float oldImpulse = joint->lowerSteeringImpulse; + float impulse = -massScale * joint->steeringMass * ( cdot + bias ) - impulseScale * oldImpulse; + joint->lowerSteeringImpulse = b3MaxFloat( oldImpulse + impulse, 0.0f ); + impulse = joint->lowerSteeringImpulse - oldImpulse; + + wA = b3Sub( wA, b3MulMV( iA, b3MulSV( impulse, steeringAxis ) ) ); + wB = b3Add( wB, b3MulMV( iB, b3MulSV( impulse, steeringAxis ) ) ); + } + + // Upper limit + // Note: signs are flipped to keep c positive when the constraint is satisfied. + // This also keeps the impulse positive when the limit is active. + { + // sign flipped + float c = joint->upperSteeringLimit - steeringAngle; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + // sign flipped on cdot + float cdot = b3Dot( steeringAxis, b3Sub( wA, wB ) ); + float oldImpulse = joint->upperSteeringImpulse; + float impulse = -massScale * joint->steeringMass * ( cdot + bias ) - impulseScale * oldImpulse; + joint->upperSteeringImpulse = b3MaxFloat( oldImpulse + impulse, 0.0f ); + impulse = joint->upperSteeringImpulse - oldImpulse; + + // sign flipped on applied impulse + wA = b3Add( wA, b3MulMV( iA, b3MulSV( impulse, steeringAxis ) ) ); + wB = b3Sub( wB, b3MulMV( iB, b3MulSV( impulse, steeringAxis ) ) ); + } + } + } + + if ( joint->enableSuspensionLimit ) + { + // Lower limit + { + float c = translation - joint->lowerSuspensionLimit; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + float cdot = b3Dot( matrixA.cx, b3Sub( vB, vA ) ) + b3Dot( sBx, wB ) - b3Dot( sAx, wA ); + float impulse = -massScale * joint->suspensionMass * ( cdot + bias ) - impulseScale * joint->lowerSuspensionImpulse; + float oldImpulse = joint->lowerSuspensionImpulse; + joint->lowerSuspensionImpulse = b3MaxFloat( oldImpulse + impulse, 0.0f ); + impulse = joint->lowerSuspensionImpulse - oldImpulse; + + b3Vec3 linearImpulse = b3MulSV( impulse, matrixA.cx ); + b3Vec3 angularImpulseA = b3MulSV( impulse, sAx ); + b3Vec3 angularImpulseB = b3MulSV( impulse, sBx ); + + vA = b3MulSub( vA, mA, linearImpulse ); + wA = b3Sub( wA, b3MulMV( iA, angularImpulseA ) ); + vB = b3MulAdd( vB, mB, linearImpulse ); + wB = b3Add( wB, b3MulMV( iB, angularImpulseB ) ); + } + + // Upper limit + // Note: signs are flipped to keep c positive when the constraint is satisfied. + // This also keeps the impulse positive when the limit is active. + { + // sign flipped + float c = joint->upperSuspensionLimit - translation; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + // sign flipped on cdot + float cdot = b3Dot( matrixA.cx, b3Sub( vA, vB ) ) + b3Dot( sAx, wA ) - b3Dot( sBx, wB ); + float impulse = -massScale * joint->suspensionMass * ( cdot + bias ) - impulseScale * joint->upperSuspensionImpulse; + float oldImpulse = joint->upperSuspensionImpulse; + joint->upperSuspensionImpulse = b3MaxFloat( oldImpulse + impulse, 0.0f ); + impulse = joint->upperSuspensionImpulse - oldImpulse; + + b3Vec3 linearImpulse = b3MulSV( impulse, matrixA.cx ); + b3Vec3 angularImpulseA = b3MulSV( impulse, sAx ); + b3Vec3 angularImpulseB = b3MulSV( impulse, sBx ); + + // sign flipped on applied impulse + vA = b3MulAdd( vA, mA, linearImpulse ); + wA = b3Add( wA, b3MulMV( iA, angularImpulseA ) ); + vB = b3MulSub( vB, mB, linearImpulse ); + wB = b3Sub( wB, b3MulMV( iB, angularImpulseB ) ); + } + } + + // Collinearity constraint + if ( fixedRotation == false ) + { + if ( joint->enableSteering == true ) + { + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias ) + { + float c = b3Dot( matrixA.cx, matrixB.cz ); + + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + b3Vec3 u = b3Cross( matrixB.cz, matrixA.cx ); + float cdot = b3Dot( b3Sub( wB, wA ), u ); + + b3Matrix3 invInertiaSum = b3AddMM( iA, iB ); + float k = b3Dot( u, b3MulMV( invInertiaSum, u ) ); + float perpMass = k > 0.0f ? 1.0f / k : 0.0f; + + float deltaImpulse = -massScale * perpMass * ( cdot + bias ) - impulseScale * joint->angularImpulse.x; + joint->angularImpulse.x += deltaImpulse; + + wA = b3MulSub( wA, deltaImpulse, b3MulMV( iA, u ) ); + wB = b3MulAdd( wB, deltaImpulse, b3MulMV( iB, u ) ); + } + else + { + b3Vec2 bias = { 0.0f, 0.0f }; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( useBias ) + { + b3Vec2 c = { relQ.v.x, relQ.v.y }; + bias = (b3Vec2){ base->constraintSoftness.biasRate * c.x, base->constraintSoftness.biasRate * c.y }; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + // Collinearity constraint as 2-by-2 + b3Vec3 perpAxisX = b3MulSV( + 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) ); + b3Vec3 perpAxisY = b3MulSV( + 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) ); + + b3Matrix3 invInertiaSum = b3AddMM( iA, iB ); + float kxx = b3Dot( perpAxisX, b3MulMV( invInertiaSum, perpAxisX ) ); + float kyy = b3Dot( perpAxisY, b3MulMV( invInertiaSum, perpAxisY ) ); + float kxy = b3Dot( perpAxisX, b3MulMV( invInertiaSum, perpAxisY ) ); + + b3Matrix2 k = { { kxx, kxy }, { kxy, kyy } }; + + b3Vec3 wRel = b3Sub( wB, wA ); + b3Vec2 cdot = { b3Dot( wRel, perpAxisX ), b3Dot( wRel, perpAxisY ) }; + b3Vec2 oldImpulse = joint->angularImpulse; + b3Vec2 cdotPlusBias = { cdot.x + bias.x, cdot.y + bias.y }; + b3Vec2 sol = b3Solve2( k, cdotPlusBias ); + b3Vec2 deltaImpulse = { + -massScale * sol.x - impulseScale * oldImpulse.x, + -massScale * sol.y - impulseScale * oldImpulse.y, + }; + joint->angularImpulse = (b3Vec2){ oldImpulse.x + deltaImpulse.x, oldImpulse.y + deltaImpulse.y }; + + b3Vec3 angularImpulse = b3Blend2( deltaImpulse.x, perpAxisX, deltaImpulse.y, perpAxisY ); + wA = b3Sub( wA, b3MulMV( iA, angularImpulse ) ); + wB = b3Add( wB, b3MulMV( iB, angularImpulse ) ); + } + } + + // Solve point-to-line constraint + { + b3Vec3 perpY = matrixA.cy; + b3Vec3 perpZ = matrixA.cz; + + b3Vec2 bias = { 0.0f, 0.0f }; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias ) + { + b3Vec2 c = { b3Dot( perpY, d ), b3Dot( perpZ, d ) }; + bias = (b3Vec2){ base->constraintSoftness.biasRate * c.x, base->constraintSoftness.biasRate * c.y }; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + b3Vec3 vRel = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, b3Add( rA, d ) ) ); + b3Vec2 cdot = { b3Dot( perpY, vRel ), b3Dot( perpZ, vRel ) }; + + //// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] + ///// Jx = [-perpX, -cross(d + rA, perpX), perpX, cross(rB, perpX)] + + float kyy = mA + mB + b3Dot( sAy, b3MulMV( iA, sAy ) ) + b3Dot( sBy, b3MulMV( iB, sBy ) ); + float kyz = b3Dot( sAy, b3MulMV( iA, sAz ) ) + b3Dot( sBy, b3MulMV( iB, sBz ) ); + float kzz = mA + mB + b3Dot( sAz, b3MulMV( iA, sAz ) ) + b3Dot( sBz, b3MulMV( iB, sBz ) ); + + b3Matrix2 k = { { kyy, kyz }, { kyz, kzz } }; + + b3Vec2 oldImpulse = joint->linearImpulse; + b3Vec2 cdotPlusBias = { cdot.x + bias.x, cdot.y + bias.y }; + b3Vec2 sol = b3Solve2( k, cdotPlusBias ); + b3Vec2 deltaImpulse = { + -massScale * sol.x - impulseScale * oldImpulse.x, + -massScale * sol.y - impulseScale * oldImpulse.y, + }; + joint->linearImpulse = (b3Vec2){ oldImpulse.x + deltaImpulse.x, oldImpulse.y + deltaImpulse.y }; + + b3Vec3 linearImpulse = b3Blend2( deltaImpulse.x, perpY, deltaImpulse.y, perpZ ); + + vA = b3MulSub( vA, mA, linearImpulse ); + wA = b3Sub( wA, b3MulMV( iA, b3Blend2( deltaImpulse.x, sAy, deltaImpulse.y, sAz ) ) ); + vB = b3MulAdd( vB, mB, linearImpulse ); + wB = b3Add( wB, b3MulMV( iB, b3Blend2( deltaImpulse.x, sBy, deltaImpulse.y, sBz ) ) ); + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +#if 0 +void b3WheelJoint_Dump() +{ + int32 indexA = joint->bodyA->joint->islandIndex; + int32 indexB = joint->bodyB->joint->islandIndex; + + b3Dump(" b3WheelJointDef jd;\n"); + b3Dump(" jd.bodyA = sims[%d];\n", indexA); + b3Dump(" jd.bodyB = sims[%d];\n", indexB); + b3Dump(" jd.collideConnected = bool(%d);\n", joint->collideConnected); + b3Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", joint->localAnchorA.x, joint->localAnchorA.y); + b3Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", joint->localAnchorB.x, joint->localAnchorB.y); + b3Dump(" jd.referenceAngle = %.9g;\n", joint->referenceAngle); + b3Dump(" jd.enableLimit = bool(%d);\n", joint->enableLimit); + b3Dump(" jd.lowerAngle = %.9g;\n", joint->lowerAngle); + b3Dump(" jd.upperAngle = %.9g;\n", joint->upperAngle); + b3Dump(" jd.enableMotor = bool(%d);\n", joint->enableMotor); + b3Dump(" jd.motorSpeed = %.9g;\n", joint->motorSpeed); + b3Dump(" jd.maxMotorTorque = %.9g;\n", joint->maxMotorTorque); + b3Dump(" joints[%d] = joint->world->CreateJoint(&jd);\n", joint->index); +} +#endif + +void b3DrawWheelJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale ) +{ + B3_ASSERT( base->type == b3_wheelJoint ); + + b3WheelJoint* joint = &base->wheelJoint; + + b3Transform frameA = b3MulTransforms( transformA, base->localFrameA ); + b3Transform frameB = b3MulTransforms( transformB, base->localFrameB ); + + b3Matrix3 matrixA = b3MakeMatrixFromQuat( frameA.q ); + b3Matrix3 matrixB = b3MakeMatrixFromQuat( frameB.q ); + + draw->DrawSegmentFcn( frameA.p, frameB.p, b3_colorBlue, draw->context ); + + if ( joint->enableSuspensionLimit ) + { + b3Vec3 lower = b3MulAdd( frameA.p, joint->lowerSuspensionLimit, matrixA.cx ); + b3Vec3 upper = b3MulAdd( frameA.p, joint->upperSuspensionLimit, matrixA.cx ); + b3Vec3 perp = matrixA.cy; + draw->DrawSegmentFcn( lower, upper, b3_colorGray, draw->context ); + draw->DrawSegmentFcn( b3MulSub( lower, 0.1f * scale, perp ), b3MulAdd( lower, 0.1f * scale, perp ), b3_colorGreen, + draw->context ); + draw->DrawSegmentFcn( b3MulSub( upper, 0.1f * scale, perp ), b3MulAdd( upper, 0.1f * scale, perp ), b3_colorRed, + draw->context ); + } + else + { + draw->DrawSegmentFcn( b3MulSub( frameA.p, 1.0f * scale, matrixA.cx ), b3MulAdd( frameA.p, 1.0f * scale, matrixA.cx ), + b3_colorGray, draw->context ); + } + + if ( joint->enableSteering && joint->enableSteeringLimit ) + { + // b3Quat quatA = frameA.q; + // b3Quat quatB = frameB.q; + + // if ( b3DotQuat( quatA, quatB ) < 0.0f ) + //{ + // // this keeps the twist angle in the range [-pi, pi] + // quatB = -quatB; + // } + + // b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + b3Transform frame = { + .p = frameB.p, + .q = frameA.q, + }; + + const float radius = 0.5f * scale; + const int sliceCount = 16; + float lower = joint->lowerSteeringLimit; + float upper = joint->upperSteeringLimit; + + b3CosSin cs = b3ComputeCosSin( lower ); + b3Vec3 vertex1 = b3TransformPoint( frame, (b3Vec3){ 0.0f, -radius * cs.sine, radius * cs.cosine } ); + + for ( int index = 0; index < sliceCount; ++index ) + { + float t2 = ( index + 1.0f ) / sliceCount; + float phi = b3LerpFloat( lower, upper, t2 ); + + cs = b3ComputeCosSin( phi ); + b3Vec3 vertex2 = b3TransformPoint( frame, (b3Vec3){ 0.0f, -radius * cs.sine, radius * cs.cosine } ); + + if ( index == 0 ) + { + draw->DrawSegmentFcn( frame.p, vertex1, b3_colorCyan, draw->context ); + } + + if ( index == sliceCount - 1 ) + { + draw->DrawSegmentFcn( vertex2, frame.p, b3_colorCyan, draw->context ); + } + draw->DrawSegmentFcn( vertex1, vertex2, b3_colorCyan, draw->context ); + + vertex1 = vertex2; + } + } + + draw->DrawSegmentFcn( b3MulSub( frameB.p, 0.5f * scale, matrixB.cz ), b3MulAdd( frameB.p, 0.5f * scale, matrixB.cz ), + b3_colorMagenta, draw->context ); + + draw->DrawPointFcn( frameA.p, 5.0f, b3_colorGray, draw->context ); + draw->DrawPointFcn( frameB.p, 5.0f, b3_colorDimGray, draw->context ); +} diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..9d19d35 --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,40 @@ +# Box3D unit test app + +set(BOX3D_TEST_FILES + main.c + test_allocator.c + test_bitset.c + test_collision.c + test_compound.c + test_container.c + test_determinism.c + #test_distance.c + test_height_field.c + test_hull.c + test_id.c + test_macros.h + test_math.c + test_mover.c + test_shape.c + #test_table.c + test_world.c +) + +add_executable(test ${BOX3D_TEST_FILES}) + +set_target_properties(test PROPERTIES + C_STANDARD 17 + C_STANDARD_REQUIRED YES + C_EXTENSIONS NO +) + +# Special access to Box3D internals for testing +target_include_directories(test PRIVATE ${CMAKE_SOURCE_DIR}/src) + +if(BOX3D_VALIDATE) + target_compile_definitions(test PRIVATE BOX3D_VALIDATE) +endif() + +target_link_libraries(test PRIVATE box3d shared) + +source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "" FILES ${BOX3D_TEST_FILES}) diff --git a/test/main.c b/test/main.c new file mode 100644 index 0000000..feb5125 --- /dev/null +++ b/test/main.c @@ -0,0 +1,122 @@ +// SPDX-FileCopyrightText: 2024 Erin Catto +// SPDX-License-Identifier: MIT + +#include "test_macros.h" +#include "box3d/base.h" + +#include + +#if defined( _WIN32 ) +#include + +// int MyAllocHook(int allocType, void* userData, size_t size, int blockType, long requestNumber, const unsigned char* filename, +// int lineNumber) +//{ +// if (size == 16416) +// { +// size += 0; +// } +// +// return 1; +// } +#endif + +#ifdef TRACY_ENABLE +#include +#endif + +extern int AllocatorTest( void ); +extern int BitTest( void ); +extern int CollisionTest( void ); +extern int CompoundTest( void ); +extern int ContainerTest( void ); +extern int DeterminismTest( void ); +// extern int DistanceTest( void ); +extern int HeightFieldTest( void ); +extern int HullTest( void ); +extern int IdTest( void ); +extern int MathTest( void ); +extern int MoverTest( void ); +extern int ShapeTest( void ); +// extern int TableTest( void ); +extern int WorldTest( void ); + +// Filter-aware test runner: skips tests that don't match the filter +#define MAYBE_RUN_TEST( T ) \ + do \ + { \ + if ( filter != NULL && strcmp( filter, #T ) != 0 ) \ + { \ + printf( "test skipped: " #T "\n" ); \ + break; \ + } \ + RUN_TEST( T ); \ + } \ + while ( false ) + +int main( int argc, char** argv ) +{ +#if defined( _WIN32 ) + // Enable memory-leak reports + //_CrtSetBreakAlloc(196); + _CrtSetReportMode( _CRT_WARN, _CRTDBG_MODE_DEBUG | _CRTDBG_MODE_FILE ); + _CrtSetReportFile( _CRT_WARN, _CRTDBG_FILE_STDERR ); + //_CrtSetDbgFlag(_CRTDBG_LEAK_CHECK_DF | _CrtSetDbgFlag(_CRTDBG_REPORT_FLAG)); + //_CrtSetAllocHook(MyAllocHook); +#endif + +#ifdef TRACY_ENABLE + ___tracy_startup_profiler(); +#endif + + const char* filter = NULL; + if ( argc > 1 ) + { + filter = argv[1]; + } + + uint64_t ticks = b3GetTicks(); + + printf( "Starting Box3D unit tests\n" ); + if ( filter != NULL ) + { + printf( "Filter: %s\n", filter ); + } + printf( "======================================\n" ); + + MAYBE_RUN_TEST( AllocatorTest ); + MAYBE_RUN_TEST( BitTest ); + MAYBE_RUN_TEST( CollisionTest ); + MAYBE_RUN_TEST( CompoundTest ); + MAYBE_RUN_TEST( ContainerTest ); + MAYBE_RUN_TEST( DeterminismTest ); + MAYBE_RUN_TEST( HeightFieldTest ); + MAYBE_RUN_TEST( HullTest ); + MAYBE_RUN_TEST( IdTest ); + MAYBE_RUN_TEST( MathTest ); + MAYBE_RUN_TEST( MoverTest ); + MAYBE_RUN_TEST( ShapeTest ); + MAYBE_RUN_TEST( WorldTest ); + + // RUN_TEST( DistanceTest ); + // RUN_TEST( TableTest ); + + printf( "======================================\n" ); + printf( "All Box3D tests passed!\n" ); + + float duration = b3GetMilliseconds( ticks ); + printf( "Test duration = %.2f s\n", 0.001f * duration ); + +#ifdef TRACY_ENABLE + ___tracy_shutdown_profiler(); +#endif + +#if defined( _WIN32 ) + if ( _CrtDumpMemoryLeaks() ) + { + return 1; + } +#endif + + return 0; +} diff --git a/test/test_allocator.c b/test/test_allocator.c new file mode 100644 index 0000000..ca7acaf --- /dev/null +++ b/test/test_allocator.c @@ -0,0 +1,49 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "block_allocator.h" +#include "test_macros.h" + +typedef struct +{ + int value1; + float value2; +} Foo; + +_Static_assert( sizeof( Foo ) >= sizeof( void* ), "too small" ); + +static int TestBlockAllocate( void ) +{ + b3BlockAllocator allocator = b3CreateBlockAllocator( (int)sizeof( Foo ), 2 ); + + Foo* item1 = b3AllocateElement( &allocator ); + ENSURE( item1 != NULL ); + + Foo* item2 = b3AllocateElement( &allocator ); + ENSURE( item2 != NULL ); + + b3DestroyBlockAllocator( &allocator ); + return 0; +} + +static int TestBlockClear( void ) +{ + b3BlockAllocator allocator = b3CreateBlockAllocator( (int)sizeof( Foo ), 0 ); + + Foo* item1 = b3AllocateElement( &allocator ); + Foo* item2 = b3AllocateElement( &allocator ); + + b3FreeElement( &allocator, item1 ); + b3FreeElement( &allocator, item2 ); + + b3DestroyBlockAllocator( &allocator ); + return 0; +} + +int AllocatorTest( void ) +{ + RUN_SUBTEST( TestBlockAllocate ); + RUN_SUBTEST( TestBlockClear ); + + return 0; +} diff --git a/test/test_bitset.c b/test/test_bitset.c new file mode 100644 index 0000000..893cdf8 --- /dev/null +++ b/test/test_bitset.c @@ -0,0 +1,63 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "bitset.h" +#include "ctz.h" +#include "test_macros.h" + +#include + +#define COUNT 169 + +int TestBitMath( void ) +{ + uint32_t r1 = b3CLZ32( 9 ); + ENSURE( r1 == 31 - 3 ); + + for (int i = 1; i < 1000; ++i) + { + int e1 = b3LowerPowerOf2Exponent( i ); + int e2 = (int)floorf( log2f( (float)i ) ); + ENSURE(e1 == e2); + } + + return 0; +} + +int TestBitSet( void ) +{ + struct b3BitSet bitSet = b3CreateBitSet( COUNT ); + + b3SetBitCountAndClear( &bitSet, COUNT ); + bool values[COUNT] = { false }; + + int32_t i1 = 0, i2 = 1; + b3SetBit( &bitSet, i1 ); + values[i1] = true; + + while ( i2 < COUNT ) + { + b3SetBit( &bitSet, i2 ); + values[i2] = true; + int32_t next = i1 + i2; + i1 = i2; + i2 = next; + } + + for ( int32_t i = 0; i < COUNT; ++i ) + { + bool value = b3GetBit( &bitSet, i ); + ENSURE( value == values[i] ); + } + + b3DestroyBitSet( &bitSet ); + + return 0; +} + +int BitTest( void ) +{ + RUN_SUBTEST( TestBitMath ); + RUN_SUBTEST( TestBitSet ); + return 0; +} diff --git a/test/test_collision.c b/test/test_collision.c new file mode 100644 index 0000000..87fcea2 --- /dev/null +++ b/test/test_collision.c @@ -0,0 +1,210 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "aabb.h" +#include "test_macros.h" + +#include "box3d/math_functions.h" + +static int AABBTest( void ) +{ + b3AABB a; + a.lowerBound = ( b3Vec3 ){ -1.0f, -1.0f, -1.0f }; + a.upperBound = ( b3Vec3 ){ -2.0f, -2.0f, -2.0f }; + + ENSURE( b3IsValidAABB( a ) == false ); + + a.upperBound = ( b3Vec3 ){ 1.0f, 1.0f }; + ENSURE( b3IsValidAABB( a ) == true ); + + b3AABB b = { { 2.0f, 2.0f }, { 4.0f, 4.0f } }; + ENSURE( b3AABB_Overlaps( a, b ) == false ); + ENSURE( b3AABB_Contains( a, b ) == false ); + + return 0; +} + +static int TestRayAABBIntersection( void ) +{ + // Test 1: Ray passing through center of AABB + { + b3Vec3 lowerBound = { -1.0f, -1.0f, -1.0f }; + b3Vec3 upperBound = { 1.0f, 1.0f, 1.0f }; + b3Vec3 p1 = { -2.0f, 0.0f, 0.0f }; + b3Vec3 p2 = { 2.0f, 0.0f, 0.0f }; + float minFraction, maxFraction; + + bool hit = b3IntersectRayAndAABB( lowerBound, upperBound, p1, p2, &minFraction, &maxFraction ); + + ENSURE( hit == true ); + ENSURE( b3AbsFloat( minFraction - 0.25f ) < 0.001f ); // Enters at 25% of ray + ENSURE( b3AbsFloat( maxFraction - 0.75f ) < 0.001f ); // Exits at 75% of ray + } + + // Test 2: Ray starting inside AABB + { + b3Vec3 lowerBound = { -1.0f, -1.0f, -1.0f }; + b3Vec3 upperBound = { 1.0f, 1.0f, 1.0f }; + b3Vec3 p1 = { 0.0f, 0.0f, 0.0f }; + b3Vec3 p2 = { 2.0f, 0.0f, 0.0f }; + float minFraction, maxFraction; + + bool hit = b3IntersectRayAndAABB( lowerBound, upperBound, p1, p2, &minFraction, &maxFraction ); + + ENSURE( hit == true ); + ENSURE( minFraction == 0.0f ); // Starts inside + ENSURE( b3AbsFloat( maxFraction - 0.5f ) < 0.001f ); // Exits at 50% of ray + } + + // Test 3: Ray ending inside AABB + { + b3Vec3 lowerBound = { -1.0f, -1.0f, -1.0f }; + b3Vec3 upperBound = { 1.0f, 1.0f, 1.0f }; + b3Vec3 p1 = { -2.0f, 0.0f, 0.0f }; + b3Vec3 p2 = { 0.0f, 0.0f, 0.0f }; + float minFraction, maxFraction; + + bool hit = b3IntersectRayAndAABB( lowerBound, upperBound, p1, p2, &minFraction, &maxFraction ); + + ENSURE( hit == true ); + ENSURE( b3AbsFloat( minFraction - 0.5f ) < 0.001f ); // Enters at 50% of ray + ENSURE( maxFraction == 1.0f ); // Ends inside + } + + // Test 4: Ray completely inside AABB + { + b3Vec3 lowerBound = { -2.0f, -2.0f, -2.0f }; + b3Vec3 upperBound = { 2.0f, 2.0f, 2.0f }; + b3Vec3 p1 = { -1.0f, 0.0f, 0.0f }; + b3Vec3 p2 = { 1.0f, 0.0f, 0.0f }; + float minFraction, maxFraction; + + bool hit = b3IntersectRayAndAABB( lowerBound, upperBound, p1, p2, &minFraction, &maxFraction ); + + ENSURE( hit == true ); + ENSURE( minFraction == 0.0f ); + ENSURE( maxFraction == 1.0f ); + } + + // Test 5: Ray missing AABB + { + b3Vec3 lowerBound = { 0.0f, 0.0f, 0.0f }; + b3Vec3 upperBound = { 1.0f, 1.0f, 1.0f }; + b3Vec3 p1 = { -1.0f, 2.0f, 0.5f }; + b3Vec3 p2 = { 2.0f, 2.0f, 0.5f }; + float minFraction, maxFraction; + + bool hit = b3IntersectRayAndAABB( lowerBound, upperBound, p1, p2, &minFraction, &maxFraction ); + + ENSURE( hit == false ); + } + + // Test 6: Ray parallel to AABB face (no intersection) + { + b3Vec3 lowerBound = { 0.0f, 0.0f, 0.0f }; + b3Vec3 upperBound = { 1.0f, 1.0f, 1.0f }; + b3Vec3 p1 = { -1.0f, 2.0f, 0.5f }; + b3Vec3 p2 = { 2.0f, 2.0f, 0.5f }; + float minFraction, maxFraction; + + bool hit = b3IntersectRayAndAABB( lowerBound, upperBound, p1, p2, &minFraction, &maxFraction ); + + ENSURE( hit == false ); + } + + // Test 7: Ray parallel to AABB face (within bounds) + { + b3Vec3 lowerBound = { 0.0f, 0.0f, 0.0f }; + b3Vec3 upperBound = { 1.0f, 1.0f, 1.0f }; + b3Vec3 p1 = { -1.0f, 0.5f, 0.5f }; + b3Vec3 p2 = { 2.0f, 0.5f, 0.5f }; + float minFraction, maxFraction; + + bool hit = b3IntersectRayAndAABB( lowerBound, upperBound, p1, p2, &minFraction, &maxFraction ); + + ENSURE( hit == true ); + ENSURE( b3AbsFloat( minFraction - 1.0f / 3.0f ) < 0.001f ); + ENSURE( b3AbsFloat( maxFraction - 2.0f / 3.0f ) < 0.001f ); + } + + // Test 8: Degenerate ray (point) inside AABB + { + b3Vec3 lowerBound = { 0.0f, 0.0f, 0.0f }; + b3Vec3 upperBound = { 1.0f, 1.0f, 1.0f }; + b3Vec3 p1 = { 0.5f, 0.5f, 0.5f }; + b3Vec3 p2 = { 0.5f, 0.5f, 0.5f }; + float minFraction, maxFraction; + + bool hit = b3IntersectRayAndAABB( lowerBound, upperBound, p1, p2, &minFraction, &maxFraction ); + + ENSURE( hit == true ); + ENSURE( minFraction == 0.0f ); + ENSURE( maxFraction == 0.0f ); + } + + // Test 9: Degenerate ray (point) outside AABB + { + b3Vec3 lowerBound = { 0.0f, 0.0f, 0.0f }; + b3Vec3 upperBound = { 1.0f, 1.0f, 1.0f }; + b3Vec3 p1 = { 2.0f, 2.0f, 2.0f }; + b3Vec3 p2 = { 2.0f, 2.0f, 2.0f }; + float minFraction, maxFraction; + + bool hit = b3IntersectRayAndAABB( lowerBound, upperBound, p1, p2, &minFraction, &maxFraction ); + + ENSURE( hit == false ); + } + + // Test 10: Ray pointing away from AABB + { + b3Vec3 lowerBound = { 0.0f, 0.0f, 0.0f }; + b3Vec3 upperBound = { 1.0f, 1.0f, 1.0f }; + b3Vec3 p1 = { -1.0f, 0.5f, 0.5f }; + b3Vec3 p2 = { -2.0f, 0.5f, 0.5f }; + float minFraction, maxFraction; + + bool hit = b3IntersectRayAndAABB( lowerBound, upperBound, p1, p2, &minFraction, &maxFraction ); + + ENSURE( hit == false ); + } + + // Test 11: Ray hitting corner of AABB + { + b3Vec3 lowerBound = { 0.0f, 0.0f, 0.0f }; + b3Vec3 upperBound = { 1.0f, 1.0f, 1.0f }; + b3Vec3 p1 = { -1.0f, -1.0f, -1.0f }; + b3Vec3 p2 = { 2.0f, 2.0f, 2.0f }; + float minFraction, maxFraction; + + bool hit = b3IntersectRayAndAABB( lowerBound, upperBound, p1, p2, &minFraction, &maxFraction ); + + ENSURE( hit == true ); + ENSURE( b3AbsFloat( minFraction - 1.0f / 3.0f ) < 0.001f ); + ENSURE( b3AbsFloat( maxFraction - 2.0f / 3.0f ) < 0.001f ); + } + + // Test 12: Ray grazing edge of AABB + { + b3Vec3 lowerBound = { 0.0f, 0.0f, 0.0f }; + b3Vec3 upperBound = { 1.0f, 1.0f, 1.0f }; + b3Vec3 p1 = { -1.0f, 0.0f, 0.5f }; + b3Vec3 p2 = { 2.0f, 0.0f, 0.5f }; + float minFraction, maxFraction; + + bool hit = b3IntersectRayAndAABB( lowerBound, upperBound, p1, p2, &minFraction, &maxFraction ); + + ENSURE( hit == true ); + ENSURE( b3AbsFloat( minFraction - 1.0f / 3.0f ) < 0.001f ); + ENSURE( b3AbsFloat( maxFraction - 2.0f / 3.0f ) < 0.001f ); + } + + return 0; +} + +int CollisionTest( void ) +{ + RUN_SUBTEST( AABBTest ); + RUN_SUBTEST( TestRayAABBIntersection ); + + return 0; +} diff --git a/test/test_compound.c b/test/test_compound.c new file mode 100644 index 0000000..1dcb2ed --- /dev/null +++ b/test/test_compound.c @@ -0,0 +1,876 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "test_macros.h" + +// b3CollideMoverAndCompound, b3GetCompoundChild, b3QueryCompound are internal +#include "compound.h" + +#include "box3d/collision.h" +#include "box3d/math_functions.h" + +#include +#include +#include +#include + +static b3SurfaceMaterial MakeMaterial( float friction, uint64_t userId ) +{ + b3SurfaceMaterial m = b3DefaultSurfaceMaterial(); + m.friction = friction; + m.userMaterialId = userId; + return m; +} + +// --------------------------------------------------------------------------- +// Construction & layout +// --------------------------------------------------------------------------- + +static int CompoundCreateMixed( void ) +{ + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + + b3CompoundCapsuleDef capsules[1] = { + { .capsule = { { -1.0f, 0.0f, 0.0f }, { 1.0f, 0.0f, 0.0f }, 0.25f }, .material = mat }, + }; + + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3CompoundHullDef hulls[1] = { + { .hull = &box.base, .transform = b3Transform_identity, .material = mat }, + }; + + b3MeshData* meshData = b3CreateBoxMesh( b3Vec3_zero, (b3Vec3){ 0.5f, 0.5f, 0.5f }, false ); + b3CompoundMeshDef meshes[1] = { + { + .meshData = meshData, + .transform = b3Transform_identity, + .scale = { 1.0f, 1.0f, 1.0f }, + .materials = &mat, + .materialCount = 1, + }, + }; + + b3CompoundSphereDef spheres[2] = { + { .sphere = { { 5.0f, 0.0f, 0.0f }, 0.5f }, .material = mat }, + { .sphere = { { -5.0f, 0.0f, 0.0f }, 0.5f }, .material = mat }, + }; + + b3CompoundDef def = { + .capsules = capsules, .capsuleCount = 1, + .hulls = hulls, .hullCount = 1, + .meshes = meshes, .meshCount = 1, + .spheres = spheres, .sphereCount = 2, + }; + + b3Compound* compound = b3CreateCompound( &def ); + ENSURE( compound != NULL ); + ENSURE( compound->version == B3_COMPOUND_VERSION ); + ENSURE( compound->byteCount > (int)sizeof( b3Compound ) ); + + ENSURE( compound->capsuleCount == 1 ); + ENSURE( compound->hullCount == 1 ); + ENSURE( compound->meshCount == 1 ); + ENSURE( compound->sphereCount == 2 ); + + ENSURE( compound->materialCount == 1 ); + ENSURE( compound->sharedHullCount == 1 ); + ENSURE( compound->sharedMeshCount == 1 ); + + ENSURE( compound->tree.nodeCount > 0 ); + ENSURE( compound->tree.nodes != NULL ); + + b3DestroyCompound( compound ); + b3DestroyMesh( meshData ); + return 0; +} + +static int CompoundCreateSingleType( void ) +{ + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + + // Capsule only + { + b3CompoundCapsuleDef cap = { .capsule = { { 0, 0, 0 }, { 1, 0, 0 }, 0.5f }, .material = mat }; + b3CompoundDef def = { .capsules = &cap, .capsuleCount = 1 }; + b3Compound* c = b3CreateCompound( &def ); + ENSURE( c != NULL && c->capsuleCount == 1 && c->hullCount == 0 && c->meshCount == 0 && c->sphereCount == 0 ); + b3DestroyCompound( c ); + } + + // Hull only + { + b3BoxHull box = b3MakeBoxHull( 1, 1, 1 ); + b3CompoundHullDef h = { .hull = &box.base, .transform = b3Transform_identity, .material = mat }; + b3CompoundDef def = { .hulls = &h, .hullCount = 1 }; + b3Compound* c = b3CreateCompound( &def ); + ENSURE( c != NULL && c->hullCount == 1 && c->sharedHullCount == 1 && c->capsuleCount == 0 ); + b3DestroyCompound( c ); + } + + // Mesh only + { + b3MeshData* md = b3CreateBoxMesh( b3Vec3_zero, (b3Vec3){ 1, 1, 1 }, false ); + b3CompoundMeshDef m = { + .meshData = md, + .transform = b3Transform_identity, + .scale = { 1, 1, 1 }, + .materials = &mat, + .materialCount = 1, + }; + b3CompoundDef def = { .meshes = &m, .meshCount = 1 }; + b3Compound* c = b3CreateCompound( &def ); + ENSURE( c != NULL && c->meshCount == 1 && c->sharedMeshCount == 1 ); + b3DestroyCompound( c ); + b3DestroyMesh( md ); + } + + // Sphere only + { + b3CompoundSphereDef s = { .sphere = { { 0, 0, 0 }, 1.0f }, .material = mat }; + b3CompoundDef def = { .spheres = &s, .sphereCount = 1 }; + b3Compound* c = b3CreateCompound( &def ); + ENSURE( c != NULL && c->sphereCount == 1 ); + b3DestroyCompound( c ); + } + + return 0; +} + +// --------------------------------------------------------------------------- +// Material deduplication +// --------------------------------------------------------------------------- + +static int CompoundMaterialDedup( void ) +{ + b3SurfaceMaterial mat = MakeMaterial( 0.4f, 7 ); + + b3CompoundCapsuleDef caps[3]; + for ( int i = 0; i < 3; ++i ) + { + caps[i].capsule = (b3Capsule){ { (float)i, 0, 0 }, { (float)i + 1, 0, 0 }, 0.25f }; + caps[i].material = mat; + } + + b3CompoundDef def = { .capsules = caps, .capsuleCount = 3 }; + b3Compound* c = b3CreateCompound( &def ); + ENSURE( c->materialCount == 1 ); + for ( int i = 0; i < 3; ++i ) + { + b3CompoundCapsule cc = b3GetCompoundCapsule( c, i ); + ENSURE( cc.materialIndex == 0 ); + } + b3DestroyCompound( c ); + return 0; +} + +static int CompoundMaterialDistinct( void ) +{ + b3CompoundCapsuleDef caps[3]; + for ( int i = 0; i < 3; ++i ) + { + caps[i].capsule = (b3Capsule){ { (float)i, 0, 0 }, { (float)i + 1, 0, 0 }, 0.25f }; + caps[i].material = MakeMaterial( 0.1f * (float)( i + 1 ), (uint64_t)( i + 1 ) ); + } + + b3CompoundDef def = { .capsules = caps, .capsuleCount = 3 }; + b3Compound* c = b3CreateCompound( &def ); + ENSURE( c->materialCount == 3 ); + + const b3SurfaceMaterial* mats = b3GetCompoundMaterials( c ); + ENSURE( mats != NULL ); + + for ( int i = 0; i < 3; ++i ) + { + b3CompoundCapsule cc = b3GetCompoundCapsule( c, i ); + ENSURE( cc.materialIndex >= 0 && cc.materialIndex < 3 ); + ENSURE( mats[cc.materialIndex].userMaterialId == (uint64_t)( i + 1 ) ); + } + + b3DestroyCompound( c ); + return 0; +} + +static int CompoundMaterialCrossShape( void ) +{ + // One material shared across capsule, hull, and sphere → 1 material slot. + b3SurfaceMaterial mat = MakeMaterial( 0.5f, 99 ); + + b3CompoundCapsuleDef cap = { .capsule = { { 0, 0, 0 }, { 1, 0, 0 }, 0.25f }, .material = mat }; + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3CompoundHullDef hull = { .hull = &box.base, .transform = b3Transform_identity, .material = mat }; + b3CompoundSphereDef sph = { .sphere = { { 5, 0, 0 }, 0.5f }, .material = mat }; + + b3CompoundDef def = { + .capsules = &cap, .capsuleCount = 1, + .hulls = &hull, .hullCount = 1, + .spheres = &sph, .sphereCount = 1, + }; + b3Compound* c = b3CreateCompound( &def ); + ENSURE( c->materialCount == 1 ); + + ENSURE( b3GetCompoundCapsule( c, 0 ).materialIndex == 0 ); + ENSURE( b3GetCompoundHull( c, 0 ).materialIndex == 0 ); + ENSURE( b3GetCompoundSphere( c, 0 ).materialIndex == 0 ); + + b3DestroyCompound( c ); + return 0; +} + +static int CompoundMaterialMeshShared( void ) +{ + // Mesh material entries are routed through the same material map as convex + // materials, so an identical material is deduped across mesh and convex. + // (The comment in compound.c about meshes "not being shared" refers to the + // per-instance materialIndices arrays, not the b3SurfaceMaterial table.) + b3SurfaceMaterial mat = MakeMaterial( 0.3f, 11 ); + + b3MeshData* md = b3CreateBoxMesh( b3Vec3_zero, (b3Vec3){ 1, 1, 1 }, false ); + ENSURE( md->materialCount == 1 ); + + b3CompoundSphereDef sph = { .sphere = { { 5, 0, 0 }, 0.5f }, .material = mat }; + b3CompoundMeshDef mesh = { + .meshData = md, + .transform = b3Transform_identity, + .scale = { 1, 1, 1 }, + .materials = &mat, + .materialCount = 1, + }; + + b3CompoundDef def = { + .meshes = &mesh, .meshCount = 1, + .spheres = &sph, .sphereCount = 1, + }; + b3Compound* c = b3CreateCompound( &def ); + ENSURE( c->materialCount == 1 ); + + b3DestroyCompound( c ); + b3DestroyMesh( md ); + return 0; +} + +// --------------------------------------------------------------------------- +// Hull / mesh blob sharing +// --------------------------------------------------------------------------- + +static int CompoundHullSharingPointer( void ) +{ + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + b3BoxHull box = b3MakeBoxHull( 1, 1, 1 ); + + b3CompoundHullDef hulls[3]; + for ( int i = 0; i < 3; ++i ) + { + hulls[i].hull = &box.base; + hulls[i].transform = b3Transform_identity; + hulls[i].transform.p.x = (float)( 4 * i ); + hulls[i].material = mat; + } + + b3CompoundDef def = { .hulls = hulls, .hullCount = 3 }; + b3Compound* c = b3CreateCompound( &def ); + ENSURE( c->hullCount == 3 ); + ENSURE( c->sharedHullCount == 1 ); + b3DestroyCompound( c ); + return 0; +} + +static int CompoundHullSharingContent( void ) +{ + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + + // Two box hulls built independently with identical args are byte-identical + // (b3MakeBoxHull is deterministic and the hash is computed over the bytes). + b3BoxHull boxA = b3MakeBoxHull( 1, 1, 1 ); + b3BoxHull boxB = b3MakeBoxHull( 1, 1, 1 ); + ENSURE( &boxA != &boxB ); + + b3CompoundHullDef hulls[2] = { + { .hull = &boxA.base, .transform = b3Transform_identity, .material = mat }, + { .hull = &boxB.base, .transform = b3Transform_identity, .material = mat }, + }; + hulls[1].transform.p.x = 5.0f; + + b3CompoundDef def = { .hulls = hulls, .hullCount = 2 }; + b3Compound* c = b3CreateCompound( &def ); + ENSURE( c->sharedHullCount == 1 ); + b3DestroyCompound( c ); + return 0; +} + +static int CompoundHullDistinct( void ) +{ + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + b3BoxHull boxA = b3MakeBoxHull( 1, 1, 1 ); + b3BoxHull boxB = b3MakeBoxHull( 2, 1, 1 ); + + b3CompoundHullDef hulls[2] = { + { .hull = &boxA.base, .transform = b3Transform_identity, .material = mat }, + { .hull = &boxB.base, .transform = b3Transform_identity, .material = mat }, + }; + hulls[1].transform.p.x = 5.0f; + + b3CompoundDef def = { .hulls = hulls, .hullCount = 2 }; + b3Compound* c = b3CreateCompound( &def ); + ENSURE( c->sharedHullCount == 2 ); + b3DestroyCompound( c ); + return 0; +} + +static int CompoundMeshSharingPointer( void ) +{ + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + b3MeshData* md = b3CreateBoxMesh( b3Vec3_zero, (b3Vec3){ 1, 1, 1 }, false ); + + b3CompoundMeshDef meshes[3]; + for ( int i = 0; i < 3; ++i ) + { + meshes[i].meshData = md; + meshes[i].transform = b3Transform_identity; + meshes[i].transform.p.x = (float)( 4 * i ); + meshes[i].scale = (b3Vec3){ 1, 1, 1 }; + meshes[i].materials = &mat; + meshes[i].materialCount = 1; + } + + b3CompoundDef def = { .meshes = meshes, .meshCount = 3 }; + b3Compound* c = b3CreateCompound( &def ); + ENSURE( c->meshCount == 3 ); + ENSURE( c->sharedMeshCount == 1 ); + + b3DestroyCompound( c ); + b3DestroyMesh( md ); + return 0; +} + +static int CompoundMeshSharingContent( void ) +{ + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + b3MeshData* mdA = b3CreateBoxMesh( b3Vec3_zero, (b3Vec3){ 1, 1, 1 }, false ); + b3MeshData* mdB = b3CreateBoxMesh( b3Vec3_zero, (b3Vec3){ 1, 1, 1 }, false ); + ENSURE( mdA != mdB ); + + b3CompoundMeshDef meshes[2] = { + { .meshData = mdA, .transform = b3Transform_identity, .scale = { 1, 1, 1 }, .materials = &mat, .materialCount = 1 }, + { .meshData = mdB, .transform = b3Transform_identity, .scale = { 1, 1, 1 }, .materials = &mat, .materialCount = 1 }, + }; + meshes[1].transform.p.x = 5.0f; + + b3CompoundDef def = { .meshes = meshes, .meshCount = 2 }; + b3Compound* c = b3CreateCompound( &def ); + ENSURE( c->sharedMeshCount == 1 ); + + b3DestroyCompound( c ); + b3DestroyMesh( mdA ); + b3DestroyMesh( mdB ); + return 0; +} + +static int CompoundMeshDistinct( void ) +{ + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + b3MeshData* mdA = b3CreateBoxMesh( b3Vec3_zero, (b3Vec3){ 1, 1, 1 }, false ); + b3MeshData* mdB = b3CreateBoxMesh( b3Vec3_zero, (b3Vec3){ 2, 1, 1 }, false ); + + b3CompoundMeshDef meshes[2] = { + { .meshData = mdA, .transform = b3Transform_identity, .scale = { 1, 1, 1 }, .materials = &mat, .materialCount = 1 }, + { .meshData = mdB, .transform = b3Transform_identity, .scale = { 1, 1, 1 }, .materials = &mat, .materialCount = 1 }, + }; + meshes[1].transform.p.x = 5.0f; + + b3CompoundDef def = { .meshes = meshes, .meshCount = 2 }; + b3Compound* c = b3CreateCompound( &def ); + ENSURE( c->sharedMeshCount == 2 ); + + b3DestroyCompound( c ); + b3DestroyMesh( mdA ); + b3DestroyMesh( mdB ); + return 0; +} + +// --------------------------------------------------------------------------- +// Child accessors and dispatch +// --------------------------------------------------------------------------- + +static int CompoundChildDispatch( void ) +{ + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3MeshData* md = b3CreateBoxMesh( b3Vec3_zero, (b3Vec3){ 0.5f, 0.5f, 0.5f }, false ); + + b3CompoundCapsuleDef caps[2] = { + { .capsule = { { 0, 0, 0 }, { 1, 0, 0 }, 0.2f }, .material = mat }, + { .capsule = { { 0, 2, 0 }, { 1, 2, 0 }, 0.2f }, .material = mat }, + }; + b3CompoundHullDef hulls[1] = { { .hull = &box.base, .transform = b3Transform_identity, .material = mat } }; + hulls[0].transform.p = (b3Vec3){ 5, 0, 0 }; + b3CompoundMeshDef meshes[1] = { { + .meshData = md, + .transform = b3Transform_identity, + .scale = { 1, 1, 1 }, + .materials = &mat, + .materialCount = 1, + } }; + meshes[0].transform.p = (b3Vec3){ 0, 0, 5 }; + b3CompoundSphereDef spheres[1] = { { .sphere = { { -5, 0, 0 }, 0.5f }, .material = mat } }; + + b3CompoundDef def = { + .capsules = caps, .capsuleCount = 2, + .hulls = hulls, .hullCount = 1, + .meshes = meshes, .meshCount = 1, + .spheres = spheres, .sphereCount = 1, + }; + b3Compound* c = b3CreateCompound( &def ); + + // Index ordering is capsules → hulls → meshes → spheres. + ENSURE( b3GetCompoundChild( c, 0 ).type == b3_capsuleShape ); + ENSURE( b3GetCompoundChild( c, 1 ).type == b3_capsuleShape ); + ENSURE( b3GetCompoundChild( c, 2 ).type == b3_hullShape ); + ENSURE( b3GetCompoundChild( c, 3 ).type == b3_meshShape ); + ENSURE( b3GetCompoundChild( c, 4 ).type == b3_sphereShape ); + + // Capsule and sphere children always report identity transform — the position + // is encoded in the shape itself (capsule->center{1,2}, sphere->center). + b3ChildShape cap0 = b3GetCompoundChild( c, 0 ); + ENSURE_SMALL( cap0.transform.p.x, FLT_EPSILON ); + ENSURE_SMALL( cap0.transform.p.y, FLT_EPSILON ); + ENSURE_SMALL( cap0.transform.p.z, FLT_EPSILON ); + ENSURE_SMALL( cap0.capsule.center2.x - 1.0f, FLT_EPSILON ); + + b3ChildShape sph = b3GetCompoundChild( c, 4 ); + ENSURE_SMALL( sph.transform.p.x, FLT_EPSILON ); + ENSURE_SMALL( sph.sphere.center.x + 5.0f, FLT_EPSILON ); + + // Hull and mesh children carry their stored transform. + b3ChildShape hull = b3GetCompoundChild( c, 2 ); + ENSURE_SMALL( hull.transform.p.x - 5.0f, FLT_EPSILON ); + + b3ChildShape mesh = b3GetCompoundChild( c, 3 ); + ENSURE_SMALL( mesh.transform.p.z - 5.0f, FLT_EPSILON ); + ENSURE( mesh.mesh.data != NULL ); + + b3DestroyCompound( c ); + b3DestroyMesh( md ); + return 0; +} + +// --------------------------------------------------------------------------- +// AABB +// --------------------------------------------------------------------------- + +static int CompoundAABBContainsChildren( void ) +{ + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + b3CompoundSphereDef spheres[2] = { + { .sphere = { { -3, 0, 0 }, 1.0f }, .material = mat }, + { .sphere = { { 4, 0, 0 }, 0.5f }, .material = mat }, + }; + b3CompoundDef def = { .spheres = spheres, .sphereCount = 2 }; + b3Compound* c = b3CreateCompound( &def ); + + b3AABB local = b3ComputeCompoundAABB( c, b3Transform_identity ); + ENSURE( local.lowerBound.x <= -4.0f + 1e-5f ); + ENSURE( local.upperBound.x >= 4.5f - 1e-5f ); + ENSURE( local.lowerBound.y <= -1.0f + 1e-5f ); + ENSURE( local.upperBound.y >= 1.0f - 1e-5f ); + + // Translation commutes through the bounding-box transform. + b3Transform xf = { .p = { 10, 20, 30 }, .q = b3Quat_identity }; + b3AABB world = b3ComputeCompoundAABB( c, xf ); + ENSURE_SMALL( world.lowerBound.x - ( local.lowerBound.x + 10.0f ), 1e-4f ); + ENSURE_SMALL( world.upperBound.y - ( local.upperBound.y + 20.0f ), 1e-4f ); + ENSURE_SMALL( world.lowerBound.z - ( local.lowerBound.z + 30.0f ), 1e-4f ); + + b3DestroyCompound( c ); + return 0; +} + +// --------------------------------------------------------------------------- +// Ray cast / shape cast +// --------------------------------------------------------------------------- + +static int CompoundRayCastMiss( void ) +{ + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + b3CompoundSphereDef sph = { .sphere = { { 0, 0, 0 }, 0.5f }, .material = mat }; + b3CompoundDef def = { .spheres = &sph, .sphereCount = 1 }; + b3Compound* c = b3CreateCompound( &def ); + + // Ray well above the sphere on a parallel path. + b3RayCastInput input = { .origin = { -5, 5, 0 }, .translation = { 10, 0, 0 }, .maxFraction = 1.0f }; + b3CastOutput out = b3RayCastCompound( c, &input ); + ENSURE( out.hit == false ); + + b3DestroyCompound( c ); + return 0; +} + +static int CompoundRayCastClosest( void ) +{ + b3SurfaceMaterial matA = MakeMaterial( 0.4f, 100 ); + b3SurfaceMaterial matB = MakeMaterial( 0.4f, 200 ); + + // Two unit spheres along +X. Ray from origin must hit the nearer one first. + b3CompoundSphereDef spheres[2] = { + { .sphere = { { 5, 0, 0 }, 1.0f }, .material = matA }, + { .sphere = { { 10, 0, 0 }, 1.0f }, .material = matB }, + }; + b3CompoundDef def = { .spheres = spheres, .sphereCount = 2 }; + b3Compound* c = b3CreateCompound( &def ); + + b3RayCastInput input = { .origin = { 0, 0, 0 }, .translation = { 20, 0, 0 }, .maxFraction = 1.0f }; + b3CastOutput out = b3RayCastCompound( c, &input ); + ENSURE( out.hit == true ); + + // Front face of the nearer sphere is at x=4 → fraction 4/20 = 0.2. + ENSURE_SMALL( out.fraction - 0.2f, 1e-4f ); + ENSURE_SMALL( out.normal.x + 1.0f, 1e-4f ); + ENSURE( out.childIndex == 0 ); + + const b3SurfaceMaterial* mats = b3GetCompoundMaterials( c ); + ENSURE( mats[out.materialIndex].userMaterialId == 100 ); + + b3DestroyCompound( c ); + return 0; +} + +static int CompoundRayCastHullNormalRotation( void ) +{ + // A unit box rotated 90° about Z, placed at compound +X. The ray hits the + // face that, in compound space, points back toward -X. Verifies that the + // normal returned by the cast has been rotated from hull-local space back + // into compound space. + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + b3BoxHull box = b3MakeBoxHull( 1, 1, 1 ); + + b3CompoundHullDef hull = { + .hull = &box.base, + .transform = { .p = { 5, 0, 0 }, .q = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.5f * B3_PI ) }, + .material = mat, + }; + b3CompoundDef def = { .hulls = &hull, .hullCount = 1 }; + b3Compound* c = b3CreateCompound( &def ); + + b3RayCastInput input = { .origin = { 0, 0, 0 }, .translation = { 20, 0, 0 }, .maxFraction = 1.0f }; + b3CastOutput out = b3RayCastCompound( c, &input ); + ENSURE( out.hit == true ); + // Box has |hx|=1; with the rotation a face still intersects the +X ray at x=4. + ENSURE_SMALL( out.fraction - 0.2f, 1e-4f ); + ENSURE_SMALL( out.normal.x + 1.0f, 1e-3f ); + ENSURE_SMALL( out.normal.y, 1e-3f ); + ENSURE_SMALL( out.normal.z, 1e-3f ); + + b3DestroyCompound( c ); + return 0; +} + +static int CompoundShapeCastClosest( void ) +{ + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + b3CompoundSphereDef spheres[2] = { + { .sphere = { { 5, 0, 0 }, 1.0f }, .material = mat }, + { .sphere = { { 10, 0, 0 }, 1.0f }, .material = mat }, + }; + b3CompoundDef def = { .spheres = spheres, .sphereCount = 2 }; + b3Compound* c = b3CreateCompound( &def ); + + b3Vec3 point = { 0, 0, 0 }; + b3ShapeCastInput input = { + .proxy = { .points = &point, .count = 1, .radius = 0.25f }, + .translation = { 20, 0, 0 }, + .maxFraction = 1.0f, + .canEncroach = false, + }; + b3CastOutput out = b3ShapeCastCompound( c, &input ); + ENSURE( out.hit == true ); + // Closest contact: caster radius 0.25 + sphere radius 1.0 → first contact at x ≈ 3.75. + ENSURE_SMALL( out.fraction - 3.75f / 20.0f, 1e-3f ); + ENSURE( out.childIndex == 0 ); + + b3DestroyCompound( c ); + return 0; +} + +// --------------------------------------------------------------------------- +// Overlap +// --------------------------------------------------------------------------- + +static int CompoundOverlap( void ) +{ + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + b3CompoundSphereDef spheres[2] = { + { .sphere = { { -3, 0, 0 }, 0.5f }, .material = mat }, + { .sphere = { { 3, 0, 0 }, 0.5f }, .material = mat }, + }; + b3CompoundDef def = { .spheres = spheres, .sphereCount = 2 }; + b3Compound* c = b3CreateCompound( &def ); + + // Proxy at the origin lies in the gap between the two spheres. + b3Vec3 origin = { 0, 0, 0 }; + b3ShapeProxy gap = { .points = &origin, .count = 1, .radius = 0.25f }; + ENSURE( b3OverlapCompound( c, b3Transform_identity, &gap ) == false ); + + // Proxy at the center of the second sphere overlaps it. + b3Vec3 onSecond = { 3, 0, 0 }; + b3ShapeProxy hit = { .points = &onSecond, .count = 1, .radius = 0.1f }; + ENSURE( b3OverlapCompound( c, b3Transform_identity, &hit ) == true ); + + b3DestroyCompound( c ); + return 0; +} + +// --------------------------------------------------------------------------- +// Query +// --------------------------------------------------------------------------- + +struct QueryAccum +{ + int childIndices[8]; + int count; + int stopAfter; // -1 = never stop early +}; + +static bool QueryCallback( const b3Compound* compound, int childIndex, void* context ) +{ + (void)compound; + struct QueryAccum* acc = context; + if ( acc->count < (int)( sizeof( acc->childIndices ) / sizeof( acc->childIndices[0] ) ) ) + { + acc->childIndices[acc->count++] = childIndex; + } + if ( acc->stopAfter >= 0 && acc->count >= acc->stopAfter ) + { + return false; + } + return true; +} + +static int CompoundQuery( void ) +{ + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + b3CompoundSphereDef spheres[3] = { + { .sphere = { { -10, 0, 0 }, 0.5f }, .material = mat }, + { .sphere = { { 0, 0, 0 }, 0.5f }, .material = mat }, + { .sphere = { { 10, 0, 0 }, 0.5f }, .material = mat }, + }; + b3CompoundDef def = { .spheres = spheres, .sphereCount = 3 }; + b3Compound* c = b3CreateCompound( &def ); + + // Tight box around the middle sphere — only it should be reported. + b3AABB middle = { { -1, -1, -1 }, { 1, 1, 1 } }; + struct QueryAccum acc = { .stopAfter = -1 }; + b3QueryCompound( c, middle, QueryCallback, &acc ); + ENSURE( acc.count == 1 ); + ENSURE( acc.childIndices[0] == 1 ); + + // Wide box overlapping all three; early-exit after the first reported child. + b3AABB wide = { { -20, -1, -1 }, { 20, 1, 1 } }; + struct QueryAccum stop = { .stopAfter = 1 }; + b3QueryCompound( c, wide, QueryCallback, &stop ); + ENSURE( stop.count == 1 ); + + // Without early-exit, all three are visited. + struct QueryAccum all = { .stopAfter = -1 }; + b3QueryCompound( c, wide, QueryCallback, &all ); + ENSURE( all.count == 3 ); + + b3DestroyCompound( c ); + return 0; +} + +// --------------------------------------------------------------------------- +// Mover +// --------------------------------------------------------------------------- + +static int CompoundMover( void ) +{ + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + + // Two boxes side-by-side along X, gap of 1 between them. + b3CompoundHullDef hulls[2] = { + { .hull = &box.base, .transform = { .p = { -1.0f, 0, 0 }, .q = b3Quat_identity }, .material = mat }, + { .hull = &box.base, .transform = { .p = { 1.0f, 0, 0 }, .q = b3Quat_identity }, .material = mat }, + }; + b3CompoundDef def = { .hulls = hulls, .hullCount = 2 }; + b3Compound* c = b3CreateCompound( &def ); + + // Small capsule mover sitting on top of the boxes, low enough to penetrate both. + b3Capsule mover = { { -1.0f, 0.6f, 0 }, { 1.0f, 0.6f, 0 }, 0.2f }; + + b3PlaneResult planes[8] = { 0 }; + int planeCount = b3CollideMoverAndCompound( planes, 8, c, &mover ); + + // Both boxes contribute at least one plane each; the +Y face of each box + // should produce a plane whose normal points roughly +Y in compound space. + ENSURE( planeCount >= 2 ); + + int upPlanes = 0; + for ( int i = 0; i < planeCount; ++i ) + { + if ( planes[i].plane.normal.y > 0.9f ) + { + upPlanes += 1; + } + } + ENSURE( upPlanes >= 2 ); + + // Capacity cap is honored: ask for 1 and the call must not exceed it. + b3PlaneResult one[1] = { 0 }; + int capped = b3CollideMoverAndCompound( one, 1, c, &mover ); + ENSURE( capped <= 1 ); + + b3DestroyCompound( c ); + return 0; +} + +// --------------------------------------------------------------------------- +// Serialization round-trip +// --------------------------------------------------------------------------- + +static b3Compound* BuildSerializableCompound( b3MeshData* md ) +{ + b3SurfaceMaterial mat = b3DefaultSurfaceMaterial(); + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + + b3CompoundCapsuleDef cap = { .capsule = { { -2, 0, 0 }, { -1, 0, 0 }, 0.2f }, .material = mat }; + b3CompoundHullDef hull = { .hull = &box.base, .transform = { .p = { 5, 0, 0 }, .q = b3Quat_identity }, .material = mat }; + b3CompoundMeshDef mesh = { + .meshData = md, + .transform = { .p = { 0, 0, 5 }, .q = b3Quat_identity }, + .scale = { 1, 1, 1 }, + .materials = &mat, + .materialCount = 1, + }; + b3CompoundSphereDef sph = { .sphere = { { -5, 0, 0 }, 0.5f }, .material = mat }; + + b3CompoundDef def = { + .capsules = &cap, .capsuleCount = 1, + .hulls = &hull, .hullCount = 1, + .meshes = &mesh, .meshCount = 1, + .spheres = &sph, .sphereCount = 1, + }; + return b3CreateCompound( &def ); +} + +static int CompoundSerializeRoundtrip( void ) +{ + b3MeshData* md = b3CreateBoxMesh( b3Vec3_zero, (b3Vec3){ 0.5f, 0.5f, 0.5f }, false ); + b3Compound* a = BuildSerializableCompound( md ); + + // Snapshot a few queries on the original. + b3AABB aabbA = b3ComputeCompoundAABB( a, b3Transform_identity ); + b3RayCastInput rayInput = { .origin = { 0, 0, 0 }, .translation = { 20, 0, 0 }, .maxFraction = 1.0f }; + b3CastOutput rayA = b3RayCastCompound( a, &rayInput ); + ENSURE( rayA.hit ); + + int byteCount = a->byteCount; + uint8_t* buffer = (uint8_t*)malloc( (size_t)byteCount ); + ENSURE( buffer != NULL ); + + uint8_t* serialized = b3ConvertCompoundToBytes( a ); + memcpy( buffer, serialized, (size_t)byteCount ); + b3DestroyCompound( a ); + + b3Compound* b = b3ConvertBytesToCompound( buffer, byteCount ); + ENSURE( b != NULL ); + ENSURE( b->byteCount == byteCount ); + ENSURE( b->capsuleCount == 1 ); + ENSURE( b->hullCount == 1 ); + ENSURE( b->meshCount == 1 ); + ENSURE( b->sphereCount == 1 ); + ENSURE( b->tree.nodes != NULL ); + + // Bounds and ray cast on the deserialized compound match the original. + b3AABB aabbB = b3ComputeCompoundAABB( b, b3Transform_identity ); + ENSURE_SMALL( aabbA.lowerBound.x - aabbB.lowerBound.x, 1e-5f ); + ENSURE_SMALL( aabbA.upperBound.z - aabbB.upperBound.z, 1e-5f ); + + b3CastOutput rayB = b3RayCastCompound( b, &rayInput ); + ENSURE( rayB.hit == rayA.hit ); + ENSURE_SMALL( rayB.fraction - rayA.fraction, 1e-5f ); + ENSURE( rayB.childIndex == rayA.childIndex ); + + free( buffer ); + b3DestroyMesh( md ); + return 0; +} + +static int CompoundSerializeBadVersion( void ) +{ + b3MeshData* md = b3CreateBoxMesh( b3Vec3_zero, (b3Vec3){ 0.5f, 0.5f, 0.5f }, false ); + b3Compound* a = BuildSerializableCompound( md ); + + int byteCount = a->byteCount; + uint8_t* buffer = (uint8_t*)malloc( (size_t)byteCount ); + memcpy( buffer, b3ConvertCompoundToBytes( a ), (size_t)byteCount ); + b3DestroyCompound( a ); + + // Corrupt the version word (first 8 bytes of b3Compound). + ((b3Compound*)buffer)->version ^= 0x1ull; + + b3Compound* b = b3ConvertBytesToCompound( buffer, byteCount ); + ENSURE( b == NULL ); + + free( buffer ); + b3DestroyMesh( md ); + return 0; +} + +static int CompoundSerializeWrongByteCount( void ) +{ + b3MeshData* md = b3CreateBoxMesh( b3Vec3_zero, (b3Vec3){ 0.5f, 0.5f, 0.5f }, false ); + b3Compound* a = BuildSerializableCompound( md ); + + int byteCount = a->byteCount; + uint8_t* buffer = (uint8_t*)malloc( (size_t)byteCount ); + memcpy( buffer, b3ConvertCompoundToBytes( a ), (size_t)byteCount ); + b3DestroyCompound( a ); + + // Off-by-one in the declared length is rejected. + b3Compound* b = b3ConvertBytesToCompound( buffer, byteCount + 1 ); + ENSURE( b == NULL ); + + free( buffer ); + b3DestroyMesh( md ); + return 0; +} + +// --------------------------------------------------------------------------- + +int CompoundTest( void ) +{ + RUN_SUBTEST( CompoundCreateMixed ); + RUN_SUBTEST( CompoundCreateSingleType ); + + RUN_SUBTEST( CompoundMaterialDedup ); + RUN_SUBTEST( CompoundMaterialDistinct ); + RUN_SUBTEST( CompoundMaterialCrossShape ); + RUN_SUBTEST( CompoundMaterialMeshShared ); + + RUN_SUBTEST( CompoundHullSharingPointer ); + RUN_SUBTEST( CompoundHullSharingContent ); + RUN_SUBTEST( CompoundHullDistinct ); + RUN_SUBTEST( CompoundMeshSharingPointer ); + RUN_SUBTEST( CompoundMeshSharingContent ); + RUN_SUBTEST( CompoundMeshDistinct ); + + RUN_SUBTEST( CompoundChildDispatch ); + + RUN_SUBTEST( CompoundAABBContainsChildren ); + + RUN_SUBTEST( CompoundRayCastMiss ); + RUN_SUBTEST( CompoundRayCastClosest ); + RUN_SUBTEST( CompoundRayCastHullNormalRotation ); + RUN_SUBTEST( CompoundShapeCastClosest ); + + RUN_SUBTEST( CompoundOverlap ); + RUN_SUBTEST( CompoundQuery ); + + RUN_SUBTEST( CompoundMover ); + + RUN_SUBTEST( CompoundSerializeRoundtrip ); + RUN_SUBTEST( CompoundSerializeBadVersion ); + RUN_SUBTEST( CompoundSerializeWrongByteCount ); + + return 0; +} diff --git a/test/test_container.c b/test/test_container.c new file mode 100644 index 0000000..51cc32b --- /dev/null +++ b/test/test_container.c @@ -0,0 +1,195 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "container.h" +#include "core.h" +#include "test_macros.h" + +#include +#include + +b3DeclareArrayNative( uint64_t ); +b3DeclareArrayNative( int16_t ); +b3DeclareArrayNative( uint8_t ); + +typedef struct Foo +{ + int a; + float b; +} Foo; + +b3DeclareArray( Foo ); + +typedef struct Bar +{ + b3ArrayC( int ) a; +} Bar; + +static int TestCreateDestroy( void ) +{ + b3ArrayC( int ) a; + b3Array_Create( a ); + b3Array_Destroy( a ); + return 0; +} + +static int TestAccess( void ) +{ + b3ArrayC( int ) a; + b3Array_Create( a ); + b3Array_Push( a, 42 ); + int* element = b3Array_Get( a, 0 ); + ENSURE( *element == 42 ); + b3Array_Destroy( a ); + return 0; +} + +static int TestIteration( void ) +{ + b3ArrayC( int ) a = { 0 }; + b3Array_Push( a, 1 ); + b3Array_Push( a, 2 ); + b3Array_Push( a, 3 ); + + int sum = 0; + for ( int i = 0; i < a.count; ++i ) + { + sum += a.data[i]; + } + + ENSURE( sum == 6 ); + b3Array_Destroy( a ); + return 0; +} + +static int TestArrayOfStruct( void ) +{ + b3ArrayC( Foo ) a; + b3Array_Create( a ); + b3Array_Push( a, ( (Foo){ .a = 1, .b = 5.0f } ) ); + b3Array_Push( a, ( (Foo){ .a = 2, .b = 6.0f } ) ); + b3Array_Push( a, ( (Foo){ .a = 3, .b = 7.0f } ) ); + + int sum1 = 0; + float sum2 = 0.0f; + for ( int i = 0; i < a.count; ++i ) + { + sum1 += a.data[i].a; + sum2 += a.data[i].b; + } + + ENSURE( sum1 == 6 ); + ENSURE( sum2 == 18.0f ); + + b3Array_Destroy( a ); + return 0; +} + +static int TestStructWithArray( void ) +{ + Bar a; + b3Array_Create( a.a ); + b3Array_Push( a.a, 1 ); + b3Array_Push( a.a, 2 ); + b3Array_Push( a.a, 3 ); + + int sum1 = 0; + for ( int i = 0; i < a.a.count; ++i ) + { + sum1 += a.a.data[i]; + } + + ENSURE( sum1 == 6 ); + + b3Array_Destroy( a.a ); + return 0; +} + +static int TestArrayEmplace( void ) +{ + b3ArrayC( int ) a = { 0 }; + + int n = 100; + for ( int i = 0; i < n; ++i ) + { + int* j = b3Array_Emplace( a ); + *j = i; + } + + int sum = 0; + for ( int i = 0; i < a.count; ++i ) + { + sum += a.data[i]; + } + + ENSURE( sum == n * (n - 1) / 2); + + b3Array_Destroy( a ); + return 0; +} + +static int TestArrayRemove( void ) +{ + b3ArrayC( int16_t ) a = { 0 }; + + int n = 100; + b3Array_Reserve( a, n ); + ENSURE( a.capacity == n && a.count == 0 ); + + for ( int i = 0; i < n; ++i ) + { + b3Array_Push( a, i ); + } + + int sum = 0; + for ( int i = 0; i < n; ++i ) + { + int16_t* value = b3Array_Get( a, 0 ); + sum += *value; + b3Array_RemoveSwap(a, 0); + } + + ENSURE( sum == n * ( n - 1 ) / 2 ); + + b3Array_Destroy( a ); + return 0; +} + +static int TestArrayPop( void ) +{ + b3ArrayC( int ) a = { 0 }; + + int n = 10; + b3Array_Resize( a, n ); + ENSURE( a.capacity == n && a.count == n ); + + for ( int i = 0; i < n; ++i ) + { + a.data[i] = i; + } + + int sum = 0; + while (a.count > 0) + { + sum += b3Array_Pop( a ); + } + + ENSURE( sum == n * ( n - 1 ) / 2 ); + + b3Array_Destroy( a ); + return 0; +} + +int ContainerTest( void ) +{ + RUN_SUBTEST( TestCreateDestroy ); + RUN_SUBTEST( TestAccess ); + RUN_SUBTEST( TestIteration ); + RUN_SUBTEST( TestArrayOfStruct ); + RUN_SUBTEST( TestStructWithArray ); + RUN_SUBTEST( TestArrayEmplace ); + RUN_SUBTEST( TestArrayRemove ); + RUN_SUBTEST( TestArrayPop ); + + return 0; +} diff --git a/test/test_determinism.c b/test/test_determinism.c new file mode 100644 index 0000000..1d97f8f --- /dev/null +++ b/test/test_determinism.c @@ -0,0 +1,113 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "box3d/box3d.h" +#include "determinism.h" +#include "test_macros.h" + +#include +#include + +#ifdef BOX3D_PROFILE + #include +#else + #define TracyCFrameMark +#endif + +#define EXPECTED_SLEEP_STEP 306 +#define EXPECTED_HASH 0xCBF958A1 + +static int SingleMultithreadingTest( int workerCount ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.workerCount = workerCount; + + b3WorldId worldId = b3CreateWorld( &worldDef ); + + FallingRagdollData data = CreateFallingRagdolls( worldId ); + + float timeStep = 1.0f / 60.0f; + + int stepLimit = 500; + for ( int i = 0; i < stepLimit; ++i ) + { + int subStepCount = 4; + b3World_Step( worldId, timeStep, subStepCount ); + TracyCFrameMark; + + bool done = UpdateFallingRagdolls( worldId, &data ); + if ( done ) + { + break; + } + } + + b3DestroyWorld( worldId ); + + if ( data.sleepStep != EXPECTED_SLEEP_STEP || data.hash != EXPECTED_HASH ) + { + printf( " workers=%d sleepStep=%d hash=0x%08X\n", workerCount, data.sleepStep, data.hash ); + } + + ENSURE( data.sleepStep == EXPECTED_SLEEP_STEP ); + ENSURE( data.hash == EXPECTED_HASH ); + + DestroyFallingRagdolls( &data ); + + return 0; +} + +// Test multithreaded determinism. +static int MultithreadingTest( void ) +{ + for ( int workerCount = 1; workerCount < 6; ++workerCount ) + { + int result = SingleMultithreadingTest( workerCount ); + ENSURE( result == 0 ); + } + + return 0; +} + +// Test cross platform determinism. +static int CrossPlatformTest( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + FallingRagdollData data = CreateFallingRagdolls( worldId ); + + float timeStep = 1.0f / 60.0f; + + bool done = false; + while ( done == false ) + { + int subStepCount = 4; + b3World_Step( worldId, timeStep, subStepCount ); + TracyCFrameMark; + + done = UpdateFallingRagdolls( worldId, &data ); + } + + if ( data.sleepStep != EXPECTED_SLEEP_STEP || data.hash != EXPECTED_HASH ) + { + printf( " cross-platform sleepStep=%d hash=0x%08X\n", data.sleepStep, data.hash ); + } + + ENSURE( data.sleepStep == EXPECTED_SLEEP_STEP ); + ENSURE( data.hash == EXPECTED_HASH ); + + DestroyFallingRagdolls( &data ); + + b3DestroyWorld( worldId ); + + return 0; +} + +int DeterminismTest( void ) +{ + RUN_SUBTEST( MultithreadingTest ); + RUN_SUBTEST( CrossPlatformTest ); + + return 0; +} diff --git a/test/test_distance.c b/test/test_distance.c new file mode 100644 index 0000000..27cee4d --- /dev/null +++ b/test/test_distance.c @@ -0,0 +1,112 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "test_macros.h" + +#include "box2d/collision.h" +#include "box2d/math_functions.h" + +#include + +static int SegmentDistanceTest( void ) +{ + b2Vec2 p1 = { -1.0f, -1.0f }; + b2Vec2 q1 = { -1.0f, 1.0f }; + b2Vec2 p2 = { 2.0f, 0.0f }; + b2Vec2 q2 = { 1.0f, 0.0f }; + + b2SegmentDistanceResult result = b2SegmentDistance( p1, q1, p2, q2 ); + + ENSURE_SMALL( result.fraction1 - 0.5f, FLT_EPSILON ); + ENSURE_SMALL( result.fraction2 - 1.0f, FLT_EPSILON ); + ENSURE_SMALL( result.closest1.x + 1.0f, FLT_EPSILON ); + ENSURE_SMALL( result.closest1.y, FLT_EPSILON ); + ENSURE_SMALL( result.closest2.x - 1.0f, FLT_EPSILON ); + ENSURE_SMALL( result.closest2.y, FLT_EPSILON ); + ENSURE_SMALL( result.distanceSquared - 4.0f, FLT_EPSILON ); + + return 0; +} + +static int ShapeDistanceTest( void ) +{ + b2Vec2 vas[] = { ( b2Vec2 ){ -1.0f, -1.0f }, ( b2Vec2 ){ 1.0f, -1.0f }, ( b2Vec2 ){ 1.0f, 1.0f }, ( b2Vec2 ){ -1.0f, 1.0f } }; + + b2Vec2 vbs[] = { + ( b2Vec2 ){ 2.0f, -1.0f }, + ( b2Vec2 ){ 2.0f, 1.0f }, + }; + + b2DistanceInput input; + input.proxyA = b2MakeProxy( vas, ARRAY_COUNT( vas ), 0.0f ); + input.proxyB = b2MakeProxy( vbs, ARRAY_COUNT( vbs ), 0.0f ); + input.transformA = b2Transform_identity; + input.transformB = b2Transform_identity; + input.useRadii = false; + + b2DistanceCache cache = { 0 }; + b2DistanceOutput output = b2ShapeDistance( &cache, &input, NULL, 0 ); + + ENSURE_SMALL( output.distance - 1.0f, FLT_EPSILON ); + + return 0; +} + +static int ShapeCastTest( void ) +{ + b2Vec2 vas[] = { ( b2Vec2 ){ -1.0f, -1.0f }, ( b2Vec2 ){ 1.0f, -1.0f }, ( b2Vec2 ){ 1.0f, 1.0f }, ( b2Vec2 ){ -1.0f, 1.0f } }; + + b2Vec2 vbs[] = { + ( b2Vec2 ){ 2.0f, -1.0f }, + ( b2Vec2 ){ 2.0f, 1.0f }, + }; + + b2ShapeCastPairInput input; + input.proxyA = b2MakeProxy( vas, ARRAY_COUNT( vas ), 0.0f ); + input.proxyB = b2MakeProxy( vbs, ARRAY_COUNT( vbs ), 0.0f ); + input.transformA = b2Transform_identity; + input.transformB = b2Transform_identity; + input.translationB = ( b2Vec2 ){ -2.0f, 0.0f }; + input.maxFraction = 1.0f; + + b2CastOutput output = b2ShapeCast( &input ); + + ENSURE( output.hit ); + ENSURE_SMALL( output.fraction - 0.5f, 0.005f ); + + return 0; +} + +static int TimeOfImpactTest( void ) +{ + b2Vec2 vas[] = { { -1.0f, -1.0f }, { 1.0f, -1.0f }, { 1.0f, 1.0f }, { -1.0f, 1.0f } }; + + b2Vec2 vbs[] = { + { 2.0f, -1.0f }, + { 2.0f, 1.0f }, + }; + + b2TOIInput input; + input.proxyA = b2MakeProxy( vas, ARRAY_COUNT( vas ), 0.0f ); + input.proxyB = b2MakeProxy( vbs, ARRAY_COUNT( vbs ), 0.0f ); + input.sweepA = ( b2Sweep ){ b2Vec2_zero, b2Vec2_zero, b2Vec2_zero, b2Rot_identity, b2Rot_identity }; + input.sweepB = ( b2Sweep ){ b2Vec2_zero, b2Vec2_zero, ( b2Vec2 ){ -2.0f, 0.0f }, b2Rot_identity, b2Rot_identity }; + input.tMax = 1.0f; + + b2TOIOutput output = b2TimeOfImpact( &input ); + + ENSURE( output.state == b2_toiStateHit ); + ENSURE_SMALL( output.t - 0.5f, 0.005f ); + + return 0; +} + +int DistanceTest( void ) +{ + RUN_SUBTEST( SegmentDistanceTest ); + RUN_SUBTEST( ShapeDistanceTest ); + RUN_SUBTEST( ShapeCastTest ); + RUN_SUBTEST( TimeOfImpactTest ); + + return 0; +} diff --git a/test/test_height_field.c b/test/test_height_field.c new file mode 100644 index 0000000..a664e68 --- /dev/null +++ b/test/test_height_field.c @@ -0,0 +1,246 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "test_macros.h" + +// b3GetHeightFieldTriangle is internal +#include "shape.h" + +#include "box3d/collision.h" +#include "box3d/math_functions.h" + +#include +#include + +static int HeightFieldCreate( void ) +{ + b3Vec3 scale = { 1.0f, 1.0f, 1.0f }; + b3HeightField* hf = b3CreateGrid( 4, 4, scale, false ); + + ENSURE( hf->rowCount == 4 ); + ENSURE( hf->columnCount == 4 ); + ENSURE( hf->clockwise == false ); + ENSURE( hf->version == B3_HEIGHT_FIELD_VERSION ); + + ENSURE_SMALL( hf->aabb.lowerBound.x, FLT_EPSILON ); + ENSURE_SMALL( hf->aabb.lowerBound.y, FLT_EPSILON ); + ENSURE_SMALL( hf->aabb.lowerBound.z, FLT_EPSILON ); + + ENSURE_SMALL( hf->aabb.upperBound.x - 3.0f, FLT_EPSILON ); + ENSURE_SMALL( hf->aabb.upperBound.y, FLT_EPSILON ); + ENSURE_SMALL( hf->aabb.upperBound.z - 3.0f, FLT_EPSILON ); + + b3DestroyHeightField( hf ); + return 0; +} + +static int HeightFieldTriangleIndex( void ) +{ + // Asymmetric grid (rows != cols) catches off-by-one errors between + // vertex stride (columnCount) and cell stride (columnCount - 1). + int rowCount = 4; + int columnCount = 5; + b3Vec3 scale = { 1.0f, 1.0f, 1.0f }; + b3HeightField* hf = b3CreateGrid( rowCount, columnCount, scale, false ); + + int triangleCount = 2 * ( rowCount - 1 ) * ( columnCount - 1 ); + + for ( int triangleIndex = 0; triangleIndex < triangleCount; ++triangleIndex ) + { + int quadIndex = triangleIndex >> 1; + int sub = triangleIndex & 1; + int row = quadIndex / ( columnCount - 1 ); + int column = quadIndex - row * ( columnCount - 1 ); + + int index11 = row * columnCount + column; + int index12 = index11 + 1; + int index21 = ( row + 1 ) * columnCount + column; + int index22 = index21 + 1; + + b3Triangle t = b3GetHeightFieldTriangle( hf, triangleIndex ); + + if ( sub == 0 ) + { + // Triangle 0 (CCW): {11, 21, 12} + ENSURE( t.i1 == index11 ); + ENSURE( t.i2 == index21 ); + ENSURE( t.i3 == index12 ); + } + else + { + // Triangle 1 (CCW): {22, 12, 21} + ENSURE( t.i1 == index22 ); + ENSURE( t.i2 == index12 ); + ENSURE( t.i3 == index21 ); + } + } + + b3DestroyHeightField( hf ); + return 0; +} + +static int HeightFieldWinding( void ) +{ + // Build the same flat 3x3 field with CCW and CW winding. The cross-product + // normal of triangle 0 must flip sign accordingly. + float heights[9] = { 0 }; + uint8_t materials[4] = { 0, 0, 0, 0 }; + + b3HeightFieldDef def = { 0 }; + def.heights = heights; + def.materialIndices = materials; + def.scale = (b3Vec3){ 1.0f, 1.0f, 1.0f }; + def.countX = 3; + def.countZ = 3; + def.globalMinimumHeight = -1.0f; + def.globalMaximumHeight = 1.0f; + + def.clockwiseWinding = false; + b3HeightField* ccw = b3CreateHeightField( &def ); + + def.clockwiseWinding = true; + b3HeightField* cw = b3CreateHeightField( &def ); + + b3Triangle ta = b3GetHeightFieldTriangle( ccw, 0 ); + b3Triangle tb = b3GetHeightFieldTriangle( cw, 0 ); + + b3Vec3 na = + b3Normalize( b3Cross( b3Sub( ta.vertices[1], ta.vertices[0] ), b3Sub( ta.vertices[2], ta.vertices[0] ) ) ); + b3Vec3 nb = + b3Normalize( b3Cross( b3Sub( tb.vertices[1], tb.vertices[0] ), b3Sub( tb.vertices[2], tb.vertices[0] ) ) ); + + ENSURE_SMALL( na.x, FLT_EPSILON ); + ENSURE_SMALL( na.y - 1.0f, FLT_EPSILON ); + ENSURE_SMALL( na.z, FLT_EPSILON ); + + ENSURE_SMALL( nb.x, FLT_EPSILON ); + ENSURE_SMALL( nb.y + 1.0f, FLT_EPSILON ); + ENSURE_SMALL( nb.z, FLT_EPSILON ); + + b3DestroyHeightField( ccw ); + b3DestroyHeightField( cw ); + return 0; +} + +static int RayCastFlatField( void ) +{ + // Build a flat 4x4 field with a tight quantization range so the recovered + // surface stays within ~1e-5 of y=0 (b3CreateGrid uses -256..256 which + // blows the 1/UINT16_MAX quantum up to ~4e-3 in y). + float heights[16] = { 0 }; + uint8_t materials[9] = { 0 }; + + b3HeightFieldDef def = { 0 }; + def.heights = heights; + def.materialIndices = materials; + def.scale = (b3Vec3){ 1.0f, 1.0f, 1.0f }; + def.countX = 4; + def.countZ = 4; + def.globalMinimumHeight = -1.0f; + def.globalMaximumHeight = 1.0f; + def.clockwiseWinding = false; + + b3HeightField* hf = b3CreateHeightField( &def ); + + // Origin sits clearly inside triangle 0 of cell (1, 1) — off the cell + // diagonal x+z = 3. The translation overshoots the surface so the hit + // fraction is strictly less than maxFraction. + b3RayCastInput input = { 0 }; + input.origin = (b3Vec3){ 1.25f, 10.0f, 1.25f }; + input.translation = (b3Vec3){ 0.0f, -20.0f, 0.0f }; + input.maxFraction = 1.0f; + + b3CastOutput out = b3RayCastHeightField( hf, &input ); + + ENSURE( out.hit == true ); + ENSURE_SMALL( out.fraction - 0.5f, 1e-5f ); + ENSURE_SMALL( out.normal.x, 1e-5f ); + ENSURE_SMALL( out.normal.y - 1.0f, 1e-5f ); + ENSURE_SMALL( out.normal.z, 1e-5f ); + + b3DestroyHeightField( hf ); + return 0; +} + +static int OverlapAtSurface( void ) +{ + b3Vec3 scale = { 1.0f, 1.0f, 1.0f }; + b3HeightField* hf = b3CreateGrid( 4, 4, scale, false ); + + // Sphere center 1.0 above the surface, radius 0.5 — clear gap. + b3Vec3 above = { 1.5f, 1.0f, 1.5f }; + b3ShapeProxy proxyAbove = { &above, 1, 0.5f }; + bool hitAbove = b3OverlapHeightField( hf, b3Transform_identity, &proxyAbove ); + ENSURE( hitAbove == false ); + + // Sphere centered on the surface — radius pokes through. + b3Vec3 through = { 1.5f, 0.0f, 1.5f }; + b3ShapeProxy proxyThrough = { &through, 1, 0.5f }; + bool hitThrough = b3OverlapHeightField( hf, b3Transform_identity, &proxyThrough ); + ENSURE( hitThrough == true ); + + b3DestroyHeightField( hf ); + return 0; +} + +static int FileRoundtrip( void ) +{ + float heights[9] = { 0.0f, 0.5f, -0.3f, 0.1f, 0.0f, 0.0f, 0.0f, 0.2f, 0.0f }; + uint8_t materials[4] = { 0, B3_HEIGHT_FIELD_HOLE, 1, 2 }; + + b3HeightFieldDef def = { 0 }; + def.heights = heights; + def.materialIndices = materials; + def.scale = (b3Vec3){ 1.5f, 2.0f, 0.75f }; + def.countX = 3; + def.countZ = 3; + def.globalMinimumHeight = -1.0f; + def.globalMaximumHeight = 1.0f; + def.clockwiseWinding = true; + + const char* path = "test_height_field_roundtrip.dat"; + b3DumpHeightData( &def, path ); + + b3HeightField* loaded = b3LoadHeightField( path ); + remove( path ); + + ENSURE( loaded != NULL ); + ENSURE( loaded->rowCount == def.countZ ); + ENSURE( loaded->columnCount == def.countX ); + ENSURE( loaded->clockwise == def.clockwiseWinding ); + + ENSURE_SMALL( loaded->scale.x - def.scale.x, FLT_EPSILON ); + ENSURE_SMALL( loaded->scale.y - def.scale.y, FLT_EPSILON ); + ENSURE_SMALL( loaded->scale.z - def.scale.z, FLT_EPSILON ); + ENSURE_SMALL( loaded->minHeight - def.globalMinimumHeight, FLT_EPSILON ); + ENSURE_SMALL( loaded->maxHeight - def.globalMaximumHeight, FLT_EPSILON ); + + int cellCount = ( def.countX - 1 ) * ( def.countZ - 1 ); + for ( int i = 0; i < cellCount; ++i ) + { + ENSURE( loaded->materialIndices[i] == materials[i] ); + } + + // Recovered heights round-trip within the quantization tolerance. + float quantum = ( def.globalMaximumHeight - def.globalMinimumHeight ) / (float)UINT16_MAX; + for ( int i = 0; i < def.countX * def.countZ; ++i ) + { + float recovered = loaded->minHeight + loaded->heightScale * loaded->compressedHeights[i]; + ENSURE_SMALL( recovered - heights[i], 2.0f * quantum ); + } + + b3DestroyHeightField( loaded ); + return 0; +} + +int HeightFieldTest( void ) +{ + RUN_SUBTEST( HeightFieldCreate ); + RUN_SUBTEST( HeightFieldTriangleIndex ); + RUN_SUBTEST( HeightFieldWinding ); + RUN_SUBTEST( RayCastFlatField ); + RUN_SUBTEST( OverlapAtSurface ); + RUN_SUBTEST( FileRoundtrip ); + + return 0; +} diff --git a/test/test_hull.c b/test/test_hull.c new file mode 100644 index 0000000..ac37252 --- /dev/null +++ b/test/test_hull.c @@ -0,0 +1,462 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "test_macros.h" + +#include "box3d/collision.h" +#include "box3d/math_functions.h" + +#include +#include +#include + +static const b3Vec3 s_cubeCorners[8] = { + { 1.0f, 1.0f, 1.0f }, { -1.0f, 1.0f, 1.0f }, { -1.0f, -1.0f, 1.0f }, { 1.0f, -1.0f, 1.0f }, + { 1.0f, 1.0f, -1.0f }, { -1.0f, 1.0f, -1.0f }, { -1.0f, -1.0f, -1.0f }, { 1.0f, -1.0f, -1.0f }, +}; + +static int CreateHullCubeTest( void ) +{ + b3Hull* hull = b3CreateHull( s_cubeCorners, 8, 8 ); + ENSURE( hull != NULL ); + + ENSURE( hull->vertexCount == 8 ); + ENSURE( hull->edgeCount == 24 ); + ENSURE( hull->faceCount == 6 ); + + // Euler's identity for convex polyhedron + ENSURE( hull->vertexCount - hull->edgeCount / 2 + hull->faceCount == 2 ); + + b3BoxHull ref = b3MakeBoxHull( 1.0f, 1.0f, 1.0f ); + + ENSURE_SMALL( hull->volume - ref.base.volume, 1e-4f ); + ENSURE_SMALL( hull->surfaceArea - ref.base.surfaceArea, 1e-4f ); + ENSURE_SMALL( hull->innerRadius - ref.base.innerRadius, FLT_EPSILON ); + + ENSURE_SMALL( hull->center.x - ref.base.center.x, 1e-5f ); + ENSURE_SMALL( hull->center.y - ref.base.center.y, 1e-5f ); + ENSURE_SMALL( hull->center.z - ref.base.center.z, 1e-5f ); + + ENSURE_SMALL( hull->aabb.lowerBound.x + 1.0f, FLT_EPSILON ); + ENSURE_SMALL( hull->aabb.lowerBound.y + 1.0f, FLT_EPSILON ); + ENSURE_SMALL( hull->aabb.lowerBound.z + 1.0f, FLT_EPSILON ); + ENSURE_SMALL( hull->aabb.upperBound.x - 1.0f, FLT_EPSILON ); + ENSURE_SMALL( hull->aabb.upperBound.y - 1.0f, FLT_EPSILON ); + ENSURE_SMALL( hull->aabb.upperBound.z - 1.0f, FLT_EPSILON ); + + b3Matrix3 d = b3SubMM( hull->centralInertia, ref.base.centralInertia ); + ENSURE_SMALL( d.cx.x, 1e-4f ); + ENSURE_SMALL( d.cy.y, 1e-4f ); + ENSURE_SMALL( d.cz.z, 1e-4f ); + ENSURE_SMALL( d.cx.y, 1e-4f ); + ENSURE_SMALL( d.cx.z, 1e-4f ); + ENSURE_SMALL( d.cy.z, 1e-4f ); + ENSURE_SMALL( d.cy.x, 1e-4f ); + ENSURE_SMALL( d.cz.x, 1e-4f ); + ENSURE_SMALL( d.cz.y, 1e-4f ); + + b3DestroyHull( hull ); + return 0; +} + +static int CreateHullTetrahedronTest( void ) +{ + b3Vec3 points[4] = { + { 0.0f, 0.0f, 0.0f }, + { 1.0f, 0.0f, 0.0f }, + { 0.0f, 1.0f, 0.0f }, + { 0.0f, 0.0f, 1.0f }, + }; + + b3Hull* hull = b3CreateHull( points, 4, 4 ); + ENSURE( hull != NULL ); + + ENSURE( hull->vertexCount == 4 ); + ENSURE( hull->edgeCount == 12 ); + ENSURE( hull->faceCount == 4 ); + ENSURE( hull->vertexCount - hull->edgeCount / 2 + hull->faceCount == 2 ); + + // Analytic values for the unit-corner tetrahedron at the origin. + float expectedVolume = 1.0f / 6.0f; + float expectedSurfaceArea = 1.5f + 0.5f * sqrtf( 3.0f ); + float expectedInnerRadius = 0.25f / sqrtf( 3.0f ); + + ENSURE_SMALL( hull->volume - expectedVolume, 1e-5f ); + ENSURE_SMALL( hull->surfaceArea - expectedSurfaceArea, 1e-5f ); + ENSURE_SMALL( hull->innerRadius - expectedInnerRadius, 1e-5f ); + + ENSURE_SMALL( hull->center.x - 0.25f, 1e-5f ); + ENSURE_SMALL( hull->center.y - 0.25f, 1e-5f ); + ENSURE_SMALL( hull->center.z - 0.25f, 1e-5f ); + + ENSURE_SMALL( hull->aabb.lowerBound.x, FLT_EPSILON ); + ENSURE_SMALL( hull->aabb.lowerBound.y, FLT_EPSILON ); + ENSURE_SMALL( hull->aabb.lowerBound.z, FLT_EPSILON ); + ENSURE_SMALL( hull->aabb.upperBound.x - 1.0f, FLT_EPSILON ); + ENSURE_SMALL( hull->aabb.upperBound.y - 1.0f, FLT_EPSILON ); + ENSURE_SMALL( hull->aabb.upperBound.z - 1.0f, FLT_EPSILON ); + + b3DestroyHull( hull ); + return 0; +} + +static int CreateHullDeterminismTest( void ) +{ + b3Hull* h1 = b3CreateHull( s_cubeCorners, 8, 8 ); + b3Hull* h2 = b3CreateHull( s_cubeCorners, 8, 8 ); + ENSURE( h1 != NULL && h2 != NULL ); + + ENSURE( h1->byteCount == h2->byteCount ); + ENSURE( h1->hash != 0 ); + ENSURE( h1->hash == h2->hash ); + ENSURE( memcmp( h1, h2, h1->byteCount ) == 0 ); + + b3DestroyHull( h1 ); + b3DestroyHull( h2 ); + return 0; +} + +#define SPHERE_N 6 + +static int CreateHullMaxVertexTest( void ) +{ + // Sphere-sampled point cloud, dense enough that the builder has room to grow. + b3Vec3 points[SPHERE_N * SPHERE_N]; + int index = 0; + for ( int i = 0; i < SPHERE_N; ++i ) + { + float theta = B3_PI * (float)i / (float)( SPHERE_N - 1 ); + for ( int j = 0; j < SPHERE_N; ++j ) + { + float phi = 2.0f * B3_PI * (float)j / (float)SPHERE_N; + points[index].x = sinf( theta ) * cosf( phi ); + points[index].y = sinf( theta ) * sinf( phi ); + points[index].z = cosf( theta ); + index += 1; + } + } + int count = SPHERE_N * SPHERE_N; + + // maxVertexCount honored as a strict cap. + b3Hull* h1 = b3CreateHull( points, count, 8 ); + ENSURE( h1 != NULL ); + ENSURE( h1->vertexCount <= 8 ); + b3DestroyHull( h1 ); + + // Below the floor: clamps up to 4. + b3Hull* h2 = b3CreateHull( points, count, 1 ); + ENSURE( h2 != NULL ); + ENSURE( h2->vertexCount >= 4 && h2->vertexCount <= 255 ); + b3DestroyHull( h2 ); + + // Above the ceiling: clamps down to 255. + b3Hull* h3 = b3CreateHull( points, count, 1000 ); + ENSURE( h3 != NULL ); + ENSURE( h3->vertexCount >= 4 && h3->vertexCount <= 255 ); + b3DestroyHull( h3 ); + + return 0; +} + +static int CreateHullRedundantInputTest( void ) +{ + // 8 cube corners + duplicates + interior points. Builder should produce the cube. + b3Vec3 points[20] = { + { 1.0f, 1.0f, 1.0f }, // corners + { -1.0f, 1.0f, 1.0f }, + { -1.0f, -1.0f, 1.0f }, + { 1.0f, -1.0f, 1.0f }, + { 1.0f, 1.0f, -1.0f }, + { -1.0f, 1.0f, -1.0f }, + { -1.0f, -1.0f, -1.0f }, + { 1.0f, -1.0f, -1.0f }, + { 1.0f, 1.0f, 1.0f }, // duplicates + { 1.0f, 1.0f, 1.0f }, + { 0.0f, 0.0f, 0.0f }, // interior points + { 0.5f, 0.0f, 0.0f }, + { 0.0f, 0.5f, 0.0f }, + { 0.0f, 0.0f, 0.5f }, + { -0.5f, 0.0f, 0.0f }, + { 0.0f, -0.5f, 0.0f }, + { 0.0f, 0.0f, -0.5f }, + { 0.25f, 0.25f, 0.25f }, + { -0.25f, -0.25f, -0.25f }, + { 0.5f, 0.5f, 0.5f }, + }; + + b3Hull* hull = b3CreateHull( points, 20, 8 ); + ENSURE( hull != NULL ); + + ENSURE( hull->vertexCount == 8 ); + ENSURE( hull->edgeCount == 24 ); + ENSURE( hull->faceCount == 6 ); + + b3BoxHull ref = b3MakeBoxHull( 1.0f, 1.0f, 1.0f ); + + ENSURE_SMALL( hull->volume - ref.base.volume, 1e-4f ); + ENSURE_SMALL( hull->surfaceArea - ref.base.surfaceArea, 1e-4f ); + ENSURE_SMALL( hull->innerRadius - ref.base.innerRadius, FLT_EPSILON ); + + ENSURE_SMALL( hull->center.x - ref.base.center.x, 1e-5f ); + ENSURE_SMALL( hull->center.y - ref.base.center.y, 1e-5f ); + ENSURE_SMALL( hull->center.z - ref.base.center.z, 1e-5f ); + + ENSURE_SMALL( hull->aabb.lowerBound.x + 1.0f, FLT_EPSILON ); + ENSURE_SMALL( hull->aabb.lowerBound.y + 1.0f, FLT_EPSILON ); + ENSURE_SMALL( hull->aabb.lowerBound.z + 1.0f, FLT_EPSILON ); + ENSURE_SMALL( hull->aabb.upperBound.x - 1.0f, FLT_EPSILON ); + ENSURE_SMALL( hull->aabb.upperBound.y - 1.0f, FLT_EPSILON ); + ENSURE_SMALL( hull->aabb.upperBound.z - 1.0f, FLT_EPSILON ); + + b3DestroyHull( hull ); + return 0; +} + +static int CreateHullCloneTest( void ) +{ + b3Hull* original = b3CreateHull( s_cubeCorners, 8, 8 ); + ENSURE( original != NULL ); + + b3Hull* clone = b3CloneHull( original ); + ENSURE( clone != NULL ); + ENSURE( clone->byteCount == original->byteCount ); + ENSURE( memcmp( clone, original, original->byteCount ) == 0 ); + + b3DestroyHull( clone ); + b3DestroyHull( original ); + return 0; +} + +static int CreateHullCylinderTest( void ) +{ + const float height = 2.0f; + const float radius = 1.0f; + const int sides = 8; + const float yOffset = 0.0f; + + b3Hull* hull = b3CreateCylinder( height, radius, yOffset, sides ); + ENSURE( hull != NULL ); + + ENSURE( hull->vertexCount == 2 * sides ); + ENSURE( hull->edgeCount == 6 * sides ); + ENSURE( hull->faceCount == sides + 2 ); + + // Analytic n-gon prism values (exact targets, not the circular cylinder approximations). + float halfAngle = B3_PI / (float)sides; + float capArea = (float)sides * 0.5f * radius * radius * sinf( 2.0f * halfAngle ); + float chordLen = 2.0f * radius * sinf( halfAngle ); + float lateralArea = (float)sides * chordLen * height; + + float expectedVolume = capArea * height; + float expectedSurfaceArea = 2.0f * capArea + lateralArea; + float expectedInnerRadius = radius * cosf( halfAngle ); + + ENSURE_SMALL( ( hull->volume - expectedVolume ) / expectedVolume, 1e-4f ); + ENSURE_SMALL( ( hull->surfaceArea - expectedSurfaceArea ) / expectedSurfaceArea, 1e-4f ); + ENSURE_SMALL( hull->innerRadius - expectedInnerRadius, 1e-5f ); + + ENSURE_SMALL( hull->center.x, 1e-5f ); + ENSURE_SMALL( hull->center.y - ( yOffset + 0.5f * height ), 1e-5f ); + ENSURE_SMALL( hull->center.z, 1e-5f ); + + ENSURE_SMALL( hull->aabb.lowerBound.x + radius, FLT_EPSILON ); + ENSURE_SMALL( hull->aabb.lowerBound.y - yOffset, FLT_EPSILON ); + ENSURE_SMALL( hull->aabb.lowerBound.z + radius, FLT_EPSILON ); + ENSURE_SMALL( hull->aabb.upperBound.x - radius, FLT_EPSILON ); + ENSURE_SMALL( hull->aabb.upperBound.y - ( yOffset + height ), FLT_EPSILON ); + ENSURE_SMALL( hull->aabb.upperBound.z - radius, FLT_EPSILON ); + + b3DestroyHull( hull ); + return 0; +} + +// Inlined XorShift32 + Shoemake unit-vector recipe matches shared/utils.h exactly so +// generated points are bit-identical to samples that share the seed. +static void FillSphereSample( b3Vec3* points, int count, uint32_t seed ) +{ + const uint32_t RAND_LIMIT_LOCAL = 32767u; + for ( int i = 0; i < count; ++i ) + { + float u[3]; + for ( int k = 0; k < 3; ++k ) + { + seed ^= seed << 13; + seed ^= seed >> 17; + seed ^= seed << 5; + float r = (float)( seed & RAND_LIMIT_LOCAL ); + r /= (float)RAND_LIMIT_LOCAL; + u[k] = r; + } + float u1 = u[0]; + float u2 = 2.0f * B3_PI * u[1]; + float u3 = 2.0f * B3_PI * u[2]; + float sqrt1MinusU1 = sqrtf( 1.0f - u1 ); + float sqrtU1 = sqrtf( u1 ); + points[i].x = sqrt1MinusU1 * sinf( u2 ); + points[i].y = sqrt1MinusU1 * cosf( u2 ); + points[i].z = sqrtU1 * sinf( u3 ); + } +} + +// Reproduces the HullReduction sample (Sphere, 64 points, count=20) that used to assert on +// b->faceCount < b->faceCapacity. The free-list reclaim in NewFace/NewEdge keeps the bump +// counts proportional to the live hull instead of cumulative creations. +static int CreateHullSphereReductionTest( void ) +{ + b3Vec3 points[64]; + FillSphereSample( points, 64, 12345 ); // RAND_SEED + + b3Hull* hull = b3CreateHull( points, 64, 20 ); + ENSURE( hull != NULL ); + ENSURE( hull->vertexCount >= 4 && hull->vertexCount <= 20 ); + ENSURE( hull->vertexCount - hull->edgeCount / 2 + hull->faceCount == 2 ); + b3DestroyHull( hull ); + + return 0; +} + +// XorShift32 + uniform-cube point generator. Same engine as FillSphereSample. +static void FillCubeSample( b3Vec3* points, int count, uint32_t seed ) +{ + const uint32_t RAND_LIMIT_LOCAL = 32767u; + for ( int i = 0; i < count; ++i ) + { + float v[3]; + for ( int k = 0; k < 3; ++k ) + { + seed ^= seed << 13; + seed ^= seed >> 17; + seed ^= seed << 5; + float r = (float)( seed & RAND_LIMIT_LOCAL ); + r /= (float)RAND_LIMIT_LOCAL; + v[k] = 2.0f * r - 1.0f; + } + points[i].x = v[0]; + points[i].y = v[1]; + points[i].z = v[2]; + } +} + +// Pushes the working-stage bump capacities (faceCapacity = 5*M - 10, edgeCapacity = +// 24*M - 48 in src/hull.c::b3ComputeHullWorkSizes) close to their peak by sweeping M +// on a dense random sphere. If the peak face/edge count exceeds the cap and the free +// list fails to reclaim slots in time, the assertion at src/hull.c::NewFace/NewEdge +// fires (exit code 3). M is held <= 80 because the finalization stage caps the final +// hull's face/edge count at B3_HULL_LIMIT (255), and a fully-triangulated hull with +// V > 87 vertices has 3V - 6 > 255 edges (src/hull.c:2010 returns NULL). +static int CreateHullSphereStressTest( void ) +{ + enum { N = 512 }; + b3Vec3 points[N]; + + // Multiple seeds exercise different horizon-size / merge-cascade sequences. + const uint32_t seeds[] = { 12345u, 1u, 0xdeadbeefu, 0xcafef00du }; + + // M kept <= 40: random sphere inputs at higher M exceed the half edge limit of UINT8_MAX + const int M_values[] = { 16, 24, 32, 40 }; + + for ( int s = 0; s < ARRAY_COUNT( seeds ); ++s ) + { + FillSphereSample( points, N, seeds[s] ); + + for ( int m = 0; m < ARRAY_COUNT( M_values ); ++m ) + { + int M = M_values[m]; + b3Hull* hull = b3CreateHull( points, N, M ); + ENSURE( hull != NULL ); + ENSURE( hull->vertexCount >= 4 && hull->vertexCount <= M ); + ENSURE( hull->vertexCount - hull->edgeCount / 2 + hull->faceCount == 2 ); + ENSURE( hull->faceCount >= 4 ); + b3DestroyHull( hull ); + } + } + + return 0; +} + +// Random points inside a cube produce a small final hull (8 corners, 6 faces) but +// generate heavy internal churn: every interior point gets fed to the conflict-list +// machinery, and most cone faces created during apex insertion are then merged out +// when their newly-created neighbors are coplanar. Exercises the merge cascade in +// b3HullBuilder_ConnectEdges/ConnectFaces and the corresponding RetireFace/RetireEdge +// reclaim path much harder than the sphere case (which has few merges). +static int CreateHullMergeChurnStressTest( void ) +{ + enum { N = 4096 }; + static b3Vec3 points[N]; + + const uint32_t seeds[] = { 12345u, 0xdeadbeefu }; + + for ( int s = 0; s < ARRAY_COUNT( seeds ); ++s ) + { + FillCubeSample( points, N, seeds[s] ); + + // Stamp the 8 corners last so they're guaranteed extremes. + for ( int c = 0; c < 8; ++c ) + { + points[N - 8 + c].x = ( c & 1 ) ? 1.0f : -1.0f; + points[N - 8 + c].y = ( c & 2 ) ? 1.0f : -1.0f; + points[N - 8 + c].z = ( c & 4 ) ? 1.0f : -1.0f; + } + + b3Hull* hull = b3CreateHull( points, N, 64 ); + ENSURE( hull != NULL ); + ENSURE( hull->vertexCount == 8 ); + ENSURE( hull->edgeCount == 24 ); + ENSURE( hull->faceCount == 6 ); + b3DestroyHull( hull ); + } + + return 0; +} + +static int CreateHullDegenerateTest( void ) +{ + // Real (non-null) buffer; pointCount < 4 cases are guarded inside Construct(). + b3Vec3 collinear[8] = { + { 0.0f, 0.0f, 0.0f }, { 1.0f, 0.0f, 0.0f }, { 2.0f, 0.0f, 0.0f }, { 3.0f, 0.0f, 0.0f }, + { 4.0f, 0.0f, 0.0f }, { 5.0f, 0.0f, 0.0f }, { 6.0f, 0.0f, 0.0f }, { 7.0f, 0.0f, 0.0f }, + }; + + // Empty input. + ENSURE( b3CreateHull( collinear, 0, 8 ) == NULL ); + + // Fewer than 4 points. + ENSURE( b3CreateHull( collinear, 3, 8 ) == NULL ); + + // 8 coincident points. + b3Vec3 coincident[8] = { + { 1.0f, 2.0f, 3.0f }, { 1.0f, 2.0f, 3.0f }, { 1.0f, 2.0f, 3.0f }, { 1.0f, 2.0f, 3.0f }, + { 1.0f, 2.0f, 3.0f }, { 1.0f, 2.0f, 3.0f }, { 1.0f, 2.0f, 3.0f }, { 1.0f, 2.0f, 3.0f }, + }; + ENSURE( b3CreateHull( coincident, 8, 8 ) == NULL ); + + // Collinear (along x-axis). + ENSURE( b3CreateHull( collinear, 8, 8 ) == NULL ); + + // Coplanar (in the xy-plane). + b3Vec3 coplanar[6] = { + { 0.0f, 0.0f, 0.0f }, { 1.0f, 0.0f, 0.0f }, { 0.0f, 1.0f, 0.0f }, + { 1.0f, 1.0f, 0.0f }, { 2.0f, 0.5f, 0.0f }, { 0.5f, 2.0f, 0.0f }, + }; + ENSURE( b3CreateHull( coplanar, 6, 8 ) == NULL ); + + return 0; +} + +int HullTest( void ) +{ + RUN_SUBTEST( CreateHullCubeTest ); + RUN_SUBTEST( CreateHullTetrahedronTest ); + RUN_SUBTEST( CreateHullDeterminismTest ); + RUN_SUBTEST( CreateHullMaxVertexTest ); + RUN_SUBTEST( CreateHullRedundantInputTest ); + RUN_SUBTEST( CreateHullCloneTest ); + RUN_SUBTEST( CreateHullCylinderTest ); + RUN_SUBTEST( CreateHullSphereReductionTest ); + RUN_SUBTEST( CreateHullSphereStressTest ); + RUN_SUBTEST( CreateHullMergeChurnStressTest ); + RUN_SUBTEST( CreateHullDegenerateTest ); + + return 0; +} diff --git a/test/test_id.c b/test/test_id.c new file mode 100644 index 0000000..4736ca5 --- /dev/null +++ b/test/test_id.c @@ -0,0 +1,31 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "test_macros.h" + +#include "box3d/types.h" + +int IdTest( void ) +{ + uint64_t x = 0x0123456789ABCDEFull; + + { + b3BodyId id = b3LoadBodyId( x ); + uint64_t y = b3StoreBodyId( id ); + ENSURE( x == y ); + } + + { + b3ShapeId id = b3LoadShapeId( x ); + uint64_t y = b3StoreShapeId( id ); + ENSURE( x == y ); + } + + { + b3JointId id = b3LoadJointId( x ); + uint64_t y = b3StoreJointId( id ); + ENSURE( x == y ); + } + + return 0; +} diff --git a/test/test_macros.h b/test/test_macros.h new file mode 100644 index 0000000..95b4412 --- /dev/null +++ b/test/test_macros.h @@ -0,0 +1,66 @@ +// SPDX-FileCopyrightText: 2024 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +#define RUN_TEST( T ) \ + do \ + { \ + int result = T(); \ + if ( result == 1 ) \ + { \ + printf( "test failed: " #T "\n" ); \ + return 1; \ + } \ + else \ + { \ + printf( "test passed: " #T "\n" ); \ + } \ + } \ + while ( false ) + +#define RUN_SUBTEST( T ) \ + do \ + { \ + int result = T(); \ + if ( result == 1 ) \ + { \ + printf( " subtest failed: " #T "\n" ); \ + return 1; \ + } \ + else \ + { \ + printf( " subtest passed: " #T "\n" ); \ + } \ + } \ + while ( false ) + +#define ENSURE( C ) \ + do \ + { \ + if ( ( C ) == false ) \ + { \ + printf( "condition false: " #C "\n" ); \ + return 1; \ + } \ + } \ + while ( false ) + +#define ENSURE_SMALL( C, tol ) \ + do \ + { \ + if ( ( C ) < -( tol ) || ( tol ) < ( C ) ) \ + { \ + printf( "condition false: abs(" #C ") < %g\n", tol ); \ + return 1; \ + } \ + } \ + while ( false ) + +#define ARRAY_COUNT( A ) (int)( sizeof( A ) / sizeof( A[0] ) ) + +/// Used to prevent the compiler from warning about unused variables +#define MAYBE_UNUSED( x ) ( (void)( x ) ) diff --git a/test/test_math.c b/test/test_math.c new file mode 100644 index 0000000..074fcef --- /dev/null +++ b/test/test_math.c @@ -0,0 +1,277 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "math_internal.h" +#include "utils.h" +#include "test_macros.h" + +#include +#include + +// 0.0023 degrees +#define ATAN_TOL 0.00004f + +int MathTest( void ) +{ + for ( float t = -10.0f; t < 10.0f; t += 0.01f ) + { + float angle = B3_PI * t; + b3CosSin cs = b3ComputeCosSin( angle ); + float c = cosf( angle ); + float s = sinf( angle ); + + // The cosine and sine approximations are accurate to about 0.1 degrees (0.002 radians) + // printf( "%g %g\n", r.c - c, r.s - s ); + ENSURE_SMALL( cs.cosine - c, 0.002f ); + ENSURE_SMALL( cs.sine - s, 0.002f ); + + float xn = b3UnwindAngle( angle ); + float a = b3Atan2( s, c ); + ENSURE( b3IsValidFloat( a ) ); + + float diff = b3AbsFloat( a - xn ); + + // The two results can be off by 360 degrees (-pi and pi) + if ( diff > B3_PI ) + { + diff -= 2.0f * B3_PI; + } + + // The approximate atan2 is quite accurate + ENSURE_SMALL( diff, ATAN_TOL ); + } + + for ( float y = -1.0f; y <= 1.0f; y += 0.01f ) + { + for ( float x = -1.0f; x <= 1.0f; x += 0.01f ) + { + float a1 = b3Atan2( y, x ); + float a2 = atan2f( y, x ); + float diff = b3AbsFloat( a1 - a2 ); + ENSURE( b3IsValidFloat( a1 ) ); + ENSURE_SMALL( diff, ATAN_TOL ); + } + } + + { + float a1 = b3Atan2( 1.0f, 0.0f ); + float a2 = atan2f( 1.0f, 0.0f ); + float diff = b3AbsFloat( a1 - a2 ); + ENSURE( b3IsValidFloat( a1 ) ); + ENSURE_SMALL( diff, ATAN_TOL ); + } + + { + float a1 = b3Atan2( -1.0f, 0.0f ); + float a2 = atan2f( -1.0f, 0.0f ); + float diff = b3AbsFloat( a1 - a2 ); + ENSURE( b3IsValidFloat( a1 ) ); + ENSURE_SMALL( diff, ATAN_TOL ); + } + + { + float a1 = b3Atan2( 0.0f, 1.0f ); + float a2 = atan2f( 0.0f, 1.0f ); + float diff = b3AbsFloat( a1 - a2 ); + ENSURE( b3IsValidFloat( a1 ) ); + ENSURE_SMALL( diff, ATAN_TOL ); + } + + { + float a1 = b3Atan2( 0.0f, -1.0f ); + float a2 = atan2f( 0.0f, -1.0f ); + float diff = b3AbsFloat( a1 - a2 ); + ENSURE( b3IsValidFloat( a1 ) ); + ENSURE_SMALL( diff, ATAN_TOL ); + } + + { + float a1 = b3Atan2( 0.0f, 0.0f ); + float a2 = atan2f( 0.0f, 0.0f ); + float diff = b3AbsFloat( a1 - a2 ); + ENSURE( b3IsValidFloat( a1 ) ); + ENSURE_SMALL( diff, ATAN_TOL ); + } + + b3Vec3 zero = { 0 }; + b3Vec3 one = { 1.0f, 1.0f, 1.0f }; + b3Vec3 two = { 2.0f, 2.0f, 2.0f }; + + b3Vec3 v = b3Add( one, two ); + ENSURE( v.x == 3.0f && v.y == 3.0f ); + + v = b3Sub( zero, two ); + ENSURE( v.x == -2.0f && v.y == -2.0f ); + + v = b3Add( two, two ); + ENSURE( v.x != 5.0f && v.y != 5.0f ); + + b3Vec3 axis = b3Normalize( (b3Vec3){ -0.75f, 0.5f, 1.0f } ); + b3Transform transform1 = { { -2.0f, 3.0f, 0.0f }, b3Quat_identity }; + b3Transform transform2 = { { 1.0f, 0.0f, 0.0f }, b3MakeQuatFromAxisAngle( axis, B3_PI ) }; + + b3Transform transform = b3MulTransforms( transform2, transform1 ); + + v = b3TransformPoint( transform2, b3TransformPoint( transform1, two ) ); + + b3Vec3 u = b3TransformPoint( transform, two ); + + ENSURE_SMALL( u.x - v.x, 10.0f * FLT_EPSILON ); + ENSURE_SMALL( u.y - v.y, 10.0f * FLT_EPSILON ); + + v = b3TransformPoint( transform1, two ); + v = b3InvTransformPoint( transform1, v ); + + ENSURE_SMALL( v.x - two.x, 8.0f * FLT_EPSILON ); + ENSURE_SMALL( v.y - two.y, 8.0f * FLT_EPSILON ); + + b3Transform relTransform = b3InvMulTransforms( transform1, transform2 ); + v = b3InvTransformPoint( transform1, b3TransformPoint( transform2, two ) ); + u = b3TransformPoint( relTransform, two ); + ENSURE_SMALL( u.x - v.x, 10.0f * FLT_EPSILON ); + ENSURE_SMALL( u.y - v.y, 10.0f * FLT_EPSILON ); + + { + axis = (b3Vec3){ 0.0f, 0.0f, 1.0f }; + b3Quat q1 = b3MakeQuatFromAxisAngle( axis, -0.5f * B3_PI ); + b3Quat q2 = b3ComputeQuatBetweenUnitVectors( (b3Vec3){ 1.0f, 0.0f, 0.0f }, (b3Vec3){ 0.0f, -1.0f, 0.0f } ); + + ENSURE_SMALL( q1.v.x - q2.v.x, FLT_EPSILON ); + ENSURE_SMALL( q1.v.y - q2.v.y, FLT_EPSILON ); + ENSURE_SMALL( q1.v.z - q2.v.z, FLT_EPSILON ); + ENSURE_SMALL( q1.s - q2.s, FLT_EPSILON ); + + b3Quat q3 = b3NormalizeQuat( (b3Quat){ { 1.0f, -2.0f, 3.0f }, 4.0f } ); + b3Quat q4 = b3InvMulQuat( q3, q1 ); + b3Quat q5 = b3MulQuat( q3, q4 ); + ENSURE_SMALL( q1.v.x - q5.v.x, FLT_EPSILON ); + ENSURE_SMALL( q1.v.y - q5.v.y, FLT_EPSILON ); + ENSURE_SMALL( q1.v.z - q5.v.z, FLT_EPSILON ); + ENSURE_SMALL( q1.s - q5.s, FLT_EPSILON ); + + b3Quat q6 = b3ComputeQuatBetweenUnitVectors( (b3Vec3){ 0.0f, 1.0f, 0.0f }, (b3Vec3){ 0.0f, -1.0f, 0.0f } ); + ENSURE_SMALL( q6.s, FLT_EPSILON ); + (void)q6; + } + + v = b3Normalize( (b3Vec3){ 0.2f, -0.5f, 3.0f } ); + for ( float z = -1.0f; z <= 1.0f; z += 0.02f ) + { + for ( float y = -1.0f; y <= 1.0f; y += 0.02f ) + { + for ( float x = -1.0f; x <= 1.0f; x += 0.02f ) + { + if ( x == 0.0f && y == 0.0f && z == 0.0f ) + { + continue; + } + + u = b3Normalize( (b3Vec3){ x, y, z } ); + + b3Quat r = b3ComputeQuatBetweenUnitVectors( v, u ); + ENSURE( b3IsValidQuat( r ) ); + + b3Vec3 w = b3RotateVector( r, v ); + + ENSURE_SMALL( b3Dot( r.v, b3Cross( u, w ) ) - b3ScalarTripleProduct( r.v, u, w ), FLT_EPSILON ); + + // The quaternion between vectors can have lots of round off error at large angles. + ENSURE_SMALL( w.x - u.x, 0.001f ); + ENSURE_SMALL( w.y - u.y, 0.001f ); + ENSURE_SMALL( w.z - u.z, 0.001f ); + + // Twist angle testing + float twist = r.s < 0.0f ? b3Atan2( -r.v.z, -r.s ) : b3Atan2( r.v.z, r.s ); + twist *= 2.0f; + ENSURE( -B3_PI <= twist && twist <= B3_PI ); + } + } + } + + { + // More twist angle testing + b3Quat q = { .v = { -0.0558656752f, -0.188799798f, 0.00689807534f }, .s = -0.980401039f }; + float twist = q.s < 0.0f ? b3Atan2( -q.v.z, -q.s ) : b3Atan2( q.v.z, q.s ); + twist *= 2.0f; + ENSURE( -B3_PI <= twist && twist <= B3_PI ); + } + + { + b3Matrix3 m = { { 3.0f, 1.0f, -1.0f }, { -1.0f, 3.0f, 1.0f }, { 1.0f, -1.0f, 3.0f } }; + b3Matrix3 invM = b3InvertMatrix( m ); + b3Matrix3 a = b3MulMM( m, invM ); + ENSURE_SMALL( a.cx.x - 1.0f, FLT_EPSILON ); + ENSURE_SMALL( a.cx.y, FLT_EPSILON ); + ENSURE_SMALL( a.cx.z, FLT_EPSILON ); + ENSURE_SMALL( a.cy.x, FLT_EPSILON ); + ENSURE_SMALL( a.cy.y - 1.0f, FLT_EPSILON ); + ENSURE_SMALL( a.cy.z, FLT_EPSILON ); + ENSURE_SMALL( a.cz.x, FLT_EPSILON ); + ENSURE_SMALL( a.cz.y, FLT_EPSILON ); + ENSURE_SMALL( a.cz.z - 1.0f, FLT_EPSILON ); + + v = (b3Vec3){ 1.0f, -2.0f, 3.0f }; + u = b3MulMV( invM, b3MulMV( m, v ) ); + ENSURE_SMALL( v.x - u.x, FLT_EPSILON ); + ENSURE_SMALL( v.y - u.y, FLT_EPSILON ); + ENSURE_SMALL( v.z - u.z, FLT_EPSILON ); + + b3Vec3 w = b3MulMV( invM, v ); + u = b3Solve3( m, v ); + ENSURE_SMALL( w.x - u.x, FLT_EPSILON ); + ENSURE_SMALL( w.y - u.y, FLT_EPSILON ); + ENSURE_SMALL( w.z - u.z, FLT_EPSILON ); + } + + { + b3Matrix2 m = { { 3.0f, 1.0f }, { -1.0f, 3.0f } }; + b3Matrix2 invM = b3Invert2( m ); + b3Matrix2 a = b3MulMM2( m, invM ); + ENSURE_SMALL( a.cx.x - 1.0f, FLT_EPSILON ); + ENSURE_SMALL( a.cx.y, FLT_EPSILON ); + ENSURE_SMALL( a.cy.x, FLT_EPSILON ); + ENSURE_SMALL( a.cy.y - 1.0f, FLT_EPSILON ); + + b3Vec2 v2 = { 1.0f, -2.0f }; + b3Vec2 u2 = b3MulMV2( invM, b3MulMV2( m, v2 ) ); + ENSURE_SMALL( v2.x - u2.x, FLT_EPSILON ); + ENSURE_SMALL( v2.y - u2.y, FLT_EPSILON ); + + b3Vec2 w = b3MulMV2( invM, v2 ); + u2 = b3Solve2( m, v2 ); + ENSURE_SMALL( w.x - u2.x, FLT_EPSILON ); + ENSURE_SMALL( w.y - u2.y, FLT_EPSILON ); + + w = b3MulMV2( m, u2 ); + ENSURE_SMALL( w.x - v2.x, 10.0f * FLT_EPSILON ); + ENSURE_SMALL( w.y - v2.y, 10.0f * FLT_EPSILON ); + } + + for (int i = 0; i < 100; ++i) + { + float a = RandomFloat(); + double b = a; + float c = (float)b; + ENSURE( c == a ); + } + + b3Quat q1 = b3Quat_identity; + b3Quat q2 = b3MakeQuatFromAxisAngle(b3Vec3_axisZ, 0.5f * B3_PI ); + int n = 100; + for ( int i = 0; i <= n; ++i ) + { + float alpha = (float)i / (float)n; + b3Quat q = b3NLerp( q1, q2, alpha ); + float angle = b3GetTwistAngle( q ); + ENSURE_SMALL( alpha * 0.5f * B3_PI - angle, 1.0f * B3_DEG_TO_RAD ); + //printf( "angle = [%g %g %g]\n", alpha, alpha * 0.5f * B3_PI, angle ); + } + + { + b3Vec3 normal = { 0.504055440f, 0.621548057f, 0.599671543f }; + b3Vec3 perp = b3ArbitraryPerp( normal ); + ENSURE_SMALL( b3Dot( normal, perp ), 2.0f * FLT_EPSILON ); + } + + return 0; +} diff --git a/test/test_mover.c b/test/test_mover.c new file mode 100644 index 0000000..2844ae3 --- /dev/null +++ b/test/test_mover.c @@ -0,0 +1,60 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "test_macros.h" + +#include "box3d/collision.h" + +static int ParallelPlanes( void ) +{ + b3CollisionPlane planes[3] = { 0 }; + planes[0].plane.normal = (b3Vec3){ 0.0f, 0.0f, 1.0f }; + planes[0].plane.offset = 0.5f; + planes[0].pushLimit = FLT_MAX; + planes[1].plane.normal = (b3Vec3){ 0.0f, 0.0f, 1.0f }; + planes[1].plane.offset = 1.0f; + planes[1].pushLimit = FLT_MAX; + // planes[2].plane.normal = b3Normalize((b3Vec3){ 0.2f, 0.0f, 0.9f }); + // planes[2].plane.offset = 0.25f; + // planes[2].pushLimit = FLT_MAX; + + b3Vec3 target = { 0.0f, 0.0f, 0.0f }; + b3PlaneSolverResult result = b3SolvePlanes( target, planes, 2 ); + + ENSURE( result.iterationCount == 2 ); + ENSURE_SMALL( result.delta.z - 1.0f, 0.0055f ); + + return 0; +} + +static int GamePlanes( void ) +{ + // This scenario takes many iterations because the target is deep into the plane. + b3CollisionPlane planes[3] = { 0 }; + planes[0].plane.normal = (b3Vec3){ 0.0f, -0.23941046f, 0.970918416f }; + planes[0].plane.offset = 0.390724182f; + planes[0].pushLimit = FLT_MAX; + planes[1].plane.normal = (b3Vec3){ 0.0f, 0.0f, 1.0f }; + planes[1].plane.offset = 1.49998093f; + planes[1].pushLimit = FLT_MAX; + + b3Vec3 target = { -2.5390625f, 0.0f, -73.6880798f }; + + planes[0].plane.offset -= b3Dot( planes[0].plane.normal, target ); + planes[1].plane.offset -= b3Dot( planes[1].plane.normal, target ); + target = b3Vec3_zero; + + b3PlaneSolverResult result = b3SolvePlanes( target, planes, 2 ); + + ENSURE( result.iterationCount == 20 ); + + return 0; +} + +int MoverTest( void ) +{ + RUN_SUBTEST( GamePlanes ); + RUN_SUBTEST( ParallelPlanes ); + + return 0; +} diff --git a/test/test_shape.c b/test/test_shape.c new file mode 100644 index 0000000..436e5b3 --- /dev/null +++ b/test/test_shape.c @@ -0,0 +1,322 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "test_macros.h" + +#include "box3d/collision.h" +#include "box3d/math_functions.h" + +#include + +static b3Capsule capsule = { { -1.0f, 0.0f, 0.0f }, { 1.0f, 0.0f, 0.0f }, 1.0f }; +static b3Sphere sphere = { { 1.0f, 0.0f, 0.0f }, 1.0f }; +static b3BoxHull box = { 0 }; + +#define N 4 + +static int ShapeMassTest( void ) +{ + // Sphere + { + b3MassData md = b3ComputeSphereMass( &sphere, 1.0f ); + float mass = 4.0f / 3.0f * B3_PI; + ENSURE_SMALL( md.mass - mass, FLT_EPSILON ); + ENSURE( md.center.x == 1.0f && md.center.y == 0.0f ); + float inertia = 2.0f / 5.0f * mass; + float offsetInertia = inertia + mass; + ENSURE_SMALL( md.inertia.cx.x - inertia, FLT_EPSILON ); + ENSURE_SMALL( md.inertia.cy.y - offsetInertia, FLT_EPSILON ); + ENSURE_SMALL( md.inertia.cz.z - offsetInertia, FLT_EPSILON ); + } + + // Analytic box hull + { + b3MassData md = b3ComputeHullMass( &box.base, 1.0f ); + float mass = 2.0f * 2.0f * 2.0f; + ENSURE_SMALL( md.mass - mass, FLT_EPSILON ); + ENSURE_SMALL( md.center.x, FLT_EPSILON ); + ENSURE_SMALL( md.center.y, FLT_EPSILON ); + ENSURE_SMALL( md.center.z, FLT_EPSILON ); + float inertia = ( 1.0f / 12.0f ) * mass * ( 2.0f * 2.0f + 2.0f * 2.0f ); + ENSURE_SMALL( md.inertia.cx.x - inertia, 2.0f * FLT_EPSILON ); + ENSURE_SMALL( md.inertia.cy.y - inertia, 2.0f * FLT_EPSILON ); + ENSURE_SMALL( md.inertia.cz.z - inertia, 2.0f * FLT_EPSILON ); + } + + // Translated box + { + b3Vec3 offset = { 0.4f, -0.7f, 0.1f }; + b3Transform transform = { + .p = offset, + .q = b3Quat_identity, + }; + b3Vec3 h = { 0.25f, 0.5f, 0.3f }; + b3BoxHull b1 = b3MakeBoxHull( h.x, h.y, h.z ); + b3BoxHull b2 = b3MakeTransformedBoxHull( h.x, h.y, h.z, transform ); + + b3MassData m1 = b3ComputeHullMass( &b1.base, 1.0f ); + b3MassData m2 = b3ComputeHullMass( &b2.base, 1.0f ); + + ENSURE_SMALL( m1.mass - m2.mass, FLT_EPSILON ); + + b3Matrix3 d = b3SubMM( b1.base.centralInertia, b2.base.centralInertia ); + ENSURE_SMALL( d.cx.x, FLT_EPSILON ); + ENSURE_SMALL( d.cx.y, FLT_EPSILON ); + ENSURE_SMALL( d.cx.z, FLT_EPSILON ); + ENSURE_SMALL( d.cy.x, FLT_EPSILON ); + ENSURE_SMALL( d.cy.y, FLT_EPSILON ); + ENSURE_SMALL( d.cy.z, FLT_EPSILON ); + ENSURE_SMALL( d.cz.x, FLT_EPSILON ); + ENSURE_SMALL( d.cz.y, FLT_EPSILON ); + ENSURE_SMALL( d.cz.z, FLT_EPSILON ); + + ENSURE_SMALL( m2.center.x - offset.x, FLT_EPSILON ); + ENSURE_SMALL( m2.center.y - offset.y, FLT_EPSILON ); + ENSURE_SMALL( m2.center.z - offset.z, FLT_EPSILON ); + } + + // Rotated box + { + b3Vec3 h1 = { 0.25f, 0.5f, 0.3f }; + b3Vec3 h2 = { 0.25f, 0.3f, 0.5f }; + b3Quat q = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisY, b3Vec3_axisZ ); + b3Transform transform = { + .p = b3Vec3_zero, + .q = q, + }; + b3BoxHull b1 = b3MakeTransformedBoxHull( h1.x, h1.y, h1.z, transform ); + b3BoxHull b2 = b3MakeBoxHull( h2.x, h2.y, h2.z ); + + b3MassData m1 = b3ComputeHullMass( &b1.base, 1.0f ); + b3MassData m2 = b3ComputeHullMass( &b2.base, 1.0f ); + + ENSURE_SMALL( m1.mass - m2.mass, FLT_EPSILON ); + + b3Matrix3 d = b3SubMM( b1.base.centralInertia, b2.base.centralInertia ); + ENSURE_SMALL( d.cx.x, FLT_EPSILON ); + ENSURE_SMALL( d.cx.y, FLT_EPSILON ); + ENSURE_SMALL( d.cx.z, FLT_EPSILON ); + ENSURE_SMALL( d.cy.x, FLT_EPSILON ); + ENSURE_SMALL( d.cy.y, FLT_EPSILON ); + ENSURE_SMALL( d.cy.z, FLT_EPSILON ); + ENSURE_SMALL( d.cz.x, FLT_EPSILON ); + ENSURE_SMALL( d.cz.y, FLT_EPSILON ); + ENSURE_SMALL( d.cz.z, FLT_EPSILON ); + + ENSURE_SMALL( m1.center.x - m2.center.x, FLT_EPSILON ); + ENSURE_SMALL( m1.center.y - m2.center.y, FLT_EPSILON ); + ENSURE_SMALL( m1.center.z - m2.center.z, FLT_EPSILON ); + } + + // Transformed box + { + b3Vec3 offset = { 0.4f, -0.7f, 0.1f }; + b3Vec3 h1 = { 0.25f, 0.5f, 0.3f }; + b3Vec3 h2 = { 0.25f, 0.3f, 0.5f }; + b3Quat q = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisY, b3Vec3_axisZ ); + b3Transform transform = { + .p = offset, + .q = q, + }; + b3BoxHull b1 = b3MakeTransformedBoxHull( h1.x, h1.y, h1.z, transform ); + b3BoxHull b2 = b3MakeBoxHull( h2.x, h2.y, h2.z ); + + b3MassData m1 = b3ComputeHullMass( &b1.base, 1.0f ); + b3MassData m2 = b3ComputeHullMass( &b2.base, 1.0f ); + + ENSURE_SMALL( m1.mass - m2.mass, FLT_EPSILON ); + + b3Matrix3 d = b3SubMM( b1.base.centralInertia, b2.base.centralInertia ); + ENSURE_SMALL( d.cx.x, FLT_EPSILON ); + ENSURE_SMALL( d.cx.y, FLT_EPSILON ); + ENSURE_SMALL( d.cx.z, FLT_EPSILON ); + ENSURE_SMALL( d.cy.x, FLT_EPSILON ); + ENSURE_SMALL( d.cy.y, FLT_EPSILON ); + ENSURE_SMALL( d.cy.z, FLT_EPSILON ); + ENSURE_SMALL( d.cz.x, FLT_EPSILON ); + ENSURE_SMALL( d.cz.y, FLT_EPSILON ); + ENSURE_SMALL( d.cz.z, FLT_EPSILON ); + + ENSURE_SMALL( m1.center.x - offset.x, FLT_EPSILON ); + ENSURE_SMALL( m1.center.y - offset.y, FLT_EPSILON ); + ENSURE_SMALL( m1.center.z - offset.z, FLT_EPSILON ); + } + + // Capsule + { + float radius = capsule.radius; + float length = b3Distance( capsule.center1, capsule.center2 ); + + // Capsule along x-axis + b3MassData md = b3ComputeCapsuleMass( &capsule, 1.0f ); + + // Box that fully contains capsule. Upper bound on capsule mass. + b3BoxHull r = b3MakeBoxHull( radius + 0.5f * length, radius, radius ); + b3MassData mdUpper = b3ComputeHullMass( &r.base, 1.0f ); + + // Approximate capsule using convex hull. This should be a lower bound on the + // capsule mass. + b3Vec3 points[2 * N * N]; + float d = B3_PI / ( N - 1.0f ); + float angle1 = -0.5f * B3_PI; + int index = 0; + for ( int i = 0; i < N; ++i ) + { + float s1 = sinf( angle1 ); + float c1 = cosf( angle1 ); + float angle2 = -0.5f * B3_PI; + for ( int j = 0; j < N; ++j ) + { + points[index].x = 1.0f + radius * c1; + points[index].y = radius * s1 * cosf( angle2 ); + points[index].z = radius * s1 * sinf( angle2 ); + angle2 += d; + index += 1; + } + + angle1 += d; + } + + angle1 = 0.5f * B3_PI; + for ( int i = 0; i < N; ++i ) + { + float s1 = sinf( angle1 ); + float c1 = cosf( angle1 ); + float angle2 = -0.5f * B3_PI; + for ( int j = 0; j < N; ++j ) + { + points[index].x = -1.0f + radius * c1; + points[index].y = radius * s1 * cosf( angle2 ); + points[index].z = radius * s1 * sinf( angle2 ); + angle2 += d; + index += 1; + } + + angle1 += d; + } + + ENSURE( index == 2 * N * N ); + + b3Hull* hull = b3CreateHull( points, 2 * N * N, 2 * N * N ); + b3MassData mdLower = b3ComputeHullMass( hull, 1.0f ); + + ENSURE( mdLower.mass < md.mass && md.mass < mdUpper.mass ); + ENSURE( mdLower.inertia.cx.x < md.inertia.cx.x && md.inertia.cx.x < mdUpper.inertia.cx.x ); + ENSURE( mdLower.inertia.cy.y < md.inertia.cy.y && md.inertia.cy.y < mdUpper.inertia.cy.y ); + ENSURE( mdLower.inertia.cz.z < md.inertia.cz.z && md.inertia.cz.z < mdUpper.inertia.cz.z ); + + b3DestroyHull( hull ); + } + + return 0; +} + +static int ShapeAABBTest( void ) +{ + { + b3AABB b = b3ComputeSphereAABB( &sphere, b3Transform_identity ); + ENSURE_SMALL( b.lowerBound.x, FLT_EPSILON ); + ENSURE_SMALL( b.lowerBound.y + 1.0f, FLT_EPSILON ); + ENSURE_SMALL( b.lowerBound.z + 1.0f, FLT_EPSILON ); + ENSURE_SMALL( b.upperBound.x - 2.0f, FLT_EPSILON ); + ENSURE_SMALL( b.upperBound.y - 1.0f, FLT_EPSILON ); + ENSURE_SMALL( b.upperBound.z - 1.0f, FLT_EPSILON ); + } + + { + b3AABB b = b3ComputeCapsuleAABB( &capsule, b3Transform_identity ); + ENSURE_SMALL( b.lowerBound.x + 2.0f, FLT_EPSILON ); + ENSURE_SMALL( b.lowerBound.y + 1.0f, FLT_EPSILON ); + ENSURE_SMALL( b.lowerBound.z + 1.0f, FLT_EPSILON ); + ENSURE_SMALL( b.upperBound.x - 2.0f, FLT_EPSILON ); + ENSURE_SMALL( b.upperBound.y - 1.0f, FLT_EPSILON ); + ENSURE_SMALL( b.upperBound.z - 1.0f, FLT_EPSILON ); + } + { + b3AABB b = b3ComputeHullAABB( &box.base, b3Transform_identity ); + ENSURE_SMALL( b.lowerBound.x + 1.0f, FLT_EPSILON ); + ENSURE_SMALL( b.lowerBound.y + 1.0f, FLT_EPSILON ); + ENSURE_SMALL( b.lowerBound.z + 1.0f, FLT_EPSILON ); + ENSURE_SMALL( b.upperBound.x - 1.0f, FLT_EPSILON ); + ENSURE_SMALL( b.upperBound.y - 1.0f, FLT_EPSILON ); + ENSURE_SMALL( b.upperBound.z - 1.0f, FLT_EPSILON ); + } + + return 0; +} + +#if 0 +static int PointInShapeTest( void ) +{ + b3Vec2 p1 = { 0.5f, 0.5f }; + b3Vec2 p2 = { 4.0f, -4.0f }; + + { + bool hit; + hit = b3PointInSphere( p1, &sphere ); + ENSURE( hit == true ); + hit = b3PointInSphere( p2, &sphere ); + ENSURE( hit == false ); + } + + { + bool hit; + hit = b3PointInPolygon( p1, &box ); + ENSURE( hit == true ); + hit = b3PointInPolygon( p2, &box ); + ENSURE( hit == false ); + } + + return 0; +} +#endif + +static int RayCastShapeTest( void ) +{ + b3RayCastInput input = { + .origin = { -4.0f, 0.0f, 0.0f }, + .translation = { 8.0f, 0.0f, 0.0f }, + .maxFraction = 1.0f, + }; + + { + b3CastOutput output = b3RayCastSphere( &sphere, &input ); + ENSURE( output.hit ); + ENSURE_SMALL( output.normal.x + 1.0f, FLT_EPSILON ); + ENSURE_SMALL( output.normal.y, FLT_EPSILON ); + ENSURE_SMALL( output.normal.z, FLT_EPSILON ); + ENSURE_SMALL( output.fraction - 0.5f, FLT_EPSILON ); + } + + { + b3CastOutput output = b3RayCastCapsule( &capsule, &input ); + ENSURE( output.hit ); + ENSURE_SMALL( output.normal.x + 1.0f, FLT_EPSILON ); + ENSURE_SMALL( output.normal.y, FLT_EPSILON ); + ENSURE_SMALL( output.normal.z, FLT_EPSILON ); + ENSURE_SMALL( output.fraction - 1.0f / 4.0f, FLT_EPSILON ); + } + + { + b3CastOutput output = b3RayCastHull( &box.base, &input ); + ENSURE( output.hit ); + ENSURE_SMALL( output.normal.x + 1.0f, FLT_EPSILON ); + ENSURE_SMALL( output.normal.y, FLT_EPSILON ); + ENSURE_SMALL( output.normal.z, FLT_EPSILON ); + ENSURE_SMALL( output.fraction - 3.0f / 8.0f, FLT_EPSILON ); + } + + return 0; +} + +int ShapeTest( void ) +{ + box = b3MakeBoxHull( 1.0f, 1.0f, 1.0f ); + + RUN_SUBTEST( ShapeMassTest ); + RUN_SUBTEST( ShapeAABBTest ); + // RUN_SUBTEST( PointInShapeTest ); + RUN_SUBTEST( RayCastShapeTest ); + + return 0; +} diff --git a/test/test_table.c b/test/test_table.c new file mode 100644 index 0000000..3cfe788 --- /dev/null +++ b/test/test_table.c @@ -0,0 +1,113 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "ctz.h" +#include "table.h" +#include "test_macros.h" + +#include "box2d/base.h" + +#include + +#define SET_SPAN 317 +#define ITEM_COUNT ( ( SET_SPAN * SET_SPAN - SET_SPAN ) / 2 ) + +int TableTest( void ) +{ + int power = b2BoundingPowerOf2( 3008 ); + ENSURE( power == 12 ); + + int nextPowerOf2 = b2RoundUpPowerOf2( 3008 ); + ENSURE( nextPowerOf2 == ( 1 << power ) ); + + const int32_t N = SET_SPAN; + const uint32_t itemCount = ITEM_COUNT; + bool removed[ITEM_COUNT] = { 0 }; + + for ( int32_t iter = 0; iter < 1; ++iter ) + { + b2HashSet set = b2CreateSet( 16 ); + + // Fill set + for ( int32_t i = 0; i < N; ++i ) + { + for ( int32_t j = i + 1; j < N; ++j ) + { + uint64_t key = B2_SHAPE_PAIR_KEY( i, j ); + b2AddKey( &set, key ); + } + } + + ENSURE( set.count == itemCount ); + + // Remove a portion of the set + int32_t k = 0; + uint32_t removeCount = 0; + for ( int32_t i = 0; i < N; ++i ) + { + for ( int32_t j = i + 1; j < N; ++j ) + { + if ( j == i + 1 ) + { + uint64_t key = B2_SHAPE_PAIR_KEY( i, j ); + b2RemoveKey( &set, key ); + removed[k++] = true; + removeCount += 1; + } + else + { + removed[k++] = false; + } + } + } + + ENSURE( set.count == ( itemCount - removeCount ) ); + +#ifndef NDEBUG + extern _Atomic int g_probeCount; + g_probeCount = 0; +#endif + + // Test key search + // ~5ns per search on an AMD 7950x + b2Timer timer = b2CreateTimer(); + + k = 0; + for ( int32_t i = 0; i < N; ++i ) + { + for ( int32_t j = i + 1; j < N; ++j ) + { + uint64_t key = B2_SHAPE_PAIR_KEY( j, i ); + ENSURE( b2ContainsKey( &set, key ) || removed[k] ); + k += 1; + } + } + + // uint64_t ticks = b2GetTicks(&timer); + // printf("set ticks = %llu\n", ticks); + + float ms = b2GetMilliseconds( &timer ); + printf( "set: count = %d, b2ContainsKey = %.5f ms, ave = %.5f us\n", itemCount, ms, 1000.0f * ms / itemCount ); + +#if !NDEBUG + float aveProbeCount = (float)g_probeCount / (float)itemCount; + printf( "item count = %d, probe count = %d, ave probe count %.2f\n", itemCount, g_probeCount, aveProbeCount ); +#endif + + // Remove all keys from set + for ( int32_t i = 0; i < N; ++i ) + { + for ( int32_t j = i + 1; j < N; ++j ) + { + uint64_t key = B2_SHAPE_PAIR_KEY( i, j ); + b2RemoveKey( &set, key ); + } + } + + ENSURE( set.count == 0 ); + + b2DestroySet( &set ); + } + + return 0; +} diff --git a/test/test_world.c b/test/test_world.c new file mode 100644 index 0000000..b82ef3d --- /dev/null +++ b/test/test_world.c @@ -0,0 +1,720 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "benchmarks.h" +#include "stability.h" +#include "test_macros.h" + +#include "box3d/box3d.h" +#include "box3d/collision.h" +#include "box3d/constants.h" +#include "box3d/math_functions.h" + +#include + +// This is a simple example of building and running a simulation +// using Box3D. Here we create a large ground box and a small dynamic +// box. +// There are no graphics for this example. Box3D is meant to be used +// with your rendering engine in your game engine. +int HelloWorld( void ) +{ + // Construct a world object, which will hold and simulate the rigid bodies. + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.gravity = (b3Vec3){ 0.0f, -10.0f, 0.0f }; + + b3WorldId worldId = b3CreateWorld( &worldDef ); + ENSURE( b3World_IsValid( worldId ) ); + + // Define the ground body. + b3BodyDef groundBodyDef = b3DefaultBodyDef(); + groundBodyDef.position = (b3Vec3){ 0.0f, -10.0f, 0.0f }; + + // Call the body factory which allocates memory for the ground body + // from a pool and creates the ground box shape (also from a pool). + // The body is also added to the world. + b3BodyId groundId = b3CreateBody( worldId, &groundBodyDef ); + ENSURE( b3Body_IsValid( groundId ) ); + + // Define the ground box shape. The extents are the half-widths of the box. + b3BoxHull groundBox = b3MakeBoxHull( 50.0f, 10.0f, 50.0f ); + + // Add the box shape to the ground body. + b3ShapeDef groundShapeDef = b3DefaultShapeDef(); + b3CreateHullShape( groundId, &groundShapeDef, &groundBox.base ); + + // Define the dynamic body. We set its position and call the body factory. + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Vec3){ 0.0f, 4.0f, 0.0f }; + + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + // Define another box shape for our dynamic body. + b3BoxHull dynamicBox = b3MakeCubeHull( 1.0f ); + + // Define the dynamic body shape + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + // Set the box density to be non-zero, so it will be dynamic. + shapeDef.density = 1.0f; + + // Override the default friction. + shapeDef.baseMaterial.friction = 0.3f; + + // Add the shape to the body. + b3CreateHullShape( bodyId, &shapeDef, &dynamicBox.base ); + + // Prepare for simulation. Typically we use a time step of 1/60 of a + // second (60Hz) and 4 sub-steps. This provides a high quality simulation + // in most game scenarios. + float timeStep = 1.0f / 60.0f; + int subStepCount = 4; + + b3Vec3 position = b3Body_GetPosition( bodyId ); + b3Quat rotation = b3Body_GetRotation( bodyId ); + + // This is our little game loop. + for ( int i = 0; i < 90; ++i ) + { + // Instruct the world to perform a single step of simulation. + // It is generally best to keep the time step and iterations fixed. + b3World_Step( worldId, timeStep, subStepCount ); + + // Now print the position and angle of the body. + position = b3Body_GetPosition( bodyId ); + rotation = b3Body_GetRotation( bodyId ); + + // printf("%4.2f %4.2f %4.2f\n", position.x, position.y, b3Rot_GetAngle(rotation)); + } + + // When the world destructor is called, all bodies and joints are freed. This can + // create orphaned ids, so be careful about your world management. + b3DestroyWorld( worldId ); + + ENSURE_SMALL( position.y - 1.00f, 0.01f ); + ENSURE_SMALL( rotation.v.x, 0.01f ); + ENSURE_SMALL( rotation.v.z, 0.01f ); + + return 0; +} + +int EmptyWorld( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + ENSURE( b3World_IsValid( worldId ) == true ); + + float timeStep = 1.0f / 60.0f; + int subStepCount = 1; + + for ( int i = 0; i < 60; ++i ) + { + b3World_Step( worldId, timeStep, subStepCount ); + } + + b3DestroyWorld( worldId ); + + ENSURE( b3World_IsValid( worldId ) == false ); + + return 0; +} + +#define BODY_COUNT 10 +int DestroyAllBodiesWorld( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + ENSURE( b3World_IsValid( worldId ) == true ); + + int count = 0; + bool creating = true; + + b3BodyId bodyIds[BODY_COUNT]; + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + b3BoxHull cube = b3MakeCubeHull( 0.5f ); + + for ( int i = 0; i < 2 * BODY_COUNT + 10; ++i ) + { + if ( creating ) + { + if ( count < BODY_COUNT ) + { + bodyIds[count] = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( bodyIds[count], &shapeDef, &cube.base ); + count += 1; + } + else + { + creating = false; + } + } + else if ( count > 0 ) + { + b3DestroyBody( bodyIds[count - 1] ); + bodyIds[count - 1] = b3_nullBodyId; + count -= 1; + } + + b3World_Step( worldId, 1.0f / 60.0f, 3 ); + } + + b3Counters counters = b3World_GetCounters( worldId ); + ENSURE( counters.bodyCount == 0 ); + + b3DestroyWorld( worldId ); + + ENSURE( b3World_IsValid( worldId ) == false ); + + return 0; +} + +static int TestIsValid( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + ENSURE( b3World_IsValid( worldId ) ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + + b3BodyId bodyId1 = b3CreateBody( worldId, &bodyDef ); + ENSURE( b3Body_IsValid( bodyId1 ) == true ); + + b3BodyId bodyId2 = b3CreateBody( worldId, &bodyDef ); + ENSURE( b3Body_IsValid( bodyId2 ) == true ); + + b3DestroyBody( bodyId1 ); + ENSURE( b3Body_IsValid( bodyId1 ) == false ); + + b3DestroyBody( bodyId2 ); + ENSURE( b3Body_IsValid( bodyId2 ) == false ); + + b3DestroyWorld( worldId ); + + ENSURE( b3World_IsValid( worldId ) == false ); + ENSURE( b3Body_IsValid( bodyId2 ) == false ); + ENSURE( b3Body_IsValid( bodyId1 ) == false ); + + return 0; +} + +#define WORLD_COUNT ( B3_MAX_WORLDS / 2 ) + +int TestWorldRecycle( void ) +{ + _Static_assert( WORLD_COUNT > 0, "world count" ); + + int count = 100; + + b3WorldId worldIds[WORLD_COUNT]; + + for ( int i = 0; i < count; ++i ) + { + b3WorldDef worldDef = b3DefaultWorldDef(); + for ( int j = 0; j < WORLD_COUNT; ++j ) + { + worldIds[j] = b3CreateWorld( &worldDef ); + ENSURE( b3World_IsValid( worldIds[j] ) == true ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3CreateBody( worldIds[j], &bodyDef ); + } + + for ( int j = 0; j < WORLD_COUNT; ++j ) + { + float timeStep = 1.0f / 60.0f; + int subStepCount = 1; + + for ( int k = 0; k < 10; ++k ) + { + b3World_Step( worldIds[j], timeStep, subStepCount ); + } + } + + for ( int j = WORLD_COUNT - 1; j >= 0; --j ) + { + b3DestroyWorld( worldIds[j] ); + ENSURE( b3World_IsValid( worldIds[j] ) == false ); + worldIds[j] = b3_nullWorldId; + } + } + + return 0; +} + +static bool CustomFilter( b3ShapeId shapeIdA, b3ShapeId shapeIdB, void* context ) +{ + (void)shapeIdA; + (void)shapeIdB; + ENSURE( context == NULL ); + return true; +} + +static bool PreSolveStatic( b3ShapeId shapeIdA, b3ShapeId shapeIdB, b3Vec3 point, b3Vec3 normal, void* context ) +{ + (void)shapeIdA; + (void)shapeIdB; + (void)point; + (void)normal; + ENSURE( context == NULL ); + return false; +} + +// This test is here to ensure all API functions link correctly. +int TestWorldCoverage( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + + b3WorldId worldId = b3CreateWorld( &worldDef ); + ENSURE( b3World_IsValid( worldId ) ); + + b3World_EnableSleeping( worldId, true ); + b3World_EnableSleeping( worldId, false ); + bool flag = b3World_IsSleepingEnabled( worldId ); + ENSURE( flag == false ); + + b3World_EnableContinuous( worldId, false ); + b3World_EnableContinuous( worldId, true ); + flag = b3World_IsContinuousEnabled( worldId ); + ENSURE( flag == true ); + + b3World_SetRestitutionThreshold( worldId, 0.0f ); + b3World_SetRestitutionThreshold( worldId, 2.0f ); + float value = b3World_GetRestitutionThreshold( worldId ); + ENSURE( value == 2.0f ); + + b3World_SetHitEventThreshold( worldId, 0.0f ); + b3World_SetHitEventThreshold( worldId, 100.0f ); + value = b3World_GetHitEventThreshold( worldId ); + ENSURE( value == 100.0f ); + + b3World_SetCustomFilterCallback( worldId, CustomFilter, NULL ); + b3World_SetPreSolveCallback( worldId, PreSolveStatic, NULL ); + + b3Vec3 g = { 1.0f, 2.0f }; + b3World_SetGravity( worldId, g ); + b3Vec3 v = b3World_GetGravity( worldId ); + ENSURE( v.x == g.x ); + ENSURE( v.y == g.y ); + + b3ExplosionDef explosionDef = b3DefaultExplosionDef(); + b3World_Explode( worldId, &explosionDef ); + + b3World_SetContactTuning( worldId, 10.0f, 2.0f, 4.0f ); + + b3World_SetMaximumLinearSpeed( worldId, 10.0f ); + value = b3World_GetMaximumLinearSpeed( worldId ); + ENSURE( value == 10.0f ); + + b3World_EnableWarmStarting( worldId, true ); + flag = b3World_IsWarmStartingEnabled( worldId ); + ENSURE( flag == true ); + + int count = b3World_GetAwakeBodyCount( worldId ); + ENSURE( count == 0 ); + + b3World_SetUserData( worldId, &value ); + void* userData = b3World_GetUserData( worldId ); + ENSURE( userData == &value ); + + b3World_Step( worldId, 1.0f, 1 ); + + b3DestroyWorld( worldId ); + + return 0; +} + +static int TestSensor( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + // Wall from x = 1 to x = 2 + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_staticBody; + bodyDef.position = (b3Vec3){ 1.5f, 11.0f, 0.0f }; + b3BodyId wallId = b3CreateBody( worldId, &bodyDef ); + b3BoxHull box = b3MakeBoxHull( 0.5f, 10.0f, 1.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.enableSensorEvents = true; + b3CreateHullShape( wallId, &shapeDef, &box.base ); + + // Bullet fired towards the wall + bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.isBullet = true; + bodyDef.gravityScale = 0.0f; + bodyDef.position = (b3Vec3){ 7.39814f, 4.0f, 0.0f }; + bodyDef.linearVelocity = (b3Vec3){ -20.0f, 0.0f, 0.0f }; + b3BodyId bulletId = b3CreateBody( worldId, &bodyDef ); + shapeDef = b3DefaultShapeDef(); + shapeDef.isSensor = true; + shapeDef.enableSensorEvents = true; + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.1f }; + b3CreateSphereShape( bulletId, &shapeDef, &sphere ); + + int beginCount = 0; + int endCount = 0; + + while ( true ) + { + float timeStep = 1.0f / 60.0f; + int subStepCount = 4; + b3World_Step( worldId, timeStep, subStepCount ); + + b3Vec3 bulletPos = b3Body_GetPosition( bulletId ); + // printf( "Bullet pos: %g %g\n", bulletPos.x, bulletPos.y ); + + b3SensorEvents events = b3World_GetSensorEvents( worldId ); + + if ( events.beginCount > 0 ) + { + beginCount += 1; + } + + if ( events.endCount > 0 ) + { + endCount += 1; + } + + if ( bulletPos.x < -1.0f ) + { + break; + } + } + + b3DestroyWorld( worldId ); + + ENSURE( beginCount == 1 ); + ENSURE( endCount == 1 ); + + return 0; +} + +static int TestContactEvents( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + // Static ground + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_staticBody; + bodyDef.position = (b3Vec3){ 0.0f, -0.5f, 0.0f }; + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + b3BoxHull groundBox = b3MakeBoxHull( 10.0f, 0.5f, 10.0f ); + b3ShapeDef groundShapeDef = b3DefaultShapeDef(); + b3ShapeId groundShapeId = b3CreateHullShape( groundId, &groundShapeDef, &groundBox.base ); + + // Dynamic sphere dropped onto the ground; restitution causes it to bounce so we get end events + bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Vec3){ 0.0f, 5.0f, 0.0f }; + b3BodyId sphereBodyId = b3CreateBody( worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.enableContactEvents = true; + shapeDef.baseMaterial.restitution = 0.6f; + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f }; + b3ShapeId sphereShapeId = b3CreateSphereShape( sphereBodyId, &shapeDef, &sphere ); + + int beginCount = 0; + int endCount = 0; + bool idsChecked = false; + + for ( int i = 0; i < 120; ++i ) + { + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + + b3ContactEvents events = b3World_GetContactEvents( worldId ); + + if ( events.beginCount > 0 && idsChecked == false ) + { + b3ContactBeginTouchEvent be = events.beginEvents[0]; + bool aIsSphere = B3_ID_EQUALS( be.shapeIdA, sphereShapeId ); + bool bIsSphere = B3_ID_EQUALS( be.shapeIdB, sphereShapeId ); + bool aIsGround = B3_ID_EQUALS( be.shapeIdA, groundShapeId ); + bool bIsGround = B3_ID_EQUALS( be.shapeIdB, groundShapeId ); + ENSURE( ( aIsSphere && bIsGround ) || ( aIsGround && bIsSphere ) ); + ENSURE( b3Contact_IsValid( be.contactId ) ); + idsChecked = true; + } + + beginCount += events.beginCount; + endCount += events.endCount; + } + + b3DestroyWorld( worldId ); + + ENSURE( idsChecked ); + ENSURE( beginCount >= 1 ); + ENSURE( endCount >= 1 ); + + return 0; +} + +static int TestHitEvents( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.hitEventThreshold = 1.0f; + b3WorldId worldId = b3CreateWorld( &worldDef ); + + // Static ground + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_staticBody; + bodyDef.position = (b3Vec3){ 0.0f, -0.5f, 0.0f }; + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + b3BoxHull groundBox = b3MakeBoxHull( 10.0f, 0.5f, 10.0f ); + b3ShapeDef groundShapeDef = b3DefaultShapeDef(); + b3CreateHullShape( groundId, &groundShapeDef, &groundBox.base ); + + // Sphere driven into the ground fast enough to clear the hit threshold + bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.gravityScale = 0.0f; + bodyDef.position = (b3Vec3){ 0.0f, 2.0f, 0.0f }; + bodyDef.linearVelocity = (b3Vec3){ 0.0f, -30.0f, 0.0f }; + b3BodyId sphereBodyId = b3CreateBody( worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.enableHitEvents = true; + shapeDef.baseMaterial.userMaterialId = 7; + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f }; + b3CreateSphereShape( sphereBodyId, &shapeDef, &sphere ); + + int hitCount = 0; + float capturedSpeed = 0.0f; + uint64_t capturedMaterialA = 0; + uint64_t capturedMaterialB = 0; + b3Vec3 capturedNormal = { 0.0f, 0.0f, 0.0f }; + + for ( int i = 0; i < 30; ++i ) + { + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + + b3ContactEvents events = b3World_GetContactEvents( worldId ); + if ( events.hitCount > 0 && hitCount == 0 ) + { + b3ContactHitEvent hit = events.hitEvents[0]; + capturedSpeed = hit.approachSpeed; + capturedNormal = hit.normal; + capturedMaterialA = hit.userMaterialIdA; + capturedMaterialB = hit.userMaterialIdB; + } + + hitCount += events.hitCount; + } + + b3DestroyWorld( worldId ); + + ENSURE( hitCount >= 1 ); + ENSURE( capturedSpeed > 1.0f ); + // Head-on vertical impact: normal lies along Y + ENSURE_SMALL( capturedNormal.x, 0.01f ); + ENSURE_SMALL( capturedNormal.z, 0.01f ); + // One side of the contact carries the sphere's user material + ENSURE( capturedMaterialA == 7 || capturedMaterialB == 7 ); + + return 0; +} + +// Hit-event material lookup must respect the compound child that participated in the +// contact. Two children with distinct userMaterialIds at separated positions, dropped +// sphere strikes one specifically. Without the fix, both children would report +// materials[0] and the strike on hull 1 would be misattributed. +static int TestCompoundHitEvents( void ) +{ + const uint64_t kHullMaterialA = 11; + const uint64_t kHullMaterialB = 22; + const uint64_t kSphereMaterial = 99; + const float kHullCenterX = 3.0f; + + for ( int side = 0; side < 2; ++side ) + { + uint64_t expectedHullMaterial = ( side == 0 ) ? kHullMaterialA : kHullMaterialB; + float spawnX = ( side == 0 ) ? -kHullCenterX : kHullCenterX; + + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.hitEventThreshold = 1.0f; + b3WorldId worldId = b3CreateWorld( &worldDef ); + + // Build a compound with two hulls at opposite x positions, distinct userMaterialIds + b3BoxHull boxA = b3MakeBoxHull( 1.0f, 1.0f, 1.0f ); + b3BoxHull boxB = b3MakeBoxHull( 1.0f, 1.0f, 1.0f ); + + b3SurfaceMaterial matA = b3DefaultSurfaceMaterial(); + matA.userMaterialId = kHullMaterialA; + + b3SurfaceMaterial matB = b3DefaultSurfaceMaterial(); + matB.userMaterialId = kHullMaterialB; + + b3CompoundHullDef hulls[2]; + hulls[0].hull = &boxA.base; + hulls[0].transform = (b3Transform){ { -kHullCenterX, 0.0f, 0.0f }, b3Quat_identity }; + hulls[0].material = matA; + hulls[1].hull = &boxB.base; + hulls[1].transform = (b3Transform){ { kHullCenterX, 0.0f, 0.0f }, b3Quat_identity }; + hulls[1].material = matB; + + b3CompoundDef compoundDef = { 0 }; + compoundDef.hulls = hulls; + compoundDef.hullCount = 2; + b3Compound* compound = b3CreateCompound( &compoundDef ); + ENSURE( compound != NULL ); + + // Static body holds the compound + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_staticBody; + b3BodyId compoundBodyId = b3CreateBody( worldId, &bodyDef ); + b3ShapeDef compoundShapeDef = b3DefaultShapeDef(); + b3CreateCompoundShape( compoundBodyId, &compoundShapeDef, compound ); + + // Sphere driven straight down onto the chosen child + bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.gravityScale = 0.0f; + bodyDef.position = (b3Vec3){ spawnX, 3.0f, 0.0f }; + bodyDef.linearVelocity = (b3Vec3){ 0.0f, -30.0f, 0.0f }; + b3BodyId sphereBodyId = b3CreateBody( worldId, &bodyDef ); + b3ShapeDef sphereShapeDef = b3DefaultShapeDef(); + sphereShapeDef.density = 1.0f; + sphereShapeDef.enableHitEvents = true; + sphereShapeDef.baseMaterial.userMaterialId = kSphereMaterial; + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f }; + b3CreateSphereShape( sphereBodyId, &sphereShapeDef, &sphere ); + + int hitCount = 0; + uint64_t capturedMaterialA = 0; + uint64_t capturedMaterialB = 0; + + for ( int i = 0; i < 30; ++i ) + { + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + + b3ContactEvents events = b3World_GetContactEvents( worldId ); + if ( events.hitCount > 0 && hitCount == 0 ) + { + b3ContactHitEvent hit = events.hitEvents[0]; + capturedMaterialA = hit.userMaterialIdA; + capturedMaterialB = hit.userMaterialIdB; + } + hitCount += events.hitCount; + } + + b3DestroyWorld( worldId ); + b3DestroyCompound( compound ); + + ENSURE( hitCount >= 1 ); + // Sphere material on one side + ENSURE( capturedMaterialA == kSphereMaterial || capturedMaterialB == kSphereMaterial ); + // Struck compound child's material on the other side. The pre-fix code returned + // materials[0] (kHullMaterialA) for both sides, so a strike on the +x child would fail. + ENSURE( capturedMaterialA == expectedHullMaterial || capturedMaterialB == expectedHullMaterial ); + } + + return 0; +} + +#if 0 +static int TestSetWorkerCount( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.workerCount = 1; + b3WorldId worldId = b3CreateWorld( &worldDef ); + ENSURE( b3World_IsValid( worldId ) ); + ENSURE( b3World_GetWorkerCount( worldId ) == 1 ); + + CreateJunkyard( worldId ); + StepJunkyard( worldId, 1 ); + + b3World_SetWorkerCount( worldId, 4 ); + ENSURE( b3World_GetWorkerCount( worldId ) == 4 ); + + StepJunkyard( worldId, 2 ); + + b3World_SetWorkerCount( worldId, 4 ); + ENSURE( b3World_GetWorkerCount( worldId ) == 4 ); + + StepJunkyard( worldId, 3 ); + + b3World_SetWorkerCount( worldId, 0 ); + ENSURE( b3World_GetWorkerCount( worldId ) == 1 ); + + StepJunkyard( worldId, 4 ); + + b3World_SetWorkerCount( worldId, -5 ); + ENSURE( b3World_GetWorkerCount( worldId ) == 1 ); + + StepJunkyard( worldId, 5 ); + + b3World_SetWorkerCount( worldId, B2_MAX_WORKERS + 10 ); + ENSURE( b3World_GetWorkerCount( worldId ) == B2_MAX_WORKERS ); + + StepJunkyard( worldId, 2 ); + + b3DestroyWorld( worldId ); + + return 0; +} +#endif + +// This tests continuous collision and mesh contact stability +static int TestMeshDrop( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + // todo GetCoreCount + worldDef.workerCount = 4; + + b3WorldId worldId = b3CreateWorld( &worldDef ); + + MeshDropData data = CreateMeshDrop( worldId ); + + float timeStep = 1.0f / 60.0f; + + int stepIndex = 0; + int stepLimit = 400; + + while ( stepIndex < stepLimit ) + { + int subStepCount = 4; + b3World_Step( worldId, timeStep, subStepCount ); + + b3BodyEvents events = b3World_GetBodyEvents( worldId ); + if (events.moveCount == 0) + { + // All bodies sleeping + break; + } + + stepIndex += 1; + } + + printf( " TestMeshDrop stepIndex = %d\n", stepIndex ); + + DestroyMeshDrop( &data ); + b3DestroyWorld( worldId ); + + ENSURE( stepIndex < stepLimit ); + + return 0; +} + +int WorldTest( void ) +{ + RUN_SUBTEST( HelloWorld ); + RUN_SUBTEST( EmptyWorld ); + RUN_SUBTEST( DestroyAllBodiesWorld ); + RUN_SUBTEST( TestIsValid ); + RUN_SUBTEST( TestWorldRecycle ); + RUN_SUBTEST( TestWorldCoverage ); + RUN_SUBTEST( TestSensor ); + RUN_SUBTEST( TestContactEvents ); + RUN_SUBTEST( TestHitEvents ); + RUN_SUBTEST( TestCompoundHitEvents ); + RUN_SUBTEST( TestMeshDrop ); + // RUN_SUBTEST( TestSetWorkerCount ); + + return 0; +}