packages feed

Box3D (empty) → 0.1.0.0

raw patch · 160 files changed

+87188/−0 lines, 160 filesdep +Box3Ddep +basedep +tasty

Dependencies added: Box3D, base, tasty, tasty-bench, tasty-hunit, vector

Files

+ Box3D.cabal view
@@ -0,0 +1,270 @@+cabal-version: 2.2++-- This file has been generated from package.yaml by hpack version 0.39.6.+--+-- see: https://github.com/sol/hpack++name:           Box3D+version:        0.1.0.0+synopsis:       Low-level bindings to the Box3D physics engine+description:    Low-level FFI bindings to Box3D, Erin Catto's 3D rigid-body physics engine.+                The engine's C sources (MIT licensed) are bundled and compiled with the+                package, so no system library is needed.+                .+                Each module follows one part of the C API, and each wrapper's documentation+                names the function it binds. See the README for an example, the exact engine+                revision and the rules on callbacks and object lifetimes.+category:       Physics+author:         IC Rainbow+maintainer:     aenor.realm@gmail.com+copyright:      2026 IC Rainbow+license:        BSD-3-Clause AND MIT+license-files:  LICENSE,+                upstream/LICENSE+build-type:     Simple+tested-with:+    GHC == 9.10.3+extra-source-files:+    cbits/box3d_gen_shim.h+    cbits/box3d_shim.h+    upstream/include/box3d/base.h+    upstream/include/box3d/box3d.h+    upstream/include/box3d/collision.h+    upstream/include/box3d/config.h+    upstream/include/box3d/constants.h+    upstream/include/box3d/id.h+    upstream/include/box3d/math_functions.h+    upstream/include/box3d/types.h+    upstream/src/aabb.h+    upstream/src/algorithm.h+    upstream/src/arena_allocator.h+    upstream/src/bitset.h+    upstream/src/block_allocator.h+    upstream/src/body.h+    upstream/src/broad_phase.h+    upstream/src/compound.h+    upstream/src/constraint_graph.h+    upstream/src/contact.h+    upstream/src/contact_solver.h+    upstream/src/container.h+    upstream/src/core.h+    upstream/src/ctz.h+    upstream/src/hull.h+    upstream/src/id_pool.h+    upstream/src/island.h+    upstream/src/joint.h+    upstream/src/manifold.h+    upstream/src/math_internal.h+    upstream/src/name_cache.h+    upstream/src/parallel_for.h+    upstream/src/physics_world.h+    upstream/src/platform.h+    upstream/src/qsort.h+    upstream/src/rapidhash.h+    upstream/src/recording.h+    upstream/src/recording_replay.h+    upstream/src/scheduler.h+    upstream/src/sensor.h+    upstream/src/shape.h+    upstream/src/simd.h+    upstream/src/solver.h+    upstream/src/solver_set.h+    upstream/src/table.h+    upstream/src/verstable.h+    upstream/src/world_snapshot.h+    upstream/src/recording_ops.inl+extra-doc-files:+    README.md+    CHANGELOG.md++source-repository head+  type: git+  location: https://gitlab.com/dpwiz/box-nd++flag assertions+  description: Keep the engine's internal assertions. A failed one stops the process with a trap.+  manual: True+  default: False++flag double-precision+  description: Store world positions as doubles (the engine's large world mode). Every package in a build shares this setting.+  manual: True+  default: False++library+  exposed-modules:+      Box3D.Authoring+      Box3D.Base+      Box3D.Body+      Box3D.BoxMesh+      Box3D.Callbacks+      Box3D.Collision+      Box3D.Compound+      Box3D.Cone+      Box3D.Contact+      Box3D.Cylinder+      Box3D.DebugDraw+      Box3D.DebugShape+      Box3D.DistanceJoint+      Box3D.DynamicTree+      Box3D.Events+      Box3D.Extra+      Box3D.FilterJoint+      Box3D.Geometry+      Box3D.Grid+      Box3D.GridMesh+      Box3D.HeightField+      Box3D.HollowBoxMesh+      Box3D.Hull+      Box3D.Id+      Box3D.Internal+      Box3D.Internal.DrawClosure+      Box3D.Joint+      Box3D.Math+      Box3D.MathFunctions+      Box3D.MathTypes+      Box3D.Mesh+      Box3D.MotorJoint+      Box3D.Mover+      Box3D.ParallelJoint+      Box3D.PlatformMesh+      Box3D.Player+      Box3D.PrismaticJoint+      Box3D.Query+      Box3D.Recording+      Box3D.RecPlayer+      Box3D.RevoluteJoint+      Box3D.Rock+      Box3D.Shape+      Box3D.SphericalJoint+      Box3D.Tags+      Box3D.TorusMesh+      Box3D.UserData+      Box3D.Wave+      Box3D.WaveMesh+      Box3D.WeldJoint+      Box3D.WheelJoint+      Box3D.World+  other-modules:+      Paths_Box3D+  autogen-modules:+      Paths_Box3D+  hs-source-dirs:+      src+  default-extensions:+      StrictData+      DuplicateRecordFields+      NoFieldSelectors+      OverloadedRecordDot+  ghc-options: -Wall -Wcompat -Widentities -Wincomplete-record-updates -Wincomplete-uni-patterns -Wmissing-export-lists -Wmissing-home-modules -Wpartial-fields -Wredundant-constraints+  cc-options: -std=c17+  include-dirs:+      cbits+      upstream/include+      upstream/src+  c-sources:+      cbits/box3d_shim.c+      cbits/box3d_gen_shim.c+      upstream/src/aabb.c+      upstream/src/arena_allocator.c+      upstream/src/bitset.c+      upstream/src/block_allocator.c+      upstream/src/body.c+      upstream/src/broad_phase.c+      upstream/src/capsule.c+      upstream/src/compound.c+      upstream/src/constraint_graph.c+      upstream/src/contact.c+      upstream/src/contact_solver.c+      upstream/src/convex_manifold.c+      upstream/src/core.c+      upstream/src/distance.c+      upstream/src/distance_joint.c+      upstream/src/dynamic_tree.c+      upstream/src/height_field.c+      upstream/src/hull.c+      upstream/src/id_pool.c+      upstream/src/island.c+      upstream/src/joint.c+      upstream/src/manifold.c+      upstream/src/math_functions.c+      upstream/src/mesh.c+      upstream/src/mesh_contact.c+      upstream/src/motor_joint.c+      upstream/src/mover.c+      upstream/src/name_cache.c+      upstream/src/parallel_for.c+      upstream/src/parallel_joint.c+      upstream/src/physics_world.c+      upstream/src/prismatic_joint.c+      upstream/src/recording.c+      upstream/src/recording_replay.c+      upstream/src/revolute_joint.c+      upstream/src/scheduler.c+      upstream/src/sensor.c+      upstream/src/shape.c+      upstream/src/simd.c+      upstream/src/solver.c+      upstream/src/solver_set.c+      upstream/src/sphere.c+      upstream/src/spherical_joint.c+      upstream/src/table.c+      upstream/src/timer.c+      upstream/src/triangle_manifold.c+      upstream/src/types.c+      upstream/src/weld_joint.c+      upstream/src/wheel_joint.c+      upstream/src/world_snapshot.c+  build-depends:+      base >=4.16 && <5+    , vector ==0.13.*+  default-language: GHC2021+  if !flag(assertions)+    cc-options: -DNDEBUG+  if flag(double-precision)+    cc-options: -DBOX3D_DOUBLE_PRECISION++test-suite Box3D-test+  type: exitcode-stdio-1.0+  main-is: Spec.hs+  other-modules:+      Paths_Box3D+  autogen-modules:+      Paths_Box3D+  hs-source-dirs:+      test+  default-extensions:+      StrictData+      DuplicateRecordFields+      NoFieldSelectors+      OverloadedRecordDot+  ghc-options: -Wall -Wcompat -Widentities -Wincomplete-record-updates -Wincomplete-uni-patterns -Wmissing-export-lists -Wmissing-home-modules -Wpartial-fields -Wredundant-constraints -threaded -rtsopts -with-rtsopts=-N+  build-depends:+      Box3D+    , base >=4.16 && <5+    , tasty+    , tasty-hunit+    , vector ==0.13.*+  default-language: GHC2021++benchmark Box3D-bench+  type: exitcode-stdio-1.0+  main-is: Bench.hs+  other-modules:+      Paths_Box3D+  autogen-modules:+      Paths_Box3D+  hs-source-dirs:+      bench+  default-extensions:+      StrictData+      DuplicateRecordFields+      NoFieldSelectors+      OverloadedRecordDot+  ghc-options: -Wall -Wcompat -Widentities -Wincomplete-record-updates -Wincomplete-uni-patterns -Wmissing-export-lists -Wmissing-home-modules -Wpartial-fields -Wredundant-constraints -O2+  build-depends:+      Box3D+    , base >=4.16 && <5+    , tasty-bench+    , vector ==0.13.*+  default-language: GHC2021
+ CHANGELOG.md view
@@ -0,0 +1,15 @@+# Changelog for `Box3D`++All notable changes to this project will be documented in this file.++The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/),+and this project adheres to the+[Haskell Package Versioning Policy](https://pvp.haskell.org/).++## 0.1.0.0 - 2026-11-08++Initial release, bundling Box3D from commit `2c94ad4` of+<https://github.com/dpwiz/box3d>. That tree is upstream `v0.1.0` plus 24 commits.+On top of upstream's `30c67b5` it adds a NULL guard for `destroyDebugShape`, doc+comments on `b3Profile`, `b3Counters` and the timing and hash helpers, and+API-dump tooling.
+ LICENSE view
@@ -0,0 +1,26 @@+Copyright 2026 IC Rainbow++Redistribution and use in source and binary forms, with or without+modification, are permitted provided that the following conditions are met:++1.  Redistributions of source code must retain the above copyright notice, this+    list of conditions and the following disclaimer.++2.  Redistributions in binary form must reproduce the above copyright notice,+    this list of conditions and the following disclaimer in the documentation+    and/or other materials provided with the distribution.++3.  Neither the name of the copyright holder nor the names of its contributors+    may be used to endorse or promote products derived from this software+    without specific prior written permission.++THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ README.md view
@@ -0,0 +1,121 @@+# Box3D++Low-level Haskell bindings to [Box3D](https://box2d.org/documentation3d/), Erin+Catto's 3D rigid-body physics engine.++The engine's C sources (MIT licensed) are bundled and compiled with the package.+You need a C compiler, but no system library.++This package builds commit `2c94ad4` of+[dpwiz/box3d](https://github.com/dpwiz/box3d), which is upstream `v0.1.0` plus 24+commits and reports engine version 0.2.0. Box3D is a young engine, so expect its+API to keep moving between releases.++The bindings stay close to the C API: `b3World_Step` is `Box3D.World.step`, and+each wrapper's documentation names the function it binds. Most modules are+generated from the engine headers by `boxnd-gen`, in the+[source repository](https://gitlab.com/dpwiz/box-nd).++## Example++```haskell+{-# LANGUAGE PatternSynonyms #-}++module Main (main) where++import Control.Monad (replicateM_)++import Box3D.Body (BodyDef (..), defaultBodyDef, pattern DynamicBody)+import Box3D.Body qualified as Body+import Box3D.Geometry (Sphere (..))+import Box3D.MathTypes (Pos (..), Vec3 (..), vec3Zero)+import Box3D.Shape (defaultShapeDef)+import Box3D.Shape qualified as Shape+import Box3D.World (WorldDef (..), defaultWorldDef)+import Box3D.World qualified as World++main :: IO ()+main = do+  worldDef <- defaultWorldDef+  world <- World.create worldDef{gravity = Vec3 0 (-10) 0}++  bodyDef <- defaultBodyDef+  ball <- Body.create world bodyDef{type_ = DynamicBody, position = Pos 0 10 0}+  shapeDef <- defaultShapeDef+  _ <- Shape.createSphere ball shapeDef (Sphere vec3Zero 0.5)++  replicateM_ 60 (World.step world (1 / 60) 4)+  Body.getPosition ball >>= print++  World.destroy world+```++## Modules++The modules are designed for qualified import.++| Area | Modules |+| --- | --- |+| Simulation | `World`, `Body`, `Shape`, `Contact`, `Events` |+| Joints | `Joint`, `DistanceJoint`, `FilterJoint`, `MotorJoint`, `ParallelJoint`, `PrismaticJoint`, `RevoluteJoint`, `SphericalJoint`, `WeldJoint`, `WheelJoint` |+| Geometry | `Geometry`, `Hull`, `Mesh`, `HeightField`, `Compound`, `Authoring` |+| Shape builders | `BoxMesh`, `Cone`, `Cylinder`, `Grid`, `GridMesh`, `HollowBoxMesh`, `PlatformMesh`, `Rock`, `TorusMesh`, `Wave`, `WaveMesh` |+| Queries | `Query`, `Collision`, `DynamicTree`, `Mover` |+| Callbacks and drawing | `Callbacks`, `DebugDraw`, `DebugShape` |+| Recording | `Recording`, `RecPlayer`, `Player` |+| Foundation | `Id`, `MathTypes`, `Math`, `MathFunctions`, `Base`, `UserData`, `Extra`, `Tags` |++`Box3D.Authoring` builds meshes, height fields and compounds from Haskell vectors.++`Box3D.Internal` and its submodule are exposed, but may change in any release.++## Things to know++- **Start from the defaults.** Create definitions with `default*Def` and update+  the fields you need, rather than building them from scratch. They carry an+  initialization marker, which the engine checks when `assertions` is on.+- **World positions are `Pos`.** The engine takes and returns `Pos` for places+  in the world and `WorldTransform` for a full pose, while directions, velocities+  and shape geometry stay `Vec3`. `MathTypes.toPos`, `toVec3`, `subPos` and+  `offsetPos` convert between them.+- **Enums are open.** They are newtypes over `CInt` with pattern synonyms, and the+  engine can hand back a value with no name here, so include a wildcard when you+  `case` on one.+- **Worlds are not thread-safe.** Create, step and destroy them from one thread at+  a time, and never create or destroy two worlds at once: the world table is+  global. `Callbacks.withThreadPoolTaskSystem` runs the engine's internal work in+  parallel.+- **Callback lifetimes.** The `with*` functions in `Box3D.Callbacks` free their+  callback when the action ends, so anything holding it, a world included, must+  be finished by then. Callbacks may run on the engine's worker threads.+- **Exceptions in callbacks.** `Draw` callbacks are trapped and rethrown by+  `Callbacks.draw`. `withDebugShapes` callbacks are trapped too: call+  `Callbacks.rethrowDebugShapes` after drawing, or the first exception only+  surfaces when `withDebugShapes` returns. An exception escaping any other+  callback stops the process, because it cannot unwind through C frames.+- **Events are copied.** `Box3D.Events` copies each event buffer into a vector+  that is safe to keep. The raw buffers from `Box3D.World` are only valid until+  the next step.++## Build flags++`assertions` (off by default) keeps the engine's internal assertions. They catch+invalid arguments and corrupted state, stopping the process with a trap (`SIGILL`+on x86-64), and only with this flag does `Base.setAssertFcn` ever fire. Turn it+on while developing, for example with `stack build --flag Box3D:assertions`.++`double-precision` (off by default) builds the engine's large world mode, which+stores world positions as doubles so they stay accurate far from the origin.+`Pos` then holds `Double` instead of `Float`, and everything else stays `Float`.+Code that builds positions from literals and the `MathTypes` helpers compiles+either way. Every package in a build shares this setting.++Run `stack clean Box3D` before changing either flag on an already-built package.+A flag change does not rebuild the C objects or preprocessed modules, so without+a clean the old setting silently stays in effect.++## Limits++- x86-64 and AArch64 only: ids cross the FFI as machine words, which relies on+  those calling conventions.+- Built and tested on Linux x86-64 with GHC 9.10.3.
+ bench/Bench.hs view
@@ -0,0 +1,61 @@+{-# LANGUAGE PatternSynonyms #-}++module Main (main) where++import Control.Monad (forM_)++import Test.Tasty.Bench++import Box3D.Body (BodyDef (..), defaultBodyDef, pattern DynamicBody)+import Box3D.Body qualified as Body+import Box3D.Geometry (Sphere (..))+import Box3D.Id (WorldId)+import Box3D.MathTypes+import Box3D.Shape (defaultShapeDef)+import Box3D.Shape qualified as Shape+import Box3D.World (WorldDef (..), defaultWorldDef)+import Box3D.World qualified as World++{- | Build a world holding @n@ dynamic spheres dropped onto a static ground+sphere, so that a step exercises broad phase, narrow phase and the solver.+-}+setupWorld :: Int -> IO WorldId+setupWorld n = do+  wd <- defaultWorldDef+  -- keep the pile awake: a settled world would otherwise fall asleep and+  -- the stepping benchmarks would measure an idle no-op step+  w <- World.create wd{gravity = Vec3 0 (-10) 0, enableSleep = 0}++  gd <- defaultBodyDef+  ground <- Body.create w gd+  gsd <- defaultShapeDef+  _ <- Shape.createSphere ground gsd (Sphere vec3Zero 10)++  bd <- defaultBodyDef+  sd <- defaultShapeDef+  forM_ [0 .. n - 1] $ \i -> do+    let+      col = fromIntegral (i `mod` 8)+      row = fromIntegral (i `div` 8)+      position = Pos (col - 4) (12 + row) 0+    b <- Body.create w bd{type_ = DynamicBody, position = position}+    _ <- Shape.createSphere b sd (Sphere vec3Zero 0.5)+    pure ()++  -- let the pile settle so steps hit a steady contact load+  forM_ [1 .. 120 :: Int] $ \_ -> World.step w (1 / 60) 4+  pure w++main :: IO ()+main = do+  emptyDef <- defaultWorldDef+  world64 <- setupWorld 64+  world256 <- setupWorld 256+  defaultMain+    [ bench "World.create + World.destroy" $+        whnfIO (World.create emptyDef >>= World.destroy)+    , bench "step 64 spheres" $+        whnfIO (World.step world64 (1 / 60) 4)+    , bench "step 256 spheres" $+        whnfIO (World.step world256 (1 / 60) 4)+    ]
+ cbits/box3d_gen_shim.c view
@@ -0,0 +1,805 @@+// SPDX-License-Identifier: BSD-3-Clause+//+// Generated by boxnd-gen. See box3d_gen_shim.h.++#include "box3d_gen_shim.h"++void hsg_b3World_GetBounds(b3WorldId a0, b3AABB* out)+{+	*out = b3World_GetBounds(a0);+}++void hsg_b3World_GetBodyEvents(b3WorldId a0, b3BodyEvents* out)+{+	*out = b3World_GetBodyEvents(a0);+}++void hsg_b3World_GetSensorEvents(b3WorldId a0, b3SensorEvents* out)+{+	*out = b3World_GetSensorEvents(a0);+}++void hsg_b3World_GetContactEvents(b3WorldId a0, b3ContactEvents* out)+{+	*out = b3World_GetContactEvents(a0);+}++void hsg_b3World_GetJointEvents(b3WorldId a0, b3JointEvents* out)+{+	*out = b3World_GetJointEvents(a0);+}++void hsg_b3World_OverlapAABB(b3WorldId a0, const b3AABB* a1, const b3QueryFilter* a2, b3OverlapResultFcn* a3, void* a4, b3TreeStats* out)+{+	*out = b3World_OverlapAABB(a0, *a1, *a2, a3, a4);+}++void hsg_b3World_OverlapShape(b3WorldId a0, const b3Pos* a1, const b3ShapeProxy* a2, const b3QueryFilter* a3, b3OverlapResultFcn* a4, void* a5, b3TreeStats* out)+{+	*out = b3World_OverlapShape(a0, *a1, a2, *a3, a4, a5);+}++void hsg_b3World_CastRay(b3WorldId a0, const b3Pos* a1, const b3Vec3* a2, const b3QueryFilter* a3, b3CastResultFcn* a4, void* a5, b3TreeStats* out)+{+	*out = b3World_CastRay(a0, *a1, *a2, *a3, a4, a5);+}++void hsg_b3World_CastRayClosest(b3WorldId a0, const b3Pos* a1, const b3Vec3* a2, const b3QueryFilter* a3, b3RayResult* out)+{+	*out = b3World_CastRayClosest(a0, *a1, *a2, *a3);+}++void hsg_b3World_CastShape(b3WorldId a0, const b3Pos* a1, const b3ShapeProxy* a2, const b3Vec3* a3, const b3QueryFilter* a4, b3CastResultFcn* a5, void* a6, b3TreeStats* out)+{+	*out = b3World_CastShape(a0, *a1, a2, *a3, *a4, a5, a6);+}++float hsg_b3World_CastMover(b3WorldId a0, const b3Pos* a1, const b3Capsule* a2, const b3Vec3* a3, const b3QueryFilter* a4, b3MoverFilterFcn* a5, void* a6)+{+	return b3World_CastMover(a0, *a1, a2, *a3, *a4, a5, a6);+}++void hsg_b3World_CollideMover(b3WorldId a0, const b3Pos* a1, const b3Capsule* a2, const b3QueryFilter* a3, b3PlaneResultFcn* a4, void* a5)+{+	b3World_CollideMover(a0, *a1, a2, *a3, a4, a5);+}++void hsg_b3World_SetGravity(b3WorldId a0, const b3Vec3* a1)+{+	b3World_SetGravity(a0, *a1);+}++void hsg_b3World_GetGravity(b3WorldId a0, b3Vec3* out)+{+	*out = b3World_GetGravity(a0);+}++void hsg_b3World_GetProfile(b3WorldId a0, b3Profile* out)+{+	*out = b3World_GetProfile(a0);+}++void hsg_b3World_GetCounters(b3WorldId a0, b3Counters* out)+{+	*out = b3World_GetCounters(a0);+}++void hsg_b3World_GetMaxCapacity(b3WorldId a0, b3Capacity* out)+{+	*out = b3World_GetMaxCapacity(a0);+}++void hsg_b3RecPlayer_GetInfo(const b3RecPlayer* a0, b3RecPlayerInfo* out)+{+	*out = b3RecPlayer_GetInfo(a0);+}++void hsg_b3RecPlayer_GetFrameQuery(const b3RecPlayer* a0, int a1, b3RecQueryInfo* out)+{+	*out = b3RecPlayer_GetFrameQuery(a0, a1);+}++void hsg_b3RecPlayer_GetFrameQueryHit(const b3RecPlayer* a0, int a1, int a2, b3RecQueryHit* out)+{+	*out = b3RecPlayer_GetFrameQueryHit(a0, a1, a2);+}++void hsg_b3Body_GetPosition(b3BodyId a0, b3Pos* out)+{+	*out = b3Body_GetPosition(a0);+}++void hsg_b3Body_GetRotation(b3BodyId a0, b3Quat* out)+{+	*out = b3Body_GetRotation(a0);+}++void hsg_b3Body_GetTransform(b3BodyId a0, b3WorldTransform* out)+{+	*out = b3Body_GetTransform(a0);+}++void hsg_b3Body_SetTransform(b3BodyId a0, const b3Pos* a1, const b3Quat* a2)+{+	b3Body_SetTransform(a0, *a1, *a2);+}++void hsg_b3Body_GetLocalPoint(b3BodyId a0, const b3Pos* a1, b3Vec3* out)+{+	*out = b3Body_GetLocalPoint(a0, *a1);+}++void hsg_b3Body_GetWorldPoint(b3BodyId a0, const b3Vec3* a1, b3Pos* out)+{+	*out = b3Body_GetWorldPoint(a0, *a1);+}++void hsg_b3Body_GetLocalVector(b3BodyId a0, const b3Vec3* a1, b3Vec3* out)+{+	*out = b3Body_GetLocalVector(a0, *a1);+}++void hsg_b3Body_GetWorldVector(b3BodyId a0, const b3Vec3* a1, b3Vec3* out)+{+	*out = b3Body_GetWorldVector(a0, *a1);+}++void hsg_b3Body_GetLinearVelocity(b3BodyId a0, b3Vec3* out)+{+	*out = b3Body_GetLinearVelocity(a0);+}++void hsg_b3Body_GetAngularVelocity(b3BodyId a0, b3Vec3* out)+{+	*out = b3Body_GetAngularVelocity(a0);+}++void hsg_b3Body_SetLinearVelocity(b3BodyId a0, const b3Vec3* a1)+{+	b3Body_SetLinearVelocity(a0, *a1);+}++void hsg_b3Body_SetAngularVelocity(b3BodyId a0, const b3Vec3* a1)+{+	b3Body_SetAngularVelocity(a0, *a1);+}++void hsg_b3Body_SetTargetTransform(b3BodyId a0, const b3WorldTransform* a1, float a2, bool a3)+{+	b3Body_SetTargetTransform(a0, *a1, a2, a3);+}++void hsg_b3Body_GetLocalPointVelocity(b3BodyId a0, const b3Vec3* a1, b3Vec3* out)+{+	*out = b3Body_GetLocalPointVelocity(a0, *a1);+}++void hsg_b3Body_GetWorldPointVelocity(b3BodyId a0, const b3Pos* a1, b3Vec3* out)+{+	*out = b3Body_GetWorldPointVelocity(a0, *a1);+}++void hsg_b3Body_ApplyForce(b3BodyId a0, const b3Vec3* a1, const b3Pos* a2, bool a3)+{+	b3Body_ApplyForce(a0, *a1, *a2, a3);+}++void hsg_b3Body_ApplyForceToCenter(b3BodyId a0, const b3Vec3* a1, bool a2)+{+	b3Body_ApplyForceToCenter(a0, *a1, a2);+}++void hsg_b3Body_ApplyTorque(b3BodyId a0, const b3Vec3* a1, bool a2)+{+	b3Body_ApplyTorque(a0, *a1, a2);+}++void hsg_b3Body_ApplyLinearImpulse(b3BodyId a0, const b3Vec3* a1, const b3Pos* a2, bool a3)+{+	b3Body_ApplyLinearImpulse(a0, *a1, *a2, a3);+}++void hsg_b3Body_ApplyLinearImpulseToCenter(b3BodyId a0, const b3Vec3* a1, bool a2)+{+	b3Body_ApplyLinearImpulseToCenter(a0, *a1, a2);+}++void hsg_b3Body_ApplyAngularImpulse(b3BodyId a0, const b3Vec3* a1, bool a2)+{+	b3Body_ApplyAngularImpulse(a0, *a1, a2);+}++void hsg_b3Body_GetLocalRotationalInertia(b3BodyId a0, b3Matrix3* out)+{+	*out = b3Body_GetLocalRotationalInertia(a0);+}++void hsg_b3Body_GetWorldInverseRotationalInertia(b3BodyId a0, b3Matrix3* out)+{+	*out = b3Body_GetWorldInverseRotationalInertia(a0);+}++void hsg_b3Body_GetLocalCenter(b3BodyId a0, b3Vec3* out)+{+	*out = b3Body_GetLocalCenter(a0);+}++void hsg_b3Body_GetWorldCenter(b3BodyId a0, b3Pos* out)+{+	*out = b3Body_GetWorldCenter(a0);+}++void hsg_b3Body_SetMassData(b3BodyId a0, const b3MassData* a1)+{+	b3Body_SetMassData(a0, *a1);+}++void hsg_b3Body_GetMassData(b3BodyId a0, b3MassData* out)+{+	*out = b3Body_GetMassData(a0);+}++void hsg_b3Body_SetMotionLocks(b3BodyId a0, const b3MotionLocks* a1)+{+	b3Body_SetMotionLocks(a0, *a1);+}++void hsg_b3Body_GetMotionLocks(b3BodyId a0, b3MotionLocks* out)+{+	*out = b3Body_GetMotionLocks(a0);+}++void hsg_b3Body_ComputeAABB(b3BodyId a0, b3AABB* out)+{+	*out = b3Body_ComputeAABB(a0);+}++float hsg_b3Body_GetClosestPoint(b3BodyId a0, b3Vec3* a1, const b3Vec3* a2)+{+	return b3Body_GetClosestPoint(a0, a1, *a2);+}++void hsg_b3Body_CastRay(b3BodyId a0, const b3Pos* a1, const b3Vec3* a2, const b3QueryFilter* a3, float a4, const b3WorldTransform* a5, b3BodyCastResult* out)+{+	*out = b3Body_CastRay(a0, *a1, *a2, *a3, a4, *a5);+}++void hsg_b3Body_CastShape(b3BodyId a0, const b3Pos* a1, const b3ShapeProxy* a2, const b3Vec3* a3, const b3QueryFilter* a4, float a5, bool a6, const b3WorldTransform* a7, b3BodyCastResult* out)+{+	*out = b3Body_CastShape(a0, *a1, a2, *a3, *a4, a5, a6, *a7);+}++bool hsg_b3Body_OverlapShape(b3BodyId a0, const b3Pos* a1, const b3ShapeProxy* a2, const b3QueryFilter* a3, const b3WorldTransform* a4)+{+	return b3Body_OverlapShape(a0, *a1, a2, *a3, *a4);+}++int hsg_b3Body_CollideMover(b3BodyId a0, b3BodyPlaneResult* a1, int a2, const b3Pos* a3, const b3Capsule* a4, const b3QueryFilter* a5, const b3WorldTransform* a6)+{+	return b3Body_CollideMover(a0, a1, a2, *a3, a4, *a5, *a6);+}++b3ShapeId hsg_b3CreateTransformedHullShape(b3BodyId a0, const b3ShapeDef* a1, const b3HullData* a2, const b3Transform* a3, const b3Vec3* a4)+{+	return b3CreateTransformedHullShape(a0, a1, a2, *a3, *a4);+}++b3ShapeId hsg_b3CreateMeshShape(b3BodyId a0, const b3ShapeDef* a1, const b3MeshData* a2, const b3Vec3* a3)+{+	return b3CreateMeshShape(a0, a1, a2, *a3);+}++void hsg_b3Shape_SetSurfaceMaterial(b3ShapeId a0, const b3SurfaceMaterial* a1)+{+	b3Shape_SetSurfaceMaterial(a0, *a1);+}++void hsg_b3Shape_GetSurfaceMaterial(b3ShapeId a0, b3SurfaceMaterial* out)+{+	*out = b3Shape_GetSurfaceMaterial(a0);+}++void hsg_b3Shape_SetMeshMaterial(b3ShapeId a0, const b3SurfaceMaterial* a1, int a2)+{+	b3Shape_SetMeshMaterial(a0, *a1, a2);+}++void hsg_b3Shape_GetMeshSurfaceMaterial(b3ShapeId a0, int a1, b3SurfaceMaterial* out)+{+	*out = b3Shape_GetMeshSurfaceMaterial(a0, a1);+}++void hsg_b3Shape_GetFilter(b3ShapeId a0, b3Filter* out)+{+	*out = b3Shape_GetFilter(a0);+}++void hsg_b3Shape_SetFilter(b3ShapeId a0, const b3Filter* a1, bool a2)+{+	b3Shape_SetFilter(a0, *a1, a2);+}++void hsg_b3Shape_RayCast(b3ShapeId a0, const b3Pos* a1, const b3Vec3* a2, b3WorldCastOutput* out)+{+	*out = b3Shape_RayCast(a0, *a1, *a2);+}++void hsg_b3Shape_GetSphere(b3ShapeId a0, b3Sphere* out)+{+	*out = b3Shape_GetSphere(a0);+}++void hsg_b3Shape_GetCapsule(b3ShapeId a0, b3Capsule* out)+{+	*out = b3Shape_GetCapsule(a0);+}++void hsg_b3Shape_GetMesh(b3ShapeId a0, b3Mesh* out)+{+	*out = b3Shape_GetMesh(a0);+}++void hsg_b3Shape_SetMesh(b3ShapeId a0, const b3MeshData* a1, const b3Vec3* a2)+{+	b3Shape_SetMesh(a0, a1, *a2);+}++void hsg_b3Shape_GetAABB(b3ShapeId a0, b3AABB* out)+{+	*out = b3Shape_GetAABB(a0);+}++void hsg_b3Shape_ComputeMassData(b3ShapeId a0, b3MassData* out)+{+	*out = b3Shape_ComputeMassData(a0);+}++void hsg_b3Shape_GetClosestPoint(b3ShapeId a0, const b3Vec3* a1, b3Vec3* out)+{+	*out = b3Shape_GetClosestPoint(a0, *a1);+}++void hsg_b3Shape_ApplyWind(b3ShapeId a0, const b3Vec3* a1, float a2, float a3, float a4, bool a5)+{+	b3Shape_ApplyWind(a0, *a1, a2, a3, a4, a5);+}++void hsg_b3Joint_SetLocalFrameA(b3JointId a0, const b3Transform* a1)+{+	b3Joint_SetLocalFrameA(a0, *a1);+}++void hsg_b3Joint_GetLocalFrameA(b3JointId a0, b3Transform* out)+{+	*out = b3Joint_GetLocalFrameA(a0);+}++void hsg_b3Joint_SetLocalFrameB(b3JointId a0, const b3Transform* a1)+{+	b3Joint_SetLocalFrameB(a0, *a1);+}++void hsg_b3Joint_GetLocalFrameB(b3JointId a0, b3Transform* out)+{+	*out = b3Joint_GetLocalFrameB(a0);+}++void hsg_b3Joint_GetConstraintForce(b3JointId a0, b3Vec3* out)+{+	*out = b3Joint_GetConstraintForce(a0);+}++void hsg_b3Joint_GetConstraintTorque(b3JointId a0, b3Vec3* out)+{+	*out = b3Joint_GetConstraintTorque(a0);+}++void hsg_b3MotorJoint_SetLinearVelocity(b3JointId a0, const b3Vec3* a1)+{+	b3MotorJoint_SetLinearVelocity(a0, *a1);+}++void hsg_b3MotorJoint_GetLinearVelocity(b3JointId a0, b3Vec3* out)+{+	*out = b3MotorJoint_GetLinearVelocity(a0);+}++void hsg_b3MotorJoint_SetAngularVelocity(b3JointId a0, const b3Vec3* a1)+{+	b3MotorJoint_SetAngularVelocity(a0, *a1);+}++void hsg_b3MotorJoint_GetAngularVelocity(b3JointId a0, b3Vec3* out)+{+	*out = b3MotorJoint_GetAngularVelocity(a0);+}++void hsg_b3SphericalJoint_SetTargetRotation(b3JointId a0, const b3Quat* a1)+{+	b3SphericalJoint_SetTargetRotation(a0, *a1);+}++void hsg_b3SphericalJoint_GetTargetRotation(b3JointId a0, b3Quat* out)+{+	*out = b3SphericalJoint_GetTargetRotation(a0);+}++void hsg_b3SphericalJoint_SetMotorVelocity(b3JointId a0, const b3Vec3* a1)+{+	b3SphericalJoint_SetMotorVelocity(a0, *a1);+}++void hsg_b3SphericalJoint_GetMotorVelocity(b3JointId a0, b3Vec3* out)+{+	*out = b3SphericalJoint_GetMotorVelocity(a0);+}++void hsg_b3SphericalJoint_GetMotorTorque(b3JointId a0, b3Vec3* out)+{+	*out = b3SphericalJoint_GetMotorTorque(a0);+}++bool hsg_b3Contact_IsValid(const b3ContactId* a0)+{+	return b3Contact_IsValid(*a0);+}++void hsg_b3Contact_GetData(const b3ContactId* a0, b3ContactData* out)+{+	*out = b3Contact_GetData(*a0);+}++void hsg_b3DynamicTree_Create(int a0, b3DynamicTree* out)+{+	*out = b3DynamicTree_Create(a0);+}++int hsg_b3DynamicTree_CreateProxy(b3DynamicTree* a0, const b3AABB* a1, uint64_t a2, uint64_t a3)+{+	return b3DynamicTree_CreateProxy(a0, *a1, a2, a3);+}++void hsg_b3DynamicTree_MoveProxy(b3DynamicTree* a0, int a1, const b3AABB* a2)+{+	b3DynamicTree_MoveProxy(a0, a1, *a2);+}++void hsg_b3DynamicTree_EnlargeProxy(b3DynamicTree* a0, int a1, const b3AABB* a2)+{+	b3DynamicTree_EnlargeProxy(a0, a1, *a2);+}++void hsg_b3DynamicTree_Query(const b3DynamicTree* a0, const b3AABB* a1, uint64_t a2, bool a3, b3TreeQueryCallbackFcn* a4, void* a5, b3TreeStats* out)+{+	*out = b3DynamicTree_Query(a0, *a1, a2, a3, a4, a5);+}++void hsg_b3DynamicTree_QueryClosest(const b3DynamicTree* a0, const b3Vec3* a1, uint64_t a2, bool a3, b3TreeQueryClosestCallbackFcn* a4, void* a5, float* a6, b3TreeStats* out)+{+	*out = b3DynamicTree_QueryClosest(a0, *a1, a2, a3, a4, a5, a6);+}++void hsg_b3DynamicTree_RayCast(const b3DynamicTree* a0, const b3RayCastInput* a1, uint64_t a2, bool a3, b3TreeRayCastCallbackFcn* a4, void* a5, b3TreeStats* out)+{+	*out = b3DynamicTree_RayCast(a0, a1, a2, a3, a4, a5);+}++void hsg_b3DynamicTree_BoxCast(const b3DynamicTree* a0, const b3BoxCastInput* a1, uint64_t a2, bool a3, b3TreeBoxCastCallbackFcn* a4, void* a5, b3TreeStats* out)+{+	*out = b3DynamicTree_BoxCast(a0, a1, a2, a3, a4, a5);+}++void hsg_b3DynamicTree_GetRootBounds(const b3DynamicTree* a0, b3AABB* out)+{+	*out = b3DynamicTree_GetRootBounds(a0);+}++void hsg_b3DynamicTree_Load(const char* a0, float a1, b3DynamicTree* out)+{+	*out = b3DynamicTree_Load(a0, a1);+}++b3HullData* hsg_b3CloneAndTransformHull(const b3HullData* a0, const b3Transform* a1, const b3Vec3* a2)+{+	return b3CloneAndTransformHull(a0, *a1, *a2);+}++void hsg_b3MakeCubeHull(float a0, b3BoxHull* out)+{+	*out = b3MakeCubeHull(a0);+}++void hsg_b3MakeBoxHull(float a0, float a1, float a2, b3BoxHull* out)+{+	*out = b3MakeBoxHull(a0, a1, a2);+}++void hsg_b3MakeOffsetBoxHull(float a0, float a1, float a2, const b3Vec3* a3, b3BoxHull* out)+{+	*out = b3MakeOffsetBoxHull(a0, a1, a2, *a3);+}++void hsg_b3MakeTransformedBoxHull(float a0, float a1, float a2, const b3Transform* a3, b3BoxHull* out)+{+	*out = b3MakeTransformedBoxHull(a0, a1, a2, *a3);+}++void hsg_b3MakeScaledBoxHull(const b3Vec3* a0, const b3Transform* a1, const b3Vec3* a2, b3BoxHull* out)+{+	*out = b3MakeScaledBoxHull(*a0, *a1, *a2);+}++void hsg_b3ScaleBox(b3Vec3* a0, b3Transform* a1, const b3Vec3* a2, float a3)+{+	b3ScaleBox(a0, a1, *a2, a3);+}++b3MeshData* hsg_b3CreateBoxMesh(const b3Vec3* a0, const b3Vec3* a1, bool a2)+{+	return b3CreateBoxMesh(*a0, *a1, a2);+}++b3MeshData* hsg_b3CreateHollowBoxMesh(const b3Vec3* a0, const b3Vec3* a1)+{+	return b3CreateHollowBoxMesh(*a0, *a1);+}++b3MeshData* hsg_b3CreatePlatformMesh(const b3Vec3* a0, float a1, float a2, float a3)+{+	return b3CreatePlatformMesh(*a0, a1, a2, a3);+}++b3HeightFieldData* hsg_b3CreateGrid(int a0, int a1, const b3Vec3* a2, bool a3)+{+	return b3CreateGrid(a0, a1, *a2, a3);+}++b3HeightFieldData* hsg_b3CreateWave(int a0, int a1, const b3Vec3* a2, float a3, float a4, bool a5)+{+	return b3CreateWave(a0, a1, *a2, a3, a4, a5);+}++void hsg_b3GetCompoundChild(const b3CompoundData* a0, int a1, b3ChildShape* out)+{+	*out = b3GetCompoundChild(a0, a1);+}++void hsg_b3QueryCompound(const b3CompoundData* a0, const b3AABB* a1, b3CompoundQueryFcn* a2, void* a3)+{+	b3QueryCompound(a0, *a1, a2, a3);+}++void hsg_b3GetCompoundCapsule(const b3CompoundData* a0, int a1, b3CompoundCapsule* out)+{+	*out = b3GetCompoundCapsule(a0, a1);+}++void hsg_b3GetCompoundHull(const b3CompoundData* a0, int a1, b3CompoundHull* out)+{+	*out = b3GetCompoundHull(a0, a1);+}++void hsg_b3GetCompoundMesh(const b3CompoundData* a0, int a1, b3CompoundMesh* out)+{+	*out = b3GetCompoundMesh(a0, a1);+}++void hsg_b3GetCompoundSphere(const b3CompoundData* a0, int a1, b3CompoundSphere* out)+{+	*out = b3GetCompoundSphere(a0, a1);+}++void hsg_b3ComputeSphereMass(const b3Sphere* a0, float a1, b3MassData* out)+{+	*out = b3ComputeSphereMass(a0, a1);+}++void hsg_b3ComputeCapsuleMass(const b3Capsule* a0, float a1, b3MassData* out)+{+	*out = b3ComputeCapsuleMass(a0, a1);+}++void hsg_b3ComputeHullMass(const b3HullData* a0, float a1, b3MassData* out)+{+	*out = b3ComputeHullMass(a0, a1);+}++void hsg_b3ComputeSphereAABB(const b3Sphere* a0, const b3Transform* a1, b3AABB* out)+{+	*out = b3ComputeSphereAABB(a0, *a1);+}++void hsg_b3ComputeCapsuleAABB(const b3Capsule* a0, const b3Transform* a1, b3AABB* out)+{+	*out = b3ComputeCapsuleAABB(a0, *a1);+}++void hsg_b3ComputeHullAABB(const b3HullData* a0, const b3Transform* a1, b3AABB* out)+{+	*out = b3ComputeHullAABB(a0, *a1);+}++void hsg_b3ComputeMeshAABB(const b3MeshData* a0, const b3Transform* a1, const b3Vec3* a2, b3AABB* out)+{+	*out = b3ComputeMeshAABB(a0, *a1, *a2);+}++void hsg_b3ComputeHeightFieldAABB(const b3HeightFieldData* a0, const b3Transform* a1, b3AABB* out)+{+	*out = b3ComputeHeightFieldAABB(a0, *a1);+}++void hsg_b3ComputeCompoundAABB(const b3CompoundData* a0, const b3Transform* a1, b3AABB* out)+{+	*out = b3ComputeCompoundAABB(a0, *a1);+}++bool hsg_b3OverlapCapsule(const b3Capsule* a0, const b3Transform* a1, const b3ShapeProxy* a2)+{+	return b3OverlapCapsule(a0, *a1, a2);+}++bool hsg_b3OverlapCompound(const b3CompoundData* a0, const b3Transform* a1, const b3ShapeProxy* a2)+{+	return b3OverlapCompound(a0, *a1, a2);+}++bool hsg_b3OverlapHeightField(const b3HeightFieldData* a0, const b3Transform* a1, const b3ShapeProxy* a2)+{+	return b3OverlapHeightField(a0, *a1, a2);+}++bool hsg_b3OverlapHull(const b3HullData* a0, const b3Transform* a1, const b3ShapeProxy* a2)+{+	return b3OverlapHull(a0, *a1, a2);+}++bool hsg_b3OverlapMesh(const b3Mesh* a0, const b3Transform* a1, const b3ShapeProxy* a2)+{+	return b3OverlapMesh(a0, *a1, a2);+}++bool hsg_b3OverlapSphere(const b3Sphere* a0, const b3Transform* a1, const b3ShapeProxy* a2)+{+	return b3OverlapSphere(a0, *a1, a2);+}++void hsg_b3RayCastSphere(const b3Sphere* a0, const b3RayCastInput* a1, b3CastOutput* out)+{+	*out = b3RayCastSphere(a0, a1);+}++void hsg_b3RayCastHollowSphere(const b3Sphere* a0, const b3RayCastInput* a1, b3CastOutput* out)+{+	*out = b3RayCastHollowSphere(a0, a1);+}++void hsg_b3RayCastCapsule(const b3Capsule* a0, const b3RayCastInput* a1, b3CastOutput* out)+{+	*out = b3RayCastCapsule(a0, a1);+}++void hsg_b3RayCastCompound(const b3CompoundData* a0, const b3RayCastInput* a1, b3CastOutput* out)+{+	*out = b3RayCastCompound(a0, a1);+}++void hsg_b3RayCastHull(const b3HullData* a0, const b3RayCastInput* a1, b3CastOutput* out)+{+	*out = b3RayCastHull(a0, a1);+}++void hsg_b3RayCastMesh(const b3Mesh* a0, const b3RayCastInput* a1, b3CastOutput* out)+{+	*out = b3RayCastMesh(a0, a1);+}++void hsg_b3RayCastHeightField(const b3HeightFieldData* a0, const b3RayCastInput* a1, b3CastOutput* out)+{+	*out = b3RayCastHeightField(a0, a1);+}++void hsg_b3ShapeCastSphere(const b3Sphere* a0, const b3ShapeCastInput* a1, b3CastOutput* out)+{+	*out = b3ShapeCastSphere(a0, a1);+}++void hsg_b3ShapeCastCapsule(const b3Capsule* a0, const b3ShapeCastInput* a1, b3CastOutput* out)+{+	*out = b3ShapeCastCapsule(a0, a1);+}++void hsg_b3ShapeCastCompound(const b3CompoundData* a0, const b3ShapeCastInput* a1, b3CastOutput* out)+{+	*out = b3ShapeCastCompound(a0, a1);+}++void hsg_b3ShapeCastHull(const b3HullData* a0, const b3ShapeCastInput* a1, b3CastOutput* out)+{+	*out = b3ShapeCastHull(a0, a1);+}++void hsg_b3ShapeCastMesh(const b3Mesh* a0, const b3ShapeCastInput* a1, b3CastOutput* out)+{+	*out = b3ShapeCastMesh(a0, a1);+}++void hsg_b3ShapeCastHeightField(const b3HeightFieldData* a0, const b3ShapeCastInput* a1, b3CastOutput* out)+{+	*out = b3ShapeCastHeightField(a0, a1);+}++void hsg_b3QueryMesh(const b3Mesh* a0, const b3AABB* a1, b3MeshQueryFcn* a2, void* a3)+{+	b3QueryMesh(a0, *a1, a2, a3);+}++void hsg_b3QueryHeightField(const b3HeightFieldData* a0, const b3AABB* a1, b3MeshQueryFcn* a2, void* a3)+{+	b3QueryHeightField(a0, *a1, a2, a3);+}++void hsg_b3ShapeDistance(const b3DistanceInput* a0, b3SimplexCache* a1, b3Simplex* a2, int a3, b3DistanceOutput* out)+{+	*out = b3ShapeDistance(a0, a1, a2, a3);+}++void hsg_b3ShapeCast(const b3ShapeCastPairInput* a0, b3CastOutput* out)+{+	*out = b3ShapeCast(a0);+}++void hsg_b3GetSweepTransform(const b3Sweep* a0, float a1, b3Transform* out)+{+	*out = b3GetSweepTransform(a0, a1);+}++void hsg_b3TimeOfImpact(const b3TOIInput* a0, b3TOIOutput* out)+{+	*out = b3TimeOfImpact(a0);+}++void hsg_b3CollideSpheres(b3LocalManifold* a0, int a1, const b3Sphere* a2, const b3Sphere* a3, const b3Transform* a4)+{+	b3CollideSpheres(a0, a1, a2, a3, *a4);+}++void hsg_b3CollideCapsuleAndSphere(b3LocalManifold* a0, int a1, const b3Capsule* a2, const b3Sphere* a3, const b3Transform* a4)+{+	b3CollideCapsuleAndSphere(a0, a1, a2, a3, *a4);+}++void hsg_b3CollideHullAndSphere(b3LocalManifold* a0, int a1, const b3HullData* a2, const b3Sphere* a3, const b3Transform* a4, b3SimplexCache* a5)+{+	b3CollideHullAndSphere(a0, a1, a2, a3, *a4, a5);+}++void hsg_b3CollideCapsules(b3LocalManifold* a0, int a1, const b3Capsule* a2, const b3Capsule* a3, const b3Transform* a4)+{+	b3CollideCapsules(a0, a1, a2, a3, *a4);+}++void hsg_b3CollideHullAndCapsule(b3LocalManifold* a0, int a1, const b3HullData* a2, const b3Capsule* a3, const b3Transform* a4, b3SimplexCache* a5)+{+	b3CollideHullAndCapsule(a0, a1, a2, a3, *a4, a5);+}++void hsg_b3CollideHulls(b3LocalManifold* a0, int a1, const b3HullData* a2, const b3HullData* a3, const b3Transform* a4, b3SATCache* a5)+{+	b3CollideHulls(a0, a1, a2, a3, *a4, a5);+}++void hsg_b3CollideTriangleAndHull(b3LocalManifold* a0, int a1, const b3Vec3* a2, const b3Vec3* a3, const b3Vec3* a4, int a5, const b3HullData* a6, b3SATCache* a7, bool a8)+{+	b3CollideTriangleAndHull(a0, a1, *a2, *a3, *a4, a5, a6, a7, a8);+}++void hsg_b3SolvePlanes(const b3Vec3* a0, b3CollisionPlane* a1, int a2, b3PlaneSolverResult* out)+{+	*out = b3SolvePlanes(*a0, a1, a2);+}++void hsg_b3ClipVector(const b3Vec3* a0, const b3CollisionPlane* a1, int a2, b3Vec3* out)+{+	*out = b3ClipVector(*a0, a1, a2);+}
+ cbits/box3d_gen_shim.h view
@@ -0,0 +1,172 @@+// SPDX-License-Identifier: BSD-3-Clause+//+// Generated by boxnd-gen. By-value struct arguments/returns of the+// Box3D API are wrapped here as pointers (GHC's FFI cannot pass+// structs by value). A returned struct is written through a trailing out+// pointer; a by-value struct argument is taken through a const in pointer.+// Prefix hsg_ keeps these distinct from the hand-written hs_ shim.++#pragma once++#include "box3d/box3d.h"++void hsg_b3World_GetBounds(b3WorldId a0, b3AABB* out);+void hsg_b3World_GetBodyEvents(b3WorldId a0, b3BodyEvents* out);+void hsg_b3World_GetSensorEvents(b3WorldId a0, b3SensorEvents* out);+void hsg_b3World_GetContactEvents(b3WorldId a0, b3ContactEvents* out);+void hsg_b3World_GetJointEvents(b3WorldId a0, b3JointEvents* out);+void hsg_b3World_OverlapAABB(b3WorldId a0, const b3AABB* a1, const b3QueryFilter* a2, b3OverlapResultFcn* a3, void* a4, b3TreeStats* out);+void hsg_b3World_OverlapShape(b3WorldId a0, const b3Pos* a1, const b3ShapeProxy* a2, const b3QueryFilter* a3, b3OverlapResultFcn* a4, void* a5, b3TreeStats* out);+void hsg_b3World_CastRay(b3WorldId a0, const b3Pos* a1, const b3Vec3* a2, const b3QueryFilter* a3, b3CastResultFcn* a4, void* a5, b3TreeStats* out);+void hsg_b3World_CastRayClosest(b3WorldId a0, const b3Pos* a1, const b3Vec3* a2, const b3QueryFilter* a3, b3RayResult* out);+void hsg_b3World_CastShape(b3WorldId a0, const b3Pos* a1, const b3ShapeProxy* a2, const b3Vec3* a3, const b3QueryFilter* a4, b3CastResultFcn* a5, void* a6, b3TreeStats* out);+float hsg_b3World_CastMover(b3WorldId a0, const b3Pos* a1, const b3Capsule* a2, const b3Vec3* a3, const b3QueryFilter* a4, b3MoverFilterFcn* a5, void* a6);+void hsg_b3World_CollideMover(b3WorldId a0, const b3Pos* a1, const b3Capsule* a2, const b3QueryFilter* a3, b3PlaneResultFcn* a4, void* a5);+void hsg_b3World_SetGravity(b3WorldId a0, const b3Vec3* a1);+void hsg_b3World_GetGravity(b3WorldId a0, b3Vec3* out);+void hsg_b3World_GetProfile(b3WorldId a0, b3Profile* out);+void hsg_b3World_GetCounters(b3WorldId a0, b3Counters* out);+void hsg_b3World_GetMaxCapacity(b3WorldId a0, b3Capacity* out);+void hsg_b3RecPlayer_GetInfo(const b3RecPlayer* a0, b3RecPlayerInfo* out);+void hsg_b3RecPlayer_GetFrameQuery(const b3RecPlayer* a0, int a1, b3RecQueryInfo* out);+void hsg_b3RecPlayer_GetFrameQueryHit(const b3RecPlayer* a0, int a1, int a2, b3RecQueryHit* out);+void hsg_b3Body_GetPosition(b3BodyId a0, b3Pos* out);+void hsg_b3Body_GetRotation(b3BodyId a0, b3Quat* out);+void hsg_b3Body_GetTransform(b3BodyId a0, b3WorldTransform* out);+void hsg_b3Body_SetTransform(b3BodyId a0, const b3Pos* a1, const b3Quat* a2);+void hsg_b3Body_GetLocalPoint(b3BodyId a0, const b3Pos* a1, b3Vec3* out);+void hsg_b3Body_GetWorldPoint(b3BodyId a0, const b3Vec3* a1, b3Pos* out);+void hsg_b3Body_GetLocalVector(b3BodyId a0, const b3Vec3* a1, b3Vec3* out);+void hsg_b3Body_GetWorldVector(b3BodyId a0, const b3Vec3* a1, b3Vec3* out);+void hsg_b3Body_GetLinearVelocity(b3BodyId a0, b3Vec3* out);+void hsg_b3Body_GetAngularVelocity(b3BodyId a0, b3Vec3* out);+void hsg_b3Body_SetLinearVelocity(b3BodyId a0, const b3Vec3* a1);+void hsg_b3Body_SetAngularVelocity(b3BodyId a0, const b3Vec3* a1);+void hsg_b3Body_SetTargetTransform(b3BodyId a0, const b3WorldTransform* a1, float a2, bool a3);+void hsg_b3Body_GetLocalPointVelocity(b3BodyId a0, const b3Vec3* a1, b3Vec3* out);+void hsg_b3Body_GetWorldPointVelocity(b3BodyId a0, const b3Pos* a1, b3Vec3* out);+void hsg_b3Body_ApplyForce(b3BodyId a0, const b3Vec3* a1, const b3Pos* a2, bool a3);+void hsg_b3Body_ApplyForceToCenter(b3BodyId a0, const b3Vec3* a1, bool a2);+void hsg_b3Body_ApplyTorque(b3BodyId a0, const b3Vec3* a1, bool a2);+void hsg_b3Body_ApplyLinearImpulse(b3BodyId a0, const b3Vec3* a1, const b3Pos* a2, bool a3);+void hsg_b3Body_ApplyLinearImpulseToCenter(b3BodyId a0, const b3Vec3* a1, bool a2);+void hsg_b3Body_ApplyAngularImpulse(b3BodyId a0, const b3Vec3* a1, bool a2);+void hsg_b3Body_GetLocalRotationalInertia(b3BodyId a0, b3Matrix3* out);+void hsg_b3Body_GetWorldInverseRotationalInertia(b3BodyId a0, b3Matrix3* out);+void hsg_b3Body_GetLocalCenter(b3BodyId a0, b3Vec3* out);+void hsg_b3Body_GetWorldCenter(b3BodyId a0, b3Pos* out);+void hsg_b3Body_SetMassData(b3BodyId a0, const b3MassData* a1);+void hsg_b3Body_GetMassData(b3BodyId a0, b3MassData* out);+void hsg_b3Body_SetMotionLocks(b3BodyId a0, const b3MotionLocks* a1);+void hsg_b3Body_GetMotionLocks(b3BodyId a0, b3MotionLocks* out);+void hsg_b3Body_ComputeAABB(b3BodyId a0, b3AABB* out);+float hsg_b3Body_GetClosestPoint(b3BodyId a0, b3Vec3* a1, const b3Vec3* a2);+void hsg_b3Body_CastRay(b3BodyId a0, const b3Pos* a1, const b3Vec3* a2, const b3QueryFilter* a3, float a4, const b3WorldTransform* a5, b3BodyCastResult* out);+void hsg_b3Body_CastShape(b3BodyId a0, const b3Pos* a1, const b3ShapeProxy* a2, const b3Vec3* a3, const b3QueryFilter* a4, float a5, bool a6, const b3WorldTransform* a7, b3BodyCastResult* out);+bool hsg_b3Body_OverlapShape(b3BodyId a0, const b3Pos* a1, const b3ShapeProxy* a2, const b3QueryFilter* a3, const b3WorldTransform* a4);+int hsg_b3Body_CollideMover(b3BodyId a0, b3BodyPlaneResult* a1, int a2, const b3Pos* a3, const b3Capsule* a4, const b3QueryFilter* a5, const b3WorldTransform* a6);+b3ShapeId hsg_b3CreateTransformedHullShape(b3BodyId a0, const b3ShapeDef* a1, const b3HullData* a2, const b3Transform* a3, const b3Vec3* a4);+b3ShapeId hsg_b3CreateMeshShape(b3BodyId a0, const b3ShapeDef* a1, const b3MeshData* a2, const b3Vec3* a3);+void hsg_b3Shape_SetSurfaceMaterial(b3ShapeId a0, const b3SurfaceMaterial* a1);+void hsg_b3Shape_GetSurfaceMaterial(b3ShapeId a0, b3SurfaceMaterial* out);+void hsg_b3Shape_SetMeshMaterial(b3ShapeId a0, const b3SurfaceMaterial* a1, int a2);+void hsg_b3Shape_GetMeshSurfaceMaterial(b3ShapeId a0, int a1, b3SurfaceMaterial* out);+void hsg_b3Shape_GetFilter(b3ShapeId a0, b3Filter* out);+void hsg_b3Shape_SetFilter(b3ShapeId a0, const b3Filter* a1, bool a2);+void hsg_b3Shape_RayCast(b3ShapeId a0, const b3Pos* a1, const b3Vec3* a2, b3WorldCastOutput* out);+void hsg_b3Shape_GetSphere(b3ShapeId a0, b3Sphere* out);+void hsg_b3Shape_GetCapsule(b3ShapeId a0, b3Capsule* out);+void hsg_b3Shape_GetMesh(b3ShapeId a0, b3Mesh* out);+void hsg_b3Shape_SetMesh(b3ShapeId a0, const b3MeshData* a1, const b3Vec3* a2);+void hsg_b3Shape_GetAABB(b3ShapeId a0, b3AABB* out);+void hsg_b3Shape_ComputeMassData(b3ShapeId a0, b3MassData* out);+void hsg_b3Shape_GetClosestPoint(b3ShapeId a0, const b3Vec3* a1, b3Vec3* out);+void hsg_b3Shape_ApplyWind(b3ShapeId a0, const b3Vec3* a1, float a2, float a3, float a4, bool a5);+void hsg_b3Joint_SetLocalFrameA(b3JointId a0, const b3Transform* a1);+void hsg_b3Joint_GetLocalFrameA(b3JointId a0, b3Transform* out);+void hsg_b3Joint_SetLocalFrameB(b3JointId a0, const b3Transform* a1);+void hsg_b3Joint_GetLocalFrameB(b3JointId a0, b3Transform* out);+void hsg_b3Joint_GetConstraintForce(b3JointId a0, b3Vec3* out);+void hsg_b3Joint_GetConstraintTorque(b3JointId a0, b3Vec3* out);+void hsg_b3MotorJoint_SetLinearVelocity(b3JointId a0, const b3Vec3* a1);+void hsg_b3MotorJoint_GetLinearVelocity(b3JointId a0, b3Vec3* out);+void hsg_b3MotorJoint_SetAngularVelocity(b3JointId a0, const b3Vec3* a1);+void hsg_b3MotorJoint_GetAngularVelocity(b3JointId a0, b3Vec3* out);+void hsg_b3SphericalJoint_SetTargetRotation(b3JointId a0, const b3Quat* a1);+void hsg_b3SphericalJoint_GetTargetRotation(b3JointId a0, b3Quat* out);+void hsg_b3SphericalJoint_SetMotorVelocity(b3JointId a0, const b3Vec3* a1);+void hsg_b3SphericalJoint_GetMotorVelocity(b3JointId a0, b3Vec3* out);+void hsg_b3SphericalJoint_GetMotorTorque(b3JointId a0, b3Vec3* out);+bool hsg_b3Contact_IsValid(const b3ContactId* a0);+void hsg_b3Contact_GetData(const b3ContactId* a0, b3ContactData* out);+void hsg_b3DynamicTree_Create(int a0, b3DynamicTree* out);+int hsg_b3DynamicTree_CreateProxy(b3DynamicTree* a0, const b3AABB* a1, uint64_t a2, uint64_t a3);+void hsg_b3DynamicTree_MoveProxy(b3DynamicTree* a0, int a1, const b3AABB* a2);+void hsg_b3DynamicTree_EnlargeProxy(b3DynamicTree* a0, int a1, const b3AABB* a2);+void hsg_b3DynamicTree_Query(const b3DynamicTree* a0, const b3AABB* a1, uint64_t a2, bool a3, b3TreeQueryCallbackFcn* a4, void* a5, b3TreeStats* out);+void hsg_b3DynamicTree_QueryClosest(const b3DynamicTree* a0, const b3Vec3* a1, uint64_t a2, bool a3, b3TreeQueryClosestCallbackFcn* a4, void* a5, float* a6, b3TreeStats* out);+void hsg_b3DynamicTree_RayCast(const b3DynamicTree* a0, const b3RayCastInput* a1, uint64_t a2, bool a3, b3TreeRayCastCallbackFcn* a4, void* a5, b3TreeStats* out);+void hsg_b3DynamicTree_BoxCast(const b3DynamicTree* a0, const b3BoxCastInput* a1, uint64_t a2, bool a3, b3TreeBoxCastCallbackFcn* a4, void* a5, b3TreeStats* out);+void hsg_b3DynamicTree_GetRootBounds(const b3DynamicTree* a0, b3AABB* out);+void hsg_b3DynamicTree_Load(const char* a0, float a1, b3DynamicTree* out);+b3HullData* hsg_b3CloneAndTransformHull(const b3HullData* a0, const b3Transform* a1, const b3Vec3* a2);+void hsg_b3MakeCubeHull(float a0, b3BoxHull* out);+void hsg_b3MakeBoxHull(float a0, float a1, float a2, b3BoxHull* out);+void hsg_b3MakeOffsetBoxHull(float a0, float a1, float a2, const b3Vec3* a3, b3BoxHull* out);+void hsg_b3MakeTransformedBoxHull(float a0, float a1, float a2, const b3Transform* a3, b3BoxHull* out);+void hsg_b3MakeScaledBoxHull(const b3Vec3* a0, const b3Transform* a1, const b3Vec3* a2, b3BoxHull* out);+void hsg_b3ScaleBox(b3Vec3* a0, b3Transform* a1, const b3Vec3* a2, float a3);+b3MeshData* hsg_b3CreateBoxMesh(const b3Vec3* a0, const b3Vec3* a1, bool a2);+b3MeshData* hsg_b3CreateHollowBoxMesh(const b3Vec3* a0, const b3Vec3* a1);+b3MeshData* hsg_b3CreatePlatformMesh(const b3Vec3* a0, float a1, float a2, float a3);+b3HeightFieldData* hsg_b3CreateGrid(int a0, int a1, const b3Vec3* a2, bool a3);+b3HeightFieldData* hsg_b3CreateWave(int a0, int a1, const b3Vec3* a2, float a3, float a4, bool a5);+void hsg_b3GetCompoundChild(const b3CompoundData* a0, int a1, b3ChildShape* out);+void hsg_b3QueryCompound(const b3CompoundData* a0, const b3AABB* a1, b3CompoundQueryFcn* a2, void* a3);+void hsg_b3GetCompoundCapsule(const b3CompoundData* a0, int a1, b3CompoundCapsule* out);+void hsg_b3GetCompoundHull(const b3CompoundData* a0, int a1, b3CompoundHull* out);+void hsg_b3GetCompoundMesh(const b3CompoundData* a0, int a1, b3CompoundMesh* out);+void hsg_b3GetCompoundSphere(const b3CompoundData* a0, int a1, b3CompoundSphere* out);+void hsg_b3ComputeSphereMass(const b3Sphere* a0, float a1, b3MassData* out);+void hsg_b3ComputeCapsuleMass(const b3Capsule* a0, float a1, b3MassData* out);+void hsg_b3ComputeHullMass(const b3HullData* a0, float a1, b3MassData* out);+void hsg_b3ComputeSphereAABB(const b3Sphere* a0, const b3Transform* a1, b3AABB* out);+void hsg_b3ComputeCapsuleAABB(const b3Capsule* a0, const b3Transform* a1, b3AABB* out);+void hsg_b3ComputeHullAABB(const b3HullData* a0, const b3Transform* a1, b3AABB* out);+void hsg_b3ComputeMeshAABB(const b3MeshData* a0, const b3Transform* a1, const b3Vec3* a2, b3AABB* out);+void hsg_b3ComputeHeightFieldAABB(const b3HeightFieldData* a0, const b3Transform* a1, b3AABB* out);+void hsg_b3ComputeCompoundAABB(const b3CompoundData* a0, const b3Transform* a1, b3AABB* out);+bool hsg_b3OverlapCapsule(const b3Capsule* a0, const b3Transform* a1, const b3ShapeProxy* a2);+bool hsg_b3OverlapCompound(const b3CompoundData* a0, const b3Transform* a1, const b3ShapeProxy* a2);+bool hsg_b3OverlapHeightField(const b3HeightFieldData* a0, const b3Transform* a1, const b3ShapeProxy* a2);+bool hsg_b3OverlapHull(const b3HullData* a0, const b3Transform* a1, const b3ShapeProxy* a2);+bool hsg_b3OverlapMesh(const b3Mesh* a0, const b3Transform* a1, const b3ShapeProxy* a2);+bool hsg_b3OverlapSphere(const b3Sphere* a0, const b3Transform* a1, const b3ShapeProxy* a2);+void hsg_b3RayCastSphere(const b3Sphere* a0, const b3RayCastInput* a1, b3CastOutput* out);+void hsg_b3RayCastHollowSphere(const b3Sphere* a0, const b3RayCastInput* a1, b3CastOutput* out);+void hsg_b3RayCastCapsule(const b3Capsule* a0, const b3RayCastInput* a1, b3CastOutput* out);+void hsg_b3RayCastCompound(const b3CompoundData* a0, const b3RayCastInput* a1, b3CastOutput* out);+void hsg_b3RayCastHull(const b3HullData* a0, const b3RayCastInput* a1, b3CastOutput* out);+void hsg_b3RayCastMesh(const b3Mesh* a0, const b3RayCastInput* a1, b3CastOutput* out);+void hsg_b3RayCastHeightField(const b3HeightFieldData* a0, const b3RayCastInput* a1, b3CastOutput* out);+void hsg_b3ShapeCastSphere(const b3Sphere* a0, const b3ShapeCastInput* a1, b3CastOutput* out);+void hsg_b3ShapeCastCapsule(const b3Capsule* a0, const b3ShapeCastInput* a1, b3CastOutput* out);+void hsg_b3ShapeCastCompound(const b3CompoundData* a0, const b3ShapeCastInput* a1, b3CastOutput* out);+void hsg_b3ShapeCastHull(const b3HullData* a0, const b3ShapeCastInput* a1, b3CastOutput* out);+void hsg_b3ShapeCastMesh(const b3Mesh* a0, const b3ShapeCastInput* a1, b3CastOutput* out);+void hsg_b3ShapeCastHeightField(const b3HeightFieldData* a0, const b3ShapeCastInput* a1, b3CastOutput* out);+void hsg_b3QueryMesh(const b3Mesh* a0, const b3AABB* a1, b3MeshQueryFcn* a2, void* a3);+void hsg_b3QueryHeightField(const b3HeightFieldData* a0, const b3AABB* a1, b3MeshQueryFcn* a2, void* a3);+void hsg_b3ShapeDistance(const b3DistanceInput* a0, b3SimplexCache* a1, b3Simplex* a2, int a3, b3DistanceOutput* out);+void hsg_b3ShapeCast(const b3ShapeCastPairInput* a0, b3CastOutput* out);+void hsg_b3GetSweepTransform(const b3Sweep* a0, float a1, b3Transform* out);+void hsg_b3TimeOfImpact(const b3TOIInput* a0, b3TOIOutput* out);+void hsg_b3CollideSpheres(b3LocalManifold* a0, int a1, const b3Sphere* a2, const b3Sphere* a3, const b3Transform* a4);+void hsg_b3CollideCapsuleAndSphere(b3LocalManifold* a0, int a1, const b3Capsule* a2, const b3Sphere* a3, const b3Transform* a4);+void hsg_b3CollideHullAndSphere(b3LocalManifold* a0, int a1, const b3HullData* a2, const b3Sphere* a3, const b3Transform* a4, b3SimplexCache* a5);+void hsg_b3CollideCapsules(b3LocalManifold* a0, int a1, const b3Capsule* a2, const b3Capsule* a3, const b3Transform* a4);+void hsg_b3CollideHullAndCapsule(b3LocalManifold* a0, int a1, const b3HullData* a2, const b3Capsule* a3, const b3Transform* a4, b3SimplexCache* a5);+void hsg_b3CollideHulls(b3LocalManifold* a0, int a1, const b3HullData* a2, const b3HullData* a3, const b3Transform* a4, b3SATCache* a5);+void hsg_b3CollideTriangleAndHull(b3LocalManifold* a0, int a1, const b3Vec3* a2, const b3Vec3* a3, const b3Vec3* a4, int a5, const b3HullData* a6, b3SATCache* a7, bool a8);+void hsg_b3SolvePlanes(const b3Vec3* a0, b3CollisionPlane* a1, int a2, b3PlaneSolverResult* out);+void hsg_b3ClipVector(const b3Vec3* a0, const b3CollisionPlane* a1, int a2, b3Vec3* out);
+ cbits/box3d_shim.c view
@@ -0,0 +1,341 @@+// SPDX-License-Identifier: BSD-3-Clause+//+// See box3d_shim.h for the rationale behind this file.++#include "box3d_shim.h"++// The Haskell bindings pass ids by value as Word32/Word64; that pun is only+// sound while the ids stay register-sized integer structs.+_Static_assert(sizeof(b3WorldId) == 4, "b3WorldId is passed as Word32");+_Static_assert(sizeof(b3BodyId) == 8, "b3BodyId is passed as Word64");+_Static_assert(sizeof(b3ShapeId) == 8, "b3ShapeId is passed as Word64");+_Static_assert(sizeof(b3JointId) == 8, "b3JointId is passed as Word64");++// base.h -----------------------------------------------------------------++void hs_b3GetVersion(b3Version* out)+{+	*out = b3GetVersion();+}++// math_functions.h -------------------------------------------------------++bool hs_b3IsValidVec3(const b3Vec3* a)+{+	return b3IsValidVec3(*a);+}++bool hs_b3IsValidQuat(const b3Quat* q)+{+	return b3IsValidQuat(*q);+}++bool hs_b3IsValidTransform(const b3Transform* a)+{+	return b3IsValidTransform(*a);+}++bool hs_b3IsValidMatrix3(const b3Matrix3* a)+{+	return b3IsValidMatrix3(*a);+}++bool hs_b3IsValidAABB(const b3AABB* a)+{+	return b3IsValidAABB(*a);+}++bool hs_b3IsValidPlane(const b3Plane* a)+{+	return b3IsValidPlane(*a);+}++void hs_b3ComputeCosSin(float radians, b3CosSin* out)+{+	*out = b3ComputeCosSin(radians);+}++void hs_b3ComputeQuatBetweenUnitVectors(const b3Vec3* v1, const b3Vec3* v2, b3Quat* out)+{+	*out = b3ComputeQuatBetweenUnitVectors(*v1, *v2);+}++// types.h: default definition constructors --------------------------------++void hs_b3DefaultWorldDef(b3WorldDef* out)+{+	*out = b3DefaultWorldDef();+}++void hs_b3DefaultBodyDef(b3BodyDef* out)+{+	*out = b3DefaultBodyDef();+}++void hs_b3DefaultShapeDef(b3ShapeDef* out)+{+	*out = b3DefaultShapeDef();+}++void hs_b3DefaultFilter(b3Filter* out)+{+	*out = b3DefaultFilter();+}++void hs_b3DefaultSurfaceMaterial(b3SurfaceMaterial* out)+{+	*out = b3DefaultSurfaceMaterial();+}++void hs_b3DefaultQueryFilter(b3QueryFilter* out)+{+	*out = b3DefaultQueryFilter();+}++void hs_b3DefaultExplosionDef(b3ExplosionDef* out)+{+	*out = b3DefaultExplosionDef();+}++void hs_b3DefaultDebugDraw(b3DebugDraw* out)+{+	*out = b3DefaultDebugDraw();+}++// types.h: joint definition defaults ---------------------------------------++void hs_b3DefaultDistanceJointDef(b3DistanceJointDef* out)+{+	*out = b3DefaultDistanceJointDef();+}++void hs_b3DefaultMotorJointDef(b3MotorJointDef* out)+{+	*out = b3DefaultMotorJointDef();+}++void hs_b3DefaultParallelJointDef(b3ParallelJointDef* out)+{+	*out = b3DefaultParallelJointDef();+}++void hs_b3DefaultPrismaticJointDef(b3PrismaticJointDef* out)+{+	*out = b3DefaultPrismaticJointDef();+}++void hs_b3DefaultRevoluteJointDef(b3RevoluteJointDef* out)+{+	*out = b3DefaultRevoluteJointDef();+}++void hs_b3DefaultSphericalJointDef(b3SphericalJointDef* out)+{+	*out = b3DefaultSphericalJointDef();+}++void hs_b3DefaultWeldJointDef(b3WeldJointDef* out)+{+	*out = b3DefaultWeldJointDef();+}++void hs_b3DefaultWheelJointDef(b3WheelJointDef* out)+{+	*out = b3DefaultWheelJointDef();+}++void hs_b3DefaultFilterJointDef(b3FilterJointDef* out)+{+	*out = b3DefaultFilterJointDef();+}+++// Callback trampolines ------------------------------------------------------+// GHC callbacks cannot receive structs by value, so these fixed C functions+// take the by-value b3Pos/b3Vec3 arguments and forward pointers to a+// Haskell-wrapped function carried in the context slot.++typedef float (*hs_b3CastResultHsFcn)(b3ShapeId shapeId, const b3Pos* point, const b3Vec3* normal, float fraction,+									  uint64_t userMaterialId, int triangleIndex, int childIndex, void* context);++typedef bool (*hs_b3PreSolveHsFcn)(b3ShapeId shapeIdA, b3ShapeId shapeIdB, const b3Pos* point, const b3Vec3* normal,+								   void* context);++float hs_b3CastResultTrampoline(b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId,+								int triangleIndex, int childIndex, void* context)+{+	hs_b3Closure* c = context;+	return ((hs_b3CastResultHsFcn)c->fn)(shapeId, &point, &normal, fraction, userMaterialId, triangleIndex, childIndex,+										 c->context);+}++bool hs_b3PreSolveTrampoline(b3ShapeId shapeIdA, b3ShapeId shapeIdB, b3Pos point, b3Vec3 normal, void* context)+{+	hs_b3Closure* c = context;+	return ((hs_b3PreSolveHsFcn)c->fn)(shapeIdA, shapeIdB, &point, &normal, c->context);+}++typedef bool (*hs_b3MeshQueryHsFcn)(const b3Vec3* a, const b3Vec3* b, const b3Vec3* c, int triangleIndex,+									void* context);++bool hs_b3MeshQueryTrampoline(b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleIndex, void* context)+{+	hs_b3Closure* cl = context;+	return ((hs_b3MeshQueryHsFcn)cl->fn)(&a, &b, &c, triangleIndex, cl->context);+}++// b3DebugDraw trampolines ---------------------------------------------------+// The Haskell side fills hs_b3DrawClosure field by field through hsc2hs, so+// the struct below is the single definition of its layout. A NULL slot means+// the renderer left that primitive unset; skipping it here keeps the call+// from entering Haskell at all.++typedef void (*hs_b3DrawShapeHsFcn)(void* userShape, const b3WorldTransform* transform, b3HexColor color,+									void* context);+typedef void (*hs_b3DrawSegmentHsFcn)(const b3Pos* p1, const b3Pos* p2, b3HexColor color, void* context);+typedef void (*hs_b3DrawTransformHsFcn)(const b3WorldTransform* transform, void* context);+typedef void (*hs_b3DrawPointHsFcn)(const b3Pos* p, float size, b3HexColor color, void* context);+typedef void (*hs_b3DrawSphereHsFcn)(const b3Pos* p, float radius, b3HexColor color, float alpha, void* context);+typedef void (*hs_b3DrawCapsuleHsFcn)(const b3Pos* p1, const b3Pos* p2, float radius, b3HexColor color, float alpha,+									  void* context);+typedef void (*hs_b3DrawBoundsHsFcn)(const b3AABB* aabb, b3HexColor color, void* context);+typedef void (*hs_b3DrawBoxHsFcn)(const b3Vec3* extents, const b3WorldTransform* transform, b3HexColor color,+								  void* context);+typedef void (*hs_b3DrawStringHsFcn)(const b3Pos* p, const char* s, b3HexColor color, void* context);++void hs_b3DrawShapeTrampoline(void* userShape, b3WorldTransform transform, b3HexColor color, void* context)+{+	hs_b3DrawClosure* c = context;+	if (c->shape != NULL)+	{+		((hs_b3DrawShapeHsFcn)c->shape)(userShape, &transform, color, c->context);+	}+}++void hs_b3DrawSegmentTrampoline(b3Pos p1, b3Pos p2, b3HexColor color, void* context)+{+	hs_b3DrawClosure* c = context;+	if (c->segment != NULL)+	{+		((hs_b3DrawSegmentHsFcn)c->segment)(&p1, &p2, color, c->context);+	}+}++void hs_b3DrawTransformTrampoline(b3WorldTransform transform, void* context)+{+	hs_b3DrawClosure* c = context;+	if (c->transform != NULL)+	{+		((hs_b3DrawTransformHsFcn)c->transform)(&transform, c->context);+	}+}++void hs_b3DrawPointTrampoline(b3Pos p, float size, b3HexColor color, void* context)+{+	hs_b3DrawClosure* c = context;+	if (c->point != NULL)+	{+		((hs_b3DrawPointHsFcn)c->point)(&p, size, color, c->context);+	}+}++void hs_b3DrawSphereTrampoline(b3Pos p, float radius, b3HexColor color, float alpha, void* context)+{+	hs_b3DrawClosure* c = context;+	if (c->sphere != NULL)+	{+		((hs_b3DrawSphereHsFcn)c->sphere)(&p, radius, color, alpha, c->context);+	}+}++void hs_b3DrawCapsuleTrampoline(b3Pos p1, b3Pos p2, float radius, b3HexColor color, float alpha, void* context)+{+	hs_b3DrawClosure* c = context;+	if (c->capsule != NULL)+	{+		((hs_b3DrawCapsuleHsFcn)c->capsule)(&p1, &p2, radius, color, alpha, c->context);+	}+}++void hs_b3DrawBoundsTrampoline(b3AABB aabb, b3HexColor color, void* context)+{+	hs_b3DrawClosure* c = context;+	if (c->bounds != NULL)+	{+		((hs_b3DrawBoundsHsFcn)c->bounds)(&aabb, color, c->context);+	}+}++void hs_b3DrawBoxTrampoline(b3Vec3 extents, b3WorldTransform transform, b3HexColor color, void* context)+{+	hs_b3DrawClosure* c = context;+	if (c->box != NULL)+	{+		((hs_b3DrawBoxHsFcn)c->box)(&extents, &transform, color, c->context);+	}+}++void hs_b3DrawStringTrampoline(b3Pos p, const char* s, b3HexColor color, void* context)+{+	hs_b3DrawClosure* c = context;+	if (c->string != NULL)+	{+		((hs_b3DrawStringHsFcn)c->string)(&p, s, color, c->context);+	}+}++// shape queries -------------------------------------------------------------++// hs_b3Shape_TestPoint needs the shape's GJK proxy, which the public API does+// not expose. These internal headers give it the same access+// b3Shape_GetClosestPoint uses; b3GetShape is static in shape.c, so its+// two-line lookup is inlined here.+#include "body.h"+#include "physics_world.h"+#include "shape.h"++bool hs_b3Shape_TestPoint(b3ShapeId shapeId, const b3Pos* point)+{+	b3World* world = b3GetWorld(shapeId.world0);+	if (world == NULL)+	{+		return false;+	}++	int id = shapeId.index1 - 1;+	b3Shape* shape = b3Array_Get(world->shapes, id);+	B3_ASSERT(shape->id == id && shape->generation == shapeId.generation);++	// b3MakeShapeProxy only supports the convex shapes; report false for the+	// rest like 2D's b2Shape_TestPoint does for its unsupported types+	// (instead of tripping the assert in b3MakeShapeProxy's default branch).+	if (shape->type != b3_sphereShape && shape->type != b3_capsuleShape && shape->type != b3_hullShape)+	{+		return false;+	}++	b3Body* body = b3Array_Get(world->bodies, shape->bodyId);+	b3Transform transform = b3ToRelativeTransform(b3GetBodyTransformQuick(world, body), *point);++	b3Vec3 target = b3Vec3_zero;+	b3DistanceInput input;+	input.proxyA = b3MakeShapeProxy(shape);+	input.proxyB = (b3ShapeProxy){ &target, 1, 0.0f };+	input.transform = b3InvMulTransforms(transform, b3Transform_identity);+	input.useRadii = true;++	b3SimplexCache cache = { 0 };+	b3DistanceOutput output = b3ShapeDistance(&input, &cache, NULL, 0);++	// With radii applied the distance clamps to zero inside the shape even+	// though the witness point stays on the perimeter.+	return output.distance == 0.0f;+}++// debug shapes --------------------------------------------------------------++void hs_b3NoopDestroyDebugShape(void* userShape, void* userContext)+{+	(void)userShape;+	(void)userContext;+}
+ cbits/box3d_shim.h view
@@ -0,0 +1,125 @@+// SPDX-License-Identifier: BSD-3-Clause+//+// Thin C shim for the Box3D Haskell bindings.+//+// The Box3D public API passes and returns several small structs *by value*,+// which GHC's FFI cannot express in general. Ids are exempt: they are+// integer-only structs of at most 8 bytes, and on the supported 64-bit ABIs+// (x86-64 SysV and Win64, AArch64) they travel in a general-purpose register+// exactly like a uint32_t/uint64_t, so the Haskell side imports id-only+// functions directly with word-sized newtypes (see the _Static_asserts in+// box3d_shim.c). Everything else — float structs, which travel in vector+// registers or by hidden reference, and the config defs — is wrapped here+// into a pointer-based form. The naming convention is hs_<original b3 name>;+// a value that was returned by value is written through a trailing+// out-pointer, and a non-id value that was passed by value is taken through+// a const in-pointer.++#pragma once++#include "box3d/box3d.h"++// base.h -----------------------------------------------------------------++// b3Version b3GetVersion(void)+void hs_b3GetVersion(b3Version* out);++// math_functions.h -------------------------------------------------------++bool hs_b3IsValidVec3(const b3Vec3* a);+bool hs_b3IsValidQuat(const b3Quat* q);+bool hs_b3IsValidTransform(const b3Transform* a);+bool hs_b3IsValidMatrix3(const b3Matrix3* a);+bool hs_b3IsValidAABB(const b3AABB* a);+bool hs_b3IsValidPlane(const b3Plane* a);++// b3CosSin b3ComputeCosSin(float radians)+void hs_b3ComputeCosSin(float radians, b3CosSin* out);++// b3Quat b3ComputeQuatBetweenUnitVectors(b3Vec3 v1, b3Vec3 v2)+void hs_b3ComputeQuatBetweenUnitVectors(const b3Vec3* v1, const b3Vec3* v2, b3Quat* out);++// types.h: default definition constructors --------------------------------++void hs_b3DefaultWorldDef(b3WorldDef* out);+void hs_b3DefaultBodyDef(b3BodyDef* out);+void hs_b3DefaultShapeDef(b3ShapeDef* out);+void hs_b3DefaultFilter(b3Filter* out);+void hs_b3DefaultSurfaceMaterial(b3SurfaceMaterial* out);+void hs_b3DefaultQueryFilter(b3QueryFilter* out);+void hs_b3DefaultExplosionDef(b3ExplosionDef* out);+void hs_b3DefaultDebugDraw(b3DebugDraw* out);++// The world / body by-value accessors now live in the generated+// box3d_gen_shim.{h,c} (hsg_ prefix). This file keeps only the shims backing+// the hand-written scaffolding (base.h, math_functions.h, the *Def defaults).++// types.h: joint definition defaults ---------------------------------------++void hs_b3DefaultDistanceJointDef(b3DistanceJointDef* out);+void hs_b3DefaultMotorJointDef(b3MotorJointDef* out);+void hs_b3DefaultParallelJointDef(b3ParallelJointDef* out);+void hs_b3DefaultPrismaticJointDef(b3PrismaticJointDef* out);+void hs_b3DefaultRevoluteJointDef(b3RevoluteJointDef* out);+void hs_b3DefaultSphericalJointDef(b3SphericalJointDef* out);+void hs_b3DefaultWeldJointDef(b3WeldJointDef* out);+void hs_b3DefaultWheelJointDef(b3WheelJointDef* out);+void hs_b3DefaultFilterJointDef(b3FilterJointDef* out);++// shape queries -------------------------------------------------------------++// Box3D has no b3Shape_TestPoint (unlike 2D's b2Shape_TestPoint). This runs+// the GJK distance query between the shape and the point with radii applied+// and reports a clamped distance of zero; it needs engine-internal access to+// the shape's proxy, so it lives in the hand shim. Convex shapes only: mesh,+// height field and compound shapes report false, mirroring the 2D original.+bool hs_b3Shape_TestPoint(b3ShapeId shapeId, const b3Pos* point);++// Callback trampolines: fixed C functions with the engine's callback+// signatures. The context must point at an hs_b3Closure whose fn is a+// Haskell-wrapped function of the pointer-argument variant of the signature+// and whose context is the user context it receives.++typedef struct hs_b3Closure+{+	void* fn;+	void* context;+} hs_b3Closure;++float hs_b3CastResultTrampoline(b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId,+								int triangleIndex, int childIndex, void* context);+bool hs_b3PreSolveTrampoline(b3ShapeId shapeIdA, b3ShapeId shapeIdB, b3Pos point, b3Vec3 normal, void* context);+bool hs_b3MeshQueryTrampoline(b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleIndex, void* context);++// b3DebugDraw trampolines. The struct's nine callback slots share one context+// field, so they take a wider closure than hs_b3Closure: one Haskell-wrapped+// function per slot, in the declaration order of b3DebugDraw, followed by the+// user context. A NULL slot draws nothing.++typedef struct hs_b3DrawClosure+{+	void* shape;+	void* segment;+	void* transform;+	void* point;+	void* sphere;+	void* capsule;+	void* bounds;+	void* box;+	void* string;+	void* context;+} hs_b3DrawClosure;++void hs_b3DrawShapeTrampoline(void* userShape, b3WorldTransform transform, b3HexColor color, void* context);+void hs_b3DrawSegmentTrampoline(b3Pos p1, b3Pos p2, b3HexColor color, void* context);+void hs_b3DrawTransformTrampoline(b3WorldTransform transform, void* context);+void hs_b3DrawPointTrampoline(b3Pos p, float size, b3HexColor color, void* context);+void hs_b3DrawSphereTrampoline(b3Pos p, float radius, b3HexColor color, float alpha, void* context);+void hs_b3DrawCapsuleTrampoline(b3Pos p1, b3Pos p2, float radius, b3HexColor color, float alpha, void* context);+void hs_b3DrawBoundsTrampoline(b3AABB aabb, b3HexColor color, void* context);+void hs_b3DrawBoxTrampoline(b3Vec3 extents, b3WorldTransform transform, b3HexColor color, void* context);+void hs_b3DrawStringTrampoline(b3Pos p, const char* s, b3HexColor color, void* context);++// debug shapes --------------------------------------------------------------+// This is the slot to use when there is nothing to free.+void hs_b3NoopDestroyDebugShape(void* userShape, void* userContext);
+ src/Box3D/Authoring.hsc view
@@ -0,0 +1,234 @@+{-| Authoring collision geometry from user data: meshes, height fields and+compounds loaded from model files rather than the engine's procedural+generators ("Box3D.BoxMesh", "Box3D.GridMesh", ...).++The underlying defs (@b3MeshDef@, @b3HeightFieldDef@, @b3CompoundDef@) carry+pointers into caller memory; the creators here own the borrows for the+duration of the create call. The engine clones everything it keeps, so the+input vectors can be freed afterwards.++The results are the same engine-owned handles the procedural generators+return: destroy them with @Box3D.Mesh.destroy@ / @Box3D.HeightField.destroy@+/ @Box3D.Compound.destroy@ once no shape uses them.+-}+module Box3D.Authoring+  ( -- * Meshes from model data+    MeshOptions (..)+  , defaultMeshOptions+  , createMeshFromData++    -- * Height fields from sample data+  , HeightFieldOptions (..)+  , defaultHeightFieldOptions+  , createHeightFieldFromData++    -- * Compounds+  , CompoundCapsuleDef (..)+  , CompoundHullDef (..)+  , CompoundMeshDef (..)+  , CompoundSphereDef (..)+  , withCompoundDef+  , createCompoundFromData+  ) where++import Foreign+import Foreign.C.Types (CInt)++import Data.Vector.Storable qualified as VS++import Box3D.Compound qualified as Compound+import Box3D.HeightField qualified as HeightField+import Box3D.Internal (copyVector)+import Box3D.MathTypes (Vec3)+import Box3D.Mesh qualified as Mesh+import Box3D.HeightField (HeightFieldData, HeightFieldDef (..))+import Box3D.Mesh (MeshData, MeshDef (..))+import Box3D.Compound (CompoundCapsuleDef (..), CompoundData, CompoundDef (..), CompoundHullDef (..), CompoundMeshDef (..), CompoundSphereDef (..))++#include "box3d/box3d.h"++-- Meshes --------------------------------------------------------------------++-- | The scalar knobs of @b3MeshDef@; the data arrays are arguments of+-- 'createMeshFromData'. Start from 'defaultMeshOptions'.+data MeshOptions = MeshOptions+  { weldTolerance :: Float+  -- ^ Tolerance for vertex welding in length units.+  , weldVertices :: Bool+  -- ^ Optionally weld nearby vertices.+  , useMedianSplit :: Bool+  -- ^ Median split instead of SAH speeds up creation of grid-like meshes.+  , identifyEdges :: Bool+  -- ^ Compute triangle adjacency from shared edges (better contacts).+  }+  deriving (Eq, Show)++-- | No welding, SAH splits, edge identification on (what most of the+-- engine's own generators use).+defaultMeshOptions :: MeshOptions+defaultMeshOptions =+  MeshOptions+    { weldTolerance = 0+    , weldVertices = False+    , useMedianSplit = False+    , identifyEdges = True+    }++-- | Create a re-usable collision mesh (@b3CreateMesh@) from triangle data:+-- at least 3 vertices, vertex indices (3 per triangle) and optionally one+-- material index per triangle (indexing into @shapeDefMaterials@). The+-- engine clones the data. Returns the mesh (@NULL@ when the input is+-- invalid) and the indices of any degenerate triangles it skipped.+createMeshFromData+  :: VS.Vector Vec3+  -- ^ Triangle vertices.+  -> VS.Vector Int32+  -- ^ Triangle vertex indices, 3 per triangle.+  -> Maybe (VS.Vector Word8)+  -- ^ Per-triangle material indices.+  -> MeshOptions+  -> IO (Ptr MeshData, VS.Vector Int32)+createMeshFromData vertices indices mMaterials opts+  -- @b3MeshDef@ has no material count: @b3CreateMesh@ reads one entry per+  -- triangle, so a short vector would be read out of bounds.+  | Just materials <- mMaterials+  , VS.length materials /= triangleCount =+      pure (nullPtr, VS.empty)+  | otherwise =+      VS.unsafeWith vertices $ \pVertices ->+      VS.unsafeWith indices $ \pIndices ->+      maybeWith VS.unsafeWith mMaterials $ \pMaterials ->+      with+        MeshDef+          { vertices = pVertices+          , indices = castPtr pIndices+          , materialIndices = castPtr pMaterials+          , weldTolerance = opts.weldTolerance+          , vertexCount = fromIntegral (VS.length vertices)+          , triangleCount = fromIntegral triangleCount+          , weldVertices = fromBool opts.weldVertices+          , useMedianSplit = fromBool opts.useMedianSplit+          , identifyEdges = fromBool opts.identifyEdges+          } $ \def ->+        -- Capacity is one past the triangle count: b3CreateMesh stores an index+        -- only while degenerateCount < capacity, so the extra slot lets every+        -- triangle be reported.+        allocaArray (triangleCount + 1) $ \pDegenerate -> do+          mesh <- Mesh.create def pDegenerate (triangleCount + 1)+          if mesh == nullPtr+            then pure (mesh, VS.empty)+            else do+              n <- #{peek b3MeshData, degenerateCount} mesh+              degenerate <- copyVector (fromIntegral (n :: CInt)) (castPtr pDegenerate)+              pure (mesh, degenerate)+  where+    triangleCount = VS.length indices `div` 3++-- Height fields ---------------------------------------------------------------++-- | The scalar knobs of @b3HeightFieldDef@; the sample arrays are arguments+-- of 'createHeightFieldFromData'. Start from 'defaultHeightFieldOptions'.+data HeightFieldOptions = HeightFieldOptions+  { heightRange :: Maybe (Float, Float)+  -- ^ Global minimum/maximum heights used for quantization, in unscaled+  -- space; heights are clamped to this range. Adjacent height fields line+  -- up exactly only when they share the same range. 'Nothing' takes the+  -- range of the height data.+  , clockwiseWinding :: Bool+  -- ^ Clockwise winding, effectively inverting the field along the y-axis.+  }+  deriving (Eq, Show)++-- | Height range from the data, counter-clockwise winding.+defaultHeightFieldOptions :: HeightFieldOptions+defaultHeightFieldOptions =+  HeightFieldOptions+    { heightRange = Nothing+    , clockwiseWinding = False+    }++-- | Create a height field (@b3CreateHeightField@) from grid point heights+-- (row-major, @countX * countZ@ samples) and optionally one material index+-- per grid cell (@(countX - 1) * (countZ - 1)@ entries; @0xFF@ marks a+-- hole). The engine quantizes the heights into its own storage. Returns+-- @NULL@ when the input is invalid.+createHeightFieldFromData+  :: VS.Vector Float+  -- ^ Grid point heights; must be non-empty.+  -> Maybe (VS.Vector Word8)+  -- ^ Grid cell material indices.+  -> Vec3+  -- ^ Scale; all components must be positive.+  -> Int+  -- ^ Grid lines along the x-axis.+  -> Int+  -- ^ Grid lines along the z-axis.+  -> HeightFieldOptions+  -> IO (Ptr HeightFieldData)+createHeightFieldFromData heights mMaterials scale countX countZ opts+  -- The default height range folds over the samples, and b3CreateHeightField+  -- itself never rejects an empty grid (release builds skip its assert).+  | VS.null heights = pure nullPtr+  | otherwise =+      VS.unsafeWith heights $ \pHeights ->+      maybeWith VS.unsafeWith mMaterials $ \pMaterials ->+      with+        HeightFieldDef+          { heights = castPtr pHeights+          , materialIndices = castPtr pMaterials+          , scale = scale+          , countX = fromIntegral countX+          , countZ = fromIntegral countZ+          , globalMinimumHeight = minHeight+          , globalMaximumHeight = maxHeight+          , clockwiseWinding = fromBool opts.clockwiseWinding+          }+        HeightField.create+  where+    (minHeight, maxHeight) = case opts.heightRange of+      Just range -> range+      Nothing ->+        let h0 = VS.head heights+        in VS.foldl' (\(!lo, !hi) h -> (min lo h, max hi h)) (h0, h0) heights++-- Compounds -------------------------------------------------------------------++-- | Run an action on a temporary @b3CompoundDef@ borrowing the four child+-- arrays (in the struct's field order). @b3CreateCompound@ clones all input+-- data, so the borrows end with the action.+withCompoundDef+  :: VS.Vector CompoundCapsuleDef+  -> VS.Vector CompoundHullDef+  -> VS.Vector CompoundMeshDef+  -> VS.Vector CompoundSphereDef+  -> (Ptr CompoundDef -> IO a)+  -> IO a+withCompoundDef capsules hulls meshes spheres action =+  VS.unsafeWith capsules $ \pCapsules ->+  VS.unsafeWith hulls $ \pHulls ->+  VS.unsafeWith meshes $ \pMeshes ->+  VS.unsafeWith spheres $ \pSpheres ->+  with+    CompoundDef+      { capsules = pCapsules+      , capsuleCount = fromIntegral (VS.length capsules)+      , hulls = pHulls+      , hullCount = fromIntegral (VS.length hulls)+      , meshes = pMeshes+      , meshCount = fromIntegral (VS.length meshes)+      , spheres = pSpheres+      , sphereCount = fromIntegral (VS.length spheres)+      }+    action++-- | Create a compound shape (@b3CreateCompound@) from child definitions.+-- All input data is fully cloned into the run-time compound. Returns @NULL@+-- when the input is invalid.+createCompoundFromData+  :: VS.Vector CompoundCapsuleDef+  -> VS.Vector CompoundHullDef+  -> VS.Vector CompoundMeshDef+  -> VS.Vector CompoundSphereDef+  -> IO (Ptr CompoundData)+createCompoundFromData capsules hulls meshes spheres =+  withCompoundDef capsules hulls meshes spheres Compound.create
+ src/Box3D/Base.hsc view
@@ -0,0 +1,140 @@+{-# LANGUAGE ForeignFunctionInterface #-}++-- | Low-level bindings to @box3d/base.h@: version info, global allocator /+-- assert / log hooks, and timing helpers.+module Box3D.Base+  ( -- * Version+    Version (..)+  , getVersion+  , isDoublePrecision++    -- * Allocator+  , AllocFcn+  , FreeFcn+  , setAllocator+  , getByteCount++    -- * Assert / log hooks+  , AssertFcn+  , LogFcn+  , setAssertFcn+  , setLogFcn++    -- * Timing+  , getTicks+  , getMilliseconds+  , getMillisecondsAndReset+  , yield+  , sleep+  , hash+  ) where++import Foreign+import Foreign.C.Types+import Foreign.C.String (CString)++import Box3D.Internal (allocaOut)++#include "box3d/box3d.h"++-- | Version numbering scheme, see <https://semver.org/>.+data Version = Version+  { major :: CInt+  , minor :: CInt+  , revision :: CInt+  }+  deriving (Eq, Show)++instance Storable Version where+  sizeOf _ = #{size b3Version}+  alignment _ = #{alignment b3Version}+  peek p =+    Version+      <$> #{peek b3Version, major} p+      <*> #{peek b3Version, minor} p+      <*> #{peek b3Version, revision} p+  poke p (Version ma mi re) = do+    #{poke b3Version, major} p ma+    #{poke b3Version, minor} p mi+    #{poke b3Version, revision} p re++foreign import ccall unsafe "hs_b3GetVersion"+  c_b3GetVersion :: Ptr Version -> IO ()++-- | Get the current version of Box3D.+getVersion :: IO Version+getVersion = allocaOut c_b3GetVersion++-- | 'True' if the library was built with @BOX3D_DOUBLE_PRECISION@.+foreign import ccall unsafe "b3IsDoublePrecision"+  c_b3IsDoublePrecision :: IO CBool++isDoublePrecision :: IO Bool+isDoublePrecision = (/= 0) <$> c_b3IsDoublePrecision++-- Allocator ---------------------------------------------------------------++-- | User allocation function: @void* (int32 size, int32 alignment)@.+type AllocFcn = Int32 -> Int32 -> IO (Ptr ())++-- | User free function: @void (void* mem)@.+type FreeFcn = Ptr () -> IO ()++-- | Override the default allocation functions. Set during application startup.+--+-- The 'FunPtr's must point to C functions: the engine calls the hooks from+-- inside @unsafe@ FFI calls, where re-entering Haskell is undefined+-- behaviour, so wrapping Haskell functions is not supported yet.+foreign import ccall unsafe "b3SetAllocator"+  setAllocator :: FunPtr AllocFcn -> FunPtr FreeFcn -> IO ()++-- | Total bytes allocated by Box3D.+foreign import ccall unsafe "b3GetByteCount"+  getByteCount :: IO Int32++-- Assert / log ------------------------------------------------------------++-- | Assert callback: @int (const char* condition, const char* file, int line)@.+-- Return 0 to skip the debugger break.+type AssertFcn = CString -> CString -> CInt -> IO CInt++-- | Log callback: @void (const char* message)@.+type LogFcn = CString -> IO ()++-- | Override the default assert callback. Must point to a C function, see+-- 'setAllocator'.+foreign import ccall unsafe "b3SetAssertFcn"+  setAssertFcn :: FunPtr AssertFcn -> IO ()++-- | Override the default logging callback. Must point to a C function, see+-- 'setAllocator'.+foreign import ccall unsafe "b3SetLogFcn"+  setLogFcn :: FunPtr LogFcn -> IO ()++-- Timing ------------------------------------------------------------------++-- | Absolute number of system ticks. The value is platform specific.+foreign import ccall unsafe "b3GetTicks"+  getTicks :: IO Word64++-- | Milliseconds passed from an initial tick value.+foreign import ccall unsafe "b3GetMilliseconds"+  getMilliseconds :: Word64 -> IO CFloat++-- | Milliseconds passed from an initial tick value, resetting it to now.+foreign import ccall unsafe "b3GetMillisecondsAndReset"+  getMillisecondsAndReset :: Ptr Word64 -> IO CFloat++-- | Yield to be used in a busy loop.+foreign import ccall unsafe "b3Yield"+  yield :: IO ()++-- | Sleep the current thread for a number of milliseconds. Imported as a+-- @safe@ call so the blocked thread releases its capability instead of+-- stalling the RTS for the whole duration.+foreign import ccall safe "b3Sleep"+  sleep :: CInt -> IO ()++-- | Simple djb2 hash function for determinism testing.+foreign import ccall unsafe "b3Hash"+  hash :: Word32 -> Ptr Word8 -> CInt -> IO Word32
+ src/Box3D/Body.hsc view
@@ -0,0 +1,1619 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.Body+  ( BodyDef (..)+  , MotionLocks (..)+  , BodyType (..)+  , pattern StaticBody+  , pattern KinematicBody+  , pattern DynamicBody+  , defaultBodyDef+  , create+  , destroy+  , isValid+  , getType+  , setType+  , setName+  , getName+  , setUserData+  , getUserData+  , getPosition+  , getPositionInto+  , getRotation+  , getRotationInto+  , getTransform+  , getTransformInto+  , setTransform+  , getLocalPoint+  , getWorldPoint+  , getLocalVector+  , getWorldVector+  , getLinearVelocity+  , getLinearVelocityInto+  , getAngularVelocity+  , getAngularVelocityInto+  , setLinearVelocity+  , setAngularVelocity+  , setTargetTransform+  , getLocalPointVelocity+  , getWorldPointVelocity+  , applyForce+  , applyForceToCenter+  , applyTorque+  , applyLinearImpulse+  , applyLinearImpulseToCenter+  , applyAngularImpulse+  , getMass+  , getLocalRotationalInertia+  , getLocalRotationalInertiaInto+  , getInverseMass+  , getWorldInverseRotationalInertia+  , getWorldInverseRotationalInertiaInto+  , getLocalCenter+  , getLocalCenterInto+  , getWorldCenter+  , getWorldCenterInto+  , setMassData+  , getMassData+  , getMassDataInto+  , applyMassFromShapes+  , setLinearDamping+  , getLinearDamping+  , setAngularDamping+  , getAngularDamping+  , setGravityScale+  , getGravityScale+  , isAwake+  , setAwake+  , enableSleep+  , isSleepEnabled+  , setSleepThreshold+  , getSleepThreshold+  , isEnabled+  , disable+  , enable+  , setMotionLocks+  , getMotionLocks+  , getMotionLocksInto+  , setBullet+  , isBullet+  , allowFastRotation+  , isFastRotationAllowed+  , enableContactRecycling+  , isContactRecyclingEnabled+  , enableHitEvents+  , getWorld+  , getShapeCount+  , getShapes+  , getJointCount+  , getJoints+  , getContactCapacity+  , getContactData+  , computeAABB+  , computeAABBInto+  , getClosestPoint+  , castRay+  , castShape+  , overlapShape+  , collideMover+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..))+import Foreign.C.String (CString)+import Data.Coerce (coerce)+import Box3D.Internal (allocaOut)+import Box3D.Collision (BodyPlaneResult)+import Box3D.Geometry (Capsule, MassData)+import Box3D.Id (BodyId (..), JointId (..), ShapeId (..), WorldId (..))+import Box3D.MathTypes (AABB, Matrix3, Pos, Quat, Vec3, WorldTransform)+import Box3D.Query (BodyCastResult, QueryFilter, ShapeProxy)+import Box3D.World (ContactData)++#include "box3d/box3d.h"+++{- | 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()@.+-}+data BodyDef = BodyDef+  { type_ :: BodyType+  -- ^ The body type: static, kinematic, or dynamic.+  , position :: Pos+  -- ^ 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.+  , rotation :: Quat+  -- ^ The initial world rotation of the body.+  , linearVelocity :: Vec3+  -- ^ The initial linear velocity of the body\'s origin. Usually in meters per second.+  , angularVelocity :: Vec3+  -- ^ The initial angular velocity of the body. Radians per second.+  , linearDamping :: Float+  -- ^ 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.+  , angularDamping :: Float+  -- ^ 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 used to slow down rotating bodies.+  , gravityScale :: Float+  -- ^ Scale the gravity applied to this body. Non-dimensional.+  , sleepThreshold :: Float+  -- ^ Sleep speed threshold, default is 0.05 meters per second+  , name :: CString+  -- ^ Optional body name for debugging.+  , userData :: Ptr ()+  -- ^ Use this to store application specific body data.+  , motionLocks :: MotionLocks+  -- ^ Motions locks to restrict linear and angular movement+  , enableSleep :: CBool+  -- ^ Set this flag to false if this body should never fall asleep.+  , isAwake :: CBool+  -- ^ Is this body initially awake or sleeping?+  , isBullet :: CBool+  -- ^ Treat this body as a 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. They do not guarantee accurate collision if both bodies are fast moving because the bullet does a continuous check after all non-bullet bodies have moved. You could get unlucky and have the bullet body end a time step very close to a non-bullet body and the non-bullet body then moves over the bullet body. In continuous collision, initial overlap is ignored to avoid freezing bodies in place. I do not recommend using them for game projectiles if precise collision timing is needed. Instead consider using a ray or shape cast. You can use a marching ray or shape cast for projectile that moves over time. If you want a fast moving projectile to collide with a fast moving target, you need to consider the relative movement in your ray or shape cast. This is out of the scope of Box3D. So what are good use cases for bullets? Pinball games or games with dynamic containers that hold other objects. It should be a use case where it doesn\'t break the game if there is a collision missed, but having them captured improves the quality of the game.+  , isEnabled :: CBool+  -- ^ Used to disable a body. A disabled body does not move or collide.+  , allowFastRotation :: CBool+  -- ^ This allows this body to bypass rotational speed limits. Should only be used+  -- for circular objects, like wheels.+  , enableContactRecycling :: CBool+  -- ^ Enable contact recycling. True by default. Leaving this enabled improves performance+  -- but may lead to ghost collision that should be avoided on characters.+  , internalValue :: CInt+  -- ^ Used internally to detect a valid definition. DO NOT SET.+  }+  deriving (Eq, Show)++instance Storable BodyDef where+  sizeOf _ = #{size b3BodyDef}+  alignment _ = #{alignment b3BodyDef}+  peek p =+    BodyDef+      <$> #{peek b3BodyDef, type} p+      <*> #{peek b3BodyDef, position} p+      <*> #{peek b3BodyDef, rotation} p+      <*> #{peek b3BodyDef, linearVelocity} p+      <*> #{peek b3BodyDef, angularVelocity} p+      <*> #{peek b3BodyDef, linearDamping} p+      <*> #{peek b3BodyDef, angularDamping} p+      <*> #{peek b3BodyDef, gravityScale} p+      <*> #{peek b3BodyDef, sleepThreshold} p+      <*> #{peek b3BodyDef, name} p+      <*> #{peek b3BodyDef, userData} p+      <*> #{peek b3BodyDef, motionLocks} p+      <*> #{peek b3BodyDef, enableSleep} p+      <*> #{peek b3BodyDef, isAwake} p+      <*> #{peek b3BodyDef, isBullet} p+      <*> #{peek b3BodyDef, isEnabled} p+      <*> #{peek b3BodyDef, allowFastRotation} p+      <*> #{peek b3BodyDef, enableContactRecycling} p+      <*> #{peek b3BodyDef, internalValue} p+  poke p (BodyDef f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11 f12 f13 f14 f15 f16 f17 f18 f19) = do+    #{poke b3BodyDef, type} p f1+    #{poke b3BodyDef, position} p f2+    #{poke b3BodyDef, rotation} p f3+    #{poke b3BodyDef, linearVelocity} p f4+    #{poke b3BodyDef, angularVelocity} p f5+    #{poke b3BodyDef, linearDamping} p f6+    #{poke b3BodyDef, angularDamping} p f7+    #{poke b3BodyDef, gravityScale} p f8+    #{poke b3BodyDef, sleepThreshold} p f9+    #{poke b3BodyDef, name} p f10+    #{poke b3BodyDef, userData} p f11+    #{poke b3BodyDef, motionLocks} p f12+    #{poke b3BodyDef, enableSleep} p f13+    #{poke b3BodyDef, isAwake} p f14+    #{poke b3BodyDef, isBullet} p f15+    #{poke b3BodyDef, isEnabled} p f16+    #{poke b3BodyDef, allowFastRotation} p f17+    #{poke b3BodyDef, enableContactRecycling} p f18+    #{poke b3BodyDef, internalValue} p f19++-- | Motion locks to restrict the body movement+data MotionLocks = MotionLocks+  { linearX :: CBool+  -- ^ Prevent translation along the x-axis+  , linearY :: CBool+  -- ^ Prevent translation along the y-axis+  , linearZ :: CBool+  -- ^ Prevent translation along the z-axis+  , angularX :: CBool+  -- ^ Prevent rotation around the x-axis+  , angularY :: CBool+  -- ^ Prevent rotation around the y-axis+  , angularZ :: CBool+  -- ^ Prevent rotation around the z-axis+  }+  deriving (Eq, Show)++instance Storable MotionLocks where+  sizeOf _ = #{size b3MotionLocks}+  alignment _ = #{alignment b3MotionLocks}+  peek p =+    MotionLocks+      <$> #{peek b3MotionLocks, linearX} p+      <*> #{peek b3MotionLocks, linearY} p+      <*> #{peek b3MotionLocks, linearZ} p+      <*> #{peek b3MotionLocks, angularX} p+      <*> #{peek b3MotionLocks, angularY} p+      <*> #{peek b3MotionLocks, angularZ} p+  poke p (MotionLocks f1 f2 f3 f4 f5 f6) = do+    #{poke b3MotionLocks, linearX} p f1+    #{poke b3MotionLocks, linearY} p f2+    #{poke b3MotionLocks, linearZ} p f3+    #{poke b3MotionLocks, angularX} p f4+    #{poke b3MotionLocks, angularY} p f5+    #{poke b3MotionLocks, angularZ} p f6++{- | The body simulation type.+Each body is one of these three types. The type determines how the body behaves in the simulation.+-}+newtype BodyType = BodyType CInt+  deriving (Eq, Show, Storable)++-- | zero mass, zero velocity, may be manually moved+pattern StaticBody :: BodyType+-- | zero mass, velocity set by user, moved by solver+pattern KinematicBody :: BodyType+-- | positive mass, velocity determined by forces, moved by solver+pattern DynamicBody :: BodyType+pattern StaticBody = BodyType #{const b3_staticBody}+pattern KinematicBody = BodyType #{const b3_kinematicBody}+pattern DynamicBody = BodyType #{const b3_dynamicBody}++foreign import ccall unsafe "hs_b3DefaultBodyDef"+  c_b3DefaultBodyDef :: Ptr BodyDef -> IO ()++defaultBodyDef :: IO BodyDef+defaultBodyDef = allocaOut c_b3DefaultBodyDef+++foreign import ccall unsafe "b3CreateBody"+  c_b3CreateBody :: WorldId -> Ptr BodyDef -> IO BodyId++foreign import ccall safe "b3DestroyBody"+  c_b3DestroyBody :: BodyId -> IO ()++foreign import ccall unsafe "b3Body_IsValid"+  c_b3Body_IsValid :: BodyId -> IO CBool++foreign import ccall unsafe "b3Body_GetType"+  c_b3Body_GetType :: BodyId -> IO CInt++foreign import ccall unsafe "b3Body_SetType"+  c_b3Body_SetType :: BodyId -> CInt -> IO ()++foreign import ccall unsafe "b3Body_SetName"+  c_b3Body_SetName :: BodyId -> CString -> IO ()++foreign import ccall unsafe "b3Body_GetName"+  c_b3Body_GetName :: BodyId -> IO CString++foreign import ccall unsafe "b3Body_SetUserData"+  c_b3Body_SetUserData :: BodyId -> Ptr () -> IO ()++foreign import ccall unsafe "b3Body_GetUserData"+  c_b3Body_GetUserData :: BodyId -> IO (Ptr ())++foreign import ccall unsafe "hsg_b3Body_GetPosition"+  c_b3Body_GetPosition :: BodyId -> Ptr Pos -> IO ()++foreign import ccall unsafe "hsg_b3Body_GetRotation"+  c_b3Body_GetRotation :: BodyId -> Ptr Quat -> IO ()++foreign import ccall unsafe "hsg_b3Body_GetTransform"+  c_b3Body_GetTransform :: BodyId -> Ptr WorldTransform -> IO ()++foreign import ccall unsafe "hsg_b3Body_SetTransform"+  c_b3Body_SetTransform :: BodyId -> Ptr Pos -> Ptr Quat -> IO ()++foreign import ccall unsafe "hsg_b3Body_GetLocalPoint"+  c_b3Body_GetLocalPoint :: BodyId -> Ptr Pos -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "hsg_b3Body_GetWorldPoint"+  c_b3Body_GetWorldPoint :: BodyId -> Ptr Vec3 -> Ptr Pos -> IO ()++foreign import ccall unsafe "hsg_b3Body_GetLocalVector"+  c_b3Body_GetLocalVector :: BodyId -> Ptr Vec3 -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "hsg_b3Body_GetWorldVector"+  c_b3Body_GetWorldVector :: BodyId -> Ptr Vec3 -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "hsg_b3Body_GetLinearVelocity"+  c_b3Body_GetLinearVelocity :: BodyId -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "hsg_b3Body_GetAngularVelocity"+  c_b3Body_GetAngularVelocity :: BodyId -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "hsg_b3Body_SetLinearVelocity"+  c_b3Body_SetLinearVelocity :: BodyId -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "hsg_b3Body_SetAngularVelocity"+  c_b3Body_SetAngularVelocity :: BodyId -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "hsg_b3Body_SetTargetTransform"+  c_b3Body_SetTargetTransform :: BodyId -> Ptr WorldTransform -> Float -> CBool -> IO ()++foreign import ccall unsafe "hsg_b3Body_GetLocalPointVelocity"+  c_b3Body_GetLocalPointVelocity :: BodyId -> Ptr Vec3 -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "hsg_b3Body_GetWorldPointVelocity"+  c_b3Body_GetWorldPointVelocity :: BodyId -> Ptr Pos -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "hsg_b3Body_ApplyForce"+  c_b3Body_ApplyForce :: BodyId -> Ptr Vec3 -> Ptr Pos -> CBool -> IO ()++foreign import ccall unsafe "hsg_b3Body_ApplyForceToCenter"+  c_b3Body_ApplyForceToCenter :: BodyId -> Ptr Vec3 -> CBool -> IO ()++foreign import ccall unsafe "hsg_b3Body_ApplyTorque"+  c_b3Body_ApplyTorque :: BodyId -> Ptr Vec3 -> CBool -> IO ()++foreign import ccall unsafe "hsg_b3Body_ApplyLinearImpulse"+  c_b3Body_ApplyLinearImpulse :: BodyId -> Ptr Vec3 -> Ptr Pos -> CBool -> IO ()++foreign import ccall unsafe "hsg_b3Body_ApplyLinearImpulseToCenter"+  c_b3Body_ApplyLinearImpulseToCenter :: BodyId -> Ptr Vec3 -> CBool -> IO ()++foreign import ccall unsafe "hsg_b3Body_ApplyAngularImpulse"+  c_b3Body_ApplyAngularImpulse :: BodyId -> Ptr Vec3 -> CBool -> IO ()++foreign import ccall unsafe "b3Body_GetMass"+  c_b3Body_GetMass :: BodyId -> IO Float++foreign import ccall unsafe "hsg_b3Body_GetLocalRotationalInertia"+  c_b3Body_GetLocalRotationalInertia :: BodyId -> Ptr Matrix3 -> IO ()++foreign import ccall unsafe "b3Body_GetInverseMass"+  c_b3Body_GetInverseMass :: BodyId -> IO Float++foreign import ccall unsafe "hsg_b3Body_GetWorldInverseRotationalInertia"+  c_b3Body_GetWorldInverseRotationalInertia :: BodyId -> Ptr Matrix3 -> IO ()++foreign import ccall unsafe "hsg_b3Body_GetLocalCenter"+  c_b3Body_GetLocalCenter :: BodyId -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "hsg_b3Body_GetWorldCenter"+  c_b3Body_GetWorldCenter :: BodyId -> Ptr Pos -> IO ()++foreign import ccall unsafe "hsg_b3Body_SetMassData"+  c_b3Body_SetMassData :: BodyId -> Ptr MassData -> IO ()++foreign import ccall unsafe "hsg_b3Body_GetMassData"+  c_b3Body_GetMassData :: BodyId -> Ptr MassData -> IO ()++foreign import ccall unsafe "b3Body_ApplyMassFromShapes"+  c_b3Body_ApplyMassFromShapes :: BodyId -> IO ()++foreign import ccall unsafe "b3Body_SetLinearDamping"+  c_b3Body_SetLinearDamping :: BodyId -> Float -> IO ()++foreign import ccall unsafe "b3Body_GetLinearDamping"+  c_b3Body_GetLinearDamping :: BodyId -> IO Float++foreign import ccall unsafe "b3Body_SetAngularDamping"+  c_b3Body_SetAngularDamping :: BodyId -> Float -> IO ()++foreign import ccall unsafe "b3Body_GetAngularDamping"+  c_b3Body_GetAngularDamping :: BodyId -> IO Float++foreign import ccall unsafe "b3Body_SetGravityScale"+  c_b3Body_SetGravityScale :: BodyId -> Float -> IO ()++foreign import ccall unsafe "b3Body_GetGravityScale"+  c_b3Body_GetGravityScale :: BodyId -> IO Float++foreign import ccall unsafe "b3Body_IsAwake"+  c_b3Body_IsAwake :: BodyId -> IO CBool++foreign import ccall unsafe "b3Body_SetAwake"+  c_b3Body_SetAwake :: BodyId -> CBool -> IO ()++foreign import ccall unsafe "b3Body_EnableSleep"+  c_b3Body_EnableSleep :: BodyId -> CBool -> IO ()++foreign import ccall unsafe "b3Body_IsSleepEnabled"+  c_b3Body_IsSleepEnabled :: BodyId -> IO CBool++foreign import ccall unsafe "b3Body_SetSleepThreshold"+  c_b3Body_SetSleepThreshold :: BodyId -> Float -> IO ()++foreign import ccall unsafe "b3Body_GetSleepThreshold"+  c_b3Body_GetSleepThreshold :: BodyId -> IO Float++foreign import ccall unsafe "b3Body_IsEnabled"+  c_b3Body_IsEnabled :: BodyId -> IO CBool++foreign import ccall unsafe "b3Body_Disable"+  c_b3Body_Disable :: BodyId -> IO ()++foreign import ccall unsafe "b3Body_Enable"+  c_b3Body_Enable :: BodyId -> IO ()++foreign import ccall unsafe "hsg_b3Body_SetMotionLocks"+  c_b3Body_SetMotionLocks :: BodyId -> Ptr MotionLocks -> IO ()++foreign import ccall unsafe "hsg_b3Body_GetMotionLocks"+  c_b3Body_GetMotionLocks :: BodyId -> Ptr MotionLocks -> IO ()++foreign import ccall unsafe "b3Body_SetBullet"+  c_b3Body_SetBullet :: BodyId -> CBool -> IO ()++foreign import ccall unsafe "b3Body_IsBullet"+  c_b3Body_IsBullet :: BodyId -> IO CBool++foreign import ccall unsafe "b3Body_AllowFastRotation"+  c_b3Body_AllowFastRotation :: BodyId -> CBool -> IO ()++foreign import ccall unsafe "b3Body_IsFastRotationAllowed"+  c_b3Body_IsFastRotationAllowed :: BodyId -> IO CBool++foreign import ccall unsafe "b3Body_EnableContactRecycling"+  c_b3Body_EnableContactRecycling :: BodyId -> CBool -> IO ()++foreign import ccall unsafe "b3Body_IsContactRecyclingEnabled"+  c_b3Body_IsContactRecyclingEnabled :: BodyId -> IO CBool++foreign import ccall unsafe "b3Body_EnableHitEvents"+  c_b3Body_EnableHitEvents :: BodyId -> CBool -> IO ()++foreign import ccall unsafe "b3Body_GetWorld"+  c_b3Body_GetWorld :: BodyId -> IO WorldId++foreign import ccall unsafe "b3Body_GetShapeCount"+  c_b3Body_GetShapeCount :: BodyId -> IO CInt++foreign import ccall unsafe "b3Body_GetShapes"+  c_b3Body_GetShapes :: BodyId -> Ptr ShapeId -> CInt -> IO CInt++foreign import ccall unsafe "b3Body_GetJointCount"+  c_b3Body_GetJointCount :: BodyId -> IO CInt++foreign import ccall unsafe "b3Body_GetJoints"+  c_b3Body_GetJoints :: BodyId -> Ptr JointId -> CInt -> IO CInt++foreign import ccall unsafe "b3Body_GetContactCapacity"+  c_b3Body_GetContactCapacity :: BodyId -> IO CInt++foreign import ccall unsafe "b3Body_GetContactData"+  c_b3Body_GetContactData :: BodyId -> Ptr ContactData -> CInt -> IO CInt++foreign import ccall unsafe "hsg_b3Body_ComputeAABB"+  c_b3Body_ComputeAABB :: BodyId -> Ptr AABB -> IO ()++foreign import ccall unsafe "hsg_b3Body_GetClosestPoint"+  c_b3Body_GetClosestPoint :: BodyId -> Ptr Vec3 -> Ptr Vec3 -> IO Float++foreign import ccall unsafe "hsg_b3Body_CastRay"+  c_b3Body_CastRay :: BodyId -> Ptr Pos -> Ptr Vec3 -> Ptr QueryFilter -> Float -> Ptr WorldTransform -> Ptr BodyCastResult -> IO ()++foreign import ccall unsafe "hsg_b3Body_CastShape"+  c_b3Body_CastShape :: BodyId -> Ptr Pos -> Ptr ShapeProxy -> Ptr Vec3 -> Ptr QueryFilter -> Float -> CBool -> Ptr WorldTransform -> Ptr BodyCastResult -> IO ()++foreign import ccall unsafe "hsg_b3Body_OverlapShape"+  c_b3Body_OverlapShape :: BodyId -> Ptr Pos -> Ptr ShapeProxy -> Ptr QueryFilter -> Ptr WorldTransform -> IO CBool++foreign import ccall unsafe "hsg_b3Body_CollideMover"+  c_b3Body_CollideMover :: BodyId -> Ptr BodyPlaneResult -> CInt -> Ptr Pos -> Ptr Capsule -> Ptr QueryFilter -> Ptr WorldTransform -> IO CInt++{- | 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.++> b3BodyDef bodyDef = b3DefaultBodyDef();+> b3BodyId myBodyId = b3CreateBody(myWorldId, &bodyDef);++__Warning:__ This function is locked during callbacks.++Binds @b3CreateBody@.+-}+create+  :: WorldId+  -> BodyDef+  -> IO BodyId+create a0 a1 =+  with a1 $ \p1 ->+  c_b3CreateBody a0 p1++{- | 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.++Binds @b3DestroyBody@.+-}+destroy+  :: BodyId+  -> IO ()+destroy a0 =+  c_b3DestroyBody a0++{- | 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.++Binds @b3Body_IsValid@.+-}+isValid+  :: BodyId+  -> IO Bool+isValid a0 =+  toBool <$> (c_b3Body_IsValid a0)++{- | Get the body type: static, kinematic, or dynamic++Binds @b3Body_GetType@.+-}+getType+  :: BodyId+  -> IO BodyType+getType a0 =+  coerce <$> (c_b3Body_GetType a0)++{- | Change the body type. This is an expensive operation. This automatically updates the mass+properties regardless of the automatic mass setting.++Binds @b3Body_SetType@.+-}+setType+  :: BodyId+  -> BodyType+  -> IO ()+setType a0 a1 =+  c_b3Body_SetType a0 (coerce a1)++{- | Set the body name.++Binds @b3Body_SetName@.+-}+setName+  :: BodyId+  -> CString+  -- ^ @name@+  -> IO ()+setName a0 a1 =+  c_b3Body_SetName a0 a1++{- | Get the body name. Returns an empty string if the name isn\'t set.++Binds @b3Body_GetName@.+-}+getName+  :: BodyId+  -> IO CString+getName a0 =+  c_b3Body_GetName a0++{- | Set the user data for a body++Binds @b3Body_SetUserData@.+-}+setUserData+  :: BodyId+  -> Ptr ()+  -- ^ @userData@+  -> IO ()+setUserData a0 a1 =+  c_b3Body_SetUserData a0 a1++{- | Get the user data stored in a body++Binds @b3Body_GetUserData@.+-}+getUserData+  :: BodyId+  -> IO (Ptr ())+getUserData a0 =+  c_b3Body_GetUserData a0++{- | Get the world position of a body. This is the location of the body origin.++Binds @b3Body_GetPosition@.+-}+getPosition+  :: BodyId+  -> IO Pos+getPosition a0 =+  alloca $ \pOut -> c_b3Body_GetPosition a0 pOut >> peek pOut++-- | Like 'getPosition', but the result is written into a caller-supplied buffer.+getPositionInto+  :: BodyId+  -> Ptr Pos+  -- ^ Result buffer.+  -> IO ()+getPositionInto a0 out =+  c_b3Body_GetPosition a0 out++{- | Get the world rotation of a body as a quaternion++Binds @b3Body_GetRotation@.+-}+getRotation+  :: BodyId+  -> IO Quat+getRotation a0 =+  alloca $ \pOut -> c_b3Body_GetRotation a0 pOut >> peek pOut++-- | Like 'getRotation', but the result is written into a caller-supplied buffer.+getRotationInto+  :: BodyId+  -> Ptr Quat+  -- ^ Result buffer.+  -> IO ()+getRotationInto a0 out =+  c_b3Body_GetRotation a0 out++{- | Get the world transform of a body.++Binds @b3Body_GetTransform@.+-}+getTransform+  :: BodyId+  -> IO WorldTransform+getTransform a0 =+  alloca $ \pOut -> c_b3Body_GetTransform a0 pOut >> peek pOut++-- | Like 'getTransform', but the result is written into a caller-supplied buffer.+getTransformInto+  :: BodyId+  -> Ptr WorldTransform+  -- ^ Result buffer.+  -> IO ()+getTransformInto a0 out =+  c_b3Body_GetTransform a0 out++{- | Set the world transform of a body. This acts as a teleport and is fairly expensive.++__Note:__ Generally you should create a body with the intended transform.++See also @b3BodyDef::position@ and @b3BodyDef::rotation@..++Binds @b3Body_SetTransform@.+-}+setTransform+  :: BodyId+  -> Pos+  -- ^ @position@+  -> Quat+  -- ^ @rotation@+  -> IO ()+setTransform a0 a1 a2 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b3Body_SetTransform a0 p1 p2++{- | Get a local point on a body given a world point.++Binds @b3Body_GetLocalPoint@.+-}+getLocalPoint+  :: BodyId+  -> Pos+  -- ^ @worldPoint@+  -> IO Vec3+getLocalPoint a0 a1 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b3Body_GetLocalPoint a0 p1 pOut >> peek pOut++{- | Get a world point on a body given a local point.++Binds @b3Body_GetWorldPoint@.+-}+getWorldPoint+  :: BodyId+  -> Vec3+  -- ^ @localPoint@+  -> IO Pos+getWorldPoint a0 a1 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b3Body_GetWorldPoint a0 p1 pOut >> peek pOut++{- | Get a local vector on a body given a world vector.++Binds @b3Body_GetLocalVector@.+-}+getLocalVector+  :: BodyId+  -> Vec3+  -- ^ @worldVector@+  -> IO Vec3+getLocalVector a0 a1 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b3Body_GetLocalVector a0 p1 pOut >> peek pOut++{- | Get a world vector on a body given a local vector.++Binds @b3Body_GetWorldVector@.+-}+getWorldVector+  :: BodyId+  -> Vec3+  -- ^ @localVector@+  -> IO Vec3+getWorldVector a0 a1 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b3Body_GetWorldVector a0 p1 pOut >> peek pOut++{- | Get the linear velocity of a body\'s center of mass. Usually in meters per second.++Binds @b3Body_GetLinearVelocity@.+-}+getLinearVelocity+  :: BodyId+  -> IO Vec3+getLinearVelocity a0 =+  alloca $ \pOut -> c_b3Body_GetLinearVelocity a0 pOut >> peek pOut++-- | Like 'getLinearVelocity', but the result is written into a caller-supplied buffer.+getLinearVelocityInto+  :: BodyId+  -> Ptr Vec3+  -- ^ Result buffer.+  -> IO ()+getLinearVelocityInto a0 out =+  c_b3Body_GetLinearVelocity a0 out++{- | Get the angular velocity of a body in radians per second.++Binds @b3Body_GetAngularVelocity@.+-}+getAngularVelocity+  :: BodyId+  -> IO Vec3+getAngularVelocity a0 =+  alloca $ \pOut -> c_b3Body_GetAngularVelocity a0 pOut >> peek pOut++-- | Like 'getAngularVelocity', but the result is written into a caller-supplied buffer.+getAngularVelocityInto+  :: BodyId+  -> Ptr Vec3+  -- ^ Result buffer.+  -> IO ()+getAngularVelocityInto a0 out =+  c_b3Body_GetAngularVelocity a0 out++{- | Set the linear velocity of a body at the center of mass. Usually in meters per second.++Binds @b3Body_SetLinearVelocity@.+-}+setLinearVelocity+  :: BodyId+  -> Vec3+  -- ^ @linearVelocity@+  -> IO ()+setLinearVelocity a0 a1 =+  with a1 $ \p1 ->+  c_b3Body_SetLinearVelocity a0 p1++{- | Set the angular velocity of a body in radians per second.++Binds @b3Body_SetAngularVelocity@.+-}+setAngularVelocity+  :: BodyId+  -> Vec3+  -- ^ @angularVelocity@+  -> IO ()+setAngularVelocity a0 a1 =+  with a1 $ \p1 ->+  c_b3Body_SetAngularVelocity a0 p1++{- | 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.++Binds @b3Body_SetTargetTransform@.+-}+setTargetTransform+  :: BodyId+  -> WorldTransform+  -- ^ @target@+  -> Float+  -- ^ @timeStep@+  -> Bool+  -- ^ @wake@+  -> IO ()+setTargetTransform a0 a1 a2 a3 =+  with a1 $ \p1 ->+  c_b3Body_SetTargetTransform a0 p1 a2 (fromBool a3)++{- | Get the linear velocity of a local point attached to a body. Usually in meters per second.++Binds @b3Body_GetLocalPointVelocity@.+-}+getLocalPointVelocity+  :: BodyId+  -> Vec3+  -- ^ @localPoint@+  -> IO Vec3+getLocalPointVelocity a0 a1 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b3Body_GetLocalPointVelocity a0 p1 pOut >> peek pOut++{- | Get the linear velocity of a world point attached to a body. Usually in meters per second.++Binds @b3Body_GetWorldPointVelocity@.+-}+getWorldPointVelocity+  :: BodyId+  -> Pos+  -- ^ @worldPoint@+  -> IO Vec3+getWorldPointVelocity a0 a1 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b3Body_GetWorldPointVelocity a0 p1 pOut >> peek pOut++{- | 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.++Binds @b3Body_ApplyForce@.+-}+applyForce+  :: BodyId+  -- ^ @bodyId@: The body id+  -> Vec3+  -- ^ @force@: The world force vector, usually in newtons (N)+  -> Pos+  -- ^ @point@: The world position of the point of application+  -> Bool+  -- ^ @wake@: Option to wake up the body+  -> IO ()+applyForce a0 a1 a2 a3 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b3Body_ApplyForce a0 p1 p2 (fromBool a3)++{- | Apply a force to the center of mass. This optionally wakes up the body.+The force is ignored if the body is not awake.++Binds @b3Body_ApplyForceToCenter@.+-}+applyForceToCenter+  :: BodyId+  -- ^ @bodyId@: The body id+  -> Vec3+  -- ^ @force@: the world force vector, usually in newtons (N).+  -> Bool+  -- ^ @wake@: also wake up the body+  -> IO ()+applyForceToCenter a0 a1 a2 =+  with a1 $ \p1 ->+  c_b3Body_ApplyForceToCenter a0 p1 (fromBool a2)++{- | 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.++Binds @b3Body_ApplyTorque@.+-}+applyTorque+  :: BodyId+  -- ^ @bodyId@: The body id+  -> Vec3+  -- ^ @torque@: the world torque vector, usually in N*m.+  -> Bool+  -- ^ @wake@: also wake up the body+  -> IO ()+applyTorque a0 a1 a2 =+  with a1 $ \p1 ->+  c_b3Body_ApplyTorque a0 p1 (fromBool a2)++{- | 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.++__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.++Binds @b3Body_ApplyLinearImpulse@.+-}+applyLinearImpulse+  :: BodyId+  -- ^ @bodyId@: The body id+  -> Vec3+  -- ^ @impulse@: the world impulse vector, usually in N*s or kg*m\/s.+  -> Pos+  -- ^ @point@: the world position of the point of application.+  -> Bool+  -- ^ @wake@: also wake up the body+  -> IO ()+applyLinearImpulse a0 a1 a2 a3 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b3Body_ApplyLinearImpulse a0 p1 p2 (fromBool a3)++{- | 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.++__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.++Binds @b3Body_ApplyLinearImpulseToCenter@.+-}+applyLinearImpulseToCenter+  :: BodyId+  -- ^ @bodyId@: The body id+  -> Vec3+  -- ^ @impulse@: the world impulse vector, usually in N*s or kg*m\/s.+  -> Bool+  -- ^ @wake@: also wake up the body+  -> IO ()+applyLinearImpulseToCenter a0 a1 a2 =+  with a1 $ \p1 ->+  c_b3Body_ApplyLinearImpulseToCenter a0 p1 (fromBool a2)++{- | Apply an angular impulse in world space. The impulse is ignored if the body is not awake.+This optionally wakes 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.++Binds @b3Body_ApplyAngularImpulse@.+-}+applyAngularImpulse+  :: BodyId+  -- ^ @bodyId@: The body id+  -> Vec3+  -- ^ @impulse@: the world angular impulse vector, usually in units of kg*m*m\/s+  -> Bool+  -- ^ @wake@: also wake up the body+  -> IO ()+applyAngularImpulse a0 a1 a2 =+  with a1 $ \p1 ->+  c_b3Body_ApplyAngularImpulse a0 p1 (fromBool a2)++{- | Get the mass of the body, usually in kilograms++Binds @b3Body_GetMass@.+-}+getMass+  :: BodyId+  -> IO Float+getMass a0 =+  c_b3Body_GetMass a0++{- | Get the rotational inertia of the body in local space, usually in kg*m^2++Binds @b3Body_GetLocalRotationalInertia@.+-}+getLocalRotationalInertia+  :: BodyId+  -> IO Matrix3+getLocalRotationalInertia a0 =+  alloca $ \pOut -> c_b3Body_GetLocalRotationalInertia a0 pOut >> peek pOut++-- | Like 'getLocalRotationalInertia', but the result is written into a caller-supplied buffer.+getLocalRotationalInertiaInto+  :: BodyId+  -> Ptr Matrix3+  -- ^ Result buffer.+  -> IO ()+getLocalRotationalInertiaInto a0 out =+  c_b3Body_GetLocalRotationalInertia a0 out++{- | Get the inverse mass of the body, usually in 1\/kilograms++Binds @b3Body_GetInverseMass@.+-}+getInverseMass+  :: BodyId+  -> IO Float+getInverseMass a0 =+  c_b3Body_GetInverseMass a0++{- | Get the inverse rotational inertia of the body in world space, usually in 1\/kg*m^2++Binds @b3Body_GetWorldInverseRotationalInertia@.+-}+getWorldInverseRotationalInertia+  :: BodyId+  -> IO Matrix3+getWorldInverseRotationalInertia a0 =+  alloca $ \pOut -> c_b3Body_GetWorldInverseRotationalInertia a0 pOut >> peek pOut++-- | Like 'getWorldInverseRotationalInertia', but the result is written into a caller-supplied buffer.+getWorldInverseRotationalInertiaInto+  :: BodyId+  -> Ptr Matrix3+  -- ^ Result buffer.+  -> IO ()+getWorldInverseRotationalInertiaInto a0 out =+  c_b3Body_GetWorldInverseRotationalInertia a0 out++{- | Get the center of mass position of the body in local space++Binds @b3Body_GetLocalCenter@.+-}+getLocalCenter+  :: BodyId+  -> IO Vec3+getLocalCenter a0 =+  alloca $ \pOut -> c_b3Body_GetLocalCenter a0 pOut >> peek pOut++-- | Like 'getLocalCenter', but the result is written into a caller-supplied buffer.+getLocalCenterInto+  :: BodyId+  -> Ptr Vec3+  -- ^ Result buffer.+  -> IO ()+getLocalCenterInto a0 out =+  c_b3Body_GetLocalCenter a0 out++{- | Get the center of mass position of the body in world space++Binds @b3Body_GetWorldCenter@.+-}+getWorldCenter+  :: BodyId+  -> IO Pos+getWorldCenter a0 =+  alloca $ \pOut -> c_b3Body_GetWorldCenter a0 pOut >> peek pOut++-- | Like 'getWorldCenter', but the result is written into a caller-supplied buffer.+getWorldCenterInto+  :: BodyId+  -> Ptr Pos+  -- ^ Result buffer.+  -> IO ()+getWorldCenterInto a0 out =+  c_b3Body_GetWorldCenter a0 out++{- | 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.++Binds @b3Body_SetMassData@.+-}+setMassData+  :: BodyId+  -> MassData+  -> IO ()+setMassData a0 a1 =+  with a1 $ \p1 ->+  c_b3Body_SetMassData a0 p1++{- | Get the mass data for a body++Binds @b3Body_GetMassData@.+-}+getMassData+  :: BodyId+  -> IO MassData+getMassData a0 =+  alloca $ \pOut -> c_b3Body_GetMassData a0 pOut >> peek pOut++-- | Like 'getMassData', but the result is written into a caller-supplied buffer.+getMassDataInto+  :: BodyId+  -> Ptr MassData+  -- ^ Result buffer.+  -> IO ()+getMassDataInto a0 out =+  c_b3Body_GetMassData a0 out++{- | This updates 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.++Binds @b3Body_ApplyMassFromShapes@.+-}+applyMassFromShapes+  :: BodyId+  -> IO ()+applyMassFromShapes a0 =+  c_b3Body_ApplyMassFromShapes a0++{- | Adjust the linear damping. Normally this is set in @b3BodyDef@ before creation.++Binds @b3Body_SetLinearDamping@.+-}+setLinearDamping+  :: BodyId+  -> Float+  -- ^ @linearDamping@+  -> IO ()+setLinearDamping a0 a1 =+  c_b3Body_SetLinearDamping a0 a1++{- | Get the current linear damping.++Binds @b3Body_GetLinearDamping@.+-}+getLinearDamping+  :: BodyId+  -> IO Float+getLinearDamping a0 =+  c_b3Body_GetLinearDamping a0++{- | Adjust the angular damping. Normally this is set in @b3BodyDef@ before creation.++Binds @b3Body_SetAngularDamping@.+-}+setAngularDamping+  :: BodyId+  -> Float+  -- ^ @angularDamping@+  -> IO ()+setAngularDamping a0 a1 =+  c_b3Body_SetAngularDamping a0 a1++{- | Get the current angular damping.++Binds @b3Body_GetAngularDamping@.+-}+getAngularDamping+  :: BodyId+  -> IO Float+getAngularDamping a0 =+  c_b3Body_GetAngularDamping a0++{- | Adjust the gravity scale. Normally this is set in @b3BodyDef@ before creation.++See also @b3BodyDef::gravityScale@.++Binds @b3Body_SetGravityScale@.+-}+setGravityScale+  :: BodyId+  -> Float+  -- ^ @gravityScale@+  -> IO ()+setGravityScale a0 a1 =+  c_b3Body_SetGravityScale a0 a1++{- | Get the current gravity scale++Binds @b3Body_GetGravityScale@.+-}+getGravityScale+  :: BodyId+  -> IO Float+getGravityScale a0 =+  c_b3Body_GetGravityScale a0++{- | Returns: true if this body is awake++Binds @b3Body_IsAwake@.+-}+isAwake+  :: BodyId+  -> IO Bool+isAwake a0 =+  toBool <$> (c_b3Body_IsAwake a0)++{- | 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.++Binds @b3Body_SetAwake@.+-}+setAwake+  :: BodyId+  -> Bool+  -- ^ @awake@+  -> IO ()+setAwake a0 a1 =+  c_b3Body_SetAwake a0 (fromBool a1)++{- | Enable or disable sleeping for this body. If sleeping is disabled the body will wake.++Binds @b3Body_EnableSleep@.+-}+enableSleep+  :: BodyId+  -> Bool+  -- ^ @enableSleep@+  -> IO ()+enableSleep a0 a1 =+  c_b3Body_EnableSleep a0 (fromBool a1)++{- | Returns true if sleeping is enabled for this body++Binds @b3Body_IsSleepEnabled@.+-}+isSleepEnabled+  :: BodyId+  -> IO Bool+isSleepEnabled a0 =+  toBool <$> (c_b3Body_IsSleepEnabled a0)++{- | Set the sleep threshold, usually in meters per second++Binds @b3Body_SetSleepThreshold@.+-}+setSleepThreshold+  :: BodyId+  -> Float+  -- ^ @sleepThreshold@+  -> IO ()+setSleepThreshold a0 a1 =+  c_b3Body_SetSleepThreshold a0 a1++{- | Get the sleep threshold, usually in meters per second.++Binds @b3Body_GetSleepThreshold@.+-}+getSleepThreshold+  :: BodyId+  -> IO Float+getSleepThreshold a0 =+  c_b3Body_GetSleepThreshold a0++{- | Returns true if this body is enabled++Binds @b3Body_IsEnabled@.+-}+isEnabled+  :: BodyId+  -> IO Bool+isEnabled a0 =+  toBool <$> (c_b3Body_IsEnabled a0)++{- | Disable a body by removing it completely from the simulation. This is expensive.++Binds @b3Body_Disable@.+-}+disable+  :: BodyId+  -> IO ()+disable a0 =+  c_b3Body_Disable a0++{- | Enable a body by adding it to the simulation. This is expensive.++Binds @b3Body_Enable@.+-}+enable+  :: BodyId+  -> IO ()+enable a0 =+  c_b3Body_Enable a0++{- | Set the motion locks on this body.++Binds @b3Body_SetMotionLocks@.+-}+setMotionLocks+  :: BodyId+  -> MotionLocks+  -> IO ()+setMotionLocks a0 a1 =+  with a1 $ \p1 ->+  c_b3Body_SetMotionLocks a0 p1++{- | Get the motion locks for this body.++Binds @b3Body_GetMotionLocks@.+-}+getMotionLocks+  :: BodyId+  -> IO MotionLocks+getMotionLocks a0 =+  alloca $ \pOut -> c_b3Body_GetMotionLocks a0 pOut >> peek pOut++-- | Like 'getMotionLocks', but the result is written into a caller-supplied buffer.+getMotionLocksInto+  :: BodyId+  -> Ptr MotionLocks+  -- ^ Result buffer.+  -> IO ()+getMotionLocksInto a0 out =+  c_b3Body_GetMotionLocks a0 out++{- | Set this body to be a bullet. A bullet does continuous collision detection+against dynamic bodies (but not other bullets).++Binds @b3Body_SetBullet@.+-}+setBullet+  :: BodyId+  -> Bool+  -- ^ @flag@+  -> IO ()+setBullet a0 a1 =+  c_b3Body_SetBullet a0 (fromBool a1)++{- | Is this body a bullet?++Binds @b3Body_IsBullet@.+-}+isBullet+  :: BodyId+  -> IO Bool+isBullet a0 =+  toBool <$> (c_b3Body_IsBullet a0)++{- | Allow this body to rotate fast. Useful for axially symmetric bodies, such as vehicle wheels.+Normally rotation speed is clamped to improve CCD. However, this clamping is unnecessary for+bodies that only rotate fast around an axis of symmetry.++Binds @b3Body_AllowFastRotation@.+-}+allowFastRotation+  :: BodyId+  -> Bool+  -- ^ @flag@+  -> IO ()+allowFastRotation a0 a1 =+  c_b3Body_AllowFastRotation a0 (fromBool a1)++{- | Is this body allowed to rotate fast?++Binds @b3Body_IsFastRotationAllowed@.+-}+isFastRotationAllowed+  :: BodyId+  -> IO Bool+isFastRotationAllowed a0 =+  toBool <$> (c_b3Body_IsFastRotationAllowed a0)++{- | Enable or disable contact recycling for this body. Contact recycling is a performance optimization+that reuses contact manifolds when bodies move slightly. Disabling it can avoid ghost collisions+on characters at the cost of higher per-step work. Existing contacts retain their prior setting;+only contacts created after this call see the new value.++See also @b3BodyDef::enableContactRecycling@.++Binds @b3Body_EnableContactRecycling@.+-}+enableContactRecycling+  :: BodyId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableContactRecycling a0 a1 =+  c_b3Body_EnableContactRecycling a0 (fromBool a1)++{- | Is contact recycling enabled on this body?++Binds @b3Body_IsContactRecyclingEnabled@.+-}+isContactRecyclingEnabled+  :: BodyId+  -> IO Bool+isContactRecyclingEnabled a0 =+  toBool <$> (c_b3Body_IsContactRecyclingEnabled a0)++{- | Enable\/disable hit events on all shapes++See also @b3ShapeDef::enableHitEvents@.++Binds @b3Body_EnableHitEvents@.+-}+enableHitEvents+  :: BodyId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableHitEvents a0 a1 =+  c_b3Body_EnableHitEvents a0 (fromBool a1)++{- | Get the world that owns this body++Binds @b3Body_GetWorld@.+-}+getWorld+  :: BodyId+  -> IO WorldId+getWorld a0 =+  c_b3Body_GetWorld a0++{- | Get the number of shapes on this body++Binds @b3Body_GetShapeCount@.+-}+getShapeCount+  :: BodyId+  -> IO Int+getShapeCount a0 =+  fromIntegral <$> (c_b3Body_GetShapeCount a0)++{- | 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++Binds @b3Body_GetShapes@.+-}+getShapes+  :: BodyId+  -> Ptr ShapeId+  -- ^ @shapeArray@+  -> Int+  -- ^ @capacity@+  -> IO Int+getShapes a0 a1 a2 =+  fromIntegral <$> (c_b3Body_GetShapes a0 a1 (fromIntegral a2))++{- | Get the number of joints on this body++Binds @b3Body_GetJointCount@.+-}+getJointCount+  :: BodyId+  -> IO Int+getJointCount a0 =+  fromIntegral <$> (c_b3Body_GetJointCount a0)++{- | 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++Binds @b3Body_GetJoints@.+-}+getJoints+  :: BodyId+  -> Ptr JointId+  -- ^ @jointArray@+  -> Int+  -- ^ @capacity@+  -> IO Int+getJoints a0 a1 a2 =+  fromIntegral <$> (c_b3Body_GetJoints a0 a1 (fromIntegral a2))++{- | Get the maximum capacity required for retrieving all the touching contacts on a body++Binds @b3Body_GetContactCapacity@.+-}+getContactCapacity+  :: BodyId+  -> IO Int+getContactCapacity a0 =+  fromIntegral <$> (c_b3Body_GetContactCapacity a0)++{- | Get the touching contact data for a body++Binds @b3Body_GetContactData@.+-}+getContactData+  :: BodyId+  -> Ptr ContactData+  -> Int+  -- ^ @capacity@+  -> IO Int+getContactData a0 a1 a2 =+  fromIntegral <$> (c_b3Body_GetContactData a0 a1 (fromIntegral a2))++{- | 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.++Binds @b3Body_ComputeAABB@.+-}+computeAABB+  :: BodyId+  -> IO AABB+computeAABB a0 =+  alloca $ \pOut -> c_b3Body_ComputeAABB a0 pOut >> peek pOut++-- | Like 'computeAABB', but the result is written into a caller-supplied buffer.+computeAABBInto+  :: BodyId+  -> Ptr AABB+  -- ^ Result buffer.+  -> IO ()+computeAABBInto a0 out =+  c_b3Body_ComputeAABB a0 out++{- | Get the closest point on a body to a world target.++Binds @b3Body_GetClosestPoint@.+-}+getClosestPoint+  :: BodyId+  -> Ptr Vec3+  -- ^ @result@+  -> Vec3+  -- ^ @target@+  -> IO Float+getClosestPoint a0 a1 a2 =+  with a2 $ \p2 ->+  c_b3Body_GetClosestPoint a0 a1 p2++{- | Cast a ray at a specific body using a specified body transform.++Binds @b3Body_CastRay@.+-}+castRay+  :: BodyId+  -> Pos+  -- ^ @origin@+  -> Vec3+  -- ^ @translation@+  -> QueryFilter+  -> Float+  -- ^ @maxFraction@+  -> WorldTransform+  -- ^ @bodyTransform@+  -> Ptr BodyCastResult+  -- ^ Result buffer.+  -> IO ()+castRay a0 a1 a2 a3 a4 a5 out =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  with a3 $ \p3 ->+  with a5 $ \p5 ->+  c_b3Body_CastRay a0 p1 p2 p3 a4 p5 out++{- | Cast a shape at a specific body using a specified body transform.++Binds @b3Body_CastShape@.+-}+castShape+  :: BodyId+  -> Pos+  -- ^ @origin@+  -> Ptr ShapeProxy+  -> Vec3+  -- ^ @translation@+  -> QueryFilter+  -> Float+  -- ^ @maxFraction@+  -> Bool+  -- ^ @canEncroach@+  -> WorldTransform+  -- ^ @bodyTransform@+  -> Ptr BodyCastResult+  -- ^ Result buffer.+  -> IO ()+castShape a0 a1 a2 a3 a4 a5 a6 a7 out =+  with a1 $ \p1 ->+  with a3 $ \p3 ->+  with a4 $ \p4 ->+  with a7 $ \p7 ->+  c_b3Body_CastShape a0 p1 a2 p3 p4 a5 (fromBool a6) p7 out++{- | Overlap a shape with a specific body using a specified body transform.++Binds @b3Body_OverlapShape@.+-}+overlapShape+  :: BodyId+  -> Pos+  -- ^ @origin@+  -> Ptr ShapeProxy+  -> QueryFilter+  -> WorldTransform+  -- ^ @bodyTransform@+  -> IO Bool+overlapShape a0 a1 a2 a3 a4 =+  with a1 $ \p1 ->+  with a3 $ \p3 ->+  with a4 $ \p4 ->+  toBool <$> (c_b3Body_OverlapShape a0 p1 a2 p3 p4)++{- | Collide a character mover with a specific body using a specified body transform.++Binds @b3Body_CollideMover@.+-}+collideMover+  :: BodyId+  -> Ptr BodyPlaneResult+  -- ^ @bodyPlanes@+  -> Int+  -- ^ @planeCapacity@+  -> Pos+  -- ^ @origin@+  -> Capsule+  -- ^ @mover@+  -> QueryFilter+  -> WorldTransform+  -- ^ @bodyTransform@+  -> IO Int+collideMover a0 a1 a2 a3 a4 a5 a6 =+  with a3 $ \p3 ->+  with a4 $ \p4 ->+  with a5 $ \p5 ->+  with a6 $ \p6 ->+  fromIntegral <$> (c_b3Body_CollideMover a0 a1 (fromIntegral a2) p3 p4 p5 p6)
+ src/Box3D/BoxMesh.hs view
@@ -0,0 +1,31 @@+-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.BoxMesh+  ( create+  )+where++import Box3D.MathTypes (Vec3)+import Box3D.Mesh (MeshData)+import Foreign+import Foreign.C.Types (CBool (..))++foreign import ccall unsafe "hsg_b3CreateBoxMesh"+  c_b3CreateBoxMesh :: Ptr Vec3 -> Ptr Vec3 -> CBool -> IO (Ptr MeshData)++{- | Create a box mesh.++Binds @b3CreateBoxMesh@.+-}+create+  :: Vec3+  -- ^ @center@+  -> Vec3+  -- ^ @extent@+  -> Bool+  -- ^ @identifyEdges@+  -> IO (Ptr MeshData)+create a0 a1 a2 =+  with a0 $ \p0 ->+    with a1 $ \p1 ->+      c_b3CreateBoxMesh p0 p1 (fromBool a2)
+ src/Box3D/Callbacks.hs view
@@ -0,0 +1,853 @@+{-# LANGUAGE ForeignFunctionInterface #-}++{-| Producing 'FunPtr' callbacks from Haskell closures, and a ready-made+multithreaded task system.++The generated bindings take callbacks as raw @'FunPtr' Tag@ (see+"Box3D.Tags"); this module is the hand-written counterpart that turns an+ordinary Haskell function into such a 'FunPtr' via @foreign import "wrapper"@,+and frees it again. Every @mk*@ returns a 'FunPtr' the caller must+'freeHaskellFunPtr' (use the @with*@ brackets to get that for free).++== Threads++Box3D invokes these callbacks from its solver, which runs on background+worker threads when @WorldDef.workerCount@ > 1 — either its own scheduler, or+the 'TaskSystem' installed here. A Haskell closure wrapped as a 'FunPtr' is+therefore entered concurrently from several non-Haskell OS threads, so the+program must be built with the threaded runtime (@-threaded@) and the+closure must be thread-safe. Because the solver's task work re-enters Haskell+through these callbacks, the task dispatch is a @safe@ call.+-}+module Box3D.Callbacks+  ( -- * Leaf callbacks+    CustomFilter+  , mkCustomFilterFcn+  , withCustomFilterFcn+  , Friction+  , mkFrictionCallback+  , withFrictionCallback+  , Restitution+  , mkRestitutionCallback+  , withRestitutionCallback++    -- * Query and cast visitors+  , OverlapResult+  , withOverlapResultFcn+  , CastResult+  , withCastResultFcn+  , PreSolve+  , withPreSolveFcn+  , PlaneResultVisitor+  , withPlaneResultFcn+  , MoverFilter+  , withMoverFilterFcn+  , CompoundQuery+  , withCompoundQueryFcn+  , MeshQuery+  , withMeshQueryFcn++    -- * Dynamic-tree visitors+  , TreeQuery+  , withTreeQueryFcn+  , TreeQueryClosest+  , withTreeQueryClosestFcn+  , TreeRayCast+  , withTreeRayCastFcn+  , TreeBoxCast+  , withTreeBoxCastFcn++    -- * Debug draw+  , Draw (..)+  , noDraw+  , toWorld+  , DrawTarget+  , withDraw+  , draw+  , withDrawPtr+  , Cmd (..)+  , withRecorder++    -- * Debug shapes+  , CreateDebugShape+  , DestroyDebugShape+  , DebugShapes (create, destroy)+  , withDebugShapes+  , rethrowDebugShapes++    -- * A multithreaded task system+  , TaskSystem (..)+  , withThreadPoolTaskSystem+  ) where++import Control.Applicative ((<|>))+import Control.Concurrent (forkIO)+import Control.Concurrent.MVar (MVar, newEmptyMVar, putMVar, takeMVar)+import Control.Exception (SomeException, bracket, finally, onException, throwIO, try)+import Control.Monad (unless)+import Data.IORef (IORef, atomicModifyIORef', modifyIORef', newIORef, readIORef, writeIORef)+import Data.Vector.Storable (Vector)+import Foreign+import Foreign.C.String (CString)+import Foreign.C.Types (CBool (..), CChar, CInt (..))+import GHC.Foreign qualified+import GHC.IO.Encoding.Failure (CodingFailureMode (..))+import GHC.IO.Encoding.UTF8 (mkUTF8)++import Box3D.Collision (PlaneResult)+import Box3D.Compound (CompoundData)+import Box3D.DebugDraw (DebugDraw (..), HexColor (..))+import Box3D.DebugShape (DebugShape)+import Box3D.Id (ShapeId (..), WorldId)+import Box3D.Internal (copyVector)+import Box3D.Internal.DrawClosure qualified as DrawClosure+import Box3D.MathFunctions qualified as MathFunctions+import Box3D.MathTypes (AABB, Pos, Vec3, WorldTransform, transformWorldPoint)+import Box3D.Query (BoxCastInput, RayCastInput)+import Box3D.Tags (CastResultFcn, CompoundQueryFcn, CreateDebugShapeCallback, CustomFilterFcn, DestroyDebugShapeCallback, DrawBoundsFcn, DrawBoxFcn, DrawCapsuleFcn, DrawPointFcn, DrawSegmentFcn, DrawShapeFcn, DrawSphereFcn, DrawStringFcn, DrawTransformFcn, EnqueueTaskCallback, FinishTaskCallback, FrictionCallback, MeshQueryFcn, MoverFilterFcn, OverlapResultFcn, PlaneResultFcn, PreSolveFcn, RestitutionCallback, TreeBoxCastCallbackFcn, TreeQueryCallbackFcn, TreeQueryClosestCallbackFcn, TreeRayCastCallbackFcn)+import Box3D.World qualified as World++-- Leaf callbacks ----------------------------------------------------------++{- | Decide whether two shapes should collide (see+'Box3D.World.setCustomFilterCallback'). The 'Ptr' is the user context.+-}+type CustomFilter = ShapeId -> ShapeId -> Ptr () -> IO Bool++type CustomFilterC = ShapeId -> ShapeId -> Ptr () -> IO CBool++foreign import ccall "wrapper"+  wrapCustomFilter :: CustomFilterC -> IO (FunPtr CustomFilterC)++{- | Wrap a Haskell predicate as a custom-filter callback. Free it with+'freeHaskellFunPtr' when the world no longer references it.+-}+mkCustomFilterFcn :: CustomFilter -> IO (FunPtr CustomFilterFcn)+mkCustomFilterFcn f =+  castFunPtr <$> wrapCustomFilter (\a b ctx -> fromBool <$> f a b ctx)++-- | Run an action with a custom-filter callback that is freed on exit.+withCustomFilterFcn :: CustomFilter -> (FunPtr CustomFilterFcn -> IO a) -> IO a+withCustomFilterFcn f = bracket (mkCustomFilterFcn f) freeHaskellFunPtr++{- | Combine the friction of two materials (see+'Box3D.World.setFrictionCallback'): @frictionA userMatA frictionB userMatB@.+-}+type Friction = Float -> Word64 -> Float -> Word64 -> IO Float++foreign import ccall "wrapper"+  wrapFriction :: Friction -> IO (FunPtr Friction)++-- | Wrap a Haskell function as a friction-mixing callback.+mkFrictionCallback :: Friction -> IO (FunPtr FrictionCallback)+mkFrictionCallback f = castFunPtr <$> wrapFriction f++-- | Run an action with a friction callback that is freed on exit.+withFrictionCallback :: Friction -> (FunPtr FrictionCallback -> IO a) -> IO a+withFrictionCallback f = bracket (mkFrictionCallback f) freeHaskellFunPtr++{- | Combine the restitution of two materials (see+'Box3D.World.setRestitutionCallback'); same shape as 'Friction'.+-}+type Restitution = Float -> Word64 -> Float -> Word64 -> IO Float++foreign import ccall "wrapper"+  wrapRestitution :: Restitution -> IO (FunPtr Restitution)++-- | Wrap a Haskell function as a restitution-mixing callback.+mkRestitutionCallback :: Restitution -> IO (FunPtr RestitutionCallback)+mkRestitutionCallback f = castFunPtr <$> wrapRestitution f++-- | Run an action with a restitution callback that is freed on exit.+withRestitutionCallback :: Restitution -> (FunPtr RestitutionCallback -> IO a) -> IO a+withRestitutionCallback f = bracket (mkRestitutionCallback f) freeHaskellFunPtr++-- Query and cast visitors --------------------------------------------------+--+-- The cast and pre-solve callbacks receive b3Pos/b3Vec3 arguments *by value*,+-- which a GHC "wrapper" cannot accept. Each of those routes through a fixed C+-- trampoline (see box3d_shim.h) that takes the structs by value and forwards+-- pointers to the Haskell closure, carried in an hs_b3Closure context.++{- | Overlap query visitor: called for each shape found; return 'True' to+continue the query.+-}+type OverlapResult = ShapeId -> IO Bool++type OverlapResultC = ShapeId -> Ptr () -> IO CBool++foreign import ccall "wrapper"+  wrapOverlapResult :: OverlapResultC -> IO (FunPtr OverlapResultC)++{- | Run an action with an overlap visitor and the context to pass alongside+it (e.g. to 'Box3D.World.overlapAABB'); both are released on exit.+-}+withOverlapResultFcn :: OverlapResult -> (FunPtr OverlapResultFcn -> Ptr () -> IO a) -> IO a+withOverlapResultFcn f act =+  bracket (wrapOverlapResult (\s _ -> fromBool <$> f s)) freeHaskellFunPtr $ \p ->+    act (castFunPtr p) nullPtr++{- | Cast visitor: called for each shape a world ray\/shape cast finds, with+the hit point, normal, fraction, user material id and triangle\/child+indices. The returned fraction controls the query: 0 terminates, the given+fraction clips the ray, 1 continues.+-}+type CastResult = ShapeId -> Pos -> Vec3 -> Float -> Word64 -> CInt -> CInt -> IO Float++type CastResultC = ShapeId -> Ptr Pos -> Ptr Vec3 -> Float -> Word64 -> CInt -> CInt -> Ptr () -> IO Float++foreign import ccall "wrapper"+  wrapCastResult :: CastResultC -> IO (FunPtr CastResultC)++foreign import ccall unsafe "&hs_b3CastResultTrampoline"+  p_castResultTrampoline :: FunPtr CastResultFcn++{- | Run an action with a cast visitor and the context to pass alongside it+(e.g. to 'Box3D.World.castRay'); both are released on exit.+-}+withCastResultFcn :: CastResult -> (FunPtr CastResultFcn -> Ptr () -> IO a) -> IO a+withCastResultFcn f act =+  bracket (wrapCastResult go) freeHaskellFunPtr $ \hsFn ->+    withClosure (castFunPtr hsFn) $ act p_castResultTrampoline+  where+    go sid pPoint pNormal frac matId triangleIx childIx _ctx = do+      point <- peek pPoint+      normal <- peek pNormal+      f sid point normal frac matId triangleIx childIx++{- | Contact gate, called after narrow phase but before the solver: return+'False' to disable the contact. Fired from solver worker threads when the+world is multithreaded, so it must be thread-safe. Requires+@shapeDefEnablePreSolveEvents@ on the shapes.+-}+type PreSolve = ShapeId -> ShapeId -> Pos -> Vec3 -> IO Bool++type PreSolveC = ShapeId -> ShapeId -> Ptr Pos -> Ptr Vec3 -> Ptr () -> IO CBool++foreign import ccall "wrapper"+  wrapPreSolve :: PreSolveC -> IO (FunPtr PreSolveC)++foreign import ccall unsafe "&hs_b3PreSolveTrampoline"+  p_preSolveTrampoline :: FunPtr PreSolveFcn++{- | Run an action with a pre-solve gate and the context to pass alongside it+(to 'Box3D.World.setPreSolveCallback'). Unhook the callback from the world+before the action returns: both are released on exit.+-}+withPreSolveFcn :: PreSolve -> (FunPtr PreSolveFcn -> Ptr () -> IO a) -> IO a+withPreSolveFcn f act =+  bracket (wrapPreSolve go) freeHaskellFunPtr $ \hsFn ->+    withClosure (castFunPtr hsFn) $ act p_preSolveTrampoline+  where+    go a b pPoint pNormal _ctx = do+      point <- peek pPoint+      normal <- peek pNormal+      fromBool <$> f a b point normal++{- | Mover collision visitor: the batch of collision planes gathered against+one shape (see 'Box3D.World.collideMover'); return 'True' to keep gathering.+-}+type PlaneResultVisitor = ShapeId -> Vector PlaneResult -> IO Bool++type PlaneResultC = ShapeId -> Ptr PlaneResult -> CInt -> Ptr () -> IO CBool++foreign import ccall "wrapper"+  wrapPlaneResult :: PlaneResultC -> IO (FunPtr PlaneResultC)++{- | Run an action with a mover-plane visitor and the context to pass+alongside it (to 'Box3D.World.collideMover'); both are released on exit.+-}+withPlaneResultFcn :: PlaneResultVisitor -> (FunPtr PlaneResultFcn -> Ptr () -> IO a) -> IO a+withPlaneResultFcn f act =+  bracket (wrapPlaneResult go) freeHaskellFunPtr $ \p ->+    act (castFunPtr p) nullPtr+  where+    go sid pPlanes n _ctx = do+      planes <- copyVector (fromIntegral n) pPlanes+      fromBool <$> f sid planes++{- | Mover shape filter: return 'True' to consider the shape for mover+collision.+-}+type MoverFilter = ShapeId -> IO Bool++type MoverFilterC = ShapeId -> Ptr () -> IO CBool++foreign import ccall "wrapper"+  wrapMoverFilter :: MoverFilterC -> IO (FunPtr MoverFilterC)++{- | Run an action with a mover filter and the context to pass alongside it;+both are released on exit.+-}+withMoverFilterFcn :: MoverFilter -> (FunPtr MoverFilterFcn -> Ptr () -> IO a) -> IO a+withMoverFilterFcn f act =+  bracket (wrapMoverFilter (\s _ -> fromBool <$> f s)) freeHaskellFunPtr $ \p ->+    act (castFunPtr p) nullPtr++{- | Compound query visitor: @compoundData childIndex@; return 'True' to+continue. The compound data stays a raw 'Ptr' (no 'Storable' yet).+-}+type CompoundQuery = Ptr CompoundData -> CInt -> IO Bool++type CompoundQueryC = Ptr CompoundData -> CInt -> Ptr () -> IO CBool++foreign import ccall "wrapper"+  wrapCompoundQuery :: CompoundQueryC -> IO (FunPtr CompoundQueryC)++{- | Run an action with a compound-query visitor and the context to pass+alongside it; both are released on exit.+-}+withCompoundQueryFcn :: CompoundQuery -> (FunPtr CompoundQueryFcn -> Ptr () -> IO a) -> IO a+withCompoundQueryFcn f act =+  bracket (wrapCompoundQuery (\c i _ -> fromBool <$> f c i)) freeHaskellFunPtr $ \p ->+    act (castFunPtr p) nullPtr++{- | Mesh query visitor: one triangle (three vertices and its index) per call;+return 'True' to continue. The vertices arrive by value in C, so this+routes through @hs_b3MeshQueryTrampoline@.+-}+type MeshQuery = Vec3 -> Vec3 -> Vec3 -> CInt -> IO Bool++type MeshQueryC = Ptr Vec3 -> Ptr Vec3 -> Ptr Vec3 -> CInt -> Ptr () -> IO CBool++foreign import ccall "wrapper"+  wrapMeshQuery :: MeshQueryC -> IO (FunPtr MeshQueryC)++foreign import ccall unsafe "&hs_b3MeshQueryTrampoline"+  p_meshQueryTrampoline :: FunPtr MeshQueryFcn++{- | Run an action with a mesh-query visitor and the context to pass alongside+it; both are released on exit.+-}+withMeshQueryFcn :: MeshQuery -> (FunPtr MeshQueryFcn -> Ptr () -> IO a) -> IO a+withMeshQueryFcn f act =+  bracket (wrapMeshQuery go) freeHaskellFunPtr $ \hsFn ->+    withClosure (castFunPtr hsFn) $ act p_meshQueryTrampoline+  where+    go pa pb pc ix _ctx = do+      a <- peek pa+      b <- peek pb+      c <- peek pc+      fromBool <$> f a b c ix++-- Dynamic-tree visitors -----------------------------------------------------++{- | Tree query visitor: @proxyId userData@; return 'True' to continue the+query.+-}+type TreeQuery = CInt -> Word64 -> IO Bool++type TreeQueryC = CInt -> Word64 -> Ptr () -> IO CBool++foreign import ccall "wrapper"+  wrapTreeQuery :: TreeQueryC -> IO (FunPtr TreeQueryC)++{- | Run an action with a tree-query visitor and the context to pass alongside+it (to 'Box3D.DynamicTree.query'); both are released on exit.+-}+withTreeQueryFcn :: TreeQuery -> (FunPtr TreeQueryCallbackFcn -> Ptr () -> IO a) -> IO a+withTreeQueryFcn f act =+  bracket (wrapTreeQuery (\i u _ -> fromBool <$> f i u)) freeHaskellFunPtr $ \p ->+    act (castFunPtr p) nullPtr++{- | Closest-point tree query visitor: @distanceSqrMin proxyId userData@; the+returned value narrows the search radius (squared).+-}+type TreeQueryClosest = Float -> CInt -> Word64 -> IO Float++type TreeQueryClosestC = Float -> CInt -> Word64 -> Ptr () -> IO Float++foreign import ccall "wrapper"+  wrapTreeQueryClosest :: TreeQueryClosestC -> IO (FunPtr TreeQueryClosestC)++{- | Run an action with a closest-query visitor and the context to pass+alongside it (to 'Box3D.DynamicTree.queryClosest'); both are released on+exit.+-}+withTreeQueryClosestFcn :: TreeQueryClosest -> (FunPtr TreeQueryClosestCallbackFcn -> Ptr () -> IO a) -> IO a+withTreeQueryClosestFcn f act =+  bracket (wrapTreeQueryClosest (\d i u _ -> f d i u)) freeHaskellFunPtr $ \p ->+    act (castFunPtr p) nullPtr++{- | Tree ray-cast visitor: the (possibly clipped) input, @proxyId@ and+@userData@. The returned fraction controls the query: 0 terminates, the+given fraction clips the ray, 1 continues.+-}+type TreeRayCast = RayCastInput -> CInt -> Word64 -> IO Float++type TreeRayCastC = Ptr RayCastInput -> CInt -> Word64 -> Ptr () -> IO Float++foreign import ccall "wrapper"+  wrapTreeRayCast :: TreeRayCastC -> IO (FunPtr TreeRayCastC)++{- | Run an action with a tree ray-cast visitor and the context to pass+alongside it (to 'Box3D.DynamicTree.rayCast'); both are released on exit.+-}+withTreeRayCastFcn :: TreeRayCast -> (FunPtr TreeRayCastCallbackFcn -> Ptr () -> IO a) -> IO a+withTreeRayCastFcn f act =+  bracket (wrapTreeRayCast go) freeHaskellFunPtr $ \p ->+    act (castFunPtr p) nullPtr+  where+    go pInput i u _ctx = do+      input <- peek pInput+      f input i u++{- | Tree box-cast visitor. The input stays a raw 'Ptr' ('BoxCastInput' has no+'Storable' yet: it embeds a 'Box3D.Tags.ShapeProxy' point cloud).+-}+type TreeBoxCast = Ptr BoxCastInput -> CInt -> Word64 -> IO Float++type TreeBoxCastC = Ptr BoxCastInput -> CInt -> Word64 -> Ptr () -> IO Float++foreign import ccall "wrapper"+  wrapTreeBoxCast :: TreeBoxCastC -> IO (FunPtr TreeBoxCastC)++{- | Run an action with a tree box-cast visitor and the context to pass+alongside it (to 'Box3D.DynamicTree.boxCast'); both are released on exit.+-}+withTreeBoxCastFcn :: TreeBoxCast -> (FunPtr TreeBoxCastCallbackFcn -> Ptr () -> IO a) -> IO a+withTreeBoxCastFcn f act =+  bracket (wrapTreeBoxCast (\p i u _ -> f p i u)) freeHaskellFunPtr $ \p ->+    act (castFunPtr p) nullPtr++{- | Allocate the two-pointer @hs_b3Closure@ a trampoline dereferences: the+Haskell-wrapped function and the user context it should receive.+-}+withClosure :: FunPtr a -> (Ptr () -> IO b) -> IO b+withClosure fn act =+  allocaBytes (2 * sz) $ \p -> do+    pokeByteOff p 0 (castFunPtrToPtr fn)+    pokeByteOff p sz nullPtr+    act (castPtr p)+  where+    sz = sizeOf nullPtr++-- Debug draw ----------------------------------------------------------------+--+-- b3DebugDraw is a table of nine callbacks sharing a single context field, so+-- the slots cannot each carry their own hs_b3Closure. Every slot instead gets+-- a fixed C trampoline (see box3d_shim.h) reading its own function out of one+-- wider hs_b3DrawClosure, which the context points at.++{- | The debug-draw callbacks as ordinary Haskell functions. Start from+'noDraw' and fill in the ones the renderer draws.++A 'Nothing' slot stays NULL in the closure, so its trampoline returns without+entering Haskell at all — leave the primitives you do not draw unset rather+than binding them to a no-op.++Geometry arrives in world coordinates ('Pos'); the slots that also take a+'WorldTransform' place their own local geometry in that frame, and 'toWorld'+converts.+-}+data Draw = Draw+  { shape :: Maybe (Ptr () -> WorldTransform -> HexColor -> IO ())+  {- ^ A user shape that passed the 'drawingBounds' cull, as returned by the+  world's @createDebugShape@ callback.+  -}+  , segment :: Maybe (Pos -> Pos -> HexColor -> IO ())+  , transform :: Maybe (WorldTransform -> IO ())+  -- ^ Choose your own length scale.+  , point :: Maybe (Pos -> Float -> HexColor -> IO ())+  , sphere :: Maybe (Pos -> Float -> HexColor -> Float -> IO ())+  -- ^ @center radius color alpha@+  , capsule :: Maybe (Pos -> Pos -> Float -> HexColor -> Float -> IO ())+  -- ^ @p1 p2 radius color alpha@+  , bounds :: Maybe (AABB -> HexColor -> IO ())+  , box :: Maybe (Vec3 -> WorldTransform -> HexColor -> IO ())+  -- ^ An oriented box, given by its extents in the transform's frame.+  , string :: Maybe (Pos -> String -> HexColor -> IO ())+  -- ^ Decoded as UTF-8, substituting @U+FFFD@ for undecodable bytes.+  }++-- | Every slot unset.+noDraw :: Draw+noDraw =+  Draw+    { shape = Nothing+    , segment = Nothing+    , transform = Nothing+    , point = Nothing+    , sphere = Nothing+    , capsule = Nothing+    , bounds = Nothing+    , box = Nothing+    , string = Nothing+    }++{- | Move a 'Vec3' out of a 'WorldTransform'\'s frame into world coordinates,+for the slots that hand over local geometry.+-}+toWorld :: WorldTransform -> Vec3 -> Pos+toWorld = transformWorldPoint++type DrawShapeC = Ptr () -> Ptr WorldTransform -> HexColor -> Ptr () -> IO ()+type DrawSegmentC = Ptr Pos -> Ptr Pos -> HexColor -> Ptr () -> IO ()+type DrawTransformC = Ptr WorldTransform -> Ptr () -> IO ()+type DrawPointC = Ptr Pos -> Float -> HexColor -> Ptr () -> IO ()+type DrawSphereC = Ptr Pos -> Float -> HexColor -> Float -> Ptr () -> IO ()+type DrawCapsuleC = Ptr Pos -> Ptr Pos -> Float -> HexColor -> Float -> Ptr () -> IO ()+type DrawBoundsC = Ptr AABB -> HexColor -> Ptr () -> IO ()+type DrawBoxC = Ptr Vec3 -> Ptr WorldTransform -> HexColor -> Ptr () -> IO ()+type DrawStringC = Ptr Pos -> CString -> HexColor -> Ptr () -> IO ()++foreign import ccall "wrapper" wrapDrawShape :: DrawShapeC -> IO (FunPtr DrawShapeC)+foreign import ccall "wrapper" wrapDrawSegment :: DrawSegmentC -> IO (FunPtr DrawSegmentC)+foreign import ccall "wrapper" wrapDrawTransform :: DrawTransformC -> IO (FunPtr DrawTransformC)+foreign import ccall "wrapper" wrapDrawPoint :: DrawPointC -> IO (FunPtr DrawPointC)+foreign import ccall "wrapper" wrapDrawSphere :: DrawSphereC -> IO (FunPtr DrawSphereC)+foreign import ccall "wrapper" wrapDrawCapsule :: DrawCapsuleC -> IO (FunPtr DrawCapsuleC)+foreign import ccall "wrapper" wrapDrawBounds :: DrawBoundsC -> IO (FunPtr DrawBoundsC)+foreign import ccall "wrapper" wrapDrawBox :: DrawBoxC -> IO (FunPtr DrawBoxC)+foreign import ccall "wrapper" wrapDrawString :: DrawStringC -> IO (FunPtr DrawStringC)++foreign import ccall unsafe "&hs_b3DrawShapeTrampoline" p_drawShape :: FunPtr DrawShapeFcn+foreign import ccall unsafe "&hs_b3DrawSegmentTrampoline" p_drawSegment :: FunPtr DrawSegmentFcn+foreign import ccall unsafe "&hs_b3DrawTransformTrampoline" p_drawTransform :: FunPtr DrawTransformFcn+foreign import ccall unsafe "&hs_b3DrawPointTrampoline" p_drawPoint :: FunPtr DrawPointFcn+foreign import ccall unsafe "&hs_b3DrawSphereTrampoline" p_drawSphere :: FunPtr DrawSphereFcn+foreign import ccall unsafe "&hs_b3DrawCapsuleTrampoline" p_drawCapsule :: FunPtr DrawCapsuleFcn+foreign import ccall unsafe "&hs_b3DrawBoundsTrampoline" p_drawBounds :: FunPtr DrawBoundsFcn+foreign import ccall unsafe "&hs_b3DrawBoxTrampoline" p_drawBox :: FunPtr DrawBoxFcn+foreign import ccall unsafe "&hs_b3DrawStringTrampoline" p_drawString :: FunPtr DrawStringFcn++{- | A wired-up 'DebugDraw', ready to hand to 'draw'.++Opaque because the underlying @b3DebugDraw@ and its nine trampoline slots are+only alive for the duration of the 'withDraw' that produced it, and because+overwriting the struct would unwire them.+-}+data DrawTarget = DrawTarget+  { pointer :: Ptr DebugDraw+  , failure :: IORef (Maybe SomeException)+  }++{- | Run an action with a 'DebugDraw' wired to a Haskell 'Draw', to be handed+to 'draw'. The flags, drawing bounds and scales are taken from the given+'DebugDraw' — start from 'Box3D.DebugDraw.defaultDebugDraw' — while its+callback slots and context are replaced.++Everything is released on exit, so the 'DrawTarget' must not escape the+action. Reuse it across frames by keeping the render loop inside: each call+wraps (and frees) a 'FunPtr' stub per set slot.+-}+withDraw :: Draw -> DebugDraw -> (DrawTarget -> IO a) -> IO a+withDraw d base act = do+  validBounds <- MathFunctions.isValidAABB base.drawingBounds+  unless validBounds $+    error "Box3D.Callbacks.withDraw: DebugDraw.drawingBounds is not a valid AABB"+  failed <- newIORef Nothing+  allocaBytes DrawClosure.size $ \closure ->+    withSlots+      [ mkSlot (DrawClosure.shape closure) wrapDrawShape d.shape $+          \f userShape pXf color _ -> trapping failed $ do+            xf <- peek pXf+            f userShape xf color+      , mkSlot (DrawClosure.segment closure) wrapDrawSegment d.segment $+          \f pP1 pP2 color _ -> trapping failed $ do+            p1 <- peek pP1+            p2 <- peek pP2+            f p1 p2 color+      , mkSlot (DrawClosure.transform closure) wrapDrawTransform d.transform $+          \f pXf _ -> trapping failed (peek pXf >>= f)+      , mkSlot (DrawClosure.point closure) wrapDrawPoint d.point $+          \f pP size color _ -> trapping failed $ do+            p <- peek pP+            f p size color+      , mkSlot (DrawClosure.sphere closure) wrapDrawSphere d.sphere $+          \f pP radius color alpha _ -> trapping failed $ do+            p <- peek pP+            f p radius color alpha+      , mkSlot (DrawClosure.capsule closure) wrapDrawCapsule d.capsule $+          \f pP1 pP2 radius color alpha _ -> trapping failed $ do+            p1 <- peek pP1+            p2 <- peek pP2+            f p1 p2 radius color alpha+      , mkSlot (DrawClosure.bounds closure) wrapDrawBounds d.bounds $+          \f pAabb color _ -> trapping failed $ do+            aabb <- peek pAabb+            f aabb color+      , mkSlot (DrawClosure.box closure) wrapDrawBox d.box $+          \f pExtents pXf color _ -> trapping failed $ do+            extents <- peek pExtents+            xf <- peek pXf+            f extents xf color+      , mkSlot (DrawClosure.string closure) wrapDrawString d.string $+          \f pP cs color _ -> trapping failed $ do+            p <- peek pP+            s <- peekUtf8 cs+            f p s color+      ]+      $ do+        DrawClosure.context closure base.context+        with+          base+            { drawShapeFcn = p_drawShape+            , drawSegmentFcn = p_drawSegment+            , drawTransformFcn = p_drawTransform+            , drawPointFcn = p_drawPoint+            , drawSphereFcn = p_drawSphere+            , drawCapsuleFcn = p_drawCapsule+            , drawBoundsFcn = p_drawBounds+            , drawBoxFcn = p_drawBox+            , drawStringFcn = p_drawString+            , context = closure+            }+          $ \p -> do+            let target = DrawTarget{pointer = p, failure = failed}+            r <- act target+            rethrowTrapped target+            pure r++{- | Draw the world through a 'DrawTarget'.++An exception from a 'Draw' callback surfaces here. Binds @b3World_Draw@ via+'Box3D.World.draw'.+-}+draw :: WorldId -> Word64 -> DrawTarget -> IO ()+draw worldId maskBits target = withDrawPtr target $ \p -> World.draw worldId p maskBits++{- | Hand a 'DrawTarget'\'s @b3DebugDraw@ to a C call other than+'draw' — @b3RecPlayer_DrawFrameQueries@, say — rethrowing a trapped callback+exception afterwards. An action that throws on its own drops any exception+trapped meanwhile, so the next frame starts with every slot live. The pointer+must not escape the action.+-}+withDrawPtr :: DrawTarget -> (Ptr DebugDraw -> IO a) -> IO a+withDrawPtr target act =+  (act target.pointer <* rethrowTrapped target)+    `onException` writeIORef target.failure Nothing++{- | Run one draw callback, keeping any exception on the Haskell side of the+call.++An exception cannot unwind through the engine frames between a callback and+'draw' — GHC terminates the program instead. So the first one is stashed and+every later slot becomes a no-op, leaving the engine to finish its walk over+a world it is midway through iterating; 'draw' then rethrows.+-}+trapping :: IORef (Maybe SomeException) -> IO () -> IO ()+trapping ref act = do+  prior <- readIORef ref+  case prior of+    Just _ -> pure ()+    Nothing -> keepFirst ref () act++keepFirst :: IORef (Maybe SomeException) -> a -> IO a -> IO a+keepFirst ref fallback act =+  try act >>= either (\e -> fallback <$ atomicModifyIORef' ref (\prior -> (prior <|> Just e, ()))) pure++-- | Take and clear a trapped callback exception.+rethrowTrapped :: DrawTarget -> IO ()+rethrowTrapped target = rethrowFirst target.failure++rethrowFirst :: IORef (Maybe SomeException) -> IO ()+rethrowFirst ref = atomicModifyIORef' ref (Nothing,) >>= mapM_ throwIO++-- | One debug-draw primitive, as the engine emitted it.+data Cmd+  = DrawShape (Ptr ()) WorldTransform HexColor+  | DrawSegment Pos Pos HexColor+  | DrawTransform WorldTransform+  | DrawPoint Pos Float HexColor+  | DrawSphere Pos Float HexColor Float+  | DrawCapsule Pos Pos Float HexColor Float+  | DrawBounds AABB HexColor+  | DrawBox Vec3 WorldTransform HexColor+  | DrawString Pos String HexColor+  deriving (Eq, Show)++{- | Collect a frame's primitives instead of rendering from inside the+callbacks, in the order the engine emitted them.++The engine leaves the world unlocked while drawing but is midway through+iterating it, so mutating from a 'Draw' callback corrupts that walk. Rendering+from the returned list happens after 'draw' has returned, where the world is+free to touch again.++A 'DrawShape' carries a raw handle from 'CreateDebugShape', which the matching+'DestroyDebugShape' reclaims as soon as its shape is changed or destroyed —+so render the list before touching the shapes it names, not after.+-}+withRecorder :: DebugDraw -> (DrawTarget -> IO a) -> IO (a, [Cmd])+withRecorder base act = do+  acc <- newIORef []+  let+    record c = modifyIORef' acc (c :)+    recording =+      Draw+        { shape = Just $ \u xf c -> record (DrawShape u xf c)+        , segment = Just $ \p1 p2 c -> record (DrawSegment p1 p2 c)+        , transform = Just $ record . DrawTransform+        , point = Just $ \p s c -> record (DrawPoint p s c)+        , sphere = Just $ \p r c a -> record (DrawSphere p r c a)+        , capsule = Just $ \p1 p2 r c a -> record (DrawCapsule p1 p2 r c a)+        , bounds = Just $ \b c -> record (DrawBounds b c)+        , box = Just $ \e xf c -> record (DrawBox e xf c)+        , string = Just $ \p s c -> record (DrawString p s c)+        }+  r <- withDraw recording base act+  cmds <- readIORef acc+  pure (r, reverse cmds)++-- Debug shapes --------------------------------------------------------------++{- | Build the renderer's representation of a shape, returning the handle the+'Draw' @shape@ slot will hand back. Called from @b3World_Draw@ for every+shape in the drawing bounds that has no handle yet.++The engine takes 'nullPtr' as "no handle yet", not as "nothing to draw": it+asks again on every draw, never hands the shape to the @shape@ slot and never+runs 'DestroyDebugShape' for it. Return a real handle for a shape the renderer+skips, and have the @shape@ slot ignore it.+-}+type CreateDebugShape = DebugShape -> IO (Ptr ())++-- | Release a handle 'CreateDebugShape' returned.+type DestroyDebugShape = Ptr () -> IO ()++type CreateDebugShapeC = Ptr DebugShape -> Ptr () -> IO (Ptr ())+type DestroyDebugShapeC = Ptr () -> Ptr () -> IO ()++foreign import ccall "wrapper" wrapCreateDebugShape :: CreateDebugShapeC -> IO (FunPtr CreateDebugShapeC)+foreign import ccall "wrapper" wrapDestroyDebugShape :: DestroyDebugShapeC -> IO (FunPtr DestroyDebugShapeC)++foreign import ccall unsafe "&hs_b3NoopDestroyDebugShape"+  p_noopDestroyDebugShape :: FunPtr DestroyDebugShapeCallback++{- | The pair to poke into @WorldDef.createDebugShape@ /+@WorldDef.destroyDebugShape@ before creating the world.+-}+data DebugShapes = DebugShapes+  { create :: FunPtr CreateDebugShapeCallback+  , destroy :: FunPtr DestroyDebugShapeCallback+  , failure :: IORef (Maybe SomeException)+  }++{- | Install a matched pair of debug-shape callbacks for the duration of an+action; both are freed on exit, so the world must not outlive it.++Passing 'Nothing' for the destructor installs a C no-op, so the handles are+dropped without being released.++The destructor runs on paths that are plain teardown from Haskell's side —+'Box3D.Shape.destroy', 'Box3D.Body.destroy', 'Box3D.World.destroy', the+@Shape.set*@ family and replay seeks — which is why those are @safe@ imports.++An exception from either callback is kept on the Haskell side of the engine+call. A shape whose 'CreateDebugShape' threw is left without a handle, so the+engine asks again on the next draw, and a throwing 'DestroyDebugShape' does not+stop the remaining handles being released. The first exception is rethrown by+'rethrowDebugShapes', which a render loop should call after 'draw', or else when+the action returns.+-}+withDebugShapes :: CreateDebugShape -> Maybe DestroyDebugShape -> (DebugShapes -> IO a) -> IO a+withDebugShapes mk free_ act = do+  failed <- newIORef Nothing+  bracket (wrapCreateDebugShape (\p _ -> keepFirst failed nullPtr (peek p >>= mk))) freeHaskellFunPtr $ \c ->+    withDestroy failed $ \d -> do+      let shapes = DebugShapes{create = castFunPtr c, destroy = d, failure = failed}+      r <- act shapes+      rethrowDebugShapes shapes+      pure r+  where+    withDestroy failed k = case free_ of+      Nothing -> k p_noopDestroyDebugShape+      Just f -> bracket (wrapDestroyDebugShape (\u _ -> keepFirst failed () (f u))) freeHaskellFunPtr (k . castFunPtr)++rethrowDebugShapes :: DebugShapes -> IO ()+rethrowDebugShapes shapes = rethrowFirst shapes.failure++{- | One @hs_b3DrawClosure@ slot: where the wrapped callback is stored, and+how to make it. Unset slots are stored as NULL, which the trampoline+short-circuits without entering Haskell.+-}+data Slot = Slot (FunPtr () -> IO ()) (Maybe (IO (FunPtr ())))++{- | Build a 'Slot' from a 'Draw' field: @adapt@ turns the Haskell callback+into the C signature its @wrapper@ import expects.+-}+mkSlot :: (FunPtr () -> IO ()) -> (c -> IO (FunPtr c)) -> Maybe hs -> (hs -> c) -> Slot+mkSlot poker wrap mf adapt =+  Slot poker (fmap (\f -> castFunPtr <$> wrap (adapt f)) mf)++{- | Fill the draw closure, freeing every wrapper on exit.++Each slot names the @hs_b3DrawClosure@ field it writes, so the struct's layout+is hsc2hs's problem: adding a callback upstream is a compile error here rather+than a silent read past the end of a flat array.+-}+withSlots :: [Slot] -> IO a -> IO a+withSlots [] act = act+withSlots (Slot poker make : rest) act =+  case make of+    Nothing -> poker nullFunPtr >> withSlots rest act+    Just mk -> bracket mk freeHaskellFunPtr $ \p -> poker p >> withSlots rest act++{- | Decode a C string as UTF-8.++Box3D hands back names the caller gave it, so the encoding is really the+caller's; UTF-8 with @U+FFFD@ for undecodable bytes keeps a mislabelled name+from throwing mid-frame the way the locale-dependent 'peekCString' would.+-}+peekUtf8 :: CString -> IO String+peekUtf8 = GHC.Foreign.peekCString (mkUTF8 TransliterateCodingFailure)++-- A multithreaded task system --------------------------------------------++{- | The two callbacks (ready to poke into @WorldDef.enqueueTask@ /+@WorldDef.finishTask@) plus the worker count to set on @WorldDef.workerCount@.+-}+data TaskSystem = TaskSystem+  { enqueue :: FunPtr EnqueueTaskCallback+  , finish :: FunPtr FinishTaskCallback+  , workerCount :: Int+  }++-- Box3D's enqueue takes a trailing @const char* taskName@ we ignore.+-- The engine's per-task work function: @void b2TaskCallback(void* taskContext)@.+type TaskC = Ptr () -> IO ()++-- Calling it re-enters Haskell (the solver work invokes our other callbacks),+-- so it must be a `safe` call.+foreign import ccall safe "dynamic"+  callTask :: FunPtr TaskC -> TaskC++type EnqueueC = FunPtr TaskC -> Ptr () -> Ptr () -> Ptr CChar -> IO (Ptr ())+type FinishC = Ptr () -> Ptr () -> IO ()++foreign import ccall "wrapper" wrapEnqueue :: EnqueueC -> IO (FunPtr EnqueueC)+foreign import ccall "wrapper" wrapFinish :: FinishC -> IO (FunPtr FinishC)++{- | Install a Haskell thread-pool task system for the duration of an action,+so the Box3D solver runs across the threaded runtime's capabilities. Set+@WorldDef.workerCount@, @WorldDef.enqueueTask@ and @WorldDef.finishTask@ from the+'TaskSystem' before creating the world.++Each enqueued task is run on a fresh 'forkIO' thread; @finishTask@ joins it.+The task's handle travels through the engine as a 'StablePtr' to its join+'MVar'. Requires @-threaded@.+-}+withThreadPoolTaskSystem :: Int -> (TaskSystem -> IO a) -> IO a+withThreadPoolTaskSystem workers act =+  bracket (wrapEnqueue enqueue) freeHaskellFunPtr $ \enq ->+    bracket (wrapFinish finish) freeHaskellFunPtr $ \fin ->+      act+        TaskSystem+          { enqueue = castFunPtr enq+          , finish = castFunPtr fin+          , workerCount = workers+          }+  where+    enqueue :: EnqueueC+    enqueue task taskCtx _userCtx _taskName = do+      done <- newEmptyMVar+      _ <- forkIO (callTask task taskCtx `finally` putMVar done ())+      castStablePtrToPtr <$> newStablePtr done++    finish :: FinishC+    finish userTask _userCtx = do+      let sp = castPtrToStablePtr userTask :: StablePtr (MVar ())+      done <- deRefStablePtr sp+      takeMVar done+      freeStablePtr sp
+ src/Box3D/Collision.hsc view
@@ -0,0 +1,1819 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.Collision+  ( LocalManifoldPoint (..)+  , FeaturePair (..)+  , SATCache (..)+  , TriangleFeature (..)+  , pattern FeatureNone+  , pattern FeatureTriangleFace+  , pattern FeatureHullFace+  , pattern FeatureEdge1+  , pattern FeatureEdge2+  , pattern FeatureEdge3+  , pattern FeatureVertex1+  , pattern FeatureVertex2+  , pattern FeatureVertex3+  , SeparatingFeature (..)+  , pattern InvalidAxis+  , pattern BacksideAxis+  , pattern FaceAxisA+  , pattern FaceAxisB+  , pattern EdgePairAxis+  , pattern ClosestPointsAxis+  , pattern ManualFaceAxisA+  , pattern ManualFaceAxisB+  , pattern ManualEdgePairAxis+  , Manifold (..)+  , ManifoldPoint (..)+  , BodyPlaneResult (..)+  , PlaneSolverResult (..)+  , CollisionPlane (..)+  , PlaneResult (..)+  , maxManifoldPoints+  , getWorldCount+  , getMaxWorldCount+  , saveRecordingToFile+  , loadRecordingFromFile+  , validateReplay+  , cloneHull+  , cloneAndTransformHull+  , makeCubeHull+  , makeBoxHull+  , makeOffsetBoxHull+  , makeTransformedBoxHull+  , makeScaledBoxHull+  , scaleBox+  , getHeight+  , dumpHeightData+  , loadHeightField+  , getCompoundChild+  , queryCompound+  , getCompoundCapsule+  , getCompoundHull+  , getCompoundMesh+  , getCompoundSphere+  , getCompoundMaterials+  , convertCompoundToBytes+  , convertBytesToCompound+  , computeSphereMass+  , computeCapsuleMass+  , computeHullMass+  , computeHullMassInto+  , computeSphereAABB+  , computeCapsuleAABB+  , computeHullAABB+  , computeMeshAABB+  , computeHeightFieldAABB+  , computeCompoundAABB+  , isValidRay+  , overlapCapsule+  , overlapCompound+  , overlapHeightField+  , overlapHull+  , overlapMesh+  , overlapSphere+  , rayCastSphere+  , rayCastHollowSphere+  , rayCastCapsule+  , rayCastCompound+  , rayCastHull+  , rayCastMesh+  , rayCastHeightField+  , shapeCastSphere+  , shapeCastCapsule+  , shapeCastCompound+  , shapeCastCompoundInto+  , shapeCastHull+  , shapeCastHullInto+  , shapeCastMesh+  , shapeCastMeshInto+  , shapeCastHeightField+  , shapeCastHeightFieldInto+  , queryMesh+  , queryHeightField+  , shapeDistance+  , shapeCast+  , shapeCastInto+  , getSweepTransform+  , timeOfImpact+  , collideSpheres+  , collideCapsuleAndSphere+  , collideHullAndSphere+  , collideCapsules+  , collideHullAndCapsule+  , collideHulls+  , collideTriangleAndCapsule+  , collideTriangleAndHull+  , collideTriangleAndSphere+  , solvePlanes+  , clipVector+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..))+import Foreign.C.String (CString)+import Data.Vector.Storable qualified as VS+import Box3D.Internal (copyBounded, pokeBounded)+import Box3D.Compound (CompoundCapsule, CompoundData, CompoundHull, CompoundMesh, CompoundSphere)+import Box3D.Geometry (Capsule, MassData, Sphere)+import Box3D.HeightField (HeightFieldData, HeightFieldDef)+import Box3D.Hull (HullData)+import Box3D.Id (ShapeId)+import Box3D.MathTypes (AABB, Plane, Transform, Vec3)+import Box3D.Mesh (Mesh, MeshData)+import Box3D.Query (CastOutput, DistanceInput, DistanceOutput, RayCastInput, ShapeCastInput, ShapeCastPairInput, ShapeProxy, SimplexCache, Sweep, TOIInput, TOIOutput)+import {-# SOURCE #-} Box3D.Shape (SurfaceMaterial)+import Box3D.Tags (BoxHull, ChildShape, CompoundQueryFcn, LocalManifold, MeshQueryFcn, Recording, Simplex)++#include "box3d/box3d.h"+++-- | A local manifold point and normal in frame A.+data LocalManifoldPoint = LocalManifoldPoint+  { point :: Vec3+  -- ^ Local point in frame A.+  , separation :: Float+  -- ^ The contact point separation. Negative for overlap.+  , pair :: FeaturePair+  -- ^ The feature pair for this point.+  , triangleIndex :: CInt+  -- ^ The triangle index when collide with a mesh or height-field.+  }+  deriving (Eq, Show)++instance Storable LocalManifoldPoint where+  sizeOf _ = #{size b3LocalManifoldPoint}+  alignment _ = #{alignment b3LocalManifoldPoint}+  peek p =+    LocalManifoldPoint+      <$> #{peek b3LocalManifoldPoint, point} p+      <*> #{peek b3LocalManifoldPoint, separation} p+      <*> #{peek b3LocalManifoldPoint, pair} p+      <*> #{peek b3LocalManifoldPoint, triangleIndex} p+  poke p (LocalManifoldPoint f1 f2 f3 f4) = do+    #{poke b3LocalManifoldPoint, point} p f1+    #{poke b3LocalManifoldPoint, separation} p f2+    #{poke b3LocalManifoldPoint, pair} p f3+    #{poke b3LocalManifoldPoint, triangleIndex} p f4++{- | 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.+-}+data FeaturePair = FeaturePair+  { owner1 :: Word8+  -- ^ Incoming type (either edge on shape A or shape B)+  , index1 :: Word8+  -- ^ Incoming edge index (into associated shape array)+  , owner2 :: Word8+  -- ^ Outgoing type (either edge on shape A or shape B)+  , index2 :: Word8+  -- ^ Outgoing edge index (into associated shape array)+  }+  deriving (Eq, Show)++instance Storable FeaturePair where+  sizeOf _ = #{size b3FeaturePair}+  alignment _ = #{alignment b3FeaturePair}+  peek p =+    FeaturePair+      <$> #{peek b3FeaturePair, owner1} p+      <*> #{peek b3FeaturePair, index1} p+      <*> #{peek b3FeaturePair, owner2} p+      <*> #{peek b3FeaturePair, index2} p+  poke p (FeaturePair f1 f2 f3 f4) = do+    #{poke b3FeaturePair, owner1} p f1+    #{poke b3FeaturePair, index1} p f2+    #{poke b3FeaturePair, owner2} p f3+    #{poke b3FeaturePair, index2} p f4++-- | Separating axis test cache. Provides temporal acceleration of collision routines.+data SATCache = SATCache+  { separation :: Float+  -- ^ The separation when the cache is populated. Negative for overlap.+  , type_ :: Word8+  -- ^ @b3SeparatingFeature@.+  , indexA :: Word8+  -- ^ Index of the feature on shape A.+  , indexB :: Word8+  -- ^ Index of the feature on shape B.+  , hit :: Word8+  -- ^ Was the cache re-used?+  }+  deriving (Eq, Show)++instance Storable SATCache where+  sizeOf _ = #{size b3SATCache}+  alignment _ = #{alignment b3SATCache}+  peek p =+    SATCache+      <$> #{peek b3SATCache, separation} p+      <*> #{peek b3SATCache, type} p+      <*> #{peek b3SATCache, indexA} p+      <*> #{peek b3SATCache, indexB} p+      <*> #{peek b3SATCache, hit} p+  poke p (SATCache f1 f2 f3 f4 f5) = do+    #{poke b3SATCache, separation} p f1+    #{poke b3SATCache, type} p f2+    #{poke b3SATCache, indexA} p f3+    #{poke b3SATCache, indexB} p f4+    #{poke b3SATCache, hit} p f5++-- | Cached triangle feature.+newtype TriangleFeature = TriangleFeature CInt+  deriving (Eq, Show, Storable)++pattern FeatureNone :: TriangleFeature+pattern FeatureTriangleFace :: TriangleFeature+pattern FeatureHullFace :: TriangleFeature+-- | v1-v2+pattern FeatureEdge1 :: TriangleFeature+-- | v2-v3+pattern FeatureEdge2 :: TriangleFeature+-- | v3-v1+pattern FeatureEdge3 :: TriangleFeature+pattern FeatureVertex1 :: TriangleFeature+pattern FeatureVertex2 :: TriangleFeature+pattern FeatureVertex3 :: TriangleFeature+pattern FeatureNone = TriangleFeature #{const b3_featureNone}+pattern FeatureTriangleFace = TriangleFeature #{const b3_featureTriangleFace}+pattern FeatureHullFace = TriangleFeature #{const b3_featureHullFace}+pattern FeatureEdge1 = TriangleFeature #{const b3_featureEdge1}+pattern FeatureEdge2 = TriangleFeature #{const b3_featureEdge2}+pattern FeatureEdge3 = TriangleFeature #{const b3_featureEdge3}+pattern FeatureVertex1 = TriangleFeature #{const b3_featureVertex1}+pattern FeatureVertex2 = TriangleFeature #{const b3_featureVertex2}+pattern FeatureVertex3 = TriangleFeature #{const b3_featureVertex3}++-- | Cached separating axis feature.+newtype SeparatingFeature = SeparatingFeature CInt+  deriving (Eq, Show, Storable)++pattern InvalidAxis :: SeparatingFeature+pattern BacksideAxis :: SeparatingFeature+pattern FaceAxisA :: SeparatingFeature+pattern FaceAxisB :: SeparatingFeature+pattern EdgePairAxis :: SeparatingFeature+pattern ClosestPointsAxis :: SeparatingFeature+-- | These are for testing+pattern ManualFaceAxisA :: SeparatingFeature+pattern ManualFaceAxisB :: SeparatingFeature+pattern ManualEdgePairAxis :: SeparatingFeature+pattern InvalidAxis = SeparatingFeature #{const b3_invalidAxis}+pattern BacksideAxis = SeparatingFeature #{const b3_backsideAxis}+pattern FaceAxisA = SeparatingFeature #{const b3_faceAxisA}+pattern FaceAxisB = SeparatingFeature #{const b3_faceAxisB}+pattern EdgePairAxis = SeparatingFeature #{const b3_edgePairAxis}+pattern ClosestPointsAxis = SeparatingFeature #{const b3_closestPointsAxis}+pattern ManualFaceAxisA = SeparatingFeature #{const b3_manualFaceAxisA}+pattern ManualFaceAxisB = SeparatingFeature #{const b3_manualFaceAxisB}+pattern ManualEdgePairAxis = SeparatingFeature #{const b3_manualEdgePairAxis}++{- | A contact manifold describes the contact points between colliding shapes.++__Note:__ Box3D uses speculative collision so some contact points may be separated.+-}+data Manifold = Manifold+  { points :: VS.Vector ManifoldPoint+  -- ^ The manifold points. There may be 1 to 4 valid points.+  , normal :: Vec3+  -- ^ The unit normal vector in world space, points from shape A to shape B+  , twistImpulse :: Float+  -- ^ Central friction angular impulse (applied about the normal)+  , frictionImpulse :: Vec3+  -- ^ Central friction linear impulse+  , rollingImpulse :: Vec3+  -- ^ Rolling resistance angular impulse+  }+  deriving (Eq, Show)++instance Storable Manifold where+  sizeOf _ = #{size b3Manifold}+  alignment _ = #{alignment b3Manifold}+  peek p = do+    nPointCount <- #{peek b3Manifold, pointCount} p :: IO CInt+    Manifold+      <$> copyBounded maxManifoldPoints nPointCount (#{ptr b3Manifold, points} p)+      <*> #{peek b3Manifold, normal} p+      <*> #{peek b3Manifold, twistImpulse} p+      <*> #{peek b3Manifold, frictionImpulse} p+      <*> #{peek b3Manifold, rollingImpulse} p+  poke p x = do+    pokeBounded maxManifoldPoints (#{ptr b3Manifold, points} p) x.points+    #{poke b3Manifold, normal} p x.normal+    #{poke b3Manifold, twistImpulse} p x.twistImpulse+    #{poke b3Manifold, frictionImpulse} p x.frictionImpulse+    #{poke b3Manifold, rollingImpulse} p x.rollingImpulse+    #{poke b3Manifold, pointCount} p (fromIntegral (minimum [maxManifoldPoints, VS.length x.points]) :: CInt)++{- | 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.+-}+data ManifoldPoint = ManifoldPoint+  { anchorA :: Vec3+  -- ^ Location of the contact point relative to the bodyA center of mass in world space.+  , anchorB :: Vec3+  -- ^ Location of the contact point relative to the bodyB center of mass in world space.+  , separation :: Float+  -- ^ The separation of the contact point, negative if penetrating+  , baseSeparation :: Float+  -- ^ Cached separation used for contact recycling+  , normalImpulse :: Float+  -- ^ The impulse along the manifold normal vector. Since Box3D uses sub-stepping, this is+  -- result from the final sub-step.+  , totalNormalImpulse :: Float+  -- ^ The total normal impulse applied during sub-stepping. This is important+  -- to identify speculative contact points that had an interaction in the time step.+  , normalVelocity :: Float+  -- ^ 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.+  , featureId :: Word32+  -- ^ Local point for matching+  -- Uniquely identifies a contact point between two shapes+  , triangleIndex :: CInt+  -- ^ Triangle index if one of the shapes is a mesh or height field+  , persisted :: CBool+  -- ^ Did this contact point exist in the previous step?+  }+  deriving (Eq, Show)++instance Storable ManifoldPoint where+  sizeOf _ = #{size b3ManifoldPoint}+  alignment _ = #{alignment b3ManifoldPoint}+  peek p =+    ManifoldPoint+      <$> #{peek b3ManifoldPoint, anchorA} p+      <*> #{peek b3ManifoldPoint, anchorB} p+      <*> #{peek b3ManifoldPoint, separation} p+      <*> #{peek b3ManifoldPoint, baseSeparation} p+      <*> #{peek b3ManifoldPoint, normalImpulse} p+      <*> #{peek b3ManifoldPoint, totalNormalImpulse} p+      <*> #{peek b3ManifoldPoint, normalVelocity} p+      <*> #{peek b3ManifoldPoint, featureId} p+      <*> #{peek b3ManifoldPoint, triangleIndex} p+      <*> #{peek b3ManifoldPoint, persisted} p+  poke p (ManifoldPoint f1 f2 f3 f4 f5 f6 f7 f8 f9 f10) = do+    #{poke b3ManifoldPoint, anchorA} p f1+    #{poke b3ManifoldPoint, anchorB} p f2+    #{poke b3ManifoldPoint, separation} p f3+    #{poke b3ManifoldPoint, baseSeparation} p f4+    #{poke b3ManifoldPoint, normalImpulse} p f5+    #{poke b3ManifoldPoint, totalNormalImpulse} p f6+    #{poke b3ManifoldPoint, normalVelocity} p f7+    #{poke b3ManifoldPoint, featureId} p f8+    #{poke b3ManifoldPoint, triangleIndex} p f9+    #{poke b3ManifoldPoint, persisted} p f10++-- | Body plane result for movers.+data BodyPlaneResult = BodyPlaneResult+  { shapeId :: ShapeId+  -- ^ The shape id on the body.+  , result :: PlaneResult+  -- ^ The plane result.+  }+  deriving (Eq, Show)++instance Storable BodyPlaneResult where+  sizeOf _ = #{size b3BodyPlaneResult}+  alignment _ = #{alignment b3BodyPlaneResult}+  peek p =+    BodyPlaneResult+      <$> #{peek b3BodyPlaneResult, shapeId} p+      <*> #{peek b3BodyPlaneResult, result} p+  poke p (BodyPlaneResult f1 f2) = do+    #{poke b3BodyPlaneResult, shapeId} p f1+    #{poke b3BodyPlaneResult, result} p f2++-- | Result returned by @b3SolvePlanes@.+data PlaneSolverResult = PlaneSolverResult+  { delta :: Vec3+  -- ^ The final relative translation.+  , iterationCount :: CInt+  -- ^ The number of iterations used by the plane solver. For diagnostics.+  }+  deriving (Eq, Show)++instance Storable PlaneSolverResult where+  sizeOf _ = #{size b3PlaneSolverResult}+  alignment _ = #{alignment b3PlaneSolverResult}+  peek p =+    PlaneSolverResult+      <$> #{peek b3PlaneSolverResult, delta} p+      <*> #{peek b3PlaneSolverResult, iterationCount} p+  poke p (PlaneSolverResult f1 f2) = do+    #{poke b3PlaneSolverResult, delta} p f1+    #{poke b3PlaneSolverResult, iterationCount} p f2++{- | These are collision planes that can be fed to @b3SolvePlanes@. Normally+this is assembled by the user from plane results in @b3PlaneResult@.+-}+data CollisionPlane = CollisionPlane+  { plane :: Plane+  -- ^ The collision plane between the mover and some shape.+  , pushLimit :: Float+  -- ^ Setting this to FLT_MAX makes the plane as rigid as possible. Lower values can+  -- make the plane collision soft. Usually in meters.+  , push :: Float+  -- ^ The push on the mover determined by @b3SolvePlanes@. Usually in meters.+  , clipVelocity :: CBool+  -- ^ Indicates if @b3ClipVector@ should clip against this plane. Should be false for soft collision.+  }+  deriving (Eq, Show)++instance Storable CollisionPlane where+  sizeOf _ = #{size b3CollisionPlane}+  alignment _ = #{alignment b3CollisionPlane}+  peek p =+    CollisionPlane+      <$> #{peek b3CollisionPlane, plane} p+      <*> #{peek b3CollisionPlane, pushLimit} p+      <*> #{peek b3CollisionPlane, push} p+      <*> #{peek b3CollisionPlane, clipVelocity} p+  poke p (CollisionPlane f1 f2 f3 f4) = do+    #{poke b3CollisionPlane, plane} p f1+    #{poke b3CollisionPlane, pushLimit} p f2+    #{poke b3CollisionPlane, push} p f3+    #{poke b3CollisionPlane, clipVelocity} p f4++-- | The plane between a character mover and a shape+data PlaneResult = PlaneResult+  { plane :: Plane+  -- ^ Outward pointing plane.+  , point :: Vec3+  -- ^ Closest point on the shape. May not be unique.+  }+  deriving (Eq, Show)++instance Storable PlaneResult where+  sizeOf _ = #{size b3PlaneResult}+  alignment _ = #{alignment b3PlaneResult}+  peek p =+    PlaneResult+      <$> #{peek b3PlaneResult, plane} p+      <*> #{peek b3PlaneResult, point} p+  poke p (PlaneResult f1 f2) = do+    #{poke b3PlaneResult, plane} p f1+    #{poke b3PlaneResult, point} p f2++-- | @B3_MAX_MANIFOLD_POINTS@+maxManifoldPoints :: Int+maxManifoldPoints = #{const B3_MAX_MANIFOLD_POINTS}++++foreign import ccall unsafe "b3GetWorldCount"+  c_b3GetWorldCount :: IO CInt++foreign import ccall unsafe "b3GetMaxWorldCount"+  c_b3GetMaxWorldCount :: IO CInt++foreign import ccall unsafe "b3SaveRecordingToFile"+  c_b3SaveRecordingToFile :: Ptr Recording -> CString -> IO CBool++foreign import ccall unsafe "b3LoadRecordingFromFile"+  c_b3LoadRecordingFromFile :: CString -> IO (Ptr Recording)++foreign import ccall unsafe "b3ValidateReplay"+  c_b3ValidateReplay :: Ptr () -> CInt -> CInt -> IO CBool++foreign import ccall unsafe "b3CloneHull"+  c_b3CloneHull :: Ptr HullData -> IO (Ptr HullData)++foreign import ccall unsafe "hsg_b3CloneAndTransformHull"+  c_b3CloneAndTransformHull :: Ptr HullData -> Ptr Transform -> Ptr Vec3 -> IO (Ptr HullData)++foreign import ccall unsafe "hsg_b3MakeCubeHull"+  c_b3MakeCubeHull :: Float -> Ptr BoxHull -> IO ()++foreign import ccall unsafe "hsg_b3MakeBoxHull"+  c_b3MakeBoxHull :: Float -> Float -> Float -> Ptr BoxHull -> IO ()++foreign import ccall unsafe "hsg_b3MakeOffsetBoxHull"+  c_b3MakeOffsetBoxHull :: Float -> Float -> Float -> Ptr Vec3 -> Ptr BoxHull -> IO ()++foreign import ccall unsafe "hsg_b3MakeTransformedBoxHull"+  c_b3MakeTransformedBoxHull :: Float -> Float -> Float -> Ptr Transform -> Ptr BoxHull -> IO ()++foreign import ccall unsafe "hsg_b3MakeScaledBoxHull"+  c_b3MakeScaledBoxHull :: Ptr Vec3 -> Ptr Transform -> Ptr Vec3 -> Ptr BoxHull -> IO ()++foreign import ccall unsafe "hsg_b3ScaleBox"+  c_b3ScaleBox :: Ptr Vec3 -> Ptr Transform -> Ptr Vec3 -> Float -> IO ()++foreign import ccall unsafe "b3GetHeight"+  c_b3GetHeight :: Ptr MeshData -> IO CInt++foreign import ccall unsafe "b3DumpHeightData"+  c_b3DumpHeightData :: Ptr HeightFieldDef -> CString -> IO ()++foreign import ccall unsafe "b3LoadHeightField"+  c_b3LoadHeightField :: CString -> IO (Ptr HeightFieldData)++foreign import ccall unsafe "hsg_b3GetCompoundChild"+  c_b3GetCompoundChild :: Ptr CompoundData -> CInt -> Ptr ChildShape -> IO ()++foreign import ccall safe "hsg_b3QueryCompound"+  c_b3QueryCompound :: Ptr CompoundData -> Ptr AABB -> FunPtr CompoundQueryFcn -> Ptr () -> IO ()++foreign import ccall unsafe "hsg_b3GetCompoundCapsule"+  c_b3GetCompoundCapsule :: Ptr CompoundData -> CInt -> Ptr CompoundCapsule -> IO ()++foreign import ccall unsafe "hsg_b3GetCompoundHull"+  c_b3GetCompoundHull :: Ptr CompoundData -> CInt -> Ptr CompoundHull -> IO ()++foreign import ccall unsafe "hsg_b3GetCompoundMesh"+  c_b3GetCompoundMesh :: Ptr CompoundData -> CInt -> Ptr CompoundMesh -> IO ()++foreign import ccall unsafe "hsg_b3GetCompoundSphere"+  c_b3GetCompoundSphere :: Ptr CompoundData -> CInt -> Ptr CompoundSphere -> IO ()++foreign import ccall unsafe "b3GetCompoundMaterials"+  c_b3GetCompoundMaterials :: Ptr CompoundData -> IO (Ptr SurfaceMaterial)++foreign import ccall unsafe "b3ConvertCompoundToBytes"+  c_b3ConvertCompoundToBytes :: Ptr CompoundData -> IO (Ptr Word8)++foreign import ccall unsafe "b3ConvertBytesToCompound"+  c_b3ConvertBytesToCompound :: Ptr Word8 -> CInt -> IO (Ptr CompoundData)++foreign import ccall unsafe "hsg_b3ComputeSphereMass"+  c_b3ComputeSphereMass :: Ptr Sphere -> Float -> Ptr MassData -> IO ()++foreign import ccall unsafe "hsg_b3ComputeCapsuleMass"+  c_b3ComputeCapsuleMass :: Ptr Capsule -> Float -> Ptr MassData -> IO ()++foreign import ccall unsafe "hsg_b3ComputeHullMass"+  c_b3ComputeHullMass :: Ptr HullData -> Float -> Ptr MassData -> IO ()++foreign import ccall unsafe "hsg_b3ComputeSphereAABB"+  c_b3ComputeSphereAABB :: Ptr Sphere -> Ptr Transform -> Ptr AABB -> IO ()++foreign import ccall unsafe "hsg_b3ComputeCapsuleAABB"+  c_b3ComputeCapsuleAABB :: Ptr Capsule -> Ptr Transform -> Ptr AABB -> IO ()++foreign import ccall unsafe "hsg_b3ComputeHullAABB"+  c_b3ComputeHullAABB :: Ptr HullData -> Ptr Transform -> Ptr AABB -> IO ()++foreign import ccall unsafe "hsg_b3ComputeMeshAABB"+  c_b3ComputeMeshAABB :: Ptr MeshData -> Ptr Transform -> Ptr Vec3 -> Ptr AABB -> IO ()++foreign import ccall unsafe "hsg_b3ComputeHeightFieldAABB"+  c_b3ComputeHeightFieldAABB :: Ptr HeightFieldData -> Ptr Transform -> Ptr AABB -> IO ()++foreign import ccall unsafe "hsg_b3ComputeCompoundAABB"+  c_b3ComputeCompoundAABB :: Ptr CompoundData -> Ptr Transform -> Ptr AABB -> IO ()++foreign import ccall unsafe "b3IsValidRay"+  c_b3IsValidRay :: Ptr RayCastInput -> IO CBool++foreign import ccall unsafe "hsg_b3OverlapCapsule"+  c_b3OverlapCapsule :: Ptr Capsule -> Ptr Transform -> Ptr ShapeProxy -> IO CBool++foreign import ccall unsafe "hsg_b3OverlapCompound"+  c_b3OverlapCompound :: Ptr CompoundData -> Ptr Transform -> Ptr ShapeProxy -> IO CBool++foreign import ccall unsafe "hsg_b3OverlapHeightField"+  c_b3OverlapHeightField :: Ptr HeightFieldData -> Ptr Transform -> Ptr ShapeProxy -> IO CBool++foreign import ccall unsafe "hsg_b3OverlapHull"+  c_b3OverlapHull :: Ptr HullData -> Ptr Transform -> Ptr ShapeProxy -> IO CBool++foreign import ccall unsafe "hsg_b3OverlapMesh"+  c_b3OverlapMesh :: Ptr Mesh -> Ptr Transform -> Ptr ShapeProxy -> IO CBool++foreign import ccall unsafe "hsg_b3OverlapSphere"+  c_b3OverlapSphere :: Ptr Sphere -> Ptr Transform -> Ptr ShapeProxy -> IO CBool++foreign import ccall unsafe "hsg_b3RayCastSphere"+  c_b3RayCastSphere :: Ptr Sphere -> Ptr RayCastInput -> Ptr CastOutput -> IO ()++foreign import ccall unsafe "hsg_b3RayCastHollowSphere"+  c_b3RayCastHollowSphere :: Ptr Sphere -> Ptr RayCastInput -> Ptr CastOutput -> IO ()++foreign import ccall unsafe "hsg_b3RayCastCapsule"+  c_b3RayCastCapsule :: Ptr Capsule -> Ptr RayCastInput -> Ptr CastOutput -> IO ()++foreign import ccall unsafe "hsg_b3RayCastCompound"+  c_b3RayCastCompound :: Ptr CompoundData -> Ptr RayCastInput -> Ptr CastOutput -> IO ()++foreign import ccall unsafe "hsg_b3RayCastHull"+  c_b3RayCastHull :: Ptr HullData -> Ptr RayCastInput -> Ptr CastOutput -> IO ()++foreign import ccall unsafe "hsg_b3RayCastMesh"+  c_b3RayCastMesh :: Ptr Mesh -> Ptr RayCastInput -> Ptr CastOutput -> IO ()++foreign import ccall unsafe "hsg_b3RayCastHeightField"+  c_b3RayCastHeightField :: Ptr HeightFieldData -> Ptr RayCastInput -> Ptr CastOutput -> IO ()++foreign import ccall unsafe "hsg_b3ShapeCastSphere"+  c_b3ShapeCastSphere :: Ptr Sphere -> Ptr ShapeCastInput -> Ptr CastOutput -> IO ()++foreign import ccall unsafe "hsg_b3ShapeCastCapsule"+  c_b3ShapeCastCapsule :: Ptr Capsule -> Ptr ShapeCastInput -> Ptr CastOutput -> IO ()++foreign import ccall unsafe "hsg_b3ShapeCastCompound"+  c_b3ShapeCastCompound :: Ptr CompoundData -> Ptr ShapeCastInput -> Ptr CastOutput -> IO ()++foreign import ccall unsafe "hsg_b3ShapeCastHull"+  c_b3ShapeCastHull :: Ptr HullData -> Ptr ShapeCastInput -> Ptr CastOutput -> IO ()++foreign import ccall unsafe "hsg_b3ShapeCastMesh"+  c_b3ShapeCastMesh :: Ptr Mesh -> Ptr ShapeCastInput -> Ptr CastOutput -> IO ()++foreign import ccall unsafe "hsg_b3ShapeCastHeightField"+  c_b3ShapeCastHeightField :: Ptr HeightFieldData -> Ptr ShapeCastInput -> Ptr CastOutput -> IO ()++foreign import ccall safe "hsg_b3QueryMesh"+  c_b3QueryMesh :: Ptr Mesh -> Ptr AABB -> FunPtr MeshQueryFcn -> Ptr () -> IO ()++foreign import ccall safe "hsg_b3QueryHeightField"+  c_b3QueryHeightField :: Ptr HeightFieldData -> Ptr AABB -> FunPtr MeshQueryFcn -> Ptr () -> IO ()++foreign import ccall unsafe "hsg_b3ShapeDistance"+  c_b3ShapeDistance :: Ptr DistanceInput -> Ptr SimplexCache -> Ptr Simplex -> CInt -> Ptr DistanceOutput -> IO ()++foreign import ccall unsafe "hsg_b3ShapeCast"+  c_b3ShapeCast :: Ptr ShapeCastPairInput -> Ptr CastOutput -> IO ()++foreign import ccall unsafe "hsg_b3GetSweepTransform"+  c_b3GetSweepTransform :: Ptr Sweep -> Float -> Ptr Transform -> IO ()++foreign import ccall unsafe "hsg_b3TimeOfImpact"+  c_b3TimeOfImpact :: Ptr TOIInput -> Ptr TOIOutput -> IO ()++foreign import ccall unsafe "hsg_b3CollideSpheres"+  c_b3CollideSpheres :: Ptr LocalManifold -> CInt -> Ptr Sphere -> Ptr Sphere -> Ptr Transform -> IO ()++foreign import ccall unsafe "hsg_b3CollideCapsuleAndSphere"+  c_b3CollideCapsuleAndSphere :: Ptr LocalManifold -> CInt -> Ptr Capsule -> Ptr Sphere -> Ptr Transform -> IO ()++foreign import ccall unsafe "hsg_b3CollideHullAndSphere"+  c_b3CollideHullAndSphere :: Ptr LocalManifold -> CInt -> Ptr HullData -> Ptr Sphere -> Ptr Transform -> Ptr SimplexCache -> IO ()++foreign import ccall unsafe "hsg_b3CollideCapsules"+  c_b3CollideCapsules :: Ptr LocalManifold -> CInt -> Ptr Capsule -> Ptr Capsule -> Ptr Transform -> IO ()++foreign import ccall unsafe "hsg_b3CollideHullAndCapsule"+  c_b3CollideHullAndCapsule :: Ptr LocalManifold -> CInt -> Ptr HullData -> Ptr Capsule -> Ptr Transform -> Ptr SimplexCache -> IO ()++foreign import ccall unsafe "hsg_b3CollideHulls"+  c_b3CollideHulls :: Ptr LocalManifold -> CInt -> Ptr HullData -> Ptr HullData -> Ptr Transform -> Ptr SATCache -> IO ()++foreign import ccall unsafe "b3CollideTriangleAndCapsule"+  c_b3CollideTriangleAndCapsule :: Ptr LocalManifold -> CInt -> Ptr Vec3 -> Ptr Capsule -> Ptr SimplexCache -> IO ()++foreign import ccall unsafe "hsg_b3CollideTriangleAndHull"+  c_b3CollideTriangleAndHull :: Ptr LocalManifold -> CInt -> Ptr Vec3 -> Ptr Vec3 -> Ptr Vec3 -> CInt -> Ptr HullData -> Ptr SATCache -> CBool -> IO ()++foreign import ccall unsafe "b3CollideTriangleAndSphere"+  c_b3CollideTriangleAndSphere :: Ptr LocalManifold -> CInt -> Ptr Vec3 -> Ptr Sphere -> IO ()++foreign import ccall unsafe "hsg_b3SolvePlanes"+  c_b3SolvePlanes :: Ptr Vec3 -> Ptr CollisionPlane -> CInt -> Ptr PlaneSolverResult -> IO ()++foreign import ccall unsafe "hsg_b3ClipVector"+  c_b3ClipVector :: Ptr Vec3 -> Ptr CollisionPlane -> CInt -> Ptr Vec3 -> IO ()++{- | Get the current number of worlds++Binds @b3GetWorldCount@.+-}+getWorldCount :: IO Int+getWorldCount  =+  fromIntegral <$> (c_b3GetWorldCount)++{- | Get the maximum number of simultaneous worlds that have been created++Binds @b3GetMaxWorldCount@.+-}+getMaxWorldCount :: IO Int+getMaxWorldCount  =+  fromIntegral <$> (c_b3GetMaxWorldCount)++{- | Save the recording buffer to a file. Returns true on success.++Binds @b3SaveRecordingToFile@.+-}+saveRecordingToFile+  :: Ptr Recording+  -- ^ @recording@: the recording to save+  -> CString+  -- ^ @path@: file path to write+  -> IO Bool+saveRecordingToFile a0 a1 =+  toBool <$> (c_b3SaveRecordingToFile a0 a1)++{- | Load a recording from a file. Returns NULL on failure (file not found, wrong magic).+The caller owns the returned recording and must destroy it with @b3DestroyRecording@.++Binds @b3LoadRecordingFromFile@.+-}+loadRecordingFromFile+  :: CString+  -- ^ @path@: file path to read+  -> IO (Ptr Recording)+loadRecordingFromFile a0 =+  c_b3LoadRecordingFromFile a0++{- | Replay a recording from memory and verify it reproduces the same world-state hashes.+Stands up a fresh world, restores the seed snapshot, replays every op, and checks each embedded+StateHash record. Returns true if replay completed without id mismatches or hash divergences.++Binds @b3ValidateReplay@.+-}+validateReplay+  :: Ptr ()+  -- ^ @data@: pointer to recording bytes+  -> Int+  -- ^ @size@: byte count of the recording+  -> Int+  -- ^ @workerCount@: reserved for future multithreaded replay; pass 1 for now+  -> IO Bool+validateReplay a0 a1 a2 =+  toBool <$> (c_b3ValidateReplay a0 (fromIntegral a1) (fromIntegral a2))++{- | Deep clone a hull.++Binds @b3CloneHull@.+-}+cloneHull+  :: Ptr HullData+  -> IO (Ptr HullData)+cloneHull a0 =+  c_b3CloneHull a0++{- | Clone and transform a hull. Supports non-uniform and mirroring scale.++Binds @b3CloneAndTransformHull@.+-}+cloneAndTransformHull+  :: Ptr HullData+  -- ^ @original@+  -> Transform+  -> Vec3+  -- ^ @scale@+  -> IO (Ptr HullData)+cloneAndTransformHull a0 a1 a2 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b3CloneAndTransformHull a0 p1 p2++{- | Make a cube as a hull. Do not call @b3DestroyHull@ on this.++Binds @b3MakeCubeHull@.+-}+makeCubeHull+  :: Float+  -- ^ @halfWidth@+  -> Ptr BoxHull+  -- ^ Result buffer.+  -> IO ()+makeCubeHull a0 out =+  c_b3MakeCubeHull a0 out++{- | Make a box as a hull. Do not call @b3DestroyHull@ on this.++Binds @b3MakeBoxHull@.+-}+makeBoxHull+  :: Float+  -- ^ @hx@+  -> Float+  -- ^ @hy@+  -> Float+  -- ^ @hz@+  -> Ptr BoxHull+  -- ^ Result buffer.+  -> IO ()+makeBoxHull a0 a1 a2 out =+  c_b3MakeBoxHull a0 a1 a2 out++{- | Make an offset box as a hull. Do not call @b3DestroyHull@ on this.++Binds @b3MakeOffsetBoxHull@.+-}+makeOffsetBoxHull+  :: Float+  -- ^ @hx@+  -> Float+  -- ^ @hy@+  -> Float+  -- ^ @hz@+  -> Vec3+  -- ^ @offset@+  -> Ptr BoxHull+  -- ^ Result buffer.+  -> IO ()+makeOffsetBoxHull a0 a1 a2 a3 out =+  with a3 $ \p3 ->+  c_b3MakeOffsetBoxHull a0 a1 a2 p3 out++{- | Make a transformed box as a hull. Do not call @b3DestroyHull@ on this.++Binds @b3MakeTransformedBoxHull@.+-}+makeTransformedBoxHull+  :: Float+  -- ^ @hx@: positive half widths+  -> Float+  -- ^ @hy@: positive half widths+  -> Float+  -- ^ @hz@: positive half widths+  -> Transform+  -- ^ @transform@: local transform of box+  -> Ptr BoxHull+  -- ^ Result buffer.+  -> IO ()+makeTransformedBoxHull a0 a1 a2 a3 out =+  with a3 $ \p3 ->+  c_b3MakeTransformedBoxHull a0 a1 a2 p3 out++{- | This makes a transformed box hull with post scaling. This is useful for boxes that are scaled in+a level editor. Such scaling can have reflection and shear. In the case of shear the result+may be approximate. If you need to support shear consider using @b3CreateHull@.+Do not call @b3DestroyHull@ on this.++Binds @b3MakeScaledBoxHull@.+-}+makeScaledBoxHull+  :: Vec3+  -- ^ @halfWidths@: positive half widths+  -> Transform+  -- ^ @transform@: local transform of box+  -> Vec3+  -- ^ @postScale@: scale applied after the transform, may be negative+  -> Ptr BoxHull+  -- ^ Result buffer.+  -> IO ()+makeScaledBoxHull a0 a1 a2 out =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b3MakeScaledBoxHull p0 p1 p2 out++{- | 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.++Binds @b3ScaleBox@.+-}+scaleBox+  :: Ptr Vec3+  -- ^ @halfWidths@: \[in\/out\] the box half widths+  -> Ptr Transform+  -- ^ @transform@: \[in\/out\] the box transform with rotation and translation+  -> Vec3+  -- ^ @postScale@: the post scale being applied to the box after the transform+  -> Float+  -- ^ @minHalfWidth@: the minimum half width after scale is applied+  -> IO ()+scaleBox a0 a1 a2 a3 =+  with a2 $ \p2 ->+  c_b3ScaleBox a0 a1 p2 a3++{- | Get the height of the mesh BVH.++Binds @b3GetHeight@.+-}+getHeight+  :: Ptr MeshData+  -> IO Int+getHeight a0 =+  fromIntegral <$> (c_b3GetHeight a0)++{- | Save input height data to a file++Binds @b3DumpHeightData@.+-}+dumpHeightData+  :: Ptr HeightFieldDef+  -- ^ @data@+  -> CString+  -- ^ @fileName@+  -> IO ()+dumpHeightData a0 a1 =+  c_b3DumpHeightData a0 a1++{- | Create a height field by loading a previously saved height data++Binds @b3LoadHeightField@.+-}+loadHeightField+  :: CString+  -- ^ @fileName@+  -> IO (Ptr HeightFieldData)+loadHeightField a0 =+  c_b3LoadHeightField a0++{- | Get a child shape of a compound.++Binds @b3GetCompoundChild@.+-}+getCompoundChild+  :: Ptr CompoundData+  -> Int+  -- ^ @childIndex@+  -> Ptr ChildShape+  -- ^ Result buffer.+  -> IO ()+getCompoundChild a0 a1 out =+  c_b3GetCompoundChild a0 (fromIntegral a1) out++{- | Query a compound shape for children that overlap an AABB.++Binds @b3QueryCompound@.+-}+queryCompound+  :: Ptr CompoundData+  -> AABB+  -> FunPtr CompoundQueryFcn+  -> Ptr ()+  -- ^ @context@+  -> IO ()+queryCompound a0 a1 a2 a3 =+  with a1 $ \p1 ->+  c_b3QueryCompound a0 p1 a2 a3++{- | Access a child capsule by index.++Binds @b3GetCompoundCapsule@.+-}+getCompoundCapsule+  :: Ptr CompoundData+  -> Int+  -- ^ @index@+  -> Ptr CompoundCapsule+  -- ^ Result buffer.+  -> IO ()+getCompoundCapsule a0 a1 out =+  c_b3GetCompoundCapsule a0 (fromIntegral a1) out++{- | Access a child hull by index.++Binds @b3GetCompoundHull@.+-}+getCompoundHull+  :: Ptr CompoundData+  -> Int+  -- ^ @index@+  -> Ptr CompoundHull+  -- ^ Result buffer.+  -> IO ()+getCompoundHull a0 a1 out =+  c_b3GetCompoundHull a0 (fromIntegral a1) out++{- | Access a child mesh by index.++Binds @b3GetCompoundMesh@.+-}+getCompoundMesh+  :: Ptr CompoundData+  -> Int+  -- ^ @index@+  -> Ptr CompoundMesh+  -- ^ Result buffer.+  -> IO ()+getCompoundMesh a0 a1 out =+  c_b3GetCompoundMesh a0 (fromIntegral a1) out++{- | Access a child sphere by index.++Binds @b3GetCompoundSphere@.+-}+getCompoundSphere+  :: Ptr CompoundData+  -> Int+  -- ^ @index@+  -> Ptr CompoundSphere+  -- ^ Result buffer.+  -> IO ()+getCompoundSphere a0 a1 out =+  c_b3GetCompoundSphere a0 (fromIntegral a1) out++{- | Access the compound material array.++Binds @b3GetCompoundMaterials@.+-}+getCompoundMaterials+  :: Ptr CompoundData+  -> IO (Ptr SurfaceMaterial)+getCompoundMaterials a0 =+  c_b3GetCompoundMaterials a0++{- | Cast the provided compound data to bytes, setting the internal pointers to null.+Use this before serializing the compound bytes.++Binds @b3ConvertCompoundToBytes@.+-}+convertCompoundToBytes+  :: Ptr CompoundData+  -> IO (Ptr Word8)+convertCompoundToBytes a0 =+  c_b3ConvertCompoundToBytes a0++{- | Cast the provided bytes to compound data, setting up internal pointers.+Use this after de-serializing the compound bytes.++Binds @b3ConvertBytesToCompound@.+-}+convertBytesToCompound+  :: Ptr Word8+  -- ^ @bytes@+  -> Int+  -- ^ @byteCount@+  -> IO (Ptr CompoundData)+convertBytesToCompound a0 a1 =+  c_b3ConvertBytesToCompound a0 (fromIntegral a1)++{- | Compute mass properties of a sphere++Binds @b3ComputeSphereMass@.+-}+computeSphereMass+  :: Sphere+  -- ^ @shape@+  -> Float+  -- ^ @density@+  -> IO MassData+computeSphereMass a0 a1 =+  with a0 $ \p0 ->+  alloca $ \pOut -> c_b3ComputeSphereMass p0 a1 pOut >> peek pOut++{- | Compute mass properties of a capsule++Binds @b3ComputeCapsuleMass@.+-}+computeCapsuleMass+  :: Capsule+  -- ^ @shape@+  -> Float+  -- ^ @density@+  -> IO MassData+computeCapsuleMass a0 a1 =+  with a0 $ \p0 ->+  alloca $ \pOut -> c_b3ComputeCapsuleMass p0 a1 pOut >> peek pOut++{- | Compute mass properties of a hull++Binds @b3ComputeHullMass@.+-}+computeHullMass+  :: Ptr HullData+  -- ^ @shape@+  -> Float+  -- ^ @density@+  -> IO MassData+computeHullMass a0 a1 =+  alloca $ \pOut -> c_b3ComputeHullMass a0 a1 pOut >> peek pOut++-- | Like 'computeHullMass', but the result is written into a caller-supplied buffer.+computeHullMassInto+  :: Ptr HullData+  -- ^ @shape@+  -> Float+  -- ^ @density@+  -> Ptr MassData+  -- ^ Result buffer.+  -> IO ()+computeHullMassInto a0 a1 out =+  c_b3ComputeHullMass a0 a1 out++{- | Compute the bounding box of a transformed sphere++Binds @b3ComputeSphereAABB@.+-}+computeSphereAABB+  :: Sphere+  -- ^ @shape@+  -> Transform+  -> IO AABB+computeSphereAABB a0 a1 =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b3ComputeSphereAABB p0 p1 pOut >> peek pOut++{- | Compute the bounding box of a transformed capsule++Binds @b3ComputeCapsuleAABB@.+-}+computeCapsuleAABB+  :: Capsule+  -- ^ @shape@+  -> Transform+  -> IO AABB+computeCapsuleAABB a0 a1 =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b3ComputeCapsuleAABB p0 p1 pOut >> peek pOut++{- | Compute the bounding box of a transformed hull++Binds @b3ComputeHullAABB@.+-}+computeHullAABB+  :: Ptr HullData+  -- ^ @shape@+  -> Transform+  -> IO AABB+computeHullAABB a0 a1 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b3ComputeHullAABB a0 p1 pOut >> peek pOut++{- | Compute the bounding box of a transformed mesh. Scale may be non-uniform and have negative components.++Binds @b3ComputeMeshAABB@.+-}+computeMeshAABB+  :: Ptr MeshData+  -- ^ @shape@+  -> Transform+  -> Vec3+  -- ^ @scale@+  -> IO AABB+computeMeshAABB a0 a1 a2 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  alloca $ \pOut -> c_b3ComputeMeshAABB a0 p1 p2 pOut >> peek pOut++{- | Compute the bounding box of a transformed height-field++Binds @b3ComputeHeightFieldAABB@.+-}+computeHeightFieldAABB+  :: Ptr HeightFieldData+  -- ^ @shape@+  -> Transform+  -> IO AABB+computeHeightFieldAABB a0 a1 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b3ComputeHeightFieldAABB a0 p1 pOut >> peek pOut++{- | Compute the bounding box of a compound++Binds @b3ComputeCompoundAABB@.+-}+computeCompoundAABB+  :: Ptr CompoundData+  -- ^ @shape@+  -> Transform+  -> IO AABB+computeCompoundAABB a0 a1 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b3ComputeCompoundAABB a0 p1 pOut >> peek pOut++{- | Use this to ensure your ray cast input is valid and avoid internal assertions.++Binds @b3IsValidRay@.+-}+isValidRay+  :: RayCastInput+  -> IO Bool+isValidRay a0 =+  with a0 $ \p0 ->+  toBool <$> (c_b3IsValidRay p0)++{- | Overlap shape versus capsule++Binds @b3OverlapCapsule@.+-}+overlapCapsule+  :: Capsule+  -- ^ @shape@+  -> Transform+  -- ^ @shapeTransform@+  -> Ptr ShapeProxy+  -> IO Bool+overlapCapsule a0 a1 a2 =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  toBool <$> (c_b3OverlapCapsule p0 p1 a2)++{- | Overlap shape versus compound++Binds @b3OverlapCompound@.+-}+overlapCompound+  :: Ptr CompoundData+  -- ^ @shape@+  -> Transform+  -- ^ @shapeTransform@+  -> Ptr ShapeProxy+  -> IO Bool+overlapCompound a0 a1 a2 =+  with a1 $ \p1 ->+  toBool <$> (c_b3OverlapCompound a0 p1 a2)++{- | Overlap shape versus height field++Binds @b3OverlapHeightField@.+-}+overlapHeightField+  :: Ptr HeightFieldData+  -- ^ @shape@+  -> Transform+  -- ^ @shapeTransform@+  -> Ptr ShapeProxy+  -> IO Bool+overlapHeightField a0 a1 a2 =+  with a1 $ \p1 ->+  toBool <$> (c_b3OverlapHeightField a0 p1 a2)++{- | Overlap shape versus hull++Binds @b3OverlapHull@.+-}+overlapHull+  :: Ptr HullData+  -- ^ @shape@+  -> Transform+  -- ^ @shapeTransform@+  -> Ptr ShapeProxy+  -> IO Bool+overlapHull a0 a1 a2 =+  with a1 $ \p1 ->+  toBool <$> (c_b3OverlapHull a0 p1 a2)++{- | Overlap shape versus mesh++Binds @b3OverlapMesh@.+-}+overlapMesh+  :: Ptr Mesh+  -- ^ @shape@+  -> Transform+  -- ^ @shapeTransform@+  -> Ptr ShapeProxy+  -> IO Bool+overlapMesh a0 a1 a2 =+  with a1 $ \p1 ->+  toBool <$> (c_b3OverlapMesh a0 p1 a2)++{- | Overlap shape versus sphere++Binds @b3OverlapSphere@.+-}+overlapSphere+  :: Sphere+  -- ^ @shape@+  -> Transform+  -- ^ @shapeTransform@+  -> Ptr ShapeProxy+  -> IO Bool+overlapSphere a0 a1 a2 =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  toBool <$> (c_b3OverlapSphere p0 p1 a2)++{- | Ray cast versus sphere in local space. A zero length ray is a point query. Initial overlap+reports a hit at the ray origin with zero fraction and zero normal.++Binds @b3RayCastSphere@.+-}+rayCastSphere+  :: Sphere+  -- ^ @shape@+  -> RayCastInput+  -> IO CastOutput+rayCastSphere a0 a1 =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b3RayCastSphere p0 p1 pOut >> peek pOut++{- | Ray cast versus a hollow sphere shell in local space. Unlike the solid sphere a ray starting+inside is not an overlap: it passes through and hits the far wall.++Binds @b3RayCastHollowSphere@.+-}+rayCastHollowSphere+  :: Sphere+  -- ^ @shape@+  -> RayCastInput+  -> IO CastOutput+rayCastHollowSphere a0 a1 =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b3RayCastHollowSphere p0 p1 pOut >> peek pOut++{- | Ray cast versus capsule in local space. A zero length ray is a point query. Initial overlap+reports a hit at the ray origin with zero fraction and zero normal.++Binds @b3RayCastCapsule@.+-}+rayCastCapsule+  :: Capsule+  -- ^ @shape@+  -> RayCastInput+  -> IO CastOutput+rayCastCapsule a0 a1 =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b3RayCastCapsule p0 p1 pOut >> peek pOut++{- | Ray cast versus compound in local space. A zero length ray is a point query. Initial overlap+with a child reports a hit at the ray origin with zero fraction and zero normal.++Binds @b3RayCastCompound@.+-}+rayCastCompound+  :: Ptr CompoundData+  -- ^ @shape@+  -> RayCastInput+  -> IO CastOutput+rayCastCompound a0 a1 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b3RayCastCompound a0 p1 pOut >> peek pOut++{- | Ray cast versus hull shape in local space. A zero length ray is a point query. Initial overlap+reports a hit at the ray origin with zero fraction and zero normal.++Binds @b3RayCastHull@.+-}+rayCastHull+  :: Ptr HullData+  -- ^ @shape@+  -> RayCastInput+  -> IO CastOutput+rayCastHull a0 a1 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b3RayCastHull a0 p1 pOut >> peek pOut++{- | Ray cast versus mesh in local space. A thin surface with no interior, so there is no overlap case.++Binds @b3RayCastMesh@.+-}+rayCastMesh+  :: Ptr Mesh+  -- ^ @shape@+  -> RayCastInput+  -> IO CastOutput+rayCastMesh a0 a1 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b3RayCastMesh a0 p1 pOut >> peek pOut++{- | Ray cast versus height field in local space. A thin surface with no interior, so there is no overlap case.++Binds @b3RayCastHeightField@.+-}+rayCastHeightField+  :: Ptr HeightFieldData+  -- ^ @shape@+  -> RayCastInput+  -> IO CastOutput+rayCastHeightField a0 a1 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b3RayCastHeightField a0 p1 pOut >> peek pOut++{- | Shape cast versus a sphere. Initial overlap is treated as a miss.++Binds @b3ShapeCastSphere@.+-}+shapeCastSphere+  :: Sphere+  -- ^ @shape@+  -> Ptr ShapeCastInput+  -> IO CastOutput+shapeCastSphere a0 a1 =+  with a0 $ \p0 ->+  alloca $ \pOut -> c_b3ShapeCastSphere p0 a1 pOut >> peek pOut++{- | Shape cast versus a capsule. Initial overlap is treated as a miss.++Binds @b3ShapeCastCapsule@.+-}+shapeCastCapsule+  :: Capsule+  -- ^ @shape@+  -> Ptr ShapeCastInput+  -> IO CastOutput+shapeCastCapsule a0 a1 =+  with a0 $ \p0 ->+  alloca $ \pOut -> c_b3ShapeCastCapsule p0 a1 pOut >> peek pOut++{- | Shape cast versus compound. Initial overlap is treated as a miss.++Binds @b3ShapeCastCompound@.+-}+shapeCastCompound+  :: Ptr CompoundData+  -- ^ @shape@+  -> Ptr ShapeCastInput+  -> IO CastOutput+shapeCastCompound a0 a1 =+  alloca $ \pOut -> c_b3ShapeCastCompound a0 a1 pOut >> peek pOut++-- | Like 'shapeCastCompound', but the result is written into a caller-supplied buffer.+shapeCastCompoundInto+  :: Ptr CompoundData+  -- ^ @shape@+  -> Ptr ShapeCastInput+  -> Ptr CastOutput+  -- ^ Result buffer.+  -> IO ()+shapeCastCompoundInto a0 a1 out =+  c_b3ShapeCastCompound a0 a1 out++{- | Shape cast versus a hull. Initial overlap is treated as a miss.++Binds @b3ShapeCastHull@.+-}+shapeCastHull+  :: Ptr HullData+  -- ^ @shape@+  -> Ptr ShapeCastInput+  -> IO CastOutput+shapeCastHull a0 a1 =+  alloca $ \pOut -> c_b3ShapeCastHull a0 a1 pOut >> peek pOut++-- | Like 'shapeCastHull', but the result is written into a caller-supplied buffer.+shapeCastHullInto+  :: Ptr HullData+  -- ^ @shape@+  -> Ptr ShapeCastInput+  -> Ptr CastOutput+  -- ^ Result buffer.+  -> IO ()+shapeCastHullInto a0 a1 out =+  c_b3ShapeCastHull a0 a1 out++{- | Shape cast versus a mesh. Initial overlap is treated as a miss.++Binds @b3ShapeCastMesh@.+-}+shapeCastMesh+  :: Ptr Mesh+  -- ^ @shape@+  -> Ptr ShapeCastInput+  -> IO CastOutput+shapeCastMesh a0 a1 =+  alloca $ \pOut -> c_b3ShapeCastMesh a0 a1 pOut >> peek pOut++-- | Like 'shapeCastMesh', but the result is written into a caller-supplied buffer.+shapeCastMeshInto+  :: Ptr Mesh+  -- ^ @shape@+  -> Ptr ShapeCastInput+  -> Ptr CastOutput+  -- ^ Result buffer.+  -> IO ()+shapeCastMeshInto a0 a1 out =+  c_b3ShapeCastMesh a0 a1 out++{- | Shape cast versus a height field. Initial overlap is treated as a miss.++Binds @b3ShapeCastHeightField@.+-}+shapeCastHeightField+  :: Ptr HeightFieldData+  -- ^ @shape@+  -> Ptr ShapeCastInput+  -> IO CastOutput+shapeCastHeightField a0 a1 =+  alloca $ \pOut -> c_b3ShapeCastHeightField a0 a1 pOut >> peek pOut++-- | Like 'shapeCastHeightField', but the result is written into a caller-supplied buffer.+shapeCastHeightFieldInto+  :: Ptr HeightFieldData+  -- ^ @shape@+  -> Ptr ShapeCastInput+  -> Ptr CastOutput+  -- ^ Result buffer.+  -> IO ()+shapeCastHeightFieldInto a0 a1 out =+  c_b3ShapeCastHeightField a0 a1 out++{- | Query a mesh for triangles overlapping a bounding box in local space. May have false positives. Useful for debug draw.++Binds @b3QueryMesh@.+-}+queryMesh+  :: Ptr Mesh+  -- ^ @mesh@: the mesh to query, includes scale+  -> AABB+  -- ^ @bounds@: the bounding box in local space+  -> FunPtr MeshQueryFcn+  -- ^ @fcn@: a user function to collect triangles+  -> Ptr ()+  -- ^ @context@: the context sent to the user function.+  -> IO ()+queryMesh a0 a1 a2 a3 =+  with a1 $ \p1 ->+  c_b3QueryMesh a0 p1 a2 a3++{- | Query a height field for triangles overlapping a bounding box in local space. May have false positives. Useful for debug draw.++Binds @b3QueryHeightField@.+-}+queryHeightField+  :: Ptr HeightFieldData+  -- ^ @heightField@: the height field to query+  -> AABB+  -- ^ @bounds@: the bounding box in local space+  -> FunPtr MeshQueryFcn+  -- ^ @fcn@: a user function to collect triangles+  -> Ptr ()+  -- ^ @context@: the context sent to the user function.+  -> IO ()+queryHeightField a0 a1 a2 a3 =+  with a1 $ \p1 ->+  c_b3QueryHeightField a0 p1 a2 a3++{- | 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 query runs in frame A, so the witness points and normal are returned in frame A.+The underlying GJK algorithm may be debugged by passing in debug simplexes and capacity. You may pass in NULL and 0 for these.++Binds @b3ShapeDistance@.+-}+shapeDistance+  :: Ptr DistanceInput+  -> Ptr SimplexCache+  -> Ptr Simplex+  -- ^ @simplexes@+  -> Int+  -- ^ @simplexCapacity@+  -> Ptr DistanceOutput+  -- ^ Result buffer.+  -> IO ()+shapeDistance a0 a1 a2 a3 out =+  c_b3ShapeDistance a0 a1 a2 (fromIntegral a3) out++{- | Perform a linear shape cast of shape B moving and shape A fixed. Determines the hit point, normal, and translation fraction.+The query runs in frame A, so the hit point and normal are returned in frame A. Initially touching shapes are a miss.++Binds @b3ShapeCast@.+-}+shapeCast+  :: Ptr ShapeCastPairInput+  -> IO CastOutput+shapeCast a0 =+  alloca $ \pOut -> c_b3ShapeCast a0 pOut >> peek pOut++-- | Like 'shapeCast', but the result is written into a caller-supplied buffer.+shapeCastInto+  :: Ptr ShapeCastPairInput+  -> Ptr CastOutput+  -- ^ Result buffer.+  -> IO ()+shapeCastInto a0 out =+  c_b3ShapeCast a0 out++{- | Evaluate the transform sweep at a specific time.++Binds @b3GetSweepTransform@.+-}+getSweepTransform+  :: Sweep+  -> Float+  -- ^ @time@+  -> IO Transform+getSweepTransform a0 a1 =+  with a0 $ \p0 ->+  alloca $ \pOut -> c_b3GetSweepTransform p0 a1 pOut >> peek pOut++{- | 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.++Binds @b3TimeOfImpact@.+-}+timeOfImpact+  :: Ptr TOIInput+  -> Ptr TOIOutput+  -- ^ Result buffer.+  -> IO ()+timeOfImpact a0 out =+  c_b3TimeOfImpact a0 out++{- | Collide two spheres.++Binds @b3CollideSpheres@.+-}+collideSpheres+  :: Ptr LocalManifold+  -> Int+  -- ^ @capacity@+  -> Sphere+  -- ^ @sphereA@+  -> Sphere+  -- ^ @sphereB@+  -> Transform+  -- ^ @transformBtoA@+  -> IO ()+collideSpheres a0 a1 a2 a3 a4 =+  with a2 $ \p2 ->+  with a3 $ \p3 ->+  with a4 $ \p4 ->+  c_b3CollideSpheres a0 (fromIntegral a1) p2 p3 p4++{- | Collide a capsule and a sphere.++Binds @b3CollideCapsuleAndSphere@.+-}+collideCapsuleAndSphere+  :: Ptr LocalManifold+  -> Int+  -- ^ @capacity@+  -> Capsule+  -- ^ @capsuleA@+  -> Sphere+  -- ^ @sphereB@+  -> Transform+  -- ^ @transformBtoA@+  -> IO ()+collideCapsuleAndSphere a0 a1 a2 a3 a4 =+  with a2 $ \p2 ->+  with a3 $ \p3 ->+  with a4 $ \p4 ->+  c_b3CollideCapsuleAndSphere a0 (fromIntegral a1) p2 p3 p4++{- | Collide a hull and a sphere.++Binds @b3CollideHullAndSphere@.+-}+collideHullAndSphere+  :: Ptr LocalManifold+  -> Int+  -- ^ @capacity@+  -> Ptr HullData+  -- ^ @hullA@+  -> Sphere+  -- ^ @sphereB@+  -> Transform+  -- ^ @transformBtoA@+  -> Ptr SimplexCache+  -> IO ()+collideHullAndSphere a0 a1 a2 a3 a4 a5 =+  with a3 $ \p3 ->+  with a4 $ \p4 ->+  c_b3CollideHullAndSphere a0 (fromIntegral a1) a2 p3 p4 a5++{- | Collide two capsules.++Binds @b3CollideCapsules@.+-}+collideCapsules+  :: Ptr LocalManifold+  -> Int+  -- ^ @capacity@+  -> Capsule+  -- ^ @capsuleA@+  -> Capsule+  -- ^ @capsuleB@+  -> Transform+  -- ^ @transformBtoA@+  -> IO ()+collideCapsules a0 a1 a2 a3 a4 =+  with a2 $ \p2 ->+  with a3 $ \p3 ->+  with a4 $ \p4 ->+  c_b3CollideCapsules a0 (fromIntegral a1) p2 p3 p4++{- | Collide a hull and a capsule.++Binds @b3CollideHullAndCapsule@.+-}+collideHullAndCapsule+  :: Ptr LocalManifold+  -> Int+  -- ^ @capacity@+  -> Ptr HullData+  -- ^ @hullA@+  -> Capsule+  -- ^ @capsuleB@+  -> Transform+  -- ^ @transformBtoA@+  -> Ptr SimplexCache+  -> IO ()+collideHullAndCapsule a0 a1 a2 a3 a4 a5 =+  with a3 $ \p3 ->+  with a4 $ \p4 ->+  c_b3CollideHullAndCapsule a0 (fromIntegral a1) a2 p3 p4 a5++{- | Collide two hulls.++Binds @b3CollideHulls@.+-}+collideHulls+  :: Ptr LocalManifold+  -> Int+  -- ^ @capacity@+  -> Ptr HullData+  -- ^ @hullA@+  -> Ptr HullData+  -- ^ @hullB@+  -> Transform+  -- ^ @transformBtoA@+  -> Ptr SATCache+  -> IO ()+collideHulls a0 a1 a2 a3 a4 a5 =+  with a4 $ \p4 ->+  c_b3CollideHulls a0 (fromIntegral a1) a2 a3 p4 a5++{- | Collide a triangle and capsule. Normal points from triangle to capsule.++Binds @b3CollideTriangleAndCapsule@.+-}+collideTriangleAndCapsule+  :: Ptr LocalManifold+  -> Int+  -- ^ @capacity@+  -> Ptr Vec3+  -- ^ @triangleA@+  -> Capsule+  -- ^ @capsuleB@+  -> Ptr SimplexCache+  -> IO ()+collideTriangleAndCapsule a0 a1 a2 a3 a4 =+  with a3 $ \p3 ->+  c_b3CollideTriangleAndCapsule a0 (fromIntegral a1) a2 p3 a4++{- | Collide a triangle and hull. Normal points from triangle to hull.++Binds @b3CollideTriangleAndHull@.+-}+collideTriangleAndHull+  :: Ptr LocalManifold+  -> Int+  -- ^ @capacity@+  -> Vec3+  -- ^ @v1@+  -> Vec3+  -- ^ @v2@+  -> Vec3+  -- ^ @v3@+  -> Int+  -- ^ @triangleFlags@+  -> Ptr HullData+  -- ^ @hullB@+  -> Ptr SATCache+  -> Bool+  -- ^ @enableSpeculative@+  -> IO ()+collideTriangleAndHull a0 a1 a2 a3 a4 a5 a6 a7 a8 =+  with a2 $ \p2 ->+  with a3 $ \p3 ->+  with a4 $ \p4 ->+  c_b3CollideTriangleAndHull a0 (fromIntegral a1) p2 p3 p4 (fromIntegral a5) a6 a7 (fromBool a8)++{- | Collide a triangle and sphere. Normal points from triangle to sphere.++Binds @b3CollideTriangleAndSphere@.+-}+collideTriangleAndSphere+  :: Ptr LocalManifold+  -> Int+  -- ^ @capacity@+  -> Ptr Vec3+  -- ^ @triangleA@+  -> Sphere+  -- ^ @sphereB@+  -> IO ()+collideTriangleAndSphere a0 a1 a2 a3 =+  with a3 $ \p3 ->+  c_b3CollideTriangleAndSphere a0 (fromIntegral a1) a2 p3++{- | Solves the position of a mover that satisfies the given collision planes.++Binds @b3SolvePlanes@.+-}+solvePlanes+  :: Vec3+  -- ^ @targetDelta@: the desired translation from the position used to generate the collision planes+  -> Ptr CollisionPlane+  -- ^ @planes@: the collision planes+  -> Int+  -- ^ @count@: the number of collision planes+  -> IO PlaneSolverResult+solvePlanes a0 a1 a2 =+  with a0 $ \p0 ->+  alloca $ \pOut -> c_b3SolvePlanes p0 a1 (fromIntegral a2) pOut >> peek pOut++{- | Clips the velocity against the given collision planes. Planes with zero push or clipVelocity+set to false are skipped.++Binds @b3ClipVector@.+-}+clipVector+  :: Vec3+  -- ^ @vector@+  -> Ptr CollisionPlane+  -- ^ @planes@+  -> Int+  -- ^ @count@+  -> IO Vec3+clipVector a0 a1 a2 =+  with a0 $ \p0 ->+  alloca $ \pOut -> c_b3ClipVector p0 a1 (fromIntegral a2) pOut >> peek pOut
+ src/Box3D/Compound.hsc view
@@ -0,0 +1,389 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.Compound+  ( CompoundSphere (..)+  , CompoundMesh (..)+  , CompoundHull (..)+  , CompoundCapsule (..)+  , CompoundData (..)+  , CompoundDef (..)+  , CompoundSphereDef (..)+  , CompoundMeshDef (..)+  , CompoundHullDef (..)+  , CompoundCapsuleDef (..)+  , maxCompoundMeshMaterials+  , create+  , destroy+  )+  where++import Foreign+import Foreign.C.Types (CInt (..))+import Data.Vector.Storable qualified as VS+import Box3D.Internal (copyVector, pokeBounded)+import Box3D.DynamicTree (DynamicTree)+import Box3D.Geometry (Capsule, Sphere)+import Box3D.Hull (HullData)+import Box3D.MathTypes (Transform, Vec3)+import Box3D.Mesh (MeshData)+import {-# SOURCE #-} Box3D.Shape (SurfaceMaterial)++#include "box3d/box3d.h"+++-- | A sphere that lives in a compound.+data CompoundSphere = CompoundSphere+  { sphere :: Sphere+  -- ^ Local sphere.+  , materialIndex :: CInt+  -- ^ Index to a shared material.+  }+  deriving (Eq, Show)++instance Storable CompoundSphere where+  sizeOf _ = #{size b3CompoundSphere}+  alignment _ = #{alignment b3CompoundSphere}+  peek p =+    CompoundSphere+      <$> #{peek b3CompoundSphere, sphere} p+      <*> #{peek b3CompoundSphere, materialIndex} p+  poke p (CompoundSphere f1 f2) = do+    #{poke b3CompoundSphere, sphere} p f1+    #{poke b3CompoundSphere, materialIndex} p f2++-- | A mesh with non-uniform scale that lives in a compound.+data CompoundMesh = CompoundMesh+  { meshData :: Ptr MeshData+  -- ^ Pointer to the unique shared mesh.+  , transform :: Transform+  -- ^ The transform of this mesh instance.+  , scale :: Vec3+  -- ^ Non-uniform scale of this mesh instance.+  , materialIndices :: VS.Vector CInt+  -- ^ This is used to access the surface material from @b3GetCompoundMaterials@.+  -- Requires an extra level of indirection. The triangle material index+  -- is clamped to @B3_MAX_COMPOUND_MESH_MATERIALS@.+  -- materialIndex = materialIndices\[triangle-\>materialIndex\]+  }+  deriving (Eq, Show)++instance Storable CompoundMesh where+  sizeOf _ = #{size b3CompoundMesh}+  alignment _ = #{alignment b3CompoundMesh}+  peek p =+    CompoundMesh+      <$> #{peek b3CompoundMesh, meshData} p+      <*> #{peek b3CompoundMesh, transform} p+      <*> #{peek b3CompoundMesh, scale} p+      <*> copyVector maxCompoundMeshMaterials (#{ptr b3CompoundMesh, materialIndices} p)+  poke p x = do+    #{poke b3CompoundMesh, meshData} p x.meshData+    #{poke b3CompoundMesh, transform} p x.transform+    #{poke b3CompoundMesh, scale} p x.scale+    pokeBounded maxCompoundMeshMaterials (#{ptr b3CompoundMesh, materialIndices} p) x.materialIndices++-- | A hull that lives in a compound.+data CompoundHull = CompoundHull+  { hull :: Ptr HullData+  -- ^ Pointer to the unique shared hull.+  , transform :: Transform+  -- ^ The transform of this hull instance.+  , materialIndex :: CInt+  -- ^ Index to a shared material.+  }+  deriving (Eq, Show)++instance Storable CompoundHull where+  sizeOf _ = #{size b3CompoundHull}+  alignment _ = #{alignment b3CompoundHull}+  peek p =+    CompoundHull+      <$> #{peek b3CompoundHull, hull} p+      <*> #{peek b3CompoundHull, transform} p+      <*> #{peek b3CompoundHull, materialIndex} p+  poke p (CompoundHull f1 f2 f3) = do+    #{poke b3CompoundHull, hull} p f1+    #{poke b3CompoundHull, transform} p f2+    #{poke b3CompoundHull, materialIndex} p f3++-- | A capsule that lives in a compound.+data CompoundCapsule = CompoundCapsule+  { capsule :: Capsule+  -- ^ Local capsule.+  , materialIndex :: CInt+  -- ^ Index to a shared material.+  }+  deriving (Eq, Show)++instance Storable CompoundCapsule where+  sizeOf _ = #{size b3CompoundCapsule}+  alignment _ = #{alignment b3CompoundCapsule}+  peek p =+    CompoundCapsule+      <$> #{peek b3CompoundCapsule, capsule} p+      <*> #{peek b3CompoundCapsule, materialIndex} p+  poke p (CompoundCapsule f1 f2) = do+    #{poke b3CompoundCapsule, capsule} p f1+    #{poke b3CompoundCapsule, materialIndex} p f2++{- | The data for a baked compound shape. This is a potentially large yet highly optimized+data structure. It can contain thousands of child shapes, yet at runtime it populates+into the world as a single shape in the runtime broad-phase.+This data structure has data living off the end and must be accessed using offsets.+Accessors are provided for user relevant data.+Note: you don\'t need to use this to create runtime compounds. For runtime compounds you can+add multiple shapes to a body using the regular shape creation functions.+-}+data CompoundData = CompoundData+  { version :: Word64+  -- ^ The compound version is always first.+  , byteCount :: CInt+  -- ^ The total number of bytes for this compound.+  , nodeOffset :: CInt+  -- ^ Offset of the tree node array in bytes from the struct address.+  , tree :: DynamicTree+  -- ^ Immutable dynamic tree. The tree node pointer must be fixed up using the node offset+  , materialOffset :: CInt+  -- ^ Offset of the material array in bytes from the struct address.+  , materialCount :: CInt+  -- ^ The number of materials.+  , capsuleOffset :: CInt+  -- ^ Offset of the capsule array in bytes from the struct address.+  , capsuleCount :: CInt+  -- ^ The number of capsules.+  , hullOffset :: CInt+  -- ^ Offset of the hull instance array in bytes from the struct address.+  , hullCount :: CInt+  -- ^ The number of hull instances.+  , sharedHullCount :: CInt+  -- ^ The number of unique hulls. Diagnostic.+  , meshOffset :: CInt+  -- ^ Offset of the mesh instance array in bytes from the struct address.+  , meshCount :: CInt+  -- ^ The number of mesh instances.+  , sharedMeshCount :: CInt+  -- ^ The number of unique meshes. Diagnostic.+  , sphereOffset :: CInt+  -- ^ Offset of the sphere array in bytes from the struct address.+  , sphereCount :: CInt+  -- ^ The number of spheres.+  }+  deriving (Eq, Show)++instance Storable CompoundData where+  sizeOf _ = #{size b3CompoundData}+  alignment _ = #{alignment b3CompoundData}+  peek p =+    CompoundData+      <$> #{peek b3CompoundData, version} p+      <*> #{peek b3CompoundData, byteCount} p+      <*> #{peek b3CompoundData, nodeOffset} p+      <*> #{peek b3CompoundData, tree} p+      <*> #{peek b3CompoundData, materialOffset} p+      <*> #{peek b3CompoundData, materialCount} p+      <*> #{peek b3CompoundData, capsuleOffset} p+      <*> #{peek b3CompoundData, capsuleCount} p+      <*> #{peek b3CompoundData, hullOffset} p+      <*> #{peek b3CompoundData, hullCount} p+      <*> #{peek b3CompoundData, sharedHullCount} p+      <*> #{peek b3CompoundData, meshOffset} p+      <*> #{peek b3CompoundData, meshCount} p+      <*> #{peek b3CompoundData, sharedMeshCount} p+      <*> #{peek b3CompoundData, sphereOffset} p+      <*> #{peek b3CompoundData, sphereCount} p+  poke p (CompoundData f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11 f12 f13 f14 f15 f16) = do+    #{poke b3CompoundData, version} p f1+    #{poke b3CompoundData, byteCount} p f2+    #{poke b3CompoundData, nodeOffset} p f3+    #{poke b3CompoundData, tree} p f4+    #{poke b3CompoundData, materialOffset} p f5+    #{poke b3CompoundData, materialCount} p f6+    #{poke b3CompoundData, capsuleOffset} p f7+    #{poke b3CompoundData, capsuleCount} p f8+    #{poke b3CompoundData, hullOffset} p f9+    #{poke b3CompoundData, hullCount} p f10+    #{poke b3CompoundData, sharedHullCount} p f11+    #{poke b3CompoundData, meshOffset} p f12+    #{poke b3CompoundData, meshCount} p f13+    #{poke b3CompoundData, sharedMeshCount} p f14+    #{poke b3CompoundData, sphereOffset} p f15+    #{poke b3CompoundData, sphereCount} p f16++{- | Definition for creating a compound shape. All this data is fully cloned+into the run-time compound shape.+-}+data CompoundDef = CompoundDef+  { capsules :: Ptr CompoundCapsuleDef+  -- ^ Capsule instances.+  , capsuleCount :: CInt+  -- ^ Number of capsules.+  , hulls :: Ptr CompoundHullDef+  -- ^ Hulls instances.+  , hullCount :: CInt+  -- ^ Number of hull instances.+  , meshes :: Ptr CompoundMeshDef+  -- ^ Mesh instances.+  , meshCount :: CInt+  -- ^ Number of mesh instances.+  , spheres :: Ptr CompoundSphereDef+  -- ^ Sphere instances.+  , sphereCount :: CInt+  -- ^ Number of spheres.+  }+  deriving (Eq, Show)++instance Storable CompoundDef where+  sizeOf _ = #{size b3CompoundDef}+  alignment _ = #{alignment b3CompoundDef}+  peek p =+    CompoundDef+      <$> #{peek b3CompoundDef, capsules} p+      <*> #{peek b3CompoundDef, capsuleCount} p+      <*> #{peek b3CompoundDef, hulls} p+      <*> #{peek b3CompoundDef, hullCount} p+      <*> #{peek b3CompoundDef, meshes} p+      <*> #{peek b3CompoundDef, meshCount} p+      <*> #{peek b3CompoundDef, spheres} p+      <*> #{peek b3CompoundDef, sphereCount} p+  poke p (CompoundDef f1 f2 f3 f4 f5 f6 f7 f8) = do+    #{poke b3CompoundDef, capsules} p f1+    #{poke b3CompoundDef, capsuleCount} p f2+    #{poke b3CompoundDef, hulls} p f3+    #{poke b3CompoundDef, hullCount} p f4+    #{poke b3CompoundDef, meshes} p f5+    #{poke b3CompoundDef, meshCount} p f6+    #{poke b3CompoundDef, spheres} p f7+    #{poke b3CompoundDef, sphereCount} p f8++-- | Definition for a sphere in a compound shape.+data CompoundSphereDef = CompoundSphereDef+  { sphere :: Sphere+  -- ^ Local sphere.+  , material :: SurfaceMaterial+  -- ^ Material properties.+  }+  deriving (Eq, Show)++instance Storable CompoundSphereDef where+  sizeOf _ = #{size b3CompoundSphereDef}+  alignment _ = #{alignment b3CompoundSphereDef}+  peek p =+    CompoundSphereDef+      <$> #{peek b3CompoundSphereDef, sphere} p+      <*> #{peek b3CompoundSphereDef, material} p+  poke p (CompoundSphereDef f1 f2) = do+    #{poke b3CompoundSphereDef, sphere} p f1+    #{poke b3CompoundSphereDef, material} p f2++-- | Definition for a triangle mesh in a compound shape.+data CompoundMeshDef = CompoundMeshDef+  { meshData :: Ptr MeshData+  -- ^ Shared mesh.+  , transform :: Transform+  -- ^ Transform of the shared mesh into compound local space.+  , scale :: Vec3+  -- ^ Local space non-uniform mesh scale. May have negative components.+  , materials :: Ptr SurfaceMaterial+  -- ^ Material properties.+  -- This array must line up with the material indices on the triangles.+  , materialCount :: CInt+  -- ^ Number of materials.+  }+  deriving (Eq, Show)++instance Storable CompoundMeshDef where+  sizeOf _ = #{size b3CompoundMeshDef}+  alignment _ = #{alignment b3CompoundMeshDef}+  peek p =+    CompoundMeshDef+      <$> #{peek b3CompoundMeshDef, meshData} p+      <*> #{peek b3CompoundMeshDef, transform} p+      <*> #{peek b3CompoundMeshDef, scale} p+      <*> #{peek b3CompoundMeshDef, materials} p+      <*> #{peek b3CompoundMeshDef, materialCount} p+  poke p (CompoundMeshDef f1 f2 f3 f4 f5) = do+    #{poke b3CompoundMeshDef, meshData} p f1+    #{poke b3CompoundMeshDef, transform} p f2+    #{poke b3CompoundMeshDef, scale} p f3+    #{poke b3CompoundMeshDef, materials} p f4+    #{poke b3CompoundMeshDef, materialCount} p f5++-- | Definition for a convex hull in a compound shape.+data CompoundHullDef = CompoundHullDef+  { hull :: Ptr HullData+  -- ^ Shared hull.+  , transform :: Transform+  -- ^ Transform of the shared hull into compound local space.+  , material :: SurfaceMaterial+  -- ^ Material properties.+  }+  deriving (Eq, Show)++instance Storable CompoundHullDef where+  sizeOf _ = #{size b3CompoundHullDef}+  alignment _ = #{alignment b3CompoundHullDef}+  peek p =+    CompoundHullDef+      <$> #{peek b3CompoundHullDef, hull} p+      <*> #{peek b3CompoundHullDef, transform} p+      <*> #{peek b3CompoundHullDef, material} p+  poke p (CompoundHullDef f1 f2 f3) = do+    #{poke b3CompoundHullDef, hull} p f1+    #{poke b3CompoundHullDef, transform} p f2+    #{poke b3CompoundHullDef, material} p f3++-- | Definition for a capsule in a compound shape.+data CompoundCapsuleDef = CompoundCapsuleDef+  { capsule :: Capsule+  -- ^ Local capsule.+  , material :: SurfaceMaterial+  -- ^ Material properties.+  }+  deriving (Eq, Show)++instance Storable CompoundCapsuleDef where+  sizeOf _ = #{size b3CompoundCapsuleDef}+  alignment _ = #{alignment b3CompoundCapsuleDef}+  peek p =+    CompoundCapsuleDef+      <$> #{peek b3CompoundCapsuleDef, capsule} p+      <*> #{peek b3CompoundCapsuleDef, material} p+  poke p (CompoundCapsuleDef f1 f2) = do+    #{poke b3CompoundCapsuleDef, capsule} p f1+    #{poke b3CompoundCapsuleDef, material} p f2++-- | @B3_MAX_COMPOUND_MESH_MATERIALS@+maxCompoundMeshMaterials :: Int+maxCompoundMeshMaterials = #{const B3_MAX_COMPOUND_MESH_MATERIALS}++++foreign import ccall unsafe "b3CreateCompound"+  c_b3CreateCompound :: Ptr CompoundDef -> IO (Ptr CompoundData)++foreign import ccall unsafe "b3DestroyCompound"+  c_b3DestroyCompound :: Ptr CompoundData -> IO ()++{- | Create a compound shape. All input data in the definition is cloned into the resulting compound.++Binds @b3CreateCompound@.+-}+create+  :: Ptr CompoundDef+  -> IO (Ptr CompoundData)+create a0 =+  c_b3CreateCompound a0++{- | Destroy a compound shape.++Binds @b3DestroyCompound@.+-}+destroy+  :: Ptr CompoundData+  -> IO ()+destroy a0 =+  c_b3DestroyCompound a0
+ src/Box3D/Cone.hs view
@@ -0,0 +1,30 @@+-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.Cone+  ( create+  )+where++import Box3D.Hull (HullData)+import Foreign+import Foreign.C.Types (CInt (..))++foreign import ccall unsafe "b3CreateCone"+  c_b3CreateCone :: Float -> Float -> Float -> CInt -> IO (Ptr HullData)++{- | Create a tessellated cone as a hull.++Binds @b3CreateCone@.+-}+create+  :: Float+  -- ^ @height@+  -> Float+  -- ^ @radius1@+  -> Float+  -- ^ @radius2@+  -> Int+  -- ^ @slices@+  -> IO (Ptr HullData)+create a0 a1 a2 a3 =+  c_b3CreateCone a0 a1 a2 (fromIntegral a3)
+ src/Box3D/Contact.hs view
@@ -0,0 +1,40 @@+-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.Contact+  ( isValid+  , getData+  )+where++import Box3D.Id (ContactId)+import Box3D.World (ContactData)+import Foreign+import Foreign.C.Types (CBool (..))++foreign import ccall unsafe "hsg_b3Contact_IsValid"+  c_b3Contact_IsValid :: Ptr ContactId -> IO CBool++foreign import ccall unsafe "hsg_b3Contact_GetData"+  c_b3Contact_GetData :: Ptr ContactId -> Ptr ContactData -> IO ()++{- | Contact identifier validation. Provides validation for up to 2^32 allocations.++Binds @b3Contact_IsValid@.+-}+isValid+  :: ContactId+  -> IO Bool+isValid a0 =+  with a0 $ \p0 ->+    toBool <$> (c_b3Contact_IsValid p0)++{- | Get the manifolds for a contact. The manifold may have no points if the contact is not touching.++Binds @b3Contact_GetData@.+-}+getData+  :: ContactId+  -> IO ContactData+getData a0 =+  with a0 $ \p0 ->+    alloca $ \pOut -> c_b3Contact_GetData p0 pOut >> peek pOut
+ src/Box3D/Cylinder.hs view
@@ -0,0 +1,30 @@+-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.Cylinder+  ( create+  )+where++import Box3D.Hull (HullData)+import Foreign+import Foreign.C.Types (CInt (..))++foreign import ccall unsafe "b3CreateCylinder"+  c_b3CreateCylinder :: Float -> Float -> Float -> CInt -> IO (Ptr HullData)++{- | Create a tessellated cylinder as a hull.++Binds @b3CreateCylinder@.+-}+create+  :: Float+  -- ^ @height@+  -> Float+  -- ^ @radius@+  -> Float+  -- ^ @yOffset@+  -> Int+  -- ^ @sides@+  -> IO (Ptr HullData)+create a0 a1 a2 a3 =+  c_b3CreateCylinder a0 a1 a2 (fromIntegral a3)
+ src/Box3D/DebugDraw.hsc view
@@ -0,0 +1,629 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.DebugDraw+  ( DebugDraw (..)+  , DebugMaterial (..)+  , pattern DebugMaterialDefault+  , pattern DebugMaterialMatte+  , pattern DebugMaterialSoft+  , pattern DebugMaterialDead+  , pattern DebugMaterialGlossy+  , pattern DebugMaterialMetallic+  , HexColor (..)+  , pattern ColorAliceBlue+  , pattern ColorAntiqueWhite+  , pattern ColorAqua+  , pattern ColorAquamarine+  , pattern ColorAzure+  , pattern ColorBeige+  , pattern ColorBisque+  , pattern ColorBlack+  , pattern ColorBlanchedAlmond+  , pattern ColorBlue+  , pattern ColorBlueViolet+  , pattern ColorBrown+  , pattern ColorBurlywood+  , pattern ColorCadetBlue+  , pattern ColorChartreuse+  , pattern ColorChocolate+  , pattern ColorCoral+  , pattern ColorCornflowerBlue+  , pattern ColorCornsilk+  , pattern ColorCrimson+  , pattern ColorCyan+  , pattern ColorDarkBlue+  , pattern ColorDarkCyan+  , pattern ColorDarkGoldenRod+  , pattern ColorDarkGray+  , pattern ColorDarkGreen+  , pattern ColorDarkKhaki+  , pattern ColorDarkMagenta+  , pattern ColorDarkOliveGreen+  , pattern ColorDarkOrange+  , pattern ColorDarkOrchid+  , pattern ColorDarkRed+  , pattern ColorDarkSalmon+  , pattern ColorDarkSeaGreen+  , pattern ColorDarkSlateBlue+  , pattern ColorDarkSlateGray+  , pattern ColorDarkTurquoise+  , pattern ColorDarkViolet+  , pattern ColorDeepPink+  , pattern ColorDeepSkyBlue+  , pattern ColorDimGray+  , pattern ColorDodgerBlue+  , pattern ColorFireBrick+  , pattern ColorFloralWhite+  , pattern ColorForestGreen+  , pattern ColorFuchsia+  , pattern ColorGainsboro+  , pattern ColorGhostWhite+  , pattern ColorGold+  , pattern ColorGoldenRod+  , pattern ColorGray+  , pattern ColorGreen+  , pattern ColorGreenYellow+  , pattern ColorHoneyDew+  , pattern ColorHotPink+  , pattern ColorIndianRed+  , pattern ColorIndigo+  , pattern ColorIvory+  , pattern ColorKhaki+  , pattern ColorLavender+  , pattern ColorLavenderBlush+  , pattern ColorLawnGreen+  , pattern ColorLemonChiffon+  , pattern ColorLightBlue+  , pattern ColorLightCoral+  , pattern ColorLightCyan+  , pattern ColorLightGoldenRodYellow+  , pattern ColorLightGray+  , pattern ColorLightGreen+  , pattern ColorLightPink+  , pattern ColorLightSalmon+  , pattern ColorLightSeaGreen+  , pattern ColorLightSkyBlue+  , pattern ColorLightSlateGray+  , pattern ColorLightSteelBlue+  , pattern ColorLightYellow+  , pattern ColorLime+  , pattern ColorLimeGreen+  , pattern ColorLinen+  , pattern ColorMagenta+  , pattern ColorMaroon+  , pattern ColorMediumAquaMarine+  , pattern ColorMediumBlue+  , pattern ColorMediumOrchid+  , pattern ColorMediumPurple+  , pattern ColorMediumSeaGreen+  , pattern ColorMediumSlateBlue+  , pattern ColorMediumSpringGreen+  , pattern ColorMediumTurquoise+  , pattern ColorMediumVioletRed+  , pattern ColorMidnightBlue+  , pattern ColorMintCream+  , pattern ColorMistyRose+  , pattern ColorMoccasin+  , pattern ColorNavajoWhite+  , pattern ColorNavy+  , pattern ColorOldLace+  , pattern ColorOlive+  , pattern ColorOliveDrab+  , pattern ColorOrange+  , pattern ColorOrangeRed+  , pattern ColorOrchid+  , pattern ColorPaleGoldenRod+  , pattern ColorPaleGreen+  , pattern ColorPaleTurquoise+  , pattern ColorPaleVioletRed+  , pattern ColorPapayaWhip+  , pattern ColorPeachPuff+  , pattern ColorPeru+  , pattern ColorPink+  , pattern ColorPlum+  , pattern ColorPowderBlue+  , pattern ColorPurple+  , pattern ColorRebeccaPurple+  , pattern ColorRed+  , pattern ColorRosyBrown+  , pattern ColorRoyalBlue+  , pattern ColorSaddleBrown+  , pattern ColorSalmon+  , pattern ColorSandyBrown+  , pattern ColorSeaGreen+  , pattern ColorSeaShell+  , pattern ColorSienna+  , pattern ColorSilver+  , pattern ColorSkyBlue+  , pattern ColorSlateBlue+  , pattern ColorSlateGray+  , pattern ColorSnow+  , pattern ColorSpringGreen+  , pattern ColorSteelBlue+  , pattern ColorTan+  , pattern ColorTeal+  , pattern ColorThistle+  , pattern ColorTomato+  , pattern ColorTurquoise+  , pattern ColorViolet+  , pattern ColorWheat+  , pattern ColorWhite+  , pattern ColorWhiteSmoke+  , pattern ColorYellow+  , pattern ColorYellowGreen+  , pattern ColorBox2DRed+  , pattern ColorBox2DBlue+  , pattern ColorBox2DGreen+  , pattern ColorBox2DYellow+  , defaultDebugDraw+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..))+import Box3D.Internal (allocaOut)+import Box3D.MathTypes (AABB)+import Box3D.Tags (DrawBoundsFcn, DrawBoxFcn, DrawCapsuleFcn, DrawPointFcn, DrawSegmentFcn, DrawShapeFcn, DrawSphereFcn, DrawStringFcn, DrawTransformFcn)++#include "box3d/box3d.h"+++{- | This struct is passed to @b3World_Draw@ to draw a debug view of the simulation world.+Callbacks receive world coordinates. In large world mode the translation is double precision so+it stays accurate far from the origin. Shift into your own camera frame inside the callbacks.+-}+data DebugDraw = DebugDraw+  { drawShapeFcn :: FunPtr DrawShapeFcn+  -- ^ Draws a user shape. The userShape pointer is owned by the application and is known to Box3D as+  -- an opaque pointer returned from @b3CreateDebugShapeCallback@. When this is called the drawn shape has+  -- passed a culling test against drawingBounds below.+  , drawSegmentFcn :: FunPtr DrawSegmentFcn+  -- ^ Draw a line segment.+  , drawTransformFcn :: FunPtr DrawTransformFcn+  -- ^ Draw a transform. Choose your own length scale.+  , drawPointFcn :: FunPtr DrawPointFcn+  -- ^ Draw a point.+  , drawSphereFcn :: FunPtr DrawSphereFcn+  -- ^ Draw a sphere.+  , drawCapsuleFcn :: FunPtr DrawCapsuleFcn+  -- ^ Draw a capsule.+  , drawBoundsFcn :: FunPtr DrawBoundsFcn+  -- ^ Draw a bounding box.+  , drawBoxFcn :: FunPtr DrawBoxFcn+  -- ^ Draw an oriented box.+  , drawStringFcn :: FunPtr DrawStringFcn+  -- ^ Draw a string in world space+  , drawingBounds :: AABB+  -- ^ World bounds to use for debug draw+  , forceScale :: Float+  -- ^ Scale to use when drawing forces+  , jointScale :: Float+  -- ^ Global scaling for joint drawing+  , drawShapes :: CBool+  -- ^ Option to draw shapes+  , drawJoints :: CBool+  -- ^ Option to draw joints+  , drawJointExtras :: CBool+  -- ^ Option to draw additional information for joints+  , drawBounds :: CBool+  -- ^ Option to draw the bounding boxes for shapes+  , drawMass :: CBool+  -- ^ Option to draw the mass and center of mass of dynamic bodies+  , drawSleep :: CBool+  -- ^ Option to draw the sleep information for dynamic and kinematic bodies+  , drawBodyNames :: CBool+  -- ^ Option to draw body names+  , drawContacts :: CBool+  -- ^ Option to draw contact points+  , drawAnchorA :: CBool+  -- ^ Draw contact anchor A or B+  , drawGraphColors :: CBool+  -- ^ Option to visualize the graph coloring used for contacts and joints+  , drawContactFeatures :: CBool+  -- ^ Option to draw contact features+  , drawContactNormals :: CBool+  -- ^ Option to draw contact normals+  , drawContactForces :: CBool+  -- ^ Option to draw contact normal forces+  , drawIslands :: CBool+  -- ^ Option to draw islands as bounding boxes+  , context :: Ptr ()+  -- ^ Draws a user shape. The userShape pointer is owned by the application and is known to Box3D as+  -- an opaque pointer returned from @b3CreateDebugShapeCallback@. When this is called the drawn shape has+  -- passed a culling test against drawingBounds below.+  }+  deriving (Eq, Show)++instance Storable DebugDraw where+  sizeOf _ = #{size b3DebugDraw}+  alignment _ = #{alignment b3DebugDraw}+  peek p =+    DebugDraw+      <$> #{peek b3DebugDraw, DrawShapeFcn} p+      <*> #{peek b3DebugDraw, DrawSegmentFcn} p+      <*> #{peek b3DebugDraw, DrawTransformFcn} p+      <*> #{peek b3DebugDraw, DrawPointFcn} p+      <*> #{peek b3DebugDraw, DrawSphereFcn} p+      <*> #{peek b3DebugDraw, DrawCapsuleFcn} p+      <*> #{peek b3DebugDraw, DrawBoundsFcn} p+      <*> #{peek b3DebugDraw, DrawBoxFcn} p+      <*> #{peek b3DebugDraw, DrawStringFcn} p+      <*> #{peek b3DebugDraw, drawingBounds} p+      <*> #{peek b3DebugDraw, forceScale} p+      <*> #{peek b3DebugDraw, jointScale} p+      <*> #{peek b3DebugDraw, drawShapes} p+      <*> #{peek b3DebugDraw, drawJoints} p+      <*> #{peek b3DebugDraw, drawJointExtras} p+      <*> #{peek b3DebugDraw, drawBounds} p+      <*> #{peek b3DebugDraw, drawMass} p+      <*> #{peek b3DebugDraw, drawSleep} p+      <*> #{peek b3DebugDraw, drawBodyNames} p+      <*> #{peek b3DebugDraw, drawContacts} p+      <*> #{peek b3DebugDraw, drawAnchorA} p+      <*> #{peek b3DebugDraw, drawGraphColors} p+      <*> #{peek b3DebugDraw, drawContactFeatures} p+      <*> #{peek b3DebugDraw, drawContactNormals} p+      <*> #{peek b3DebugDraw, drawContactForces} p+      <*> #{peek b3DebugDraw, drawIslands} p+      <*> #{peek b3DebugDraw, context} p+  poke p (DebugDraw f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11 f12 f13 f14 f15 f16 f17 f18 f19 f20 f21 f22 f23 f24 f25 f26 f27) = do+    #{poke b3DebugDraw, DrawShapeFcn} p f1+    #{poke b3DebugDraw, DrawSegmentFcn} p f2+    #{poke b3DebugDraw, DrawTransformFcn} p f3+    #{poke b3DebugDraw, DrawPointFcn} p f4+    #{poke b3DebugDraw, DrawSphereFcn} p f5+    #{poke b3DebugDraw, DrawCapsuleFcn} p f6+    #{poke b3DebugDraw, DrawBoundsFcn} p f7+    #{poke b3DebugDraw, DrawBoxFcn} p f8+    #{poke b3DebugDraw, DrawStringFcn} p f9+    #{poke b3DebugDraw, drawingBounds} p f10+    #{poke b3DebugDraw, forceScale} p f11+    #{poke b3DebugDraw, jointScale} p f12+    #{poke b3DebugDraw, drawShapes} p f13+    #{poke b3DebugDraw, drawJoints} p f14+    #{poke b3DebugDraw, drawJointExtras} p f15+    #{poke b3DebugDraw, drawBounds} p f16+    #{poke b3DebugDraw, drawMass} p f17+    #{poke b3DebugDraw, drawSleep} p f18+    #{poke b3DebugDraw, drawBodyNames} p f19+    #{poke b3DebugDraw, drawContacts} p f20+    #{poke b3DebugDraw, drawAnchorA} p f21+    #{poke b3DebugDraw, drawGraphColors} p f22+    #{poke b3DebugDraw, drawContactFeatures} p f23+    #{poke b3DebugDraw, drawContactNormals} p f24+    #{poke b3DebugDraw, drawContactForces} p f25+    #{poke b3DebugDraw, drawIslands} p f26+    #{poke b3DebugDraw, context} p f27++{- | Debug draw material preset. Optionally packed into the unused high byte of a+@b3HexColor@ (or @b3SurfaceMaterial::customColor@) to drive the renderer\'s PBR+roughness and metalness. The low 24 bits stay RGB, so a plain 0xRRGGBB color+reads as @b3_debugMaterialDefault@ and keeps the renderer\'s per-body-type look.+-}+newtype DebugMaterial = DebugMaterial CInt+  deriving (Eq, Show, Storable)++pattern DebugMaterialDefault :: DebugMaterial+pattern DebugMaterialMatte :: DebugMaterial+pattern DebugMaterialSoft :: DebugMaterial+pattern DebugMaterialDead :: DebugMaterial+pattern DebugMaterialGlossy :: DebugMaterial+pattern DebugMaterialMetallic :: DebugMaterial+pattern DebugMaterialDefault = DebugMaterial #{const b3_debugMaterialDefault}+pattern DebugMaterialMatte = DebugMaterial #{const b3_debugMaterialMatte}+pattern DebugMaterialSoft = DebugMaterial #{const b3_debugMaterialSoft}+pattern DebugMaterialDead = DebugMaterial #{const b3_debugMaterialDead}+pattern DebugMaterialGlossy = DebugMaterial #{const b3_debugMaterialGlossy}+pattern DebugMaterialMetallic = DebugMaterial #{const b3_debugMaterialMetallic}++{- | 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>+-}+newtype HexColor = HexColor CInt+  deriving (Eq, Show, Storable)++pattern ColorAliceBlue :: HexColor+pattern ColorAntiqueWhite :: HexColor+pattern ColorAqua :: HexColor+pattern ColorAquamarine :: HexColor+pattern ColorAzure :: HexColor+pattern ColorBeige :: HexColor+pattern ColorBisque :: HexColor+pattern ColorBlack :: HexColor+pattern ColorBlanchedAlmond :: HexColor+pattern ColorBlue :: HexColor+pattern ColorBlueViolet :: HexColor+pattern ColorBrown :: HexColor+pattern ColorBurlywood :: HexColor+pattern ColorCadetBlue :: HexColor+pattern ColorChartreuse :: HexColor+pattern ColorChocolate :: HexColor+pattern ColorCoral :: HexColor+pattern ColorCornflowerBlue :: HexColor+pattern ColorCornsilk :: HexColor+pattern ColorCrimson :: HexColor+pattern ColorCyan :: HexColor+pattern ColorDarkBlue :: HexColor+pattern ColorDarkCyan :: HexColor+pattern ColorDarkGoldenRod :: HexColor+pattern ColorDarkGray :: HexColor+pattern ColorDarkGreen :: HexColor+pattern ColorDarkKhaki :: HexColor+pattern ColorDarkMagenta :: HexColor+pattern ColorDarkOliveGreen :: HexColor+pattern ColorDarkOrange :: HexColor+pattern ColorDarkOrchid :: HexColor+pattern ColorDarkRed :: HexColor+pattern ColorDarkSalmon :: HexColor+pattern ColorDarkSeaGreen :: HexColor+pattern ColorDarkSlateBlue :: HexColor+pattern ColorDarkSlateGray :: HexColor+pattern ColorDarkTurquoise :: HexColor+pattern ColorDarkViolet :: HexColor+pattern ColorDeepPink :: HexColor+pattern ColorDeepSkyBlue :: HexColor+pattern ColorDimGray :: HexColor+pattern ColorDodgerBlue :: HexColor+pattern ColorFireBrick :: HexColor+pattern ColorFloralWhite :: HexColor+pattern ColorForestGreen :: HexColor+pattern ColorFuchsia :: HexColor+pattern ColorGainsboro :: HexColor+pattern ColorGhostWhite :: HexColor+pattern ColorGold :: HexColor+pattern ColorGoldenRod :: HexColor+pattern ColorGray :: HexColor+pattern ColorGreen :: HexColor+pattern ColorGreenYellow :: HexColor+pattern ColorHoneyDew :: HexColor+pattern ColorHotPink :: HexColor+pattern ColorIndianRed :: HexColor+pattern ColorIndigo :: HexColor+pattern ColorIvory :: HexColor+pattern ColorKhaki :: HexColor+pattern ColorLavender :: HexColor+pattern ColorLavenderBlush :: HexColor+pattern ColorLawnGreen :: HexColor+pattern ColorLemonChiffon :: HexColor+pattern ColorLightBlue :: HexColor+pattern ColorLightCoral :: HexColor+pattern ColorLightCyan :: HexColor+pattern ColorLightGoldenRodYellow :: HexColor+pattern ColorLightGray :: HexColor+pattern ColorLightGreen :: HexColor+pattern ColorLightPink :: HexColor+pattern ColorLightSalmon :: HexColor+pattern ColorLightSeaGreen :: HexColor+pattern ColorLightSkyBlue :: HexColor+pattern ColorLightSlateGray :: HexColor+pattern ColorLightSteelBlue :: HexColor+pattern ColorLightYellow :: HexColor+pattern ColorLime :: HexColor+pattern ColorLimeGreen :: HexColor+pattern ColorLinen :: HexColor+pattern ColorMagenta :: HexColor+pattern ColorMaroon :: HexColor+pattern ColorMediumAquaMarine :: HexColor+pattern ColorMediumBlue :: HexColor+pattern ColorMediumOrchid :: HexColor+pattern ColorMediumPurple :: HexColor+pattern ColorMediumSeaGreen :: HexColor+pattern ColorMediumSlateBlue :: HexColor+pattern ColorMediumSpringGreen :: HexColor+pattern ColorMediumTurquoise :: HexColor+pattern ColorMediumVioletRed :: HexColor+pattern ColorMidnightBlue :: HexColor+pattern ColorMintCream :: HexColor+pattern ColorMistyRose :: HexColor+pattern ColorMoccasin :: HexColor+pattern ColorNavajoWhite :: HexColor+pattern ColorNavy :: HexColor+pattern ColorOldLace :: HexColor+pattern ColorOlive :: HexColor+pattern ColorOliveDrab :: HexColor+pattern ColorOrange :: HexColor+pattern ColorOrangeRed :: HexColor+pattern ColorOrchid :: HexColor+pattern ColorPaleGoldenRod :: HexColor+pattern ColorPaleGreen :: HexColor+pattern ColorPaleTurquoise :: HexColor+pattern ColorPaleVioletRed :: HexColor+pattern ColorPapayaWhip :: HexColor+pattern ColorPeachPuff :: HexColor+pattern ColorPeru :: HexColor+pattern ColorPink :: HexColor+pattern ColorPlum :: HexColor+pattern ColorPowderBlue :: HexColor+pattern ColorPurple :: HexColor+pattern ColorRebeccaPurple :: HexColor+pattern ColorRed :: HexColor+pattern ColorRosyBrown :: HexColor+pattern ColorRoyalBlue :: HexColor+pattern ColorSaddleBrown :: HexColor+pattern ColorSalmon :: HexColor+pattern ColorSandyBrown :: HexColor+pattern ColorSeaGreen :: HexColor+pattern ColorSeaShell :: HexColor+pattern ColorSienna :: HexColor+pattern ColorSilver :: HexColor+pattern ColorSkyBlue :: HexColor+pattern ColorSlateBlue :: HexColor+pattern ColorSlateGray :: HexColor+pattern ColorSnow :: HexColor+pattern ColorSpringGreen :: HexColor+pattern ColorSteelBlue :: HexColor+pattern ColorTan :: HexColor+pattern ColorTeal :: HexColor+pattern ColorThistle :: HexColor+pattern ColorTomato :: HexColor+pattern ColorTurquoise :: HexColor+pattern ColorViolet :: HexColor+pattern ColorWheat :: HexColor+pattern ColorWhite :: HexColor+pattern ColorWhiteSmoke :: HexColor+pattern ColorYellow :: HexColor+pattern ColorYellowGreen :: HexColor+pattern ColorBox2DRed :: HexColor+pattern ColorBox2DBlue :: HexColor+pattern ColorBox2DGreen :: HexColor+pattern ColorBox2DYellow :: HexColor+pattern ColorAliceBlue = HexColor #{const b3_colorAliceBlue}+pattern ColorAntiqueWhite = HexColor #{const b3_colorAntiqueWhite}+pattern ColorAqua = HexColor #{const b3_colorAqua}+pattern ColorAquamarine = HexColor #{const b3_colorAquamarine}+pattern ColorAzure = HexColor #{const b3_colorAzure}+pattern ColorBeige = HexColor #{const b3_colorBeige}+pattern ColorBisque = HexColor #{const b3_colorBisque}+pattern ColorBlack = HexColor #{const b3_colorBlack}+pattern ColorBlanchedAlmond = HexColor #{const b3_colorBlanchedAlmond}+pattern ColorBlue = HexColor #{const b3_colorBlue}+pattern ColorBlueViolet = HexColor #{const b3_colorBlueViolet}+pattern ColorBrown = HexColor #{const b3_colorBrown}+pattern ColorBurlywood = HexColor #{const b3_colorBurlywood}+pattern ColorCadetBlue = HexColor #{const b3_colorCadetBlue}+pattern ColorChartreuse = HexColor #{const b3_colorChartreuse}+pattern ColorChocolate = HexColor #{const b3_colorChocolate}+pattern ColorCoral = HexColor #{const b3_colorCoral}+pattern ColorCornflowerBlue = HexColor #{const b3_colorCornflowerBlue}+pattern ColorCornsilk = HexColor #{const b3_colorCornsilk}+pattern ColorCrimson = HexColor #{const b3_colorCrimson}+pattern ColorCyan = HexColor #{const b3_colorCyan}+pattern ColorDarkBlue = HexColor #{const b3_colorDarkBlue}+pattern ColorDarkCyan = HexColor #{const b3_colorDarkCyan}+pattern ColorDarkGoldenRod = HexColor #{const b3_colorDarkGoldenRod}+pattern ColorDarkGray = HexColor #{const b3_colorDarkGray}+pattern ColorDarkGreen = HexColor #{const b3_colorDarkGreen}+pattern ColorDarkKhaki = HexColor #{const b3_colorDarkKhaki}+pattern ColorDarkMagenta = HexColor #{const b3_colorDarkMagenta}+pattern ColorDarkOliveGreen = HexColor #{const b3_colorDarkOliveGreen}+pattern ColorDarkOrange = HexColor #{const b3_colorDarkOrange}+pattern ColorDarkOrchid = HexColor #{const b3_colorDarkOrchid}+pattern ColorDarkRed = HexColor #{const b3_colorDarkRed}+pattern ColorDarkSalmon = HexColor #{const b3_colorDarkSalmon}+pattern ColorDarkSeaGreen = HexColor #{const b3_colorDarkSeaGreen}+pattern ColorDarkSlateBlue = HexColor #{const b3_colorDarkSlateBlue}+pattern ColorDarkSlateGray = HexColor #{const b3_colorDarkSlateGray}+pattern ColorDarkTurquoise = HexColor #{const b3_colorDarkTurquoise}+pattern ColorDarkViolet = HexColor #{const b3_colorDarkViolet}+pattern ColorDeepPink = HexColor #{const b3_colorDeepPink}+pattern ColorDeepSkyBlue = HexColor #{const b3_colorDeepSkyBlue}+pattern ColorDimGray = HexColor #{const b3_colorDimGray}+pattern ColorDodgerBlue = HexColor #{const b3_colorDodgerBlue}+pattern ColorFireBrick = HexColor #{const b3_colorFireBrick}+pattern ColorFloralWhite = HexColor #{const b3_colorFloralWhite}+pattern ColorForestGreen = HexColor #{const b3_colorForestGreen}+pattern ColorFuchsia = HexColor #{const b3_colorFuchsia}+pattern ColorGainsboro = HexColor #{const b3_colorGainsboro}+pattern ColorGhostWhite = HexColor #{const b3_colorGhostWhite}+pattern ColorGold = HexColor #{const b3_colorGold}+pattern ColorGoldenRod = HexColor #{const b3_colorGoldenRod}+pattern ColorGray = HexColor #{const b3_colorGray}+pattern ColorGreen = HexColor #{const b3_colorGreen}+pattern ColorGreenYellow = HexColor #{const b3_colorGreenYellow}+pattern ColorHoneyDew = HexColor #{const b3_colorHoneyDew}+pattern ColorHotPink = HexColor #{const b3_colorHotPink}+pattern ColorIndianRed = HexColor #{const b3_colorIndianRed}+pattern ColorIndigo = HexColor #{const b3_colorIndigo}+pattern ColorIvory = HexColor #{const b3_colorIvory}+pattern ColorKhaki = HexColor #{const b3_colorKhaki}+pattern ColorLavender = HexColor #{const b3_colorLavender}+pattern ColorLavenderBlush = HexColor #{const b3_colorLavenderBlush}+pattern ColorLawnGreen = HexColor #{const b3_colorLawnGreen}+pattern ColorLemonChiffon = HexColor #{const b3_colorLemonChiffon}+pattern ColorLightBlue = HexColor #{const b3_colorLightBlue}+pattern ColorLightCoral = HexColor #{const b3_colorLightCoral}+pattern ColorLightCyan = HexColor #{const b3_colorLightCyan}+pattern ColorLightGoldenRodYellow = HexColor #{const b3_colorLightGoldenRodYellow}+pattern ColorLightGray = HexColor #{const b3_colorLightGray}+pattern ColorLightGreen = HexColor #{const b3_colorLightGreen}+pattern ColorLightPink = HexColor #{const b3_colorLightPink}+pattern ColorLightSalmon = HexColor #{const b3_colorLightSalmon}+pattern ColorLightSeaGreen = HexColor #{const b3_colorLightSeaGreen}+pattern ColorLightSkyBlue = HexColor #{const b3_colorLightSkyBlue}+pattern ColorLightSlateGray = HexColor #{const b3_colorLightSlateGray}+pattern ColorLightSteelBlue = HexColor #{const b3_colorLightSteelBlue}+pattern ColorLightYellow = HexColor #{const b3_colorLightYellow}+pattern ColorLime = HexColor #{const b3_colorLime}+pattern ColorLimeGreen = HexColor #{const b3_colorLimeGreen}+pattern ColorLinen = HexColor #{const b3_colorLinen}+pattern ColorMagenta = HexColor #{const b3_colorMagenta}+pattern ColorMaroon = HexColor #{const b3_colorMaroon}+pattern ColorMediumAquaMarine = HexColor #{const b3_colorMediumAquaMarine}+pattern ColorMediumBlue = HexColor #{const b3_colorMediumBlue}+pattern ColorMediumOrchid = HexColor #{const b3_colorMediumOrchid}+pattern ColorMediumPurple = HexColor #{const b3_colorMediumPurple}+pattern ColorMediumSeaGreen = HexColor #{const b3_colorMediumSeaGreen}+pattern ColorMediumSlateBlue = HexColor #{const b3_colorMediumSlateBlue}+pattern ColorMediumSpringGreen = HexColor #{const b3_colorMediumSpringGreen}+pattern ColorMediumTurquoise = HexColor #{const b3_colorMediumTurquoise}+pattern ColorMediumVioletRed = HexColor #{const b3_colorMediumVioletRed}+pattern ColorMidnightBlue = HexColor #{const b3_colorMidnightBlue}+pattern ColorMintCream = HexColor #{const b3_colorMintCream}+pattern ColorMistyRose = HexColor #{const b3_colorMistyRose}+pattern ColorMoccasin = HexColor #{const b3_colorMoccasin}+pattern ColorNavajoWhite = HexColor #{const b3_colorNavajoWhite}+pattern ColorNavy = HexColor #{const b3_colorNavy}+pattern ColorOldLace = HexColor #{const b3_colorOldLace}+pattern ColorOlive = HexColor #{const b3_colorOlive}+pattern ColorOliveDrab = HexColor #{const b3_colorOliveDrab}+pattern ColorOrange = HexColor #{const b3_colorOrange}+pattern ColorOrangeRed = HexColor #{const b3_colorOrangeRed}+pattern ColorOrchid = HexColor #{const b3_colorOrchid}+pattern ColorPaleGoldenRod = HexColor #{const b3_colorPaleGoldenRod}+pattern ColorPaleGreen = HexColor #{const b3_colorPaleGreen}+pattern ColorPaleTurquoise = HexColor #{const b3_colorPaleTurquoise}+pattern ColorPaleVioletRed = HexColor #{const b3_colorPaleVioletRed}+pattern ColorPapayaWhip = HexColor #{const b3_colorPapayaWhip}+pattern ColorPeachPuff = HexColor #{const b3_colorPeachPuff}+pattern ColorPeru = HexColor #{const b3_colorPeru}+pattern ColorPink = HexColor #{const b3_colorPink}+pattern ColorPlum = HexColor #{const b3_colorPlum}+pattern ColorPowderBlue = HexColor #{const b3_colorPowderBlue}+pattern ColorPurple = HexColor #{const b3_colorPurple}+pattern ColorRebeccaPurple = HexColor #{const b3_colorRebeccaPurple}+pattern ColorRed = HexColor #{const b3_colorRed}+pattern ColorRosyBrown = HexColor #{const b3_colorRosyBrown}+pattern ColorRoyalBlue = HexColor #{const b3_colorRoyalBlue}+pattern ColorSaddleBrown = HexColor #{const b3_colorSaddleBrown}+pattern ColorSalmon = HexColor #{const b3_colorSalmon}+pattern ColorSandyBrown = HexColor #{const b3_colorSandyBrown}+pattern ColorSeaGreen = HexColor #{const b3_colorSeaGreen}+pattern ColorSeaShell = HexColor #{const b3_colorSeaShell}+pattern ColorSienna = HexColor #{const b3_colorSienna}+pattern ColorSilver = HexColor #{const b3_colorSilver}+pattern ColorSkyBlue = HexColor #{const b3_colorSkyBlue}+pattern ColorSlateBlue = HexColor #{const b3_colorSlateBlue}+pattern ColorSlateGray = HexColor #{const b3_colorSlateGray}+pattern ColorSnow = HexColor #{const b3_colorSnow}+pattern ColorSpringGreen = HexColor #{const b3_colorSpringGreen}+pattern ColorSteelBlue = HexColor #{const b3_colorSteelBlue}+pattern ColorTan = HexColor #{const b3_colorTan}+pattern ColorTeal = HexColor #{const b3_colorTeal}+pattern ColorThistle = HexColor #{const b3_colorThistle}+pattern ColorTomato = HexColor #{const b3_colorTomato}+pattern ColorTurquoise = HexColor #{const b3_colorTurquoise}+pattern ColorViolet = HexColor #{const b3_colorViolet}+pattern ColorWheat = HexColor #{const b3_colorWheat}+pattern ColorWhite = HexColor #{const b3_colorWhite}+pattern ColorWhiteSmoke = HexColor #{const b3_colorWhiteSmoke}+pattern ColorYellow = HexColor #{const b3_colorYellow}+pattern ColorYellowGreen = HexColor #{const b3_colorYellowGreen}+pattern ColorBox2DRed = HexColor #{const b3_colorBox2DRed}+pattern ColorBox2DBlue = HexColor #{const b3_colorBox2DBlue}+pattern ColorBox2DGreen = HexColor #{const b3_colorBox2DGreen}+pattern ColorBox2DYellow = HexColor #{const b3_colorBox2DYellow}++foreign import ccall unsafe "hs_b3DefaultDebugDraw"+  c_b3DefaultDebugDraw :: Ptr DebugDraw -> IO ()++defaultDebugDraw :: IO DebugDraw+defaultDebugDraw = allocaOut c_b3DefaultDebugDraw+++
+ src/Box3D/DebugShape.hsc view
@@ -0,0 +1,90 @@+{-# LANGUAGE PatternSynonyms #-}++{-| The shape description handed to a debug-shape constructor.++@b3DebugShape@ is a tagged union, which the generator does not model, so this+module is hand-written; see 'Box3D.Callbacks.withDebugShapes'.+-}+module Box3D.DebugShape+  ( DebugShape (..)+  , Geometry (..)+  ) where++import Foreign++import Box3D.Compound (CompoundData)+import Box3D.Geometry (Capsule, Sphere)+import Box3D.HeightField (HeightFieldData)+import Box3D.Hull (HullData)+import Box3D.Id (ShapeId (..))+import Box3D.Mesh (Mesh)+import Box3D.Shape (ShapeType (..), pattern CapsuleShape, pattern CompoundShape, pattern HeightShape, pattern HullShape, pattern MeshShape, pattern SphereShape)++#include "box3d/box3d.h"++-- | A shape the renderer is being asked to build its own representation of.+data DebugShape = DebugShape+  { shapeId :: ShapeId+  , geometry :: Geometry+  }+  deriving (Eq, Show)++{- | The union arm selected by @b3DebugShape.type@.++Each pointer is the world's own geometry, not a copy. Peek it with the+'Storable' instance of the pointee inside the constructor and keep the value,+not the pointer: the 'Sphere', 'Capsule' and 'Mesh' arms point into the+world's shape array, which moves whenever it grows, so they are only good for+the duration of the callback. The 'Hull', 'Compound' and 'HeightField' arms+are separately owned and stay put until the shape is changed or destroyed,+which is when the matching 'Box3D.Callbacks.DestroyDebugShape' runs.+-}+data Geometry+  = Sphere (Ptr Sphere)+  | Capsule (Ptr Capsule)+  | Hull (Ptr HullData)+  | Mesh (Ptr Mesh)+  | Compound (Ptr CompoundData)+  | HeightField (Ptr HeightFieldData)+  | Other ShapeType (Ptr ())+  -- ^ A shape type this binding predates.+  deriving (Eq, Show)++instance Storable DebugShape where+  sizeOf _ = #{size b3DebugShape}+  alignment _ = #{alignment b3DebugShape}+  peek p = do+    shapeId <- #{peek b3DebugShape, shapeId} p+    ptr <- #{peek b3DebugShape, sphere} p+    ty <- #{peek b3DebugShape, type} p+    pure DebugShape{shapeId, geometry = tagged ty ptr}+  poke p (DebugShape shapeId geometry) = do+    #{poke b3DebugShape, shapeId} p shapeId+    #{poke b3DebugShape, type} p ty+    #{poke b3DebugShape, sphere} p ptr+    where+      (ty, ptr) = untagged geometry++{- | Guards rather than a @case@: 'ShapeType' is a newtype over @CInt@, so a+type this binding predates must land in 'Other' instead of a pattern-match+failure inside a callback.+-}+tagged :: ShapeType -> Ptr () -> Geometry+tagged ty p+  | ty == SphereShape = Sphere (castPtr p)+  | ty == CapsuleShape = Capsule (castPtr p)+  | ty == HullShape = Hull (castPtr p)+  | ty == MeshShape = Mesh (castPtr p)+  | ty == CompoundShape = Compound (castPtr p)+  | ty == HeightShape = HeightField (castPtr p)+  | otherwise = Other ty p++untagged :: Geometry -> (ShapeType, Ptr ())+untagged g = case g of+  Sphere p -> (SphereShape, castPtr p)+  Capsule p -> (CapsuleShape, castPtr p)+  Hull p -> (HullShape, castPtr p)+  Mesh p -> (MeshShape, castPtr p)+  Compound p -> (CompoundShape, castPtr p)+  HeightField p -> (HeightShape, castPtr p)+  Other ty p -> (ty, p)
+ src/Box3D/DistanceJoint.hsc view
@@ -0,0 +1,465 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.DistanceJoint+  ( DistanceJointDef (..)+  , defaultDistanceJointDef+  , create+  , setLength+  , getLength+  , enableSpring+  , isSpringEnabled+  , setSpringForceRange+  , getSpringForceRange+  , setSpringHertz+  , setSpringDampingRatio+  , getSpringHertz+  , getSpringDampingRatio+  , enableLimit+  , isLimitEnabled+  , setLengthRange+  , getMinLength+  , getMaxLength+  , getCurrentLength+  , enableMotor+  , isMotorEnabled+  , setMotorSpeed+  , getMotorSpeed+  , setMaxMotorForce+  , getMaxMotorForce+  , getMotorForce+  )+  where++import Foreign+import Foreign.C.Types (CBool (..))+import Box3D.Internal (allocaOut)+import Box3D.Id (JointId (..), WorldId (..))+import Box3D.Joint (JointDef)++#include "box3d/box3d.h"+++{- | Distance joint definition.+Connects a point on body A with a point on body B by a segment.+Useful for ropes and springs.+-}+data DistanceJointDef = DistanceJointDef+  { base :: JointDef+  -- ^ Base joint definition+  , length :: Float+  -- ^ The rest length of this joint. Clamped to a stable minimum value.+  , enableSpring :: CBool+  -- ^ Enable the distance constraint to behave like a spring. If false+  -- then the distance joint will be rigid, overriding the limit and motor.+  , lowerSpringForce :: Float+  -- ^ The lower spring force controls how much tension it can sustain+  , upperSpringForce :: Float+  -- ^ The upper spring force controls how much compression it can sustain+  , hertz :: Float+  -- ^ The spring linear stiffness Hertz, cycles per second+  , dampingRatio :: Float+  -- ^ The spring linear damping ratio, non-dimensional+  , enableLimit :: CBool+  -- ^ Enable\/disable the joint limit+  , minLength :: Float+  -- ^ Minimum length. Clamped to a stable minimum value.+  , maxLength :: Float+  -- ^ Maximum length. Must be greater than or equal to the minimum length.+  , enableMotor :: CBool+  -- ^ Enable\/disable the joint motor+  , maxMotorForce :: Float+  -- ^ The maximum motor force, usually in newtons+  , motorSpeed :: Float+  -- ^ The desired motor speed, usually in meters per second+  }+  deriving (Eq, Show)++instance Storable DistanceJointDef where+  sizeOf _ = #{size b3DistanceJointDef}+  alignment _ = #{alignment b3DistanceJointDef}+  peek p =+    DistanceJointDef+      <$> #{peek b3DistanceJointDef, base} p+      <*> #{peek b3DistanceJointDef, length} p+      <*> #{peek b3DistanceJointDef, enableSpring} p+      <*> #{peek b3DistanceJointDef, lowerSpringForce} p+      <*> #{peek b3DistanceJointDef, upperSpringForce} p+      <*> #{peek b3DistanceJointDef, hertz} p+      <*> #{peek b3DistanceJointDef, dampingRatio} p+      <*> #{peek b3DistanceJointDef, enableLimit} p+      <*> #{peek b3DistanceJointDef, minLength} p+      <*> #{peek b3DistanceJointDef, maxLength} p+      <*> #{peek b3DistanceJointDef, enableMotor} p+      <*> #{peek b3DistanceJointDef, maxMotorForce} p+      <*> #{peek b3DistanceJointDef, motorSpeed} p+  poke p (DistanceJointDef f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11 f12 f13) = do+    #{poke b3DistanceJointDef, base} p f1+    #{poke b3DistanceJointDef, length} p f2+    #{poke b3DistanceJointDef, enableSpring} p f3+    #{poke b3DistanceJointDef, lowerSpringForce} p f4+    #{poke b3DistanceJointDef, upperSpringForce} p f5+    #{poke b3DistanceJointDef, hertz} p f6+    #{poke b3DistanceJointDef, dampingRatio} p f7+    #{poke b3DistanceJointDef, enableLimit} p f8+    #{poke b3DistanceJointDef, minLength} p f9+    #{poke b3DistanceJointDef, maxLength} p f10+    #{poke b3DistanceJointDef, enableMotor} p f11+    #{poke b3DistanceJointDef, maxMotorForce} p f12+    #{poke b3DistanceJointDef, motorSpeed} p f13++foreign import ccall unsafe "hs_b3DefaultDistanceJointDef"+  c_b3DefaultDistanceJointDef :: Ptr DistanceJointDef -> IO ()++defaultDistanceJointDef :: IO DistanceJointDef+defaultDistanceJointDef = allocaOut c_b3DefaultDistanceJointDef+++foreign import ccall unsafe "b3CreateDistanceJoint"+  c_b3CreateDistanceJoint :: WorldId -> Ptr DistanceJointDef -> IO JointId++foreign import ccall unsafe "b3DistanceJoint_SetLength"+  c_b3DistanceJoint_SetLength :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3DistanceJoint_GetLength"+  c_b3DistanceJoint_GetLength :: JointId -> IO Float++foreign import ccall unsafe "b3DistanceJoint_EnableSpring"+  c_b3DistanceJoint_EnableSpring :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b3DistanceJoint_IsSpringEnabled"+  c_b3DistanceJoint_IsSpringEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b3DistanceJoint_SetSpringForceRange"+  c_b3DistanceJoint_SetSpringForceRange :: JointId -> Float -> Float -> IO ()++foreign import ccall unsafe "b3DistanceJoint_GetSpringForceRange"+  c_b3DistanceJoint_GetSpringForceRange :: JointId -> Ptr Float -> Ptr Float -> IO ()++foreign import ccall unsafe "b3DistanceJoint_SetSpringHertz"+  c_b3DistanceJoint_SetSpringHertz :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3DistanceJoint_SetSpringDampingRatio"+  c_b3DistanceJoint_SetSpringDampingRatio :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3DistanceJoint_GetSpringHertz"+  c_b3DistanceJoint_GetSpringHertz :: JointId -> IO Float++foreign import ccall unsafe "b3DistanceJoint_GetSpringDampingRatio"+  c_b3DistanceJoint_GetSpringDampingRatio :: JointId -> IO Float++foreign import ccall unsafe "b3DistanceJoint_EnableLimit"+  c_b3DistanceJoint_EnableLimit :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b3DistanceJoint_IsLimitEnabled"+  c_b3DistanceJoint_IsLimitEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b3DistanceJoint_SetLengthRange"+  c_b3DistanceJoint_SetLengthRange :: JointId -> Float -> Float -> IO ()++foreign import ccall unsafe "b3DistanceJoint_GetMinLength"+  c_b3DistanceJoint_GetMinLength :: JointId -> IO Float++foreign import ccall unsafe "b3DistanceJoint_GetMaxLength"+  c_b3DistanceJoint_GetMaxLength :: JointId -> IO Float++foreign import ccall unsafe "b3DistanceJoint_GetCurrentLength"+  c_b3DistanceJoint_GetCurrentLength :: JointId -> IO Float++foreign import ccall unsafe "b3DistanceJoint_EnableMotor"+  c_b3DistanceJoint_EnableMotor :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b3DistanceJoint_IsMotorEnabled"+  c_b3DistanceJoint_IsMotorEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b3DistanceJoint_SetMotorSpeed"+  c_b3DistanceJoint_SetMotorSpeed :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3DistanceJoint_GetMotorSpeed"+  c_b3DistanceJoint_GetMotorSpeed :: JointId -> IO Float++foreign import ccall unsafe "b3DistanceJoint_SetMaxMotorForce"+  c_b3DistanceJoint_SetMaxMotorForce :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3DistanceJoint_GetMaxMotorForce"+  c_b3DistanceJoint_GetMaxMotorForce :: JointId -> IO Float++foreign import ccall unsafe "b3DistanceJoint_GetMotorForce"+  c_b3DistanceJoint_GetMotorForce :: JointId -> IO Float++{- | Create a distance joint++See also @b3DistanceJointDef@ for details.++Binds @b3CreateDistanceJoint@.+-}+create+  :: WorldId+  -> DistanceJointDef+  -> IO JointId+create a0 a1 =+  with a1 $ \p1 ->+  c_b3CreateDistanceJoint a0 p1++{- | Set the rest length of a distance joint++Binds @b3DistanceJoint_SetLength@.+-}+setLength+  :: JointId+  -- ^ @jointId@: The id for a distance joint+  -> Float+  -- ^ @length@: The new distance joint length+  -> IO ()+setLength a0 a1 =+  c_b3DistanceJoint_SetLength a0 a1++{- | Get the rest length of a distance joint++Binds @b3DistanceJoint_GetLength@.+-}+getLength+  :: JointId+  -> IO Float+getLength a0 =+  c_b3DistanceJoint_GetLength a0++{- | Enable\/disable the distance joint spring. When disabled the distance joint is rigid.++Binds @b3DistanceJoint_EnableSpring@.+-}+enableSpring+  :: JointId+  -> Bool+  -- ^ @enableSpring@+  -> IO ()+enableSpring a0 a1 =+  c_b3DistanceJoint_EnableSpring a0 (fromBool a1)++{- | Is the distance joint spring enabled?++Binds @b3DistanceJoint_IsSpringEnabled@.+-}+isSpringEnabled+  :: JointId+  -> IO Bool+isSpringEnabled a0 =+  toBool <$> (c_b3DistanceJoint_IsSpringEnabled a0)++{- | Set the force range for the spring.++Binds @b3DistanceJoint_SetSpringForceRange@.+-}+setSpringForceRange+  :: JointId+  -> Float+  -- ^ @lowerForce@+  -> Float+  -- ^ @upperForce@+  -> IO ()+setSpringForceRange a0 a1 a2 =+  c_b3DistanceJoint_SetSpringForceRange a0 a1 a2++{- | Get the force range for the spring.++Binds @b3DistanceJoint_GetSpringForceRange@.+-}+getSpringForceRange+  :: JointId+  -> Ptr Float+  -- ^ @lowerForce@+  -> Ptr Float+  -- ^ @upperForce@+  -> IO ()+getSpringForceRange a0 a1 a2 =+  c_b3DistanceJoint_GetSpringForceRange a0 a1 a2++{- | Set the spring stiffness in Hertz++Binds @b3DistanceJoint_SetSpringHertz@.+-}+setSpringHertz+  :: JointId+  -> Float+  -- ^ @hertz@+  -> IO ()+setSpringHertz a0 a1 =+  c_b3DistanceJoint_SetSpringHertz a0 a1++{- | Set the spring damping ratio, non-dimensional++Binds @b3DistanceJoint_SetSpringDampingRatio@.+-}+setSpringDampingRatio+  :: JointId+  -> Float+  -- ^ @dampingRatio@+  -> IO ()+setSpringDampingRatio a0 a1 =+  c_b3DistanceJoint_SetSpringDampingRatio a0 a1++{- | Get the spring Hertz++Binds @b3DistanceJoint_GetSpringHertz@.+-}+getSpringHertz+  :: JointId+  -> IO Float+getSpringHertz a0 =+  c_b3DistanceJoint_GetSpringHertz a0++{- | Get the spring damping ratio++Binds @b3DistanceJoint_GetSpringDampingRatio@.+-}+getSpringDampingRatio+  :: JointId+  -> IO Float+getSpringDampingRatio a0 =+  c_b3DistanceJoint_GetSpringDampingRatio a0++{- | Enable joint limit. The limit only works if the joint spring is enabled. Otherwise the joint is rigid+and the limit has no effect.++Binds @b3DistanceJoint_EnableLimit@.+-}+enableLimit+  :: JointId+  -> Bool+  -- ^ @enableLimit@+  -> IO ()+enableLimit a0 a1 =+  c_b3DistanceJoint_EnableLimit a0 (fromBool a1)++{- | Is the distance joint limit enabled?++Binds @b3DistanceJoint_IsLimitEnabled@.+-}+isLimitEnabled+  :: JointId+  -> IO Bool+isLimitEnabled a0 =+  toBool <$> (c_b3DistanceJoint_IsLimitEnabled a0)++{- | Set the minimum and maximum length parameters of a distance joint++Binds @b3DistanceJoint_SetLengthRange@.+-}+setLengthRange+  :: JointId+  -> Float+  -- ^ @minLength@+  -> Float+  -- ^ @maxLength@+  -> IO ()+setLengthRange a0 a1 a2 =+  c_b3DistanceJoint_SetLengthRange a0 a1 a2++{- | Get the distance joint minimum length++Binds @b3DistanceJoint_GetMinLength@.+-}+getMinLength+  :: JointId+  -> IO Float+getMinLength a0 =+  c_b3DistanceJoint_GetMinLength a0++{- | Get the distance joint maximum length++Binds @b3DistanceJoint_GetMaxLength@.+-}+getMaxLength+  :: JointId+  -> IO Float+getMaxLength a0 =+  c_b3DistanceJoint_GetMaxLength a0++{- | Get the current length of a distance joint++Binds @b3DistanceJoint_GetCurrentLength@.+-}+getCurrentLength+  :: JointId+  -> IO Float+getCurrentLength a0 =+  c_b3DistanceJoint_GetCurrentLength a0++{- | Enable\/disable the distance joint motor++Binds @b3DistanceJoint_EnableMotor@.+-}+enableMotor+  :: JointId+  -> Bool+  -- ^ @enableMotor@+  -> IO ()+enableMotor a0 a1 =+  c_b3DistanceJoint_EnableMotor a0 (fromBool a1)++{- | Is the distance joint motor enabled?++Binds @b3DistanceJoint_IsMotorEnabled@.+-}+isMotorEnabled+  :: JointId+  -> IO Bool+isMotorEnabled a0 =+  toBool <$> (c_b3DistanceJoint_IsMotorEnabled a0)++{- | Set the distance joint motor speed, usually in meters per second++Binds @b3DistanceJoint_SetMotorSpeed@.+-}+setMotorSpeed+  :: JointId+  -> Float+  -- ^ @motorSpeed@+  -> IO ()+setMotorSpeed a0 a1 =+  c_b3DistanceJoint_SetMotorSpeed a0 a1++{- | Get the distance joint motor speed, usually in meters per second++Binds @b3DistanceJoint_GetMotorSpeed@.+-}+getMotorSpeed+  :: JointId+  -> IO Float+getMotorSpeed a0 =+  c_b3DistanceJoint_GetMotorSpeed a0++{- | Set the distance joint maximum motor force, usually in newtons++Binds @b3DistanceJoint_SetMaxMotorForce@.+-}+setMaxMotorForce+  :: JointId+  -> Float+  -- ^ @force@+  -> IO ()+setMaxMotorForce a0 a1 =+  c_b3DistanceJoint_SetMaxMotorForce a0 a1++{- | Get the distance joint maximum motor force, usually in newtons++Binds @b3DistanceJoint_GetMaxMotorForce@.+-}+getMaxMotorForce+  :: JointId+  -> IO Float+getMaxMotorForce a0 =+  c_b3DistanceJoint_GetMaxMotorForce a0++{- | Get the distance joint current motor force, usually in newtons++Binds @b3DistanceJoint_GetMotorForce@.+-}+getMotorForce+  :: JointId+  -> IO Float+getMotorForce a0 =+  c_b3DistanceJoint_GetMotorForce a0
+ src/Box3D/DynamicTree.hsc view
@@ -0,0 +1,573 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.DynamicTree+  ( TreeStats (..)+  , DynamicTree (..)+  , TreeNodeChildren (..)+  , TreeNodeFlags (..)+  , pattern AllocatedNode+  , pattern EnlargedNode+  , pattern LeafNode+  , dynamicTreeByteCount+  , create+  , destroy+  , createProxy+  , destroyProxy+  , moveProxy+  , enlargeProxy+  , setCategoryBits+  , getCategoryBits+  , query+  , queryClosest+  , rayCast+  , boxCast+  , boxCastInto+  , getHeight+  , getAreaRatio+  , getRootBounds+  , getRootBoundsInto+  , getProxyCount+  , rebuild+  , getByteCount+  , validate+  , validateNoEnlarged+  , save+  , load+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..))+import Foreign.C.String (CString)+import Box3D.MathTypes (AABB, Vec3)+import Box3D.Query (BoxCastInput, RayCastInput)+import Box3D.Tags (TreeBoxCastCallbackFcn, TreeNode, TreeQueryCallbackFcn, TreeQueryClosestCallbackFcn, TreeRayCastCallbackFcn)++#include "box3d/box3d.h"+++-- | These are performance results returned by dynamic tree queries.+data TreeStats = TreeStats+  { nodeVisits :: CInt+  -- ^ Number of internal nodes visited during the query+  , leafVisits :: CInt+  -- ^ Number of leaf nodes visited during the query+  }+  deriving (Eq, Show)++instance Storable TreeStats where+  sizeOf _ = #{size b3TreeStats}+  alignment _ = #{alignment b3TreeStats}+  peek p =+    TreeStats+      <$> #{peek b3TreeStats, nodeVisits} p+      <*> #{peek b3TreeStats, leafVisits} p+  poke p (TreeStats f1 f2) = do+    #{poke b3TreeStats, nodeVisits} p f1+    #{poke b3TreeStats, leafVisits} p f2++{- | The dynamic tree structure. This should be considered private data.+It is placed here for performance reasons.+-}+data DynamicTree = DynamicTree+  { version :: Word64+  -- ^ The dynamic tree version. Always the first field. Useful+  -- if the tree is serialized.+  , nodes :: Ptr TreeNode+  -- ^ The tree nodes+  , root :: CInt+  -- ^ The root index+  , nodeCount :: CInt+  -- ^ The number of nodes+  , nodeCapacity :: CInt+  -- ^ The allocated node space+  , proxyCount :: CInt+  -- ^ Number of proxies created+  , freeList :: CInt+  -- ^ Node free list+  , leafIndices :: Ptr ()+  -- ^ Leaf indices for rebuild+  , leafBoxes :: Ptr AABB+  -- ^ Leaf bounding boxes for rebuild+  , leafCenters :: Ptr Vec3+  -- ^ Leaf bounding box centers for rebuild+  , binIndices :: Ptr ()+  -- ^ Bins for sorting during rebuild+  , rebuildCapacity :: CInt+  -- ^ Allocated space for rebuilding+  }+  deriving (Eq, Show)++instance Storable DynamicTree where+  sizeOf _ = #{size b3DynamicTree}+  alignment _ = #{alignment b3DynamicTree}+  peek p =+    DynamicTree+      <$> #{peek b3DynamicTree, version} p+      <*> #{peek b3DynamicTree, nodes} p+      <*> #{peek b3DynamicTree, root} p+      <*> #{peek b3DynamicTree, nodeCount} p+      <*> #{peek b3DynamicTree, nodeCapacity} p+      <*> #{peek b3DynamicTree, proxyCount} p+      <*> #{peek b3DynamicTree, freeList} p+      <*> #{peek b3DynamicTree, leafIndices} p+      <*> #{peek b3DynamicTree, leafBoxes} p+      <*> #{peek b3DynamicTree, leafCenters} p+      <*> #{peek b3DynamicTree, binIndices} p+      <*> #{peek b3DynamicTree, rebuildCapacity} p+  poke p (DynamicTree f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11 f12) = do+    #{poke b3DynamicTree, version} p f1+    #{poke b3DynamicTree, nodes} p f2+    #{poke b3DynamicTree, root} p f3+    #{poke b3DynamicTree, nodeCount} p f4+    #{poke b3DynamicTree, nodeCapacity} p f5+    #{poke b3DynamicTree, proxyCount} p f6+    #{poke b3DynamicTree, freeList} p f7+    #{poke b3DynamicTree, leafIndices} p f8+    #{poke b3DynamicTree, leafBoxes} p f9+    #{poke b3DynamicTree, leafCenters} p f10+    #{poke b3DynamicTree, binIndices} p f11+    #{poke b3DynamicTree, rebuildCapacity} p f12++-- | Tree node child indices. For internal usage.+data TreeNodeChildren = TreeNodeChildren+  { child1 :: CInt+  -- ^ child node index 1+  , child2 :: CInt+  -- ^ child node index 2+  }+  deriving (Eq, Show)++instance Storable TreeNodeChildren where+  sizeOf _ = #{size b3TreeNodeChildren}+  alignment _ = #{alignment b3TreeNodeChildren}+  peek p =+    TreeNodeChildren+      <$> #{peek b3TreeNodeChildren, child1} p+      <*> #{peek b3TreeNodeChildren, child2} p+  poke p (TreeNodeChildren f1 f2) = do+    #{poke b3TreeNodeChildren, child1} p f1+    #{poke b3TreeNodeChildren, child2} p f2++-- | Flags for tree nodes. For internal usage.+newtype TreeNodeFlags = TreeNodeFlags CInt+  deriving (Eq, Show, Storable)++pattern AllocatedNode :: TreeNodeFlags+pattern EnlargedNode :: TreeNodeFlags+pattern LeafNode :: TreeNodeFlags+pattern AllocatedNode = TreeNodeFlags #{const b3_allocatedNode}+pattern EnlargedNode = TreeNodeFlags #{const b3_enlargedNode}+pattern LeafNode = TreeNodeFlags #{const b3_leafNode}++dynamicTreeByteCount :: Int+dynamicTreeByteCount = #{size b3DynamicTree}+++foreign import ccall unsafe "hsg_b3DynamicTree_Create"+  c_b3DynamicTree_Create :: CInt -> Ptr DynamicTree -> IO ()++foreign import ccall unsafe "b3DynamicTree_Destroy"+  c_b3DynamicTree_Destroy :: Ptr DynamicTree -> IO ()++foreign import ccall unsafe "hsg_b3DynamicTree_CreateProxy"+  c_b3DynamicTree_CreateProxy :: Ptr DynamicTree -> Ptr AABB -> Word64 -> Word64 -> IO CInt++foreign import ccall unsafe "b3DynamicTree_DestroyProxy"+  c_b3DynamicTree_DestroyProxy :: Ptr DynamicTree -> CInt -> IO ()++foreign import ccall unsafe "hsg_b3DynamicTree_MoveProxy"+  c_b3DynamicTree_MoveProxy :: Ptr DynamicTree -> CInt -> Ptr AABB -> IO ()++foreign import ccall unsafe "hsg_b3DynamicTree_EnlargeProxy"+  c_b3DynamicTree_EnlargeProxy :: Ptr DynamicTree -> CInt -> Ptr AABB -> IO ()++foreign import ccall unsafe "b3DynamicTree_SetCategoryBits"+  c_b3DynamicTree_SetCategoryBits :: Ptr DynamicTree -> CInt -> Word64 -> IO ()++foreign import ccall unsafe "b3DynamicTree_GetCategoryBits"+  c_b3DynamicTree_GetCategoryBits :: Ptr DynamicTree -> CInt -> IO Word64++foreign import ccall safe "hsg_b3DynamicTree_Query"+  c_b3DynamicTree_Query :: Ptr DynamicTree -> Ptr AABB -> Word64 -> CBool -> FunPtr TreeQueryCallbackFcn -> Ptr () -> Ptr TreeStats -> IO ()++foreign import ccall safe "hsg_b3DynamicTree_QueryClosest"+  c_b3DynamicTree_QueryClosest :: Ptr DynamicTree -> Ptr Vec3 -> Word64 -> CBool -> FunPtr TreeQueryClosestCallbackFcn -> Ptr () -> Ptr Float -> Ptr TreeStats -> IO ()++foreign import ccall safe "hsg_b3DynamicTree_RayCast"+  c_b3DynamicTree_RayCast :: Ptr DynamicTree -> Ptr RayCastInput -> Word64 -> CBool -> FunPtr TreeRayCastCallbackFcn -> Ptr () -> Ptr TreeStats -> IO ()++foreign import ccall safe "hsg_b3DynamicTree_BoxCast"+  c_b3DynamicTree_BoxCast :: Ptr DynamicTree -> Ptr BoxCastInput -> Word64 -> CBool -> FunPtr TreeBoxCastCallbackFcn -> Ptr () -> Ptr TreeStats -> IO ()++foreign import ccall unsafe "b3DynamicTree_GetHeight"+  c_b3DynamicTree_GetHeight :: Ptr DynamicTree -> IO CInt++foreign import ccall unsafe "b3DynamicTree_GetAreaRatio"+  c_b3DynamicTree_GetAreaRatio :: Ptr DynamicTree -> IO Float++foreign import ccall unsafe "hsg_b3DynamicTree_GetRootBounds"+  c_b3DynamicTree_GetRootBounds :: Ptr DynamicTree -> Ptr AABB -> IO ()++foreign import ccall unsafe "b3DynamicTree_GetProxyCount"+  c_b3DynamicTree_GetProxyCount :: Ptr DynamicTree -> IO CInt++foreign import ccall unsafe "b3DynamicTree_Rebuild"+  c_b3DynamicTree_Rebuild :: Ptr DynamicTree -> CBool -> IO CInt++foreign import ccall unsafe "b3DynamicTree_GetByteCount"+  c_b3DynamicTree_GetByteCount :: Ptr DynamicTree -> IO CInt++foreign import ccall unsafe "b3DynamicTree_Validate"+  c_b3DynamicTree_Validate :: Ptr DynamicTree -> IO ()++foreign import ccall unsafe "b3DynamicTree_ValidateNoEnlarged"+  c_b3DynamicTree_ValidateNoEnlarged :: Ptr DynamicTree -> IO ()++foreign import ccall unsafe "b3DynamicTree_Save"+  c_b3DynamicTree_Save :: Ptr DynamicTree -> CString -> IO ()++foreign import ccall unsafe "hsg_b3DynamicTree_Load"+  c_b3DynamicTree_Load :: CString -> Float -> Ptr DynamicTree -> IO ()++{- | Constructing the tree initializes the node pool.++Binds @b3DynamicTree_Create@.+-}+create+  :: Int+  -- ^ @proxyCapacity@+  -> Ptr DynamicTree+  -- ^ Result buffer.+  -> IO ()+create a0 out =+  c_b3DynamicTree_Create (fromIntegral a0) out++{- | Destroy the tree, freeing the node pool.++Binds @b3DynamicTree_Destroy@.+-}+destroy+  :: Ptr DynamicTree+  -> IO ()+destroy a0 =+  c_b3DynamicTree_Destroy a0++{- | Create a proxy. Provide an AABB and a userData value.++Binds @b3DynamicTree_CreateProxy@.+-}+createProxy+  :: Ptr DynamicTree+  -> AABB+  -> Word64+  -- ^ @categoryBits@+  -> Word64+  -- ^ @userData@+  -> IO Int+createProxy a0 a1 a2 a3 =+  with a1 $ \p1 ->+  fromIntegral <$> (c_b3DynamicTree_CreateProxy a0 p1 a2 a3)++{- | Destroy a proxy. This asserts if the id is invalid.++Binds @b3DynamicTree_DestroyProxy@.+-}+destroyProxy+  :: Ptr DynamicTree+  -> Int+  -- ^ @proxyId@+  -> IO ()+destroyProxy a0 a1 =+  c_b3DynamicTree_DestroyProxy a0 (fromIntegral a1)++{- | Move a proxy to a new AABB by removing and reinserting into the tree.++Binds @b3DynamicTree_MoveProxy@.+-}+moveProxy+  :: Ptr DynamicTree+  -> Int+  -- ^ @proxyId@+  -> AABB+  -> IO ()+moveProxy a0 a1 a2 =+  with a2 $ \p2 ->+  c_b3DynamicTree_MoveProxy a0 (fromIntegral a1) p2++{- | Enlarge a proxy and enlarge ancestors as necessary.++Binds @b3DynamicTree_EnlargeProxy@.+-}+enlargeProxy+  :: Ptr DynamicTree+  -> Int+  -- ^ @proxyId@+  -> AABB+  -> IO ()+enlargeProxy a0 a1 a2 =+  with a2 $ \p2 ->+  c_b3DynamicTree_EnlargeProxy a0 (fromIntegral a1) p2++{- | Modify the category bits on a proxy. This is an expensive operation.++Binds @b3DynamicTree_SetCategoryBits@.+-}+setCategoryBits+  :: Ptr DynamicTree+  -> Int+  -- ^ @proxyId@+  -> Word64+  -- ^ @categoryBits@+  -> IO ()+setCategoryBits a0 a1 a2 =+  c_b3DynamicTree_SetCategoryBits a0 (fromIntegral a1) a2++{- | Get the category bits on a proxy.++Binds @b3DynamicTree_GetCategoryBits@.+-}+getCategoryBits+  :: Ptr DynamicTree+  -> Int+  -- ^ @proxyId@+  -> IO Word64+getCategoryBits a0 a1 =+  c_b3DynamicTree_GetCategoryBits a0 (fromIntegral a1)++{- | Query an AABB for overlapping proxies. The callback function is called for each proxy that overlaps the supplied AABB.++Returns: performance data++Binds @b3DynamicTree_Query@.+-}+query+  :: Ptr DynamicTree+  -> AABB+  -> Word64+  -- ^ @maskBits@+  -> Bool+  -- ^ @requireAllBits@+  -> FunPtr TreeQueryCallbackFcn+  -> Ptr ()+  -- ^ @context@+  -> IO TreeStats+query a0 a1 a2 a3 a4 a5 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b3DynamicTree_Query a0 p1 a2 (fromBool a3) a4 a5 pOut >> peek pOut++{- | Query an AABB for the closest object. The callback function is called for each proxy that might be closest to the supplied+point.++Returns: performance data++Binds @b3DynamicTree_QueryClosest@.+-}+queryClosest+  :: Ptr DynamicTree+  -- ^ @tree@: the dynamic tree to query+  -> Vec3+  -- ^ @point@: the query point+  -> Word64+  -- ^ @maskBits@: nodes are skipped if the bit-wise AND with the node category bits is zero+  -> Bool+  -- ^ @requireAllBits@: nodes are skipped if the bit-wise AND with the node category bits does not equal the maskBits+  -> FunPtr TreeQueryClosestCallbackFcn+  -- ^ @callback@: a user provided instance of @b3TreeQueryClosestCallbackFcn@+  -> Ptr ()+  -- ^ @context@: a user context object that is provided to the callback+  -> Ptr Float+  -- ^ @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.+  -> IO TreeStats+queryClosest a0 a1 a2 a3 a4 a5 a6 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b3DynamicTree_QueryClosest a0 p1 a2 (fromBool a3) a4 a5 a6 pOut >> peek pOut++{- | Ray cast against the proxies in the tree. This relies on the callback+to perform an exact ray cast in the case where the proxy contains a shape.+The callback also performs 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.++Returns: performance data++Binds @b3DynamicTree_RayCast@.+-}+rayCast+  :: Ptr DynamicTree+  -- ^ @tree@: the dynamic tree to ray cast+  -> RayCastInput+  -- ^ @input@: the ray cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1)+  -> Word64+  -- ^ @maskBits@: bit mask test: \`bool accept = (maskBits & node-\>categoryBits) != 0;\`+  -> Bool+  -- ^ @requireAllBits@: modifies bit mask test: \`bool accept = (maskBits & node-\>categoryBits) == maskBits;\`+  -> FunPtr TreeRayCastCallbackFcn+  -- ^ @callback@: a callback function that is called for each proxy that is hit by the ray+  -> Ptr ()+  -- ^ @context@: user context that is passed to the callback+  -> IO TreeStats+rayCast a0 a1 a2 a3 a4 a5 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b3DynamicTree_RayCast a0 p1 a2 (fromBool a3) a4 a5 pOut >> peek pOut++{- | Sweep an AABB through the tree. The box is in the tree\'s world float frame and the callback+re-differences each shape at full precision against the query origin. Used by the large world+spatial queries so the tree traversal stays float while the narrow phase stays precise.++Binds @b3DynamicTree_BoxCast@.+-}+boxCast+  :: Ptr DynamicTree+  -> Ptr BoxCastInput+  -> Word64+  -- ^ @maskBits@+  -> Bool+  -- ^ @requireAllBits@+  -> FunPtr TreeBoxCastCallbackFcn+  -> Ptr ()+  -- ^ @context@+  -> IO TreeStats+boxCast a0 a1 a2 a3 a4 a5 =+  alloca $ \pOut -> c_b3DynamicTree_BoxCast a0 a1 a2 (fromBool a3) a4 a5 pOut >> peek pOut++-- | Like 'boxCast', but the result is written into a caller-supplied buffer.+boxCastInto+  :: Ptr DynamicTree+  -> Ptr BoxCastInput+  -> Word64+  -- ^ @maskBits@+  -> Bool+  -- ^ @requireAllBits@+  -> FunPtr TreeBoxCastCallbackFcn+  -> Ptr ()+  -- ^ @context@+  -> Ptr TreeStats+  -- ^ Result buffer.+  -> IO ()+boxCastInto a0 a1 a2 a3 a4 a5 out =+  c_b3DynamicTree_BoxCast a0 a1 a2 (fromBool a3) a4 a5 out++{- | Get the height of the binary tree.++Binds @b3DynamicTree_GetHeight@.+-}+getHeight+  :: Ptr DynamicTree+  -> IO Int+getHeight a0 =+  fromIntegral <$> (c_b3DynamicTree_GetHeight a0)++{- | Get the ratio of the sum of the node areas to the root area.++Binds @b3DynamicTree_GetAreaRatio@.+-}+getAreaRatio+  :: Ptr DynamicTree+  -> IO Float+getAreaRatio a0 =+  c_b3DynamicTree_GetAreaRatio a0++{- | Get the bounding box that contains the entire tree++Binds @b3DynamicTree_GetRootBounds@.+-}+getRootBounds+  :: Ptr DynamicTree+  -> IO AABB+getRootBounds a0 =+  alloca $ \pOut -> c_b3DynamicTree_GetRootBounds a0 pOut >> peek pOut++-- | Like 'getRootBounds', but the result is written into a caller-supplied buffer.+getRootBoundsInto+  :: Ptr DynamicTree+  -> Ptr AABB+  -- ^ Result buffer.+  -> IO ()+getRootBoundsInto a0 out =+  c_b3DynamicTree_GetRootBounds a0 out++{- | Get the number of proxies created++Binds @b3DynamicTree_GetProxyCount@.+-}+getProxyCount+  :: Ptr DynamicTree+  -> IO Int+getProxyCount a0 =+  fromIntegral <$> (c_b3DynamicTree_GetProxyCount a0)++{- | Rebuild the tree while retaining subtrees that haven\'t changed. Returns the number of boxes sorted.++Binds @b3DynamicTree_Rebuild@.+-}+rebuild+  :: Ptr DynamicTree+  -> Bool+  -- ^ @fullBuild@+  -> IO Int+rebuild a0 a1 =+  fromIntegral <$> (c_b3DynamicTree_Rebuild a0 (fromBool a1))++{- | Get the number of bytes used by this tree++Binds @b3DynamicTree_GetByteCount@.+-}+getByteCount+  :: Ptr DynamicTree+  -> IO Int+getByteCount a0 =+  fromIntegral <$> (c_b3DynamicTree_GetByteCount a0)++{- | Validate this tree. For testing.++Binds @b3DynamicTree_Validate@.+-}+validate+  :: Ptr DynamicTree+  -> IO ()+validate a0 =+  c_b3DynamicTree_Validate a0++{- | Validate this tree has no enlarged AABBs. For testing.++Binds @b3DynamicTree_ValidateNoEnlarged@.+-}+validateNoEnlarged+  :: Ptr DynamicTree+  -> IO ()+validateNoEnlarged a0 =+  c_b3DynamicTree_ValidateNoEnlarged a0++{- | Save this tree to a file for debugging++Binds @b3DynamicTree_Save@.+-}+save+  :: Ptr DynamicTree+  -> CString+  -- ^ @fileName@+  -> IO ()+save a0 a1 =+  c_b3DynamicTree_Save a0 a1++{- | Load a file for debugging++Binds @b3DynamicTree_Load@.+-}+load+  :: CString+  -- ^ @fileName@+  -> Float+  -- ^ @scale@+  -> Ptr DynamicTree+  -- ^ Result buffer.+  -> IO ()+load a0 a1 out =+  c_b3DynamicTree_Load a0 a1 out
+ src/Box3D/Events.hs view
@@ -0,0 +1,88 @@+{-| Reading the per-step event buffers as ordinary Haskell values.++Box3D exposes events as C-owned arrays that live only between one+'Box3D.World.step' and the next. The generated @World.get*Events@ primitives+hand back the raw @{ pointer, count }@ buffer struct; this module copies the+elements out (one @memcpy@ each) into Haskell-owned storable vectors so+they outlive the step. For zero-copy step-scoped views, wrap the raw+buffers with 'Box3D.Internal.unsafeViewVector'. Call it after stepping.+-}+module Box3D.Events+  ( bodyMoveEvents+  , sensorBeginTouchEvents+  , sensorEndTouchEvents+  , contactBeginTouchEvents+  , contactEndTouchEvents+  , contactHitEvents+  , jointEvents+  ) where++import Data.Vector.Storable (Vector)++import Box3D.Id (WorldId)+import Box3D.Internal (copyVector)+import Box3D.World+  ( BodyEvents (..)+  , BodyMoveEvent+  , ContactBeginTouchEvent+  , ContactEndTouchEvent+  , ContactEvents (..)+  , ContactHitEvent+  , JointEvent+  , JointEvents (..)+  , SensorBeginTouchEvent+  , SensorEndTouchEvent+  , SensorEvents (..)+  )+import Box3D.World qualified as World++{- | The bodies that moved during the last 'Box3D.World.step', copied out of the+world-owned buffer.+-}+bodyMoveEvents :: WorldId -> IO (Vector BodyMoveEvent)+bodyMoveEvents w = do+  BodyEvents ptr n <- World.getBodyEvents w+  copyVector (fromIntegral n) ptr++-- | The shapes that started overlapping a sensor during the last step.+sensorBeginTouchEvents :: WorldId -> IO (Vector SensorBeginTouchEvent)+sensorBeginTouchEvents w = do+  SensorEvents beginPtr _ beginN _ <- World.getSensorEvents w+  copyVector (fromIntegral beginN) beginPtr++-- | The shapes that stopped overlapping a sensor during the last step.+sensorEndTouchEvents :: WorldId -> IO (Vector SensorEndTouchEvent)+sensorEndTouchEvents w = do+  SensorEvents _ endPtr _ endN <- World.getSensorEvents w+  copyVector (fromIntegral endN) endPtr++{- | The shape pairs that began touching during the last step. Requires+@shapeDefEnableContactEvents@ on both shapes.+-}+contactBeginTouchEvents :: WorldId -> IO (Vector ContactBeginTouchEvent)+contactBeginTouchEvents w = do+  ContactEvents beginPtr _ _ beginN _ _ <- World.getContactEvents w+  copyVector (fromIntegral beginN) beginPtr++-- | The shape pairs that stopped touching during the last step.+contactEndTouchEvents :: WorldId -> IO (Vector ContactEndTouchEvent)+contactEndTouchEvents w = do+  ContactEvents _ endPtr _ _ endN _ <- World.getContactEvents w+  copyVector (fromIntegral endN) endPtr++{- | The impacts faster than @WorldDef.hitEventThreshold@ during the last step.+Requires @shapeDefEnableHitEvents@.+-}+contactHitEvents :: WorldId -> IO (Vector ContactHitEvent)+contactHitEvents w = do+  ContactEvents _ _ hitPtr _ _ hitN <- World.getContactEvents w+  copyVector (fromIntegral hitN) hitPtr++{- | The joints that exceeded their force/torque event threshold during the+last step. Requires @Box3D.Joint.setForceThreshold@ \/ torque threshold and+per-joint event enabling.+-}+jointEvents :: WorldId -> IO (Vector JointEvent)+jointEvents w = do+  JointEvents ptr n <- World.getJointEvents w+  copyVector (fromIntegral n) ptr
+ src/Box3D/Extra.hsc view
@@ -0,0 +1,28 @@+{-# LANGUAGE ForeignFunctionInterface #-}++-- | Hand-written type helpers that complement the generated modules.+--+-- @b3ShapeProxy@ points into caller memory, so it stays an opaque tag and its+-- buffer is supplied for the duration of a bracket rather than owned.+module Box3D.Extra+  ( withShapeProxy+  ) where++import Data.Vector.Storable qualified as VS+import Foreign+import Foreign.C.Types (CInt)++import Box3D.Query (ShapeProxy (..))+import Box3D.MathTypes (Vec3)++#include "box3d/box3d.h"++-- | Marshal a point cloud + radius as a 'ShapeProxy' for the duration of an action.+withShapeProxy :: VS.Vector Vec3 -> Float -> (Ptr ShapeProxy -> IO a) -> IO a+withShapeProxy points radius action =+  VS.unsafeWith points $ \pts ->+    allocaBytes #{size b3ShapeProxy} $ \proxy -> do+      #{poke b3ShapeProxy, points} proxy pts+      #{poke b3ShapeProxy, count} proxy (fromIntegral (VS.length points) :: CInt)+      #{poke b3ShapeProxy, radius} proxy radius+      action proxy
+ src/Box3D/FilterJoint.hsc view
@@ -0,0 +1,60 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.FilterJoint+  ( FilterJointDef (..)+  , defaultFilterJointDef+  , create+  )+  where++import Foreign+import Box3D.Internal (allocaOut)+import Box3D.Id (JointId (..), WorldId (..))+import Box3D.Joint (JointDef)++#include "box3d/box3d.h"+++-- | A filter joint is used to disable collision between two specific bodies.+data FilterJointDef = FilterJointDef+  { base :: JointDef+  -- ^ Base joint definition+  }+  deriving (Eq, Show)++instance Storable FilterJointDef where+  sizeOf _ = #{size b3FilterJointDef}+  alignment _ = #{alignment b3FilterJointDef}+  peek p =+    FilterJointDef+      <$> #{peek b3FilterJointDef, base} p+  poke p (FilterJointDef f1) = do+    #{poke b3FilterJointDef, base} p f1++foreign import ccall unsafe "hs_b3DefaultFilterJointDef"+  c_b3DefaultFilterJointDef :: Ptr FilterJointDef -> IO ()++defaultFilterJointDef :: IO FilterJointDef+defaultFilterJointDef = allocaOut c_b3DefaultFilterJointDef+++foreign import ccall unsafe "b3CreateFilterJoint"+  c_b3CreateFilterJoint :: WorldId -> Ptr FilterJointDef -> IO JointId++{- | Create a filter joint.++See also @b3FilterJointDef@ for details.++Binds @b3CreateFilterJoint@.+-}+create+  :: WorldId+  -> FilterJointDef+  -> IO JointId+create a0 a1 =+  with a1 $ \p1 ->+  c_b3CreateFilterJoint a0 p1
+ src/Box3D/Geometry.hsc view
@@ -0,0 +1,91 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.Geometry+  ( Capsule (..)+  , Sphere (..)+  , MassData (..)+  )+  where++import Foreign+import Box3D.MathTypes (Matrix3, Vec3)++#include "box3d/box3d.h"+++{- | A solid capsule can be viewed as two hemispheres connected+by a rectangle.+-}+data Capsule = Capsule+  { center1 :: Vec3+  -- ^ Local center of the first hemisphere+  , center2 :: Vec3+  -- ^ Local center of the second hemisphere+  , radius :: Float+  -- ^ The radius of the hemispheres+  }+  deriving (Eq, Show)++instance Storable Capsule where+  sizeOf _ = #{size b3Capsule}+  alignment _ = #{alignment b3Capsule}+  peek p =+    Capsule+      <$> #{peek b3Capsule, center1} p+      <*> #{peek b3Capsule, center2} p+      <*> #{peek b3Capsule, radius} p+  poke p (Capsule f1 f2 f3) = do+    #{poke b3Capsule, center1} p f1+    #{poke b3Capsule, center2} p f2+    #{poke b3Capsule, radius} p f3++-- | A solid sphere+data Sphere = Sphere+  { center :: Vec3+  -- ^ The local center+  , radius :: Float+  -- ^ The radius+  }+  deriving (Eq, Show)++instance Storable Sphere where+  sizeOf _ = #{size b3Sphere}+  alignment _ = #{alignment b3Sphere}+  peek p =+    Sphere+      <$> #{peek b3Sphere, center} p+      <*> #{peek b3Sphere, radius} p+  poke p (Sphere f1 f2) = do+    #{poke b3Sphere, center} p f1+    #{poke b3Sphere, radius} p f2++-- | This holds the mass data computed for a shape.+data MassData = MassData+  { mass :: Float+  -- ^ The shape mass+  , center :: Vec3+  -- ^ The local center of mass position.+  , inertia :: Matrix3+  -- ^ The inertia tensor about the shape center of mass.+  }+  deriving (Eq, Show)++instance Storable MassData where+  sizeOf _ = #{size b3MassData}+  alignment _ = #{alignment b3MassData}+  peek p =+    MassData+      <$> #{peek b3MassData, mass} p+      <*> #{peek b3MassData, center} p+      <*> #{peek b3MassData, inertia} p+  poke p (MassData f1 f2 f3) = do+    #{poke b3MassData, mass} p f1+    #{poke b3MassData, center} p f2+    #{poke b3MassData, inertia} p f3+++
+ src/Box3D/Grid.hs view
@@ -0,0 +1,32 @@+-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.Grid+  ( create+  )+where++import Box3D.HeightField (HeightFieldData)+import Box3D.MathTypes (Vec3)+import Foreign+import Foreign.C.Types (CBool (..), CInt (..))++foreign import ccall unsafe "hsg_b3CreateGrid"+  c_b3CreateGrid :: CInt -> CInt -> Ptr Vec3 -> CBool -> IO (Ptr HeightFieldData)++{- | Create a grid as a height field.++Binds @b3CreateGrid@.+-}+create+  :: Int+  -- ^ @rowCount@+  -> Int+  -- ^ @columnCount@+  -> Vec3+  -- ^ @scale@+  -> Bool+  -- ^ @makeHoles@+  -> IO (Ptr HeightFieldData)+create a0 a1 a2 a3 =+  with a2 $ \p2 ->+    c_b3CreateGrid (fromIntegral a0) (fromIntegral a1) p2 (fromBool a3)
+ src/Box3D/GridMesh.hs view
@@ -0,0 +1,32 @@+-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.GridMesh+  ( create+  )+where++import Box3D.Mesh (MeshData)+import Foreign+import Foreign.C.Types (CBool (..), CInt (..))++foreign import ccall unsafe "b3CreateGridMesh"+  c_b3CreateGridMesh :: CInt -> CInt -> Float -> CInt -> CBool -> IO (Ptr MeshData)++{- | Create a grid mesh along the x and z axes.++Binds @b3CreateGridMesh@.+-}+create+  :: Int+  -- ^ @xCount@: the number of rows in the x direction+  -> Int+  -- ^ @zCount@: the number of rows in the z direction+  -> Float+  -- ^ @cellWidth@: the width of each cell+  -> Int+  -- ^ @materialCount@: the number of materials to generate+  -> Bool+  -- ^ @identifyEdges@: compute adjacency information+  -> IO (Ptr MeshData)+create a0 a1 a2 a3 a4 =+  c_b3CreateGridMesh (fromIntegral a0) (fromIntegral a1) a2 (fromIntegral a3) (fromBool a4)
+ src/Box3D/HeightField.hsc view
@@ -0,0 +1,179 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.HeightField+  ( HeightFieldData (..)+  , HeightFieldDef (..)+  , create+  , destroy+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..))+import Data.Vector.Storable qualified as VS+import Box3D.Internal (copyVector, pokeBounded)+import Box3D.MathTypes (AABB, Vec3)++#include "box3d/box3d.h"+++{- | A height field with compressed storage.++__Note:__ This data structure has data hanging off the end and cannot be directly copied.+-}+data HeightFieldData = HeightFieldData+  { version :: Word64+  -- ^ Version must be first and match @B3_HEIGHT_FIELD_VERSION@+  , hash :: Word64+  -- ^ Hash of this height field (this field is zero when the hash is computed).+  , byteCount :: CInt+  -- ^ The total number of bytes for this height field.+  , aabb :: AABB+  -- ^ The local axis-aligned bounding box.+  , minHeight :: Float+  -- ^ The minimum y value.+  , maxHeight :: Float+  -- ^ The maximum y value+  , heightScale :: Float+  -- ^ The quantization scale.+  , scale :: Vec3+  -- ^ The overall scale.+  , columnCount :: Int32+  -- ^ The number of grid columns along the local x-axis.+  , rowCount :: Int32+  -- ^ The number of grid rows along the local z-axis.+  , heightsOffset :: Int32+  -- ^ Offset of the compressed height array in bytes from the struct address.+  -- uint16_t, one per grid point.+  , materialOffset :: Int32+  -- ^ Offset of the material index array in bytes from the struct address.+  -- uint8_t, one per cell.+  , flagsOffset :: Int32+  -- ^ Offset of the flag array in bytes from the struct address.+  -- uint8_t, one per triangle.+  , clockwise :: Word8+  -- ^ Triangle winding.+  , padding :: VS.Vector Word8+  -- ^ Explicit padding.+  }+  deriving (Eq, Show)++instance Storable HeightFieldData where+  sizeOf _ = #{size b3HeightFieldData}+  alignment _ = #{alignment b3HeightFieldData}+  peek p =+    HeightFieldData+      <$> #{peek b3HeightFieldData, version} p+      <*> #{peek b3HeightFieldData, hash} p+      <*> #{peek b3HeightFieldData, byteCount} p+      <*> #{peek b3HeightFieldData, aabb} p+      <*> #{peek b3HeightFieldData, minHeight} p+      <*> #{peek b3HeightFieldData, maxHeight} p+      <*> #{peek b3HeightFieldData, heightScale} p+      <*> #{peek b3HeightFieldData, scale} p+      <*> #{peek b3HeightFieldData, columnCount} p+      <*> #{peek b3HeightFieldData, rowCount} p+      <*> #{peek b3HeightFieldData, heightsOffset} p+      <*> #{peek b3HeightFieldData, materialOffset} p+      <*> #{peek b3HeightFieldData, flagsOffset} p+      <*> #{peek b3HeightFieldData, clockwise} p+      <*> copyVector (#{const 7}) (#{ptr b3HeightFieldData, padding} p)+  poke p x = do+    #{poke b3HeightFieldData, version} p x.version+    #{poke b3HeightFieldData, hash} p x.hash+    #{poke b3HeightFieldData, byteCount} p x.byteCount+    #{poke b3HeightFieldData, aabb} p x.aabb+    #{poke b3HeightFieldData, minHeight} p x.minHeight+    #{poke b3HeightFieldData, maxHeight} p x.maxHeight+    #{poke b3HeightFieldData, heightScale} p x.heightScale+    #{poke b3HeightFieldData, scale} p x.scale+    #{poke b3HeightFieldData, columnCount} p x.columnCount+    #{poke b3HeightFieldData, rowCount} p x.rowCount+    #{poke b3HeightFieldData, heightsOffset} p x.heightsOffset+    #{poke b3HeightFieldData, materialOffset} p x.materialOffset+    #{poke b3HeightFieldData, flagsOffset} p x.flagsOffset+    #{poke b3HeightFieldData, clockwise} p x.clockwise+    pokeBounded (#{const 7}) (#{ptr b3HeightFieldData, padding} p) x.padding++-- | Data used to create a height field. No pointers are held to this data.+data HeightFieldDef = HeightFieldDef+  { heights :: Ptr ()+  -- ^ Grid point heights+  -- count = countX * countZ+  , materialIndices :: Ptr ()+  -- ^ Grid cell material+  -- A value of 0xFF is reserved for holes+  -- count = (countX - 1) * (countZ - 1)+  , scale :: Vec3+  -- ^ The height field scale. All components must be positive values.+  , countX :: CInt+  -- ^ The number of grid lines along the x-axis.+  , countZ :: CInt+  -- ^ The number of grid lines along the z-axis.+  , globalMinimumHeight :: Float+  -- ^ 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.+  , globalMaximumHeight :: Float+  -- ^ The maximum.+  , clockwiseWinding :: CBool+  -- ^ Use clock-wise winding. This effectively inverts the height-field along the y-axis.+  }+  deriving (Eq, Show)++instance Storable HeightFieldDef where+  sizeOf _ = #{size b3HeightFieldDef}+  alignment _ = #{alignment b3HeightFieldDef}+  peek p =+    HeightFieldDef+      <$> #{peek b3HeightFieldDef, heights} p+      <*> #{peek b3HeightFieldDef, materialIndices} p+      <*> #{peek b3HeightFieldDef, scale} p+      <*> #{peek b3HeightFieldDef, countX} p+      <*> #{peek b3HeightFieldDef, countZ} p+      <*> #{peek b3HeightFieldDef, globalMinimumHeight} p+      <*> #{peek b3HeightFieldDef, globalMaximumHeight} p+      <*> #{peek b3HeightFieldDef, clockwiseWinding} p+  poke p (HeightFieldDef f1 f2 f3 f4 f5 f6 f7 f8) = do+    #{poke b3HeightFieldDef, heights} p f1+    #{poke b3HeightFieldDef, materialIndices} p f2+    #{poke b3HeightFieldDef, scale} p f3+    #{poke b3HeightFieldDef, countX} p f4+    #{poke b3HeightFieldDef, countZ} p f5+    #{poke b3HeightFieldDef, globalMinimumHeight} p f6+    #{poke b3HeightFieldDef, globalMaximumHeight} p f7+    #{poke b3HeightFieldDef, clockwiseWinding} p f8+++foreign import ccall unsafe "b3CreateHeightField"+  c_b3CreateHeightField :: Ptr HeightFieldDef -> IO (Ptr HeightFieldData)++foreign import ccall unsafe "b3DestroyHeightField"+  c_b3DestroyHeightField :: Ptr HeightFieldData -> IO ()++{- | Create a generic height field.++Binds @b3CreateHeightField@.+-}+create+  :: Ptr HeightFieldDef+  -- ^ @data@+  -> IO (Ptr HeightFieldData)+create a0 =+  c_b3CreateHeightField a0++{- | Destroy a height field.++Binds @b3DestroyHeightField@.+-}+destroy+  :: Ptr HeightFieldData+  -> IO ()+destroy a0 =+  c_b3DestroyHeightField a0
+ src/Box3D/HollowBoxMesh.hs view
@@ -0,0 +1,28 @@+-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.HollowBoxMesh+  ( create+  )+where++import Box3D.MathTypes (Vec3)+import Box3D.Mesh (MeshData)+import Foreign++foreign import ccall unsafe "hsg_b3CreateHollowBoxMesh"+  c_b3CreateHollowBoxMesh :: Ptr Vec3 -> Ptr Vec3 -> IO (Ptr MeshData)++{- | Create a hollow box mesh.++Binds @b3CreateHollowBoxMesh@.+-}+create+  :: Vec3+  -- ^ @center@+  -> Vec3+  -- ^ @extent@+  -> IO (Ptr MeshData)+create a0 a1 =+  with a0 $ \p0 ->+    with a1 $ \p1 ->+      c_b3CreateHollowBoxMesh p0 p1
+ src/Box3D/Hull.hsc view
@@ -0,0 +1,209 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.Hull+  ( HullData (..)+  , HullFace (..)+  , HullHalfEdge (..)+  , HullVertex (..)+  , create+  , destroy+  )+  where++import Foreign+import Foreign.C.Types (CInt (..))+import Box3D.MathTypes (AABB, Matrix3, Vec3)++#include "box3d/box3d.h"+++{- | A convex hull.++__Note:__ This data structure has data hanging off the end and cannot be directly copied.+-}+data HullData = HullData+  { version :: Word64+  -- ^ Version must be first and match @B3_HULL_VERSION@+  , hash :: Word64+  -- ^ Hash of this hull (this field is zero when the hash is computed).+  , aabb :: AABB+  -- ^ Axis-aligned box in local space.+  , surfaceArea :: Float+  -- ^ Surface area, typically in squared meters.+  , volume :: Float+  -- ^ Volume, typically in m^3.+  , innerRadius :: Float+  -- ^ The radius of the largest sphere at the center.+  , center :: Vec3+  -- ^ The local centroid+  , centralInertia :: Matrix3+  -- ^ The inertia tensor about the centroid.+  , vertexCount :: Int32+  -- ^ The vertex count.+  , vertexOffset :: Int32+  -- ^ Offset of the vertex array in bytes from the struct address.+  , pointOffset :: Int32+  -- ^ Offset of the point array in bytes from the struct address.+  , edgeCount :: Int32+  -- ^ This is the half-edge count (double the edge count)+  , edgeOffset :: Int32+  -- ^ Offset of the edge array in bytes from the struct address.+  , faceCount :: Int32+  -- ^ The face count. Hulls faces are convex polygons.+  , planeOffset :: Int32+  -- ^ Offset of the face plane array in bytes from the struct address.+  , faceOffset :: Int32+  -- ^ Offset of the face array in bytes from the struct address.+  , soaVertexOffset :: Int32+  -- ^ Offset of structure of array (SOA) vertices+  , soaNormalOffset :: Int32+  -- ^ Offset of structure of array (SOA) unit normal vectors+  , byteCount :: Int32+  -- ^ The total number of bytes for this hull.+  }+  deriving (Eq, Show)++instance Storable HullData where+  sizeOf _ = #{size b3HullData}+  alignment _ = #{alignment b3HullData}+  peek p =+    HullData+      <$> #{peek b3HullData, version} p+      <*> #{peek b3HullData, hash} p+      <*> #{peek b3HullData, aabb} p+      <*> #{peek b3HullData, surfaceArea} p+      <*> #{peek b3HullData, volume} p+      <*> #{peek b3HullData, innerRadius} p+      <*> #{peek b3HullData, center} p+      <*> #{peek b3HullData, centralInertia} p+      <*> #{peek b3HullData, vertexCount} p+      <*> #{peek b3HullData, vertexOffset} p+      <*> #{peek b3HullData, pointOffset} p+      <*> #{peek b3HullData, edgeCount} p+      <*> #{peek b3HullData, edgeOffset} p+      <*> #{peek b3HullData, faceCount} p+      <*> #{peek b3HullData, planeOffset} p+      <*> #{peek b3HullData, faceOffset} p+      <*> #{peek b3HullData, soaVertexOffset} p+      <*> #{peek b3HullData, soaNormalOffset} p+      <*> #{peek b3HullData, byteCount} p+  poke p (HullData f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11 f12 f13 f14 f15 f16 f17 f18 f19) = do+    #{poke b3HullData, version} p f1+    #{poke b3HullData, hash} p f2+    #{poke b3HullData, aabb} p f3+    #{poke b3HullData, surfaceArea} p f4+    #{poke b3HullData, volume} p f5+    #{poke b3HullData, innerRadius} p f6+    #{poke b3HullData, center} p f7+    #{poke b3HullData, centralInertia} p f8+    #{poke b3HullData, vertexCount} p f9+    #{poke b3HullData, vertexOffset} p f10+    #{poke b3HullData, pointOffset} p f11+    #{poke b3HullData, edgeCount} p f12+    #{poke b3HullData, edgeOffset} p f13+    #{poke b3HullData, faceCount} p f14+    #{poke b3HullData, planeOffset} p f15+    #{poke b3HullData, faceOffset} p f16+    #{poke b3HullData, soaVertexOffset} p f17+    #{poke b3HullData, soaNormalOffset} p f18+    #{poke b3HullData, byteCount} p f19++{- | A hull face. Hulls use a half-edge data structure, so a face+can be determined from a single half-edge index.+-}+data HullFace = HullFace+  { edge :: Word8+  -- ^ An arbitrary half-edge on this face+  }+  deriving (Eq, Show)++instance Storable HullFace where+  sizeOf _ = #{size b3HullFace}+  alignment _ = #{alignment b3HullFace}+  peek p =+    HullFace+      <$> #{peek b3HullFace, edge} p+  poke p (HullFace f1) = do+    #{poke b3HullFace, edge} p f1++-- | Half-edge for hull data structure+data HullHalfEdge = HullHalfEdge+  { next :: Word8+  -- ^ Next edge index CCW+  , twin :: Word8+  -- ^ Twin edge index+  , origin :: Word8+  -- ^ index of origin vertex and point+  , face :: Word8+  -- ^ Face to the left of this edge+  }+  deriving (Eq, Show)++instance Storable HullHalfEdge where+  sizeOf _ = #{size b3HullHalfEdge}+  alignment _ = #{alignment b3HullHalfEdge}+  peek p =+    HullHalfEdge+      <$> #{peek b3HullHalfEdge, next} p+      <*> #{peek b3HullHalfEdge, twin} p+      <*> #{peek b3HullHalfEdge, origin} p+      <*> #{peek b3HullHalfEdge, face} p+  poke p (HullHalfEdge f1 f2 f3 f4) = do+    #{poke b3HullHalfEdge, next} p f1+    #{poke b3HullHalfEdge, twin} p f2+    #{poke b3HullHalfEdge, origin} p f3+    #{poke b3HullHalfEdge, face} p f4++-- | A hull vertex. Identified by a half-edge with this vertex as its tail.+data HullVertex = HullVertex+  { edge :: Word8+  -- ^ 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.+  }+  deriving (Eq, Show)++instance Storable HullVertex where+  sizeOf _ = #{size b3HullVertex}+  alignment _ = #{alignment b3HullVertex}+  peek p =+    HullVertex+      <$> #{peek b3HullVertex, edge} p+  poke p (HullVertex f1) = do+    #{poke b3HullVertex, edge} p f1+++foreign import ccall unsafe "b3CreateHull"+  c_b3CreateHull :: Ptr Vec3 -> CInt -> CInt -> IO (Ptr HullData)++foreign import ccall unsafe "b3DestroyHull"+  c_b3DestroyHull :: Ptr HullData -> IO ()++{- | Create a generic convex hull.++Binds @b3CreateHull@.+-}+create+  :: Ptr Vec3+  -- ^ @points@+  -> Int+  -- ^ @pointCount@+  -> Int+  -- ^ @maxVertexCount@+  -> IO (Ptr HullData)+create a0 a1 a2 =+  c_b3CreateHull a0 (fromIntegral a1) (fromIntegral a2)++{- | Destroy a hull.++Binds @b3DestroyHull@.+-}+destroy+  :: Ptr HullData+  -> IO ()+destroy a0 =+  c_b3DestroyHull a0
+ src/Box3D/Id.hsc view
@@ -0,0 +1,107 @@+{- | Low-level bindings to @box3d/id.h@: opaque handles to Box3D objects.++The C API passes these as integer-only structs of at most 8 bytes by value.+On the 64-bit ABIs this package supports (x86-64 SysV and Win64, AArch64)+such a struct travels in a general-purpose register exactly like a+@uint32_t@/@uint64_t@, so the handles are newtypes over words and cross the+FFI directly, with no shim. The payload is the raw struct bytes and carries+no stable meaning; the constructors are exported only so foreign imports can+see through the newtypes. A zero word is the null id, which is also what+zero-initialisation produces on the C side. Struct sizes are pinned by+@_Static_assert@s in @cbits/box3d_shim.c@.++'ContactId' is 12 bytes and does not fit a word, so it keeps a struct-shaped+representation; nothing passes it by value yet.+-}+module Box3D.Id+  ( WorldId (..)+  , BodyId (..)+  , ShapeId (..)+  , JointId (..)+  , ContactId (..)+  , nullWorldId+  , nullBodyId+  , nullShapeId+  , nullJointId+  , nullContactId+  ) where++import Foreign++#include "box3d/id.h"++-- | Handle to a world instance.+newtype WorldId = WorldId Word32+  deriving (Eq, Show)++-- | Handle to a body instance.+newtype BodyId = BodyId Word64+  deriving (Eq, Show)++-- | Handle to a shape instance.+newtype ShapeId = ShapeId Word64+  deriving (Eq, Show)++-- | Handle to a joint instance.+newtype JointId = JointId Word64+  deriving (Eq, Show)++-- | Handle to a contact instance.+data ContactId = ContactId Int32 Word16 Word32+  deriving (Eq, Show)++-- The word's alignment is at least the struct's, so alloca'd space is always+-- aligned enough for the aliased peek/poke.++instance Storable WorldId where+  sizeOf _ = #{size b3WorldId}+  alignment _ = alignment (0 :: Word32)+  peek p = WorldId <$> peek (castPtr p)+  poke p (WorldId w) = poke (castPtr p) w++instance Storable BodyId where+  sizeOf _ = #{size b3BodyId}+  alignment _ = alignment (0 :: Word64)+  peek p = BodyId <$> peek (castPtr p)+  poke p (BodyId w) = poke (castPtr p) w++instance Storable ShapeId where+  sizeOf _ = #{size b3ShapeId}+  alignment _ = alignment (0 :: Word64)+  peek p = ShapeId <$> peek (castPtr p)+  poke p (ShapeId w) = poke (castPtr p) w++instance Storable JointId where+  sizeOf _ = #{size b3JointId}+  alignment _ = alignment (0 :: Word64)+  peek p = JointId <$> peek (castPtr p)+  poke p (JointId w) = poke (castPtr p) w++instance Storable ContactId where+  sizeOf _ = #{size b3ContactId}+  alignment _ = #{alignment b3ContactId}+  peek p =+    ContactId+      <$> #{peek b3ContactId, index1} p+      <*> #{peek b3ContactId, world0} p+      <*> #{peek b3ContactId, generation} p+  poke p (ContactId i w g) = do+    #{poke b3ContactId, index1} p i+    #{poke b3ContactId, world0} p w+    #{poke b3ContactId, padding} p (0 :: Int16)+    #{poke b3ContactId, generation} p g++nullWorldId :: WorldId+nullWorldId = WorldId 0++nullBodyId :: BodyId+nullBodyId = BodyId 0++nullShapeId :: ShapeId+nullShapeId = ShapeId 0++nullJointId :: JointId+nullJointId = JointId 0++nullContactId :: ContactId+nullContactId = ContactId 0 0 0
+ src/Box3D/Internal.hs view
@@ -0,0 +1,63 @@+{-| Marshalling helpers shared by the binding modules.++By-value calls involving float structs or defs go through the shim as+pointers (ids pass directly as words), so the same two idioms recur: read+a struct back from an out parameter, and decode a C boolean returned for+a marshalled argument.+-}+module Box3D.Internal+  ( allocaOut+  , withValid+  , copyVector+  , copyBounded+  , pokeBounded+  , unsafeViewVector+  ) where++import Data.Vector.Storable qualified as VS+import Foreign+import Foreign.C.Types (CBool)++-- | Run a shim call that writes its result to an out parameter and read it.+{-# INLINE allocaOut #-}+allocaOut :: (Storable a) => (Ptr a -> IO ()) -> IO a+allocaOut f = alloca $ \p -> f p >> peek p++-- | Marshal a value in and decode the returned C boolean.+{-# INLINE withValid #-}+withValid :: (Storable a) => (Ptr a -> IO CBool) -> a -> IO Bool+withValid f a = with a (fmap (/= 0) . f)++{- | Copy a C-owned array into a Haskell-owned storable vector: one @memcpy@,+no per-element marshalling. Safe to keep after the source is invalidated.+-}+{-# INLINE copyVector #-}+copyVector :: (Storable a) => Int -> Ptr a -> IO (VS.Vector a)+copyVector n src = do+  fp <- mallocForeignPtrArray n+  withForeignPtr fp $ \dst -> copyArray dst src n+  pure (VS.unsafeFromForeignPtr0 fp n)++{-# INLINE copyBounded #-}+copyBounded :: (Integral n, Storable a) => Int -> n -> Ptr a -> IO (VS.Vector a)+copyBounded capacity count = copyVector (max 0 (min capacity (fromIntegral count)))++{-# INLINE pokeBounded #-}+pokeBounded :: forall a. (Storable a) => Int -> Ptr a -> VS.Vector a -> IO ()+pokeBounded capacity dst v = do+  let+    kept = VS.take capacity v+    copied = VS.length kept+  VS.unsafeWith kept $ \src -> copyArray dst src copied+  fillBytes (advancePtr dst copied) 0 ((capacity - copied) * sizeOf (undefined :: a))++{- | View a C-owned array as a storable vector without copying. The vector+aliases engine memory and carries no finalizer: it is only valid while the+source buffer is (for event buffers, until the next world step). Prefer+'copyVector' unless the copy shows up in a profile.+-}+{-# INLINE unsafeViewVector #-}+unsafeViewVector :: Int -> Ptr a -> IO (VS.Vector a)+unsafeViewVector n p = do+  fp <- newForeignPtr_ p+  pure (VS.unsafeFromForeignPtr0 fp n)
+ src/Box3D/Internal/DrawClosure.hsc view
@@ -0,0 +1,59 @@+{-| Field-by-field access to the shim's @hs_b3DrawClosure@.++Split out of "Box3D.Callbacks" so that module stays plain Haskell: this is the+only part that needs hsc2hs. Each writer names its field, so the struct's+layout stays the C compiler's problem and a slot added upstream is a compile+error rather than a read past the end of an array.+-}+module Box3D.Internal.DrawClosure+  ( size+  , shape+  , segment+  , transform+  , point+  , sphere+  , capsule+  , bounds+  , box+  , string+  , context+  ) where++import Foreign++#include "box3d_shim.h"++-- | Bytes to allocate for one closure.+size :: Int+size = #{size hs_b3DrawClosure}++shape :: Ptr () -> FunPtr () -> IO ()+shape = #{poke hs_b3DrawClosure, shape}++segment :: Ptr () -> FunPtr () -> IO ()+segment = #{poke hs_b3DrawClosure, segment}++transform :: Ptr () -> FunPtr () -> IO ()+transform = #{poke hs_b3DrawClosure, transform}++point :: Ptr () -> FunPtr () -> IO ()+point = #{poke hs_b3DrawClosure, point}++sphere :: Ptr () -> FunPtr () -> IO ()+sphere = #{poke hs_b3DrawClosure, sphere}++capsule :: Ptr () -> FunPtr () -> IO ()+capsule = #{poke hs_b3DrawClosure, capsule}++bounds :: Ptr () -> FunPtr () -> IO ()+bounds = #{poke hs_b3DrawClosure, bounds}++box :: Ptr () -> FunPtr () -> IO ()+box = #{poke hs_b3DrawClosure, box}++string :: Ptr () -> FunPtr () -> IO ()+string = #{poke hs_b3DrawClosure, string}++-- | The user context the trampolines pass through to each callback.+context :: Ptr () -> Ptr () -> IO ()+context = #{poke hs_b3DrawClosure, context}
+ src/Box3D/Joint.hsc view
@@ -0,0 +1,535 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.Joint+  ( JointDef (..)+  , JointType (..)+  , pattern ParallelJoint+  , pattern DistanceJoint+  , pattern FilterJoint+  , pattern MotorJoint+  , pattern PrismaticJoint+  , pattern RevoluteJoint+  , pattern SphericalJoint+  , pattern WeldJoint+  , pattern WheelJoint+  , destroy+  , isValid+  , getType+  , getBodyA+  , getBodyB+  , getWorld+  , setLocalFrameA+  , getLocalFrameA+  , getLocalFrameAInto+  , setLocalFrameB+  , getLocalFrameB+  , getLocalFrameBInto+  , setCollideConnected+  , getCollideConnected+  , setUserData+  , getUserData+  , wakeBodies+  , getConstraintForce+  , getConstraintForceInto+  , getConstraintTorque+  , getConstraintTorqueInto+  , getLinearSeparation+  , getAngularSeparation+  , setConstraintTuning+  , getConstraintTuning+  , setForceThreshold+  , getForceThreshold+  , setTorqueThreshold+  , getTorqueThreshold+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..))+import Data.Coerce (coerce)+import Box3D.Id (BodyId (..), JointId (..), WorldId (..))+import Box3D.MathTypes (Transform, Vec3)++#include "box3d/box3d.h"+++{- | 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.+-}+data JointDef = JointDef+  { userData :: Ptr ()+  -- ^ User data pointer+  , bodyIdA :: BodyId+  -- ^ The first attached body+  , bodyIdB :: BodyId+  -- ^ The second attached body+  , localFrameA :: Transform+  -- ^ The first local joint frame+  , localFrameB :: Transform+  -- ^ The second local joint frame+  , forceThreshold :: Float+  -- ^ Force threshold for joint events+  , torqueThreshold :: Float+  -- ^ Torque threshold for joint events+  , constraintHertz :: Float+  -- ^ Constraint hertz (advanced feature)+  , constraintDampingRatio :: Float+  -- ^ Constraint damping ratio (advanced feature)+  , drawScale :: Float+  -- ^ Debug draw scale+  , collideConnected :: CBool+  -- ^ Set this flag to true if the attached bodies should collide+  , internalValue :: CInt+  -- ^ Used internally to detect a valid definition. DO NOT SET.+  }+  deriving (Eq, Show)++instance Storable JointDef where+  sizeOf _ = #{size b3JointDef}+  alignment _ = #{alignment b3JointDef}+  peek p =+    JointDef+      <$> #{peek b3JointDef, userData} p+      <*> #{peek b3JointDef, bodyIdA} p+      <*> #{peek b3JointDef, bodyIdB} p+      <*> #{peek b3JointDef, localFrameA} p+      <*> #{peek b3JointDef, localFrameB} p+      <*> #{peek b3JointDef, forceThreshold} p+      <*> #{peek b3JointDef, torqueThreshold} p+      <*> #{peek b3JointDef, constraintHertz} p+      <*> #{peek b3JointDef, constraintDampingRatio} p+      <*> #{peek b3JointDef, drawScale} p+      <*> #{peek b3JointDef, collideConnected} p+      <*> #{peek b3JointDef, internalValue} p+  poke p (JointDef f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11 f12) = do+    #{poke b3JointDef, userData} p f1+    #{poke b3JointDef, bodyIdA} p f2+    #{poke b3JointDef, bodyIdB} p f3+    #{poke b3JointDef, localFrameA} p f4+    #{poke b3JointDef, localFrameB} p f5+    #{poke b3JointDef, forceThreshold} p f6+    #{poke b3JointDef, torqueThreshold} p f7+    #{poke b3JointDef, constraintHertz} p f8+    #{poke b3JointDef, constraintDampingRatio} p f9+    #{poke b3JointDef, drawScale} p f10+    #{poke b3JointDef, collideConnected} p f11+    #{poke b3JointDef, internalValue} p f12++{- | Joint type enumeration. This is useful because all joint types use @b3JointId@ and sometimes you+want to get the type of a joint.+-}+newtype JointType = JointType CInt+  deriving (Eq, Show, Storable)++pattern ParallelJoint :: JointType+pattern DistanceJoint :: JointType+pattern FilterJoint :: JointType+pattern MotorJoint :: JointType+pattern PrismaticJoint :: JointType+pattern RevoluteJoint :: JointType+pattern SphericalJoint :: JointType+pattern WeldJoint :: JointType+pattern WheelJoint :: JointType+pattern ParallelJoint = JointType #{const b3_parallelJoint}+pattern DistanceJoint = JointType #{const b3_distanceJoint}+pattern FilterJoint = JointType #{const b3_filterJoint}+pattern MotorJoint = JointType #{const b3_motorJoint}+pattern PrismaticJoint = JointType #{const b3_prismaticJoint}+pattern RevoluteJoint = JointType #{const b3_revoluteJoint}+pattern SphericalJoint = JointType #{const b3_sphericalJoint}+pattern WeldJoint = JointType #{const b3_weldJoint}+pattern WheelJoint = JointType #{const b3_wheelJoint}+++foreign import ccall unsafe "b3DestroyJoint"+  c_b3DestroyJoint :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b3Joint_IsValid"+  c_b3Joint_IsValid :: JointId -> IO CBool++foreign import ccall unsafe "b3Joint_GetType"+  c_b3Joint_GetType :: JointId -> IO CInt++foreign import ccall unsafe "b3Joint_GetBodyA"+  c_b3Joint_GetBodyA :: JointId -> IO BodyId++foreign import ccall unsafe "b3Joint_GetBodyB"+  c_b3Joint_GetBodyB :: JointId -> IO BodyId++foreign import ccall unsafe "b3Joint_GetWorld"+  c_b3Joint_GetWorld :: JointId -> IO WorldId++foreign import ccall unsafe "hsg_b3Joint_SetLocalFrameA"+  c_b3Joint_SetLocalFrameA :: JointId -> Ptr Transform -> IO ()++foreign import ccall unsafe "hsg_b3Joint_GetLocalFrameA"+  c_b3Joint_GetLocalFrameA :: JointId -> Ptr Transform -> IO ()++foreign import ccall unsafe "hsg_b3Joint_SetLocalFrameB"+  c_b3Joint_SetLocalFrameB :: JointId -> Ptr Transform -> IO ()++foreign import ccall unsafe "hsg_b3Joint_GetLocalFrameB"+  c_b3Joint_GetLocalFrameB :: JointId -> Ptr Transform -> IO ()++foreign import ccall unsafe "b3Joint_SetCollideConnected"+  c_b3Joint_SetCollideConnected :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b3Joint_GetCollideConnected"+  c_b3Joint_GetCollideConnected :: JointId -> IO CBool++foreign import ccall unsafe "b3Joint_SetUserData"+  c_b3Joint_SetUserData :: JointId -> Ptr () -> IO ()++foreign import ccall unsafe "b3Joint_GetUserData"+  c_b3Joint_GetUserData :: JointId -> IO (Ptr ())++foreign import ccall unsafe "b3Joint_WakeBodies"+  c_b3Joint_WakeBodies :: JointId -> IO ()++foreign import ccall unsafe "hsg_b3Joint_GetConstraintForce"+  c_b3Joint_GetConstraintForce :: JointId -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "hsg_b3Joint_GetConstraintTorque"+  c_b3Joint_GetConstraintTorque :: JointId -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "b3Joint_GetLinearSeparation"+  c_b3Joint_GetLinearSeparation :: JointId -> IO Float++foreign import ccall unsafe "b3Joint_GetAngularSeparation"+  c_b3Joint_GetAngularSeparation :: JointId -> IO Float++foreign import ccall unsafe "b3Joint_SetConstraintTuning"+  c_b3Joint_SetConstraintTuning :: JointId -> Float -> Float -> IO ()++foreign import ccall unsafe "b3Joint_GetConstraintTuning"+  c_b3Joint_GetConstraintTuning :: JointId -> Ptr Float -> Ptr Float -> IO ()++foreign import ccall unsafe "b3Joint_SetForceThreshold"+  c_b3Joint_SetForceThreshold :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3Joint_GetForceThreshold"+  c_b3Joint_GetForceThreshold :: JointId -> IO Float++foreign import ccall unsafe "b3Joint_SetTorqueThreshold"+  c_b3Joint_SetTorqueThreshold :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3Joint_GetTorqueThreshold"+  c_b3Joint_GetTorqueThreshold :: JointId -> IO Float++{- | Destroy a joint++Binds @b3DestroyJoint@.+-}+destroy+  :: JointId+  -> Bool+  -- ^ @wakeAttached@+  -> IO ()+destroy a0 a1 =+  c_b3DestroyJoint a0 (fromBool a1)++{- | Joint identifier validation. Provides validation for up to 64K allocations.++Binds @b3Joint_IsValid@.+-}+isValid+  :: JointId+  -> IO Bool+isValid a0 =+  toBool <$> (c_b3Joint_IsValid a0)++{- | Get the joint type++Binds @b3Joint_GetType@.+-}+getType+  :: JointId+  -> IO JointType+getType a0 =+  coerce <$> (c_b3Joint_GetType a0)++{- | Get body A id on a joint++Binds @b3Joint_GetBodyA@.+-}+getBodyA+  :: JointId+  -> IO BodyId+getBodyA a0 =+  c_b3Joint_GetBodyA a0++{- | Get body B id on a joint++Binds @b3Joint_GetBodyB@.+-}+getBodyB+  :: JointId+  -> IO BodyId+getBodyB a0 =+  c_b3Joint_GetBodyB a0++{- | Get the world that owns this joint++Binds @b3Joint_GetWorld@.+-}+getWorld+  :: JointId+  -> IO WorldId+getWorld a0 =+  c_b3Joint_GetWorld a0++{- | Set the local frame on bodyA++Binds @b3Joint_SetLocalFrameA@.+-}+setLocalFrameA+  :: JointId+  -> Transform+  -- ^ @localFrame@+  -> IO ()+setLocalFrameA a0 a1 =+  with a1 $ \p1 ->+  c_b3Joint_SetLocalFrameA a0 p1++{- | Get the local frame on bodyA++Binds @b3Joint_GetLocalFrameA@.+-}+getLocalFrameA+  :: JointId+  -> IO Transform+getLocalFrameA a0 =+  alloca $ \pOut -> c_b3Joint_GetLocalFrameA a0 pOut >> peek pOut++-- | Like 'getLocalFrameA', but the result is written into a caller-supplied buffer.+getLocalFrameAInto+  :: JointId+  -> Ptr Transform+  -- ^ Result buffer.+  -> IO ()+getLocalFrameAInto a0 out =+  c_b3Joint_GetLocalFrameA a0 out++{- | Set the local frame on bodyB++Binds @b3Joint_SetLocalFrameB@.+-}+setLocalFrameB+  :: JointId+  -> Transform+  -- ^ @localFrame@+  -> IO ()+setLocalFrameB a0 a1 =+  with a1 $ \p1 ->+  c_b3Joint_SetLocalFrameB a0 p1++{- | Get the local frame on bodyB++Binds @b3Joint_GetLocalFrameB@.+-}+getLocalFrameB+  :: JointId+  -> IO Transform+getLocalFrameB a0 =+  alloca $ \pOut -> c_b3Joint_GetLocalFrameB a0 pOut >> peek pOut++-- | Like 'getLocalFrameB', but the result is written into a caller-supplied buffer.+getLocalFrameBInto+  :: JointId+  -> Ptr Transform+  -- ^ Result buffer.+  -> IO ()+getLocalFrameBInto a0 out =+  c_b3Joint_GetLocalFrameB a0 out++{- | Toggle collision between connected bodies++Binds @b3Joint_SetCollideConnected@.+-}+setCollideConnected+  :: JointId+  -> Bool+  -- ^ @shouldCollide@+  -> IO ()+setCollideConnected a0 a1 =+  c_b3Joint_SetCollideConnected a0 (fromBool a1)++{- | Is collision allowed between connected bodies?++Binds @b3Joint_GetCollideConnected@.+-}+getCollideConnected+  :: JointId+  -> IO Bool+getCollideConnected a0 =+  toBool <$> (c_b3Joint_GetCollideConnected a0)++{- | Set the user data on a joint++Binds @b3Joint_SetUserData@.+-}+setUserData+  :: JointId+  -> Ptr ()+  -- ^ @userData@+  -> IO ()+setUserData a0 a1 =+  c_b3Joint_SetUserData a0 a1++{- | Get the user data on a joint++Binds @b3Joint_GetUserData@.+-}+getUserData+  :: JointId+  -> IO (Ptr ())+getUserData a0 =+  c_b3Joint_GetUserData a0++{- | Wake the bodies connect to this joint++Binds @b3Joint_WakeBodies@.+-}+wakeBodies+  :: JointId+  -> IO ()+wakeBodies a0 =+  c_b3Joint_WakeBodies a0++{- | Get the current constraint force for this joint++Binds @b3Joint_GetConstraintForce@.+-}+getConstraintForce+  :: JointId+  -> IO Vec3+getConstraintForce a0 =+  alloca $ \pOut -> c_b3Joint_GetConstraintForce a0 pOut >> peek pOut++-- | Like 'getConstraintForce', but the result is written into a caller-supplied buffer.+getConstraintForceInto+  :: JointId+  -> Ptr Vec3+  -- ^ Result buffer.+  -> IO ()+getConstraintForceInto a0 out =+  c_b3Joint_GetConstraintForce a0 out++{- | Get the current constraint torque for this joint++Binds @b3Joint_GetConstraintTorque@.+-}+getConstraintTorque+  :: JointId+  -> IO Vec3+getConstraintTorque a0 =+  alloca $ \pOut -> c_b3Joint_GetConstraintTorque a0 pOut >> peek pOut++-- | Like 'getConstraintTorque', but the result is written into a caller-supplied buffer.+getConstraintTorqueInto+  :: JointId+  -> Ptr Vec3+  -- ^ Result buffer.+  -> IO ()+getConstraintTorqueInto a0 out =+  c_b3Joint_GetConstraintTorque a0 out++{- | Get the current linear separation error for this joint. Does not consider admissible movement. Usually in meters.++Binds @b3Joint_GetLinearSeparation@.+-}+getLinearSeparation+  :: JointId+  -> IO Float+getLinearSeparation a0 =+  c_b3Joint_GetLinearSeparation a0++{- | Get the current angular separation error for this joint. Does not consider admissible movement. Usually in radians.++Binds @b3Joint_GetAngularSeparation@.+-}+getAngularSeparation+  :: JointId+  -> IO Float+getAngularSeparation a0 =+  c_b3Joint_GetAngularSeparation a0++{- | Set the joint constraint tuning. Advanced feature.++Binds @b3Joint_SetConstraintTuning@.+-}+setConstraintTuning+  :: JointId+  -- ^ @jointId@: the joint+  -> Float+  -- ^ @hertz@: the stiffness in Hertz (cycles per second)+  -> Float+  -- ^ @dampingRatio@: the non-dimensional damping ratio (one for critical damping)+  -> IO ()+setConstraintTuning a0 a1 a2 =+  c_b3Joint_SetConstraintTuning a0 a1 a2++{- | Get the joint constraint tuning. Advanced feature.++Binds @b3Joint_GetConstraintTuning@.+-}+getConstraintTuning+  :: JointId+  -> Ptr Float+  -- ^ @hertz@+  -> Ptr Float+  -- ^ @dampingRatio@+  -> IO ()+getConstraintTuning a0 a1 a2 =+  c_b3Joint_GetConstraintTuning a0 a1 a2++{- | Set the force threshold for joint events (Newtons)++Binds @b3Joint_SetForceThreshold@.+-}+setForceThreshold+  :: JointId+  -> Float+  -- ^ @threshold@+  -> IO ()+setForceThreshold a0 a1 =+  c_b3Joint_SetForceThreshold a0 a1++{- | Get the force threshold for joint events (Newtons)++Binds @b3Joint_GetForceThreshold@.+-}+getForceThreshold+  :: JointId+  -> IO Float+getForceThreshold a0 =+  c_b3Joint_GetForceThreshold a0++{- | Set the torque threshold for joint events (N-m)++Binds @b3Joint_SetTorqueThreshold@.+-}+setTorqueThreshold+  :: JointId+  -> Float+  -- ^ @threshold@+  -> IO ()+setTorqueThreshold a0 a1 =+  c_b3Joint_SetTorqueThreshold a0 a1++{- | Get the torque threshold for joint events (N-m)++Binds @b3Joint_GetTorqueThreshold@.+-}+getTorqueThreshold+  :: JointId+  -> IO Float+getTorqueThreshold a0 =+  c_b3Joint_GetTorqueThreshold a0
+ src/Box3D/Math.hsc view
@@ -0,0 +1,43 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.Math+  ( SegmentDistanceResult (..)+  )+  where++import Foreign+import Box3D.MathTypes (Vec3)++#include "box3d/box3d.h"+++-- | The closest points between to segments or infinite lines.+data SegmentDistanceResult = SegmentDistanceResult+  { point1 :: Vec3+  , fraction1 :: Float+  , point2 :: Vec3+  , fraction2 :: Float+  }+  deriving (Eq, Show)++instance Storable SegmentDistanceResult where+  sizeOf _ = #{size b3SegmentDistanceResult}+  alignment _ = #{alignment b3SegmentDistanceResult}+  peek p =+    SegmentDistanceResult+      <$> #{peek b3SegmentDistanceResult, point1} p+      <*> #{peek b3SegmentDistanceResult, fraction1} p+      <*> #{peek b3SegmentDistanceResult, point2} p+      <*> #{peek b3SegmentDistanceResult, fraction2} p+  poke p (SegmentDistanceResult f1 f2 f3 f4) = do+    #{poke b3SegmentDistanceResult, point1} p f1+    #{poke b3SegmentDistanceResult, fraction1} p f2+    #{poke b3SegmentDistanceResult, point2} p f3+    #{poke b3SegmentDistanceResult, fraction2} p f4+++
+ src/Box3D/MathFunctions.hs view
@@ -0,0 +1,123 @@+{-# LANGUAGE ForeignFunctionInterface #-}++{-| Low-level bindings to the exported (non-@inline@) functions of+@box3d/math_functions.h@.++Box3D implements most of its vector math as @static inline@ helpers, which+have no linkable symbols and are therefore out of scope for FFI. Only the+handful of functions marked @B3_API@ are bound here: the validators (handy+as ABI oracles for the "Box3D.MathTypes" 'Foreign.Storable.Storable'+instances) and a couple of by-value constructors.+-}+module Box3D.MathFunctions+  ( isValidVec3+  , isValidQuat+  , isValidTransform+  , isValidMatrix3+  , isValidAABB+  , isValidPlane+  , computeCosSin+  , computeQuatBetweenUnitVectors+  , getLengthUnitsPerMeter+  , setLengthUnitsPerMeter+  , getStallThreshold+  , setStallThreshold+  ) where++import Foreign+import Foreign.C.Types (CBool (..))++import Box3D.Internal (allocaOut, withValid)+import Box3D.MathTypes++foreign import ccall unsafe "hs_b3IsValidVec3"+  c_b3IsValidVec3 :: Ptr Vec3 -> IO CBool++foreign import ccall unsafe "hs_b3IsValidQuat"+  c_b3IsValidQuat :: Ptr Quat -> IO CBool++foreign import ccall unsafe "hs_b3IsValidTransform"+  c_b3IsValidTransform :: Ptr Transform -> IO CBool++foreign import ccall unsafe "hs_b3IsValidMatrix3"+  c_b3IsValidMatrix3 :: Ptr Matrix3 -> IO CBool++foreign import ccall unsafe "hs_b3IsValidAABB"+  c_b3IsValidAABB :: Ptr AABB -> IO CBool++foreign import ccall unsafe "hs_b3IsValidPlane"+  c_b3IsValidPlane :: Ptr Plane -> IO CBool++foreign import ccall unsafe "hs_b3ComputeCosSin"+  c_b3ComputeCosSin :: Float -> Ptr CosSin -> IO ()++foreign import ccall unsafe "hs_b3ComputeQuatBetweenUnitVectors"+  c_b3ComputeQuatBetweenUnitVectors :: Ptr Vec3 -> Ptr Vec3 -> Ptr Quat -> IO ()++-- | Is this a valid (finite, non-NaN) vector?+isValidVec3 :: Vec3 -> IO Bool+isValidVec3 = withValid c_b3IsValidVec3++-- | Is this a valid, normalized quaternion?+isValidQuat :: Quat -> IO Bool+isValidQuat = withValid c_b3IsValidQuat++-- | Is this a valid rigid transform?+isValidTransform :: Transform -> IO Bool+isValidTransform = withValid c_b3IsValidTransform++-- | Is this a valid 3x3 matrix?+isValidMatrix3 :: Matrix3 -> IO Bool+isValidMatrix3 = withValid c_b3IsValidMatrix3++-- | Is this a valid, bounded AABB?+isValidAABB :: AABB -> IO Bool+isValidAABB = withValid c_b3IsValidAABB++-- | Is this a valid plane (finite, unit normal)?+isValidPlane :: Plane -> IO Bool+isValidPlane = withValid c_b3IsValidPlane++-- | Compute a deterministic cosine/sine pair for the given angle in radians.+computeCosSin :: Float -> IO CosSin+computeCosSin radians = allocaOut (c_b3ComputeCosSin radians)++-- | Compute the shortest-arc rotation between two unit vectors.+computeQuatBetweenUnitVectors :: Vec3 -> Vec3 -> IO Quat+computeQuatBetweenUnitVectors v1 v2 =+  with v1 $ \p1 -> with v2 $ \p2 -> allocaOut (c_b3ComputeQuatBetweenUnitVectors p1 p2)++foreign import ccall unsafe "b3GetLengthUnitsPerMeter"+  c_b3GetLengthUnitsPerMeter :: IO Float++foreign import ccall unsafe "b3SetLengthUnitsPerMeter"+  c_b3SetLengthUnitsPerMeter :: Float -> IO ()++{- | The engine's current length units per meter (default 1; from+@box3d/constants.h@). Tuning constants scale with it: e.g.+@B3_LINEAR_SLOP@ is 0.005 of this.+-}+getLengthUnitsPerMeter :: IO Float+getLengthUnitsPerMeter = c_b3GetLengthUnitsPerMeter++{- | Change the length units, e.g. 128 for pixels-as-units. Do this before+creating any world.+-}+setLengthUnitsPerMeter :: Float -> IO ()+setLengthUnitsPerMeter = c_b3SetLengthUnitsPerMeter++foreign import ccall unsafe "b3GetStallThreshold"+  c_b3GetStallThreshold :: IO Float++foreign import ccall unsafe "b3SetStallThreshold"+  c_b3SetStallThreshold :: Float -> IO ()++{- | The threshold, in seconds, above which the engine logs a stall (from+@box3d/constants.h@; default @FLT_MAX@, i.e. never).+-}+getStallThreshold :: IO Float+getStallThreshold = c_b3GetStallThreshold++-- | Set the stall logging threshold in seconds.+setStallThreshold :: Float -> IO ()+setStallThreshold = c_b3SetStallThreshold
+ src/Box3D/MathTypes.hsc view
@@ -0,0 +1,366 @@+-- | Low-level bindings to the value types in @box3d/math_functions.h@.+--+-- These are the small POD math structs that the Box3D API passes and returns+-- by value. Field order matches the C layout; 'Storable' offsets come straight+-- from the header via @hsc2hs@. Fields are 'Float' because C @float@ maps to+-- the Haskell FFI 'Float' with an identical representation.+--+-- 'Pos' and 'WorldTransform' follow the @double-precision@ package flag: their+-- translation is 'Coord', which is 'Float' by default and 'Double' with the flag.+module Box3D.MathTypes+  ( Vec2 (..)+  , Vec3 (..)+  , CosSin (..)+  , Quat (..)+  , Transform (..)+  , Matrix3 (..)+  , AABB (..)+  , Plane (..)+  , Coord+  , Pos (..)+  , WorldTransform (..)++    -- * Constants+  , vec3Zero+  , vec3One+  , vec3AxisX+  , vec3AxisY+  , vec3AxisZ+  , quatIdentity+  , transformIdentity+  , posZero+  , worldTransformIdentity+  , matrix3Zero+  , matrix3Identity++    -- * World positions+  , toPos+  , toVec3+  , subPos+  , offsetPos+  , transformWorldPoint+  , invTransformWorldPoint+  , toRelativeTransform+  , makeWorldTransform++    -- * Vector math+  , vec3Add+  , vec3Sub+  , vec3Neg+  , vec3Scale+  , vec3MulAdd+  , vec3MulSub+  , vec3Dot+  , vec3Cross+  , vec3LengthSquared+  , vec3Length+  , vec3Normalize+  , vec3Lerp++    -- * Quaternion and plane math+  , quatMul+  , quatNormalize+  , rotateVector+  , invRotateVector+  , planeSeparation+  ) where++import Foreign+#if defined(BOX3D_DOUBLE_PRECISION)+import GHC.Float (double2Float, float2Double)+#endif++#include "box3d/box3d.h"++-- | A 2D vector.+data Vec2 = Vec2 Float Float+  deriving (Eq, Show)++-- | A 3D vector.+data Vec3 = Vec3 Float Float Float+  deriving (Eq, Show)++-- | Cosine and sine pair (@cosine@, @sine@).+data CosSin = CosSin Float Float+  deriving (Eq, Show)++-- | A quaternion: vector part @v@ and scalar part @s@.+data Quat = Quat Vec3 Float+  deriving (Eq, Show)++-- | A rigid transform: position @p@ and rotation @q@.+data Transform = Transform Vec3 Quat+  deriving (Eq, Show)++-- | A 3x3 column-major matrix (columns @cx@, @cy@, @cz@).+data Matrix3 = Matrix3 Vec3 Vec3 Vec3+  deriving (Eq, Show)++-- | Axis-aligned bounding box (@lowerBound@, @upperBound@).+data AABB = AABB Vec3 Vec3+  deriving (Eq, Show)++-- | A plane: @normal@ and @offset@ where @separation = dot(normal, point) - offset@.+data Plane = Plane Vec3 Float+  deriving (Eq, Show)++#if defined(BOX3D_DOUBLE_PRECISION)+type Coord = Double++widen :: Float -> Coord+widen = float2Double++narrow :: Coord -> Float+narrow = double2Float+#else+type Coord = Float++widen :: Float -> Coord+widen = id++narrow :: Coord -> Float+narrow = id+#endif++-- | World position.+data Pos = Pos Coord Coord Coord+  deriving (Eq, Show)++-- | World transform: position @p@ and rotation @q@.+data WorldTransform = WorldTransform Pos Quat+  deriving (Eq, Show)++instance Storable Vec2 where+  sizeOf _ = #{size b3Vec2}+  alignment _ = #{alignment b3Vec2}+  peek p = Vec2 <$> #{peek b3Vec2, x} p <*> #{peek b3Vec2, y} p+  poke p (Vec2 x y) = do+    #{poke b3Vec2, x} p x+    #{poke b3Vec2, y} p y++instance Storable Vec3 where+  sizeOf _ = #{size b3Vec3}+  alignment _ = #{alignment b3Vec3}+  peek p = Vec3 <$> #{peek b3Vec3, x} p <*> #{peek b3Vec3, y} p <*> #{peek b3Vec3, z} p+  poke p (Vec3 x y z) = do+    #{poke b3Vec3, x} p x+    #{poke b3Vec3, y} p y+    #{poke b3Vec3, z} p z++instance Storable CosSin where+  sizeOf _ = #{size b3CosSin}+  alignment _ = #{alignment b3CosSin}+  peek p = CosSin <$> #{peek b3CosSin, cosine} p <*> #{peek b3CosSin, sine} p+  poke p (CosSin c s) = do+    #{poke b3CosSin, cosine} p c+    #{poke b3CosSin, sine} p s++instance Storable Quat where+  sizeOf _ = #{size b3Quat}+  alignment _ = #{alignment b3Quat}+  peek p = Quat <$> #{peek b3Quat, v} p <*> #{peek b3Quat, s} p+  poke p (Quat v s) = do+    #{poke b3Quat, v} p v+    #{poke b3Quat, s} p s++instance Storable Transform where+  sizeOf _ = #{size b3Transform}+  alignment _ = #{alignment b3Transform}+  peek p = Transform <$> #{peek b3Transform, p} p <*> #{peek b3Transform, q} p+  poke ptr (Transform p q) = do+    #{poke b3Transform, p} ptr p+    #{poke b3Transform, q} ptr q++instance Storable Pos where+  sizeOf _ = #{size b3Pos}+  alignment _ = #{alignment b3Pos}+  peek p = Pos <$> #{peek b3Pos, x} p <*> #{peek b3Pos, y} p <*> #{peek b3Pos, z} p+  poke p (Pos x y z) = do+    #{poke b3Pos, x} p x+    #{poke b3Pos, y} p y+    #{poke b3Pos, z} p z++instance Storable WorldTransform where+  sizeOf _ = #{size b3WorldTransform}+  alignment _ = #{alignment b3WorldTransform}+  peek p = WorldTransform <$> #{peek b3WorldTransform, p} p <*> #{peek b3WorldTransform, q} p+  poke ptr (WorldTransform p q) = do+    #{poke b3WorldTransform, p} ptr p+    #{poke b3WorldTransform, q} ptr q++instance Storable Matrix3 where+  sizeOf _ = #{size b3Matrix3}+  alignment _ = #{alignment b3Matrix3}+  peek p = Matrix3 <$> #{peek b3Matrix3, cx} p <*> #{peek b3Matrix3, cy} p <*> #{peek b3Matrix3, cz} p+  poke p (Matrix3 cx cy cz) = do+    #{poke b3Matrix3, cx} p cx+    #{poke b3Matrix3, cy} p cy+    #{poke b3Matrix3, cz} p cz++instance Storable AABB where+  sizeOf _ = #{size b3AABB}+  alignment _ = #{alignment b3AABB}+  peek p = AABB <$> #{peek b3AABB, lowerBound} p <*> #{peek b3AABB, upperBound} p+  poke p (AABB lo hi) = do+    #{poke b3AABB, lowerBound} p lo+    #{poke b3AABB, upperBound} p hi++instance Storable Plane where+  sizeOf _ = #{size b3Plane}+  alignment _ = #{alignment b3Plane}+  peek p = Plane <$> #{peek b3Plane, normal} p <*> #{peek b3Plane, offset} p+  poke p (Plane n o) = do+    #{poke b3Plane, normal} p n+    #{poke b3Plane, offset} p o++vec3Zero :: Vec3+vec3Zero = Vec3 0 0 0++vec3One :: Vec3+vec3One = Vec3 1 1 1++vec3AxisX :: Vec3+vec3AxisX = Vec3 1 0 0++vec3AxisY :: Vec3+vec3AxisY = Vec3 0 1 0++vec3AxisZ :: Vec3+vec3AxisZ = Vec3 0 0 1++quatIdentity :: Quat+quatIdentity = Quat vec3Zero 1++transformIdentity :: Transform+transformIdentity = Transform vec3Zero quatIdentity++posZero :: Pos+posZero = Pos 0 0 0++worldTransformIdentity :: WorldTransform+worldTransformIdentity = WorldTransform posZero quatIdentity++toPos :: Vec3 -> Pos+toPos (Vec3 x y z) = Pos (widen x) (widen y) (widen z)++toVec3 :: Pos -> Vec3+toVec3 (Pos x y z) = Vec3 (narrow x) (narrow y) (narrow z)++subPos :: Pos -> Pos -> Vec3+subPos (Pos ax ay az) (Pos bx by bz) = Vec3 (narrow (ax - bx)) (narrow (ay - by)) (narrow (az - bz))++offsetPos :: Pos -> Vec3 -> Pos+offsetPos (Pos x y z) (Vec3 dx dy dz) = Pos (x + widen dx) (y + widen dy) (z + widen dz)++transformWorldPoint :: WorldTransform -> Vec3 -> Pos+transformWorldPoint (WorldTransform p q) v = offsetPos p (rotateVector q v)++invTransformWorldPoint :: WorldTransform -> Pos -> Vec3+invTransformWorldPoint (WorldTransform origin q) p = invRotateVector q (subPos p origin)++toRelativeTransform :: WorldTransform -> Pos -> Transform+toRelativeTransform (WorldTransform p q) base = Transform (subPos p base) q++makeWorldTransform :: Transform -> WorldTransform+makeWorldTransform (Transform p q) = WorldTransform (toPos p) q++matrix3Zero :: Matrix3+matrix3Zero = Matrix3 vec3Zero vec3Zero vec3Zero++matrix3Identity :: Matrix3+matrix3Identity = Matrix3 vec3AxisX vec3AxisY vec3AxisZ++-- Vector math ---------------------------------------------------------------+--+-- Pure ports of the @static inline@ vector helpers of+-- @box3d/math_functions.h@, which have no linkable symbols and cannot be+-- bound by FFI. Each names its upstream original.++-- | @a + b@ (upstream @b3Add@).+vec3Add :: Vec3 -> Vec3 -> Vec3+vec3Add (Vec3 ax ay az) (Vec3 bx by bz) = Vec3 (ax + bx) (ay + by) (az + bz)++-- | @a - b@ (upstream @b3Sub@).+vec3Sub :: Vec3 -> Vec3 -> Vec3+vec3Sub (Vec3 ax ay az) (Vec3 bx by bz) = Vec3 (ax - bx) (ay - by) (az - bz)++-- | @-a@ (upstream @b3Neg@).+vec3Neg :: Vec3 -> Vec3+vec3Neg (Vec3 x y z) = Vec3 (negate x) (negate y) (negate z)++-- | @s * v@ (upstream @b3MulSV@).+vec3Scale :: Float -> Vec3 -> Vec3+vec3Scale s (Vec3 x y z) = Vec3 (s * x) (s * y) (s * z)++-- | @a + s * b@ (upstream @b3MulAdd@).+vec3MulAdd :: Vec3 -> Float -> Vec3 -> Vec3+vec3MulAdd a s b = vec3Add a (vec3Scale s b)++-- | @a - s * b@ (upstream @b3MulSub@).+vec3MulSub :: Vec3 -> Float -> Vec3 -> Vec3+vec3MulSub a s b = vec3Sub a (vec3Scale s b)++-- | Dot product (upstream @b3Dot@).+vec3Dot :: Vec3 -> Vec3 -> Float+vec3Dot (Vec3 ax ay az) (Vec3 bx by bz) = ax * bx + ay * by + az * bz++-- | Cross product (upstream @b3Cross@).+vec3Cross :: Vec3 -> Vec3 -> Vec3+vec3Cross (Vec3 ax ay az) (Vec3 bx by bz) =+  Vec3 (ay * bz - az * by) (az * bx - ax * bz) (ax * by - ay * bx)++-- | Squared length (upstream @b3LengthSquared@).+vec3LengthSquared :: Vec3 -> Float+vec3LengthSquared v = vec3Dot v v++-- | Length (upstream @b3Length@).+vec3Length :: Vec3 -> Float+vec3Length = sqrt . vec3LengthSquared++-- | A unit vector in the direction of @v@, or 'vec3Zero' when @v@ is too+-- short to normalize — below @FLT_EPSILON@, like upstream @b3Normalize@.+vec3Normalize :: Vec3 -> Vec3+vec3Normalize v+  | len < 1.1920929e-7 = vec3Zero+  | otherwise = vec3Scale (1 / len) v+  where+    len = vec3Length v++-- | Linear interpolation from @a@ at 0 to @b@ at 1 (upstream @b3Lerp@).+vec3Lerp :: Vec3 -> Vec3 -> Float -> Vec3+vec3Lerp (Vec3 ax ay az) (Vec3 bx by bz) t =+  Vec3 ((1 - t) * ax + t * bx) ((1 - t) * ay + t * by) ((1 - t) * az + t * bz)++-- | Compose two rotations: @q1 * q2@ rotates by @q2@ then @q1@ (upstream+-- @b3MulQuat@).+quatMul :: Quat -> Quat -> Quat+quatMul (Quat v1 s1) (Quat v2 s2) =+  Quat+    (vec3MulAdd (vec3MulAdd (vec3Cross v1 v2) s1 v2) s2 v1)+    (s1 * s2 - vec3Dot v1 v2)++-- | Normalize a quaternion, or 'quatIdentity' when it is too short to+-- normalize (upstream @b3NormalizeQuat@).+quatNormalize :: Quat -> Quat+quatNormalize (Quat v s)+  | lengthSq > 1000 * 1.17549435e-38 = Quat (vec3Scale inv v) (inv * s)+  | otherwise = quatIdentity+  where+    lengthSq = vec3Dot v v + s * s+    inv = 1 / sqrt lengthSq++-- | Rotate a vector by a unit quaternion (upstream @b3RotateVector@).+rotateVector :: Quat -> Vec3 -> Vec3+rotateVector (Quat qv qs) v =+  vec3MulAdd v 2 (vec3Cross qv (vec3MulAdd (vec3Cross qv v) qs v))++-- | Inverse-rotate a vector by a unit quaternion (upstream+-- @b3InvRotateVector@).+invRotateVector :: Quat -> Vec3 -> Vec3+invRotateVector (Quat qv qs) v =+  vec3MulAdd v 2 (vec3Cross qv (vec3MulSub (vec3Cross qv v) qs v))++-- | Signed distance of a point from a plane, using the engine's+-- @dot(normal, point) - offset@ convention (upstream @b3PlaneSeparation@).+planeSeparation :: Plane -> Vec3 -> Float+planeSeparation (Plane n o) p = vec3Dot n p - o
+ src/Box3D/Mesh.hsc view
@@ -0,0 +1,273 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.Mesh+  ( Mesh (..)+  , MeshData (..)+  , MeshTriangle (..)+  , MeshEdgeFlags (..)+  , pattern ConcaveEdge1+  , pattern ConcaveEdge2+  , pattern ConcaveEdge3+  , pattern InverseConcaveEdge1+  , pattern InverseConcaveEdge2+  , pattern InverseConcaveEdge3+  , pattern AllConcaveEdges+  , pattern FlatEdge1+  , pattern FlatEdge2+  , pattern FlatEdge3+  , pattern AllFlatEdges+  , MeshDef (..)+  , create+  , destroy+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..))+import Box3D.MathTypes (AABB, Vec3)++#include "box3d/box3d.h"+++-- | This allows mesh data to be re-used with different scales.+data Mesh = Mesh+  { data_ :: Ptr MeshData+  -- ^ Immutable pointer to the mesh data.+  , scale :: Vec3+  -- ^ This scale may be non-uniform and have negative components. However,+  -- no component may be very small in magnitude.+  }+  deriving (Eq, Show)++instance Storable Mesh where+  sizeOf _ = #{size b3Mesh}+  alignment _ = #{alignment b3Mesh}+  peek p =+    Mesh+      <$> #{peek b3Mesh, data} p+      <*> #{peek b3Mesh, scale} p+  poke p (Mesh f1 f2) = do+    #{poke b3Mesh, data} p f1+    #{poke b3Mesh, scale} p f2++{- | This is a sorted triangle collision bounding volume hierarchy.++__Note:__ This struct has data hanging off the end and cannot be directly copied.+-}+data MeshData = MeshData+  { version :: Word64+  -- ^ Version must be first.+  , hash :: Word64+  -- ^ Hash of this mesh (this field is zero when the hash is computed)+  , byteCount :: Int32+  -- ^ The total number of bytes for this mesh.+  , bounds :: AABB+  -- ^ Local axis-aligned box.+  , surfaceArea :: Float+  -- ^ Combined surface area of all triangles. Single-sided.+  , treeHeight :: Int32+  -- ^ The height of the bounding volume hierarchy.+  , degenerateCount :: Int32+  -- ^ The number of degenerate triangles. Diagnostic.+  , nodeOffset :: Int32+  -- ^ Offset of the node array in bytes from the struct address.+  , nodeCount :: Int32+  -- ^ The number of BVH nodes.+  , vertexOffset :: Int32+  -- ^ Offset of the vertex array in bytes from the struct address.+  , vertexCount :: Int32+  -- ^ The number of vertices.+  , triangleOffset :: Int32+  -- ^ Offset of the triangle array in bytes from the struct address.+  , triangleCount :: Int32+  -- ^ The number of triangles.+  , materialOffset :: Int32+  -- ^ Offset of the material array in bytes from the struct address.+  , materialCount :: Int32+  -- ^ The number of materials.+  , flagsOffset :: Int32+  -- ^ Offset of the triangle flag array in bytes from the struct address.+  , padding :: Int32+  -- ^ Explicit padding.+  }+  deriving (Eq, Show)++instance Storable MeshData where+  sizeOf _ = #{size b3MeshData}+  alignment _ = #{alignment b3MeshData}+  peek p =+    MeshData+      <$> #{peek b3MeshData, version} p+      <*> #{peek b3MeshData, hash} p+      <*> #{peek b3MeshData, byteCount} p+      <*> #{peek b3MeshData, bounds} p+      <*> #{peek b3MeshData, surfaceArea} p+      <*> #{peek b3MeshData, treeHeight} p+      <*> #{peek b3MeshData, degenerateCount} p+      <*> #{peek b3MeshData, nodeOffset} p+      <*> #{peek b3MeshData, nodeCount} p+      <*> #{peek b3MeshData, vertexOffset} p+      <*> #{peek b3MeshData, vertexCount} p+      <*> #{peek b3MeshData, triangleOffset} p+      <*> #{peek b3MeshData, triangleCount} p+      <*> #{peek b3MeshData, materialOffset} p+      <*> #{peek b3MeshData, materialCount} p+      <*> #{peek b3MeshData, flagsOffset} p+      <*> #{peek b3MeshData, padding} p+  poke p (MeshData f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11 f12 f13 f14 f15 f16 f17) = do+    #{poke b3MeshData, version} p f1+    #{poke b3MeshData, hash} p f2+    #{poke b3MeshData, byteCount} p f3+    #{poke b3MeshData, bounds} p f4+    #{poke b3MeshData, surfaceArea} p f5+    #{poke b3MeshData, treeHeight} p f6+    #{poke b3MeshData, degenerateCount} p f7+    #{poke b3MeshData, nodeOffset} p f8+    #{poke b3MeshData, nodeCount} p f9+    #{poke b3MeshData, vertexOffset} p f10+    #{poke b3MeshData, vertexCount} p f11+    #{poke b3MeshData, triangleOffset} p f12+    #{poke b3MeshData, triangleCount} p f13+    #{poke b3MeshData, materialOffset} p f14+    #{poke b3MeshData, materialCount} p f15+    #{poke b3MeshData, flagsOffset} p f16+    #{poke b3MeshData, padding} p f17++-- | A mesh triangle.+data MeshTriangle = MeshTriangle+  { index1 :: Int32+  -- ^ Index of vertex 1.+  , index2 :: Int32+  -- ^ Index of vertex 2.+  , index3 :: Int32+  -- ^ Index of vertex 3.+  }+  deriving (Eq, Show)++instance Storable MeshTriangle where+  sizeOf _ = #{size b3MeshTriangle}+  alignment _ = #{alignment b3MeshTriangle}+  peek p =+    MeshTriangle+      <$> #{peek b3MeshTriangle, index1} p+      <*> #{peek b3MeshTriangle, index2} p+      <*> #{peek b3MeshTriangle, index3} p+  poke p (MeshTriangle f1 f2 f3) = do+    #{poke b3MeshTriangle, index1} p f1+    #{poke b3MeshTriangle, index2} p f2+    #{poke b3MeshTriangle, index3} p f3++-- | Triangle mesh edge flags.+newtype MeshEdgeFlags = MeshEdgeFlags CInt+  deriving (Eq, Show, Storable)++pattern ConcaveEdge1 :: MeshEdgeFlags+pattern ConcaveEdge2 :: MeshEdgeFlags+pattern ConcaveEdge3 :: MeshEdgeFlags+pattern InverseConcaveEdge1 :: MeshEdgeFlags+pattern InverseConcaveEdge2 :: MeshEdgeFlags+pattern InverseConcaveEdge3 :: MeshEdgeFlags+pattern AllConcaveEdges :: MeshEdgeFlags+pattern FlatEdge1 :: MeshEdgeFlags+pattern FlatEdge2 :: MeshEdgeFlags+pattern FlatEdge3 :: MeshEdgeFlags+pattern AllFlatEdges :: MeshEdgeFlags+pattern ConcaveEdge1 = MeshEdgeFlags #{const b3_concaveEdge1}+pattern ConcaveEdge2 = MeshEdgeFlags #{const b3_concaveEdge2}+pattern ConcaveEdge3 = MeshEdgeFlags #{const b3_concaveEdge3}+pattern InverseConcaveEdge1 = MeshEdgeFlags #{const b3_inverseConcaveEdge1}+pattern InverseConcaveEdge2 = MeshEdgeFlags #{const b3_inverseConcaveEdge2}+pattern InverseConcaveEdge3 = MeshEdgeFlags #{const b3_inverseConcaveEdge3}+pattern AllConcaveEdges = MeshEdgeFlags #{const b3_allConcaveEdges}+pattern FlatEdge1 = MeshEdgeFlags #{const b3_flatEdge1}+pattern FlatEdge2 = MeshEdgeFlags #{const b3_flatEdge2}+pattern FlatEdge3 = MeshEdgeFlags #{const b3_flatEdge3}+pattern AllFlatEdges = MeshEdgeFlags #{const b3_allFlatEdges}++{- | This is used to create a re-usable collision mesh. No pointers+are held to this data in @b3MeshData@. So all this data can be temporary.+-}+data MeshDef = MeshDef+  { vertices :: Ptr Vec3+  -- ^ Triangle vertices.+  , indices :: Ptr ()+  -- ^ Triangle vertex indices. 3 for each triangle. CCW winding.+  , materialIndices :: Ptr ()+  -- ^ 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.+  , weldTolerance :: Float+  -- ^ Tolerance for vertex welding in length units.+  , vertexCount :: CInt+  -- ^ The vertex count. Must be 3 or more.+  , triangleCount :: CInt+  -- ^ The triangle count. Must be 1 or more.+  , weldVertices :: CBool+  -- ^ Optionally weld nearby vertices.+  , useMedianSplit :: CBool+  -- ^ Use the median split instead of SAH to speed up mesh creation. Good+  -- for meshes that are structured like a grid.+  , identifyEdges :: CBool+  -- ^ Compute triangle adjacency information using shared edges+  }+  deriving (Eq, Show)++instance Storable MeshDef where+  sizeOf _ = #{size b3MeshDef}+  alignment _ = #{alignment b3MeshDef}+  peek p =+    MeshDef+      <$> #{peek b3MeshDef, vertices} p+      <*> #{peek b3MeshDef, indices} p+      <*> #{peek b3MeshDef, materialIndices} p+      <*> #{peek b3MeshDef, weldTolerance} p+      <*> #{peek b3MeshDef, vertexCount} p+      <*> #{peek b3MeshDef, triangleCount} p+      <*> #{peek b3MeshDef, weldVertices} p+      <*> #{peek b3MeshDef, useMedianSplit} p+      <*> #{peek b3MeshDef, identifyEdges} p+  poke p (MeshDef f1 f2 f3 f4 f5 f6 f7 f8 f9) = do+    #{poke b3MeshDef, vertices} p f1+    #{poke b3MeshDef, indices} p f2+    #{poke b3MeshDef, materialIndices} p f3+    #{poke b3MeshDef, weldTolerance} p f4+    #{poke b3MeshDef, vertexCount} p f5+    #{poke b3MeshDef, triangleCount} p f6+    #{poke b3MeshDef, weldVertices} p f7+    #{poke b3MeshDef, useMedianSplit} p f8+    #{poke b3MeshDef, identifyEdges} p f9+++foreign import ccall unsafe "b3CreateMesh"+  c_b3CreateMesh :: Ptr MeshDef -> Ptr CInt -> CInt -> IO (Ptr MeshData)++foreign import ccall unsafe "b3DestroyMesh"+  c_b3DestroyMesh :: Ptr MeshData -> IO ()++{- | Create a generic mesh.++Binds @b3CreateMesh@.+-}+create+  :: Ptr MeshDef+  -> Ptr CInt+  -- ^ @degenerateTriangleIndices@+  -> Int+  -- ^ @degenerateCapacity@+  -> IO (Ptr MeshData)+create a0 a1 a2 =+  c_b3CreateMesh a0 a1 (fromIntegral a2)++{- | Destroy a mesh.++Binds @b3DestroyMesh@.+-}+destroy+  :: Ptr MeshData+  -> IO ()+destroy a0 =+  c_b3DestroyMesh a0
+ src/Box3D/MotorJoint.hsc view
@@ -0,0 +1,423 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.MotorJoint+  ( MotorJointDef (..)+  , defaultMotorJointDef+  , create+  , setLinearVelocity+  , getLinearVelocity+  , getLinearVelocityInto+  , setAngularVelocity+  , getAngularVelocity+  , getAngularVelocityInto+  , setMaxVelocityForce+  , getMaxVelocityForce+  , setMaxVelocityTorque+  , getMaxVelocityTorque+  , setLinearHertz+  , getLinearHertz+  , setLinearDampingRatio+  , getLinearDampingRatio+  , setAngularHertz+  , getAngularHertz+  , setAngularDampingRatio+  , getAngularDampingRatio+  , setMaxSpringForce+  , getMaxSpringForce+  , setMaxSpringTorque+  , getMaxSpringTorque+  )+  where++import Foreign+import Box3D.Internal (allocaOut)+import Box3D.Id (JointId (..), WorldId (..))+import Box3D.Joint (JointDef)+import Box3D.MathTypes (Vec3)++#include "box3d/box3d.h"+++-- | A motor joint is used to control the relative position and velocity between two bodies.+data MotorJointDef = MotorJointDef+  { base :: JointDef+  -- ^ Base joint definition+  , linearVelocity :: Vec3+  -- ^ The desired linear velocity+  , maxVelocityForce :: Float+  -- ^ The maximum motor force in newtons+  , angularVelocity :: Vec3+  -- ^ The desired angular velocity+  , maxVelocityTorque :: Float+  -- ^ The maximum motor torque in newton-meters+  , linearHertz :: Float+  -- ^ Linear spring hertz for position control+  , linearDampingRatio :: Float+  -- ^ Linear spring damping ratio+  , maxSpringForce :: Float+  -- ^ Maximum spring force in newtons+  , angularHertz :: Float+  -- ^ Angular spring hertz for position control+  , angularDampingRatio :: Float+  -- ^ Angular spring damping ratio+  , maxSpringTorque :: Float+  -- ^ Maximum spring torque in newton-meters+  }+  deriving (Eq, Show)++instance Storable MotorJointDef where+  sizeOf _ = #{size b3MotorJointDef}+  alignment _ = #{alignment b3MotorJointDef}+  peek p =+    MotorJointDef+      <$> #{peek b3MotorJointDef, base} p+      <*> #{peek b3MotorJointDef, linearVelocity} p+      <*> #{peek b3MotorJointDef, maxVelocityForce} p+      <*> #{peek b3MotorJointDef, angularVelocity} p+      <*> #{peek b3MotorJointDef, maxVelocityTorque} p+      <*> #{peek b3MotorJointDef, linearHertz} p+      <*> #{peek b3MotorJointDef, linearDampingRatio} p+      <*> #{peek b3MotorJointDef, maxSpringForce} p+      <*> #{peek b3MotorJointDef, angularHertz} p+      <*> #{peek b3MotorJointDef, angularDampingRatio} p+      <*> #{peek b3MotorJointDef, maxSpringTorque} p+  poke p (MotorJointDef f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11) = do+    #{poke b3MotorJointDef, base} p f1+    #{poke b3MotorJointDef, linearVelocity} p f2+    #{poke b3MotorJointDef, maxVelocityForce} p f3+    #{poke b3MotorJointDef, angularVelocity} p f4+    #{poke b3MotorJointDef, maxVelocityTorque} p f5+    #{poke b3MotorJointDef, linearHertz} p f6+    #{poke b3MotorJointDef, linearDampingRatio} p f7+    #{poke b3MotorJointDef, maxSpringForce} p f8+    #{poke b3MotorJointDef, angularHertz} p f9+    #{poke b3MotorJointDef, angularDampingRatio} p f10+    #{poke b3MotorJointDef, maxSpringTorque} p f11++foreign import ccall unsafe "hs_b3DefaultMotorJointDef"+  c_b3DefaultMotorJointDef :: Ptr MotorJointDef -> IO ()++defaultMotorJointDef :: IO MotorJointDef+defaultMotorJointDef = allocaOut c_b3DefaultMotorJointDef+++foreign import ccall unsafe "b3CreateMotorJoint"+  c_b3CreateMotorJoint :: WorldId -> Ptr MotorJointDef -> IO JointId++foreign import ccall unsafe "hsg_b3MotorJoint_SetLinearVelocity"+  c_b3MotorJoint_SetLinearVelocity :: JointId -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "hsg_b3MotorJoint_GetLinearVelocity"+  c_b3MotorJoint_GetLinearVelocity :: JointId -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "hsg_b3MotorJoint_SetAngularVelocity"+  c_b3MotorJoint_SetAngularVelocity :: JointId -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "hsg_b3MotorJoint_GetAngularVelocity"+  c_b3MotorJoint_GetAngularVelocity :: JointId -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "b3MotorJoint_SetMaxVelocityForce"+  c_b3MotorJoint_SetMaxVelocityForce :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3MotorJoint_GetMaxVelocityForce"+  c_b3MotorJoint_GetMaxVelocityForce :: JointId -> IO Float++foreign import ccall unsafe "b3MotorJoint_SetMaxVelocityTorque"+  c_b3MotorJoint_SetMaxVelocityTorque :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3MotorJoint_GetMaxVelocityTorque"+  c_b3MotorJoint_GetMaxVelocityTorque :: JointId -> IO Float++foreign import ccall unsafe "b3MotorJoint_SetLinearHertz"+  c_b3MotorJoint_SetLinearHertz :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3MotorJoint_GetLinearHertz"+  c_b3MotorJoint_GetLinearHertz :: JointId -> IO Float++foreign import ccall unsafe "b3MotorJoint_SetLinearDampingRatio"+  c_b3MotorJoint_SetLinearDampingRatio :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3MotorJoint_GetLinearDampingRatio"+  c_b3MotorJoint_GetLinearDampingRatio :: JointId -> IO Float++foreign import ccall unsafe "b3MotorJoint_SetAngularHertz"+  c_b3MotorJoint_SetAngularHertz :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3MotorJoint_GetAngularHertz"+  c_b3MotorJoint_GetAngularHertz :: JointId -> IO Float++foreign import ccall unsafe "b3MotorJoint_SetAngularDampingRatio"+  c_b3MotorJoint_SetAngularDampingRatio :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3MotorJoint_GetAngularDampingRatio"+  c_b3MotorJoint_GetAngularDampingRatio :: JointId -> IO Float++foreign import ccall unsafe "b3MotorJoint_SetMaxSpringForce"+  c_b3MotorJoint_SetMaxSpringForce :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3MotorJoint_GetMaxSpringForce"+  c_b3MotorJoint_GetMaxSpringForce :: JointId -> IO Float++foreign import ccall unsafe "b3MotorJoint_SetMaxSpringTorque"+  c_b3MotorJoint_SetMaxSpringTorque :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3MotorJoint_GetMaxSpringTorque"+  c_b3MotorJoint_GetMaxSpringTorque :: JointId -> IO Float++{- | Create a motor joint++See also @b3MotorJointDef@ for details.++Binds @b3CreateMotorJoint@.+-}+create+  :: WorldId+  -> MotorJointDef+  -> IO JointId+create a0 a1 =+  with a1 $ \p1 ->+  c_b3CreateMotorJoint a0 p1++{- | Set the desired relative linear velocity in meters per second++Binds @b3MotorJoint_SetLinearVelocity@.+-}+setLinearVelocity+  :: JointId+  -> Vec3+  -- ^ @velocity@+  -> IO ()+setLinearVelocity a0 a1 =+  with a1 $ \p1 ->+  c_b3MotorJoint_SetLinearVelocity a0 p1++{- | Get the desired relative linear velocity in meters per second++Binds @b3MotorJoint_GetLinearVelocity@.+-}+getLinearVelocity+  :: JointId+  -> IO Vec3+getLinearVelocity a0 =+  alloca $ \pOut -> c_b3MotorJoint_GetLinearVelocity a0 pOut >> peek pOut++-- | Like 'getLinearVelocity', but the result is written into a caller-supplied buffer.+getLinearVelocityInto+  :: JointId+  -> Ptr Vec3+  -- ^ Result buffer.+  -> IO ()+getLinearVelocityInto a0 out =+  c_b3MotorJoint_GetLinearVelocity a0 out++{- | Set the desired relative angular velocity in radians per second++Binds @b3MotorJoint_SetAngularVelocity@.+-}+setAngularVelocity+  :: JointId+  -> Vec3+  -- ^ @velocity@+  -> IO ()+setAngularVelocity a0 a1 =+  with a1 $ \p1 ->+  c_b3MotorJoint_SetAngularVelocity a0 p1++{- | Get the desired relative angular velocity in radians per second++Binds @b3MotorJoint_GetAngularVelocity@.+-}+getAngularVelocity+  :: JointId+  -> IO Vec3+getAngularVelocity a0 =+  alloca $ \pOut -> c_b3MotorJoint_GetAngularVelocity a0 pOut >> peek pOut++-- | Like 'getAngularVelocity', but the result is written into a caller-supplied buffer.+getAngularVelocityInto+  :: JointId+  -> Ptr Vec3+  -- ^ Result buffer.+  -> IO ()+getAngularVelocityInto a0 out =+  c_b3MotorJoint_GetAngularVelocity a0 out++{- | Set the motor joint maximum force, usually in newtons++Binds @b3MotorJoint_SetMaxVelocityForce@.+-}+setMaxVelocityForce+  :: JointId+  -> Float+  -- ^ @maxForce@+  -> IO ()+setMaxVelocityForce a0 a1 =+  c_b3MotorJoint_SetMaxVelocityForce a0 a1++{- | Get the motor joint maximum force, usually in newtons++Binds @b3MotorJoint_GetMaxVelocityForce@.+-}+getMaxVelocityForce+  :: JointId+  -> IO Float+getMaxVelocityForce a0 =+  c_b3MotorJoint_GetMaxVelocityForce a0++{- | Set the motor joint maximum torque, usually in newton-meters++Binds @b3MotorJoint_SetMaxVelocityTorque@.+-}+setMaxVelocityTorque+  :: JointId+  -> Float+  -- ^ @maxTorque@+  -> IO ()+setMaxVelocityTorque a0 a1 =+  c_b3MotorJoint_SetMaxVelocityTorque a0 a1++{- | Get the motor joint maximum torque, usually in newton-meters++Binds @b3MotorJoint_GetMaxVelocityTorque@.+-}+getMaxVelocityTorque+  :: JointId+  -> IO Float+getMaxVelocityTorque a0 =+  c_b3MotorJoint_GetMaxVelocityTorque a0++{- | Set the spring linear hertz stiffness++Binds @b3MotorJoint_SetLinearHertz@.+-}+setLinearHertz+  :: JointId+  -> Float+  -- ^ @hertz@+  -> IO ()+setLinearHertz a0 a1 =+  c_b3MotorJoint_SetLinearHertz a0 a1++{- | Get the spring linear hertz stiffness++Binds @b3MotorJoint_GetLinearHertz@.+-}+getLinearHertz+  :: JointId+  -> IO Float+getLinearHertz a0 =+  c_b3MotorJoint_GetLinearHertz a0++{- | Set the spring linear damping ratio. Use 1.0 for critical damping.++Binds @b3MotorJoint_SetLinearDampingRatio@.+-}+setLinearDampingRatio+  :: JointId+  -> Float+  -- ^ @damping@+  -> IO ()+setLinearDampingRatio a0 a1 =+  c_b3MotorJoint_SetLinearDampingRatio a0 a1++{- | Get the spring linear damping ratio.++Binds @b3MotorJoint_GetLinearDampingRatio@.+-}+getLinearDampingRatio+  :: JointId+  -> IO Float+getLinearDampingRatio a0 =+  c_b3MotorJoint_GetLinearDampingRatio a0++{- | Set the spring angular hertz stiffness++Binds @b3MotorJoint_SetAngularHertz@.+-}+setAngularHertz+  :: JointId+  -> Float+  -- ^ @hertz@+  -> IO ()+setAngularHertz a0 a1 =+  c_b3MotorJoint_SetAngularHertz a0 a1++{- | Get the spring angular hertz stiffness++Binds @b3MotorJoint_GetAngularHertz@.+-}+getAngularHertz+  :: JointId+  -> IO Float+getAngularHertz a0 =+  c_b3MotorJoint_GetAngularHertz a0++{- | Set the spring angular damping ratio. Use 1.0 for critical damping.++Binds @b3MotorJoint_SetAngularDampingRatio@.+-}+setAngularDampingRatio+  :: JointId+  -> Float+  -- ^ @damping@+  -> IO ()+setAngularDampingRatio a0 a1 =+  c_b3MotorJoint_SetAngularDampingRatio a0 a1++{- | Get the spring angular damping ratio.++Binds @b3MotorJoint_GetAngularDampingRatio@.+-}+getAngularDampingRatio+  :: JointId+  -> IO Float+getAngularDampingRatio a0 =+  c_b3MotorJoint_GetAngularDampingRatio a0++{- | Set the maximum spring force in newtons.++Binds @b3MotorJoint_SetMaxSpringForce@.+-}+setMaxSpringForce+  :: JointId+  -> Float+  -- ^ @maxForce@+  -> IO ()+setMaxSpringForce a0 a1 =+  c_b3MotorJoint_SetMaxSpringForce a0 a1++{- | Get the maximum spring force in newtons.++Binds @b3MotorJoint_GetMaxSpringForce@.+-}+getMaxSpringForce+  :: JointId+  -> IO Float+getMaxSpringForce a0 =+  c_b3MotorJoint_GetMaxSpringForce a0++{- | Set the maximum spring torque in newtons * meters++Binds @b3MotorJoint_SetMaxSpringTorque@.+-}+setMaxSpringTorque+  :: JointId+  -> Float+  -- ^ @maxTorque@+  -> IO ()+setMaxSpringTorque a0 a1 =+  c_b3MotorJoint_SetMaxSpringTorque a0 a1++{- | Get the maximum spring torque in newtons * meters++Binds @b3MotorJoint_GetMaxSpringTorque@.+-}+getMaxSpringTorque+  :: JointId+  -> IO Float+getMaxSpringTorque a0 =+  c_b3MotorJoint_GetMaxSpringTorque a0
+ src/Box3D/Mover.hs view
@@ -0,0 +1,53 @@+{-| Friendly surface over the engine's mover plane solver.++The raw bindings ("Box3D.Collision".@solvePlanes@ / @clipVector@) work on a+mutable @b3CollisionPlane@ array: @b3SolvePlanes@ accumulates each plane's+@push@ in place and @b3ClipVector@ reads those accumulators back. The+wrappers here own that mutation, so a kinematic character controller can+stay in vector land:++@+(delta, planes', _iters) <- solvePlanes targetDelta planes+velocity' <- clipVector velocity planes'+@++Planes are normally assembled from the 'Box3D.Collision.PlaneResult's gathered+by 'Box3D.World.collideMover'.+-}+module Box3D.Mover+  ( solvePlanes+  , clipVector+  ) where++import Data.Vector.Storable qualified as VS+import Data.Vector.Storable.Mutable qualified as VSM++import Box3D.Collision (CollisionPlane, PlaneSolverResult (..))+import Box3D.Collision qualified as Collision+import Box3D.MathTypes (Vec3)++{- | Solve the position of a mover that satisfies the given collision+planes. Returns the resolved translation, the planes with their final+@push@ accumulators (feed these to 'clipVector'), and the solver+iteration count (for diagnostics).+-}+solvePlanes :: Vec3 -> VS.Vector CollisionPlane -> IO (Vec3, VS.Vector CollisionPlane, Int)+solvePlanes targetDelta planes = do+  mplanes <- VS.thaw planes+  result <- VSM.unsafeWith mplanes $ \p ->+    Collision.solvePlanes targetDelta p (VSM.length mplanes)+  planes' <- VS.unsafeFreeze mplanes+  pure+    ( result.delta+    , planes'+    , fromIntegral result.iterationCount+    )++{- | Clip a velocity against the given collision planes; planes with zero+@push@ or with @clipVelocity@ unset are skipped. Call it on the planes+returned by 'solvePlanes'.+-}+clipVector :: Vec3 -> VS.Vector CollisionPlane -> IO Vec3+clipVector vector planes =+  VS.unsafeWith planes $ \p ->+    Collision.clipVector vector p (VS.length planes)
+ src/Box3D/ParallelJoint.hsc view
@@ -0,0 +1,164 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.ParallelJoint+  ( ParallelJointDef (..)+  , defaultParallelJointDef+  , create+  , setSpringHertz+  , setSpringDampingRatio+  , getSpringHertz+  , getSpringDampingRatio+  , setMaxTorque+  , getMaxTorque+  )+  where++import Foreign+import Box3D.Internal (allocaOut)+import Box3D.Id (JointId (..), WorldId (..))+import Box3D.Joint (JointDef)++#include "box3d/box3d.h"+++{- | 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.+-}+data ParallelJointDef = ParallelJointDef+  { base :: JointDef+  -- ^ Base joint definition+  , hertz :: Float+  -- ^ The spring stiffness Hertz, cycles per second+  , dampingRatio :: Float+  -- ^ The spring damping ratio, non-dimensional+  , maxTorque :: Float+  -- ^ The maximum spring torque, typically in newton-meters.+  }+  deriving (Eq, Show)++instance Storable ParallelJointDef where+  sizeOf _ = #{size b3ParallelJointDef}+  alignment _ = #{alignment b3ParallelJointDef}+  peek p =+    ParallelJointDef+      <$> #{peek b3ParallelJointDef, base} p+      <*> #{peek b3ParallelJointDef, hertz} p+      <*> #{peek b3ParallelJointDef, dampingRatio} p+      <*> #{peek b3ParallelJointDef, maxTorque} p+  poke p (ParallelJointDef f1 f2 f3 f4) = do+    #{poke b3ParallelJointDef, base} p f1+    #{poke b3ParallelJointDef, hertz} p f2+    #{poke b3ParallelJointDef, dampingRatio} p f3+    #{poke b3ParallelJointDef, maxTorque} p f4++foreign import ccall unsafe "hs_b3DefaultParallelJointDef"+  c_b3DefaultParallelJointDef :: Ptr ParallelJointDef -> IO ()++defaultParallelJointDef :: IO ParallelJointDef+defaultParallelJointDef = allocaOut c_b3DefaultParallelJointDef+++foreign import ccall unsafe "b3CreateParallelJoint"+  c_b3CreateParallelJoint :: WorldId -> Ptr ParallelJointDef -> IO JointId++foreign import ccall unsafe "b3ParallelJoint_SetSpringHertz"+  c_b3ParallelJoint_SetSpringHertz :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3ParallelJoint_SetSpringDampingRatio"+  c_b3ParallelJoint_SetSpringDampingRatio :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3ParallelJoint_GetSpringHertz"+  c_b3ParallelJoint_GetSpringHertz :: JointId -> IO Float++foreign import ccall unsafe "b3ParallelJoint_GetSpringDampingRatio"+  c_b3ParallelJoint_GetSpringDampingRatio :: JointId -> IO Float++foreign import ccall unsafe "b3ParallelJoint_SetMaxTorque"+  c_b3ParallelJoint_SetMaxTorque :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3ParallelJoint_GetMaxTorque"+  c_b3ParallelJoint_GetMaxTorque :: JointId -> IO Float++{- | Create a parallel joint++See also @b3ParallelJointDef@ for details.++Binds @b3CreateParallelJoint@.+-}+create+  :: WorldId+  -> ParallelJointDef+  -> IO JointId+create a0 a1 =+  with a1 $ \p1 ->+  c_b3CreateParallelJoint a0 p1++{- | Set the spring stiffness in Hertz++Binds @b3ParallelJoint_SetSpringHertz@.+-}+setSpringHertz+  :: JointId+  -> Float+  -- ^ @hertz@+  -> IO ()+setSpringHertz a0 a1 =+  c_b3ParallelJoint_SetSpringHertz a0 a1++{- | Set the spring damping ratio, non-dimensional++Binds @b3ParallelJoint_SetSpringDampingRatio@.+-}+setSpringDampingRatio+  :: JointId+  -> Float+  -- ^ @dampingRatio@+  -> IO ()+setSpringDampingRatio a0 a1 =+  c_b3ParallelJoint_SetSpringDampingRatio a0 a1++{- | Get the spring Hertz++Binds @b3ParallelJoint_GetSpringHertz@.+-}+getSpringHertz+  :: JointId+  -> IO Float+getSpringHertz a0 =+  c_b3ParallelJoint_GetSpringHertz a0++{- | Get the spring damping ratio++Binds @b3ParallelJoint_GetSpringDampingRatio@.+-}+getSpringDampingRatio+  :: JointId+  -> IO Float+getSpringDampingRatio a0 =+  c_b3ParallelJoint_GetSpringDampingRatio a0++{- | Set the maximum spring torque, usually in newton-meters++Binds @b3ParallelJoint_SetMaxTorque@.+-}+setMaxTorque+  :: JointId+  -> Float+  -- ^ @force@+  -> IO ()+setMaxTorque a0 a1 =+  c_b3ParallelJoint_SetMaxTorque a0 a1++{- | Get the maximum spring torque, usually in newton-meters++Binds @b3ParallelJoint_GetMaxTorque@.+-}+getMaxTorque+  :: JointId+  -> IO Float+getMaxTorque a0 =+  c_b3ParallelJoint_GetMaxTorque a0
+ src/Box3D/PlatformMesh.hs view
@@ -0,0 +1,31 @@+-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.PlatformMesh+  ( create+  )+where++import Box3D.MathTypes (Vec3)+import Box3D.Mesh (MeshData)+import Foreign++foreign import ccall unsafe "hsg_b3CreatePlatformMesh"+  c_b3CreatePlatformMesh :: Ptr Vec3 -> Float -> Float -> Float -> IO (Ptr MeshData)++{- | Create a platform mesh. A truncated pyramid.++Binds @b3CreatePlatformMesh@.+-}+create+  :: Vec3+  -- ^ @center@+  -> Float+  -- ^ @height@+  -> Float+  -- ^ @topWidth@+  -> Float+  -- ^ @bottomWidth@+  -> IO (Ptr MeshData)+create a0 a1 a2 a3 =+  with a0 $ \p0 ->+    c_b3CreatePlatformMesh p0 a1 a2 a3
+ src/Box3D/Player.hs view
@@ -0,0 +1,44 @@+-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.Player+  ( create+  , destroy+  )+where++import Box3D.Tags (RecPlayer)+import Foreign+import Foreign.C.Types (CInt (..))++foreign import ccall unsafe "b3CreatePlayer"+  c_b3CreatePlayer :: Ptr () -> CInt -> CInt -> IO (Ptr RecPlayer)++foreign import ccall safe "b3DestroyPlayer"+  c_b3DestroyPlayer :: Ptr RecPlayer -> IO ()++{- | Create a player over a recording. Owns a private copy of the bytes.++Returns: a new player, or NULL on bad header or deserialization failure++Binds @b3CreatePlayer@.+-}+create+  :: Ptr ()+  -- ^ @data@: pointer to recording bytes+  -> Int+  -- ^ @size@: byte count of the recording+  -> Int+  -- ^ @workerCount@: worker count for the replay world; pass 1 to match a serial recording. Replaying at a different count re-partitions the constraint graph, so the StateHash check becomes a cross-thread determinism test. Adjustable later with @b3RecPlayer_SetWorkerCount@.+  -> IO (Ptr RecPlayer)+create a0 a1 a2 =+  c_b3CreatePlayer a0 (fromIntegral a1) (fromIntegral a2)++{- | Destroy the player and free all memory. Restores the previous global length scale.++Binds @b3DestroyPlayer@.+-}+destroy+  :: Ptr RecPlayer+  -> IO ()+destroy a0 =+  c_b3DestroyPlayer a0
+ src/Box3D/PrismaticJoint.hsc view
@@ -0,0 +1,435 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.PrismaticJoint+  ( PrismaticJointDef (..)+  , defaultPrismaticJointDef+  , create+  , enableSpring+  , isSpringEnabled+  , setSpringHertz+  , getSpringHertz+  , setSpringDampingRatio+  , getSpringDampingRatio+  , setTargetTranslation+  , getTargetTranslation+  , enableLimit+  , isLimitEnabled+  , getLowerLimit+  , getUpperLimit+  , setLimits+  , enableMotor+  , isMotorEnabled+  , setMotorSpeed+  , getMotorSpeed+  , setMaxMotorForce+  , getMaxMotorForce+  , getMotorForce+  , getTranslation+  , getSpeed+  )+  where++import Foreign+import Foreign.C.Types (CBool (..))+import Box3D.Internal (allocaOut)+import Box3D.Id (JointId (..), WorldId (..))+import Box3D.Joint (JointDef)++#include "box3d/box3d.h"+++{- | 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.+-}+data PrismaticJointDef = PrismaticJointDef+  { base :: JointDef+  -- ^ Base joint definition+  , enableSpring :: CBool+  -- ^ Enable a linear spring along the prismatic joint axis+  , hertz :: Float+  -- ^ The spring stiffness Hertz, cycles per second+  , dampingRatio :: Float+  -- ^ The spring damping ratio, non-dimensional+  , targetTranslation :: Float+  -- ^ The target translation for the joint in meters. The spring-damper will drive+  -- to this translation.+  , enableLimit :: CBool+  -- ^ Enable\/disable the joint limit+  , lowerTranslation :: Float+  -- ^ The lower translation limit+  , upperTranslation :: Float+  -- ^ The upper translation limit+  , enableMotor :: CBool+  -- ^ Enable\/disable the joint motor+  , maxMotorForce :: Float+  -- ^ The maximum motor force, typically in newtons+  , motorSpeed :: Float+  -- ^ The desired motor speed, typically in meters per second+  }+  deriving (Eq, Show)++instance Storable PrismaticJointDef where+  sizeOf _ = #{size b3PrismaticJointDef}+  alignment _ = #{alignment b3PrismaticJointDef}+  peek p =+    PrismaticJointDef+      <$> #{peek b3PrismaticJointDef, base} p+      <*> #{peek b3PrismaticJointDef, enableSpring} p+      <*> #{peek b3PrismaticJointDef, hertz} p+      <*> #{peek b3PrismaticJointDef, dampingRatio} p+      <*> #{peek b3PrismaticJointDef, targetTranslation} p+      <*> #{peek b3PrismaticJointDef, enableLimit} p+      <*> #{peek b3PrismaticJointDef, lowerTranslation} p+      <*> #{peek b3PrismaticJointDef, upperTranslation} p+      <*> #{peek b3PrismaticJointDef, enableMotor} p+      <*> #{peek b3PrismaticJointDef, maxMotorForce} p+      <*> #{peek b3PrismaticJointDef, motorSpeed} p+  poke p (PrismaticJointDef f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11) = do+    #{poke b3PrismaticJointDef, base} p f1+    #{poke b3PrismaticJointDef, enableSpring} p f2+    #{poke b3PrismaticJointDef, hertz} p f3+    #{poke b3PrismaticJointDef, dampingRatio} p f4+    #{poke b3PrismaticJointDef, targetTranslation} p f5+    #{poke b3PrismaticJointDef, enableLimit} p f6+    #{poke b3PrismaticJointDef, lowerTranslation} p f7+    #{poke b3PrismaticJointDef, upperTranslation} p f8+    #{poke b3PrismaticJointDef, enableMotor} p f9+    #{poke b3PrismaticJointDef, maxMotorForce} p f10+    #{poke b3PrismaticJointDef, motorSpeed} p f11++foreign import ccall unsafe "hs_b3DefaultPrismaticJointDef"+  c_b3DefaultPrismaticJointDef :: Ptr PrismaticJointDef -> IO ()++defaultPrismaticJointDef :: IO PrismaticJointDef+defaultPrismaticJointDef = allocaOut c_b3DefaultPrismaticJointDef+++foreign import ccall unsafe "b3CreatePrismaticJoint"+  c_b3CreatePrismaticJoint :: WorldId -> Ptr PrismaticJointDef -> IO JointId++foreign import ccall unsafe "b3PrismaticJoint_EnableSpring"+  c_b3PrismaticJoint_EnableSpring :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b3PrismaticJoint_IsSpringEnabled"+  c_b3PrismaticJoint_IsSpringEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b3PrismaticJoint_SetSpringHertz"+  c_b3PrismaticJoint_SetSpringHertz :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3PrismaticJoint_GetSpringHertz"+  c_b3PrismaticJoint_GetSpringHertz :: JointId -> IO Float++foreign import ccall unsafe "b3PrismaticJoint_SetSpringDampingRatio"+  c_b3PrismaticJoint_SetSpringDampingRatio :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3PrismaticJoint_GetSpringDampingRatio"+  c_b3PrismaticJoint_GetSpringDampingRatio :: JointId -> IO Float++foreign import ccall unsafe "b3PrismaticJoint_SetTargetTranslation"+  c_b3PrismaticJoint_SetTargetTranslation :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3PrismaticJoint_GetTargetTranslation"+  c_b3PrismaticJoint_GetTargetTranslation :: JointId -> IO Float++foreign import ccall unsafe "b3PrismaticJoint_EnableLimit"+  c_b3PrismaticJoint_EnableLimit :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b3PrismaticJoint_IsLimitEnabled"+  c_b3PrismaticJoint_IsLimitEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b3PrismaticJoint_GetLowerLimit"+  c_b3PrismaticJoint_GetLowerLimit :: JointId -> IO Float++foreign import ccall unsafe "b3PrismaticJoint_GetUpperLimit"+  c_b3PrismaticJoint_GetUpperLimit :: JointId -> IO Float++foreign import ccall unsafe "b3PrismaticJoint_SetLimits"+  c_b3PrismaticJoint_SetLimits :: JointId -> Float -> Float -> IO ()++foreign import ccall unsafe "b3PrismaticJoint_EnableMotor"+  c_b3PrismaticJoint_EnableMotor :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b3PrismaticJoint_IsMotorEnabled"+  c_b3PrismaticJoint_IsMotorEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b3PrismaticJoint_SetMotorSpeed"+  c_b3PrismaticJoint_SetMotorSpeed :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3PrismaticJoint_GetMotorSpeed"+  c_b3PrismaticJoint_GetMotorSpeed :: JointId -> IO Float++foreign import ccall unsafe "b3PrismaticJoint_SetMaxMotorForce"+  c_b3PrismaticJoint_SetMaxMotorForce :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3PrismaticJoint_GetMaxMotorForce"+  c_b3PrismaticJoint_GetMaxMotorForce :: JointId -> IO Float++foreign import ccall unsafe "b3PrismaticJoint_GetMotorForce"+  c_b3PrismaticJoint_GetMotorForce :: JointId -> IO Float++foreign import ccall unsafe "b3PrismaticJoint_GetTranslation"+  c_b3PrismaticJoint_GetTranslation :: JointId -> IO Float++foreign import ccall unsafe "b3PrismaticJoint_GetSpeed"+  c_b3PrismaticJoint_GetSpeed :: JointId -> IO Float++{- | Create a prismatic (slider) joint.++See also @b3PrismaticJointDef@ for details.++Binds @b3CreatePrismaticJoint@.+-}+create+  :: WorldId+  -> PrismaticJointDef+  -> IO JointId+create a0 a1 =+  with a1 $ \p1 ->+  c_b3CreatePrismaticJoint a0 p1++{- | Enable\/disable the joint spring.++Binds @b3PrismaticJoint_EnableSpring@.+-}+enableSpring+  :: JointId+  -> Bool+  -- ^ @enableSpring@+  -> IO ()+enableSpring a0 a1 =+  c_b3PrismaticJoint_EnableSpring a0 (fromBool a1)++{- | Is the prismatic joint spring enabled or not?++Binds @b3PrismaticJoint_IsSpringEnabled@.+-}+isSpringEnabled+  :: JointId+  -> IO Bool+isSpringEnabled a0 =+  toBool <$> (c_b3PrismaticJoint_IsSpringEnabled a0)++{- | 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.++Binds @b3PrismaticJoint_SetSpringHertz@.+-}+setSpringHertz+  :: JointId+  -> Float+  -- ^ @hertz@+  -> IO ()+setSpringHertz a0 a1 =+  c_b3PrismaticJoint_SetSpringHertz a0 a1++{- | Get the prismatic joint stiffness in Hertz++Binds @b3PrismaticJoint_GetSpringHertz@.+-}+getSpringHertz+  :: JointId+  -> IO Float+getSpringHertz a0 =+  c_b3PrismaticJoint_GetSpringHertz a0++{- | Set the prismatic joint damping ratio (non-dimensional)++Binds @b3PrismaticJoint_SetSpringDampingRatio@.+-}+setSpringDampingRatio+  :: JointId+  -> Float+  -- ^ @dampingRatio@+  -> IO ()+setSpringDampingRatio a0 a1 =+  c_b3PrismaticJoint_SetSpringDampingRatio a0 a1++{- | Get the prismatic spring damping ratio (non-dimensional)++Binds @b3PrismaticJoint_GetSpringDampingRatio@.+-}+getSpringDampingRatio+  :: JointId+  -> IO Float+getSpringDampingRatio a0 =+  c_b3PrismaticJoint_GetSpringDampingRatio a0++{- | Set the prismatic joint target translation. Usually in meters.++Binds @b3PrismaticJoint_SetTargetTranslation@.+-}+setTargetTranslation+  :: JointId+  -> Float+  -- ^ @targetTranslation@+  -> IO ()+setTargetTranslation a0 a1 =+  c_b3PrismaticJoint_SetTargetTranslation a0 a1++{- | Get the prismatic joint target translation. Usually in meters.++Binds @b3PrismaticJoint_GetTargetTranslation@.+-}+getTargetTranslation+  :: JointId+  -> IO Float+getTargetTranslation a0 =+  c_b3PrismaticJoint_GetTargetTranslation a0++{- | Enable\/disable a prismatic joint limit++Binds @b3PrismaticJoint_EnableLimit@.+-}+enableLimit+  :: JointId+  -> Bool+  -- ^ @enableLimit@+  -> IO ()+enableLimit a0 a1 =+  c_b3PrismaticJoint_EnableLimit a0 (fromBool a1)++{- | Is the prismatic joint limit enabled?++Binds @b3PrismaticJoint_IsLimitEnabled@.+-}+isLimitEnabled+  :: JointId+  -> IO Bool+isLimitEnabled a0 =+  toBool <$> (c_b3PrismaticJoint_IsLimitEnabled a0)++{- | Get the prismatic joint lower limit++Binds @b3PrismaticJoint_GetLowerLimit@.+-}+getLowerLimit+  :: JointId+  -> IO Float+getLowerLimit a0 =+  c_b3PrismaticJoint_GetLowerLimit a0++{- | Get the prismatic joint upper limit++Binds @b3PrismaticJoint_GetUpperLimit@.+-}+getUpperLimit+  :: JointId+  -> IO Float+getUpperLimit a0 =+  c_b3PrismaticJoint_GetUpperLimit a0++{- | Set the prismatic joint limits++Binds @b3PrismaticJoint_SetLimits@.+-}+setLimits+  :: JointId+  -> Float+  -- ^ @lower@+  -> Float+  -- ^ @upper@+  -> IO ()+setLimits a0 a1 a2 =+  c_b3PrismaticJoint_SetLimits a0 a1 a2++{- | Enable\/disable a prismatic joint motor++Binds @b3PrismaticJoint_EnableMotor@.+-}+enableMotor+  :: JointId+  -> Bool+  -- ^ @enableMotor@+  -> IO ()+enableMotor a0 a1 =+  c_b3PrismaticJoint_EnableMotor a0 (fromBool a1)++{- | Is the prismatic joint motor enabled?++Binds @b3PrismaticJoint_IsMotorEnabled@.+-}+isMotorEnabled+  :: JointId+  -> IO Bool+isMotorEnabled a0 =+  toBool <$> (c_b3PrismaticJoint_IsMotorEnabled a0)++{- | Set the prismatic joint motor speed, usually in meters per second++Binds @b3PrismaticJoint_SetMotorSpeed@.+-}+setMotorSpeed+  :: JointId+  -> Float+  -- ^ @motorSpeed@+  -> IO ()+setMotorSpeed a0 a1 =+  c_b3PrismaticJoint_SetMotorSpeed a0 a1++{- | Get the prismatic joint motor speed, usually in meters per second++Binds @b3PrismaticJoint_GetMotorSpeed@.+-}+getMotorSpeed+  :: JointId+  -> IO Float+getMotorSpeed a0 =+  c_b3PrismaticJoint_GetMotorSpeed a0++{- | Set the prismatic joint maximum motor force, usually in newtons++Binds @b3PrismaticJoint_SetMaxMotorForce@.+-}+setMaxMotorForce+  :: JointId+  -> Float+  -- ^ @force@+  -> IO ()+setMaxMotorForce a0 a1 =+  c_b3PrismaticJoint_SetMaxMotorForce a0 a1++{- | Get the prismatic joint maximum motor force, usually in newtons++Binds @b3PrismaticJoint_GetMaxMotorForce@.+-}+getMaxMotorForce+  :: JointId+  -> IO Float+getMaxMotorForce a0 =+  c_b3PrismaticJoint_GetMaxMotorForce a0++{- | Get the prismatic joint current motor force, usually in newtons++Binds @b3PrismaticJoint_GetMotorForce@.+-}+getMotorForce+  :: JointId+  -> IO Float+getMotorForce a0 =+  c_b3PrismaticJoint_GetMotorForce a0++{- | Get the current joint translation, usually in meters.++Binds @b3PrismaticJoint_GetTranslation@.+-}+getTranslation+  :: JointId+  -> IO Float+getTranslation a0 =+  c_b3PrismaticJoint_GetTranslation a0++{- | Get the current joint translation speed, usually in meters per second.++Binds @b3PrismaticJoint_GetSpeed@.+-}+getSpeed+  :: JointId+  -> IO Float+getSpeed a0 =+  c_b3PrismaticJoint_GetSpeed a0
+ src/Box3D/Query.hsc view
@@ -0,0 +1,685 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.Query+  ( TOIOutput (..)+  , TOIState (..)+  , pattern ToiStateUnknown+  , pattern ToiStateFailed+  , pattern ToiStateOverlapped+  , pattern ToiStateHit+  , pattern ToiStateSeparated+  , TOIInput (..)+  , Sweep (..)+  , SimplexVertex (..)+  , DistanceOutput (..)+  , DistanceInput (..)+  , ShapeCastPairInput (..)+  , SimplexCache (..)+  , BodyCastResult (..)+  , WorldCastOutput (..)+  , CastOutput (..)+  , BoxCastInput (..)+  , ShapeCastInput (..)+  , ShapeProxy (..)+  , RayResult (..)+  , RayCastInput (..)+  , QueryFilter (..)+  , defaultQueryFilter+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..))+import Foreign.C.String (CString)+import Data.Vector.Storable qualified as VS+import Box3D.Internal (allocaOut, copyBounded, pokeBounded)+import Box3D.Id (ShapeId)+import Box3D.MathTypes (AABB, Pos, Quat, Transform, Vec3)++#include "box3d/box3d.h"+++-- | Time of impact output+data TOIOutput = TOIOutput+  { state :: CInt+  -- ^ The type of result+  , point :: Vec3+  -- ^ The hit point+  , normal :: Vec3+  -- ^ The hit normal+  , fraction :: Float+  -- ^ The sweep time of the collision+  , distance :: Float+  -- ^ The final distance+  , distanceIterations :: CInt+  -- ^ Number of outer iterations+  , pushBackIterations :: CInt+  -- ^ Total number of push back iterations+  , rootIterations :: CInt+  -- ^ Total number of root iterations+  , usedFallback :: CBool+  -- ^ Indicates that the time of impact detected initial+  -- overlap and used a fallback sphere as a last ditch effort+  -- to prevent tunneling.+  }+  deriving (Eq, Show)++instance Storable TOIOutput where+  sizeOf _ = #{size b3TOIOutput}+  alignment _ = #{alignment b3TOIOutput}+  peek p =+    TOIOutput+      <$> #{peek b3TOIOutput, state} p+      <*> #{peek b3TOIOutput, point} p+      <*> #{peek b3TOIOutput, normal} p+      <*> #{peek b3TOIOutput, fraction} p+      <*> #{peek b3TOIOutput, distance} p+      <*> #{peek b3TOIOutput, distanceIterations} p+      <*> #{peek b3TOIOutput, pushBackIterations} p+      <*> #{peek b3TOIOutput, rootIterations} p+      <*> #{peek b3TOIOutput, usedFallback} p+  poke p (TOIOutput f1 f2 f3 f4 f5 f6 f7 f8 f9) = do+    #{poke b3TOIOutput, state} p f1+    #{poke b3TOIOutput, point} p f2+    #{poke b3TOIOutput, normal} p f3+    #{poke b3TOIOutput, fraction} p f4+    #{poke b3TOIOutput, distance} p f5+    #{poke b3TOIOutput, distanceIterations} p f6+    #{poke b3TOIOutput, pushBackIterations} p f7+    #{poke b3TOIOutput, rootIterations} p f8+    #{poke b3TOIOutput, usedFallback} p f9++-- | Describes the TOI output+newtype TOIState = TOIState CInt+  deriving (Eq, Show, Storable)++pattern ToiStateUnknown :: TOIState+pattern ToiStateFailed :: TOIState+pattern ToiStateOverlapped :: TOIState+pattern ToiStateHit :: TOIState+pattern ToiStateSeparated :: TOIState+pattern ToiStateUnknown = TOIState #{const b3_toiStateUnknown}+pattern ToiStateFailed = TOIState #{const b3_toiStateFailed}+pattern ToiStateOverlapped = TOIState #{const b3_toiStateOverlapped}+pattern ToiStateHit = TOIState #{const b3_toiStateHit}+pattern ToiStateSeparated = TOIState #{const b3_toiStateSeparated}++-- | Time of impact input+data TOIInput = TOIInput+  { proxyA :: ShapeProxy+  -- ^ The proxy for shape A+  , proxyB :: ShapeProxy+  -- ^ The proxy for shape B+  , sweepA :: Sweep+  -- ^ The movement of shape A+  , sweepB :: Sweep+  -- ^ The movement of shape B+  , maxFraction :: Float+  -- ^ Defines the sweep interval \[0, tMax\]+  }+  deriving (Eq, Show)++instance Storable TOIInput where+  sizeOf _ = #{size b3TOIInput}+  alignment _ = #{alignment b3TOIInput}+  peek p =+    TOIInput+      <$> #{peek b3TOIInput, proxyA} p+      <*> #{peek b3TOIInput, proxyB} p+      <*> #{peek b3TOIInput, sweepA} p+      <*> #{peek b3TOIInput, sweepB} p+      <*> #{peek b3TOIInput, maxFraction} p+  poke p (TOIInput f1 f2 f3 f4 f5) = do+    #{poke b3TOIInput, proxyA} p f1+    #{poke b3TOIInput, proxyB} p f2+    #{poke b3TOIInput, sweepA} p f3+    #{poke b3TOIInput, sweepB} p f4+    #{poke b3TOIInput, maxFraction} p f5++{- | 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.+-}+data Sweep = Sweep+  { localCenter :: Vec3+  -- ^ Local center of mass position+  , c1 :: Vec3+  -- ^ Starting center of mass world position+  , c2 :: Vec3+  -- ^ Ending center of mass world position+  , q1 :: Quat+  -- ^ Starting world rotation+  , q2 :: Quat+  -- ^ Ending world rotation+  }+  deriving (Eq, Show)++instance Storable Sweep where+  sizeOf _ = #{size b3Sweep}+  alignment _ = #{alignment b3Sweep}+  peek p =+    Sweep+      <$> #{peek b3Sweep, localCenter} p+      <*> #{peek b3Sweep, c1} p+      <*> #{peek b3Sweep, c2} p+      <*> #{peek b3Sweep, q1} p+      <*> #{peek b3Sweep, q2} p+  poke p (Sweep f1 f2 f3 f4 f5) = do+    #{poke b3Sweep, localCenter} p f1+    #{poke b3Sweep, c1} p f2+    #{poke b3Sweep, c2} p f3+    #{poke b3Sweep, q1} p f4+    #{poke b3Sweep, q2} p f5++-- | Simplex vertex for debugging the GJK algorithm+data SimplexVertex = SimplexVertex+  { wA :: Vec3+  -- ^ support point in proxyA+  , wB :: Vec3+  -- ^ support point in proxyB+  , w :: Vec3+  -- ^ wB - wA+  , a :: Float+  -- ^ barycentric coordinates+  , indexA :: CInt+  -- ^ wA index+  , indexB :: CInt+  -- ^ wB index+  }+  deriving (Eq, Show)++instance Storable SimplexVertex where+  sizeOf _ = #{size b3SimplexVertex}+  alignment _ = #{alignment b3SimplexVertex}+  peek p =+    SimplexVertex+      <$> #{peek b3SimplexVertex, wA} p+      <*> #{peek b3SimplexVertex, wB} p+      <*> #{peek b3SimplexVertex, w} p+      <*> #{peek b3SimplexVertex, a} p+      <*> #{peek b3SimplexVertex, indexA} p+      <*> #{peek b3SimplexVertex, indexB} p+  poke p (SimplexVertex f1 f2 f3 f4 f5 f6) = do+    #{poke b3SimplexVertex, wA} p f1+    #{poke b3SimplexVertex, wB} p f2+    #{poke b3SimplexVertex, w} p f3+    #{poke b3SimplexVertex, a} p f4+    #{poke b3SimplexVertex, indexA} p f5+    #{poke b3SimplexVertex, indexB} p f6++-- | Output for @b3ShapeDistance@+data DistanceOutput = DistanceOutput+  { pointA :: Vec3+  -- ^ Closest point on shapeA, in shape A\'s frame+  , pointB :: Vec3+  -- ^ Closest point on shapeB, in shape A\'s frame+  , normal :: Vec3+  -- ^ A to B normal in shape A\'s frame. Invalid if distance is zero.+  , distance :: Float+  -- ^ The final distance, zero if overlapped+  , iterations :: CInt+  -- ^ Number of GJK iterations used+  , simplexCount :: CInt+  -- ^ The number of simplexes stored in the simplex array+  }+  deriving (Eq, Show)++instance Storable DistanceOutput where+  sizeOf _ = #{size b3DistanceOutput}+  alignment _ = #{alignment b3DistanceOutput}+  peek p =+    DistanceOutput+      <$> #{peek b3DistanceOutput, pointA} p+      <*> #{peek b3DistanceOutput, pointB} p+      <*> #{peek b3DistanceOutput, normal} p+      <*> #{peek b3DistanceOutput, distance} p+      <*> #{peek b3DistanceOutput, iterations} p+      <*> #{peek b3DistanceOutput, simplexCount} p+  poke p (DistanceOutput f1 f2 f3 f4 f5 f6) = do+    #{poke b3DistanceOutput, pointA} p f1+    #{poke b3DistanceOutput, pointB} p f2+    #{poke b3DistanceOutput, normal} p f3+    #{poke b3DistanceOutput, distance} p f4+    #{poke b3DistanceOutput, iterations} p f5+    #{poke b3DistanceOutput, simplexCount} p f6++-- | Input for @b3ShapeDistance@+data DistanceInput = DistanceInput+  { proxyA :: ShapeProxy+  -- ^ The proxy for shape A+  , proxyB :: ShapeProxy+  -- ^ The proxy for shape B+  , transform :: Transform+  -- ^ Transform of shape B in shape A\'s frame, the relative pose B in A+  -- (b3InvMulWorldTransforms( worldA, worldB )). The query is origin independent and runs in frame A.+  , useRadii :: CBool+  -- ^ Should the proxy radius be considered?+  }+  deriving (Eq, Show)++instance Storable DistanceInput where+  sizeOf _ = #{size b3DistanceInput}+  alignment _ = #{alignment b3DistanceInput}+  peek p =+    DistanceInput+      <$> #{peek b3DistanceInput, proxyA} p+      <*> #{peek b3DistanceInput, proxyB} p+      <*> #{peek b3DistanceInput, transform} p+      <*> #{peek b3DistanceInput, useRadii} p+  poke p (DistanceInput f1 f2 f3 f4) = do+    #{poke b3DistanceInput, proxyA} p f1+    #{poke b3DistanceInput, proxyB} p f2+    #{poke b3DistanceInput, transform} p f3+    #{poke b3DistanceInput, useRadii} p f4++-- | Input parameters for @b3ShapeCast@+data ShapeCastPairInput = ShapeCastPairInput+  { proxyA :: ShapeProxy+  -- ^ The proxy for shape A+  , proxyB :: ShapeProxy+  -- ^ The proxy for shape B+  , transform :: Transform+  -- ^ Transform of shape B in shape A\'s frame, the relative pose B in A+  , translationB :: Vec3+  -- ^ The translation of shape B, in A\'s frame+  , maxFraction :: Float+  -- ^ The fraction of the translation to consider, typically 1+  , canEncroach :: CBool+  -- ^ Allows shapes with a radius to move slightly closer if already touching+  }+  deriving (Eq, Show)++instance Storable ShapeCastPairInput where+  sizeOf _ = #{size b3ShapeCastPairInput}+  alignment _ = #{alignment b3ShapeCastPairInput}+  peek p =+    ShapeCastPairInput+      <$> #{peek b3ShapeCastPairInput, proxyA} p+      <*> #{peek b3ShapeCastPairInput, proxyB} p+      <*> #{peek b3ShapeCastPairInput, transform} p+      <*> #{peek b3ShapeCastPairInput, translationB} p+      <*> #{peek b3ShapeCastPairInput, maxFraction} p+      <*> #{peek b3ShapeCastPairInput, canEncroach} p+  poke p (ShapeCastPairInput f1 f2 f3 f4 f5 f6) = do+    #{poke b3ShapeCastPairInput, proxyA} p f1+    #{poke b3ShapeCastPairInput, proxyB} p f2+    #{poke b3ShapeCastPairInput, transform} p f3+    #{poke b3ShapeCastPairInput, translationB} p f4+    #{poke b3ShapeCastPairInput, maxFraction} p f5+    #{poke b3ShapeCastPairInput, canEncroach} p f6++{- | 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.+-}+data SimplexCache = SimplexCache+  { metric :: Float+  -- ^ Value use to compare length, area, volume of two simplexes.+  , indexA :: VS.Vector Word8+  -- ^ The cached simplex indices on shape A+  , indexB :: VS.Vector Word8+  -- ^ The cached simplex indices on shape B+  }+  deriving (Eq, Show)++instance Storable SimplexCache where+  sizeOf _ = #{size b3SimplexCache}+  alignment _ = #{alignment b3SimplexCache}+  peek p = do+    nCount <- #{peek b3SimplexCache, count} p :: IO Word16+    SimplexCache+      <$> #{peek b3SimplexCache, metric} p+      <*> copyBounded (#{const 4}) nCount (#{ptr b3SimplexCache, indexA} p)+      <*> copyBounded (#{const 4}) nCount (#{ptr b3SimplexCache, indexB} p)+  poke p x = do+    #{poke b3SimplexCache, metric} p x.metric+    pokeBounded (#{const 4}) (#{ptr b3SimplexCache, indexA} p) x.indexA+    pokeBounded (#{const 4}) (#{ptr b3SimplexCache, indexB} p) x.indexB+    #{poke b3SimplexCache, count} p (fromIntegral (minimum [(#{const 4}), VS.length x.indexA, VS.length x.indexB]) :: Word16)++-- | Body cast result for ray and shape casts.+data BodyCastResult = BodyCastResult+  { shapeId :: ShapeId+  -- ^ The shape hit.+  , point :: Pos+  -- ^ The world point on the shape surface.+  , normal :: Vec3+  -- ^ The world normal vector on the shape surface.+  , fraction :: Float+  -- ^ The fraction along the ray hit.+  -- hit point = origin + fraction * translation+  , triangleIndex :: CInt+  -- ^ The triangle index if the shape is a mesh or height-field.+  , userMaterialId :: Word64+  -- ^ The user material id at the hit point. This can be per triangle+  -- if the shape is a mesh, height-field, or compound with child mesh.+  , iterations :: CInt+  -- ^ The number of iterations used. Diagnostic.+  , hit :: CBool+  -- ^ Did the cast hit? If false, all other fields are invalid.+  }+  deriving (Eq, Show)++instance Storable BodyCastResult where+  sizeOf _ = #{size b3BodyCastResult}+  alignment _ = #{alignment b3BodyCastResult}+  peek p =+    BodyCastResult+      <$> #{peek b3BodyCastResult, shapeId} p+      <*> #{peek b3BodyCastResult, point} p+      <*> #{peek b3BodyCastResult, normal} p+      <*> #{peek b3BodyCastResult, fraction} p+      <*> #{peek b3BodyCastResult, triangleIndex} p+      <*> #{peek b3BodyCastResult, userMaterialId} p+      <*> #{peek b3BodyCastResult, iterations} p+      <*> #{peek b3BodyCastResult, hit} p+  poke p (BodyCastResult f1 f2 f3 f4 f5 f6 f7 f8) = do+    #{poke b3BodyCastResult, shapeId} p f1+    #{poke b3BodyCastResult, point} p f2+    #{poke b3BodyCastResult, normal} p f3+    #{poke b3BodyCastResult, fraction} p f4+    #{poke b3BodyCastResult, triangleIndex} p f5+    #{poke b3BodyCastResult, userMaterialId} p f6+    #{poke b3BodyCastResult, iterations} p f7+    #{poke b3BodyCastResult, hit} p f8++{- | Ray cast or shape-cast output in world space. The hit point is a world position so the result+stays precise far from the world origin. Mirrors @b3CastOutput@ with a double precision point.+-}+data WorldCastOutput = WorldCastOutput+  { normal :: Vec3+  -- ^ The surface normal at the hit point.+  , point :: Pos+  -- ^ The surface hit point in world space.+  , fraction :: Float+  -- ^ The fraction of the input translation at collision.+  , iterations :: CInt+  -- ^ The number of iterations used.+  , triangleIndex :: CInt+  -- ^ The index of the mesh or height field triangle hit.+  , childIndex :: CInt+  -- ^ The index of the compound child shape.+  , materialIndex :: CInt+  -- ^ The material index. May be -1 for null.+  , hit :: CBool+  -- ^ Did the cast hit?+  }+  deriving (Eq, Show)++instance Storable WorldCastOutput where+  sizeOf _ = #{size b3WorldCastOutput}+  alignment _ = #{alignment b3WorldCastOutput}+  peek p =+    WorldCastOutput+      <$> #{peek b3WorldCastOutput, normal} p+      <*> #{peek b3WorldCastOutput, point} p+      <*> #{peek b3WorldCastOutput, fraction} p+      <*> #{peek b3WorldCastOutput, iterations} p+      <*> #{peek b3WorldCastOutput, triangleIndex} p+      <*> #{peek b3WorldCastOutput, childIndex} p+      <*> #{peek b3WorldCastOutput, materialIndex} p+      <*> #{peek b3WorldCastOutput, hit} p+  poke p (WorldCastOutput f1 f2 f3 f4 f5 f6 f7 f8) = do+    #{poke b3WorldCastOutput, normal} p f1+    #{poke b3WorldCastOutput, point} p f2+    #{poke b3WorldCastOutput, fraction} p f3+    #{poke b3WorldCastOutput, iterations} p f4+    #{poke b3WorldCastOutput, triangleIndex} p f5+    #{poke b3WorldCastOutput, childIndex} p f6+    #{poke b3WorldCastOutput, materialIndex} p f7+    #{poke b3WorldCastOutput, hit} p f8++-- | Low level ray cast or shape-cast output data.+data CastOutput = CastOutput+  { normal :: Vec3+  -- ^ The surface normal at the hit point.+  , point :: Vec3+  -- ^ The surface hit point.+  , fraction :: Float+  -- ^ The fraction of the input translation at collision.+  , iterations :: CInt+  -- ^ The number of iterations used.+  , triangleIndex :: CInt+  -- ^ The index of the mesh or height field triangle hit.+  , childIndex :: CInt+  -- ^ The index of the compound child shape.+  , materialIndex :: CInt+  -- ^ The material index. May be -1 for null.+  , hit :: CBool+  -- ^ Did the cast hit?+  }+  deriving (Eq, Show)++instance Storable CastOutput where+  sizeOf _ = #{size b3CastOutput}+  alignment _ = #{alignment b3CastOutput}+  peek p =+    CastOutput+      <$> #{peek b3CastOutput, normal} p+      <*> #{peek b3CastOutput, point} p+      <*> #{peek b3CastOutput, fraction} p+      <*> #{peek b3CastOutput, iterations} p+      <*> #{peek b3CastOutput, triangleIndex} p+      <*> #{peek b3CastOutput, childIndex} p+      <*> #{peek b3CastOutput, materialIndex} p+      <*> #{peek b3CastOutput, hit} p+  poke p (CastOutput f1 f2 f3 f4 f5 f6 f7 f8) = do+    #{poke b3CastOutput, normal} p f1+    #{poke b3CastOutput, point} p f2+    #{poke b3CastOutput, fraction} p f3+    #{poke b3CastOutput, iterations} p f4+    #{poke b3CastOutput, triangleIndex} p f5+    #{poke b3CastOutput, childIndex} p f6+    #{poke b3CastOutput, materialIndex} p f7+    #{poke b3CastOutput, hit} p f8++{- | Input for sweeping an AABB through a dynamic tree. The box is in the tree\'s world float frame.+The caller folds the cast shape radius and any world origin into the box, so the tree traversal+stays a conservative box sweep and the precise narrow phase happens per shape in the callback.+-}+data BoxCastInput = BoxCastInput+  { box :: AABB+  -- ^ The AABB to cast, in the tree\'s frame.+  , translation :: Vec3+  -- ^ The sweep translation.+  , maxFraction :: Float+  -- ^ The maximum fraction of the translation to consider, typically 1.+  }+  deriving (Eq, Show)++instance Storable BoxCastInput where+  sizeOf _ = #{size b3BoxCastInput}+  alignment _ = #{alignment b3BoxCastInput}+  peek p =+    BoxCastInput+      <$> #{peek b3BoxCastInput, box} p+      <*> #{peek b3BoxCastInput, translation} p+      <*> #{peek b3BoxCastInput, maxFraction} p+  poke p (BoxCastInput f1 f2 f3) = do+    #{poke b3BoxCastInput, box} p f1+    #{poke b3BoxCastInput, translation} p f2+    #{poke b3BoxCastInput, maxFraction} p f3++{- | 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.+-}+data ShapeCastInput = ShapeCastInput+  { proxy :: ShapeProxy+  -- ^ A generic query shape.+  , translation :: Vec3+  -- ^ The translation of the shape cast.+  , maxFraction :: Float+  -- ^ The maximum fraction of the translation to consider, typically 1.+  , canEncroach :: CBool+  -- ^ Allow shape cast to encroach when initially touching. This only works if the radius is greater than zero.+  }+  deriving (Eq, Show)++instance Storable ShapeCastInput where+  sizeOf _ = #{size b3ShapeCastInput}+  alignment _ = #{alignment b3ShapeCastInput}+  peek p =+    ShapeCastInput+      <$> #{peek b3ShapeCastInput, proxy} p+      <*> #{peek b3ShapeCastInput, translation} p+      <*> #{peek b3ShapeCastInput, maxFraction} p+      <*> #{peek b3ShapeCastInput, canEncroach} p+  poke p (ShapeCastInput f1 f2 f3 f4) = do+    #{poke b3ShapeCastInput, proxy} p f1+    #{poke b3ShapeCastInput, translation} p f2+    #{poke b3ShapeCastInput, maxFraction} p f3+    #{poke b3ShapeCastInput, canEncroach} p f4++-- | A shape proxy is used by the GJK algorithm. It can represent a convex shape.+data ShapeProxy = ShapeProxy+  { points :: Ptr Vec3+  -- ^ The point cloud.+  , count :: CInt+  -- ^ The number of points. Do not exceed @B3_MAX_SHAPE_CAST_POINTS@.+  , radius :: Float+  -- ^ The external radius of the point cloud.+  }+  deriving (Eq, Show)++instance Storable ShapeProxy where+  sizeOf _ = #{size b3ShapeProxy}+  alignment _ = #{alignment b3ShapeProxy}+  peek p =+    ShapeProxy+      <$> #{peek b3ShapeProxy, points} p+      <*> #{peek b3ShapeProxy, count} p+      <*> #{peek b3ShapeProxy, radius} p+  poke p (ShapeProxy f1 f2 f3) = do+    #{poke b3ShapeProxy, points} p f1+    #{poke b3ShapeProxy, count} p f2+    #{poke b3ShapeProxy, radius} p f3++-- | Result from @b3World_RayCastClosest@.+data RayResult = RayResult+  { shapeId :: ShapeId+  -- ^ The shape hit.+  , point :: Pos+  -- ^ The world point of the hit.+  , normal :: Vec3+  -- ^ The world normal of the shape surface at the hit point.+  , userMaterialId :: Word64+  -- ^ The user material id at the hit point. This can be per triangle+  -- if the shape is a mesh, height-field, or compound with child mesh.+  , fraction :: Float+  -- ^ The fraction of the input ray.+  , triangleIndex :: CInt+  -- ^ The triangle index if the shape is a mesh, height-field, or compound with+  -- child mesh.+  , childIndex :: CInt+  -- ^ The child index if the shape is a compound.+  , nodeVisits :: CInt+  -- ^ The number of BVH nodes visited. Diagnostic.+  , leafVisits :: CInt+  -- ^ The number of BVH leaves visited. Diagnostic.+  , hit :: CBool+  -- ^ Did the ray hit? If false, all other data is invalid.+  }+  deriving (Eq, Show)++instance Storable RayResult where+  sizeOf _ = #{size b3RayResult}+  alignment _ = #{alignment b3RayResult}+  peek p =+    RayResult+      <$> #{peek b3RayResult, shapeId} p+      <*> #{peek b3RayResult, point} p+      <*> #{peek b3RayResult, normal} p+      <*> #{peek b3RayResult, userMaterialId} p+      <*> #{peek b3RayResult, fraction} p+      <*> #{peek b3RayResult, triangleIndex} p+      <*> #{peek b3RayResult, childIndex} p+      <*> #{peek b3RayResult, nodeVisits} p+      <*> #{peek b3RayResult, leafVisits} p+      <*> #{peek b3RayResult, hit} p+  poke p (RayResult f1 f2 f3 f4 f5 f6 f7 f8 f9 f10) = do+    #{poke b3RayResult, shapeId} p f1+    #{poke b3RayResult, point} p f2+    #{poke b3RayResult, normal} p f3+    #{poke b3RayResult, userMaterialId} p f4+    #{poke b3RayResult, fraction} p f5+    #{poke b3RayResult, triangleIndex} p f6+    #{poke b3RayResult, childIndex} p f7+    #{poke b3RayResult, nodeVisits} p f8+    #{poke b3RayResult, leafVisits} p f9+    #{poke b3RayResult, hit} p f10++-- | Low level ray cast input data.+data RayCastInput = RayCastInput+  { origin :: Vec3+  -- ^ Start point of the ray cast.+  , translation :: Vec3+  -- ^ Translation of the ray cast.+  -- end = start + translation.+  , maxFraction :: Float+  -- ^ The maximum fraction of the translation to consider, typically 1+  }+  deriving (Eq, Show)++instance Storable RayCastInput where+  sizeOf _ = #{size b3RayCastInput}+  alignment _ = #{alignment b3RayCastInput}+  peek p =+    RayCastInput+      <$> #{peek b3RayCastInput, origin} p+      <*> #{peek b3RayCastInput, translation} p+      <*> #{peek b3RayCastInput, maxFraction} p+  poke p (RayCastInput f1 f2 f3) = do+    #{poke b3RayCastInput, origin} p f1+    #{poke b3RayCastInput, translation} p f2+    #{poke b3RayCastInput, maxFraction} p f3++{- | 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.+-}+data QueryFilter = QueryFilter+  { categoryBits :: Word64+  -- ^ The collision category bits of this query. Normally you would just set one bit.+  , maskBits :: Word64+  -- ^ The collision mask bits. This states the shape categories that this+  -- query would accept for collision.+  , id :: Word64+  -- ^ Optional id combined with \@ref name to identify this query in a recording, e.g. an entity id.+  -- Need not be unique on its own. 0 with a null name means untagged. Ignored when not recording.+  , name :: CString+  -- ^ Optional label combined with \@ref id to identify this query, e.g. \"bullet\". Need not be unique+  -- on its own. The recorder hashes (id, name) into one stable key the viewer tracks the query by,+  -- so the same id and name pair identifies the same query across frames. NULL means none. Ignored+  -- when not recording.+  }+  deriving (Eq, Show)++instance Storable QueryFilter where+  sizeOf _ = #{size b3QueryFilter}+  alignment _ = #{alignment b3QueryFilter}+  peek p =+    QueryFilter+      <$> #{peek b3QueryFilter, categoryBits} p+      <*> #{peek b3QueryFilter, maskBits} p+      <*> #{peek b3QueryFilter, id} p+      <*> #{peek b3QueryFilter, name} p+  poke p (QueryFilter f1 f2 f3 f4) = do+    #{poke b3QueryFilter, categoryBits} p f1+    #{poke b3QueryFilter, maskBits} p f2+    #{poke b3QueryFilter, id} p f3+    #{poke b3QueryFilter, name} p f4++foreign import ccall unsafe "hs_b3DefaultQueryFilter"+  c_b3DefaultQueryFilter :: Ptr QueryFilter -> IO ()++defaultQueryFilter :: IO QueryFilter+defaultQueryFilter = allocaOut c_b3DefaultQueryFilter+++
+ src/Box3D/RecPlayer.hs view
@@ -0,0 +1,415 @@+-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.RecPlayer+  ( stepFrame+  , subStepFrame+  , restart+  , seekFrame+  , getWorldId+  , getFrame+  , getFrameCount+  , isAtEnd+  , isAtPreStep+  , hasDiverged+  , getInfo+  , getDivergeFrame+  , setWorkerCount+  , setKeyframePolicy+  , getKeyframeBudget+  , getKeyframeMinInterval+  , getKeyframeInterval+  , getKeyframeBytes+  , getBodyCount+  , getBodyId+  , setDebugShapeCallbacks+  , drawFrameQueries+  , getFrameQueryCount+  , getFrameQuery+  , getFrameQueryHit+  )+where++import Box3D.DebugDraw (DebugDraw)+import Box3D.Id (BodyId (..), WorldId (..))+import Box3D.Recording (RecPlayerInfo, RecQueryHit, RecQueryInfo)+import Box3D.Tags (CreateDebugShapeCallback, DestroyDebugShapeCallback, RecPlayer)+import Foreign+import Foreign.C.Types (CBool (..), CInt (..), CSize (..))++foreign import ccall safe "b3RecPlayer_StepFrame"+  c_b3RecPlayer_StepFrame :: Ptr RecPlayer -> IO CBool++foreign import ccall safe "b3RecPlayer_SubStepFrame"+  c_b3RecPlayer_SubStepFrame :: Ptr RecPlayer -> IO ()++foreign import ccall safe "b3RecPlayer_Restart"+  c_b3RecPlayer_Restart :: Ptr RecPlayer -> IO ()++foreign import ccall safe "b3RecPlayer_SeekFrame"+  c_b3RecPlayer_SeekFrame :: Ptr RecPlayer -> CInt -> IO ()++foreign import ccall unsafe "b3RecPlayer_GetWorldId"+  c_b3RecPlayer_GetWorldId :: Ptr RecPlayer -> IO WorldId++foreign import ccall unsafe "b3RecPlayer_GetFrame"+  c_b3RecPlayer_GetFrame :: Ptr RecPlayer -> IO CInt++foreign import ccall unsafe "b3RecPlayer_GetFrameCount"+  c_b3RecPlayer_GetFrameCount :: Ptr RecPlayer -> IO CInt++foreign import ccall unsafe "b3RecPlayer_IsAtEnd"+  c_b3RecPlayer_IsAtEnd :: Ptr RecPlayer -> IO CBool++foreign import ccall unsafe "b3RecPlayer_IsAtPreStep"+  c_b3RecPlayer_IsAtPreStep :: Ptr RecPlayer -> IO CBool++foreign import ccall unsafe "b3RecPlayer_HasDiverged"+  c_b3RecPlayer_HasDiverged :: Ptr RecPlayer -> IO CBool++foreign import ccall unsafe "hsg_b3RecPlayer_GetInfo"+  c_b3RecPlayer_GetInfo :: Ptr RecPlayer -> Ptr RecPlayerInfo -> IO ()++foreign import ccall unsafe "b3RecPlayer_GetDivergeFrame"+  c_b3RecPlayer_GetDivergeFrame :: Ptr RecPlayer -> IO CInt++foreign import ccall unsafe "b3RecPlayer_SetWorkerCount"+  c_b3RecPlayer_SetWorkerCount :: Ptr RecPlayer -> CInt -> IO ()++foreign import ccall unsafe "b3RecPlayer_SetKeyframePolicy"+  c_b3RecPlayer_SetKeyframePolicy :: Ptr RecPlayer -> CSize -> CInt -> IO ()++foreign import ccall unsafe "b3RecPlayer_GetKeyframeBudget"+  c_b3RecPlayer_GetKeyframeBudget :: Ptr RecPlayer -> IO CSize++foreign import ccall unsafe "b3RecPlayer_GetKeyframeMinInterval"+  c_b3RecPlayer_GetKeyframeMinInterval :: Ptr RecPlayer -> IO CInt++foreign import ccall unsafe "b3RecPlayer_GetKeyframeInterval"+  c_b3RecPlayer_GetKeyframeInterval :: Ptr RecPlayer -> IO CInt++foreign import ccall unsafe "b3RecPlayer_GetKeyframeBytes"+  c_b3RecPlayer_GetKeyframeBytes :: Ptr RecPlayer -> IO CSize++foreign import ccall unsafe "b3RecPlayer_GetBodyCount"+  c_b3RecPlayer_GetBodyCount :: Ptr RecPlayer -> IO CInt++foreign import ccall unsafe "b3RecPlayer_GetBodyId"+  c_b3RecPlayer_GetBodyId :: Ptr RecPlayer -> CInt -> IO BodyId++foreign import ccall safe "b3RecPlayer_SetDebugShapeCallbacks"+  c_b3RecPlayer_SetDebugShapeCallbacks :: Ptr RecPlayer -> FunPtr CreateDebugShapeCallback -> FunPtr DestroyDebugShapeCallback -> Ptr () -> IO ()++foreign import ccall safe "b3RecPlayer_DrawFrameQueries"+  c_b3RecPlayer_DrawFrameQueries :: Ptr RecPlayer -> Ptr DebugDraw -> CInt -> CInt -> IO ()++foreign import ccall unsafe "b3RecPlayer_GetFrameQueryCount"+  c_b3RecPlayer_GetFrameQueryCount :: Ptr RecPlayer -> IO CInt++foreign import ccall unsafe "hsg_b3RecPlayer_GetFrameQuery"+  c_b3RecPlayer_GetFrameQuery :: Ptr RecPlayer -> CInt -> Ptr RecQueryInfo -> IO ()++foreign import ccall unsafe "hsg_b3RecPlayer_GetFrameQueryHit"+  c_b3RecPlayer_GetFrameQueryHit :: Ptr RecPlayer -> CInt -> CInt -> Ptr RecQueryHit -> IO ()++{- | Advance one frame. dispatch ops until the next Step completes.++Returns: true when a frame was stepped, false at end-of-recording++Binds @b3RecPlayer_StepFrame@.+-}+stepFrame+  :: Ptr RecPlayer+  -> IO Bool+stepFrame a0 =+  toBool <$> (c_b3RecPlayer_StepFrame a0)++{- | Sub-step one frame. This will sub-step and return immediately after body creation.+The next call will execute the time step. This allows bodies to be rendered+at the creation pose.++Binds @b3RecPlayer_SubStepFrame@.+-}+subStepFrame+  :: Ptr RecPlayer+  -> IO ()+subStepFrame a0 =+  c_b3RecPlayer_SubStepFrame a0++{- | Rewind to frame 0 (in-place restore so the world id stays stable).++Binds @b3RecPlayer_Restart@.+-}+restart+  :: Ptr RecPlayer+  -> IO ()+restart a0 =+  c_b3RecPlayer_Restart a0++{- | Seek to a specific frame. Forward seek steps op-by-op; backward seek restores+the nearest keyframe then re-steps the remaining gap.++Binds @b3RecPlayer_SeekFrame@.+-}+seekFrame+  :: Ptr RecPlayer+  -> Int+  -- ^ @targetFrame@+  -> IO ()+seekFrame a0 a1 =+  c_b3RecPlayer_SeekFrame a0 (fromIntegral a1)++{- | Returns: the world currently driven by this player++Binds @b3RecPlayer_GetWorldId@.+-}+getWorldId+  :: Ptr RecPlayer+  -> IO WorldId+getWorldId a0 =+  c_b3RecPlayer_GetWorldId a0++{- | Returns: the last fully-stepped frame index (0 before any step)++Binds @b3RecPlayer_GetFrame@.+-}+getFrame+  :: Ptr RecPlayer+  -> IO Int+getFrame a0 =+  fromIntegral <$> (c_b3RecPlayer_GetFrame a0)++{- | Returns: total number of recorded frames++Binds @b3RecPlayer_GetFrameCount@.+-}+getFrameCount+  :: Ptr RecPlayer+  -> IO Int+getFrameCount a0 =+  fromIntegral <$> (c_b3RecPlayer_GetFrameCount a0)++{- | Returns: true when the op stream is exhausted++Binds @b3RecPlayer_IsAtEnd@.+-}+isAtEnd+  :: Ptr RecPlayer+  -> IO Bool+isAtEnd a0 =+  toBool <$> (c_b3RecPlayer_IsAtEnd a0)++{- | Returns: true when the op stream is paused between body creation and world step.++Binds @b3RecPlayer_IsAtPreStep@.+-}+isAtPreStep+  :: Ptr RecPlayer+  -> IO Bool+isAtPreStep a0 =+  toBool <$> (c_b3RecPlayer_IsAtPreStep a0)++{- | Returns: true when any StateHash mismatch has been detected++Binds @b3RecPlayer_HasDiverged@.+-}+hasDiverged+  :: Ptr RecPlayer+  -> IO Bool+hasDiverged a0 =+  toBool <$> (c_b3RecPlayer_HasDiverged a0)++{- | Returns: a summary of the recording read at open: frame count, recorded tuning, and bounds++Binds @b3RecPlayer_GetInfo@.+-}+getInfo+  :: Ptr RecPlayer+  -> Ptr RecPlayerInfo+  -- ^ Result buffer.+  -> IO ()+getInfo a0 out =+  c_b3RecPlayer_GetInfo a0 out++{- | Returns: the first frame at which replay diverged, or -1 if it has not diverged++Binds @b3RecPlayer_GetDivergeFrame@.+-}+getDivergeFrame+  :: Ptr RecPlayer+  -> IO Int+getDivergeFrame a0 =+  fromIntegral <$> (c_b3RecPlayer_GetDivergeFrame a0)++{- | Set the worker count of the replay world. Clamped to \[1, B3_MAX_WORKERS\]. Applied to the live+world at once and reused whenever the player rebuilds its world on Restart or a backward seek.+Replaying at a different count than recorded re-partitions the constraint graph, so the StateHash+check becomes a cross-thread determinism test.++Binds @b3RecPlayer_SetWorkerCount@.+-}+setWorkerCount+  :: Ptr RecPlayer+  -> Int+  -- ^ @count@+  -> IO ()+setWorkerCount a0 a1 =+  c_b3RecPlayer_SetWorkerCount a0 (fromIntegral a1)++{- | Tune the keyframe ring used to speed up backward seeking. A keyframe is a periodic snapshot the+player restores from instead of replaying from the start, trading memory for seek speed.++Binds @b3RecPlayer_SetKeyframePolicy@.+-}+setKeyframePolicy+  :: Ptr RecPlayer+  -- ^ @player@: the recording player+  -> CSize+  -- ^ @budgetBytes@: memory cap for the kept snapshots; the spacing widens to stay under it+  -> Int+  -- ^ @minIntervalFrames@: finest spacing between keyframes, in frames A zero budget or a non-positive interval keeps that value. Clears the existing ring, so call @b3RecPlayer_Restart@ afterward to repopulate it under the new policy.+  -> IO ()+setKeyframePolicy a0 a1 a2 =+  c_b3RecPlayer_SetKeyframePolicy a0 a1 (fromIntegral a2)++{- | Returns: the keyframe memory budget in bytes++Binds @b3RecPlayer_GetKeyframeBudget@.+-}+getKeyframeBudget+  :: Ptr RecPlayer+  -> IO CSize+getKeyframeBudget a0 =+  c_b3RecPlayer_GetKeyframeBudget a0++{- | Returns: the finest keyframe spacing in frames++Binds @b3RecPlayer_GetKeyframeMinInterval@.+-}+getKeyframeMinInterval+  :: Ptr RecPlayer+  -> IO Int+getKeyframeMinInterval a0 =+  fromIntegral <$> (c_b3RecPlayer_GetKeyframeMinInterval a0)++{- | Returns: the current keyframe spacing in frames; starts at the min interval and doubles as the ring evicts to stay under budget, so it reflects the effective backward-seek granularity now++Binds @b3RecPlayer_GetKeyframeInterval@.+-}+getKeyframeInterval+  :: Ptr RecPlayer+  -> IO Int+getKeyframeInterval a0 =+  fromIntegral <$> (c_b3RecPlayer_GetKeyframeInterval a0)++{- | Returns: the memory currently held by keyframe snapshots, in bytes++Binds @b3RecPlayer_GetKeyframeBytes@.+-}+getKeyframeBytes+  :: Ptr RecPlayer+  -> IO CSize+getKeyframeBytes a0 =+  c_b3RecPlayer_GetKeyframeBytes a0++{- | Returns: the number of bodies tracked in creation order (including holes for destroyed bodies)++Binds @b3RecPlayer_GetBodyCount@.+-}+getBodyCount+  :: Ptr RecPlayer+  -> IO Int+getBodyCount a0 =+  fromIntegral <$> (c_b3RecPlayer_GetBodyCount a0)++{- | Resolve a creation ordinal to the live body id at the current frame.++Returns: the body id, or a null id if that ordinal is out of range or its body is destroyed++Binds @b3RecPlayer_GetBodyId@.+-}+getBodyId+  :: Ptr RecPlayer+  -> Int+  -- ^ @index@+  -> IO BodyId+getBodyId a0 a1 =+  c_b3RecPlayer_GetBodyId a0 (fromIntegral a1)++{- | Wire host debug-shape callbacks into the player\'s replay world so a renderer can build+per-shape draw resources (the 3D sample needs this or the replay world draws nothing).+Rebuilds the current world under the new callbacks and rewinds to frame 0, so call it+once right after @b3RecPlayer_Create@ and re-read the world id afterward. The callbacks+persist across Restart and backward seeks, which recreate the world internally.++Binds @b3RecPlayer_SetDebugShapeCallbacks@.+-}+setDebugShapeCallbacks+  :: Ptr RecPlayer+  -- ^ @player@: the player to configure+  -> FunPtr CreateDebugShapeCallback+  -- ^ @createDebugShape@: called when a replayed shape is added; returns a user draw handle+  -> FunPtr DestroyDebugShapeCallback+  -- ^ @destroyDebugShape@: called when a replayed shape is removed; may be NULL+  -> Ptr ()+  -- ^ @context@: user context passed to both callbacks+  -> IO ()+setDebugShapeCallbacks a0 a1 a2 a3 =+  c_b3RecPlayer_SetDebugShapeCallbacks a0 a1 a2 a3++{- | Draw the spatial queries recorded during the most recently replayed frame, layered on top of the+world. Call after @b3World_Draw@. NULL draw function pointers are skipped.++Binds @b3RecPlayer_DrawFrameQueries@.+-}+drawFrameQueries+  :: Ptr RecPlayer+  -- ^ @player@: a valid player handle+  -> Ptr DebugDraw+  -- ^ @draw@: debug draw callbacks+  -> Int+  -- ^ @queryIndex@: index of the frame query to draw, or -1 to draw all of them+  -> Int+  -- ^ @selectedIndex@: index of the query to emphasize (reserved color plus a label), or -1 for none+  -> IO ()+drawFrameQueries a0 a1 a2 a3 =+  c_b3RecPlayer_DrawFrameQueries a0 a1 (fromIntegral a2) (fromIntegral a3)++{- | Returns: the number of spatial queries recorded for the most recently replayed frame++Binds @b3RecPlayer_GetFrameQueryCount@.+-}+getFrameQueryCount+  :: Ptr RecPlayer+  -> IO Int+getFrameQueryCount a0 =+  fromIntegral <$> (c_b3RecPlayer_GetFrameQueryCount a0)++{- | Get a recorded query from the most recently replayed frame by index.++Binds @b3RecPlayer_GetFrameQuery@.+-}+getFrameQuery+  :: Ptr RecPlayer+  -> Int+  -- ^ @index@+  -> Ptr RecQueryInfo+  -- ^ Result buffer.+  -> IO ()+getFrameQuery a0 a1 out =+  c_b3RecPlayer_GetFrameQuery a0 (fromIntegral a1) out++{- | Get one result of a recorded query from the most recently replayed frame.++Binds @b3RecPlayer_GetFrameQueryHit@.+-}+getFrameQueryHit+  :: Ptr RecPlayer+  -> Int+  -- ^ @queryIndex@+  -> Int+  -- ^ @hitIndex@+  -> Ptr RecQueryHit+  -- ^ Result buffer.+  -> IO ()+getFrameQueryHit a0 a1 a2 out =+  c_b3RecPlayer_GetFrameQueryHit a0 (fromIntegral a1) (fromIntegral a2) out
+ src/Box3D/Recording.hsc view
@@ -0,0 +1,210 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.Recording+  ( RecQueryHit (..)+  , RecQueryInfo (..)+  , RecQueryType (..)+  , pattern RecQueryOverlapAABB+  , pattern RecQueryOverlapShape+  , pattern RecQueryCastRay+  , pattern RecQueryCastShape+  , pattern RecQueryCastRayClosest+  , pattern RecQueryCastMover+  , pattern RecQueryCollideMover+  , RecPlayerInfo (..)+  , create+  , destroy+  , getData+  , getSize+  )+  where++import Foreign+import Foreign.C.Types (CInt (..))+import Foreign.C.String (CString)+import Box3D.Id (ShapeId)+import Box3D.MathTypes (AABB, Pos, Vec3)+import Box3D.Query (QueryFilter)+import Box3D.Tags (Recording)++#include "box3d/box3d.h"+++-- | One result of a recorded spatial query.+data RecQueryHit = RecQueryHit+  { shape :: ShapeId+  , point :: Pos+  , normal :: Vec3+  , fraction :: Float+  }+  deriving (Eq, Show)++instance Storable RecQueryHit where+  sizeOf _ = #{size b3RecQueryHit}+  alignment _ = #{alignment b3RecQueryHit}+  peek p =+    RecQueryHit+      <$> #{peek b3RecQueryHit, shape} p+      <*> #{peek b3RecQueryHit, point} p+      <*> #{peek b3RecQueryHit, normal} p+      <*> #{peek b3RecQueryHit, fraction} p+  poke p (RecQueryHit f1 f2 f3 f4) = do+    #{poke b3RecQueryHit, shape} p f1+    #{poke b3RecQueryHit, point} p f2+    #{poke b3RecQueryHit, normal} p f3+    #{poke b3RecQueryHit, fraction} p f4++-- | A spatial query recorded during a replayed frame, exposed for inspection.+data RecQueryInfo = RecQueryInfo+  { type_ :: CInt+  , filter :: QueryFilter+  , aabb :: AABB+  , origin :: Pos+  , translation :: Vec3+  , hitCount :: CInt+  , key :: Word64+  , id :: Word64+  , name :: CString+  }+  deriving (Eq, Show)++instance Storable RecQueryInfo where+  sizeOf _ = #{size b3RecQueryInfo}+  alignment _ = #{alignment b3RecQueryInfo}+  peek p =+    RecQueryInfo+      <$> #{peek b3RecQueryInfo, type} p+      <*> #{peek b3RecQueryInfo, filter} p+      <*> #{peek b3RecQueryInfo, aabb} p+      <*> #{peek b3RecQueryInfo, origin} p+      <*> #{peek b3RecQueryInfo, translation} p+      <*> #{peek b3RecQueryInfo, hitCount} p+      <*> #{peek b3RecQueryInfo, key} p+      <*> #{peek b3RecQueryInfo, id} p+      <*> #{peek b3RecQueryInfo, name} p+  poke p (RecQueryInfo f1 f2 f3 f4 f5 f6 f7 f8 f9) = do+    #{poke b3RecQueryInfo, type} p f1+    #{poke b3RecQueryInfo, filter} p f2+    #{poke b3RecQueryInfo, aabb} p f3+    #{poke b3RecQueryInfo, origin} p f4+    #{poke b3RecQueryInfo, translation} p f5+    #{poke b3RecQueryInfo, hitCount} p f6+    #{poke b3RecQueryInfo, key} p f7+    #{poke b3RecQueryInfo, id} p f8+    #{poke b3RecQueryInfo, name} p f9++-- | The kind of a recorded spatial query, matching the public query and cast functions.+newtype RecQueryType = RecQueryType CInt+  deriving (Eq, Show, Storable)++pattern RecQueryOverlapAABB :: RecQueryType+pattern RecQueryOverlapShape :: RecQueryType+pattern RecQueryCastRay :: RecQueryType+pattern RecQueryCastShape :: RecQueryType+pattern RecQueryCastRayClosest :: RecQueryType+pattern RecQueryCastMover :: RecQueryType+pattern RecQueryCollideMover :: RecQueryType+pattern RecQueryOverlapAABB = RecQueryType #{const b3_recQueryOverlapAABB}+pattern RecQueryOverlapShape = RecQueryType #{const b3_recQueryOverlapShape}+pattern RecQueryCastRay = RecQueryType #{const b3_recQueryCastRay}+pattern RecQueryCastShape = RecQueryType #{const b3_recQueryCastShape}+pattern RecQueryCastRayClosest = RecQueryType #{const b3_recQueryCastRayClosest}+pattern RecQueryCastMover = RecQueryType #{const b3_recQueryCastMover}+pattern RecQueryCollideMover = RecQueryType #{const b3_recQueryCollideMover}++-- | Summary of a recording, read once at open so a viewer can frame and label it.+data RecPlayerInfo = RecPlayerInfo+  { frameCount :: CInt+  , workerCount :: CInt+  , timeStep :: Float+  , subStepCount :: CInt+  , lengthScale :: Float+  , bounds :: AABB+  }+  deriving (Eq, Show)++instance Storable RecPlayerInfo where+  sizeOf _ = #{size b3RecPlayerInfo}+  alignment _ = #{alignment b3RecPlayerInfo}+  peek p =+    RecPlayerInfo+      <$> #{peek b3RecPlayerInfo, frameCount} p+      <*> #{peek b3RecPlayerInfo, workerCount} p+      <*> #{peek b3RecPlayerInfo, timeStep} p+      <*> #{peek b3RecPlayerInfo, subStepCount} p+      <*> #{peek b3RecPlayerInfo, lengthScale} p+      <*> #{peek b3RecPlayerInfo, bounds} p+  poke p (RecPlayerInfo f1 f2 f3 f4 f5 f6) = do+    #{poke b3RecPlayerInfo, frameCount} p f1+    #{poke b3RecPlayerInfo, workerCount} p f2+    #{poke b3RecPlayerInfo, timeStep} p f3+    #{poke b3RecPlayerInfo, subStepCount} p f4+    #{poke b3RecPlayerInfo, lengthScale} p f5+    #{poke b3RecPlayerInfo, bounds} p f6+++foreign import ccall unsafe "b3CreateRecording"+  c_b3CreateRecording :: CInt -> IO (Ptr Recording)++foreign import ccall unsafe "b3DestroyRecording"+  c_b3DestroyRecording :: Ptr Recording -> IO ()++foreign import ccall unsafe "b3Recording_GetData"+  c_b3Recording_GetData :: Ptr Recording -> IO (Ptr Word8)++foreign import ccall unsafe "b3Recording_GetSize"+  c_b3Recording_GetSize :: Ptr Recording -> IO CInt++{- | Create a recording buffer with an optional initial byte capacity.+Pass 0 to use the default (64 KiB). The buffer grows on demand.++Returns: a new recording, owned by the caller++Binds @b3CreateRecording@.+-}+create+  :: Int+  -- ^ @byteCapacity@+  -> IO (Ptr Recording)+create a0 =+  c_b3CreateRecording (fromIntegral a0)++{- | Destroy a recording and free its buffer.++Binds @b3DestroyRecording@.+-}+destroy+  :: Ptr Recording+  -- ^ @recording@: may be NULL+  -> IO ()+destroy a0 =+  c_b3DestroyRecording a0++{- | Get a pointer to the raw recording bytes.+Valid until the recording buffer is modified or destroyed.++Returns: pointer to the byte buffer, or NULL if no bytes have been written++Binds @b3Recording_GetData@.+-}+getData+  :: Ptr Recording+  -- ^ @recording@: the recording handle+  -> IO (Ptr Word8)+getData a0 =+  c_b3Recording_GetData a0++{- | Get the number of bytes currently in the recording buffer.++Binds @b3Recording_GetSize@.+-}+getSize+  :: Ptr Recording+  -- ^ @recording@: the recording handle+  -> IO Int+getSize a0 =+  fromIntegral <$> (c_b3Recording_GetSize a0)
+ src/Box3D/RevoluteJoint.hsc view
@@ -0,0 +1,420 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.RevoluteJoint+  ( RevoluteJointDef (..)+  , defaultRevoluteJointDef+  , create+  , enableSpring+  , isSpringEnabled+  , setSpringHertz+  , getSpringHertz+  , setSpringDampingRatio+  , getSpringDampingRatio+  , setTargetAngle+  , getTargetAngle+  , getAngle+  , enableLimit+  , isLimitEnabled+  , getLowerLimit+  , getUpperLimit+  , setLimits+  , enableMotor+  , isMotorEnabled+  , setMotorSpeed+  , getMotorSpeed+  , getMotorTorque+  , setMaxMotorTorque+  , getMaxMotorTorque+  )+  where++import Foreign+import Foreign.C.Types (CBool (..))+import Box3D.Internal (allocaOut)+import Box3D.Id (JointId (..), WorldId (..))+import Box3D.Joint (JointDef)++#include "box3d/box3d.h"+++{- | Revolute joint definition. A point on body B is fixed to a point on body A.+Allows relative rotation about the z-axis.+-}+data RevoluteJointDef = RevoluteJointDef+  { base :: JointDef+  -- ^ Base joint definition.+  , targetAngle :: Float+  -- ^ The bodyB angle minus bodyA angle in the reference state (radians).+  -- This defines the zero angle for the joint limit.+  , enableSpring :: CBool+  -- ^ Enable a rotational spring on the revolute hinge axis.+  , hertz :: Float+  -- ^ The spring stiffness Hertz, cycles per second.+  , dampingRatio :: Float+  -- ^ The spring damping ratio, non-dimensional.+  , enableLimit :: CBool+  -- ^ A flag to enable joint limits.+  , lowerAngle :: Float+  -- ^ The lower angle for the joint limit in radians. Minimum of -0.99*pi radians.+  , upperAngle :: Float+  -- ^ The upper angle for the joint limit in radians. Maximum of 0.99*pi radians.+  , enableMotor :: CBool+  -- ^ A flag to enable the joint motor.+  , maxMotorTorque :: Float+  -- ^ The maximum motor torque, typically in newton-meters.+  , motorSpeed :: Float+  -- ^ The desired motor speed in radians per second.+  }+  deriving (Eq, Show)++instance Storable RevoluteJointDef where+  sizeOf _ = #{size b3RevoluteJointDef}+  alignment _ = #{alignment b3RevoluteJointDef}+  peek p =+    RevoluteJointDef+      <$> #{peek b3RevoluteJointDef, base} p+      <*> #{peek b3RevoluteJointDef, targetAngle} p+      <*> #{peek b3RevoluteJointDef, enableSpring} p+      <*> #{peek b3RevoluteJointDef, hertz} p+      <*> #{peek b3RevoluteJointDef, dampingRatio} p+      <*> #{peek b3RevoluteJointDef, enableLimit} p+      <*> #{peek b3RevoluteJointDef, lowerAngle} p+      <*> #{peek b3RevoluteJointDef, upperAngle} p+      <*> #{peek b3RevoluteJointDef, enableMotor} p+      <*> #{peek b3RevoluteJointDef, maxMotorTorque} p+      <*> #{peek b3RevoluteJointDef, motorSpeed} p+  poke p (RevoluteJointDef f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11) = do+    #{poke b3RevoluteJointDef, base} p f1+    #{poke b3RevoluteJointDef, targetAngle} p f2+    #{poke b3RevoluteJointDef, enableSpring} p f3+    #{poke b3RevoluteJointDef, hertz} p f4+    #{poke b3RevoluteJointDef, dampingRatio} p f5+    #{poke b3RevoluteJointDef, enableLimit} p f6+    #{poke b3RevoluteJointDef, lowerAngle} p f7+    #{poke b3RevoluteJointDef, upperAngle} p f8+    #{poke b3RevoluteJointDef, enableMotor} p f9+    #{poke b3RevoluteJointDef, maxMotorTorque} p f10+    #{poke b3RevoluteJointDef, motorSpeed} p f11++foreign import ccall unsafe "hs_b3DefaultRevoluteJointDef"+  c_b3DefaultRevoluteJointDef :: Ptr RevoluteJointDef -> IO ()++defaultRevoluteJointDef :: IO RevoluteJointDef+defaultRevoluteJointDef = allocaOut c_b3DefaultRevoluteJointDef+++foreign import ccall unsafe "b3CreateRevoluteJoint"+  c_b3CreateRevoluteJoint :: WorldId -> Ptr RevoluteJointDef -> IO JointId++foreign import ccall unsafe "b3RevoluteJoint_EnableSpring"+  c_b3RevoluteJoint_EnableSpring :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b3RevoluteJoint_IsSpringEnabled"+  c_b3RevoluteJoint_IsSpringEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b3RevoluteJoint_SetSpringHertz"+  c_b3RevoluteJoint_SetSpringHertz :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3RevoluteJoint_GetSpringHertz"+  c_b3RevoluteJoint_GetSpringHertz :: JointId -> IO Float++foreign import ccall unsafe "b3RevoluteJoint_SetSpringDampingRatio"+  c_b3RevoluteJoint_SetSpringDampingRatio :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3RevoluteJoint_GetSpringDampingRatio"+  c_b3RevoluteJoint_GetSpringDampingRatio :: JointId -> IO Float++foreign import ccall unsafe "b3RevoluteJoint_SetTargetAngle"+  c_b3RevoluteJoint_SetTargetAngle :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3RevoluteJoint_GetTargetAngle"+  c_b3RevoluteJoint_GetTargetAngle :: JointId -> IO Float++foreign import ccall unsafe "b3RevoluteJoint_GetAngle"+  c_b3RevoluteJoint_GetAngle :: JointId -> IO Float++foreign import ccall unsafe "b3RevoluteJoint_EnableLimit"+  c_b3RevoluteJoint_EnableLimit :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b3RevoluteJoint_IsLimitEnabled"+  c_b3RevoluteJoint_IsLimitEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b3RevoluteJoint_GetLowerLimit"+  c_b3RevoluteJoint_GetLowerLimit :: JointId -> IO Float++foreign import ccall unsafe "b3RevoluteJoint_GetUpperLimit"+  c_b3RevoluteJoint_GetUpperLimit :: JointId -> IO Float++foreign import ccall unsafe "b3RevoluteJoint_SetLimits"+  c_b3RevoluteJoint_SetLimits :: JointId -> Float -> Float -> IO ()++foreign import ccall unsafe "b3RevoluteJoint_EnableMotor"+  c_b3RevoluteJoint_EnableMotor :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b3RevoluteJoint_IsMotorEnabled"+  c_b3RevoluteJoint_IsMotorEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b3RevoluteJoint_SetMotorSpeed"+  c_b3RevoluteJoint_SetMotorSpeed :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3RevoluteJoint_GetMotorSpeed"+  c_b3RevoluteJoint_GetMotorSpeed :: JointId -> IO Float++foreign import ccall unsafe "b3RevoluteJoint_GetMotorTorque"+  c_b3RevoluteJoint_GetMotorTorque :: JointId -> IO Float++foreign import ccall unsafe "b3RevoluteJoint_SetMaxMotorTorque"+  c_b3RevoluteJoint_SetMaxMotorTorque :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3RevoluteJoint_GetMaxMotorTorque"+  c_b3RevoluteJoint_GetMaxMotorTorque :: JointId -> IO Float++{- | Create a revolute joint++See also @b3RevoluteJointDef@ for details.++Binds @b3CreateRevoluteJoint@.+-}+create+  :: WorldId+  -> RevoluteJointDef+  -> IO JointId+create a0 a1 =+  with a1 $ \p1 ->+  c_b3CreateRevoluteJoint a0 p1++{- | Enable\/disable the revolute joint spring++Binds @b3RevoluteJoint_EnableSpring@.+-}+enableSpring+  :: JointId+  -> Bool+  -- ^ @enableSpring@+  -> IO ()+enableSpring a0 a1 =+  c_b3RevoluteJoint_EnableSpring a0 (fromBool a1)++{- | Is the revolute angular spring enabled?++Binds @b3RevoluteJoint_IsSpringEnabled@.+-}+isSpringEnabled+  :: JointId+  -> IO Bool+isSpringEnabled a0 =+  toBool <$> (c_b3RevoluteJoint_IsSpringEnabled a0)++{- | Set the revolute joint spring stiffness in Hertz++Binds @b3RevoluteJoint_SetSpringHertz@.+-}+setSpringHertz+  :: JointId+  -> Float+  -- ^ @hertz@+  -> IO ()+setSpringHertz a0 a1 =+  c_b3RevoluteJoint_SetSpringHertz a0 a1++{- | Get the revolute joint spring stiffness in Hertz++Binds @b3RevoluteJoint_GetSpringHertz@.+-}+getSpringHertz+  :: JointId+  -> IO Float+getSpringHertz a0 =+  c_b3RevoluteJoint_GetSpringHertz a0++{- | Set the revolute joint spring damping ratio, non-dimensional++Binds @b3RevoluteJoint_SetSpringDampingRatio@.+-}+setSpringDampingRatio+  :: JointId+  -> Float+  -- ^ @dampingRatio@+  -> IO ()+setSpringDampingRatio a0 a1 =+  c_b3RevoluteJoint_SetSpringDampingRatio a0 a1++{- | Get the revolute joint spring damping ratio, non-dimensional++Binds @b3RevoluteJoint_GetSpringDampingRatio@.+-}+getSpringDampingRatio+  :: JointId+  -> IO Float+getSpringDampingRatio a0 =+  c_b3RevoluteJoint_GetSpringDampingRatio a0++{- | Set the revolute joint target angle in radians++Binds @b3RevoluteJoint_SetTargetAngle@.+-}+setTargetAngle+  :: JointId+  -> Float+  -- ^ @targetRadians@+  -> IO ()+setTargetAngle a0 a1 =+  c_b3RevoluteJoint_SetTargetAngle a0 a1++{- | Get the revolute joint target angle in radians++Binds @b3RevoluteJoint_GetTargetAngle@.+-}+getTargetAngle+  :: JointId+  -> IO Float+getTargetAngle a0 =+  c_b3RevoluteJoint_GetTargetAngle a0++{- | Get the revolute joint current angle in radians relative to the reference angle++See also @b3RevoluteJointDef::referenceAngle@.++Binds @b3RevoluteJoint_GetAngle@.+-}+getAngle+  :: JointId+  -> IO Float+getAngle a0 =+  c_b3RevoluteJoint_GetAngle a0++{- | Enable\/disable the revolute joint limit++Binds @b3RevoluteJoint_EnableLimit@.+-}+enableLimit+  :: JointId+  -> Bool+  -- ^ @enableLimit@+  -> IO ()+enableLimit a0 a1 =+  c_b3RevoluteJoint_EnableLimit a0 (fromBool a1)++{- | Is the revolute joint limit enabled?++Binds @b3RevoluteJoint_IsLimitEnabled@.+-}+isLimitEnabled+  :: JointId+  -> IO Bool+isLimitEnabled a0 =+  toBool <$> (c_b3RevoluteJoint_IsLimitEnabled a0)++{- | Get the revolute joint lower limit in radians++Binds @b3RevoluteJoint_GetLowerLimit@.+-}+getLowerLimit+  :: JointId+  -> IO Float+getLowerLimit a0 =+  c_b3RevoluteJoint_GetLowerLimit a0++{- | Get the revolute joint upper limit in radians++Binds @b3RevoluteJoint_GetUpperLimit@.+-}+getUpperLimit+  :: JointId+  -> IO Float+getUpperLimit a0 =+  c_b3RevoluteJoint_GetUpperLimit a0++{- | Set the revolute joint limits in radians++Binds @b3RevoluteJoint_SetLimits@.+-}+setLimits+  :: JointId+  -> Float+  -- ^ @lowerLimitRadians@+  -> Float+  -- ^ @upperLimitRadians@+  -> IO ()+setLimits a0 a1 a2 =+  c_b3RevoluteJoint_SetLimits a0 a1 a2++{- | Enable\/disable a revolute joint motor++Binds @b3RevoluteJoint_EnableMotor@.+-}+enableMotor+  :: JointId+  -> Bool+  -- ^ @enableMotor@+  -> IO ()+enableMotor a0 a1 =+  c_b3RevoluteJoint_EnableMotor a0 (fromBool a1)++{- | Is the revolute joint motor enabled?++Binds @b3RevoluteJoint_IsMotorEnabled@.+-}+isMotorEnabled+  :: JointId+  -> IO Bool+isMotorEnabled a0 =+  toBool <$> (c_b3RevoluteJoint_IsMotorEnabled a0)++{- | Set the revolute joint motor speed in radians per second++Binds @b3RevoluteJoint_SetMotorSpeed@.+-}+setMotorSpeed+  :: JointId+  -> Float+  -- ^ @motorSpeed@+  -> IO ()+setMotorSpeed a0 a1 =+  c_b3RevoluteJoint_SetMotorSpeed a0 a1++{- | Get the revolute joint motor speed in radians per second++Binds @b3RevoluteJoint_GetMotorSpeed@.+-}+getMotorSpeed+  :: JointId+  -> IO Float+getMotorSpeed a0 =+  c_b3RevoluteJoint_GetMotorSpeed a0++{- | Get the revolute joint current motor torque, usually in newton-meters++Binds @b3RevoluteJoint_GetMotorTorque@.+-}+getMotorTorque+  :: JointId+  -> IO Float+getMotorTorque a0 =+  c_b3RevoluteJoint_GetMotorTorque a0++{- | Set the revolute joint maximum motor torque, usually in newton-meters++Binds @b3RevoluteJoint_SetMaxMotorTorque@.+-}+setMaxMotorTorque+  :: JointId+  -> Float+  -- ^ @torque@+  -> IO ()+setMaxMotorTorque a0 a1 =+  c_b3RevoluteJoint_SetMaxMotorTorque a0 a1++{- | Get the revolute joint maximum motor torque, usually in newton-meters++Binds @b3RevoluteJoint_GetMaxMotorTorque@.+-}+getMaxMotorTorque+  :: JointId+  -> IO Float+getMaxMotorTorque a0 =+  c_b3RevoluteJoint_GetMaxMotorTorque a0
+ src/Box3D/Rock.hs view
@@ -0,0 +1,23 @@+-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.Rock+  ( create+  )+where++import Box3D.Hull (HullData)+import Foreign++foreign import ccall unsafe "b3CreateRock"+  c_b3CreateRock :: Float -> IO (Ptr HullData)++{- | Create a rock shaped hull.++Binds @b3CreateRock@.+-}+create+  :: Float+  -- ^ @radius@+  -> IO (Ptr HullData)+create a0 =+  c_b3CreateRock a0
+ src/Box3D/Shape.hs-boot view
@@ -0,0 +1,13 @@+-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.Shape+  ( SurfaceMaterial+  )+  where++import Foreign (Storable)++data SurfaceMaterial+instance Eq SurfaceMaterial+instance Show SurfaceMaterial+instance Storable SurfaceMaterial
+ src/Box3D/Shape.hsc view
@@ -0,0 +1,1331 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.Shape+  ( ShapeDef (..)+  , ShapeType (..)+  , pattern CapsuleShape+  , pattern CompoundShape+  , pattern HeightShape+  , pattern HullShape+  , pattern MeshShape+  , pattern SphereShape+  , SurfaceMaterial (..)+  , Filter (..)+  , defaultFilter+  , defaultSurfaceMaterial+  , defaultShapeDef+  , createSphere+  , createCapsule+  , createHull+  , createTransformedHull+  , createMesh+  , createHeightField+  , createBakedCompound+  , destroy+  , isValid+  , getType+  , getBody+  , getWorld+  , isSensor+  , setName+  , getName+  , setUserData+  , getUserData+  , setDensity+  , getDensity+  , setFriction+  , getFriction+  , setRestitution+  , getRestitution+  , setSurfaceMaterial+  , getSurfaceMaterial+  , getSurfaceMaterialInto+  , getMeshMaterialCount+  , setMeshMaterial+  , getMeshSurfaceMaterial+  , getMeshSurfaceMaterialInto+  , getFilter+  , getFilterInto+  , setFilter+  , enableSensorEvents+  , areSensorEventsEnabled+  , enableContactEvents+  , areContactEventsEnabled+  , enablePreSolveEvents+  , arePreSolveEventsEnabled+  , enableHitEvents+  , areHitEventsEnabled+  , rayCast+  , getSphere+  , getSphereInto+  , getCapsule+  , getCapsuleInto+  , getHull+  , getMesh+  , getHeightField+  , setSphere+  , setCapsule+  , setHull+  , setMesh+  , getContactCapacity+  , getContactData+  , getSensorCapacity+  , getSensorData+  , getAABB+  , getAABBInto+  , computeMassData+  , computeMassDataInto+  , getClosestPoint+  , applyWind+  , testPoint+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..))+import Foreign.C.String (CString)+import Data.Coerce (coerce)+import Box3D.Internal (allocaOut)+import Box3D.Compound (CompoundData)+import Box3D.Geometry (Capsule, MassData, Sphere)+import Box3D.HeightField (HeightFieldData)+import Box3D.Hull (HullData)+import Box3D.Id (BodyId (..), ShapeId (..), WorldId (..))+import Box3D.MathTypes (AABB, Pos, Transform, Vec3)+import Box3D.Mesh (Mesh, MeshData)+import Box3D.Query (WorldCastOutput)+import Box3D.World (ContactData)++#include "box3d/box3d.h"+++-- | Used to create a shape+data ShapeDef = ShapeDef+  { name :: CString+  -- ^ Optional shape name for debugging+  , userData :: Ptr ()+  -- ^ Use this to store application specific shape data.+  , materials :: Ptr SurfaceMaterial+  -- ^ Surface material used on mesh shapes per triangle. Ignored for convex shapes. Ignored for compound shapes.+  , materialCount :: CInt+  -- ^ Surface material count.+  , baseMaterial :: SurfaceMaterial+  -- ^ The base surface material. Ignored for compound shapes.+  , density :: Float+  -- ^ The density, usually in kg\/m^3.+  , explosionScale :: Float+  -- ^ Explosion scale for @b3World_Explode@. non-dimensional+  , filter :: Filter+  -- ^ Contact filtering data.+  , enableCustomFiltering :: CBool+  -- ^ Enable custom filtering. Only one of the two shapes needs to enable custom filtering. See @b3WorldDef@.+  , isSensor :: CBool+  -- ^ 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 also enableSensorEvents.+  , enableSensorEvents :: CBool+  -- ^ Enable sensor events for this shape. This applies to sensors and non-sensors. False by default, even for sensors.+  -- Only convex shapes may act as sensor visitors.+  , enableContactEvents :: CBool+  -- ^ Enable contact events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. False by default.+  , enableHitEvents :: CBool+  -- ^ Enable hit events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. False by default.+  , enablePreSolveEvents :: CBool+  -- ^ 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.+  , invokeContactCreation :: CBool+  -- ^ 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.+  , updateBodyMass :: CBool+  -- ^ Should the body update the mass properties when this shape is created. Default is true.+  -- Warning: if this is false, you MUST call @b3Body_ApplyMassFromShapes@ or @b3Body_SetMassData@ before simulating the world.+  , enableSpeculativeContact :: CBool+  -- ^ Enable speculative collision. Leave this true unless you care about reducing ghost collision+  -- more than continuous collision under rotation.+  -- Experimental: this can only disable speculative contact between hulls and triangles (meshes and height fields).+  , internalValue :: CInt+  -- ^ Used internally to detect a valid definition. DO NOT SET.+  }+  deriving (Eq, Show)++instance Storable ShapeDef where+  sizeOf _ = #{size b3ShapeDef}+  alignment _ = #{alignment b3ShapeDef}+  peek p =+    ShapeDef+      <$> #{peek b3ShapeDef, name} p+      <*> #{peek b3ShapeDef, userData} p+      <*> #{peek b3ShapeDef, materials} p+      <*> #{peek b3ShapeDef, materialCount} p+      <*> #{peek b3ShapeDef, baseMaterial} p+      <*> #{peek b3ShapeDef, density} p+      <*> #{peek b3ShapeDef, explosionScale} p+      <*> #{peek b3ShapeDef, filter} p+      <*> #{peek b3ShapeDef, enableCustomFiltering} p+      <*> #{peek b3ShapeDef, isSensor} p+      <*> #{peek b3ShapeDef, enableSensorEvents} p+      <*> #{peek b3ShapeDef, enableContactEvents} p+      <*> #{peek b3ShapeDef, enableHitEvents} p+      <*> #{peek b3ShapeDef, enablePreSolveEvents} p+      <*> #{peek b3ShapeDef, invokeContactCreation} p+      <*> #{peek b3ShapeDef, updateBodyMass} p+      <*> #{peek b3ShapeDef, enableSpeculativeContact} p+      <*> #{peek b3ShapeDef, internalValue} p+  poke p (ShapeDef f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11 f12 f13 f14 f15 f16 f17 f18) = do+    #{poke b3ShapeDef, name} p f1+    #{poke b3ShapeDef, userData} p f2+    #{poke b3ShapeDef, materials} p f3+    #{poke b3ShapeDef, materialCount} p f4+    #{poke b3ShapeDef, baseMaterial} p f5+    #{poke b3ShapeDef, density} p f6+    #{poke b3ShapeDef, explosionScale} p f7+    #{poke b3ShapeDef, filter} p f8+    #{poke b3ShapeDef, enableCustomFiltering} p f9+    #{poke b3ShapeDef, isSensor} p f10+    #{poke b3ShapeDef, enableSensorEvents} p f11+    #{poke b3ShapeDef, enableContactEvents} p f12+    #{poke b3ShapeDef, enableHitEvents} p f13+    #{poke b3ShapeDef, enablePreSolveEvents} p f14+    #{poke b3ShapeDef, invokeContactCreation} p f15+    #{poke b3ShapeDef, updateBodyMass} p f16+    #{poke b3ShapeDef, enableSpeculativeContact} p f17+    #{poke b3ShapeDef, internalValue} p f18++-- | Shape type+newtype ShapeType = ShapeType CInt+  deriving (Eq, Show, Storable)++-- | A capsule is an extruded sphere+pattern CapsuleShape :: ShapeType+-- | A baked compound shape composed of spheres, capsules, hulls, and meshes+pattern CompoundShape :: ShapeType+-- | A height field useful for terrain+pattern HeightShape :: ShapeType+-- | A convex hull+pattern HullShape :: ShapeType+-- | A triangle soup+pattern MeshShape :: ShapeType+-- | A sphere with an offset+pattern SphereShape :: ShapeType+pattern CapsuleShape = ShapeType #{const b3_capsuleShape}+pattern CompoundShape = ShapeType #{const b3_compoundShape}+pattern HeightShape = ShapeType #{const b3_heightShape}+pattern HullShape = ShapeType #{const b3_hullShape}+pattern MeshShape = ShapeType #{const b3_meshShape}+pattern SphereShape = ShapeType #{const b3_sphereShape}++-- | Material properties supported per triangle on meshes and height fields+data SurfaceMaterial = SurfaceMaterial+  { friction :: Float+  -- ^ The Coulomb (dry) friction coefficient, usually in the range \[0,1\].+  , restitution :: Float+  -- ^ The coefficient of restitution (bounce) usually in the range \[0,1\].+  -- <https://en.wikipedia.org/wiki/Coefficient_of_restitution>+  , rollingResistance :: Float+  -- ^ The rolling resistance usually in the range \[0,1\]. This is only used for spheres and capsules.+  , tangentVelocity :: Vec3+  -- ^ The tangent velocity for conveyor belts. This is local to the shape and will be projected+  -- onto the contact surface.+  , userMaterialId :: Word64+  -- ^ User material identifier. This is passed with query results and to friction and restitution+  -- combining functions. It is not used internally.+  , customColor :: Word32+  -- ^ Custom debug draw color. Ignored if 0. The low 24 bits are RGB. The high byte may+  -- carry a @b3DebugMaterial@ preset, see @b3MakeDebugColor@.+  --+  -- See also @b3HexColor@.+  , padding :: Word32+  -- ^ Explicit padding. Must be zero.+  }+  deriving (Eq, Show)++instance Storable SurfaceMaterial where+  sizeOf _ = #{size b3SurfaceMaterial}+  alignment _ = #{alignment b3SurfaceMaterial}+  peek p =+    SurfaceMaterial+      <$> #{peek b3SurfaceMaterial, friction} p+      <*> #{peek b3SurfaceMaterial, restitution} p+      <*> #{peek b3SurfaceMaterial, rollingResistance} p+      <*> #{peek b3SurfaceMaterial, tangentVelocity} p+      <*> #{peek b3SurfaceMaterial, userMaterialId} p+      <*> #{peek b3SurfaceMaterial, customColor} p+      <*> #{peek b3SurfaceMaterial, padding} p+  poke p (SurfaceMaterial f1 f2 f3 f4 f5 f6 f7) = do+    #{poke b3SurfaceMaterial, friction} p f1+    #{poke b3SurfaceMaterial, restitution} p f2+    #{poke b3SurfaceMaterial, rollingResistance} p f3+    #{poke b3SurfaceMaterial, tangentVelocity} p f4+    #{poke b3SurfaceMaterial, userMaterialId} p f5+    #{poke b3SurfaceMaterial, customColor} p f6+    #{poke b3SurfaceMaterial, padding} p f7++{- | This is used to filter collision on shapes. It affects shape-vs-shape collision+and shape-versus-query collision (such as @b3World_CastRay@).+-}+data Filter = Filter+  { categoryBits :: Word64+  -- ^ The collision category bits. Normally you would just set one bit. The category bits should+  -- represent your application object types. For example:+  --+  -- > enum MyCategories+  -- > {+  -- > Static  = 0x00000001,+  -- > Dynamic = 0x00000002,+  -- > Debris  = 0x00000004,+  -- > Player  = 0x00000008,+  -- > // etc+  -- > };+  , maskBits :: Word64+  -- ^ 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.+  --+  -- > maskBits = Static | Player;+  , groupIndex :: CInt+  -- ^ 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.+  }+  deriving (Eq, Show)++instance Storable Filter where+  sizeOf _ = #{size b3Filter}+  alignment _ = #{alignment b3Filter}+  peek p =+    Filter+      <$> #{peek b3Filter, categoryBits} p+      <*> #{peek b3Filter, maskBits} p+      <*> #{peek b3Filter, groupIndex} p+  poke p (Filter f1 f2 f3) = do+    #{poke b3Filter, categoryBits} p f1+    #{poke b3Filter, maskBits} p f2+    #{poke b3Filter, groupIndex} p f3++foreign import ccall unsafe "hs_b3DefaultFilter"+  c_b3DefaultFilter :: Ptr Filter -> IO ()++defaultFilter :: IO Filter+defaultFilter = allocaOut c_b3DefaultFilter++foreign import ccall unsafe "hs_b3DefaultSurfaceMaterial"+  c_b3DefaultSurfaceMaterial :: Ptr SurfaceMaterial -> IO ()++defaultSurfaceMaterial :: IO SurfaceMaterial+defaultSurfaceMaterial = allocaOut c_b3DefaultSurfaceMaterial++foreign import ccall unsafe "hs_b3DefaultShapeDef"+  c_b3DefaultShapeDef :: Ptr ShapeDef -> IO ()++defaultShapeDef :: IO ShapeDef+defaultShapeDef = allocaOut c_b3DefaultShapeDef+++foreign import ccall unsafe "b3CreateSphereShape"+  c_b3CreateSphereShape :: BodyId -> Ptr ShapeDef -> Ptr Sphere -> IO ShapeId++foreign import ccall unsafe "b3CreateCapsuleShape"+  c_b3CreateCapsuleShape :: BodyId -> Ptr ShapeDef -> Ptr Capsule -> IO ShapeId++foreign import ccall unsafe "b3CreateHullShape"+  c_b3CreateHullShape :: BodyId -> Ptr ShapeDef -> Ptr HullData -> IO ShapeId++foreign import ccall unsafe "hsg_b3CreateTransformedHullShape"+  c_b3CreateTransformedHullShape :: BodyId -> Ptr ShapeDef -> Ptr HullData -> Ptr Transform -> Ptr Vec3 -> IO ShapeId++foreign import ccall unsafe "hsg_b3CreateMeshShape"+  c_b3CreateMeshShape :: BodyId -> Ptr ShapeDef -> Ptr MeshData -> Ptr Vec3 -> IO ShapeId++foreign import ccall unsafe "b3CreateHeightFieldShape"+  c_b3CreateHeightFieldShape :: BodyId -> Ptr ShapeDef -> Ptr HeightFieldData -> IO ShapeId++foreign import ccall unsafe "b3CreateBakedCompoundShape"+  c_b3CreateBakedCompoundShape :: BodyId -> Ptr ShapeDef -> Ptr CompoundData -> IO ShapeId++foreign import ccall safe "b3DestroyShape"+  c_b3DestroyShape :: ShapeId -> CBool -> IO ()++foreign import ccall unsafe "b3Shape_IsValid"+  c_b3Shape_IsValid :: ShapeId -> IO CBool++foreign import ccall unsafe "b3Shape_GetType"+  c_b3Shape_GetType :: ShapeId -> IO CInt++foreign import ccall unsafe "b3Shape_GetBody"+  c_b3Shape_GetBody :: ShapeId -> IO BodyId++foreign import ccall unsafe "b3Shape_GetWorld"+  c_b3Shape_GetWorld :: ShapeId -> IO WorldId++foreign import ccall unsafe "b3Shape_IsSensor"+  c_b3Shape_IsSensor :: ShapeId -> IO CBool++foreign import ccall unsafe "b3Shape_SetName"+  c_b3Shape_SetName :: ShapeId -> CString -> IO ()++foreign import ccall unsafe "b3Shape_GetName"+  c_b3Shape_GetName :: ShapeId -> IO CString++foreign import ccall unsafe "b3Shape_SetUserData"+  c_b3Shape_SetUserData :: ShapeId -> Ptr () -> IO ()++foreign import ccall unsafe "b3Shape_GetUserData"+  c_b3Shape_GetUserData :: ShapeId -> IO (Ptr ())++foreign import ccall unsafe "b3Shape_SetDensity"+  c_b3Shape_SetDensity :: ShapeId -> Float -> CBool -> IO ()++foreign import ccall unsafe "b3Shape_GetDensity"+  c_b3Shape_GetDensity :: ShapeId -> IO Float++foreign import ccall unsafe "b3Shape_SetFriction"+  c_b3Shape_SetFriction :: ShapeId -> Float -> IO ()++foreign import ccall unsafe "b3Shape_GetFriction"+  c_b3Shape_GetFriction :: ShapeId -> IO Float++foreign import ccall unsafe "b3Shape_SetRestitution"+  c_b3Shape_SetRestitution :: ShapeId -> Float -> IO ()++foreign import ccall unsafe "b3Shape_GetRestitution"+  c_b3Shape_GetRestitution :: ShapeId -> IO Float++foreign import ccall unsafe "hsg_b3Shape_SetSurfaceMaterial"+  c_b3Shape_SetSurfaceMaterial :: ShapeId -> Ptr SurfaceMaterial -> IO ()++foreign import ccall unsafe "hsg_b3Shape_GetSurfaceMaterial"+  c_b3Shape_GetSurfaceMaterial :: ShapeId -> Ptr SurfaceMaterial -> IO ()++foreign import ccall unsafe "b3Shape_GetMeshMaterialCount"+  c_b3Shape_GetMeshMaterialCount :: ShapeId -> IO CInt++foreign import ccall unsafe "hsg_b3Shape_SetMeshMaterial"+  c_b3Shape_SetMeshMaterial :: ShapeId -> Ptr SurfaceMaterial -> CInt -> IO ()++foreign import ccall unsafe "hsg_b3Shape_GetMeshSurfaceMaterial"+  c_b3Shape_GetMeshSurfaceMaterial :: ShapeId -> CInt -> Ptr SurfaceMaterial -> IO ()++foreign import ccall unsafe "hsg_b3Shape_GetFilter"+  c_b3Shape_GetFilter :: ShapeId -> Ptr Filter -> IO ()++foreign import ccall unsafe "hsg_b3Shape_SetFilter"+  c_b3Shape_SetFilter :: ShapeId -> Ptr Filter -> CBool -> IO ()++foreign import ccall unsafe "b3Shape_EnableSensorEvents"+  c_b3Shape_EnableSensorEvents :: ShapeId -> CBool -> IO ()++foreign import ccall unsafe "b3Shape_AreSensorEventsEnabled"+  c_b3Shape_AreSensorEventsEnabled :: ShapeId -> IO CBool++foreign import ccall unsafe "b3Shape_EnableContactEvents"+  c_b3Shape_EnableContactEvents :: ShapeId -> CBool -> IO ()++foreign import ccall unsafe "b3Shape_AreContactEventsEnabled"+  c_b3Shape_AreContactEventsEnabled :: ShapeId -> IO CBool++foreign import ccall unsafe "b3Shape_EnablePreSolveEvents"+  c_b3Shape_EnablePreSolveEvents :: ShapeId -> CBool -> IO ()++foreign import ccall unsafe "b3Shape_ArePreSolveEventsEnabled"+  c_b3Shape_ArePreSolveEventsEnabled :: ShapeId -> IO CBool++foreign import ccall unsafe "b3Shape_EnableHitEvents"+  c_b3Shape_EnableHitEvents :: ShapeId -> CBool -> IO ()++foreign import ccall unsafe "b3Shape_AreHitEventsEnabled"+  c_b3Shape_AreHitEventsEnabled :: ShapeId -> IO CBool++foreign import ccall unsafe "hsg_b3Shape_RayCast"+  c_b3Shape_RayCast :: ShapeId -> Ptr Pos -> Ptr Vec3 -> Ptr WorldCastOutput -> IO ()++foreign import ccall unsafe "hsg_b3Shape_GetSphere"+  c_b3Shape_GetSphere :: ShapeId -> Ptr Sphere -> IO ()++foreign import ccall unsafe "hsg_b3Shape_GetCapsule"+  c_b3Shape_GetCapsule :: ShapeId -> Ptr Capsule -> IO ()++foreign import ccall unsafe "b3Shape_GetHull"+  c_b3Shape_GetHull :: ShapeId -> IO (Ptr HullData)++foreign import ccall unsafe "hsg_b3Shape_GetMesh"+  c_b3Shape_GetMesh :: ShapeId -> Ptr Mesh -> IO ()++foreign import ccall unsafe "b3Shape_GetHeightField"+  c_b3Shape_GetHeightField :: ShapeId -> IO (Ptr HeightFieldData)++foreign import ccall safe "b3Shape_SetSphere"+  c_b3Shape_SetSphere :: ShapeId -> Ptr Sphere -> IO ()++foreign import ccall safe "b3Shape_SetCapsule"+  c_b3Shape_SetCapsule :: ShapeId -> Ptr Capsule -> IO ()++foreign import ccall safe "b3Shape_SetHull"+  c_b3Shape_SetHull :: ShapeId -> Ptr HullData -> IO ()++foreign import ccall safe "hsg_b3Shape_SetMesh"+  c_b3Shape_SetMesh :: ShapeId -> Ptr MeshData -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "b3Shape_GetContactCapacity"+  c_b3Shape_GetContactCapacity :: ShapeId -> IO CInt++foreign import ccall unsafe "b3Shape_GetContactData"+  c_b3Shape_GetContactData :: ShapeId -> Ptr ContactData -> CInt -> IO CInt++foreign import ccall unsafe "b3Shape_GetSensorCapacity"+  c_b3Shape_GetSensorCapacity :: ShapeId -> IO CInt++foreign import ccall unsafe "b3Shape_GetSensorData"+  c_b3Shape_GetSensorData :: ShapeId -> Ptr ShapeId -> CInt -> IO CInt++foreign import ccall unsafe "hsg_b3Shape_GetAABB"+  c_b3Shape_GetAABB :: ShapeId -> Ptr AABB -> IO ()++foreign import ccall unsafe "hsg_b3Shape_ComputeMassData"+  c_b3Shape_ComputeMassData :: ShapeId -> Ptr MassData -> IO ()++foreign import ccall unsafe "hsg_b3Shape_GetClosestPoint"+  c_b3Shape_GetClosestPoint :: ShapeId -> Ptr Vec3 -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "hsg_b3Shape_ApplyWind"+  c_b3Shape_ApplyWind :: ShapeId -> Ptr Vec3 -> Float -> Float -> Float -> CBool -> IO ()++foreign import ccall unsafe "hs_b3Shape_TestPoint"+  c_b3Shape_TestPoint :: ShapeId -> Ptr Pos -> IO CBool++{- | 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.++Returns: the shape id for accessing the shape++Binds @b3CreateSphereShape@.+-}+createSphere+  :: BodyId+  -> ShapeDef+  -> Sphere+  -> IO ShapeId+createSphere a0 a1 a2 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b3CreateSphereShape a0 p1 p2++{- | 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.++Returns: the shape id for accessing the shape++Binds @b3CreateCapsuleShape@.+-}+createCapsule+  :: BodyId+  -> ShapeDef+  -> Capsule+  -> IO ShapeId+createCapsule a0 a1 a2 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b3CreateCapsuleShape a0 p1 p2++{- | Create a convex hull shape and attach it to a body. The shape definition is fully cloned. Contacts are not created+until the next time step.++Returns: the shape id for accessing the shape++Binds @b3CreateHullShape@.+-}+createHull+  :: BodyId+  -> ShapeDef+  -> Ptr HullData+  -> IO ShapeId+createHull a0 a1 a2 =+  with a1 $ \p1 ->+  c_b3CreateHullShape a0 p1 a2++{- | Create a convex hull shape and attach it to a body. The hull is cloned then transformed with scale applied first.+Use this for non-uniform or mirrored scale or a baked local transform. The baked result is shared through the+world hull database. The shape definition and geometry are fully cloned. Contacts are not created until the next time step.++Returns: the shape id for accessing the shape++Binds @b3CreateTransformedHullShape@.+-}+createTransformedHull+  :: BodyId+  -> ShapeDef+  -> Ptr HullData+  -> Transform+  -> Vec3+  -- ^ @scale@+  -> IO ShapeId+createTransformedHull a0 a1 a2 a3 a4 =+  with a1 $ \p1 ->+  with a3 $ \p3 ->+  with a4 $ \p4 ->+  c_b3CreateTransformedHullShape a0 p1 a2 p3 p4++{- | 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++Returns: the shape id for accessing the shape++Binds @b3CreateMeshShape@.+-}+createMesh+  :: BodyId+  -> ShapeDef+  -> Ptr MeshData+  -> Vec3+  -- ^ @scale@+  -> IO ShapeId+createMesh a0 a1 a2 a3 =+  with a1 $ \p1 ->+  with a3 $ \p3 ->+  c_b3CreateMeshShape a0 p1 a2 p3++{- | 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++Returns: the shape id for accessing the shape++Binds @b3CreateHeightFieldShape@.+-}+createHeightField+  :: BodyId+  -> ShapeDef+  -> Ptr HeightFieldData+  -> IO ShapeId+createHeightField a0 a1 a2 =+  with a1 $ \p1 ->+  c_b3CreateHeightFieldShape a0 p1 a2++{- | Baked compound shapes are only allowed on static bodies.+Note: runtime compounds are achieved by adding multiple shapes to a body.+Runtime compounds can be dynamic and\/or kinematic.++Binds @b3CreateBakedCompoundShape@.+-}+createBakedCompound+  :: BodyId+  -> Ptr ShapeDef+  -> Ptr CompoundData+  -> IO ShapeId+createBakedCompound a0 a1 a2 =+  c_b3CreateBakedCompoundShape a0 a1 a2++{- | 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 also @b3Body_ApplyMassFromShapes@.++Binds @b3DestroyShape@.+-}+destroy+  :: ShapeId+  -> Bool+  -- ^ @updateBodyMass@+  -> IO ()+destroy a0 a1 =+  c_b3DestroyShape a0 (fromBool a1)++{- | Shape identifier validation. Provides validation for up to 64K allocations.++Binds @b3Shape_IsValid@.+-}+isValid+  :: ShapeId+  -> IO Bool+isValid a0 =+  toBool <$> (c_b3Shape_IsValid a0)++{- | Get the type of a shape++Binds @b3Shape_GetType@.+-}+getType+  :: ShapeId+  -> IO ShapeType+getType a0 =+  coerce <$> (c_b3Shape_GetType a0)++{- | Get the id of the body that a shape is attached to++Binds @b3Shape_GetBody@.+-}+getBody+  :: ShapeId+  -> IO BodyId+getBody a0 =+  c_b3Shape_GetBody a0++{- | Get the world that owns this shape++Binds @b3Shape_GetWorld@.+-}+getWorld+  :: ShapeId+  -> IO WorldId+getWorld a0 =+  c_b3Shape_GetWorld a0++{- | Returns true if the shape is a sensor++Binds @b3Shape_IsSensor@.+-}+isSensor+  :: ShapeId+  -> IO Bool+isSensor a0 =+  toBool <$> (c_b3Shape_IsSensor a0)++{- | Set the shape name.++Binds @b3Shape_SetName@.+-}+setName+  :: ShapeId+  -> CString+  -- ^ @name@+  -> IO ()+setName a0 a1 =+  c_b3Shape_SetName a0 a1++{- | Get the shape name. Returns an empty string if the name isn\'t set.++Binds @b3Shape_GetName@.+-}+getName+  :: ShapeId+  -> IO CString+getName a0 =+  c_b3Shape_GetName a0++{- | Set the user data for a shape++Binds @b3Shape_SetUserData@.+-}+setUserData+  :: ShapeId+  -> Ptr ()+  -- ^ @userData@+  -> IO ()+setUserData a0 a1 =+  c_b3Shape_SetUserData a0 a1++{- | Get the user data for a shape. This is useful when you get a shape id+from an event or query.++Binds @b3Shape_GetUserData@.+-}+getUserData+  :: ShapeId+  -> IO (Ptr ())+getUserData a0 =+  c_b3Shape_GetUserData a0++{- | Set the mass density of a shape, usually in kg\/m^3.+This will optionally update the mass properties on the parent body.++See also @b3ShapeDef::density@, @b3Body_ApplyMassFromShapes@.++Binds @b3Shape_SetDensity@.+-}+setDensity+  :: ShapeId+  -> Float+  -- ^ @density@+  -> Bool+  -- ^ @updateBodyMass@+  -> IO ()+setDensity a0 a1 a2 =+  c_b3Shape_SetDensity a0 a1 (fromBool a2)++{- | Get the density of a shape, usually in kg\/m^3++Binds @b3Shape_GetDensity@.+-}+getDensity+  :: ShapeId+  -> IO Float+getDensity a0 =+  c_b3Shape_GetDensity a0++{- | Set the friction on a shape++Binds @b3Shape_SetFriction@.+-}+setFriction+  :: ShapeId+  -> Float+  -- ^ @friction@+  -> IO ()+setFriction a0 a1 =+  c_b3Shape_SetFriction a0 a1++{- | Get the friction of a shape++Binds @b3Shape_GetFriction@.+-}+getFriction+  :: ShapeId+  -> IO Float+getFriction a0 =+  c_b3Shape_GetFriction a0++{- | Set the shape restitution (bounciness)++Binds @b3Shape_SetRestitution@.+-}+setRestitution+  :: ShapeId+  -> Float+  -- ^ @restitution@+  -> IO ()+setRestitution a0 a1 =+  c_b3Shape_SetRestitution a0 a1++{- | Get the shape restitution++Binds @b3Shape_GetRestitution@.+-}+getRestitution+  :: ShapeId+  -> IO Float+getRestitution a0 =+  c_b3Shape_GetRestitution a0++{- | Set the shape base surface material. Does not change per triangle materials.++Binds @b3Shape_SetSurfaceMaterial@.+-}+setSurfaceMaterial+  :: ShapeId+  -> SurfaceMaterial+  -> IO ()+setSurfaceMaterial a0 a1 =+  with a1 $ \p1 ->+  c_b3Shape_SetSurfaceMaterial a0 p1++{- | Get the base shape surface material.++Binds @b3Shape_GetSurfaceMaterial@.+-}+getSurfaceMaterial+  :: ShapeId+  -> IO SurfaceMaterial+getSurfaceMaterial a0 =+  alloca $ \pOut -> c_b3Shape_GetSurfaceMaterial a0 pOut >> peek pOut++-- | Like 'getSurfaceMaterial', but the result is written into a caller-supplied buffer.+getSurfaceMaterialInto+  :: ShapeId+  -> Ptr SurfaceMaterial+  -- ^ Result buffer.+  -> IO ()+getSurfaceMaterialInto a0 out =+  c_b3Shape_GetSurfaceMaterial a0 out++{- | Get the number of mesh surface materials.++Binds @b3Shape_GetMeshMaterialCount@.+-}+getMeshMaterialCount+  :: ShapeId+  -> IO Int+getMeshMaterialCount a0 =+  fromIntegral <$> (c_b3Shape_GetMeshMaterialCount a0)++{- | Set a surface material for a mesh shape.++Binds @b3Shape_SetMeshMaterial@.+-}+setMeshMaterial+  :: ShapeId+  -> SurfaceMaterial+  -> Int+  -- ^ @index@+  -> IO ()+setMeshMaterial a0 a1 a2 =+  with a1 $ \p1 ->+  c_b3Shape_SetMeshMaterial a0 p1 (fromIntegral a2)++{- | Get a surface material for a mesh shape++Binds @b3Shape_GetMeshSurfaceMaterial@.+-}+getMeshSurfaceMaterial+  :: ShapeId+  -> Int+  -- ^ @index@+  -> IO SurfaceMaterial+getMeshSurfaceMaterial a0 a1 =+  alloca $ \pOut -> c_b3Shape_GetMeshSurfaceMaterial a0 (fromIntegral a1) pOut >> peek pOut++-- | Like 'getMeshSurfaceMaterial', but the result is written into a caller-supplied buffer.+getMeshSurfaceMaterialInto+  :: ShapeId+  -> Int+  -- ^ @index@+  -> Ptr SurfaceMaterial+  -- ^ Result buffer.+  -> IO ()+getMeshSurfaceMaterialInto a0 a1 out =+  c_b3Shape_GetMeshSurfaceMaterial a0 (fromIntegral a1) out++{- | Get the shape filter++Binds @b3Shape_GetFilter@.+-}+getFilter+  :: ShapeId+  -> IO Filter+getFilter a0 =+  alloca $ \pOut -> c_b3Shape_GetFilter a0 pOut >> peek pOut++-- | Like 'getFilter', but the result is written into a caller-supplied buffer.+getFilterInto+  :: ShapeId+  -> Ptr Filter+  -- ^ Result buffer.+  -> IO ()+getFilterInto a0 out =+  c_b3Shape_GetFilter a0 out++{- | Set the current filter. This is almost as expensive as recreating the shape.++See also @b3ShapeDef::filter@.++Binds @b3Shape_SetFilter@.+-}+setFilter+  :: ShapeId+  -- ^ @shapeId@: the shape+  -> Filter+  -- ^ @filter@: the new filter+  -> Bool+  -- ^ @invokeContacts@: if true then the shape will have all contacts recomputed the next time step (expensive)+  -> IO ()+setFilter a0 a1 a2 =+  with a1 $ \p1 ->+  c_b3Shape_SetFilter a0 p1 (fromBool a2)++{- | Enable sensor events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors.++See also @b3ShapeDef::isSensor@.++Binds @b3Shape_EnableSensorEvents@.+-}+enableSensorEvents+  :: ShapeId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableSensorEvents a0 a1 =+  c_b3Shape_EnableSensorEvents a0 (fromBool a1)++{- | Returns true if sensor events are enabled++Binds @b3Shape_AreSensorEventsEnabled@.+-}+areSensorEventsEnabled+  :: ShapeId+  -> IO Bool+areSensorEventsEnabled a0 =+  toBool <$> (c_b3Shape_AreSensorEventsEnabled a0)++{- | Enable contact events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors.++See also @b3ShapeDef::enableContactEvents@.++Binds @b3Shape_EnableContactEvents@.+-}+enableContactEvents+  :: ShapeId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableContactEvents a0 a1 =+  c_b3Shape_EnableContactEvents a0 (fromBool a1)++{- | Returns true if contact events are enabled++Binds @b3Shape_AreContactEventsEnabled@.+-}+areContactEventsEnabled+  :: ShapeId+  -> IO Bool+areContactEventsEnabled a0 =+  toBool <$> (c_b3Shape_AreContactEventsEnabled a0)++{- | 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 also @b3PreSolveFcn@.++Binds @b3Shape_EnablePreSolveEvents@.+-}+enablePreSolveEvents+  :: ShapeId+  -> Bool+  -- ^ @flag@+  -> IO ()+enablePreSolveEvents a0 a1 =+  c_b3Shape_EnablePreSolveEvents a0 (fromBool a1)++{- | Returns true if pre-solve events are enabled++Binds @b3Shape_ArePreSolveEventsEnabled@.+-}+arePreSolveEventsEnabled+  :: ShapeId+  -> IO Bool+arePreSolveEventsEnabled a0 =+  toBool <$> (c_b3Shape_ArePreSolveEventsEnabled a0)++{- | Enable contact hit events for this shape. Ignored for sensors.++See also @b3WorldDef.hitEventThreshold@.++Binds @b3Shape_EnableHitEvents@.+-}+enableHitEvents+  :: ShapeId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableHitEvents a0 a1 =+  c_b3Shape_EnableHitEvents a0 (fromBool a1)++{- | Returns true if hit events are enabled++Binds @b3Shape_AreHitEventsEnabled@.+-}+areHitEventsEnabled+  :: ShapeId+  -> IO Bool+areHitEventsEnabled a0 =+  toBool <$> (c_b3Shape_AreHitEventsEnabled a0)++{- | Ray cast a shape directly. The ray runs from origin to origin + translation and the hit point+comes back as a world position, so the cast stays precise far from the world origin.++Binds @b3Shape_RayCast@.+-}+rayCast+  :: ShapeId+  -> Pos+  -- ^ @origin@+  -> Vec3+  -- ^ @translation@+  -> IO WorldCastOutput+rayCast a0 a1 a2 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  alloca $ \pOut -> c_b3Shape_RayCast a0 p1 p2 pOut >> peek pOut++{- | Get a copy of the shape\'s sphere. Asserts the type is correct.++Binds @b3Shape_GetSphere@.+-}+getSphere+  :: ShapeId+  -> IO Sphere+getSphere a0 =+  alloca $ \pOut -> c_b3Shape_GetSphere a0 pOut >> peek pOut++-- | Like 'getSphere', but the result is written into a caller-supplied buffer.+getSphereInto+  :: ShapeId+  -> Ptr Sphere+  -- ^ Result buffer.+  -> IO ()+getSphereInto a0 out =+  c_b3Shape_GetSphere a0 out++{- | Get a copy of the shape\'s capsule. Asserts the type is correct.++Binds @b3Shape_GetCapsule@.+-}+getCapsule+  :: ShapeId+  -> IO Capsule+getCapsule a0 =+  alloca $ \pOut -> c_b3Shape_GetCapsule a0 pOut >> peek pOut++-- | Like 'getCapsule', but the result is written into a caller-supplied buffer.+getCapsuleInto+  :: ShapeId+  -> Ptr Capsule+  -- ^ Result buffer.+  -> IO ()+getCapsuleInto a0 out =+  c_b3Shape_GetCapsule a0 out++{- | Get the shape\'s convex hull. Asserts the type is correct.++Binds @b3Shape_GetHull@.+-}+getHull+  :: ShapeId+  -> IO (Ptr HullData)+getHull a0 =+  c_b3Shape_GetHull a0++{- | Get the shape\'s mesh. Asserts the type is correct.++Binds @b3Shape_GetMesh@.+-}+getMesh+  :: ShapeId+  -> Ptr Mesh+  -- ^ Result buffer.+  -> IO ()+getMesh a0 out =+  c_b3Shape_GetMesh a0 out++{- | Get the shape\'s height field. Asserts the type is correct.++Binds @b3Shape_GetHeightField@.+-}+getHeightField+  :: ShapeId+  -> IO (Ptr HeightFieldData)+getHeightField a0 =+  c_b3Shape_GetHeightField a0++{- | Allows you to change a shape to be a sphere or update the current sphere.+This does not modify the mass properties.++See also @b3Body_ApplyMassFromShapes@.++Binds @b3Shape_SetSphere@.+-}+setSphere+  :: ShapeId+  -> Sphere+  -> IO ()+setSphere a0 a1 =+  with a1 $ \p1 ->+  c_b3Shape_SetSphere a0 p1++{- | Allows you to change a shape to be a capsule or update the current capsule.+This does not modify the mass properties.++See also @b3Body_ApplyMassFromShapes@.++Binds @b3Shape_SetCapsule@.+-}+setCapsule+  :: ShapeId+  -> Capsule+  -> IO ()+setCapsule a0 a1 =+  with a1 $ \p1 ->+  c_b3Shape_SetCapsule a0 p1++{- | Allows you to change a shape to be a hull or update the current hull.+This does not modify the mass properties.++See also @b3Body_ApplyMassFromShapes@.++Binds @b3Shape_SetHull@.+-}+setHull+  :: ShapeId+  -> Ptr HullData+  -> IO ()+setHull a0 a1 =+  c_b3Shape_SetHull a0 a1++{- | Allows you to change a shape to be a mesh or update the current mesh.+This does not modify the mass properties.++See also @b3Body_ApplyMassFromShapes@.++Binds @b3Shape_SetMesh@.+-}+setMesh+  :: ShapeId+  -> Ptr MeshData+  -> Vec3+  -- ^ @scale@+  -> IO ()+setMesh a0 a1 a2 =+  with a2 $ \p2 ->+  c_b3Shape_SetMesh a0 a1 p2++{- | Get the maximum capacity required for retrieving all the touching contacts on a shape++Binds @b3Shape_GetContactCapacity@.+-}+getContactCapacity+  :: ShapeId+  -> IO Int+getContactCapacity a0 =+  fromIntegral <$> (c_b3Shape_GetContactCapacity a0)++{- | 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.++__Warning:__ do not ignore the return value, it specifies the valid number of elements++Returns: the number of elements filled in the provided array++Binds @b3Shape_GetContactData@.+-}+getContactData+  :: ShapeId+  -> Ptr ContactData+  -> Int+  -- ^ @capacity@+  -> IO Int+getContactData a0 a1 a2 =+  fromIntegral <$> (c_b3Shape_GetContactData a0 a1 (fromIntegral a2))++{- | 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.++Returns: the required capacity to get all the overlaps in @b3Shape_GetSensorOverlaps@++Binds @b3Shape_GetSensorCapacity@.+-}+getSensorCapacity+  :: ShapeId+  -- ^ @shapeId@: the id of a sensor shape+  -> IO Int+getSensorCapacity a0 =+  fromIntegral <$> (c_b3Shape_GetSensorCapacity a0)++{- | Get the overlap data for a sensor shape.++__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++Returns: the number of elements filled in the provided array++Binds @b3Shape_GetSensorData@.+-}+getSensorData+  :: ShapeId+  -- ^ @shapeId@: the id of a sensor shape+  -> Ptr ShapeId+  -- ^ @visitorIds@: a user allocated array that is filled with the overlapping shapes (visitors)+  -> Int+  -- ^ @capacity@: the capacity of overlappedShapes+  -> IO Int+getSensorData a0 a1 a2 =+  fromIntegral <$> (c_b3Shape_GetSensorData a0 a1 (fromIntegral a2))++{- | Get the current world AABB++Binds @b3Shape_GetAABB@.+-}+getAABB+  :: ShapeId+  -> IO AABB+getAABB a0 =+  alloca $ \pOut -> c_b3Shape_GetAABB a0 pOut >> peek pOut++-- | Like 'getAABB', but the result is written into a caller-supplied buffer.+getAABBInto+  :: ShapeId+  -> Ptr AABB+  -- ^ Result buffer.+  -> IO ()+getAABBInto a0 out =+  c_b3Shape_GetAABB a0 out++{- | Compute the mass data for a shape++Binds @b3Shape_ComputeMassData@.+-}+computeMassData+  :: ShapeId+  -> IO MassData+computeMassData a0 =+  alloca $ \pOut -> c_b3Shape_ComputeMassData a0 pOut >> peek pOut++-- | Like 'computeMassData', but the result is written into a caller-supplied buffer.+computeMassDataInto+  :: ShapeId+  -> Ptr MassData+  -- ^ Result buffer.+  -> IO ()+computeMassDataInto a0 out =+  c_b3Shape_ComputeMassData a0 out++{- | Get the closest point on a shape to a target point. Target and result are in world space.++Binds @b3Shape_GetClosestPoint@.+-}+getClosestPoint+  :: ShapeId+  -> Vec3+  -- ^ @target@+  -> IO Vec3+getClosestPoint a0 a1 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b3Shape_GetClosestPoint a0 p1 pOut >> peek pOut++{- | 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.++Binds @b3Shape_ApplyWind@.+-}+applyWind+  :: ShapeId+  -- ^ @shapeId@: the shape id+  -> Vec3+  -- ^ @wind@: the wind velocity in world space+  -> Float+  -- ^ @drag@: the drag coefficient, the force that opposes the relative velocity+  -> Float+  -- ^ @lift@: the lift coefficient, the force that is perpendicular to the relative velocity+  -> Float+  -- ^ @maxSpeed@: the maximum relative speed. Speed cap is necessary for stability. Typically 10m\/s or less.+  -> Bool+  -- ^ @wake@: should this wake the body+  -> IO ()+applyWind a0 a1 a2 a3 a4 a5 =+  with a1 $ \p1 ->+  c_b3Shape_ApplyWind a0 p1 a2 a3 a4 (fromBool a5)++{- | Test a point for overlap with a shape. Mirrors the 2D testPoint,+which Box3D has no engine equivalent for: the shim runs the GJK+distance query with radii applied and reports a clamped distance+of zero. Convex shapes only: mesh, height field and compound+shapes report false, like the 2D original does for its chain+segments.++Provided by the hand-written shim @hs_b3Shape_TestPoint@; no upstream equivalent.+-}+testPoint+  :: ShapeId+  -> Pos+  -- ^ @point@+  -> IO Bool+testPoint a0 a1 =+  with a1 $ \p1 ->+  toBool <$> (c_b3Shape_TestPoint a0 p1)
+ src/Box3D/SphericalJoint.hsc view
@@ -0,0 +1,533 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.SphericalJoint+  ( SphericalJointDef (..)+  , defaultSphericalJointDef+  , create+  , enableConeLimit+  , isConeLimitEnabled+  , getConeLimit+  , setConeLimit+  , getConeAngle+  , enableTwistLimit+  , isTwistLimitEnabled+  , getLowerTwistLimit+  , getUpperTwistLimit+  , setTwistLimits+  , getTwistAngle+  , enableSpring+  , isSpringEnabled+  , setSpringHertz+  , getSpringHertz+  , setSpringDampingRatio+  , getSpringDampingRatio+  , setTargetRotation+  , getTargetRotation+  , getTargetRotationInto+  , enableMotor+  , isMotorEnabled+  , setMotorVelocity+  , getMotorVelocity+  , getMotorVelocityInto+  , getMotorTorque+  , getMotorTorqueInto+  , setMaxMotorTorque+  , getMaxMotorTorque+  )+  where++import Foreign+import Foreign.C.Types (CBool (..))+import Box3D.Internal (allocaOut)+import Box3D.Id (JointId (..), WorldId (..))+import Box3D.Joint (JointDef)+import Box3D.MathTypes (Quat, Vec3)++#include "box3d/box3d.h"+++{- | Spherical joint definition. A point on body B is fixed to a point on body A.+Allows rotation about the shared point.+-}+data SphericalJointDef = SphericalJointDef+  { base :: JointDef+  -- ^ Base joint definition+  , enableSpring :: CBool+  -- ^ Enable a rotational spring that attempts to align the two joint frames.+  , hertz :: Float+  -- ^ The spring stiffness Hertz, cycles per second. This may be clamped internally+  -- according to the time step to maintain stability. Non-negative number.+  , dampingRatio :: Float+  -- ^ The spring damping ratio, non-dimensional. Non-negative number.+  , targetRotation :: Quat+  -- ^ Target spring rotation, joint frame B relative to joint frame A.+  , enableConeLimit :: CBool+  -- ^ A flag to enable the cone limit. The cone is centered on the frameA z-axis.+  , coneAngle :: Float+  -- ^ The angle for the cone limit in radians. Valid range is \[0, pi\]+  , enableTwistLimit :: CBool+  -- ^ A flag to enable the twist limit. The twist is centered on the frameB z-axis.+  , lowerTwistAngle :: Float+  -- ^ The angle for the lower twist limit in radians. Minimum of -0.99*pi radians.+  , upperTwistAngle :: Float+  -- ^ The angle for the upper twist limit in radians. Maximum of 0.99*pi radians.+  , enableMotor :: CBool+  -- ^ A flag to enable the joint motor+  , maxMotorTorque :: Float+  -- ^ The maximum motor torque, typically in newton-meters. Non-negative number.+  , motorVelocity :: Vec3+  -- ^ The desired motor angular velocity in radians per second.+  }+  deriving (Eq, Show)++instance Storable SphericalJointDef where+  sizeOf _ = #{size b3SphericalJointDef}+  alignment _ = #{alignment b3SphericalJointDef}+  peek p =+    SphericalJointDef+      <$> #{peek b3SphericalJointDef, base} p+      <*> #{peek b3SphericalJointDef, enableSpring} p+      <*> #{peek b3SphericalJointDef, hertz} p+      <*> #{peek b3SphericalJointDef, dampingRatio} p+      <*> #{peek b3SphericalJointDef, targetRotation} p+      <*> #{peek b3SphericalJointDef, enableConeLimit} p+      <*> #{peek b3SphericalJointDef, coneAngle} p+      <*> #{peek b3SphericalJointDef, enableTwistLimit} p+      <*> #{peek b3SphericalJointDef, lowerTwistAngle} p+      <*> #{peek b3SphericalJointDef, upperTwistAngle} p+      <*> #{peek b3SphericalJointDef, enableMotor} p+      <*> #{peek b3SphericalJointDef, maxMotorTorque} p+      <*> #{peek b3SphericalJointDef, motorVelocity} p+  poke p (SphericalJointDef f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11 f12 f13) = do+    #{poke b3SphericalJointDef, base} p f1+    #{poke b3SphericalJointDef, enableSpring} p f2+    #{poke b3SphericalJointDef, hertz} p f3+    #{poke b3SphericalJointDef, dampingRatio} p f4+    #{poke b3SphericalJointDef, targetRotation} p f5+    #{poke b3SphericalJointDef, enableConeLimit} p f6+    #{poke b3SphericalJointDef, coneAngle} p f7+    #{poke b3SphericalJointDef, enableTwistLimit} p f8+    #{poke b3SphericalJointDef, lowerTwistAngle} p f9+    #{poke b3SphericalJointDef, upperTwistAngle} p f10+    #{poke b3SphericalJointDef, enableMotor} p f11+    #{poke b3SphericalJointDef, maxMotorTorque} p f12+    #{poke b3SphericalJointDef, motorVelocity} p f13++foreign import ccall unsafe "hs_b3DefaultSphericalJointDef"+  c_b3DefaultSphericalJointDef :: Ptr SphericalJointDef -> IO ()++defaultSphericalJointDef :: IO SphericalJointDef+defaultSphericalJointDef = allocaOut c_b3DefaultSphericalJointDef+++foreign import ccall unsafe "b3CreateSphericalJoint"+  c_b3CreateSphericalJoint :: WorldId -> Ptr SphericalJointDef -> IO JointId++foreign import ccall unsafe "b3SphericalJoint_EnableConeLimit"+  c_b3SphericalJoint_EnableConeLimit :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b3SphericalJoint_IsConeLimitEnabled"+  c_b3SphericalJoint_IsConeLimitEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b3SphericalJoint_GetConeLimit"+  c_b3SphericalJoint_GetConeLimit :: JointId -> IO Float++foreign import ccall unsafe "b3SphericalJoint_SetConeLimit"+  c_b3SphericalJoint_SetConeLimit :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3SphericalJoint_GetConeAngle"+  c_b3SphericalJoint_GetConeAngle :: JointId -> IO Float++foreign import ccall unsafe "b3SphericalJoint_EnableTwistLimit"+  c_b3SphericalJoint_EnableTwistLimit :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b3SphericalJoint_IsTwistLimitEnabled"+  c_b3SphericalJoint_IsTwistLimitEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b3SphericalJoint_GetLowerTwistLimit"+  c_b3SphericalJoint_GetLowerTwistLimit :: JointId -> IO Float++foreign import ccall unsafe "b3SphericalJoint_GetUpperTwistLimit"+  c_b3SphericalJoint_GetUpperTwistLimit :: JointId -> IO Float++foreign import ccall unsafe "b3SphericalJoint_SetTwistLimits"+  c_b3SphericalJoint_SetTwistLimits :: JointId -> Float -> Float -> IO ()++foreign import ccall unsafe "b3SphericalJoint_GetTwistAngle"+  c_b3SphericalJoint_GetTwistAngle :: JointId -> IO Float++foreign import ccall unsafe "b3SphericalJoint_EnableSpring"+  c_b3SphericalJoint_EnableSpring :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b3SphericalJoint_IsSpringEnabled"+  c_b3SphericalJoint_IsSpringEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b3SphericalJoint_SetSpringHertz"+  c_b3SphericalJoint_SetSpringHertz :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3SphericalJoint_GetSpringHertz"+  c_b3SphericalJoint_GetSpringHertz :: JointId -> IO Float++foreign import ccall unsafe "b3SphericalJoint_SetSpringDampingRatio"+  c_b3SphericalJoint_SetSpringDampingRatio :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3SphericalJoint_GetSpringDampingRatio"+  c_b3SphericalJoint_GetSpringDampingRatio :: JointId -> IO Float++foreign import ccall unsafe "hsg_b3SphericalJoint_SetTargetRotation"+  c_b3SphericalJoint_SetTargetRotation :: JointId -> Ptr Quat -> IO ()++foreign import ccall unsafe "hsg_b3SphericalJoint_GetTargetRotation"+  c_b3SphericalJoint_GetTargetRotation :: JointId -> Ptr Quat -> IO ()++foreign import ccall unsafe "b3SphericalJoint_EnableMotor"+  c_b3SphericalJoint_EnableMotor :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b3SphericalJoint_IsMotorEnabled"+  c_b3SphericalJoint_IsMotorEnabled :: JointId -> IO CBool++foreign import ccall unsafe "hsg_b3SphericalJoint_SetMotorVelocity"+  c_b3SphericalJoint_SetMotorVelocity :: JointId -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "hsg_b3SphericalJoint_GetMotorVelocity"+  c_b3SphericalJoint_GetMotorVelocity :: JointId -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "hsg_b3SphericalJoint_GetMotorTorque"+  c_b3SphericalJoint_GetMotorTorque :: JointId -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "b3SphericalJoint_SetMaxMotorTorque"+  c_b3SphericalJoint_SetMaxMotorTorque :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3SphericalJoint_GetMaxMotorTorque"+  c_b3SphericalJoint_GetMaxMotorTorque :: JointId -> IO Float++{- | Create a spherical joint++See also @b3SphericalJointDef@ for details.++Binds @b3CreateSphericalJoint@.+-}+create+  :: WorldId+  -> SphericalJointDef+  -> IO JointId+create a0 a1 =+  with a1 $ \p1 ->+  c_b3CreateSphericalJoint a0 p1++{- | Enable\/disable the spherical joint cone limit++Binds @b3SphericalJoint_EnableConeLimit@.+-}+enableConeLimit+  :: JointId+  -> Bool+  -- ^ @enableLimit@+  -> IO ()+enableConeLimit a0 a1 =+  c_b3SphericalJoint_EnableConeLimit a0 (fromBool a1)++{- | Is the spherical joint cone limit enabled?++Binds @b3SphericalJoint_IsConeLimitEnabled@.+-}+isConeLimitEnabled+  :: JointId+  -> IO Bool+isConeLimitEnabled a0 =+  toBool <$> (c_b3SphericalJoint_IsConeLimitEnabled a0)++{- | Get the spherical joint cone limit in radians++Binds @b3SphericalJoint_GetConeLimit@.+-}+getConeLimit+  :: JointId+  -> IO Float+getConeLimit a0 =+  c_b3SphericalJoint_GetConeLimit a0++{- | Set the spherical joint limits in radians++Binds @b3SphericalJoint_SetConeLimit@.+-}+setConeLimit+  :: JointId+  -> Float+  -- ^ @angleRadians@+  -> IO ()+setConeLimit a0 a1 =+  c_b3SphericalJoint_SetConeLimit a0 a1++{- | Get the spherical joint current cone angle in radians.++Binds @b3SphericalJoint_GetConeAngle@.+-}+getConeAngle+  :: JointId+  -> IO Float+getConeAngle a0 =+  c_b3SphericalJoint_GetConeAngle a0++{- | Enable\/disable the spherical joint limit++Binds @b3SphericalJoint_EnableTwistLimit@.+-}+enableTwistLimit+  :: JointId+  -> Bool+  -- ^ @enableLimit@+  -> IO ()+enableTwistLimit a0 a1 =+  c_b3SphericalJoint_EnableTwistLimit a0 (fromBool a1)++{- | Is the spherical joint limit enabled?++Binds @b3SphericalJoint_IsTwistLimitEnabled@.+-}+isTwistLimitEnabled+  :: JointId+  -> IO Bool+isTwistLimitEnabled a0 =+  toBool <$> (c_b3SphericalJoint_IsTwistLimitEnabled a0)++{- | Get the spherical joint lower limit in radians++Binds @b3SphericalJoint_GetLowerTwistLimit@.+-}+getLowerTwistLimit+  :: JointId+  -> IO Float+getLowerTwistLimit a0 =+  c_b3SphericalJoint_GetLowerTwistLimit a0++{- | Get the spherical joint upper limit in radians++Binds @b3SphericalJoint_GetUpperTwistLimit@.+-}+getUpperTwistLimit+  :: JointId+  -> IO Float+getUpperTwistLimit a0 =+  c_b3SphericalJoint_GetUpperTwistLimit a0++{- | Set the spherical joint limits in radians++Binds @b3SphericalJoint_SetTwistLimits@.+-}+setTwistLimits+  :: JointId+  -> Float+  -- ^ @lowerLimitRadians@+  -> Float+  -- ^ @upperLimitRadians@+  -> IO ()+setTwistLimits a0 a1 a2 =+  c_b3SphericalJoint_SetTwistLimits a0 a1 a2++{- | Get the spherical joint current twist angle in radians.++Binds @b3SphericalJoint_GetTwistAngle@.+-}+getTwistAngle+  :: JointId+  -> IO Float+getTwistAngle a0 =+  c_b3SphericalJoint_GetTwistAngle a0++{- | Enable\/disable the spherical joint spring++Binds @b3SphericalJoint_EnableSpring@.+-}+enableSpring+  :: JointId+  -> Bool+  -- ^ @enableSpring@+  -> IO ()+enableSpring a0 a1 =+  c_b3SphericalJoint_EnableSpring a0 (fromBool a1)++{- | Is the spherical angular spring enabled?++Binds @b3SphericalJoint_IsSpringEnabled@.+-}+isSpringEnabled+  :: JointId+  -> IO Bool+isSpringEnabled a0 =+  toBool <$> (c_b3SphericalJoint_IsSpringEnabled a0)++{- | Set the spherical joint spring stiffness in Hertz++Binds @b3SphericalJoint_SetSpringHertz@.+-}+setSpringHertz+  :: JointId+  -> Float+  -- ^ @hertz@+  -> IO ()+setSpringHertz a0 a1 =+  c_b3SphericalJoint_SetSpringHertz a0 a1++{- | Get the spherical joint spring stiffness in Hertz++Binds @b3SphericalJoint_GetSpringHertz@.+-}+getSpringHertz+  :: JointId+  -> IO Float+getSpringHertz a0 =+  c_b3SphericalJoint_GetSpringHertz a0++{- | Set the spherical joint spring damping ratio, non-dimensional++Binds @b3SphericalJoint_SetSpringDampingRatio@.+-}+setSpringDampingRatio+  :: JointId+  -> Float+  -- ^ @dampingRatio@+  -> IO ()+setSpringDampingRatio a0 a1 =+  c_b3SphericalJoint_SetSpringDampingRatio a0 a1++{- | Get the spherical joint spring damping ratio, non-dimensional++Binds @b3SphericalJoint_GetSpringDampingRatio@.+-}+getSpringDampingRatio+  :: JointId+  -> IO Float+getSpringDampingRatio a0 =+  c_b3SphericalJoint_GetSpringDampingRatio a0++{- | Set the spherical joint spring target rotation++Binds @b3SphericalJoint_SetTargetRotation@.+-}+setTargetRotation+  :: JointId+  -> Quat+  -- ^ @targetRotation@+  -> IO ()+setTargetRotation a0 a1 =+  with a1 $ \p1 ->+  c_b3SphericalJoint_SetTargetRotation a0 p1++{- | Get the spherical joint spring target rotation++Binds @b3SphericalJoint_GetTargetRotation@.+-}+getTargetRotation+  :: JointId+  -> IO Quat+getTargetRotation a0 =+  alloca $ \pOut -> c_b3SphericalJoint_GetTargetRotation a0 pOut >> peek pOut++-- | Like 'getTargetRotation', but the result is written into a caller-supplied buffer.+getTargetRotationInto+  :: JointId+  -> Ptr Quat+  -- ^ Result buffer.+  -> IO ()+getTargetRotationInto a0 out =+  c_b3SphericalJoint_GetTargetRotation a0 out++{- | Enable\/disable a spherical joint motor++Binds @b3SphericalJoint_EnableMotor@.+-}+enableMotor+  :: JointId+  -> Bool+  -- ^ @enableMotor@+  -> IO ()+enableMotor a0 a1 =+  c_b3SphericalJoint_EnableMotor a0 (fromBool a1)++{- | Is the spherical joint motor enabled?++Binds @b3SphericalJoint_IsMotorEnabled@.+-}+isMotorEnabled+  :: JointId+  -> IO Bool+isMotorEnabled a0 =+  toBool <$> (c_b3SphericalJoint_IsMotorEnabled a0)++{- | Set the spherical joint motor velocity in radians per second++Binds @b3SphericalJoint_SetMotorVelocity@.+-}+setMotorVelocity+  :: JointId+  -> Vec3+  -- ^ @motorVelocity@+  -> IO ()+setMotorVelocity a0 a1 =+  with a1 $ \p1 ->+  c_b3SphericalJoint_SetMotorVelocity a0 p1++{- | Get the spherical joint motor velocity in radians per second++Binds @b3SphericalJoint_GetMotorVelocity@.+-}+getMotorVelocity+  :: JointId+  -> IO Vec3+getMotorVelocity a0 =+  alloca $ \pOut -> c_b3SphericalJoint_GetMotorVelocity a0 pOut >> peek pOut++-- | Like 'getMotorVelocity', but the result is written into a caller-supplied buffer.+getMotorVelocityInto+  :: JointId+  -> Ptr Vec3+  -- ^ Result buffer.+  -> IO ()+getMotorVelocityInto a0 out =+  c_b3SphericalJoint_GetMotorVelocity a0 out++{- | Get the spherical joint current motor torque, usually in newton-meters++Binds @b3SphericalJoint_GetMotorTorque@.+-}+getMotorTorque+  :: JointId+  -> IO Vec3+getMotorTorque a0 =+  alloca $ \pOut -> c_b3SphericalJoint_GetMotorTorque a0 pOut >> peek pOut++-- | Like 'getMotorTorque', but the result is written into a caller-supplied buffer.+getMotorTorqueInto+  :: JointId+  -> Ptr Vec3+  -- ^ Result buffer.+  -> IO ()+getMotorTorqueInto a0 out =+  c_b3SphericalJoint_GetMotorTorque a0 out++{- | Set the spherical joint maximum motor torque, usually in newton-meters++Binds @b3SphericalJoint_SetMaxMotorTorque@.+-}+setMaxMotorTorque+  :: JointId+  -> Float+  -- ^ @torque@+  -> IO ()+setMaxMotorTorque a0 a1 =+  c_b3SphericalJoint_SetMaxMotorTorque a0 a1++{- | Get the spherical joint maximum motor torque, usually in newton-meters++Binds @b3SphericalJoint_GetMaxMotorTorque@.+-}+getMaxMotorTorque+  :: JointId+  -> IO Float+getMaxMotorTorque a0 =+  c_b3SphericalJoint_GetMaxMotorTorque a0
+ src/Box3D/Tags.hs view
@@ -0,0 +1,233 @@+{-| Phantom tag types for the by-reference structs and callbacks of the Box3D API.++These carry no fields and no 'Storable'; a raw FFI signature reads+@Ptr Foo@ / @FunPtr Foo@ while the caller supplies the value.+-}+module Box3D.Tags+  ( AllocFcn+  , AssertFcn+  , BoxHull+  , CastResultFcn+  , ChildShape+  , CompoundQueryFcn+  , CreateDebugShapeCallback+  , CustomFilterFcn+  , DestroyDebugShapeCallback+  , DrawBoundsFcn+  , DrawBoxFcn+  , DrawCapsuleFcn+  , DrawPointFcn+  , DrawSegmentFcn+  , DrawShapeFcn+  , DrawSphereFcn+  , DrawStringFcn+  , DrawTransformFcn+  , EnqueueTaskCallback+  , FinishTaskCallback+  , FreeFcn+  , FrictionCallback+  , LocalManifold+  , LogFcn+  , MeshQueryFcn+  , MoverFilterFcn+  , OverlapResultFcn+  , PlaneResultFcn+  , PreSolveFcn+  , RecPlayer+  , Recording+  , RestitutionCallback+  , Simplex+  , TaskCallback+  , TreeBoxCastCallbackFcn+  , TreeNode+  , TreeQueryCallbackFcn+  , TreeQueryClosestCallbackFcn+  , TreeRayCastCallbackFcn+  )+where++-- | Query callback.+data MeshQueryFcn++{- | 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@.+-}+data TaskCallback++{- | 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.+-}+data EnqueueTaskCallback++{- | Finishes a user task object that wraps a Box3D task. This must block until the task has completed.+The step blocks here on the tasks it spawned, so @b3World_Step@ holds its stack across every+fork\/join. Drive it from a thread you can dedicate to the step, or from a fiber this callback can+park to free the underlying thread. In a job system that cannot park a job\'s stack, do not call+@b3World_Step@ from inside a job: a job that blocks on its own sub-jobs without yielding its thread+can deadlock. The in-tree scheduler instead runs other pending tasks on the waiting thread.+-}+data FinishTaskCallback++{- | 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.+-}+data CreateDebugShapeCallback++data DestroyDebugShapeCallback++{- | 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.+-}+data FrictionCallback++{- | 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.+-}+data RestitutionCallback++{- | 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+-}+data CustomFilterFcn++{- | 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+-}+data PreSolveFcn++{- | Prototype callback for overlap queries.+Called for each shape found in the query.++See also @b3World_OverlapAABB@.+-}+data OverlapResultFcn++{- | 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++See also @b3World_CastRay@.+-}+data CastResultFcn++-- | This function receives proxies found in the AABB query.+data TreeQueryCallbackFcn++-- | This function receives the minimum distance squared so far and proxy to check in the closest query.+data TreeQueryClosestCallbackFcn++{- | This function receives clipped AABB cast input for a proxy. The function returns the new cast+fraction.++- return a value of 0 to terminate the cast+- return a value less than input-\>maxFraction to clip the cast+- return a value of input-\>maxFraction to continue the cast without clipping+-}+data TreeBoxCastCallbackFcn++{- | 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+-}+data TreeRayCastCallbackFcn++{- | Used to collect collision planes for character movers.+Return true to continue gathering planes.+-}+data PlaneResultFcn++{- | Used to filter shapes for shape casting character movers.+Return true to accept the collision+-}+data MoverFilterFcn++-- | Callback for compound overlap queries.+data CompoundQueryFcn++-- | Prototype for user allocation function.+data AllocFcn++-- | Prototype for user free function.+data FreeFcn++-- | Prototype for the user assert callback. Return 0 to skip the debugger break.+data AssertFcn++-- | Prototype for user log callback. Used to log warnings.+data LogFcn++{- | Draws a user shape. The userShape pointer is owned by the application and is known to Box3D as+an opaque pointer returned from @b3CreateDebugShapeCallback@. When this is called the drawn shape has+passed a culling test against drawingBounds below.+-}+data DrawShapeFcn++-- | Draw a line segment.+data DrawSegmentFcn++-- | Draw a transform. Choose your own length scale.+data DrawTransformFcn++-- | Draw a point.+data DrawPointFcn++-- | Draw a sphere.+data DrawSphereFcn++-- | Draw a capsule.+data DrawCapsuleFcn++-- | Draw a bounding box.+data DrawBoundsFcn++-- | Draw an oriented box.+data DrawBoxFcn++-- | Draw a string in world space+data DrawStringFcn++data BoxHull+data ChildShape+data LocalManifold+data RecPlayer+data Recording+data Simplex+data TreeNode
+ src/Box3D/TorusMesh.hs view
@@ -0,0 +1,30 @@+-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.TorusMesh+  ( create+  )+where++import Box3D.Mesh (MeshData)+import Foreign+import Foreign.C.Types (CInt (..))++foreign import ccall unsafe "b3CreateTorusMesh"+  c_b3CreateTorusMesh :: CInt -> CInt -> Float -> Float -> IO (Ptr MeshData)++{- | Create a torus mesh.++Binds @b3CreateTorusMesh@.+-}+create+  :: Int+  -- ^ @radialResolution@+  -> Int+  -- ^ @tubularResolution@+  -> Float+  -- ^ @radius@+  -> Float+  -- ^ @thickness@+  -> IO (Ptr MeshData)+create a0 a1 a2 a3 =+  c_b3CreateTorusMesh (fromIntegral a0) (fromIntegral a1) a2 a3
+ src/Box3D/UserData.hs view
@@ -0,0 +1,55 @@+{-| Integer-friendly user data: stash an entity\/index id in the engine's+@void* userData@ slots without manual pointer casts. The id travels as the+pointer's bit pattern (it is never dereferenced), so any 'Int' that fits a+word round-trips — which makes contact events and queries resolvable back+to your entities.+-}+module Box3D.UserData+  ( HasUserData (..)+  , setUserIndex+  , getUserIndex+  , userIndexToPtr+  , ptrToUserIndex+  ) where++import Foreign.Ptr (IntPtr (..), Ptr, intPtrToPtr, ptrToIntPtr)++import Box3D.Body qualified as Body+import Box3D.Id (BodyId, JointId, ShapeId)+import Box3D.Joint qualified as Joint+import Box3D.Shape qualified as Shape++-- | Handles carrying a @userData@ pointer slot.+class HasUserData id where+  setUserData :: id -> Ptr () -> IO ()+  getUserData :: id -> IO (Ptr ())++instance HasUserData BodyId where+  setUserData = Body.setUserData+  getUserData = Body.getUserData++instance HasUserData ShapeId where+  setUserData = Shape.setUserData+  getUserData = Shape.getUserData++instance HasUserData JointId where+  setUserData = Joint.setUserData+  getUserData = Joint.getUserData++{- | Encode an id as a user-data pointer (also for the @*Def@ records'+@userData@ fields).+-}+userIndexToPtr :: Int -> Ptr ()+userIndexToPtr = intPtrToPtr . IntPtr++-- | Decode a user-data pointer written by 'userIndexToPtr'.+ptrToUserIndex :: Ptr () -> Int+ptrToUserIndex p = case ptrToIntPtr p of IntPtr i -> i++-- | Stash an entity\/index id on a body, shape or joint.+setUserIndex :: (HasUserData id) => id -> Int -> IO ()+setUserIndex x = setUserData x . userIndexToPtr++-- | Read back an id stored with 'setUserIndex'.+getUserIndex :: (HasUserData id) => id -> IO Int+getUserIndex x = ptrToUserIndex <$> getUserData x
+ src/Box3D/Wave.hs view
@@ -0,0 +1,36 @@+-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.Wave+  ( create+  )+where++import Box3D.HeightField (HeightFieldData)+import Box3D.MathTypes (Vec3)+import Foreign+import Foreign.C.Types (CBool (..), CInt (..))++foreign import ccall unsafe "hsg_b3CreateWave"+  c_b3CreateWave :: CInt -> CInt -> Ptr Vec3 -> Float -> Float -> CBool -> IO (Ptr HeightFieldData)++{- | Create a wave grid as a height field.++Binds @b3CreateWave@.+-}+create+  :: Int+  -- ^ @rowCount@+  -> Int+  -- ^ @columnCount@+  -> Vec3+  -- ^ @scale@+  -> Float+  -- ^ @rowFrequency@+  -> Float+  -- ^ @columnFrequency@+  -> Bool+  -- ^ @makeHoles@+  -> IO (Ptr HeightFieldData)+create a0 a1 a2 a3 a4 a5 =+  with a2 $ \p2 ->+    c_b3CreateWave (fromIntegral a0) (fromIntegral a1) p2 a3 a4 (fromBool a5)
+ src/Box3D/WaveMesh.hs view
@@ -0,0 +1,34 @@+-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.WaveMesh+  ( create+  )+where++import Box3D.Mesh (MeshData)+import Foreign+import Foreign.C.Types (CInt (..))++foreign import ccall unsafe "b3CreateWaveMesh"+  c_b3CreateWaveMesh :: CInt -> CInt -> Float -> Float -> Float -> Float -> IO (Ptr MeshData)++{- | Create a wave mesh along the x and z axes.++Binds @b3CreateWaveMesh@.+-}+create+  :: Int+  -- ^ @xCount@+  -> Int+  -- ^ @zCount@+  -> Float+  -- ^ @cellWidth@+  -> Float+  -- ^ @amplitude@+  -> Float+  -- ^ @rowFrequency@+  -> Float+  -- ^ @columnFrequency@+  -> IO (Ptr MeshData)+create a0 a1 a2 a3 a4 a5 =+  c_b3CreateWaveMesh (fromIntegral a0) (fromIntegral a1) a2 a3 a4 a5
+ src/Box3D/WeldJoint.hsc view
@@ -0,0 +1,201 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.WeldJoint+  ( WeldJointDef (..)+  , defaultWeldJointDef+  , create+  , setLinearHertz+  , getLinearHertz+  , setLinearDampingRatio+  , getLinearDampingRatio+  , setAngularHertz+  , getAngularHertz+  , setAngularDampingRatio+  , getAngularDampingRatio+  )+  where++import Foreign+import Box3D.Internal (allocaOut)+import Box3D.Id (JointId (..), WorldId (..))+import Box3D.Joint (JointDef)++#include "box3d/box3d.h"+++{- | 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+-}+data WeldJointDef = WeldJointDef+  { base :: JointDef+  -- ^ Base joint definition+  , linearHertz :: Float+  -- ^ Linear stiffness expressed as Hertz (cycles per second). Use zero for maximum stiffness.+  , angularHertz :: Float+  -- ^ Angular stiffness as Hertz (cycles per second). Use zero for maximum stiffness.+  , linearDampingRatio :: Float+  -- ^ Linear damping ratio, non-dimensional. Use 1 for critical damping.+  , angularDampingRatio :: Float+  -- ^ Linear damping ratio, non-dimensional. Use 1 for critical damping.+  }+  deriving (Eq, Show)++instance Storable WeldJointDef where+  sizeOf _ = #{size b3WeldJointDef}+  alignment _ = #{alignment b3WeldJointDef}+  peek p =+    WeldJointDef+      <$> #{peek b3WeldJointDef, base} p+      <*> #{peek b3WeldJointDef, linearHertz} p+      <*> #{peek b3WeldJointDef, angularHertz} p+      <*> #{peek b3WeldJointDef, linearDampingRatio} p+      <*> #{peek b3WeldJointDef, angularDampingRatio} p+  poke p (WeldJointDef f1 f2 f3 f4 f5) = do+    #{poke b3WeldJointDef, base} p f1+    #{poke b3WeldJointDef, linearHertz} p f2+    #{poke b3WeldJointDef, angularHertz} p f3+    #{poke b3WeldJointDef, linearDampingRatio} p f4+    #{poke b3WeldJointDef, angularDampingRatio} p f5++foreign import ccall unsafe "hs_b3DefaultWeldJointDef"+  c_b3DefaultWeldJointDef :: Ptr WeldJointDef -> IO ()++defaultWeldJointDef :: IO WeldJointDef+defaultWeldJointDef = allocaOut c_b3DefaultWeldJointDef+++foreign import ccall unsafe "b3CreateWeldJoint"+  c_b3CreateWeldJoint :: WorldId -> Ptr WeldJointDef -> IO JointId++foreign import ccall unsafe "b3WeldJoint_SetLinearHertz"+  c_b3WeldJoint_SetLinearHertz :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3WeldJoint_GetLinearHertz"+  c_b3WeldJoint_GetLinearHertz :: JointId -> IO Float++foreign import ccall unsafe "b3WeldJoint_SetLinearDampingRatio"+  c_b3WeldJoint_SetLinearDampingRatio :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3WeldJoint_GetLinearDampingRatio"+  c_b3WeldJoint_GetLinearDampingRatio :: JointId -> IO Float++foreign import ccall unsafe "b3WeldJoint_SetAngularHertz"+  c_b3WeldJoint_SetAngularHertz :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3WeldJoint_GetAngularHertz"+  c_b3WeldJoint_GetAngularHertz :: JointId -> IO Float++foreign import ccall unsafe "b3WeldJoint_SetAngularDampingRatio"+  c_b3WeldJoint_SetAngularDampingRatio :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3WeldJoint_GetAngularDampingRatio"+  c_b3WeldJoint_GetAngularDampingRatio :: JointId -> IO Float++{- | Create a weld joint++See also @b3WeldJointDef@ for details.++Binds @b3CreateWeldJoint@.+-}+create+  :: WorldId+  -> WeldJointDef+  -> IO JointId+create a0 a1 =+  with a1 $ \p1 ->+  c_b3CreateWeldJoint a0 p1++{- | Set the weld joint linear stiffness in Hertz. 0 is rigid.++Binds @b3WeldJoint_SetLinearHertz@.+-}+setLinearHertz+  :: JointId+  -> Float+  -- ^ @hertz@+  -> IO ()+setLinearHertz a0 a1 =+  c_b3WeldJoint_SetLinearHertz a0 a1++{- | Get the weld joint linear stiffness in Hertz++Binds @b3WeldJoint_GetLinearHertz@.+-}+getLinearHertz+  :: JointId+  -> IO Float+getLinearHertz a0 =+  c_b3WeldJoint_GetLinearHertz a0++{- | Set the weld joint linear damping ratio (non-dimensional)++Binds @b3WeldJoint_SetLinearDampingRatio@.+-}+setLinearDampingRatio+  :: JointId+  -> Float+  -- ^ @dampingRatio@+  -> IO ()+setLinearDampingRatio a0 a1 =+  c_b3WeldJoint_SetLinearDampingRatio a0 a1++{- | Get the weld joint linear damping ratio (non-dimensional)++Binds @b3WeldJoint_GetLinearDampingRatio@.+-}+getLinearDampingRatio+  :: JointId+  -> IO Float+getLinearDampingRatio a0 =+  c_b3WeldJoint_GetLinearDampingRatio a0++{- | Set the weld joint angular stiffness in Hertz. 0 is rigid.++Binds @b3WeldJoint_SetAngularHertz@.+-}+setAngularHertz+  :: JointId+  -> Float+  -- ^ @hertz@+  -> IO ()+setAngularHertz a0 a1 =+  c_b3WeldJoint_SetAngularHertz a0 a1++{- | Get the weld joint angular stiffness in Hertz++Binds @b3WeldJoint_GetAngularHertz@.+-}+getAngularHertz+  :: JointId+  -> IO Float+getAngularHertz a0 =+  c_b3WeldJoint_GetAngularHertz a0++{- | Set weld joint angular damping ratio, non-dimensional++Binds @b3WeldJoint_SetAngularDampingRatio@.+-}+setAngularDampingRatio+  :: JointId+  -> Float+  -- ^ @dampingRatio@+  -> IO ()+setAngularDampingRatio a0 a1 =+  c_b3WeldJoint_SetAngularDampingRatio a0 a1++{- | Get the weld joint angular damping ratio, non-dimensional++Binds @b3WeldJoint_GetAngularDampingRatio@.+-}+getAngularDampingRatio+  :: JointId+  -> IO Float+getAngularDampingRatio a0 =+  c_b3WeldJoint_GetAngularDampingRatio a0
+ src/Box3D/WheelJoint.hsc view
@@ -0,0 +1,672 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.WheelJoint+  ( WheelJointDef (..)+  , defaultWheelJointDef+  , create+  , enableSuspension+  , isSuspensionEnabled+  , setSuspensionHertz+  , getSuspensionHertz+  , setSuspensionDampingRatio+  , getSuspensionDampingRatio+  , enableSuspensionLimit+  , isSuspensionLimitEnabled+  , getLowerSuspensionLimit+  , getUpperSuspensionLimit+  , setSuspensionLimits+  , enableSpinMotor+  , isSpinMotorEnabled+  , setSpinMotorSpeed+  , getSpinMotorSpeed+  , setMaxSpinTorque+  , getMaxSpinTorque+  , getSpinSpeed+  , getSpinTorque+  , enableSteering+  , isSteeringEnabled+  , setSteeringHertz+  , getSteeringHertz+  , setSteeringDampingRatio+  , getSteeringDampingRatio+  , setMaxSteeringTorque+  , getMaxSteeringTorque+  , enableSteeringLimit+  , isSteeringLimitEnabled+  , getLowerSteeringLimit+  , getUpperSteeringLimit+  , setSteeringLimits+  , setTargetSteeringAngle+  , getTargetSteeringAngle+  , getSteeringAngle+  , getSteeringTorque+  )+  where++import Foreign+import Foreign.C.Types (CBool (..))+import Box3D.Internal (allocaOut)+import Box3D.Id (JointId (..), WorldId (..))+import Box3D.Joint (JointDef)++#include "box3d/box3d.h"+++{- | 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 translates along the local x-axis in frame A.+The wheel can optionally steer along the x-axis in frame A.+-}+data WheelJointDef = WheelJointDef+  { base :: JointDef+  -- ^ Base joint definition+  , enableSuspensionSpring :: CBool+  -- ^ Enable a linear spring along the local axis+  , suspensionHertz :: Float+  -- ^ Spring stiffness in Hertz+  , suspensionDampingRatio :: Float+  -- ^ Spring damping ratio, non-dimensional+  , enableSuspensionLimit :: CBool+  -- ^ Enable\/disable the joint linear limit+  , lowerSuspensionLimit :: Float+  -- ^ The lower suspension translation limit+  , upperSuspensionLimit :: Float+  -- ^ The upper translation limit+  , enableSpinMotor :: CBool+  -- ^ Enable\/disable the joint rotational motor+  , maxSpinTorque :: Float+  -- ^ The maximum motor torque, typically in newton-meters+  , spinSpeed :: Float+  -- ^ The desired motor speed in radians per second+  , enableSteering :: CBool+  -- ^ Enable steering, otherwise the steering is fixed forward+  , steeringHertz :: Float+  -- ^ Steering stiffness in Hertz+  , steeringDampingRatio :: Float+  -- ^ Spring damping ratio, non-dimensional+  , targetSteeringAngle :: Float+  -- ^ The target steering angle in radians+  , maxSteeringTorque :: Float+  -- ^ The maximum steering torque in N*m+  , enableSteeringLimit :: CBool+  -- ^ Enable\/disable the steering angular limit+  , lowerSteeringLimit :: Float+  -- ^ The lower steering angle in radians+  , upperSteeringLimit :: Float+  -- ^ The upper steering angle in radians+  }+  deriving (Eq, Show)++instance Storable WheelJointDef where+  sizeOf _ = #{size b3WheelJointDef}+  alignment _ = #{alignment b3WheelJointDef}+  peek p =+    WheelJointDef+      <$> #{peek b3WheelJointDef, base} p+      <*> #{peek b3WheelJointDef, enableSuspensionSpring} p+      <*> #{peek b3WheelJointDef, suspensionHertz} p+      <*> #{peek b3WheelJointDef, suspensionDampingRatio} p+      <*> #{peek b3WheelJointDef, enableSuspensionLimit} p+      <*> #{peek b3WheelJointDef, lowerSuspensionLimit} p+      <*> #{peek b3WheelJointDef, upperSuspensionLimit} p+      <*> #{peek b3WheelJointDef, enableSpinMotor} p+      <*> #{peek b3WheelJointDef, maxSpinTorque} p+      <*> #{peek b3WheelJointDef, spinSpeed} p+      <*> #{peek b3WheelJointDef, enableSteering} p+      <*> #{peek b3WheelJointDef, steeringHertz} p+      <*> #{peek b3WheelJointDef, steeringDampingRatio} p+      <*> #{peek b3WheelJointDef, targetSteeringAngle} p+      <*> #{peek b3WheelJointDef, maxSteeringTorque} p+      <*> #{peek b3WheelJointDef, enableSteeringLimit} p+      <*> #{peek b3WheelJointDef, lowerSteeringLimit} p+      <*> #{peek b3WheelJointDef, upperSteeringLimit} p+  poke p (WheelJointDef f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11 f12 f13 f14 f15 f16 f17 f18) = do+    #{poke b3WheelJointDef, base} p f1+    #{poke b3WheelJointDef, enableSuspensionSpring} p f2+    #{poke b3WheelJointDef, suspensionHertz} p f3+    #{poke b3WheelJointDef, suspensionDampingRatio} p f4+    #{poke b3WheelJointDef, enableSuspensionLimit} p f5+    #{poke b3WheelJointDef, lowerSuspensionLimit} p f6+    #{poke b3WheelJointDef, upperSuspensionLimit} p f7+    #{poke b3WheelJointDef, enableSpinMotor} p f8+    #{poke b3WheelJointDef, maxSpinTorque} p f9+    #{poke b3WheelJointDef, spinSpeed} p f10+    #{poke b3WheelJointDef, enableSteering} p f11+    #{poke b3WheelJointDef, steeringHertz} p f12+    #{poke b3WheelJointDef, steeringDampingRatio} p f13+    #{poke b3WheelJointDef, targetSteeringAngle} p f14+    #{poke b3WheelJointDef, maxSteeringTorque} p f15+    #{poke b3WheelJointDef, enableSteeringLimit} p f16+    #{poke b3WheelJointDef, lowerSteeringLimit} p f17+    #{poke b3WheelJointDef, upperSteeringLimit} p f18++foreign import ccall unsafe "hs_b3DefaultWheelJointDef"+  c_b3DefaultWheelJointDef :: Ptr WheelJointDef -> IO ()++defaultWheelJointDef :: IO WheelJointDef+defaultWheelJointDef = allocaOut c_b3DefaultWheelJointDef+++foreign import ccall unsafe "b3CreateWheelJoint"+  c_b3CreateWheelJoint :: WorldId -> Ptr WheelJointDef -> IO JointId++foreign import ccall unsafe "b3WheelJoint_EnableSuspension"+  c_b3WheelJoint_EnableSuspension :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b3WheelJoint_IsSuspensionEnabled"+  c_b3WheelJoint_IsSuspensionEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b3WheelJoint_SetSuspensionHertz"+  c_b3WheelJoint_SetSuspensionHertz :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3WheelJoint_GetSuspensionHertz"+  c_b3WheelJoint_GetSuspensionHertz :: JointId -> IO Float++foreign import ccall unsafe "b3WheelJoint_SetSuspensionDampingRatio"+  c_b3WheelJoint_SetSuspensionDampingRatio :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3WheelJoint_GetSuspensionDampingRatio"+  c_b3WheelJoint_GetSuspensionDampingRatio :: JointId -> IO Float++foreign import ccall unsafe "b3WheelJoint_EnableSuspensionLimit"+  c_b3WheelJoint_EnableSuspensionLimit :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b3WheelJoint_IsSuspensionLimitEnabled"+  c_b3WheelJoint_IsSuspensionLimitEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b3WheelJoint_GetLowerSuspensionLimit"+  c_b3WheelJoint_GetLowerSuspensionLimit :: JointId -> IO Float++foreign import ccall unsafe "b3WheelJoint_GetUpperSuspensionLimit"+  c_b3WheelJoint_GetUpperSuspensionLimit :: JointId -> IO Float++foreign import ccall unsafe "b3WheelJoint_SetSuspensionLimits"+  c_b3WheelJoint_SetSuspensionLimits :: JointId -> Float -> Float -> IO ()++foreign import ccall unsafe "b3WheelJoint_EnableSpinMotor"+  c_b3WheelJoint_EnableSpinMotor :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b3WheelJoint_IsSpinMotorEnabled"+  c_b3WheelJoint_IsSpinMotorEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b3WheelJoint_SetSpinMotorSpeed"+  c_b3WheelJoint_SetSpinMotorSpeed :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3WheelJoint_GetSpinMotorSpeed"+  c_b3WheelJoint_GetSpinMotorSpeed :: JointId -> IO Float++foreign import ccall unsafe "b3WheelJoint_SetMaxSpinTorque"+  c_b3WheelJoint_SetMaxSpinTorque :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3WheelJoint_GetMaxSpinTorque"+  c_b3WheelJoint_GetMaxSpinTorque :: JointId -> IO Float++foreign import ccall unsafe "b3WheelJoint_GetSpinSpeed"+  c_b3WheelJoint_GetSpinSpeed :: JointId -> IO Float++foreign import ccall unsafe "b3WheelJoint_GetSpinTorque"+  c_b3WheelJoint_GetSpinTorque :: JointId -> IO Float++foreign import ccall unsafe "b3WheelJoint_EnableSteering"+  c_b3WheelJoint_EnableSteering :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b3WheelJoint_IsSteeringEnabled"+  c_b3WheelJoint_IsSteeringEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b3WheelJoint_SetSteeringHertz"+  c_b3WheelJoint_SetSteeringHertz :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3WheelJoint_GetSteeringHertz"+  c_b3WheelJoint_GetSteeringHertz :: JointId -> IO Float++foreign import ccall unsafe "b3WheelJoint_SetSteeringDampingRatio"+  c_b3WheelJoint_SetSteeringDampingRatio :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3WheelJoint_GetSteeringDampingRatio"+  c_b3WheelJoint_GetSteeringDampingRatio :: JointId -> IO Float++foreign import ccall unsafe "b3WheelJoint_SetMaxSteeringTorque"+  c_b3WheelJoint_SetMaxSteeringTorque :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3WheelJoint_GetMaxSteeringTorque"+  c_b3WheelJoint_GetMaxSteeringTorque :: JointId -> IO Float++foreign import ccall unsafe "b3WheelJoint_EnableSteeringLimit"+  c_b3WheelJoint_EnableSteeringLimit :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b3WheelJoint_IsSteeringLimitEnabled"+  c_b3WheelJoint_IsSteeringLimitEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b3WheelJoint_GetLowerSteeringLimit"+  c_b3WheelJoint_GetLowerSteeringLimit :: JointId -> IO Float++foreign import ccall unsafe "b3WheelJoint_GetUpperSteeringLimit"+  c_b3WheelJoint_GetUpperSteeringLimit :: JointId -> IO Float++foreign import ccall unsafe "b3WheelJoint_SetSteeringLimits"+  c_b3WheelJoint_SetSteeringLimits :: JointId -> Float -> Float -> IO ()++foreign import ccall unsafe "b3WheelJoint_SetTargetSteeringAngle"+  c_b3WheelJoint_SetTargetSteeringAngle :: JointId -> Float -> IO ()++foreign import ccall unsafe "b3WheelJoint_GetTargetSteeringAngle"+  c_b3WheelJoint_GetTargetSteeringAngle :: JointId -> IO Float++foreign import ccall unsafe "b3WheelJoint_GetSteeringAngle"+  c_b3WheelJoint_GetSteeringAngle :: JointId -> IO Float++foreign import ccall unsafe "b3WheelJoint_GetSteeringTorque"+  c_b3WheelJoint_GetSteeringTorque :: JointId -> IO Float++{- | Create a wheel joint.++See also @b3WheelJointDef@ for details..++Binds @b3CreateWheelJoint@.+-}+create+  :: WorldId+  -> WheelJointDef+  -> IO JointId+create a0 a1 =+  with a1 $ \p1 ->+  c_b3CreateWheelJoint a0 p1++{- | Enable\/disable the wheel joint spring.++Binds @b3WheelJoint_EnableSuspension@.+-}+enableSuspension+  :: JointId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableSuspension a0 a1 =+  c_b3WheelJoint_EnableSuspension a0 (fromBool a1)++{- | Is the wheel joint spring enabled?++Binds @b3WheelJoint_IsSuspensionEnabled@.+-}+isSuspensionEnabled+  :: JointId+  -> IO Bool+isSuspensionEnabled a0 =+  toBool <$> (c_b3WheelJoint_IsSuspensionEnabled a0)++{- | Set the wheel joint stiffness in Hertz.++Binds @b3WheelJoint_SetSuspensionHertz@.+-}+setSuspensionHertz+  :: JointId+  -> Float+  -- ^ @hertz@+  -> IO ()+setSuspensionHertz a0 a1 =+  c_b3WheelJoint_SetSuspensionHertz a0 a1++{- | Get the wheel joint stiffness in Hertz.++Binds @b3WheelJoint_GetSuspensionHertz@.+-}+getSuspensionHertz+  :: JointId+  -> IO Float+getSuspensionHertz a0 =+  c_b3WheelJoint_GetSuspensionHertz a0++{- | Set the wheel joint damping ratio, non-dimensional.++Binds @b3WheelJoint_SetSuspensionDampingRatio@.+-}+setSuspensionDampingRatio+  :: JointId+  -> Float+  -- ^ @dampingRatio@+  -> IO ()+setSuspensionDampingRatio a0 a1 =+  c_b3WheelJoint_SetSuspensionDampingRatio a0 a1++{- | Get the wheel joint damping ratio, non-dimensional.++Binds @b3WheelJoint_GetSuspensionDampingRatio@.+-}+getSuspensionDampingRatio+  :: JointId+  -> IO Float+getSuspensionDampingRatio a0 =+  c_b3WheelJoint_GetSuspensionDampingRatio a0++{- | Enable\/disable the wheel joint limit.++Binds @b3WheelJoint_EnableSuspensionLimit@.+-}+enableSuspensionLimit+  :: JointId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableSuspensionLimit a0 a1 =+  c_b3WheelJoint_EnableSuspensionLimit a0 (fromBool a1)++{- | Is the wheel joint limit enabled?++Binds @b3WheelJoint_IsSuspensionLimitEnabled@.+-}+isSuspensionLimitEnabled+  :: JointId+  -> IO Bool+isSuspensionLimitEnabled a0 =+  toBool <$> (c_b3WheelJoint_IsSuspensionLimitEnabled a0)++{- | Get the wheel joint lower limit.++Binds @b3WheelJoint_GetLowerSuspensionLimit@.+-}+getLowerSuspensionLimit+  :: JointId+  -> IO Float+getLowerSuspensionLimit a0 =+  c_b3WheelJoint_GetLowerSuspensionLimit a0++{- | Get the wheel joint upper limit.++Binds @b3WheelJoint_GetUpperSuspensionLimit@.+-}+getUpperSuspensionLimit+  :: JointId+  -> IO Float+getUpperSuspensionLimit a0 =+  c_b3WheelJoint_GetUpperSuspensionLimit a0++{- | Set the wheel joint limits.++Binds @b3WheelJoint_SetSuspensionLimits@.+-}+setSuspensionLimits+  :: JointId+  -> Float+  -- ^ @lower@+  -> Float+  -- ^ @upper@+  -> IO ()+setSuspensionLimits a0 a1 a2 =+  c_b3WheelJoint_SetSuspensionLimits a0 a1 a2++{- | Enable\/disable the wheel joint motor.++Binds @b3WheelJoint_EnableSpinMotor@.+-}+enableSpinMotor+  :: JointId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableSpinMotor a0 a1 =+  c_b3WheelJoint_EnableSpinMotor a0 (fromBool a1)++{- | Is the wheel joint motor enabled?++Binds @b3WheelJoint_IsSpinMotorEnabled@.+-}+isSpinMotorEnabled+  :: JointId+  -> IO Bool+isSpinMotorEnabled a0 =+  toBool <$> (c_b3WheelJoint_IsSpinMotorEnabled a0)++{- | Set the wheel joint motor speed in radians per second.++Binds @b3WheelJoint_SetSpinMotorSpeed@.+-}+setSpinMotorSpeed+  :: JointId+  -> Float+  -- ^ @speed@+  -> IO ()+setSpinMotorSpeed a0 a1 =+  c_b3WheelJoint_SetSpinMotorSpeed a0 a1++{- | Get the wheel joint motor speed in radians per second.++Binds @b3WheelJoint_GetSpinMotorSpeed@.+-}+getSpinMotorSpeed+  :: JointId+  -> IO Float+getSpinMotorSpeed a0 =+  c_b3WheelJoint_GetSpinMotorSpeed a0++{- | Set the wheel joint maximum motor torque, usually in newton-meters.++Binds @b3WheelJoint_SetMaxSpinTorque@.+-}+setMaxSpinTorque+  :: JointId+  -> Float+  -- ^ @torque@+  -> IO ()+setMaxSpinTorque a0 a1 =+  c_b3WheelJoint_SetMaxSpinTorque a0 a1++{- | Get the wheel joint maximum motor torque, usually in newton-meters.++Binds @b3WheelJoint_GetMaxSpinTorque@.+-}+getMaxSpinTorque+  :: JointId+  -> IO Float+getMaxSpinTorque a0 =+  c_b3WheelJoint_GetMaxSpinTorque a0++{- | Get the current spin speed in radians per second.++Binds @b3WheelJoint_GetSpinSpeed@.+-}+getSpinSpeed+  :: JointId+  -> IO Float+getSpinSpeed a0 =+  c_b3WheelJoint_GetSpinSpeed a0++{- | Get the wheel joint current motor torque, usually in newton-meters.++Binds @b3WheelJoint_GetSpinTorque@.+-}+getSpinTorque+  :: JointId+  -> IO Float+getSpinTorque a0 =+  c_b3WheelJoint_GetSpinTorque a0++{- | Enable\/disable wheel steering. Steering allows the wheel to rotate about the suspension axis.++Binds @b3WheelJoint_EnableSteering@.+-}+enableSteering+  :: JointId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableSteering a0 a1 =+  c_b3WheelJoint_EnableSteering a0 (fromBool a1)++{- | Can the wheel steer?++Binds @b3WheelJoint_IsSteeringEnabled@.+-}+isSteeringEnabled+  :: JointId+  -> IO Bool+isSteeringEnabled a0 =+  toBool <$> (c_b3WheelJoint_IsSteeringEnabled a0)++{- | Set the wheel joint steering stiffness in Hertz.++Binds @b3WheelJoint_SetSteeringHertz@.+-}+setSteeringHertz+  :: JointId+  -> Float+  -- ^ @hertz@+  -> IO ()+setSteeringHertz a0 a1 =+  c_b3WheelJoint_SetSteeringHertz a0 a1++{- | Get the wheel joint steering stiffness in Hertz.++Binds @b3WheelJoint_GetSteeringHertz@.+-}+getSteeringHertz+  :: JointId+  -> IO Float+getSteeringHertz a0 =+  c_b3WheelJoint_GetSteeringHertz a0++{- | Set the wheel joint steering damping ratio, non-dimensional.++Binds @b3WheelJoint_SetSteeringDampingRatio@.+-}+setSteeringDampingRatio+  :: JointId+  -> Float+  -- ^ @dampingRatio@+  -> IO ()+setSteeringDampingRatio a0 a1 =+  c_b3WheelJoint_SetSteeringDampingRatio a0 a1++{- | Get the wheel joint steering damping ratio, non-dimensional.++Binds @b3WheelJoint_GetSteeringDampingRatio@.+-}+getSteeringDampingRatio+  :: JointId+  -> IO Float+getSteeringDampingRatio a0 =+  c_b3WheelJoint_GetSteeringDampingRatio a0++{- | Set the wheel joint maximum steering torque in N*m.++Binds @b3WheelJoint_SetMaxSteeringTorque@.+-}+setMaxSteeringTorque+  :: JointId+  -> Float+  -- ^ @torque@+  -> IO ()+setMaxSteeringTorque a0 a1 =+  c_b3WheelJoint_SetMaxSteeringTorque a0 a1++{- | Get the wheel joint maximum steering torque in N*m.++Binds @b3WheelJoint_GetMaxSteeringTorque@.+-}+getMaxSteeringTorque+  :: JointId+  -> IO Float+getMaxSteeringTorque a0 =+  c_b3WheelJoint_GetMaxSteeringTorque a0++{- | Enable\/disable the wheel joint steering limit.++Binds @b3WheelJoint_EnableSteeringLimit@.+-}+enableSteeringLimit+  :: JointId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableSteeringLimit a0 a1 =+  c_b3WheelJoint_EnableSteeringLimit a0 (fromBool a1)++{- | Is the wheel joint steering limit enabled?++Binds @b3WheelJoint_IsSteeringLimitEnabled@.+-}+isSteeringLimitEnabled+  :: JointId+  -> IO Bool+isSteeringLimitEnabled a0 =+  toBool <$> (c_b3WheelJoint_IsSteeringLimitEnabled a0)++{- | Get the wheel joint lower steering limit in radians.++Binds @b3WheelJoint_GetLowerSteeringLimit@.+-}+getLowerSteeringLimit+  :: JointId+  -> IO Float+getLowerSteeringLimit a0 =+  c_b3WheelJoint_GetLowerSteeringLimit a0++{- | Get the wheel joint upper steering limit in radians.++Binds @b3WheelJoint_GetUpperSteeringLimit@.+-}+getUpperSteeringLimit+  :: JointId+  -> IO Float+getUpperSteeringLimit a0 =+  c_b3WheelJoint_GetUpperSteeringLimit a0++{- | Set the wheel joint steering limits in radians.++Binds @b3WheelJoint_SetSteeringLimits@.+-}+setSteeringLimits+  :: JointId+  -> Float+  -- ^ @lowerRadians@+  -> Float+  -- ^ @upperRadians@+  -> IO ()+setSteeringLimits a0 a1 a2 =+  c_b3WheelJoint_SetSteeringLimits a0 a1 a2++{- | Set the wheel joint target steering angle in radians.++Binds @b3WheelJoint_SetTargetSteeringAngle@.+-}+setTargetSteeringAngle+  :: JointId+  -> Float+  -- ^ @radians@+  -> IO ()+setTargetSteeringAngle a0 a1 =+  c_b3WheelJoint_SetTargetSteeringAngle a0 a1++{- | Get the wheel joint target steering angle in radians.++Binds @b3WheelJoint_GetTargetSteeringAngle@.+-}+getTargetSteeringAngle+  :: JointId+  -> IO Float+getTargetSteeringAngle a0 =+  c_b3WheelJoint_GetTargetSteeringAngle a0++{- | Get the current steering angle in radians.++Binds @b3WheelJoint_GetSteeringAngle@.+-}+getSteeringAngle+  :: JointId+  -> IO Float+getSteeringAngle a0 =+  c_b3WheelJoint_GetSteeringAngle a0++{- | Get the current steering torque in N*m.++Binds @b3WheelJoint_GetSteeringTorque@.+-}+getSteeringTorque+  :: JointId+  -> IO Float+getSteeringTorque a0 =+  c_b3WheelJoint_GetSteeringTorque a0
+ src/Box3D/World.hsc view
@@ -0,0 +1,1760 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box3D.World+  ( ContactData (..)+  , JointEvents (..)+  , JointEvent (..)+  , BodyEvents (..)+  , BodyMoveEvent (..)+  , ContactEvents (..)+  , ContactHitEvent (..)+  , ContactEndTouchEvent (..)+  , ContactBeginTouchEvent (..)+  , SensorEvents (..)+  , SensorEndTouchEvent (..)+  , SensorBeginTouchEvent (..)+  , ExplosionDef (..)+  , Counters (..)+  , Profile (..)+  , WorldDef (..)+  , Capacity (..)+  , contactManifoldCountBuckets+  , defaultWorldDef+  , defaultExplosionDef+  , create+  , destroy+  , isValid+  , step+  , draw+  , getBounds+  , getBoundsInto+  , getBodyEvents+  , getBodyEventsInto+  , getSensorEvents+  , getSensorEventsInto+  , getContactEvents+  , getContactEventsInto+  , getJointEvents+  , getJointEventsInto+  , overlapAABB+  , overlapShape+  , castRay+  , castRayClosest+  , castShape+  , castMover+  , collideMover+  , enableSleeping+  , isSleepingEnabled+  , enableContinuous+  , isContinuousEnabled+  , setRestitutionThreshold+  , getRestitutionThreshold+  , setHitEventThreshold+  , getHitEventThreshold+  , setCustomFilterCallback+  , setPreSolveCallback+  , setGravity+  , getGravity+  , getGravityInto+  , explode+  , setContactTuning+  , setContactRecycleDistance+  , getContactRecycleDistance+  , setMaximumLinearSpeed+  , getMaximumLinearSpeed+  , enableWarmStarting+  , isWarmStartingEnabled+  , getAwakeBodyCount+  , getProfile+  , getCounters+  , getMaxCapacity+  , getMaxCapacityInto+  , setUserData+  , getUserData+  , setFrictionCallback+  , setRestitutionCallback+  , setWorkerCount+  , getWorkerCount+  , dumpMemoryStats+  , rebuildStaticTree+  , enableSpeculative+  , startRecording+  , stopRecording+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..))+import Data.Vector.Storable qualified as VS+import Box3D.Internal (allocaOut, copyVector, pokeBounded)+import Box3D.Collision (Manifold)+import Box3D.DebugDraw (DebugDraw)+import Box3D.DynamicTree (TreeStats)+import Box3D.Geometry (Capsule)+import Box3D.Id (BodyId, ContactId, JointId, ShapeId, WorldId (..))+import Box3D.MathTypes (AABB, Pos, Vec3, WorldTransform)+import Box3D.Query (QueryFilter, RayResult, ShapeProxy)+import Box3D.Tags (CastResultFcn, CreateDebugShapeCallback, CustomFilterFcn, DestroyDebugShapeCallback, EnqueueTaskCallback, FinishTaskCallback, FrictionCallback, MoverFilterFcn, OverlapResultFcn, PlaneResultFcn, PreSolveFcn, Recording, RestitutionCallback)++#include "box3d/box3d.h"+++{- | The contact data for two shapes. By convention the manifold normal points+from shape A to shape B.++See also @b3Shape_GetContactData()@ and @b3Body_GetContactData()@.+-}+data ContactData = ContactData+  { contactId :: ContactId+  -- ^ The contact id. You may hold onto this to track a contact across time steps.+  -- This id may become orphaned. Use @b3Contact_IsValid@ before using it for other functions.+  , shapeIdA :: ShapeId+  -- ^ The first shape id.+  , shapeIdB :: ShapeId+  -- ^ The second shape id.+  , manifolds :: Ptr Manifold+  -- ^ The contact manifold. This points to internal data and may become invalid. Do not store+  -- this pointer.+  , manifoldCount :: CInt+  -- ^ The number of contact manifolds. For mesh and height-field collision there can be multiple manifolds.+  }+  deriving (Eq, Show)++instance Storable ContactData where+  sizeOf _ = #{size b3ContactData}+  alignment _ = #{alignment b3ContactData}+  peek p =+    ContactData+      <$> #{peek b3ContactData, contactId} p+      <*> #{peek b3ContactData, shapeIdA} p+      <*> #{peek b3ContactData, shapeIdB} p+      <*> #{peek b3ContactData, manifolds} p+      <*> #{peek b3ContactData, manifoldCount} p+  poke p (ContactData f1 f2 f3 f4 f5) = do+    #{poke b3ContactData, contactId} p f1+    #{poke b3ContactData, shapeIdA} p f2+    #{poke b3ContactData, shapeIdB} p f3+    #{poke b3ContactData, manifolds} p f4+    #{poke b3ContactData, manifoldCount} p f5++{- | 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+-}+data JointEvents = JointEvents+  { jointEvents :: Ptr JointEvent+  -- ^ Array of events+  , count :: CInt+  -- ^ Number of events+  }+  deriving (Eq, Show)++instance Storable JointEvents where+  sizeOf _ = #{size b3JointEvents}+  alignment _ = #{alignment b3JointEvents}+  peek p =+    JointEvents+      <$> #{peek b3JointEvents, jointEvents} p+      <*> #{peek b3JointEvents, count} p+  poke p (JointEvents f1 f2) = do+    #{poke b3JointEvents, jointEvents} p f1+    #{poke b3JointEvents, count} p f2++{- | 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.+-}+data JointEvent = JointEvent+  { jointId :: JointId+  -- ^ The joint id+  , userData :: Ptr ()+  -- ^ The user data from the joint for convenience+  }+  deriving (Eq, Show)++instance Storable JointEvent where+  sizeOf _ = #{size b3JointEvent}+  alignment _ = #{alignment b3JointEvent}+  peek p =+    JointEvent+      <$> #{peek b3JointEvent, jointId} p+      <*> #{peek b3JointEvent, userData} p+  poke p (JointEvent f1 f2) = do+    #{poke b3JointEvent, jointId} p f1+    #{poke b3JointEvent, userData} p f2++{- | Body events are buffered in the world and are available+as event arrays after the time step is complete.+Note: this data becomes invalid if bodies are destroyed+-}+data BodyEvents = BodyEvents+  { moveEvents :: Ptr BodyMoveEvent+  -- ^ Array of move events+  , moveCount :: CInt+  -- ^ Number of move events+  }+  deriving (Eq, Show)++instance Storable BodyEvents where+  sizeOf _ = #{size b3BodyEvents}+  alignment _ = #{alignment b3BodyEvents}+  peek p =+    BodyEvents+      <$> #{peek b3BodyEvents, moveEvents} p+      <*> #{peek b3BodyEvents, moveCount} p+  poke p (BodyEvents f1 f2) = do+    #{poke b3BodyEvents, moveEvents} p f1+    #{poke b3BodyEvents, moveCount} p f2++{- | 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.+-}+data BodyMoveEvent = BodyMoveEvent+  { userData :: Ptr ()+  -- ^ The body user data.+  , transform :: WorldTransform+  -- ^ The body transform.+  , bodyId :: BodyId+  -- ^ The body id.+  , fellAsleep :: CBool+  -- ^ Did the body fall asleep this time step?+  }+  deriving (Eq, Show)++instance Storable BodyMoveEvent where+  sizeOf _ = #{size b3BodyMoveEvent}+  alignment _ = #{alignment b3BodyMoveEvent}+  peek p =+    BodyMoveEvent+      <$> #{peek b3BodyMoveEvent, userData} p+      <*> #{peek b3BodyMoveEvent, transform} p+      <*> #{peek b3BodyMoveEvent, bodyId} p+      <*> #{peek b3BodyMoveEvent, fellAsleep} p+  poke p (BodyMoveEvent f1 f2 f3 f4) = do+    #{poke b3BodyMoveEvent, userData} p f1+    #{poke b3BodyMoveEvent, transform} p f2+    #{poke b3BodyMoveEvent, bodyId} p f3+    #{poke b3BodyMoveEvent, fellAsleep} p f4++{- | 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+-}+data ContactEvents = ContactEvents+  { beginEvents :: Ptr ContactBeginTouchEvent+  -- ^ Array of begin touch events+  , endEvents :: Ptr ContactEndTouchEvent+  -- ^ Array of end touch events+  , hitEvents :: Ptr ContactHitEvent+  -- ^ Array of hit events+  , beginCount :: CInt+  -- ^ Number of begin touch events+  , endCount :: CInt+  -- ^ Number of end touch events+  , hitCount :: CInt+  -- ^ Number of hit events+  }+  deriving (Eq, Show)++instance Storable ContactEvents where+  sizeOf _ = #{size b3ContactEvents}+  alignment _ = #{alignment b3ContactEvents}+  peek p =+    ContactEvents+      <$> #{peek b3ContactEvents, beginEvents} p+      <*> #{peek b3ContactEvents, endEvents} p+      <*> #{peek b3ContactEvents, hitEvents} p+      <*> #{peek b3ContactEvents, beginCount} p+      <*> #{peek b3ContactEvents, endCount} p+      <*> #{peek b3ContactEvents, hitCount} p+  poke p (ContactEvents f1 f2 f3 f4 f5 f6) = do+    #{poke b3ContactEvents, beginEvents} p f1+    #{poke b3ContactEvents, endEvents} p f2+    #{poke b3ContactEvents, hitEvents} p f3+    #{poke b3ContactEvents, beginCount} p f4+    #{poke b3ContactEvents, endCount} p f5+    #{poke b3ContactEvents, hitCount} p f6++{- | 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.+-}+data ContactHitEvent = ContactHitEvent+  { shapeIdA :: ShapeId+  -- ^ Id of the first shape+  , shapeIdB :: ShapeId+  -- ^ Id of the second shape+  , contactId :: ContactId+  -- ^ Id of the contact.+  --+  -- __Warning:__ this contact may have been destroyed+  --+  -- See also @b3Contact_IsValid@.+  , point :: Pos+  -- ^ 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.+  , normal :: Vec3+  -- ^ Normal vector pointing from shape A to shape B+  , approachSpeed :: Float+  -- ^ The speed the shapes are approaching. Always positive. Typically in meters per second.+  , userMaterialIdA :: Word64+  -- ^ User material on shape A+  , userMaterialIdB :: Word64+  -- ^ User material on shape B+  }+  deriving (Eq, Show)++instance Storable ContactHitEvent where+  sizeOf _ = #{size b3ContactHitEvent}+  alignment _ = #{alignment b3ContactHitEvent}+  peek p =+    ContactHitEvent+      <$> #{peek b3ContactHitEvent, shapeIdA} p+      <*> #{peek b3ContactHitEvent, shapeIdB} p+      <*> #{peek b3ContactHitEvent, contactId} p+      <*> #{peek b3ContactHitEvent, point} p+      <*> #{peek b3ContactHitEvent, normal} p+      <*> #{peek b3ContactHitEvent, approachSpeed} p+      <*> #{peek b3ContactHitEvent, userMaterialIdA} p+      <*> #{peek b3ContactHitEvent, userMaterialIdB} p+  poke p (ContactHitEvent f1 f2 f3 f4 f5 f6 f7 f8) = do+    #{poke b3ContactHitEvent, shapeIdA} p f1+    #{poke b3ContactHitEvent, shapeIdB} p f2+    #{poke b3ContactHitEvent, contactId} p f3+    #{poke b3ContactHitEvent, point} p f4+    #{poke b3ContactHitEvent, normal} p f5+    #{poke b3ContactHitEvent, approachSpeed} p f6+    #{poke b3ContactHitEvent, userMaterialIdA} p f7+    #{poke b3ContactHitEvent, userMaterialIdB} p f8++{- | 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.+-}+data ContactEndTouchEvent = ContactEndTouchEvent+  { shapeIdA :: ShapeId+  -- ^ Id of the first shape+  --+  -- __Warning:__ this shape may have been destroyed+  --+  -- See also @b3Shape_IsValid@.+  , shapeIdB :: ShapeId+  -- ^ Id of the first shape+  --+  -- __Warning:__ this shape may have been destroyed+  --+  -- See also @b3Shape_IsValid@.+  , contactId :: ContactId+  -- ^ Id of the contact.+  --+  -- __Warning:__ this contact may have been destroyed+  --+  -- See also @b3Contact_IsValid@.+  }+  deriving (Eq, Show)++instance Storable ContactEndTouchEvent where+  sizeOf _ = #{size b3ContactEndTouchEvent}+  alignment _ = #{alignment b3ContactEndTouchEvent}+  peek p =+    ContactEndTouchEvent+      <$> #{peek b3ContactEndTouchEvent, shapeIdA} p+      <*> #{peek b3ContactEndTouchEvent, shapeIdB} p+      <*> #{peek b3ContactEndTouchEvent, contactId} p+  poke p (ContactEndTouchEvent f1 f2 f3) = do+    #{poke b3ContactEndTouchEvent, shapeIdA} p f1+    #{poke b3ContactEndTouchEvent, shapeIdB} p f2+    #{poke b3ContactEndTouchEvent, contactId} p f3++-- | A begin-touch event is generated when two shapes begin touching.+data ContactBeginTouchEvent = ContactBeginTouchEvent+  { shapeIdA :: ShapeId+  -- ^ Id of the first shape+  , shapeIdB :: ShapeId+  -- ^ Id of the second shape+  , contactId :: ContactId+  -- ^ The transient contact id. This contact may be destroyed automatically when the world is modified or simulated.+  -- Use @b3Contact_IsValid@ before using this id.+  }+  deriving (Eq, Show)++instance Storable ContactBeginTouchEvent where+  sizeOf _ = #{size b3ContactBeginTouchEvent}+  alignment _ = #{alignment b3ContactBeginTouchEvent}+  peek p =+    ContactBeginTouchEvent+      <$> #{peek b3ContactBeginTouchEvent, shapeIdA} p+      <*> #{peek b3ContactBeginTouchEvent, shapeIdB} p+      <*> #{peek b3ContactBeginTouchEvent, contactId} p+  poke p (ContactBeginTouchEvent f1 f2 f3) = do+    #{poke b3ContactBeginTouchEvent, shapeIdA} p f1+    #{poke b3ContactBeginTouchEvent, shapeIdB} p f2+    #{poke b3ContactBeginTouchEvent, contactId} p f3++{- | 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+-}+data SensorEvents = SensorEvents+  { beginEvents :: Ptr SensorBeginTouchEvent+  -- ^ Array of sensor begin touch events+  , endEvents :: Ptr SensorEndTouchEvent+  -- ^ Array of sensor end touch events+  , beginCount :: CInt+  -- ^ The number of begin touch events+  , endCount :: CInt+  -- ^ The number of end touch events+  }+  deriving (Eq, Show)++instance Storable SensorEvents where+  sizeOf _ = #{size b3SensorEvents}+  alignment _ = #{alignment b3SensorEvents}+  peek p =+    SensorEvents+      <$> #{peek b3SensorEvents, beginEvents} p+      <*> #{peek b3SensorEvents, endEvents} p+      <*> #{peek b3SensorEvents, beginCount} p+      <*> #{peek b3SensorEvents, endCount} p+  poke p (SensorEvents f1 f2 f3 f4) = do+    #{poke b3SensorEvents, beginEvents} p f1+    #{poke b3SensorEvents, endEvents} p f2+    #{poke b3SensorEvents, beginCount} p f3+    #{poke b3SensorEvents, endCount} p f4++{- | 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@.+-}+data SensorEndTouchEvent = SensorEndTouchEvent+  { sensorShapeId :: ShapeId+  -- ^ The id of the sensor shape+  --+  -- __Warning:__ this shape may have been destroyed+  --+  -- See also @b3Shape_IsValid@.+  , visitorShapeId :: ShapeId+  -- ^ The id of the shape that stopped touching the sensor shape+  --+  -- __Warning:__ this shape may have been destroyed+  --+  -- See also @b3Shape_IsValid@.+  }+  deriving (Eq, Show)++instance Storable SensorEndTouchEvent where+  sizeOf _ = #{size b3SensorEndTouchEvent}+  alignment _ = #{alignment b3SensorEndTouchEvent}+  peek p =+    SensorEndTouchEvent+      <$> #{peek b3SensorEndTouchEvent, sensorShapeId} p+      <*> #{peek b3SensorEndTouchEvent, visitorShapeId} p+  poke p (SensorEndTouchEvent f1 f2) = do+    #{poke b3SensorEndTouchEvent, sensorShapeId} p f1+    #{poke b3SensorEndTouchEvent, visitorShapeId} p f2++-- | A begin-touch event is generated when a shape starts to overlap a sensor shape.+data SensorBeginTouchEvent = SensorBeginTouchEvent+  { sensorShapeId :: ShapeId+  -- ^ The id of the sensor shape+  , visitorShapeId :: ShapeId+  -- ^ The id of the shape that began touching the sensor shape+  }+  deriving (Eq, Show)++instance Storable SensorBeginTouchEvent where+  sizeOf _ = #{size b3SensorBeginTouchEvent}+  alignment _ = #{alignment b3SensorBeginTouchEvent}+  peek p =+    SensorBeginTouchEvent+      <$> #{peek b3SensorBeginTouchEvent, sensorShapeId} p+      <*> #{peek b3SensorBeginTouchEvent, visitorShapeId} p+  poke p (SensorBeginTouchEvent f1 f2) = do+    #{poke b3SensorBeginTouchEvent, sensorShapeId} p f1+    #{poke b3SensorBeginTouchEvent, visitorShapeId} p f2++{- | The explosion definition is used to configure options for explosions. Explosions+consider shape geometry when computing the impulse.+-}+data ExplosionDef = ExplosionDef+  { maskBits :: Word64+  -- ^ Mask bits to filter shapes+  , position :: Pos+  -- ^ The center of the explosion in world space+  , radius :: Float+  -- ^ The radius of the explosion+  , falloff :: Float+  -- ^ The falloff distance beyond the radius. Impulse is reduced to zero at this distance.+  , impulsePerArea :: Float+  -- ^ 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.+  }+  deriving (Eq, Show)++instance Storable ExplosionDef where+  sizeOf _ = #{size b3ExplosionDef}+  alignment _ = #{alignment b3ExplosionDef}+  peek p =+    ExplosionDef+      <$> #{peek b3ExplosionDef, maskBits} p+      <*> #{peek b3ExplosionDef, position} p+      <*> #{peek b3ExplosionDef, radius} p+      <*> #{peek b3ExplosionDef, falloff} p+      <*> #{peek b3ExplosionDef, impulsePerArea} p+  poke p (ExplosionDef f1 f2 f3 f4 f5) = do+    #{poke b3ExplosionDef, maskBits} p f1+    #{poke b3ExplosionDef, position} p f2+    #{poke b3ExplosionDef, radius} p f3+    #{poke b3ExplosionDef, falloff} p f4+    #{poke b3ExplosionDef, impulsePerArea} p f5++-- | Counters that give details of the simulation size.+data Counters = Counters+  { bodyCount :: CInt+  , shapeCount :: CInt+  , contactCount :: CInt+  , jointCount :: CInt+  , islandCount :: CInt+  , stackUsed :: CInt+  , arenaCapacity :: CInt+  , staticTreeHeight :: CInt+  , treeHeight :: CInt+  , satCallCount :: CInt+  , satCacheHitCount :: CInt+  , byteCount :: CInt+  , taskCount :: CInt+  , colorCounts :: VS.Vector CInt+  , manifoldCounts :: VS.Vector CInt+  , awakeContactCount :: CInt+  -- ^ Number of contacts touched by the collide pass+  -- graph contacts + awake-set non-touching+  , recycledContactCount :: CInt+  -- ^ Number of contacts recycled in the most recent step.+  , distanceIterations :: CInt+  -- ^ Maximum number of time of impact iterations+  , pushBackIterations :: CInt+  , rootIterations :: CInt+  }+  deriving (Eq, Show)++instance Storable Counters where+  sizeOf _ = #{size b3Counters}+  alignment _ = #{alignment b3Counters}+  peek p =+    Counters+      <$> #{peek b3Counters, bodyCount} p+      <*> #{peek b3Counters, shapeCount} p+      <*> #{peek b3Counters, contactCount} p+      <*> #{peek b3Counters, jointCount} p+      <*> #{peek b3Counters, islandCount} p+      <*> #{peek b3Counters, stackUsed} p+      <*> #{peek b3Counters, arenaCapacity} p+      <*> #{peek b3Counters, staticTreeHeight} p+      <*> #{peek b3Counters, treeHeight} p+      <*> #{peek b3Counters, satCallCount} p+      <*> #{peek b3Counters, satCacheHitCount} p+      <*> #{peek b3Counters, byteCount} p+      <*> #{peek b3Counters, taskCount} p+      <*> copyVector (#{const 24}) (#{ptr b3Counters, colorCounts} p)+      <*> copyVector contactManifoldCountBuckets (#{ptr b3Counters, manifoldCounts} p)+      <*> #{peek b3Counters, awakeContactCount} p+      <*> #{peek b3Counters, recycledContactCount} p+      <*> #{peek b3Counters, distanceIterations} p+      <*> #{peek b3Counters, pushBackIterations} p+      <*> #{peek b3Counters, rootIterations} p+  poke p x = do+    #{poke b3Counters, bodyCount} p x.bodyCount+    #{poke b3Counters, shapeCount} p x.shapeCount+    #{poke b3Counters, contactCount} p x.contactCount+    #{poke b3Counters, jointCount} p x.jointCount+    #{poke b3Counters, islandCount} p x.islandCount+    #{poke b3Counters, stackUsed} p x.stackUsed+    #{poke b3Counters, arenaCapacity} p x.arenaCapacity+    #{poke b3Counters, staticTreeHeight} p x.staticTreeHeight+    #{poke b3Counters, treeHeight} p x.treeHeight+    #{poke b3Counters, satCallCount} p x.satCallCount+    #{poke b3Counters, satCacheHitCount} p x.satCacheHitCount+    #{poke b3Counters, byteCount} p x.byteCount+    #{poke b3Counters, taskCount} p x.taskCount+    pokeBounded (#{const 24}) (#{ptr b3Counters, colorCounts} p) x.colorCounts+    pokeBounded contactManifoldCountBuckets (#{ptr b3Counters, manifoldCounts} p) x.manifoldCounts+    #{poke b3Counters, awakeContactCount} p x.awakeContactCount+    #{poke b3Counters, recycledContactCount} p x.recycledContactCount+    #{poke b3Counters, distanceIterations} p x.distanceIterations+    #{poke b3Counters, pushBackIterations} p x.pushBackIterations+    #{poke b3Counters, rootIterations} p x.rootIterations++-- | Profiling data. Times are in milliseconds.+data Profile = Profile+  { 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 :: Float+  }+  deriving (Eq, Show)++instance Storable Profile where+  sizeOf _ = #{size b3Profile}+  alignment _ = #{alignment b3Profile}+  peek p =+    Profile+      <$> #{peek b3Profile, step} p+      <*> #{peek b3Profile, pairs} p+      <*> #{peek b3Profile, collide} p+      <*> #{peek b3Profile, solve} p+      <*> #{peek b3Profile, solverSetup} p+      <*> #{peek b3Profile, constraints} p+      <*> #{peek b3Profile, prepareConstraints} p+      <*> #{peek b3Profile, integrateVelocities} p+      <*> #{peek b3Profile, warmStart} p+      <*> #{peek b3Profile, solveImpulses} p+      <*> #{peek b3Profile, integratePositions} p+      <*> #{peek b3Profile, relaxImpulses} p+      <*> #{peek b3Profile, applyRestitution} p+      <*> #{peek b3Profile, storeImpulses} p+      <*> #{peek b3Profile, splitIslands} p+      <*> #{peek b3Profile, transforms} p+      <*> #{peek b3Profile, sensorHits} p+      <*> #{peek b3Profile, jointEvents} p+      <*> #{peek b3Profile, hitEvents} p+      <*> #{peek b3Profile, refit} p+      <*> #{peek b3Profile, bullets} p+      <*> #{peek b3Profile, sleepIslands} p+      <*> #{peek b3Profile, sensors} p+  poke p (Profile f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11 f12 f13 f14 f15 f16 f17 f18 f19 f20 f21 f22 f23) = do+    #{poke b3Profile, step} p f1+    #{poke b3Profile, pairs} p f2+    #{poke b3Profile, collide} p f3+    #{poke b3Profile, solve} p f4+    #{poke b3Profile, solverSetup} p f5+    #{poke b3Profile, constraints} p f6+    #{poke b3Profile, prepareConstraints} p f7+    #{poke b3Profile, integrateVelocities} p f8+    #{poke b3Profile, warmStart} p f9+    #{poke b3Profile, solveImpulses} p f10+    #{poke b3Profile, integratePositions} p f11+    #{poke b3Profile, relaxImpulses} p f12+    #{poke b3Profile, applyRestitution} p f13+    #{poke b3Profile, storeImpulses} p f14+    #{poke b3Profile, splitIslands} p f15+    #{poke b3Profile, transforms} p f16+    #{poke b3Profile, sensorHits} p f17+    #{poke b3Profile, jointEvents} p f18+    #{poke b3Profile, hitEvents} p f19+    #{poke b3Profile, refit} p f20+    #{poke b3Profile, bullets} p f21+    #{poke b3Profile, sleepIslands} p f22+    #{poke b3Profile, sensors} p f23++-- | World definition used to create a simulation world. Must be initialized using @b3DefaultWorldDef@.+data WorldDef = WorldDef+  { gravity :: Vec3+  -- ^ Gravity vector. Box3D has no up-vector defined.+  , restitutionThreshold :: Float+  -- ^ Restitution speed threshold, usually in m\/s. Collisions above this+  -- speed have restitution applied (will bounce).+  , hitEventThreshold :: Float+  -- ^ Hit event speed threshold, usually in m\/s. Collisions above this+  -- speed can generate hit events if the shape also enables hit events.+  , contactHertz :: Float+  -- ^ Contact stiffness. Cycles per second. Increasing this increases the speed of overlap recovery, but can introduce jitter.+  , contactDampingRatio :: Float+  -- ^ Contact bounciness. Non-dimensional. You can speed up overlap recovery by decreasing this with+  -- the trade-off that overlap resolution becomes more energetic.+  , contactSpeed :: Float+  -- ^ 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.+  , maximumLinearSpeed :: Float+  -- ^ Maximum linear speed. Usually meters per second.+  , frictionCallback :: FunPtr FrictionCallback+  -- ^ Optional mixing callback for friction. The default uses sqrt(frictionA * frictionB).+  , restitutionCallback :: FunPtr RestitutionCallback+  -- ^ Optional mixing callback for restitution. The default uses max(restitutionA, restitutionB).+  , enableSleep :: CBool+  -- ^ Can bodies go to sleep to improve performance+  , enableContinuous :: CBool+  -- ^ Enable continuous collision+  , workerCount :: Word32+  -- ^ 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.+  -- This is clamped to the range \[1, B3_MAX_WORKERS\]. Using a value above 1 will turn on multithreading.+  -- If task callbacks are provided then Box3D will use the user provided task system. Otherwise Box3D+  -- will create threads and use an internal scheduler.+  , enqueueTask :: FunPtr EnqueueTaskCallback+  -- ^ function to spawn task+  , finishTask :: FunPtr FinishTaskCallback+  -- ^ function to finish a task+  , userTaskContext :: Ptr ()+  -- ^ User context that is provided to enqueueTask and finishTask+  , userData :: Ptr ()+  -- ^ User data associated with a world+  , createDebugShape :: FunPtr CreateDebugShapeCallback+  -- ^ Used to create debug draw shapes. This is called when a shape is+  -- first drawn using @b3DebugDraw@.+  , destroyDebugShape :: FunPtr DestroyDebugShapeCallback+  -- ^ Used to destroy debug draw shapes. This is called when a shape is modified or destroyed.+  , userDebugShapeContext :: Ptr ()+  -- ^ This is passed to the debug shape callbacks to provide a user context.+  , capacity :: Capacity+  -- ^ Optional initial capacities+  , internalValue :: CInt+  -- ^ Used internally to detect a valid definition. DO NOT SET.+  }+  deriving (Eq, Show)++instance Storable WorldDef where+  sizeOf _ = #{size b3WorldDef}+  alignment _ = #{alignment b3WorldDef}+  peek p =+    WorldDef+      <$> #{peek b3WorldDef, gravity} p+      <*> #{peek b3WorldDef, restitutionThreshold} p+      <*> #{peek b3WorldDef, hitEventThreshold} p+      <*> #{peek b3WorldDef, contactHertz} p+      <*> #{peek b3WorldDef, contactDampingRatio} p+      <*> #{peek b3WorldDef, contactSpeed} p+      <*> #{peek b3WorldDef, maximumLinearSpeed} p+      <*> #{peek b3WorldDef, frictionCallback} p+      <*> #{peek b3WorldDef, restitutionCallback} p+      <*> #{peek b3WorldDef, enableSleep} p+      <*> #{peek b3WorldDef, enableContinuous} p+      <*> #{peek b3WorldDef, workerCount} p+      <*> #{peek b3WorldDef, enqueueTask} p+      <*> #{peek b3WorldDef, finishTask} p+      <*> #{peek b3WorldDef, userTaskContext} p+      <*> #{peek b3WorldDef, userData} p+      <*> #{peek b3WorldDef, createDebugShape} p+      <*> #{peek b3WorldDef, destroyDebugShape} p+      <*> #{peek b3WorldDef, userDebugShapeContext} p+      <*> #{peek b3WorldDef, capacity} p+      <*> #{peek b3WorldDef, internalValue} p+  poke p (WorldDef f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11 f12 f13 f14 f15 f16 f17 f18 f19 f20 f21) = do+    #{poke b3WorldDef, gravity} p f1+    #{poke b3WorldDef, restitutionThreshold} p f2+    #{poke b3WorldDef, hitEventThreshold} p f3+    #{poke b3WorldDef, contactHertz} p f4+    #{poke b3WorldDef, contactDampingRatio} p f5+    #{poke b3WorldDef, contactSpeed} p f6+    #{poke b3WorldDef, maximumLinearSpeed} p f7+    #{poke b3WorldDef, frictionCallback} p f8+    #{poke b3WorldDef, restitutionCallback} p f9+    #{poke b3WorldDef, enableSleep} p f10+    #{poke b3WorldDef, enableContinuous} p f11+    #{poke b3WorldDef, workerCount} p f12+    #{poke b3WorldDef, enqueueTask} p f13+    #{poke b3WorldDef, finishTask} p f14+    #{poke b3WorldDef, userTaskContext} p f15+    #{poke b3WorldDef, userData} p f16+    #{poke b3WorldDef, createDebugShape} p f17+    #{poke b3WorldDef, destroyDebugShape} p f18+    #{poke b3WorldDef, userDebugShapeContext} p f19+    #{poke b3WorldDef, capacity} p f20+    #{poke b3WorldDef, internalValue} p f21++-- | Optional world capacities that can be use to avoid run-time allocations+data Capacity = Capacity+  { staticShapeCount :: CInt+  -- ^ Number of expected static shapes.+  , dynamicShapeCount :: CInt+  -- ^ Number of expected dynamic and kinematic shapes.+  , staticBodyCount :: CInt+  -- ^ Number of expected static bodies.+  , dynamicBodyCount :: CInt+  -- ^ Number of expected dynamic and kinematic bodies.+  , contactCount :: CInt+  -- ^ Number of expected contacts.+  }+  deriving (Eq, Show)++instance Storable Capacity where+  sizeOf _ = #{size b3Capacity}+  alignment _ = #{alignment b3Capacity}+  peek p =+    Capacity+      <$> #{peek b3Capacity, staticShapeCount} p+      <*> #{peek b3Capacity, dynamicShapeCount} p+      <*> #{peek b3Capacity, staticBodyCount} p+      <*> #{peek b3Capacity, dynamicBodyCount} p+      <*> #{peek b3Capacity, contactCount} p+  poke p (Capacity f1 f2 f3 f4 f5) = do+    #{poke b3Capacity, staticShapeCount} p f1+    #{poke b3Capacity, dynamicShapeCount} p f2+    #{poke b3Capacity, staticBodyCount} p f3+    #{poke b3Capacity, dynamicBodyCount} p f4+    #{poke b3Capacity, contactCount} p f5++-- | @B3_CONTACT_MANIFOLD_COUNT_BUCKETS@+contactManifoldCountBuckets :: Int+contactManifoldCountBuckets = #{const B3_CONTACT_MANIFOLD_COUNT_BUCKETS}+++foreign import ccall unsafe "hs_b3DefaultWorldDef"+  c_b3DefaultWorldDef :: Ptr WorldDef -> IO ()++defaultWorldDef :: IO WorldDef+defaultWorldDef = allocaOut c_b3DefaultWorldDef++foreign import ccall unsafe "hs_b3DefaultExplosionDef"+  c_b3DefaultExplosionDef :: Ptr ExplosionDef -> IO ()++defaultExplosionDef :: IO ExplosionDef+defaultExplosionDef = allocaOut c_b3DefaultExplosionDef+++#if defined(BOX3D_DOUBLE_PRECISION)+foreign import ccall unsafe "b3CreateWorldDoublePrecision"+#else+foreign import ccall unsafe "b3CreateWorld"+#endif+  c_b3CreateWorld :: Ptr WorldDef -> IO WorldId++foreign import ccall safe "b3DestroyWorld"+  c_b3DestroyWorld :: WorldId -> IO ()++foreign import ccall unsafe "b3World_IsValid"+  c_b3World_IsValid :: WorldId -> IO CBool++foreign import ccall safe "b3World_Step"+  c_b3World_Step :: WorldId -> Float -> CInt -> IO ()++foreign import ccall safe "b3World_Draw"+  c_b3World_Draw :: WorldId -> Ptr DebugDraw -> Word64 -> IO ()++foreign import ccall unsafe "hsg_b3World_GetBounds"+  c_b3World_GetBounds :: WorldId -> Ptr AABB -> IO ()++foreign import ccall unsafe "hsg_b3World_GetBodyEvents"+  c_b3World_GetBodyEvents :: WorldId -> Ptr BodyEvents -> IO ()++foreign import ccall unsafe "hsg_b3World_GetSensorEvents"+  c_b3World_GetSensorEvents :: WorldId -> Ptr SensorEvents -> IO ()++foreign import ccall unsafe "hsg_b3World_GetContactEvents"+  c_b3World_GetContactEvents :: WorldId -> Ptr ContactEvents -> IO ()++foreign import ccall unsafe "hsg_b3World_GetJointEvents"+  c_b3World_GetJointEvents :: WorldId -> Ptr JointEvents -> IO ()++foreign import ccall safe "hsg_b3World_OverlapAABB"+  c_b3World_OverlapAABB :: WorldId -> Ptr AABB -> Ptr QueryFilter -> FunPtr OverlapResultFcn -> Ptr () -> Ptr TreeStats -> IO ()++foreign import ccall safe "hsg_b3World_OverlapShape"+  c_b3World_OverlapShape :: WorldId -> Ptr Pos -> Ptr ShapeProxy -> Ptr QueryFilter -> FunPtr OverlapResultFcn -> Ptr () -> Ptr TreeStats -> IO ()++foreign import ccall safe "hsg_b3World_CastRay"+  c_b3World_CastRay :: WorldId -> Ptr Pos -> Ptr Vec3 -> Ptr QueryFilter -> FunPtr CastResultFcn -> Ptr () -> Ptr TreeStats -> IO ()++foreign import ccall unsafe "hsg_b3World_CastRayClosest"+  c_b3World_CastRayClosest :: WorldId -> Ptr Pos -> Ptr Vec3 -> Ptr QueryFilter -> Ptr RayResult -> IO ()++foreign import ccall safe "hsg_b3World_CastShape"+  c_b3World_CastShape :: WorldId -> Ptr Pos -> Ptr ShapeProxy -> Ptr Vec3 -> Ptr QueryFilter -> FunPtr CastResultFcn -> Ptr () -> Ptr TreeStats -> IO ()++foreign import ccall safe "hsg_b3World_CastMover"+  c_b3World_CastMover :: WorldId -> Ptr Pos -> Ptr Capsule -> Ptr Vec3 -> Ptr QueryFilter -> FunPtr MoverFilterFcn -> Ptr () -> IO Float++foreign import ccall safe "hsg_b3World_CollideMover"+  c_b3World_CollideMover :: WorldId -> Ptr Pos -> Ptr Capsule -> Ptr QueryFilter -> FunPtr PlaneResultFcn -> Ptr () -> IO ()++foreign import ccall unsafe "b3World_EnableSleeping"+  c_b3World_EnableSleeping :: WorldId -> CBool -> IO ()++foreign import ccall unsafe "b3World_IsSleepingEnabled"+  c_b3World_IsSleepingEnabled :: WorldId -> IO CBool++foreign import ccall unsafe "b3World_EnableContinuous"+  c_b3World_EnableContinuous :: WorldId -> CBool -> IO ()++foreign import ccall unsafe "b3World_IsContinuousEnabled"+  c_b3World_IsContinuousEnabled :: WorldId -> IO CBool++foreign import ccall unsafe "b3World_SetRestitutionThreshold"+  c_b3World_SetRestitutionThreshold :: WorldId -> Float -> IO ()++foreign import ccall unsafe "b3World_GetRestitutionThreshold"+  c_b3World_GetRestitutionThreshold :: WorldId -> IO Float++foreign import ccall unsafe "b3World_SetHitEventThreshold"+  c_b3World_SetHitEventThreshold :: WorldId -> Float -> IO ()++foreign import ccall unsafe "b3World_GetHitEventThreshold"+  c_b3World_GetHitEventThreshold :: WorldId -> IO Float++foreign import ccall safe "b3World_SetCustomFilterCallback"+  c_b3World_SetCustomFilterCallback :: WorldId -> FunPtr CustomFilterFcn -> Ptr () -> IO ()++foreign import ccall safe "b3World_SetPreSolveCallback"+  c_b3World_SetPreSolveCallback :: WorldId -> FunPtr PreSolveFcn -> Ptr () -> IO ()++foreign import ccall unsafe "hsg_b3World_SetGravity"+  c_b3World_SetGravity :: WorldId -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "hsg_b3World_GetGravity"+  c_b3World_GetGravity :: WorldId -> Ptr Vec3 -> IO ()++foreign import ccall unsafe "b3World_Explode"+  c_b3World_Explode :: WorldId -> Ptr ExplosionDef -> IO ()++foreign import ccall unsafe "b3World_SetContactTuning"+  c_b3World_SetContactTuning :: WorldId -> Float -> Float -> Float -> IO ()++foreign import ccall unsafe "b3World_SetContactRecycleDistance"+  c_b3World_SetContactRecycleDistance :: WorldId -> Float -> IO ()++foreign import ccall unsafe "b3World_GetContactRecycleDistance"+  c_b3World_GetContactRecycleDistance :: WorldId -> IO Float++foreign import ccall unsafe "b3World_SetMaximumLinearSpeed"+  c_b3World_SetMaximumLinearSpeed :: WorldId -> Float -> IO ()++foreign import ccall unsafe "b3World_GetMaximumLinearSpeed"+  c_b3World_GetMaximumLinearSpeed :: WorldId -> IO Float++foreign import ccall unsafe "b3World_EnableWarmStarting"+  c_b3World_EnableWarmStarting :: WorldId -> CBool -> IO ()++foreign import ccall unsafe "b3World_IsWarmStartingEnabled"+  c_b3World_IsWarmStartingEnabled :: WorldId -> IO CBool++foreign import ccall unsafe "b3World_GetAwakeBodyCount"+  c_b3World_GetAwakeBodyCount :: WorldId -> IO CInt++foreign import ccall unsafe "hsg_b3World_GetProfile"+  c_b3World_GetProfile :: WorldId -> Ptr Profile -> IO ()++foreign import ccall unsafe "hsg_b3World_GetCounters"+  c_b3World_GetCounters :: WorldId -> Ptr Counters -> IO ()++foreign import ccall unsafe "hsg_b3World_GetMaxCapacity"+  c_b3World_GetMaxCapacity :: WorldId -> Ptr Capacity -> IO ()++foreign import ccall unsafe "b3World_SetUserData"+  c_b3World_SetUserData :: WorldId -> Ptr () -> IO ()++foreign import ccall unsafe "b3World_GetUserData"+  c_b3World_GetUserData :: WorldId -> IO (Ptr ())++foreign import ccall safe "b3World_SetFrictionCallback"+  c_b3World_SetFrictionCallback :: WorldId -> FunPtr FrictionCallback -> IO ()++foreign import ccall safe "b3World_SetRestitutionCallback"+  c_b3World_SetRestitutionCallback :: WorldId -> FunPtr RestitutionCallback -> IO ()++foreign import ccall unsafe "b3World_SetWorkerCount"+  c_b3World_SetWorkerCount :: WorldId -> CInt -> IO ()++foreign import ccall unsafe "b3World_GetWorkerCount"+  c_b3World_GetWorkerCount :: WorldId -> IO CInt++foreign import ccall unsafe "b3World_DumpMemoryStats"+  c_b3World_DumpMemoryStats :: WorldId -> IO ()++foreign import ccall unsafe "b3World_RebuildStaticTree"+  c_b3World_RebuildStaticTree :: WorldId -> IO ()++foreign import ccall unsafe "b3World_EnableSpeculative"+  c_b3World_EnableSpeculative :: WorldId -> CBool -> IO ()++foreign import ccall unsafe "b3World_StartRecording"+  c_b3World_StartRecording :: WorldId -> Ptr Recording -> IO ()++foreign import ccall unsafe "b3World_StopRecording"+  c_b3World_StopRecording :: WorldId -> IO ()++{- | Create a world for rigid body simulation. A world contains bodies, shapes, and constraints. You may create+up to 128 worlds. Each world is completely independent and may be simulated in parallel.++Returns: the world id.++Binds @b3CreateWorld@.+-}+create+  :: WorldDef+  -> IO WorldId+create a0 =+  with a0 $ \p0 ->+  c_b3CreateWorld p0++{- | Destroy a world++Binds @b3DestroyWorld@.+-}+destroy+  :: WorldId+  -> IO ()+destroy a0 =+  c_b3DestroyWorld a0++{- | World id validation. Provides validation for up to 64K allocations.++Binds @b3World_IsValid@.+-}+isValid+  :: WorldId+  -> IO Bool+isValid a0 =+  toBool <$> (c_b3World_IsValid a0)++{- | Simulate a world for one time step. This performs collision detection, integration, and constraint solution.++Binds @b3World_Step@.+-}+step+  :: WorldId+  -- ^ @worldId@: The world to simulate+  -> Float+  -- ^ @timeStep@: The amount of time to simulate, this should be a fixed number. Usually 1\/60.+  -> Int+  -- ^ @subStepCount@: The number of sub-steps, increasing the sub-step count can increase accuracy. Usually 4.+  -> IO ()+step a0 a1 a2 =+  c_b3World_Step a0 a1 (fromIntegral a2)++{- | Call this to draw shapes and other debug draw data++Binds @b3World_Draw@.+-}+draw+  :: WorldId+  -> Ptr DebugDraw+  -> Word64+  -- ^ @maskBits@+  -> IO ()+draw a0 a1 a2 =+  c_b3World_Draw a0 a1 a2++{- | Get the world\'s bounds. This is the bounding box that covers the current simulation. May have a small+amount of padding.++Binds @b3World_GetBounds@.+-}+getBounds+  :: WorldId+  -> IO AABB+getBounds a0 =+  alloca $ \pOut -> c_b3World_GetBounds a0 pOut >> peek pOut++-- | Like 'getBounds', but the result is written into a caller-supplied buffer.+getBoundsInto+  :: WorldId+  -> Ptr AABB+  -- ^ Result buffer.+  -> IO ()+getBoundsInto a0 out =+  c_b3World_GetBounds a0 out++{- | Get the body events for the current time step. The event data is transient. Do not store a reference to this data.++Binds @b3World_GetBodyEvents@.+-}+getBodyEvents+  :: WorldId+  -> IO BodyEvents+getBodyEvents a0 =+  alloca $ \pOut -> c_b3World_GetBodyEvents a0 pOut >> peek pOut++-- | Like 'getBodyEvents', but the result is written into a caller-supplied buffer.+getBodyEventsInto+  :: WorldId+  -> Ptr BodyEvents+  -- ^ Result buffer.+  -> IO ()+getBodyEventsInto a0 out =+  c_b3World_GetBodyEvents a0 out++{- | Get sensor events for the current time step. The event data is transient. Do not store a reference to this data.++Binds @b3World_GetSensorEvents@.+-}+getSensorEvents+  :: WorldId+  -> IO SensorEvents+getSensorEvents a0 =+  alloca $ \pOut -> c_b3World_GetSensorEvents a0 pOut >> peek pOut++-- | Like 'getSensorEvents', but the result is written into a caller-supplied buffer.+getSensorEventsInto+  :: WorldId+  -> Ptr SensorEvents+  -- ^ Result buffer.+  -> IO ()+getSensorEventsInto a0 out =+  c_b3World_GetSensorEvents a0 out++{- | Get contact events for this current time step. The event data is transient. Do not store a reference to this data.++Binds @b3World_GetContactEvents@.+-}+getContactEvents+  :: WorldId+  -> IO ContactEvents+getContactEvents a0 =+  alloca $ \pOut -> c_b3World_GetContactEvents a0 pOut >> peek pOut++-- | Like 'getContactEvents', but the result is written into a caller-supplied buffer.+getContactEventsInto+  :: WorldId+  -> Ptr ContactEvents+  -- ^ Result buffer.+  -> IO ()+getContactEventsInto a0 out =+  c_b3World_GetContactEvents a0 out++{- | Get the joint events for the current time step. The event data is transient. Do not store a reference to this data.++Binds @b3World_GetJointEvents@.+-}+getJointEvents+  :: WorldId+  -> IO JointEvents+getJointEvents a0 =+  alloca $ \pOut -> c_b3World_GetJointEvents a0 pOut >> peek pOut++-- | Like 'getJointEvents', but the result is written into a caller-supplied buffer.+getJointEventsInto+  :: WorldId+  -> Ptr JointEvents+  -- ^ Result buffer.+  -> IO ()+getJointEventsInto a0 out =+  c_b3World_GetJointEvents a0 out++{- | Overlap test for all shapes that *potentially* overlap the provided AABB++Binds @b3World_OverlapAABB@.+-}+overlapAABB+  :: WorldId+  -> AABB+  -> QueryFilter+  -> FunPtr OverlapResultFcn+  -> Ptr ()+  -- ^ @context@+  -> IO TreeStats+overlapAABB a0 a1 a2 a3 a4 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  alloca $ \pOut -> c_b3World_OverlapAABB a0 p1 p2 a3 a4 pOut >> peek pOut++{- | Overlap test for all shapes that overlap the provided shape proxy. The proxy points are relative+to the world origin, which lets the query stay precise far from the world origin.++Binds @b3World_OverlapShape@.+-}+overlapShape+  :: WorldId+  -> Pos+  -- ^ @origin@+  -> Ptr ShapeProxy+  -> QueryFilter+  -> FunPtr OverlapResultFcn+  -> Ptr ()+  -- ^ @context@+  -> IO TreeStats+overlapShape a0 a1 a2 a3 a4 a5 =+  with a1 $ \p1 ->+  with a3 $ \p3 ->+  alloca $ \pOut -> c_b3World_OverlapShape a0 p1 a2 p3 a4 a5 pOut >> peek pOut++{- | 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++Returns: traversal performance counters++Binds @b3World_CastRay@.+-}+castRay+  :: WorldId+  -- ^ @worldId@: The world to cast the ray against+  -> Pos+  -- ^ @origin@: The start point of the ray+  -> Vec3+  -- ^ @translation@: The translation of the ray from the start point to the end point+  -> QueryFilter+  -- ^ @filter@: Contains bit flags to filter unwanted shapes from the results+  -> FunPtr CastResultFcn+  -- ^ @fcn@: A user implemented callback function+  -> Ptr ()+  -- ^ @context@: A user context that is passed along to the callback function+  -> IO TreeStats+castRay a0 a1 a2 a3 a4 a5 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  with a3 $ \p3 ->+  alloca $ \pOut -> c_b3World_CastRay a0 p1 p2 p3 a4 a5 pOut >> peek pOut++{- | 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.++Binds @b3World_CastRayClosest@.+-}+castRayClosest+  :: WorldId+  -> Pos+  -- ^ @origin@+  -> Vec3+  -- ^ @translation@+  -> QueryFilter+  -> IO RayResult+castRayClosest a0 a1 a2 a3 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  with a3 $ \p3 ->+  alloca $ \pOut -> c_b3World_CastRayClosest a0 p1 p2 p3 pOut >> peek pOut++{- | Cast a shape through the world. Similar to a cast ray except that a shape is cast instead of a point.+The proxy points are relative to the origin and the hit points come back as world positions, so the+cast stays precise far from the world origin.++See also @b3World_CastRay@.++Binds @b3World_CastShape@.+-}+castShape+  :: WorldId+  -> Pos+  -- ^ @origin@+  -> Ptr ShapeProxy+  -> Vec3+  -- ^ @translation@+  -> QueryFilter+  -> FunPtr CastResultFcn+  -> Ptr ()+  -- ^ @context@+  -> IO TreeStats+castShape a0 a1 a2 a3 a4 a5 a6 =+  with a1 $ \p1 ->+  with a3 $ \p3 ->+  with a4 $ \p4 ->+  alloca $ \pOut -> c_b3World_CastShape a0 p1 a2 p3 p4 a5 a6 pOut >> peek pOut++{- | 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@.++Returns: the translation fraction++Binds @b3World_CastMover@.+-}+castMover+  :: WorldId+  -- ^ @worldId@: World to cast the mover against+  -> Pos+  -- ^ @origin@: World position the mover capsule is relative to+  -> Capsule+  -- ^ @mover@: Capsule mover, relative to the origin+  -> Vec3+  -- ^ @translation@: Desired mover translation+  -> QueryFilter+  -- ^ @filter@: Contains bit flags to filter unwanted shapes from the results+  -> FunPtr MoverFilterFcn+  -- ^ @fcn@: Optional callback for custom shape filtering+  -> Ptr ()+  -- ^ @context@: A user context that is passed along to the callback function+  -> IO Float+castMover a0 a1 a2 a3 a4 a5 a6 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  with a3 $ \p3 ->+  with a4 $ \p4 ->+  c_b3World_CastMover a0 p1 p2 p3 p4 a5 a6++{- | Collide a capsule mover with the world, gathering collision planes that can be fed to @b3SolvePlanes@. Useful for+kinematic character movement. The mover and the returned planes are relative to the origin.++Binds @b3World_CollideMover@.+-}+collideMover+  :: WorldId+  -> Pos+  -- ^ @origin@+  -> Capsule+  -- ^ @mover@+  -> QueryFilter+  -> FunPtr PlaneResultFcn+  -> Ptr ()+  -- ^ @context@+  -> IO ()+collideMover a0 a1 a2 a3 a4 a5 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  with a3 $ \p3 ->+  c_b3World_CollideMover a0 p1 p2 p3 a4 a5++{- | Enable\/disable sleep. If your application does not need sleeping, you can gain some performance+by disabling sleep completely at the world level.++See also @b3WorldDef@.++Binds @b3World_EnableSleeping@.+-}+enableSleeping+  :: WorldId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableSleeping a0 a1 =+  c_b3World_EnableSleeping a0 (fromBool a1)++{- | Is body sleeping enabled?++Binds @b3World_IsSleepingEnabled@.+-}+isSleepingEnabled+  :: WorldId+  -> IO Bool+isSleepingEnabled a0 =+  toBool <$> (c_b3World_IsSleepingEnabled a0)++{- | 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 also @b3WorldDef@.++Binds @b3World_EnableContinuous@.+-}+enableContinuous+  :: WorldId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableContinuous a0 a1 =+  c_b3World_EnableContinuous a0 (fromBool a1)++{- | Is continuous collision enabled?++Binds @b3World_IsContinuousEnabled@.+-}+isContinuousEnabled+  :: WorldId+  -> IO Bool+isContinuousEnabled a0 =+  toBool <$> (c_b3World_IsContinuousEnabled a0)++{- | 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 also @b3WorldDef@.++Binds @b3World_SetRestitutionThreshold@.+-}+setRestitutionThreshold+  :: WorldId+  -> Float+  -- ^ @value@+  -> IO ()+setRestitutionThreshold a0 a1 =+  c_b3World_SetRestitutionThreshold a0 a1++{- | Get the restitution speed threshold. Usually in meters per second.++Binds @b3World_GetRestitutionThreshold@.+-}+getRestitutionThreshold+  :: WorldId+  -> IO Float+getRestitutionThreshold a0 =+  c_b3World_GetRestitutionThreshold a0++{- | Adjust the hit event threshold. This controls the collision speed needed to generate a @b3ContactHitEvent@.+Usually in meters per second.++See also @b3WorldDef::hitEventThreshold@.++Binds @b3World_SetHitEventThreshold@.+-}+setHitEventThreshold+  :: WorldId+  -> Float+  -- ^ @value@+  -> IO ()+setHitEventThreshold a0 a1 =+  c_b3World_SetHitEventThreshold a0 a1++{- | Get the hit event speed threshold. Usually in meters per second.++Binds @b3World_GetHitEventThreshold@.+-}+getHitEventThreshold+  :: WorldId+  -> IO Float+getHitEventThreshold a0 =+  c_b3World_GetHitEventThreshold a0++{- | Register the custom filter callback. This is optional.++Binds @b3World_SetCustomFilterCallback@.+-}+setCustomFilterCallback+  :: WorldId+  -> FunPtr CustomFilterFcn+  -> Ptr ()+  -- ^ @context@+  -> IO ()+setCustomFilterCallback a0 a1 a2 =+  c_b3World_SetCustomFilterCallback a0 a1 a2++{- | Register the pre-solve callback. This is optional.++Binds @b3World_SetPreSolveCallback@.+-}+setPreSolveCallback+  :: WorldId+  -> FunPtr PreSolveFcn+  -> Ptr ()+  -- ^ @context@+  -> IO ()+setPreSolveCallback a0 a1 a2 =+  c_b3World_SetPreSolveCallback a0 a1 a2++{- | Set the gravity vector for the entire world. Box3D has no concept of an up direction and this+is left as a decision for the application. Usually in m\/s^2.++See also @b3WorldDef@.++Binds @b3World_SetGravity@.+-}+setGravity+  :: WorldId+  -> Vec3+  -- ^ @gravity@+  -> IO ()+setGravity a0 a1 =+  with a1 $ \p1 ->+  c_b3World_SetGravity a0 p1++{- | Get the gravity vector++Binds @b3World_GetGravity@.+-}+getGravity+  :: WorldId+  -> IO Vec3+getGravity a0 =+  alloca $ \pOut -> c_b3World_GetGravity a0 pOut >> peek pOut++-- | Like 'getGravity', but the result is written into a caller-supplied buffer.+getGravityInto+  :: WorldId+  -> Ptr Vec3+  -- ^ Result buffer.+  -> IO ()+getGravityInto a0 out =+  c_b3World_GetGravity a0 out++{- | Apply a radial explosion++Binds @b3World_Explode@.+-}+explode+  :: WorldId+  -- ^ @worldId@: The world id+  -> ExplosionDef+  -- ^ @explosionDef@: The explosion definition+  -> IO ()+explode a0 a1 =+  with a1 $ \p1 ->+  c_b3World_Explode a0 p1++{- | Adjust contact tuning parameters++__Note:__ Advanced feature++Binds @b3World_SetContactTuning@.+-}+setContactTuning+  :: WorldId+  -- ^ @worldId@: The world id+  -> Float+  -- ^ @hertz@: The contact stiffness (cycles per second)+  -> Float+  -- ^ @dampingRatio@: The contact bounciness with 1 being critical damping (non-dimensional)+  -> Float+  -- ^ @contactSpeed@: The maximum contact constraint push out speed (meters per second)+  -> IO ()+setContactTuning a0 a1 a2 a3 =+  c_b3World_SetContactTuning a0 a1 a2 a3++{- | Set the contact point recycling distance. Setting this to zero disables contact point recycling.+Usually in meters.++Binds @b3World_SetContactRecycleDistance@.+-}+setContactRecycleDistance+  :: WorldId+  -> Float+  -- ^ @recycleDistance@+  -> IO ()+setContactRecycleDistance a0 a1 =+  c_b3World_SetContactRecycleDistance a0 a1++{- | Get the contact point recycling distance. Usually in meters.++Binds @b3World_GetContactRecycleDistance@.+-}+getContactRecycleDistance+  :: WorldId+  -> IO Float+getContactRecycleDistance a0 =+  c_b3World_GetContactRecycleDistance a0++{- | Set the maximum linear speed. Usually in m\/s.++Binds @b3World_SetMaximumLinearSpeed@.+-}+setMaximumLinearSpeed+  :: WorldId+  -> Float+  -- ^ @maximumLinearSpeed@+  -> IO ()+setMaximumLinearSpeed a0 a1 =+  c_b3World_SetMaximumLinearSpeed a0 a1++{- | Get the maximum linear speed. Usually in m\/s.++Binds @b3World_GetMaximumLinearSpeed@.+-}+getMaximumLinearSpeed+  :: WorldId+  -> IO Float+getMaximumLinearSpeed a0 =+  c_b3World_GetMaximumLinearSpeed a0++{- | Enable\/disable constraint warm starting. Advanced feature for testing. Disabling+warm starting greatly reduces stability and provides no performance gain.++Binds @b3World_EnableWarmStarting@.+-}+enableWarmStarting+  :: WorldId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableWarmStarting a0 a1 =+  c_b3World_EnableWarmStarting a0 (fromBool a1)++{- | Is constraint warm starting enabled?++Binds @b3World_IsWarmStartingEnabled@.+-}+isWarmStartingEnabled+  :: WorldId+  -> IO Bool+isWarmStartingEnabled a0 =+  toBool <$> (c_b3World_IsWarmStartingEnabled a0)++{- | Get the number of awake bodies++Binds @b3World_GetAwakeBodyCount@.+-}+getAwakeBodyCount+  :: WorldId+  -> IO Int+getAwakeBodyCount a0 =+  fromIntegral <$> (c_b3World_GetAwakeBodyCount a0)++{- | Get the current world performance profile++Binds @b3World_GetProfile@.+-}+getProfile+  :: WorldId+  -> Ptr Profile+  -- ^ Result buffer.+  -> IO ()+getProfile a0 out =+  c_b3World_GetProfile a0 out++{- | Get world counters and sizes++Binds @b3World_GetCounters@.+-}+getCounters+  :: WorldId+  -> Ptr Counters+  -- ^ Result buffer.+  -> IO ()+getCounters a0 out =+  c_b3World_GetCounters a0 out++{- | Get max capacity. This can be used with @b3WorldDef@ to avoid run-time allocations and copies++Binds @b3World_GetMaxCapacity@.+-}+getMaxCapacity+  :: WorldId+  -> IO Capacity+getMaxCapacity a0 =+  alloca $ \pOut -> c_b3World_GetMaxCapacity a0 pOut >> peek pOut++-- | Like 'getMaxCapacity', but the result is written into a caller-supplied buffer.+getMaxCapacityInto+  :: WorldId+  -> Ptr Capacity+  -- ^ Result buffer.+  -> IO ()+getMaxCapacityInto a0 out =+  c_b3World_GetMaxCapacity a0 out++{- | Set the user data pointer.++Binds @b3World_SetUserData@.+-}+setUserData+  :: WorldId+  -> Ptr ()+  -- ^ @userData@+  -> IO ()+setUserData a0 a1 =+  c_b3World_SetUserData a0 a1++{- | Get the user data pointer.++Binds @b3World_GetUserData@.+-}+getUserData+  :: WorldId+  -> IO (Ptr ())+getUserData a0 =+  c_b3World_GetUserData a0++{- | Set the friction callback. Passing NULL resets to default.++Binds @b3World_SetFrictionCallback@.+-}+setFrictionCallback+  :: WorldId+  -> FunPtr FrictionCallback+  -> IO ()+setFrictionCallback a0 a1 =+  c_b3World_SetFrictionCallback a0 a1++{- | Set the restitution callback. Passing NULL resets to default.++Binds @b3World_SetRestitutionCallback@.+-}+setRestitutionCallback+  :: WorldId+  -> FunPtr RestitutionCallback+  -> IO ()+setRestitutionCallback a0 a1 =+  c_b3World_SetRestitutionCallback a0 a1++{- | Set the worker count. Must be in the range \[1, B3_MAX_WORKERS\]++Binds @b3World_SetWorkerCount@.+-}+setWorkerCount+  :: WorldId+  -> Int+  -- ^ @count@+  -> IO ()+setWorkerCount a0 a1 =+  c_b3World_SetWorkerCount a0 (fromIntegral a1)++{- | Get the worker count.++Binds @b3World_GetWorkerCount@.+-}+getWorkerCount+  :: WorldId+  -> IO Int+getWorkerCount a0 =+  fromIntegral <$> (c_b3World_GetWorkerCount a0)++{- | Dump memory stats to log.++Binds @b3World_DumpMemoryStats@.+-}+dumpMemoryStats+  :: WorldId+  -> IO ()+dumpMemoryStats a0 =+  c_b3World_DumpMemoryStats a0++{- | This is for internal testing++Binds @b3World_RebuildStaticTree@.+-}+rebuildStaticTree+  :: WorldId+  -> IO ()+rebuildStaticTree a0 =+  c_b3World_RebuildStaticTree a0++{- | This is for internal testing++Binds @b3World_EnableSpeculative@.+-}+enableSpeculative+  :: WorldId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableSpeculative a0 a1 =+  c_b3World_EnableSpeculative a0 (fromBool a1)++{- | Begin recording world mutations into the provided buffer.+The buffer is reset on each call so a single @b3Recording@ can be reused for multiple sessions.++Binds @b3World_StartRecording@.+-}+startRecording+  :: WorldId+  -- ^ @worldId@: the world to record+  -> Ptr Recording+  -- ^ @recording@: the recording handle to write into+  -> IO ()+startRecording a0 a1 =+  c_b3World_StartRecording a0 a1++{- | End the current recording session. Writes the trailing geometry registry and+backpatches the header. The buffer remains valid until the recording is destroyed.++Binds @b3World_StopRecording@.+-}+stopRecording+  :: WorldId+  -- ^ @worldId@: the world currently being recorded+  -> IO ()+stopRecording a0 =+  c_b3World_StopRecording a0
+ test/Spec.hs view
@@ -0,0 +1,1128 @@+{-# LANGUAGE PatternSynonyms #-}++module Main (main) where++import Control.Concurrent (getNumCapabilities, yield)+import Control.Exception (ErrorCall (..), evaluate, finally, throwIO, try)+import Control.Monad (forM_, replicateM_, unless, when)+import Data.IORef+import Data.Vector.Storable qualified as VS+import Foreign+import GHC.Clock (getMonotonicTime)++import Test.Tasty+import Test.Tasty.HUnit+import Test.Tasty.Runners (NumThreads (..))++import Box3D.Authoring qualified as Authoring+import Box3D.Base qualified as Base+import Box3D.Body qualified as Body+import Box3D.Callbacks qualified as CB+import Box3D.Collision qualified as Collision+import Box3D.Compound qualified as Compound+import Box3D.Contact qualified as Contact+import Box3D.DistanceJoint qualified as DistanceJoint+import Box3D.DynamicTree qualified as DynamicTree+import Box3D.Events qualified as Events+import Box3D.Extra (withShapeProxy)+import Box3D.HeightField qualified as HeightField+import Box3D.Id+import Box3D.Joint qualified as Joint+import Box3D.MathFunctions+import Box3D.MathTypes+import Box3D.Mesh qualified as Mesh+import Box3D.Mover qualified as Mover+import Box3D.Shape qualified as Shape+import Box3D.UserData qualified as UD+import Box3D.World qualified as World++import Box3D.Body (BodyDef (..), defaultBodyDef, pattern DynamicBody, pattern StaticBody)+import Box3D.Collision (CollisionPlane (..), Manifold (..))+import Box3D.DebugDraw (DebugDraw (..), defaultDebugDraw)+import Box3D.DebugShape qualified as DebugShape+import Box3D.DistanceJoint (DistanceJointDef (..), defaultDistanceJointDef)+import Box3D.DynamicTree (dynamicTreeByteCount)+import Box3D.FilterJoint (defaultFilterJointDef)+import Box3D.Geometry (Capsule (..), MassData (..), Sphere (..))+import Box3D.Joint (JointDef (..), pattern DistanceJoint)+import Box3D.MotorJoint (defaultMotorJointDef)+import Box3D.ParallelJoint (defaultParallelJointDef)+import Box3D.PrismaticJoint (defaultPrismaticJointDef)+import Box3D.Query (CastOutput (..), QueryFilter (..), RayCastInput (..), RayResult (..), SimplexCache (..), defaultQueryFilter)+import Box3D.RevoluteJoint (defaultRevoluteJointDef)+import Box3D.Shape (ShapeDef (..), defaultFilter, defaultShapeDef, defaultSurfaceMaterial, pattern CapsuleShape, pattern CompoundShape, pattern HeightShape, pattern HullShape, pattern MeshShape, pattern SphereShape)+import Box3D.SphericalJoint (defaultSphericalJointDef)+import Box3D.WeldJoint (defaultWeldJointDef)+import Box3D.WheelJoint (defaultWheelJointDef)+import Box3D.World (BodyMoveEvent (..), ContactBeginTouchEvent (..), ContactData (..), ContactEndTouchEvent (..), ContactHitEvent (..), ExplosionDef (..), JointEvent (..), SensorBeginTouchEvent (..), SensorEndTouchEvent (..), WorldDef (..), defaultExplosionDef, defaultWorldDef)++main :: IO ()+-- Box3D keeps a global registry of worlds and a world is single-threaded+-- state, so the suite must run sequentially rather than tasty's default+-- parallel execution.+main = defaultMain (localOption (NumThreads 1) tests)++tests :: TestTree+tests =+  testGroup+    "Box3D"+    [ testGroup "Base" baseTests+    , testGroup "MathTypes" mathTypesTests+    , testGroup "MathFunctions" mathFunctionsTests+    , testGroup "Id" idTests+    , testGroup "Types" typesTests+    , testGroup "Simulation" simulationTests+    , testGroup "Joints" jointTests+    , testGroup "Events" eventTests+    , testGroup "Queries" queryTests+    , testGroup "Proxies, contacts and the mover" moverTests+    , testGroup "Geometry from data" geometryTests+    , testGroup "Callbacks" callbackTests+    , testGroup "Debug shapes" debugShapeTests+    ]++-- | Poke a value and read it back, exercising the 'Storable' instance.+roundTrip :: (Storable a) => a -> IO a+roundTrip x = alloca $ \p -> poke p x >> peek p++approx :: Float -> Float -> Assertion+approx expected actual =+  assertBool+    ("expected " <> show expected <> " but got " <> show actual)+    (abs (expected - actual) < 1e-5)++baseTests :: [TestTree]+baseTests =+  [ testCase "getVersion is 0.2.0" $ do+      v <- Base.getVersion+      v.major @?= 0+      v.minor @?= 2+      v.revision @?= 0+  , testCase "getByteCount is non-negative" $ do+      n <- Base.getByteCount+      assertBool "byte count >= 0" (n >= 0)+  , testCase "isDoublePrecision matches the size of Pos" $ do+      dp <- Base.isDoublePrecision+      sizeOf (undefined :: Pos) @?= if dp then 24 else 12+  ]++mathTypesTests :: [TestTree]+mathTypesTests =+  [ testCase "Pos and WorldTransform round-trip through Storable" $ do+      roundTrip (Pos 1.5 (-2.5) 3.5) >>= (@?= Pos 1.5 (-2.5) 3.5)+      let xf = WorldTransform (Pos 3 4 5) quatIdentity+      roundTrip xf >>= (@?= xf)+  , testCase "world position helpers cross the Pos/Vec3 boundary" $ do+      let xf = WorldTransform (Pos 10 20 30) quatIdentity+      toVec3 (toPos (Vec3 1.5 2.5 3.5)) @?= Vec3 1.5 2.5 3.5+      subPos (Pos 5 7 9) (Pos 2 3 4) @?= Vec3 3 4 5+      offsetPos (Pos 1 2 3) (Vec3 3 4 5) @?= Pos 4 6 8+      transformWorldPoint xf (Vec3 1 0 0) @?= Pos 11 20 30+      invTransformWorldPoint xf (Pos 11 20 30) @?= Vec3 1 0 0+      toRelativeTransform xf (Pos 9 18 27) @?= Transform (Vec3 1 2 3) quatIdentity+      makeWorldTransform (Transform (Vec3 1 2 3) quatIdentity) @?= WorldTransform (Pos 1 2 3) quatIdentity+  , testCase "Vec3 size/align match the C ABI" $ do+      sizeOf (Vec2 0 0) @?= 8+      sizeOf (Vec3 0 0 0) @?= 12+      sizeOf (Quat vec3Zero 0) @?= 16+      sizeOf (Transform vec3Zero quatIdentity) @?= 28+  , testCase "Vec3 round-trips" $+      roundTrip (Vec3 1 2 3) >>= (@?= Vec3 1 2 3)+  , testCase "Quat round-trips" $+      roundTrip (Quat (Vec3 1 2 3) 4) >>= (@?= Quat (Vec3 1 2 3) 4)+  , testCase "Transform round-trips" $ do+      let t = Transform (Vec3 1 2 3) (Quat (Vec3 4 5 6) 7)+      roundTrip t >>= (@?= t)+  , testCase "Matrix3 round-trips" $ do+      let m = Matrix3 (Vec3 1 2 3) (Vec3 4 5 6) (Vec3 7 8 9)+      roundTrip m >>= (@?= m)+  ]++mathFunctionsTests :: [TestTree]+mathFunctionsTests =+  [ testCase "finite vector is valid" $+      isValidVec3 (Vec3 1 2 3) >>= (@?= True)+  , testCase "NaN vector is invalid" $+      isValidVec3 (Vec3 (0 / 0) 0 0) >>= (@?= False)+  , testCase "infinite vector is invalid" $+      isValidVec3 (Vec3 (1 / 0) 0 0) >>= (@?= False)+  , testCase "identity quaternion is valid" $+      isValidQuat quatIdentity >>= (@?= True)+  , testCase "identity transform is valid" $+      isValidTransform transformIdentity >>= (@?= True)+  , testCase "identity matrix is valid" $+      isValidMatrix3 matrix3Identity >>= (@?= True)+  , testCase "computeCosSin 0 = (1, 0)" $ do+      CosSin c s <- computeCosSin 0+      approx 1 c+      approx 0 s+  , testCase "computeCosSin pi/2 = (0, 1)" $ do+      CosSin c s <- computeCosSin (pi / 2)+      approx 0 c+      approx 1 s+  , testCase "rotation between identical axes is identity" $ do+      let ax = Vec3 1 0 0+      Quat _ s <- computeQuatBetweenUnitVectors ax ax+      approx 1 s+  , testCase "rotation from +X to +Y is 90 degrees about +Z" $ do+      let+        ax = Vec3 1 0 0+        ay = Vec3 0 1 0+      Quat (Vec3 vx vy vz) s <- computeQuatBetweenUnitVectors ax ay+      approx 0 vx+      approx 0 vy+      approx 0.70710677 vz+      approx 0.70710677 s+  ]++idTests :: [TestTree]+idTests =+  [ testCase "id sizes match the C ABI" $ do+      sizeOf nullWorldId @?= 4+      sizeOf nullBodyId @?= 8+      sizeOf nullContactId @?= 12+  , testCase "WorldId round-trips" $+      roundTrip (WorldId 0x50009) >>= (@?= WorldId 0x50009)+  , testCase "BodyId round-trips" $+      roundTrip (BodyId 0x1122334455667788) >>= (@?= BodyId 0x1122334455667788)+  , testCase "ContactId round-trips" $+      roundTrip (ContactId 1 2 4) >>= (@?= ContactId 1 2 4)+  ]++typesTests :: [TestTree]+typesTests =+  [ testCase "default world def has expected fields" $ do+      d <- defaultWorldDef+      d.gravity @?= Vec3 0 (-10) 0+      d.contactHertz @?= 30+      d.enableSleep @?= 1+      assertBool "internalValue cookie is set" (d.internalValue /= 0)+  , testCase "default world def round-trips" $ do+      d <- defaultWorldDef+      roundTrip d >>= (@?= d)+  , testCase "default body def has expected fields" $ do+      d <- defaultBodyDef+      d.type_ @?= StaticBody+      d.rotation @?= quatIdentity+      d.gravityScale @?= 1+      assertBool "internalValue cookie is set" (d.internalValue /= 0)+  , testCase "default body def round-trips" $ do+      d <- defaultBodyDef+      roundTrip d >>= (@?= d)+  , testCase "default shape def has expected fields" $ do+      d <- defaultShapeDef+      d.density @?= 1000+      d.updateBodyMass @?= 1+      assertBool "internalValue cookie is set" (d.internalValue /= 0)+  , testCase "default shape def round-trips" $ do+      d <- defaultShapeDef+      roundTrip d >>= (@?= d)+  , testCase "default filter round-trips" $ do+      f <- defaultFilter+      roundTrip f >>= (@?= f)+  , testCase "default surface material round-trips" $ do+      m <- defaultSurfaceMaterial+      roundTrip m >>= (@?= m)+  , testCase "Sphere round-trips" $ do+      let s = Sphere (Vec3 1 2 3) 0.5+      roundTrip s >>= (@?= s)+  , testCase "Capsule round-trips" $ do+      let c = Capsule (Vec3 0 0 0) (Vec3 0 1 0) 0.25+      roundTrip c >>= (@?= c)+  , testCase "shape types case as patterns" $+      withWorld (Vec3 0 0 0) $ \w -> do+        bd <- defaultBodyDef+        b <- Body.create w bd+        sd <- defaultShapeDef+        sh <- Shape.createSphere b sd (Sphere vec3Zero 0.5)+        ty <- Shape.getType sh+        let name = case ty of+              CapsuleShape -> "capsule"+              CompoundShape -> "compound"+              HeightShape -> "height field"+              HullShape -> "hull"+              MeshShape -> "mesh"+              SphereShape -> "sphere"+              _ -> "unknown"+        name @?= ("sphere" :: String)+  , testCase "a warm SimplexCache round-trips its indices" $ do+      let cache = SimplexCache 0.5 (VS.fromList [1, 2, 3, 4]) (VS.fromList [5, 6, 7, 8])+      roundTrip cache >>= (@?= cache)+  , testCase "a SimplexCache count follows its shorter index array" $+      roundTrip (SimplexCache 0.5 (VS.fromList [1, 2]) (VS.fromList [3]))+        >>= (@?= SimplexCache 0.5 (VS.fromList [1]) (VS.fromList [3]))+  , testCase "a SimplexCache with a garbage count stays inside its arrays" $+      alloca $ \(p :: Ptr SimplexCache) -> do+        fillBytes p 0xff (sizeOf (undefined :: SimplexCache))+        cache <- peek p+        VS.length cache.indexA @?= 4+        VS.length cache.indexB @?= 4+  , testCase "Counters reads a live world with full-length arrays" $+      withWorld (Vec3 0 (-10) 0) $ \w -> do+        _ <- dynamicSphere w (Pos 0 5 0) 0.5+        _ <- dynamicSphere w (Pos 3 5 0) 0.5+        World.step w (1 / 60) 4+        counters <- alloca $ \p -> World.getCounters w p >> peek p+        counters.bodyCount @?= 2+        VS.length counters.colorCounts @?= 24+        VS.length counters.manifoldCounts @?= World.contactManifoldCountBuckets+  , testCase "an explosion pushes a nearby body away" $+      withWorld (Vec3 0 0 0) $ \w -> do+        b <- dynamicSphere w (Pos 1 0 0) 0.5+        ed <- defaultExplosionDef+        World.explode+          w+          ed+            { position = Pos 0 0 0+            , radius = 2+            , impulsePerArea = 5+            }+        Vec3 vx _ _ <- Body.getLinearVelocity b+        assertBool ("pushed away from the blast, vx = " <> show vx) (vx > 0)+  , testCase "user indices round-trip through body, shape and joint slots" $+      withWorld (Vec3 0 0 0) $ \w -> do+        bd <- defaultBodyDef+        b <- Body.create w bd+        sd <- defaultShapeDef+        sh <- Shape.createSphere b sd (Sphere vec3Zero 0.5)+        UD.setUserIndex b 42+        UD.getUserIndex b >>= (@?= 42)+        UD.setUserIndex sh maxBound+        UD.getUserIndex sh >>= (@?= maxBound)+        UD.setUserIndex b (-7)+        UD.getUserIndex b >>= (@?= (-7))+  ]++-- | Build a world with the given gravity and default settings.+withWorld :: Vec3 -> (WorldId -> IO a) -> IO a+withWorld gravity act = do+  wd <- defaultWorldDef+  w <- World.create wd{gravity}+  r <- act w+  World.destroy w+  pure r++-- | A dynamic body carrying a unit sphere at the given position.+dynamicSphere :: WorldId -> Pos -> Float -> IO BodyId+dynamicSphere w position radius = do+  bd <- defaultBodyDef+  b <- Body.create w bd{type_ = DynamicBody, position = position}+  sd <- defaultShapeDef+  _ <- Shape.createSphere b sd (Sphere vec3Zero radius)+  pure b++posY :: Pos -> Float+posY p = y+  where+    Vec3 _ y _ = toVec3 p++simulationTests :: [TestTree]+simulationTests =+  [ testCase "world is valid until destroyed" $ do+      wd <- defaultWorldDef+      w <- World.create wd+      World.isValid w >>= (@?= True)+      World.destroy w+      World.isValid w >>= (@?= False)+  , testCase "gravity can be read and set" $+      withWorld (Vec3 0 (-10) 0) $ \w -> do+        World.getGravity w >>= (@?= Vec3 0 (-10) 0)+        World.setGravity w (Vec3 0 0 (-20))+        World.getGravity w >>= (@?= Vec3 0 0 (-20))+  , testCase "a body and its shape wire up correctly" $+      withWorld (Vec3 0 (-10) 0) $ \w -> do+        bd <- defaultBodyDef+        b <- Body.create w bd{type_ = DynamicBody, position = Pos 0 10 0}+        Body.isValid b >>= (@?= True)+        Body.getType b >>= (@?= DynamicBody)+        Body.getPosition b >>= (@?= Pos 0 10 0)+        sd <- defaultShapeDef+        s <- Shape.createSphere b sd (Sphere vec3Zero 0.5)+        Shape.isValid s >>= (@?= True)+        Shape.getType s >>= (@?= SphereShape)+        Shape.getBody s >>= (@?= b)+  , testCase "a dynamic body falls under gravity" $+      withWorld (Vec3 0 (-10) 0) $ \w -> do+        b <- dynamicSphere w (Pos 0 10 0) 0.5+        replicateM_ 60 (World.step w (1 / 60) 4)+        y <- posY <$> Body.getPosition b+        -- one second of free fall from rest under g = -10 lands near y = 5+        assertBool ("body fell, y = " <> show y) (y > 4 && y < 6)+        Vec3 _ vy _ <- Body.getLinearVelocity b+        assertBool ("velocity is downward, vy = " <> show vy) (vy < -8)+  , testCase "a dynamic body rests on a static ground" $+      withWorld (Vec3 0 (-10) 0) $ \w -> do+        -- static ground: a large sphere centred at the origin (top at y = 5)+        gd <- defaultBodyDef+        ground <- Body.create w gd+        gsd <- defaultShapeDef+        _ <- Shape.createSphere ground gsd (Sphere vec3Zero 5)+        ball <- dynamicSphere w (Pos 0 8 0) 0.5+        replicateM_ 240 (World.step w (1 / 60) 4)+        y <- posY <$> Body.getPosition ball+        -- the ball comes to rest on top of the ground: y ~= 5 + 0.5+        assertBool ("ball rests on the ground, y = " <> show y) (y > 5 && y < 6.5)+  ]++{- | Exercise the Tier B path unlocked by the hand-written joint-def Storables:+build a @DistanceJointDef@ value (setting fields on the embedded base),+create the joint, and confirm it constrains the bodies.+-}+jointTests :: [TestTree]+jointTests =+  [ testCase "a rigid distance joint hangs a body below a static anchor" $+      withWorld (Vec3 0 (-10) 0) $ \w -> do+        gd <- defaultBodyDef+        anchor <- Body.create w gd{Body.position = Pos 0 10 0}+        bd <- defaultBodyDef+        hanging <- Body.create w bd{type_ = DynamicBody, position = Pos 0 8 0}+        sd <- defaultShapeDef+        _ <- Shape.createSphere hanging sd (Sphere vec3Zero 0.25)+        djd <- defaultDistanceJointDef+        let+          base = djd.base+          def =+            djd+              { base =+                  base{bodyIdA = anchor, bodyIdB = hanging}+              , length = 2+              }+        j <- DistanceJoint.create w def+        Joint.isValid j >>= (@?= True)+        Joint.getType j >>= (@?= DistanceJoint)+        Joint.getBodyA j >>= (@?= anchor)+        Joint.getBodyB j >>= (@?= hanging)+        DistanceJoint.getLength j >>= approx 2+        replicateM_ 120 (World.step w (1 / 60) 4)+        -- rigid length 2 below the anchor at (0, 10, 0): settles near y = 8+        y <- posY <$> Body.getPosition hanging+        assertBool ("body hangs ~2 below anchor, y = " <> show y) (y > 7.5 && y < 8.5)+  , testCase "default joint defs round-trip through their Storable" $ do+      let rt mk = mk >>= \x -> roundTrip x >>= (@?= x)+      rt defaultDistanceJointDef+      rt defaultMotorJointDef+      rt defaultParallelJointDef+      rt defaultPrismaticJointDef+      rt defaultRevoluteJointDef+      rt defaultSphericalJointDef+      rt defaultWeldJointDef+      rt defaultWheelJointDef+      rt defaultFilterJointDef+  ]++-- | Exercise the hand-written event-buffer marshalling (roadmap phase 5).+eventTests :: [TestTree]+eventTests =+  [ testCase "a moved body appears in the body move events" $+      withWorld (Vec3 0 (-10) 0) $ \w -> do+        b <- dynamicSphere w (Pos 0 10 0) 0.5+        replicateM_ 4 (World.step w (1 / 60) 4)+        evs <- Events.bodyMoveEvents w+        assertBool "at least one move event" (not (VS.null evs))+        assertBool+          "the falling body is among the moved bodies"+          (b `VS.elem` VS.map (.bodyId) evs)+  , testCase "a body falling through a sensor produces begin and end events" $+      withWorld (Vec3 0 (-10) 0) $ \w -> do+        gd <- defaultBodyDef+        sensorBody <- Body.create w gd+        gsd <- defaultShapeDef+        sensor <-+          Shape.createSphere+            sensorBody+            gsd{isSensor = 1, enableSensorEvents = 1}+            (Sphere vec3Zero 1)+        bd <- defaultBodyDef+        visitor <- Body.create w bd{type_ = DynamicBody, position = Pos 0 4 0}+        sd <- defaultShapeDef+        _ <-+          Shape.createSphere+            visitor+            sd{Shape.enableSensorEvents = 1}+            (Sphere vec3Zero 0.25)+        (begins, ends) <- collectEvents 240 w $ \acc -> do+          bs <- Events.sensorBeginTouchEvents w+          es <- Events.sensorEndTouchEvents w+          let (b0, e0) = acc+          pure (b0 <> bs, e0 <> es)+        assertBool+          "a begin touch event names the sensor"+          (sensor `VS.elem` VS.map (.sensorShapeId) begins)+        assertBool+          "an end touch event names the sensor"+          (sensor `VS.elem` VS.map (.sensorShapeId) ends)+  , testCase "a landing body produces contact begin, hit, and (once destroyed) end events" $+      withWorld (Vec3 0 (-10) 0) $ \w -> do+        -- static ground: a large sphere centred at the origin (top at y = 5)+        gd <- defaultBodyDef+        ground <- Body.create w gd+        gsd <- defaultShapeDef+        _ <-+          Shape.createSphere+            ground+            gsd{enableContactEvents = 1, enableHitEvents = 1}+            (Sphere vec3Zero 5)+        bd <- defaultBodyDef+        ball <- Body.create w bd{type_ = DynamicBody, position = Pos 0 8 0}+        sd <- defaultShapeDef+        bshape <-+          Shape.createSphere+            ball+            sd{enableContactEvents = 1, enableHitEvents = 1}+            (Sphere vec3Zero 0.5)+        (begins, hits) <- collectEvents 120 w $ \acc -> do+          bs <- Events.contactBeginTouchEvents w+          hs <- Events.contactHitEvents w+          let (b0, h0) = acc+          pure (b0 <> bs, h0 <> hs)+        let involves a b e = a e == bshape || b e == bshape+        assertBool+          "a contact begin event involves the ball"+          (VS.any (involves (.shapeIdA) (.shapeIdB)) begins)+        assertBool+          "a hit event involves the ball"+          (VS.any (involves (.shapeIdA) (.shapeIdB)) hits)+        assertBool+          "the hit approach speed is positive"+          (VS.all ((> 0) . (.approachSpeed)) hits && not (VS.null hits))+        -- destroying the touching body ends the contact on the next step+        Body.destroy ball+        World.step w (1 / 60) 4+        ends <- Events.contactEndTouchEvents w+        assertBool+          "a contact end event involves the (destroyed) ball shape"+          (VS.any (involves (.shapeIdA) (.shapeIdB)) ends)+  , testCase "a loaded joint with a low force threshold produces joint events" $+      withWorld (Vec3 0 (-10) 0) $ \w -> do+        gd <- defaultBodyDef+        anchor <- Body.create w gd{Body.position = Pos 0 10 0}+        bd <- defaultBodyDef+        hanging <- Body.create w bd{type_ = DynamicBody, position = Pos 0 8 0}+        sd <- defaultShapeDef+        _ <- Shape.createSphere hanging sd (Sphere vec3Zero 0.25)+        djd <- defaultDistanceJointDef+        let+          base = djd.base+          def =+            djd+              { base =+                  base{bodyIdA = anchor, bodyIdB = hanging}+              , length = 2+              }+        j <- DistanceJoint.create w def+        -- the hanging body loads the joint with ~mg newtons; any tiny+        -- threshold guarantees an event+        Joint.setForceThreshold j 0.01+        (evs, _) <- collectEvents 60 w $ \acc -> do+          es <- Events.jointEvents w+          let (e0, u) = acc+          pure (e0 <> es, u :: ())+        assertBool+          "a joint event names the loaded joint"+          (j `VS.elem` VS.map (.jointId) evs)+  ]++{- | Step the world @n@ times, folding the per-step event buffers into an+accumulator (events only survive until the next step, so they must be+collected inside the loop).+-}+collectEvents :: (Monoid acc) => Int -> WorldId -> (acc -> IO acc) -> IO acc+collectEvents n w gather = go n mempty+  where+    go 0 acc = pure acc+    go k acc = do+      World.step w (1 / 60) 4+      acc' <- gather acc+      go (k - 1) acc'++-- | A static body carrying a sphere at the given position.+staticSphere :: WorldId -> Pos -> Float -> IO ShapeId+staticSphere w position radius = do+  bd <- defaultBodyDef+  b <- Body.create w bd{Body.position = position}+  sd <- defaultShapeDef+  Shape.createSphere b sd (Sphere vec3Zero radius)++{- | Exercise the query/cast structs upgraded to value APIs: 'RayResult',+'QueryFilter', 'MassData', 'RayCastInput' and 'CastOutput' cross the FFI+by value instead of as phantom pointers.+-}+queryTests :: [TestTree]+queryTests =+  [ testCase "the default query filter round-trips and accepts everything" $ do+      qf <- defaultQueryFilter+      roundTrip qf >>= (@?= qf)+      assertBool "mask accepts everything" (qf.maskBits /= 0)+  , testCase "castRayClosest reports the nearest shape, and misses cleanly" $+      withWorld (Vec3 0 0 0) $ \w -> do+        near <- staticSphere w (Pos 2 0 0) 0.5+        _far <- staticSphere w (Pos 4 0 0) 0.5+        qf <- defaultQueryFilter+        r <- World.castRayClosest w (Pos 0 0 0) (Vec3 10 0 0) qf+        r.hit @?= 1+        r.shapeId @?= near+        -- the near sphere's surface is at x = 1.5, 15% along the ray+        approx 0.15 (r.fraction)+        miss <- World.castRayClosest w (Pos 0 0 0) (Vec3 (-10) 0 0) qf+        miss.hit @?= 0+  , testCase "body mass data matches the sphere's volume" $+      withWorld (Vec3 0 0 0) $ \w -> do+        b <- dynamicSphere w (Pos 0 0 0) 0.5+        md <- Body.getMassData b+        -- default density 1000 * (4/3) pi r^3; compare with a relative+        -- tolerance because the absolute value is large for Float+        let+          expected = 1000 * 4 / 3 * pi * 0.125+          m = md.mass+        assertBool+          ("mass ~ sphere volume * density, m = " <> show m)+          (abs (m - expected) / expected < 1e-5)+  , testCase "a local-frame ray cast hits a sphere" $ do+      out <- Collision.rayCastSphere (Sphere vec3Zero 1) (RayCastInput (Vec3 (-3) 0 0) (Vec3 6 0 0) 1)+      out.hit @?= 1+      -- enters the unit sphere at x = -1: a third of the way along (6, 0, 0)+      approx (1 / 3) (out.fraction)+  ]++-- | A tiny busy-wait that GHC will not optimise away, to widen a race window.+spin :: Int -> IO ()+spin = go+  where+    go 0 = pure ()+    go k = evaluate k >> go (k - 1)++awaitOverlap :: Double -> IORef Int -> IO ()+awaitOverlap deadline maxSeen = do+  peak <- readIORef maxSeen+  now <- getMonotonicTime+  unless (peak >= 2 || now >= deadline) (yield >> awaitOverlap deadline maxSeen)++{- | Stress the closure→'FunPtr' callback path under the multithreaded task+system: a custom-filter closure fires from the parallel solver, invoked+concurrently from several worker threads.+-}+callbackTests :: [TestTree]+callbackTests =+  [ testCase "overlap and cast-ray closures visit shapes (cast via the C trampoline)" $+      withWorld (Vec3 0 0 0) $ \w -> do+        _ <- staticSphere w (Pos 2 0 0) 0.5+        _ <- staticSphere w (Pos 4 0 0) 0.5+        _ <- staticSphere w (Pos 40 0 0) 0.5+        qf <- defaultQueryFilter+        -- overlap: a plain scalar-args closure, no trampoline needed+        seen <- newIORef (0 :: Int)+        _ <- CB.withOverlapResultFcn (\_ -> modifyIORef' seen (+ 1) >> pure True) $ \fcn ctx ->+          World.overlapAABB w (AABB (Vec3 0 (-1) (-1)) (Vec3 5 1 1)) qf fcn ctx+        readIORef seen >>= (@?= 2)+        -- cast ray: the by-value point/normal cross through the trampoline+        hitsRef <- newIORef []+        _ <-+          CB.withCastResultFcn+            ( \sid point normal frac _matId _triIx _childIx ->+                modifyIORef' hitsRef ((sid, point, normal, frac) :) >> pure 1+            )+            (\fcn ctx -> World.castRay w (Pos 0 0 0) (Vec3 10 0 0) qf fcn ctx)+        hits <- readIORef hitsRef+        length hits @?= 2+        let fracs = map (\(_, _, _, f) -> f) hits+        -- surfaces at x = 1.5 and x = 3.5 along a length-10 ray+        approx 0.15 (minimum fracs)+        approx 0.35 (maximum fracs)+        forM_ hits $ \(_, Pos px _ _, Vec3 nx _ _, _) -> do+          assertBool ("hit point on a surface, x = " <> show px) (px > 1 && px < 4)+          approx (-1) nx+  , testCase "a pre-solve gate disables contacts (via the C trampoline)" $+      withWorld (Vec3 0 (-10) 0) $ \w -> do+        gd <- defaultBodyDef+        ground <- Body.create w gd+        gsd <- defaultShapeDef+        _ <-+          Shape.createSphere+            ground+            gsd{enablePreSolveEvents = 1}+            (Sphere vec3Zero 5)+        bd <- defaultBodyDef+        ball <- Body.create w bd{type_ = DynamicBody, position = Pos 0 8 0}+        sd <- defaultShapeDef+        _ <-+          Shape.createSphere+            ball+            sd{enablePreSolveEvents = 1}+            (Sphere vec3Zero 0.5)+        fired <- newIORef (0 :: Int)+        CB.withPreSolveFcn+          (\_ _ _ _ -> modifyIORef' fired (+ 1) >> pure False)+          $ \fcn ctx -> do+            World.setPreSolveCallback w fcn ctx+            replicateM_ 240 (World.step w (1 / 60) 4)+            World.setPreSolveCallback w nullFunPtr nullPtr+        readIORef fired >>= \n ->+          assertBool ("the gate fired, n = " <> show n) (n > 0)+        -- every contact was vetoed, so the ball fell through the ground+        y <- posY <$> Body.getPosition ball+        assertBool ("ball fell through, y = " <> show y) (y < -5)+  , testCase "friction and restitution mixing closures fire on contact" $+      withWorld (Vec3 0 (-10) 0) $ \w -> do+        fCount <- newIORef (0 :: Int)+        rCount <- newIORef (0 :: Int)+        CB.withFrictionCallback (\fa _ fb _ -> modifyIORef' fCount (+ 1) >> pure (max fa fb)) $ \ffcn ->+          CB.withRestitutionCallback (\ra _ rb _ -> modifyIORef' rCount (+ 1) >> pure (max ra rb)) $ \rfcn -> do+            World.setFrictionCallback w ffcn+            World.setRestitutionCallback w rfcn+            gd <- defaultBodyDef+            ground <- Body.create w gd+            gsd <- defaultShapeDef+            _ <- Shape.createSphere ground gsd (Sphere vec3Zero 5)+            _ <- dynamicSphere w (Pos 0 6 0) 0.5+            replicateM_ 60 (World.step w (1 / 60) 4)+            -- NULL resets the world to the default mixers before the wrappers die+            World.setFrictionCallback w nullFunPtr+            World.setRestitutionCallback w nullFunPtr+        readIORef fCount >>= \n ->+          assertBool ("friction mixer fired, n = " <> show n) (n > 0)+        readIORef rCount >>= \n ->+          assertBool ("restitution mixer fired, n = " <> show n) (n > 0)+  , testCase "a mover capsule gathers collision planes through the closure" $+      withWorld (Vec3 0 0 0) $ \w -> do+        _ <- staticSphere w (Pos 0 0 0) 1+        qf <- defaultQueryFilter+        planes <- newIORef []+        CB.withPlaneResultFcn+          (\sid ps -> modifyIORef' planes ((sid, ps) :) >> pure True)+          (\fcn ctx -> World.collideMover w (Pos 0 0 0) (Capsule (Vec3 1.2 0 0) (Vec3 1.2 0.5 0) 0.4) qf fcn ctx)+        batches <- readIORef planes+        assertBool "at least one plane batch gathered" (not (null batches))+        assertBool "the batches carry planes" (any (not . VS.null . snd) batches)+  , testCase "dynamic-tree query, closest-query and ray-cast closures visit proxies" $+      allocaBytes dynamicTreeByteCount $ \tree -> do+        DynamicTree.create 16 tree+        p0 <- DynamicTree.createProxy tree (AABB (Vec3 0 0 0) (Vec3 1 1 1)) 1 42+        _ <- DynamicTree.createProxy tree (AABB (Vec3 3 0 0) (Vec3 4 1 1)) 1 43+        -- query: only the first box intersects the probe AABB+        seen <- newIORef []+        _ <- CB.withTreeQueryFcn (\pid ud -> modifyIORef' seen ((pid, ud) :) >> pure True) $ \fcn ctx ->+          DynamicTree.query tree (AABB (Vec3 (-0.5) 0.25 0.25) (Vec3 0.5 0.75 0.75)) maxBound False fcn ctx+        readIORef seen >>= (@?= [(fromIntegral p0, 42)])+        -- closest query from between the boxes visits at least one proxy+        closestSeen <- newIORef (0 :: Int)+        _ <- alloca $ \pOut -> do+          poke pOut 1e9+          CB.withTreeQueryClosestFcn+            (\dmin _pid _ud -> modifyIORef' closestSeen (+ 1) >> pure dmin)+            (\fcn ctx -> DynamicTree.queryClosest tree (Vec3 2 0.5 0.5) maxBound False fcn ctx pOut)+        readIORef closestSeen >>= \n ->+          assertBool ("closest query visited proxies, n = " <> show n) (n >= 1)+        -- a ray along y = z = 0.5 pierces both boxes+        hits <- newIORef (0 :: Int)+        _ <-+          CB.withTreeRayCastFcn+            ( \input _pid _ud -> do+                input.maxFraction @?= 1+                modifyIORef' hits (+ 1)+                pure 1+            )+            ( \fcn ctx ->+                DynamicTree.rayCast tree (RayCastInput (Vec3 (-1) 0.5 0.5) (Vec3 6 0 0) 1) maxBound False fcn ctx+            )+        readIORef hits >>= (@?= 2)+        DynamicTree.destroy tree+  , testCase "custom-filter closures survive concurrent hot-path invocation" $+      CB.withThreadPoolTaskSystem 4 $ \ts -> do+        total <- newIORef (0 :: Int)+        inFlight <- newIORef (0 :: Int)+        maxSeen <- newIORef (0 :: Int)+        canOverlap <- (>= 2) <$> getNumCapabilities+        deadline <- (+ 5) <$> getMonotonicTime+        let onFilter _ _ _ = do+              n <- atomicModifyIORef' inFlight (\x -> (x + 1, x + 1))+              atomicModifyIORef' maxSeen (\m -> (max m n, ()))+              when canOverlap (awaitOverlap deadline maxSeen)+              spin 4000+              atomicModifyIORef' total (\x -> (x + 1, ()))+              atomicModifyIORef' inFlight (\x -> (x - 1, ()))+              pure True+        CB.withCustomFilterFcn onFilter $ \fcn -> do+          wd <- defaultWorldDef+          w <-+            World.create+              wd+                { gravity = Vec3 0 (-10) 0+                , enableSleep = 0+                , workerCount = fromIntegral ts.workerCount+                , enqueueTask = ts.enqueue+                , finishTask = ts.finish+                }+          World.setCustomFilterCallback w fcn nullPtr+          bd <- defaultBodyDef+          sd <- defaultShapeDef+          forM_ [0 .. 255 :: Int] $ \i -> do+            let+              col = fromIntegral (i `mod` 8)+              row = fromIntegral ((i `div` 8) `mod` 8)+              lay = fromIntegral (i `div` 64)+              pos = Pos (col - 4) (1.5 + lay * 1.1) (row - 4)+            b <- Body.create w bd{type_ = DynamicBody, position = pos}+            _ <- Shape.createSphere b sd{enableCustomFiltering = 1} (Sphere vec3Zero 0.5)+            pure ()+          replicateM_ 300 (World.step w (1 / 60) 4)+          World.setCustomFilterCallback w nullFunPtr nullPtr+          World.isValid w >>= (@?= True)+          World.destroy w+        calls <- readIORef total+        peak <- readIORef maxSeen+        assertBool ("callback fired under load, calls = " <> show calls) (calls > 100)+        when canOverlap $+          assertBool+            ("callback was invoked concurrently, peak overlap = " <> show peak)+            (peak >= 2)+  , testCase "the debug-draw table dispatches to Haskell closures" $+      withWorld (Vec3 0 (-10) 0) $ \w -> do+        _ <- dynamicSphere w (Pos 1 2 3) 0.5+        _ <- dynamicSphere w (Pos 5 2 3) 0.5+        World.step w (1 / 60) 4+        boundsSeen <- newIORef []+        xforms <- newIORef (0 :: Int)+        base <- defaultDebugDraw+        let draw =+              CB.noDraw+                { CB.bounds = Just $ \aabb c -> modifyIORef' boundsSeen ((aabb, c) :)+                , CB.transform = Just $ \_ -> modifyIORef' xforms (+ 1)+                }+        CB.withDraw draw base{drawBounds = 1, drawMass = 1} (CB.draw w maxBound)+        bs <- readIORef boundsSeen+        length bs @?= 2+        -- one fat AABB straddles each sphere (the engine picks the draw order)+        let centreX (AABB (Vec3 lx _ _) (Vec3 hx _ _)) = (lx + hx) / 2+        forM_ [1, 5] $ \x ->+          assertBool+            ("a box is centred on x = " <> show x <> ", got " <> show (map (centreX . fst) bs))+            (any ((< 0.5) . abs . subtract x . centreX . fst) bs)+        readIORef xforms >>= \n ->+          assertBool ("body transforms were drawn, n = " <> show n) (n == 2)+  , testCase "an exception in a draw callback surfaces at draw, not at the callback" $+      withWorld (Vec3 0 (-10) 0) $ \w -> do+        _ <- dynamicSphere w (Pos 1 2 3) 0.5+        World.step w (1 / 60) 4+        later <- newIORef (0 :: Int)+        -- a per-frame message, so a stale trap is not mistaken for a fresh one+        frame <- newIORef (1 :: Int)+        base <- defaultDebugDraw+        let draw =+              CB.noDraw+                { CB.bounds = Just $ \_ _ -> readIORef frame >>= \i -> error ("boom" <> show i)+                , -- the engine keeps walking, but every later slot is skipped+                  CB.transform = Just $ \_ -> modifyIORef' later (+ 1)+                }+        CB.withDraw draw base{drawBounds = 1, drawMass = 1} $ \target -> do+          CB.draw w maxBound target `catchErrorCall` \msg -> msg @?= "boom1"+          readIORef later >>= (@?= 0)+          -- the trap is cleared, so the next frame starts clean+          writeIORef frame 2+          CB.draw w maxBound target `catchErrorCall` \msg -> msg @?= "boom2"+        World.isValid w >>= (@?= True)+  , testCase "an unset slot never enters Haskell, so its exception cannot fire" $+      withWorld (Vec3 0 (-10) 0) $ \w -> do+        _ <- dynamicSphere w (Pos 1 2 3) 0.5+        World.step w (1 / 60) 4+        seen <- newIORef (0 :: Int)+        base <- defaultDebugDraw+        -- transform is left unset: the trampoline sees NULL and returns+        let draw = CB.noDraw{CB.bounds = Just $ \_ _ -> modifyIORef' seen (+ 1)}+        CB.withDraw draw base{drawBounds = 1, drawMass = 1} (CB.draw w maxBound)+        readIORef seen >>= (@?= 1)+  , testCase "withRecorder collects a frame for rendering after draw returns" $+      withWorld (Vec3 0 (-10) 0) $ \w -> do+        b <- dynamicSphere w (Pos 1 2 3) 0.5+        World.step w (1 / 60) 4+        base <- defaultDebugDraw+        ((), cmds) <- CB.withRecorder base{drawBounds = 1, drawMass = 1} (CB.draw w maxBound)+        assertBool "bounds were recorded" (any isBounds cmds)+        assertBool "a body transform was recorded" (any isTransform cmds)+        -- the world is safe to mutate now that draw has returned+        Body.destroy b+        World.isValid w >>= (@?= True)+  , testCase "toWorld moves local geometry into the transform's frame" $ do+      -- a quarter turn about +Z sends +X to +Y+      q <- computeQuatBetweenUnitVectors (Vec3 1 0 0) (Vec3 0 1 0)+      let Vec3 x y z = toVec3 (CB.toWorld (WorldTransform (Pos 10 20 30) q) (Vec3 1 0 0))+      approx 10 x+      approx 21 y+      approx 30 z+  ]++isBounds :: CB.Cmd -> Bool+isBounds (CB.DrawBounds _ _) = True+isBounds _ = False++debugShapeTests :: [TestTree]+debugShapeTests =+  [ testCase "withDebugShapes rounds a handle through draw and shape teardown" $ do+      made <- newIORef []+      freed <- newIORef (0 :: Int)+      drawn <- newIORef []+      let+        mk ds = do+          radius <- case ds.geometry of+            DebugShape.Sphere p -> Just <$> peek p+            _ -> pure Nothing+          n <- atomicModifyIORef' made (\ks -> ((ds, radius) : ks, length ks + 1))+          pure (intPtrToPtr (fromIntegral n))+        free_ p = modifyIORef' freed (+ 1) >> evaluate (p `seq` ())+      CB.withDebugShapes mk (Just free_) $ \cbs -> do+        wd <- defaultWorldDef+        w <-+          World.create+            wd+              { gravity = Vec3 0 (-10) 0+              , createDebugShape = cbs.create+              , destroyDebugShape = cbs.destroy+              }+        s <- staticSphere w (Pos 0 0 0) 1+        World.step w (1 / 60) 4+        base <- defaultDebugDraw+        let draw = CB.noDraw{CB.shape = Just $ \u _ _ -> modifyIORef' drawn (u :)}+        CB.withDraw draw base{drawShapes = 1} (CB.draw w maxBound)+        readIORef made >>= \ds -> case ds of+          [(d, peeked)] -> do+            d.shapeId @?= s+            case peeked of+              Just (Sphere _ r) -> r @?= 1+              Nothing -> assertFailure ("expected a sphere arm, got " <> show d.geometry)+          _ -> assertFailure ("expected one debug shape, got " <> show (length ds))+        readIORef drawn >>= \us -> map ptrToIntPtr us @?= [1]+        -- destroying the shape runs the destructor back through a safe call+        Shape.destroy s True+        readIORef freed >>= (@?= 1)+        World.destroy w+  , testCase "a throwing constructor costs only its own shape and surfaces on demand" $ do+      calls <- newIORef (0 :: Int)+      let mk _ = do+            n <- atomicModifyIORef' calls (\c -> (c + 1, c))+            if n == 0 then throwIO (ErrorCall "boom") else pure (intPtrToPtr (fromIntegral n))+      CB.withDebugShapes mk Nothing $ \cbs -> withDebugSpheres cbs $ \w -> do+        drawDebugSpheres w+        readIORef calls >>= (@?= 2)+        CB.rethrowDebugShapes cbs `catchErrorCall` (@?= "boom")+        drawDebugSpheres w+        readIORef calls >>= (@?= 3)+        drawDebugSpheres w+        readIORef calls >>= (@?= 3)+        World.destroy w+  , testCase "a throwing destructor still runs for every handle" $ do+      freed <- newIORef (0 :: Int)+      let free_ _ = modifyIORef' freed (+ 1) >> throwIO (ErrorCall "boom")+      CB.withDebugShapes (\_ -> pure (intPtrToPtr 1)) (Just free_) $ \cbs -> withDebugSpheres cbs $ \w -> do+        drawDebugSpheres w+        World.destroy w+        CB.rethrowDebugShapes cbs `catchErrorCall` (@?= "boom")+      readIORef freed >>= (@?= 2)+  , testCase "debug-shape failures nobody polled surface when the scope ends" $ do+      let+        failing _ = throwIO (ErrorCall "create")+        destroyFailing _ = throwIO (ErrorCall "destroy")+        scope mk free_ = CB.withDebugShapes mk free_ $ \cbs -> withDebugSpheres cbs $ \w -> do+          drawDebugSpheres w+          World.destroy w+      scope failing Nothing `catchErrorCall` (@?= "create")+      scope (\_ -> pure (intPtrToPtr 1)) (Just destroyFailing) `catchErrorCall` (@?= "destroy")+  , testCase "a world with only a constructor survives teardown (no-op destructor)" $+      CB.withDebugShapes (\_ -> pure (intPtrToPtr 1)) Nothing $ \cbs -> do+        wd <- defaultWorldDef+        w <-+          World.create+            wd+              { gravity = Vec3 0 (-10) 0+              , createDebugShape = cbs.create+              , destroyDebugShape = cbs.destroy+              }+        _ <- staticSphere w (Pos 0 0 0) 1+        World.step w (1 / 60) 4+        base <- defaultDebugDraw+        CB.withDraw CB.noDraw{CB.shape = Just $ \_ _ _ -> pure ()} base{drawShapes = 1} (CB.draw w maxBound)+        -- shape.c calls destroyDebugShape unguarded, so this is the fault case+        World.destroy w+        World.isValid w >>= (@?= False)+  ]++withDebugSpheres :: CB.DebugShapes -> (WorldId -> IO ()) -> IO ()+withDebugSpheres cbs act = do+  wd <- defaultWorldDef+  w <-+    World.create+      wd+        { gravity = Vec3 0 (-10) 0+        , createDebugShape = cbs.create+        , destroyDebugShape = cbs.destroy+        }+  _ <- staticSphere w (Pos 0 0 0) 1+  _ <- staticSphere w (Pos 3 0 0) 1+  World.step w (1 / 60) 4+  act w `finally` (World.isValid w >>= \alive -> when alive (World.destroy w))++drawDebugSpheres :: WorldId -> IO ()+drawDebugSpheres w = do+  base <- defaultDebugDraw+  CB.withDraw CB.noDraw{CB.shape = Just $ \_ _ _ -> pure ()} base{drawShapes = 1} (CB.draw w maxBound)++isTransform :: CB.Cmd -> Bool+isTransform (CB.DrawTransform _) = True+isTransform _ = False++catchErrorCall :: IO () -> (String -> IO ()) -> IO ()+catchErrorCall act onError = do+  thrown <- try act+  case thrown of+    Left (ErrorCall msg) -> onError msg+    Right () -> assertFailure "expected an ErrorCall"++moverTests :: [TestTree]+moverTests =+  [ testCase "testPoint reports containment against a static sphere" $+      withWorld (Vec3 0 0 0) $ \w -> do+        s <- staticSphere w (Pos 0 0 0) 1+        Shape.testPoint s (Pos 0 0.5 0) >>= (@?= True)+        Shape.testPoint s (Pos 0.9 0 0) >>= (@?= True)+        Shape.testPoint s (Pos 0 3 0) >>= (@?= False)+  , testCase "double mode keeps fractions of a metre far from the origin" $ do+      dp <- Base.isDoublePrecision+      when dp $ withWorld (Vec3 0 (-10) 0) $ \w -> do+        b <- dynamicSphere w (Pos 10000000.25 10 0) 0.5+        World.step w (1 / 60) 4+        Pos x _ _ <- Body.getPosition b+        x @?= 10000000.25+        WorldTransform (Pos tx _ _) _ <- Body.getTransform b+        tx @?= 10000000.25+  , testCase "double mode tests points accurately far from the origin" $ do+      dp <- Base.isDoublePrecision+      when dp $ withWorld (Vec3 0 0 0) $ \w -> do+        s <- staticSphere w (Pos 10000000.25 0 0) 1+        Shape.testPoint s (Pos 10000000.75 0 0) >>= (@?= True)+        Shape.testPoint s (Pos 10000001.45 0 0) >>= (@?= False)+  , testCase "withShapeProxy drives a low-level overlap test" $ do+      let far = VS.fromList [Vec3 5 5 5]+      hit <-+        withShapeProxy (VS.singleton (Vec3 0 0.5 0)) 0.1 $+          Collision.overlapSphere (Sphere vec3Zero 1) transformIdentity+      hit @?= True+      miss <-+        withShapeProxy far 0.1 $+          Collision.overlapSphere (Sphere vec3Zero 1) transformIdentity+      miss @?= False+  , testCase "a begun contact exposes its manifolds through Contact.getData" $+      withWorld (Vec3 0 (-10) 0) $ \w -> do+        gd <- defaultBodyDef+        ground <- Body.create w gd+        gsd <- defaultShapeDef+        _ <- Shape.createSphere ground gsd{enableContactEvents = 1} (Sphere vec3Zero 5)+        bd <- defaultBodyDef+        ball <- Body.create w bd{type_ = DynamicBody, position = Pos 0 5.6 0}+        sd <- defaultShapeDef+        bshape <- Shape.createSphere ball sd{enableContactEvents = 1} (Sphere vec3Zero 0.5)+        let loop 0 = assertFailure "no contact begin event within 120 steps"+            loop n = do+              World.step w (1 / 60) 4+              evs <- Events.contactBeginTouchEvents w+              if VS.null evs then loop (n - 1 :: Int) else pure (VS.head evs)+        ev <- loop 120+        let cid = ev.contactId+        Contact.isValid cid >>= (@?= True)+        cd <- Contact.getData cid+        assertBool+          "the contact involves the ball shape"+          (cd.shapeIdA == bshape || cd.shapeIdB == bshape)+        assertBool "the contact carries a manifold" (cd.manifoldCount >= 1)+        m <- peekElemOff (cd.manifolds) 0+        assertBool "the manifold has a contact point" (not (VS.null (m.points)))+        let Vec3 _ ny _ = m.normal+        assertBool ("the contact normal is vertical, " <> show (m.normal)) (abs ny > 0.9)+  , testCase "solvePlanes pushes a mover out and clipVector kills the inward velocity" $ do+      let plane = CollisionPlane (Plane (Vec3 0 1 0) 0) (1 / 0) 0 1+      (delta, planes', iters) <- Mover.solvePlanes (Vec3 0 (-1) 0) (VS.singleton plane)+      let Vec3 _ dy _ = delta+      assertBool ("the mover ends up at the plane, delta = " <> show delta) (abs dy < 0.01)+      assertBool ("the solver reports iterations, " <> show iters) (iters >= 1)+      assertBool "the plane accumulated push" ((VS.head planes').push > 0)+      clipped <- Mover.clipVector (Vec3 3 (-5) 2) planes'+      clipped @?= Vec3 3 0 2+  , testCase "the default length scale is one unit per meter" $+      getLengthUnitsPerMeter >>= (@?= 1)+  , testCase "the stall threshold round-trips" $ do+      initial <- getStallThreshold+      setStallThreshold 0.25+      getStallThreshold >>= (@?= 0.25)+      setStallThreshold initial+      getStallThreshold >>= (@?= initial)+  , testCase "the pure vector helpers follow the engine conventions" $ do+      vec3Add (Vec3 1 2 3) (Vec3 4 5 6) @?= Vec3 5 7 9+      vec3MulSub (Vec3 1 1 1) 2 (Vec3 0 1 0) @?= Vec3 1 (-1) 1+      vec3Cross vec3AxisX vec3AxisY @?= vec3AxisZ+      vec3Normalize (Vec3 0 3 4) @?= Vec3 0 0.6 0.8+      vec3Normalize vec3Zero @?= vec3Zero+      quatMul quatIdentity quatIdentity @?= quatIdentity+      quatNormalize (Quat vec3Zero 2) @?= quatIdentity+      rotateVector quatIdentity (Vec3 5 6 7) @?= Vec3 5 6 7+      -- a quarter turn about z maps x to y (loosely: float cross products)+      let+        qz = quatNormalize (Quat (Vec3 0 0 (sin (pi / 4))) (cos (pi / 4)))+        Vec3 rx ry rz = rotateVector qz vec3AxisX+      approx 0 rx+      approx 1 ry+      approx 0 rz+      let Vec3 ix iy iz = invRotateVector qz (rotateVector qz (Vec3 1 2 3))+      approx 1 ix+      approx 2 iy+      approx 3 iz+      planeSeparation (Plane (Vec3 0 1 0) 2) (Vec3 0 5 0) @?= 3+  ]++geometryTests :: [TestTree]+geometryTests =+  [ testCase "createMeshFromData builds a mesh a shape can use" $+      withWorld (Vec3 0 0 0) $ \w -> do+        let+          vertices = VS.fromList [Vec3 0 0 0, Vec3 1 0 0, Vec3 0 0 1, Vec3 1 0 1]+          indices = VS.fromList [0, 1, 2, 2, 1, 3]+        (mesh, degenerate) <- Authoring.createMeshFromData vertices indices Nothing Authoring.defaultMeshOptions+        assertBool "mesh was created" (mesh /= nullPtr)+        degenerate @?= VS.empty+        bd <- defaultBodyDef+        b <- Body.create w bd+        sd <- defaultShapeDef+        sh <- Shape.createMesh b sd mesh (Vec3 1 1 1)+        Shape.isValid sh >>= (@?= True)+        Body.destroy b+        Mesh.destroy mesh+  , testCase "degenerate triangles are reported by index" $ do+      -- triangle 1 has distinct indices but collinear (zero-area) vertices;+      -- repeated-index triangles are dropped without being reported+      let+        vertices = VS.fromList [Vec3 0 0 0, Vec3 1 0 0, Vec3 0 0 1, Vec3 0.5 0 0]+        indices = VS.fromList [0, 1, 2, 0, 3, 1]+      (mesh, degenerate) <- Authoring.createMeshFromData vertices indices Nothing Authoring.defaultMeshOptions+      assertBool "mesh was created" (mesh /= nullPtr)+      degenerate @?= VS.fromList [1]+      Mesh.destroy mesh+  , testCase "createHeightFieldFromData builds a height field" $ do+      let heights = VS.fromList [0, 0.5, 0.25, 1]+      hf <-+        Authoring.createHeightFieldFromData+          heights+          Nothing+          (Vec3 1 1 1)+          2+          2+          Authoring.defaultHeightFieldOptions+      assertBool "height field was created" (hf /= nullPtr)+      fieldData <- peek hf+      assertBool "byte count is positive" (fieldData.byteCount > 0)+      VS.length fieldData.padding @?= 7+      HeightField.destroy hf+  , testCase "createCompoundFromData builds a compound from sphere and capsule children" $ do+      material <- defaultSurfaceMaterial+      let+        spheres = VS.fromList [Authoring.CompoundSphereDef (Sphere (Vec3 0 1 0) 0.5) material]+        capsules = VS.fromList [Authoring.CompoundCapsuleDef (Capsule (Vec3 0 0 0) (Vec3 0 1 0) 0.25) material]+      compound <- Authoring.createCompoundFromData capsules VS.empty VS.empty spheres+      assertBool "compound was created" (compound /= nullPtr)+      Compound.destroy compound+  ]
+ upstream/LICENSE view
@@ -0,0 +1,21 @@+MIT License++Copyright (c) 2026 Erin Catto++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.
+ upstream/include/box3d/base.h view
@@ -0,0 +1,203 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include <stdbool.h>+#include <stdint.h>++// Compile-time options. Edit box3d/config.h, or define BOX3D_USER_CONFIG to+// point at your own copy.+#ifdef BOX3D_USER_CONFIG+#include BOX3D_USER_CONFIG+#endif+#include "config.h"++// clang-format off+// +// Shared library macros+// Predefine BOX3D_EXPORT to reuse an existing export/import scheme, for example+// when compiling Box3D into another shared library.+#ifndef BOX3D_EXPORT+#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+#endif++// C++ macros+#ifdef __cplusplus+	#define B3_API extern "C" BOX3D_EXPORT+	#define B3_INLINE inline+	#define B3_ALIGN_AS(N) alignas(N)+#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+	#define B3_ALIGN_AS(N) _Alignas(N)++#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++// This is used to validate arguments for functions similar to printf.+#if defined( __GNUC__ ) || defined( __clang__ )+#define B3_PRINTF_FORMAT( INDEX1, INDEX2 ) __attribute__( ( format( printf, INDEX1, INDEX2 ) ) )+#else+#define B3_PRINTF_FORMAT( INDEX1, INDEX2 )+#endif++#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 int 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 )+/// Break to the debugger+#define B3_BREAKPOINT __debugbreak()+#elif defined( __GNUC__ ) || defined( __clang__ )+#define B3_BREAKPOINT __builtin_trap()+#else+/// Unknown compiler+#include <assert.h>+#define B3_BREAKPOINT assert( 0 )+#endif++#if !defined( NDEBUG ) || defined( B3_ENABLE_ASSERT )+/// Internal assertion handler. Allows for host intervention.+B3_API int b3InternalAssert( const char* condition, const char* fileName, int lineNumber );+/// Assert that a condition is true.+#define B3_ASSERT( condition )                                                                                                   \+	( (void)( ( !!( condition ) ) || ( b3InternalAssert( #condition, __FILE__, (int)( __LINE__ ) ), 0 ) ) )+#else+#define B3_ASSERT( ... ) ( (void)0 )+#endif++#if B3_ENABLE_VALIDATION+/// Validation is typically only enabled in debug builds.+/// Floating point tolerance checks should use this instead of the regular assertion+#define B3_VALIDATE( condition ) B3_ASSERT( condition )+#else+/// Validation is typically only enabled in debug builds.+/// Floating point tolerance checks should use this instead of the regular assertion+#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 Box3D+B3_API b3Version b3GetVersion( void );++/// @return true if the library was built with BOX3D_DOUBLE_PRECISION (large world mode)+B3_API bool b3IsDoublePrecision( void );++/**@}*/++/// Get the absolute number of system ticks. The value is platform specific.+/// @ingroup base+B3_API uint64_t b3GetTicks( void );++/// Get the milliseconds passed from an initial tick value.+/// @ingroup base+B3_API float b3GetMilliseconds( uint64_t ticks );++/// Get the milliseconds passed from an initial tick value. Resets the passed in+/// value to the current tick value.+/// @ingroup base+B3_API float b3GetMillisecondsAndReset( uint64_t* ticks );++/// Yield to be used in a busy loop.+/// @ingroup base+B3_API void b3Yield( void );++/// Sleep the current thread for a number of milliseconds.+/// @ingroup base+B3_API void b3Sleep( int milliseconds );++/// Initial value for b3Hash+/// @ingroup base+#define B3_HASH_INIT 5381++/// Simple djb2 hash function for determinism testing+/// @ingroup base+B3_API uint32_t b3Hash( uint32_t hash, const uint8_t* data, int count );
+ upstream/include/box3d/box3d.h view
@@ -0,0 +1,1751 @@+// 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 <stdbool.h>++/**+ * @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 the 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/+ * @{+ */++#if defined( BOX3D_DOUBLE_PRECISION )+// Force a link error if the application and library disagree on precision. A float app linking+// a double precision library, or the reverse, gets one unresolved external on the first call+// every program makes. CMake consumers inherit the define and cannot mismatch.+#define b3CreateWorld b3CreateWorldDoublePrecision+#endif++/// Create a world for rigid body simulation. A world contains bodies, shapes, and constraints. You may 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 world's bounds. This is the bounding box that covers the current simulation. May have a small+/// amount of padding.+B3_API b3AABB b3World_GetBounds( b3WorldId worldId );++/// 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. The proxy points are relative+/// to the world origin, which lets the query stay precise far from the world origin.+B3_API b3TreeStats b3World_OverlapShape( b3WorldId worldId, b3Pos origin, 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, b3Pos 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, b3Pos 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.+/// The proxy points are relative to the origin and the hit points come back as world positions, so the+/// cast stays precise far from the world origin.+///	@see b3World_CastRay+B3_API b3TreeStats b3World_CastShape( b3WorldId worldId, b3Pos origin, 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 origin World position the mover capsule is relative to+/// @param mover Capsule mover, relative to the origin+/// @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, b3Pos origin, 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. The mover and the returned planes are relative to the origin.+B3_API void b3World_CollideMover( b3WorldId worldId, b3Pos origin, 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 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 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. Box3D 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+/// warm starting 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 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 log.+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 );++/**+ * @defgroup recording Recording+ * @brief Record and replay world state for debugging.+ * @{+ */++/// Opaque recording handle. Create with b3CreateRecording, destroy with b3DestroyRecording.+typedef struct b3Recording b3Recording;++/// Create a recording buffer with an optional initial byte capacity.+/// Pass 0 to use the default (64 KiB). The buffer grows on demand.+/// @return a new recording, owned by the caller+B3_API b3Recording* b3CreateRecording( int byteCapacity );++/// Destroy a recording and free its buffer.+/// @param recording may be NULL+B3_API void b3DestroyRecording( b3Recording* recording );++/// Get a pointer to the raw recording bytes.+/// Valid until the recording buffer is modified or destroyed.+/// @param recording the recording handle+/// @return pointer to the byte buffer, or NULL if no bytes have been written+B3_API const uint8_t* b3Recording_GetData( const b3Recording* recording );++/// Get the number of bytes currently in the recording buffer.+/// @param recording the recording handle+B3_API int b3Recording_GetSize( const b3Recording* recording );++/// Begin recording world mutations into the provided buffer.+/// The buffer is reset on each call so a single b3Recording can be reused for multiple sessions.+/// @param worldId the world to record+/// @param recording the recording handle to write into+B3_API void b3World_StartRecording( b3WorldId worldId, b3Recording* recording );++/// End the current recording session. Writes the trailing geometry registry and+/// backpatches the header. The buffer remains valid until the recording is destroyed.+/// @param worldId the world currently being recorded+B3_API void b3World_StopRecording( b3WorldId worldId );++/// Save the recording buffer to a file. Returns true on success.+/// @param recording the recording to save+/// @param path file path to write+B3_API bool b3SaveRecordingToFile( const b3Recording* recording, const char* path );++/// Load a recording from a file. Returns NULL on failure (file not found, wrong magic).+/// The caller owns the returned recording and must destroy it with b3DestroyRecording.+/// @param path file path to read+B3_API b3Recording* b3LoadRecordingFromFile( const char* path );++/// Replay a recording from memory and verify it reproduces the same world-state hashes.+/// Stands up a fresh world, restores the seed snapshot, replays every op, and checks each embedded+/// StateHash record. Returns true if replay completed without id mismatches or hash divergences.+/// @param data pointer to recording bytes+/// @param size byte count of the recording+/// @param workerCount reserved for future multithreaded replay; pass 1 for now+B3_API bool b3ValidateReplay( const void* data, int size, int workerCount );++/// Opaque incremental replay player with a keyframe ring for O(interval) backward seek.+typedef struct b3RecPlayer b3RecPlayer;++/// Summary of a recording, read once at open so a viewer can frame and label it.+typedef struct b3RecPlayerInfo+{+	int frameCount;	   // total recorded steps+	int workerCount;   // worker count requested for the replay world+	float timeStep;	   // dt of the recorded steps+	int subStepCount;  // recorded sub-steps+	float lengthScale; // length units per meter in effect when recorded+	b3AABB bounds;	   // accumulated world bounds over the recording, zero-extent if unavailable+} b3RecPlayerInfo;++/// Create a player over a recording. Owns a private copy of the bytes.+/// @param data pointer to recording bytes+/// @param size byte count of the recording+/// @param workerCount worker count for the replay world; pass 1 to match a serial recording.+/// Replaying at a different count re-partitions the constraint graph, so the StateHash check+/// becomes a cross-thread determinism test. Adjustable later with b3RecPlayer_SetWorkerCount.+/// @return a new player, or NULL on bad header or deserialization failure+B3_API b3RecPlayer* b3CreatePlayer( const void* data, int size, int workerCount );++/// Destroy the player and free all memory. Restores the previous global length scale.+B3_API void b3DestroyPlayer( b3RecPlayer* player );++/// Advance one frame. dispatch ops until the next Step completes.+/// @return true when a frame was stepped, false at end-of-recording+B3_API bool b3RecPlayer_StepFrame( b3RecPlayer* player );++/// Sub-step one frame. This will sub-step and return immediately after body creation.+/// The next call will execute the time step. This allows bodies to be rendered+/// at the creation pose.+B3_API void b3RecPlayer_SubStepFrame( b3RecPlayer* player );++/// Rewind to frame 0 (in-place restore so the world id stays stable).+B3_API void b3RecPlayer_Restart( b3RecPlayer* player );++/// Seek to a specific frame. Forward seek steps op-by-op; backward seek restores+/// the nearest keyframe then re-steps the remaining gap.+B3_API void b3RecPlayer_SeekFrame( b3RecPlayer* player, int targetFrame );++/// @return the world currently driven by this player+B3_API b3WorldId b3RecPlayer_GetWorldId( const b3RecPlayer* player );++/// @return the last fully-stepped frame index (0 before any step)+B3_API int b3RecPlayer_GetFrame( const b3RecPlayer* player );++/// @return total number of recorded frames+B3_API int b3RecPlayer_GetFrameCount( const b3RecPlayer* player );++/// @return true when the op stream is exhausted+B3_API bool b3RecPlayer_IsAtEnd( const b3RecPlayer* player );++/// @return true when the op stream is paused between body creation and world step.+B3_API bool b3RecPlayer_IsAtPreStep( const b3RecPlayer* player );++/// @return true when any StateHash mismatch has been detected+B3_API bool b3RecPlayer_HasDiverged( const b3RecPlayer* player );++/// @return a summary of the recording read at open: frame count, recorded tuning, and bounds+B3_API b3RecPlayerInfo b3RecPlayer_GetInfo( const b3RecPlayer* player );++/// @return the first frame at which replay diverged, or -1 if it has not diverged+B3_API int b3RecPlayer_GetDivergeFrame( const b3RecPlayer* player );++/// Set the worker count of the replay world. Clamped to [1, B3_MAX_WORKERS]. Applied to the live+/// world at once and reused whenever the player rebuilds its world on Restart or a backward seek.+/// Replaying at a different count than recorded re-partitions the constraint graph, so the StateHash+/// check becomes a cross-thread determinism test.+B3_API void b3RecPlayer_SetWorkerCount( b3RecPlayer* player, int count );++/// Tune the keyframe ring used to speed up backward seeking. A keyframe is a periodic snapshot the+/// player restores from instead of replaying from the start, trading memory for seek speed.+/// @param player the recording player+/// @param budgetBytes memory cap for the kept snapshots; the spacing widens to stay under it+/// @param minIntervalFrames finest spacing between keyframes, in frames+/// A zero budget or a non-positive interval keeps that value. Clears the existing ring, so call+/// b3RecPlayer_Restart afterward to repopulate it under the new policy.+B3_API void b3RecPlayer_SetKeyframePolicy( b3RecPlayer* player, size_t budgetBytes, int minIntervalFrames );++/// @return the keyframe memory budget in bytes+B3_API size_t b3RecPlayer_GetKeyframeBudget( const b3RecPlayer* player );++/// @return the finest keyframe spacing in frames+B3_API int b3RecPlayer_GetKeyframeMinInterval( const b3RecPlayer* player );++/// @return the current keyframe spacing in frames; starts at the min interval and doubles as the+/// ring evicts to stay under budget, so it reflects the effective backward-seek granularity now+B3_API int b3RecPlayer_GetKeyframeInterval( const b3RecPlayer* player );++/// @return the memory currently held by keyframe snapshots, in bytes+B3_API size_t b3RecPlayer_GetKeyframeBytes( const b3RecPlayer* player );++/// @return the number of bodies tracked in creation order (including holes for destroyed bodies)+B3_API int b3RecPlayer_GetBodyCount( const b3RecPlayer* player );++/// Resolve a creation ordinal to the live body id at the current frame.+/// @return the body id, or a null id if that ordinal is out of range or its body is destroyed+B3_API b3BodyId b3RecPlayer_GetBodyId( const b3RecPlayer* player, int index );++/// Wire host debug-shape callbacks into the player's replay world so a renderer can build+/// per-shape draw resources (the 3D sample needs this or the replay world draws nothing).+/// Rebuilds the current world under the new callbacks and rewinds to frame 0, so call it+/// once right after b3RecPlayer_Create and re-read the world id afterward. The callbacks+/// persist across Restart and backward seeks, which recreate the world internally.+/// @param player the player to configure+/// @param createDebugShape called when a replayed shape is added; returns a user draw handle+/// @param destroyDebugShape called when a replayed shape is removed; may be NULL+/// @param context user context passed to both callbacks+B3_API void b3RecPlayer_SetDebugShapeCallbacks( b3RecPlayer* player, b3CreateDebugShapeCallback* createDebugShape,+												b3DestroyDebugShapeCallback* destroyDebugShape, void* context );++/// Draw the spatial queries recorded during the most recently replayed frame, layered on top of the+/// world. Call after b3World_Draw. NULL draw function pointers are skipped.+/// @param player a valid player handle+/// @param draw debug draw callbacks+/// @param queryIndex index of the frame query to draw, or -1 to draw all of them+/// @param selectedIndex index of the query to emphasize (reserved color plus a label), or -1 for none+B3_API void b3RecPlayer_DrawFrameQueries( b3RecPlayer* player, b3DebugDraw* draw, int queryIndex, int selectedIndex );++/// The kind of a recorded spatial query, matching the public query and cast functions.+typedef enum b3RecQueryType+{+	b3_recQueryOverlapAABB,+	b3_recQueryOverlapShape,+	b3_recQueryCastRay,+	b3_recQueryCastShape,+	b3_recQueryCastRayClosest,+	b3_recQueryCastMover,+	b3_recQueryCollideMover,+} b3RecQueryType;++/// A spatial query recorded during a replayed frame, exposed for inspection.+typedef struct b3RecQueryInfo+{+	b3RecQueryType type;+	b3QueryFilter filter;+	b3AABB aabb;		// world-space bounds of the query, swept for casts+	b3Pos origin;		// query origin (zero for overlap AABB)+	b3Vec3 translation; // ray and cast translation+	int hitCount;		// number of recorded results+	uint64_t key;		// identity key, the hash of (id, name), 0 if untagged+	uint64_t id;		// query id, 0 if none+	const char* name;	// query label, NULL if none+} b3RecQueryInfo;++/// One result of a recorded spatial query.+typedef struct b3RecQueryHit+{+	b3ShapeId shape;+	b3Pos point;+	b3Vec3 normal;+	float fraction;+} b3RecQueryHit;++/// @return the number of spatial queries recorded for the most recently replayed frame+B3_API int b3RecPlayer_GetFrameQueryCount( const b3RecPlayer* player );++/// Get a recorded query from the most recently replayed frame by index.+B3_API b3RecQueryInfo b3RecPlayer_GetFrameQuery( const b3RecPlayer* player, int index );++/// Get one result of a recorded query from the most recently replayed frame.+B3_API b3RecQueryHit b3RecPlayer_GetFrameQueryHit( const b3RecPlayer* player, int queryIndex, int hitIndex );++/**@}*/ // recording++/** @} */ // world++/**+ * @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.+B3_API void b3Body_SetName( b3BodyId bodyId, const char* name );++/// Get the body name. Returns an empty string if the name isn't set.+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 b3Pos b3Body_GetPosition( b3BodyId bodyId );++/// Get the world rotation of a body as a quaternion+B3_API b3Quat b3Body_GetRotation( b3BodyId bodyId );++/// Get the world transform of a body.+B3_API b3WorldTransform 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 the intended transform.+/// @see b3BodyDef::position and b3BodyDef::rotation.+B3_API void b3Body_SetTransform( b3BodyId bodyId, b3Pos position, b3Quat rotation );++/// Get a local point on a body given a world point.+B3_API b3Vec3 b3Body_GetLocalPoint( b3BodyId bodyId, b3Pos worldPoint );++/// Get a world point on a body given a local point.+B3_API b3Pos 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 at the center of mass. 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, b3WorldTransform 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, b3Pos 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, b3Pos 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, b3Pos 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_GetLocalCenter( b3BodyId bodyId );++/// Get the center of mass position of the body in world space+B3_API b3Pos b3Body_GetWorldCenter( 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 updates 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 );++/// Allow this body to rotate fast. Useful for axially symmetric bodies, such as vehicle wheels.+/// Normally rotation speed is clamped to improve CCD. However, this clamping is unnecessary for+/// bodies that only rotate fast around an axis of symmetry.+B3_API void b3Body_AllowFastRotation( b3BodyId bodyId, bool flag );++/// Is this body allowed to rotate fast?+B3_API bool b3Body_IsFastRotationAllowed( b3BodyId bodyId );++/// Enable or disable contact recycling for this body. Contact recycling is a performance optimization+/// that reuses contact manifolds when bodies move slightly. Disabling it can avoid ghost collisions+/// on characters at the cost of higher per-step work. Existing contacts retain their prior setting;+/// only contacts created after this call see the new value.+/// @see b3BodyDef::enableContactRecycling+B3_API void b3Body_EnableContactRecycling( b3BodyId bodyId, bool flag );++/// Is contact recycling enabled on this body?+B3_API bool b3Body_IsContactRecyclingEnabled( b3BodyId bodyId );++/// Enable/disable hit events on all shapes+/// @see b3ShapeDef::enableHitEvents+B3_API void b3Body_EnableHitEvents( b3BodyId bodyId, bool flag );++/// 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, b3Pos origin, b3Vec3 translation, b3QueryFilter filter,+										float maxFraction, b3WorldTransform bodyTransform );++/// Cast a shape at a specific body using a specified body transform.+B3_API b3BodyCastResult b3Body_CastShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation,+										  b3QueryFilter filter, float maxFraction, bool canEncroach,+										  b3WorldTransform bodyTransform );++/// Overlap a shape with a specific body using a specified body transform.+B3_API bool b3Body_OverlapShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, b3QueryFilter filter,+								 b3WorldTransform 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, b3Pos origin,+								const b3Capsule* mover, b3QueryFilter filter, b3WorldTransform bodyTransform );++/** @} */ // body++/**+ * @defgroup shape Shape+ * Functions to create, destroy, and access.+ * Shapes bind raw geometry to bodies and hold material properties including friction and restitution.+ * You may add multiple shapes to a single body. There are no hard limits on shape count per body.+ * + * When you create a shape on a body the center of mass moves. This can lead to the body linear velocity+ * changing if the angular velocity is non-zero.+ * @{+ */++/// 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 shape definition is 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 b3HullData* hull );++/// Create a convex hull shape and attach it to a body. The hull is cloned then transformed with scale applied first.+/// Use this for non-uniform or mirrored scale or a baked local transform. The baked result is shared through the+/// world hull database. 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 b3HullData* 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 b3HeightFieldData* heightField );++/// Baked compound shapes are only allowed on static bodies.+/// Note: runtime compounds are achieved by adding multiple shapes to a body.+/// Runtime compounds can be dynamic and/or kinematic.+B3_API b3ShapeId b3CreateBakedCompoundShape( b3BodyId bodyId, b3ShapeDef* def, const b3CompoundData* 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.+B3_API void b3Shape_SetName( b3ShapeId shapeId, const char* name );++/// Get the shape name. Returns an empty string if the name isn't set.+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^3.+/// 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^3+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. The ray runs from origin to origin + translation and the hit point+/// comes back as a world position, so the cast stays precise far from the world origin.+B3_API b3WorldCastOutput b3Shape_RayCast( b3ShapeId shapeId, b3Pos origin, b3Vec3 translation );++/// 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 b3HullData* 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 b3HeightFieldData* 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 b3HullData* 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 );++/** @} */ // shape++/**+ * @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 radians.+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 parallel_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 newton-meters+B3_API void b3ParallelJoint_SetMaxTorque( b3JointId jointId, float force );++/// Get the maximum spring torque, usually in newton-meters+B3_API float b3ParallelJoint_GetMaxTorque( b3JointId jointId );++/** @} */ // parallel_joint++/**+ * @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 );++/** @} */ // distance_joint++/**+ * @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 );++/**@}*/ // motor_joint++/**+ * @defgroup filter_joint Filter Joint+ * @brief Functions for the filter joint.+ *+ * The filter joint is used to disable collision between two bodies. As a side effect of being a joint, it also+ * keeps the two bodies in the same simulation island.+ * @{+ */++/// Create a filter joint.+/// @see b3FilterJointDef for details+B3_API b3JointId b3CreateFilterJoint( b3WorldId worldId, const b3FilterJointDef* def );++/**@}*/ // filter_joint++/**+ * @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 );++/**@}*/ // prismatic_joint++/**+ * @defgroup revolute_joint Revolute Joint+ * @brief A revolute joint allows for relative rotation about a single axis with no relative translation.+ *+ * 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 );++/// Is 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 );++/**@}*/ // revolute_joint++/**+ * @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 spherical joint+/// @see b3SphericalJointDef 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 );++/// Is 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 );++/**@}*/ // spherical_joint++/**+ * @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 );++/**@}*/ // weld_joint++/**+ * @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 current spin speed in radians per second.+B3_API float b3WheelJoint_GetSpinSpeed( 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 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 );++/**@}*/ // wheel_joint++/**@}*/ // joint++/**+ * @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 );++/**@}*/ // contact
+ upstream/include/box3d/collision.h view
@@ -0,0 +1,667 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "base.h"+#include "math_functions.h"+#include "types.h"++#include <stdbool.h>+#include <stddef.h>++/**+ * @addtogroup tree+ * @{+ */++/// 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 );++/// Query an AABB for overlapping proxies. The callback function 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 );++/// Query an AABB for the closest object. The callback function 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 provided 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 );++/// Ray cast against the proxies in the tree. This relies on the callback+/// to perform an exact ray cast in the case where the proxy contains a shape.+/// The callback also performs 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 function 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 );++/// Sweep an AABB through the tree. The box is in the tree's world float frame and the callback+/// re-differences each shape at full precision against the query origin. Used by the large world+/// spatial queries so the tree traversal stays float while the narrow phase stays precise.+B3_API b3TreeStats b3DynamicTree_BoxCast( const b3DynamicTree* tree, const b3BoxCastInput* input, uint64_t maskBits,+										  bool requireAllBits, b3TreeBoxCastCallbackFcn* callback, void* context );++/// 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;+}++/**@}*/ // tree++/**+ * @addtogroup hull+ * @{+ */++/// Get read only hull vertices.+B3_INLINE const b3HullVertex* b3GetHullVertices( const b3HullData* hull )+{+	if ( hull->vertexOffset == 0 )+	{+		return NULL;+	}++	return (const b3HullVertex*)( (intptr_t)hull + hull->vertexOffset );+}++/// Get read only hull points.+B3_INLINE const b3Vec3* b3GetHullPoints( const b3HullData* hull )+{+	if ( hull->pointOffset == 0 )+	{+		return NULL;+	}++	return (const b3Vec3*)( (intptr_t)hull + hull->pointOffset );+}++/// Get read only hull half edges.+B3_INLINE const b3HullHalfEdge* b3GetHullEdges( const b3HullData* hull )+{+	if ( hull->edgeOffset == 0 )+	{+		return NULL;+	}++	return (const b3HullHalfEdge*)( (intptr_t)hull + hull->edgeOffset );+}++/// Get read only hull planes.+B3_INLINE const b3Plane* b3GetHullPlanes( const b3HullData* hull )+{+	if ( hull->planeOffset == 0 )+	{+		return NULL;+	}++	return (const b3Plane*)( (intptr_t)hull + hull->planeOffset );+}++/// Get read only hull faces.+B3_INLINE const b3HullFace* b3GetHullFaces( const b3HullData* hull )+{+	if ( hull->faceOffset == 0 )+	{+		return NULL;+	}++	return (const b3HullFace*)( (intptr_t)hull + hull->faceOffset );+}++/// Get read only SOA vertices. This is an array of vertices with all x values,+/// y values, and z values as separate arrays. The array lengths are padded to+/// a multiple of 4. The padded values are repeats of the first value.+B3_INLINE const float* b3GetHullSoaVertices( const b3HullData* hull )+{+	if ( hull->soaVertexOffset == 0 )+	{+		return NULL;+	}++	return (const float*)( (intptr_t)hull + hull->soaVertexOffset );+}++/// Get read only SOA unit normal vectors. This is an array of normals with all x values,+/// y values, and z values as separate arrays. The array lengths are padded to+/// a multiple of 4. The padded values are repeats of the first value.+B3_INLINE const float* b3GetHullSoaNormals( const b3HullData* hull )+{+	if ( hull->soaNormalOffset == 0 )+	{+		return NULL;+	}++	return (const float*)( (intptr_t)hull + hull->soaNormalOffset );+}++/// Create a tessellated cylinder as a hull.+B3_API b3HullData* b3CreateCylinder( float height, float radius, float yOffset, int sides );++/// Create a tessellated cone as a hull.+B3_API b3HullData* b3CreateCone( float height, float radius1, float radius2, int slices );++/// Create a rock shaped hull.+B3_API b3HullData* b3CreateRock( float radius );++/// Create a generic convex hull.+B3_API b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCount );++/// Deep clone a hull.+B3_API b3HullData* b3CloneHull( const b3HullData* hull );++/// Clone and transform a hull. Supports non-uniform and mirroring scale.+B3_API b3HullData* b3CloneAndTransformHull( const b3HullData* original, b3Transform transform, b3Vec3 scale );++/// Destroy a hull.+B3_API void b3DestroyHull( b3HullData* hull );++/// Make a cube as a hull. Do not call b3DestroyHull on this.+B3_API b3BoxHull b3MakeCubeHull( float halfWidth );++/// Make a box as a hull. Do not call b3DestroyHull on this.+B3_API b3BoxHull b3MakeBoxHull( float hx, float hy, float hz );++/// Make an offset box as a hull. Do not call b3DestroyHull on this.+B3_API b3BoxHull b3MakeOffsetBoxHull( float hx, float hy, float hz, b3Vec3 offset );++/// Make a transformed box as a 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 a transformed box hull with post scaling. This is useful for boxes that are scaled in+/// a level editor. Such scaling can have reflection and shear. In the case of shear the result+/// may be approximate. If you need to support shear 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 );++/**@}*/ // hull++/**+ * @addtogroup mesh+ * @{+ */++/// Get read only mesh BVH nodes.+B3_INLINE const b3MeshNode* b3GetMeshNodes( const b3MeshData* mesh )+{+	if ( mesh->nodeOffset == 0 )+	{+		return NULL;+	}++	return (const b3MeshNode*)( (intptr_t)mesh + mesh->nodeOffset );+}++/// Get read only mesh vertices.+B3_INLINE const b3Vec3* b3GetMeshVertices( const b3MeshData* mesh )+{+	if ( mesh->vertexOffset == 0 )+	{+		return NULL;+	}++	return (const b3Vec3*)( (intptr_t)mesh + mesh->vertexOffset );+}++/// Get read only mesh triangles.+B3_INLINE const b3MeshTriangle* b3GetMeshTriangles( const b3MeshData* mesh )+{+	if ( mesh->triangleOffset == 0 )+	{+		return NULL;+	}++	return (const b3MeshTriangle*)( (intptr_t)mesh + mesh->triangleOffset );+}++/// Get read only mesh materials. The 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 );+}++/// Get read only mesh flags. The 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 );+}++/// Create 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 );++/// Create a wave mesh along the x and z axes.+B3_API b3MeshData* b3CreateWaveMesh( int xCount, int zCount, float cellWidth, float amplitude, float rowFrequency,+									 float columnFrequency );++/// Create a torus mesh.+B3_API b3MeshData* b3CreateTorusMesh( int radialResolution, int tubularResolution, float radius, float thickness );++/// Create a box mesh.+B3_API b3MeshData* b3CreateBoxMesh( b3Vec3 center, b3Vec3 extent, bool identifyEdges );++/// Create a hollow box mesh.+B3_API b3MeshData* b3CreateHollowBoxMesh( b3Vec3 center, b3Vec3 extent );++/// Create a platform mesh. A truncated pyramid.+B3_API b3MeshData* b3CreatePlatformMesh( b3Vec3 center, float height, float topWidth, float bottomWidth );++/// Create a generic mesh.+B3_API b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, int degenerateCapacity );++/// Destroy a mesh.+B3_API void b3DestroyMesh( b3MeshData* mesh );++/// Get the height of the mesh BVH.+B3_API int b3GetHeight( const b3MeshData* mesh );++/**@}*/ // mesh++/**+ * @addtogroup height_field+ * @{+ */++/// Get read only compressed heights. One uint16_t per grid point.+B3_INLINE const uint16_t* b3GetHeightFieldCompressedHeights( const b3HeightFieldData* hf )+{+	if ( hf->heightsOffset == 0 )+	{+		return NULL;+	}++	return (const uint16_t*)( (intptr_t)hf + hf->heightsOffset );+}++/// Get read only material indices. One uint8_t per cell.+B3_INLINE const uint8_t* b3GetHeightFieldMaterialIndices( const b3HeightFieldData* hf )+{+	if ( hf->materialOffset == 0 )+	{+		return NULL;+	}++	return (const uint8_t*)( (intptr_t)hf + hf->materialOffset );+}++/// Get read only triangle flags. One uint8_t per triangle.+B3_INLINE const uint8_t* b3GetHeightFieldFlags( const b3HeightFieldData* hf )+{+	if ( hf->flagsOffset == 0 )+	{+		return NULL;+	}++	return (const uint8_t*)( (intptr_t)hf + hf->flagsOffset );+}++/// Create a generic height field.+B3_API b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data );++/// Create a grid as a height field.+B3_API b3HeightFieldData* b3CreateGrid( int rowCount, int columnCount, b3Vec3 scale, bool makeHoles );++/// Create a wave grid as a height field.+B3_API b3HeightFieldData* b3CreateWave( int rowCount, int columnCount, b3Vec3 scale, float rowFrequency, float columnFrequency,+										bool makeHoles );++/// Destroy a height field.+B3_API void b3DestroyHeightField( b3HeightFieldData* 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 b3HeightFieldData* b3LoadHeightField( const char* fileName );++/**@}*/ // height_field++/**+ * @addtogroup compound+ * @{+ */++/// Get a child shape of a compound.+B3_API b3ChildShape b3GetCompoundChild( const b3CompoundData* compound, int childIndex );++/// Query a compound shape for children that overlap an AABB.+B3_API void b3QueryCompound( const b3CompoundData* compound, b3AABB aabb, b3CompoundQueryFcn* fcn, void* context );++/// Access a child capsule by index.+B3_API b3CompoundCapsule b3GetCompoundCapsule( const b3CompoundData* compound, int index );++/// Access a child hull by index.+B3_API b3CompoundHull b3GetCompoundHull( const b3CompoundData* compound, int index );++/// Access a child mesh by index.+B3_API b3CompoundMesh b3GetCompoundMesh( const b3CompoundData* compound, int index );++/// Access a child sphere by index.+B3_API b3CompoundSphere b3GetCompoundSphere( const b3CompoundData* compound, int index );++/// Access the compound material array.+B3_API const b3SurfaceMaterial* b3GetCompoundMaterials( const b3CompoundData* compound );++/// Create a compound shape. All input data in the definition is cloned into the resulting compound.+B3_API b3CompoundData* b3CreateCompound( const b3CompoundDef* def );++/// Destroy a compound shape.+B3_API void b3DestroyCompound( b3CompoundData* compound );++/// Cast the provided compound data to bytes, setting the internal pointers to null.+/// Use this before serializing the compound bytes.+B3_API uint8_t* b3ConvertCompoundToBytes( b3CompoundData* compound );++/// Cast the provided bytes to compound data, setting up internal pointers.+/// Use this after de-serializing the compound bytes.+B3_API b3CompoundData* b3ConvertBytesToCompound( uint8_t* bytes, int byteCount );++/**@}*/ // compound++/**+ * @addtogroup geometry+ * @{+ */++/// 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 b3HullData* 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 b3HullData* 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 b3HeightFieldData* shape, b3Transform transform );++/// Compute the bounding box of a compound+B3_API b3AABB b3ComputeCompoundAABB( const b3CompoundData* shape, b3Transform transform );++/**@}*/ // geometry++/**+ * @addtogroup query+ * @{+ */++/// 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 b3CompoundData* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy );++/// Overlap shape versus height field+B3_API bool b3OverlapHeightField( const b3HeightFieldData* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy );++/// Overlap shape versus hull+B3_API bool b3OverlapHull( const b3HullData* 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. A zero length ray is a point query. Initial overlap+/// reports a hit at the ray origin with zero fraction and zero normal.+B3_API b3CastOutput b3RayCastSphere( const b3Sphere* shape, const b3RayCastInput* input );++/// Ray cast versus a hollow sphere shell in local space. Unlike the solid sphere a ray starting+/// inside is not an overlap: it passes through and hits the far wall.+B3_API b3CastOutput b3RayCastHollowSphere( const b3Sphere* shape, const b3RayCastInput* input );++/// Ray cast versus capsule in local space. A zero length ray is a point query. Initial overlap+/// reports a hit at the ray origin with zero fraction and zero normal.+B3_API b3CastOutput b3RayCastCapsule( const b3Capsule* shape, const b3RayCastInput* input );++/// Ray cast versus compound in local space. A zero length ray is a point query. Initial overlap+/// with a child reports a hit at the ray origin with zero fraction and zero normal.+B3_API b3CastOutput b3RayCastCompound( const b3CompoundData* shape, const b3RayCastInput* input );++/// Ray cast versus hull shape in local space. A zero length ray is a point query. Initial overlap+/// reports a hit at the ray origin with zero fraction and zero normal.+B3_API b3CastOutput b3RayCastHull( const b3HullData* shape, const b3RayCastInput* input );++/// Ray cast versus mesh in local space. A thin surface with no interior, so there is no overlap case.+B3_API b3CastOutput b3RayCastMesh( const b3Mesh* shape, const b3RayCastInput* input );++/// Ray cast versus height field in local space. A thin surface with no interior, so there is no overlap case.+B3_API b3CastOutput b3RayCastHeightField( const b3HeightFieldData* 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 b3CompoundData* shape, const b3ShapeCastInput* input );++/// Shape cast versus a hull. Initial overlap is treated as a miss.+B3_API b3CastOutput b3ShapeCastHull( const b3HullData* 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 b3HeightFieldData* 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 b3HeightFieldData* heightField, b3AABB bounds, b3MeshQueryFcn* fcn, void* context );++/// 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 query runs in frame A, so the witness points and normal are returned in frame A.+/// The underlying GJK algorithm may be debugged by passing in debug simplexes and capacity. You may pass in NULL and 0 for these.+B3_API b3DistanceOutput b3ShapeDistance( const b3DistanceInput* input, b3SimplexCache* cache, b3Simplex* simplexes,+										 int simplexCapacity );++/// Perform a linear shape cast of shape B moving and shape A fixed. Determines the hit point, normal, and translation fraction.+/// The query runs in frame A, so the hit point and normal are returned in frame A. Initially touching shapes are a miss.+B3_API b3CastOutput b3ShapeCast( const b3ShapeCastPairInput* input );++/// Evaluate the transform sweep at a specific time.+B3_API b3Transform b3GetSweepTransform( const b3Sweep* sweep, float time );++/// 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 );++/**@}*/ // query++/**+ * @addtogroup collision+ * @{+ */++/// Collide two spheres.+B3_API void b3CollideSpheres( b3LocalManifold* manifold, int capacity, const b3Sphere* sphereA, const b3Sphere* sphereB,+							  b3Transform transformBtoA );++/// Collide a capsule and a sphere.+B3_API void b3CollideCapsuleAndSphere( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA,+									   const b3Sphere* sphereB, b3Transform transformBtoA );++/// Collide a hull and a sphere.+B3_API void b3CollideHullAndSphere( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3Sphere* sphereB,+									b3Transform transformBtoA, b3SimplexCache* cache );++/// Collide two capsules.+B3_API void b3CollideCapsules( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA, const b3Capsule* capsuleB,+							   b3Transform transformBtoA );++/// Collide a hull and a capsule.+B3_API void b3CollideHullAndCapsule( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3Capsule* capsuleB,+									 b3Transform transformBtoA, b3SimplexCache* cache );++/// Collide two hulls.+B3_API void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB,+							b3Transform transformBtoA, b3SATCache* cache );++/// Collide a triangle and capsule. Normal points from triangle to capsule.+B3_API void b3CollideTriangleAndCapsule( b3LocalManifold* manifold, int capacity, const b3Vec3* triangleA,+										 const b3Capsule* capsuleB, b3SimplexCache* cache );++/// Collide a triangle and hull. Normal points from triangle to hull.+B3_API void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, int triangleFlags,+									  const b3HullData* hullB, b3SATCache* cache, bool enableSpeculative );++/// Collide a triangle and sphere. Normal points from triangle to sphere.+B3_API void b3CollideTriangleAndSphere( b3LocalManifold* manifold, int capacity, const b3Vec3* triangleA,+										const b3Sphere* sphereB );++/**@}*/ // collision++/**+ * @addtogroup character+ * @{+ */++/// Solves the position of a mover that satisfies the given collision planes.+/// @param targetDelta the desired translation from the position used to generate the collision planes+/// @param planes the collision planes+/// @param count the number of collision planes+B3_API b3PlaneSolverResult b3SolvePlanes( b3Vec3 targetDelta, b3CollisionPlane* planes, int count );++/// Clips the velocity against the given collision planes. Planes with zero push or clipVelocity+/// set to false are skipped.+B3_API b3Vec3 b3ClipVector( b3Vec3 vector, const b3CollisionPlane* planes, int count );++/**@}*/ // character
+ upstream/include/box3d/config.h view
@@ -0,0 +1,35 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++// Box3D compile-time options.+//+// Normally set by the CMake BOX3D_* options. If you build Box3D without its+// CMake (dropping the sources into another project), set them here so the+// library and your code agree. Edit this file, or keep your settings elsewhere+// and point Box3D at them from your build:+//+//   #define BOX3D_USER_CONFIG "my_box3d_config.h"+//+// A define passed on the compiler command line still wins over this file.++// Large world mode. Stores world positions in double precision. Affects ABI.+//#define BOX3D_DOUBLE_PRECISION++// Build the scalar fallback instead of SSE2/NEON.+//#define BOX3D_DISABLE_SIMD++// Enable internal validation in debug builds.+//#define BOX3D_VALIDATE++// Decorate the public API with your own export macro instead of Box3D's+// box3d_EXPORTS/BOX3D_DLL scheme, for example when compiling Box3D into another+// shared library. A single value cannot switch between dllexport and dllimport,+// so this suits embedding more than shipping Box3D as its own DLL.+//#define BOX3D_EXPORT MYENGINE_API++// Enable assertions when NDEBUG is defined+// #define B3_ENABLE_ASSERT++// #define B3_RESTITUTION_ITERATIONS
+ upstream/include/box3d/constants.h view
@@ -0,0 +1,139 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "base.h"++/// Box3D 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 Box3D+B3_API void b3SetLengthUnitsPerMeter( float lengthUnits );++/// Get the current length units per meter.+B3_API float b3GetLengthUnitsPerMeter( void );++/// Set the threshold for logging stalls.+B3_API void b3SetStallThreshold( float seconds );++/// Get the threshold for logging stalls.+B3_API float b3GetStallThreshold( void );++// Used to detect bad values. In float mode positions greater than about 16km have precision+// problems, so 100km is a safe limit. Large world mode keeps coordinates accurate much farther+// from the origin, so the sanity limit widens to keep valid far-field positions from tripping it.+#if defined( BOX3D_DOUBLE_PRECISION )+#define B3_HUGE ( 1.0e9f * b3GetLengthUnitsPerMeter() )+#else+#define B3_HUGE ( 1.0e5f * b3GetLengthUnitsPerMeter() )+#endif++/// Maximum parallel workers. Used for some fixed size arrays.+#define B3_MAX_WORKERS 32++/// Maximum number of tasks queued per world step. b3EnqueueTaskCallback 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() )++/// The minimum length of a capsules. Very short capsules should be created as spheres+/// to avoid numerical problems.+#define B3_MIN_CAPSULE_LENGTH ( B3_LINEAR_SLOP )++/// Minimum contact point friction weight, lower bound for speculative points. Made small+/// enough to be washed away by weights that hit 1.+#define B3_MIN_FRICTION_WEIGHT ( 1e-10f )++/// 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 rest offset is used for mesh contact to reduce ghost collisions and assist with CCD.+/// The rest offset adjusts the contact point separation value, making the solver push the shapes+/// apart by this distance.+/// Must be at least B3_LINEAR_SLOP and less than B3_SPECULATIVE_DISTANCE.+#define B3_MESH_REST_OFFSET ( 1.0f * B3_LINEAR_SLOP )++/// The default contact recycling distance.+#define B3_CONTACT_RECYCLE_DISTANCE ( 10.0f * B3_LINEAR_SLOP )++/// The default contact recycling world angle threshold. For performance this value+/// is cos(angle/2)^2. This value corresponds to 10 degrees.+#define B3_CONTACT_RECYCLE_ANGULAR_DISTANCE ( 0.99240388f )++/// 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_MAX_AABB_MARGIN ( 0.05f * b3GetLengthUnitsPerMeter() )++/// Per-shape AABB margin is a fraction of the shape extent (capped by B3_MAX_AABB_MARGIN).+/// Small shapes get small margins; large shapes are clamped to the cap.+#define B3_AABB_MARGIN_FRACTION 0.125f++/// The time that a body must be still before it will go to sleep. In seconds.+#define B3_TIME_TO_SLEEP 0.5f++/// The maximum number of contact points between two touching shapes.+#define B3_MAX_MANIFOLD_POINTS 4++/// The number of iterations for gyroscopic torques.+#ifndef B3_GYROSCOPIC_ITERATIONS+#define B3_GYROSCOPIC_ITERATIONS 1+#endif++/// The maximum number of convex hull vertices. This is fixed for performance reasons.+#define B3_MAX_HULL_VERTICES 128++/// The maximum number of convex hull faces.+#define B3_MAX_HULL_FACES 128++/// The maximum number of convex hull edges. Full edges, not half-edges.+#define B3_MAX_HULL_EDGES 128++/// Relative tolerance used to determine if two edges are parallel.+#define B3_PARALLEL_EDGE_TOL 0.005f++/// The maximum number points to use for shape cast proxies (swept point cloud).+#define B3_MAX_SHAPE_CAST_POINTS B3_MAX_HULL_VERTICES++/// 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 )++/// Increase this if your application needs more accurate restitution. Doing so will+/// slow down the simulation. Must be 1 or more.+#ifndef B3_RESTITUTION_ITERATIONS+#define B3_RESTITUTION_ITERATIONS 1+#endif
+ upstream/include/box3d/id.h view
@@ -0,0 +1,179 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include <stdint.h>++// Note: this file should be stand-alone++/**+ * @defgroup id Ids+ * These ids serve as handles to internal Box3D objects.+ * These should be considered opaque data and passed by value.+ * Include this header if you need the id types and not the whole Box3D 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 Box3D. 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 handle.+typedef struct b3ContactId+{+	int32_t index1;+	uint16_t world0;+	int16_t padding;+	uint32_t generation;+} b3ContactId;++// clang-format off+#ifdef __cplusplus+	/// A null id. Works for any id type.+	#define B3_NULL_ID {}+	#define B3_ID_INLINE inline+#else+	/// A null id. Works for any id type.+	#define B3_NULL_ID { 0 }++	/// This macro bridges C and C++ inline functions. C++ has the one definition rule that C lacks.+	#define B3_ID_INLINE static inline+#endif+// clang-format on++/// 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 three uint32 values+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 contact id from three uint32 values.+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;+}++/**@}*/
+ upstream/include/box3d/math_functions.h view
@@ -0,0 +1,1205 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "base.h"++#include <float.h>++// for sqrtf and remainderf+#include <math.h>+#include <stdbool.h>++/**+ * @defgroup math Math+ * @brief Vector math types and functions+ * @{+ */++/// https://en.wikipedia.org/wiki/Pi+#define B3_PI 3.14159265359f++/// Convenience macro to convert from degrees to radians.+#define B3_DEG_TO_RAD 0.01745329251f++/// Convenience macro to convert from radians to degrees.+#define B3_RAD_TO_DEG 57.2957795131f++/// Minimum scale used for scaling collision meshes, etc.+#define B3_MIN_SCALE 0.01f++/// A 2D vector.+typedef struct b3Vec2+{+	float x;+	float y;+} b3Vec2;++/// A 3D vector.+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;++/// A quaternion.+typedef struct b3Quat+{+	b3Vec3 v;+	float s;+} b3Quat;++/// A rigid transform.+typedef struct b3Transform+{+	b3Vec3 p;+	b3Quat q;+} b3Transform;++#if defined( BOX3D_DOUBLE_PRECISION )++/// A world position. Double precision in large world mode so coordinates stay accurate far+/// from the origin.+typedef struct b3Pos+{+	double x, y, z;+} b3Pos;++/// A world transform with double precision translation and float quaternion rotation. Rotation+/// is frame local and never needs the extra range, the same split as Jolt's DMat44.+typedef struct b3WorldTransform+{+	b3Pos p;+	b3Quat q;+} b3WorldTransform;++#else++/// In single precision mode these types are the same.+typedef b3Vec3 b3Pos;++/// In single precision mode these types are the same.+typedef b3Transform b3WorldTransform;++#endif++/// A 3x3 matrix.+typedef struct b3Matrix3+{+	b3Vec3 cx, cy, cz;+} b3Matrix3;++/// Axis aligned bounding box.+typedef struct b3AABB+{+	b3Vec3 lowerBound;+	b3Vec3 upperBound;+} b3AABB;++/// A plane.+/// separation = dot(normal, point) - offset+typedef struct b3Plane+{+	b3Vec3 normal;+	float offset;+} b3Plane;++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 },+};++// Valid in both modes: 0.0f promotes to double, the identity rotation stays float+static const b3Pos b3Pos_zero = { 0.0f, 0.0f, 0.0f };+static const b3WorldTransform b3WorldTransform_identity = { { 0.0f, 0.0f, 0.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 );+}++/// @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 );++/// @deprecated +B3_INLINE float b3Sin( float radians )+{+	b3CosSin cs = b3ComputeCosSin( radians );+	return cs.sine;+}++/// @deprecated +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;+}++/// Distance between two points.+B3_INLINE float b3Distance( b3Vec3 a, b3Vec3 b )+{+	b3Vec3 dv = { b.x - a.x, b.y - a.y, b.z - a.z };+	return b3Length( dv );+}++/// Squared distance between two points.+B3_INLINE float b3DistanceSquared( b3Vec3 a, b3Vec3 b )+{+	b3Vec3 dv = { b.x - a.x, b.y - a.y, b.z - a.z };+	return dv.x * dv.x + dv.y * dv.y + dv.z * dv.z;+}++/// Normalize a vector. Returns a zero vector if the input vector is very small.+B3_FORCE_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 };+}++/// Normalize a vector and return the length. Returns a zero vector+/// if the input is very small.+B3_INLINE b3Vec3 b3GetLengthAndNormalize( float* length, b3Vec3 a )+{+	*length = b3Length( a );+	if ( *length < FLT_EPSILON )+	{+		return b3Vec3_zero;+	}++	float invLength = 1.0f / *length;+	b3Vec3 n = { invLength * a.x, invLength * a.y, invLength * a.z };+	return n;+}++/// Get a unit vector that is perpendicular to the supplied vector.+B3_INLINE b3Vec3 b3Perp( b3Vec3 a )+{+	// 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 ( a.x < -0.5f || 0.5f < a.x )+	{+		p = B3_LITERAL( b3Vec3 ){ a.y, -a.x, 0.0f };+	}+	else+	{+		p = B3_LITERAL( b3Vec3 ){ 0.0f, a.z, -a.y };+	}++	return b3Normalize( p );+}++/// Is a vector normalized? In other words, does it have unit length?+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 };+}++/// s * a+B3_INLINE b3Vec3 b3MulSV( float s, b3Vec3 a )+{+	return B3_LITERAL( b3Vec3 ){ s * a.x, s * a.y, s * a.z };+}++/// https://en.wikipedia.org/wiki/Cross_product+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;+}++/// Linearly interpolate between two vectors.+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;+}++/// Blend two vectors: s * a + t * b+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;+}++/// Component-wise absolute value.+B3_INLINE b3Vec3 b3Abs( b3Vec3 a )+{+	return B3_LITERAL( b3Vec3 ){+		b3AbsFloat( a.x ),+		b3AbsFloat( a.y ),+		b3AbsFloat( a.z ),+	};+}++/// Component-wise -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,+	};+}++/// Component-wise minimum value.+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 ),+	};+}++/// Component-wise maximum value.+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 ),+	};+}++/// Component-wise clamped value.+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;+}++/// Does the supplied quaternion have unit length?+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 );+}++/// Compute dot product of two quaternions. Useful for polarity tests.+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;+}++/// Multiply two quaternions.+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;+}++/// Compute a relative quaternion.+/// 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;+}++/// Quaternion conjugate (cheap inverse).+B3_INLINE b3Quat b3Conjugate( b3Quat q )+{+	return B3_LITERAL( b3Quat ){ { -q.v.x, -q.v.y, -q.v.z }, q.s };+}++/// Component-wise quaternion negation.+B3_INLINE b3Quat b3NegateQuat( b3Quat q )+{+	return B3_LITERAL( b3Quat ){ { -q.v.x, -q.v.y, -q.v.z }, -q.s };+}++/// Normalize a quaternion.+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;+}++/// Make a quaternion that is equivalent to rotating around an axis by a specified angle.+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;+}++/// Get the axis and angle from a quaternion. Assumes the quaternion is normalized.+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 );+}++/// Extract a quaternion from a rotation matrix.+B3_API b3Quat b3MakeQuatFromMatrix( const b3Matrix3* m );++/// Find a quaternion that rotates one vector to another.+B3_API b3Quat b3ComputeQuatBetweenUnitVectors( b3Vec3 v1, b3Vec3 v2 );++/// 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_FORCE_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;+}++/// Linearly interpolate and normalize between two quaternions+B3_INLINE b3Quat b3NLerp( b3Quat q1, b3Quat q2, float alpha )+{+	B3_VALIDATE( 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;+}++/// Get the inverse of a transform.+B3_INLINE b3Transform b3InvertTransform( b3Transform t )+{+	b3Transform out;+	out.p = b3InvRotateVector( t.q, b3Neg( t.p ) );+	out.q = b3Conjugate( t.q );+	return out;+}++/// Transform a point.+B3_FORCE_INLINE b3Vec3 b3TransformPoint( b3Transform t, b3Vec3 v )+{+	b3Vec3 rv = b3RotateVector( t.q, v );+	return b3Add( rv, t.p );+}++/// Inverse transform a point.+B3_INLINE b3Vec3 b3InvTransformPoint( b3Transform t, b3Vec3 v )+{+	return b3InvRotateVector( t.q, b3Sub( v, t.p ) );+}++// World position boundary. These cross between the double precision world space at the public+// boundary and the float interior. One set of bodies serves both modes: the typedefs collapse+// the types in float mode and the explicit float casts become no-ops.++/// Convert a vector to a world position.+B3_INLINE b3Pos b3ToPos( b3Vec3 v )+{+	return B3_LITERAL( b3Pos ){ v.x, v.y, v.z };+}++/// Lossy conversion of a world position to a float vector.+B3_INLINE b3Vec3 b3ToVec3( b3Pos p )+{+	return B3_LITERAL( b3Vec3 ){ (float)p.x, (float)p.y, (float)p.z };+}++/// Narrow a world coordinate to float, rounding toward negative infinity. Use with+/// b3RoundUpFloat to build a conservative float box that always contains the double bounds,+/// where plain rounding far from the origin could clip. nextafterf is an exact IEEE operation,+/// so this is cross-platform deterministic. With large world mode off this is a plain conversion.+B3_INLINE float b3RoundDownFloat( double x )+{+#if defined( BOX3D_DOUBLE_PRECISION )+	float f = (float)x;+	return (double)f > x ? nextafterf( f, -FLT_MAX ) : f;+#else+	return (float)x;+#endif+}++/// Narrow a world coordinate to float, rounding toward positive infinity.+B3_INLINE float b3RoundUpFloat( double x )+{+#if defined( BOX3D_DOUBLE_PRECISION )+	float f = (float)x;+	return (double)f < x ? nextafterf( f, FLT_MAX ) : f;+#else+	return (float)x;+#endif+}++/// a - b, demoted to float. The primary precision boundary operation.+B3_INLINE b3Vec3 b3SubPos( b3Pos a, b3Pos b )+{+	return B3_LITERAL( b3Vec3 ){ (float)( a.x - b.x ), (float)( a.y - b.y ), (float)( a.z - b.z ) };+}++/// p + d+B3_INLINE b3Pos b3OffsetPos( b3Pos p, b3Vec3 d )+{+	return B3_LITERAL( b3Pos ){ p.x + d.x, p.y + d.y, p.z + d.z };+}++/// World position interpolation for sweeps and sampling.+B3_INLINE b3Pos b3LerpPosition( b3Pos a, b3Pos b, float t )+{+	return B3_LITERAL( b3Pos ){+		( 1.0f - t ) * a.x + t * b.x,+		( 1.0f - t ) * a.y + t * b.y,+		( 1.0f - t ) * a.z + t * b.z,+	};+}++/// Transform a local point to a world position. Rotation in float, translation in double.+B3_INLINE b3Pos b3TransformWorldPoint( b3WorldTransform t, b3Vec3 p )+{+	b3Vec3 r = b3RotateVector( t.q, p );+	return B3_LITERAL( b3Pos ){ t.p.x + r.x, t.p.y + r.y, t.p.z + r.z };+}++/// Transform a world position to a local point. One double subtraction, then float.+B3_INLINE b3Vec3 b3InvTransformWorldPoint( b3WorldTransform t, b3Pos p )+{+	b3Vec3 d = { (float)( p.x - t.p.x ), (float)( p.y - t.p.y ), (float)( p.z - t.p.z ) };+	return b3InvRotateVector( t.q, d );+}++/// Compose a world transform with a local transform.+B3_FORCE_INLINE b3WorldTransform b3MulWorldTransforms( b3WorldTransform A, b3Transform B )+{+	b3WorldTransform C;+	C.q = b3MulQuat( A.q, B.q );+	b3Vec3 r = b3RotateVector( A.q, B.p );+	C.p = B3_LITERAL( b3Pos ){ A.p.x + r.x, A.p.y + r.y, A.p.z + r.z };+	return C;+}++/// Relative transform of frame B in frame A. The narrow phase boundary.+B3_FORCE_INLINE b3Transform b3InvMulWorldTransforms( b3WorldTransform A, b3WorldTransform B )+{+	b3Transform C;+	C.q = b3InvMulQuat( A.q, B.q );+	b3Vec3 d = { (float)( B.p.x - A.p.x ), (float)( B.p.y - A.p.y ), (float)( B.p.z - A.p.z ) };+	C.p = b3InvRotateVector( A.q, d );+	return C;+}++/// Shift a world transform into the frame of a base position.+B3_INLINE b3Transform b3ToRelativeTransform( b3WorldTransform t, b3Pos base )+{+	b3Transform r;+	r.q = t.q;+	r.p = B3_LITERAL( b3Vec3 ){ (float)( t.p.x - base.x ), (float)( t.p.y - base.y ), (float)( t.p.z - base.z ) };+	return r;+}++/// Promote a float transform to a world transform. Lossless.+B3_INLINE b3WorldTransform b3MakeWorldTransform( b3Transform t )+{+	b3WorldTransform w;+	w.p = b3ToPos( t.p );+	w.q = t.q;+	return w;+}++/// Translate a local AABB by a world origin, rounding outward so the float box always contains+/// the double box. Far from the origin a plain conversion could clip a shape out of its own box.+/// In float mode the origin is float and the rounding is a no-op.+B3_INLINE b3AABB b3OffsetAABB( b3AABB localBox, b3Pos origin )+{+	b3AABB out;+	out.lowerBound.x = b3RoundDownFloat( origin.x + localBox.lowerBound.x );+	out.lowerBound.y = b3RoundDownFloat( origin.y + localBox.lowerBound.y );+	out.lowerBound.z = b3RoundDownFloat( origin.z + localBox.lowerBound.z );+	out.upperBound.x = b3RoundUpFloat( origin.x + localBox.upperBound.x );+	out.upperBound.y = b3RoundUpFloat( origin.y + localBox.upperBound.y );+	out.upperBound.z = b3RoundUpFloat( origin.z + localBox.upperBound.z );+	return out;+}++/// Compute the determinant of a 3-by-3 matrix.+B3_INLINE float b3Det( b3Matrix3 m )+{+	return b3Dot( m.cx, b3Cross( m.cy, m.cz ) );+}++/// Multiply a matrix times a column vector.+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;+}++/// Negate a matrix.+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 },+	};+}++/// Matrix addition.+/// @return a + b+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 },+	};+}++/// Matrix subtraction.+/// @return a - b+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 },+	};+}++/// Multiply a matrix by a scalar, component-wise.+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 },+	};+}++/// Matrix multiplication.+/// @return a * b+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;+}++/// Matrix transpose.+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;+}++/// General matrix inverse.+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;+}++/// Solve a matrix equation.+/// @return inv(m) * a+B3_FORCE_INLINE b3Vec3 b3Solve3( b3Matrix3 m, b3Vec3 a )+{+	float det = b3Det( m );+	if ( b3AbsFloat( det ) > 1000.0f * FLT_MIN )+	{+		float invDet = 1.0f / det;+		b3Matrix3 s;+		s.cx = b3Cross( m.cy, m.cz );+		s.cy = b3Cross( m.cz, m.cx );+		s.cz = b3Cross( m.cx, m.cy );++		b3Vec3 b = {+			invDet * b3Dot( s.cx, a ),+			invDet * b3Dot( s.cy, a ),+			invDet * b3Dot( s.cz, a ),+		};++		return b;+	}++	return b3Vec3_zero;+}++/// Invert a matrix.+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;+}++/// Get the component-wise absolute value of a matrix.+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;+}++/// Make a matrix from a quaternion. This is useful if you need to+/// rotate many vectors.+/// The 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 ) },+	};+}++/// Get the inertia tensor of an offset point.+/// https://en.wikipedia.org/wiki/Parallel_axis_theorem+B3_API b3Matrix3 b3Steiner( float mass, b3Vec3 origin );++/// Get the AABB of a point cloud.+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;+}++/// Does a fully contain b?+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;+}++/// Get the surface area of an axis-aligned bounding box.+B3_INLINE float b3AABB_Area( b3AABB a )+{+	b3Vec3 delta = b3Sub( a.upperBound, a.lowerBound );+	return 2.0f * ( delta.x * delta.y + delta.y * delta.z + delta.z * delta.x );+}++/// Get the center of an axis-aligned bounding box.+B3_INLINE b3Vec3 b3AABB_Center( b3AABB a )+{+	return b3MulSV( 0.5f, b3Add( a.upperBound, a.lowerBound ) );+}++/// Get the extents (half-widths) of an axis-aligned bounding box.+B3_INLINE b3Vec3 b3AABB_Extents( b3AABB a )+{+	return b3MulSV( 0.5f, b3Sub( a.upperBound, a.lowerBound ) );+}++/// Get the union of two axis-aligned bounding boxes.+B3_INLINE b3AABB b3AABB_Union( b3AABB a, b3AABB b )+{+	b3AABB out;+	out.lowerBound = b3Min( a.lowerBound, b.lowerBound );+	out.upperBound = b3Max( a.upperBound, b.upperBound );+	return out;+}++/// Add uniform padding to an axis-aligned bounding box.+B3_INLINE b3AABB b3AABB_Inflate( b3AABB a, float extension )+{+	b3Vec3 radius = { extension, extension, extension };++	b3AABB out;+	out.lowerBound = b3Sub( a.lowerBound, radius );+	out.upperBound = b3Add( a.upperBound, radius );+	return out;+}++/// Do two axis-aligned boxes overlap?+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;+}++/// Transform an axis-aligned bounding box. This can create a larger box+/// than if you recomputed the AABB of the original shape with the transform+/// applied.+B3_FORCE_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;+}++/// Get the closest point on an axis-aligned bounding box.+B3_INLINE b3Vec3 b3ClosestPointToAABB( b3Vec3 point, b3AABB a )+{+	return b3Clamp( point, a.lowerBound, a.upperBound );+}++/// The closest points between to segments or infinite lines.+typedef struct b3SegmentDistanceResult+{+	b3Vec3 point1;+	float fraction1;+	b3Vec3 point2;+	float fraction2;+} b3SegmentDistanceResult;++/// Compute the closest point on the segment a-b to the target q.+B3_API b3Vec3 b3PointToSegmentDistance( b3Vec3 a, b3Vec3 b, b3Vec3 q );++/// Compute the closest points on two infinite lines.+B3_API b3SegmentDistanceResult b3LineDistance( b3Vec3 p1, b3Vec3 d1, b3Vec3 p2, b3Vec3 d2 );++/// Compute the closest points on two line segments.+B3_API b3SegmentDistanceResult b3SegmentDistance( b3Vec3 p1, b3Vec3 q1, b3Vec3 p2, b3Vec3 q2 );++/// 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 reasonably close to the origin? See B3_HUGE.+B3_API bool b3IsBoundedAABB( b3AABB a );++/// Is this AABB valid and reasonable?+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 );++/// Is this a valid world position? Not NaN or infinity.+B3_API bool b3IsValidPosition( b3Pos p );++/// Is this a valid world transform? Not NaN or infinity. Rotation is normalized.+B3_API bool b3IsValidWorldTransform( b3WorldTransform t );++/**@}*/ // math++/**+ * @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++/// Vector addition.+B3_FORCE_INLINE b3Vec3& operator+=( b3Vec3& a, b3Vec3 b )+{+	a.x += b.x;+	a.y += b.y;+	a.z += b.z;+	return a;+}++/// Vector subtraction.+B3_FORCE_INLINE b3Vec3& operator-=( b3Vec3& a, b3Vec3 b )+{+	a.x -= b.x;+	a.y -= b.y;+	a.z -= b.z;+	return a;+}++/// Vector scaling.+B3_FORCE_INLINE b3Vec3& operator*=( b3Vec3& a, float s )+{+	a.x *= s;+	a.y *= s;+	a.z *= s;+	return a;+}++/// Vector negation.+B3_FORCE_INLINE b3Vec3 operator-( b3Vec3 a )+{+	return { -a.x, -a.y, -a.z };+}++/// Vector scaling.+B3_FORCE_INLINE b3Vec3 operator*( float s, b3Vec3 a )+{+	return { s * a.x, s * a.y, s * a.z };+}++/// Vector scaling.+B3_FORCE_INLINE b3Vec3 operator*( b3Vec3 a, float s )+{+	return { s * a.x, s * a.y, s * a.z };+}++/// Component-wise vector multiplication.+B3_FORCE_INLINE b3Vec3 operator*( b3Vec3 a, b3Vec3 b )+{+	return { a.x * b.x, a.y * b.y, a.z * b.z };+}++/// Vector addition.+B3_FORCE_INLINE b3Vec3 operator+( b3Vec3 a, b3Vec3 b )+{+	return { a.x + b.x, a.y + b.y, a.z + b.z };+}++/// Vector subtraction.+B3_FORCE_INLINE b3Vec3 operator-( b3Vec3 a, b3Vec3 b )+{+	return { a.x - b.x, a.y - b.y, a.z - b.z };+}++#if defined( BOX3D_DOUBLE_PRECISION )++/// Offset a world position by a vector.+B3_FORCE_INLINE b3Pos operator+( b3Pos a, b3Vec3 b )+{+	return { a.x + b.x, a.y + b.y, a.z + b.z };+}++/// Offset a world position by a vector.+B3_FORCE_INLINE b3Pos operator-( b3Pos a, b3Vec3 b )+{+	return { a.x - b.x, a.y - b.y, a.z - b.z };+}++/// Delta between two world positions, demoted to float.+B3_FORCE_INLINE b3Vec3 operator-( b3Pos a, b3Pos b )+{+	return { (float)( a.x - b.x ), (float)( a.y - b.y ), (float)( a.z - b.z ) };+}++#endif++#endif++/**@}*/ // math_cpp
+ upstream/include/box3d/types.h view
@@ -0,0 +1,3061 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "base.h"+#include "constants.h"+#include "id.h"+#include "math_functions.h"++#include <stdint.h>++#define B3_DEFAULT_CATEGORY_BITS UINT64_MAX+#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. This must block until the task has completed.+/// The step blocks here on the tasks it spawned, so b3World_Step holds its stack across every+/// fork/join. Drive it from a thread you can dedicate to the step, or from a fiber this callback can+/// park to free the underlying thread. In a job system that cannot park a job's stack, do not call+/// b3World_Step from inside a job: a job that blocks on its own sub-jobs without yielding its thread+/// can deadlock. The in-tree scheduler instead runs other pending tasks on the waiting thread.+/// @ingroup world+typedef void b3FinishTaskCallback( void* userTask, void* userContext );++typedef struct b3DebugShape b3DebugShape;++/// 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.+/// @ingroup debug_draw+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.+/// @ingroup world+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.+/// @ingroup world+typedef float b3RestitutionCallback( float restitutionA, uint64_t userMaterialIdA, float restitutionB, uint64_t userMaterialIdB );++/// 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, b3Pos point, b3Vec3 normal, void* context );++/// Prototype callback for overlap queries.+/// Called for each shape found in the query.+/// @see b3World_OverlapAABB+/// @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, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId,+							   int triangleIndex, int childIndex, void* context );++/// Optional world capacities that can be use to avoid run-time allocations+/// @ingroup world+typedef struct b3Capacity+{+	/// Number of expected static shapes.+	int staticShapeCount;++	/// Number of expected dynamic and kinematic shapes.+	int dynamicShapeCount;++	/// Number of expected static bodies.+	int staticBodyCount;++	/// Number of expected dynamic and kinematic bodies.+	int dynamicBodyCount;++	/// Number of expected contacts.+	int contactCount;+} b3Capacity;++/// World definition used to create a simulation world. Must be initialized using b3DefaultWorldDef.+/// @ingroup world+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.+	/// This is clamped to the range [1, B3_MAX_WORKERS]. Using a value above 1 will turn on multithreading.+	/// If task callbacks are provided then Box3D will use the user provided task system. Otherwise Box3D+	/// will create threads and use an internal scheduler.+	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+/// @ingroup body+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.+	b3Pos 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 used to 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.+	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 a 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. They do not guarantee accurate collision if both bodies are fast moving because+	/// the bullet does a continuous check after all non-bullet bodies have moved. You could get unlucky and have+	/// the bullet body end a time step very close to a non-bullet body and the non-bullet body then moves over+	/// the bullet body. In continuous collision, initial overlap is ignored to avoid freezing bodies in place.+	/// I do not recommend using them for game projectiles if precise collision timing is needed. Instead consider+	/// using a ray or shape cast. You can use a marching ray or shape cast for projectile that moves over time.+	/// If you want a fast moving projectile to collide with a fast moving target, you need to consider the relative+	/// movement in your ray or shape cast. This is out of the scope of Box3D.+	/// So what are good use cases for bullets? Pinball games or games with dynamic containers that hold other objects.+	/// It should be a use case where it doesn't break the game if there is a collision missed, but having them+	/// captured improves the quality of the game.+	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 contact recycling. True by default. Leaving this enabled improves performance+	/// but may lead to ghost collision that should be avoided on characters.+	bool enableContactRecycling;++	/// 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 );++/// 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. The low 24 bits are RGB. The high byte may+	/// carry a b3DebugMaterial preset, see b3MakeDebugColor.+	/// @see b3HexColor+	uint32_t customColor;++	/// Explicit padding. Must be zero.+	uint32_t padding;+} b3SurfaceMaterial;++/// Use this to initialize your surface material+/// @ingroup shape+B3_API b3SurfaceMaterial b3DefaultSurfaceMaterial( void );++/// Shape type+/// @ingroup shape+typedef enum b3ShapeType+{+	/// A capsule is an extruded sphere+	b3_capsuleShape,++	/// A baked compound shape composed of 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+/// @ingroup shape+typedef struct b3ShapeDef+{+	/// Optional shape name for debugging+	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^3.+	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 b3WorldDef.+	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.+	/// Only convex shapes may act as sensor visitors.+	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.+	/// Warning: if this is false, you MUST call b3Body_ApplyMassFromShapes or b3Body_SetMassData before simulating the world.+	bool updateBodyMass;++	/// Enable speculative collision. Leave this true unless you care about reducing ghost collision+	/// more than continuous collision under rotation.+	/// Experimental: this can only disable speculative contact between hulls and triangles (meshes and height fields).+	bool enableSpeculativeContact;++	/// 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 );++/// Profiling data. Times are in milliseconds.+/// @ingroup world+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.+/// @ingroup world+typedef struct b3Counters+{+	int bodyCount;+	int shapeCount;+	int contactCount;+	int jointCount;+	int islandCount;+	int stackUsed;+	int arenaCapacity;+	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;++	/// Maximum number of time of impact iterations+	int distanceIterations;+	int pushBackIterations;+	int rootIterations;+} b3Counters;++/// 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.+/// @ingroup joint+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 can 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 parallel_joint+typedef struct b3ParallelJointDef+{+	/// Base joint definition+	b3JointDef base;++	/// The spring stiffness Hertz, cycles per second+	float hertz;++	/// The spring damping ratio, non-dimensional+	float dampingRatio;++	/// The maximum spring torque, typically in newton-meters.+	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+/// @ingroup 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. Minimum of -0.99*pi radians.+	float lowerAngle;++	/// The upper angle for the joint limit in radians. Maximum of 0.99*pi 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. Minimum of -0.99*pi radians.+	float lowerTwistAngle;++	/// The angle for the upper twist limit in radians. Maximum of 0.99*pi radians.+	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 translates 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+	b3Pos 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 event 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 b3Shape_IsValid+	b3ShapeId sensorShapeId;++	/// The id of the shape that stopped touching the sensor shape+	///	@warning this shape may have been destroyed+	///	@see b3Shape_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 may be destroyed automatically when the world is modified or simulated.+	/// Use 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.+	b3Pos 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+{+	/// The body user data.+	void* userData;++	/// The body transform.+	b3WorldTransform transform;++	/// The body id.+	b3BodyId bodyId;++	/// Did the body fall asleep this time step?+	bool fellAsleep;+} b3BodyMoveEvent;++/// Body events are buffered in the world and are available+///	as event arrays after the time step is complete.+///	Note: this data 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+{+	/// The contact id. You may hold onto this to track a contact across time steps.+	/// This id may become orphaned. Use b3Contact_IsValid before using it for other functions.+	b3ContactId contactId;++	/// The first shape id.+	b3ShapeId shapeIdA;++	/// The second shape id.+	b3ShapeId shapeIdB;++	/// The contact manifold. This points to internal data and may become invalid. Do not store+	/// this pointer.+	const struct b3Manifold* manifolds;++	/// The number of contact manifolds. For mesh and height-field collision there can be multiple manifolds.+	int manifoldCount;+} b3ContactData;++/**@}*/ // event++/**+ * @defgroup query Query+ * @brief Query types and functions+ *+ * Queries include ray casts, shapes casts, overlap, distance, and time of impact.+ * @{+ */++/// 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.+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;++	/// Optional id combined with @ref name to identify this query in a recording, e.g. an entity id.+	/// Need not be unique on its own. 0 with a null name means untagged. Ignored when not recording.+	uint64_t id;++	/// Optional label combined with @ref id to identify this query, e.g. "bullet". Need not be unique+	/// on its own. The recorder hashes (id, name) into one stable key the viewer tracks the query by,+	/// so the same id and name pair identifies the same query across frames. NULL means none. Ignored+	/// when not recording.+	const char* name;+} b3QueryFilter;++/// Use this to initialize your query filter+B3_API b3QueryFilter b3DefaultQueryFilter( void );++/// Low level ray cast input data.+typedef struct b3RayCastInput+{+	/// Start point of the ray cast.+	b3Vec3 origin;++	/// Translation of the ray cast.+	/// end = start + translation.+	b3Vec3 translation;++	/// The maximum fraction of the translation to consider, typically 1+	float maxFraction;+} b3RayCastInput;++/// Result from b3World_RayCastClosest.+typedef struct b3RayResult+{+	/// The shape hit.+	b3ShapeId shapeId;++	/// The world point of the hit.+	b3Pos point;++	/// The world normal of the shape surface at the hit point.+	b3Vec3 normal;++	/// The user material id at the hit point. This can be per triangle+	/// if the shape is a mesh, height-field, or compound with child mesh.+	uint64_t userMaterialId;++	/// The fraction of the input ray.+	float fraction;++	/// The triangle index if the shape is a mesh, height-field, or compound with+	/// child mesh.+	int triangleIndex;++	/// The child index if the shape is a compound.+	int childIndex;++	/// The number of BVH nodes visited. Diagnostic.+	int nodeVisits;++	/// The number of BVH leaves visited. Diagnostic.+	int leafVisits;++	/// Did the ray hit? If false, all other data is invalid.+	bool hit;+} b3RayResult;++/// 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;++/// Input for sweeping an AABB through a dynamic tree. The box is in the tree's world float frame.+/// The caller folds the cast shape radius and any world origin into the box, so the tree traversal+/// stays a conservative box sweep and the precise narrow phase happens per shape in the callback.+typedef struct b3BoxCastInput+{+	/// The AABB to cast, in the tree's frame.+	b3AABB box;++	/// The sweep translation.+	b3Vec3 translation;++	/// The maximum fraction of the translation to consider, typically 1.+	float maxFraction;+} b3BoxCastInput;++/// 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;++#if defined( BOX3D_DOUBLE_PRECISION )++/// Ray cast or shape-cast output in world space. The hit point is a world position so the result+/// stays precise far from the world origin. Mirrors b3CastOutput with a double precision point.+typedef struct b3WorldCastOutput+{+	/// The surface normal at the hit point.+	b3Vec3 normal;++	/// The surface hit point in world space.+	b3Pos 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;+} b3WorldCastOutput;++#else++/// Same type in single precision.+typedef b3CastOutput b3WorldCastOutput;++#endif++/// Body cast result for ray and shape casts.+typedef struct b3BodyCastResult+{+	/// The shape hit.+	b3ShapeId shapeId;++	/// The world point on the shape surface.+	b3Pos point;++	/// The world normal vector on the shape surface.+	b3Vec3 normal;++	/// The fraction along the ray hit.+	/// hit point = origin + fraction * translation+	float fraction;++	/// The triangle index if the shape is a mesh or height-field.+	int triangleIndex;++	/// The user material id at the hit point. This can be per triangle+	/// if the shape is a mesh, height-field, or compound with child mesh.+	uint64_t userMaterialId;++	/// The number of iterations used. Diagnostic.+	int iterations;++	/// Did the cast hit? If false, all other fields are invalid.+	bool hit;+} b3BodyCastResult;++/// 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 parameters for b3ShapeCast+typedef struct b3ShapeCastPairInput+{+	b3ShapeProxy proxyA;   ///< The proxy for shape A+	b3ShapeProxy proxyB;   ///< The proxy for shape B+	b3Transform transform; ///< Transform of shape B in shape A's frame, the relative pose B in A+	b3Vec3 translationB;   ///< The translation of shape B, in A's frame+	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;++/// Input for b3ShapeDistance+typedef struct b3DistanceInput+{+	/// The proxy for shape A+	b3ShapeProxy proxyA;++	/// The proxy for shape B+	b3ShapeProxy proxyB;++	/// Transform of shape B in shape A's frame, the relative pose B in A+	/// (b3InvMulWorldTransforms( worldA, worldB )). The query is origin independent and runs in frame A.+	b3Transform transform;++	/// Should the proxy radius be considered?+	bool useRadii;+} b3DistanceInput;++/// Output for b3ShapeDistance+typedef struct b3DistanceOutput+{+	b3Vec3 pointA;	  ///< Closest point on shapeA, in shape A's frame+	b3Vec3 pointB;	  ///< Closest point on shapeB, in shape A's frame+	b3Vec3 normal;	  ///< A to B normal in shape A's frame. Invalid if distance is zero.+	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;++/// 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;++/// 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;++	/// Indicates that the time of impact detected initial+	/// overlap and used a fallback sphere as a last ditch effort+	/// to prevent tunneling.+	bool usedFallback;+} b3TOIOutput;++/**@}*/ // query++/**+ * @defgroup tree Dynamic Tree+ * The dynamic tree is a binary AABB tree to organize and query large numbers of geometric objects+ *+ * Box3D 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 Box3D 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.+ * @{+ */++/// Flags for tree nodes. For internal usage.+typedef enum b3TreeNodeFlags+{+	b3_allocatedNode = 0x0001,+	b3_enlargedNode = 0x0002,+	b3_leafNode = 0x0004,+} b3TreeNodeFlags;++/// Tree node child indices. For internal usage.+typedef struct b3TreeNodeChildren+{+	int child1; ///< child node index 1+	int child2; ///< child node index 2+} 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++	/// @see b3TreeNodeFlags+	uint16_t flags; // 2+} b3TreeNode;++/// Dynamic tree version for compatibility testing.+#define B3_DYNAMIC_TREE_VERSION 0x93EDAF889FD30B4Aull++/// The dynamic tree structure. This should be considered private data.+/// It is placed here for performance reasons.+typedef struct b3DynamicTree+{+	/// The dynamic tree version. Always the first field. Useful+	/// if the tree is serialized.+	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;++/// 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 );++/// 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 );++/// This function receives clipped AABB cast input for a proxy. The function returns the new cast+/// fraction.+/// - return a value of 0 to terminate the cast+/// - return a value less than input->maxFraction to clip the cast+/// - return a value of input->maxFraction to continue the cast without clipping+typedef float b3TreeBoxCastCallbackFcn( const b3BoxCastInput* input, int proxyId, uint64_t userData, 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 b3TreeRayCastCallbackFcn( const b3RayCastInput* input, int proxyId, uint64_t userData, void* context );++/**@}*/ // tree++/**+ * @defgroup character Character Mover+ * Character movement solver+ * @{+ */++/// The plane between a character mover and a shape+typedef struct b3PlaneResult+{+	/// Outward pointing plane.+	b3Plane plane;++	/// Closest point on the shape. May not be unique.+	b3Vec3 point;++} b3PlaneResult;++/// These are collision planes that can be fed to b3SolvePlanes. Normally+/// this is assembled by the user from plane results in b3PlaneResult.+typedef struct b3CollisionPlane+{+	/// The collision plane between the mover and some shape.+	b3Plane plane;++	/// Setting this to FLT_MAX makes the plane as rigid as possible. Lower values can+	/// make the plane collision soft. Usually in meters.+	float pushLimit;++	/// The push on the mover determined by b3SolvePlanes. Usually in meters.+	float push;++	/// Indicates if b3ClipVector should clip against this plane. Should be false for soft collision.+	bool clipVelocity;+} b3CollisionPlane;++/// Result returned by b3SolvePlanes.+typedef struct b3PlaneSolverResult+{+	/// The final relative translation.+	b3Vec3 delta;++	/// The number of iterations used by the plane solver. For diagnostics.+	int iterationCount;+} b3PlaneSolverResult;++/// Body plane result for movers.+typedef struct b3BodyPlaneResult+{+	/// The shape id on the body.+	b3ShapeId shapeId;++	/// The plane result.+	b3PlaneResult result;+} b3BodyPlaneResult;++/// 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 );++/**@}*/ // mover++/**+ * @defgroup geometry Geometry+ * @brief Geometry types and algorithms+ *+ * Definitions of spheres, capsules, hulls, meshes, height fields, and compounds.+ * @{+ */++/// This holds the mass data computed for a shape.+typedef struct b3MassData+{+	/// The shape mass+	float mass;++	/// The local center of mass position.+	b3Vec3 center;++	/// The inertia tensor about the shape center of mass.+	b3Matrix3 inertia;+} b3MassData;++/**+ * @defgroup sphere Sphere+ * @brief Sphere primitive+ * @{+ */++/// A solid sphere+typedef struct b3Sphere+{+	/// The local center+	b3Vec3 center;++	/// The radius+	float radius;+} b3Sphere;++/**@}*/ // sphere++/**+ * @defgroup capsule Capsule+ * @brief Capsule primitive+ * @{+ */++/// 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;++/**@}*/ // capsule++/**+ * @defgroup hull Convex Hull+ * @brief Convex hull primitive+ * @{+ */++/// A hull vertex. Identified by a half-edge with this vertex as its tail.+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;++/// A hull face. Hulls use a half-edge data structure, so a face+/// can be determined from a single half-edge index.+typedef struct b3HullFace+{+	/// An arbitrary half-edge on this face+	uint8_t edge;+} b3HullFace;++/// 64-bit hull version. Useful for validating serialized data.+#define B3_HULL_VERSION 0x4A4C9587DE57485Cull++/// A convex hull.+/// @note This data structure has data hanging off the end and cannot be directly copied.+typedef struct b3HullData+{+	/// Version must be first and match B3_HULL_VERSION+	uint64_t version;++	/// Hash of this hull (this field is zero when the hash is computed).+	uint64_t hash;++	/// Axis-aligned box in local space.+	b3AABB aabb;++	/// Surface area, typically in squared meters.+	float surfaceArea;++	/// Volume, typically in m^3.+	float volume;++	/// The radius of the largest sphere at the center.+	float innerRadius;++	/// The local centroid+	b3Vec3 center;++	/// The inertia tensor about the centroid.+	b3Matrix3 centralInertia;++	/// The vertex count.+	int32_t vertexCount;++	/// Offset of the vertex array in bytes from the struct address.+	int32_t vertexOffset;++	/// Offset of the point array in bytes from the struct address.+	int32_t pointOffset;++	/// This is the half-edge count (double the edge count)+	int32_t edgeCount;++	/// Offset of the edge array in bytes from the struct address.+	int32_t edgeOffset;++	/// The face count. Hulls faces are convex polygons.+	int32_t faceCount;++	/// Offset of the face plane array in bytes from the struct address.+	int32_t planeOffset;++	/// Offset of the face array in bytes from the struct address.+	int32_t faceOffset;++	/// Offset of structure of array (SOA) vertices+	int32_t soaVertexOffset;++	/// Offset of structure of array (SOA) unit normal vectors+	int32_t soaNormalOffset;++	/// The total number of bytes for this hull.+	int32_t byteCount;++	/// Any padding must be explicit.+} b3HullData;++/// Efficient box hull+typedef struct b3BoxHull+{+	/// The embedded hull. So the offsets index into the arrays that follow.+	b3HullData base;+	b3HullVertex boxVertices[8]; ///< Box vertices.+	b3Vec3 boxPoints[8];		 ///< Box points.+	b3HullHalfEdge boxEdges[24]; ///< Box half-edges.+	b3Plane boxPlanes[6];		 ///< Box face planes.+	b3HullFace boxFaces[6];		 ///< Box faces.+	uint8_t padding[2];			 ///< Explicit padding.+	float vx[8];				 ///< vertex x+	float vy[8];				 ///< vertex y+	float vz[8];				 ///< vertex z+	float nx[8];				 ///< normal x, padded to multiple of 4+	float ny[8];				 ///< normal y, padded to multiple of 4+	float nz[8];				 ///< normal z, padded to multiple of 4+} b3BoxHull;++/**@}*/ // hull++/**+ * @defgroup mesh Triangle Mesh+ * @brief Triangle mesh collision shape+ * @{+ */++/// This is used to create a re-usable collision mesh. No pointers+/// are held to this data in b3MeshData. So all this data can be temporary.+typedef struct b3MeshDef+{+	/// Triangle vertices.+	b3Vec3* vertices;++	/// Triangle vertex indices. 3 for each triangle. CCW winding.+	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;++	/// The vertex count. Must be 3 or more.+	int vertexCount;++	/// The triangle count. Must be 1 or more.+	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;++/// 64-bit mesh version. Useful for validating serialized data.+#define B3_MESH_VERSION 0xAAAB9A00F1A8AAF7ull++/// Triangle mesh edge flags.+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;++/// A mesh triangle.+typedef struct b3MeshTriangle+{+	int32_t index1; ///< Index of vertex 1.+	int32_t index2; ///< Index of vertex 2.+	int32_t index3; ///< Index of vertex 3.+} b3MeshTriangle;++/// A mesh BVH node.+typedef struct b3MeshNode+{+	/// The lower bound of the node AABB. Strategic placement for SIMD.+	b3Vec3 lowerBound;++	/// Anonymous union.+	union+	{+		/// Internal node+		struct+		{+			/// Split axis. 0, 1, or 2.+			uint32_t axis : 2;+			/// Offset of the second child node.+			uint32_t childOffset : 30;+		} asNode;++		/// Leaf node+		struct+		{+			/// Aligned with axis above and has value of 3 if this is a leaf.+			uint32_t type : 2;++			/// The number of triangles for this leaf node.+			uint32_t triangleCount : 30;+		} asLeaf;+	} data;++	/// The upper bound of the node AABB.  Strategic placement for SIMD.+	b3Vec3 upperBound;++	/// The index of the leaf triangles.+	uint32_t triangleOffset;+} b3MeshNode;++/// This is a sorted triangle collision bounding volume hierarchy.+/// @note This struct has data hanging off the end and cannot be directly copied.+typedef struct b3MeshData+{+	/// Version must be first.+	uint64_t version;++	/// Hash of this mesh (this field is zero when the hash is computed)+	uint64_t hash;++	/// The total number of bytes for this mesh.+	int32_t byteCount;++	/// Local axis-aligned box.+	b3AABB bounds;++	/// Combined surface area of all triangles. Single-sided.+	float surfaceArea;++	/// The height of the bounding volume hierarchy.+	int32_t treeHeight;++	/// The number of degenerate triangles. Diagnostic.+	int32_t degenerateCount;++	/// Offset of the node array in bytes from the struct address.+	int32_t nodeOffset;++	/// The number of BVH nodes.+	int32_t nodeCount;++	/// Offset of the vertex array in bytes from the struct address.+	int32_t vertexOffset;++	/// The number of vertices.+	int32_t vertexCount;++	/// Offset of the triangle array in bytes from the struct address.+	int32_t triangleOffset;++	/// The number of triangles.+	int32_t triangleCount;++	/// Offset of the material array in bytes from the struct address.+	int32_t materialOffset;++	/// The number of materials.+	int32_t materialCount;++	/// Offset of the triangle flag array in bytes from the struct address.+	int32_t flagsOffset;++	/// Explicit padding.+	int32_t padding;+} b3MeshData;++/// This allows mesh data to be re-used with different scales.+typedef struct b3Mesh+{+	/// Immutable pointer to the mesh data.+	const b3MeshData* data;++	/// This scale may be non-uniform and have negative components. However,+	/// no component may be very small in magnitude.+	b3Vec3 scale;+} b3Mesh;++/**@}*/ // mesh++/**+ * @defgroup height_field Height Field+ * @brief Height field collision shape+ * @{+ */++/// Data used to create a height field. No pointers are held to this data.+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;++	/// The height field scale. All components must be positive values.+	b3Vec3 scale;++	/// The number of grid lines along the x-axis.+	int countX;++	/// The number of grid lines along the z-axis.+	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;++	/// The maximum.+	float globalMaximumHeight;++	/// Use clock-wise winding. This effectively inverts the height-field along the y-axis.+	bool clockwiseWinding;+} b3HeightFieldDef;++/// This material index is used to designate holes in a height field.+#define B3_HEIGHT_FIELD_HOLE 0xFF++/// 64-bit height-field version. Useful for validating serialized data.+#define B3_HEIGHT_FIELD_VERSION 0x8E41E5FB084848F8ull++/// A height field with compressed storage.+/// @note This data structure has data hanging off the end and cannot be directly copied.+typedef struct b3HeightFieldData+{+	/// Version must be first and match B3_HEIGHT_FIELD_VERSION+	uint64_t version;++	/// Hash of this height field (this field is zero when the hash is computed).+	uint64_t hash;++	/// The total number of bytes for this height field.+	int byteCount;++	/// The local axis-aligned bounding box.+	b3AABB aabb;++	/// The minimum y value.+	float minHeight;++	/// The maximum y value+	float maxHeight;++	/// The quantization scale.+	float heightScale;++	/// The overall scale.+	b3Vec3 scale;++	/// The number of grid columns along the local x-axis.+	int32_t columnCount;++	/// The number of grid rows along the local z-axis.+	int32_t rowCount;++	/// Offset of the compressed height array in bytes from the struct address.+	/// uint16_t, one per grid point.+	int32_t heightsOffset;++	/// Offset of the material index array in bytes from the struct address.+	/// uint8_t, one per cell.+	int32_t materialOffset;++	/// Offset of the flag array in bytes from the struct address.+	/// uint8_t, one per triangle.+	int32_t flagsOffset;++	/// Triangle winding.+	uint8_t clockwise;++	/// Explicit padding.+	uint8_t padding[7];+} b3HeightFieldData;++/**@}*/ // height_field++/**+ * @defgroup compound Compound+ * @brief Compound collision shape+ * @{+ */++/// Definition for a capsule in a compound shape.+typedef struct b3CompoundCapsuleDef+{+	/// Local capsule.+	b3Capsule capsule;++	/// Material properties.+	b3SurfaceMaterial material;+} b3CompoundCapsuleDef;++/// Definition for a convex hull in a compound shape.+typedef struct b3CompoundHullDef+{+	/// Shared hull.+	const b3HullData* hull;++	/// Transform of the shared hull into compound local space.+	b3Transform transform;++	/// Material properties.+	b3SurfaceMaterial material;+} b3CompoundHullDef;++/// Definition for a triangle mesh in a compound shape.+typedef struct b3CompoundMeshDef+{+	/// Shared mesh.+	const b3MeshData* meshData;++	/// Transform of the shared mesh into compound local space.+	b3Transform transform;++	/// Local space non-uniform mesh scale. May have negative components.+	b3Vec3 scale;++	/// Material properties.+	/// This array must line up with the material indices on the triangles.+	const b3SurfaceMaterial* materials;++	/// Number of materials.+	int materialCount;+} b3CompoundMeshDef;++/// Definition for a sphere in a compound shape.+typedef struct b3CompoundSphereDef+{+	/// Local sphere.+	b3Sphere sphere;++	/// Material properties.+	b3SurfaceMaterial material;+} b3CompoundSphereDef;++/// Definition for creating a compound shape. All this data is fully cloned+/// into the run-time compound shape.+typedef struct b3CompoundDef+{+	/// Capsule instances.+	b3CompoundCapsuleDef* capsules;++	/// Number of capsules.+	int capsuleCount;++	/// Hulls instances.+	b3CompoundHullDef* hulls;++	/// Number of hull instances.+	int hullCount;++	/// Mesh instances.+	b3CompoundMeshDef* meshes;++	/// Number of mesh instances.+	int meshCount;++	/// Sphere instances.+	b3CompoundSphereDef* spheres;++	/// Number of spheres.+	int sphereCount;+} b3CompoundDef;++/// The baked compound version depends on the tree, mesh, and hull versions.+#define B3_COMPOUND_VERSION ( 0xB11DCE70FAD5622Bull ^ 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 compound.+#define B3_MAX_COMPOUND_MESH_MATERIALS 4++/// The data for a baked compound shape. This is a potentially large yet highly optimized+/// data structure. It can contain thousands of child shapes, yet at runtime it populates+/// into the world as a single shape in the runtime broad-phase.+/// This data structure has data living off the end and must be accessed using offsets.+/// Accessors are provided for user relevant data.+/// Note: you don't need to use this to create runtime compounds. For runtime compounds you can+/// add multiple shapes to a body using the regular shape creation functions.+typedef struct b3CompoundData+{+	/// The compound version is always first.+	uint64_t version;++	/// The total number of bytes for this compound.+	int byteCount;++	/// Offset of the tree node array in bytes from the struct address.+	int nodeOffset;++	/// Immutable dynamic tree. The tree node pointer must be fixed up using the node offset+	b3DynamicTree tree;++	/// Offset of the material array in bytes from the struct address.+	int materialOffset;++	/// The number of materials.+	int materialCount;++	/// Offset of the capsule array in bytes from the struct address.+	int capsuleOffset;++	/// The number of capsules.+	int capsuleCount;++	/// Offset of the hull instance array in bytes from the struct address.+	int hullOffset;++	/// The number of hull instances.+	int hullCount;++	/// The number of unique hulls. Diagnostic.+	int sharedHullCount;++	/// Offset of the mesh instance array in bytes from the struct address.+	int meshOffset;++	/// The number of mesh instances.+	int meshCount;++	/// The number of unique meshes. Diagnostic.+	int sharedMeshCount;++	/// Offset of the sphere array in bytes from the struct address.+	int sphereOffset;++	/// The number of spheres.+	int sphereCount;+} b3CompoundData;++/// A capsule that lives in a compound.+typedef struct b3CompoundCapsule+{+	/// Local capsule.+	b3Capsule capsule;++	/// Index to a shared material.+	int materialIndex;+} b3CompoundCapsule;++/// A hull that lives in a compound.+typedef struct b3CompoundHull+{+	/// Pointer to the unique shared hull.+	const b3HullData* hull;++	/// The transform of this hull instance.+	b3Transform transform;++	/// Index to a shared material.+	int materialIndex;+} b3CompoundHull;++/// A mesh with non-uniform scale that lives in a compound.+typedef struct b3CompoundMesh+{+	/// Pointer to the unique shared mesh.+	const b3MeshData* meshData;++	/// The transform of this mesh instance.+	b3Transform transform;++	/// Non-uniform scale of this mesh instance.+	b3Vec3 scale;++	/// This is used to access the surface material from b3GetCompoundMaterials.+	/// Requires an extra level of indirection. The triangle material index+	/// is clamped to B3_MAX_COMPOUND_MESH_MATERIALS.+	/// materialIndex = materialIndices[triangle->materialIndex]+	int materialIndices[B3_MAX_COMPOUND_MESH_MATERIALS];+} b3CompoundMesh;++/// A sphere that lives in a compound.+typedef struct b3CompoundSphere+{+	/// Local sphere.+	b3Sphere sphere;++	/// Index to a shared material.+	int materialIndex;+} b3CompoundSphere;++/// Child shape of a compound+typedef struct b3ChildShape+{+	/// Tagged union.+	union+	{+		b3Capsule capsule;		///< Capsule.+		const b3HullData* hull; ///< Hull.+		b3Mesh mesh;			///< Mesh.+		b3Sphere sphere;		///< Sphere.+	};++	/// Transform of the shape into compound local space.+	b3Transform transform;++	/// Material indices. Index 0 is used for convex shapes.+	/// todo limit to 64K?+	int materialIndices[B3_MAX_COMPOUND_MESH_MATERIALS];++	/// The shape type (union tag).+	b3ShapeType type;+} b3ChildShape;++/// Callback for compound overlap queries.+typedef bool b3CompoundQueryFcn( const b3CompoundData* compound, int childIndex, void* context );++/**@}*/ // compound++/**@}*/ // geometry++/**+ * @defgroup collision Shape Collision+ * Collide pairs of shapes.+ * @{+ */++/// 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 in the previous step?+	bool persisted;+} b3ManifoldPoint;++/// 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 shape B+	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 contact points, will be 0 to 4+	int pointCount;++} b3Manifold;++/// Cached separating axis feature.+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;++/// Cached triangle feature.+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;++/// Separating axis test cache. Provides temporal acceleration of collision routines.+typedef struct+{+	/// The separation when the cache is populated. Negative for overlap.+	float separation;++	/// b3SeparatingFeature.+	uint8_t type;++	/// Index of the feature on shape A.+	uint8_t indexA;++	/// Index of the feature on shape B.+	uint8_t indexB;++	/// Was the cache re-used?+	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+{+	/// Local point in frame A.+	b3Vec3 point;++	/// The contact point separation. Negative for overlap.+	float separation;++	/// The feature pair for this point.+	b3FeaturePair pair;++	/// The triangle index when collide with a mesh or height-field.+	int triangleIndex;+} b3LocalManifoldPoint;++/// A local manifold with no dynamic information. Used by b3Collide functions.+typedef struct b3LocalManifold+{+	/// Local normal in frame A.+	b3Vec3 normal;++	/// The triangle normal.+	b3Vec3 triangleNormal;++	/// The manifold points. From a point buffer.+	b3LocalManifoldPoint* points;++	/// The number of manifold points. Only bounded by the buffer capacity.+	int pointCount;++	/// The index of the triangle.+	int triangleIndex;++	int i1; ///< Vertex 1 index.+	int i2; ///< Vertex 2 index.+	int i3; ///< Vertex 3 index.++	/// The squared distance of a sphere from a triangle. For ghost collision reduction.+	float squaredDistance;++	/// The triangle feature involved.+	b3TriangleFeature feature;++	/// b3MeshEdgeFlags.+	int triangleFlags;+} b3LocalManifold;++/**@}*/ // collision++/**+ * @defgroup debug_draw Debug Draw+ * @{+ */++/// 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;++/// Debug draw material preset. Optionally packed into the unused high byte of a+/// b3HexColor (or b3SurfaceMaterial::customColor) to drive the renderer's PBR+/// roughness and metalness. The low 24 bits stay RGB, so a plain 0xRRGGBB color+/// reads as b3_debugMaterialDefault and keeps the renderer's per-body-type look.+typedef enum b3DebugMaterial+{+	b3_debugMaterialDefault = 0,+	b3_debugMaterialMatte,+	b3_debugMaterialSoft,+	b3_debugMaterialDead,+	b3_debugMaterialGlossy,+	b3_debugMaterialMetallic+} b3DebugMaterial;++/// Pack an RGB color with a material preset for debug draw. The preset rides in+/// the high byte where the color converters ignore it.+B3_INLINE uint32_t b3MakeDebugColor( b3HexColor rgb, b3DebugMaterial material )+{+	return ( (uint32_t)rgb & 0x00FFFFFFu ) | ( (uint32_t)material << 24 );+}++/// 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+{+	/// Shape id.+	b3ShapeId shapeId;++	/// Shape type.+	b3ShapeType type;++	/// Tagged union.+	union+	{+		const b3Capsule* capsule;			  ///< Capsule shape.+		const b3CompoundData* compound;		  ///< Compound shape.+		const b3HeightFieldData* heightField; ///< Height-field shape.+		const b3HullData* hull;				  ///< Convex hull shape.+		const b3Mesh* mesh;					  ///< Mesh shape with scale.+		const b3Sphere* sphere;				  ///< Sphere shape.+	};+} b3DebugShape;++/// This struct is passed to b3World_Draw to draw a debug view of the simulation world.+/// Callbacks receive world coordinates. In large world mode the translation is double precision so+/// it stays accurate far from the origin. Shift into your own camera frame inside the callbacks.+typedef struct b3DebugDraw+{+	/// Draws a user shape. The userShape pointer is owned by the application and is known to Box3D as+	/// an opaque pointer returned from b3CreateDebugShapeCallback. When this is called the drawn shape has+	/// passed a culling test against drawingBounds below.+	void ( *DrawShapeFcn )( void* userShape, b3WorldTransform transform, b3HexColor color, void* context );++	/// Draw a line segment.+	void ( *DrawSegmentFcn )( b3Pos p1, b3Pos p2, b3HexColor color, void* context );++	/// Draw a transform. Choose your own length scale.+	void ( *DrawTransformFcn )( b3WorldTransform transform, void* context );++	/// Draw a point.+	void ( *DrawPointFcn )( b3Pos p, float size, b3HexColor color, void* context );++	/// Draw a sphere.+	void ( *DrawSphereFcn )( b3Pos p, float radius, b3HexColor color, float alpha, void* context );++	/// Draw a capsule.+	void ( *DrawCapsuleFcn )( b3Pos p1, b3Pos p2, float radius, b3HexColor color, float alpha, void* context );++	/// Draw a bounding box.+	void ( *DrawBoundsFcn )( b3AABB aabb, b3HexColor color, void* context );++	/// Draw an oriented box.+	void ( *DrawBoxFcn )( b3Vec3 extents, b3WorldTransform transform, b3HexColor color, void* context );++	/// Draw a string in world space+	void ( *DrawStringFcn )( b3Pos 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 the sleep information for dynamic and kinematic bodies+	bool drawSleep;++	/// Option to draw body names+	bool drawBodyNames;++	/// Option to draw contact points+	bool drawContacts;++	/// Draw contact anchor A or B+	bool 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 islands as bounding boxes+	bool drawIslands;++	/// User context that is passed as an argument to drawing callback functions+	void* context;+} b3DebugDraw;++/// Create a debug draw struct with default values.+B3_API b3DebugDraw b3DefaultDebugDraw( void );++/**@}*/ // debug_draw
+ upstream/src/aabb.c view
@@ -0,0 +1,176 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#include "aabb.h"++#include "math_internal.h"++#include "box3d/math_functions.h"++#include <float.h>++// 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 b3RayCastAABB( b3AABB a, 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 >= a.lowerBound.x && p1.x <= a.upperBound.x && p1.y >= a.lowerBound.y && p1.y <= a.upperBound.y &&+			 p1.z >= a.lowerBound.z && p1.z <= a.upperBound.z )+		{+			*minFraction = 0.0f;+			*maxFraction = 0.0f;+			return true;+		}++		return false;+	}++	b3Vec3 rayDir = b3MulSV( 1.0f / rayLength, d );++	// Slab method for ray-AABB intersection+	float tMin = 0.0f;+	float tMax = rayLength;++	// x-axis+	{+		float rayComponent = rayDir.x;+		float rayStart = p1.x;+		float boxMin = a.lowerBound.x;+		float boxMax = a.upperBound.x;++		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;+			}+		}+	}+	+	// y-axis+	{+		float rayComponent = rayDir.y;+		float rayStart = p1.y;+		float boxMin = a.lowerBound.y;+		float boxMax = a.upperBound.y;++		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;+			}+		}+	}++	// z-axis+	{+		float rayComponent = rayDir.z;+		float rayStart = p1.z;+		float boxMin = a.lowerBound.z;+		float boxMax = a.upperBound.z;++		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;+}
+ upstream/src/aabb.h view
@@ -0,0 +1,80 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "box3d/types.h"++// Ray cast an AABB. This is a custom function used by height fields.+bool b3RayCastAABB( b3AABB a, b3Vec3 p1, b3Vec3 p2, float* minFraction, float* maxFraction );++// 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,+	};+}
+ upstream/src/algorithm.h view
@@ -0,0 +1,19 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include <stddef.h>+#include <string.h>++// Swap two same-size lvalues through a byte buffer. Avoids __typeof__ so it builds on any C compiler.+#define B3_SWAP( x, y )                                                                                                          \+	do                                                                                                                           \+	{                                                                                                                            \+		_Static_assert( sizeof( x ) == sizeof( y ), "size mismatch" );                                                           \+		char B3_SWAP_TEMP[sizeof( x )];                                                                                          \+		memcpy( B3_SWAP_TEMP, &( x ), sizeof( x ) );                                                                             \+		memcpy( &( x ), &( y ), sizeof( x ) );                                                                                   \+		memcpy( &( y ), B3_SWAP_TEMP, sizeof( x ) );                                                                             \+	}                                                                                                                            \+	while ( 0 )
+ upstream/src/arena_allocator.c view
@@ -0,0 +1,178 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#include "arena_allocator.h"++#include "core.h"++#include <stdbool.h>+#include <stddef.h>++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;+	}++	int alignedSize = ( ( size - 1 ) | ( B3_ALIGNMENT - 1 ) ) + 1;++	b3StackEntry entry;+	entry.size = alignedSize;+	entry.name = name;+	if ( stack->index + alignedSize > stack->capacity )+	{+		// fall back to the heap (undesirable)+		entry.data = (char*)b3Alloc( alignedSize );+		entry.usedMalloc = true;++		B3_ASSERT( ( (uintptr_t)entry.data & ( B3_ALIGNMENT - 1 ) ) == 0 );+	}+	else+	{+		entry.data = stack->memory + stack->index;+		entry.usedMalloc = false;+		stack->index += alignedSize;++		B3_ASSERT( ( (uintptr_t)entry.data & ( B3_ALIGNMENT - 1 ) ) == 0 );+	}++	stack->allocation += alignedSize;+	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;+	b3ArenaSharedState* shared = (b3ArenaSharedState*)b3Alloc( sizeof( b3ArenaSharedState ) );+	*shared = (b3ArenaSharedState){ 0 };++	return (b3Arena){+		.memory = (char*)b3Alloc( c ),+		.capacity = c,+		.index = 0,+		.shared = shared,+	};+}++void b3DestroyArena( b3Arena* arena )+{+	b3ArenaSharedState* shared = arena->shared;+	if ( shared != NULL )+	{+		for ( int i = 0; i < shared->overflows.count; ++i )+		{+			b3Free( shared->overflows.data[i].data, shared->overflows.data[i].size );+		}+		b3Array_Destroy( shared->overflows );+		b3Free( shared, sizeof( b3ArenaSharedState ) );+	}+	b3Free( arena->memory, arena->capacity );+	*arena = (b3Arena){ 0 };+}++void* b3ArenaOverflowAlloc( b3Arena* arena, int size )+{+	b3ArenaSharedState* shared = arena->shared;+	char* data = (char*)b3Alloc( size );+	b3OverflowBlock block = { data, size };+	b3Array_Push( shared->overflows, block );+	shared->overflowBytes += size;+	return data;+}++void b3ArenaSync( b3Arena* arena )+{+	b3ArenaSharedState* shared = arena->shared;++	for ( int i = 0; i < shared->overflows.count; ++i )+	{+		b3Free( shared->overflows.data[i].data, shared->overflows.data[i].size );+	}+	b3Array_Clear( shared->overflows );++	int demand = shared->maxIndex + shared->overflowBytes;+	if ( demand > shared->peakDemand )+	{+		shared->peakDemand = demand;+	}+	if ( demand > arena->capacity )+	{+		b3Free( arena->memory, arena->capacity );+		int newCapacity = demand + demand / 2;+		arena->memory = (char*)b3Alloc( newCapacity );+		arena->capacity = newCapacity;+	}++	arena->index = 0;+	shared->maxIndex = 0;+	shared->overflowBytes = 0;+}
+ upstream/src/arena_allocator.h view
@@ -0,0 +1,114 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once+#include "box3d/base.h"+#include "container.h"++#include <stdbool.h>+#include <stddef.h>++#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;++// Heap-allocated fallback block tracked when an arena bump overflows.+typedef struct b3OverflowBlock+{+	char* data;+	int size;+} b3OverflowBlock;++b3DeclareArray( b3OverflowBlock );++// Shared, heap-allocated state co-owned by every copy of a b3Arena.+// b3Arena is passed by value so its bump pointer auto-restores on+// function return, but overflow tracking and watermarks must persist+// across copies -- hence this pointer-shared block.+typedef struct b3ArenaSharedState+{+	b3Array( b3OverflowBlock ) overflows;+	int maxIndex;          // high water mark of the bump pointer this step+	int overflowBytes;     // total bytes in overflow blocks this step+	int peakDemand;        // all-time peak of (maxIndex + overflowBytes), survives sync+} b3ArenaSharedState;++typedef struct b3Arena+{+	char* memory;+	int capacity;+	int index;+	b3ArenaSharedState* shared;+} b3Arena;++// 16-byte alignment for SSE2 + typical struct alignment.+#define B3_ARENA_ALIGNMENT 16++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 );++// Heap-allocate an overflow block, register it in the shared state, return it.+void* b3ArenaOverflowAlloc( b3Arena* arena, int size );++// Call between simulation steps. Frees this step's overflow blocks and grows+// the backing capacity if last step's demand (maxIndex + overflowBytes) exceeded it.+void b3ArenaSync( b3Arena* arena );++static inline void* b3Bump( b3Arena* arena, int size )+{+	if ( size == 0 )+	{+		return NULL;+	}++	int aligned = ( arena->index + ( B3_ARENA_ALIGNMENT - 1 ) ) & ~( B3_ARENA_ALIGNMENT - 1 );++	if ( aligned + size > arena->capacity )+	{+		return b3ArenaOverflowAlloc( arena, size );+	}++	arena->index = aligned + size;+	if ( arena->index > arena->shared->maxIndex )+	{+		arena->shared->maxIndex = arena->index;+	}+	return arena->memory + aligned;+}
+ upstream/src/bitset.c view
@@ -0,0 +1,66 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#include "bitset.h"++#include <string.h>++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];+	}+}
+ upstream/src/bitset.h view
@@ -0,0 +1,67 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "core.h"++#include <stdint.h>+#include <stdbool.h>++// Bit set provides fast operations on large arrays of bits.+typedef struct b3BitSet+{+	uint64_t* bits;+	uint32_t blockCapacity;+	uint32_t blockCount;+} b3BitSet;+++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 );++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 );+}
+ upstream/src/block_allocator.c view
@@ -0,0 +1,95 @@+// 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* ) );++	// The free list stores a pointer inside each element, so the stride must keep every element aligned.+	// Blocks come from b3Alloc, so a stride that is a multiple of B3_ALIGNMENT aligns all elements.+	elementSize = ( ( elementSize - 1 ) | ( B3_ALIGNMENT - 1 ) ) + 1;++	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 index = allocator->nextIndex++;++	int requiredBlockCount = ( index >> 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 = index >> B3_BLOCK_EXPONENT;+	int blockOffset = index & ( 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 );+	B3_ASSERT( ( (uintptr_t)element & ( B3_ALIGNMENT - 1 ) ) == 0 );++	allocator->allocationCount -= 1;++	// Push onto free list+	*(void**)element = allocator->freeList;+	allocator->freeList = element;+}
+ upstream/src/block_allocator.h view
@@ -0,0 +1,35 @@+// 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+{+	b3Array( b3Block ) blocks;+	void* freeList;+	int elementSize;+	int nextIndex;+	int allocationCount;+} b3BlockAllocator;++// Element must be large enough to hold a pointer. The element size is rounded up to B3_ALIGNMENT.+b3BlockAllocator b3CreateBlockAllocator( int elementSize, int initialCount );+void b3DestroyBlockAllocator( b3BlockAllocator* allocator );++// Returns one element of elementSize contiguous bytes. Address is stable until freed.+void* b3AllocateElement( b3BlockAllocator* allocator );+void b3FreeElement( b3BlockAllocator* allocator, void* element );
+ upstream/src/body.c view
@@ -0,0 +1,2483 @@+// 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 "recording.h"+#include "sensor.h"+#include "shape.h"+#include "solver_set.h"++// needed for dll export+#include "box3d/box3d.h"+#include "box3d/id.h"++#include <stddef.h>++// 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 );+}++b3WorldTransform b3GetBodyTransformQuick( b3World* world, b3Body* body )+{+	b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex );+	b3BodySim* bodySim = b3Array_Get( set->bodySims, body->localIndex );+	return bodySim->transform;+}++b3WorldTransform 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;+}++void b3SyncBodyFlags( b3World* world, b3Body* body )+{+	// Never sync transient flags+	uint32_t flags = body->flags & ~b3_bodyTransientFlags;++	b3BodySim* bodySim = b3GetBodySim( world, body );+	bodySim->flags = flags;++	b3BodyState* bodyState = b3GetBodyState( world, body );+	if ( bodyState != NULL )+	{+		bodyState->flags = flags;+	}+}++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( b3IsValidPosition( 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;+	bodySim->flags |= def->enableSleep ? b3_enableSleep : 0;+	bodySim->flags |= def->enableContactRecycling ? b3_bodyEnableContactRecycling : 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;+	}++	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 );+	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->sleepVelocity = 0.0f;+	body->mass = 0.0f;+	body->inertia = b3Mat3_zero;+	body->nameId = b3AddName( &world->names, def->name );+	body->type = def->type;+	body->flags = bodySim->flags;++	// 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;++	B3_REC_CREATE( world, CreateBody, id, worldId, *def );++	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;+	}++	B3_REC( world, DestroyBody, bodyId );++	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 )+	{+		b3WorldTransform transform = b3GetBodyTransform( world, body->id );+		b3Vec3 p = b3ToVec3( transform.p );+		return (b3AABB){ p, 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 );+	b3WorldTransform worldTransform = b3GetBodyTransform( world, body->id );+	b3Transform transform = b3ToRelativeTransform( worldTransform, b3Pos_zero );++	float closestDistance = FLT_MAX;+	b3Vec3 closestPoint = transform.p;++	b3DistanceInput input = { 0 };+	input.proxyA = (b3ShapeProxy){ &target, 1, 0.0f };++	// Target rides in frame A at the origin, so the relative pose of the shape in A is the body transform+	input.transform = 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, b3Pos origin, b3Vec3 translation, b3QueryFilter filter, float maxFraction,+								 b3WorldTransform bodyTransform )+{+	b3World* world = b3GetUnlockedWorld( bodyId.world0 );+	if ( world == NULL )+	{+		return (b3BodyCastResult){ 0 };+	}++	b3BodyCastResult result = { 0 };+	b3Body* body = b3GetBodyFullId( world, bodyId );++	// The consistent framing is to center on the ray origin.+	b3RayCastInput shapeInput = { 0 };+	shapeInput.origin = b3Vec3_zero;+	shapeInput.translation = translation;+	shapeInput.maxFraction = maxFraction;++	b3Transform transform = b3ToRelativeTransform( bodyTransform, origin );++	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;+		}++		b3CastOutput shapeOutput = b3RayCastShape( shape, transform, &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 = b3GetShapeMaterials( shape )[materialIndex].userMaterialId;++		result = (b3BodyCastResult){+			.shapeId = id,+			.point = b3OffsetPos( origin, 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, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation,+								   b3QueryFilter filter, float maxFraction, bool canEncroach, b3WorldTransform bodyTransform )+{+	b3World* world = b3GetUnlockedWorld( bodyId.world0 );+	if ( world == NULL )+	{+		return (b3BodyCastResult){ 0 };+	}++	b3BodyCastResult result = { 0 };+	b3Body* body = b3GetBodyFullId( world, bodyId );++	b3Transform transform = b3ToRelativeTransform( bodyTransform, origin );++	b3ShapeCastInput shapeInput = { 0 };+	shapeInput.proxy = *proxy;+	shapeInput.translation = translation;+	shapeInput.maxFraction = maxFraction;+	shapeInput.canEncroach = canEncroach;++	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;+		}++		b3CastOutput shapeOutput = b3ShapeCastShape( shape, transform, &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 = b3GetShapeMaterials( shape )[materialIndex].userMaterialId;++		result = (b3BodyCastResult){+			.shapeId = id,+			.point = b3OffsetPos( origin, 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, b3Pos origin, const b3ShapeProxy* proxy, b3QueryFilter filter,+						  b3WorldTransform bodyTransform )+{+	b3World* world = b3GetUnlockedWorld( bodyId.world0 );+	if ( world == NULL )+	{+		return false;+	}++	b3Body* body = b3GetBodyFullId( world, bodyId );+	b3Transform transform = b3ToRelativeTransform( bodyTransform, origin );++	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, transform, proxy );+		if ( overlaps )+		{+			return true;+		}+	}++	return false;+}++int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int planeCapacity, b3Pos origin, const b3Capsule* mover,+						 b3QueryFilter filter, b3WorldTransform bodyTransform )+{+	b3World* world = b3GetUnlockedWorld( bodyId.world0 );+	if ( world == NULL )+	{+		return 0;+	}++	if ( planeCapacity == 0 )+	{+		return 0;+	}++	int resultCount = 0;+	b3Body* body = b3GetBodyFullId( world, bodyId );++	b3Transform transform = b3ToRelativeTransform( bodyTransform, origin );++	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, transform, mover );++		if ( count > 0 )+		{+			b3ShapeId id = { shape->id + 1, bodyId.world0, 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 );++	// Mass is no longer dirty+	body->flags &= ~b3_dirtyMass;+	b3SyncBodyFlags( 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;+	}++	int shapeCount = body->shapeCount;+	b3MassData* masses = b3StackAlloc( &world->stack, shapeCount * sizeof( b3MassData ), "mass data" );++	// Accumulate mass over all shapes.+	b3Vec3 localCenter = b3Vec3_zero;+	int shapeId = body->headShapeId;+	int shapeIndex = 0;+	while ( shapeId != B3_NULL_INDEX )+	{+		const b3Shape* s = b3Array_Get( world->shapes, shapeId );+		shapeId = s->nextShapeId;++		if ( s->density == 0.0f )+		{+			masses[shapeIndex] = (b3MassData){ 0 };+			shapeIndex += 1;+			continue;+		}++		b3MassData massData = b3ComputeShapeMass( s );+		body->mass += massData.mass;+		localCenter = b3MulAdd( localCenter, massData.mass, massData.center );++		masses[shapeIndex] = massData;+		shapeIndex += 1;+	}++	// Compute center of mass.+	if ( body->mass > 0.0f )+	{+		bodySim->invMass = 1.0f / body->mass;+		localCenter = b3MulSV( bodySim->invMass, localCenter );+	}++	// Second loop to accumulate the rotational inertia about the center of mass+	for ( shapeIndex = 0; shapeIndex < shapeCount; ++shapeIndex )+	{+		b3MassData massData = masses[shapeIndex];+		if ( massData.mass == 0.0f )+		{+			continue;+		}++		// Shift to center of mass. This is safe because it can only increase.+		b3Vec3 offset = b3Sub( localCenter, massData.center );+		b3Matrix3 inertia = b3AddMM( massData.inertia, b3Steiner( massData.mass, offset ) );+		body->inertia = b3AddMM( body->inertia, inertia );+	}++	b3StackFree( &world->stack, masses );+	masses = NULL;++	float det = b3Det( body->inertia );+	B3_ASSERT( det >= 0.0f );++	if ( det > 0.0f )+	{+		// 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.+	b3Pos oldCenter = bodySim->center;+	bodySim->localCenter = localCenter;+	bodySim->center = b3TransformWorldPoint( 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, b3SubPos( 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;+	}++	// Apply fixed rotation+	if ( ( bodySim->flags & b3_fixedRotation ) == b3_fixedRotation )+	{+		body->inertia = b3Mat3_zero;+		bodySim->invInertiaLocal = b3Mat3_zero;+		bodySim->invInertiaWorld = b3Mat3_zero;+	}+}++b3Pos b3Body_GetPosition( b3BodyId bodyId )+{+	b3World* world = b3GetWorld( bodyId.world0 );+	b3Body* body = b3GetBodyFullId( world, bodyId );+	b3WorldTransform transform = b3GetBodyTransformQuick( world, body );+	return transform.p;+}++b3Quat b3Body_GetRotation( b3BodyId bodyId )+{+	b3World* world = b3GetWorld( bodyId.world0 );+	b3Body* body = b3GetBodyFullId( world, bodyId );+	b3WorldTransform transform = b3GetBodyTransformQuick( world, body );+	return transform.q;+}++b3WorldTransform b3Body_GetTransform( b3BodyId bodyId )+{+	b3World* world = b3GetWorld( bodyId.world0 );+	b3Body* body = b3GetBodyFullId( world, bodyId );+	return b3GetBodyTransformQuick( world, body );+}++b3Vec3 b3Body_GetLocalPoint( b3BodyId bodyId, b3Pos worldPoint )+{+	b3World* world = b3GetWorld( bodyId.world0 );+	b3Body* body = b3GetBodyFullId( world, bodyId );+	b3WorldTransform transform = b3GetBodyTransformQuick( world, body );+	return b3InvTransformWorldPoint( transform, worldPoint );+}++b3Pos b3Body_GetWorldPoint( b3BodyId bodyId, b3Vec3 localPoint )+{+	b3World* world = b3GetWorld( bodyId.world0 );+	b3Body* body = b3GetBodyFullId( world, bodyId );+	b3WorldTransform transform = b3GetBodyTransformQuick( world, body );+	return b3TransformWorldPoint( transform, localPoint );+}++b3Vec3 b3Body_GetLocalVector( b3BodyId bodyId, b3Vec3 worldVector )+{+	b3World* world = b3GetWorld( bodyId.world0 );+	b3Body* body = b3GetBodyFullId( world, bodyId );+	b3WorldTransform 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 );+	b3WorldTransform transform = b3GetBodyTransformQuick( world, body );+	return b3RotateVector( transform.q, localVector );+}++void b3Body_SetTransform( b3BodyId bodyId, b3Pos position, b3Quat rotation )+{+	B3_ASSERT( b3IsValidPosition( position ) );+	B3_ASSERT( b3IsValidQuat( rotation ) );+	B3_ASSERT( b3Body_IsValid( bodyId ) );+	b3World* world = b3GetWorld( bodyId.world0 );+	B3_ASSERT( world->locked == false );++	B3_REC( world, BodySetTransform, bodyId, position, rotation );++	b3Body* body = b3GetBodyFullId( world, bodyId );+	b3BodySim* bodySim = b3GetBodySim( world, body );++	bodySim->transform.p = position;+	bodySim->transform.q = rotation;+	bodySim->center = b3TransformWorldPoint( 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;++	b3WorldTransform transform = bodySim->transform;+	const float speculativeDistance = B3_SPECULATIVE_DISTANCE;++	int shapeId = body->headShapeId;+	while ( shapeId != B3_NULL_INDEX )+	{+		b3Shape* shape = b3Array_Get( world->shapes, shapeId );+		b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeDistance );+		shape->aabb = aabb;++		if ( b3AABB_Contains( shape->fatAABB, aabb ) == false )+		{+			float margin = shape->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;++			// 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 );++	B3_REC( world, BodySetLinearVelocity, bodyId, linearVelocity );++	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 );++	B3_REC( world, BodySetAngularVelocity, bodyId, angularVelocity );++	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, b3WorldTransform target, float timeStep, bool wake )+{+	B3_ASSERT( b3IsValidWorldTransform( target ) );++	b3World* world = b3GetWorld( bodyId.world0 );++	B3_REC( world, BodySetTargetTransform, bodyId, target, timeStep, wake );++	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. The center difference is taken in world precision then demoted.+	b3Pos center1 = sim->center;+	b3Pos center2 = b3TransformWorldPoint( target, sim->localCenter );+	float invTimeStep = 1.0f / timeStep;+	b3Vec3 linearVelocity = b3MulSV( invTimeStep, b3SubPos( 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, b3Pos 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 = b3SubPos( worldPoint, bodySim->center );+	b3Vec3 v = b3Add( state->linearVelocity, b3Cross( state->angularVelocity, r ) );+	return v;+}++void b3Body_ApplyForce( b3BodyId bodyId, b3Vec3 force, b3Pos point, bool wake )+{+	B3_ASSERT( b3IsValidVec3( force ) );++	b3World* world = b3GetWorld( bodyId.world0 );++	B3_REC( world, BodyApplyForce, bodyId, force, point, wake );++	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( b3SubPos( point, bodySim->center ), force ) );+	}+}++void b3Body_ApplyForceToCenter( b3BodyId bodyId, b3Vec3 force, bool wake )+{+	B3_ASSERT( b3IsValidVec3( force ) );++	b3World* world = b3GetWorld( bodyId.world0 );++	B3_REC( world, BodyApplyForceToCenter, bodyId, force, wake );++	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 );++	B3_REC( world, BodyApplyTorque, bodyId, torque, wake );++	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, b3Pos point, bool wake )+{+	B3_ASSERT( b3IsValidVec3( impulse ) );+	B3_ASSERT( b3IsValidPosition( point ) );++	b3World* world = b3GetWorld( bodyId.world0 );++	B3_REC( world, BodyApplyLinearImpulse, bodyId, impulse, point, wake );++	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( b3SubPos( 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 );++	B3_REC( world, BodyApplyLinearImpulseToCenter, bodyId, impulse, wake );++	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 );++	B3_REC( world, BodyApplyAngularImpulse, bodyId, impulse, wake );++	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;+	}++	B3_REC( world, BodySetType, bodyId, (int32_t)type );++	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;+		}++		b3SyncBodyFlags( world, body );++		// 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+	b3WorldTransform 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 );+	}++	b3SyncBodyFlags( world, body );++	// Body type affects the mass+	b3UpdateBodyMassData( world, body );++	b3ValidateSolverSets( world );+	b3ValidateIsland( world, body->islandId );++	world->locked = false;+}++void b3Body_SetName( b3BodyId bodyId, const char* name )+{+	b3World* world = b3GetWorld( bodyId.world0 );++	B3_REC( world, BodySetName, bodyId, name );++	b3Body* body = b3GetBodyFullId( world, bodyId );+	body->nameId = b3AddName( &world->names, name );+}++const char* b3Body_GetName( b3BodyId bodyId )+{+	b3World* world = b3GetWorld( bodyId.world0 );+	b3Body* body = b3GetBodyFullId( world, bodyId );+	return b3FindNameWithDefault( &world->names, body->nameId, "" );+}++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_GetLocalCenter( b3BodyId bodyId )+{+	b3World* world = b3GetWorld( bodyId.world0 );+	b3Body* body = b3GetBodyFullId( world, bodyId );+	b3BodySim* bodySim = b3GetBodySim( world, body );+	return bodySim->localCenter;+}++b3Pos b3Body_GetWorldCenter( 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;+	}++	B3_REC( world, BodySetMassData, bodyId, massData );++	b3Body* body = b3GetBodyFullId( world, bodyId );+	b3BodySim* bodySim = b3GetBodySim( world, body );++	// Mass is no longer dirty+	body->flags &= ~b3_dirtyMass;+	b3SyncBodyFlags( world, body );++	body->mass = massData.mass;+	body->inertia = massData.inertia;+	bodySim->localCenter = massData.center;++	b3Pos oldCenter = bodySim->center;+	b3Pos center = b3TransformWorldPoint( bodySim->transform, massData.center );+	bodySim->center = center;+	bodySim->center0 = center;+	bodySim->invMass = body->mass > 0.0f ? 1.0f / body->mass : 0.0f;++	// Update center of mass velocity+	b3BodyState* state = b3GetBodyState( world, body );+	if ( state != NULL )+	{+		b3Vec3 deltaLinear = b3Cross( state->angularVelocity, b3SubPos( bodySim->center, oldCenter ) );+		state->linearVelocity = b3Add( state->linearVelocity, deltaLinear );+	}++	float det = b3Det( body->inertia );+	B3_ASSERT( det >= 0.0f );++	if ( det > 0.0f )+	{+		// 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 ) );+	}+	else+	{+		bodySim->invInertiaLocal = b3Mat3_zero;+		bodySim->invInertiaWorld = b3Mat3_zero;+	}++	// Apply fixed rotation+	if ( ( bodySim->flags & b3_fixedRotation ) == b3_fixedRotation )+	{+		body->inertia = b3Mat3_zero;+		bodySim->invInertiaLocal = b3Mat3_zero;+		bodySim->invInertiaWorld = b3Mat3_zero;+	}++	// Update extents using supplied mass center.+	bodySim->minExtent = B3_HUGE;+	bodySim->maxExtent = b3Vec3_zero;+	int shapeId = body->headShapeId;+	while ( shapeId != B3_NULL_INDEX )+	{+		const b3Shape* s = b3Array_Get( world->shapes, shapeId );+		b3ShapeExtent extent = b3ComputeShapeExtent( s, massData.center );+		bodySim->minExtent = b3MinFloat( bodySim->minExtent, extent.minExtent );+		bodySim->maxExtent = b3Max( bodySim->maxExtent, extent.maxExtent );+		shapeId = s->nextShapeId;+	}+}++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;+	}++	B3_REC( world, BodyApplyMassFromShapes, bodyId );++	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;+	}++	B3_REC( world, BodySetLinearDamping, bodyId, linearDamping );++	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;+	}++	B3_REC( world, BodySetAngularDamping, bodyId, angularDamping );++	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;+	}++	B3_REC( world, BodySetGravityScale, bodyId, gravityScale );++	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;+	}++	B3_REC( world, BodySetAwake, bodyId, awake );++	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->flags & b3_enableSleep ) == b3_enableSleep;+}++void b3Body_SetSleepThreshold( b3BodyId bodyId, float sleepThreshold )+{+	b3World* world = b3GetWorld( bodyId.world0 );++	B3_REC( world, BodySetSleepThreshold, bodyId, sleepThreshold );++	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;+	}++	B3_REC( world, BodyEnableSleep, bodyId, enableSleep );++	b3Body* body = b3GetBodyFullId( world, bodyId );++	bool flag = ( body->flags & b3_enableSleep ) == b3_enableSleep;+	if ( enableSleep == flag )+	{+		return;+	}++	world->locked = true;++	body->flags = enableSleep ? body->flags | b3_enableSleep : body->flags & ~b3_enableSleep;+	b3SyncBodyFlags( world, body );++	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;+	}++	B3_REC( world, BodyDisable, bodyId );++	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;+	}++	B3_REC( world, BodyEnable, bodyId );++	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 );++	b3WorldTransform 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;+	}++	B3_REC( world, BodySetMotionLocks, bodyId, locks );++	uint32_t newLocks = 0;+	newLocks |= locks.linearX ? b3_lockLinearX : 0;+	newLocks |= locks.linearY ? b3_lockLinearY : 0;+	newLocks |= locks.linearZ ? b3_lockLinearZ : 0;+	newLocks |= locks.angularX ? b3_lockAngularX : 0;+	newLocks |= locks.angularY ? b3_lockAngularY : 0;+	newLocks |= locks.angularZ ? b3_lockAngularZ : 0;++	b3Body* body = b3GetBodyFullId( world, bodyId );+	if ( ( body->flags & b3_allLocks ) == newLocks )+	{+		return;+	}++	bool fixedRotation1 = ( body->flags & b3_fixedRotation ) == b3_fixedRotation;+	bool fixedRotation2 = ( newLocks & b3_fixedRotation ) == b3_fixedRotation;++	body->flags &= ~b3_allLocks;+	body->flags |= newLocks;++	b3SyncBodyFlags( world, body );++	b3BodyState* state = b3GetBodyState( world, body );++	if ( state != NULL )+	{+		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;+		}+	}++	if ( fixedRotation1 != fixedRotation2 )+	{+		b3UpdateBodyMassData( world, body );+	}+}++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;+	}++	B3_REC( world, BodySetBullet, bodyId, flag );++	uint32_t newFlag = flag ? b3_isBullet : 0;++	b3Body* body = b3GetBodyFullId( world, bodyId );+	if ( ( body->flags & b3_isBullet ) == newFlag )+	{+		return;+	}++	body->flags &= ~b3_isBullet;+	body->flags |= newFlag;++	b3SyncBodyFlags( world, body );+}++bool b3Body_IsBullet( b3BodyId bodyId )+{+	b3World* world = b3GetWorld( bodyId.world0 );+	b3Body* body = b3GetBodyFullId( world, bodyId );+	return ( body->flags & b3_isBullet ) != 0;+}++void b3Body_AllowFastRotation(b3BodyId bodyId, bool flag)+{+	b3World* world = b3GetUnlockedWorld( bodyId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B3_REC( world, BodyAllowFastRotation, bodyId, flag );++	uint32_t newFlag = flag ? b3_allowFastRotation : 0;++	b3Body* body = b3GetBodyFullId( world, bodyId );+	if ( ( body->flags & b3_allowFastRotation ) == newFlag )+	{+		return;+	}++	body->flags &= ~b3_allowFastRotation;+	body->flags |= newFlag;++	b3SyncBodyFlags( world, body );+}++bool b3Body_IsFastRotationAllowed(b3BodyId bodyId)+{+	b3World* world = b3GetWorld( bodyId.world0 );+	b3Body* body = b3GetBodyFullId( world, bodyId );+	return ( body->flags & b3_allowFastRotation ) != 0;+}++void b3Body_EnableContactRecycling( b3BodyId bodyId, bool flag )+{+	b3World* world = b3GetUnlockedWorld( bodyId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B3_REC( world, BodyEnableContactRecycling, bodyId, flag );++	uint32_t newFlag = flag ? b3_bodyEnableContactRecycling : 0;++	b3Body* body = b3GetBodyFullId( world, bodyId );+	if ( ( body->flags & b3_bodyEnableContactRecycling ) == newFlag )+	{+		return;+	}++	body->flags &= ~b3_bodyEnableContactRecycling;+	body->flags |= newFlag;++	b3SyncBodyFlags( world, body );+}++bool b3Body_IsContactRecyclingEnabled( b3BodyId bodyId )+{+	b3World* world = b3GetWorld( bodyId.world0 );+	b3Body* body = b3GetBodyFullId( world, bodyId );+	return ( body->flags & b3_bodyEnableContactRecycling ) != 0;+}++void b3Body_EnableHitEvents( b3BodyId bodyId, bool flag )+{+	b3World* world = b3GetWorld( bodyId.world0 );++	B3_REC( world, BodyEnableHitEvents, bodyId, flag );++	b3Body* body = b3GetBodyFullId( world, bodyId );+	int shapeId = body->headShapeId;+	while ( shapeId != B3_NULL_INDEX )+	{+		b3Shape* shape = b3Array_Get( world->shapes, shapeId );+		shape->flags = flag ? shape->flags | b3_enableHitEvents : shape->flags & ~b3_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;+}
+ upstream/src/body.h view
@@ -0,0 +1,255 @@+// 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,++	b3_enableSleep = 0x00002000,++	b3_bodyEnableContactRecycling = 0x00004000,++	// The user deferred mass computation via the updateBodyMass shape option and mass+	// data still hasn't been set.+	b3_dirtyMass = 0x00008000,++	// All lock flags+	b3_allLocks = b3_lockLinearX | b3_lockLinearY | b3_lockLinearZ | b3_lockAngularX | b3_lockAngularY | b3_lockAngularZ,++	// If all these flags are set then the body has fixed rotation+	b3_fixedRotation = b3_lockAngularX | b3_lockAngularY | b3_lockAngularZ,++	// These flags are transient per time step. These may be different across b3Body, b3BodySim, and b3BodyState.+	b3_bodyTransientFlags = b3_isFast | b3_isSpeedCapped | b3_hadTimeOfImpact,+};++// Body organizational details that are not used in the solver.+typedef struct b3Body+{+	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 sleepVelocity;+	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;+	uint32_t nameId;++	b3BodyType type;++	// This is monotonically advanced when a body is allocated in this slot+	// Used to check for invalid b3BodyId+	uint16_t generation;+} 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, double translation in large world mode+	b3WorldTransform transform;++	// center of mass position in world space+	b3Pos center;++	// previous rotation and COM for TOI+	b3Quat rotation0;+	b3Pos 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 linearDamping;+	float angularDamping;+	float gravityScale;++	// Index of b3Body+	int bodyId;++	// b3BodyFlags+	uint32_t flags;+} b3BodySim;++// Get a validated body from a world using an id.+b3Body* b3GetBodyFullId( b3World* world, b3BodyId bodyId );++b3WorldTransform b3GetBodyTransformQuick( b3World* world, b3Body* body );+b3WorldTransform 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 b3SyncBodyFlags( b3World* world, b3Body* body );++// Make a sweep relative to a base position to keep TOI in float precision far from the origin.+static inline b3Sweep b3MakeRelativeSweep( const b3BodySim* bodySim, b3Pos base )+{+	b3Sweep s;+	s.c1 = b3SubPos( bodySim->center0, base );+	s.c2 = b3SubPos( bodySim->center, base );+	s.q1 = bodySim->rotation0;+	s.q2 = bodySim->transform.q;+	s.localCenter = bodySim->localCenter;+	return s;+}+
+ upstream/src/broad_phase.c view
@@ -0,0 +1,535 @@+// 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 <string.h>++void b3CreateBroadPhase( b3BroadPhase* bp, const b3Capacity* capacity )+{+	_Static_assert( b3_bodyTypeCount == 3, "must be three body types" );++	bp->movedProxies[b3_staticBody] = b3CreateBitSet( b3MaxInt( 16, capacity->staticShapeCount ) );+	bp->movedProxies[b3_kinematicBody] = b3CreateBitSet( 16 );+	bp->movedProxies[b3_dynamicBody] = b3CreateBitSet( b3MaxInt( 16, 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 );+	}++	for ( int i = 0; i < b3_bodyTypeCount; ++i )+	{+		b3DestroyBitSet( &bp->movedProxies[i] );+	}+	b3Array_Destroy( bp->moveArray );+	b3DestroySet( &bp->pairSet );++	*bp = (b3BroadPhase){ 0 };++	memset( bp, 0, sizeof( b3BroadPhase ) );+}++static void b3UnBufferMove( b3BroadPhase* bp, int proxyKey )+{+	b3BodyType proxyType = B3_PROXY_TYPE( proxyKey );+	int proxyId = B3_PROXY_ID( proxyKey );+	b3BitSet* set = &bp->movedProxies[proxyType];++	if ( b3GetBit( set, proxyId ) )+	{+		b3ClearBit( set, proxyId );++		// 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 )+{+	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 )+		{+			// Query bounds are float world space, so the demoted transform is the matching float frame+			b3Transform compoundTransform = b3ToRelativeTransform( b3GetBodyTransform( world, shape->bodyId ), b3Pos_zero );+			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 movedProxies bit sets contain dynamic and kinematic+	// proxies, but sometimes static proxies are in there too (b3ShapeDef::invokeContactCreation or a+	// modified static shape), so we always have to check.++	// Is this proxy also moving?+	if ( queryProxyType == b3_dynamicBody )+	{+		if ( treeType == b3_dynamicBody && proxyKey < queryProxyKey )+		{+			bool moved = b3GetBit( &broadPhase->movedProxies[treeType], proxyId );+			if ( moved )+			{+				// Both proxies are moving. Avoid duplicate pairs.+				return true;+			}+		}+	}+	else+	{+		B3_ASSERT( treeType == b3_dynamicBody );+		bool moved = b3GetBit( &broadPhase->movedProxies[treeType], proxyId );+		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->flags & b3_enableCustomFiltering ) || ( shapeB->flags & b3_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];+		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;++	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: clear only the bits that were set this step.+	// Invariant: bit set in movedProxies[type] iff proxyKey is present in moveArray.+	for ( int i = 0; i < bp->moveArray.count; ++i )+	{+		int proxyKey = bp->moveArray.data[i];+		b3ClearBit( &bp->movedProxies[B3_PROXY_TYPE( proxyKey )], B3_PROXY_ID( proxyKey ) );+	}+	b3Array_Clear( bp->moveArray );++	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+}
+ upstream/src/broad_phase.h view
@@ -0,0 +1,79 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "bitset.h"+#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];++	// Per body-type bit sets indexed by proxyId, marking proxies moved this step.+	// Paired with moveArray which preserves deterministic insertion order for pair queries.+	b3BitSet movedProxies[b3_bodyTypeCount];+	b3Array( 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;++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 );++// 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 )+{+	b3BodyType proxyType = B3_PROXY_TYPE( queryProxy );+	int proxyId = B3_PROXY_ID( queryProxy );+	b3BitSet* set = &bp->movedProxies[proxyType];+	if ( b3GetBit( set, proxyId ) == false )+	{+		b3SetBitGrow( set, proxyId );+		b3Array_Push( bp->moveArray, queryProxy );+	}+}
+ upstream/src/capsule.c view
@@ -0,0 +1,385 @@+// 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 );+	}++	float mass = sphereMass + cylinderMass;+	b3Vec3 center = b3MulSV( 0.5f, b3Add( c1, c2 ) );++	b3MassData out;+	out.mass = mass;+	out.center = center;++	// Rotate the central inertia into the shape frame+	out.inertia = b3MulMM( rotation, b3MulMM( inertia, b3Transpose( rotation ) ) );++	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;+}++bool b3OverlapCapsule( const b3Capsule* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy )+{+	b3DistanceInput input;+	input.proxyA = (b3ShapeProxy){ &shape->center1, 2, shape->radius };+	input.proxyB = *proxy;+	input.transform = b3InvMulTransforms( shapeTransform, 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 b3RayCastCapsule( const b3Capsule* shape, const b3RayCastInput* input )+{+	B3_ASSERT( b3IsValidRay( input ) );++	b3Vec3 c1 = shape->center1;+	b3Vec3 c2 = shape->center2;+	float r = shape->radius;++	// Initialize result structure+	b3CastOutput output = { 0 };++	b3Vec3 d = b3Sub( c2, c1 );++	// Fall back to sphere if the capsule is short+	float tol = 0.01f * B3_LINEAR_SLOP;+	float lengthSquared = b3LengthSquared( d );+	if ( lengthSquared < tol * tol )+	{+		b3Vec3 sphereCenter = b3MulSV( 0.5f, b3Add( shape->center1, shape->center2 ) );+		b3Sphere sphere = { sphereCenter, shape->radius };+		return b3RayCastSphere( &sphere, input );+	}++	// Vector from first center to ray origin.+	b3Vec3 s = b3Sub( input->origin, c1 );++	// Capsule axis+	float length = sqrtf( lengthSquared );+	b3Vec3 axis = b3MulSV( 1.0f / length, d );++	// Project ray origin onto capsule axis.+	float u = b3Dot( s, axis );++	// Closest point on infinite capsule axis, relative to c1.+	b3Vec3 c = b3MulSV( u, axis );++	// Vector from closest point to ray origin+	b3Vec3 sc = b3Sub( s, c );++	// Squared distance from ray origin to capsule axis+	float sc2 = b3LengthSquared( sc );++	// Is the ray origin within the infinite cylinder along the capsule axis?+	if ( sc2 < r * r )+	{+		// Clamped barycentric coordinate of ray origin projected onto capsule axis.+		float uClamped = b3ClampFloat( u, 0.0f, length );++		// The closest point on the bounded capsule segment, relative to c1.+		b3Vec3 cp = b3MulSV( uClamped, axis );++		// Vector from ray origin to closest point on segment.+		b3Vec3 scp = b3Sub( s, cp );++		// Squared distance of ray origin from capsule segment.+		float scp2 = b3LengthSquared( scp );++		// Is the ray origin within the capsule?+		if ( scp2 < r * r )+		{+			output.hit = true;+			output.point = input->origin;+			return output;+		}++		// The ray can hit an endcap.+		b3Sphere sphere = {+			.center = b3Add( c1, cp ),+			.radius = r,+		};++		return b3RayCastSphere( &sphere, input );+	}++	// Ray axis. A zero length ray reaching here starts outside the capsule, so it misses.+	// Same zero length convention as b3RayCastSphere.+	b3Vec3 dr = input->translation;+	float rayLength;+	b3Vec3 rayAxis = b3GetLengthAndNormalize( &rayLength, dr );+	if ( rayLength == 0.0f )+	{+		return output;+	}++	// Barycentric coordinate of ray end point.+	float v = u + input->maxFraction * b3Dot( dr, axis );++	// Early out: does the projected ray fall outside the capsule?+	if ( ( u < -r && v < -r ) || ( length + r < u && length + r < v ) )+	{+		return output;+	}++	// Compute the closest point between the ray segment and the capsule segment.+	// See Real-Time Collision Detection, section 5.1.9++	// Closest point on capsule : a1 = segment unit axis, t1 = unknown fraction+	// p1 = t1 * a1++	// Closet point on ray : a2 = ray unit axis, t2 = unknown fraction+	// p2 = s + t2 * a2++	// Closest point perpendicularity conditions.+	// dot(p2 - p1, a1) = 0+	// dot(p2 - p1, a2) = 0++	// Expand+	// dot(t1 * a1 - s - t2 * a2, a1) = 0+	// dot(t1 * a1 - s - t2 * a2, a2) = 0++	// Expand+	// t1 - dot(s, a1) - t2 * dot(a1, a2) = 0+	// t1 * dot(a1, a2) - dot(s, a2) - t2 = 0++	// Group : a12 = dot(a1, a2), sa1 = dot(s, a1), sa2 = dot(s, a2)+	// t1       - a12 * t2 = sa1+	// a12 * t1 -       t2 = sa2++	// Solve+	// https://en.wikipedia.org/wiki/Cramer%27s_rule+	// I've flipped the signs of the numerator and denominator to give a positive determinant.+	// det = 1 - a12 * a12+	// t1 = (sa1 - a12 * sa2) / det+	// t2 = (a12 * sa1 - sa2) / det++	b3Vec3 a1 = axis;+	b3Vec3 a2 = rayAxis;+	float a12 = b3Dot( a1, a2 );++	// Ray distance to the near intersection with the infinite cylinder. Length units.+	float tr;++	float det = 1.0f - a12 * a12;+	if ( det < FLT_EPSILON )+	{+		// Solve the 2D problem of ray versus circle starting at the ray origin, where the circle is+		// the axial view of the infinite capsule cylinder. This works well when the ray origin is+		// not too far from the capsule axis.++		// Instead of a cross product, subtract the parallel part to get a perpendicular vector. Non-dimensional.+		b3Vec3 perp = b3MulSub( a2, a12, a1 );+		float perp2 = b3LengthSquared( perp );++		// Project to origin to infinite capsule axis vector onto the perpendicular vector. beta has length units.+		float beta = b3Dot( sc, perp );++		// Setup quadratic root finder.+		float gamma = sc2 - r * r;++		// Discriminant+		float disc = beta * beta - perp2 * gamma;++		// Casting away from the axis, or the perpendicular gap never closes to the radius.+		if ( beta >= 0.0f || disc < 0.0f )+		{+			return output;+		}++		// Quadratic near root. Expressed in an alternate form to avoid the (-beta - sqrt) cancellation as+		// the ray nears parallel.+		tr = gamma / ( -beta + sqrtf( disc ) );+	}+	else+	{+		// Ray and capsules axes are not parallel.++		// Closest points between the infinite ray and the infinite capsule axis.+		float invDet = 1.0f / det;+		float sa1 = u;+		float sa2 = b3Dot( s, a2 );++		float t1 = ( sa1 - a12 * sa2 ) * invDet;+		float t2 = ( a12 * sa1 - sa2 ) * invDet;++		// Closest points+		b3Vec3 p1 = b3MulSV( t1, a1 );+		b3Vec3 p2 = b3MulAdd( s, t2, a2 );++		// Vector from closest point on infinite capsule to infinite ray.+		b3Vec3 g = b3Sub( p2, p1 );++		float g2 = b3LengthSquared( g );+		if ( g2 > r * r )+		{+			// Early out: closest point on infinite ray is outside infinite cylinder.+			return output;+		}++		// Intersect the infinite ray with the infinite cylinder. Like ray versus sphere this is done+		// relative to the closest point to avoid round-off errors. Length units, not a fraction.+		// https://en.wikipedia.org/wiki/Line-cylinder_intersection+		float h = sqrtf( ( r * r - g2 ) * invDet );++		tr = t2 - h;+	}++	// Outside ray?+	if ( tr < 0.0f || input->maxFraction * rayLength < tr )+	{+		return output;+	}++	// The corresponding distance on the capsule axis. Length units.+	float tc = u + tr * a12;++	// Outside c1 end?+	if ( tc < 0.0f )+	{+		// Ray cast sphere 1.+		b3Sphere sphere = {+			.center = c1,+			.radius = r,+		};++		return b3RayCastSphere( &sphere, input );+	}++	// Outside c2 end?+	if (length < tc)+	{+		// Ray cast sphere 2.+		b3Sphere sphere = {+			.center = c2,+			.radius = r,+		};++		return b3RayCastSphere( &sphere, input );+	}++	// Hit point on capsule side, relative to c1.+	b3Vec3 p = b3MulAdd( s, tr, rayAxis );++	// Hit normal.+	b3Vec3 normal = b3MulSub( p, tc, axis );+	normal = b3Normalize( normal );++	output.point = b3Add( c1, p );+	output.normal = normal;+	output.fraction = b3ClampFloat( tr / rayLength, 0.0f, input->maxFraction );+	output.hit = true;+	return output;+}++b3CastOutput b3ShapeCastCapsule( const b3Capsule* capsule, const b3ShapeCastInput* input )+{+	b3ShapeCastPairInput pairInput;+	pairInput.proxyA = (b3ShapeProxy){ &capsule->center1, 2, capsule->radius };+	pairInput.proxyB = input->proxy;+	pairInput.transform = 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 )+{+	float totalRadius = mover->radius + shape->radius;++	b3SegmentDistanceResult approach = b3SegmentDistance( shape->center1, shape->center2, mover->center1, mover->center2 );++	// The normal points from the shape toward the mover.+	float distance;+	b3Vec3 normal = b3GetLengthAndNormalize( &distance, b3Sub( approach.point2, approach.point1 ) );++	if ( distance > totalRadius )+	{+		return 0;+	}++	float linearSlop = B3_LINEAR_SLOP;+	if ( distance < linearSlop )+	{+		// Deep overlap: the core segments intersect. Pick an arbitrary direction perpendicular+		// the to capsule axis.+		float moverLength;+		b3Vec3 moverAxis = b3GetLengthAndNormalize( &moverLength, b3Sub( mover->center2, mover->center1 ) );+		normal = moverLength > linearSlop ? b3Perp( moverAxis ) : b3Vec3_axisY;+		distance = 0.0f;+	}++	b3Plane plane = { normal, totalRadius - distance };+	*result = (b3PlaneResult){ plane, approach.point1 };+	return 1;+}
+ upstream/src/compound.c view
@@ -0,0 +1,1199 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#include "compound.h"++#include "hull.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 <stdint.h>+#include <string.h>++typedef struct b3SharedHull+{+	const b3HullData* 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( b3CompoundData* compound )+{+	if ( compound->nodeOffset == 0 )+	{+		return NULL;+	}++	return (b3TreeNode*)( (intptr_t)compound + compound->nodeOffset );+}++const b3SurfaceMaterial* b3GetCompoundMaterials( const b3CompoundData* compound )+{+	if ( compound->materialOffset == 0 )+	{+		return NULL;+	}++	return (b3SurfaceMaterial*)( (intptr_t)compound + compound->materialOffset );+}++b3CompoundCapsule b3GetCompoundCapsule( const b3CompoundData* 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 b3CompoundData* 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 b3HullData*)( (intptr_t)compound + hullOffset );+	result.transform = hullInstances[index].transform;+	result.materialIndex = hullInstances[index].materialIndex;+	return result;+}++b3CompoundMesh b3GetCompoundMesh( const b3CompoundData* 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 b3CompoundData* 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 b3CompoundData* 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 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_assert( sizeof( b3SurfaceMaterial ) == 40, "review padding" );++static inline uint64_t b3HashMaterial( const b3SurfaceMaterial* material )+{+	B3_ASSERT( material->padding == 0 );++	return vt_wyhash( material, sizeof( b3SurfaceMaterial ) );+}++static bool b3CompareMaterials( const b3SurfaceMaterial* mat1, const b3SurfaceMaterial* mat2 )+{+	B3_ASSERT( mat1->padding == 0 && mat2->padding == 0);++	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"++b3CompoundData* 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 b3HullData* 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 );++	// Tree nodes+	size_t byteCount = b3AlignUp8( sizeof( b3CompoundData ) );+	int nodeOffset = (int)byteCount;+	byteCount += b3AlignUp8( tree.nodeCapacity * sizeof( b3TreeNode ) );+	int materialOffset = (int)byteCount;+	byteCount += b3AlignUp8( materialCount * sizeof( b3SurfaceMaterial ) );+	int capsuleOffset = (int)byteCount;+	byteCount += b3AlignUp8( 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 = (int)byteCount;++	// Array of hull instances+	byteCount += b3AlignUp8( hullCount * sizeof( b3HullInstance ) );++	// Packed shared hull blobs+	for ( int i = 0; i < sharedHullCount; ++i )+	{+		sharedHulls[i].hullOffset = (int)byteCount;+		byteCount += b3AlignUp8( 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 = (int)byteCount;++	// Array of mesh instances+	byteCount += b3AlignUp8( meshCount * sizeof( b3MeshInstance ) );++	// Packed shared mesh blobs+	for ( int i = 0; i < sharedMeshCount; ++i )+	{+		sharedMeshes[i].meshOffset = (int)byteCount;+		byteCount += b3AlignUp8( sharedMeshes[i].meshData->byteCount );+	}++	int sphereOffset = (int)byteCount;+	byteCount += b3AlignUp8( def->sphereCount * sizeof( b3CompoundSphere ) );++	b3CompoundData* compound = b3Alloc( byteCount );+	memset( compound, 0, byteCount );++	compound->version = B3_COMPOUND_VERSION;+	compound->byteCount = (int)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;+		b3HullData* destinationHull = (b3HullData*)( (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( b3CompoundData* compound )+{+	b3Free( compound, compound->byteCount );+}++uint8_t* b3ConvertCompoundToBytes( b3CompoundData* compound )+{+	// scrub this pointer before serialization+	compound->tree.nodes = NULL;+	return (uint8_t*)compound;+}++b3CompoundData* b3ConvertBytesToCompound( uint8_t* bytes, int byteCount )+{+	b3CompoundData* compound = (b3CompoundData*)bytes;+	if ( compound->version != B3_COMPOUND_VERSION )+	{+		return NULL;+	}++	if ( compound->byteCount < (int)sizeof( b3CompoundData ) )+	{+		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 b3CompoundData* 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 b3CompoundData* 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 b3CompoundData* 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 b3CompoundData* compound;+	b3CastOutput* output;+	// origin of the shape cast, the box cast callback only carries the advancing fraction+	const b3ShapeCastInput* shapeInput;+};++static float b3CompoundRayCastCallback( const b3RayCastInput* input, int proxyId, uint64_t userData, void* context )+{+	B3_UNUSED( proxyId );++	struct b3CompoundCastContext* castContext = context;+	const b3CompoundData* 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 b3CompoundData* 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 b3BoxCastInput* input, int proxyId, uint64_t userData, void* context )+{+	B3_UNUSED( proxyId );++	struct b3CompoundCastContext* castContext = context;+	const b3CompoundData* compound = castContext->compound;+	const b3ShapeCastInput* shapeInput = castContext->shapeInput;++	int childIndex = (int)userData;++	b3ChildShape child = b3GetCompoundChild( compound, childIndex );++	// Rebuild from the carried shape cast input, taking only the advancing fraction from the tree+	b3ShapeCastInput localInput = *shapeInput;+	localInput.maxFraction = input->maxFraction;+	b3Vec3 localPoints[B3_MAX_SHAPE_CAST_POINTS];++	localInput.proxy.count = b3MinInt( shapeInput->proxy.count, B3_MAX_SHAPE_CAST_POINTS );++	b3Transform invTransform = b3InvertTransform( child.transform );+	b3Matrix3 R = b3MakeMatrixFromQuat( invTransform.q );++	for ( int i = 0; i < localInput.proxy.count; ++i )+	{+		localPoints[i] = b3Add( b3MulMV( R, shapeInput->proxy.points[i] ), invTransform.p );+	}++	localInput.proxy.points = localPoints;+	localInput.translation = b3MulMV( R, shapeInput->translation );++	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 b3CompoundData* shape, const b3ShapeCastInput* input )+{+	b3CastOutput result = { 0 };++	if ( input->proxy.count == 0 )+	{+		return result;+	}++	struct b3CompoundCastContext context = {+		.compound = shape,+		.output = &result,+		.shapeInput = input,+	};++	// The compound tree is in the compound local frame, so the proxy box needs no origin offset+	b3AABB box = b3MakeAABB( input->proxy.points, input->proxy.count, input->proxy.radius );+	b3BoxCastInput treeInput = { box, input->translation, input->maxFraction };+	(void)b3DynamicTree_BoxCast( &shape->tree, &treeInput, ~0ull, false, b3CompoundShapeCastCallback, &context );+	return result;+}++struct b3CompoundQueryContext+{+	const b3CompoundData* 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 b3CompoundData* 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 b3CompoundData* 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 b3CompoundData* 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 b3CompoundData* 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 b3CompoundData* 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;+}
+ upstream/src/compound.h view
@@ -0,0 +1,14 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "box3d/types.h"++b3TOIOutput b3CompoundTimeOfImpact( const b3CompoundData* 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 b3CompoundData* shape, const b3Capsule* mover );
+ upstream/src/constraint_graph.c view
@@ -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 <string.h>++// 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;+	}+}
+ upstream/src/constraint_graph.h view
@@ -0,0 +1,77 @@+// 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+	b3Array( b3JointSim ) jointSims;++	b3Array( int ) convexContacts;+	b3Array( 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;++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 );
+ upstream/src/contact.c view
@@ -0,0 +1,883 @@+// 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;++	// Both bodies must enable recycling+	if ( ( bodyA->flags & b3_bodyEnableContactRecycling ) != 0 && ( bodyB->flags & b3_bodyEnableContactRecycling ) != 0 )+	{+		contact->flags |= b3_contactRecycleFlag;+	}++	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->flags & b3_enableContactEvents ) || ( shapeB->flags & b3_enableContactEvents ) )+	{+		contact->flags |= b3_contactEnableContactEvents;+	}++	if ( ( shapeA->flags & b3_enableSpeculative ) && ( shapeB->flags & b3_enableSpeculative ) )+	{+		contact->flags |= b3_enableSpeculativePoints;+	}++	// 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( b3GetShapeMaterials( shapeA )[0].rollingResistance, b3GetShapeMaterials( shapeB )[0].rollingResistance ) *+		maxRadius;++	if ( ( shapeA->flags & b3_enablePreSolveEvents ) || ( shapeB->flags & b3_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,+									 b3WorldTransform xfA, const b3Shape* shapeB, b3WorldTransform 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 = b3InvMulWorldTransforms( 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, b3SubPos( 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, b3WorldTransform xfA,+								   b3Shape* shapeB, b3WorldTransform 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 = b3GetShapeMaterials( shapeA );+	const b3SurfaceMaterial* materialB = b3GetShapeMaterials( shapeB );++	// 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+		b3Pos point = b3OffsetPos( xfA.p, contact->manifolds[0].points[0].anchorA );+		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->flags & b3_enableHitEvents ) || ( shapeB->flags & b3_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,+					  b3WorldTransform xfA, b3Shape* shapeB, b3Vec3 localCenterB, b3WorldTransform 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;++		// Handle child material for non-meshes.+		if ( child.type != b3_meshShape )+		{+			B3_ASSERT( 0 <= child.materialIndices[0] && child.materialIndices[0] < shapeA->materialCount );+			const b3SurfaceMaterial* parentMaterials = b3GetShapeMaterials( shapeA );+			childShapeA.material = parentMaterials[child.materialIndices[0]];+			childShapeA.materials = NULL;+			childShapeA.materialCount = 1;+		}++		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;+			b3WorldTransform xfChild = b3MulWorldTransforms( 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;+			b3WorldTransform xfChild = b3MulWorldTransforms( xfA, child.transform );++			touching = b3ComputeMeshManifolds( world, workerIndex, contact, &childShapeA, child.materialIndices, xfChild, shapeB,+											   xfB, isFast, arena );++			if ( touching && ( ( shapeA->flags & b3_enableHitEvents ) || ( shapeB->flags & b3_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->flags & b3_enableHitEvents ) || ( shapeB->flags & b3_enableHitEvents ) ) )+		{+			contact->flags |= b3_simEnableHitEvent;+		}+		else+		{+			contact->flags &= ~b3_simEnableHitEvent;+		}++		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;+}
+ upstream/src/contact.h view
@@ -0,0 +1,183 @@+// 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,++	b3_contactRecycleFlag = 0x00000010,++	// 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,++	// Relative transform is cached for contact recycling+	b3_relativeTransformValid = 0x00800000,++	// Enable speculative contact points+	b3_enableSpeculativePoints = 0x01000000,+};++// 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+{+	b3Array( 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;++	// Cache for contact recycling.+	b3Quat cachedRotationA;+	b3Quat cachedRotationB;+	b3Transform cachedRelativePose;++	// 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 );++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, b3WorldTransform xfA,+					  b3Shape* shapeB, b3Vec3 localCenterB, b3WorldTransform xfB, bool isFast, b3Arena arena );++bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact, const b3Shape* shapeA, const int* materialMap,+							 b3WorldTransform xfA, const b3Shape* shapeB, b3WorldTransform xfB, bool isFast, b3Arena arena );
+ upstream/src/contact_solver.c view
@@ -0,0 +1,2124 @@+// 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 "simd.h"+#include "solver_set.h"++#if B3_ENABLE_VALIDATION+#include "shape.h"+#endif++// 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;++	// Used for friction center weighting.+	float invTau = 1.0f / B3_SPECULATIVE_DISTANCE;++	// 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;++	// Overflow constraints are stored separately+	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 );+			B3_ASSERT( contact->contactId == 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;+				float totalFrictionWeight = 0.0f;++				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;++					float s = mp->separation;+					cp->baseSeparation = s - 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 ) );++					// C0 friction center decay. Needed to prevent spinning top drift (GyroscopicPrecession sample).+					// Contacts with separation greater than twice the speculative distance only matter for CCD and+					// should not contribute to the friction center. They are not important for jitter reduction. Closer+					// points may begin to touch on and off, so the friction center needs to move smoothly.+					// Epsilon to avoid a branch below (or divide by zero). Small enough to get washed out normally.+					float weight = b3ClampFloat( 2.0f - s * invTau, B3_MIN_FRICTION_WEIGHT, 1.0f );+					centerA = b3MulAdd( centerA, weight, rA );+					centerB = b3MulAdd( centerB, weight, rB );+					totalFrictionWeight += weight;+				}++				float invWeight = 1.0f / totalFrictionWeight;+				centerA = b3MulSV( invWeight, centerA );+				centerB = b3MulSV( invWeight, centerB );+				constraint->centerA = centerA;+				constraint->centerB = 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->centerA;+				b3Vec3 rB = constraint->centerB;+				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->centerA;+				b3Vec3 rB = constraint->centerB;++				// 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;++	// Mirror b3PrepareContacts_Mesh: the per-color flat arrays and the overflow color+	// each have their own (base, spans, manifoldBase).+	b3ContactPrepareSpan* spans = context->contactPrepareSpans;+	b3ContactConstraint* base = context->contactConstraints;++	if ( block.blockType == b3_overflowBlock )+	{+		b3GraphColor* overflow = world->constraintGraph.colors + B3_OVERFLOW_INDEX;+		spans = context->overflowSpans;+		base = overflow->contactConstraints;+	}++	b3TaskContext* taskContext = world->taskContexts.data + workerIndex;+	b3BitSet* hitEventBitSet = &taskContext->hitEventBitSet;+	bool hasHitEvents = taskContext->hasHitEvents;+	float negHitThreshold = -world->hitEventThreshold;++	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 );++		// Loop over color+		for ( ; index < colorEndIndex; ++index )+		{+			b3ContactConstraint* contactConstraint = base + index;++			int localIndex = index - colorStart;+			B3_UNUSED( localIndex );+			B3_ASSERT( 0 <= localIndex && localIndex < spans[colorIndex].count );++			// Having this contact pointer simplifies impulse storage+			b3Contact* contact = contactConstraint->contact;+			B3_ASSERT( contact != NULL );++			// Catches the wrong-(base, spans) pairing: the contact pointer stashed by+			// b3PrepareContacts_Mesh at this flat slot must reference the same contact+			// the span at this slot describes.+			B3_VALIDATE( contact->contactId == spans[colorIndex].contacts[localIndex].contactId );++			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;+					}+				}+			}+		}++		// Advance to next color+		colorIndex += 1;+	}++	taskContext->hasHitEvents = hasHitEvents;+}++// 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;++// 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];++	int pointCounts[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;++	// Friction centers+	b3Vec3W centerA, centerB;+	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;+	float invTau = 1.0f / B3_SPECULATIVE_DISTANCE;++	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;+				constraint->pointCounts[lane] = pointCount;++				b3Vec3 centerA = b3Vec3_zero;+				b3Vec3 centerB = b3Vec3_zero;+				float totalFrictionWeight = 0.0f;++				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;+					float s = mp->separation;++					// C0 friction center decay. Needed to prevent spinning top drift (GyroscopicPrecession sample).+					// See details in b3PrepareContacts_Mesh. This code should stay in sync.+					float weight = b3ClampFloat( 2.0f - s * invTau, B3_MIN_FRICTION_WEIGHT, 1.0f );+					centerA = b3MulAdd( centerA, weight, rA );+					centerB = b3MulAdd( centerB, weight, rB );+					totalFrictionWeight += weight;++					( (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 = s - 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 invWeight = 1.0f / totalFrictionWeight;+				centerA = b3MulSV( invWeight, centerA );+				centerB = b3MulSV( invWeight, centerB );++				( (float*)&constraint->centerA.X )[lane] = centerA.x;+				( (float*)&constraint->centerA.Y )[lane] = centerA.y;+				( (float*)&constraint->centerA.Z )[lane] = centerA.z;+				( (float*)&constraint->centerB.X )[lane] = centerB.x;+				( (float*)&constraint->centerB.Y )[lane] = centerB.y;+				( (float*)&constraint->centerB.Z )[lane] = centerB.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, 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 ) );+					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 );++		_Static_assert( B3_SIMD_WIDTH == 4, "width" );+		int pointCount1 = b3MaxInt( c->pointCounts[0], c->pointCounts[1] );+		int pointCount2 = b3MaxInt( c->pointCounts[2], c->pointCounts[3] );+		int pointCount = b3MaxInt( pointCount1, pointCount2 );+		B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS );++		// Normal impulses+		for ( int j = 0; j < pointCount; ++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->centerA;+			b3Vec3W rB = c->centerB;+			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;++		_Static_assert( B3_SIMD_WIDTH == 4, "width" );+		int pointCount1 = b3MaxInt( c->pointCounts[0], c->pointCounts[1] );+		int pointCount2 = b3MaxInt( c->pointCounts[2], c->pointCounts[3] );+		int pointCount = b3MaxInt( pointCount1, pointCount2 );+		B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS );++		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();++		for ( int pointIndex = 0; pointIndex < pointCount; ++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->centerA;+				b3Vec3W rB = c->centerB;++				// 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;+		}++		_Static_assert( B3_SIMD_WIDTH == 4, "width" );+		int pointCount1 = b3MaxInt( c->pointCounts[0], c->pointCounts[1] );+		int pointCount2 = b3MaxInt( c->pointCounts[2], c->pointCounts[3] );+		int pointCount = b3MaxInt( pointCount1, pointCount2 );+		B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS );++		// 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 < pointCount; ++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,+	};++	b3StoreImpulses_Mesh( block, context, 0 );+}
+ upstream/src/contact_solver.h view
@@ -0,0 +1,73 @@+// 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+{+	b3ManifoldConstraintPoint points[4];+	int pointCount;+	b3Vec3 normal;+	b3Vec3 tangent1;+	b3Vec3 tangent2;+	// Friction centers+	b3Vec3 centerA, centerB;+	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;++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 );
+ upstream/src/container.h view
@@ -0,0 +1,184 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "algorithm.h"+#include "core.h"++#include <stddef.h>+#include <string.h>++#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:+// b3Array(int) myArray = { 0 };+#define b3Array( 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 ? 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 = 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 = 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 )                                                                                                     \+	( 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. _n is used to cache the input count while avoiding naming conflicts.+#define b3Array_Append( a, src, n )                                                                                              \+	do                                                                                                                           \+	{                                                                                                                            \+		int _n = ( n );                                                                                                          \+		if ( ( a ).count + _n > ( a ).capacity )                                                                                 \+		{                                                                                                                        \+			int req = ( a ).count + _n;                                                                                          \+			int newCapacity = req > 2 ? req + ( req >> 1 ) : 8;                                                                  \+			int oldSize = ( a ).capacity * sizeof( *( a ).data );                                                                \+			int newSize = newCapacity * sizeof( *( a ).data );                                                                   \+			( a ).data = b3GrowAlloc( ( a ).data, oldSize, newSize );                                  \+			( a ).capacity = newCapacity;                                                                                        \+		}                                                                                                                        \+		memcpy( ( a ).data + ( a ).count, ( src ), _n * sizeof( *( a ).data ) );                                                 \+		( a ).count += _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 );
+ upstream/src/convex_manifold.c view
@@ -0,0 +1,2588 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#include "algorithm.h"+#include "manifold.h"+#include "shape.h"+#include "simd.h"++#include "box3d/base.h"+#include "box3d/collision.h"+#include "box3d/constants.h"++#include <stdbool.h>+#include <stddef.h>++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 b3HullData* 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 b3SeparatingAxis b3QueryFaceDirectionHullAndCapsule( const b3HullData* 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 (b3SeparatingAxis){+		.normal = planes[maxFaceIndex].normal,+		.separation = maxFaceSeparation,+		.indexA = (uint8_t)maxFaceIndex,+		.indexB = (uint8_t)maxVertexIndex,+	};+}++static b3SeparatingAxis b3QueryEdgeDirectionHullAndCapsule( const b3HullData* hull, const b3Capsule* capsule,+															b3Transform capsuleTransform )+{+	// Find axis of minimum penetration+	b3Vec3 maxNormal = b3Vec3_zero;+	float maxSeparation = -FLT_MAX;+	int maxIndexA = B3_NULL_INDEX;+	int maxIndexB = B3_NULL_INDEX;++	// We perform all computations in local space of the hull+	b3Vec3 pA = b3TransformPoint( capsuleTransform, capsule->center1 );+	b3Vec3 qA = b3TransformPoint( capsuleTransform, capsule->center2 );+	b3Vec3 eA = b3Sub( qA, pA );++	const b3HullHalfEdge* edges = b3GetHullEdges( hull );+	const b3Vec3* points = b3GetHullPoints( hull );+	const b3Plane* planes = b3GetHullPlanes( hull );+	float squaredTolerance = B3_PARALLEL_EDGE_TOL * B3_PARALLEL_EDGE_TOL;++	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 qB = points[twin->origin];+		b3Vec3 uB = planes[edge->face].normal;+		b3Vec3 vB = planes[twin->face].normal;++		// 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 plane test.+		float cba = b3Dot( uB, eA );+		float dba = b3Dot( vB, eA );++		if ( cba * dba < 0.0f )+		{+			// Avoid nearly parallel edges that may lead to invalid separation values at the noise floor.+			if ( b3MaxFloat( cba * cba, dba * dba ) < squaredTolerance * b3LengthSquared( eA ) )+			{+				continue;+			}++			// The intersection of the arcs on the Gauss map is the edge pair axis. Cast the+			// arc of hull B (from uB to vB) against the plane containing the arc of hull A:+			// dot(uB + t * (vB - uB), eA) == 0+			// then+			// t = cba / (cba - dba)+			//+			// The signs of cba and dba differ (Minkowski test), so the division is safe.+			//+			// The axis generated points from B to A by construction since it lands between+			// two face normals on B. This removes the need to orient the separation axis+			// using the hull centers.+			//+			// The axis is perpendicular to both edges so I can use qA and qB as arbitrary+			// points on edgeA and edgeB to measure the separation.++			float t = cba / ( cba - dba );+			b3Vec3 axis = b3Lerp( uB, vB, t );+			B3_VALIDATE( b3LengthSquared( axis ) > 1000.0f * FLT_MIN );+			axis = b3Normalize( axis );+			float separation = b3Dot( axis, b3Sub( qA, qB ) );++			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.+				maxNormal = axis;+				maxSeparation = separation;+				maxIndexA = 0;+				maxIndexB = index;+			}+		}+	}++	// Save result+	return (b3SeparatingAxis){+		.normal = maxNormal,+		.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 = b3PointToSegmentDistance( 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 b3HullData* 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){ &center, 1, 0.0f };+	distanceInput.transform = 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;++	b3SegmentDistanceResult result = b3SegmentDistance( 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 = b3PointToSegmentDistance( centerA1, centerA2, verticesB[0].position );+			b3Vec3 closestPoint2 = b3PointToSegmentDistance( 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( verticesB[0].position, closestPoint1 ) );+				b3Vec3 normal2 = b3MulSV( 1.0f / distance2, b3Sub( verticesB[1].position, closestPoint2 ) );+				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 b3HullData* hullA, const b3Capsule* capsuleB,+											  b3Transform transformBtoA, b3SeparatingAxis 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.indexA;+	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 bool b3BuildHullAndCapsuleEdgeContact( b3LocalManifold* manifold, int capacity, const b3HullData* hullA,+											  const b3Capsule* capsuleB, b3Transform transformBtoA, b3SeparatingAxis 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 ph = points[edge2->origin];+	b3Vec3 qh = points[twin2->origin];+	b3Vec3 eh = b3Sub( qh, ph );++	b3Vec3 normal = query.normal;++	b3SegmentDistanceResult result = b3LineDistance( 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 b3HullData* 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.transform = 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++	b3SeparatingAxis faceQuery = b3QueryFaceDirectionHullAndCapsule( hullA, capsuleB, transformBtoA );+	if ( faceQuery.separation > capsuleB->radius )+	{+		// We found a separating axis+		return;+	}++	b3SeparatingAxis 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 );+	B3_VALIDATE( manifold->pointCount == 0 || manifold->pointCount == 2 );+	if ( manifold->pointCount == 2 )+	{+		// This becomes the clipped separation.+		faceSeparation = b3MinFloat( manifold->points[0].separation, manifold->points[1].separation );+	}++	// Is there a valid edge-edge axis?+	if ( edgeQuery.indexA == B3_NULL_INDEX )+	{+		return;+	}++	// 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!+	float linearSlop = B3_LINEAR_SLOP;+	float edgeSeparation = edgeQuery.separation - capsuleB->radius;+	if ( manifold->pointCount == 0 || edgeSeparation > faceSeparation + linearSlop )+	{+		// Edge contact+		b3BuildHullAndCapsuleEdgeContact( manifold, capacity, hullA, capsuleB, transformBtoA, edgeQuery );+	}+}++static int b3BuildPolygon( b3ClipVertex* out, b3Transform transform, const b3HullData* 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 b3HullData* hullA, const b3HullData* hullB,+								 b3Transform transformBtoA, b3SeparatingAxis query, b3SATCache* cache )+{+	B3_VALIDATE( query.type == b3_faceAxisA );+	B3_VALIDATE( 0 <= query.indexA && query.indexA < hullA->faceCount );+	B3_VALIDATE( 0 <= query.indexB && query.indexB < hullB->vertexCount );++	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.indexA;+	b3Plane refPlane = planesA[refFace];++	// Find incident face+	b3Vec3 refNormalInB = b3InvRotateVector( transformBtoA.q, refPlane.normal );+	int incFace = b3FindIncidentFace( hullB, refNormalInB, query.indexB );++	// 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->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.indexA;+	cache->indexB = (uint8_t)query.indexB;++	return true;+}++static bool b3BuildFaceBContact( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB,+								 b3Transform transformBtoA, b3SeparatingAxis query, b3SATCache* cache )+{+	B3_VALIDATE( query.type == b3_faceAxisB );++	b3Transform transformAtoB = b3InvertTransform( transformBtoA );+	b3SeparatingAxis flippedQuery = {+		.normal = b3Neg( query.normal ),+		.separation = query.separation,+		.indexA = query.indexB,+		.indexB = query.indexA,+		.type = b3_faceAxisA,+	};++	bool touching = b3BuildFaceAContact( manifold, capacity, hullB, hullA, transformAtoB, flippedQuery, cache );+	if ( touching == false )+	{+		*cache = (b3SATCache){ 0 };+		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 ) );++	// 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 );+	}++	cache->type = (uint8_t)b3_faceAxisB;+	cache->indexA = (uint8_t)query.indexA;+	cache->indexB = (uint8_t)query.indexB;++	return true;+}++static bool b3BuildEdgeContact( b3LocalManifold* manifold, const b3HullData* hullA, const b3HullData* hullB,+								b3Transform transformBtoA, b3SeparatingAxis query, b3SATCache* cache )+{+	B3_VALIDATE( query.type == b3_edgePairAxis );+	B3_VALIDATE( 0 <= query.indexA && query.indexA < hullA->edgeCount );+	B3_VALIDATE( 0 <= query.indexB && query.indexB < hullB->edgeCount );++	// 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 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 = query.normal;+	b3SegmentDistanceResult result = b3LineDistance( 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 ) );+	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;+}++// Transform a SoA point/normal stream (already split into X/Y/Z) by out = -(R*v (+t)).+// The inputs come straight from the hull's stored SoA arrays, so there's no transpose here.+// IsPoint is a template arg so the translation add is only emitted for points.+static inline void b3NegativeTransformFromSoA( b3Matrix3* R, b3Vec3 p, const float* inX, const float* inY, const float* inZ,+											   int n, float* outX, float* outY, float* outZ, bool isPoint )+{+	B3_VALIDATE( ( (uintptr_t)outX & 0xF ) == 0 );+	B3_VALIDATE( ( (uintptr_t)outY & 0xF ) == 0 );+	B3_VALIDATE( ( (uintptr_t)outZ & 0xF ) == 0 );++	// row-column+	b3FloatW r00 = b3SplatW( R->cx.x );+	b3FloatW r01 = b3SplatW( R->cy.x );+	b3FloatW r02 = b3SplatW( R->cz.x );+	b3FloatW r10 = b3SplatW( R->cx.y );+	b3FloatW r11 = b3SplatW( R->cy.y );+	b3FloatW r12 = b3SplatW( R->cz.y );+	b3FloatW r20 = b3SplatW( R->cx.z );+	b3FloatW r21 = b3SplatW( R->cy.z );+	b3FloatW r22 = b3SplatW( R->cz.z );++	b3FloatW tx = b3ZeroW();+	b3FloatW ty = b3ZeroW();+	b3FloatW tz = b3ZeroW();++	if ( isPoint )+	{+		tx = b3SplatW( p.x );+		ty = b3SplatW( p.y );+		tz = b3SplatW( p.z );+	}++	for ( int i = 0; i < n; i += 4 )+	{+		b3FloatW x = b3LoadW( inX + i );+		b3FloatW y = b3LoadW( inY + i );+		b3FloatW z = b3LoadW( inZ + i );++		// Rotate four vectors at a time+		b3FloatW ox = b3Dot3W( r00, r01, r02, x, y, z );+		b3FloatW oy = b3Dot3W( r10, r11, r12, x, y, z );+		b3FloatW oz = b3Dot3W( r20, r21, r22, x, y, z );++		if ( isPoint )+		{+			ox = b3AddW( ox, tx );+			oy = b3AddW( oy, ty );+			oz = b3AddW( oz, tz );+		}++		b3StoreW( outX + i, b3NegW( ox ) );+		b3StoreW( outY + i, b3NegW( oy ) );+		b3StoreW( outZ + i, b3NegW( oz ) );+	}+}++_Static_assert( B3_MAX_HULL_VERTICES == 128, "must be 128" );++#define B3_HULL_BIT_COUNT 7++// SIMD support point calculation using a SoA vertex array padded with repeats of the first vertex+// to a multiple of 4.+//+// This minimizes (bias - dot), where the caller is expected to provide a bias that makes this always positive.+// It can be direction dependent. The bias should be just big enough to ensure the value if positive because+// an excessive bias causes a precision loss in the support calculation.+//+// The vertex index is embedded in the low B3_HULL_BIT_COUNT mantissa bits of the value. By minimizing a value that+// is always positive, the minimum carries the smallest index so that padded SoA values will never win. This is+// purpose of using the bias instead of maximizing the dot directly.+//+// The support is then recomputed exactly as dot(normal, vertex), without the embedded index.+// todo consider using this for GJK+static inline void b3GetSupportWide( b3Vec3 normal, const float* vx, const float* vy, const float* vz, int n, float bias,+									 float* support, int* vertexIndex )+{+	const b3FloatW nx = b3SplatW( normal.x );+	const b3FloatW ny = b3SplatW( normal.y );+	const b3FloatW nz = b3SplatW( normal.z );+	const b3FloatW biasV = b3SplatW( bias );++	// Start the minimum at a large value.+	b3FloatW minValue = b3SplatW( B3_HUGE );++	// Tail lanes hold vertex 0 with index bits >= vertexCount, so they never become the min value.+	for ( int i = 0; i < n; i += 4 )+	{+		b3FloatW x = b3LoadW( vx + i );+		b3FloatW y = b3LoadW( vy + i );+		b3FloatW z = b3LoadW( vz + i );+		b3FloatW d = b3AddW( b3MulW( nz, z ), b3AddW( b3MulW( ny, y ), b3MulW( nx, x ) ) );++		// This is always positive.+		b3FloatW value = b3SubW( biasV, d );+		b3FloatW augmentedValue = b3EmbedIndexW( value, i, B3_HULL_BIT_COUNT );+		minValue = b3MinW( minValue, augmentedValue );+	}++	// One horizontal min, the winning lane's value and index bits ride through.+	int vi = b3MinIndexW( minValue, B3_HULL_BIT_COUNT );++	// Exact support for the chosen vertex.+	*vertexIndex = vi;++	// Dot product+	*support = normal.x * vx[vi] + normal.y * vy[vi] + normal.z * vz[vi];+}++#define NE ( B3_MAX_HULL_EDGES + 4 )+#define NF ( B3_MAX_HULL_FACES + 4 )+#define NV ( B3_MAX_HULL_VERTICES + 4 )++// SIMD separating axis test based on an implementation developed by Cairn Overturf.+// See his article: https://cairno.substack.com/p/improvements-to-the-separating-axis+b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, bool earlyReturn )+{+	b3Matrix3 R = b3MakeMatrixFromQuat( xfB.q );+	b3Matrix3 invR = b3Transpose( R );++	float speculativeDistance = B3_SPECULATIVE_DISTANCE;++	b3AxisQuery res = {+		.faceA =+			{+				.normal = b3Vec3_zero,+				.separation = -INFINITY,+				.indexA = B3_NULL_INDEX,+				.indexB = B3_NULL_INDEX,+				.type = b3_faceAxisA,+			},+		.faceB =+			{+				.normal = b3Vec3_zero,+				.separation = -INFINITY,+				.indexA = B3_NULL_INDEX,+				.indexB = B3_NULL_INDEX,+				.type = b3_faceAxisB,+			},+		.edge =+			{+				.normal = b3Vec3_zero,+				.separation = -INFINITY,+				.indexA = B3_NULL_INDEX,+				.indexB = B3_NULL_INDEX,+				.type = b3_edgePairAxis,+			},+		.separatedFeature = b3_invalidAxis,+	};++	int faceCountA = hullA->faceCount;+	const b3Plane* planesA = b3GetHullPlanes( hullA );++	int soaVertexCountB = ( hullB->vertexCount + 3 ) & ~3;+	const float* vxB = b3GetHullSoaVertices( hullB );+	const float* vyB = vxB + soaVertexCountB;+	const float* vzB = vyB + soaVertexCountB;++	b3Vec3 cB = b3AABB_Center( hullB->aabb );+	b3Vec3 hB = b3AABB_Extents( hullB->aabb );++	// Test A's face planes against B's vertices.+	for ( int i = 0; i < faceCountA; ++i )+	{+		b3Plane plane = planesA[i];+		b3Vec3 direction = b3Neg( b3MulMV( invR, plane.normal ) );+		float planeSeparation = b3Dot( plane.normal, xfB.p ) - plane.offset;+		float biasB = b3Dot( direction, cB ) + 1.0625f * b3Dot( b3Abs( direction ), hB );+		float support;+		int vertexIndex;+		b3GetSupportWide( direction, vxB, vyB, vzB, soaVertexCountB, biasB, &support, &vertexIndex );+		float separation = planeSeparation - support;+		if ( separation > res.faceA.separation )+		{+			res.faceA.normal = plane.normal;+			res.faceA.separation = separation;+			res.faceA.indexA = i;+			res.faceA.indexB = vertexIndex;+			if ( separation > speculativeDistance && earlyReturn )+			{+				res.separatedFeature = b3_faceAxisA;+				return res;+			}+		}+	}++	int faceCountB = hullB->faceCount;+	const b3Plane* planesB = b3GetHullPlanes( hullB );++	int soaVertexCountA = ( hullA->vertexCount + 3 ) & ~3;+	const float* vxA = b3GetHullSoaVertices( hullA );+	const float* vyA = vxA + soaVertexCountA;+	const float* vzA = vyA + soaVertexCountA;++	b3Vec3 cA = b3AABB_Center( hullA->aabb );+	b3Vec3 hA = b3AABB_Extents( hullA->aabb );++	// Test B's face planes against A's vertices.+	for ( int i = 0; i < faceCountB; ++i )+	{+		b3Plane plane = planesB[i];+		b3Vec3 direction = b3Neg( b3MulMV( R, plane.normal ) );+		float planeSeparation = b3Dot( direction, xfB.p ) - plane.offset;+		float biasA = b3Dot( direction, cA ) + 1.0625f * b3Dot( b3Abs( direction ), hA );+		float support;+		int vertexIndex;+		b3GetSupportWide( direction, vxA, vyA, vzA, soaVertexCountA, biasA, &support, &vertexIndex );+		float separation = planeSeparation - support;+		if ( separation > res.faceB.separation )+		{+			res.faceB.normal = direction;+			res.faceB.separation = separation;+			res.faceB.indexA = vertexIndex;+			res.faceB.indexB = i;+			// This points from A to B and is in frame A+			if ( separation > speculativeDistance && earlyReturn )+			{+				res.separatedFeature = b3_faceAxisB;+				return res;+			}+		}+	}++	// Transform B into A's space once, into SoA arrays. Extra space so+	// tail can be set to zero in all cases.+	_Static_assert( ( B3_MAX_HULL_EDGES & 3 ) == 0, "must be multiple of 4" );+	_Static_assert( ( B3_MAX_HULL_FACES & 3 ) == 0, "must be multiple of 4" );+	_Static_assert( ( B3_MAX_HULL_VERTICES & 3 ) == 0, "must be multiple of 4" );++	// The alignments below are not necessary, but they don't hurt.++	// B face normals in A space, negated.+	_Alignas( 16 ) float bFNx[NF];+	_Alignas( 16 ) float bFNy[NF];+	_Alignas( 16 ) float bFNz[NF];++	// B vertices in A space, negated.+	_Alignas( 16 ) float bWx[NV];+	_Alignas( 16 ) float bWy[NV];+	_Alignas( 16 ) float bWz[NV];++	int soaFaceCountB = ( faceCountB + 3 ) & ~3;+	const float* nxB = b3GetHullSoaNormals( hullB );+	const float* nyB = nxB + soaFaceCountB;+	const float* nzB = nyB + soaFaceCountB;++	b3NegativeTransformFromSoA( &R, xfB.p, nxB, nyB, nzB, soaFaceCountB, bFNx, bFNy, bFNz, false );+	b3NegativeTransformFromSoA( &R, xfB.p, vxB, vyB, vzB, soaVertexCountB, bWx, bWy, bWz, true );++	// Per B edge data. C and D are the two face normals, v0 a vertex, DC the edge vector.+	_Alignas( 16 ) float bCx[NE];+	_Alignas( 16 ) float bCy[NE];+	_Alignas( 16 ) float bCz[NE];+	_Alignas( 16 ) float bDx[NE];+	_Alignas( 16 ) float bDy[NE];+	_Alignas( 16 ) float bDz[NE];+	_Alignas( 16 ) float bV0x[NE];+	_Alignas( 16 ) float bV0y[NE];+	_Alignas( 16 ) float bV0z[NE];+	_Alignas( 16 ) float bDCx[NE];+	_Alignas( 16 ) float bDCy[NE];+	_Alignas( 16 ) float bDCz[NE];++	int halfEdgeCountB = hullB->edgeCount;+	const b3HullHalfEdge* halfEdgesB = b3GetHullEdges( hullB );+	int nb = 0;+	for ( int i = 0; i < halfEdgeCountB; i += 2 )+	{+		const b3HullHalfEdge* edge = halfEdgesB + i;+		const b3HullHalfEdge* twin = edge + 1;+		int f0 = edge->face;+		int f1 = twin->face;+		int v0 = edge->origin;+		int v1 = twin->origin;++		bCx[nb] = bFNx[f0];+		bCy[nb] = bFNy[f0];+		bCz[nb] = bFNz[f0];+		bDx[nb] = bFNx[f1];+		bDy[nb] = bFNy[f1];+		bDz[nb] = bFNz[f1];+		bV0x[nb] = bWx[v0];+		bV0y[nb] = bWy[v0];+		bV0z[nb] = bWz[v0];+		bDCx[nb] = bWx[v1] - bWx[v0];+		bDCy[nb] = bWy[v1] - bWy[v0];+		bDCz[nb] = bWz[v1] - bWz[v0];+		nb += 1;+	}++	// Per A edge data, already in A's space so just gathered. n0 and n1 are the two face+	// normals, d the edge vector av1-av0, v0 the first vertex. Tol is the+	// parallel edge tolerance, scaled by the edge length.+	_Alignas( 16 ) float aN0x[NE];+	_Alignas( 16 ) float aN0y[NE];+	_Alignas( 16 ) float aN0z[NE];+	_Alignas( 16 ) float aN1x[NE];+	_Alignas( 16 ) float aN1y[NE];+	_Alignas( 16 ) float aN1z[NE];+	// dir = av1 - av0+	_Alignas( 16 ) float aDx[NE];+	_Alignas( 16 ) float aDy[NE];+	_Alignas( 16 ) float aDz[NE];+	_Alignas( 16 ) float aV0x[NE];+	_Alignas( 16 ) float aV0y[NE];+	_Alignas( 16 ) float aV0z[NE];+	_Alignas( 16 ) float aTol[NE];++	int halfEdgeCountA = hullA->edgeCount;+	const b3HullHalfEdge* halfEdgesA = b3GetHullEdges( hullA );+	int na = 0;++	float squaredTol = B3_PARALLEL_EDGE_TOL * B3_PARALLEL_EDGE_TOL;+	for ( int i = 0; i < halfEdgeCountA; i += 2 )+	{+		const b3HullHalfEdge* edge = halfEdgesA + i;+		const b3HullHalfEdge* twin = edge + 1;++		b3Vec3 A = planesA[edge->face].normal;+		b3Vec3 B = planesA[twin->face].normal;+		aN0x[na] = A.x;+		aN0y[na] = A.y;+		aN0z[na] = A.z;+		aN1x[na] = B.x;+		aN1y[na] = B.y;+		aN1z[na] = B.z;++		int v0 = edge->origin;+		int v1 = twin->origin;++		aDx[na] = vxA[v1] - vxA[v0];+		aDy[na] = vyA[v1] - vyA[v0];+		aDz[na] = vzA[v1] - vzA[v0];+		aV0x[na] = vxA[v0];+		aV0y[na] = vyA[v0];+		aV0z[na] = vzA[v0];++		aTol[na] = squaredTol * ( aDx[na] * aDx[na] + aDy[na] * aDy[na] + aDz[na] * aDz[na] );+		na += 1;+	}++	// Zero the tail lanes.+	b3FloatW zero = b3ZeroW();+	b3StoreW( aN0x + na, zero );+	b3StoreW( aN0y + na, zero );+	b3StoreW( aN0z + na, zero );+	b3StoreW( aN1x + na, zero );+	b3StoreW( aN1y + na, zero );+	b3StoreW( aN1z + na, zero );+	b3StoreW( aDx + na, zero );+	b3StoreW( aDy + na, zero );+	b3StoreW( aDz + na, zero );+	b3StoreW( aV0x + na, zero );+	b3StoreW( aV0y + na, zero );+	b3StoreW( aV0z + na, zero );+	b3StoreW( aTol + na, zero );++	int edgeCountB = halfEdgeCountB / 2;++#if defined( B3_SIMD_NONE )++	// The SIMD emulated version of this code is very slow. This is a purely scalar version+	// for platforms that don't have SIMD capability. It is much faster than SIMD emulation.+	// WARNING: this math needs to match the SIMD version for cross platform determinism.++	const float EPS = -0.0001f;++	for ( int j = 0; j < edgeCountB; ++j )+	{+		float Cx = bCx[j];+		float Cy = bCy[j];+		float Cz = bCz[j];+		float Dx = bDx[j];+		float Dy = bDy[j];+		float Dz = bDz[j];+		float DCx = bDCx[j];+		float DCy = bDCy[j];+		float DCz = bDCz[j];+		float bv0x = bV0x[j];+		float bv0y = bV0y[j];+		float bv0z = bV0z[j];++		for ( int i = 0; i < na; ++i )+		{+			// CBA = C.dir, DBA = D.dir, where dir = B_x_A+			float CBA = Cx * aDx[i] + ( Cy * aDy[i] + Cz * aDz[i] );+			float DBA = Dx * aDx[i] + ( Dy * aDy[i] + Dz * aDz[i] );+			if ( CBA * DBA >= EPS )+			{+				continue;+			}++			// ADC = n0.DC, BDC = n1.DC, where DC = D_x_C+			float ADC = aN0x[i] * DCx + ( aN0y[i] * DCy + aN0z[i] * DCz );+			float BDC = aN1x[i] * DCx + ( aN1y[i] * DCy + aN1z[i] * DCz );+			if ( ADC * BDC >= EPS || CBA * BDC >= EPS )+			{+				continue;+			}++			// Reject near parallel edges+			float maxCD = b3MaxFloat( CBA * CBA, DBA * DBA );+			if ( maxCD <= aTol[i] )+			{+				continue;+			}++			// t = -CBA / (DBA - CBA)+			float t = -CBA / ( DBA - CBA );++			// normal = lerp(t, C, D) = C + (D-C)*t+			float nx = Cx + t * ( Dx - Cx );+			float ny = Cy + t * ( Dy - Cy );+			float nz = Cz + t * ( Dz - Cz );+			float len2 = nx * nx + ( ny * ny + nz * nz );+			float inv = 1.0f / sqrtf( len2 );+			nx *= inv;+			ny *= inv;+			nz *= inv;++			// separation = -dot(normal, av0 + bv0)+			float sx = aV0x[i] + bv0x;+			float sy = aV0y[i] + bv0y;+			float sz = aV0z[i] + bv0z;++			float separation = -( sx * nx + ( sy * ny + sz * nz ) );+			if ( separation > res.edge.separation )+			{+				res.edge.normal = (b3Vec3){ nx, ny, nz };+				res.edge.separation = separation;++				// Half edge index+				res.edge.indexA = 2 * i;+				res.edge.indexB = 2 * j;++				if ( separation > speculativeDistance && earlyReturn )+				{+					res.separatedFeature = b3_edgePairAxis;+					return res;+				}+			}+		}+	}++#else++	// Edge phase, one B edge against four A edges at a time, no transforms in the loop.+	const b3FloatW EPS = b3SplatW( -0.0001f );+	const b3FloatW INF = b3SplatW( INFINITY );++	for ( int j = 0; j < edgeCountB; ++j )+	{+		const b3FloatW Cx = b3SplatW( bCx[j] );+		const b3FloatW Cy = b3SplatW( bCy[j] );+		const b3FloatW Cz = b3SplatW( bCz[j] );+		const b3FloatW Dx = b3SplatW( bDx[j] );+		const b3FloatW Dy = b3SplatW( bDy[j] );+		const b3FloatW Dz = b3SplatW( bDz[j] );+		const b3FloatW DCx = b3SplatW( bDCx[j] );+		const b3FloatW DCy = b3SplatW( bDCy[j] );+		const b3FloatW DCz = b3SplatW( bDCz[j] );+		const b3FloatW bv0x = b3SplatW( bV0x[j] );+		const b3FloatW bv0y = b3SplatW( bV0y[j] );+		const b3FloatW bv0z = b3SplatW( bV0z[j] );++		for ( int i = 0; i < na; i += 4 )+		{+			b3FloatW n0x = b3LoadW( aN0x + i );+			b3FloatW n0y = b3LoadW( aN0y + i );+			b3FloatW n0z = b3LoadW( aN0z + i );+			b3FloatW n1x = b3LoadW( aN1x + i );+			b3FloatW n1y = b3LoadW( aN1y + i );+			b3FloatW n1z = b3LoadW( aN1z + i );+			b3FloatW dx = b3LoadW( aDx + i );+			b3FloatW dy = b3LoadW( aDy + i );+			b3FloatW dz = b3LoadW( aDz + i );+			b3FloatW v0x = b3LoadW( aV0x + i );+			b3FloatW v0y = b3LoadW( aV0y + i );+			b3FloatW v0z = b3LoadW( aV0z + i );+			b3FloatW tol = b3LoadW( aTol + i );++			// CBA = C.dir, DBA = D.dir, where dir = B_x_A+			b3FloatW CBA = b3Dot3W( Cx, Cy, Cz, dx, dy, dz );+			b3FloatW DBA = b3Dot3W( Dx, Dy, Dz, dx, dy, dz );+			// ADC = n0.DC, BDC = n1.DC, where DC = D_x_C+			b3FloatW ADC = b3Dot3W( n0x, n0y, n0z, DCx, DCy, DCz );+			b3FloatW BDC = b3Dot3W( n1x, n1y, n1z, DCx, DCy, DCz );++			// Gauss map arc crossing test, CBA*DBA<eps and ADC*BDC<eps and CBA*BDC<eps+			b3FloatW m1 = b3LessThanW( b3MulW( CBA, DBA ), EPS );+			b3FloatW m2 = b3LessThanW( b3MulW( ADC, BDC ), EPS );+			b3FloatW m3 = b3LessThanW( b3MulW( CBA, BDC ), EPS );++			// Reject near parallel edges. The arc lerp is ill conditioned when both of B's normals are nearly+			// perpendicular to edge A, a scale invariant sine threshold relative to the edge length.+			b3FloatW maxCD = b3MaxW( b3MulW( CBA, CBA ), b3MulW( DBA, DBA ) );+			b3FloatW notParallel = b3GreaterThanW( maxCD, tol );+			b3FloatW mask = b3AndW( b3AndW( m1, m2 ), b3AndW( m3, notParallel ) );++			// Most A-edges fail the Gauss test, so skip the divide, sqrt and support work when no+			// lane passed.+			if ( b3AnyTrueW( mask ) == false )+			{+				continue;+			}++			// t = -CBA / (DBA - CBA)+			b3FloatW t = b3DivW( b3SubW( zero, CBA ), b3SubW( DBA, CBA ) );++			// normal = lerp(t, C, D) = C + (D-C)*t+			b3FloatW nx = b3MulAddW( Cx, t, b3SubW( Dx, Cx ) );+			b3FloatW ny = b3MulAddW( Cy, t, b3SubW( Dy, Cy ) );+			b3FloatW nz = b3MulAddW( Cz, t, b3SubW( Dz, Cz ) );++			// normalize+			b3FloatW len2 = b3Dot3W( nx, ny, nz, nx, ny, nz );+			b3FloatW inv = b3DivW( b3SplatW( 1.0f ), b3SqrtW( len2 ) );+			nx = b3MulW( nx, inv );+			ny = b3MulW( ny, inv );+			nz = b3MulW( nz, inv );++			// support = dot(normal, av0 + bv0)+			b3FloatW sx = b3AddW( v0x, bv0x );+			b3FloatW sy = b3AddW( v0y, bv0y );+			b3FloatW sz = b3AddW( v0z, bv0z );+			b3FloatW support = b3Dot3W( sx, sy, sz, nx, ny, nz );++			// Lanes that fail the Gauss test can never win.+			support = b3BlendW( INF, support, mask );+			b3FloatW separation = b3SubW( zero, support );++			// Test all 4 supports against the running best at once. If none beats it, skip the+			// store and scalar reduction. res->support only turns negative just before returning,+			// so this never skips a lane that would trigger the early out.+			b3FloatW improves = b3GreaterThanW( separation, b3SplatW( res.edge.separation ) );+			if ( b3AnyTrueW( improves ) == false )+			{+				continue;+			}++			_Alignas( 16 ) float sA[4];+			_Alignas( 16 ) float nxA[4];+			_Alignas( 16 ) float nyA[4];+			_Alignas( 16 ) float nzA[4];+			b3StoreW( sA, separation );+			b3StoreW( nxA, nx );+			b3StoreW( nyA, ny );+			b3StoreW( nzA, nz );++			// Reduce in lane order so ties keep the first edge and the early out takes the first+			// improving support below zero. Padded tail lanes carry +INF support, so they never+			// update or index a->edges out of range.+			for ( int lane = 0; lane < 4; lane++ )+			{+				int ei = i + lane;+				float s = sA[lane];+				if ( s > res.edge.separation )+				{+					res.edge.normal = (b3Vec3){ nxA[lane], nyA[lane], nzA[lane] };+					res.edge.separation = s;++					// Half edge index+					res.edge.indexA = 2 * ei;+					res.edge.indexB = 2 * j;++					if ( s > speculativeDistance && earlyReturn )+					{+						res.separatedFeature = b3_edgePairAxis;+						return res;+					}+				}+			}+		}+	}+#endif++	return res;+}++#undef NE+#undef NF+#undef NV++#define B3_SIMD_COLLIDE_HULLS 1++#if B3_SIMD_COLLIDE_HULLS == 1++void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* 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 );++	cache->hit = 0;++	// Attempt to use the cache to speed up collision+	switch ( cache->type )+	{+		case b3_invalidAxis:+			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 ) );++			// todo use b3GetSupportWide+			int vertexIndex = b3FindHullSupportVertex( hullB, searchDirectionInB );+			b3Vec3 support = b3TransformPoint( transformBtoA, pointsB[vertexIndex] );+			float separation = b3PlaneSeparation( plane, support );++			if ( separation >= speculativeDistance )+			{+				// Cache hit, shapes are separated+				cache->hit = 1;+				return;+			}++			// Attempt face contact using cached feature+			b3SeparatingAxis faceQuery;+			faceQuery.normal = plane.normal;+			faceQuery.separation = 0.0f;+			faceQuery.indexA = cache->indexA;+			faceQuery.indexB = vertexIndex;+			faceQuery.type = b3_faceAxisA;++			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+				cache->hit = 1;+				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 ) );++			// todo use b3GetSupportWide+			int vertexIndex = b3FindHullSupportVertex( hullA, searchDirectionInA );+			b3Vec3 support = b3InvTransformPoint( transformBtoA, pointsA[vertexIndex] );+			float separation = b3PlaneSeparation( plane, support );++			if ( separation >= speculativeDistance )+			{+				// Cache hit, shapes are separated+				cache->hit = 1;+				return;+			}++			// Attempt face contact using cached feature+			b3SeparatingAxis faceQuery;+			faceQuery.normal = b3Neg( plane.normal );+			faceQuery.separation = 0.0f;+			faceQuery.indexA = vertexIndex;+			faceQuery.indexB = cache->indexB;+			faceQuery.type = b3_faceAxisB;++			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+				cache->hit = 1;+				return;+			}+		}+		break;++		case b3_edgePairAxis:+		{+			int indexA = cache->indexA;+			const b3HullHalfEdge* edge1 = edgesA + indexA;+			const b3HullHalfEdge* twin1 = edgesA + indexA + 1;+			B3_ASSERT( edge1->twin == indexA + 1 && twin1->twin == indexA );++			b3Vec3 pA = pointsA[edge1->origin];+			b3Vec3 qA = pointsA[twin1->origin];+			b3Vec3 eA = b3Sub( qA, pA );++			b3Vec3 uA = planesA[edge1->face].normal;+			b3Vec3 vA = planesA[twin1->face].normal;++			int indexB = cache->indexB;+			const b3HullHalfEdge* edge2 = edgesB + indexB;+			const b3HullHalfEdge* twin2 = edgesB + indexB + 1;+			B3_ASSERT( edge2->twin == indexB + 1 && twin2->twin == indexB );++			b3Vec3 pB = b3TransformPoint( transformBtoA, pointsB[edge2->origin] );+			b3Vec3 qB = b3TransformPoint( transformBtoA, pointsB[twin2->origin] );+			b3Vec3 eB = b3Sub( qB, pB );++			b3Vec3 uB = b3RotateVector( transformBtoA.q, planesB[edge2->face].normal );+			b3Vec3 vB = 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)+			float cba = b3Dot( uB, eA );+			float dba = b3Dot( vB, eA );+			float adc = -b3Dot( uA, eB );+			float bdc = -b3Dot( vA, eB );++			if ( cba * dba < 0.0f && adc * bdc < 0.0f && cba * bdc > 0.0f )+			{+				// Avoid nearly parallel edges that may lead to invalid separation values at the noise floor.+				float squaredTolerance = B3_PARALLEL_EDGE_TOL * B3_PARALLEL_EDGE_TOL;+				if ( b3MaxFloat( cba * cba, dba * dba ) >= squaredTolerance * b3LengthSquared( eA ) )+				{+					// Transform reference center of the first hull into local space of the second hull+					float t = cba / ( cba - dba );+					b3Vec3 axis = b3Lerp( uB, vB, t );+					B3_VALIDATE( b3LengthSquared( axis ) > 1000.0f * FLT_MIN );+					axis = b3Normalize( axis );+					float separation = b3Dot( axis, b3Sub( qA, qB ) );++					if ( separation > speculativeDistance )+					{+						// Cache hit, shapes are separated+						cache->hit = 1;+						return;+					}++					// Try to rebuild contact from last features+					b3SeparatingAxis edgeQuery = { 0 };+					edgeQuery.normal = b3Neg( axis );+					edgeQuery.separation = 0.0f;+					edgeQuery.indexA = cache->indexA;+					edgeQuery.indexB = cache->indexB;+					edgeQuery.type = b3_edgePairAxis;++					b3SATCache localCache = { 0 };+					bool touching = b3BuildEdgeContact( manifold, hullA, hullB, transformBtoA, edgeQuery, &localCache );++					// This separation tolerance may have a big impact on performance in some benchmarks.+					if ( touching && b3AbsFloat( cache->separation - localCache.separation ) < linearSlop )+					{+						// Cache hit, contact point generated+						cache->hit = 1;+						return;+					}+				}+			}+		}+		break;++			// This case is for testing+		case b3_manualFaceAxisA:+		{+			b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, false );+			b3SeparatingAxis faceQuery = axisQuery.faceA;+			b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, cache );+			return;+		}++			// This case is for testing+		case b3_manualFaceAxisB:+		{+			b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, false );+			b3SeparatingAxis faceQuery = axisQuery.faceB;+			b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, cache );+			return;+		}++			// This case is for testing+		case b3_manualEdgePairAxis:+		{+			b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, false );+			b3SeparatingAxis edgeQuery = axisQuery.edge;+			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 };++	b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, true );++	if ( axisQuery.separatedFeature != b3_invalidAxis )+	{+		// We found a separating axis+		cache->type = axisQuery.separatedFeature;++		if ( axisQuery.separatedFeature == b3_faceAxisA )+		{+			B3_VALIDATE( axisQuery.faceA.separation > speculativeDistance );+			cache->separation = axisQuery.faceA.separation;+			cache->indexA = (uint8_t)axisQuery.faceA.indexA;+			cache->indexB = (uint8_t)axisQuery.faceA.indexB;+		}+		else if ( axisQuery.separatedFeature == b3_faceAxisB )+		{+			B3_VALIDATE( axisQuery.faceB.separation > speculativeDistance );+			cache->separation = axisQuery.faceB.separation;+			cache->indexA = (uint8_t)axisQuery.faceB.indexA;+			cache->indexB = (uint8_t)axisQuery.faceB.indexB;+		}+		else+		{+			B3_ASSERT( axisQuery.separatedFeature == b3_edgePairAxis );+			B3_VALIDATE( axisQuery.edge.separation > speculativeDistance );+			cache->separation = axisQuery.edge.separation;+			cache->indexA = (uint8_t)axisQuery.edge.indexA;+			cache->indexB = (uint8_t)axisQuery.edge.indexB;+		}+		return;+	}++	B3_VALIDATE( axisQuery.faceA.separation <= speculativeDistance || axisQuery.faceB.separation <= speculativeDistance ||+				 axisQuery.edge.separation <= speculativeDistance );++	if ( axisQuery.faceA.separation > axisQuery.faceB.separation )+	{+		b3SeparatingAxis faceQuery = axisQuery.faceA;+		B3_VALIDATE( 0 <= faceQuery.indexA && faceQuery.indexA < hullA->faceCount );+		B3_VALIDATE( 0 <= faceQuery.indexB && faceQuery.indexB < hullB->vertexCount );++		// Face contact A+		b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, cache );++		B3_VALIDATE( cache->indexA < hullA->faceCount );+		B3_VALIDATE( cache->indexB < hullB->vertexCount );+	}+	else+	{+		b3SeparatingAxis faceQuery = axisQuery.faceB;+		B3_VALIDATE( 0 <= faceQuery.indexA && faceQuery.indexA < hullA->vertexCount );+		B3_VALIDATE( 0 <= faceQuery.indexB && faceQuery.indexB < hullB->faceCount );++		// Face contact B+		b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, cache );++		B3_VALIDATE( cache->indexA < hullA->vertexCount );+		B3_VALIDATE( cache->indexB < hullB->faceCount );+	}++	b3SeparatingAxis edgeQuery = axisQuery.edge;++	if ( edgeQuery.indexA == B3_NULL_INDEX )+	{+		// There are no valid edge pairs (all edges parallel)+		return;+	}++	float clipSeparation = cache->separation;+	float edgeTol = 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 > clipSeparation + edgeTol )+	{+		B3_ASSERT( 0 <= edgeQuery.indexA && edgeQuery.indexA < hullA->edgeCount );+		B3_ASSERT( 0 <= edgeQuery.indexB && edgeQuery.indexB < hullB->edgeCount );++		// Edge contact+		b3LocalManifold edgeManifold = { 0 };+		b3LocalManifoldPoint edgePoint = { 0 };+		edgeManifold.points = &edgePoint;++		b3SATCache edgeCache = { 0 };+		b3BuildEdgeContact( &edgeManifold, hullA, hullB, transformBtoA, edgeQuery, &edgeCache );++		// It is possible with speculation to have vertex-vertex collision that is missed by SAT,+		// so edge contact yields no points. In that case perhaps the face contact has some points.+		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;+			*cache = edgeCache;+		}+	}+}++#else++// todo this code has gone stale, will be deleted soon++typedef struct b3FaceQuery+{+	float separation;+	int faceIndex;+	int vertexIndex;+} b3FaceQuery;++typedef struct b3EdgeQuery+{+	b3Vec3 normal;+	float separation;+	int indexA;+	int indexB;+} b3EdgeQuery;++// Old non-SIMD version. Keeping this for testing and comparisons++static b3FaceQuery b3QueryFaceDirections( const b3HullData* hullA, const b3HullData* 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;+	float speculativeDistance = B3_SPECULATIVE_DISTANCE;++	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;++			if ( separation >= speculativeDistance )+			{+				return (b3FaceQuery){+					.separation = maxFaceSeparation,+					.faceIndex = (uint8_t)maxFaceIndex,+					.vertexIndex = (uint8_t)maxVertexIndex,+				};+			}+		}+	}++	return (b3FaceQuery){+		.separation = maxFaceSeparation,+		.faceIndex = (uint8_t)maxFaceIndex,+		.vertexIndex = (uint8_t)maxVertexIndex,+	};+}++static b3EdgeQuery b3QueryEdgeDirections( const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA )+{+	// Find axis of minimum penetration+	b3Vec3 maxNormal = b3Vec3_zero;+	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 );+	float speculativeDistance = B3_SPECULATIVE_DISTANCE;+	float squaredTolerance = B3_PARALLEL_EDGE_TOL * B3_PARALLEL_EDGE_TOL;++	// 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;++			// See "Collision Detection of Convex Polyhedra Based on Duality Transformation"+			// 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.++			// These are signed volumes with an edge optimization to avoid cross products+			// eA parallel to cross(vA, uA)+			// eB parallel to cross(vB, uB)+			// Since only signs are tested, length doesn't matter.++			float cba = b3Dot( uB, eA );+			float dba = b3Dot( vB, eA );+			float adc = -b3Dot( uA, eB );+			float bdc = -b3Dot( vA, eB );++			if ( cba * dba < 0.0f && adc * bdc < 0.0f && cba * bdc > 0.0f )+			{+				// Avoid nearly parallel edges that may lead to invalid separation values at the noise floor.+				if ( b3MaxFloat( cba * cba, dba * dba ) < squaredTolerance * b3LengthSquared( eA ) )+				{+					continue;+				}++				// The intersection of the arcs on the Gauss map is the edge pair axis. Cast the+				// arc of hull B (from uB to vB) against the plane containing the arc of hull A:+				// dot(uB + t * (vB - uB), eA) == 0+				// then+				// t = cba / (cba - dba)+				//+				// The signs of cba and dba differ (Minkowski test), so the division is safe.+				//+				// The axis generated points from B to A by construction since it lands between+				// two face normals on B. This removes the need to orient the separation axis+				// using the hull centers.+				//+				// The axis is perpendicular to both edges so I can use qA and qB as arbitrary+				// points on edgeA and edgeB to measure the separation.+				float t = cba / ( cba - dba );+				b3Vec3 axis = b3Lerp( uB, vB, t );+				B3_VALIDATE( b3LengthSquared( axis ) > 1000.0f * FLT_MIN );+				axis = b3Normalize( axis );+				float separation = b3Dot( axis, b3Sub( qA, qB ) );++				if ( separation > maxSeparation )+				{+					// Continues to find the maximum separating axis+					// Flip normal so it points from A to B+					maxNormal = b3Neg( axis );+					maxSeparation = separation;+					maxIndexA = indexA;+					maxIndexB = indexB;++					if ( separation >= speculativeDistance )+					{+						// Cache hit, shapes are separated+						return (b3EdgeQuery){+							.normal = maxNormal,+							.separation = maxSeparation,+							.indexA = maxIndexA,+							.indexB = maxIndexB,+						};+					}+				}+			}+		}+	}++	return (b3EdgeQuery){+		.normal = maxNormal,+		.separation = maxSeparation,+		.indexA = maxIndexA,+		.indexB = maxIndexB,+	};+}++void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* 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 indexA = cache->indexA;+			const b3HullHalfEdge* edge1 = edgesA + indexA;+			const b3HullHalfEdge* twin1 = edgesA + indexA + 1;+			B3_ASSERT( edge1->twin == indexA + 1 && twin1->twin == indexA );++			b3Vec3 pA = pointsA[edge1->origin];+			b3Vec3 qA = pointsA[twin1->origin];+			b3Vec3 eA = b3Sub( qA, pA );++			b3Vec3 uA = planesA[edge1->face].normal;+			b3Vec3 vA = planesA[twin1->face].normal;++			int indexB = cache->indexB;+			const b3HullHalfEdge* edge2 = edgesB + indexB;+			const b3HullHalfEdge* twin2 = edgesB + indexB + 1;+			B3_ASSERT( edge2->twin == indexB + 1 && twin2->twin == indexB );++			b3Vec3 pB = b3TransformPoint( transformBtoA, pointsB[edge2->origin] );+			b3Vec3 qB = b3TransformPoint( transformBtoA, pointsB[twin2->origin] );+			b3Vec3 eB = b3Sub( qB, pB );++			b3Vec3 uB = b3RotateVector( transformBtoA.q, planesB[edge2->face].normal );+			b3Vec3 vB = 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)+			float cba = b3Dot( uB, eA );+			float dba = b3Dot( vB, eA );+			float adc = -b3Dot( uA, eB );+			float bdc = -b3Dot( vA, eB );++			if ( cba * dba < 0.0f && adc * bdc < 0.0f && cba * bdc > 0.0f )+			{+				// Avoid nearly parallel edges that may lead to invalid separation values at the noise floor.+				float squaredTolerance = B3_PARALLEL_EDGE_TOL * B3_PARALLEL_EDGE_TOL;+				if ( b3MaxFloat( cba * cba, dba * dba ) >= squaredTolerance * b3LengthSquared( eA ) )+				{+					// Transform reference center of the first hull into local space of the second hull+					float t = cba / ( cba - dba );+					b3Vec3 axis = b3Lerp( uB, vB, t );+					B3_VALIDATE( b3LengthSquared( axis ) > 1000.0f * FLT_MIN );+					axis = b3Normalize( axis );+					float separation = b3Dot( axis, b3Sub( qA, qB ) );++					if ( separation > speculativeDistance )+					{+						// Cache hit, shapes are separated+						return;+					}++					// Try to rebuild contact from last features+					b3EdgeQuery edgeQuery = { 0 };+					edgeQuery.normal = b3Neg( axis );+					edgeQuery.separation = 0.0f;+					edgeQuery.indexA = cache->indexA;+					edgeQuery.indexB = cache->indexB;++					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;+		}+	}+}++#endif
+ upstream/src/core.c view
@@ -0,0 +1,267 @@+// 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 <crtdbg.h>+#include <stdlib.h>+#else+#include <stdlib.h>+#endif++#include "core.h"+#include "rapidhash.h"++#include "box3d/constants.h"+#include "box3d/math_functions.h"++#include <stdarg.h>+#include <string.h>++#ifdef BOX2D_PROFILE++#include <tracy/TracyC.h>+#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 <stdio.h>++// This allows the user to change the length units at runtime+static 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 float b3_stallThreshold = FLT_MAX;++void b3SetStallThreshold( float seconds )+{+	B3_ASSERT( b3IsValidFloat( seconds ) && seconds > 0.0f );+	b3_stallThreshold = seconds;+}++float b3GetStallThreshold( void )+{+	return b3_stallThreshold;+}++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 };+}++bool b3IsDoublePrecision( void )+{+#if defined( BOX3D_DOUBLE_PRECISION )+	return true;+#else+	return false;+#endif+}++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;+}++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 B3_ALIGNMENT (required by spec).+	// https://en.cppreference.com/w/c/memory/aligned_alloc+	int alignedSize = ( ( (int)size - 1 ) | ( B3_ALIGNMENT - 1 ) ) + 1;++	if ( b3_allocFcn != NULL )+	{+		void* ptr = b3_allocFcn( alignedSize, B3_ALIGNMENT );+		b3TracyCAlloc( ptr, size );++		B3_ASSERT( ptr != NULL );+		B3_ASSERT( ( (uintptr_t)ptr & ( B3_ALIGNMENT - 1 ) ) == 0 );++		return ptr;+	}++#ifdef B3_PLATFORM_WINDOWS+	void* ptr = _aligned_malloc( alignedSize, B3_ALIGNMENT );+#elif defined( B3_PLATFORM_ANDROID )+	void* ptr = NULL;+	if ( posix_memalign( &ptr, B3_ALIGNMENT, alignedSize ) != 0 )+	{+		// allocation failed, exit the application+		exit( EXIT_FAILURE );+	}+#else+	void* ptr = aligned_alloc( B3_ALIGNMENT, alignedSize );+#endif++	b3TracyCAlloc( ptr, size );++	B3_ASSERT( ptr != NULL );+	B3_ASSERT( ( (uintptr_t)ptr & ( B3_ALIGNMENT - 1 ) ) == 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 )+{+	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;+}++// Not used. Keeping around in case I need this.+void b3StrCpy( char* dst, int size, const char* src )+{+	B3_ASSERT( size > 0 );++	if ( src != NULL )+	{+#if defined( _MSC_VER )+		strncpy_s( dst, size, src, size - 1 );+#else+		strncpy( dst, src, size - 1 );+		dst[size - 1] = 0;+#endif+	}+	else+	{+		memset( dst, 0, size );+	}+}++uint64_t b3Hash64NonZero( const uint8_t* bytes, int n )+{+	if ( n <= 0 )+	{+		return 1;+	}++	uint64_t h = rapidhash( bytes, n );+	return h == 0 ? 1 : h;+}
+ upstream/src/core.h view
@@ -0,0 +1,163 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "box3d/base.h"++#include <stddef.h>++// 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 <TargetConditionals.h>+	#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 )+	// ARMv7 Neon doesn't have divide or sqrt so cannot be used.+	#if defined( __aarch64__ ) || defined( _M_ARM64 )+		#define B3_SIMD_NEON+	#else+		#define B3_SIMD_NONE+	#endif+		#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 <tracy/TracyC.h>+	#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;++// Minimum memory alignment used for all allocations+#define B3_ALIGNMENT 16++// 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 ) );++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 );++B3_PRINTF_FORMAT( 1, 2 )+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 );++void b3StrCpy( char* dst, int size, const char* src );++uint64_t b3Hash64NonZero( const uint8_t* bytes, int n );
+ upstream/src/ctz.h view
@@ -0,0 +1,134 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "box3d/base.h"++#include <stdbool.h>+#include <stdint.h>++#if defined( _MSC_VER ) && !defined( __clang__ )++#include <intrin.h>++// https://en.wikipedia.org/wiki/Find_first_set++static 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.+static inline uint32_t b3CLZ32( uint32_t value )+{+#if 1++	// 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+}++static 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;+}++static inline int b3PopCount64( uint64_t block )+{+	return (int)__popcnt64( block );+}++#else++static inline uint32_t b3CTZ32( uint32_t block )+{+	return __builtin_ctz( block );+}++static inline uint32_t b3CLZ32( uint32_t value )+{+	return __builtin_clz( value );+}++static inline uint32_t b3CTZ64( uint64_t block )+{+	return __builtin_ctzll( block );+}++static inline int b3PopCount64( uint64_t block )+{+	return __builtin_popcountll( block );+}++#endif++static inline bool b3IsPowerOf2( int x )+{+	return ( x & ( x - 1 ) ) == 0;+}++static inline int b3BoundingPowerOf2( int x )+{+	if ( x <= 1 )+	{+		return 1;+	}++	return 32 - (int)b3CLZ32( (uint32_t)x - 1 );+}++static inline int b3RoundUpPowerOf2( int x )+{+	if ( x <= 1 )+	{+		return 1;+	}++	return 1 << ( 32 - (int)b3CLZ32( (uint32_t)x - 1 ) );+}++static 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;+}
+ upstream/src/distance.c view
@@ -0,0 +1,1904 @@+// 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;+}++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 )+{+	// The query runs in frame A using the relative pose of B in A.+	b3Transform xf = input->transform;++	// Use matrices for faster math+	b3Matrix3 m = b3MakeMatrixFromQuat( xf.q );+	b3Matrix3 mt = b3Transpose( m );++	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 = localPointA;+			distanceOutput.pointB = localPointB;+			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 = localPointA;+			distanceOutput.pointB = localPointB;+			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 );++	// Results stay in frame A+	distanceOutput.pointA = localPointA;+	distanceOutput.pointB = localPointB;+	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.useRadii = false;++	// The whole cast runs in frame A. Advance the relative pose of B in float each iteration,+	// which keeps the math near the local origin and avoids re-relativizing world poses.+	distanceInput.transform = input->transform;++	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 xf = input->transform;+						b3Log( "transform = {{%.9f, %.9f, %.9f}, {{%.9f, %.9f, %.9f}, %.9f}", xf.p.x, xf.p.y, xf.p.z, xf.q.v.x,+							   xf.q.v.y, xf.q.v.z, xf.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.transform.p = b3MulAdd( input->transform.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,+													  b3Vec3 worldNormal, float t1 )+{+	B3_ASSERT( 1 <= cache.count && cache.count <= 3 );+	B3_VALIDATE( b3IsNormalized( worldNormal ) );++	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 = worldNormal;+		}+		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 = worldNormal;+				}+				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( worldNormal ) );++				// Vertex versus edge, use world axis witness+				fcn.type = b3_separationVertices;+				fcn.witness1 = worldNormal;+			}+		}+		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 = worldNormal;+				}+				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.+		b3Transform xfA = b3GetSweepTransform( &sweepA, t1 );+		b3Transform xfB = b3GetSweepTransform( &sweepB, t1 );+		distanceInput.transform = b3InvMulTransforms( xfA, xfB );+		b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 );+		output.distance = distanceOutput.distance;++		// The distance query runs in frame A, project the witness data back to the shifted world+		b3Vec3 worldNormal = b3RotateVector( xfA.q, distanceOutput.normal );+		b3Vec3 worldPointA = b3TransformPoint( xfA, distanceOutput.pointA );+		b3Vec3 worldPointB = b3TransformPoint( xfA, distanceOutput.pointB );++		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( worldPointA, proxyA.radius, worldNormal );+			b3Vec3 pB = b3MulAdd( worldPointB, -proxyB.radius, worldNormal );+			output.point = b3Lerp( pA, pB, 0.5f );+			output.point = b3Add( output.point, origin );+			output.normal = worldNormal;+			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( worldPointA, input->proxyA.radius, worldNormal );+			b3Vec3 pB = b3MulAdd( worldPointB, -input->proxyB.radius, worldNormal );+			output.point = b3Lerp( pA, pB, 0.5f );+			output.point = b3Add( output.point, origin );+			output.normal = worldNormal;+			break;+		}++		// Initialize the separating axis.+		b3SeparationFunction function =+			b3MakeSeparationFunction( cache, &proxyA, &sweepA, &proxyB, &sweepB, worldNormal, 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( worldPointA, input->proxyA.radius, worldNormal );+			b3Vec3 pB = b3MulAdd( worldPointB, -input->proxyB.radius, worldNormal );+			output.point = b3Lerp( pA, pB, 0.5f );+			output.point = b3Add( output.point, origin );+			output.normal = worldNormal;+			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;+}
+ upstream/src/distance_joint.c view
@@ -0,0 +1,579 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#include "body.h"+#include "core.h"+#include "joint.h"+#include "physics_world.h"+#include "recording.h"+#include "solver.h"+#include "solver_set.h"++// needed for dll export+#include "box3d/box3d.h"++void b3DistanceJoint_SetLength( b3JointId jointId, float length )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, DistanceJointSetLength, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, DistanceJointEnableLimit, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, DistanceJointSetLengthRange, jointId, minLength, 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;+	}++	b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );+	b3WorldTransform transformB = b3GetBodyTransform( world, base->bodyIdB );++	b3Pos pA = b3TransformWorldPoint( transformA, base->localFrameA.p );+	b3Pos pB = b3TransformWorldPoint( transformB, base->localFrameB.p );+	b3Vec3 d = b3SubPos( pB, pA );+	float length = b3Length( d );+	return length;+}++void b3DistanceJoint_EnableSpring( b3JointId jointId, bool enableSpring )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, DistanceJointEnableSpring, jointId, 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 );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, DistanceJointSetSpringForceRange, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, DistanceJointSetSpringHertz, jointId, hertz );+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint );+	base->distanceJoint.hertz = hertz;+}++void b3DistanceJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, DistanceJointSetSpringDampingRatio, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, DistanceJointEnableMotor, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, DistanceJointSetMotorSpeed, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, DistanceJointSetMaxMotorForce, jointId, 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;++	b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );+	b3WorldTransform transformB = b3GetBodyTransform( world, base->bodyIdB );++	b3Pos pA = b3TransformWorldPoint( transformA, base->localFrameA.p );+	b3Pos pB = b3TransformWorldPoint( transformB, base->localFrameB.p );+	b3Vec3 d = b3SubPos( 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 = b3SubPos( 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, b3WorldTransform transformA, b3WorldTransform transformB )+{+	B3_ASSERT( base->type == b3_distanceJoint );++	b3DistanceJoint* joint = &base->distanceJoint;++	b3Pos pA = b3TransformWorldPoint( transformA, base->localFrameA.p );+	b3Pos pB = b3TransformWorldPoint( transformB, base->localFrameB.p );++	b3Vec3 axis = b3Normalize( b3SubPos( pB, pA ) );++	if ( joint->minLength < joint->maxLength && joint->enableLimit )+	{+		b3Pos pMin = b3OffsetPos( pA, b3MulSV( joint->minLength, axis ) );+		b3Pos pMax = b3OffsetPos( pA, b3MulSV( 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 )+	{+		b3Pos pRest = b3OffsetPos( pA, b3MulSV( joint->length, axis ) );+		draw->DrawPointFcn( pRest, 4.0f, b3_colorBlue, draw->context );+	}+}
+ upstream/src/dynamic_tree.c view
@@ -0,0 +1,2186 @@+// 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 <float.h>+#include <stdio.h>+#include <string.h>++#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 ) );++	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 ) );++	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_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_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( b3IsValidAABB( 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_BoxCast( const b3DynamicTree* tree, const b3BoxCastInput* input, uint64_t maskBits, bool requireAllBits,+								   b3TreeBoxCastCallbackFcn* callback, void* context )+{+	b3TreeStats stats = { 0 };++	if ( tree->nodeCount == 0 )+	{+		return stats;+	}++	// The caller folds the shape radius and the world origin into the box+	b3AABB originAABB = input->box;++	b3Vec3 p1 = b3AABB_Center( originAABB );+	b3Vec3 extension = b3AABB_Extents( originAABB );++	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 cast+	b3Vec3 t = b3MulSV( maxFraction, input->translation );+	b3AABB totalAABB = {+		b3Min( originAABB.lowerBound, b3Add( originAABB.lowerBound, t ) ),+		b3Max( originAABB.upperBound, b3Add( originAABB.upperBound, t ) ),+	};++	b3BoxCastInput 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 )+		{+			B3_ASSERT( false );+			continue;+		}++		const b3TreeNode* node = nodes + nodeId;+		stats.nodeVisits += 1;++		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 cast.+				return stats;+			}++			if ( 0.0f < value && value < maxFraction )+			{+				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_MAX_AABB_MARGIN+	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;+}
+ upstream/src/height_field.c view
@@ -0,0 +1,1586 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#include "aabb.h"+#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 <stddef.h>+#include <stdio.h>+#include <string.h>++/*+	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+ */++b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data )+{+	int columnCount = data->countX;+	int rowCount = data->countZ;++	int heightCount = columnCount * rowCount;+	B3_ASSERT( heightCount >= 4 );++	int cellCount = ( columnCount - 1 ) * ( rowCount - 1 );+	int triangleCount = 2 * cellCount;++	// Single blob: struct followed by the height, material, and flag arrays. Layout+	// mirrors b3HullData/b3MeshData so the recording path can copy it with one memcpy.+	size_t byteCount = b3AlignUp8( sizeof( b3HeightFieldData ) );+	int heightsOffset = (int)byteCount;+	byteCount += b3AlignUp8( heightCount * sizeof( uint16_t ) );+	int materialOffset = (int)byteCount;+	byteCount += b3AlignUp8( cellCount * sizeof( uint8_t ) );+	int flagsOffset = (int)byteCount;+	byteCount += b3AlignUp8( triangleCount * sizeof( uint8_t ) );++	// Zero the whole blob so alignment padding is defined. The construction-time hash+	// sweeps raw bytes and would otherwise pick up uninitialized padding.+	b3HeightFieldData* hf = (b3HeightFieldData*)b3Alloc( byteCount );+	memset( hf, 0, byteCount );++	hf->version = B3_HEIGHT_FIELD_VERSION;+	hf->byteCount = (int)byteCount;+	hf->scale = data->scale;+	hf->columnCount = columnCount;+	hf->rowCount = rowCount;+	hf->heightsOffset = heightsOffset;+	hf->materialOffset = materialOffset;+	hf->flagsOffset = flagsOffset;+	hf->clockwise = data->clockwiseWinding;++	uint16_t* compressedHeights = (uint16_t*)( (intptr_t)hf + heightsOffset );+	uint8_t* materialIndices = (uint8_t*)( (intptr_t)hf + materialOffset );+	uint8_t* flags = (uint8_t*)( (intptr_t)hf + flagsOffset );++	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;+		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 * compressedHeights[i];+	}+	heights = decompressedHeights;++	if ( data->materialIndices != NULL )+	{+		for ( int i = 0; i < cellCount; ++i )+		{+			materialIndices[i] = data->materialIndices[i];+		}+	}+	else+	{+		for ( int i = 0; i < cellCount; ++i )+		{+			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 ) };++	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 ( 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;+				}+				if ( separation < 0.0f || cosAngle > cos5Deg )+				{+					flags1 |= b3_inverseConcaveEdge2;+					flags2 |= b3_inverseConcaveEdge2;+				}+			}++			B3_UNUSED( center1 );+			B3_UNUSED( center2 );++			// top+			int topCellIndex = ( row - 1 ) * ( columnCount - 1 ) + column;+			if ( row > 0 && 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;+				}+				if ( separation < 0.0f || cosAngle > cos5Deg )+				{+					flags1 |= b3_inverseConcaveEdge3;+				}+			}++			int bottomCellIndex = ( row + 1 ) * ( columnCount - 1 ) + column;+			if ( row + 1 < rowCount - 1 && 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;+				}+				if ( separation < 0.0f || cosAngle > cos5Deg )+				{+					flags2 |= b3_inverseConcaveEdge3;+				}+			}++			int leftCellIndex = row * ( columnCount - 1 ) + column - 1;+			if ( column - 1 >= 0 && 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;+				}+				if ( separation < 0.0f || cosAngle > cos5Deg )+				{+					flags1 |= b3_inverseConcaveEdge1;+				}+			}++			int rightCellIndex = row * ( columnCount - 1 ) + column + 1;+			if ( column + 1 < columnCount - 1 && 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;+				}+				if ( separation < 0.0f || cosAngle > cos5Deg )+				{+					flags2 |= b3_inverseConcaveEdge1;+				}+			}++			B3_ASSERT( 0 <= flags1 && flags1 <= UINT8_MAX );+			B3_ASSERT( 0 <= flags2 && flags2 <= UINT8_MAX );++			flags[triangleIndex1] = (uint8_t)flags1;+			flags[triangleIndex2] = (uint8_t)flags2;+		}+	}++	B3_ASSERT( triangleIndex == triangleCount );++	b3Free( decompressedHeights, heightCount * sizeof( float ) );++	hf->hash = 0;+	hf->hash = b3Hash64NonZero( (const uint8_t*)hf, hf->byteCount );++	return hf;+}++_Static_assert( b3_concaveEdge3 == 4 * b3_concaveEdge1, "bit math" );+_Static_assert( b3_inverseConcaveEdge3 == 4 * b3_inverseConcaveEdge1, "bit math" );++// Decode the four corner vertices of a height field cell into local space.+// Output order matches the index naming used throughout this file:+// corners[0] = (column, row), corners[1] = (column + 1, row),+// corners[2] = (column, row + 1), corners[3] = (column + 1, row + 1).+static inline void b3GetHeightFieldCellCorners( const b3HeightFieldData* hf, int row, int column, b3Vec3 corners[4] )+{+	B3_ASSERT( 0 <= row && row < hf->rowCount - 1 && 0 <= column && column < hf->columnCount - 1 );++	int columnCount = hf->columnCount;+	int index11 = row * columnCount + column;+	int index12 = index11 + 1;+	int index21 = ( row + 1 ) * columnCount + column;+	int index22 = index21 + 1;++	float minHeight = hf->minHeight;+	float heightScale = hf->heightScale;+	const uint16_t* heights = b3GetHeightFieldCompressedHeights( hf );++	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 = hf->scale;+	corners[0] = b3Mul( scale, (b3Vec3){ x1, height11, z1 } );+	corners[1] = b3Mul( scale, (b3Vec3){ x2, height12, z1 } );+	corners[2] = b3Mul( scale, (b3Vec3){ x1, height21, z2 } );+	corners[3] = b3Mul( scale, (b3Vec3){ x2, height22, z2 } );+}++b3Triangle b3GetHeightFieldTriangle( const b3HeightFieldData* heightField, int triangleIndex )+{+	B3_ASSERT( 0 <= triangleIndex );+	B3_ASSERT( triangleIndex < 2 * ( heightField->columnCount - 1 ) * ( heightField->rowCount - 1 ) );++	b3Triangle triangle;+	triangle.flags = b3GetHeightFieldFlags( heightField )[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( b3GetHeightFieldMaterialIndices( heightField )[cellIndex] != B3_HEIGHT_FIELD_HOLE );+	B3_UNUSED( cellIndex );++	b3Vec3 corners[4];+	b3GetHeightFieldCellCorners( heightField, row, column, corners );++	if ( ( triangleIndex & 1 ) == 0 )+	{+		triangle.vertices[0] = corners[0];+		triangle.vertices[1] = corners[2];+		triangle.vertices[2] = corners[1];+		triangle.i1 = index11;+		triangle.i2 = index21;+		triangle.i3 = index12;+	}+	else+	{+		triangle.vertices[0] = corners[3];+		triangle.vertices[1] = corners[1];+		triangle.vertices[2] = corners[2];+		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 b3HeightFieldData* heightField, int triangleIndex )+{+	B3_ASSERT( 0 <= triangleIndex );+	B3_ASSERT( triangleIndex < 2 * ( heightField->columnCount - 1 ) * ( heightField->rowCount - 1 ) );++	int cellIndex = triangleIndex >> 1;+	return b3GetHeightFieldMaterialIndices( heightField )[cellIndex];+}++b3AABB b3ComputeHeightFieldAABB( const b3HeightFieldData* shape, b3Transform transform )+{+	return b3AABB_Transform( transform, shape->aabb );+}++b3CastOutput b3RayCastHeightField( const b3HeightFieldData* 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 b3HeightFieldData* 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_MAX_AABB_MARGIN, B3_MAX_AABB_MARGIN, B3_MAX_AABB_MARGIN };+	b3AABB combinedBounds = { b3Sub( b3Sub( heightField->aabb.lowerBound, shapeExtents ), margin ),+							  b3Add( b3Add( heightField->aabb.upperBound, shapeExtents ), margin ) };++	float minFraction, maxFraction;+	bool intersects = b3RayCastAABB( combinedBounds, 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 );++	// Preserve the un-shifted center sweep. clampedStart/clampedEnd get pushed out to the+	// leading box corner below to drive the grid DDA, but the swept-volume AABB used to+	// cull cells must stay centered on the actual shape path.+	b3Vec3 centerStart = clampedStart;+	b3Vec3 centerEnd = clampedEnd;++	// 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;++	// nextFractionX / nextFractionZ advance in units of the clamped sweep+	// [minFraction, maxFraction], but bestFraction is a fraction of the full input+	// translation. Precompute the affine map from clamped space to input space so+	// the loop termination test compares like with like. Otherwise it can exit+	// early and miss a closer hit in a later cell.+	float gridFractionScale = input->maxFraction * ( maxFraction - minFraction );+	float gridFractionOffset = input->maxFraction * minFraction;++	int rowCount = heightField->rowCount;+	int columnCount = heightField->columnCount;+	int cellCount = ( heightField->rowCount - 1 ) * ( heightField->columnCount - 1 );+	B3_UNUSED( cellCount );++	b3ShapeCastPairInput pairInput = { 0 };+	pairInput.proxyB = input->proxy;+	pairInput.transform = b3Transform_identity;+	pairInput.translationB = input->translation;+	pairInput.canEncroach = input->canEncroach;++	b3AABB castBounds;+	castBounds.lowerBound = b3Sub( b3Min( centerStart, centerEnd ), shapeExtents );+	castBounds.upperBound = b3Add( b3Max( centerStart, centerEnd ), 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 = b3GetHeightFieldMaterialIndices( heightField )[cellIndex];+				if ( materialIndex == B3_HEIGHT_FIELD_HOLE )+				{+					continue;+				}++				b3Vec3 corners[4];+				b3GetHeightFieldCellCorners( heightField, row, column, corners );+				b3Vec3 point11 = corners[0];+				b3Vec3 point12 = corners[1];+				b3Vec3 point21 = corners[2];+				b3Vec3 point22 = corners[3];++				// 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.transform.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.transform.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.+		// Map them from clamped-sweep space into input-translation space before+		// comparing against bestFraction.+		float inputFractionX = nextFractionX == FLT_MAX ? FLT_MAX : gridFractionOffset + nextFractionX * gridFractionScale;+		float inputFractionZ = nextFractionZ == FLT_MAX ? FLT_MAX : gridFractionOffset + nextFractionZ * gridFractionScale;+		if ( inputFractionX > bestFraction && inputFractionZ > 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 b3HeightFieldData* 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.transform = b3Transform_identity;+	input.useRadii = true;++	b3SimplexCache cache = { 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 = b3GetHeightFieldMaterialIndices( shape )[cellIndex];+			if ( material == B3_HEIGHT_FIELD_HOLE )+			{+				continue;+			}++			b3Vec3 corners[4];+			b3GetHeightFieldCellCorners( shape, row, column, corners );+			b3Vec3 point11 = corners[0];+			b3Vec3 point12 = corners[1];+			b3Vec3 point21 = corners[2];+			b3Vec3 point22 = corners[3];++			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 b3HeightFieldData* 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 );++	// 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 = b3GetHeightFieldMaterialIndices( heightField )[cellIndex];+			if ( material == B3_HEIGHT_FIELD_HOLE )+			{+				continue;+			}++			b3Vec3 corners[4];+			b3GetHeightFieldCellCorners( heightField, row, column, corners );+			b3Vec3 point11 = corners[0];+			b3Vec3 point12 = corners[1];+			b3Vec3 point21 = corners[2];+			b3Vec3 point22 = corners[3];++			// 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 b3HeightFieldData* shape, const b3Capsule* mover )+{+	b3DistanceInput distanceInput = { 0 };+	distanceInput.proxyB = (b3ShapeProxy){ &mover->center1, 2, 0.0f };+	distanceInput.transform = 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 );++	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 = b3GetHeightFieldMaterialIndices( shape )[cellIndex];+			if ( material == B3_HEIGHT_FIELD_HOLE )+			{+				continue;+			}++			b3Vec3 corners[4];+			b3GetHeightFieldCellCorners( shape, row, column, corners );+			b3Vec3 point11 = corners[0];+			b3Vec3 point12 = corners[1];+			b3Vec3 point21 = corners[2];+			b3Vec3 point22 = corners[3];++			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;+}++b3HeightFieldData* 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;++	b3HeightFieldData* heightField = b3CreateHeightField( &data );++	b3Free( heights, heightCount * sizeof( float ) );+	b3Free( materialIndices, cellCount * sizeof( uint8_t ) );++	return heightField;+}++b3HeightFieldData* 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;++	b3CosSin cs = { 0 };++	for ( int i = 0; i < rowCount; ++i )+	{+		cs = b3ComputeCosSin( omegaZ * i );+		float rowHeight = cs.sine;++		for ( int j = 0; j < columnCount; ++j )+		{+			int k = i * columnCount + j;+			cs = b3ComputeCosSin( omegaX * j );+			float columnHeight = cs.sine;+			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+			{+				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;++	b3HeightFieldData* heightField = b3CreateHeightField( &data );++	b3Free( heights, heightCount * sizeof( float ) );+	b3Free( materialIndices, cellCount * sizeof( uint8_t ) );++	return heightField;+}++void b3DestroyHeightField( b3HeightFieldData* heightField )+{+	b3Free( heightField, heightField->byteCount );+}++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++b3HeightFieldData* 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+	b3HeightFieldData* heightField = b3CreateHeightField( &data );++	// Clean up temporary allocations+	b3Free( data.heights, heightCount * sizeof( float ) );+	b3Free( data.materialIndices, materialCount * sizeof( uint8_t ) );++	return heightField;+}
+ upstream/src/hull.c view
@@ -0,0 +1,2947 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++// Dirk Gregorius contributed portions of this code++#include "hull.h"++#include "algorithm.h"+#include "math_internal.h"+#include "shape.h"++#include "box3d/collision.h"+#include "box3d/constants.h"+#include "box3d/math_functions.h"++#include <float.h>+#include <stdbool.h>+#include <stddef.h>+#include <stdint.h>+#include <stdlib.h>+#include <string.h>++#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 256.+#define B3_HULL_MAX_COUNT ( UINT8_MAX + 1 )++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;++	// LIFO free list. Built with edge->next.+	b3QHHalfEdge* edgeFreeHead;++	b3QHFace* faceBase;+	int faceCapacity;+	int faceCount;++	// LIFO free list. Built with face->link.next.+	b3QHFace* faceFreeHead;++	// 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).+	int finalVertexCount;+	int finalHalfEdgeCount;+	int finalFaceCount;+} b3HullBuilder;++static inline void b3QHList_Init( b3QHListNode* head )+{+	head->prev = head;+	head->next = head;+}++#define B3_LIST_EMPTY( A ) ( ( A )->next == ( A ) )++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 and add to free list.+static void b3HullBuilder_RetireFace( b3HullBuilder* b, b3QHFace* face )+{+	// Sometimes a cone face gets merged and never added to the list.+	if ( b3QHList_Contains( &face->link ) )+	{+		b3QHList_Remove( &face->link );+	}++	face->edge = NULL;+	// link.prev is already NULL after Remove (or was never set).+	B3_VALIDATE( face->link.prev == NULL );+	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( B3_LIST_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( B3_LIST_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( B3_LIST_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 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( B3_LIST_EMPTY( &face->conflictListHead.link ) );++			// Each half-edge is owned by exactly one face, so ring walks over the+			// dead region retire every interior edge exactly once. Merge deleted+			// faces are already off the face list.+			b3QHHalfEdge* start = face->edge;+			b3QHHalfEdge* edge = start;+			do+			{+				b3QHHalfEdge* next = edge->next;+				b3HullBuilder_RetireEdge( b, edge );+				edge = next;+			}+			while ( edge != start );++			b3HullBuilder_RetireFace( b, face );+		}+	}++	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 );+	bool haveInitialHull = b3HullBuilder_BuildInitialHull( b, pointCount, shiftedPoints );+	if ( haveInitialHull == false )+	{+		return false;+	}++	int budget = b3ClampInt( maxVertexCount - 4, 0, B3_HULL_MAX_COUNT - 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 );+}++typedef struct b3HullWorkSizes+{+	// Input point count+	int N;+	// Output point limit+	int M;+	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. Minimum 48.+	s.edgeCapacity = b3MaxInt( 48, 24 * s.M - 48 );++	// faceCapacity: peak intermediate state live faces (<=2*M-4) plus full cone (<=3*M-6). Minimum 16.+	s.faceCapacity = b3MaxInt( 16, 5 * s.M - 10 );++	// Horizon/cone bounded by current half-edge count. Merged faces by face count.+	s.horizonCapacity = b3MaxInt( 6, 3 * s.M - 6 );+	s.coneCapacity = s.horizonCapacity;+	s.mergedFacesCapacity = b3MaxInt( 4, 2 * s.M - 4 );++	// Horizon DFS depth is bounded by the number of live faces (Euler: <=2*M-4).+	s.horizonStackCapacity = b3MaxInt( 4, 2 * s.M - 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( b3HullData* hull )+{+	if ( hull->pointOffset == 0 )+	{+		return NULL;+	}+	return (b3Vec3*)( (intptr_t)hull + hull->pointOffset );+}++static b3Plane* b3GetHullPlanesWrite( b3HullData* hull )+{+	if ( hull->planeOffset == 0 )+	{+		return NULL;+	}+	return (b3Plane*)( (intptr_t)hull + hull->planeOffset );+}++static b3HullVertex* b3GetHullVerticesWrite( b3HullData* hull )+{+	if ( hull->vertexOffset == 0 )+	{+		return NULL;+	}+	return (b3HullVertex*)( (intptr_t)hull + hull->vertexOffset );+}++static b3HullHalfEdge* b3GetHullEdgesWrite( b3HullData* hull )+{+	if ( hull->edgeOffset == 0 )+	{+		return NULL;+	}+	return (b3HullHalfEdge*)( (intptr_t)hull + hull->edgeOffset );+}++static float* b3GetHullSoaVerticesWrite( b3HullData* hull )+{+	if ( hull->soaVertexOffset == 0 )+	{+		return NULL;+	}+	return (float*)( (intptr_t)hull + hull->soaVertexOffset );+}++static float* b3GetHullSoaNormalsWrite( b3HullData* hull )+{+	if ( hull->soaNormalOffset == 0 )+	{+		return NULL;+	}+	return (float*)( (intptr_t)hull + hull->soaNormalOffset );+}++int b3FindHullSupportVertex( const b3HullData* 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 b3HullData* 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 b3HullData* 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 b3HullData* hull )+{+	B3_UNUSED( hull );+	return true;+}++#endif++b3HullData* 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;+	}++	b3HullData* 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;+}++b3HullData* 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;+	}++	b3HullData* 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;+}++b3HullData* b3CreateRock( float radius )+{+	int pointCount = 10;++	// Golden ratio+	const float phi = ( 1.0f + sqrtf( 5.0f ) ) / 2.0f;++	// Fibonacci lattice+	b3Vec3 points[10];++	// Azimuthal angle+	float theta = 2.0f * B3_PI / phi;++	b3CosSin cs = { 1.0f, 0.0 };+	b3CosSin deltaCS = b3ComputeCosSin( theta );++	for ( int i = 0; i < pointCount; ++i )+	{+		// Z coordinate+		float z = 1.0f - ( 2.0f * i + 1.0f ) / pointCount;+		// Radius in xy-plane+		float radius_XY = sqrtf( 1.0f - z * z );++		points[i].x = radius * radius_XY * cs.cosine;+		points[i].y = radius * radius_XY * cs.sine;+		points[i].z = radius * z;++		b3CosSin cs0 = cs;+		cs.cosine = deltaCS.cosine * cs0.cosine - deltaCS.sine * cs0.sine;+		cs.sine = deltaCS.sine * cs0.cosine + deltaCS.cosine * cs0.sine;+	}++	return b3CreateHull( points, pointCount, pointCount );+}++static void b3UpdateHullBounds( b3HullData* 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( b3HullData* 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;+}++b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCount )+{+	if ( pointCount < 4 )+	{+		return NULL;+	}++	b3Vec3 origin = points[0];+	int clampedMaxCount = b3ClampInt( maxVertexCount, 4, B3_MAX_HULL_VERTICES );++	// 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 == false )+	{+		b3Free( work, sizes.totalBytes );+		return NULL;+	}++	if ( builder.finalVertexCount > B3_MAX_HULL_VERTICES )+	{+		b3Log( "hull final vertex count of %d exceeds limit of %d", builder.finalVertexCount, B3_MAX_HULL_VERTICES );+		b3Free( work, sizes.totalBytes );+		return NULL;+	}++	if ( builder.finalFaceCount > B3_MAX_HULL_FACES )+	{+		b3Log( "hull final face count of %d exceeds limit of %d", builder.finalFaceCount, B3_MAX_HULL_FACES );+		b3Free( work, sizes.totalBytes );+		return NULL;+	}++	int maxHalfEdgeCount = 2 * B3_MAX_HULL_EDGES;+	if ( builder.finalHalfEdgeCount > maxHalfEdgeCount )+	{+		b3Log( "hull final half edge count of %d exceeds limit of %d", builder.finalHalfEdgeCount, maxHalfEdgeCount );+		b3Free( work, sizes.totalBytes );+		return NULL;+	}++	// Walk lists into temp arrays bounded by B3_HULL_MAX_COUNT, 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_MAX_COUNT];+	int vertexCount = 0;+	for ( b3QHListNode* node = builder.vertexList.link.next; node != &builder.vertexList.link; node = node->next )+	{+		B3_ASSERT( vertexCount < B3_HULL_MAX_COUNT );++		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.+	const b3QHFace* tempFaces[B3_HULL_MAX_COUNT];+	const b3QHHalfEdge* tempEdges[B3_HULL_MAX_COUNT];+	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_MAX_COUNT );++		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_MAX_COUNT );++				edge->finalIndex = edgeCount;+				tempEdges[edgeCount++] = edge;+				edge->twin->finalIndex = edgeCount;+				tempEdges[edgeCount++] = edge->twin;+			}+			edge = edge->next;+		}+		while ( edge != face->edge );+	}++	int soaVertexCount = ( vertexCount + 3 ) & ~3;+	int soaNormalCount = ( faceCount + 3 ) & ~3;++	// Allocate the hull. Arrays hang off the end.+	size_t byteCount = b3AlignUp8( sizeof( b3HullData ) );+	int vertexOffset = (int)byteCount;+	byteCount += b3AlignUp8( vertexCount * (int)sizeof( b3HullVertex ) );+	int pointOffset = (int)byteCount;+	byteCount += b3AlignUp8( vertexCount * (int)sizeof( b3Vec3 ) );+	int edgeOffset = (int)byteCount;+	byteCount += b3AlignUp8( edgeCount * (int)sizeof( b3HullHalfEdge ) );+	int planeOffset = (int)byteCount;+	byteCount += b3AlignUp8( faceCount * (int)sizeof( b3Plane ) );+	int faceOffset = (int)byteCount;+	byteCount += b3AlignUp8( faceCount * (int)sizeof( b3HullFace ) );+	int soaVertexOffset = (int)byteCount;+	byteCount += b3AlignUp8( 3 * soaVertexCount * (int)sizeof( float ) );+	int soaNormalOffset = (int)byteCount;+	byteCount += b3AlignUp8( 3 * soaNormalCount * (int)sizeof( float ) );++	b3HullData* hull = b3Alloc( byteCount );+	memset( hull, 0, byteCount );++	hull->version = B3_HULL_VERSION;+	hull->vertexOffset = vertexOffset;+	hull->pointOffset = pointOffset;+	hull->edgeOffset = edgeOffset;+	hull->planeOffset = planeOffset;+	hull->faceOffset = faceOffset;+	hull->soaVertexOffset = soaVertexOffset;+	hull->soaNormalOffset = soaNormalOffset;++	hull->vertexCount = vertexCount;+	hull->edgeCount = edgeCount;+	hull->faceCount = faceCount;++	hull->byteCount = (int)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 );+	float* soaVertices = b3GetHullSoaVerticesWrite( hull );+	float* soaNormals = b3GetHullSoaNormalsWrite( hull );++	float* vx = soaVertices;+	float* vy = vx + soaVertexCount;+	float* vz = vy + soaVertexCount;++	for ( int index = 0; index < vertexCount; ++index )+	{+		vertices[index].edge = 0;+		b3Vec3 p = tempVertices[index]->position;+		finalPoints[index] = p;++		vx[index] = p.x;+		vy[index] = p.y;+		vz[index] = p.z;+	}++	for ( int index = vertexCount; index < soaVertexCount; ++index )+	{+		vx[index] = vx[0];+		vy[index] = vy[0];+		vz[index] = vz[0];+	}++	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;+	}++	float* nx = soaNormals;+	float* ny = nx + soaNormalCount;+	float* nz = ny + soaNormalCount;++	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;++		b3Vec3 n = face->plane.normal;+		nx[index] = n.x;+		ny[index] = n.y;+		nz[index] = n.z;+	}++	for ( int index = faceCount; index < soaNormalCount; ++index )+	{+		nx[index] = 0.0f;+		ny[index] = 0.0f;+		nz[index] = 0.0f;+	}++	// 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 = b3Hash64NonZero( (uint8_t*)hull, hull->byteCount );++	return hull;+}++b3HullData* b3CloneHull( const b3HullData* hull )+{+	if ( hull == NULL || b3IsValidHull( hull ) == false )+	{+		return NULL;+	}++	b3HullData* clone = b3Alloc( hull->byteCount );+	memcpy( clone, hull, hull->byteCount );++	return clone;+}++uint64_t b3HashHullData( const b3HullData* hull )+{+	return hull->hash;+}++bool b3CompareHullData( const b3HullData* hull1, const b3HullData* hull2 )+{+	if ( hull1 == hull2 )+	{+		return true;+	}++	if ( hull1->byteCount != hull2->byteCount )+	{+		return false;+	}++	return memcmp( hull1, hull2, hull1->byteCount ) == 0;+}++// Hull identity covers every byte, so the structs carry explicit padding. These lock+// the layout, re-audit padding if a size changes.+_Static_assert( sizeof( b3HullData ) == 144, "unexpected hull data size" );+_Static_assert( sizeof( b3BoxHull ) == 640, "unexpected box hull size" );++// Implement b3HullMap.+#define NAME b3HullMap+#define KEY_TY const b3HullData*+#define VAL_TY int+#define HASH_FN b3HashHullData+#define CMPR_FN b3CompareHullData+#define MALLOC_FN b3Alloc+#define FREE_FN b3Free+#define IMPLEMENTATION_MODE+#include "verstable.h"++size_t b3HullMapByteCount( b3HullMap* map )+{+	// The map owns a combined bucket and metadata allocation, valid only when buckets exist+	size_t byteCount = sizeof( b3HullMap );+	if ( b3HullMap_bucket_count( map ) > 0 )+	{+		byteCount += b3HullMap_total_alloc_size( map );+	}+	return byteCount;+}++b3HullData* b3CloneAndTransformHull( const b3HullData* original, b3Transform transform, b3Vec3 scale )+{+	if ( original == NULL || b3IsValidHull( original ) == false )+	{+		return NULL;+	}++	b3HullData* hull = 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;++			int startEdgeIndex = face->edge;+			int currentEdgeIndex = startEdgeIndex;+			int prevEdgeIndex = B3_NULL_INDEX;++			do+			{+				b3HullHalfEdge* edge = edges + currentEdgeIndex;++				if ( edge->next == startEdgeIndex )+				{+					prevEdgeIndex = currentEdgeIndex;+					break;+				}++				currentEdgeIndex = edge->next;+			}+			while ( currentEdgeIndex != startEdgeIndex );++			B3_ASSERT( prevEdgeIndex != B3_NULL_INDEX );++			currentEdgeIndex = startEdgeIndex;++			do+			{+				b3HullHalfEdge* edge = edges + currentEdgeIndex;+				uint8_t nextIndex = edge->next;+				edge->next = (uint8_t)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 );++	int soaVertexCount = ( vertexCount + 3 ) & ~3;+	float* vx = b3GetHullSoaVerticesWrite( hull );+	float* vy = vx + soaVertexCount;+	float* vz = vy + soaVertexCount;++	for ( int i = 0; i < vertexCount; ++i )+	{+		b3Vec3 p = b3Add( b3MulMV( matrix, b3Mul( safeScale, points[i] ) ), transform.p );+		points[i] = p;+		vx[i] = p.x;+		vy[i] = p.y;+		vz[i] = p.z;+	}++	for ( int i = vertexCount; i < soaVertexCount; ++i )+	{+		vx[i] = vx[0];+		vy[i] = vy[0];+		vz[i] = vz[0];+	}++	b3Plane* planes = b3GetHullPlanesWrite( hull );+	int soaNormalCount = ( faceCount + 3 ) & ~3;+	float* nx = b3GetHullSoaNormalsWrite( hull );+	float* ny = nx + soaNormalCount;+	float* nz = ny + soaNormalCount;++	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 );++		nx[i] = normal.x;+		ny[i] = normal.y;+		nz[i] = normal.z;+	}++	for ( int i = faceCount; i < soaNormalCount; ++i )+	{+		nx[i] = 0.0f;+		ny[i] = 0.0f;+		nz[i] = 0.0f;+	}++	b3UpdateHullBounds( hull );+	bool success = b3UpdateHullBulkProperties( hull );+	if ( success == false )+	{+		b3Free( hull, original->byteCount );+		return NULL;+	}++	// Must ensure the hash is 0 so it doesn't contribute to itself.+	hull->hash = 0;+	hull->hash = b3Hash64NonZero( (uint8_t*)hull, hull->byteCount );++	B3_VALIDATE( b3IsValidHull( hull ) );++	return hull;+}++void b3DestroyHull( b3HullData* hull )+{+	b3Free( hull, hull->byteCount );+}++b3MassData b3ComputeHullMass( const b3HullData* shape, float density )+{+	b3MassData out;+	out.mass = density * shape->volume;+	out.center = shape->center;++	// Inertia about the center of mass+	out.inertia = b3MulSM( density, shape->centralInertia );+	return out;+}++b3AABB b3ComputeHullAABB( const b3HullData* shape, b3Transform transform )+{+	return b3AABB_Transform( transform, shape->aabb );+}++b3AABB b3ComputeSweptHullAABB( const b3HullData* 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 b3HullData* 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.transform = b3InvMulTransforms( shapeTransform, b3Transform_identity );+	input.useRadii = true;++	b3SimplexCache cache = { 0 };+	b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 );+	return output.distance < B3_OVERLAP_SLOP;+}++b3CastOutput b3RayCastHull( const b3HullData* 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 b3HullData* shape, const b3ShapeCastInput* input )+{+	const b3Vec3* points = b3GetHullPoints( shape );++	b3ShapeCastPairInput pairInput;+	pairInput.proxyA = (b3ShapeProxy){ points, shape->vertexCount, 0.0f };+	pairInput.proxyB = input->proxy;+	pairInput.transform = 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 b3HullData* 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.transform = b3Transform_identity;+	distanceInput.useRadii = false;++	float totalRadius = mover->radius;++	b3SimplexCache cache = { 0 };+	b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 );++	if ( distanceOutput.distance == 0.0f )+	{+		// I could handle deep overlap on hulls, but there is no reasonable solution for+		// deep overlap on meshes. So if someone converted a hull to a mesh there would be+		// different behavior. So I think there is not a good reason to handle deep overlap+		// on hulls.+		return 0;+	}++	if ( distanceOutput.distance <= totalRadius )+	{+		b3Plane plane = { distanceOutput.normal, totalRadius - distanceOutput.distance };+		*result = (b3PlaneResult){ plane, distanceOutput.pointA };+		return 1;+	}++	return 0;+}++b3ShapeExtent b3ComputeHullExtent( const b3HullData* 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 b3HullData* 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 ),+			.planeOffset = offsetof( b3BoxHull, boxPlanes ),+			.faceOffset = offsetof( b3BoxHull, boxFaces ),+			.soaVertexOffset = offsetof( b3BoxHull, vx ),+			.soaNormalOffset = offsetof( b3BoxHull, nx ),+		},+	.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 )+{+	B3_ASSERT( b3IsValidTransform( 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 } );++	// SOA+	boxHull.vx[0] = boxHull.boxPoints[0].x;+	boxHull.vx[1] = boxHull.boxPoints[1].x;+	boxHull.vx[2] = boxHull.boxPoints[2].x;+	boxHull.vx[3] = boxHull.boxPoints[3].x;+	boxHull.vx[4] = boxHull.boxPoints[4].x;+	boxHull.vx[5] = boxHull.boxPoints[5].x;+	boxHull.vx[6] = boxHull.boxPoints[6].x;+	boxHull.vx[7] = boxHull.boxPoints[7].x;++	boxHull.vy[0] = boxHull.boxPoints[0].y;+	boxHull.vy[1] = boxHull.boxPoints[1].y;+	boxHull.vy[2] = boxHull.boxPoints[2].y;+	boxHull.vy[3] = boxHull.boxPoints[3].y;+	boxHull.vy[4] = boxHull.boxPoints[4].y;+	boxHull.vy[5] = boxHull.boxPoints[5].y;+	boxHull.vy[6] = boxHull.boxPoints[6].y;+	boxHull.vy[7] = boxHull.boxPoints[7].y;++	boxHull.vz[0] = boxHull.boxPoints[0].z;+	boxHull.vz[1] = boxHull.boxPoints[1].z;+	boxHull.vz[2] = boxHull.boxPoints[2].z;+	boxHull.vz[3] = boxHull.boxPoints[3].z;+	boxHull.vz[4] = boxHull.boxPoints[4].z;+	boxHull.vz[5] = boxHull.boxPoints[5].z;+	boxHull.vz[6] = boxHull.boxPoints[6].z;+	boxHull.vz[7] = boxHull.boxPoints[7].z;++	boxHull.nx[0] = boxHull.boxPlanes[0].normal.x;+	boxHull.nx[1] = boxHull.boxPlanes[1].normal.x;+	boxHull.nx[2] = boxHull.boxPlanes[2].normal.x;+	boxHull.nx[3] = boxHull.boxPlanes[3].normal.x;+	boxHull.nx[4] = boxHull.boxPlanes[4].normal.x;+	boxHull.nx[5] = boxHull.boxPlanes[5].normal.x;+	boxHull.nx[6] = 0.0f;+	boxHull.nx[7] = 0.0f;++	boxHull.ny[0] = boxHull.boxPlanes[0].normal.y;+	boxHull.ny[1] = boxHull.boxPlanes[1].normal.y;+	boxHull.ny[2] = boxHull.boxPlanes[2].normal.y;+	boxHull.ny[3] = boxHull.boxPlanes[3].normal.y;+	boxHull.ny[4] = boxHull.boxPlanes[4].normal.y;+	boxHull.ny[5] = boxHull.boxPlanes[5].normal.y;+	boxHull.ny[6] = 0.0f;+	boxHull.ny[7] = 0.0f;++	boxHull.nz[0] = boxHull.boxPlanes[0].normal.z;+	boxHull.nz[1] = boxHull.boxPlanes[1].normal.z;+	boxHull.nz[2] = boxHull.boxPlanes[2].normal.z;+	boxHull.nz[3] = boxHull.boxPlanes[3].normal.z;+	boxHull.nz[4] = boxHull.boxPlanes[4].normal.z;+	boxHull.nz[5] = boxHull.boxPlanes[5].normal.z;+	boxHull.nz[6] = 0.0f;+	boxHull.nz[7] = 0.0f;++	// Must ensure the hash is 0 so it doesn't contribute to itself.+	boxHull.base.hash = 0;+	boxHull.base.hash = b3Hash64NonZero( (uint8_t*)&boxHull.base, boxHull.base.byteCount );++	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 )+{+	B3_ASSERT( b3IsValidTransform( transform ) );++	b3Vec3 h = halfWidths;+	b3Transform xf = transform;+	b3ScaleBox( &h, &xf, postScale, 4.0f * B3_LINEAR_SLOP );+	return b3MakeTransformedBoxHull( h.x, h.y, h.z, xf );+}
+ upstream/src/hull.h view
@@ -0,0 +1,22 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "box3d/types.h"++#include <stddef.h>++uint64_t b3HashHullData( const b3HullData* hull );+bool b3CompareHullData( const b3HullData* hull1, const b3HullData* hull2 );++// Map keyed by hull content. The world hull database uses the value as a reference count,+// while compound baking stores uses the value as a byte offset. Implementation is in hull.c.+#define NAME b3HullMap+#define KEY_TY const b3HullData*+#define VAL_TY int+#define HEADER_MODE+#include "verstable.h"++// Total map allocation in bytes, excluding the stored hull data+size_t b3HullMapByteCount( b3HullMap* map );
+ upstream/src/id_pool.c view
@@ -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
+ upstream/src/id_pool.h view
@@ -0,0 +1,34 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "container.h"++typedef struct b3IdPool+{+	b3Array( int ) freeArray;+	int nextIndex;+} b3IdPool;++b3IdPool b3CreateIdPool( void );+void b3DestroyIdPool( b3IdPool* pool );++int b3AllocId( b3IdPool* pool );+void b3FreeId( b3IdPool* pool, int id );+void b3ValidateFreeId( const b3IdPool* pool, int id );++static inline int b3GetIdCount( const b3IdPool* pool )+{+	return pool->nextIndex - pool->freeArray.count;+}++static inline int b3GetIdCapacity( const b3IdPool* pool )+{+	return pool->nextIndex;+}++static inline int b3GetIdBytes( const b3IdPool* pool )+{+	return b3Array_ByteCount( pool->freeArray );+}
+ upstream/src/island.c view
@@ -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 <stddef.h>++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;+		}+	}++	// 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
+ upstream/src/island.h view
@@ -0,0 +1,101 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "container.h"++#include <stdint.h>++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.+	b3Array( 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.+	b3Array( b3ContactLink ) contacts;+	b3Array( b3JointLink ) joints;++} b3Island;++// This is used to move islands across solver sets+typedef struct b3IslandSim+{+	int islandId;+} b3IslandSim;++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 );
+ upstream/src/joint.c view
@@ -0,0 +1,1748 @@+// 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 "recording.h"+#include "shape.h"+#include "solver.h"+#include "solver_set.h"++// needed for dll export+#include "box3d/box3d.h"++#include <stdbool.h>+#include <stdio.h>+#include <string.h>++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 );+	B3_REC( world, JointSetConstraintTuning, jointId, hertz, dampingRatio );+	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 );+	B3_REC( world, JointSetForceThreshold, jointId, threshold );+	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 );+	B3_REC( world, JointSetTorqueThreshold, jointId, threshold );+	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 };+	B3_REC_CREATE( world, CreateDistanceJoint, jointId, worldId, *def );+	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 };+	B3_REC_CREATE( world, CreateMotorJoint, jointId, worldId, *def );+	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 };+	B3_REC_CREATE( world, CreateFilterJoint, jointId, worldId, *def );+	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 };+	B3_REC_CREATE( world, CreateParallelJoint, jointId, worldId, *def );+	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 };+	B3_REC_CREATE( world, CreatePrismaticJoint, jointId, worldId, *def );+	return jointId;+}++b3JointId b3CreateRevoluteJoint( b3WorldId worldId, const b3RevoluteJointDef* def )+{+	B3_CHECK_JOINT_DEF( def );++	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 );++	float lowerAngle = b3MinFloat( def->lowerAngle, def->upperAngle );+	float upperAngle = b3MaxFloat( def->lowerAngle, def->upperAngle );+	joint->revoluteJoint.lowerAngle = b3ClampFloat( lowerAngle, -0.99f * B3_PI, 0.99f * B3_PI );+	joint->revoluteJoint.upperAngle = b3ClampFloat( upperAngle, -0.99f * B3_PI, 0.99f * 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 };+	B3_REC_CREATE( world, CreateRevoluteJoint, jointId, worldId, *def );+	return jointId;+}++b3JointId b3CreateSphericalJoint( b3WorldId worldId, const b3SphericalJointDef* def )+{+	B3_CHECK_JOINT_DEF( def );+	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 );++	float lowerAngle = b3MinFloat( def->lowerTwistAngle, def->upperTwistAngle );+	float upperAngle = b3MaxFloat( def->lowerTwistAngle, def->upperTwistAngle );+	joint->sphericalJoint.lowerTwistAngle = b3ClampFloat( lowerAngle, -0.99f * B3_PI, 0.99f * B3_PI );+	joint->sphericalJoint.upperTwistAngle = b3ClampFloat( upperAngle, -0.99f * B3_PI, 0.99f * 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 };+	B3_REC_CREATE( world, CreateSphericalJoint, jointId, worldId, *def );+	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 };+	B3_REC_CREATE( world, CreateWeldJoint, jointId, worldId, *def );+	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 };+	B3_REC_CREATE( world, CreateWheelJoint, jointId, worldId, *def );+	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;+	}++	B3_REC( world, DestroyJoint, jointId, wakeAttached );++	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 );+	B3_REC( world, JointSetLocalFrameA, jointId, localFrame );+	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 );+	B3_REC( world, JointSetLocalFrameB, jointId, localFrame );+	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;+	}++	B3_REC( world, JointSetCollideConnected, jointId, shouldCollide );++	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;+	}++	B3_REC( world, JointWakeBodies, jointId );++	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 );++			b3WorldTransform xfA = b3GetBodyTransform( world, sim->bodyIdA );+			b3WorldTransform 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_parallelJoint:+			return b3GetParallelJointTorque( world, base );++		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 );++	b3WorldTransform xfA = b3GetBodyTransform( world, joint->edges[0].bodyId );+	b3WorldTransform xfB = b3GetBodyTransform( world, joint->edges[1].bodyId );++	b3Pos pA = b3TransformWorldPoint( xfA, base->localFrameA.p );+	b3Pos pB = b3TransformWorldPoint( xfB, base->localFrameB.p );+	b3Vec3 dp = b3SubPos( 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 );++	b3WorldTransform xfA = b3GetBodyTransform( world, joint->edges[0].bodyId );+	b3WorldTransform 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 );++	b3WorldTransform transformA = b3GetBodyTransformQuick( world, bodyA );+	b3WorldTransform transformB = b3GetBodyTransformQuick( world, bodyB );+	b3Pos pA = b3TransformWorldPoint( transformA, jointSim->localFrameA.p );+	b3Pos pB = b3TransformWorldPoint( 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->DrawSegmentFcn( pA, pB, b3_colorPlum, draw->context );+			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 )+		{+			b3Pos p = b3LerpPosition( 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 );+		b3Pos p = b3LerpPosition( pA, pB, 0.5f );++		draw->DrawSegmentFcn( p, b3OffsetPos( p, b3MulSV( 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 );+	}+}
+ upstream/src/joint.h view
@@ -0,0 +1,415 @@+// 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;++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, b3WorldTransform transformA, b3WorldTransform transformB );+void b3DrawParallelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );+void b3DrawPrismaticJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );+void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );+void b3DrawSphericalJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );+void b3DrawWeldJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );+void b3DrawWheelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );
+ upstream/src/manifold.c view
@@ -0,0 +1,196 @@+// 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"++// 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 b3HullData* hull, b3Vec3 refNormal, int vertexIndex )+{+	B3_ASSERT( 0 <= vertexIndex && vertexIndex < hull->vertexCount );++	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;+}
+ upstream/src/manifold.h view
@@ -0,0 +1,85 @@+// 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 b3SeparatingAxis+{+	b3Vec3 normal;+	float separation;+	int indexA;+	int indexB;+	b3SeparatingFeature type;+} b3SeparatingAxis;++typedef struct b3AxisQuery+{+	b3SeparatingAxis faceA;+	b3SeparatingAxis faceB;+	b3SeparatingAxis edge;+	b3SeparatingFeature separatedFeature;+} b3AxisQuery;++typedef struct b3ClipVertex+{+	b3Vec3 position;+	float separation;+	b3FeaturePair pair;+} b3ClipVertex;++typedef enum b3FeatureOwner+{+	b3_featureShapeA = 0,+	b3_featureShapeB = 1+} b3FeatureOwner;++int b3FindIncidentFace( const b3HullData* 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 );++b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, bool earlyReturn );++#if B3_ENABLE_VALIDATION+bool b3ValidatePolygon( b3ClipVertex* polygon, int count );+#endif++// For single point contact, such as sphere-sphere, sphere-capsule, sphere-triangle+static const b3FeaturePair b3FeaturePair_single = { 0 };++static 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;+}++static inline b3SeparatingAxis b3GetBestAxis( const b3AxisQuery* query )+{+	B3_VALIDATE( query->faceA.type == b3_faceAxisA );+	B3_VALIDATE( query->edge.type == b3_edgePairAxis );+	B3_VALIDATE( query->faceB.type == b3_faceAxisB );++	if ( query->faceA.separation > query->faceB.separation )+	{+		if ( query->edge.separation > query->faceA.separation )+		{+			return query->edge;+		}++		return query->faceA;+	}++	if ( query->edge.separation > query->faceB.separation )+	{+		return query->edge;+	}++	return query->faceB;+}
+ upstream/src/math_functions.c view
@@ -0,0 +1,616 @@+// 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 <math.h>+#include <string.h>++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 b3IsBoundedAABB( b3AABB a )+{+	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 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 );+}++bool b3IsValidPosition( b3Pos p )+{+	if ( isnan( p.x ) || isnan( p.y ) || isnan( p.z ) )+	{+		return false;+	}++	if ( isinf( p.x ) || isinf( p.y ) || isinf( p.z ) )+	{+		return false;+	}++	return true;+}++bool b3IsValidWorldTransform( b3WorldTransform t )+{+	return b3IsValidPosition( t.p ) && b3IsValidQuat( t.q );+}++// 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 = m->cx.x + m->cy.y + m->cz.z;+	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 );+}++b3SegmentDistanceResult b3LineDistance( b3Vec3 p1, b3Vec3 d1, b3Vec3 p2, b3Vec3 d2 )+{+	b3SegmentDistanceResult 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.fraction1 = s1;+		result.point2 = b3MulAdd( p2, s2, d2 );+		result.fraction2 = s2;++		return result;+	}++	float s1 = ( a22 * b1 - a12 * b2 ) / det;+	float s2 = ( a11 * b2 - a21 * b1 ) / det;++	result.point1 = b3MulAdd( p1, s1, d1 );+	result.fraction1 = s1;+	result.point2 = b3MulAdd( p2, s2, d2 );+	result.fraction2 = s2;+	return result;+}++b3SegmentDistanceResult b3SegmentDistance( b3Vec3 p1, b3Vec3 q1, b3Vec3 p2, b3Vec3 q2 )+{+	b3SegmentDistanceResult 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.fraction1 = 0.0f;+		result.point2 = p2;+		result.fraction2 = 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.fraction1 = 0.0f;+		result.point2 = b3MulAdd( p2, s2, d2 );+		result.fraction2 = 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.fraction1 = s1;+		result.point2 = p2;+		result.fraction2 = 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.fraction1 = s1;+	result.point2 = b3MulAdd( p2, s2, d2 );+	result.fraction2 = s2;++	return result;+}++b3Vec3 b3PointToSegmentDistance( 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;+}
+ upstream/src/math_internal.h view
@@ -0,0 +1,481 @@+// 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 b3TrianglePoint+{+	b3Vec3 point;+	b3TriangleFeature feature;+} b3TrianglePoint;++typedef struct b3ShapeExtent+{+	float minExtent;+	b3Vec3 maxExtent;+} b3ShapeExtent;++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)+int b3GetProxySupport( const b3ShapeProxy* proxy, b3Vec3 axis );+int b3GetPointSupport( const b3Vec3* points, int count, b3Vec3 axis );++// Align up to 8 byte alignment.+static inline size_t b3AlignUp8( size_t x )+{+	return ( x + 7u ) & ~(size_t)7u;+}++// https://en.wikipedia.org/wiki/Floor_and_ceiling_functions+static inline int b3CeilingInt( int numerator, int denominator )+{+	B3_VALIDATE( denominator > 0 );+	return ( numerator + denominator - 1 ) / denominator;+}++// Assumes denominator == 2^exponent+static 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 );++static inline float b3Dot2( b3Vec2 v1, b3Vec2 v2 )+{+	return v1.x * v2.x + v1.y * v2.y;+}++static inline float b3Length2( b3Vec2 v )+{+	return sqrtf( b3Dot2( v, v ) );+}++static inline float b3LengthSquared2( b3Vec2 v )+{+	return b3Dot2( v, v );+}++static 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;+}++static 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;+}++static inline void b3Store( float* dst, b3Vec3 src )+{+	dst[0] = src.x;+	dst[1] = src.y;+	dst[2] = src.z;+}++static 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+static 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 );+}++static 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+static inline b3Quat b3IntegrateRotation( b3Quat q1, b3Vec3 deltaRotation )+{+#if 1+	// 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;+#else+	return b3NormalizeQuat( b3MulQuat(b3QuatFromExponentialMap( deltaRotation ), q1) );+#endif+}++// Pseudo angular velocity from a quaternion target+// w = 2 * (target - q) * conj(q)+static 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 );+}++static 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].+static inline float b3GetByIndex( b3Vec3 v, int index )+{+	B3_VALIDATE( 0 <= index && index < 3 );+	float temp[3] = { v.x, v.y, v.z };+	return temp[index];+}++static inline int b3MajorAxis( b3Vec3 v )+{+	return v.x < v.y ? ( v.y < v.z ? 2 : 1 ) : ( v.x < v.z ? 2 : 0 );+}++static inline float b3MinElement( b3Vec3 v )+{+	return b3MinFloat( v.x, b3MinFloat( v.y, v.z ) );+}++static inline float b3MaxElement( b3Vec3 v )+{+	return b3MaxFloat( v.x, b3MaxFloat( v.y, v.z ) );+}++static inline int b3MaxElementIndex( b3Vec3 v )+{+	return v.x < v.y ? ( v.y < v.z ? 2 : 1 ) : ( v.x < v.z ? 2 : 0 );+}++static inline b3Vec2 b3Add2( b3Vec2 a, b3Vec2 b )+{+	b3Vec2 c = { a.x + b.x, a.y + b.y };+	return c;+}++static inline b3Vec2 b3Sub2( b3Vec2 a, b3Vec2 b )+{+	b3Vec2 c = { a.x - b.x, a.y - b.y };+	return c;+}++static inline b3Vec2 b3Neg2( b3Vec2 v )+{+	b3Vec2 c = { -v.x, -v.y };+	return c;+}++static inline b3Vec2 b3MulSV2( float s, b3Vec2 v )+{+	b3Vec2 c = { s * v.x, s * v.y };+	return c;+}++// a + s * b+static 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+static inline b3Vec2 b3MulSub2( b3Vec2 a, float s, b3Vec2 b )+{+	b3Vec2 c = { a.x - s * b.x, a.y - s * b.y };+	return c;+}++static inline float b3Cross2( b3Vec2 a, b3Vec2 b )+{+	return a.x * b.y - a.y * b.x;+}++static inline float b3DistanceSquared2( b3Vec2 a, b3Vec2 b )+{+	float dx = b.x - a.x;+	float dy = b.y - a.y;+	return dx * dx + dy * dy;+}++static 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;+}++static inline b3Matrix2 b3MulMM2( b3Matrix2 m1, b3Matrix2 m2 )+{+	b3Matrix2 out;+	out.cx = b3MulMV2( m1, m2.cx );+	out.cy = b3MulMV2( m1, m2.cy );+	return out;+}++static inline float b3Det2( b3Matrix2 m )+{+	return m.cx.x * m.cy.y - m.cx.y * m.cy.x;+}++static 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+static 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 };+}++// Convenience function: s * a + t * b + u * c+static 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;+}++static 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;+}++static inline b3Matrix3 b3MakeDiagonalMatrix( float a, float b, float c )+{+	return (b3Matrix3){ { a, 0.0f, 0.0f }, { 0.0f, b, 0.0f }, { 0.0f, 0.0f, c } };+}++static inline b3Matrix3 b3Skew( b3Vec3 v )+{+	b3Matrix3 out;+	out.cx = (b3Vec3){ 0, v.z, -v.y };+	out.cy = (b3Vec3){ -v.z, 0, v.x };+	out.cz = (b3Vec3){ v.y, -v.x, 0 };++	return out;+}++static inline b3Plane b3NormalizePlane( b3Plane plane )+{+	float invLength = 1.0f / b3Length( plane.normal );+	return (b3Plane){ b3MulSV( invLength, plane.normal ), invLength * plane.offset };+}++static inline b3Plane b3MakePlaneFromNormalAndPoint( b3Vec3 normal, b3Vec3 point )+{+	return (b3Plane){ normal, b3Dot( normal, point ) };+}++static 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;+}++static inline b3Vec3 b3MakeNormalFromPoints( b3Vec3 point1, b3Vec3 point2, b3Vec3 point3 )+{+	b3Vec3 normal = b3Cross( b3Sub( point2, point1 ), b3Sub( point3, point1 ) );+	return b3Normalize( normal );+}++// normal2 = q * normal1+// offset2 = dot(normal2, p) + offset1+static 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 ) };+}++/// Signed separation of a point from a plane+static inline float b3PlaneSeparation( b3Plane plane, b3Vec3 point )+{+	return b3Dot( plane.normal, point ) - plane.offset;+}++// Negative if p is below the triangle v1-v2-v3+static 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+static inline bool b3IsWithinSegments( const b3SegmentDistanceResult* result )+{+	return ( 0.0f <= result->fraction1 && result->fraction1 <= 1.0f ) &&+		   ( 0.0f <= result->fraction2 && result->fraction2 <= 1.0f );+}++static inline b3Matrix3 b3RotateInertia( b3Quat q, b3Matrix3 centralInertia )+{+	b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( q );+	b3Matrix3 inertia = b3MulMM( rotationMatrix, b3MulMM( centralInertia, b3Transpose( rotationMatrix ) ) );+	return inertia;+}++static inline b3Matrix3 b3TransformInertia( b3Transform transform, b3Matrix3 centralInertia, float mass )+{+	b3Matrix3 inertia = b3RotateInertia( transform.q, centralInertia );+	inertia = b3AddMM( inertia, b3Steiner( mass, transform.p ) );+	return inertia;+}++// Add a point to an AABB.+static inline b3AABB b3AABB_AddPoint( b3AABB a, b3Vec3 point )+{+	return (b3AABB){ b3Min( a.lowerBound, point ), b3Max( a.upperBound, point ) };+}
+ upstream/src/mesh.c view
@@ -0,0 +1,2415 @@+// 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 <stdint.h>++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+{+	b3Array( 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 );+	b3Array( 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( b3Array( 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;++		// Zero so mesh->hash is deterministic+		node->triangleOffset = 0;++		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;+	b3Array( b3MeshTriangle ) tempTriangles;+	b3Array_CreateN( tempTriangles, mesh->triangleCount );++	b3Array( 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 );++	b3Array( 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;++	b3Array( int ) indices = { 0 };+	b3Array_Resize( indices, 3 * triangleCount );++	b3Array( 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 );++	b3Array( 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 * cellWidth;+	float omegaX = 2.0f * B3_PI * columnFrequency * cellWidth;++	float x = -0.5f * xWidth;+	b3CosSin cs = { 0 };+	for ( int ix = 0; ix <= xCount; ++ix )+	{+		cs = b3ComputeCosSin( omegaX * ix );+		float rowHeight = cs.sine;++		float z = -0.5f * zWidth;+		for ( int iz = 0; iz <= zCount; ++iz )+		{+			cs = b3ComputeCosSin( omegaZ * iz );+			float columnHeight = cs.sine;++			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;++	b3Array( 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+	b3Array( 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+	b3Array( 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 );+}++b3MeshData* b3CreateHollowBoxMesh( b3Vec3 center, b3Vec3 extent )+{+	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[] = {+		3, 1, 0, 3, 2, 1, // front+		1, 4, 0, 5, 4, 1, // top+		7, 3, 0, 7, 0, 4, // right+		5, 7, 4, 7, 5, 6, // back+		2, 5, 1, 5, 2, 6, // left+		7, 2, 3, 2, 7, 6, // bottom+	};++	b3MeshDef def = { 0 };+	def.vertexCount = 8;+	def.vertices = vertices;+	def.triangleCount = 12;+	def.indices = indices;+	def.useMedianSplit = false;+	def.identifyEdges = true;++	return b3CreateMesh( &def, NULL, 0 );+}++b3MeshData* b3CreatePlatformMesh( b3Vec3 center, float height, float topWidth, float bottomWidth )+{+	float hb = 0.5f * bottomWidth;+	float ht = 0.5f * topWidth;+	float hy = 0.5f * height;+	b3Vec3 vertices[] = {+		{ ht, hy, ht },	 { -ht, hy, ht },  { -hb, -hy, hb },  { hb, -hy, hb },+		{ ht, hy, -ht }, { -ht, hy, -ht }, { -hb, -hy, -hb }, { hb, -hy, -hb },+	};++	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 = true;+	def.identifyEdges = true;++	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+	b3Array( int ) indices;+	b3Array_CreateN( indices, 3 * triangleCount );++	b3Array( 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 );+	}++	b3Array( 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)+	b3Array( b3MeshNode ) tempNodes;+	b3Array_CreateN( tempNodes, 2 * triangleCount - 1 );++	int treeHeight = 0;+	b3BuildRecursive( &tempNodes, triangleCount, primitives.data, primitives.data, def->useMedianSplit, &treeHeight );++	// Allocate the mesh+	size_t byteCount = b3AlignUp8( sizeof( b3MeshData ) );+	int nodeOffset = (int)byteCount;+	byteCount += b3AlignUp8( tempNodes.count * sizeof( b3MeshNode ) );+	int vertexOffset = (int)byteCount;+	byteCount += b3AlignUp8( vertexCount * sizeof( b3Vec3 ) );+	int triangleOffset = (int)byteCount;+	byteCount += b3AlignUp8( triangleCount * sizeof( b3MeshTriangle ) );+	int materialIndicesOffset = (int)byteCount;+	byteCount += b3AlignUp8( triangleCount * sizeof( uint8_t ) );+	int flagsOffset = (int)byteCount;+	byteCount += b3AlignUp8( triangleCount * sizeof( uint8_t ) );++	b3MeshData* mesh = b3Alloc( byteCount );++	// zero initialize for determinism+	memset( mesh, 0, byteCount );++	mesh->version = B3_MESH_VERSION;+	mesh->byteCount = (int)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 = b3Hash64NonZero((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.transform = 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( &center.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.transform = 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.transform = 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];+	}+}
+ upstream/src/mesh_contact.c view
@@ -0,0 +1,1186 @@+// 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 <stdio.h>++// This guards against excessive memory usage and complex collision+#define B3_MAX_MESH_CONTACT_TRIANGLES 256+#define B3_MAX_POINTS_PER_TRIANGLE 32++#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 b3HeightFieldData* 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, b3WorldTransform xfA, const b3AABB* bounds )+{+	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_MAX_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 = B3_MAX_MESH_CONTACT_TRIANGLES;++	int triangleIndices[B3_MAX_MESH_CONTACT_TRIANGLES];++	// Bounds are in world space. Convert to the local mesh frame. The broadphase bounds are float,+	// so the demoted mesh transform is the matching float world frame (exact in float mode).+	b3Transform meshTransform = b3ToRelativeTransform( xfA, b3Pos_zero );+	b3AABB localBounds = b3AABB_Transform( b3InvertTransform( meshTransform ), 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 )+	{+		static bool s_once = false;+		if ( s_once == false )+		{+			b3Log( "WARNING: complex mesh detected, triangle buffer capacity of %d reached", triangleCapacity );+			s_once = true;+		}+	}++	// Triangle indices must be sorted to match caches.+	B3_VALIDATE( b3IsSorted( triangleIndices, triangleCount ) );++	// Create new contact cache and match with old one+	b3ContactCache contactCache[B3_MAX_MESH_CONTACT_TRIANGLES];++	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;+}++// Returns true if (score, separation) should replace (bestScore, bestSeparation).+static inline bool b3IsBetterCullCandidate( float score, float separation, float bestScore, float bestSeparation, float scoreTol,+											float separationTol )+{+	if ( score > bestScore + scoreTol )+	{+		return true;+	}+	if ( score < bestScore - scoreTol )+	{+		return false;+	}++	// Break the tie using separation+	return separation < bestSeparation - separationTol;+}++typedef struct b3Point2D+{+	b3Vec2 p;+	float separation;+	int originalIndex;+} b3Point2D;++static int b3CullPoints( b3Point2D* points, int count )+{+	if ( count <= 1 )+	{+		return count;+	}++	float tol = 0.25f * B3_LINEAR_SLOP;+	float tolSqr = tol * tol;+	float separationTol = B3_LINEAR_SLOP;++	b3Point2D finalPoints[4];+	int count1 = count;++	// Step 1: the two points with the largest distance, ties broken by deepest combined separation+	float bestScore = 0.0f;+	float bestSeparation = FLT_MAX;+	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 );+			// Separation sum heuristic+			float separation = points[i].separation + points[j].separation;++			if ( b3IsBetterCullCandidate( score, separation, bestScore, bestSeparation, tolSqr, separationTol ) )+			{+				bestIndex1 = i;+				bestIndex2 = j;+				bestScore = score;+				bestSeparation = separation;+			}+		}+	}++	if ( bestScore < tolSqr )+	{+		// Choose deepest point+		int deepestIndex = 0;+		for ( int i = 1; i < count1; ++i )+		{+			if ( points[i].separation < points[deepestIndex].separation )+			{+				deepestIndex = i;+			}+		}++		if ( deepestIndex != 0 )+		{+			points[0] = points[deepestIndex];+		}+		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 )+	{+		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, ties broken by deepest separation+	bestScore = 0.0f;+	bestSeparation = FLT_MAX;+	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 );++		if ( b3IsBetterCullCandidate( score, points[i].separation, bestScore, bestSeparation, tolSqr, separationTol ) )+		{+			bestSignedArea = signedArea;+			bestScore = score;+			bestSeparation = points[i].separation;+			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 )+	{+		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;+	bestSeparation = FLT_MAX;+	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 ( b3IsBetterCullCandidate( score, points[i].separation, bestScore, bestSeparation, tolSqr, separationTol ) )+		{+			bestScore = score;+			bestSeparation = points[i].separation;+			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 = 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 )+	{+		b3Vec3 d = b3Sub( points[i].point, origin );+		pts[i].p = (b3Vec2){ b3Dot( d, u ), b3Dot( d, v ) };+		pts[i].separation = points[i].separation;+		pts[i].originalIndex = i;+	}++	int count2 = b3CullPoints( pts, count1 );+	B3_ASSERT( count2 <= B3_MAX_MANIFOLD_POINTS );++	b3LocalManifoldPoint finalPoints[B3_MAX_MANIFOLD_POINTS];+	for ( int i = 0; i < count2; ++i )+	{+		int index = pts[i].originalIndex;+		B3_ASSERT( 0 <= index && index < count1 );+		finalPoints[i] = points[index];+	}++	memcpy( points, finalPoints, count2 * sizeof( b3LocalManifoldPoint ) );+	return count2;+}++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,+							 b3WorldTransform xfA, const b3Shape* shapeB, b3WorldTransform 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 );++	b3RefreshCache( contact, shapeA, xfA, &shapeB->aabb );++	// Collide with triangles and build manifolds+	b3MeshContact* meshContact = &contact->meshContact;+	int triangleCount = meshContact->triangleCache.count;++	b3LocalManifold** acceptedManifolds = b3Bump( &arena, triangleCount * sizeof( b3LocalManifold* ) );+	int acceptedManifoldCount = 0;+	b3LocalManifold** tentativeManifolds = b3Bump( &arena, triangleCount * sizeof( b3LocalManifold* ) );+	int tentativeManifoldCount = 0;+	b3TentativeTriangle* tentativeTriangles = 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 = b3InvMulWorldTransforms( xfB, xfA );+	b3Matrix3 relativeMatrix = b3MakeMatrixFromQuat( transformAtoB.q );+	float linearSlop = B3_LINEAR_SLOP;++	// This should push apart shapes after a time of impact event.+	// In the past I've called this `polygon skin`, but PhysX and Unreal+	// call it `rest offset` which seems appropriate in this case.+	// It leads to a small visual gap but seems to improve the quality of mesh+	// collision, especially for hull versus mesh.+	float restOffset = B3_MESH_REST_OFFSET;+	bool enableSpeculative = contact->flags & b3_enableSpeculativePoints;++	// Make room for clip points+	int pointBufferCapacity = B3_MAX_POINTS_PER_TRIANGLE * triangleCount;++	b3LocalManifoldPoint* pointBuffer = b3Bump( &arena, pointBufferCapacity * sizeof( b3LocalManifoldPoint ) );+	int totalPointCount = 0;++	b3LocalManifold* manifoldBuffer = b3Bump( &arena, triangleCount * sizeof( b3LocalManifold ) );+	int manifoldCount = 0;++	b3TriangleCache* triangleCaches = meshContact->triangleCache.data;++	const b3HullData* hullB = shapeB->type == b3_hullShape ? shapeB->hull : NULL;++	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:+				b3CollideTriangleAndCapsule( manifold, pointCapacity, vertices, &shapeB->capsule, &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 };+				}++				b3CollideTriangleAndHull( manifold, pointCapacity, vertices[0], vertices[1], vertices[2], triangle.flags, hullB,+										  &cache->satCache, enableSpeculative );+				context->satCallCount += 1;+				context->satCacheHitCount += cache->satCache.hit;+				break;++			case b3_sphereShape:+				b3CollideTriangleAndSphere( manifold, pointCapacity, vertices, &shapeB->sphere );+				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->triangleNormal = b3MakeNormalFromPoints( 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->triangleNormal, 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;+	}++	b3Cluster* clusters = b3Bump( &arena, acceptedManifoldCount * sizeof( b3Cluster ) );+	int* clusterMemberships = b3Bump( &arena, acceptedManifoldCount * sizeof( int ) );++	// Cluster tolerance is tighter than the warm starting manifold matching tolerance. These+	// serve different purposes.+	const float clusterThreshold = 0.996f;+	int clusterCount = 0;+	int clusterPointCount = 0;+	for ( int i = 0; i < acceptedManifoldCount; ++i )+	{+		clusterMemberships[i] = B3_NULL_INDEX;++		const b3LocalManifold* manifold = acceptedManifolds[i];+		clusterPointCount += 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->triangleNormal;+		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 ( clusterPointCount == 0 )+	{+		return false;+	}++	// Setup clusters+	b3LocalManifoldPoint* clusterPoints = b3Bump( &arena, clusterPointCount * 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;++			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 = 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 = b3Bump( &arena, oldManifoldCount * sizeof( bool ) );+		memset( consumed, 0, oldManifoldCount * sizeof( bool ) );+	}++	b3Matrix3 matrixB = b3MakeMatrixFromQuat( xfB.q );+	b3Vec3 offsetA = b3SubPos( 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* materialsA = b3GetShapeMaterials( shapeA );+	const b3SurfaceMaterial* materialB = b3GetShapeMaterials( shapeB );+	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 = b3GetHeightFieldMaterialIndices( shapeA->heightField );+		}++		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 = materialsA[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( materialsA[0].friction, materialsA[0].userMaterialId, materialB->friction,+													 materialB->userMaterialId );+		contact->restitution = world->restitutionCallback( materialsA[0].restitution, materialsA[0].userMaterialId,+														   materialB->restitution, materialB->userMaterialId );+		tangentVelocityA = materialsA[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 = materialB->rollingResistance * radiusB;++	b3Vec3 tangentVelocityB = b3RotateVector( xfB.q, materialB->tangentVelocity );+	contact->tangentVelocity = b3Sub( tangentVelocityA, tangentVelocityB );+	return true;+}
+ upstream/src/motor_joint.c view
@@ -0,0 +1,439 @@+// 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"+#include "recording.h"++// needed for dll export+#include "box3d/box3d.h"++void b3MotorJoint_SetLinearVelocity( b3JointId jointId, b3Vec3 velocity )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, MotorJointSetLinearVelocity, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, MotorJointSetAngularVelocity, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, MotorJointSetMaxVelocityTorque, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, MotorJointSetMaxVelocityForce, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, MotorJointSetLinearHertz, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, MotorJointSetLinearDampingRatio, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, MotorJointSetAngularHertz, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, MotorJointSetAngularDampingRatio, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, MotorJointSetMaxSpringForce, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, MotorJointSetMaxSpringTorque, jointId, 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 = b3SubPos( 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;+	}+}
+ upstream/src/mover.c view
@@ -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;+}
+ upstream/src/name_cache.c view
@@ -0,0 +1,155 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#include "name_cache.h"++#include "recording.h"++#include <string.h>++#define NAME b3NameMap+#define KEY_TY uint32_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"++#define FNV32_OFFSET_BASIS 0x811c9dc5u+#define FNV32_PRIME 0x01000193u++// This is designed to hash small strings and while avoiding collisions+// for a 32-bit hash.+// Note: changing this will break recordings+uint32_t b3Hash32( const void* data, size_t length )+{+	// Cast to unsigned to avoid sign extension+	const unsigned char* p = data;++	// FNV-1a+	uint32_t h = FNV32_OFFSET_BASIS;+	for ( size_t i = 0; i < length; ++i )+	{+		h ^= p[i];+		h *= FNV32_PRIME;+	}++	// Murmur3 finalizer to help with short strings+	h ^= h >> 16;+	h *= 0x85ebca6bu;+	h ^= h >> 13;+	h *= 0xc2b2ae35u;+	h ^= h >> 16;+	return h;+}++b3NameCache b3CreateNameCache( void )+{+	b3NameCache cache = { 0 };+	b3Array_Create( cache.entries );++	b3NameMap* map = b3Alloc( sizeof( b3NameMap ) );+	b3NameMap_init( map );+	cache.map = map;+	return cache;+}++void b3DestroyNameCache( b3NameCache* cache )+{+	int count = cache->entries.count;+	for ( int i = 0; i < count; ++i )+	{+		b3NameEntry* entry = cache->entries.data + i;+		b3Free( entry->name, entry->length + 1 );+	}++	b3Array_Destroy( cache->entries );+	b3NameMap_cleanup( (b3NameMap*)cache->map );+	b3Free( cache->map, sizeof( b3NameMap ) );+}++uint32_t b3AddName( b3NameCache* cache, const char* name )+{+	if ( name == NULL || name[0] == 0 )+	{+		return B3_NULL_NAME;+	}++	size_t length = strlen( name );+	uint32_t id = b3Hash32( name, length );++	id = id == 0 ? 1 : id;++	b3NameMap* map = cache->map;+	b3NameMap_itr itr = b3NameMap_get( map, id );+	if ( b3NameMap_is_end( itr ) == false )+	{+		if ( strcmp( cache->entries.data[itr.data->val].name, name ) != 0 )+		{+			// Different name, same hash+			b3Log( "Hash collision on %s", name );+		}+		return id;+	}++	char* clone = b3Alloc( length + 1 );+	memcpy( clone, name, length + 1 );+	int index = cache->entries.count;+	b3NameEntry entry = {+		.hash = id,+		.name = clone,+		.length = (int)length,+	};++	b3Array_Push( cache->entries, entry );++	b3NameMap_insert( map, id, index );+	return id;+}++const char* b3FindName( const b3NameCache* cache, uint32_t id )+{+	b3NameMap* map = cache->map;+	b3NameMap_itr itr = b3NameMap_get( map, id );+	if ( b3NameMap_is_end( itr ) == false )+	{+		int index = itr.data->val;+		const b3NameEntry* entry = b3Array_Get( cache->entries, index );+		return entry->name;+	}++	return NULL;+}++void b3LoadName( b3NameCache* cache, uint32_t id, char* name, int length )+{+	if ( name == NULL )+	{+		return;+	}++	if ( name[0] == 0 || id == B3_NULL_NAME )+	{+		b3Free( name, length + 1 );+		return;+	}++	b3NameMap* map = cache->map;+	b3NameMap_itr itr = b3NameMap_get( map, id );+	if ( b3NameMap_is_end( itr ) == false )+	{+		b3Free( name, length + 1 );+		return;+	}++	int index = cache->entries.count;+	b3NameEntry entry = {+		.hash = id,+		.name = name,+		.length = length,+	};++	b3Array_Push( cache->entries, entry );+	b3NameMap_insert( map, id, index );+}
+ upstream/src/name_cache.h view
@@ -0,0 +1,42 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "container.h"++#define B3_NULL_NAME 0u++typedef struct b3NameEntry+{+	// Names are used for debugging and I'm favoring simplicity and+	// minimal storage. Collisions will be logged.+	uint32_t hash;+	int length;+	char* name;+} b3NameEntry;++b3DeclareArray( b3NameEntry );++typedef struct b3NameCache+{+	b3Array( b3NameEntry ) entries;+	void* map;+} b3NameCache;++b3NameCache b3CreateNameCache( void );+void b3DestroyNameCache( b3NameCache* cache );++uint32_t b3AddName( b3NameCache* cache, const char* name );+const char* b3FindName( const b3NameCache* cache, uint32_t id );++// Load a name from a recording. Name ownership is transferred.+void b3LoadName( b3NameCache* cache, uint32_t id, char* name, int length );++uint32_t b3Hash32( const void* data, size_t length );++static inline const char* b3FindNameWithDefault( const b3NameCache* cache, uint32_t id, const char* def )+{+	const char* name = b3FindName( cache, id );+	return name == NULL ? def : name;+}
+ upstream/src/parallel_for.c view
@@ -0,0 +1,136 @@+// 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 <stddef.h>++// 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.+	// Benchmarking shows 32 is optimal for the convex pile benchmark and others.+	int blocksPerWorker = 32;+	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 );+		}+	}+}
+ upstream/src/parallel_for.h view
@@ -0,0 +1,23 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++typedef struct b3World b3World;++// Callback invoked by b3ParallelFor 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 );++// 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 );
+ upstream/src/parallel_joint.c view
@@ -0,0 +1,251 @@+// 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"+#include "recording.h"++// needed for dll export+#include "box3d/box3d.h"++void b3ParallelJoint_SetSpringHertz( b3JointId jointId, float hertz )+{+	B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, ParallelJointSetSpringHertz, jointId, hertz );+	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 );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, ParallelJointSetSpringDampingRatio, jointId, dampingRatio );+	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 );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, ParallelJointSetMaxTorque, jointId, maxForce );+	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, b3WorldTransform transformA, b3WorldTransform transformB, float scale )+{+	float length = 0.1f * scale;++	b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA );+	draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length, b3RotateVector( frameA.q, b3Vec3_axisZ ) ) ), b3_colorGreen, draw->context );++	b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB );+	draw->DrawSegmentFcn( frameB.p, b3OffsetPos( frameB.p, b3MulSV( length, b3RotateVector( frameB.q, b3Vec3_axisZ ) ) ), b3_colorBlue, draw->context );+}
+ upstream/src/physics_world.c view
@@ -0,0 +1,4112 @@+// 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 "constraint_graph.h"+#include "contact.h"+#include "core.h"+#include "ctz.h"+#include "hull.h"+#include "island.h"+#include "joint.h"+#include "parallel_for.h"+#include "platform.h"+#include "recording.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 <float.h>+#include <inttypes.h>+#include <stdio.h>+#include <string.h>++_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;++const b3HullData* b3AddHullToDatabase( b3World* world, const b3HullData* src )+{+	b3HullMap* database = world->hullDatabase;++	// Compare by content to de-duplicate. Not trusting the hash.+	b3HullMap_itr itr = b3HullMap_get( database, src );+	if ( b3HullMap_is_end( itr ) == false )+	{+		// Bump reference count.+		itr.data->val += 1;+		return itr.data->key;+	}++	b3HullData* clone = b3CloneHull( src );+	B3_ASSERT( clone != NULL );++	// Start with reference count of 1.+	b3HullMap_insert( database, clone, 1 );+	return clone;+}++const b3HullData* b3AddOwnedHullToDatabase( b3World* world, b3HullData* owned )+{+	b3HullMap* database = world->hullDatabase;++	b3HullMap_itr itr = b3HullMap_get( database, owned );+	if ( b3HullMap_is_end( itr ) == false )+	{+		itr.data->val += 1;+		b3DestroyHull( owned );+		return itr.data->key;+	}++	// Take ownership of input hull.+	b3HullMap_insert( database, owned, 1 );+	return owned;+}++void b3RemoveHullFromDatabase( b3World* world, const b3HullData* data )+{+	b3HullMap* database = world->hullDatabase;++	b3HullMap_itr itr = b3HullMap_get( database, data );+	B3_ASSERT( b3HullMap_is_end( itr ) == false );++	if ( --itr.data->val == 0 )+	{+		// Erase through the iterator we already have so the lookup runs once.+		b3HullData* owned = (b3HullData*)itr.data->key;+		b3HullMap_erase_itr( database, itr );+		b3DestroyHull( owned );+	}+}++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( 128 * 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 );++	B3_ASSERT( B3_LINEAR_SLOP <= B3_MESH_REST_OFFSET );+	B3_ASSERT( B3_MESH_REST_OFFSET < B3_SPECULATIVE_DISTANCE );++	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->hullDatabase = b3Alloc( sizeof( b3HullMap ) );+	b3HullMap_init( world->hullDatabase );++	world->names = b3CreateNameCache();++	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;++	// Detach any recording before teardown. The user owns and frees the recording buffer.+	b3StopRecordingInternal( world );++	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 );+			}+		}+	}++	// Destroying every shape above released all hull references, so the database is empty.+	B3_ASSERT( b3HullMap_size( (b3HullMap*)world->hullDatabase ) == 0 );+	b3HullMap_cleanup( world->hullDatabase );+	b3Free( world->hullDatabase, sizeof( b3HullMap ) );+	world->hullDatabase = NULL;++	b3DestroyNameCache( &world->names );++	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 );+			}+		}++		b3WorldTransform transformA = bodySimA->transform;+		b3WorldTransform 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 ) && ( contact->flags & b3_contactRecycleFlag ) )+		{+			// The scalar part of b3InvMulQuat is just the quaternion dot product.+			// cos(relative_angle/2) = scalar(conj(q1) * q2) = dot(q1, q2)+			// A small relative angle means this value is close to 1. Need to use abs or square+			// due to double cover.+			float angleA = b3DotQuat( transformA.q, contact->cachedRotationA );+			float angleB = b3DotQuat( transformB.q, contact->cachedRotationB );+			float angularDistance = b3MinFloat( angleA * angleA, angleB * angleB );++			b3Transform xf = b3InvMulWorldTransforms( transformA, transformB );+			b3Transform xfc = contact->cachedRelativePose;+			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 ( angularDistance > B3_CONTACT_RECYCLE_ANGULAR_DISTANCE && 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->cachedRotationA ) );+					b3Quat dqB = b3MulQuat( transformB.q, b3Conjugate( contact->cachedRotationB ) );+					b3Matrix3 matrixA = b3MakeMatrixFromQuat( dqA );+					b3Matrix3 matrixB = b3MakeMatrixFromQuat( dqB );++					// Minimize round-off+					b3Vec3 dc = b3SubPos( 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->cachedRotationA = transformA.q;+		contact->cachedRotationB = transformB.q;+		contact->cachedRelativePose = b3InvMulWorldTransforms( transformA, 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];+		}+	}++	// Release per-step overflow blocks and grow the backing capacity if last+	// step's demand exceeded it. Contact processing is the only consumer of+	// these arenas and is finished by this point.+	for ( int i = 0; i < world->workerCount; ++i )+	{+		b3ArenaSync( &world->taskContexts.data[i].arena );+	}++	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;+	}++	B3_REC( world, Step, worldId, timeStep, subStepCount );++	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;++	if ( world->recording != NULL )+	{+		uint64_t hash = b3HashWorldState( world );+		b3RecArgs_StateHash stateHash = { worldId, hash };+		b3RecWrite_StateHash( world->recording, &stateHash );++		// Fold this step's world bounds into the recording so a viewer can frame the whole motion.+		b3AABB worldBounds = { 0 };+		bool haveBounds = false;+		for ( int i = 0; i < b3_bodyTypeCount; ++i )+		{+			b3DynamicTree* tree = world->broadPhase.trees + i;+			if ( b3DynamicTree_GetProxyCount( tree ) == 0 )+			{+				continue;+			}+			b3AABB bounds = b3DynamicTree_GetRootBounds( tree );+			worldBounds = haveBounds ? b3AABB_Union( worldBounds, bounds ) : bounds;+			haveBounds = true;+		}+		if ( haveBounds )+		{+			b3RecAccumulateBounds( world->recording, worldBounds );+		}+	}++	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;++		const b3SurfaceMaterial* surfaceMaterial = b3GetShapeMaterials( shape );+		if ( surfaceMaterial[0].customColor != 0 )+		{+			// May already carry a packed material preset, pass through unchanged+			color = (b3HexColor)surfaceMaterial[0].customColor;+		}+		else+		{+			// Hue carries the state, material carries its energy. Calm matte for the+			// resting masses, glossy for fast bodies, metallic for the driven kinematic.+			// Diagnostic states keep a saturated hue and the default material so they pop.+			b3HexColor rgb;+			b3DebugMaterial material = b3_debugMaterialDefault;++			if ( body->type == b3_dynamicBody && body->mass == 0.0f )+			{+				// Bad body+				rgb = b3_colorRed;+			}+			else if ( body->setIndex == b3_disabledSet )+			{+				rgb = b3_colorSlateGray;+			}+			else if ( shape->sensorIndex != B3_NULL_INDEX )+			{+				rgb = b3_colorWheat;+			}+			else if ( body->flags & b3_hadTimeOfImpact )+			{+				rgb = b3_colorLime;+			}+			else if ( ( bodySim->flags & b3_isBullet ) && body->setIndex == b3_awakeSet )+			{+				rgb = b3_colorTurquoise;+			}+			else if ( body->flags & b3_isSpeedCapped )+			{+				rgb = b3_colorYellow;+			}+			else if ( bodySim->flags & b3_isFast )+			{+				rgb = b3_colorOrange;+				material = b3_debugMaterialGlossy;+			}+			else if ( body->type == b3_staticBody )+			{+				rgb = b3_colorDarkGray;+				material = b3_debugMaterialMatte;+			}+			else if ( body->type == b3_kinematicBody )+			{+				if ( body->setIndex == b3_awakeSet )+				{+					rgb = b3_colorSteelBlue;+					material = b3_debugMaterialMetallic;+				}+				else+				{+					rgb = b3_colorLightSteelBlue;+					material = b3_debugMaterialMatte;+				}+			}+			else if ( body->setIndex == b3_awakeSet )+			{+				rgb = b3_colorTan;+				material = b3_debugMaterialSoft;+			}+			else+			{+				rgb = b3_colorLightSlateGray;+				material = b3_debugMaterialDead;+			}++			color = (b3HexColor)b3MakeDebugColor( rgb, material );+		}++		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 )+	{+		draw->DrawBoundsFcn( shape->fatAABB, 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 ) );++	float lengthScale = b3GetLengthUnitsPerMeter();+	float axisScale = 0.3f * lengthScale;+	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->nameId != B3_NULL_NAME )+			{+				b3Vec3 offset = { 0.05f, 0.05f, 0.05f };+				b3WorldTransform transform = { bodySim->center, bodySim->transform.q };+				b3Pos p = b3TransformWorldPoint( transform, offset );+				const char* name = b3FindName( &world->names, body->nameId );+				if ( name != NULL )+				{+					draw->DrawStringFcn( p, name, b3_colorWhite, draw->context );+				}+			}++			if ( draw->drawMass )+			{+				b3WorldTransform transform = { bodySim->center, bodySim->transform.q };+				draw->DrawTransformFcn( transform, draw->context );++				if (body->type == b3_dynamicBody)+				{+					b3Vec3 offset = { 0.05f, 0.05f, 0.05f };+					b3Pos p = b3TransformWorldPoint( transform, offset );+					char buffer[32];+					snprintf( buffer, 32, "%.2f", body->mass );+					draw->DrawStringFcn( p, buffer, b3_colorWhite, draw->context );+				}+			}++			if ( draw->drawSleep )+			{+				b3BodyState* bodyState = b3GetBodyState( world, body );++				if ( bodyState != NULL )+				{+					b3HexColor colors[4] = { b3_colorBlue, b3_colorSkyBlue, b3_colorOrange, b3_colorRed };++					b3HexColor color = b3_colorBlack;+					if ( body->sleepThreshold > 0.0f )+					{+						float ratio = body->sleepVelocity / body->sleepThreshold;+						int index = b3ClampInt( (int)ratio, 0, 3 );+						color = colors[index];+					}++					b3Pos center = bodySim->center;+					draw->DrawPointFcn( center, 10.0f, color, draw->context );++					b3Vec3 offset = { 0.1f, 0.1f, 0.1f };+					b3Pos p = b3OffsetPos( center, offset );++					char buffer[32];+					snprintf( buffer, 32, "  %.3f", body->sleepVelocity );+					draw->DrawStringFcn( p, buffer, color, 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;++							// Average the anchors not the world points so the friction center stays exact far from the origin+							b3Pos contactCenter = draw->drawAnchorA ? bodySimA->center : bodySimB->center;+							b3Vec3 frictionAnchor = b3Vec3_zero;+							float totalWeight = 0.0f;+							float invTau = 1.0f / B3_SPECULATIVE_DISTANCE;++							const b3ManifoldPoint* points = manifold->points;+							for ( int pointIndex = 0; pointIndex < manifold->pointCount; ++pointIndex )+							{+								const b3ManifoldPoint* mp = points + pointIndex;++								char buffer[32];++								b3Vec3 anchor = draw->drawAnchorA ? mp->anchorA : mp->anchorB;+								b3Pos p = b3OffsetPos( contactCenter, anchor );++								// See similar friction anchor weights in b3PrepareContacts_Mesh.+								float weight = b3ClampFloat( 2.0f - mp->separation * invTau, B3_MIN_FRICTION_WEIGHT, 1.0f );+								frictionAnchor = b3MulAdd( frictionAnchor, weight, anchor );+								totalWeight += weight;++								if ( draw->drawContactNormals )+								{+									b3Pos p1 = p;+									b3Pos p2 = b3OffsetPos( p1, b3MulSV( 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;+									b3Pos p1 = p;+									b3Pos p2 = b3OffsetPos( p1, b3MulSV( 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;+								frictionAnchor = b3MulSV( 1.0f / totalWeight, frictionAnchor );+								b3Pos p1 = b3OffsetPos( contactCenter, frictionAnchor );+								b3Vec3 frictionForce = b3MulSV( 0.5f * inv_dt, manifold->frictionImpulse );+								b3Pos p2 = b3OffsetPos( p1, b3MulSV( draw->forceScale, frictionForce ) );+								draw->DrawSegmentFcn( p1, p2, frictionColor, draw->context );+								draw->DrawPointFcn( p1, 5.0f, frictionColor, draw->context );++								p1 = b3OffsetPos( p1, b3MulSV( 0.05f * lengthScale, normal ) );+								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;+			b3Pos p = point->p;+			draw->DrawPointFcn( p, 5.0f, point->color, draw->context );+			snprintf( buffer, 32, "   %d, %.2f", point->label, point->value );+			b3Pos ps = b3OffsetPos( 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;+			b3Pos p1 = line->p1;+			b3Pos p2 = line->p2;+			draw->DrawSegmentFcn( p1, p2, line->color, draw->context );+			draw->DrawPointFcn( p1, 10.0f, line->color, draw->context );+			snprintf( buffer, 32, "%d", line->label );+			b3Pos ps = b3OffsetPos( p1, offset );+			draw->DrawStringFcn( ps, buffer, line->color, draw->context );+		}+	}+}++b3AABB b3World_GetBounds( b3WorldId worldId )+{+	b3World* world = b3GetUnlockedWorldFromId( worldId );+	if ( world == NULL )+	{+		return (b3AABB){ 0 };+	}++	b3AABB worldBounds = { 0 };+	bool haveBounds = false;++	for ( int i = 0; i < b3_bodyTypeCount; ++i )+	{+		b3DynamicTree* tree = world->broadPhase.trees + i;+		if ( b3DynamicTree_GetProxyCount( tree ) == 0 )+		{+			continue;+		}++		b3AABB bounds = b3DynamicTree_GetRootBounds( world->broadPhase.trees + i );++		if ( haveBounds )+		{+			worldBounds = b3AABB_Union( worldBounds, bounds );+		}+		else+		{+			worldBounds = bounds;+			haveBounds = true;+		}+	}++	return worldBounds;+}++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;+	}++	B3_REC( world, WorldEnableSleeping, worldId, flag );++	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;+	}++	B3_REC( world, WorldEnableWarmStarting, worldId, flag );++	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;+	}++	B3_REC( world, WorldEnableContinuous, worldId, flag );++	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;+	}++	B3_REC( world, WorldSetRestitutionThreshold, worldId, value );++	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;+	}++	B3_REC( world, WorldSetHitEventThreshold, worldId, value );++	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;+	}++	B3_REC( world, WorldSetContactTuning, worldId, hertz, dampingRatio, contactSpeed );++	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;+	}++	B3_REC( world, WorldSetContactRecycleDistance, worldId, recycleDistance );++	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;+	}++	B3_REC( world, WorldSetMaximumLinearSpeed, worldId, maximumLinearSpeed );++	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;+	s.arenaCapacity = 0;+	s.distanceIterations = 0;+	s.pushBackIterations = 0;+	s.rootIterations = 0;+	for ( int i = 0; i < world->workerCount; ++i )+	{+		s.recycledContactCount += world->taskContexts.data[i].recycledContactCount;++		s.distanceIterations = b3MaxInt( s.distanceIterations, world->taskContexts.data[i].distanceIterations );+		s.pushBackIterations = b3MaxInt( s.pushBackIterations, world->taskContexts.data[i].pushBackIterations );+		s.rootIterations = b3MaxInt( s.rootIterations, world->taskContexts.data[i].rootIterations );++		int peak = world->taskContexts.data[i].arena.shared->peakDemand;+		if ( peak > s.arenaCapacity )+		{+			s.arenaCapacity = peak;+		}+	}++	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_SetFrictionCallback( b3WorldId worldId, b3FrictionCallback* callback )+{+	b3World* world = b3GetUnlockedWorldFromId( worldId );+	if ( world == NULL )+	{+		return;+	}++	world->frictionCallback = callback != NULL ? callback : b3DefaultFrictionCallback;+}++void b3World_SetRestitutionCallback( b3WorldId worldId, b3RestitutionCallback* callback )+{+	b3World* world = b3GetUnlockedWorldFromId( worldId );+	if ( world == NULL )+	{+		return;+	}++	world->restitutionCallback = callback != NULL ? callback : b3DefaultRestitutionCallback;+}++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;+}++void b3World_StartRecording( b3WorldId worldId, b3Recording* recording )+{+	// Must be a step boundary, so refuse a locked world+	b3World* world = b3GetUnlockedWorldFromId( worldId );++	if ( world == NULL || recording == NULL || world->recording != NULL )+	{+		return;+	}++	b3StartRecordingIntoBuffer( world, recording );+}++void b3World_StopRecording( b3WorldId worldId )+{+	b3World* world = b3GetUnlockedWorldFromId( worldId );+	if ( world == NULL )+	{+		return;+	}++	b3StopRecordingInternal( world );+}++void b3World_DumpMemoryStats( b3WorldId worldId )+{+	b3World* world = b3GetUnlockedWorldFromId( worldId );+	if ( world == NULL )+	{+		return;+	}++	// Large worlds can exceed 2GB, sum in 64 bits+	uint64_t total = 0;++	// id pools+	int bodyIdBytes = b3GetIdBytes( &world->bodyIdPool );+	int solverSetIdBytes = b3GetIdBytes( &world->solverSetIdPool );+	int jointIdBytes = b3GetIdBytes( &world->jointIdPool );+	int contactIdBytes = b3GetIdBytes( &world->contactIdPool );+	int islandIdBytes = b3GetIdBytes( &world->islandIdPool );+	int shapeIdBytes = b3GetIdBytes( &world->shapeIdPool );+	total += (uint64_t)bodyIdBytes + solverSetIdBytes + jointIdBytes + contactIdBytes + islandIdBytes + shapeIdBytes;++	b3Log( "id pools" );+	b3Log( "body ids: %d", bodyIdBytes );+	b3Log( "solver set ids: %d", solverSetIdBytes );+	b3Log( "joint ids: %d", jointIdBytes );+	b3Log( "contact ids: %d", contactIdBytes );+	b3Log( "island ids: %d", islandIdBytes );+	b3Log( "shape ids: %d", shapeIdBytes );++	// Islands own per-island body/contact/joint link arrays+	int islandLinkBytes = 0;+	for ( int i = 0; i < world->islands.count; ++i )+	{+		b3Island* island = world->islands.data + i;+		islandLinkBytes += b3Array_ByteCount( island->bodies );+		islandLinkBytes += b3Array_ByteCount( island->contacts );+		islandLinkBytes += b3Array_ByteCount( island->joints );+	}++	// world arrays+	int bodyArrayBytes = b3Array_ByteCount( world->bodies );+	int solverSetArrayBytes = b3Array_ByteCount( world->solverSets );+	int jointArrayBytes = b3Array_ByteCount( world->joints );+	int contactArrayBytes = b3Array_ByteCount( world->contacts );+	int islandArrayBytes = b3Array_ByteCount( world->islands );+	int shapeArrayBytes = b3Array_ByteCount( world->shapes );+	int sensorArrayBytes = b3Array_ByteCount( world->sensors );+	total += (uint64_t)bodyArrayBytes + solverSetArrayBytes + jointArrayBytes + contactArrayBytes + islandArrayBytes ++			 islandLinkBytes + shapeArrayBytes + sensorArrayBytes;++	b3Log( "world arrays" );+	b3Log( "bodies: %d", bodyArrayBytes );+	b3Log( "solver sets: %d", solverSetArrayBytes );+	b3Log( "joints: %d", jointArrayBytes );+	b3Log( "contacts: %d", contactArrayBytes );+	b3Log( "islands: %d", islandArrayBytes );+	b3Log( "island links: %d", islandLinkBytes );+	b3Log( "shapes: %d", shapeArrayBytes );+	b3Log( "sensors: %d", sensorArrayBytes );++	// Sensors own overlap tracking arrays. The sensor array is dense.+	int sensorOverlapBytes = 0;+	for ( int i = 0; i < world->sensors.count; ++i )+	{+		b3Sensor* sensor = world->sensors.data + i;+		sensorOverlapBytes += b3Array_ByteCount( sensor->hits );+		sensorOverlapBytes += b3Array_ByteCount( sensor->overlaps1 );+		sensorOverlapBytes += b3Array_ByteCount( sensor->overlaps2 );+	}+	total += sensorOverlapBytes;++	b3Log( "owned arrays" );+	b3Log( "sensor overlaps: %d", sensorOverlapBytes );++	// Shared hull database. The map owns a combined bucket and metadata allocation+	// plus the small map struct. Each stored key is an owned clone sized by byteCount.+	b3HullMap* hullDatabase = world->hullDatabase;+	int hullCount = (int)b3HullMap_size( hullDatabase );+	int hullBucketCount = (int)b3HullMap_bucket_count( hullDatabase );+	uint64_t hullMapBytes = b3HullMapByteCount( hullDatabase );+	uint64_t hullDataBytes = 0;+	for ( b3HullMap_itr itr = b3HullMap_first( hullDatabase ); b3HullMap_is_end( itr ) == false; itr = b3HullMap_next( itr ) )+	{+		hullDataBytes += itr.data->key->byteCount;+	}+	total += hullMapBytes + hullDataBytes;++	b3Log( "hulls" );+	b3Log( "database: %d (%d, %d)", (int)hullMapBytes, hullCount, hullBucketCount );+	b3Log( "hull data: %d", (int)hullDataBytes );++	// broad-phase+	int staticTreeBytes = b3DynamicTree_GetByteCount( world->broadPhase.trees + b3_staticBody );+	int kinematicTreeBytes = b3DynamicTree_GetByteCount( world->broadPhase.trees + b3_kinematicBody );+	int dynamicTreeBytes = b3DynamicTree_GetByteCount( world->broadPhase.trees + b3_dynamicBody );+	int movedBytes = 0;+	for ( int i = 0; i < b3_bodyTypeCount; ++i )+	{+		movedBytes += b3GetBitSetBytes( &world->broadPhase.movedProxies[i] );+	}+	int moveArrayBytes = b3Array_ByteCount( world->broadPhase.moveArray );+	b3HashSet* pairSet = &world->broadPhase.pairSet;+	int pairSetBytes = b3GetHashSetBytes( pairSet );+	total += (uint64_t)staticTreeBytes + kinematicTreeBytes + dynamicTreeBytes + movedBytes + moveArrayBytes + pairSetBytes;++	b3Log( "broad-phase" );+	b3Log( "static tree: %d", staticTreeBytes );+	b3Log( "kinematic tree: %d", kinematicTreeBytes );+	b3Log( "dynamic tree: %d", dynamicTreeBytes );+	b3Log( "movedProxies: %d", movedBytes );+	b3Log( "moveArray: %d", moveArrayBytes );+	b3Log( "pairSet: %d (%d, %d)", pairSetBytes, pairSet->count, pairSet->capacity );++	// Manifold block allocators, one per manifold point count+	int manifoldArrayBytes = b3Array_ByteCount( world->manifoldAllocators );+	int manifoldBlockBytes = 0;+	for ( int i = 0; i < world->manifoldAllocators.count; ++i )+	{+		b3BlockAllocator* allocator = world->manifoldAllocators.data + i;+		manifoldBlockBytes += b3Array_ByteCount( allocator->blocks );+		manifoldBlockBytes += allocator->blocks.count * B3_BLOCK_SIZE * allocator->elementSize;+	}+	total += (uint64_t)manifoldArrayBytes + manifoldBlockBytes;++	b3Log( "manifold allocators" );+	b3Log( "allocator array: %d", manifoldArrayBytes );+	b3Log( "blocks: %d", manifoldBlockBytes );++	// 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;+	}++	int setBodySimBytes = bodySimCapacity * (int)sizeof( b3BodySim );+	int setBodyStateBytes = bodyStateCapacity * (int)sizeof( b3BodyState );+	int setJointSimBytes = jointSimCapacity * (int)sizeof( b3JointSim );+	int setContactSimBytes = contactIndexCapacity * (int)sizeof( int );+	int setIslandSimBytes = islandSimCapacity * (int)sizeof( b3IslandSim );+	total += (uint64_t)setBodySimBytes + setBodyStateBytes + setJointSimBytes + setContactSimBytes + setIslandSimBytes;++	b3Log( "solver sets" );+	b3Log( "body sim: %d", setBodySimBytes );+	b3Log( "body state: %d", setBodyStateBytes );+	b3Log( "joint sim: %d", setJointSimBytes );+	b3Log( "contact sim: %d", setContactSimBytes );+	b3Log( "island sim: %d", setIslandSimBytes );++	// constraint graph+	int bodyBitSetBytes = 0;+	int graphContactBytes = 0;+	int graphJointSimBytes = 0;+	for ( int i = 0; i < B3_GRAPH_COLOR_COUNT; ++i )+	{+		b3GraphColor* c = world->constraintGraph.colors + i;+		bodyBitSetBytes += b3GetBitSetBytes( &c->bodySet );+		graphContactBytes += b3Array_ByteCount( c->convexContacts ) + b3Array_ByteCount( c->contacts );+		graphJointSimBytes += b3Array_ByteCount( c->jointSims );+	}+	total += (uint64_t)bodyBitSetBytes + graphJointSimBytes + graphContactBytes;++	b3Log( "constraint graph" );+	b3Log( "body bit sets: %d", bodyBitSetBytes );+	b3Log( "joint sim: %d", graphJointSimBytes );+	b3Log( "contact sim: %d", graphContactBytes );++	// Per worker task storage and its bit sets+	int taskContextBytes = b3Array_ByteCount( world->taskContexts );+	for ( int i = 0; i < world->taskContexts.count; ++i )+	{+		b3TaskContext* taskContext = world->taskContexts.data + i;+		taskContextBytes += b3Array_ByteCount( taskContext->sensorHits );+		taskContextBytes += b3GetBitSetBytes( &taskContext->contactStateBitSet );+		taskContextBytes += b3GetBitSetBytes( &taskContext->jointStateBitSet );+		taskContextBytes += b3GetBitSetBytes( &taskContext->hitEventBitSet );+		taskContextBytes += b3GetBitSetBytes( &taskContext->enlargedSimBitSet );+		taskContextBytes += b3GetBitSetBytes( &taskContext->awakeIslandBitSet );+	}++	int sensorTaskContextBytes = b3Array_ByteCount( world->sensorTaskContexts );+	for ( int i = 0; i < world->sensorTaskContexts.count; ++i )+	{+		b3SensorTaskContext* taskContext = world->sensorTaskContexts.data + i;+		sensorTaskContextBytes += b3GetBitSetBytes( &taskContext->eventBits );+	}+	total += (uint64_t)taskContextBytes + sensorTaskContextBytes;++	b3Log( "task contexts" );+	b3Log( "worker: %d", taskContextBytes );+	b3Log( "sensor: %d", sensorTaskContextBytes );++	// Double buffered event arrays+	int eventBytes = 0;+	eventBytes += b3Array_ByteCount( world->bodyMoveEvents );+	eventBytes += b3Array_ByteCount( world->sensorBeginEvents );+	eventBytes += b3Array_ByteCount( world->contactBeginEvents );+	eventBytes += b3Array_ByteCount( world->sensorEndEvents[0] );+	eventBytes += b3Array_ByteCount( world->sensorEndEvents[1] );+	eventBytes += b3Array_ByteCount( world->contactEndEvents[0] );+	eventBytes += b3Array_ByteCount( world->contactEndEvents[1] );+	eventBytes += b3Array_ByteCount( world->contactHitEvents );+	eventBytes += b3Array_ByteCount( world->jointEvents );+	total += eventBytes;++	b3Log( "events: %d", eventBytes );++	// Debug draw bit sets+	int debugBytes = 0;+	debugBytes += b3GetBitSetBytes( &world->debugBodySet );+	debugBytes += b3GetBitSetBytes( &world->debugJointSet );+	debugBytes += b3GetBitSetBytes( &world->debugContactSet );+	debugBytes += b3GetBitSetBytes( &world->debugIslandSet );+	total += debugBytes;++	b3Log( "debug draw: %d", debugBytes );++	// stack allocator+	total += world->stack.capacity;+	b3Log( "stack allocator: %d", world->stack.capacity );++	b3Log( "total: %u KB", (uint32_t)( total / 1024 ) );+}++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 ) );++	b3RecQueryWriter recWriter = { 0 };+	if ( world->recording != NULL )+	{+		b3RecQueryBegin( &recWriter, context, filter.id, filter.name );+		recWriter.userFcn.overlapFcn = fcn;+		b3RecW_WORLDID( &recWriter.buf, worldId );+		b3RecW_AABB( &recWriter.buf, aabb );+		b3RecW_QUERYFILTER( &recWriter.buf, filter );+		recWriter.countOffset = b3RecReserveU32( &recWriter.buf );+		fcn = b3RecOverlapTrampoline;+		context = &recWriter;+	}++	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;+	}++	if ( world->recording != NULL )+	{+		b3RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount );+		b3RecW_TREESTATS( &recWriter.buf, treeStats );+		b3RecQueryCommit( world->recording, b3_recOpQueryOverlapAABB, &recWriter );+	}++	return treeStats;+}++typedef struct WorldOverlapContext+{+	b3World* world;+	b3OverlapResultFcn* fcn;+	b3QueryFilter filter;+	b3ShapeProxy proxy;+	b3Pos origin;+	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;+	}++	// Re-center on the query origin so the overlap test stays in float precision far from the origin+	b3Body* body = b3Array_Get( world->bodies, shape->bodyId );+	b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), worldContext->origin );++	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, b3Pos origin, const b3ShapeProxy* proxy, b3QueryFilter filter,+								  b3OverlapResultFcn* fcn, void* context )+{+	b3TreeStats treeStats = { 0 };++	b3World* world = b3GetUnlockedWorldFromId( worldId );+	if ( world == NULL )+	{+		return treeStats;+	}++	B3_ASSERT( b3IsValidPosition( origin ) );++	b3RecQueryWriter recWriter = { 0 };+	if ( world->recording != NULL )+	{+		b3RecQueryBegin( &recWriter, context, filter.id, filter.name );+		recWriter.userFcn.overlapFcn = fcn;+		b3RecW_WORLDID( &recWriter.buf, worldId );+		b3RecW_POSITION( &recWriter.buf, origin );+		b3RecW_SHAPEPROXY( &recWriter.buf, *proxy );+		b3RecW_QUERYFILTER( &recWriter.buf, filter );+		recWriter.countOffset = b3RecReserveU32( &recWriter.buf );+		fcn = b3RecOverlapTrampoline;+		context = &recWriter;+	}++	// Bound the proxy in origin relative space then lift to a conservative world float box+	b3AABB aabb = b3OffsetAABB( b3MakeAABB( proxy->points, proxy->count, proxy->radius ), origin );+	WorldOverlapContext worldContext = {+		world, fcn, filter, *proxy, origin, 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;+	}++	if ( world->recording != NULL )+	{+		b3RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount );+		b3RecW_TREESTATS( &recWriter.buf, treeStats );+		b3RecQueryCommit( world->recording, b3_recOpQueryOverlapShape, &recWriter );+	}++	return treeStats;+}++typedef struct WorldMoverContext+{+	b3World* world;+	b3PlaneResultFcn* fcn;+	b3QueryFilter filter;+	b3Capsule mover;+	b3Pos origin;+	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;+	}++	// Re-center on the query origin, the mover and the resulting planes are origin relative+	b3Body* body = b3Array_Get( world->bodies, shape->bodyId );+	b3WorldTransform bodyTransform = b3GetBodyTransformQuick( world, body );+	b3Transform transform = b3ToRelativeTransform( bodyTransform, worldContext->origin );++	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, b3Pos origin, const b3Capsule* mover, b3QueryFilter filter, b3PlaneResultFcn* fcn,+						   void* context )+{+	b3World* world = b3GetUnlockedWorldFromId( worldId );+	if ( world == NULL )+	{+		return;+	}++	B3_ASSERT( b3IsValidPosition( origin ) );++	b3RecQueryWriter recWriter = { 0 };+	if ( world->recording != NULL )+	{+		b3RecQueryBegin( &recWriter, context, filter.id, filter.name );+		recWriter.userFcn.planeFcn = fcn;+		b3RecW_WORLDID( &recWriter.buf, worldId );+		b3RecW_POSITION( &recWriter.buf, origin );+		b3RecW_CAPSULE( &recWriter.buf, *mover );+		b3RecW_QUERYFILTER( &recWriter.buf, filter );+		recWriter.countOffset = b3RecReserveU32( &recWriter.buf );+		fcn = b3RecPlaneTrampoline;+		context = &recWriter;+	}++	b3Vec3 r = { mover->radius, mover->radius, mover->radius };++	// Relative box lifted to world float with outward rounding, conservative for the tree+	b3AABB relBox;+	relBox.lowerBound = b3Sub( b3Min( mover->center1, mover->center2 ), r );+	relBox.upperBound = b3Add( b3Max( mover->center1, mover->center2 ), r );+	b3AABB aabb = b3OffsetAABB( relBox, origin );++	WorldMoverContext worldContext = {+		world, fcn, filter, *mover, origin, context,+	};++	for ( int i = 0; i < b3_bodyTypeCount; ++i )+	{+		b3DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, false, TreeCollideCallback, &worldContext );+	}++	if ( world->recording != NULL )+	{+		// CollideMover returns void: no treestats tail, just the per-shape plane batches.+		b3RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount );+		b3RecQueryCommit( world->recording, b3_recOpQueryCollideMover, &recWriter );+	}+}++typedef struct WorldRayCastContext+{+	b3World* world;+	b3CastResultFcn* fcn;+	b3QueryFilter filter;+	float fraction;+	b3Pos origin;+	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 );+	b3WorldTransform bodyTransform = b3GetBodyTransformQuick( world, body );+	b3Transform transform = b3ToRelativeTransform( bodyTransform, worldContext->origin );++	b3RayCastInput localInput = *input;+	localInput.origin = b3Vec3_zero;+	b3CastOutput output = b3RayCastShape( shape, transform, &localInput );++	if ( output.hit )+	{+		B3_ASSERT( output.fraction <= input->maxFraction );++		b3ShapeId id = { shapeId + 1, world->worldId, shape->generation };+		b3Pos point = b3OffsetPos( worldContext->origin, output.point );+		int materialIndex = b3ClampInt( output.materialIndex, 0, shape->materialCount - 1 );+		uint64_t userMaterialId = b3GetShapeMaterials( shape )[materialIndex].userMaterialId;++		int triangleIndex = output.triangleIndex;+		int childIndex = output.childIndex;+		float fraction = worldContext->fcn( id, 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, b3Pos origin, b3Vec3 translation, b3QueryFilter filter, b3CastResultFcn* fcn,+							 void* context )+{+	b3TreeStats treeStats = { 0 };++	b3World* world = b3GetUnlockedWorldFromId( worldId );+	if ( world == NULL )+	{+		return treeStats;+	}++	B3_ASSERT( b3IsValidPosition( origin ) );+	B3_ASSERT( b3IsValidVec3( translation ) );++	b3RecQueryWriter recWriter = { 0 };+	if ( world->recording != NULL )+	{+		b3RecQueryBegin( &recWriter, context, filter.id, filter.name );+		recWriter.userFcn.castFcn = fcn;+		b3RecW_WORLDID( &recWriter.buf, worldId );+		b3RecW_POSITION( &recWriter.buf, origin );+		b3RecW_VEC3( &recWriter.buf, translation );+		b3RecW_QUERYFILTER( &recWriter.buf, filter );+		recWriter.countOffset = b3RecReserveU32( &recWriter.buf );+		fcn = b3RecCastTrampoline;+		context = &recWriter;+	}++	// The tree traverses in float relative to the world origin. Each shape is then re-differenced at+	// full precision against the origin, so a hit stays accurate far from the origin.+	b3RayCastInput input = { b3ToVec3( origin ), translation, 1.0f };++	WorldRayCastContext worldContext = { world, fcn, filter, 1.0f, origin, 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 )+		{+			break;+		}++		input.maxFraction = worldContext.fraction;+	}++	if ( world->recording != NULL )+	{+		b3RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount );+		b3RecW_TREESTATS( &recWriter.buf, treeStats );+		b3RecQueryCommit( world->recording, b3_recOpQueryCastRay, &recWriter );+	}++	return treeStats;+}++// This callback finds the closest hit. This is the most common callback used in games.+static float b3RayCastClosestFcn( b3ShapeId shapeId, b3Pos 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, b3Pos origin, b3Vec3 translation, b3QueryFilter filter )+{+	b3RayResult result = { 0 };++	b3World* world = b3GetUnlockedWorldFromId( worldId );+	if ( world == NULL )+	{+		return result;+	}++	B3_ASSERT( b3IsValidPosition( origin ) );+	B3_ASSERT( b3IsValidVec3( translation ) );++	// The tree traverses in float relative to the world origin. Each shape is then re-differenced at+	// full precision against its body, so a hit stays accurate far from the origin.+	b3RayCastInput input = { b3ToVec3( origin ), translation, 1.0f };+	WorldRayCastContext worldContext = {+		.world = world,+		.fcn = b3RayCastClosestFcn,+		.filter = filter,+		.fraction = 1.0f,+		.origin = origin,+		.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 )+		{+			break;+		}++		input.maxFraction = worldContext.fraction;+	}++	// Closed query, no user callback: record the inputs and the single result for the replay compare.+	if ( world->recording != NULL )+	{+		b3RecQueryWriter recWriter = { 0 };+		b3RecQueryBegin( &recWriter, NULL, filter.id, filter.name );+		b3RecW_WORLDID( &recWriter.buf, worldId );+		b3RecW_POSITION( &recWriter.buf, origin );+		b3RecW_VEC3( &recWriter.buf, translation );+		b3RecW_QUERYFILTER( &recWriter.buf, filter );+		b3RecW_RAYRESULT( &recWriter.buf, result );+		b3RecQueryCommit( world->recording, b3_recOpQueryCastRayClosest, &recWriter );+	}++	return result;+}++typedef struct WorldShapeCastContext+{+	b3World* world;+	b3CastResultFcn* fcn;+	b3QueryFilter filter;+	float fraction;+	b3Pos origin;+	// origin relative input+	b3ShapeCastInput input;+	void* userContext;+} WorldShapeCastContext;++static float b3ShapeCastCallback( const b3BoxCastInput* input, int proxyId, uint64_t userData, void* context )+{+	B3_UNUSED( proxyId );++	int shapeId = (int)userData;+	WorldShapeCastContext* worldContext = (WorldShapeCastContext*)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;+	}++	// Rebuild from the origin relative input, taking only the advancing fraction from the tree.+	// The tree box is world float and would lose the cast far from the origin.+	b3ShapeCastInput localInput = worldContext->input;+	localInput.maxFraction = input->maxFraction;++	// Re-center on the query origin so the per-shape cast stays in float precision far from the origin+	b3Body* body = b3Array_Get( world->bodies, shape->bodyId );+	b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), worldContext->origin );++	b3CastOutput output = b3ShapeCastShape( shape, transform, &localInput );++	if ( output.hit )+	{+		b3ShapeId id = { shapeId + 1, world->worldId, shape->generation };+		int materialIndex = b3ClampInt( output.materialIndex, 0, shape->materialCount - 1 );+		uint64_t userMaterialId = b3GetShapeMaterials( shape )[materialIndex].userMaterialId;++		int triangleIndex = output.triangleIndex;+		int childIndex = output.childIndex;+		float fraction = worldContext->fcn( id, b3OffsetPos( worldContext->origin, 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, b3Pos origin, 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( b3IsValidPosition( origin ) );+	B3_ASSERT( b3IsValidVec3( translation ) );++	b3RecQueryWriter recWriter = { 0 };+	if ( world->recording != NULL )+	{+		b3RecQueryBegin( &recWriter, context, filter.id, filter.name );+		recWriter.userFcn.castFcn = fcn;+		b3RecW_WORLDID( &recWriter.buf, worldId );+		b3RecW_POSITION( &recWriter.buf, origin );+		b3RecW_SHAPEPROXY( &recWriter.buf, *proxy );+		b3RecW_VEC3( &recWriter.buf, translation );+		b3RecW_QUERYFILTER( &recWriter.buf, filter );+		recWriter.countOffset = b3RecReserveU32( &recWriter.buf );+		fcn = b3RecCastTrampoline;+		context = &recWriter;+	}++	WorldShapeCastContext worldContext = { 0 };+	worldContext.world = world;+	worldContext.fcn = fcn;+	worldContext.filter = filter;+	worldContext.fraction = 1.0f;+	worldContext.origin = origin;+	worldContext.input.proxy = *proxy;+	worldContext.input.translation = translation;+	worldContext.input.maxFraction = 1.0f;+	worldContext.input.canEncroach = false;+	worldContext.userContext = context;++	// Bound the proxy in origin relative space then lift to a conservative world float box. The tree+	// node boxes use the same directed rounding, so the swept box never clips a shape far from the+	// origin. Per shape casts re-difference at full precision against the carried origin.+	b3AABB localBox = b3MakeAABB( proxy->points, proxy->count, proxy->radius );+	b3BoxCastInput treeInput = { b3OffsetAABB( localBox, origin ), translation, 1.0f };++	for ( int i = 0; i < b3_bodyTypeCount; ++i )+	{+		b3TreeStats treeResult = b3DynamicTree_BoxCast( world->broadPhase.trees + i, &treeInput, filter.maskBits, false,+														b3ShapeCastCallback, &worldContext );+		treeStats.nodeVisits += treeResult.nodeVisits;+		treeStats.leafVisits += treeResult.leafVisits;++		if ( worldContext.fraction == 0.0f )+		{+			break;+		}++		treeInput.maxFraction = worldContext.fraction;+	}++	if ( world->recording != NULL )+	{+		b3RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount );+		b3RecW_TREESTATS( &recWriter.buf, treeStats );+		b3RecQueryCommit( world->recording, b3_recOpQueryCastShape, &recWriter );+	}++	return treeStats;+}++typedef struct WorldMoverCastContext+{+	b3World* world;+	b3MoverFilterFcn* fcn;+	b3QueryFilter filter;+	float fraction;+	b3Pos origin;+	// origin relative input+	b3ShapeCastInput input;+	void* userContext;+} WorldMoverCastContext;++static float MoverCastCallback( const b3BoxCastInput* 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;+		}+	}++	// Rebuild from the origin relative input, taking only the advancing fraction from the tree+	b3ShapeCastInput localInput = worldContext->input;+	localInput.maxFraction = input->maxFraction;++	// Re-center on the query origin so the per-shape cast stays in float precision far from the origin+	b3Body* body = b3Array_Get( world->bodies, shape->bodyId );+	b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), worldContext->origin );++	b3CastOutput output = b3ShapeCastShape( shape, transform, &localInput );+	if ( output.fraction == 0.0f )+	{+		// Ignore overlapping shapes+		return worldContext->fraction;+	}++	worldContext->fraction = output.fraction;+	return output.fraction;+}++float b3World_CastMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mover, b3Vec3 translation, b3QueryFilter filter,+						 b3MoverFilterFcn* fcn, void* context )+{+	B3_ASSERT( b3IsValidPosition( origin ) );+	B3_ASSERT( b3IsValidVec3( translation ) );++	b3World* world = b3GetUnlockedWorldFromId( worldId );+	if ( world == NULL )+	{+		return 1.0f;+	}++	// The mover filter is a per-shape bool(shapeId, ctx) decision, the same shape as an overlap+	// callback, so the overlap trampoline captures it. Installing it even when the user passed no+	// filter records an accept-all stream that replays identically.+	b3RecQueryWriter recWriter = { 0 };+	if ( world->recording != NULL )+	{+		b3RecQueryBegin( &recWriter, context, filter.id, filter.name );+		recWriter.userFcn.moverFilterFcn = fcn;+		b3RecW_WORLDID( &recWriter.buf, worldId );+		b3RecW_POSITION( &recWriter.buf, origin );+		b3RecW_CAPSULE( &recWriter.buf, *mover );+		b3RecW_VEC3( &recWriter.buf, translation );+		b3RecW_QUERYFILTER( &recWriter.buf, filter );+		recWriter.countOffset = b3RecReserveU32( &recWriter.buf );+		fcn = b3RecOverlapTrampoline;+		context = &recWriter;+	}++	WorldMoverCastContext worldContext = {+		.world = world,+		.fcn = fcn,+		.filter = filter,+		.fraction = 1.0f,+		.origin = origin,+		.userContext = context,+	};+	worldContext.input.proxy = (b3ShapeProxy){ &mover->center1, 2, mover->radius };+	worldContext.input.translation = translation;+	worldContext.input.maxFraction = 1.0f;+	worldContext.input.canEncroach = mover->radius > 0.0f;++	// Bound the capsule in origin relative space then lift to a conservative world float box+	b3Vec3 centers[2] = { mover->center1, mover->center2 };+	b3BoxCastInput treeInput = { b3OffsetAABB( b3MakeAABB( centers, 2, mover->radius ), origin ), translation, 1.0f };++	for ( int i = 0; i < b3_bodyTypeCount; ++i )+	{+		b3DynamicTree_BoxCast( world->broadPhase.trees + i, &treeInput, filter.maskBits, false, MoverCastCallback,+							   &worldContext );++		if ( worldContext.fraction == 0.0f )+		{+			break;+		}++		treeInput.maxFraction = worldContext.fraction;+	}++	if ( world->recording != NULL )+	{+		// The mover filter type aliases the overlap trampoline, so the user fcn lands in the same+		// union slot. Backpatch the accept count, then record the returned fraction as the tail.+		b3RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount );+		b3RecW_F32( &recWriter.buf, worldContext.fraction );+		b3RecQueryCommit( world->recording, b3_recOpQueryCastMover, &recWriter );+	}++	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 );++	B3_REC( world, WorldSetGravity, worldId, gravity );++	world->gravity = gravity;+}++b3Vec3 b3World_GetGravity( b3WorldId worldId )+{+	b3World* world = b3GetWorldFromId( worldId );+	return world->gravity;+}++typedef struct ExplosionContext+{+	b3World* world;+	b3Pos 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 );++	b3WorldTransform xf = b3GetBodyTransformQuick( world, body );++	// Re-center the explosion into the shape local frame so distance and direction stay precise+	// far from the origin. Everything below runs in that near-origin frame.+	b3Vec3 localPosition = b3InvTransformWorldPoint( xf, explosionContext->position );++	b3DistanceInput input;+	input.proxyA = b3MakeShapeProxy( shape );+	input.proxyB = (b3ShapeProxy){ &localPosition, 1, 0.0f };+	input.transform = 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;+	}++	// Witness point is already in the body local query frame+	b3Vec3 closestPoint = output.pointA;+	if ( output.distance == 0.0f )+	{+		closestPoint = b3GetShapeCentroid( shape );+	}++	b3Vec3 direction = b3Sub( closestPoint, localPosition );+	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, b3RotateVector( xf.q, 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 );++	// Lever arm from the center of mass to the closest point, rotated to world+	b3Vec3 r = b3RotateVector( xf.q, b3Sub( closestPoint, bodySim->localCenter ) );+	state->angularVelocity = b3Add( state->angularVelocity, b3MulMV( bodySim->invInertiaWorld, b3Cross( r, impulse ) ) );++	return true;+}++void b3World_Explode( b3WorldId worldId, const b3ExplosionDef* explosionDef )+{+	uint64_t maskBits = explosionDef->maskBits;+	b3Pos position = explosionDef->position;+	float radius = explosionDef->radius;+	float falloff = explosionDef->falloff;+	float impulsePerArea = explosionDef->impulsePerArea;++	B3_ASSERT( b3IsValidPosition( 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;+	}++	B3_REC( world, WorldExplode, worldId, *explosionDef );++	// Locked due to waking+	world->locked = true;++	struct ExplosionContext explosionContext = { world, position, radius, falloff, impulsePerArea };++	// The broad-phase tree is float, so translate a local query box out to world with outward rounding+	float extent = radius + falloff;+	b3AABB localBox = { { -extent, -extent, -extent }, { extent, extent, extent } };+	b3AABB aabb = b3OffsetAABB( localBox, position );++	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;+	}++	B3_REC( world, WorldRebuildStaticTree, worldId );++	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;+	}++	B3_REC( world, WorldEnableSpeculative, worldId, flag );++	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 );++					uint32_t syncedFlags = body->flags & ~b3_bodyTransientFlags;+					B3_ASSERT( ( bodySim->flags & syncedFlags ) == syncedFlags );++					b3BodyState* bodyState = b3GetBodyState( world, body );+					if ( bodyState != NULL )+					{+						B3_ASSERT( ( bodyState->flags & syncedFlags ) == syncedFlags );+					}++					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
+ upstream/src/physics_world.h view
@@ -0,0 +1,350 @@+// 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 "name_cache.h"++#include "box3d/types.h"++#define B3_DEBUG_POINT_CAPACITY 64+#define B3_DEBUG_LINE_CAPACITY 64++typedef struct b3Body b3Body;+typedef struct b3Recording b3Recording;+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+{+	b3Pos p;+	int label;+	float value;+	b3HexColor color;+} b3DebugPoint;++typedef struct b3DebugLine+{+	b3Pos p1, p2;+	int label;+	b3HexColor color;+} b3DebugLine;++// Per thread task storage+typedef struct b3TaskContext+{+	b3Arena arena;++	// Collect per thread sensor continuous hit events.+	b3Array( 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;+	int distanceIterations;+	int pushBackIterations;+	int rootIterations;++	// Number of contacts recycled this step (collide pass).+	int recycledContactCount;++	b3DebugPoint points[B3_DEBUG_POINT_CAPACITY];+	int pointCount;++	b3DebugLine lines[B3_DEBUG_LINE_CAPACITY];+	int lineCount;++	int manifoldCounts[B3_CONTACT_MANIFOLD_COUNT_BUCKETS];+	// 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.+	b3Array( 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.+	b3Array( 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.+	b3Array( 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.+	b3Array( 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.+	b3Array( 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.+	b3Array( b3Island ) islands;++	b3IdPool shapeIdPool;++	// These are sparse arrays that point into the pools above+	b3Array( b3Shape ) shapes;++	// Reference counted store of shared hull data keyed by content. Shapes hold a+	// pointer to the hull stored in the db. Type erased to avoid leaking the verstable map+	// type into this header.+	void* hullDatabase;++	// Name cache for shape and body names. This works with recording.+	b3NameCache names;++	// This is a dense array of sensor data.+	b3Array( b3Sensor ) sensors;++	// Per thread storage+	b3Array( b3TaskContext ) taskContexts;+	b3Array( b3SensorTaskContext ) sensorTaskContexts;++	b3Array( b3BodyMoveEvent ) bodyMoveEvents;+	b3Array( b3SensorBeginTouchEvent ) sensorBeginEvents;+	b3Array( b3ContactBeginTouchEvent ) contactBeginEvents;++	// End events are double buffered so that the user doesn't need to flush events+	b3Array( b3SensorEndTouchEvent ) sensorEndEvents[2];+	b3Array( b3ContactEndTouchEvent ) contactEndEvents[2];+	int endEventArrayIndex;++	b3Array( b3ContactHitEvent ) contactHitEvents;+	b3Array( 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;++	// Non-NULL while a recording session is active. Set by b3World_StartRecording,+	// cleared by b3World_StopRecording. Hooks in mutators check this before writing.+	struct b3Recording* recording;++	// 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;++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 );++// Register a hull in the world database, returning the owned shared copy. Identical hulls+// share one copy with a reference count. The input may be freed after this call.+const b3HullData* b3AddHullToDatabase( b3World* world, const b3HullData* src );++// Like b3AddHullToDatabase but takes ownership of a heap hull: inserted directly on a miss,+// freed on a hit. Avoids cloning data the caller already allocated.+const b3HullData* b3AddOwnedHullToDatabase( b3World* world, b3HullData* owned );++// Release a reference to a shared hull. The owned copy is freed when the count reaches zero.+void b3RemoveHullFromDatabase( b3World* world, const b3HullData* data );++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 );+}+
+ upstream/src/platform.h view
@@ -0,0 +1,104 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "core.h"++#include <stdbool.h>+#include <stdint.h>++// Software prefetch hint. T0 brings the line into all cache levels.+// On x86 MSVC exposes _mm_prefetch, ARM MSVC uses __prefetch instead.+// clang/gcc provide __builtin_prefetch on every target.+#if defined( B3_COMPILER_MSVC )+#include <intrin.h>+#if defined( B3_CPU_X86_X64 )+#define b3Prefetch( addr ) _mm_prefetch( (const char*)( addr ), _MM_HINT_T0 )+#else+#define b3Prefetch( addr ) __prefetch( (const void*)( addr ) )+#endif+#elif defined( B3_COMPILER_CLANG ) || defined( B3_COMPILER_GCC )+#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 ) && !defined( __clang__ )+	int value = __iso_volatile_load32( (volatile __int32*)&a->value );+#if defined( _M_ARM ) || defined( _M_ARM64 ) || defined( _M_ARM64EC )+	__dmb( 0xB );+#else+	_ReadWriteBarrier();+#endif+	return value;+#elif defined( __GNUC__ ) || defined( __clang__ )+	return __atomic_load_n( &a->value, __ATOMIC_SEQ_CST );+#else+#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 ) && !defined( __clang__ )+	uint32_t value = (uint32_t)__iso_volatile_load32( (volatile __int32*)&a->value );+#if defined( _M_ARM ) || defined( _M_ARM64 ) || defined( _M_ARM64EC )+	__dmb( 0xB );+#else+	_ReadWriteBarrier();+#endif+	return value;+#elif defined( __GNUC__ ) || defined( __clang__ )+	return __atomic_load_n( &a->value, __ATOMIC_SEQ_CST );+#else+#error "Unsupported platform"+#endif+}
+ upstream/src/prismatic_joint.c view
@@ -0,0 +1,725 @@+// 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"+#include "recording.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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, PrismaticJointEnableLimit, jointId, 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 ) );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, PrismaticJointSetLimits, jointId, lower, 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 );+	b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );+	b3WorldTransform 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( b3SubPos( transformB.p, transformA.p ), b3Sub( anchorB, anchorA ) );+	float translation = b3Dot( d, jointAxis );+	return translation;+}++void b3PrismaticJoint_EnableSpring( b3JointId jointId, bool enableSpring )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, PrismaticJointEnableSpring, jointId, 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 ) );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, PrismaticJointSetTargetTranslation, jointId, 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 );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, PrismaticJointSetSpringHertz, jointId, hertz );+	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 );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, PrismaticJointSetSpringDampingRatio, jointId, dampingRatio );+	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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, PrismaticJointEnableMotor, jointId, 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 ) );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, PrismaticJointSetMotorSpeed, jointId, 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 );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, PrismaticJointSetMaxMotorForce, jointId, maxForce );+	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;+}++float b3PrismaticJoint_GetMotorForce( b3JointId jointId )+{+	b3World* world = b3GetWorld( jointId.world0 );+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );+	return world->inv_h * base->prismaticJoint.motorImpulse;+}++float b3PrismaticJoint_GetSpeed( b3JointId jointId )+{+	b3World* world = b3GetWorld( jointId.world0 );+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );++	b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA );+	b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB );+	b3BodySim* bodySimA = b3GetBodySim( world, bodyA );+	b3BodySim* bodySimB = b3GetBodySim( world, bodyB );+	b3BodyState* stateA = b3GetBodyState( world, bodyA );+	b3BodyState* stateB = b3GetBodyState( world, bodyB );++	b3Quat qA = bodySimA->transform.q;+	b3Quat qB = bodySimB->transform.q;++	b3Vec3 axisA = b3RotateVector( qA, b3RotateVector( base->localFrameA.q, b3Vec3_axisX ) );+	b3Vec3 rA = b3RotateVector( qA, b3Sub( base->localFrameA.p, bodySimA->localCenter ) );+	b3Vec3 rB = b3RotateVector( qB, b3Sub( base->localFrameB.p, bodySimB->localCenter ) );++	// Difference the centers in double so the speed stays exact far from the origin.+	b3Vec3 d = b3Add( b3SubPos( bodySimB->center, bodySimA->center ), b3Sub( rB, rA ) );++	b3Vec3 vA = stateA ? stateA->linearVelocity : b3Vec3_zero;+	b3Vec3 vB = stateB ? stateB->linearVelocity : b3Vec3_zero;+	b3Vec3 wA = stateA ? stateA->angularVelocity : b3Vec3_zero;+	b3Vec3 wB = stateB ? stateB->angularVelocity : b3Vec3_zero;++	b3Vec3 vRel = b3Sub( b3Add( vB, b3Cross( wB, rB ) ), b3Add( vA, b3Cross( wA, rA ) ) );++	// The axis moves with body A, so account for its rotation.+	float speed = b3Dot( d, b3Cross( wA, axisA ) ) + b3Dot( axisA, vRel );+	return speed;+}++b3Vec3 b3GetPrismaticJointForce( b3World* world, b3JointSim* base )+{+	b3WorldTransform 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 )+{+	b3WorldTransform 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 = b3SubPos( 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, b3WorldTransform transformA, b3WorldTransform transformB, float scale )+{+	b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA );+	b3WorldTransform frameB = b3MulWorldTransforms( 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, b3OffsetPos( frameA.p, b3MulSV( s, perpY ) ), b3_colorGreen, draw->context );+	draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( s, perpZ ) ), b3_colorBlue, draw->context );++	b3PrismaticJoint* joint = &base->prismaticJoint;+	if ( joint->enableLimit )+	{+		b3Pos p1 = b3OffsetPos( frameA.p, b3MulSV( joint->lowerTranslation, axis ) );+		b3Pos p2 = b3OffsetPos( frameA.p, b3MulSV( 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+	{+		b3Pos p1 = b3OffsetPos( frameA.p, b3MulSV( -0.5f * scale, axis ) );+		b3Pos p2 = b3OffsetPos( frameA.p, b3MulSV( 0.5f * scale, axis ) );+		draw->DrawSegmentFcn( p1, p2, b3_colorOrange, draw->context );+	}++	draw->DrawPointFcn( frameB.p, 8.0f, b3_colorViolet, draw->context );+}
+ upstream/src/qsort.h view
@@ -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: */
+ upstream/src/rapidhash.h view
@@ -0,0 +1,568 @@+/*+ * rapidhash V3 - Very fast, high quality, platform-independent hashing algorithm.+ *+ * Based on 'wyhash', by Wang Yi <godspeed_china@yeah.net>+ * + * Copyright (C) 2025 Nicolas De Carli+ *+ * 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.+ *+ * You can contact the author at:+ *   - rapidhash source repository: https://github.com/Nicoshev/rapidhash+ */++ #pragma once+ +/*+ *  Includes.+ */+ #include <stdint.h>+ #include <string.h>+ #if defined(_MSC_VER)+ # include <intrin.h>+ # if defined(_M_X64) && !defined(_M_ARM64EC)+ #   pragma intrinsic(_umul128)+ # endif+ #endif+ + /*+  *  C/C++ macros.+  */+ + #ifdef _MSC_VER+ # define RAPIDHASH_ALWAYS_INLINE __forceinline+ #elif defined(__GNUC__)+ # define RAPIDHASH_ALWAYS_INLINE inline __attribute__((__always_inline__))+ #else+ # define RAPIDHASH_ALWAYS_INLINE inline+ #endif+ + #ifdef __cplusplus+ # define RAPIDHASH_NOEXCEPT noexcept+ # define RAPIDHASH_CONSTEXPR constexpr+ # ifndef RAPIDHASH_INLINE+ #   define RAPIDHASH_INLINE RAPIDHASH_ALWAYS_INLINE+ # endif+ # if __cplusplus >= 201402L && !defined(_MSC_VER)+ #   define RAPIDHASH_INLINE_CONSTEXPR RAPIDHASH_ALWAYS_INLINE constexpr+ # else+ #   define RAPIDHASH_INLINE_CONSTEXPR RAPIDHASH_ALWAYS_INLINE+ # endif+ #else+ # define RAPIDHASH_NOEXCEPT+ # define RAPIDHASH_CONSTEXPR static const+ # ifndef RAPIDHASH_INLINE+ #   define RAPIDHASH_INLINE static RAPIDHASH_ALWAYS_INLINE+ # endif+ # define RAPIDHASH_INLINE_CONSTEXPR RAPIDHASH_INLINE+ #endif++ /*+  *  Unrolled macro.+  *  Improves large input speed, but increases code size and worsens small input speed.+  *+  *  RAPIDHASH_COMPACT: Normal behavior.+  *  RAPIDHASH_UNROLLED: +  *+  */+  #ifndef RAPIDHASH_UNROLLED+  # define RAPIDHASH_COMPACT+  #elif defined(RAPIDHASH_COMPACT)+  # error "cannot define RAPIDHASH_COMPACT and RAPIDHASH_UNROLLED simultaneously."+  #endif+ + /*+  *  Protection macro, alters behaviour of rapid_mum multiplication function.+  *+  *  RAPIDHASH_FAST: Normal behavior, max speed.+  *  RAPIDHASH_PROTECTED: Extra protection against entropy loss.+  */+ #ifndef RAPIDHASH_PROTECTED+ # define RAPIDHASH_FAST+ #elif defined(RAPIDHASH_FAST)+ # error "cannot define RAPIDHASH_PROTECTED and RAPIDHASH_FAST simultaneously."+ #endif+ + /*+  *  Likely and unlikely macros.+  */+ #if defined(__GNUC__) || defined(__INTEL_COMPILER) || defined(__clang__)+ # define _likely_(x)  __builtin_expect(x,1)+ # define _unlikely_(x)  __builtin_expect(x,0)+ #else+ # define _likely_(x) (x)+ # define _unlikely_(x) (x)+ #endif+ + /*+  *  Endianness macros.+  */+ #ifndef RAPIDHASH_LITTLE_ENDIAN+ # if defined(_WIN32) || defined(__LITTLE_ENDIAN__) || (defined(__BYTE_ORDER__) && __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__)+ #   define RAPIDHASH_LITTLE_ENDIAN+ # elif defined(__BIG_ENDIAN__) || (defined(__BYTE_ORDER__) && __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__)+ #   define RAPIDHASH_BIG_ENDIAN+ # else+ #   warning "could not determine endianness! Falling back to little endian."+ #   define RAPIDHASH_LITTLE_ENDIAN+ # endif+ #endif+ + /*+  *  Default secret parameters.+  */+   RAPIDHASH_CONSTEXPR uint64_t rapid_secret[8] = {+     0x2d358dccaa6c78a5ull,+     0x8bb84b93962eacc9ull,+     0x4b33a62ed433d4a3ull,+     0x4d5a2da51de1aa47ull,+     0xa0761d6478bd642full,+     0xe7037ed1a0b428dbull,+     0x90ed1765281c388cull,+     0xaaaaaaaaaaaaaaaaull};+ + /*+  *  64*64 -> 128bit multiply function.+  *+  *  @param A  Address of 64-bit number.+  *  @param B  Address of 64-bit number.+  *+  *  Calculates 128-bit C = *A * *B.+  *+  *  When RAPIDHASH_FAST is defined:+  *  Overwrites A contents with C's low 64 bits.+  *  Overwrites B contents with C's high 64 bits.+  *+  *  When RAPIDHASH_PROTECTED is defined:+  *  Xors and overwrites A contents with C's low 64 bits.+  *  Xors and overwrites B contents with C's high 64 bits.+  */+ RAPIDHASH_INLINE_CONSTEXPR void rapid_mum(uint64_t *A, uint64_t *B) RAPIDHASH_NOEXCEPT {+ #if defined(__SIZEOF_INT128__)+   __uint128_t r=*A; r*=*B;+   #ifdef RAPIDHASH_PROTECTED+   *A^=(uint64_t)r; *B^=(uint64_t)(r>>64);+   #else+   *A=(uint64_t)r; *B=(uint64_t)(r>>64);+   #endif+ #elif defined(_MSC_VER) && (defined(_WIN64) || defined(_M_HYBRID_CHPE_ARM64))+   #if defined(_M_X64)+     #ifdef RAPIDHASH_PROTECTED+     uint64_t a, b;+     a=_umul128(*A,*B,&b);+     *A^=a;  *B^=b;+     #else+     *A=_umul128(*A,*B,B);+     #endif+   #else+     #ifdef RAPIDHASH_PROTECTED+     uint64_t a, b;+     b = __umulh(*A, *B);+     a = *A * *B;+     *A^=a;  *B^=b;+     #else+     uint64_t c = __umulh(*A, *B);+     *A = *A * *B;+     *B = c;+     #endif+   #endif+ #else+   uint64_t ha=*A>>32, hb=*B>>32, la=(uint32_t)*A, lb=(uint32_t)*B;+   uint64_t rh=ha*hb, rm0=ha*lb, rm1=hb*la, rl=la*lb, t=rl+(rm0<<32), c=t<rl;+   uint64_t lo=t+(rm1<<32); +   c+=lo<t; +   uint64_t hi=rh+(rm0>>32)+(rm1>>32)+c;+   #ifdef RAPIDHASH_PROTECTED+   *A^=lo;  *B^=hi;+   #else+   *A=lo;  *B=hi;+   #endif+ #endif+ }+ + /*+  *  Multiply and xor mix function.+  *+  *  @param A  64-bit number.+  *  @param B  64-bit number.+  *+  *  Calculates 128-bit C = A * B.+  *  Returns 64-bit xor between high and low 64 bits of C.+  */+  RAPIDHASH_INLINE_CONSTEXPR uint64_t rapid_mix(uint64_t A, uint64_t B) RAPIDHASH_NOEXCEPT { rapid_mum(&A,&B); return A^B; }+ + /*+  *  Read functions.+  */+ #ifdef RAPIDHASH_LITTLE_ENDIAN+ RAPIDHASH_INLINE uint64_t rapid_read64(const uint8_t *p) RAPIDHASH_NOEXCEPT { uint64_t v; memcpy(&v, p, sizeof(uint64_t)); return v;}+ RAPIDHASH_INLINE uint64_t rapid_read32(const uint8_t *p) RAPIDHASH_NOEXCEPT { uint32_t v; memcpy(&v, p, sizeof(uint32_t)); return v;}+ #elif defined(__GNUC__) || defined(__INTEL_COMPILER) || defined(__clang__)+ RAPIDHASH_INLINE uint64_t rapid_read64(const uint8_t *p) RAPIDHASH_NOEXCEPT { uint64_t v; memcpy(&v, p, sizeof(uint64_t)); return __builtin_bswap64(v);}+ RAPIDHASH_INLINE uint64_t rapid_read32(const uint8_t *p) RAPIDHASH_NOEXCEPT { uint32_t v; memcpy(&v, p, sizeof(uint32_t)); return __builtin_bswap32(v);}+ #elif defined(_MSC_VER)+ RAPIDHASH_INLINE uint64_t rapid_read64(const uint8_t *p) RAPIDHASH_NOEXCEPT { uint64_t v; memcpy(&v, p, sizeof(uint64_t)); return _byteswap_uint64(v);}+ RAPIDHASH_INLINE uint64_t rapid_read32(const uint8_t *p) RAPIDHASH_NOEXCEPT { uint32_t v; memcpy(&v, p, sizeof(uint32_t)); return _byteswap_ulong(v);}+ #else+ RAPIDHASH_INLINE uint64_t rapid_read64(const uint8_t *p) RAPIDHASH_NOEXCEPT {+   uint64_t v; memcpy(&v, p, 8);+   return (((v >> 56) & 0xff)| ((v >> 40) & 0xff00)| ((v >> 24) & 0xff0000)| ((v >>  8) & 0xff000000)| ((v <<  8) & 0xff00000000)| ((v << 24) & 0xff0000000000)| ((v << 40) & 0xff000000000000)| ((v << 56) & 0xff00000000000000));+ }+ RAPIDHASH_INLINE uint64_t rapid_read32(const uint8_t *p) RAPIDHASH_NOEXCEPT {+   uint32_t v; memcpy(&v, p, 4);+   return (((v >> 24) & 0xff)| ((v >>  8) & 0xff00)| ((v <<  8) & 0xff0000)| ((v << 24) & 0xff000000));+ }+ #endif+ + /*+  *  rapidhash main function.+  *+  *  @param key     Buffer to be hashed.+  *  @param len     @key length, in bytes.+  *  @param seed    64-bit seed used to alter the hash result predictably.+  *  @param secret  Triplet of 64-bit secrets used to alter hash result predictably.+  *+  *  Returns a 64-bit hash.+  */+RAPIDHASH_INLINE_CONSTEXPR uint64_t rapidhash_internal(const void *key, size_t len, uint64_t seed, const uint64_t* secret) RAPIDHASH_NOEXCEPT {+  const uint8_t *p=(const uint8_t *)key;+  seed ^= rapid_mix(seed ^ secret[2], secret[1]);+  uint64_t a=0, b=0;+  size_t i = len;+  if (_likely_(len <= 16)) {+    if (len >= 4) {+      seed ^= len;+      if (len >= 8) {+        const uint8_t* plast = p + len - 8;+        a = rapid_read64(p);+        b = rapid_read64(plast);+      } else {+        const uint8_t* plast = p + len - 4;+        a = rapid_read32(p);+        b = rapid_read32(plast);+      }+    } else if (len > 0) {+      a = (((uint64_t)p[0])<<45)|p[len-1];+      b = p[len>>1];+    } else+      a = b = 0;+  } else {+    if (len > 112) {+      uint64_t see1 = seed, see2 = seed;+      uint64_t see3 = seed, see4 = seed;+      uint64_t see5 = seed, see6 = seed;+#ifdef RAPIDHASH_COMPACT+      do {+        seed = rapid_mix(rapid_read64(p) ^ secret[0], rapid_read64(p + 8) ^ seed);+        see1 = rapid_mix(rapid_read64(p + 16) ^ secret[1], rapid_read64(p + 24) ^ see1);+        see2 = rapid_mix(rapid_read64(p + 32) ^ secret[2], rapid_read64(p + 40) ^ see2);+        see3 = rapid_mix(rapid_read64(p + 48) ^ secret[3], rapid_read64(p + 56) ^ see3);+        see4 = rapid_mix(rapid_read64(p + 64) ^ secret[4], rapid_read64(p + 72) ^ see4);+        see5 = rapid_mix(rapid_read64(p + 80) ^ secret[5], rapid_read64(p + 88) ^ see5);+        see6 = rapid_mix(rapid_read64(p + 96) ^ secret[6], rapid_read64(p + 104) ^ see6);+        p += 112;+        i -= 112;+      } while(i > 112);+#else+      while (i > 224) {+        seed = rapid_mix(rapid_read64(p) ^ secret[0], rapid_read64(p + 8) ^ seed);+        see1 = rapid_mix(rapid_read64(p + 16) ^ secret[1], rapid_read64(p + 24) ^ see1);+        see2 = rapid_mix(rapid_read64(p + 32) ^ secret[2], rapid_read64(p + 40) ^ see2);+        see3 = rapid_mix(rapid_read64(p + 48) ^ secret[3], rapid_read64(p + 56) ^ see3);+        see4 = rapid_mix(rapid_read64(p + 64) ^ secret[4], rapid_read64(p + 72) ^ see4);+        see5 = rapid_mix(rapid_read64(p + 80) ^ secret[5], rapid_read64(p + 88) ^ see5);+        see6 = rapid_mix(rapid_read64(p + 96) ^ secret[6], rapid_read64(p + 104) ^ see6);+        seed = rapid_mix(rapid_read64(p + 112) ^ secret[0], rapid_read64(p + 120) ^ seed);+        see1 = rapid_mix(rapid_read64(p + 128) ^ secret[1], rapid_read64(p + 136) ^ see1);+        see2 = rapid_mix(rapid_read64(p + 144) ^ secret[2], rapid_read64(p + 152) ^ see2);+        see3 = rapid_mix(rapid_read64(p + 160) ^ secret[3], rapid_read64(p + 168) ^ see3);+        see4 = rapid_mix(rapid_read64(p + 176) ^ secret[4], rapid_read64(p + 184) ^ see4);+        see5 = rapid_mix(rapid_read64(p + 192) ^ secret[5], rapid_read64(p + 200) ^ see5);+        see6 = rapid_mix(rapid_read64(p + 208) ^ secret[6], rapid_read64(p + 216) ^ see6);+        p += 224;+        i -= 224;+      }+      if (i > 112) {+        seed = rapid_mix(rapid_read64(p) ^ secret[0], rapid_read64(p + 8) ^ seed);+        see1 = rapid_mix(rapid_read64(p + 16) ^ secret[1], rapid_read64(p + 24) ^ see1);+        see2 = rapid_mix(rapid_read64(p + 32) ^ secret[2], rapid_read64(p + 40) ^ see2);+        see3 = rapid_mix(rapid_read64(p + 48) ^ secret[3], rapid_read64(p + 56) ^ see3);+        see4 = rapid_mix(rapid_read64(p + 64) ^ secret[4], rapid_read64(p + 72) ^ see4);+        see5 = rapid_mix(rapid_read64(p + 80) ^ secret[5], rapid_read64(p + 88) ^ see5);+        see6 = rapid_mix(rapid_read64(p + 96) ^ secret[6], rapid_read64(p + 104) ^ see6);+        p += 112;+        i -= 112;+      }+#endif+      seed ^= see1;+      see2 ^= see3;+      see4 ^= see5;+      seed ^= see6;+      see2 ^= see4;+      seed ^= see2;+    }+    if (i > 16) {+      seed = rapid_mix(rapid_read64(p) ^ secret[2], rapid_read64(p + 8) ^ seed);+      if (i > 32) {+          seed = rapid_mix(rapid_read64(p + 16) ^ secret[2], rapid_read64(p + 24) ^ seed);+          if (i > 48) {+              seed = rapid_mix(rapid_read64(p + 32) ^ secret[1], rapid_read64(p + 40) ^ seed);+              if (i > 64) {+                  seed = rapid_mix(rapid_read64(p + 48) ^ secret[1], rapid_read64(p + 56) ^ seed);+                  if (i > 80) {+                      seed = rapid_mix(rapid_read64(p + 64) ^ secret[2], rapid_read64(p + 72) ^ seed);+                      if (i > 96) {+                          seed = rapid_mix(rapid_read64(p + 80) ^ secret[1], rapid_read64(p + 88) ^ seed);+                      }+                  }+              }+          }+      }+    }+    a=rapid_read64(p+i-16) ^ i;  b=rapid_read64(p+i-8);+  }+  a ^= secret[1];+  b ^= seed;+  rapid_mum(&a, &b);+  return rapid_mix(a ^ secret[7], b ^ secret[1] ^ i);+}++ /*+  *  rapidhashMicro main function.+  *+  *  @param key     Buffer to be hashed.+  *  @param len     @key length, in bytes.+  *  @param seed    64-bit seed used to alter the hash result predictably.+  *  @param secret  Triplet of 64-bit secrets used to alter hash result predictably.+  *+  *  Returns a 64-bit hash.+  */+  RAPIDHASH_INLINE_CONSTEXPR uint64_t rapidhashMicro_internal(const void *key, size_t len, uint64_t seed, const uint64_t* secret) RAPIDHASH_NOEXCEPT {+    const uint8_t *p=(const uint8_t *)key;+    seed ^= rapid_mix(seed ^ secret[2], secret[1]);+    uint64_t a=0, b=0;+    size_t i = len;+    if (_likely_(len <= 16)) {+      if (len >= 4) {+        seed ^= len;+        if (len >= 8) {+          const uint8_t* plast = p + len - 8;+          a = rapid_read64(p);+          b = rapid_read64(plast);+        } else {+          const uint8_t* plast = p + len - 4;+          a = rapid_read32(p);+          b = rapid_read32(plast);+        }+      } else if (len > 0) {+        a = (((uint64_t)p[0])<<45)|p[len-1];+        b = p[len>>1];+      } else+        a = b = 0;+    } else {+      if (i > 80) {+        uint64_t see1 = seed, see2 = seed;+        uint64_t see3 = seed, see4 = seed;+        do {+          seed = rapid_mix(rapid_read64(p) ^ secret[0], rapid_read64(p + 8) ^ seed);+          see1 = rapid_mix(rapid_read64(p + 16) ^ secret[1], rapid_read64(p + 24) ^ see1);+          see2 = rapid_mix(rapid_read64(p + 32) ^ secret[2], rapid_read64(p + 40) ^ see2);+          see3 = rapid_mix(rapid_read64(p + 48) ^ secret[3], rapid_read64(p + 56) ^ see3);+          see4 = rapid_mix(rapid_read64(p + 64) ^ secret[4], rapid_read64(p + 72) ^ see4);+          p += 80;+          i -= 80;+        } while(i > 80);+        seed ^= see1;+        see2 ^= see3;+        seed ^= see4;+        seed ^= see2;+      }+      if (i > 16) {+        seed = rapid_mix(rapid_read64(p) ^ secret[2], rapid_read64(p + 8) ^ seed);+        if (i > 32) {+            seed = rapid_mix(rapid_read64(p + 16) ^ secret[2], rapid_read64(p + 24) ^ seed);+            if (i > 48) {+                seed = rapid_mix(rapid_read64(p + 32) ^ secret[1], rapid_read64(p + 40) ^ seed);+                if (i > 64) {+                    seed = rapid_mix(rapid_read64(p + 48) ^ secret[1], rapid_read64(p + 56) ^ seed);+                }+            }+        }+      }+      a=rapid_read64(p+i-16) ^ i;  b=rapid_read64(p+i-8);+    }+    a ^= secret[1];+    b ^= seed;+    rapid_mum(&a, &b);+    return rapid_mix(a ^ secret[7], b ^ secret[1] ^ i);+  }++  /*+  *  rapidhashNano main function.+  *+  *  @param key     Buffer to be hashed.+  *  @param len     @key length, in bytes.+  *  @param seed    64-bit seed used to alter the hash result predictably.+  *  @param secret  Triplet of 64-bit secrets used to alter hash result predictably.+  *+  *  Returns a 64-bit hash.+  */+  RAPIDHASH_INLINE_CONSTEXPR uint64_t rapidhashNano_internal(const void *key, size_t len, uint64_t seed, const uint64_t* secret) RAPIDHASH_NOEXCEPT {+    const uint8_t *p=(const uint8_t *)key;+    seed ^= rapid_mix(seed ^ secret[2], secret[1]);+    uint64_t a=0, b=0;+    size_t i = len;+    if (_likely_(len <= 16)) {+      if (len >= 4) {+        seed ^= len;+        if (len >= 8) {+          const uint8_t* plast = p + len - 8;+          a = rapid_read64(p);+          b = rapid_read64(plast);+        } else {+          const uint8_t* plast = p + len - 4;+          a = rapid_read32(p);+          b = rapid_read32(plast);+        }+      } else if (len > 0) {+        a = (((uint64_t)p[0])<<45)|p[len-1];+        b = p[len>>1];+      } else+        a = b = 0;+    } else {+      if (i > 48) {+        uint64_t see1 = seed, see2 = seed;+        do {+          seed = rapid_mix(rapid_read64(p) ^ secret[0], rapid_read64(p + 8) ^ seed);+          see1 = rapid_mix(rapid_read64(p + 16) ^ secret[1], rapid_read64(p + 24) ^ see1);+          see2 = rapid_mix(rapid_read64(p + 32) ^ secret[2], rapid_read64(p + 40) ^ see2);+          p += 48;+          i -= 48;+        } while(i > 48);+        seed ^= see1;+        seed ^= see2;+      }+      if (i > 16) {+        seed = rapid_mix(rapid_read64(p) ^ secret[2], rapid_read64(p + 8) ^ seed);+        if (i > 32) {+            seed = rapid_mix(rapid_read64(p + 16) ^ secret[2], rapid_read64(p + 24) ^ seed);+        }+      }+      a=rapid_read64(p+i-16) ^ i;  b=rapid_read64(p+i-8);+    }+    a ^= secret[1];+    b ^= seed;+    rapid_mum(&a, &b);+    return rapid_mix(a ^ secret[7], b ^ secret[1] ^ i);+  }+ +/*+ *  rapidhash seeded hash function.+ *+ *  @param key     Buffer to be hashed.+ *  @param len     @key length, in bytes.+ *  @param seed    64-bit seed used to alter the hash result predictably.+ *+ *  Calls rapidhash_internal using provided parameters and default secrets.+ *+ *  Returns a 64-bit hash.+ */+RAPIDHASH_INLINE_CONSTEXPR uint64_t rapidhash_withSeed(const void *key, size_t len, uint64_t seed) RAPIDHASH_NOEXCEPT {+  return rapidhash_internal(key, len, seed, rapid_secret);+}+ +/*+ *  rapidhash general purpose hash function.+ *+ *  @param key     Buffer to be hashed.+ *  @param len     @key length, in bytes.+ *+ *  Calls rapidhash_withSeed using provided parameters and the default seed.+ *+ *  Returns a 64-bit hash.+ */+RAPIDHASH_INLINE_CONSTEXPR uint64_t rapidhash(const void *key, size_t len) RAPIDHASH_NOEXCEPT {+  return rapidhash_withSeed(key, len, 0);+}++/*+ *  rapidhashMicro seeded hash function.+ *+ *  Designed for HPC and server applications, where cache misses make a noticeable performance detriment.+ *  Clang-18+ compiles it to ~140 instructions without stack usage, both on x86-64 and aarch64.+ *  Faster for sizes up to 512 bytes, just 15%-20% slower for inputs above 1kb.+ *+ *  @param key     Buffer to be hashed.+ *  @param len     @key length, in bytes.+ *  @param seed    64-bit seed used to alter the hash result predictably.+ *+ *  Calls rapidhash_internal using provided parameters and default secrets.+ *+ *  Returns a 64-bit hash.+ */+ RAPIDHASH_INLINE_CONSTEXPR uint64_t rapidhashMicro_withSeed(const void *key, size_t len, uint64_t seed) RAPIDHASH_NOEXCEPT {+  return rapidhashMicro_internal(key, len, seed, rapid_secret);+}+ +/*+ *  rapidhashMicro hash function.+ *+ *  @param key     Buffer to be hashed.+ *  @param len     @key length, in bytes.+ *+ *  Calls rapidhash_withSeed using provided parameters and the default seed.+ *+ *  Returns a 64-bit hash.+ */+RAPIDHASH_INLINE_CONSTEXPR uint64_t rapidhashMicro(const void *key, size_t len) RAPIDHASH_NOEXCEPT {+  return rapidhashMicro_withSeed(key, len, 0);+}++/*+ *  rapidhashNano seeded hash function.+ *+ *  @param key     Buffer to be hashed.+ *  @param len     @key length, in bytes.+ *  @param seed    64-bit seed used to alter the hash result predictably.+ *+ *  Calls rapidhash_internal using provided parameters and default secrets.+ *+ *  Returns a 64-bit hash.+ */+ RAPIDHASH_INLINE_CONSTEXPR uint64_t rapidhashNano_withSeed(const void *key, size_t len, uint64_t seed) RAPIDHASH_NOEXCEPT {+  return rapidhashNano_internal(key, len, seed, rapid_secret);+}+ +/*+ *  rapidhashNano hash function.+ *+ *  Designed for Mobile and embedded applications, where keeping a small code size is a top priority.+ *  Clang-18+ compiles it to less than 100 instructions without stack usage, both on x86-64 and aarch64.+ *  The fastest for sizes up to 48 bytes, but may be considerably slower for larger inputs.+ *+ *  @param key     Buffer to be hashed.+ *  @param len     @key length, in bytes.+ *+ *  Calls rapidhash_withSeed using provided parameters and the default seed.+ *+ *  Returns a 64-bit hash.+ */+RAPIDHASH_INLINE_CONSTEXPR uint64_t rapidhashNano(const void *key, size_t len) RAPIDHASH_NOEXCEPT {+  return rapidhashNano_withSeed(key, len, 0);+}
+ upstream/src/recording.c view
@@ -0,0 +1,1228 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS )+#define _CRT_SECURE_NO_WARNINGS+#endif++#include "recording.h"++#include "body.h"+#include "compound.h"+#include "physics_world.h"+#include "world_snapshot.h"++#include "box3d/box3d.h"+#include "box3d/constants.h"++#include <limits.h>+#include <stddef.h>++// Buffer helpers++void b3RecBufAppend( b3RecBuffer* buf, const void* data, int size )+{+	if ( size <= 0 )+	{+		return;+	}++	if ( buf->countOnly )+	{+		buf->size += size;+		return;+	}++	if ( buf->size + size > buf->capacity )+	{+		int newCap = buf->capacity * 2;+		if ( newCap < buf->size + size + 64 )+		{+			newCap = buf->size + size + 64;+		}+		if ( buf->data == NULL )+		{+			buf->data = b3Alloc( (size_t)newCap );+		}+		else+		{+			buf->data = b3GrowAlloc( buf->data, buf->capacity, newCap );+		}+		buf->capacity = newCap;+	}++	memcpy( buf->data + buf->size, data, (size_t)size );+	buf->size += size;+}++void b3RecBufFree( b3RecBuffer* buf )+{+	if ( buf->data != NULL )+	{+		b3Free( buf->data, (size_t)buf->capacity );+		buf->data = NULL;+		buf->capacity = 0;+		buf->size = 0;+	}+}++// Write primitives++void b3RecW_U8( b3RecBuffer* buf, uint8_t v )+{+	b3RecBufAppend( buf, &v, 1 );+}++void b3RecW_U16( b3RecBuffer* buf, uint16_t v )+{+	uint8_t b[2] = { (uint8_t)v, (uint8_t)( v >> 8 ) };+	b3RecBufAppend( buf, b, 2 );+}++void b3RecW_U32( b3RecBuffer* buf, uint32_t v )+{+	uint8_t b[4] = { (uint8_t)v, (uint8_t)( v >> 8 ), (uint8_t)( v >> 16 ), (uint8_t)( v >> 24 ) };+	b3RecBufAppend( buf, b, 4 );+}++void b3RecW_U64( b3RecBuffer* buf, uint64_t v )+{+	uint8_t b[8] = { (uint8_t)v,		   (uint8_t)( v >> 8 ),	 (uint8_t)( v >> 16 ), (uint8_t)( v >> 24 ),+					 (uint8_t)( v >> 32 ), (uint8_t)( v >> 40 ), (uint8_t)( v >> 48 ), (uint8_t)( v >> 56 ) };+	b3RecBufAppend( buf, b, 8 );+}++void b3RecW_I32( b3RecBuffer* buf, int32_t v )+{+	b3RecW_U32( buf, (uint32_t)v );+}++void b3RecW_F32( b3RecBuffer* buf, float v )+{+	uint32_t bits;+	memcpy( &bits, &v, 4 );+	b3RecW_U32( buf, bits );+}++void b3RecW_F64( b3RecBuffer* buf, double v )+{+	uint64_t bits;+	memcpy( &bits, &v, 8 );+	b3RecW_U64( buf, bits );+}++void b3RecW_BOOL( b3RecBuffer* buf, bool v )+{+	b3RecW_U8( buf, v ? 1u : 0u );+}++void b3RecW_VEC3( b3RecBuffer* buf, b3Vec3 v )+{+	b3RecW_F32( buf, v.x );+	b3RecW_F32( buf, v.y );+	b3RecW_F32( buf, v.z );+}++void b3RecW_QUAT( b3RecBuffer* buf, b3Quat v )+{+	b3RecW_F32( buf, v.v.x );+	b3RecW_F32( buf, v.v.y );+	b3RecW_F32( buf, v.v.z );+	b3RecW_F32( buf, v.s );+}++void b3RecW_TRANSFORM( b3RecBuffer* buf, b3Transform v )+{+	b3RecW_VEC3( buf, v.p );+	b3RecW_QUAT( buf, v.q );+}++// World position at full precision so recordings reproduce the simulation far from the origin.+// In the float build this is three floats, wire-identical to VEC3.+void b3RecW_POSITION( b3RecBuffer* buf, b3Pos v )+{+#if defined( BOX3D_DOUBLE_PRECISION )+	b3RecW_F64( buf, v.x );+	b3RecW_F64( buf, v.y );+	b3RecW_F64( buf, v.z );+#else+	b3RecW_F32( buf, v.x );+	b3RecW_F32( buf, v.y );+	b3RecW_F32( buf, v.z );+#endif+}++void b3RecW_WORLDXF( b3RecBuffer* buf, b3WorldTransform v )+{+	b3RecW_POSITION( buf, v.p );+	b3RecW_QUAT( buf, v.q );+}++void b3RecW_MATRIX3( b3RecBuffer* buf, b3Matrix3 v )+{+	b3RecW_VEC3( buf, v.cx );+	b3RecW_VEC3( buf, v.cy );+	b3RecW_VEC3( buf, v.cz );+}++void b3RecW_AABB( b3RecBuffer* buf, b3AABB v )+{+	b3RecW_VEC3( buf, v.lowerBound );+	b3RecW_VEC3( buf, v.upperBound );+}++void b3RecW_QUERYFILTER( b3RecBuffer* buf, b3QueryFilter v )+{+	b3RecW_U64( buf, v.categoryBits );+	b3RecW_U64( buf, v.maskBits );+}++// Variable length: count, then count points, then radius. The point cloud lives behind a pointer so+// it cannot ride along as POD.+void b3RecW_SHAPEPROXY( b3RecBuffer* buf, b3ShapeProxy v )+{+	int count = v.count;+	if ( count < 0 )+		count = 0;+	if ( count > B3_MAX_SHAPE_CAST_POINTS )+		count = B3_MAX_SHAPE_CAST_POINTS;+	b3RecW_I32( buf, count );+	for ( int i = 0; i < count; ++i )+	{+		b3RecW_VEC3( buf, v.points[i] );+	}+	b3RecW_F32( buf, v.radius );+}++void b3RecW_TREESTATS( b3RecBuffer* buf, b3TreeStats v )+{+	b3RecW_I32( buf, v.nodeVisits );+	b3RecW_I32( buf, v.leafVisits );+}++void b3RecW_RAYRESULT( b3RecBuffer* buf, b3RayResult v )+{+	b3RecW_SHAPEID( buf, v.shapeId );+	b3RecW_POSITION( buf, v.point );+	b3RecW_VEC3( buf, v.normal );+	b3RecW_U64( buf, v.userMaterialId );+	b3RecW_F32( buf, v.fraction );+	b3RecW_I32( buf, v.triangleIndex );+	b3RecW_I32( buf, v.childIndex );+	b3RecW_BOOL( buf, v.hit );+}++void b3RecW_PLANERESULT( b3RecBuffer* buf, b3PlaneResult v )+{+	b3RecW_VEC3( buf, v.plane.normal );+	b3RecW_F32( buf, v.plane.offset );+	b3RecW_VEC3( buf, v.point );+}++void b3RecW_WORLDID( b3RecBuffer* buf, b3WorldId v )+{+	b3RecW_U32( buf, b3StoreWorldId( v ) );+}++void b3RecW_BODYID( b3RecBuffer* buf, b3BodyId v )+{+	b3RecW_U64( buf, b3StoreBodyId( v ) );+}++void b3RecW_SHAPEID( b3RecBuffer* buf, b3ShapeId v )+{+	b3RecW_U64( buf, b3StoreShapeId( v ) );+}++void b3RecW_JOINTID( b3RecBuffer* buf, b3JointId v )+{+	b3RecW_U64( buf, b3StoreJointId( v ) );+}++// Pointer-free POD; pointerWidth in the header gates the layout on replay+void b3RecW_SPHERE( b3RecBuffer* buf, b3Sphere v )+{+	b3RecBufAppend( buf, &v, (int)sizeof( b3Sphere ) );+}++void b3RecW_CAPSULE( b3RecBuffer* buf, b3Capsule v )+{+	b3RecBufAppend( buf, &v, (int)sizeof( b3Capsule ) );+}++void b3RecW_GEOMID( b3RecBuffer* buf, uint32_t v )+{+	b3RecW_U32( buf, v );+}++void b3RecW_FILTER( b3RecBuffer* buf, b3Filter v )+{+	b3RecW_U64( buf, v.categoryBits );+	b3RecW_U64( buf, v.maskBits );+	b3RecW_I32( buf, v.groupIndex );+}++void b3RecW_MATERIAL( b3RecBuffer* buf, b3SurfaceMaterial v )+{+	b3RecW_F32( buf, v.friction );+	b3RecW_F32( buf, v.restitution );+	b3RecW_F32( buf, v.rollingResistance );+	b3RecW_VEC3( buf, v.tangentVelocity );+	b3RecW_U64( buf, v.userMaterialId );+	b3RecW_U32( buf, v.customColor );+}++void b3RecW_MASSDATA( b3RecBuffer* buf, b3MassData v )+{+	b3RecW_F32( buf, v.mass );+	b3RecW_VEC3( buf, v.center );+	b3RecW_MATRIX3( buf, v.inertia );+}++void b3RecW_LOCKS( b3RecBuffer* buf, b3MotionLocks v )+{+	b3RecW_BOOL( buf, v.linearX );+	b3RecW_BOOL( buf, v.linearY );+	b3RecW_BOOL( buf, v.linearZ );+	b3RecW_BOOL( buf, v.angularX );+	b3RecW_BOOL( buf, v.angularY );+	b3RecW_BOOL( buf, v.angularZ );+}++static void b3RecW_STR( b3RecBuffer* buf, const char* s )+{+	if ( s == NULL )+	{+		b3RecW_U16( buf, 0xFFFFu );+		return;+	}+	int len = 0;+	while ( s[len] != '\0' && len < 65534 )+	{+		len++;+	}+	b3RecW_U16( buf, (uint16_t)len );+	if ( len > 0 )+	{+		b3RecBufAppend( buf, s, len );+	}+}++// Hand-written def helpers. Zero pointer and cookie fields before serializing.+// Readers call b3Default*Def() first to get the cookie, then overwrite fields.++// Tripwire: each def serializer below is paired with a reader in recording_replay.c, and the two must+// stay field-for-field in sync. Add a field to a def and the size changes, firing the matching assert+// so the writer and reader both get updated. Only enforced on the 64-bit target; each def lists the+// single-precision and double-precision sizes (equal for most), so either build configuration passes.+_Static_assert( sizeof( void* ) != 8 || sizeof( b3ExplosionDef ) == 32 || sizeof( b3ExplosionDef ) == 48,+				"b3ExplosionDef changed: update b3RecW_EXPLOSIONDEF and b3RecR_EXPLOSIONDEF together" );+_Static_assert( sizeof( void* ) != 8 || sizeof( b3BodyDef ) == 104 || sizeof( b3BodyDef ) == 120,+				"b3BodyDef changed: update b3RecW_BODYDEF and b3RecR_BODYDEF together" );+_Static_assert( sizeof( void* ) != 8 || sizeof( b3ShapeDef ) == 120,+				"b3ShapeDef changed: update b3RecW_SHAPEDEF and b3RecR_SHAPEDEF together" );+_Static_assert( sizeof( void* ) != 8 || sizeof( b3ParallelJointDef ) == 128,+				"b3ParallelJointDef changed: update b3RecW_PARALLELJOINTDEF and its reader together" );+_Static_assert( sizeof( void* ) != 8 || sizeof( b3DistanceJointDef ) == 160,+				"b3DistanceJointDef changed: update b3RecW_DISTANCEJOINTDEF and its reader together" );+_Static_assert( sizeof( void* ) != 8 || sizeof( b3FilterJointDef ) == 112,+				"b3FilterJointDef changed: update b3RecW_FILTERJOINTDEF and its reader together" );+_Static_assert( sizeof( void* ) != 8 || sizeof( b3MotorJointDef ) == 168,+				"b3MotorJointDef changed: update b3RecW_MOTORJOINTDEF and its reader together" );+_Static_assert( sizeof( void* ) != 8 || sizeof( b3PrismaticJointDef ) == 152,+				"b3PrismaticJointDef changed: update b3RecW_PRISMATICJOINTDEF and its reader together" );+_Static_assert( sizeof( void* ) != 8 || sizeof( b3RevoluteJointDef ) == 152,+				"b3RevoluteJointDef changed: update b3RecW_REVOLUTEJOINTDEF and its reader together" );+_Static_assert( sizeof( void* ) != 8 || sizeof( b3SphericalJointDef ) == 184,+				"b3SphericalJointDef changed: update b3RecW_SPHERICALJOINTDEF and its reader together" );+_Static_assert( sizeof( void* ) != 8 || sizeof( b3WeldJointDef ) == 128,+				"b3WeldJointDef changed: update b3RecW_WELDJOINTDEF and its reader together" );+_Static_assert( sizeof( void* ) != 8 || sizeof( b3WheelJointDef ) == 184,+				"b3WheelJointDef changed: update b3RecW_WHEELJOINTDEF and its reader together" );++void b3RecW_EXPLOSIONDEF( b3RecBuffer* buf, b3ExplosionDef v )+{+	b3RecW_U64( buf, v.maskBits );+	b3RecW_POSITION( buf, v.position );+	b3RecW_F32( buf, v.radius );+	b3RecW_F32( buf, v.falloff );+	b3RecW_F32( buf, v.impulsePerArea );+}++void b3RecW_BODYDEF( b3RecBuffer* buf, b3BodyDef v )+{+	b3RecW_I32( buf, (int32_t)v.type );+	b3RecW_POSITION( buf, v.position );+	b3RecW_QUAT( buf, v.rotation );+	b3RecW_VEC3( buf, v.linearVelocity );+	b3RecW_VEC3( buf, v.angularVelocity );+	b3RecW_F32( buf, v.linearDamping );+	b3RecW_F32( buf, v.angularDamping );+	b3RecW_F32( buf, v.gravityScale );+	b3RecW_F32( buf, v.sleepThreshold );+	b3RecW_STR( buf, v.name );+	// userData: not preserved+	b3RecW_U64( buf, 0u );+	b3RecW_LOCKS( buf, v.motionLocks );+	b3RecW_BOOL( buf, v.enableSleep );+	b3RecW_BOOL( buf, v.isAwake );+	b3RecW_BOOL( buf, v.isBullet );+	b3RecW_BOOL( buf, v.isEnabled );+	b3RecW_BOOL( buf, v.allowFastRotation );+	b3RecW_BOOL( buf, v.enableContactRecycling );+	// internalValue omitted+}++void b3RecW_SHAPEDEF( b3RecBuffer* buf, b3ShapeDef v )+{+	b3RecW_STR( buf, v.name );++	// userData: not preserved+	b3RecW_U64( buf, 0u );+	// Per-triangle materials: length-prefixed so the reader can rebuild the array.+	// Guard NULL so a default def (materialCount=0, materials=NULL) round-trips cleanly.+	int matCount = ( v.materials != NULL ) ? v.materialCount : 0;+	b3RecW_I32( buf, matCount );+	for ( int i = 0; i < matCount; ++i )+	{+		b3RecW_MATERIAL( buf, v.materials[i] );+	}+	b3RecW_MATERIAL( buf, v.baseMaterial );+	b3RecW_F32( buf, v.density );+	b3RecW_F32( buf, v.explosionScale );+	b3RecW_FILTER( buf, v.filter );+	b3RecW_BOOL( buf, v.enableCustomFiltering );+	b3RecW_BOOL( buf, v.isSensor );+	b3RecW_BOOL( buf, v.enableSensorEvents );+	b3RecW_BOOL( buf, v.enableContactEvents );+	b3RecW_BOOL( buf, v.enableHitEvents );+	b3RecW_BOOL( buf, v.enablePreSolveEvents );+	b3RecW_BOOL( buf, v.invokeContactCreation );+	b3RecW_BOOL( buf, v.updateBodyMass );+	b3RecW_BOOL( buf, v.enableSpeculativeContact );+	// internalValue omitted+}++// Joint defs share a base. Body ids are written as packed ids for replay remapping.+static void b3RecW_JointBase( b3RecBuffer* buf, const b3JointDef* base )+{+	// userData: not preserved+	b3RecW_U64( buf, 0u );+	b3RecW_BODYID( buf, base->bodyIdA );+	b3RecW_BODYID( buf, base->bodyIdB );+	b3RecW_TRANSFORM( buf, base->localFrameA );+	b3RecW_TRANSFORM( buf, base->localFrameB );+	b3RecW_F32( buf, base->forceThreshold );+	b3RecW_F32( buf, base->torqueThreshold );+	b3RecW_F32( buf, base->constraintHertz );+	b3RecW_F32( buf, base->constraintDampingRatio );+	b3RecW_F32( buf, base->drawScale );+	b3RecW_BOOL( buf, base->collideConnected );+	// internalValue omitted+}++void b3RecW_PARALLELJOINTDEF( b3RecBuffer* buf, b3ParallelJointDef v )+{+	b3RecW_JointBase( buf, &v.base );+	b3RecW_F32( buf, v.hertz );+	b3RecW_F32( buf, v.dampingRatio );+	b3RecW_F32( buf, v.maxTorque );+}++void b3RecW_DISTANCEJOINTDEF( b3RecBuffer* buf, b3DistanceJointDef v )+{+	b3RecW_JointBase( buf, &v.base );+	b3RecW_F32( buf, v.length );+	b3RecW_BOOL( buf, v.enableSpring );+	b3RecW_F32( buf, v.lowerSpringForce );+	b3RecW_F32( buf, v.upperSpringForce );+	b3RecW_F32( buf, v.hertz );+	b3RecW_F32( buf, v.dampingRatio );+	b3RecW_BOOL( buf, v.enableLimit );+	b3RecW_F32( buf, v.minLength );+	b3RecW_F32( buf, v.maxLength );+	b3RecW_BOOL( buf, v.enableMotor );+	b3RecW_F32( buf, v.maxMotorForce );+	b3RecW_F32( buf, v.motorSpeed );+}++void b3RecW_FILTERJOINTDEF( b3RecBuffer* buf, b3FilterJointDef v )+{+	b3RecW_JointBase( buf, &v.base );+}++void b3RecW_MOTORJOINTDEF( b3RecBuffer* buf, b3MotorJointDef v )+{+	b3RecW_JointBase( buf, &v.base );+	b3RecW_VEC3( buf, v.linearVelocity );+	b3RecW_F32( buf, v.maxVelocityForce );+	b3RecW_VEC3( buf, v.angularVelocity );+	b3RecW_F32( buf, v.maxVelocityTorque );+	b3RecW_F32( buf, v.linearHertz );+	b3RecW_F32( buf, v.linearDampingRatio );+	b3RecW_F32( buf, v.maxSpringForce );+	b3RecW_F32( buf, v.angularHertz );+	b3RecW_F32( buf, v.angularDampingRatio );+	b3RecW_F32( buf, v.maxSpringTorque );+}++void b3RecW_PRISMATICJOINTDEF( b3RecBuffer* buf, b3PrismaticJointDef v )+{+	b3RecW_JointBase( buf, &v.base );+	b3RecW_BOOL( buf, v.enableSpring );+	b3RecW_F32( buf, v.hertz );+	b3RecW_F32( buf, v.dampingRatio );+	b3RecW_F32( buf, v.targetTranslation );+	b3RecW_BOOL( buf, v.enableLimit );+	b3RecW_F32( buf, v.lowerTranslation );+	b3RecW_F32( buf, v.upperTranslation );+	b3RecW_BOOL( buf, v.enableMotor );+	b3RecW_F32( buf, v.maxMotorForce );+	b3RecW_F32( buf, v.motorSpeed );+}++void b3RecW_REVOLUTEJOINTDEF( b3RecBuffer* buf, b3RevoluteJointDef v )+{+	b3RecW_JointBase( buf, &v.base );+	b3RecW_F32( buf, v.targetAngle );+	b3RecW_BOOL( buf, v.enableSpring );+	b3RecW_F32( buf, v.hertz );+	b3RecW_F32( buf, v.dampingRatio );+	b3RecW_BOOL( buf, v.enableLimit );+	b3RecW_F32( buf, v.lowerAngle );+	b3RecW_F32( buf, v.upperAngle );+	b3RecW_BOOL( buf, v.enableMotor );+	b3RecW_F32( buf, v.maxMotorTorque );+	b3RecW_F32( buf, v.motorSpeed );+}++void b3RecW_SPHERICALJOINTDEF( b3RecBuffer* buf, b3SphericalJointDef v )+{+	b3RecW_JointBase( buf, &v.base );+	b3RecW_BOOL( buf, v.enableSpring );+	b3RecW_F32( buf, v.hertz );+	b3RecW_F32( buf, v.dampingRatio );+	b3RecW_QUAT( buf, v.targetRotation );+	b3RecW_BOOL( buf, v.enableConeLimit );+	b3RecW_F32( buf, v.coneAngle );+	b3RecW_BOOL( buf, v.enableTwistLimit );+	b3RecW_F32( buf, v.lowerTwistAngle );+	b3RecW_F32( buf, v.upperTwistAngle );+	b3RecW_BOOL( buf, v.enableMotor );+	b3RecW_F32( buf, v.maxMotorTorque );+	b3RecW_VEC3( buf, v.motorVelocity );+}++void b3RecW_WELDJOINTDEF( b3RecBuffer* buf, b3WeldJointDef v )+{+	b3RecW_JointBase( buf, &v.base );+	b3RecW_F32( buf, v.linearHertz );+	b3RecW_F32( buf, v.angularHertz );+	b3RecW_F32( buf, v.linearDampingRatio );+	b3RecW_F32( buf, v.angularDampingRatio );+}++void b3RecW_WHEELJOINTDEF( b3RecBuffer* buf, b3WheelJointDef v )+{+	b3RecW_JointBase( buf, &v.base );+	b3RecW_BOOL( buf, v.enableSuspensionSpring );+	b3RecW_F32( buf, v.suspensionHertz );+	b3RecW_F32( buf, v.suspensionDampingRatio );+	b3RecW_BOOL( buf, v.enableSuspensionLimit );+	b3RecW_F32( buf, v.lowerSuspensionLimit );+	b3RecW_F32( buf, v.upperSuspensionLimit );+	b3RecW_BOOL( buf, v.enableSpinMotor );+	b3RecW_F32( buf, v.maxSpinTorque );+	b3RecW_F32( buf, v.spinSpeed );+	b3RecW_BOOL( buf, v.enableSteering );+	b3RecW_F32( buf, v.steeringHertz );+	b3RecW_F32( buf, v.steeringDampingRatio );+	b3RecW_F32( buf, v.targetSteeringAngle );+	b3RecW_F32( buf, v.maxSteeringTorque );+	b3RecW_BOOL( buf, v.enableSteeringLimit );+	b3RecW_F32( buf, v.lowerSteeringLimit );+	b3RecW_F32( buf, v.upperSteeringLimit );+}++// Query recording. A query collects a variable number of hits through a user callback, so the count+// is not known until the callback stops firing. The record is built in a local buffer with a+// reserved hit-count slot, then committed whole under the lock so concurrent query threads never+// interleave records in the shared buffer.++int b3RecReserveU32( b3RecBuffer* buf )+{+	int offset = buf->size;+	uint8_t zero[4] = { 0, 0, 0, 0 };+	b3RecBufAppend( buf, zero, 4 );+	return offset;+}++void b3RecPatchU32( b3RecBuffer* buf, int offset, uint32_t v )+{+	B3_ASSERT( offset >= 0 && offset + 4 <= buf->size );+	uint8_t* p = buf->data + offset;+	p[0] = (uint8_t)v;+	p[1] = (uint8_t)( v >> 8 );+	p[2] = (uint8_t)( v >> 16 );+	p[3] = (uint8_t)( v >> 24 );+}++// Frame and append one record into the buffer. Caller holds rec->lock.+static void b3RecCommitRecordLocked( b3Recording* rec, uint8_t opcode, const uint8_t* payload, int payloadSize )+{+	B3_ASSERT( payloadSize >= 0 && payloadSize < ( 1 << 24 ) );+	b3RecW_U8( &rec->buffer, opcode );+	uint8_t sz[3] = { (uint8_t)payloadSize, (uint8_t)( payloadSize >> 8 ), (uint8_t)( payloadSize >> 16 ) };+	b3RecBufAppend( &rec->buffer, sz, 3 );+	b3RecBufAppend( &rec->buffer, payload, payloadSize );+}++void b3RecCommitRecord( b3Recording* rec, uint8_t opcode, const uint8_t* payload, int payloadSize )+{+	b3LockMutex( rec->lock );+	b3RecCommitRecordLocked( rec, opcode, payload, payloadSize );+	b3UnlockMutex( rec->lock );+}++void b3RecQueryBegin( b3RecQueryWriter* w, void* context, uint64_t tagId, const char* tagName )+{+	w->buf = (b3RecBuffer){ 0 };+	w->userFcn.overlapFcn = NULL;+	w->userContext = context;+	w->hitCount = 0;+	w->countOffset = 0;+	w->tagId = tagId;+	w->tagName = tagName;+}++void b3RecQueryCommit( b3Recording* rec, uint8_t opcode, b3RecQueryWriter* w )+{+	b3LockMutex( rec->lock );+	// A tagged query writes its identity key right before the query record, under one lock so the pair+	// stays adjacent even with concurrent queries. The key is the hash of the caller (id, name), which+	// are interned once into the trailing tag table so the viewer can show them.+	bool tagged = w->tagId != 0 || ( w->tagName != NULL && w->tagName[0] != '\0' );+	if ( tagged )+	{+		uint64_t key = b3HashQueryTag( w->tagId, w->tagName );+		b3RecInternTag( rec, key, w->tagId, w->tagName );+		b3RecBuffer tagBuf = { 0 };+		b3RecW_U64( &tagBuf, key );+		b3RecCommitRecordLocked( rec, b3_recOpQueryTag, tagBuf.data, tagBuf.size );+		b3RecBufFree( &tagBuf );+	}+	b3RecCommitRecordLocked( rec, opcode, w->buf.data, w->buf.size );+	b3UnlockMutex( rec->lock );+	b3RecBufFree( &w->buf );+}++bool b3RecOverlapTrampoline( b3ShapeId id, void* ctx )+{+	b3RecQueryWriter* w = (b3RecQueryWriter*)ctx;+	// The user fcn is NULL for an unfiltered mover cast: accept all, still record the decision so+	// replay reproduces the same per-shape accept stream.+	bool ret = w->userFcn.overlapFcn != NULL ? w->userFcn.overlapFcn( id, w->userContext ) : true;+	b3RecW_SHAPEID( &w->buf, id );+	b3RecW_BOOL( &w->buf, ret );+	w->hitCount++;+	return ret;+}++float b3RecCastTrampoline( b3ShapeId id, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, int triangleIndex,+						   int childIndex, void* ctx )+{+	b3RecQueryWriter* w = (b3RecQueryWriter*)ctx;+	float ret = w->userFcn.castFcn( id, point, normal, fraction, userMaterialId, triangleIndex, childIndex, w->userContext );+	b3RecW_SHAPEID( &w->buf, id );+	b3RecW_POSITION( &w->buf, point );+	b3RecW_VEC3( &w->buf, normal );+	b3RecW_F32( &w->buf, fraction );+	b3RecW_U64( &w->buf, userMaterialId );+	b3RecW_I32( &w->buf, triangleIndex );+	b3RecW_I32( &w->buf, childIndex );+	b3RecW_F32( &w->buf, ret );+	w->hitCount++;+	return ret;+}++// 3D delivers every plane for one shape in a single call. Record the shape, its plane count, each+// plane, then the user return so replay reproduces the same per-shape batch. One hit per shape.+bool b3RecPlaneTrampoline( b3ShapeId id, const b3PlaneResult* planes, int planeCount, void* ctx )+{+	b3RecQueryWriter* w = (b3RecQueryWriter*)ctx;+	bool ret = w->userFcn.planeFcn( id, planes, planeCount, w->userContext );+	b3RecW_SHAPEID( &w->buf, id );+	b3RecW_I32( &w->buf, planeCount );+	for ( int i = 0; i < planeCount; ++i )+	{+		b3RecW_PLANERESULT( &w->buf, planes[i] );+	}+	b3RecW_BOOL( &w->buf, ret );+	w->hitCount++;+	return ret;+}++// Record framing++void b3RecBeginRecord( b3Recording* rec, uint8_t opcode )+{+	b3RecW_U8( &rec->buffer, opcode );+	rec->recordStart = rec->buffer.size;+	// Reserve 3 bytes for the u24 payload size, backpatched in b3RecEndRecord.+	uint8_t zero[3] = { 0, 0, 0 };+	b3RecBufAppend( &rec->buffer, zero, 3 );+}++void b3RecEndRecord( b3Recording* rec )+{+	int payloadSize = rec->buffer.size - rec->recordStart - 3;+	B3_ASSERT( payloadSize >= 0 && payloadSize < ( 1 << 24 ) );+	uint8_t* p = rec->buffer.data + rec->recordStart;+	p[0] = (uint8_t)payloadSize;+	p[1] = (uint8_t)( payloadSize >> 8 );+	p[2] = (uint8_t)( payloadSize >> 16 );+}++// Codegen pass 1b: arg writers+#define ARG( TAG, field ) b3RecW_##TAG( &rec->buffer, a->field );+#define B3_REC_OP( op, Name, RET, ... )                                                                                          \+	void b3RecWriteArgs_##Name( b3Recording* rec, const b3RecArgs_##Name* a )                                                    \+	{                                                                                                                            \+		__VA_ARGS__                                                                                                              \+	}+#include "recording_ops.inl"+#undef B3_REC_OP+#undef ARG++// Codegen: full writers. Setters may run on threads that each own a distinct object,+// so hold the lock across the whole record. Without it a concurrent writer splices its bytes between+// our begin and end and the record desyncs replay. Same lock the query commit path takes.+#define B3_REC_OP( op, Name, RET, ... )                                                                                          \+	void b3RecWrite_##Name( b3Recording* rec, const b3RecArgs_##Name* a )                                                        \+	{                                                                                                                            \+		b3LockMutex( rec->lock );                                                                                                \+		b3RecBeginRecord( rec, (uint8_t)( op ) );                                                                                \+		b3RecWriteArgs_##Name( rec, a );                                                                                         \+		b3RecEndRecord( rec );                                                                                                   \+		b3UnlockMutex( rec->lock );                                                                                              \+	}+#include "recording_ops.inl"+#undef B3_REC_OP++// Codegen: create-op writers that append the returned id inside the record+#define B3_REC_RETWRITE( op, Name, idType, idW )                                                                                 \+	void b3RecWriteRet_##Name( b3Recording* rec, const b3RecArgs_##Name* a, idType id )                                          \+	{                                                                                                                            \+		b3LockMutex( rec->lock );                                                                                                \+		b3RecBeginRecord( rec, (uint8_t)( op ) );                                                                                \+		b3RecWriteArgs_##Name( rec, a );                                                                                         \+		idW( &rec->buffer, id );                                                                                                 \+		b3RecEndRecord( rec );                                                                                                   \+		b3UnlockMutex( rec->lock );                                                                                              \+	}+#define B3_REC_RETWRITE_RET_NONE( op, Name )+#define B3_REC_RETWRITE_RET_BODYID( op, Name ) B3_REC_RETWRITE( op, Name, b3BodyId, b3RecW_BODYID )+#define B3_REC_RETWRITE_RET_SHAPEID( op, Name ) B3_REC_RETWRITE( op, Name, b3ShapeId, b3RecW_SHAPEID )+#define B3_REC_RETWRITE_RET_JOINTID( op, Name ) B3_REC_RETWRITE( op, Name, b3JointId, b3RecW_JOINTID )+#define B3_REC_OP( op, Name, RET, ... ) B3_REC_RETWRITE_##RET( op, Name )+#include "recording_ops.inl"+#undef B3_REC_OP+#undef B3_REC_RETWRITE_RET_NONE+#undef B3_REC_RETWRITE_RET_BODYID+#undef B3_REC_RETWRITE_RET_SHAPEID+#undef B3_REC_RETWRITE_RET_JOINTID+#undef B3_REC_RETWRITE++// Geometry registry++// Content hash to chain head, so dedup is near O(1). Colliding hashes share a head and are walked+// through b3GeometryEntry::hashNext, so the byteCount + memcmp check always finds an existing blob.+#define NAME b3GeometryHashMap+#define KEY_TY uint64_t+#define VAL_TY uint32_t+#define HASH_FN vt_hash_integer+#define CMPR_FN vt_cmpr_integer+#define MALLOC_FN b3Alloc+#define FREE_FN b3Free+#include "verstable.h"++// Tag key to tag index, so interning a query tag is O(1) rather than a linear scan over the tag table.+#define NAME b3RecTagMap+#define KEY_TY uint64_t+#define VAL_TY uint32_t+#define HASH_FN vt_hash_integer+#define CMPR_FN vt_cmpr_integer+#define MALLOC_FN b3Alloc+#define FREE_FN b3Free+#include "verstable.h"++// Append a fresh entry and splice it onto the front of its hash chain. The map value is the chain head.+static uint32_t b3RegistryPush( b3GeometryRegistry* reg, b3GeometryHashMap* map, b3GeometryHashMap_itr itr, bool hashPresent,+								b3GeometryKind kind, uint64_t contentHash, uint8_t* bytes, int byteCount )+{+	uint32_t id = (uint32_t)reg->entries.count;+	b3GeometryEntry* entry = b3Array_Emplace( reg->entries );+	entry->contentHash = contentHash;+	entry->id = id;+	entry->kind = kind;+	entry->byteCount = byteCount;+	// Take ownership.+	entry->bytes = bytes;+	entry->hashNext = hashPresent ? (int)itr.data->val : B3_NULL_INDEX;++	if ( hashPresent )+	{+		itr.data->val = id;+	}+	else+	{+		b3GeometryHashMap_insert( map, contentHash, id );+	}+	return id;+}++static b3GeometryHashMap* b3RegistryMap( b3GeometryRegistry* reg )+{+	if ( reg->dedupMap == NULL )+	{+		b3GeometryHashMap* fresh = b3Alloc( sizeof( b3GeometryHashMap ) );+		b3GeometryHashMap_init( fresh );+		reg->dedupMap = fresh;+	}+	return reg->dedupMap;+}++uint32_t b3InternGeometry( b3GeometryRegistry* reg, b3GeometryKind kind, uint64_t contentHash, uint8_t* bytes, int byteCount )+{+	b3GeometryHashMap* map = b3RegistryMap( reg );++	b3GeometryHashMap_itr itr = b3GeometryHashMap_get( map, contentHash );+	bool hashPresent = b3GeometryHashMap_is_end( itr ) == false;+	if ( hashPresent )+	{+		// Walk every entry sharing this hash so a collision still finds the identical blob.+		for ( int index = (int)itr.data->val; index != B3_NULL_INDEX; index = reg->entries.data[index].hashNext )+		{+			b3GeometryEntry* e = reg->entries.data + index;+			if ( e->byteCount == byteCount && memcmp( e->bytes, bytes, (size_t)byteCount ) == 0 )+			{+				// Duplicate. Free bytes because the caller transferred ownership. Return existing id.+				b3Free( bytes, (size_t)byteCount );+				return e->id;+			}+		}+	}++	return b3RegistryPush( reg, map, itr, hashPresent, kind, contentHash, bytes, byteCount );+}++uint32_t b3AppendGeometry( b3GeometryRegistry* reg, b3GeometryKind kind, uint64_t contentHash, uint8_t* bytes, int byteCount )+{+	b3GeometryHashMap* map = b3RegistryMap( reg );+	b3GeometryHashMap_itr itr = b3GeometryHashMap_get( map, contentHash );+	bool hashPresent = b3GeometryHashMap_is_end( itr ) == false;+	return b3RegistryPush( reg, map, itr, hashPresent, kind, contentHash, bytes, byteCount );+}++void b3FreeRegistry( b3GeometryRegistry* reg )+{+	for ( int i = 0; i < reg->entries.count; ++i )+	{+		b3Free( reg->entries.data[i].bytes, (size_t)reg->entries.data[i].byteCount );+	}++	b3Array_Destroy( reg->entries );++	if ( reg->dedupMap != NULL )+	{+		b3GeometryHashMap_cleanup( (b3GeometryHashMap*)reg->dedupMap );+		b3Free( reg->dedupMap, sizeof( b3GeometryHashMap ) );+	}+	reg->dedupMap = NULL;+}++uint64_t b3HashQueryTag( uint64_t id, const char* name )+{+	uint64_t h = B3_SNAP_FNV_INIT;+	for ( int i = 0; i < 8; ++i )+	{+		h = ( h ^ ( ( id >> ( 8 * i ) ) & 0xFFu ) ) * B3_SNAP_FNV_PRIME;+	}+	if ( name != NULL )+	{+		for ( int i = 0; name[i] != '\0'; ++i )+		{+			h = ( h ^ (uint8_t)name[i] ) * B3_SNAP_FNV_PRIME;+		}+	}+	// Never 0 so the key doubles as the tagged flag.+	return h != 0 ? h : 1;+}++static b3RecTagMap* b3RecTags( b3Recording* rec )+{+	if ( rec->tagMap == NULL )+	{+		b3RecTagMap* fresh = b3Alloc( sizeof( b3RecTagMap ) );+		b3RecTagMap_init( fresh );+		rec->tagMap = fresh;+	}+	return rec->tagMap;+}++void b3RecInternTag( b3Recording* rec, uint64_t key, uint64_t id, const char* name )+{+	b3RecTagMap* map = b3RecTags( rec );+	if ( b3RecTagMap_is_end( b3RecTagMap_get( map, key ) ) == false )+	{+		return; // first id/name for a key wins+	}++	if ( rec->tagCount == rec->tagCapacity )+	{+		int newCap = rec->tagCapacity == 0 ? 8 : 2 * rec->tagCapacity;+		rec->tags = b3GrowAlloc( rec->tags, rec->tagCapacity * (int)sizeof( b3RecTag ), newCap * (int)sizeof( b3RecTag ) );+		rec->tagCapacity = newCap;+	}++	uint32_t index = (uint32_t)rec->tagCount;+	b3RecTag* tag = &rec->tags[rec->tagCount++];+	tag->key = key;+	tag->id = id;+	int n = 0;+	while ( name != NULL && name[n] != '\0' && n < B3_MAX_QUERY_NAME_LENGTH )+	{+		tag->queryName[n] = name[n];+		n++;+	}+	tag->queryName[n] = '\0';+	b3RecTagMap_insert( map, key, index );+}++// Write the trailing registry block: u32 entryCount then per-entry { u8 kind, u32 byteCount, bytes },+// followed by the query-tag table { u32 tagCount, per-tag uu64 id, STR name }. A reader built before+// the tag table stops after the geometry entries and ignores the trailing tag bytes.+void b3RecWriteRegistry( b3Recording* rec )+{+	b3RecW_U32( &rec->buffer, (uint32_t)rec->registry.entries.count );+	for ( int i = 0; i < rec->registry.entries.count; ++i )+	{+		b3GeometryEntry* e = rec->registry.entries.data + i;+		b3RecW_U8( &rec->buffer, (uint8_t)e->kind );+		b3RecW_U32( &rec->buffer, (uint32_t)e->byteCount );+		b3RecBufAppend( &rec->buffer, e->bytes, e->byteCount );+	}++	b3RecW_U32( &rec->buffer, (uint32_t)rec->tagCount );+	for ( int i = 0; i < rec->tagCount; ++i )+	{+		b3RecW_U64( &rec->buffer, rec->tags[i].key );+		b3RecW_U64( &rec->buffer, rec->tags[i].id );+		b3RecW_STR( &rec->buffer, rec->tags[i].queryName );+	}+}++// Lifecycle++b3Recording* b3CreateRecording( int byteCapacity )+{+	b3Recording* rec = b3Alloc( sizeof( b3Recording ) );+	*rec = (b3Recording){ 0 };++	int initCap = byteCapacity > 0 ? byteCapacity : 65536;+	rec->buffer.data = (uint8_t*)b3Alloc( (size_t)initCap );+	rec->buffer.capacity = initCap;+	rec->buffer.size = 0;+	rec->lock = b3CreateMutex();+	return rec;+}++void b3DestroyRecording( b3Recording* recording )+{+	if ( recording == NULL )+	{+		return;+	}++	b3RecBufFree( &recording->buffer );+	b3FreeRegistry( &recording->registry );+	if ( recording->tags != NULL )+	{+		b3Free( recording->tags, (size_t)recording->tagCapacity * sizeof( b3RecTag ) );+	}+	if ( recording->tagMap != NULL )+	{+		b3RecTagMap_cleanup( (b3RecTagMap*)recording->tagMap );+		b3Free( recording->tagMap, sizeof( b3RecTagMap ) );+	}+	b3DestroyMutex( recording->lock );+	b3Free( recording, sizeof( b3Recording ) );+}++const uint8_t* b3Recording_GetData( const b3Recording* recording )+{+	return recording->buffer.data;+}++int b3Recording_GetSize( const b3Recording* recording )+{+	return recording->buffer.size;+}++void b3RecAccumulateBounds( b3Recording* rec, b3AABB bounds )+{+	rec->accumulatedBounds = rec->haveBounds ? b3AABB_Union( rec->accumulatedBounds, bounds ) : bounds;+	rec->haveBounds = true;+}++void b3StartRecordingIntoBuffer( b3World* world, b3Recording* recording )+{+	// Reset so a recording handle can be reused for a fresh session+	recording->buffer.size = 0;+	recording->recordStart = 0;+	recording->haveBounds = false;+	b3FreeRegistry( &recording->registry );+	if ( recording->tags != NULL )+	{+		b3Free( recording->tags, (size_t)recording->tagCapacity * sizeof( b3RecTag ) );+		recording->tags = NULL;+	}+	if ( recording->tagMap != NULL )+	{+		b3RecTagMap_cleanup( (b3RecTagMap*)recording->tagMap );+		b3Free( recording->tagMap, sizeof( b3RecTagMap ) );+		recording->tagMap = NULL;+	}+	recording->tagCount = 0;+	recording->tagCapacity = 0;++	b3RecHeader hdr = { 0 };+	hdr.magic = B3_REC_MAGIC;+	hdr.versionMajor = B3_REC_VERSION_MAJOR;+	hdr.versionMinor = B3_REC_VERSION_MINOR;+	hdr.pointerWidth = (uint8_t)sizeof( void* );+	hdr.bigEndian = 0;+	hdr.validationEnabled = B3_ENABLE_VALIDATION ? 1u : 0u;+	hdr.lengthScale = b3GetLengthUnitsPerMeter();+	hdr.registryOffset = 0; // backpatched in b3StopRecordingInternal+	hdr.registryByteCount = 0;++	world->recording = recording;++	// Every recording is snapshot-seeded. The seed blob follows the header so replay restores in+	// place and the world id stays stable across a restart or backward scrub. An empty world still+	// serializes a valid blob, so there is no from-creation special case.+	b3RecBuffer snapBuf = { 0 };+	b3SerializeWorld( world, &snapBuf, recording );+	hdr.snapshotSize = (uint64_t)snapBuf.size;++	b3RecBufAppend( &recording->buffer, &hdr, (int)sizeof( hdr ) );+	b3RecBufAppend( &recording->buffer, snapBuf.data, snapBuf.size );+	b3RecBufFree( &snapBuf );++	// Anchor the recording with the current world state hash so replay can assert+	// determinism from the very first step.+	b3WorldId worldId = { (uint16_t)( world->worldId + 1 ), world->generation };+	b3RecArgs_StateHash stateHash = { worldId, b3HashWorldState( world ) };+	b3RecWrite_StateHash( recording, &stateHash );+}++void b3StopRecordingInternal( b3World* world )+{+	if ( world->recording == NULL )+	{+		return;+	}++	b3Recording* rec = world->recording;+	world->recording = NULL;++	// Write accumulated bounds so a viewer can frame the whole recorded motion+	b3RecArgs_RecordingBounds rb = { 0 };+	if ( rec->haveBounds )+	{+		rb.bounds = rec->accumulatedBounds;+	}+	b3RecWrite_RecordingBounds( rec, &rb );++	// End-of-stream marker; the buffer is now self-contained+	b3WorldId wid = { (uint16_t)( world->worldId + 1 ), world->generation };+	b3RecArgs_DestroyWorld a = { wid };+	b3RecWrite_DestroyWorld( rec, &a );++	// Write the trailing registry block+	int registryOffset = rec->buffer.size;+	b3RecWriteRegistry( rec );+	int registryByteCount = rec->buffer.size - registryOffset;++	// Backpatch registryOffset and registryByteCount into the header+	uint8_t* hdrBytes = rec->buffer.data;+	uint64_t regOff = (uint64_t)registryOffset;+	uint64_t regSz = (uint64_t)registryByteCount;+	// Little-endian backpatch in place; offsetof keeps this correct if the header layout shifts+	uint8_t* pOff = hdrBytes + offsetof( b3RecHeader, registryOffset );+	uint8_t* pSz = hdrBytes + offsetof( b3RecHeader, registryByteCount );+	for ( int i = 0; i < 8; ++i )+	{+		pOff[i] = (uint8_t)( regOff >> ( 8 * i ) );+		pSz[i] = (uint8_t)( regSz >> ( 8 * i ) );+	}+}++// Convenience file I/O++bool b3SaveRecordingToFile( const b3Recording* recording, const char* path )+{+	if ( recording == NULL || path == NULL )+	{+		return false;+	}++	FILE* f = fopen( path, "wb" );+	if ( f == NULL )+	{+		return false;+	}++	size_t written = fwrite( recording->buffer.data, 1, (size_t)recording->buffer.size, f );+	fclose( f );+	return (int)written == recording->buffer.size;+}++b3Recording* b3LoadRecordingFromFile( const char* path )+{+	if ( path == NULL )+	{+		return NULL;+	}++	FILE* f = fopen( path, "rb" );+	if ( f == NULL )+	{+		return NULL;+	}++	if ( fseek( f, 0, SEEK_END ) != 0 )+	{+		fclose( f );+		return NULL;+	}++	long fileSize = ftell( f );+	// Anything smaller than the fixed header can't be a recording+	if ( fileSize < (long)sizeof( b3RecHeader ) || fileSize > INT_MAX )+	{+		fclose( f );+		return NULL;+	}+	fseek( f, 0, SEEK_SET );++	b3Recording* rec = b3CreateRecording( (int)fileSize );+	size_t readSize = fread( rec->buffer.data, 1, (size_t)fileSize, f );+	fclose( f );++	if ( (long)readSize != fileSize )+	{+		b3DestroyRecording( rec );+		return NULL;+	}++	// Validate magic so a wrong file fails at load rather than deep in the player+	b3RecHeader hdr;+	memcpy( &hdr, rec->buffer.data, sizeof( hdr ) );+	if ( hdr.magic != B3_REC_MAGIC )+	{+		b3DestroyRecording( rec );+		return NULL;+	}++	rec->buffer.size = (int)fileSize;+	return rec;+}++// Geometry interning helpers++uint32_t b3RecInternHull( b3Recording* rec, const b3HullData* hull )+{+	int byteCount = hull->byteCount;+	uint8_t* bytes = b3Alloc( (size_t)byteCount );+	memcpy( bytes, hull, (size_t)byteCount );+	uint64_t h = b3Hash64NonZero( bytes, byteCount );+	return b3InternGeometry( &rec->registry, b3_geometryHull, h, bytes, byteCount );+}++uint32_t b3RecInternMesh( b3Recording* rec, const b3MeshData* mesh )+{+	int byteCount = mesh->byteCount;+	uint8_t* bytes = b3Alloc( (size_t)byteCount );+	memcpy( bytes, mesh, (size_t)byteCount );+	uint64_t h = b3Hash64NonZero( bytes, byteCount );+	return b3InternGeometry( &rec->registry, b3_geometryMesh, h, bytes, byteCount );+}++uint32_t b3RecInternHeightField( b3Recording* rec, const b3HeightFieldData* hf )+{+	int byteCount = hf->byteCount;+	uint8_t* bytes = b3Alloc( (size_t)byteCount );+	memcpy( bytes, hf, (size_t)byteCount );+	uint64_t h = b3Hash64NonZero( bytes, byteCount );+	return b3InternGeometry( &rec->registry, b3_geometryHeightField, h, bytes, byteCount );+}++uint32_t b3RecInternCompound( b3Recording* rec, const b3CompoundData* compound )+{+	int byteCount = compound->byteCount;+	uint8_t* bytes = b3Alloc( (size_t)byteCount );+	memcpy( bytes, compound, (size_t)byteCount );+	// Null the tree node pointer in the copy so the canonical bytes are pointer-free.+	// b3ConvertBytesToCompound fixes it back on load via nodeOffset.+	( (b3CompoundData*)bytes )->tree.nodes = NULL;+	uint64_t h = b3Hash64NonZero( bytes, byteCount );+	return b3InternGeometry( &rec->registry, b3_geometryCompound, h, bytes, byteCount );+}++uint64_t b3HashWorldState( b3World* world )+{+	uint64_t hash = B3_SNAP_FNV_INIT;+	const uint64_t prime = B3_SNAP_FNV_PRIME;++	int bodyCount = world->bodies.count;+	for ( int i = 0; i < bodyCount; ++i )+	{+		b3Body* body = world->bodies.data + i;+		if ( body->id != i )+		{+			// Free or never-used slot+			continue;+		}++		b3BodySim* sim = b3GetBodySim( world, body );++		uint32_t bits;++#define B3_HASH_FLOAT( f )                                                                                                       \+	memcpy( &bits, &( f ), 4 );                                                                                                  \+	hash = ( hash ^ (uint64_t)bits ) * prime;++		hash = b3FnvMixPosition( hash, sim->transform.p );+		B3_HASH_FLOAT( sim->transform.q.v.x )+		B3_HASH_FLOAT( sim->transform.q.v.y )+		B3_HASH_FLOAT( sim->transform.q.v.z )+		B3_HASH_FLOAT( sim->transform.q.s )++		b3BodyState* state = b3GetBodyState( world, body );+		if ( state != NULL )+		{+			B3_HASH_FLOAT( state->linearVelocity.x )+			B3_HASH_FLOAT( state->linearVelocity.y )+			B3_HASH_FLOAT( state->linearVelocity.z )+			B3_HASH_FLOAT( state->angularVelocity.x )+			B3_HASH_FLOAT( state->angularVelocity.y )+			B3_HASH_FLOAT( state->angularVelocity.z )+		}++#undef B3_HASH_FLOAT+	}++	return hash;+}
+ upstream/src/recording.h view
@@ -0,0 +1,385 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "container.h"+#include "core.h"++#include "box3d/id.h"+#include "box3d/math_functions.h"+#include "box3d/types.h"++#include <stdbool.h>+#include <stdint.h>+#include <stdio.h>+#include <string.h>++// FNV-1a 64-bit constants+#define B3_SNAP_FNV_INIT 14695981039346656037ull+#define B3_SNAP_FNV_PRIME 1099511628211ull++// Mix a world position at full width so the determinism gate validates past float precision+// when the body is far from the origin.+static inline uint64_t b3FnvMixPosition( uint64_t hash, b3Pos p )+{+#if defined( BOX3D_DOUBLE_PRECISION )+	uint64_t bx, by, bz;+	memcpy( &bx, &p.x, 8 );+	memcpy( &by, &p.y, 8 );+	memcpy( &bz, &p.z, 8 );+#else+	uint32_t fx, fy, fz;+	memcpy( &fx, &p.x, 4 );+	memcpy( &fy, &p.y, 4 );+	memcpy( &fz, &p.z, 4 );+	uint64_t bx = fx, by = fy, bz = fz;+#endif+	hash = ( hash ^ bx ) * B3_SNAP_FNV_PRIME;+	hash = ( hash ^ by ) * B3_SNAP_FNV_PRIME;+	hash = ( hash ^ bz ) * B3_SNAP_FNV_PRIME;+	return hash;+}++typedef struct b3World b3World;++// Magic 'B3RC' in little-endian: bytes B(0x42) 3(0x33) R(0x52) C(0x43)+#define B3_REC_MAGIC 0x43523342u++// Major recording version is bumped when writers change.+// Major version 4 added b3ShapeDef::enableSpeculativeContact+#define B3_REC_VERSION_MAJOR 4++// Minor tracks op-stream additions that keep the 48 byte header shape.+// Minor version 4 added b3Shape_SetMeshMaterial, b3Shape_SetHull, b3Shape_SetMesh+#define B3_REC_VERSION_MINOR 4++// File header, fixed 48 bytes. Contains the registry locator so the player+// can load geometry before replaying any ops.+typedef struct b3RecHeader+{+	uint32_t magic;			   // 'B3RC' = 0x43523342+	uint16_t versionMajor;	   // B3_REC_VERSION_MAJOR+	uint16_t versionMinor;	   // B3_REC_VERSION_MINOR+	uint8_t pointerWidth;	   // sizeof(void*), gates POD-def struct layout+	uint8_t bigEndian;		   // 0 on all supported targets+	uint8_t validationEnabled; // 1 if built with BOX3D_VALIDATE, diagnostic only+	uint8_t reserved;+	float lengthScale; // b3GetLengthUnitsPerMeter()+	uint32_t reserved2;+	uint32_t reserved3;			// explicit pad so the 64-bit fields align with no implicit gap+	uint64_t snapshotSize;		// bytes of snapshot blob after the header (0 in Phase 1)+	uint64_t registryOffset;	// absolute offset to trailing registry block, backpatched at stop+	uint64_t registryByteCount; // size of the registry block+} b3RecHeader;++_Static_assert( sizeof( b3RecHeader ) == 48, "recording header must be 48 bytes" );++// Growable append-only byte buffer. Doubles on demand. countOnly mode tallies size without+// allocating, used to size a buffer cheaply before a second filling pass.+typedef struct b3RecBuffer+{+	uint8_t* data;+	int capacity;+	int size;+	bool countOnly;+} b3RecBuffer;++// Geometry kinds for the trailing registry section+typedef enum b3GeometryKind+{+	b3_geometryHull,+	b3_geometryMesh,+	b3_geometryHeightField,+	b3_geometryCompound,+} b3GeometryKind;++// One entry per unique geometry blob. id == index in the entries array.+// hashNext chains entries that share a content hash so dedup stays exact under a hash collision,+// which the keyframe registry depends on to never grow during capture. B3_NULL_INDEX ends the chain.+typedef struct b3GeometryEntry+{+	uint64_t contentHash;+	uint32_t id;+	b3GeometryKind kind;+	int byteCount;+	uint8_t* bytes;+	int hashNext;+} b3GeometryEntry;++b3DeclareArray( b3GeometryEntry );++// Growable array of geometry entries. Ids are array indices, so the array is serialized in order.+// dedupMap maps content hash to entry id for O(1) dedup; it is opaque here and owned by recording.c.+typedef struct b3GeometryRegistry+{+	b3Array( b3GeometryEntry ) entries;+	void* dedupMap;+} b3GeometryRegistry;++// Limit the maximum query name length to make recording simpler. Query names longer than this+// probably indicate a bug in user code.+#define B3_MAX_QUERY_NAME_LENGTH 64++// Query tag from b3QueryFilter.+// Stored once per key in the trailing block so a tagged query carries only the 8 byte key on the wire.+// Shared by the recorder (accumulate) and the player (load).+typedef struct b3RecTag+{+	// hash of (id, queryName)+	uint64_t key;+	uint64_t id;+	char queryName[B3_MAX_QUERY_NAME_LENGTH + 1];+} b3RecTag;++// User-owned recording buffer. The world appends into it while active; the host saves and+// destroys it. Opaque across the public API.+typedef struct b3Recording+{+	b3RecBuffer buffer;+	int recordStart; // offset of the 3-byte size field for u24 backpatch+	b3Mutex* lock;	 // serializes record writes from concurrent threads+	b3GeometryRegistry registry;++	// Interned query tags accumulated during capture, written to the tail of the registry block at stop.+	// tagMap maps a tag key to its index for O(1) dedup.+	b3RecTag* tags;+	int tagCount;+	int tagCapacity;+	void* tagMap;++	// Union of world bounds over every recorded step, written at stop.+	b3AABB accumulatedBounds;+	bool haveBounds;+} b3Recording;++// C type aliases per TAG, used in the X-macro codegen arg structs+typedef bool b3RecCType_BOOL;+typedef int32_t b3RecCType_I32;+typedef uint8_t b3RecCType_U8;+typedef uint16_t b3RecCType_U16;+typedef uint32_t b3RecCType_U32;+typedef uint64_t b3RecCType_U64;+typedef float b3RecCType_F32;+typedef double b3RecCType_F64;+typedef b3Vec3 b3RecCType_VEC3;+typedef b3Quat b3RecCType_QUAT;+typedef b3Transform b3RecCType_TRANSFORM;+typedef b3Pos b3RecCType_POSITION;+typedef b3WorldTransform b3RecCType_WORLDXF;+typedef b3Matrix3 b3RecCType_MATRIX3;+typedef b3AABB b3RecCType_AABB;+typedef b3Sphere b3RecCType_SPHERE;+typedef b3Capsule b3RecCType_CAPSULE;+typedef b3QueryFilter b3RecCType_QUERYFILTER;+typedef b3ShapeProxy b3RecCType_SHAPEPROXY;+// Geometry reference: a plain u32 id into the trailing registry+typedef uint32_t b3RecCType_GEOMID;+typedef b3Filter b3RecCType_FILTER;+typedef b3SurfaceMaterial b3RecCType_MATERIAL;+typedef b3MassData b3RecCType_MASSDATA;+typedef b3MotionLocks b3RecCType_LOCKS;+typedef const char* b3RecCType_STR;+typedef b3WorldId b3RecCType_WORLDID;+typedef b3BodyId b3RecCType_BODYID;+typedef b3ShapeId b3RecCType_SHAPEID;+typedef b3JointId b3RecCType_JOINTID;+typedef b3BodyDef b3RecCType_BODYDEF;+typedef b3ShapeDef b3RecCType_SHAPEDEF;+typedef b3ExplosionDef b3RecCType_EXPLOSIONDEF;+typedef b3ParallelJointDef b3RecCType_PARALLELJOINTDEF;+typedef b3DistanceJointDef b3RecCType_DISTANCEJOINTDEF;+typedef b3FilterJointDef b3RecCType_FILTERJOINTDEF;+typedef b3MotorJointDef b3RecCType_MOTORJOINTDEF;+typedef b3PrismaticJointDef b3RecCType_PRISMATICJOINTDEF;+typedef b3RevoluteJointDef b3RecCType_REVOLUTEJOINTDEF;+typedef b3SphericalJointDef b3RecCType_SPHERICALJOINTDEF;+typedef b3WeldJointDef b3RecCType_WELDJOINTDEF;+typedef b3WheelJointDef b3RecCType_WHEELJOINTDEF;++// Codegen pass 1a: arg structs. Generated here so call sites (body.c, shape.c, etc.) can see them.+#define ARG( TAG, field ) b3RecCType_##TAG field;+#define B3_REC_OP( op, Name, RET, ... )                                                                                          \+	typedef struct                                                                                                               \+	{                                                                                                                            \+		__VA_ARGS__                                                                                                              \+	} b3RecArgs_##Name;+#include "recording_ops.inl"+#undef B3_REC_OP+#undef ARG++// Opcode constants generated from the manifest, so call sites name an op instead of a raw byte and+// can't drift from the manifest if an op is renumbered.+enum+{+#define B3_REC_OP( op, Name, RET, ... ) b3_recOp##Name = ( op ),+#include "recording_ops.inl"+#undef B3_REC_OP+};++// Low-level buffer helpers+void b3RecBufAppend( b3RecBuffer* buf, const void* data, int size );+void b3RecBufFree( b3RecBuffer* buf );++// Write primitives+void b3RecW_U8( b3RecBuffer* buf, uint8_t v );+void b3RecW_U16( b3RecBuffer* buf, uint16_t v );+void b3RecW_U32( b3RecBuffer* buf, uint32_t v );+void b3RecW_U64( b3RecBuffer* buf, uint64_t v );+void b3RecW_I32( b3RecBuffer* buf, int32_t v );+void b3RecW_F32( b3RecBuffer* buf, float v );+void b3RecW_F64( b3RecBuffer* buf, double v );+void b3RecW_BOOL( b3RecBuffer* buf, bool v );+void b3RecW_VEC3( b3RecBuffer* buf, b3Vec3 v );+void b3RecW_QUAT( b3RecBuffer* buf, b3Quat v );+void b3RecW_TRANSFORM( b3RecBuffer* buf, b3Transform v );+// World position: doubles in large-world mode, floats otherwise (wire-identical to VEC3 in float build)+void b3RecW_POSITION( b3RecBuffer* buf, b3Pos v );+void b3RecW_WORLDXF( b3RecBuffer* buf, b3WorldTransform v );+void b3RecW_MATRIX3( b3RecBuffer* buf, b3Matrix3 v );+void b3RecW_AABB( b3RecBuffer* buf, b3AABB v );+void b3RecW_QUERYFILTER( b3RecBuffer* buf, b3QueryFilter v );+void b3RecW_SHAPEPROXY( b3RecBuffer* buf, b3ShapeProxy v );+void b3RecW_TREESTATS( b3RecBuffer* buf, b3TreeStats v );+void b3RecW_RAYRESULT( b3RecBuffer* buf, b3RayResult v );+void b3RecW_PLANERESULT( b3RecBuffer* buf, b3PlaneResult v );+void b3RecW_WORLDID( b3RecBuffer* buf, b3WorldId v );+void b3RecW_BODYID( b3RecBuffer* buf, b3BodyId v );+void b3RecW_SHAPEID( b3RecBuffer* buf, b3ShapeId v );+void b3RecW_JOINTID( b3RecBuffer* buf, b3JointId v );+void b3RecW_SPHERE( b3RecBuffer* buf, b3Sphere v );+void b3RecW_CAPSULE( b3RecBuffer* buf, b3Capsule v );+void b3RecW_GEOMID( b3RecBuffer* buf, uint32_t v );+void b3RecW_FILTER( b3RecBuffer* buf, b3Filter v );+void b3RecW_MATERIAL( b3RecBuffer* buf, b3SurfaceMaterial v );+void b3RecW_MASSDATA( b3RecBuffer* buf, b3MassData v );+void b3RecW_LOCKS( b3RecBuffer* buf, b3MotionLocks v );+void b3RecW_EXPLOSIONDEF( b3RecBuffer* buf, b3ExplosionDef v );+void b3RecW_BODYDEF( b3RecBuffer* buf, b3BodyDef v );+void b3RecW_SHAPEDEF( b3RecBuffer* buf, b3ShapeDef v );+void b3RecW_PARALLELJOINTDEF( b3RecBuffer* buf, b3ParallelJointDef v );+void b3RecW_DISTANCEJOINTDEF( b3RecBuffer* buf, b3DistanceJointDef v );+void b3RecW_FILTERJOINTDEF( b3RecBuffer* buf, b3FilterJointDef v );+void b3RecW_MOTORJOINTDEF( b3RecBuffer* buf, b3MotorJointDef v );+void b3RecW_PRISMATICJOINTDEF( b3RecBuffer* buf, b3PrismaticJointDef v );+void b3RecW_REVOLUTEJOINTDEF( b3RecBuffer* buf, b3RevoluteJointDef v );+void b3RecW_SPHERICALJOINTDEF( b3RecBuffer* buf, b3SphericalJointDef v );+void b3RecW_WELDJOINTDEF( b3RecBuffer* buf, b3WeldJointDef v );+void b3RecW_WHEELJOINTDEF( b3RecBuffer* buf, b3WheelJointDef v );++// Record framing+void b3RecBeginRecord( b3Recording* rec, uint8_t opcode );+void b3RecEndRecord( b3Recording* rec );++// Per-op arg writers (no framing) and full writers (framing + args), generated from the manifest.+#define B3_REC_OP( op, Name, RET, ... )                                                                                          \+	void b3RecWriteArgs_##Name( b3Recording* rec, const b3RecArgs_##Name* a );                                                   \+	void b3RecWrite_##Name( b3Recording* rec, const b3RecArgs_##Name* a );+#include "recording_ops.inl"+#undef B3_REC_OP++// Create ops: declare writers that also append the returned id inside the record.+#define B3_REC_RETDECL_RET_NONE( Name )+#define B3_REC_RETDECL_RET_BODYID( Name ) void b3RecWriteRet_##Name( b3Recording* rec, const b3RecArgs_##Name* a, b3BodyId id );+#define B3_REC_RETDECL_RET_SHAPEID( Name ) void b3RecWriteRet_##Name( b3Recording* rec, const b3RecArgs_##Name* a, b3ShapeId id );+#define B3_REC_RETDECL_RET_JOINTID( Name ) void b3RecWriteRet_##Name( b3Recording* rec, const b3RecArgs_##Name* a, b3JointId id );+#define B3_REC_OP( op, Name, RET, ... ) B3_REC_RETDECL_##RET( Name )+#include "recording_ops.inl"+#undef B3_REC_OP+#undef B3_REC_RETDECL_RET_NONE+#undef B3_REC_RETDECL_RET_BODYID+#undef B3_REC_RETDECL_RET_SHAPEID+#undef B3_REC_RETDECL_RET_JOINTID++// Record a void op. The branch is free when recording is off.+#define B3_REC( world, Name, ... )                                                                                               \+	do                                                                                                                           \+	{                                                                                                                            \+		if ( ( world )->recording != NULL )                                                                                      \+		{                                                                                                                        \+			b3RecArgs_##Name recArgs = { __VA_ARGS__ };                                                                          \+			b3RecWrite_##Name( ( world )->recording, &recArgs );                                                                 \+		}                                                                                                                        \+	}                                                                                                                            \+	while ( 0 )++// Record a create op and its returned id in one framed record. Place after the real create call.+#define B3_REC_CREATE( world, Name, id, ... )                                                                                    \+	do                                                                                                                           \+	{                                                                                                                            \+		if ( ( world )->recording != NULL )                                                                                      \+		{                                                                                                                        \+			b3RecArgs_##Name recCreateArgs = { __VA_ARGS__ };                                                                    \+			b3RecWriteRet_##Name( ( world )->recording, &recCreateArgs, id );                                                    \+		}                                                                                                                        \+	}                                                                                                                            \+	while ( 0 )++// Patch helpers for the query hit-count backfill+int b3RecReserveU32( b3RecBuffer* buf );+void b3RecPatchU32( b3RecBuffer* buf, int offset, uint32_t v );++// Commit a finished query record under the lock. The payload buffer stays owned by the caller.+void b3RecCommitRecord( b3Recording* rec, uint8_t opcode, const uint8_t* payload, int payloadSize );++// Per-query writer context: holds the user fcn+ctx, the local payload buffer, and the hit counter+typedef struct b3RecQueryWriter+{+	union+	{+		b3OverlapResultFcn* overlapFcn;+		b3CastResultFcn* castFcn;+		b3PlaneResultFcn* planeFcn;+		b3MoverFilterFcn* moverFilterFcn;+	} userFcn;+	void* userContext;+	b3RecBuffer buf; // per-call local payload, heap-backed+	int countOffset; // offset of the reserved u32 hit-count slot+	uint32_t hitCount;+	uint64_t tagId;		 // caller query id, 0 = untagged. Emitted as a QueryTag before the record.+	const char* tagName; // caller query name, interned by id. NULL = none.+} b3RecQueryWriter;++void b3RecQueryBegin( b3RecQueryWriter* w, void* context, uint64_t tagId, const char* tagName );+void b3RecQueryCommit( b3Recording* rec, uint8_t opcode, b3RecQueryWriter* w );++// Recording trampolines: replace the user fcn so hits are captured before dispatch. The overlap+// trampoline doubles for the mover filter, which has the same bool(shapeId, ctx) shape.+bool b3RecOverlapTrampoline( b3ShapeId id, void* ctx );+float b3RecCastTrampoline( b3ShapeId id, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, int triangleIndex,+						   int childIndex, void* ctx );+bool b3RecPlaneTrampoline( b3ShapeId id, const b3PlaneResult* planes, int planeCount, void* ctx );++// Geometry registry+uint32_t b3InternGeometry( b3GeometryRegistry* reg, b3GeometryKind kind, uint64_t contentHash, uint8_t* bytes, int byteCount );+// Append an entry unconditionally and return its id, which equals its array index. Unlike+// b3InternGeometry it never deduplicates, so the keyframe seed can mirror slots 1:1 even when an+// already-recorded file carries byte-identical duplicate slots (a hash collision wrote them apart).+uint32_t b3AppendGeometry( b3GeometryRegistry* reg, b3GeometryKind kind, uint64_t contentHash, uint8_t* bytes, int byteCount );+void b3FreeRegistry( b3GeometryRegistry* reg );+void b3RecWriteRegistry( b3Recording* rec );++// Hash a query (id, name) pair into the stable key the viewer tracks the query by. Never returns 0,+// so the key doubles as a tagged/untagged flag.+uint64_t b3HashQueryTag( uint64_t id, const char* name );++// Record a key->(id, name) mapping once, deduped by key (a repeated key keeps its first id/name).+void b3RecInternTag( b3Recording* rec, uint64_t key, uint64_t id, const char* name );++// Intern each large geometry kind and return a stable u32 id for use in create ops.+// Caller does NOT free bytes; b3InternGeometry takes ownership (frees on duplicate).+uint32_t b3RecInternHull( b3Recording* rec, const b3HullData* hull );+uint32_t b3RecInternMesh( b3Recording* rec, const b3MeshData* mesh );+uint32_t b3RecInternHeightField( b3Recording* rec, const b3HeightFieldData* hf );+uint32_t b3RecInternCompound( b3Recording* rec, const b3CompoundData* compound );++// Lifecycle engine-side hooks+void b3StartRecordingIntoBuffer( b3World* world, b3Recording* recording );+void b3StopRecordingInternal( b3World* world );++// Fold one step's world bounds into the running union.+void b3RecAccumulateBounds( b3Recording* rec, b3AABB bounds );++// Deterministic hash over all body transforms and velocities.+// Called by both recorder and replayer to verify simulation reproduces exactly.+uint64_t b3HashWorldState( b3World* world );
+ upstream/src/recording_ops.inl view
@@ -0,0 +1,235 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++// X-macro manifest for the recording system.+// Include with B3_REC_OP and ARG defined. No commas between ARG tokens.+//+// B3_REC_OP( opcode, Name, RET, ARGS )+//   RET in { RET_NONE, RET_BODYID, RET_SHAPEID, RET_JOINTID }+//   ARGS = zero or more ARG( TAG, fieldName ) tokens, NO commas between them+//+// Opcode ranges:+//   0x00-0x0F  world lifecycle and config+//   0x10-0x1F  body create/destroy+//   0x20-0x3F  body mutators+//   0x40-0x4F  shape create/destroy+//   0x50-0x6F  shape mutators+//   0x80       step+//   0x90-0xE7  joints (create, generic, per-type)+//   0xE8-0xEF  spatial queries+//   0xF0-0xFF  markers++// Recordings are seeded with a world snapshot, not a CreateWorld op. DestroyWorld stays as the+// end-of-session marker the viewer reads.+B3_REC_OP( 0x01, DestroyWorld, RET_NONE, ARG( WORLDID, world ) )+B3_REC_OP( 0x80, Step, RET_NONE, ARG( WORLDID, world ) ARG( F32, dt ) ARG( I32, subStepCount ) )++// World config. The world arg is informational; replay always targets its own world.+B3_REC_OP( 0x02, WorldEnableSleeping, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) )+B3_REC_OP( 0x03, WorldEnableContinuous, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) )+B3_REC_OP( 0x04, WorldSetRestitutionThreshold, RET_NONE, ARG( WORLDID, world ) ARG( F32, value ) )+B3_REC_OP( 0x05, WorldSetHitEventThreshold, RET_NONE, ARG( WORLDID, world ) ARG( F32, value ) )+B3_REC_OP( 0x06, WorldSetGravity, RET_NONE, ARG( WORLDID, world ) ARG( VEC3, gravity ) )+B3_REC_OP( 0x07, WorldExplode, RET_NONE, ARG( WORLDID, world ) ARG( EXPLOSIONDEF, def ) )+B3_REC_OP( 0x08, WorldSetContactTuning, RET_NONE,+		   ARG( WORLDID, world ) ARG( F32, hertz ) ARG( F32, dampingRatio ) ARG( F32, contactSpeed ) )+B3_REC_OP( 0x09, WorldSetContactRecycleDistance, RET_NONE, ARG( WORLDID, world ) ARG( F32, recycleDistance ) )+B3_REC_OP( 0x0A, WorldSetMaximumLinearSpeed, RET_NONE, ARG( WORLDID, world ) ARG( F32, maximumLinearSpeed ) )+B3_REC_OP( 0x0B, WorldEnableWarmStarting, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) )+B3_REC_OP( 0x0C, WorldRebuildStaticTree, RET_NONE, ARG( WORLDID, world ) )+B3_REC_OP( 0x0D, WorldEnableSpeculative, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) )++// Body+B3_REC_OP( 0x10, CreateBody, RET_BODYID, ARG( WORLDID, world ) ARG( BODYDEF, def ) )+B3_REC_OP( 0x11, DestroyBody, RET_NONE, ARG( BODYID, body ) )+B3_REC_OP( 0x20, BodySetTransform, RET_NONE, ARG( BODYID, body ) ARG( POSITION, position ) ARG( QUAT, rotation ) )+B3_REC_OP( 0x21, BodySetLinearVelocity, RET_NONE, ARG( BODYID, body ) ARG( VEC3, v ) )+B3_REC_OP( 0x22, BodySetType, RET_NONE, ARG( BODYID, body ) ARG( I32, type ) )+B3_REC_OP( 0x23, BodySetName, RET_NONE, ARG( BODYID, body ) ARG( STR, name ) )+B3_REC_OP( 0x24, BodySetAngularVelocity, RET_NONE, ARG( BODYID, body ) ARG( VEC3, w ) )+B3_REC_OP( 0x25, BodySetTargetTransform, RET_NONE, ARG( BODYID, body ) ARG( WORLDXF, target ) ARG( F32, timeStep ) ARG( BOOL, wake ) )+B3_REC_OP( 0x26, BodyApplyForce, RET_NONE, ARG( BODYID, body ) ARG( VEC3, force ) ARG( POSITION, point ) ARG( BOOL, wake ) )+B3_REC_OP( 0x27, BodyApplyForceToCenter, RET_NONE, ARG( BODYID, body ) ARG( VEC3, force ) ARG( BOOL, wake ) )+B3_REC_OP( 0x28, BodyApplyTorque, RET_NONE, ARG( BODYID, body ) ARG( VEC3, torque ) ARG( BOOL, wake ) )+B3_REC_OP( 0x29, BodyApplyLinearImpulse, RET_NONE, ARG( BODYID, body ) ARG( VEC3, impulse ) ARG( POSITION, point ) ARG( BOOL, wake ) )+B3_REC_OP( 0x2A, BodyApplyLinearImpulseToCenter, RET_NONE, ARG( BODYID, body ) ARG( VEC3, impulse ) ARG( BOOL, wake ) )+B3_REC_OP( 0x2B, BodyApplyAngularImpulse, RET_NONE, ARG( BODYID, body ) ARG( VEC3, impulse ) ARG( BOOL, wake ) )+B3_REC_OP( 0x2C, BodySetMassData, RET_NONE, ARG( BODYID, body ) ARG( MASSDATA, massData ) )+B3_REC_OP( 0x2D, BodyApplyMassFromShapes, RET_NONE, ARG( BODYID, body ) )+B3_REC_OP( 0x2E, BodySetLinearDamping, RET_NONE, ARG( BODYID, body ) ARG( F32, damping ) )+B3_REC_OP( 0x2F, BodySetAngularDamping, RET_NONE, ARG( BODYID, body ) ARG( F32, damping ) )+B3_REC_OP( 0x30, BodySetGravityScale, RET_NONE, ARG( BODYID, body ) ARG( F32, scale ) )+B3_REC_OP( 0x31, BodySetAwake, RET_NONE, ARG( BODYID, body ) ARG( BOOL, awake ) )+B3_REC_OP( 0x32, BodyEnableSleep, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) )+B3_REC_OP( 0x33, BodySetSleepThreshold, RET_NONE, ARG( BODYID, body ) ARG( F32, threshold ) )+B3_REC_OP( 0x34, BodyDisable, RET_NONE, ARG( BODYID, body ) )+B3_REC_OP( 0x35, BodyEnable, RET_NONE, ARG( BODYID, body ) )+B3_REC_OP( 0x36, BodySetMotionLocks, RET_NONE, ARG( BODYID, body ) ARG( LOCKS, locks ) )+B3_REC_OP( 0x37, BodySetBullet, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) )+B3_REC_OP( 0x38, BodyEnableContactRecycling, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) )+B3_REC_OP( 0x39, BodyEnableHitEvents, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) )+B3_REC_OP( 0x3A, BodyAllowFastRotation, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) )++// Shape create/destroy+B3_REC_OP( 0x40, CreateSphereShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( SPHERE, sphere ) )+B3_REC_OP( 0x41, CreateCapsuleShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( CAPSULE, capsule ) )+B3_REC_OP( 0x42, CreateHullShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( GEOMID, geometryId ) )+B3_REC_OP( 0x43, CreateMeshShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( GEOMID, geometryId ) ARG( VEC3, scale ) )+B3_REC_OP( 0x44, CreateHeightFieldShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( GEOMID, geometryId ) )+B3_REC_OP( 0x45, CreateCompoundShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( GEOMID, geometryId ) )+B3_REC_OP( 0x46, DestroyShape, RET_NONE, ARG( SHAPEID, shape ) ARG( BOOL, updateBodyMass ) )++// Shape mutators+B3_REC_OP( 0x50, ShapeSetDensity, RET_NONE, ARG( SHAPEID, shape ) ARG( F32, density ) ARG( BOOL, updateBodyMass ) )+B3_REC_OP( 0x51, ShapeSetFriction, RET_NONE, ARG( SHAPEID, shape ) ARG( F32, friction ) )+B3_REC_OP( 0x52, ShapeSetRestitution, RET_NONE, ARG( SHAPEID, shape ) ARG( F32, restitution ) )+B3_REC_OP( 0x53, ShapeSetSurfaceMaterial, RET_NONE, ARG( SHAPEID, shape ) ARG( MATERIAL, material ) )+B3_REC_OP( 0x54, ShapeSetFilter, RET_NONE, ARG( SHAPEID, shape ) ARG( FILTER, filter ) ARG( BOOL, invokeContacts ) )+B3_REC_OP( 0x55, ShapeEnableSensorEvents, RET_NONE, ARG( SHAPEID, shape ) ARG( BOOL, flag ) )+B3_REC_OP( 0x56, ShapeEnableContactEvents, RET_NONE, ARG( SHAPEID, shape ) ARG( BOOL, flag ) )+B3_REC_OP( 0x57, ShapeEnablePreSolveEvents, RET_NONE, ARG( SHAPEID, shape ) ARG( BOOL, flag ) )+B3_REC_OP( 0x58, ShapeEnableHitEvents, RET_NONE, ARG( SHAPEID, shape ) ARG( BOOL, flag ) )+B3_REC_OP( 0x59, ShapeSetSphere, RET_NONE, ARG( SHAPEID, shape ) ARG( SPHERE, sphere ) )+B3_REC_OP( 0x5A, ShapeSetCapsule, RET_NONE, ARG( SHAPEID, shape ) ARG( CAPSULE, capsule ) )+B3_REC_OP( 0x5B, ShapeApplyWind, RET_NONE, ARG( SHAPEID, shape ) ARG( VEC3, wind ) ARG( F32, drag ) ARG( F32, lift ) ARG( F32, maxSpeed ) ARG( BOOL, wake ) )+B3_REC_OP( 0x5C, ShapeSetName, RET_NONE, ARG( SHAPEID, shape ) ARG( STR, name ) )+B3_REC_OP( 0x5D, ShapeSetMeshMaterial, RET_NONE, ARG( SHAPEID, shape ) ARG( MATERIAL, material ) ARG( I32, index ) )+B3_REC_OP( 0x5E, ShapeSetHull, RET_NONE, ARG( SHAPEID, shape ) ARG( GEOMID, geometryId ) )+B3_REC_OP( 0x5F, ShapeSetMesh, RET_NONE, ARG( SHAPEID, shape ) ARG( GEOMID, geometryId ) ARG( VEC3, scale ) )++// Joint create and destroy+B3_REC_OP( 0x90, CreateParallelJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( PARALLELJOINTDEF, def ) )+B3_REC_OP( 0x91, CreateDistanceJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( DISTANCEJOINTDEF, def ) )+B3_REC_OP( 0x92, CreateFilterJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( FILTERJOINTDEF, def ) )+B3_REC_OP( 0x93, CreateMotorJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( MOTORJOINTDEF, def ) )+B3_REC_OP( 0x94, CreatePrismaticJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( PRISMATICJOINTDEF, def ) )+B3_REC_OP( 0x95, CreateRevoluteJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( REVOLUTEJOINTDEF, def ) )+B3_REC_OP( 0x96, CreateSphericalJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( SPHERICALJOINTDEF, def ) )+B3_REC_OP( 0x97, CreateWeldJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( WELDJOINTDEF, def ) )+B3_REC_OP( 0x98, CreateWheelJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( WHEELJOINTDEF, def ) )+B3_REC_OP( 0x99, DestroyJoint, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, wakeAttached ) )++// Generic joint mutators+B3_REC_OP( 0x9A, JointSetLocalFrameA, RET_NONE, ARG( JOINTID, joint ) ARG( TRANSFORM, localFrame ) )+B3_REC_OP( 0x9B, JointSetLocalFrameB, RET_NONE, ARG( JOINTID, joint ) ARG( TRANSFORM, localFrame ) )+B3_REC_OP( 0x9C, JointSetCollideConnected, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, shouldCollide ) )+B3_REC_OP( 0x9D, JointWakeBodies, RET_NONE, ARG( JOINTID, joint ) )+B3_REC_OP( 0x9E, JointSetConstraintTuning, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ARG( F32, dampingRatio ) )+B3_REC_OP( 0x9F, JointSetForceThreshold, RET_NONE, ARG( JOINTID, joint ) ARG( F32, threshold ) )+B3_REC_OP( 0xA0, JointSetTorqueThreshold, RET_NONE, ARG( JOINTID, joint ) ARG( F32, threshold ) )++// Parallel joint+B3_REC_OP( 0xA1, ParallelJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )+B3_REC_OP( 0xA2, ParallelJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )+B3_REC_OP( 0xA3, ParallelJointSetMaxTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxTorque ) )++// Distance joint+B3_REC_OP( 0xA4, DistanceJointSetLength, RET_NONE, ARG( JOINTID, joint ) ARG( F32, length ) )+B3_REC_OP( 0xA5, DistanceJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) )+B3_REC_OP( 0xA6, DistanceJointSetSpringForceRange, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lowerForce ) ARG( F32, upperForce ) )+B3_REC_OP( 0xA7, DistanceJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )+B3_REC_OP( 0xA8, DistanceJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )+B3_REC_OP( 0xA9, DistanceJointEnableLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) )+B3_REC_OP( 0xAA, DistanceJointSetLengthRange, RET_NONE, ARG( JOINTID, joint ) ARG( F32, minLength ) ARG( F32, maxLength ) )+B3_REC_OP( 0xAB, DistanceJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) )+B3_REC_OP( 0xAC, DistanceJointSetMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, motorSpeed ) )+B3_REC_OP( 0xAD, DistanceJointSetMaxMotorForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, force ) )++// Motor joint+B3_REC_OP( 0xAE, MotorJointSetLinearVelocity, RET_NONE, ARG( JOINTID, joint ) ARG( VEC3, velocity ) )+B3_REC_OP( 0xAF, MotorJointSetAngularVelocity, RET_NONE, ARG( JOINTID, joint ) ARG( VEC3, velocity ) )+B3_REC_OP( 0xB0, MotorJointSetMaxVelocityForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxForce ) )+B3_REC_OP( 0xB1, MotorJointSetMaxVelocityTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxTorque ) )+B3_REC_OP( 0xB2, MotorJointSetLinearHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )+B3_REC_OP( 0xB3, MotorJointSetLinearDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, damping ) )+B3_REC_OP( 0xB4, MotorJointSetAngularHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )+B3_REC_OP( 0xB5, MotorJointSetAngularDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, damping ) )+B3_REC_OP( 0xB6, MotorJointSetMaxSpringForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxForce ) )+B3_REC_OP( 0xB7, MotorJointSetMaxSpringTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxTorque ) )++// Prismatic joint+B3_REC_OP( 0xB8, PrismaticJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) )+B3_REC_OP( 0xB9, PrismaticJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )+B3_REC_OP( 0xBA, PrismaticJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )+B3_REC_OP( 0xBB, PrismaticJointSetTargetTranslation, RET_NONE, ARG( JOINTID, joint ) ARG( F32, translation ) )+B3_REC_OP( 0xBC, PrismaticJointEnableLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) )+B3_REC_OP( 0xBD, PrismaticJointSetLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) )+B3_REC_OP( 0xBE, PrismaticJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) )+B3_REC_OP( 0xBF, PrismaticJointSetMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, motorSpeed ) )+B3_REC_OP( 0xC0, PrismaticJointSetMaxMotorForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, force ) )++// Revolute joint+B3_REC_OP( 0xC1, RevoluteJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) )+B3_REC_OP( 0xC2, RevoluteJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )+B3_REC_OP( 0xC3, RevoluteJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )+B3_REC_OP( 0xC4, RevoluteJointSetTargetAngle, RET_NONE, ARG( JOINTID, joint ) ARG( F32, angle ) )+B3_REC_OP( 0xC5, RevoluteJointEnableLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) )+B3_REC_OP( 0xC6, RevoluteJointSetLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) )+B3_REC_OP( 0xC7, RevoluteJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) )+B3_REC_OP( 0xC8, RevoluteJointSetMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, motorSpeed ) )+B3_REC_OP( 0xC9, RevoluteJointSetMaxMotorTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, torque ) )++// Spherical joint+B3_REC_OP( 0xCA, SphericalJointEnableConeLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) )+B3_REC_OP( 0xCB, SphericalJointSetConeLimit, RET_NONE, ARG( JOINTID, joint ) ARG( F32, angleRadians ) )+B3_REC_OP( 0xCC, SphericalJointEnableTwistLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) )+B3_REC_OP( 0xCD, SphericalJointSetTwistLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) )+B3_REC_OP( 0xCE, SphericalJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) )+B3_REC_OP( 0xCF, SphericalJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )+B3_REC_OP( 0xD0, SphericalJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )+B3_REC_OP( 0xD1, SphericalJointSetTargetRotation, RET_NONE, ARG( JOINTID, joint ) ARG( QUAT, targetRotation ) )+B3_REC_OP( 0xD2, SphericalJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) )+B3_REC_OP( 0xD3, SphericalJointSetMotorVelocity, RET_NONE, ARG( JOINTID, joint ) ARG( VEC3, motorVelocity ) )+B3_REC_OP( 0xD4, SphericalJointSetMaxMotorTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, torque ) )++// Weld joint+B3_REC_OP( 0xD5, WeldJointSetLinearHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )+B3_REC_OP( 0xD6, WeldJointSetLinearDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )+B3_REC_OP( 0xD7, WeldJointSetAngularHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )+B3_REC_OP( 0xD8, WeldJointSetAngularDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )++// Wheel joint+B3_REC_OP( 0xD9, WheelJointEnableSuspension, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, flag ) )+B3_REC_OP( 0xDA, WheelJointSetSuspensionHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )+B3_REC_OP( 0xDB, WheelJointSetSuspensionDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )+B3_REC_OP( 0xDC, WheelJointEnableSuspensionLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, flag ) )+B3_REC_OP( 0xDD, WheelJointSetSuspensionLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) )+B3_REC_OP( 0xDE, WheelJointEnableSpinMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, flag ) )+B3_REC_OP( 0xDF, WheelJointSetSpinMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, speed ) )++// Wheel joint continued, overflow past the 0xDF range.+B3_REC_OP( 0xE0, WheelJointSetMaxSpinTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, torque ) )+B3_REC_OP( 0xE1, WheelJointEnableSteering, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, flag ) )+B3_REC_OP( 0xE2, WheelJointSetSteeringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )+B3_REC_OP( 0xE3, WheelJointSetSteeringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )+B3_REC_OP( 0xE4, WheelJointSetMaxSteeringTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, torque ) )+B3_REC_OP( 0xE5, WheelJointEnableSteeringLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, flag ) )+B3_REC_OP( 0xE6, WheelJointSetSteeringLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) )+B3_REC_OP( 0xE7, WheelJointSetTargetSteeringAngle, RET_NONE, ARG( JOINTID, joint ) ARG( F32, radians ) )++// Spatial queries. Inputs flow through the manifest (reader side). The hit tail and result are+// hand-written in recording.c / recording_replay.c since they are variable length.+B3_REC_OP( 0xE8, QueryOverlapAABB, RET_NONE, ARG( WORLDID, world ) ARG( AABB, aabb ) ARG( QUERYFILTER, filter ) )+B3_REC_OP( 0xE9, QueryOverlapShape, RET_NONE,+		   ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( SHAPEPROXY, proxy ) ARG( QUERYFILTER, filter ) )+B3_REC_OP( 0xEA, QueryCastRay, RET_NONE,+		   ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( VEC3, translation ) ARG( QUERYFILTER, filter ) )+B3_REC_OP( 0xEB, QueryCastShape, RET_NONE,+		   ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( SHAPEPROXY, proxy ) ARG( VEC3, translation )+			   ARG( QUERYFILTER, filter ) )+B3_REC_OP( 0xEC, QueryCastRayClosest, RET_NONE,+		   ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( VEC3, translation ) ARG( QUERYFILTER, filter ) )+B3_REC_OP( 0xED, QueryCastMover, RET_NONE,+		   ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( CAPSULE, mover ) ARG( VEC3, translation ) ARG( QUERYFILTER, filter ) )+B3_REC_OP( 0xEE, QueryCollideMover, RET_NONE,+		   ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( CAPSULE, mover ) ARG( QUERYFILTER, filter ) )++// Identity key (hash of the caller id + label) for the query that immediately follows. Emitted only+// for a tagged query. The id and label are interned in the trailing tag table, so only the 8 byte key+// rides the stream.+B3_REC_OP( 0xEF, QueryTag, RET_NONE, ARG( U64, key ) )++B3_REC_OP( 0xF1, StateHash, RET_NONE, ARG( WORLDID, world ) ARG( U64, hash ) )++// Accumulated world bounds over the whole recording, written once at stop.+B3_REC_OP( 0xF2, RecordingBounds, RET_NONE, ARG( AABB, bounds ) )
+ upstream/src/recording_replay.c view
@@ -0,0 +1,3672 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS )+#define _CRT_SECURE_NO_WARNINGS+#endif++#include "recording_replay.h"++#include "body.h"+#include "physics_world.h"+#include "world_snapshot.h"++#include "box3d/box3d.h"++#include <inttypes.h>+#include <limits.h>+#include <stdio.h>+#include <string.h>++// Read primitives++static void b3RecRdrCheck( b3RecReader* rdr, int size )+{+	if ( size < 0 || (int64_t)rdr->cursor + (int64_t)size > (int64_t)rdr->size )+	{+		rdr->ok = false;+	}+}++static void b3RecRdrBlob( b3RecReader* rdr, void* out, int size )+{+	b3RecRdrCheck( rdr, size );+	if ( !rdr->ok )+	{+		memset( out, 0, (size_t)size );+		return;+	}+	memcpy( out, rdr->data + rdr->cursor, (size_t)size );+	rdr->cursor += size;+}++uint8_t b3RecR_U8( b3RecReader* rdr )+{+	b3RecRdrCheck( rdr, 1 );+	if ( !rdr->ok )+	{+		return 0;+	}+	return rdr->data[rdr->cursor++];+}++uint16_t b3RecR_U16( b3RecReader* rdr )+{+	b3RecRdrCheck( rdr, 2 );+	if ( !rdr->ok )+	{+		return 0;+	}+	uint16_t v = (uint16_t)rdr->data[rdr->cursor] | ( (uint16_t)rdr->data[rdr->cursor + 1] << 8 );+	rdr->cursor += 2;+	return v;+}++uint32_t b3RecR_U24( b3RecReader* rdr )+{+	b3RecRdrCheck( rdr, 3 );+	if ( !rdr->ok )+	{+		return 0;+	}+	uint32_t v = (uint32_t)rdr->data[rdr->cursor] | ( (uint32_t)rdr->data[rdr->cursor + 1] << 8 ) |+				 ( (uint32_t)rdr->data[rdr->cursor + 2] << 16 );+	rdr->cursor += 3;+	return v;+}++uint32_t b3RecR_U32( b3RecReader* rdr )+{+	b3RecRdrCheck( rdr, 4 );+	if ( !rdr->ok )+	{+		return 0;+	}+	uint32_t v = (uint32_t)rdr->data[rdr->cursor] | ( (uint32_t)rdr->data[rdr->cursor + 1] << 8 ) |+				 ( (uint32_t)rdr->data[rdr->cursor + 2] << 16 ) | ( (uint32_t)rdr->data[rdr->cursor + 3] << 24 );+	rdr->cursor += 4;+	return v;+}++uint64_t b3RecR_U64( b3RecReader* rdr )+{+	b3RecRdrCheck( rdr, 8 );+	if ( !rdr->ok )+	{+		return 0;+	}+	uint64_t v = (uint64_t)rdr->data[rdr->cursor] | ( (uint64_t)rdr->data[rdr->cursor + 1] << 8 ) |+				 ( (uint64_t)rdr->data[rdr->cursor + 2] << 16 ) | ( (uint64_t)rdr->data[rdr->cursor + 3] << 24 ) |+				 ( (uint64_t)rdr->data[rdr->cursor + 4] << 32 ) | ( (uint64_t)rdr->data[rdr->cursor + 5] << 40 ) |+				 ( (uint64_t)rdr->data[rdr->cursor + 6] << 48 ) | ( (uint64_t)rdr->data[rdr->cursor + 7] << 56 );+	rdr->cursor += 8;+	return v;+}++int32_t b3RecR_I32( b3RecReader* rdr )+{+	return (int32_t)b3RecR_U32( rdr );+}++float b3RecR_F32( b3RecReader* rdr )+{+	uint32_t bits = b3RecR_U32( rdr );+	float v;+	memcpy( &v, &bits, 4 );+	return v;+}++double b3RecR_F64( b3RecReader* rdr )+{+	uint64_t bits = b3RecR_U64( rdr );+	double v;+	memcpy( &v, &bits, 8 );+	return v;+}++bool b3RecR_BOOL( b3RecReader* rdr )+{+	return b3RecR_U8( rdr ) != 0u;+}++b3Vec3 b3RecR_VEC3( b3RecReader* rdr )+{+	b3Vec3 v;+	v.x = b3RecR_F32( rdr );+	v.y = b3RecR_F32( rdr );+	v.z = b3RecR_F32( rdr );+	return v;+}++b3Quat b3RecR_QUAT( b3RecReader* rdr )+{+	b3Quat q;+	q.v.x = b3RecR_F32( rdr );+	q.v.y = b3RecR_F32( rdr );+	q.v.z = b3RecR_F32( rdr );+	q.s = b3RecR_F32( rdr );+	return q;+}++b3Transform b3RecR_TRANSFORM( b3RecReader* rdr )+{+	b3Transform t;+	t.p = b3RecR_VEC3( rdr );+	t.q = b3RecR_QUAT( rdr );+	return t;+}++b3Pos b3RecR_POSITION( b3RecReader* rdr )+{+	b3Pos p;+#if defined( BOX3D_DOUBLE_PRECISION )+	p.x = b3RecR_F64( rdr );+	p.y = b3RecR_F64( rdr );+	p.z = b3RecR_F64( rdr );+#else+	p.x = b3RecR_F32( rdr );+	p.y = b3RecR_F32( rdr );+	p.z = b3RecR_F32( rdr );+#endif+	return p;+}++b3WorldTransform b3RecR_WORLDXF( b3RecReader* rdr )+{+	b3WorldTransform t;+	t.p = b3RecR_POSITION( rdr );+	t.q = b3RecR_QUAT( rdr );+	return t;+}++b3Matrix3 b3RecR_MATRIX3( b3RecReader* rdr )+{+	b3Matrix3 m;+	m.cx = b3RecR_VEC3( rdr );+	m.cy = b3RecR_VEC3( rdr );+	m.cz = b3RecR_VEC3( rdr );+	return m;+}++b3AABB b3RecR_AABB( b3RecReader* rdr )+{+	b3AABB v;+	v.lowerBound = b3RecR_VEC3( rdr );+	v.upperBound = b3RecR_VEC3( rdr );+	return v;+}++b3QueryFilter b3RecR_QUERYFILTER( b3RecReader* rdr )+{+	// id and name are not on the wire here, they ride the separate QueryTag op. Start from the default+	// so they keep the untagged sentinel instead of garbage.+	b3QueryFilter f = b3DefaultQueryFilter();+	f.categoryBits = b3RecR_U64( rdr );+	f.maskBits = b3RecR_U64( rdr );+	return f;+}++// Reserve reader scratch for a count taken from an untrusted file. Every recorded element+// consumes at least one byte, so a valid count can never exceed the bytes left in the file.+// Reject anything larger (or negative, or that would overflow the byte size) by failing the read+// rather than allocating wildly. A grow keeps the old contents so callers can accumulate across+// reserves, as the collide-mover dispatcher does one shape group at a time.+static bool b3RecReserveScratch( b3RecReader* rdr, void** data, int* cap, int need, int elemSize )+{+	int remaining = rdr->size - rdr->cursor;+	if ( need < 0 || remaining < 0 || need > remaining || need > INT_MAX / elemSize )+	{+		rdr->ok = false;+		return false;+	}+	if ( need <= *cap )+	{+		return true;+	}+	int newCap = need <= INT_MAX / elemSize - 8 ? need + 8 : need;+	void* grown = b3Alloc( (size_t)newCap * (size_t)elemSize );+	if ( *data != NULL )+	{+		memcpy( grown, *data, (size_t)*cap * (size_t)elemSize );+		b3Free( *data, (size_t)*cap * (size_t)elemSize );+	}+	*data = grown;+	*cap = newCap;+	return true;+}++// Variable length, mirrors b3RecW_SHAPEPROXY: count, count points, radius. The decoded proxy borrows+// the reader's scratch for its point cloud, valid until the next proxy read.+b3ShapeProxy b3RecR_SHAPEPROXY( b3RecReader* rdr )+{+	b3ShapeProxy p = { 0 };+	int count = b3RecR_I32( rdr );+	if ( count < 0 )+		count = 0;+	if ( count > B3_MAX_SHAPE_CAST_POINTS )+		count = B3_MAX_SHAPE_CAST_POINTS;+	if ( count > 0 &&+		 b3RecReserveScratch( rdr, (void**)&rdr->proxyScratch, &rdr->proxyScratchCap, count, (int)sizeof( b3Vec3 ) ) )+	{+		for ( int i = 0; i < count; ++i )+		{+			rdr->proxyScratch[i] = b3RecR_VEC3( rdr );+		}+		p.points = rdr->proxyScratch;+		p.count = count;+	}+	p.radius = b3RecR_F32( rdr );+	return p;+}++b3TreeStats b3RecR_TREESTATS( b3RecReader* rdr )+{+	b3TreeStats v;+	v.nodeVisits = b3RecR_I32( rdr );+	v.leafVisits = b3RecR_I32( rdr );+	return v;+}++b3RayResult b3RecR_RAYRESULT( b3RecReader* rdr )+{+	b3RayResult v = { 0 };+	// shapeId keeps the recorded world0; b3RecMakeShapeId is applied at compare time+	v.shapeId = b3RecR_SHAPEID( rdr );+	v.point = b3RecR_POSITION( rdr );+	v.normal = b3RecR_VEC3( rdr );+	v.userMaterialId = b3RecR_U64( rdr );+	v.fraction = b3RecR_F32( rdr );+	v.triangleIndex = b3RecR_I32( rdr );+	v.childIndex = b3RecR_I32( rdr );+	v.hit = b3RecR_BOOL( rdr );+	return v;+}++b3PlaneResult b3RecR_PLANERESULT( b3RecReader* rdr )+{+	b3PlaneResult v;+	v.plane.normal = b3RecR_VEC3( rdr );+	v.plane.offset = b3RecR_F32( rdr );+	v.point = b3RecR_VEC3( rdr );+	return v;+}++b3WorldId b3RecR_WORLDID( b3RecReader* rdr )+{+	return b3LoadWorldId( b3RecR_U32( rdr ) );+}++b3BodyId b3RecR_BODYID( b3RecReader* rdr )+{+	return b3LoadBodyId( b3RecR_U64( rdr ) );+}++b3ShapeId b3RecR_SHAPEID( b3RecReader* rdr )+{+	return b3LoadShapeId( b3RecR_U64( rdr ) );+}++b3JointId b3RecR_JOINTID( b3RecReader* rdr )+{+	return b3LoadJointId( b3RecR_U64( rdr ) );+}++b3Sphere b3RecR_SPHERE( b3RecReader* rdr )+{+	b3Sphere s;+	b3RecRdrBlob( rdr, &s, (int)sizeof( s ) );+	return s;+}++b3Capsule b3RecR_CAPSULE( b3RecReader* rdr )+{+	b3Capsule c;+	b3RecRdrBlob( rdr, &c, (int)sizeof( c ) );+	return c;+}++uint32_t b3RecR_GEOMID( b3RecReader* rdr )+{+	return b3RecR_U32( rdr );+}++b3Filter b3RecR_FILTER( b3RecReader* rdr )+{+	b3Filter f;+	f.categoryBits = b3RecR_U64( rdr );+	f.maskBits = b3RecR_U64( rdr );+	f.groupIndex = b3RecR_I32( rdr );+	return f;+}++b3SurfaceMaterial b3RecR_MATERIAL( b3RecReader* rdr )+{+	b3SurfaceMaterial m = b3DefaultSurfaceMaterial();+	m.friction = b3RecR_F32( rdr );+	m.restitution = b3RecR_F32( rdr );+	m.rollingResistance = b3RecR_F32( rdr );+	m.tangentVelocity = b3RecR_VEC3( rdr );+	m.userMaterialId = b3RecR_U64( rdr );+	m.customColor = b3RecR_U32( rdr );+	return m;+}++b3MassData b3RecR_MASSDATA( b3RecReader* rdr )+{+	b3MassData md;+	md.mass = b3RecR_F32( rdr );+	md.center = b3RecR_VEC3( rdr );+	md.inertia = b3RecR_MATRIX3( rdr );+	return md;+}++b3MotionLocks b3RecR_LOCKS( b3RecReader* rdr )+{+	b3MotionLocks locks;+	locks.linearX = b3RecR_BOOL( rdr );+	locks.linearY = b3RecR_BOOL( rdr );+	locks.linearZ = b3RecR_BOOL( rdr );+	locks.angularX = b3RecR_BOOL( rdr );+	locks.angularY = b3RecR_BOOL( rdr );+	locks.angularZ = b3RecR_BOOL( rdr );+	return locks;+}++// Rotating set of static string buffers, valid until the next 4 STR reads.+const char* b3RecR_STR( b3RecReader* rdr )+{+	char* buf = rdr->stringBuffers[rdr->nextString];+	rdr->nextString = ( rdr->nextString + 1 ) & 3;++	uint16_t len = b3RecR_U16( rdr );+	if ( len == 0xFFFFu )+	{+		return NULL;+	}++	int n = (int)len;+	if ( n > B3_MAX_NAME_LENGTH )+	{+		n = B3_MAX_NAME_LENGTH;+	}+	b3RecRdrCheck( rdr, (int)len );+	if ( rdr->ok && n > 0 )+	{+		memcpy( buf, rdr->data + rdr->cursor, (size_t)n );+	}+	rdr->cursor += (int)len;+	buf[n] = '\0';+	return buf;+}++// Def readers: start from b3Default*Def() then overlay each serialized field+// in the exact order the writer produced them.++b3ExplosionDef b3RecR_EXPLOSIONDEF( b3RecReader* rdr )+{+	b3ExplosionDef def = b3DefaultExplosionDef();+	def.maskBits = b3RecR_U64( rdr );+	def.position = b3RecR_POSITION( rdr );+	def.radius = b3RecR_F32( rdr );+	def.falloff = b3RecR_F32( rdr );+	def.impulsePerArea = b3RecR_F32( rdr );+	return def;+}++b3BodyDef b3RecR_BODYDEF( b3RecReader* rdr )+{+	b3BodyDef def = b3DefaultBodyDef();+	def.type = (b3BodyType)b3RecR_I32( rdr );+	def.position = b3RecR_POSITION( rdr );+	def.rotation = b3RecR_QUAT( rdr );+	def.linearVelocity = b3RecR_VEC3( rdr );+	def.angularVelocity = b3RecR_VEC3( rdr );+	def.linearDamping = b3RecR_F32( rdr );+	def.angularDamping = b3RecR_F32( rdr );+	def.gravityScale = b3RecR_F32( rdr );+	def.sleepThreshold = b3RecR_F32( rdr );+	def.name = b3RecR_STR( rdr );+	(void)b3RecR_U64( rdr ); // userData placeholder+	def.motionLocks = b3RecR_LOCKS( rdr );+	def.enableSleep = b3RecR_BOOL( rdr );+	def.isAwake = b3RecR_BOOL( rdr );+	def.isBullet = b3RecR_BOOL( rdr );+	def.isEnabled = b3RecR_BOOL( rdr );+	def.allowFastRotation = b3RecR_BOOL( rdr );+	def.enableContactRecycling = b3RecR_BOOL( rdr );+	def.userData = NULL;+	return def;+}++b3ShapeDef b3RecR_SHAPEDEF( b3RecReader* rdr )+{+	b3ShapeDef def = b3DefaultShapeDef();++	def.name = b3RecR_STR( rdr );+	(void)b3RecR_U64( rdr ); // userData placeholder++	int matCount = b3RecR_I32( rdr );+	if ( matCount < 0 )+	{+		matCount = 0;+	}+	if ( matCount > 0 &&+		 b3RecReserveScratch( rdr, (void**)&rdr->matScratch, &rdr->matScratchCap, matCount, (int)sizeof( b3SurfaceMaterial ) ) )+	{+		for ( int i = 0; i < matCount; ++i )+		{+			rdr->matScratch[i] = b3RecR_MATERIAL( rdr );+		}+		def.materials = rdr->matScratch;+		def.materialCount = matCount;+	}+	else+	{+		for ( int i = 0; i < matCount; ++i )+		{+			(void)b3RecR_MATERIAL( rdr );+		}+		def.materials = NULL;+		def.materialCount = 0;+	}++	def.baseMaterial = b3RecR_MATERIAL( rdr );+	def.density = b3RecR_F32( rdr );+	def.explosionScale = b3RecR_F32( rdr );+	def.filter = b3RecR_FILTER( rdr );+	def.enableCustomFiltering = b3RecR_BOOL( rdr );+	def.isSensor = b3RecR_BOOL( rdr );+	def.enableSensorEvents = b3RecR_BOOL( rdr );+	def.enableContactEvents = b3RecR_BOOL( rdr );+	def.enableHitEvents = b3RecR_BOOL( rdr );+	def.enablePreSolveEvents = b3RecR_BOOL( rdr );+	def.invokeContactCreation = b3RecR_BOOL( rdr );+	def.updateBodyMass = b3RecR_BOOL( rdr );+	def.enableSpeculativeContact = b3RecR_BOOL( rdr );+	def.userData = NULL;+	return def;+}++// Shared base for all joint defs. Body ids come in with recorded world0; callers remap them.+static void b3RecR_JointBase( b3RecReader* rdr, b3JointDef* base )+{+	(void)b3RecR_U64( rdr ); // userData+	base->bodyIdA = b3RecR_BODYID( rdr );+	base->bodyIdB = b3RecR_BODYID( rdr );+	base->localFrameA = b3RecR_TRANSFORM( rdr );+	base->localFrameB = b3RecR_TRANSFORM( rdr );+	base->forceThreshold = b3RecR_F32( rdr );+	base->torqueThreshold = b3RecR_F32( rdr );+	base->constraintHertz = b3RecR_F32( rdr );+	base->constraintDampingRatio = b3RecR_F32( rdr );+	base->drawScale = b3RecR_F32( rdr );+	base->collideConnected = b3RecR_BOOL( rdr );+	base->userData = NULL;+}++b3ParallelJointDef b3RecR_PARALLELJOINTDEF( b3RecReader* rdr )+{+	b3ParallelJointDef def = b3DefaultParallelJointDef();+	b3RecR_JointBase( rdr, &def.base );+	def.hertz = b3RecR_F32( rdr );+	def.dampingRatio = b3RecR_F32( rdr );+	def.maxTorque = b3RecR_F32( rdr );+	return def;+}++b3DistanceJointDef b3RecR_DISTANCEJOINTDEF( b3RecReader* rdr )+{+	b3DistanceJointDef def = b3DefaultDistanceJointDef();+	b3RecR_JointBase( rdr, &def.base );+	def.length = b3RecR_F32( rdr );+	def.enableSpring = b3RecR_BOOL( rdr );+	def.lowerSpringForce = b3RecR_F32( rdr );+	def.upperSpringForce = b3RecR_F32( rdr );+	def.hertz = b3RecR_F32( rdr );+	def.dampingRatio = b3RecR_F32( rdr );+	def.enableLimit = b3RecR_BOOL( rdr );+	def.minLength = b3RecR_F32( rdr );+	def.maxLength = b3RecR_F32( rdr );+	def.enableMotor = b3RecR_BOOL( rdr );+	def.maxMotorForce = b3RecR_F32( rdr );+	def.motorSpeed = b3RecR_F32( rdr );+	return def;+}++b3FilterJointDef b3RecR_FILTERJOINTDEF( b3RecReader* rdr )+{+	b3FilterJointDef def = b3DefaultFilterJointDef();+	b3RecR_JointBase( rdr, &def.base );+	return def;+}++b3MotorJointDef b3RecR_MOTORJOINTDEF( b3RecReader* rdr )+{+	b3MotorJointDef def = b3DefaultMotorJointDef();+	b3RecR_JointBase( rdr, &def.base );+	def.linearVelocity = b3RecR_VEC3( rdr );+	def.maxVelocityForce = b3RecR_F32( rdr );+	def.angularVelocity = b3RecR_VEC3( rdr );+	def.maxVelocityTorque = b3RecR_F32( rdr );+	def.linearHertz = b3RecR_F32( rdr );+	def.linearDampingRatio = b3RecR_F32( rdr );+	def.maxSpringForce = b3RecR_F32( rdr );+	def.angularHertz = b3RecR_F32( rdr );+	def.angularDampingRatio = b3RecR_F32( rdr );+	def.maxSpringTorque = b3RecR_F32( rdr );+	return def;+}++b3PrismaticJointDef b3RecR_PRISMATICJOINTDEF( b3RecReader* rdr )+{+	b3PrismaticJointDef def = b3DefaultPrismaticJointDef();+	b3RecR_JointBase( rdr, &def.base );+	def.enableSpring = b3RecR_BOOL( rdr );+	def.hertz = b3RecR_F32( rdr );+	def.dampingRatio = b3RecR_F32( rdr );+	def.targetTranslation = b3RecR_F32( rdr );+	def.enableLimit = b3RecR_BOOL( rdr );+	def.lowerTranslation = b3RecR_F32( rdr );+	def.upperTranslation = b3RecR_F32( rdr );+	def.enableMotor = b3RecR_BOOL( rdr );+	def.maxMotorForce = b3RecR_F32( rdr );+	def.motorSpeed = b3RecR_F32( rdr );+	return def;+}++b3RevoluteJointDef b3RecR_REVOLUTEJOINTDEF( b3RecReader* rdr )+{+	b3RevoluteJointDef def = b3DefaultRevoluteJointDef();+	b3RecR_JointBase( rdr, &def.base );+	def.targetAngle = b3RecR_F32( rdr );+	def.enableSpring = b3RecR_BOOL( rdr );+	def.hertz = b3RecR_F32( rdr );+	def.dampingRatio = b3RecR_F32( rdr );+	def.enableLimit = b3RecR_BOOL( rdr );+	def.lowerAngle = b3RecR_F32( rdr );+	def.upperAngle = b3RecR_F32( rdr );+	def.enableMotor = b3RecR_BOOL( rdr );+	def.maxMotorTorque = b3RecR_F32( rdr );+	def.motorSpeed = b3RecR_F32( rdr );+	return def;+}++b3SphericalJointDef b3RecR_SPHERICALJOINTDEF( b3RecReader* rdr )+{+	b3SphericalJointDef def = b3DefaultSphericalJointDef();+	b3RecR_JointBase( rdr, &def.base );+	def.enableSpring = b3RecR_BOOL( rdr );+	def.hertz = b3RecR_F32( rdr );+	def.dampingRatio = b3RecR_F32( rdr );+	def.targetRotation = b3RecR_QUAT( rdr );+	def.enableConeLimit = b3RecR_BOOL( rdr );+	def.coneAngle = b3RecR_F32( rdr );+	def.enableTwistLimit = b3RecR_BOOL( rdr );+	def.lowerTwistAngle = b3RecR_F32( rdr );+	def.upperTwistAngle = b3RecR_F32( rdr );+	def.enableMotor = b3RecR_BOOL( rdr );+	def.maxMotorTorque = b3RecR_F32( rdr );+	def.motorVelocity = b3RecR_VEC3( rdr );+	return def;+}++b3WeldJointDef b3RecR_WELDJOINTDEF( b3RecReader* rdr )+{+	b3WeldJointDef def = b3DefaultWeldJointDef();+	b3RecR_JointBase( rdr, &def.base );+	def.linearHertz = b3RecR_F32( rdr );+	def.angularHertz = b3RecR_F32( rdr );+	def.linearDampingRatio = b3RecR_F32( rdr );+	def.angularDampingRatio = b3RecR_F32( rdr );+	return def;+}++b3WheelJointDef b3RecR_WHEELJOINTDEF( b3RecReader* rdr )+{+	b3WheelJointDef def = b3DefaultWheelJointDef();+	b3RecR_JointBase( rdr, &def.base );+	def.enableSuspensionSpring = b3RecR_BOOL( rdr );+	def.suspensionHertz = b3RecR_F32( rdr );+	def.suspensionDampingRatio = b3RecR_F32( rdr );+	def.enableSuspensionLimit = b3RecR_BOOL( rdr );+	def.lowerSuspensionLimit = b3RecR_F32( rdr );+	def.upperSuspensionLimit = b3RecR_F32( rdr );+	def.enableSpinMotor = b3RecR_BOOL( rdr );+	def.maxSpinTorque = b3RecR_F32( rdr );+	def.spinSpeed = b3RecR_F32( rdr );+	def.enableSteering = b3RecR_BOOL( rdr );+	def.steeringHertz = b3RecR_F32( rdr );+	def.steeringDampingRatio = b3RecR_F32( rdr );+	def.targetSteeringAngle = b3RecR_F32( rdr );+	def.maxSteeringTorque = b3RecR_F32( rdr );+	def.enableSteeringLimit = b3RecR_BOOL( rdr );+	def.lowerSteeringLimit = b3RecR_F32( rdr );+	def.upperSteeringLimit = b3RecR_F32( rdr );+	return def;+}++// Outliner body tracking. Defined after the player struct; forward declared here because the+// create/destroy dispatch sits above that struct.+static void b3RecTrackBodyCreate( b3RecPlayer* player, b3BodyId id );+static void b3RecTrackBodyDestroy( b3RecPlayer* player, b3BodyId id );++// Id retargeting: replace world0 with the replay world's slot index.++static b3BodyId b3RecMakeBodyId( b3RecReader* rdr, b3BodyId recorded )+{+	b3BodyId id;+	id.index1 = recorded.index1;+	id.world0 = (uint16_t)( rdr->replayWorldId.index1 - 1u );+	id.generation = recorded.generation;+	return id;+}++static b3ShapeId b3RecMakeShapeId( b3RecReader* rdr, b3ShapeId recorded )+{+	b3ShapeId id;+	id.index1 = recorded.index1;+	id.world0 = (uint16_t)( rdr->replayWorldId.index1 - 1u );+	id.generation = recorded.generation;+	return id;+}++static b3JointId b3RecMakeJointId( b3RecReader* rdr, b3JointId recorded )+{+	b3JointId id;+	id.index1 = recorded.index1;+	id.world0 = (uint16_t)( rdr->replayWorldId.index1 - 1u );+	id.generation = recorded.generation;+	return id;+}++// A create op appends the returned id after args. index1 and generation must match;+// world0 always differs so we ignore it.+static void b3RecCheckId( b3RecReader* rdr, const char* kind, int gotIndex, unsigned gotGen, int recIndex, unsigned recGen )+{+	if ( gotIndex != recIndex || gotGen != recGen )+	{+		printf( "b3ReplayFile: %s id mismatch (rec index1=%d gen=%u, got index1=%d gen=%u)\n", kind, recIndex, recGen, gotIndex,+				gotGen );+		rdr->ok = false;+	}+}++static void b3RecCheckBodyId( b3RecReader* rdr, b3BodyId got, b3BodyId rec )+{+	b3RecCheckId( rdr, "body", got.index1, got.generation, rec.index1, rec.generation );+}++static void b3RecCheckShapeId( b3RecReader* rdr, b3ShapeId got, b3ShapeId rec )+{+	b3RecCheckId( rdr, "shape", got.index1, got.generation, rec.index1, rec.generation );+}++static void b3RecCheckJointId( b3RecReader* rdr, b3JointId got, b3JointId rec )+{+	b3RecCheckId( rdr, "joint", got.index1, got.generation, rec.index1, rec.generation );+}++// Registry slot reconstruction. Returns the live pointer for the given slot, building it+// on first use. The hull case is handled inline at the call site since it doesn't cache.++static void* b3RecGetLiveMesh( b3RegistrySlot* slot )+{+	// Mesh is a self-contained blob used by reference, with no pointer fixup. Hand back the pristine+	// bytes directly: they already outlive the world and are freed at teardown, so a copy would just+	// double the memory. Compound can't do this, see b3RecGetLiveCompound.+	return slot->bytes;+}++static void* b3RecGetLiveHeightField( b3RegistrySlot* slot )+{+	// Self-contained blob used by reference, like b3RecGetLiveMesh. The bytes already are a+	// valid b3HeightFieldData with no pointer fixup, so hand them back directly.+	return slot->bytes;+}++static void* b3RecGetLiveCompound( b3RegistrySlot* slot )+{+	if ( slot->live != NULL )+	{+		return slot->live;+	}+	// The copy is unavoidable here: b3ConvertBytesToCompound rewrites its input in place, while the+	// pristine bytes must survive for keyframe registry seeding (b3RecSeedKeyframeRegistry). So we+	// keep both the serialized bytes and a separate converted live object.+	slot->live = b3Alloc( (size_t)slot->byteCount );+	memcpy( slot->live, slot->bytes, (size_t)slot->byteCount );+	b3ConvertBytesToCompound( (uint8_t*)slot->live, slot->byteCount );+	return slot->live;+}++// Dispatch functions, one per op++static void b3RecDispatch_DestroyWorld( const b3RecArgs_DestroyWorld* a, b3RecReader* rdr )+{+	(void)a;+	(void)rdr;+	// End-of-session marker. The replay world is torn down in b3ValidateReplay, not here.+}++static void b3RecDispatch_Step( const b3RecArgs_Step* a, b3RecReader* rdr )+{+	(void)a;+	b3World_Step( rdr->replayWorldId, a->dt, a->subStepCount );+}++static void b3RecDispatch_WorldEnableSleeping( const b3RecArgs_WorldEnableSleeping* a, b3RecReader* rdr )+{+	b3World_EnableSleeping( rdr->replayWorldId, a->flag );+}++static void b3RecDispatch_WorldEnableContinuous( const b3RecArgs_WorldEnableContinuous* a, b3RecReader* rdr )+{+	b3World_EnableContinuous( rdr->replayWorldId, a->flag );+}++static void b3RecDispatch_WorldSetRestitutionThreshold( const b3RecArgs_WorldSetRestitutionThreshold* a, b3RecReader* rdr )+{+	b3World_SetRestitutionThreshold( rdr->replayWorldId, a->value );+}++static void b3RecDispatch_WorldSetHitEventThreshold( const b3RecArgs_WorldSetHitEventThreshold* a, b3RecReader* rdr )+{+	b3World_SetHitEventThreshold( rdr->replayWorldId, a->value );+}++static void b3RecDispatch_WorldSetGravity( const b3RecArgs_WorldSetGravity* a, b3RecReader* rdr )+{+	b3World_SetGravity( rdr->replayWorldId, a->gravity );+}++static void b3RecDispatch_WorldExplode( const b3RecArgs_WorldExplode* a, b3RecReader* rdr )+{+	b3World_Explode( rdr->replayWorldId, &a->def );+}++static void b3RecDispatch_WorldSetContactTuning( const b3RecArgs_WorldSetContactTuning* a, b3RecReader* rdr )+{+	b3World_SetContactTuning( rdr->replayWorldId, a->hertz, a->dampingRatio, a->contactSpeed );+}++static void b3RecDispatch_WorldSetContactRecycleDistance( const b3RecArgs_WorldSetContactRecycleDistance* a, b3RecReader* rdr )+{+	b3World_SetContactRecycleDistance( rdr->replayWorldId, a->recycleDistance );+}++static void b3RecDispatch_WorldSetMaximumLinearSpeed( const b3RecArgs_WorldSetMaximumLinearSpeed* a, b3RecReader* rdr )+{+	b3World_SetMaximumLinearSpeed( rdr->replayWorldId, a->maximumLinearSpeed );+}++static void b3RecDispatch_WorldEnableWarmStarting( const b3RecArgs_WorldEnableWarmStarting* a, b3RecReader* rdr )+{+	b3World_EnableWarmStarting( rdr->replayWorldId, a->flag );+}++static void b3RecDispatch_WorldRebuildStaticTree( const b3RecArgs_WorldRebuildStaticTree* a, b3RecReader* rdr )+{+	(void)a;+	b3World_RebuildStaticTree( rdr->replayWorldId );+}++static void b3RecDispatch_WorldEnableSpeculative( const b3RecArgs_WorldEnableSpeculative* a, b3RecReader* rdr )+{+	b3World_EnableSpeculative( rdr->replayWorldId, a->flag );+}++static void b3RecDispatch_CreateBody( const b3RecArgs_CreateBody* a, b3RecReader* rdr )+{+	b3BodyId recId = b3RecR_BODYID( rdr );+	b3BodyId gotId = b3CreateBody( rdr->replayWorldId, &a->def );+	b3RecCheckBodyId( rdr, gotId, recId );+	if ( rdr->owner != NULL )+	{+		b3RecTrackBodyCreate( rdr->owner, gotId );+	}+}++static void b3RecDispatch_DestroyBody( const b3RecArgs_DestroyBody* a, b3RecReader* rdr )+{+	b3BodyId id = b3RecMakeBodyId( rdr, a->body );+	if ( rdr->owner != NULL )+	{+		b3RecTrackBodyDestroy( rdr->owner, id );+	}+	b3DestroyBody( id );+}++static void b3RecDispatch_BodySetTransform( const b3RecArgs_BodySetTransform* a, b3RecReader* rdr )+{+	b3Body_SetTransform( b3RecMakeBodyId( rdr, a->body ), a->position, a->rotation );+}++static void b3RecDispatch_BodySetLinearVelocity( const b3RecArgs_BodySetLinearVelocity* a, b3RecReader* rdr )+{+	b3Body_SetLinearVelocity( b3RecMakeBodyId( rdr, a->body ), a->v );+}++static void b3RecDispatch_BodySetType( const b3RecArgs_BodySetType* a, b3RecReader* rdr )+{+	b3Body_SetType( b3RecMakeBodyId( rdr, a->body ), (b3BodyType)a->type );+}++static void b3RecDispatch_BodySetName( const b3RecArgs_BodySetName* a, b3RecReader* rdr )+{+	b3Body_SetName( b3RecMakeBodyId( rdr, a->body ), a->name );+}++static void b3RecDispatch_BodySetAngularVelocity( const b3RecArgs_BodySetAngularVelocity* a, b3RecReader* rdr )+{+	b3Body_SetAngularVelocity( b3RecMakeBodyId( rdr, a->body ), a->w );+}++static void b3RecDispatch_BodySetTargetTransform( const b3RecArgs_BodySetTargetTransform* a, b3RecReader* rdr )+{+	b3Body_SetTargetTransform( b3RecMakeBodyId( rdr, a->body ), a->target, a->timeStep, a->wake );+}++static void b3RecDispatch_BodyApplyForce( const b3RecArgs_BodyApplyForce* a, b3RecReader* rdr )+{+	b3Body_ApplyForce( b3RecMakeBodyId( rdr, a->body ), a->force, a->point, a->wake );+}++static void b3RecDispatch_BodyApplyForceToCenter( const b3RecArgs_BodyApplyForceToCenter* a, b3RecReader* rdr )+{+	b3Body_ApplyForceToCenter( b3RecMakeBodyId( rdr, a->body ), a->force, a->wake );+}++static void b3RecDispatch_BodyApplyTorque( const b3RecArgs_BodyApplyTorque* a, b3RecReader* rdr )+{+	b3Body_ApplyTorque( b3RecMakeBodyId( rdr, a->body ), a->torque, a->wake );+}++static void b3RecDispatch_BodyApplyLinearImpulse( const b3RecArgs_BodyApplyLinearImpulse* a, b3RecReader* rdr )+{+	b3Body_ApplyLinearImpulse( b3RecMakeBodyId( rdr, a->body ), a->impulse, a->point, a->wake );+}++static void b3RecDispatch_BodyApplyLinearImpulseToCenter( const b3RecArgs_BodyApplyLinearImpulseToCenter* a, b3RecReader* rdr )+{+	b3Body_ApplyLinearImpulseToCenter( b3RecMakeBodyId( rdr, a->body ), a->impulse, a->wake );+}++static void b3RecDispatch_BodyApplyAngularImpulse( const b3RecArgs_BodyApplyAngularImpulse* a, b3RecReader* rdr )+{+	b3Body_ApplyAngularImpulse( b3RecMakeBodyId( rdr, a->body ), a->impulse, a->wake );+}++static void b3RecDispatch_BodySetMassData( const b3RecArgs_BodySetMassData* a, b3RecReader* rdr )+{+	b3Body_SetMassData( b3RecMakeBodyId( rdr, a->body ), a->massData );+}++static void b3RecDispatch_BodyApplyMassFromShapes( const b3RecArgs_BodyApplyMassFromShapes* a, b3RecReader* rdr )+{+	b3Body_ApplyMassFromShapes( b3RecMakeBodyId( rdr, a->body ) );+}++static void b3RecDispatch_BodySetLinearDamping( const b3RecArgs_BodySetLinearDamping* a, b3RecReader* rdr )+{+	b3Body_SetLinearDamping( b3RecMakeBodyId( rdr, a->body ), a->damping );+}++static void b3RecDispatch_BodySetAngularDamping( const b3RecArgs_BodySetAngularDamping* a, b3RecReader* rdr )+{+	b3Body_SetAngularDamping( b3RecMakeBodyId( rdr, a->body ), a->damping );+}++static void b3RecDispatch_BodySetGravityScale( const b3RecArgs_BodySetGravityScale* a, b3RecReader* rdr )+{+	b3Body_SetGravityScale( b3RecMakeBodyId( rdr, a->body ), a->scale );+}++static void b3RecDispatch_BodySetAwake( const b3RecArgs_BodySetAwake* a, b3RecReader* rdr )+{+	b3Body_SetAwake( b3RecMakeBodyId( rdr, a->body ), a->awake );+}++static void b3RecDispatch_BodyEnableSleep( const b3RecArgs_BodyEnableSleep* a, b3RecReader* rdr )+{+	b3Body_EnableSleep( b3RecMakeBodyId( rdr, a->body ), a->flag );+}++static void b3RecDispatch_BodySetSleepThreshold( const b3RecArgs_BodySetSleepThreshold* a, b3RecReader* rdr )+{+	b3Body_SetSleepThreshold( b3RecMakeBodyId( rdr, a->body ), a->threshold );+}++static void b3RecDispatch_BodyDisable( const b3RecArgs_BodyDisable* a, b3RecReader* rdr )+{+	b3Body_Disable( b3RecMakeBodyId( rdr, a->body ) );+}++static void b3RecDispatch_BodyEnable( const b3RecArgs_BodyEnable* a, b3RecReader* rdr )+{+	b3Body_Enable( b3RecMakeBodyId( rdr, a->body ) );+}++static void b3RecDispatch_BodySetMotionLocks( const b3RecArgs_BodySetMotionLocks* a, b3RecReader* rdr )+{+	b3Body_SetMotionLocks( b3RecMakeBodyId( rdr, a->body ), a->locks );+}++static void b3RecDispatch_BodySetBullet( const b3RecArgs_BodySetBullet* a, b3RecReader* rdr )+{+	b3Body_SetBullet( b3RecMakeBodyId( rdr, a->body ), a->flag );+}++static void b3RecDispatch_BodyAllowFastRotation( const b3RecArgs_BodyAllowFastRotation* a, b3RecReader* rdr )+{+	b3Body_AllowFastRotation( b3RecMakeBodyId( rdr, a->body ), a->flag );+}++static void b3RecDispatch_BodyEnableContactRecycling( const b3RecArgs_BodyEnableContactRecycling* a, b3RecReader* rdr )+{+	b3Body_EnableContactRecycling( b3RecMakeBodyId( rdr, a->body ), a->flag );+}++static void b3RecDispatch_BodyEnableHitEvents( const b3RecArgs_BodyEnableHitEvents* a, b3RecReader* rdr )+{+	b3Body_EnableHitEvents( b3RecMakeBodyId( rdr, a->body ), a->flag );+}++static void b3RecDispatch_CreateSphereShape( const b3RecArgs_CreateSphereShape* a, b3RecReader* rdr )+{+	b3ShapeId recId = b3RecR_SHAPEID( rdr );+	b3BodyId bodyId = b3RecMakeBodyId( rdr, a->body );+	b3ShapeId gotId = b3CreateSphereShape( bodyId, &a->def, &a->sphere );+	b3RecCheckShapeId( rdr, gotId, recId );+}++static void b3RecDispatch_CreateCapsuleShape( const b3RecArgs_CreateCapsuleShape* a, b3RecReader* rdr )+{+	b3ShapeId recId = b3RecR_SHAPEID( rdr );+	b3BodyId bodyId = b3RecMakeBodyId( rdr, a->body );+	b3ShapeId gotId = b3CreateCapsuleShape( bodyId, &a->def, &a->capsule );+	b3RecCheckShapeId( rdr, gotId, recId );+}++static void b3RecDispatch_CreateHullShape( const b3RecArgs_CreateHullShape* a, b3RecReader* rdr )+{+	b3ShapeId recId = b3RecR_SHAPEID( rdr );+	if ( !rdr->ok )+	{+		return;+	}+	uint32_t id = a->geometryId;+	if ( id >= (uint32_t)rdr->slotCount )+	{+		printf( "b3ReplayFile: hull geometryId %u out of range\n", id );+		rdr->ok = false;+		return;+	}+	b3RegistrySlot* slot = rdr->slots + id;+	b3BodyId bodyId = b3RecMakeBodyId( rdr, a->body );+	// Hull is cloned by b3CreateHullShape into the world DB; no caching needed.+	b3ShapeId gotId = b3CreateHullShape( bodyId, &a->def, (const b3HullData*)slot->bytes );+	b3RecCheckShapeId( rdr, gotId, recId );+}++static void b3RecDispatch_CreateMeshShape( const b3RecArgs_CreateMeshShape* a, b3RecReader* rdr )+{+	b3ShapeId recId = b3RecR_SHAPEID( rdr );+	if ( !rdr->ok )+	{+		return;+	}+	uint32_t id = a->geometryId;+	if ( id >= (uint32_t)rdr->slotCount )+	{+		printf( "b3ReplayFile: mesh geometryId %u out of range\n", id );+		rdr->ok = false;+		return;+	}+	b3RegistrySlot* slot = rdr->slots + id;+	const b3MeshData* mesh = b3RecGetLiveMesh( slot );+	b3BodyId bodyId = b3RecMakeBodyId( rdr, a->body );+	b3ShapeId gotId = b3CreateMeshShape( bodyId, &a->def, mesh, a->scale );+	b3RecCheckShapeId( rdr, gotId, recId );+}++static void b3RecDispatch_CreateHeightFieldShape( const b3RecArgs_CreateHeightFieldShape* a, b3RecReader* rdr )+{+	b3ShapeId recId = b3RecR_SHAPEID( rdr );+	if ( !rdr->ok )+	{+		return;+	}+	uint32_t id = a->geometryId;+	if ( id >= (uint32_t)rdr->slotCount )+	{+		printf( "b3ReplayFile: heightfield geometryId %u out of range\n", id );+		rdr->ok = false;+		return;+	}+	b3RegistrySlot* slot = rdr->slots + id;+	const b3HeightFieldData* hf = (const b3HeightFieldData*)b3RecGetLiveHeightField( slot );+	if ( hf == NULL )+	{+		printf( "b3ReplayFile: heightfield geometry %u is corrupt\n", id );+		rdr->ok = false;+		return;+	}+	b3BodyId bodyId = b3RecMakeBodyId( rdr, a->body );+	b3ShapeId gotId = b3CreateHeightFieldShape( bodyId, &a->def, hf );+	b3RecCheckShapeId( rdr, gotId, recId );+}++static void b3RecDispatch_CreateCompoundShape( const b3RecArgs_CreateCompoundShape* a, b3RecReader* rdr )+{+	b3ShapeId recId = b3RecR_SHAPEID( rdr );+	if ( !rdr->ok )+	{+		return;+	}+	uint32_t id = a->geometryId;+	if ( id >= (uint32_t)rdr->slotCount )+	{+		printf( "b3ReplayFile: compound geometryId %u out of range\n", id );+		rdr->ok = false;+		return;+	}+	b3RegistrySlot* slot = rdr->slots + id;+	const b3CompoundData* compound = (const b3CompoundData*)b3RecGetLiveCompound( slot );+	b3BodyId bodyId = b3RecMakeBodyId( rdr, a->body );+	// b3CreateCompoundShape takes a non-const def pointer; cast away const for the scratch def+	b3ShapeDef shapeDef = a->def;+	b3ShapeId gotId = b3CreateBakedCompoundShape( bodyId, &shapeDef, compound );+	b3RecCheckShapeId( rdr, gotId, recId );+}++static void b3RecDispatch_DestroyShape( const b3RecArgs_DestroyShape* a, b3RecReader* rdr )+{+	b3DestroyShape( b3RecMakeShapeId( rdr, a->shape ), a->updateBodyMass );+}++static void b3RecDispatch_ShapeSetName( const b3RecArgs_ShapeSetName* a, b3RecReader* rdr )+{+	b3Shape_SetName( b3RecMakeShapeId( rdr, a->shape ), a->name );+}++static void b3RecDispatch_ShapeSetDensity( const b3RecArgs_ShapeSetDensity* a, b3RecReader* rdr )+{+	b3Shape_SetDensity( b3RecMakeShapeId( rdr, a->shape ), a->density, a->updateBodyMass );+}++static void b3RecDispatch_ShapeSetFriction( const b3RecArgs_ShapeSetFriction* a, b3RecReader* rdr )+{+	b3Shape_SetFriction( b3RecMakeShapeId( rdr, a->shape ), a->friction );+}++static void b3RecDispatch_ShapeSetRestitution( const b3RecArgs_ShapeSetRestitution* a, b3RecReader* rdr )+{+	b3Shape_SetRestitution( b3RecMakeShapeId( rdr, a->shape ), a->restitution );+}++static void b3RecDispatch_ShapeSetSurfaceMaterial( const b3RecArgs_ShapeSetSurfaceMaterial* a, b3RecReader* rdr )+{+	b3Shape_SetSurfaceMaterial( b3RecMakeShapeId( rdr, a->shape ), a->material );+}++static void b3RecDispatch_ShapeSetMeshMaterial( const b3RecArgs_ShapeSetMeshMaterial* a, b3RecReader* rdr )+{+	b3Shape_SetMeshMaterial( b3RecMakeShapeId( rdr, a->shape ), a->material, a->index );+}++static void b3RecDispatch_ShapeSetFilter( const b3RecArgs_ShapeSetFilter* a, b3RecReader* rdr )+{+	b3Shape_SetFilter( b3RecMakeShapeId( rdr, a->shape ), a->filter, a->invokeContacts );+}++static void b3RecDispatch_ShapeEnableSensorEvents( const b3RecArgs_ShapeEnableSensorEvents* a, b3RecReader* rdr )+{+	b3Shape_EnableSensorEvents( b3RecMakeShapeId( rdr, a->shape ), a->flag );+}++static void b3RecDispatch_ShapeEnableContactEvents( const b3RecArgs_ShapeEnableContactEvents* a, b3RecReader* rdr )+{+	b3Shape_EnableContactEvents( b3RecMakeShapeId( rdr, a->shape ), a->flag );+}++static void b3RecDispatch_ShapeEnablePreSolveEvents( const b3RecArgs_ShapeEnablePreSolveEvents* a, b3RecReader* rdr )+{+	b3Shape_EnablePreSolveEvents( b3RecMakeShapeId( rdr, a->shape ), a->flag );+}++static void b3RecDispatch_ShapeEnableHitEvents( const b3RecArgs_ShapeEnableHitEvents* a, b3RecReader* rdr )+{+	b3Shape_EnableHitEvents( b3RecMakeShapeId( rdr, a->shape ), a->flag );+}++static void b3RecDispatch_ShapeSetSphere( const b3RecArgs_ShapeSetSphere* a, b3RecReader* rdr )+{+	b3Shape_SetSphere( b3RecMakeShapeId( rdr, a->shape ), &a->sphere );+}++static void b3RecDispatch_ShapeSetCapsule( const b3RecArgs_ShapeSetCapsule* a, b3RecReader* rdr )+{+	b3Shape_SetCapsule( b3RecMakeShapeId( rdr, a->shape ), &a->capsule );+}++static void b3RecDispatch_ShapeSetHull( const b3RecArgs_ShapeSetHull* a, b3RecReader* rdr )+{+	uint32_t id = a->geometryId;+	if ( id >= (uint32_t)rdr->slotCount )+	{+		printf( "b3ReplayFile: hull geometryId %u out of range\n", id );+		rdr->ok = false;+		return;+	}+	b3RegistrySlot* slot = rdr->slots + id;+	b3ShapeId shapeId = b3RecMakeShapeId( rdr, a->shape );+	b3Shape_SetHull( shapeId, (const b3HullData*)slot->bytes );+}++static void b3RecDispatch_ShapeSetMesh( const b3RecArgs_ShapeSetMesh* a, b3RecReader* rdr )+{+	uint32_t id = a->geometryId;+	if ( id >= (uint32_t)rdr->slotCount )+	{+		printf( "b3ReplayFile: mesh geometryId %u out of range\n", id );+		rdr->ok = false;+		return;+	}+	b3RegistrySlot* slot = rdr->slots + id;+	b3ShapeId shapeId = b3RecMakeShapeId( rdr, a->shape );+	const b3MeshData* mesh = b3RecGetLiveMesh( slot );+	b3Shape_SetMesh( shapeId, mesh, a->scale );+}++static void b3RecDispatch_ShapeApplyWind( const b3RecArgs_ShapeApplyWind* a, b3RecReader* rdr )+{+	b3Shape_ApplyWind( b3RecMakeShapeId( rdr, a->shape ), a->wind, a->drag, a->lift, a->maxSpeed, a->wake );+}++// Joint creates: remap body ids in the def before calling the API.++static void b3RecDispatch_CreateParallelJoint( const b3RecArgs_CreateParallelJoint* a, b3RecReader* rdr )+{+	b3JointId recId = b3RecR_JOINTID( rdr );+	b3ParallelJointDef def = a->def;+	def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA );+	def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB );+	b3RecCheckJointId( rdr, b3CreateParallelJoint( rdr->replayWorldId, &def ), recId );+}++static void b3RecDispatch_CreateDistanceJoint( const b3RecArgs_CreateDistanceJoint* a, b3RecReader* rdr )+{+	b3JointId recId = b3RecR_JOINTID( rdr );+	b3DistanceJointDef def = a->def;+	def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA );+	def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB );+	b3RecCheckJointId( rdr, b3CreateDistanceJoint( rdr->replayWorldId, &def ), recId );+}++static void b3RecDispatch_CreateFilterJoint( const b3RecArgs_CreateFilterJoint* a, b3RecReader* rdr )+{+	b3JointId recId = b3RecR_JOINTID( rdr );+	b3FilterJointDef def = a->def;+	def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA );+	def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB );+	b3RecCheckJointId( rdr, b3CreateFilterJoint( rdr->replayWorldId, &def ), recId );+}++static void b3RecDispatch_CreateMotorJoint( const b3RecArgs_CreateMotorJoint* a, b3RecReader* rdr )+{+	b3JointId recId = b3RecR_JOINTID( rdr );+	b3MotorJointDef def = a->def;+	def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA );+	def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB );+	b3RecCheckJointId( rdr, b3CreateMotorJoint( rdr->replayWorldId, &def ), recId );+}++static void b3RecDispatch_CreatePrismaticJoint( const b3RecArgs_CreatePrismaticJoint* a, b3RecReader* rdr )+{+	b3JointId recId = b3RecR_JOINTID( rdr );+	b3PrismaticJointDef def = a->def;+	def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA );+	def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB );+	b3RecCheckJointId( rdr, b3CreatePrismaticJoint( rdr->replayWorldId, &def ), recId );+}++static void b3RecDispatch_CreateRevoluteJoint( const b3RecArgs_CreateRevoluteJoint* a, b3RecReader* rdr )+{+	b3JointId recId = b3RecR_JOINTID( rdr );+	b3RevoluteJointDef def = a->def;+	def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA );+	def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB );+	b3RecCheckJointId( rdr, b3CreateRevoluteJoint( rdr->replayWorldId, &def ), recId );+}++static void b3RecDispatch_CreateSphericalJoint( const b3RecArgs_CreateSphericalJoint* a, b3RecReader* rdr )+{+	b3JointId recId = b3RecR_JOINTID( rdr );+	b3SphericalJointDef def = a->def;+	def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA );+	def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB );+	b3RecCheckJointId( rdr, b3CreateSphericalJoint( rdr->replayWorldId, &def ), recId );+}++static void b3RecDispatch_CreateWeldJoint( const b3RecArgs_CreateWeldJoint* a, b3RecReader* rdr )+{+	b3JointId recId = b3RecR_JOINTID( rdr );+	b3WeldJointDef def = a->def;+	def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA );+	def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB );+	b3RecCheckJointId( rdr, b3CreateWeldJoint( rdr->replayWorldId, &def ), recId );+}++static void b3RecDispatch_CreateWheelJoint( const b3RecArgs_CreateWheelJoint* a, b3RecReader* rdr )+{+	b3JointId recId = b3RecR_JOINTID( rdr );+	b3WheelJointDef def = a->def;+	def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA );+	def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB );+	b3RecCheckJointId( rdr, b3CreateWheelJoint( rdr->replayWorldId, &def ), recId );+}++static void b3RecDispatch_DestroyJoint( const b3RecArgs_DestroyJoint* a, b3RecReader* rdr )+{+	b3DestroyJoint( b3RecMakeJointId( rdr, a->joint ), a->wakeAttached );+}++static void b3RecDispatch_JointSetLocalFrameA( const b3RecArgs_JointSetLocalFrameA* a, b3RecReader* rdr )+{+	b3Joint_SetLocalFrameA( b3RecMakeJointId( rdr, a->joint ), a->localFrame );+}++static void b3RecDispatch_JointSetLocalFrameB( const b3RecArgs_JointSetLocalFrameB* a, b3RecReader* rdr )+{+	b3Joint_SetLocalFrameB( b3RecMakeJointId( rdr, a->joint ), a->localFrame );+}++static void b3RecDispatch_JointSetCollideConnected( const b3RecArgs_JointSetCollideConnected* a, b3RecReader* rdr )+{+	b3Joint_SetCollideConnected( b3RecMakeJointId( rdr, a->joint ), a->shouldCollide );+}++static void b3RecDispatch_JointWakeBodies( const b3RecArgs_JointWakeBodies* a, b3RecReader* rdr )+{+	b3Joint_WakeBodies( b3RecMakeJointId( rdr, a->joint ) );+}++static void b3RecDispatch_JointSetConstraintTuning( const b3RecArgs_JointSetConstraintTuning* a, b3RecReader* rdr )+{+	b3Joint_SetConstraintTuning( b3RecMakeJointId( rdr, a->joint ), a->hertz, a->dampingRatio );+}++static void b3RecDispatch_JointSetForceThreshold( const b3RecArgs_JointSetForceThreshold* a, b3RecReader* rdr )+{+	b3Joint_SetForceThreshold( b3RecMakeJointId( rdr, a->joint ), a->threshold );+}++static void b3RecDispatch_JointSetTorqueThreshold( const b3RecArgs_JointSetTorqueThreshold* a, b3RecReader* rdr )+{+	b3Joint_SetTorqueThreshold( b3RecMakeJointId( rdr, a->joint ), a->threshold );+}++static void b3RecDispatch_ParallelJointSetSpringHertz( const b3RecArgs_ParallelJointSetSpringHertz* a, b3RecReader* rdr )+{+	b3ParallelJoint_SetSpringHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz );+}++static void b3RecDispatch_ParallelJointSetSpringDampingRatio( const b3RecArgs_ParallelJointSetSpringDampingRatio* a,+															  b3RecReader* rdr )+{+	b3ParallelJoint_SetSpringDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio );+}++static void b3RecDispatch_ParallelJointSetMaxTorque( const b3RecArgs_ParallelJointSetMaxTorque* a, b3RecReader* rdr )+{+	b3ParallelJoint_SetMaxTorque( b3RecMakeJointId( rdr, a->joint ), a->maxTorque );+}++static void b3RecDispatch_DistanceJointSetLength( const b3RecArgs_DistanceJointSetLength* a, b3RecReader* rdr )+{+	b3DistanceJoint_SetLength( b3RecMakeJointId( rdr, a->joint ), a->length );+}++static void b3RecDispatch_DistanceJointEnableSpring( const b3RecArgs_DistanceJointEnableSpring* a, b3RecReader* rdr )+{+	b3DistanceJoint_EnableSpring( b3RecMakeJointId( rdr, a->joint ), a->enableSpring );+}++static void b3RecDispatch_DistanceJointSetSpringForceRange( const b3RecArgs_DistanceJointSetSpringForceRange* a,+															b3RecReader* rdr )+{+	b3DistanceJoint_SetSpringForceRange( b3RecMakeJointId( rdr, a->joint ), a->lowerForce, a->upperForce );+}++static void b3RecDispatch_DistanceJointSetSpringHertz( const b3RecArgs_DistanceJointSetSpringHertz* a, b3RecReader* rdr )+{+	b3DistanceJoint_SetSpringHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz );+}++static void b3RecDispatch_DistanceJointSetSpringDampingRatio( const b3RecArgs_DistanceJointSetSpringDampingRatio* a,+															  b3RecReader* rdr )+{+	b3DistanceJoint_SetSpringDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio );+}++static void b3RecDispatch_DistanceJointEnableLimit( const b3RecArgs_DistanceJointEnableLimit* a, b3RecReader* rdr )+{+	b3DistanceJoint_EnableLimit( b3RecMakeJointId( rdr, a->joint ), a->enableLimit );+}++static void b3RecDispatch_DistanceJointSetLengthRange( const b3RecArgs_DistanceJointSetLengthRange* a, b3RecReader* rdr )+{+	b3DistanceJoint_SetLengthRange( b3RecMakeJointId( rdr, a->joint ), a->minLength, a->maxLength );+}++static void b3RecDispatch_DistanceJointEnableMotor( const b3RecArgs_DistanceJointEnableMotor* a, b3RecReader* rdr )+{+	b3DistanceJoint_EnableMotor( b3RecMakeJointId( rdr, a->joint ), a->enableMotor );+}++static void b3RecDispatch_DistanceJointSetMotorSpeed( const b3RecArgs_DistanceJointSetMotorSpeed* a, b3RecReader* rdr )+{+	b3DistanceJoint_SetMotorSpeed( b3RecMakeJointId( rdr, a->joint ), a->motorSpeed );+}++static void b3RecDispatch_DistanceJointSetMaxMotorForce( const b3RecArgs_DistanceJointSetMaxMotorForce* a, b3RecReader* rdr )+{+	b3DistanceJoint_SetMaxMotorForce( b3RecMakeJointId( rdr, a->joint ), a->force );+}++static void b3RecDispatch_MotorJointSetLinearVelocity( const b3RecArgs_MotorJointSetLinearVelocity* a, b3RecReader* rdr )+{+	b3MotorJoint_SetLinearVelocity( b3RecMakeJointId( rdr, a->joint ), a->velocity );+}++static void b3RecDispatch_MotorJointSetAngularVelocity( const b3RecArgs_MotorJointSetAngularVelocity* a, b3RecReader* rdr )+{+	b3MotorJoint_SetAngularVelocity( b3RecMakeJointId( rdr, a->joint ), a->velocity );+}++static void b3RecDispatch_MotorJointSetMaxVelocityForce( const b3RecArgs_MotorJointSetMaxVelocityForce* a, b3RecReader* rdr )+{+	b3MotorJoint_SetMaxVelocityForce( b3RecMakeJointId( rdr, a->joint ), a->maxForce );+}++static void b3RecDispatch_MotorJointSetMaxVelocityTorque( const b3RecArgs_MotorJointSetMaxVelocityTorque* a, b3RecReader* rdr )+{+	b3MotorJoint_SetMaxVelocityTorque( b3RecMakeJointId( rdr, a->joint ), a->maxTorque );+}++static void b3RecDispatch_MotorJointSetLinearHertz( const b3RecArgs_MotorJointSetLinearHertz* a, b3RecReader* rdr )+{+	b3MotorJoint_SetLinearHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz );+}++static void b3RecDispatch_MotorJointSetLinearDampingRatio( const b3RecArgs_MotorJointSetLinearDampingRatio* a, b3RecReader* rdr )+{+	b3MotorJoint_SetLinearDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->damping );+}++static void b3RecDispatch_MotorJointSetAngularHertz( const b3RecArgs_MotorJointSetAngularHertz* a, b3RecReader* rdr )+{+	b3MotorJoint_SetAngularHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz );+}++static void b3RecDispatch_MotorJointSetAngularDampingRatio( const b3RecArgs_MotorJointSetAngularDampingRatio* a,+															b3RecReader* rdr )+{+	b3MotorJoint_SetAngularDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->damping );+}++static void b3RecDispatch_MotorJointSetMaxSpringForce( const b3RecArgs_MotorJointSetMaxSpringForce* a, b3RecReader* rdr )+{+	b3MotorJoint_SetMaxSpringForce( b3RecMakeJointId( rdr, a->joint ), a->maxForce );+}++static void b3RecDispatch_MotorJointSetMaxSpringTorque( const b3RecArgs_MotorJointSetMaxSpringTorque* a, b3RecReader* rdr )+{+	b3MotorJoint_SetMaxSpringTorque( b3RecMakeJointId( rdr, a->joint ), a->maxTorque );+}++static void b3RecDispatch_PrismaticJointEnableSpring( const b3RecArgs_PrismaticJointEnableSpring* a, b3RecReader* rdr )+{+	b3PrismaticJoint_EnableSpring( b3RecMakeJointId( rdr, a->joint ), a->enableSpring );+}++static void b3RecDispatch_PrismaticJointSetSpringHertz( const b3RecArgs_PrismaticJointSetSpringHertz* a, b3RecReader* rdr )+{+	b3PrismaticJoint_SetSpringHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz );+}++static void b3RecDispatch_PrismaticJointSetSpringDampingRatio( const b3RecArgs_PrismaticJointSetSpringDampingRatio* a,+															   b3RecReader* rdr )+{+	b3PrismaticJoint_SetSpringDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio );+}++static void b3RecDispatch_PrismaticJointSetTargetTranslation( const b3RecArgs_PrismaticJointSetTargetTranslation* a,+															  b3RecReader* rdr )+{+	b3PrismaticJoint_SetTargetTranslation( b3RecMakeJointId( rdr, a->joint ), a->translation );+}++static void b3RecDispatch_PrismaticJointEnableLimit( const b3RecArgs_PrismaticJointEnableLimit* a, b3RecReader* rdr )+{+	b3PrismaticJoint_EnableLimit( b3RecMakeJointId( rdr, a->joint ), a->enableLimit );+}++static void b3RecDispatch_PrismaticJointSetLimits( const b3RecArgs_PrismaticJointSetLimits* a, b3RecReader* rdr )+{+	b3PrismaticJoint_SetLimits( b3RecMakeJointId( rdr, a->joint ), a->lower, a->upper );+}++static void b3RecDispatch_PrismaticJointEnableMotor( const b3RecArgs_PrismaticJointEnableMotor* a, b3RecReader* rdr )+{+	b3PrismaticJoint_EnableMotor( b3RecMakeJointId( rdr, a->joint ), a->enableMotor );+}++static void b3RecDispatch_PrismaticJointSetMotorSpeed( const b3RecArgs_PrismaticJointSetMotorSpeed* a, b3RecReader* rdr )+{+	b3PrismaticJoint_SetMotorSpeed( b3RecMakeJointId( rdr, a->joint ), a->motorSpeed );+}++static void b3RecDispatch_PrismaticJointSetMaxMotorForce( const b3RecArgs_PrismaticJointSetMaxMotorForce* a, b3RecReader* rdr )+{+	b3PrismaticJoint_SetMaxMotorForce( b3RecMakeJointId( rdr, a->joint ), a->force );+}++static void b3RecDispatch_RevoluteJointEnableSpring( const b3RecArgs_RevoluteJointEnableSpring* a, b3RecReader* rdr )+{+	b3RevoluteJoint_EnableSpring( b3RecMakeJointId( rdr, a->joint ), a->enableSpring );+}++static void b3RecDispatch_RevoluteJointSetSpringHertz( const b3RecArgs_RevoluteJointSetSpringHertz* a, b3RecReader* rdr )+{+	b3RevoluteJoint_SetSpringHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz );+}++static void b3RecDispatch_RevoluteJointSetSpringDampingRatio( const b3RecArgs_RevoluteJointSetSpringDampingRatio* a,+															  b3RecReader* rdr )+{+	b3RevoluteJoint_SetSpringDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio );+}++static void b3RecDispatch_RevoluteJointSetTargetAngle( const b3RecArgs_RevoluteJointSetTargetAngle* a, b3RecReader* rdr )+{+	b3RevoluteJoint_SetTargetAngle( b3RecMakeJointId( rdr, a->joint ), a->angle );+}++static void b3RecDispatch_RevoluteJointEnableLimit( const b3RecArgs_RevoluteJointEnableLimit* a, b3RecReader* rdr )+{+	b3RevoluteJoint_EnableLimit( b3RecMakeJointId( rdr, a->joint ), a->enableLimit );+}++static void b3RecDispatch_RevoluteJointSetLimits( const b3RecArgs_RevoluteJointSetLimits* a, b3RecReader* rdr )+{+	b3RevoluteJoint_SetLimits( b3RecMakeJointId( rdr, a->joint ), a->lower, a->upper );+}++static void b3RecDispatch_RevoluteJointEnableMotor( const b3RecArgs_RevoluteJointEnableMotor* a, b3RecReader* rdr )+{+	b3RevoluteJoint_EnableMotor( b3RecMakeJointId( rdr, a->joint ), a->enableMotor );+}++static void b3RecDispatch_RevoluteJointSetMotorSpeed( const b3RecArgs_RevoluteJointSetMotorSpeed* a, b3RecReader* rdr )+{+	b3RevoluteJoint_SetMotorSpeed( b3RecMakeJointId( rdr, a->joint ), a->motorSpeed );+}++static void b3RecDispatch_RevoluteJointSetMaxMotorTorque( const b3RecArgs_RevoluteJointSetMaxMotorTorque* a, b3RecReader* rdr )+{+	b3RevoluteJoint_SetMaxMotorTorque( b3RecMakeJointId( rdr, a->joint ), a->torque );+}++static void b3RecDispatch_SphericalJointEnableConeLimit( const b3RecArgs_SphericalJointEnableConeLimit* a, b3RecReader* rdr )+{+	b3SphericalJoint_EnableConeLimit( b3RecMakeJointId( rdr, a->joint ), a->enableLimit );+}++static void b3RecDispatch_SphericalJointSetConeLimit( const b3RecArgs_SphericalJointSetConeLimit* a, b3RecReader* rdr )+{+	b3SphericalJoint_SetConeLimit( b3RecMakeJointId( rdr, a->joint ), a->angleRadians );+}++static void b3RecDispatch_SphericalJointEnableTwistLimit( const b3RecArgs_SphericalJointEnableTwistLimit* a, b3RecReader* rdr )+{+	b3SphericalJoint_EnableTwistLimit( b3RecMakeJointId( rdr, a->joint ), a->enableLimit );+}++static void b3RecDispatch_SphericalJointSetTwistLimits( const b3RecArgs_SphericalJointSetTwistLimits* a, b3RecReader* rdr )+{+	b3SphericalJoint_SetTwistLimits( b3RecMakeJointId( rdr, a->joint ), a->lower, a->upper );+}++static void b3RecDispatch_SphericalJointEnableSpring( const b3RecArgs_SphericalJointEnableSpring* a, b3RecReader* rdr )+{+	b3SphericalJoint_EnableSpring( b3RecMakeJointId( rdr, a->joint ), a->enableSpring );+}++static void b3RecDispatch_SphericalJointSetSpringHertz( const b3RecArgs_SphericalJointSetSpringHertz* a, b3RecReader* rdr )+{+	b3SphericalJoint_SetSpringHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz );+}++static void b3RecDispatch_SphericalJointSetSpringDampingRatio( const b3RecArgs_SphericalJointSetSpringDampingRatio* a,+															   b3RecReader* rdr )+{+	b3SphericalJoint_SetSpringDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio );+}++static void b3RecDispatch_SphericalJointSetTargetRotation( const b3RecArgs_SphericalJointSetTargetRotation* a, b3RecReader* rdr )+{+	b3SphericalJoint_SetTargetRotation( b3RecMakeJointId( rdr, a->joint ), a->targetRotation );+}++static void b3RecDispatch_SphericalJointEnableMotor( const b3RecArgs_SphericalJointEnableMotor* a, b3RecReader* rdr )+{+	b3SphericalJoint_EnableMotor( b3RecMakeJointId( rdr, a->joint ), a->enableMotor );+}++static void b3RecDispatch_SphericalJointSetMotorVelocity( const b3RecArgs_SphericalJointSetMotorVelocity* a, b3RecReader* rdr )+{+	b3SphericalJoint_SetMotorVelocity( b3RecMakeJointId( rdr, a->joint ), a->motorVelocity );+}++static void b3RecDispatch_SphericalJointSetMaxMotorTorque( const b3RecArgs_SphericalJointSetMaxMotorTorque* a, b3RecReader* rdr )+{+	b3SphericalJoint_SetMaxMotorTorque( b3RecMakeJointId( rdr, a->joint ), a->torque );+}++static void b3RecDispatch_WeldJointSetLinearHertz( const b3RecArgs_WeldJointSetLinearHertz* a, b3RecReader* rdr )+{+	b3WeldJoint_SetLinearHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz );+}++static void b3RecDispatch_WeldJointSetLinearDampingRatio( const b3RecArgs_WeldJointSetLinearDampingRatio* a, b3RecReader* rdr )+{+	b3WeldJoint_SetLinearDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio );+}++static void b3RecDispatch_WeldJointSetAngularHertz( const b3RecArgs_WeldJointSetAngularHertz* a, b3RecReader* rdr )+{+	b3WeldJoint_SetAngularHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz );+}++static void b3RecDispatch_WeldJointSetAngularDampingRatio( const b3RecArgs_WeldJointSetAngularDampingRatio* a, b3RecReader* rdr )+{+	b3WeldJoint_SetAngularDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio );+}++static void b3RecDispatch_WheelJointEnableSuspension( const b3RecArgs_WheelJointEnableSuspension* a, b3RecReader* rdr )+{+	b3WheelJoint_EnableSuspension( b3RecMakeJointId( rdr, a->joint ), a->flag );+}++static void b3RecDispatch_WheelJointSetSuspensionHertz( const b3RecArgs_WheelJointSetSuspensionHertz* a, b3RecReader* rdr )+{+	b3WheelJoint_SetSuspensionHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz );+}++static void b3RecDispatch_WheelJointSetSuspensionDampingRatio( const b3RecArgs_WheelJointSetSuspensionDampingRatio* a,+															   b3RecReader* rdr )+{+	b3WheelJoint_SetSuspensionDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio );+}++static void b3RecDispatch_WheelJointEnableSuspensionLimit( const b3RecArgs_WheelJointEnableSuspensionLimit* a, b3RecReader* rdr )+{+	b3WheelJoint_EnableSuspensionLimit( b3RecMakeJointId( rdr, a->joint ), a->flag );+}++static void b3RecDispatch_WheelJointSetSuspensionLimits( const b3RecArgs_WheelJointSetSuspensionLimits* a, b3RecReader* rdr )+{+	b3WheelJoint_SetSuspensionLimits( b3RecMakeJointId( rdr, a->joint ), a->lower, a->upper );+}++static void b3RecDispatch_WheelJointEnableSpinMotor( const b3RecArgs_WheelJointEnableSpinMotor* a, b3RecReader* rdr )+{+	b3WheelJoint_EnableSpinMotor( b3RecMakeJointId( rdr, a->joint ), a->flag );+}++static void b3RecDispatch_WheelJointSetSpinMotorSpeed( const b3RecArgs_WheelJointSetSpinMotorSpeed* a, b3RecReader* rdr )+{+	b3WheelJoint_SetSpinMotorSpeed( b3RecMakeJointId( rdr, a->joint ), a->speed );+}++static void b3RecDispatch_WheelJointSetMaxSpinTorque( const b3RecArgs_WheelJointSetMaxSpinTorque* a, b3RecReader* rdr )+{+	b3WheelJoint_SetMaxSpinTorque( b3RecMakeJointId( rdr, a->joint ), a->torque );+}++static void b3RecDispatch_WheelJointEnableSteering( const b3RecArgs_WheelJointEnableSteering* a, b3RecReader* rdr )+{+	b3WheelJoint_EnableSteering( b3RecMakeJointId( rdr, a->joint ), a->flag );+}++static void b3RecDispatch_WheelJointSetSteeringHertz( const b3RecArgs_WheelJointSetSteeringHertz* a, b3RecReader* rdr )+{+	b3WheelJoint_SetSteeringHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz );+}++static void b3RecDispatch_WheelJointSetSteeringDampingRatio( const b3RecArgs_WheelJointSetSteeringDampingRatio* a,+															 b3RecReader* rdr )+{+	b3WheelJoint_SetSteeringDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio );+}++static void b3RecDispatch_WheelJointSetMaxSteeringTorque( const b3RecArgs_WheelJointSetMaxSteeringTorque* a, b3RecReader* rdr )+{+	b3WheelJoint_SetMaxSteeringTorque( b3RecMakeJointId( rdr, a->joint ), a->torque );+}++static void b3RecDispatch_WheelJointEnableSteeringLimit( const b3RecArgs_WheelJointEnableSteeringLimit* a, b3RecReader* rdr )+{+	b3WheelJoint_EnableSteeringLimit( b3RecMakeJointId( rdr, a->joint ), a->flag );+}++static void b3RecDispatch_WheelJointSetSteeringLimits( const b3RecArgs_WheelJointSetSteeringLimits* a, b3RecReader* rdr )+{+	b3WheelJoint_SetSteeringLimits( b3RecMakeJointId( rdr, a->joint ), a->lower, a->upper );+}++static void b3RecDispatch_WheelJointSetTargetSteeringAngle( const b3RecArgs_WheelJointSetTargetSteeringAngle* a,+															b3RecReader* rdr )+{+	b3WheelJoint_SetTargetSteeringAngle( b3RecMakeJointId( rdr, a->joint ), a->radians );+}++static void b3RecDispatch_StateHash( const b3RecArgs_StateHash* a, b3RecReader* rdr )+{+	b3World* world = b3GetWorldFromId( rdr->replayWorldId );+	uint64_t computed = b3HashWorldState( world );+	if ( computed != a->hash )+	{+		printf( "b3ReplayFile: StateHash mismatch (recorded=0x%" PRIx64 ", computed=0x%" PRIx64 ")\n", a->hash, computed );+		rdr->diverged = true;+	}+}++static void b3RecDispatch_RecordingBounds( const b3RecArgs_RecordingBounds* a, b3RecReader* rdr )+{+	// Primary resolve is the open-time scan, this keeps the value right if it ever moves earlier+	if ( rdr->owner != NULL )+	{+		rdr->owner->bounds = a->bounds;+	}+}++// Spatial query replay. The recorded inputs come through the manifest; here the variable-length hit+// tail is read back, the query re-issued against the replay world, and each callback hit compared to+// what was recorded. Any mismatch latches rdr->diverged. When a player owns the reader the hits are+// also stashed for the viewer overlay. The stash helpers dereference the player struct, defined later+// in this file, so they are forward declared and implemented in Block B below.++static void b3RecGrow( void** data, int* capacity, int need, int keep, int elemSize );+static b3RecDrawQuery* b3RecStashQueryBegin( b3RecPlayer* player, int kind, const b3RecRecordedHit* hits, int hitCount );++// Grow the reader's hit scratch to at least n entries, preserving contents. n is bounded by the file+// size since every recorded hit consumes at least one byte, so a corrupt count fails the read.+void b3RecEnsureHits( b3RecReader* rdr, int n )+{+	b3RecReserveScratch( rdr, (void**)&rdr->hits, &rdr->hitCap, n, (int)sizeof( b3RecRecordedHit ) );+}++// Bitwise float compare so the determinism check is exact, not within a tolerance.+static bool b3RecF32Differs( float a, float b )+{+	uint32_t ua, ub;+	memcpy( &ua, &a, 4 );+	memcpy( &ub, &b, 4 );+	return ua != ub;+}++static bool b3RecVec3Differs( b3Vec3 a, b3Vec3 b )+{+	return b3RecF32Differs( a.x, b.x ) || b3RecF32Differs( a.y, b.y ) || b3RecF32Differs( a.z, b.z );+}++// Shared context for the replay trampolines: walks recorded hits in order, flagging any divergence+// from the re-issued query.+typedef struct b3RecReplayQueryCtx+{+	b3RecReader* rdr;+	const b3RecRecordedHit* hits;+	int count;+	int cursor;+} b3RecReplayQueryCtx;++static bool b3RecReplayOverlapTrampoline( b3ShapeId id, void* ctx )+{+	b3RecReplayQueryCtx* rc = ctx;+	if ( rc->cursor >= rc->count )+	{+		rc->rdr->diverged = true;+		return false;+	}+	const b3RecRecordedHit* h = &rc->hits[rc->cursor++];+	if ( id.index1 != h->id.index1 || id.generation != h->id.generation )+	{+		rc->rdr->diverged = true;+	}+	return h->userReturnB;+}++// The mover filter has the same bool(shapeId, ctx) shape as an overlap callback, so it replays the+// same way. A distinct typed wrapper keeps the function-pointer types clean.+static bool b3RecReplayMoverFilterTrampoline( b3ShapeId id, void* ctx )+{+	return b3RecReplayOverlapTrampoline( id, ctx );+}++static float b3RecReplayCastTrampoline( b3ShapeId id, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId,+										int triangleIndex, int childIndex, void* ctx )+{+	b3RecReplayQueryCtx* rc = ctx;+	if ( rc->cursor >= rc->count )+	{+		rc->rdr->diverged = true;+		return 0.0f;+	}+	const b3RecRecordedHit* h = &rc->hits[rc->cursor++];+	// Positions compared through a full-width delta, truncating both sides would pass vacuously far+	// from the origin.+	if ( id.index1 != h->id.index1 || id.generation != h->id.generation ||+		 b3RecVec3Differs( b3SubPos( point, h->point ), b3Vec3_zero ) || b3RecVec3Differs( normal, h->normal ) ||+		 b3RecF32Differs( fraction, h->fraction ) || userMaterialId != h->userMaterialId || triangleIndex != h->triangleIndex ||+		 childIndex != h->childIndex )+	{+		rc->rdr->diverged = true;+	}+	return h->userReturnF;+}++// 3D delivers a whole shape's planes in one call. The recorded group starts at the cursor with its+// plane count and user return replicated on each hit, so compare the batch and advance by the+// recorded count to stay aligned with the stream even when the count itself diverged.+static bool b3RecReplayPlaneTrampoline( b3ShapeId id, const b3PlaneResult* planes, int planeCount, void* ctx )+{+	b3RecReplayQueryCtx* rc = ctx;+	if ( rc->cursor >= rc->count )+	{+		rc->rdr->diverged = true;+		return true;+	}+	const b3RecRecordedHit* head = &rc->hits[rc->cursor];+	int recordedCount = head->planeCount;+	bool ret = head->userReturnB;+	if ( id.index1 != head->id.index1 || id.generation != head->id.generation || recordedCount != planeCount )+	{+		rc->rdr->diverged = true;+	}+	int n = recordedCount < planeCount ? recordedCount : planeCount;+	for ( int i = 0; i < n; ++i )+	{+		const b3RecRecordedHit* h = &rc->hits[rc->cursor + i];+		if ( b3RecVec3Differs( h->plane.plane.normal, planes[i].plane.normal ) ||+			 b3RecF32Differs( h->plane.plane.offset, planes[i].plane.offset ) ||+			 b3RecVec3Differs( h->plane.point, planes[i].point ) )+		{+			rc->rdr->diverged = true;+		}+	}+	rc->cursor += recordedCount;+	return ret;+}++// Copy a decoded proxy's points into a draw record so the overlay does not depend on reader scratch.+static void b3RecStashProxy( b3RecDrawQuery* q, const b3ShapeProxy* proxy )+{+	int count = proxy->count;+	if ( count > B3_MAX_SHAPE_CAST_POINTS )+		count = B3_MAX_SHAPE_CAST_POINTS;+	q->proxyCount = count;+	q->proxyRadius = proxy->radius;+	for ( int i = 0; i < count; ++i )+	{+		q->proxyPoints[i] = proxy->points[i];+	}+}++// Tight world-space bounds of a query's swept geometry, so the viewer can frame any query and not+// just the overlap AABB. Mover and proxy points are origin relative. A cast sweeps the shape from the+// origin to origin plus translation. The overlap AABB is already a world-space box.+static void b3RecComputeQueryBounds( b3RecDrawQuery* q )+{+	if ( q->kind == B3_RECQ_OVERLAP_AABB )+	{+		return;+	}++	// Shape points relative to the origin, plus the fattening radius. A ray has no shape, so it falls+	// through to a single point at the origin.+	b3Vec3 local[B3_MAX_SHAPE_CAST_POINTS];+	int count = 0;+	float radius = 0.0f;+	switch ( q->kind )+	{+		case B3_RECQ_CAST_MOVER:+		case B3_RECQ_COLLIDE_MOVER:+			local[0] = q->mover.center1;+			local[1] = q->mover.center2;+			count = 2;+			radius = q->mover.radius;+			break;++		case B3_RECQ_OVERLAP_SHAPE:+		case B3_RECQ_CAST_SHAPE:+			count = q->proxyCount;+			for ( int i = 0; i < count; ++i )+			{+				local[i] = q->proxyPoints[i];+			}+			radius = q->proxyRadius;+			break;++		default:+			break;+	}+	if ( count == 0 )+	{+		local[0] = b3Vec3_zero;+		count = 1;+	}++	// Sweep each point across the translation. A non-cast query has zero translation, so both ends+	// coincide and the duplicates fold away.+	b3Pos end = b3OffsetPos( q->origin, q->translation );+	b3Vec3 world[2 * B3_MAX_SHAPE_CAST_POINTS];+	int n = 0;+	for ( int i = 0; i < count; ++i )+	{+		world[n++] = b3ToVec3( b3OffsetPos( q->origin, local[i] ) );+		world[n++] = b3ToVec3( b3OffsetPos( end, local[i] ) );+	}+	q->aabb = b3MakeAABB( world, n, radius );+}++static void b3RecDispatch_QueryOverlapAABB( const b3RecArgs_QueryOverlapAABB* a, b3RecReader* rdr )+{+	uint32_t n = b3RecR_U32( rdr );+	b3RecEnsureHits( rdr, (int)n );+	if ( !rdr->ok )+		return;+	for ( uint32_t i = 0; i < n; ++i )+	{+		rdr->hits[i].id = b3RecMakeShapeId( rdr, b3RecR_SHAPEID( rdr ) );+		rdr->hits[i].userReturnB = b3RecR_BOOL( rdr );+	}+	(void)b3RecR_TREESTATS( rdr );+	if ( !rdr->ok )+		return;+	b3RecReplayQueryCtx rc = { rdr, rdr->hits, (int)n, 0 };+	b3World_OverlapAABB( rdr->replayWorldId, a->aabb, a->filter, b3RecReplayOverlapTrampoline, &rc );+	if ( rc.cursor != (int)n )+		rdr->diverged = true;+	if ( rdr->owner )+	{+		b3RecDrawQuery* q = b3RecStashQueryBegin( rdr->owner, B3_RECQ_OVERLAP_AABB, rdr->hits, (int)n );+		q->filter = a->filter;+		q->aabb = a->aabb;+		b3RecComputeQueryBounds( q );+	}+}++static void b3RecDispatch_QueryOverlapShape( const b3RecArgs_QueryOverlapShape* a, b3RecReader* rdr )+{+	uint32_t n = b3RecR_U32( rdr );+	b3RecEnsureHits( rdr, (int)n );+	if ( !rdr->ok )+		return;+	for ( uint32_t i = 0; i < n; ++i )+	{+		rdr->hits[i].id = b3RecMakeShapeId( rdr, b3RecR_SHAPEID( rdr ) );+		rdr->hits[i].userReturnB = b3RecR_BOOL( rdr );+	}+	(void)b3RecR_TREESTATS( rdr );+	if ( !rdr->ok )+		return;+	b3RecReplayQueryCtx rc = { rdr, rdr->hits, (int)n, 0 };+	b3World_OverlapShape( rdr->replayWorldId, a->origin, &a->proxy, a->filter, b3RecReplayOverlapTrampoline, &rc );+	if ( rc.cursor != (int)n )+		rdr->diverged = true;+	if ( rdr->owner )+	{+		b3RecDrawQuery* q = b3RecStashQueryBegin( rdr->owner, B3_RECQ_OVERLAP_SHAPE, rdr->hits, (int)n );+		q->filter = a->filter;+		q->origin = a->origin;+		b3RecStashProxy( q, &a->proxy );+		b3RecComputeQueryBounds( q );+	}+}++static void b3RecDispatch_QueryCastRay( const b3RecArgs_QueryCastRay* a, b3RecReader* rdr )+{+	uint32_t n = b3RecR_U32( rdr );+	b3RecEnsureHits( rdr, (int)n );+	if ( !rdr->ok )+		return;+	for ( uint32_t i = 0; i < n; ++i )+	{+		rdr->hits[i].id = b3RecMakeShapeId( rdr, b3RecR_SHAPEID( rdr ) );+		rdr->hits[i].point = b3RecR_POSITION( rdr );+		rdr->hits[i].normal = b3RecR_VEC3( rdr );+		rdr->hits[i].fraction = b3RecR_F32( rdr );+		rdr->hits[i].userMaterialId = b3RecR_U64( rdr );+		rdr->hits[i].triangleIndex = b3RecR_I32( rdr );+		rdr->hits[i].childIndex = b3RecR_I32( rdr );+		rdr->hits[i].userReturnF = b3RecR_F32( rdr );+	}+	(void)b3RecR_TREESTATS( rdr );+	if ( !rdr->ok )+		return;+	b3RecReplayQueryCtx rc = { rdr, rdr->hits, (int)n, 0 };+	b3World_CastRay( rdr->replayWorldId, a->origin, a->translation, a->filter, b3RecReplayCastTrampoline, &rc );+	if ( rc.cursor != (int)n )+		rdr->diverged = true;+	if ( rdr->owner )+	{+		b3RecDrawQuery* q = b3RecStashQueryBegin( rdr->owner, B3_RECQ_CAST_RAY, rdr->hits, (int)n );+		q->filter = a->filter;+		q->origin = a->origin;+		q->translation = a->translation;+		b3RecComputeQueryBounds( q );+	}+}++static void b3RecDispatch_QueryCastShape( const b3RecArgs_QueryCastShape* a, b3RecReader* rdr )+{+	uint32_t n = b3RecR_U32( rdr );+	b3RecEnsureHits( rdr, (int)n );+	if ( !rdr->ok )+		return;+	for ( uint32_t i = 0; i < n; ++i )+	{+		rdr->hits[i].id = b3RecMakeShapeId( rdr, b3RecR_SHAPEID( rdr ) );+		rdr->hits[i].point = b3RecR_POSITION( rdr );+		rdr->hits[i].normal = b3RecR_VEC3( rdr );+		rdr->hits[i].fraction = b3RecR_F32( rdr );+		rdr->hits[i].userMaterialId = b3RecR_U64( rdr );+		rdr->hits[i].triangleIndex = b3RecR_I32( rdr );+		rdr->hits[i].childIndex = b3RecR_I32( rdr );+		rdr->hits[i].userReturnF = b3RecR_F32( rdr );+	}+	(void)b3RecR_TREESTATS( rdr );+	if ( !rdr->ok )+		return;+	b3RecReplayQueryCtx rc = { rdr, rdr->hits, (int)n, 0 };+	b3World_CastShape( rdr->replayWorldId, a->origin, &a->proxy, a->translation, a->filter, b3RecReplayCastTrampoline, &rc );+	if ( rc.cursor != (int)n )+		rdr->diverged = true;+	if ( rdr->owner )+	{+		b3RecDrawQuery* q = b3RecStashQueryBegin( rdr->owner, B3_RECQ_CAST_SHAPE, rdr->hits, (int)n );+		q->filter = a->filter;+		q->origin = a->origin;+		q->translation = a->translation;+		b3RecStashProxy( q, &a->proxy );+		b3RecComputeQueryBounds( q );+	}+}++static void b3RecDispatch_QueryCastRayClosest( const b3RecArgs_QueryCastRayClosest* a, b3RecReader* rdr )+{+	b3RayResult rec = b3RecR_RAYRESULT( rdr );+	if ( !rdr->ok )+		return;+	b3RayResult got = b3World_CastRayClosest( rdr->replayWorldId, a->origin, a->translation, a->filter );+	b3ShapeId recId = b3RecMakeShapeId( rdr, rec.shapeId );+	if ( got.hit != rec.hit ||+		 ( got.hit &&+		   ( got.shapeId.index1 != recId.index1 || got.shapeId.generation != recId.generation ||+			 b3RecVec3Differs( b3SubPos( got.point, rec.point ), b3Vec3_zero ) || b3RecVec3Differs( got.normal, rec.normal ) ||+			 b3RecF32Differs( got.fraction, rec.fraction ) || got.userMaterialId != rec.userMaterialId ) ) )+	{+		rdr->diverged = true;+	}+	if ( rdr->owner )+	{+		// Stash the closest result as a single pooled hit so the shared draw loop renders its point.+		b3RecRecordedHit h = { 0 };+		h.id = recId;+		h.point = rec.point;+		h.normal = rec.normal;+		h.fraction = rec.fraction;+		b3RecDrawQuery* q = b3RecStashQueryBegin( rdr->owner, B3_RECQ_CAST_RAY_CLOSEST, &h, rec.hit ? 1 : 0 );+		q->filter = a->filter;+		q->origin = a->origin;+		q->translation = a->translation;+		q->rayResult = rec;+		b3RecComputeQueryBounds( q );+	}+}++static void b3RecDispatch_QueryCastMover( const b3RecArgs_QueryCastMover* a, b3RecReader* rdr )+{+	uint32_t n = b3RecR_U32( rdr );+	b3RecEnsureHits( rdr, (int)n );+	if ( !rdr->ok )+		return;+	for ( uint32_t i = 0; i < n; ++i )+	{+		rdr->hits[i].id = b3RecMakeShapeId( rdr, b3RecR_SHAPEID( rdr ) );+		rdr->hits[i].userReturnB = b3RecR_BOOL( rdr );+	}+	float recFraction = b3RecR_F32( rdr );+	if ( !rdr->ok )+		return;+	b3RecReplayQueryCtx rc = { rdr, rdr->hits, (int)n, 0 };+	float got = b3World_CastMover( rdr->replayWorldId, a->origin, &a->mover, a->translation, a->filter,+								   b3RecReplayMoverFilterTrampoline, &rc );+	if ( rc.cursor != (int)n || b3RecF32Differs( got, recFraction ) )+		rdr->diverged = true;+	if ( rdr->owner )+	{+		b3RecDrawQuery* q = b3RecStashQueryBegin( rdr->owner, B3_RECQ_CAST_MOVER, NULL, 0 );+		q->filter = a->filter;+		q->origin = a->origin;+		q->mover = a->mover;+		q->translation = a->translation;+		q->castFraction = recFraction;+		b3RecComputeQueryBounds( q );+	}+}++static void b3RecDispatch_QueryCollideMover( const b3RecArgs_QueryCollideMover* a, b3RecReader* rdr )+{+	// Recorded as shapeCount groups, each: shapeId, planeCount, planeCount planes, user return. Flatten+	// into one hit per plane with the group's count and return replicated, so the replay walker can+	// re-group per shape.+	uint32_t shapeCount = b3RecR_U32( rdr );+	int total = 0;+	for ( uint32_t s = 0; s < shapeCount; ++s )+	{+		b3ShapeId id = b3RecMakeShapeId( rdr, b3RecR_SHAPEID( rdr ) );+		int planeCount = b3RecR_I32( rdr );+		if ( planeCount < 0 )+			planeCount = 0;+		b3RecEnsureHits( rdr, total + planeCount );+		if ( !rdr->ok )+			return;+		for ( int i = 0; i < planeCount; ++i )+		{+			rdr->hits[total + i].plane = b3RecR_PLANERESULT( rdr );+		}+		bool ret = b3RecR_BOOL( rdr );+		for ( int i = 0; i < planeCount; ++i )+		{+			rdr->hits[total + i].id = id;+			rdr->hits[total + i].planeCount = planeCount;+			rdr->hits[total + i].userReturnB = ret;+		}+		total += planeCount;+	}+	if ( !rdr->ok )+		return;+	b3RecReplayQueryCtx rc = { rdr, rdr->hits, total, 0 };+	b3World_CollideMover( rdr->replayWorldId, a->origin, &a->mover, a->filter, b3RecReplayPlaneTrampoline, &rc );+	if ( rc.cursor != total )+		rdr->diverged = true;+	if ( rdr->owner )+	{+		b3RecDrawQuery* q = b3RecStashQueryBegin( rdr->owner, B3_RECQ_COLLIDE_MOVER, rdr->hits, total );+		q->filter = a->filter;+		q->origin = a->origin;+		q->mover = a->mover;+		b3RecComputeQueryBounds( q );+	}+}++// Stash the identity key of the query that immediately follows. Consumed by the next stash.+static void b3RecDispatch_QueryTag( const b3RecArgs_QueryTag* a, b3RecReader* rdr )+{+	rdr->pendingQueryKey = a->key;+}++// X-macro dispatch switch: read opcode+u24 payloadSize, dispatch, skip unknown ops.+// Returns the opcode dispatched, or -1 when the stream is exhausted or broken.++static int b3RecDispatchOne( b3RecReader* rdr )+{+	if ( rdr->cursor >= rdr->size || !rdr->ok )+	{+		return -1;+	}+	uint8_t opcode = b3RecR_U8( rdr );+	uint32_t payloadSize = b3RecR_U24( rdr );+	if ( !rdr->ok )+	{+		return -1;+	}++	int payloadStart = rdr->cursor;++	switch ( opcode )+	{+#define ARG( TAG, field ) a.field = b3RecR_##TAG( rdr );+#define B3_REC_OP( op, Name, RET, ... )                                                                                          \+	case op:                                                                                                                     \+	{                                                                                                                            \+		b3RecArgs_##Name a;                                                                                                      \+		memset( &a, 0, sizeof( a ) );                                                                                            \+		__VA_ARGS__                                                                                                              \+		if ( rdr->ok )                                                                                                           \+		{                                                                                                                        \+			b3RecDispatch_##Name( &a, rdr );                                                                                     \+		}                                                                                                                        \+		break;                                                                                                                   \+	}+#include "recording_ops.inl"+#undef B3_REC_OP+#undef ARG+		default:+			printf( "b3ReplayFile: unknown opcode 0x%02X, skipping %u bytes\n", opcode, payloadSize );+			// payloadStart is in bounds, so size - payloadStart is the bytes left to skip over+			if ( payloadSize > (uint32_t)( rdr->size - payloadStart ) )+			{+				rdr->ok = false;+			}+			else+			{+				rdr->cursor = payloadStart + (int)payloadSize;+			}+			break;+	}+	return (int)(unsigned)opcode;+}++// Public entry point++bool b3ValidateReplay( const void* data, int size, int workerCount )+{+	b3RecPlayer* player = b3CreatePlayer( data, size, workerCount );+	if ( player == NULL )+	{+		return false;+	}++	while ( b3RecPlayer_StepFrame( player ) )+	{+		if ( player->rdr.diverged )+		{+			break;+		}+	}++	bool ok = player->rdr.ok && player->rdr.diverged == false;+	b3DestroyPlayer( player );+	return ok;+}++// b3RecPlayer implementation++#define B3_REC_KEYFRAME_INTERVAL_DEFAULT 16+#define B3_REC_KEYFRAME_BUDGET_DEFAULT ( (size_t)512 * 1024 * 1024 )++// Overflow-safe growth for the player's accumulating arrays. Counts come from the replay itself,+// not the file, so this only guards the byte-size multiply. Preserves keep elements.+static void b3RecGrow( void** data, int* capacity, int need, int keep, int elemSize )+{+	if ( need <= *capacity )+	{+		return;+	}+	int newCap = *capacity == 0 ? 8 : 2 * *capacity;+	if ( newCap < need )+	{+		newCap = need;+	}+	void* grown = b3Alloc( (size_t)newCap * (size_t)elemSize );+	if ( *data != NULL )+	{+		if ( keep > 0 )+		{+			memcpy( grown, *data, (size_t)keep * (size_t)elemSize );+		}+		b3Free( *data, (size_t)*capacity * (size_t)elemSize );+	}+	*data = grown;+	*capacity = newCap;+}++// Block B: per-frame query store helpers. Forward declared above the query dispatchers, which run+// before the player struct is defined, so the player-dereferencing code lives here.++static void b3RecGrowFrameQueries( b3RecPlayer* player )+{+	b3RecGrow( (void**)&player->frameQueries, &player->frameQueryCap, player->frameQueryCount + 1, player->frameQueryCount,+			   (int)sizeof( b3RecDrawQuery ) );+}++static void b3RecGrowFrameHits( b3RecPlayer* player, int need )+{+	b3RecGrow( (void**)&player->frameHits, &player->frameHitCap, player->frameHitCount + need, player->frameHitCount,+			   (int)sizeof( b3RecRecordedHit ) );+}++// Push a draw record for one query and copy its hits into the per-frame store. Ids in hits[] are+// already remapped to the replay world by the dispatcher.+static b3RecDrawQuery* b3RecStashQueryBegin( b3RecPlayer* player, int kind, const b3RecRecordedHit* hits, int hitCount )+{+	b3RecGrowFrameQueries( player );+	b3RecDrawQuery* q = &player->frameQueries[player->frameQueryCount];+	memset( q, 0, sizeof( *q ) );+	q->kind = kind;+	// Pair the query with the key from its preceding QueryTag op, if any, then clear it so the next+	// untagged query reads 0.+	q->key = player->rdr.pendingQueryKey;+	player->rdr.pendingQueryKey = 0;+	q->hitStart = player->frameHitCount;+	q->hitCount = hitCount;+	b3RecGrowFrameHits( player, hitCount );+	for ( int i = 0; i < hitCount; ++i )+	{+		player->frameHits[player->frameHitCount + i] = hits[i];+	}+	player->frameHitCount += hitCount;+	player->frameQueryCount++;+	return q;+}++// Append a created body to the outliner list. Ordinals are creation order and never reused.+static void b3RecTrackBodyCreate( b3RecPlayer* player, b3BodyId id )+{+	b3RecGrow( (void**)&player->bodyIds, &player->bodyIdCap, player->bodyIdCount + 1, player->bodyIdCount,+			   (int)sizeof( b3BodyId ) );+	player->bodyIds[player->bodyIdCount] = id;+	player->bodyIdCount += 1;+}++// Leave a hole so later ordinals do not shift, keeping a stored selection stable.+static void b3RecTrackBodyDestroy( b3RecPlayer* player, b3BodyId id )+{+	for ( int i = 0; i < player->bodyIdCount; ++i )+	{+		if ( B3_ID_EQUALS( player->bodyIds[i], id ) )+		{+			player->bodyIds[i] = b3_nullBodyId;+			return;+		}+	}+}++// Snapshot bodies are restored as a struct image and never hit the CreateBody hook the tracker keys+// on, so the seed world must be walked once to populate the outliner list. Slot order is stable.+static void b3RecSeedBodyIds( b3RecPlayer* player )+{+	b3World* world = b3GetWorldFromId( player->rdr.replayWorldId );+	player->bodyIdCount = 0;+	int count = world->bodies.count;+	for ( int i = 0; i < count; ++i )+	{+		if ( world->bodies.data[i].id != i )+		{+			continue; // free slot+		}+		b3RecTrackBodyCreate( player, b3MakeBodyId( world, i ) );+	}+}++// Seed the outliner list from the current world and save it as the frame-0 restore copy.+static void b3RecSeedFrame0BodyIds( b3RecPlayer* player )+{+	b3RecSeedBodyIds( player );+	if ( player->frame0BodyIds != NULL )+	{+		b3Free( player->frame0BodyIds, (size_t)player->frame0BodyIdCount * sizeof( b3BodyId ) );+		player->frame0BodyIds = NULL;+	}+	player->frame0BodyIdCount = player->bodyIdCount;+	if ( player->bodyIdCount > 0 )+	{+		player->frame0BodyIds = (b3BodyId*)b3Alloc( (size_t)player->bodyIdCount * sizeof( b3BodyId ) );+		memcpy( player->frame0BodyIds, player->bodyIds, (size_t)player->bodyIdCount * sizeof( b3BodyId ) );+	}+}++// Tag key to tag index, so the viewer resolves a query's caller id and label in O(1) instead of a+// linear scan over the tag table.+#define NAME b3RecTagLookup+#define KEY_TY uint64_t+#define VAL_TY uint32_t+#define HASH_FN vt_hash_integer+#define CMPR_FN vt_cmpr_integer+#define MALLOC_FN b3Alloc+#define FREE_FN b3Free+#include "verstable.h"++// Read the optional query-tag table trailing the geometry entries: u32 tagCount then per tag+// { u64 key, u64 id, u16 len, name bytes }. A recording written before the tag table leaves rp at+// dataEnd, so nothing loads. Bounds-checked; tagCount reflects only the tags that fully fit, so a+// truncated tail loads what it can and reports the real count.+static void b3RecLoadTags( b3RecReader* rdr, const uint8_t* rp, const uint8_t* dataEnd )+{+	b3RecReader sub = { 0 };+	sub.data = rp;+	sub.size = (int)( dataEnd - rp );+	sub.ok = true;++	uint32_t count = b3RecR_U32( &sub );+	if ( sub.ok == false || count == 0 )+	{+		return;+	}++	// Each tag is at least 18 bytes (8 key + 8 id + 2 length). Reject a count that cannot fit the+	// remaining bytes so a corrupt table cannot request a wild allocation.+	if ( (size_t)count > (size_t)( sub.size - sub.cursor ) / 18 )+	{+		return;+	}++	b3RecTag* tags = (b3RecTag*)b3Alloc( (size_t)count * sizeof( b3RecTag ) );+	memset( tags, 0, (size_t)count * sizeof( b3RecTag ) );++	b3RecTagLookup* map = (b3RecTagLookup*)b3Alloc( sizeof( b3RecTagLookup ) );+	b3RecTagLookup_init( map );++	uint32_t loaded = 0;+	for ( uint32_t i = 0; i < count; ++i )+	{+		uint64_t key = b3RecR_U64( &sub );+		uint64_t id = b3RecR_U64( &sub );+		uint16_t len = b3RecR_U16( &sub );+		if ( sub.ok == false )+		{+			break;+		}+		if ( len == 0xFFFFu )+		{+			len = 0; // a null name is written as 0xFFFF+		}+		if ( (int64_t)sub.cursor + (int64_t)len > (int64_t)sub.size )+		{+			break;+		}+		int n = len > B3_MAX_QUERY_NAME_LENGTH ? B3_MAX_QUERY_NAME_LENGTH : (int)len;+		tags[loaded].key = key;+		tags[loaded].id = id;+		if ( n > 0 )+		{+			memcpy( tags[loaded].queryName, sub.data + sub.cursor, (size_t)n );+		}+		tags[loaded].queryName[n] = '\0';+		sub.cursor += len;+		b3RecTagLookup_insert( map, key, loaded );+		loaded += 1;+	}++	rdr->tags = tags;+	rdr->tagCount = (int)loaded;+	rdr->tagCapacity = (int)count;+	rdr->tagMap = map;+}++// Load the trailing registry block and fill rdr->slots/slotCount, then the optional tag table.+// Returns true on success. On failure sets rdr->ok = false and returns false.+static bool b3RecLoadSlots( b3RecReader* rdr, const void* data, int size, uint64_t registryOffset, uint64_t registryByteCount )+{+	if ( registryOffset == 0 || registryByteCount == 0 )+	{+		rdr->slots = NULL;+		rdr->slotCount = 0;+		return true;+	}++	int regStart = (int)registryOffset;+	int regEnd = regStart + (int)registryByteCount;+	if ( regEnd > size )+	{+		printf( "b3ReplayFile: registry block out of bounds\n" );+		return false;+	}+	if ( regStart + 4 > size )+	{+		printf( "b3ReplayFile: registry too small\n" );+		return false;+	}++	const uint8_t* dataEnd = (const uint8_t*)data + regEnd;+	const uint8_t* rp = (const uint8_t*)data + regStart;+	uint32_t count = (uint32_t)rp[0] | ( (uint32_t)rp[1] << 8 ) | ( (uint32_t)rp[2] << 16 ) | ( (uint32_t)rp[3] << 24 );+	rp += 4;++	if ( count == 0 )+	{+		rdr->slots = NULL;+		rdr->slotCount = 0;+		b3RecLoadTags( rdr, rp, dataEnd );+		return true;+	}++	// Each entry is at least 5 bytes (kind + 4-byte length). A count that cannot fit the remaining+	// registry bytes is a corrupt header, so reject it before allocating.+	if ( rp > dataEnd || (size_t)count > (size_t)( dataEnd - rp ) / 5 )+	{+		printf( "b3ReplayFile: registry count out of range\n" );+		return false;+	}++	b3RegistrySlot* slots = (b3RegistrySlot*)b3Alloc( (size_t)count * sizeof( b3RegistrySlot ) );+	memset( slots, 0, (size_t)count * sizeof( b3RegistrySlot ) );++	for ( uint32_t i = 0; i < count; ++i )+	{+		if ( rp + 5 > dataEnd )+		{+			printf( "b3ReplayFile: registry truncated at entry %u\n", i );+			for ( uint32_t j = 0; j < i; ++j )+			{+				if ( slots[j].bytes != NULL )+				{+					b3Free( slots[j].bytes, (size_t)slots[j].byteCount );+				}+			}+			b3Free( slots, (size_t)count * sizeof( b3RegistrySlot ) );+			return false;+		}+		uint8_t kind = rp[0];+		uint32_t byteCount = (uint32_t)rp[1] | ( (uint32_t)rp[2] << 8 ) | ( (uint32_t)rp[3] << 16 ) | ( (uint32_t)rp[4] << 24 );+		rp += 5;+		if ( rp + byteCount > dataEnd )+		{+			printf( "b3ReplayFile: registry entry %u bytes out of bounds\n", i );+			for ( uint32_t j = 0; j < i; ++j )+			{+				if ( slots[j].bytes != NULL )+				{+					b3Free( slots[j].bytes, (size_t)slots[j].byteCount );+				}+			}+			b3Free( slots, (size_t)count * sizeof( b3RegistrySlot ) );+			return false;+		}+		uint8_t* bytes = (uint8_t*)b3Alloc( byteCount > 0 ? (size_t)byteCount : 1u );+		if ( byteCount > 0 )+		{+			memcpy( bytes, rp, (size_t)byteCount );+		}+		rp += byteCount;+		slots[i].kind = (b3GeometryKind)kind;+		slots[i].byteCount = (int)byteCount;+		slots[i].bytes = bytes;+		slots[i].live = NULL;+	}++	rdr->slots = slots;+	rdr->slotCount = (int)count;+	b3RecLoadTags( rdr, rp, dataEnd );+	return true;+}++// Free slots loaded by b3RecLoadSlots.+static void b3RecFreeSlots( b3RegistrySlot* slots, int slotCount )+{+	if ( slots == NULL )+	{+		return;+	}+	for ( int i = 0; i < slotCount; ++i )+	{+		b3RegistrySlot* slot = slots + i;+		if ( slot->live != NULL )+		{+			switch ( slot->kind )+			{+				// Mesh and height field have no separate live object; they borrow the bytes freed below.+				case b3_geometryCompound:+					b3Free( slot->live, (size_t)slot->byteCount );+					break;+				default:+					break;+			}+		}+		if ( slot->bytes != NULL )+		{+			b3Free( slot->bytes, slot->byteCount > 0 ? (size_t)slot->byteCount : 1u );+		}+	}+	b3Free( slots, (size_t)slotCount * sizeof( b3RegistrySlot ) );+}++// Walk the op stream once without dispatching: count Step ops and grab the first step's tuning.+static void b3RecScanFile( b3RecPlayer* player )+{+	const uint8_t* data = player->data;+	int size = player->registryEnd;+	int cursor = player->headerEnd;+	int frameCount = 0;+	bool gotStep = false;++	while ( cursor + 4 <= size )+	{+		uint8_t opcode = data[cursor];+		uint32_t payloadSize =+			(uint32_t)data[cursor + 1] | ( (uint32_t)data[cursor + 2] << 8 ) | ( (uint32_t)data[cursor + 3] << 16 );+		int payloadStart = cursor + 4;+		if ( payloadStart + (int)payloadSize > size )+		{+			break;+		}+		if ( opcode == b3_recOpStep )+		{+			frameCount += 1;+			if ( !gotStep && payloadSize >= 12 )+			{+				uint32_t dtBits = (uint32_t)data[payloadStart + 4] | ( (uint32_t)data[payloadStart + 5] << 8 ) |+								  ( (uint32_t)data[payloadStart + 6] << 16 ) | ( (uint32_t)data[payloadStart + 7] << 24 );+				memcpy( &player->recordedDt, &dtBits, 4 );+				player->recordedSubStepCount =+					(int)( (uint32_t)data[payloadStart + 8] | ( (uint32_t)data[payloadStart + 9] << 8 ) |+						   ( (uint32_t)data[payloadStart + 10] << 16 ) | ( (uint32_t)data[payloadStart + 11] << 24 ) );+				gotStep = true;+			}+		}+		else if ( opcode == 0xF2 && payloadSize >= (uint32_t)sizeof( b3AABB ) ) // RecordingBounds+		{+			// Payload is a single b3AABB (lower xyz, upper xyz as f32), written at stop so the+			// viewer can frame the whole recorded motion without playing to the end.+			memcpy( &player->bounds, data + payloadStart, sizeof( b3AABB ) );+		}+		cursor = payloadStart + (int)payloadSize;+	}+	player->frameCount = frameCount;+}++// Free one keyframe's heap.+static void b3FreeKeyframe( b3RecKeyframe* kf )+{+	if ( kf->image != NULL )+	{+		b3Free( kf->image, (size_t)kf->imageCapacity );+	}+	if ( kf->bodyIds != NULL )+	{+		b3Free( kf->bodyIds, (size_t)kf->bodyIdCount * sizeof( b3BodyId ) );+	}+}++// Pre-populate keyframeRec's registry to mirror rdr.slots so geometry ids stay stable during+// b3SerializeWorld. Each slot becomes one entry with id == slot index, even byte-identical slots that+// a hash collision left undeduplicated in an already-recorded file. b3SerializeWorld then resolves a+// live blob back to a valid slot index via the registry's exact dedup, so capture never grows it.+static void b3RecSeedKeyframeRegistry( b3RecPlayer* player )+{+	b3GeometryRegistry* reg = &player->keyframeRec->registry;+	for ( int i = 0; i < player->rdr.slotCount; ++i )+	{+		b3RegistrySlot* slot = player->rdr.slots + i;+		// Copy so the registry can take ownership.+		int n = slot->byteCount > 0 ? slot->byteCount : 1;+		uint8_t* copy = (uint8_t*)b3Alloc( (size_t)n );+		if ( slot->byteCount > 0 )+		{+			memcpy( copy, slot->bytes, (size_t)slot->byteCount );+		}+		uint64_t h = b3Hash64NonZero( slot->bytes, slot->byteCount );+		uint32_t id = b3AppendGeometry( reg, slot->kind, h, copy, slot->byteCount );+		// Seeding in order without dedup keeps id == slot index.+		B3_ASSERT( id == (uint32_t)i );+		(void)id;+	}+}++// Capture a restore-point keyframe for the just-completed frame. rdr.cursor already points to+// the next frame's Step op.+static void b3RecCaptureKeyframe( b3RecPlayer* player )+{+	b3World* world = b3GetWorldFromId( player->rdr.replayWorldId );+	b3RecBuffer buf = { 0 };++	int regCountBefore = player->keyframeRec->registry.entries.count;+	B3_UNUSED( regCountBefore );++	b3SerializeWorld( world, &buf, player->keyframeRec );+	// Registry must not grow: all geometry was pre-seeded and the registry dedups exactly.+	B3_ASSERT( player->keyframeRec->registry.entries.count == regCountBefore );++	size_t bodyBytes = (size_t)player->bodyIdCount * sizeof( b3BodyId );+	size_t newBytes = (size_t)buf.capacity + bodyBytes;++	// Make room under the budget by doubling the spacing and evicting off-grid keyframes.+	while ( player->keyframeCount > 0 && player->keyframeBytes + newBytes > player->keyframeBudget )+	{+		player->keyframeInterval *= 2;+		int kept = 0;+		size_t keptBytes = 0;+		for ( int i = 0; i < player->keyframeCount; ++i )+		{+			b3RecKeyframe* kf = player->keyframes + i;+			if ( kf->frame % player->keyframeInterval == 0 )+			{+				player->keyframes[kept] = *kf;+				keptBytes += (size_t)kf->imageCapacity + (size_t)kf->bodyIdCount * sizeof( b3BodyId );+				kept += 1;+			}+			else+			{+				b3FreeKeyframe( kf );+			}+		}+		bool progress = ( kept < player->keyframeCount );+		player->keyframeCount = kept;+		player->keyframeBytes = keptBytes;+		if ( !progress )+		{+			break;+		}+	}++	// Grow the keyframe ring if needed.+	if ( player->keyframeCount >= player->keyframeCapacity )+	{+		int newCap = player->keyframeCapacity < 8 ? 8 : player->keyframeCapacity * 2;+		player->keyframes = (b3RecKeyframe*)b3GrowAlloc(+			player->keyframes, player->keyframeCapacity * (int)sizeof( b3RecKeyframe ), newCap * (int)sizeof( b3RecKeyframe ) );+		player->keyframeCapacity = newCap;+	}++	b3RecKeyframe* kf = player->keyframes + player->keyframeCount;+	kf->image = buf.data;+	kf->imageSize = buf.size;+	kf->imageCapacity = buf.capacity;+	kf->frame = player->frame;+	kf->cursor = player->rdr.cursor;+	kf->divergeFrame = player->divergeFrame;+	kf->diverged = player->rdr.diverged;+	kf->bodyIdCount = player->bodyIdCount;+	kf->bodyIds = NULL;+	if ( bodyBytes > 0 )+	{+		kf->bodyIds = (b3BodyId*)b3Alloc( bodyBytes );+		memcpy( kf->bodyIds, player->bodyIds, bodyBytes );+	}++	player->keyframeBytes += newBytes;+	player->keyframeCount += 1;+	player->lastKeyframeFrame = player->frame;+}++// Restore the world in-place from a keyframe image.+static void b3RecPlayerRestoreKeyframe( b3RecPlayer* player, const b3RecKeyframe* kf )+{+	b3World* world = b3GetWorldFromId( player->rdr.replayWorldId );+	if ( b3DeserializeIntoShell( kf->image, kf->imageSize, world, &player->rdr ) == false )+	{+		player->rdr.ok = false;+		return;+	}+	player->rdr.cursor = kf->cursor;+	player->rdr.ok = true;+	player->rdr.diverged = kf->diverged;+	player->frame = kf->frame;+	player->divergeFrame = kf->divergeFrame;+	player->atEnd = false;+	player->atPreStep = false;++	// Restore the outliner list verbatim so ordinals match this frame.+	b3RecGrow( (void**)&player->bodyIds, &player->bodyIdCap, kf->bodyIdCount, 0, (int)sizeof( b3BodyId ) );+	player->bodyIdCount = kf->bodyIdCount;+	if ( kf->bodyIdCount > 0 )+	{+		memcpy( player->bodyIds, kf->bodyIds, (size_t)kf->bodyIdCount * sizeof( b3BodyId ) );+	}+}++// Create a replay world carrying the host debug-shape callbacks. Every world the player+// stands up funnels through here so the sample renderer can draw replayed shapes.+static b3WorldId b3RecPlayerCreateWorld( const b3RecPlayer* player )+{+	b3WorldDef worldDef = b3DefaultWorldDef();+	worldDef.createDebugShape = player->createDebugShape;+	worldDef.destroyDebugShape = player->destroyDebugShape;+	worldDef.userDebugShapeContext = player->debugShapeContext;+	// Carry the requested worker count so a rebuild on Restart or backward seek keeps the same+	// graph partitioning. Replaying at a different count than recorded is a determinism check.+	worldDef.workerCount = b3MaxInt( 1, player->recordedWorkerCount );+	return b3CreateWorld( &worldDef );+}++b3RecPlayer* b3CreatePlayer( const void* data, int size, int workerCount )+{+	if ( data == NULL || size < (int)sizeof( b3RecHeader ) )+	{+		printf( "b3RecPlayer_Create: recording too small\n" );+		return NULL;+	}++	b3RecHeader hdr;+	memcpy( &hdr, data, sizeof( hdr ) );++	if ( hdr.magic != B3_REC_MAGIC )+	{+		printf( "b3RecPlayer_Create: bad magic 0x%08X\n", hdr.magic );+		return NULL;+	}+	// Only the major version is breaking. Minor bumps are additive op-stream changes that keep the+	// header shape, and the dispatcher skips opcodes it doesn't know, so a minor mismatch still loads.+	if ( hdr.versionMajor != B3_REC_VERSION_MAJOR )+	{+		printf( "b3RecPlayer_Create: version mismatch %u.%u vs %u.%u\n", hdr.versionMajor, hdr.versionMinor, B3_REC_VERSION_MAJOR,+				B3_REC_VERSION_MINOR );+		return NULL;+	}+	if ( hdr.pointerWidth != (uint8_t)sizeof( void* ) )+	{+		printf( "b3RecPlayer_Create: pointer width mismatch %u vs %u\n", hdr.pointerWidth, (unsigned)sizeof( void* ) );+		return NULL;+	}+	if ( hdr.bigEndian != 0 )+	{+		printf( "b3RecPlayer_Create: big-endian recording not supported\n" );+		return NULL;+	}++	// Every recording is snapshot-seeded: the seed blob sits between the header and the op stream.+	if ( hdr.snapshotSize == 0 )+	{+		printf( "b3RecPlayer_Create: missing snapshot seed\n" );+		return NULL;+	}++	// snapshotSize and registryOffset are 64-bit and come from the file. Validate in 64-bit so a+	// hostile value can't wrap when narrowed to int, then narrow once the bounds are known good.+	uint64_t headerEnd64 = (uint64_t)sizeof( b3RecHeader ) + hdr.snapshotSize;+	uint64_t registryEnd64 = ( hdr.registryOffset != 0 ) ? hdr.registryOffset : (uint64_t)size;++	if ( headerEnd64 < sizeof( b3RecHeader ) || headerEnd64 > registryEnd64 || registryEnd64 > (uint64_t)size )+	{+		printf( "b3RecPlayer_Create: corrupt offsets\n" );+		return NULL;+	}++	int headerEnd = (int)headerEnd64;+	int registryEnd = (int)registryEnd64;++	// Own a private copy so the caller can free their buffer right away.+	uint8_t* copy = b3Alloc( (size_t)size );+	memcpy( copy, data, (size_t)size );++	b3RecPlayer* player = b3Alloc( sizeof( b3RecPlayer ) );+	memset( player, 0, sizeof( b3RecPlayer ) );++	player->data = copy;+	player->size = size;+	player->headerEnd = headerEnd;+	player->registryEnd = registryEnd;+	player->lengthScale = hdr.lengthScale;+	player->previousLengthScale = b3GetLengthUnitsPerMeter();+	player->frame = 0;+	player->frameCount = 0;+	player->recordedDt = 0.0f;+	player->recordedSubStepCount = 0;+	player->recordedWorkerCount = workerCount;+	player->atEnd = false;+	player->atPreStep = false;+	player->divergeFrame = -1;+	player->keyframeMinInterval = B3_REC_KEYFRAME_INTERVAL_DEFAULT;+	player->keyframeInterval = B3_REC_KEYFRAME_INTERVAL_DEFAULT;+	player->keyframeBudget = B3_REC_KEYFRAME_BUDGET_DEFAULT;+	player->lastKeyframeFrame = 0;++	// Set length scale so replay reproduces the same tuning constants.+	if ( hdr.lengthScale > 0.0f )+	{+		b3SetLengthUnitsPerMeter( hdr.lengthScale );+	}++	// Count frames and read first step's dt so the viewer can show hz up front.+	b3RecScanFile( player );++	// Create the replay world. Debug-shape callbacks are NULL here; the sample wires+	// them right after Create via b3RecPlayer_SetDebugShapeCallbacks, which rebuilds.+	b3WorldId worldId = b3RecPlayerCreateWorld( player );++	// Initialize the reader.+	player->rdr.data = copy;+	player->rdr.size = size;+	player->rdr.cursor = headerEnd;+	player->rdr.replayWorldId = worldId;+	player->rdr.ok = true;+	player->rdr.diverged = false;+	player->rdr.owner = player;++	// Load the trailing geometry registry.+	if ( !b3RecLoadSlots( &player->rdr, copy, size, hdr.registryOffset, hdr.registryByteCount ) )+	{+		b3DestroyWorld( worldId );+		b3Free( copy, (size_t)size );+		b3Free( player, sizeof( b3RecPlayer ) );+		return NULL;+	}++	// Restore the seed snapshot to stand up the replay world. The blob doubles as the frame-0+	// restore image, owned by the copy held above.+	{+		int snapStart = (int)sizeof( b3RecHeader );+		int snapSize = (int)hdr.snapshotSize;+		b3World* replayWorld = b3GetWorldFromId( worldId );+		if ( b3DeserializeIntoShell( copy + snapStart, snapSize, replayWorld, &player->rdr ) == false )+		{+			printf( "b3RecPlayer_Create: snapshot deserialization failed\n" );+			b3DestroyWorld( worldId );+			b3RecFreeSlots( player->rdr.slots, player->rdr.slotCount );+			if ( player->rdr.tags != NULL )+			{+				b3Free( player->rdr.tags, (size_t)player->rdr.tagCapacity * sizeof( b3RecTag ) );+			}+			if ( player->rdr.tagMap != NULL )+			{+				b3RecTagLookup_cleanup( (b3RecTagLookup*)player->rdr.tagMap );+				b3Free( player->rdr.tagMap, sizeof( b3RecTagLookup ) );+			}+			b3Free( copy, (size_t)size );+			b3Free( player, sizeof( b3RecPlayer ) );+			return NULL;+		}+		player->rdr.cursor = headerEnd;+		player->frame0Image = copy + snapStart;+		player->frame0Size = snapSize;+	}++	// Seed the outliner from the restored world (snapshot bodies bypass the create hook) and save+	// the frame-0 restore copy.+	b3RecSeedFrame0BodyIds( player );++	// Build the keyframe recording with a pre-seeded registry that mirrors rdr.slots,+	// so b3SerializeWorld geometry ids stay stable across captures.+	player->keyframeRec = b3CreateRecording( 0 );+	b3RecSeedKeyframeRegistry( player );++	return player;+}++void b3DestroyPlayer( b3RecPlayer* player )+{+	if ( player == NULL )+	{+		return;+	}++	if ( b3World_IsValid( player->rdr.replayWorldId ) )+	{+		b3DestroyWorld( player->rdr.replayWorldId );+	}++	// Free live geometry after destroying the world (slot->live may be used by the world).+	b3RecFreeSlots( player->rdr.slots, player->rdr.slotCount );++	// Free reader scratch.+	if ( player->rdr.matScratch != NULL )+	{+		b3Free( player->rdr.matScratch, (size_t)player->rdr.matScratchCap * sizeof( b3SurfaceMaterial ) );+	}+	if ( player->rdr.proxyScratch != NULL )+	{+		b3Free( player->rdr.proxyScratch, (size_t)player->rdr.proxyScratchCap * sizeof( b3Vec3 ) );+	}+	if ( player->rdr.hits != NULL )+	{+		b3Free( player->rdr.hits, (size_t)player->rdr.hitCap * sizeof( b3RecRecordedHit ) );+	}+	if ( player->rdr.tags != NULL )+	{+		b3Free( player->rdr.tags, (size_t)player->rdr.tagCapacity * sizeof( b3RecTag ) );+	}+	if ( player->rdr.tagMap != NULL )+	{+		b3RecTagLookup_cleanup( (b3RecTagLookup*)player->rdr.tagMap );+		b3Free( player->rdr.tagMap, sizeof( b3RecTagLookup ) );+	}++	// Free the per-frame query store.+	if ( player->frameQueries != NULL )+	{+		b3Free( player->frameQueries, (size_t)player->frameQueryCap * sizeof( b3RecDrawQuery ) );+	}+	if ( player->frameHits != NULL )+	{+		b3Free( player->frameHits, (size_t)player->frameHitCap * sizeof( b3RecRecordedHit ) );+	}++	// Free keyframe ring.+	for ( int i = 0; i < player->keyframeCount; ++i )+	{+		b3FreeKeyframe( player->keyframes + i );+	}+	if ( player->keyframes != NULL )+	{+		b3Free( player->keyframes, (size_t)player->keyframeCapacity * sizeof( b3RecKeyframe ) );+	}++	// The keyframe recording owns only its buffer and registry; b3DestroyRecording frees both.+	if ( player->keyframeRec != NULL )+	{+		b3DestroyRecording( player->keyframeRec );+	}++	// Free the outliner body lists.+	if ( player->bodyIds != NULL )+	{+		b3Free( player->bodyIds, (size_t)player->bodyIdCap * sizeof( b3BodyId ) );+	}+	if ( player->frame0BodyIds != NULL )+	{+		b3Free( player->frame0BodyIds, (size_t)player->frame0BodyIdCount * sizeof( b3BodyId ) );+	}++	// frame0Image points into the owned data copy, not separately allocated.++	b3Free( player->data, (size_t)player->size );++	// Restore the global length scale.+	b3SetLengthUnitsPerMeter( player->previousLengthScale );++	b3Free( player, sizeof( b3RecPlayer ) );+}++bool b3RecPlayer_StepFrame( b3RecPlayer* player )+{+	// This is never true when full stepping+	player->atPreStep = false;++	if ( player->atEnd )+	{+		return false;+	}++	// Reset the per-frame query store before this frame's records are dispatched.+	player->frameQueryCount = 0;+	player->frameHitCount = 0;++	// A frame is its leading inputs (queries and between-step mutators), one Step, and the Step's+	// trailing StateHash. Queries are recorded before the Step they belong to, so they stash here+	// against the world state they were computed for.+	bool stepped = false;+	for ( ;; )+	{+		if ( player->rdr.cursor >= player->registryEnd || !player->rdr.ok )+		{+			player->atEnd = true;+			return stepped;+		}++		// Once stepped, the StateHash is the only record still belonging to this frame. Anything else+		// begins the next frame, so stop and let the next StepFrame consume it. Capture a keyframe at+		// the boundary.+		if ( stepped && player->rdr.data[player->rdr.cursor] != b3_recOpStateHash )+		{+			if ( player->frame > player->lastKeyframeFrame && player->frame % player->keyframeInterval == 0 )+			{+				b3RecCaptureKeyframe( player );+			}+			return true;+		}++		int op = b3RecDispatchOne( &player->rdr );+		if ( op < 0 )+		{+			player->atEnd = true;+			return stepped;+		}+		if ( op == b3_recOpDestroyWorld ) // end of recording+		{+			player->atEnd = true;+			return stepped;+		}+		if ( op == b3_recOpStep )+		{+			player->frame += 1;+			stepped = true;+		}+		else if ( op == b3_recOpStateHash ) // trailing record of the frame just stepped+		{+			// Latch the first frame whose state hash diverged. The hash belongs to the frame Step just+			// advanced, so latch against the current frame, not the next Step which would be one late.+			if ( player->divergeFrame < 0 && player->rdr.diverged )+			{+				player->divergeFrame = player->frame;+			}+		}+	}+}++void b3RecPlayer_SubStepFrame( b3RecPlayer* player )+{+	if ( player->atEnd )+	{+		return;+	}++	// Reset the per-frame query store before this frame's records are dispatched.+	if ( player->atPreStep == false )+	{+		player->frameQueryCount = 0;+		player->frameHitCount = 0;+	}++	// A frame is its leading inputs (queries and between-step mutators), one Step, and the Step's+	// trailing StateHash. Queries are recorded before the Step they belong to, so they stash here+	// against the world state they were computed for.+	bool stepped = false;+	bool haveCreateBodyOp = false;+	for ( ;; )+	{+		if ( player->rdr.cursor >= player->registryEnd || !player->rdr.ok )+		{+			player->atEnd = true;+			player->atPreStep = false;+			return;+		}++		// Once stepped, the StateHash is the only record still belonging to this frame. Anything else+		// begins the next frame, so stop and let the next StepFrame consume it. Capture a keyframe at+		// the boundary.+		uint8_t currentOpCode = player->rdr.data[player->rdr.cursor];+		if ( stepped && currentOpCode != b3_recOpStateHash )+		{+			if ( player->frame > player->lastKeyframeFrame && player->frame % player->keyframeInterval == 0 )+			{+				b3RecCaptureKeyframe( player );+			}+			return;+		}++		if ( player->atPreStep == false && haveCreateBodyOp == true && currentOpCode == b3_recOpStep )+		{+			player->atPreStep = true;+			return;+		}++		int op = b3RecDispatchOne( &player->rdr );+		if ( op < 0 )+		{+			player->atEnd = true;+			player->atPreStep = false;+			return;+		}+		if ( op == b3_recOpDestroyWorld ) // end of recording+		{+			player->atEnd = true;+			player->atPreStep = false;+			return;+		}++		if ( op == b3_recOpCreateBody )+		{+			B3_ASSERT( player->atPreStep == false );+			haveCreateBodyOp = true;+		}++		if ( op == b3_recOpStep )+		{+			player->atPreStep = false;+			player->frame += 1;+			stepped = true;+		}+		else if ( op == b3_recOpStateHash ) // trailing record of the frame just stepped+		{+			// Latch the first frame whose state hash diverged. The hash belongs to the frame Step just+			// advanced, so latch against the current frame, not the next Step which would be one late.+			if ( player->divergeFrame < 0 && player->rdr.diverged )+			{+				player->divergeFrame = player->frame;+			}+		}+	}+}++void b3RecPlayer_Restart( b3RecPlayer* player )+{+	// Restore the frame-0 image in place so the replay world id stays stable across a restart or+	// backward scrub. Stepping resumes at the first Step, which rebuilds the body list deterministically.+	b3World* world = b3GetWorldFromId( player->rdr.replayWorldId );+	if ( b3DeserializeIntoShell( player->frame0Image, player->frame0Size, world, &player->rdr ) == false )+	{+		player->rdr.ok = false;+		return;+	}+	player->rdr.cursor = player->headerEnd;+	player->rdr.ok = true;+	player->rdr.diverged = false;+	player->frame = 0;+	player->divergeFrame = -1;+	player->atEnd = false;+	player->atPreStep = false;++	// Frame 0 is the pre-step snapshot with no recorded queries, so clear the per-frame store. This+	// keeps the last stepped frame's queries from lingering on a restart or a backward scrub to 0.+	player->frameQueryCount = 0;+	player->frameHitCount = 0;++	// Roll the outliner body list back to its frame-0 contents.+	b3RecGrow( (void**)&player->bodyIds, &player->bodyIdCap, player->frame0BodyIdCount, 0, (int)sizeof( b3BodyId ) );+	player->bodyIdCount = player->frame0BodyIdCount;+	if ( player->frame0BodyIdCount > 0 )+	{+		memcpy( player->bodyIds, player->frame0BodyIds, (size_t)player->frame0BodyIdCount * sizeof( b3BodyId ) );+	}+}++void b3RecPlayer_SeekFrame( b3RecPlayer* player, int targetFrame )+{+	if ( player == NULL )+	{+		return;+	}++	player->atPreStep = false;++	if ( targetFrame < 0 )+	{+		targetFrame = 0;+	}++	// Find the best keyframe strictly before the target.+	const b3RecKeyframe* best = NULL;+	for ( int i = 0; i < player->keyframeCount; ++i )+	{+		const b3RecKeyframe* kf = player->keyframes + i;+		if ( kf->frame < targetFrame && ( best == NULL || kf->frame > best->frame ) )+		{+			best = kf;+		}+	}++	if ( targetFrame < player->frame )+	{+		// Backward seek: restore keyframe or restart from frame 0.+		if ( best != NULL )+		{+			b3RecPlayerRestoreKeyframe( player, best );+		}+		else+		{+			b3RecPlayer_Restart( player );+		}+	}+	else if ( best != NULL && best->frame > player->frame )+	{+		// Forward seek that can skip ahead via a keyframe.+		b3RecPlayerRestoreKeyframe( player, best );+	}++	while ( player->frame < targetFrame && b3RecPlayer_StepFrame( player ) )+	{+	}+}++b3WorldId b3RecPlayer_GetWorldId( const b3RecPlayer* player )+{+	return player != NULL ? player->rdr.replayWorldId : b3_nullWorldId;+}++int b3RecPlayer_GetFrame( const b3RecPlayer* player )+{+	return player != NULL ? player->frame : 0;+}++int b3RecPlayer_GetFrameCount( const b3RecPlayer* player )+{+	return player != NULL ? player->frameCount : 0;+}++bool b3RecPlayer_IsAtEnd( const b3RecPlayer* player )+{+	return player != NULL ? player->atEnd : true;+}++bool b3RecPlayer_IsAtPreStep( const b3RecPlayer* player )+{+	return player != NULL ? player->atPreStep : false;+}++bool b3RecPlayer_HasDiverged( const b3RecPlayer* player )+{+	return player != NULL ? player->rdr.diverged : false;+}++b3RecPlayerInfo b3RecPlayer_GetInfo( const b3RecPlayer* player )+{+	b3RecPlayerInfo info = { 0 };+	if ( player != NULL )+	{+		info.frameCount = player->frameCount;+		info.workerCount = player->recordedWorkerCount;+		info.timeStep = player->recordedDt;+		info.subStepCount = player->recordedSubStepCount;+		info.lengthScale = player->lengthScale;+		info.bounds = player->bounds;+	}+	return info;+}++int b3RecPlayer_GetDivergeFrame( const b3RecPlayer* player )+{+	return player != NULL ? player->divergeFrame : -1;+}++void b3RecPlayer_SetWorkerCount( b3RecPlayer* player, int count )+{+	if ( player == NULL )+	{+		return;+	}++	player->recordedWorkerCount = b3ClampInt( count, 1, B3_MAX_WORKERS );++	// Apply to the live world now so the next steps re-partition without a rebuild. Worker count is+	// host state, not part of a keyframe image, so it survives an in-place restore. A rebuild on+	// Restart or deep backward seek picks the count back up through b3RecPlayerCreateWorld.+	if ( b3World_IsValid( player->rdr.replayWorldId ) )+	{+		b3World_SetWorkerCount( player->rdr.replayWorldId, player->recordedWorkerCount );+	}+}++void b3RecPlayer_SetKeyframePolicy( b3RecPlayer* player, size_t budgetBytes, int minIntervalFrames )+{+	if ( player == NULL )+	{+		return;+	}+	if ( budgetBytes > 0 )+	{+		player->keyframeBudget = budgetBytes;+	}+	if ( minIntervalFrames > 0 )+	{+		player->keyframeMinInterval = minIntervalFrames;+	}++	// Drop the ring so it repopulates under the new policy on the next replay.+	for ( int i = 0; i < player->keyframeCount; ++i )+	{+		b3FreeKeyframe( player->keyframes + i );+	}+	player->keyframeCount = 0;+	player->keyframeBytes = 0;+	player->keyframeInterval = player->keyframeMinInterval;+	player->lastKeyframeFrame = 0;+}++size_t b3RecPlayer_GetKeyframeBudget( const b3RecPlayer* player )+{+	return player != NULL ? player->keyframeBudget : 0;+}++int b3RecPlayer_GetKeyframeMinInterval( const b3RecPlayer* player )+{+	return player != NULL ? player->keyframeMinInterval : 0;+}++int b3RecPlayer_GetKeyframeInterval( const b3RecPlayer* player )+{+	return player != NULL ? player->keyframeInterval : 0;+}++size_t b3RecPlayer_GetKeyframeBytes( const b3RecPlayer* player )+{+	return player != NULL ? player->keyframeBytes : 0;+}++int b3RecPlayer_GetBodyCount( const b3RecPlayer* player )+{+	return player != NULL ? player->bodyIdCount : 0;+}++b3BodyId b3RecPlayer_GetBodyId( const b3RecPlayer* player, int index )+{+	if ( player == NULL || index < 0 || index >= player->bodyIdCount )+	{+		return b3_nullBodyId;+	}+	return player->bodyIds[index];+}++// A selected query draws in one reserved color so it stands out when every query is drawn at once.+static b3HexColor b3RecQuerySelColor( bool selected, b3HexColor base )+{+	return selected ? b3_colorPlum : base;+}++// Highlight each reported overlap shape by its AABB. Skip any destroyed since the query, per the+// b3Shape_GetAABB contract that overlap results may contain stale shapes.+static void b3RecDrawHitBounds( const b3RecPlayer* player, const b3RecDrawQuery* q, b3DebugDraw* draw, b3HexColor color )+{+	if ( draw->DrawBoundsFcn == NULL )+	{+		return;+	}+	for ( int hi = q->hitStart; hi < q->hitStart + q->hitCount; ++hi )+	{+		b3ShapeId id = player->frameHits[hi].id;+		if ( b3Shape_IsValid( id ) == false )+		{+			continue;+		}+		draw->DrawBoundsFcn( b3Shape_GetAABB( id ), color, draw->context );+	}+}++// Draw a recorded shape proxy at basePos. A lone point with no radius draws a fat point, a lone point+// with a radius draws a translucent sphere, and a multi-point cloud draws its points. Capsule and hull+// proxies are rare and fall through to the point cloud for now.+static void b3RecDrawProxy( b3DebugDraw* draw, b3Pos basePos, const b3RecDrawQuery* q, b3HexColor color )+{+	if ( q->proxyCount == 1 )+	{+		b3Pos p = b3OffsetPos( basePos, q->proxyPoints[0] );+		if ( q->proxyRadius > 0.0f )+		{+			if ( draw->DrawSphereFcn )+			{+				draw->DrawSphereFcn( p, q->proxyRadius, color, 0.5f, draw->context );+			}+		}+		else if ( draw->DrawPointFcn )+		{+			draw->DrawPointFcn( p, 10.0f, color, draw->context );+		}+	}+	else if ( q->proxyCount == 2 && q->proxyRadius > 0.0f )+	{+		if ( draw->DrawCapsuleFcn )+		{+			b3Pos p1 = b3OffsetPos( basePos, q->proxyPoints[0] );+			b3Pos p2 = b3OffsetPos( basePos, q->proxyPoints[1] );+			draw->DrawCapsuleFcn( p1, p2, q->proxyRadius, color, 0.5f, draw->context );+		}+	}+	else if ( q->proxyCount >= 2 && draw->DrawPointFcn )+	{+		for ( int i = 0; i < q->proxyCount; ++i )+		{+			draw->DrawPointFcn( b3OffsetPos( basePos, q->proxyPoints[i] ), 6.0f, color, draw->context );+		}+	}+}++void b3RecPlayer_DrawFrameQueries( b3RecPlayer* player, b3DebugDraw* draw, int queryIndex, int selectedIndex )+{+	if ( player == NULL || draw == NULL )+	{+		return;+	}++	// queryIndex < 0 draws every query, otherwise just the one selected in the viewer. The query at+	// selectedIndex draws in one reserved color and is labeled, so it stands out among the rest.+	for ( int qi = 0; qi < player->frameQueryCount; ++qi )+	{+		if ( queryIndex >= 0 && qi != queryIndex )+		{+			continue;+		}++		const b3RecDrawQuery* q = &player->frameQueries[qi];+		bool selected = ( qi == selectedIndex );++		switch ( q->kind )+		{+			case B3_RECQ_CAST_RAY:+			case B3_RECQ_CAST_RAY_CLOSEST:+			{+				b3Pos origin = q->origin;+				b3Pos end = b3OffsetPos( origin, q->translation );+				if ( draw->DrawSegmentFcn )+				{+					draw->DrawSegmentFcn( origin, end, b3RecQuerySelColor( selected, b3_colorYellow ), draw->context );+				}+				for ( int hi = q->hitStart; hi < q->hitStart + q->hitCount; ++hi )+				{+					const b3RecRecordedHit* h = &player->frameHits[hi];+					if ( draw->DrawPointFcn )+					{+						draw->DrawPointFcn( h->point, 4.0f, b3RecQuerySelColor( selected, b3_colorYellow ), draw->context );+					}+					if ( draw->DrawSegmentFcn )+					{+						draw->DrawSegmentFcn( h->point, b3OffsetPos( h->point, b3MulSV( 0.2f, h->normal ) ),+											  b3RecQuerySelColor( selected, b3_colorYellowGreen ), draw->context );+					}+				}+				break;+			}+			case B3_RECQ_CAST_SHAPE:+			{+				// Draw the cast line and the proxy at its start, then each hit point and normal.+				if ( draw->DrawSegmentFcn )+				{+					draw->DrawSegmentFcn( q->origin, b3OffsetPos( q->origin, q->translation ),+										  b3RecQuerySelColor( selected, b3_colorSkyBlue ), draw->context );+				}+				b3RecDrawProxy( draw, q->origin, q, b3RecQuerySelColor( selected, b3_colorLightGreen ) );+				for ( int hi = q->hitStart; hi < q->hitStart + q->hitCount; ++hi )+				{+					const b3RecRecordedHit* h = &player->frameHits[hi];+					if ( draw->DrawPointFcn )+					{+						draw->DrawPointFcn( h->point, 4.0f, b3RecQuerySelColor( selected, b3_colorSkyBlue ), draw->context );+					}+					if ( draw->DrawSegmentFcn )+					{+						draw->DrawSegmentFcn( h->point, b3OffsetPos( h->point, b3MulSV( 0.2f, h->normal ) ),+											  b3RecQuerySelColor( selected, b3_colorLightSkyBlue ), draw->context );+					}+					if ( draw->DrawSphereFcn )+					{+						b3Pos p = b3OffsetPos( q->origin, b3MulSV( h->fraction, q->translation ) );+						b3RecDrawProxy( draw, p, q, b3RecQuerySelColor( selected, b3_colorSkyBlue ) );+					}+				}+				break;+			}+			case B3_RECQ_CAST_MOVER:+			{+				b3Pos c1 = b3OffsetPos( q->origin, q->mover.center1 );+				b3Pos c2 = b3OffsetPos( q->origin, q->mover.center2 );+				b3HexColor c = b3_colorLightSkyBlue;+				if ( draw->DrawCapsuleFcn )+				{+					draw->DrawCapsuleFcn( c1, c2, q->mover.radius, b3RecQuerySelColor( selected, c ), 0.6f, draw->context );++					if ( q->castFraction > 0.01f )+					{+						b3Vec3 d = b3MulSV( q->castFraction, q->translation );+						c1 = b3OffsetPos( c1, d );+						c2 = b3OffsetPos( c2, d );+						draw->DrawCapsuleFcn( c1, c2, q->mover.radius, c, 0.3f, draw->context );+					}+				}+				break;+			}++			case B3_RECQ_COLLIDE_MOVER:+			{+				b3Pos c1 = b3OffsetPos( q->origin, q->mover.center1 );+				b3Pos c2 = b3OffsetPos( q->origin, q->mover.center2 );+				b3HexColor c = b3_colorTan;+				if ( draw->DrawCapsuleFcn )+				{+					draw->DrawCapsuleFcn( c1, c2, q->mover.radius, b3RecQuerySelColor( selected, c ), 0.6f, draw->context );+				}++				for ( int hi = q->hitStart; hi < q->hitStart + q->hitCount; ++hi )+				{+					const b3RecRecordedHit* h = &player->frameHits[hi];+					b3Pos point = b3OffsetPos( q->origin, h->plane.point );+					if ( draw->DrawSegmentFcn )+					{+						draw->DrawSegmentFcn( point, b3OffsetPos( point, b3MulSV( 0.2f, h->plane.plane.normal ) ),+											  b3RecQuerySelColor( selected, b3_colorOrange ), draw->context );+					}+				}+				break;+			}++			case B3_RECQ_OVERLAP_AABB:+			{+				if ( draw->DrawBoundsFcn )+				{+					draw->DrawBoundsFcn( q->aabb, b3RecQuerySelColor( selected, b3_colorLimeGreen ), draw->context );+				}+				b3RecDrawHitBounds( player, q, draw, b3RecQuerySelColor( selected, b3_colorMagenta ) );+				break;+			}+			case B3_RECQ_OVERLAP_SHAPE:+			{+				// The overlap proxy sits at the origin; draw it, then the overlapping shape bounds.+				b3RecDrawProxy( draw, q->origin, q, b3RecQuerySelColor( selected, b3_colorLimeGreen ) );+				b3RecDrawHitBounds( player, q, draw, b3RecQuerySelColor( selected, b3_colorMagenta ) );+				break;+			}+			default:+				break;+		}++		// Label the selected query at its origin so it reads by name among the others. The overlap AABB+		// has no origin, so anchor at the box center. Untagged queries (no key) rely on the color alone.+		if ( selected && q->key != 0 && draw->DrawStringFcn != NULL )+		{+			const char* name = NULL;+			uint64_t id = 0;+			if ( player->rdr.tagMap != NULL )+			{+				b3RecTagLookup_itr it = b3RecTagLookup_get( (b3RecTagLookup*)player->rdr.tagMap, q->key );+				if ( b3RecTagLookup_is_end( it ) == false )+				{+					const b3RecTag* tag = &player->rdr.tags[it.data->val];+					name = tag->queryName;+					id = tag->id;+				}+			}+			char label[64];+			if ( name != NULL && name[0] != '\0' && id != 0 )+			{+				snprintf( label, sizeof( label ), "%.40s (%" PRIu64 ")", name, id );+			}+			else if ( name != NULL && name[0] != '\0' )+			{+				snprintf( label, sizeof( label ), "%.40s", name );+			}+			else+			{+				snprintf( label, sizeof( label ), "#%" PRIu64, id );+			}+			b3Pos labelPos = q->origin;+			if ( q->kind == B3_RECQ_OVERLAP_AABB )+			{+				labelPos = b3ToPos( b3AABB_Center( q->aabb ) );+			}+			else if ( q->kind == B3_RECQ_CAST_MOVER || q->kind == B3_RECQ_COLLIDE_MOVER )+			{+				// Sit the label just past the center2 end cap, which for an upright mover is above it.+				b3Pos c1 = b3OffsetPos( q->origin, q->mover.center1 );+				b3Pos c2 = b3OffsetPos( q->origin, q->mover.center2 );+				b3Vec3 dir = b3Normalize( b3SubPos( c2, c1 ) );+				labelPos = b3OffsetPos( c2, b3MulSV( 1.25f * q->mover.radius, dir ) );+			}+			draw->DrawStringFcn( labelPos, label, b3_colorWhite, draw->context );+		}+	}+}++// The internal b3RecQueryKind values match the public b3RecQueryType, so the kind copies across as a+// plain cast. Pin the first and last kinds to catch enum drift.+_Static_assert( b3_recQueryOverlapAABB == 0 && B3_RECQ_OVERLAP_AABB == 0, "query type enum drift" );+_Static_assert( b3_recQueryCollideMover == 6 && B3_RECQ_COLLIDE_MOVER == 6, "query type enum drift" );++int b3RecPlayer_GetFrameQueryCount( const b3RecPlayer* player )+{+	return player != NULL ? player->frameQueryCount : 0;+}++b3RecQueryInfo b3RecPlayer_GetFrameQuery( const b3RecPlayer* player, int index )+{+	b3RecQueryInfo info = { 0 };+	if ( player == NULL || index < 0 || index >= player->frameQueryCount )+	{+		return info;+	}++	const b3RecDrawQuery* q = &player->frameQueries[index];+	info.type = (b3RecQueryType)q->kind;+	info.filter = q->filter;+	info.aabb = q->aabb;+	info.origin = q->origin;+	info.translation = q->translation;+	info.hitCount = q->hitCount;+	info.key = q->key;+	info.id = 0;+	info.name = NULL;+	if ( q->key != 0 && player->rdr.tagMap != NULL )+	{+		b3RecTagLookup_itr it = b3RecTagLookup_get( (b3RecTagLookup*)player->rdr.tagMap, q->key );+		if ( b3RecTagLookup_is_end( it ) == false )+		{+			const b3RecTag* tag = &player->rdr.tags[it.data->val];+			info.id = tag->id;+			// An id-only tag interns an empty name; report it as none so the viewer shows the id alone.+			info.name = tag->queryName[0] != '\0' ? tag->queryName : NULL;+		}+	}+	return info;+}++b3RecQueryHit b3RecPlayer_GetFrameQueryHit( const b3RecPlayer* player, int queryIndex, int hitIndex )+{+	b3RecQueryHit hit = { 0 };+	if ( player == NULL || queryIndex < 0 || queryIndex >= player->frameQueryCount )+	{+		return hit;+	}++	const b3RecDrawQuery* q = &player->frameQueries[queryIndex];+	if ( hitIndex < 0 || hitIndex >= q->hitCount )+	{+		return hit;+	}++	const b3RecRecordedHit* h = &player->frameHits[q->hitStart + hitIndex];+	hit.shape = h->id;+	hit.point = h->point;+	hit.normal = h->normal;+	hit.fraction = h->fraction;+	return hit;+}++void b3RecPlayer_SetDebugShapeCallbacks( b3RecPlayer* player, b3CreateDebugShapeCallback* createDebugShape,+										 b3DestroyDebugShapeCallback* destroyDebugShape, void* context )+{+	if ( player == NULL )+	{+		return;+	}++	player->createDebugShape = createDebugShape;+	player->destroyDebugShape = destroyDebugShape;+	player->debugShapeContext = context;++	// A world fixes its debug-shape callbacks at creation, so rebuild frame 0 under the new+	// wiring. The old world held no adapter shapes (its callbacks were NULL), so the tear-down+	// is balanced. Geometry slots are byte blobs reused as-is, the same path Restart relies on.+	if ( b3World_IsValid( player->rdr.replayWorldId ) )+	{+		b3DestroyWorld( player->rdr.replayWorldId );+	}+	player->rdr.replayWorldId = b3RecPlayerCreateWorld( player );+	player->rdr.cursor = player->headerEnd;+	player->rdr.ok = true;+	player->rdr.diverged = false;+	player->frame = 0;+	player->divergeFrame = -1;+	player->atEnd = false;++	// Re-seed the world so its shapes are recreated through the new callbacks.+	b3World* world = b3GetWorldFromId( player->rdr.replayWorldId );+	if ( b3DeserializeIntoShell( player->frame0Image, player->frame0Size, world, &player->rdr ) == false )+	{+		player->rdr.ok = false;+		return;+	}+	player->rdr.cursor = player->headerEnd;++	// Rebuild the outliner from the frame-0 world that was just stood up under the new callbacks.+	b3RecSeedFrame0BodyIds( player );+}
+ upstream/src/recording_replay.h view
@@ -0,0 +1,258 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "recording.h"++#include <stdbool.h>+#include <stdint.h>++typedef struct b3RecPlayer b3RecPlayer;++// A single recorded callback hit, used both as reader scratch during a query replay and as the+// per-frame draw store. For collide-mover, one hit is one plane, with planeCount and userReturnB+// replicated across a shape's planes so the replay walker can re-group and compare per shape.+typedef struct b3RecRecordedHit+{+	b3ShapeId id;+	b3Pos point;+	b3Vec3 normal;+	float fraction;+	uint64_t userMaterialId;+	int triangleIndex;+	int childIndex;+	b3PlaneResult plane; // collide-mover: this plane+	int planeCount;		 // collide-mover: planes in this hit's shape group (replicated)+	float userReturnF;	 // cast queries+	bool userReturnB;	 // overlap / collide-mover (per shape, replicated)+} b3RecRecordedHit;++// Recorded query kind, matching the public b3RecQueryType order.+typedef enum b3RecQueryKind+{+	B3_RECQ_OVERLAP_AABB,+	B3_RECQ_OVERLAP_SHAPE,+	B3_RECQ_CAST_RAY,+	B3_RECQ_CAST_SHAPE,+	B3_RECQ_CAST_RAY_CLOSEST,+	B3_RECQ_CAST_MOVER,+	B3_RECQ_COLLIDE_MOVER,+} b3RecQueryKind;++// Per-frame draw record for one query call. Self-contained (no aliased pointers) so the player's+// frameQueries array can grow with a plain memcpy. Geometry is origin relative except the overlap+// AABB, which is world space in 3D.+typedef struct b3RecDrawQuery+{+	int kind;+	uint64_t key; // identity key (hash of caller id+name), 0 = untagged+	b3QueryFilter filter;+	b3AABB aabb;								  // world-space bounds of the query, swept for casts+	b3Vec3 proxyPoints[B3_MAX_SHAPE_CAST_POINTS]; // overlap/cast shape proxy, origin relative+	int proxyCount;+	float proxyRadius;+	b3Capsule mover; // cast/collide mover, origin relative+	b3Pos origin;+	b3Vec3 translation;+	float castFraction;	   // cast-mover result fraction+	b3RayResult rayResult; // cast-ray-closest result+	b3ShapeId shape;+	int hitStart; // first hit in the player's frameHits store+	int hitCount;+} b3RecDrawQuery;++// One slot in the preloaded geometry registry. Loaded from the trailing block before any+// ops run; live pointer built lazily on first shape create that references the id.+typedef struct b3RegistrySlot+{+	b3GeometryKind kind;+	int byteCount;+	uint8_t* bytes; // raw bytes from the file (always freed at teardown)+	void* live;		// reconstructed live object, freed after b3DestroyWorld+} b3RegistrySlot;++// This is used to simplify the scratch buffer lifetime. Names longer than this probably+// indicate a bug.+#define B3_MAX_NAME_LENGTH 256++// Reader state threaded through the replay loop and all dispatch functions+typedef struct b3RecReader+{+	const uint8_t* data;+	int size;+	int cursor;+	b3WorldId replayWorldId;+	bool ok;	   // false on read overrun or id mismatch, fatal stop+	bool diverged; // a StateHash failed, non-fatal++	// Player that owns this reader, or NULL during a headless b3ValidateReplay. Body+	// create/destroy and the bounds record fold back into it for the outliner and camera framing.+	b3RecPlayer* owner;++	// Scratch for per-triangle materials in shape defs (grown on demand, freed at teardown)+	b3SurfaceMaterial* matScratch;+	int matScratchCap;++	// Scratch for string reads. Used by b3RecR_STR to pass names to b3BodyDef and b3ShapeDef.+	char stringBuffers[4][B3_MAX_NAME_LENGTH + 1];+	int nextString;++	// Preloaded geometry registry+	b3RegistrySlot* slots;+	int slotCount;++	// Preloaded query-tag table (key -> id, name), loaded with the registry. Resolves the caller id and+	// label for the viewer. tagMap maps a key to its tag index for O(1) lookup; opaque, owned by the player.+	b3RecTag* tags;+	int tagCount;	 // tags that loaded; a truncated tail loads fewer+	int tagCapacity; // tags allocated, used to free the array+	void* tagMap;++	// Key from the QueryTag op preceding the next query, consumed by the next stash. 0 = untagged.+	uint64_t pendingQueryKey;++	// Scratch for recorded query hits; grown on demand, freed with the player.+	b3RecRecordedHit* hits;+	int hitCap;++	// Scratch for a shape-proxy point cloud read from the stream. b3ShapeProxy holds the points+	// behind a pointer, so a decoded proxy borrows this until the next proxy read or teardown.+	b3Vec3* proxyScratch;+	int proxyScratchCap;+} b3RecReader;++// Stored snapshot for fast backward seek.+typedef struct b3RecKeyframe+{+	uint8_t* image; // serialized world image at the end of this frame+	int imageSize;+	int imageCapacity; // allocation size (may exceed imageSize)+	int frame;		   // frame index this restores to+	int cursor;		   // op-stream cursor for the frame AFTER this one+	int divergeFrame;  // divergeFrame state at capture+	bool diverged;	   // rdr.diverged state at capture++	// Outliner body list as it stood at this frame, restored verbatim so ordinals are stable.+	b3BodyId* bodyIds;+	int bodyIdCount;+} b3RecKeyframe;++typedef struct b3RecPlayer+{+	uint8_t* data; // owned copy of recording bytes+	int size;+	int headerEnd;	 // first byte of op stream (past header + snapshot blob)+	int registryEnd; // end of op stream = start of registry block (or size)+	float lengthScale;+	float previousLengthScale;+	int frame;+	int frameCount;+	float recordedDt;+	int recordedSubStepCount;+	int recordedWorkerCount; // worker count requested for the replay world+	b3AABB bounds;			 // accumulated world bounds, decoded from the trailing record+	bool atEnd;+	// Indicates all ops for the step have been consumed up to the world step op. The next sub-step+	// will clear this and perform the world step.+	bool atPreStep;+	int divergeFrame; // first frame that diverged, -1 until then++	// Outliner body list, indexed by creation ordinal. Holes (null ids) mark destroyed bodies so+	// later ordinals never shift. Snapshotted into each keyframe and the frame-0 copy, not rebuilt+	// from the world, so a stored selection survives backward seeks.+	b3BodyId* bodyIds;+	int bodyIdCount;+	int bodyIdCap;+	b3BodyId* frame0BodyIds;+	int frame0BodyIdCount;++	// Per-frame query store, reset at the top of each StepFrame and filled by the query dispatchers.+	// Drawn by b3RecPlayer_DrawFrameQueries and inspected via the public GetFrameQuery API.+	b3RecDrawQuery* frameQueries;+	int frameQueryCount;+	int frameQueryCap;+	b3RecRecordedHit* frameHits;+	int frameHitCount;+	int frameHitCap;++	// Host debug-shape callbacks applied to every world the player creates. The 3D+	// sample renderer builds GPU meshes here, so a replay world without them draws+	// nothing. Set once via b3RecPlayer_SetDebugShapeCallbacks; persisted so a world+	// rebuilt under new callbacks keeps drawing.+	b3CreateDebugShapeCallback* createDebugShape;+	b3DestroyDebugShapeCallback* destroyDebugShape;+	void* debugShapeContext;++	b3RecReader rdr;++	// Frame-0 restore image, points into the owned data copy. Restart and backward seek+	// deserialize this in place so the replay world id stays stable.+	const uint8_t* frame0Image;+	int frame0Size;++	// Keyframe ring+	b3RecKeyframe* keyframes;+	int keyframeCount;+	int keyframeCapacity;+	size_t keyframeBudget;+	size_t keyframeBytes;+	int keyframeMinInterval;+	int keyframeInterval;+	int lastKeyframeFrame;++	// Pre-populated recording used by b3SerializeWorld during keyframe capture.+	// Its registry mirrors rdr.slots so geometry ids stay stable.+	b3Recording* keyframeRec;+} b3RecPlayer;++// Read primitives+uint8_t b3RecR_U8( b3RecReader* rdr );+uint16_t b3RecR_U16( b3RecReader* rdr );+uint32_t b3RecR_U24( b3RecReader* rdr );+uint32_t b3RecR_U32( b3RecReader* rdr );+uint64_t b3RecR_U64( b3RecReader* rdr );+int32_t b3RecR_I32( b3RecReader* rdr );+float b3RecR_F32( b3RecReader* rdr );+double b3RecR_F64( b3RecReader* rdr );+bool b3RecR_BOOL( b3RecReader* rdr );+b3Vec3 b3RecR_VEC3( b3RecReader* rdr );+b3Quat b3RecR_QUAT( b3RecReader* rdr );+b3Transform b3RecR_TRANSFORM( b3RecReader* rdr );+b3Pos b3RecR_POSITION( b3RecReader* rdr );+b3WorldTransform b3RecR_WORLDXF( b3RecReader* rdr );+b3Matrix3 b3RecR_MATRIX3( b3RecReader* rdr );+b3AABB b3RecR_AABB( b3RecReader* rdr );+b3WorldId b3RecR_WORLDID( b3RecReader* rdr );+b3BodyId b3RecR_BODYID( b3RecReader* rdr );+b3ShapeId b3RecR_SHAPEID( b3RecReader* rdr );+b3JointId b3RecR_JOINTID( b3RecReader* rdr );+b3Sphere b3RecR_SPHERE( b3RecReader* rdr );+b3Capsule b3RecR_CAPSULE( b3RecReader* rdr );+uint32_t b3RecR_GEOMID( b3RecReader* rdr );+b3Filter b3RecR_FILTER( b3RecReader* rdr );+b3SurfaceMaterial b3RecR_MATERIAL( b3RecReader* rdr );+b3MassData b3RecR_MASSDATA( b3RecReader* rdr );+b3MotionLocks b3RecR_LOCKS( b3RecReader* rdr );+const char* b3RecR_STR( b3RecReader* rdr );+b3ExplosionDef b3RecR_EXPLOSIONDEF( b3RecReader* rdr );+b3BodyDef b3RecR_BODYDEF( b3RecReader* rdr );+b3ShapeDef b3RecR_SHAPEDEF( b3RecReader* rdr );+b3ParallelJointDef b3RecR_PARALLELJOINTDEF( b3RecReader* rdr );+b3DistanceJointDef b3RecR_DISTANCEJOINTDEF( b3RecReader* rdr );+b3FilterJointDef b3RecR_FILTERJOINTDEF( b3RecReader* rdr );+b3MotorJointDef b3RecR_MOTORJOINTDEF( b3RecReader* rdr );+b3PrismaticJointDef b3RecR_PRISMATICJOINTDEF( b3RecReader* rdr );+b3RevoluteJointDef b3RecR_REVOLUTEJOINTDEF( b3RecReader* rdr );+b3SphericalJointDef b3RecR_SPHERICALJOINTDEF( b3RecReader* rdr );+b3WeldJointDef b3RecR_WELDJOINTDEF( b3RecReader* rdr );+b3WheelJointDef b3RecR_WHEELJOINTDEF( b3RecReader* rdr );+b3QueryFilter b3RecR_QUERYFILTER( b3RecReader* rdr );+b3ShapeProxy b3RecR_SHAPEPROXY( b3RecReader* rdr );+b3TreeStats b3RecR_TREESTATS( b3RecReader* rdr );+b3RayResult b3RecR_RAYRESULT( b3RecReader* rdr );+b3PlaneResult b3RecR_PLANERESULT( b3RecReader* rdr );++// Grow the reader's hit scratch to at least n entries, preserving contents. n is bounded by the+// file size since every recorded hit consumes at least one byte.+void b3RecEnsureHits( b3RecReader* rdr, int n );
+ upstream/src/revolute_joint.c view
@@ -0,0 +1,673 @@+// 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"+#include "recording.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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, RevoluteJointEnableLimit, jointId, 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( b3IsValidFloat( lowerLimitRadians ) && b3IsValidFloat( upperLimitRadians ) );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, RevoluteJointSetLimits, jointId, lowerLimitRadians, upperLimitRadians );++	float lowerAngle = b3MinFloat( lowerLimitRadians, upperLimitRadians );+	float upperAngle = b3MaxFloat( lowerLimitRadians, upperLimitRadians );++	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );+	base->revoluteJoint.lowerAngle = b3ClampFloat( lowerAngle, -0.99f * B3_PI, 0.99f * B3_PI );+	base->revoluteJoint.upperAngle = b3ClampFloat( upperAngle, -0.99f * B3_PI, 0.99f * B3_PI );+}++float b3RevoluteJoint_GetAngle( b3JointId jointId )+{+	b3World* world = b3GetWorld( jointId.world0 );+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );+	b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );+	b3WorldTransform 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, RevoluteJointEnableSpring, jointId, 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 );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, RevoluteJointSetTargetAngle, jointId, targetRadians );+	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 );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, RevoluteJointSetSpringHertz, jointId, hertz );+	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 );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, RevoluteJointSetSpringDampingRatio, jointId, dampingRatio );+	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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, RevoluteJointEnableMotor, jointId, 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 ) );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, RevoluteJointSetMotorSpeed, jointId, 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 );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, RevoluteJointSetMaxMotorTorque, jointId, maxForce );+	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;+}++float b3RevoluteJoint_GetMotorTorque( b3JointId jointId )+{+	b3World* world = b3GetWorld( jointId.world0 );+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );+	return world->inv_h * base->revoluteJoint.motorImpulse;+}++b3Vec3 b3GetRevoluteJointForce( b3World* world, b3JointSim* base )+{+	b3Vec3 force = b3MulSV( world->inv_h, base->revoluteJoint.linearImpulse );+	return force;+}++b3Vec3 b3GetRevoluteJointTorque( b3World* world, b3JointSim* base )+{+	b3WorldTransform 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 = b3SubPos( 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, b3WorldTransform transformA, b3WorldTransform transformB, float scale )+{+	b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA );++	float length1 = 0.1f * scale;+	draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisX ) ) ), b3_colorRed,+						  draw->context );+	draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisY ) ) ), b3_colorGreen,+						  draw->context );+	draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisZ ) ) ), b3_colorBlue,+						  draw->context );++	b3WorldTransform frameB = b3MulWorldTransforms( 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, b3TransformWorldPoint( frameA, vertex1 ), b3_colorCyan, draw->context );+			}++			if ( index == kSliceCount - 1 )+			{+				draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex2 ), frameA.p, b3_colorCyan, draw->context );+			}+			draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex1 ), b3TransformWorldPoint( frameA, vertex2 ), b3_colorCyan,+								  draw->context );+		}++		float twistAngle = b3GetTwistAngle( relQ );+		b3Vec3 p2 = { wedgeRadius * b3Cos( twistAngle ), wedgeRadius * b3Sin( twistAngle ), 0.0f };+		draw->DrawSegmentFcn( frameA.p, b3TransformWorldPoint( frameA, p2 ), b3_colorYellow, draw->context );+	}+}
+ upstream/src/scheduler.c view
@@ -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 <stdbool.h>+#include <stdio.h>+#include <string.h>++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();+		}+	}+}
+ upstream/src/scheduler.h view
@@ -0,0 +1,15 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++typedef void b3TaskCallback( void* taskContext );+typedef struct b3Scheduler b3Scheduler;++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 );
+ upstream/src/sensor.c view
@@ -0,0 +1,451 @@+// 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 <stdlib.h>++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 );++	// Visitors must be convex.+	if ( b3IsConvex( otherShape->type ) == false )+	{+		return true;+	}++	// Are sensor events enabled on the other shape?+	if ( ( otherShape->flags & b3_enableSensorEvents ) == 0 )+	{+		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->flags & b3_enableCustomFiltering ) || ( otherShape->flags & b3_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 = b3ToRelativeTransform( b3GetBodyTransform( world, otherShape->bodyId ), b3Pos_zero );++	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+		b3Array( 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->flags & b3_enableSensorEvents ) == 0 )+		{+			if ( sensor->overlaps1.count != 0 )+			{+				// This sensor is dropping all overlaps because it has been disabled.+				b3SetBit( &taskContext->eventBits, sensorIndex );+			}+			continue;+		}++		b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), b3Pos_zero );++		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;+	}+}
+ upstream/src/sensor.h view
@@ -0,0 +1,41 @@+// 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+{+	b3Array( b3Visitor ) hits;+	b3Array( b3Visitor ) overlaps1;+	b3Array( b3Visitor ) overlaps2;+	int shapeId;+} b3Sensor;++typedef struct b3SensorTaskContext+{+	b3BitSet eventBits;+} b3SensorTaskContext;++void b3OverlapSensors( b3World* world );+void b3DestroySensor( b3World* world, b3Shape* sensorShape );
+ upstream/src/shape.c view
@@ -0,0 +1,2446 @@+// 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 "recording.h"+#include "sensor.h"++// needed for dll export+#include "aabb.h"+#include "compound.h"++#include "box3d/box3d.h"++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 float b3ComputeShapeMargin( b3Shape* shape )+{+	float margin = 0.0f;++	switch ( shape->type )+	{+		case b3_sphereShape:+		{+			margin = shape->sphere.radius;+		}+		break;++		case b3_capsuleShape:+		{+			margin = 0.5f * b3Distance( shape->capsule.center2, shape->capsule.center1 ) + shape->capsule.radius;+		}+		break;++		case b3_hullShape:+		{+			const b3HullData* hull = shape->hull;+			const b3Vec3* points = b3GetHullPoints( hull );+			float maxExtentSqr = 0.0f;+			int count = hull->vertexCount;+			for ( int i = 0; i < count; ++i )+			{+				float distSqr = b3DistanceSquared( points[i], hull->center );+				maxExtentSqr = b3MaxFloat( maxExtentSqr, distSqr );+			}+			margin = sqrtf( maxExtentSqr );+		}+		break;++		case b3_meshShape:+		case b3_heightShape:+		case b3_compoundShape:+		{+			// Static-only shapes: broadphase uses speculative distance for static+			// proxies, so the per-shape margin is never consumed in practice.+			// Return the cap so any incidental use is generous.+			return B3_MAX_AABB_MARGIN;+		}++		default:+			B3_VALIDATE( false );+			return B3_MAX_AABB_MARGIN;+	}++	return b3MinFloat( B3_MAX_AABB_MARGIN, B3_AABB_MARGIN_FRACTION * margin );+}++static void b3UpdateShapeAABBs( b3Shape* shape, b3WorldTransform transform, b3BodyType proxyType )+{+	// Compute a bounding box with a speculative margin+	const float speculativeDistance = B3_SPECULATIVE_DISTANCE;+	const float aabbMargin = shape->aabbMargin;++	b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, 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;+}++static b3Shape* b3CreateShapeInternal( b3World* world, b3Body* body, b3WorldTransform 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 = (b3CompoundData*)geometry;+			break;++		case b3_sphereShape:+			shape->sphere = *(b3Sphere*)geometry;+			break;++		case b3_hullShape:+			if ( haveShapeTransform )+			{+				// The transform and non-uniform scale are baked into fresh data, then shared.+				b3HullData* baked = b3CloneAndTransformHull( (b3HullData*)geometry, shapeTransform, scale );+				if ( baked == NULL )+				{+					// This can fail to produce a valid hull in extreme cases+					b3FreeId( &world->shapeIdPool, shapeId );+					shape->id = B3_NULL_INDEX;+					return NULL;+				}++				shape->hull = b3AddOwnedHullToDatabase( world, baked );+			}+			else+			{+				shape->hull = b3AddHullToDatabase( world, (const b3HullData*)geometry );+			}+			break;++		case b3_meshShape:+		{+			shape->mesh.data = (b3MeshData*)geometry;+			shape->mesh.scale = b3SafeScale( scale );+		}+		break;++		case b3_heightShape:+			shape->heightField = (b3HeightFieldData*)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->flags = 0;+	shape->flags |= def->enableSensorEvents ? b3_enableSensorEvents : 0;+	shape->flags |= def->enableContactEvents ? b3_enableContactEvents : 0;+	shape->flags |= def->enableCustomFiltering ? b3_enableCustomFiltering : 0;+	shape->flags |= def->enableHitEvents ? b3_enableHitEvents : 0;+	shape->flags |= def->enablePreSolveEvents ? b3_enablePreSolveEvents : 0;+	shape->flags |= def->enableSpeculativeContact ? b3_enableSpeculative : 0;+	shape->proxyKey = B3_NULL_INDEX;+	shape->localCentroid = b3GetShapeCentroid( shape );+	shape->aabbMargin = b3ComputeShapeMargin( shape );+	shape->aabb = (b3AABB){ b3Vec3_zero, b3Vec3_zero };+	shape->fatAABB = (b3AABB){ b3Vec3_zero, b3Vec3_zero };+	shape->nameId = b3AddName( &world->names, def->name );+	shape->generation += 1;++	if ( shape->type == b3_compoundShape )+	{+		// Own a copy of the compound materials so every shape frees its array the same way. Compounds+		// are few, so the copy is cheap and avoids aliasing the geometry blob.+		int materialCount = shape->compound->materialCount;+		shape->materialCount = materialCount;+		shape->materials = b3Alloc( materialCount * sizeof( b3SurfaceMaterial ) );+		memcpy( shape->materials, b3GetCompoundMaterials( shape->compound ), materialCount * sizeof( b3SurfaceMaterial ) );+	}+	else if ( def->materialCount > 1 && def->materials != NULL )+	{+		// Per triangle materials need a heap array.+		shape->materialCount = def->materialCount;+		shape->materials = b3Alloc( def->materialCount * sizeof( b3SurfaceMaterial ) );+		memcpy( shape->materials, def->materials, def->materialCount * sizeof( b3SurfaceMaterial ) );+	}+	else+	{+		// The common case is one material, stored inline with no allocation.+		shape->material = ( def->materialCount == 1 && def->materials != NULL ) ? def->materials[0] : def->baseMaterial;+		shape->materialCount = 1;+		shape->materials = NULL;+	}++	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;++	b3WorldTransform 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 );+	}+	else if ( ( body->flags & b3_dirtyMass ) == 0 )+	{+		body->flags |= b3_dirtyMass;+		b3SyncBodyFlags( 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 )+{+	b3ShapeId shapeId = b3CreateShape( bodyId, def, sphere, b3_sphereShape, b3Transform_identity, b3Vec3_one, false );+	if ( shapeId.index1 != 0 )+	{+		b3World* world = b3GetUnlockedWorld( bodyId.world0 );+		if ( world != NULL )+		{+			B3_REC_CREATE( world, CreateSphereShape, shapeId, bodyId, *def, *sphere );+		}+	}+	return shapeId;+}++b3ShapeId b3CreateCapsuleShape( b3BodyId bodyId, const b3ShapeDef* def, const b3Capsule* capsule )+{+	float lengthSqr = b3DistanceSquared( capsule->center1, capsule->center2 );+	b3ShapeId shapeId;+	if ( lengthSqr <= B3_LINEAR_SLOP * B3_LINEAR_SLOP )+	{+		b3Sphere sphere = { b3Lerp( capsule->center1, capsule->center2, 0.5f ), capsule->radius };+		shapeId = b3CreateShape( bodyId, def, &sphere, b3_sphereShape, b3Transform_identity, b3Vec3_one, false );+	}+	else+	{+		shapeId = b3CreateShape( bodyId, def, capsule, b3_capsuleShape, b3Transform_identity, b3Vec3_one, false );+	}+	if ( shapeId.index1 != 0 )+	{+		b3World* world = b3GetUnlockedWorld( bodyId.world0 );+		if ( world != NULL )+		{+			B3_REC_CREATE( world, CreateCapsuleShape, shapeId, bodyId, *def, *capsule );+		}+	}+	return shapeId;+}++b3ShapeId b3CreateHullShape( b3BodyId bodyId, const b3ShapeDef* def, const b3HullData* hull )+{+	B3_VALIDATE( b3IsValidHull( hull ) );+	B3_ASSERT( hull->version == B3_HULL_VERSION );+	B3_ASSERT( hull->hash != 0 );++	if ( hull->version != B3_HULL_VERSION )+	{+		return b3_nullShapeId;+	}++	b3ShapeId shapeId = b3CreateShape( bodyId, def, hull, b3_hullShape, b3Transform_identity, b3Vec3_one, false );+	if ( shapeId.index1 != 0 )+	{+		b3World* world = b3GetUnlockedWorld( bodyId.world0 );+		if ( world != NULL && world->recording != NULL )+		{+			uint32_t geometryId = b3RecInternHull( world->recording, hull );+			b3RecArgs_CreateHullShape createArgs = { bodyId, *def, geometryId };+			b3RecWriteRet_CreateHullShape( world->recording, &createArgs, shapeId );+		}+	}+	return shapeId;+}++b3ShapeId b3CreateTransformedHullShape( b3BodyId bodyId, const b3ShapeDef* def, const b3HullData* hull, b3Transform transform,+										b3Vec3 scale )+{+	B3_VALIDATE( b3IsValidHull( hull ) );+	B3_ASSERT( hull->version == B3_HULL_VERSION );+	B3_ASSERT( hull->hash != 0 );++	if ( hull->version != B3_HULL_VERSION )+	{+		return b3_nullShapeId;+	}++	b3ShapeId shapeId = b3CreateShape( bodyId, def, hull, b3_hullShape, transform, scale, true );+	if ( shapeId.index1 != 0 )+	{+		b3World* world = b3GetUnlockedWorld( bodyId.world0 );+		if ( world != NULL && world->recording != NULL )+		{+			// The transform and scale are baked into fresh hull data at create time. Record the baked hull+			// as a plain hull shape so replay rebuilds identical geometry with no rebake, and the keyframe+			// registry, which interns the live baked hull, stays seeded.+			b3Shape* shape = b3Array_Get( world->shapes, shapeId.index1 - 1 );+			uint32_t geometryId = b3RecInternHull( world->recording, shape->hull );+			b3RecArgs_CreateHullShape createArgs = { bodyId, *def, geometryId };+			b3RecWriteRet_CreateHullShape( world->recording, &createArgs, shapeId );+		}+	}+	return shapeId;+}++b3ShapeId b3CreateMeshShape( b3BodyId bodyId, const b3ShapeDef* def, const b3MeshData* mesh, b3Vec3 scale )+{+	B3_VALIDATE( b3IsValidMesh( mesh ) );+	B3_ASSERT( mesh->version == B3_MESH_VERSION );+	B3_ASSERT( mesh->hash != 0 );++	if ( mesh->version != B3_MESH_VERSION )+	{+		return b3_nullShapeId;+	}++	b3ShapeId shapeId = b3CreateShape( bodyId, def, mesh, b3_meshShape, b3Transform_identity, scale, true );+	if ( shapeId.index1 != 0 )+	{+		b3World* world = b3GetUnlockedWorld( bodyId.world0 );+		if ( world != NULL && world->recording != NULL )+		{+			uint32_t geometryId = b3RecInternMesh( world->recording, mesh );+			b3RecArgs_CreateMeshShape createArgs = { bodyId, *def, geometryId, scale };+			b3RecWriteRet_CreateMeshShape( world->recording, &createArgs, shapeId );+		}+	}+	return shapeId;+}++b3ShapeId b3CreateHeightFieldShape( b3BodyId bodyId, const b3ShapeDef* def, const b3HeightFieldData* heightField )+{+	B3_ASSERT( heightField->version == B3_HEIGHT_FIELD_VERSION );+	B3_ASSERT( heightField->hash != 0 );++	if ( heightField->version != B3_HEIGHT_FIELD_VERSION )+	{+		return b3_nullShapeId;+	}++	b3ShapeId shapeId = b3CreateShape( bodyId, def, heightField, b3_heightShape, b3Transform_identity, b3Vec3_one, false );+	if ( shapeId.index1 != 0 )+	{+		b3World* world = b3GetUnlockedWorld( bodyId.world0 );+		if ( world != NULL && world->recording != NULL )+		{+			uint32_t geometryId = b3RecInternHeightField( world->recording, heightField );+			b3RecArgs_CreateHeightFieldShape createArgs = { bodyId, *def, geometryId };+			b3RecWriteRet_CreateHeightFieldShape( world->recording, &createArgs, shapeId );+		}+	}+	return shapeId;+}++b3ShapeId b3CreateBakedCompoundShape( b3BodyId bodyId, b3ShapeDef* def, const b3CompoundData* compound )+{+	B3_ASSERT( compound->version == B3_COMPOUND_VERSION );++	if ( compound->version != B3_COMPOUND_VERSION )+	{+		return b3_nullShapeId;+	}++	b3ShapeId shapeId = b3CreateShape( bodyId, def, compound, b3_compoundShape, b3Transform_identity, b3Vec3_one, false );+	if ( shapeId.index1 != 0 )+	{+		b3World* world = b3GetUnlockedWorld( bodyId.world0 );+		if ( world != NULL && world->recording != NULL )+		{+			uint32_t geometryId = b3RecInternCompound( world->recording, compound );+			b3RecArgs_CreateCompoundShape createArgs = { bodyId, *def, geometryId };+			b3RecWriteRet_CreateCompoundShape( world->recording, &createArgs, shapeId );+		}+	}+	return shapeId;+}++// 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;+	}++	B3_REC( world, DestroyShape, shapeId, updateBodyMass );++	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 b3ComputeFatShapeAABB( const b3Shape* shape, b3WorldTransform transform, float extra )+{+	b3Vec3 r = { extra, extra, extra };+#if defined( BOX3D_DOUBLE_PRECISION )+	// Build the box in the body local frame, inflate, then translate by the double origin and+	// round outward. Inflating before the single rounding matters far from the origin where the+	// float margin would otherwise vanish.+	b3Transform rotation = { b3Vec3_zero, transform.q };+	b3AABB localBox = b3ComputeShapeAABB( shape, rotation );+	localBox.lowerBound = b3Sub( localBox.lowerBound, r );+	localBox.upperBound = b3Add( localBox.upperBound, r );+	return b3OffsetAABB( localBox, transform.p );+#else+	b3AABB aabb = b3ComputeShapeAABB( shape, transform );+	aabb.lowerBound = b3Sub( aabb.lowerBound, r );+	aabb.upperBound = b3Add( aabb.upperBound, r );+	return aabb;+#endif+}++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, b3WorldTransform 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:+			b3RemoveHullFromDatabase( world, shape->hull );+			shape->hull = NULL;+			break;++		default:+			break;+	}++	if ( shape->userShape != NULL && world->destroyDebugShape != NULL )+	{+		world->destroyDebugShape( shape->userShape, world->userDebugShapeContext );+		shape->userShape = NULL;+	}+}++void b3DestroyShapeAllocations( b3World* world, b3Shape* shape )+{+	b3DestroyShapeAllocationForShapeChange( world, shape );++	if ( shape->materials != NULL )+	{+		B3_ASSERT( shape->materialCount > 0 );+		b3Free( shape->materials, shape->materialCount * sizeof( b3SurfaceMaterial ) );+		shape->materials = NULL;+		shape->materialCount = 0;+	}++	// Name is stored inline. 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 b3HullData* hull = shape->hull;+			const b3Vec3* points = b3GetHullPoints( hull );+			return (b3ShapeProxy){ points, 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;+}++void b3Shape_SetName( b3ShapeId shapeId, const char* name )+{+	b3World* world = b3GetWorld( shapeId.world0 );++	B3_REC( world, ShapeSetName, shapeId, name );++	b3Shape* shape = b3GetShape( world, shapeId );+	shape->nameId = b3AddName( &world->names, name );+}++const char* b3Shape_GetName( b3ShapeId shapeId )+{+	b3World* world = b3GetWorld( shapeId.world0 );+	b3Shape* shape = b3GetShape( world, shapeId );+	return b3FindNameWithDefault( &world->names, shape->nameId, "" );+}++bool b3Shape_IsSensor( b3ShapeId shapeId )+{+	b3World* world = b3GetWorld( shapeId.world0 );+	b3Shape* shape = b3GetShape( world, shapeId );+	return shape->sensorIndex != B3_NULL_INDEX;+}++// todo no tests+b3WorldCastOutput b3Shape_RayCast( b3ShapeId shapeId, b3Pos origin, b3Vec3 translation )+{+	B3_ASSERT( b3IsValidPosition( origin ) );+	B3_ASSERT( b3IsValidVec3( translation ) );++	b3World* world = b3GetWorld( shapeId.world0 );+	b3Shape* shape = b3GetShape( world, shapeId );++	// Re-center on the origin so the cast runs in float precision far from the world origin+	b3Transform transform = b3ToRelativeTransform( b3GetBodyTransform( world, shape->bodyId ), origin );++	// The ray starts at the origin, so its origin in the re-centered frame is zero+	b3RayCastInput input = { b3Vec3_zero, translation, 1.0f };++	// Lift the re-centered float result back to a world position+	b3CastOutput local = b3RayCastShape( shape, transform, &input );+	b3WorldCastOutput output;+	output.normal = local.normal;+	output.point = b3OffsetPos( origin, local.point );+	output.fraction = local.fraction;+	output.iterations = local.iterations;+	output.triangleIndex = local.triangleIndex;+	output.childIndex = local.childIndex;+	output.materialIndex = local.materialIndex;+	output.hit = local.hit;++	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;+	}++	B3_REC( world, ShapeSetDensity, shapeId, density, updateBodyMass );++	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 );+	B3_REC( world, ShapeSetFriction, shapeId, friction );+	b3Shape* shape = b3GetShape( world, shapeId );+	B3_ASSERT( shape->type != b3_compoundShape );+	b3GetShapeMaterials( shape )[0].friction = friction;+}++float b3Shape_GetFriction( b3ShapeId shapeId )+{+	b3World* world = b3GetWorld( shapeId.world0 );+	b3Shape* shape = b3GetShape( world, shapeId );+	return b3GetShapeMaterials( shape )[0].friction;+}++void b3Shape_SetRestitution( b3ShapeId shapeId, float restitution )+{+	B3_ASSERT( b3IsValidFloat( restitution ) && restitution >= 0.0f );+	b3World* world = b3GetWorld( shapeId.world0 );+	B3_REC( world, ShapeSetRestitution, shapeId, restitution );+	b3Shape* shape = b3GetShape( world, shapeId );+	B3_ASSERT( shape->type != b3_compoundShape );+	b3GetShapeMaterials( shape )[0].restitution = restitution;+}++float b3Shape_GetRestitution( b3ShapeId shapeId )+{+	b3World* world = b3GetWorld( shapeId.world0 );+	b3Shape* shape = b3GetShape( world, shapeId );+	return b3GetShapeMaterials( shape )[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 );+	B3_REC( world, ShapeSetSurfaceMaterial, shapeId, surfaceMaterial );+	b3Shape* shape = b3GetShape( world, shapeId );+	B3_ASSERT( shape->type != b3_compoundShape );+	b3GetShapeMaterials( shape )[0] = surfaceMaterial;+}++b3SurfaceMaterial b3Shape_GetSurfaceMaterial( b3ShapeId shapeId )+{+	b3World* world = b3GetWorld( shapeId.world0 );+	b3Shape* shape = b3GetShape( world, shapeId );+	return b3GetShapeMaterials( shape )[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 );++	B3_REC( world, ShapeSetMeshMaterial, shapeId, surfaceMaterial, index );+	b3GetShapeMaterials( shape )[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 b3GetShapeMaterials( shape )[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 );+		}+	}++	b3WorldTransform 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;+	}++	B3_REC( world, ShapeSetFilter, shapeId, filter, invokeContacts );++	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;+	}++	B3_REC( world, ShapeEnableSensorEvents, shapeId, flag );++	b3Shape* shape = b3GetShape( world, shapeId );+	shape->flags = flag ? shape->flags | b3_enableSensorEvents : shape->flags & ~b3_enableSensorEvents;+}++bool b3Shape_AreSensorEventsEnabled( b3ShapeId shapeId )+{+	b3World* world = b3GetWorld( shapeId.world0 );+	b3Shape* shape = b3GetShape( world, shapeId );+	return shape->flags & b3_enableSensorEvents;+}++void b3Shape_EnableContactEvents( b3ShapeId shapeId, bool flag )+{+	b3World* world = b3GetUnlockedWorld( shapeId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B3_REC( world, ShapeEnableContactEvents, shapeId, flag );++	b3Shape* shape = b3GetShape( world, shapeId );+	shape->flags = flag ? shape->flags | b3_enableContactEvents : shape->flags & ~b3_enableContactEvents;+}++bool b3Shape_AreContactEventsEnabled( b3ShapeId shapeId )+{+	b3World* world = b3GetWorld( shapeId.world0 );+	b3Shape* shape = b3GetShape( world, shapeId );+	return shape->flags & b3_enableContactEvents;+}++void b3Shape_EnablePreSolveEvents( b3ShapeId shapeId, bool flag )+{+	b3World* world = b3GetUnlockedWorld( shapeId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B3_REC( world, ShapeEnablePreSolveEvents, shapeId, flag );++	b3Shape* shape = b3GetShape( world, shapeId );+	shape->flags = flag ? shape->flags | b3_enablePreSolveEvents : shape->flags & ~b3_enablePreSolveEvents;+}++bool b3Shape_ArePreSolveEventsEnabled( b3ShapeId shapeId )+{+	b3World* world = b3GetWorld( shapeId.world0 );+	b3Shape* shape = b3GetShape( world, shapeId );+	return shape->flags & b3_enablePreSolveEvents;+}++void b3Shape_EnableHitEvents( b3ShapeId shapeId, bool flag )+{+	b3World* world = b3GetUnlockedWorld( shapeId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B3_REC( world, ShapeEnableHitEvents, shapeId, flag );++	b3Shape* shape = b3GetShape( world, shapeId );+	shape->flags = flag ? shape->flags | b3_enableHitEvents : shape->flags & ~b3_enableHitEvents;+}++bool b3Shape_AreHitEventsEnabled( b3ShapeId shapeId )+{+	b3World* world = b3GetWorld( shapeId.world0 );+	b3Shape* shape = b3GetShape( world, shapeId );+	return shape->flags & b3_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 b3HullData* 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 b3HeightFieldData* 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;+	}++	B3_REC( world, ShapeSetSphere, shapeId, *sphere );++	world->locked = true;++	b3Shape* shape = b3GetShape( world, shapeId );++	b3DestroyShapeAllocationForShapeChange( world, shape );++	shape->sphere = *sphere;+	shape->type = b3_sphereShape;+	shape->aabbMargin = b3ComputeShapeMargin( shape );++	// 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;+	}++	B3_REC( world, ShapeSetCapsule, shapeId, *capsule );++	world->locked = true;++	b3Shape* shape = b3GetShape( world, shapeId );++	b3DestroyShapeAllocationForShapeChange( world, shape );++	shape->capsule = *capsule;+	shape->type = b3_capsuleShape;+	shape->aabbMargin = b3ComputeShapeMargin( shape );++	// 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 b3HullData* hull )+{+	B3_VALIDATE( b3IsValidHull( hull ) );+	B3_VALIDATE( hull->hash != 0 );++	b3World* world = b3GetUnlockedWorld( shapeId.world0 );+	if ( world == NULL )+	{+		return;+	}++	world->locked = true;++	b3Shape* shape = b3GetShape( world, shapeId );++	// Acquire the new hull before releasing the old so the input may safely alias+	// the shape's current shared data.+	const b3HullData* data = b3AddHullToDatabase( world, hull );++	// Same shared hull, avoid destroying contacts and recreating the proxy+	if ( shape->type == b3_hullShape && data == shape->hull )+	{+		b3RemoveHullFromDatabase( world, data );+		world->locked = false;+		return;+	}++	if ( world->recording != NULL )+	{+		// Intern the shared hull.+		uint32_t geometryId = b3RecInternHull( world->recording, data );+		b3RecArgs_ShapeSetHull setArgs = { shapeId, geometryId };+		b3RecWrite_ShapeSetHull( world->recording, &setArgs );+	}++	b3DestroyShapeAllocationForShapeChange( world, shape );++	shape->hull = data;+	shape->type = b3_hullShape;+	shape->aabbMargin = b3ComputeShapeMargin( shape );++	// 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;++	if ( world->recording != NULL )+	{+		uint32_t geometryId = b3RecInternMesh( world->recording, meshData );+		b3RecArgs_ShapeSetMesh setArgs = { shapeId, geometryId, scale };+		b3RecWrite_ShapeSetMesh( world->recording, &setArgs );+	}++	b3Shape* shape = b3GetShape( world, shapeId );++	b3DestroyShapeAllocationForShapeChange( world, shape );++	shape->mesh.data = meshData;+	shape->mesh.scale = b3SafeScale( scale );+	shape->type = b3_meshShape;+	shape->aabbMargin = b3ComputeShapeMargin( shape );++	// 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 );+	// Low level closest point query is a documented float carve-out far from the origin+	b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), b3Pos_zero );++	b3DistanceInput input;+	input.proxyA = b3MakeShapeProxy( shape );+	input.proxyB = (b3ShapeProxy){ &target, 1, 0.0f };+	input.transform = b3InvMulTransforms( transform, b3Transform_identity );+	input.useRadii = true;++	b3SimplexCache cache = { 0 };+	b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 );++	// Witness point comes back in frame A, lift it back to the query frame+	return b3TransformPoint( transform, 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;+	}++	B3_REC( world, ShapeApplyWind, shapeId, wind, drag, lift, maxSpeed, wake );++	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 );+	// Only the rotation is used below, so the demoted world transform is exact+	b3Transform transform = b3ToRelativeTransform( sim->transform, b3Pos_zero );++	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 = b3OffsetPos( sim->transform.p, b3MulMV( matrix, localCenter ) );+							line->p2 = b3OffsetPos( 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;++	int visitCount;+} b3MeshImpactContext;++static bool b3MeshTimeOfImpactFcn( b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleIndex, void* context )+{+	B3_UNUSED( triangleIndex );++	b3MeshImpactContext* toiContext = context;++	toiContext->visitCount += 1;++	// 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 b3CompoundData* 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++		uint64_t ticks = b3GetTicks();++		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,+		};++		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 );+		}++		float ms = b3GetMilliseconds( ticks );+		if ( ms > 1000.0f * b3GetStallThreshold() )+		{+			b3Log( "CCD stall: visited %d triangles", context.visitCount );+		}++		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 ){ &centroid, 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 b3GetShapeMaterials( shape )[materialIndex].userMaterialId;+}
+ upstream/src/shape.h view
@@ -0,0 +1,160 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "math_internal.h"++#include "box3d/types.h"++#include <stdbool.h>++typedef struct b3BroadPhase b3BroadPhase;+typedef struct b3World b3World;++typedef enum b3ShapeFlags+{+	b3_enableSensorEvents = 0x01,+	b3_enableContactEvents = 0x02,+	b3_enableCustomFiltering = 0x04,+	b3_enableHitEvents = 0x08,+	b3_enablePreSolveEvents = 0x10,+	b3_enlargedAABB = 0x20,+	b3_enableSpeculative = 0x40,+} b3ShapeFlags;++typedef struct b3Shape+{+	int id;+	int bodyId;+	int prevShapeId;+	int nextShapeId;+	int sensorIndex;+	int proxyKey;+	b3ShapeType type;+	float density;+	float explosionScale;+	float aabbMargin;++	b3AABB aabb;+	b3AABB fatAABB;+	b3Vec3 localCentroid;++	int materialCount;+	b3SurfaceMaterial material;+	b3SurfaceMaterial* materials;++	b3Filter filter;+	void* userData;+	void* userShape;++	uint32_t nameId;+	uint16_t generation;++	// b3ShapeFlags+	uint8_t flags;++	union+	{+		b3Capsule capsule;+		b3Sphere sphere;+		const b3HullData* hull;+		b3Mesh mesh;+		const b3HeightFieldData* heightField;+		const b3CompoundData* compound;+	};++} b3Shape;++// A single material shape keeps its material inline. Multi material meshes and compounds own a heap+// array. Reach the materials the same way for both: a single material shape presents its inline+// material as a one element array. Do not cache the pointer, the shapes array can move.+static inline b3SurfaceMaterial* b3GetShapeMaterials( const b3Shape* shape )+{+	return shape->materials != NULL ? shape->materials : (b3SurfaceMaterial*)&shape->material;+}++void b3CreateShapeProxy( b3Shape* shape, b3BroadPhase* bp, b3BodyType type, b3WorldTransform 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 );++// Conservative world AABB for a shape inflated by extra margin. In double precision mode the+// box is built in the body local frame, translated by the double origin, and rounded outward.+b3AABB b3ComputeFatShapeAABB( const b3Shape* shape, b3WorldTransform transform, float extra );+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 b3HullData* shape, const b3Capsule* mover );+int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* shape, const b3Capsule* mover );+int b3CollideMoverAndHeightField( b3PlaneResult* results, int capacity, const b3HeightFieldData* shape, const b3Capsule* mover );+int b3CollideMover( b3PlaneResult* planes, int planeCapacity, const b3Shape* shape, b3Transform transform,+					const b3Capsule* mover );++// Hull+int b3FindHullSupportVertex( const b3HullData* hull, b3Vec3 direction );+int b3FindHullSupportFace( const b3HullData* hull, b3Vec3 direction );+bool b3IsValidHull( const b3HullData* hull );+b3AABB b3ComputeSweptHullAABB( const b3HullData* shape, b3Transform xf1, b3Transform xf2 );+b3ShapeExtent b3ComputeHullExtent( const b3HullData* hull, b3Vec3 origin );+float b3ComputeHullProjectedArea( const b3HullData* hull, b3Vec3 direction );++// Height field+b3Triangle b3GetHeightFieldTriangle( const b3HeightFieldData* heightField, int triangleIndex );+int b3GetHeightFieldMaterial( const b3HeightFieldData* heightField, int triangleIndex );++static inline int b3GetHeightFieldTriangleCount( const b3HeightFieldData* 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 );++static 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;+}++static inline bool b3ShouldQueryCollide( const b3Filter* shapeFilter, const b3QueryFilter* queryFilter )+{+	return ( shapeFilter->categoryBits & queryFilter->maskBits ) != 0 &&+		   ( shapeFilter->maskBits & queryFilter->categoryBits ) != 0;+}++static inline bool b3IsConvex( b3ShapeType type )+{+	return type == b3_sphereShape || type == b3_capsuleShape || type == b3_hullShape;+}
+ upstream/src/simd.c view
@@ -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 );+}
+ upstream/src/simd.h view
@@ -0,0 +1,923 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "core.h"++#include <stdbool.h>++#if defined( B3_SIMD_NEON )++#include <arm_neon.h>++// wide float holds 4 numbers+typedef float32x4_t b3FloatW;++#elif defined( B3_SIMD_SSE2 )++#include <emmintrin.h>++// wide float holds 4 numbers+typedef __m128 b3FloatW;++#else++#include <math.h>+#include <string.h>++// scalar math+typedef struct b3FloatW+{+	float x, y, z, w;+} b3FloatW;++#endif++#if defined( B3_SIMD_SSE2 )++// 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 );+}++static inline b3V32 b3LoadV( const float* src )+{+	// Loads exactly 12 bytes: 8 via movsd, 4 via movss.+	// Result lane 3 is implicitly zero from the partial loads.+	__m128 xy = _mm_castpd_ps( _mm_load_sd( (const double*)( src ) ) );+	__m128 z = _mm_load_ss( src + 2 );+	return _mm_movelh_ps( xy, z ); // { src[0], src[1], src[2], 0.0f }+}++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 );+}++bool b3TestBoundsTriangleOverlap( b3V32 nodeCenter, b3V32 nodeExtent, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 );+float b3IntersectRayTriangle( b3V32 rayStart, b3V32 rayDelta, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 );++#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 b3SetW( float a, float b, float c, float d )+{+	float32_t array[4] = { a, b, c, d };+	return vld1q_f32( array );+}++static inline b3FloatW b3LoadW( const float* data )+{+	return vld1q_f32( data );+}++static inline void b3StoreW( float* data, b3FloatW a )+{+	vst1q_f32( data, a );+}++static inline b3FloatW b3NegW( b3FloatW a )+{+	return vnegq_f32( a );+}++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 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 b3AndW( b3FloatW a, b3FloatW b )+{+	return vreinterpretq_f32_u32( vandq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( 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 b3LessThanW( b3FloatW a, b3FloatW b )+{+	return vreinterpretq_f32_u32( vcltq_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+}++// _mm_movemask_ps equivalent, compatible with ARM v7.+static inline bool b3AnyTrueW( b3FloatW mask )+{+	uint32x4_t m = vreinterpretq_u32_f32( mask );+	uint32x2_t p = vorr_u32( vget_low_u32( m ), vget_high_u32( m ) );+	return ( vget_lane_u32( p, 0 ) | vget_lane_u32( p, 1 ) ) != 0;+}++// 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 );+}++static inline b3FloatW b3Dot3W( b3FloatW ax, b3FloatW ay, b3FloatW az, b3FloatW bx, b3FloatW by, b3FloatW bz )+{+	return vaddq_f32( vmulq_f32( ax, bx ), vaddq_f32( vmulq_f32( ay, by ), vmulq_f32( az, bz ) ) );+}++static inline b3FloatW b3EmbedIndexW( b3FloatW value, int baseIndex, int bitCount )+{+	uint32_t mask = ( 1u << bitCount ) - 1;+	const int32_t lanes[4] = { 0, 1, 2, 3 };+	int32x4_t index = vaddq_s32( vdupq_n_s32( baseIndex ), vld1q_s32( lanes ) );+	uint32x4_t clearLow = vdupq_n_u32( ~mask );+	uint32x4_t bits = vorrq_u32( vandq_u32( vreinterpretq_u32_f32( value ), clearLow ), vreinterpretq_u32_s32( index ) );+	return vreinterpretq_f32_u32( bits );+}++static inline int b3MinIndexW( b3FloatW a, int bitCount )+{+	float32x2_t m = vmin_f32( vget_low_f32( a ), vget_high_f32( a ) );+	m = vpmin_f32( m, m );+	uint32_t bits = vget_lane_u32( vreinterpret_u32_f32( m ), 0 );+	return (int)( bits & ( ( 1u << bitCount ) - 1 ) );+}++#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 b3SetW( float a, float b, float c, float d )+{+	return _mm_setr_ps( a, b, c, d );+}++static inline b3FloatW b3LoadW( const float* data )+{+	return _mm_loadu_ps( data );+}++static inline void b3StoreW( float* data, b3FloatW a )+{+	_mm_storeu_ps( data, a );+}++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 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 );+}++// a + b * c+static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c )+{+	return _mm_add_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 );+}++// Horizontal min over a 4-lane vector (result broadcast to all lanes).+static inline b3FloatW b3HorizontalMinW( b3FloatW v )+{+	v = _mm_min_ps( v, _mm_shuffle_ps( v, v, _MM_SHUFFLE( 2, 3, 0, 1 ) ) );+	return _mm_min_ps( v, _mm_shuffle_ps( v, v, _MM_SHUFFLE( 1, 0, 3, 2 ) ) );+}++// 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 b3AndW( b3FloatW a, b3FloatW b )+{+	return _mm_and_ps( a, 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 b3LessThanW( b3FloatW a, b3FloatW b )+{+	return _mm_cmplt_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;+}++static inline bool b3AnyTrueW( b3FloatW mask )+{+	return _mm_movemask_ps( mask ) != 0;+}++// 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 ) );+}++static inline b3FloatW b3Dot3W( b3FloatW ax, b3FloatW ay, b3FloatW az, b3FloatW bx, b3FloatW by, b3FloatW bz )+{+	return _mm_add_ps( _mm_mul_ps( ax, bx ), _mm_add_ps( _mm_mul_ps( ay, by ), _mm_mul_ps( az, bz ) ) );+}++// Replace the low bitCount mantissa bits of each lane with baseIndex + lane. The value must be+// positive so the embedded index sorts with the value, and ties fall to the lower index.+static inline b3FloatW b3EmbedIndexW( b3FloatW value, int baseIndex, int bitCount )+{+	int mask = ( 1 << bitCount ) - 1;+	__m128i index = _mm_add_epi32( _mm_set1_epi32( baseIndex ), _mm_setr_epi32( 0, 1, 2, 3 ) );+	__m128 clearLow = _mm_castsi128_ps( _mm_set1_epi32( ~mask ) );+	return _mm_or_ps( _mm_and_ps( value, clearLow ), _mm_castsi128_ps( index ) );+}++// Recovers the index embedded by b3EmbedIndexW from the lane holding the minimum.+static inline int b3MinIndexW( b3FloatW a, int bitCount )+{+	a = _mm_min_ps( a, _mm_shuffle_ps( a, a, _MM_SHUFFLE( 2, 3, 0, 1 ) ) );+	a = _mm_min_ps( a, _mm_shuffle_ps( a, a, _MM_SHUFFLE( 1, 0, 3, 2 ) ) );+	return _mm_cvtsi128_si32( _mm_castps_si128( a ) ) & ( ( 1 << bitCount ) - 1 );+}++#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 b3SetW( float a, float b, float c, float d )+{+	return (b3FloatW){ a, b, c, d };+}++static inline b3FloatW b3LoadW( const float* data )+{+	return (b3FloatW){ data[0], data[1], data[2], data[3] };+}++static inline void b3StoreW( float* data, b3FloatW a )+{+	data[0] = a.x;+	data[1] = a.y;+	data[2] = a.z;+	data[3] = a.w;+}++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 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;+}++// Logical operations on the scalar path are 0/1 float values. Not bit-wise like SIMD.++static inline b3FloatW b3AndW( 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 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 b3LessThanW( 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;+}++static inline bool b3AnyTrueW( b3FloatW mask )+{+	return mask.x != 0.0f || mask.y != 0.0f || mask.z != 0.0f || mask.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;+}++static inline b3FloatW b3Dot3W( b3FloatW ax, b3FloatW ay, b3FloatW az, b3FloatW bx, b3FloatW by, b3FloatW bz )+{+	b3FloatW r;+	r.x = ax.x * bx.x + ( ay.x * by.x + az.x * bz.x );+	r.y = ax.y * bx.y + ( ay.y * by.y + az.y * bz.y );+	r.z = ax.z * bx.z + ( ay.z * by.z + az.z * bz.z );+	r.w = ax.w * bx.w + ( ay.w * by.w + az.w * bz.w );+	return r;+}++static inline b3FloatW b3EmbedIndexW( b3FloatW value, int baseIndex, int bitCount )+{+	uint32_t mask = ( 1u << bitCount ) - 1;+	float lanes[4] = { value.x, value.y, value.z, value.w };++	for ( int i = 0; i < 4; ++i )+	{+		uint32_t bits;+		memcpy( &bits, lanes + i, sizeof( bits ) );+		bits = ( bits & ~mask ) | (uint32_t)( baseIndex + i );+		memcpy( lanes + i, &bits, sizeof( bits ) );+	}++	return (b3FloatW){ lanes[0], lanes[1], lanes[2], lanes[3] };+}++static inline int b3MinIndexW( b3FloatW a, int bitCount )+{+	float m = a.x;+	m = a.y < m ? a.y : m;+	m = a.z < m ? a.z : m;+	m = a.w < m ? a.w : m;++	uint32_t bits;+	memcpy( &bits, &m, sizeof( bits ) );+	return (int)( bits & ( ( 1u << bitCount ) - 1 ) );+}++#endif
+ upstream/src/solver.c view
@@ -0,0 +1,2331 @@+// 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 <limits.h>+#include <stddef.h>+#include <stdio.h>++_Static_assert( B3_RESTITUTION_ITERATIONS >= 1, "must be 1 or more" );++// 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)+			float i00 = inertiaLocal.cx.x;+			float i01 = inertiaLocal.cy.x;+			float i02 = inertiaLocal.cz.x;+			float i11 = inertiaLocal.cy.y;+			float i12 = inertiaLocal.cz.y;+			float i22 = inertiaLocal.cz.z;++			for ( int gyroIteration = 0; gyroIteration < B3_GYROSCOPIC_ITERATIONS; ++gyroIteration )+			{+				float w1 = omega2.x;+				float w2 = omega2.y;+				float w3 = omega2.z;++				// Iw = I * omega2 (shared between residual and Jacobian)+				float Iw1 = i00 * w1 + i01 * w2 + i02 * w3;+				float Iw2 = i01 * w1 + i11 * w2 + i12 * w3;+				float Iw3 = i02 * w1 + i12 * w2 + i22 * w3;++				// Residual: b = I * (omega2 - omega1) + h * cross(omega2, I * omega2)+				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;+	// World base for re-centering sweeps. Keeps TOI in float precision far from the origin.+	b3Pos base;+	float fraction;+	b3SensorHit sensorHits[B2_MAX_CONTINUOUS_SENSOR_HITS];+	float sensorFractions[B2_MAX_CONTINUOUS_SENSOR_HITS];+	int sensorCount;++	int visitCount;++	int distanceIterations;+	int pushBackIterations;+	int rootIterations;+} 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 = context;+	continuousContext->visitCount += 1;++	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 both shapes want sensor events+	bool isSensor = shape->sensorIndex != B3_NULL_INDEX;+	if ( isSensor && ( ( shape->flags & b3_enableSensorEvents ) == 0 || ( fastShape->flags & b3_enableSensorEvents ) == 0 ) )+	{+		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->flags & b3_enableCustomFiltering ) != 0 || ( fastShape->flags & b3_enableCustomFiltering ) != 0 )+	{+		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;+			}+		}+	}++	uint64_t ticks = b3GetTicks();++	// todo does having a sweep on shapeA help with bullets?+	b3Sweep sweepA = b3MakeRelativeSweep( bodySim, continuousContext->base );++	// 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->flags & b3_enablePreSolveEvents ) || ( fastShape->flags & b3_enablePreSolveEvents ) ) )+		{+			b3ShapeId shapeIdA = { shape->id + 1, world->worldId, shape->generation };+			b3ShapeId shapeIdB = { fastShape->id + 1, world->worldId, fastShape->generation };+			b3Pos point = b3OffsetPos( continuousContext->base, output.point );+			didHit = world->preSolveFcn( shapeIdA, shapeIdB, point, output.normal, world->preSolveContext );+		}++		if ( didHit )+		{+			fastBodySim->flags |= b3_hadTimeOfImpact;+			continuousContext->fraction = output.fraction;+			continuousContext->distanceIterations = b3MaxInt( continuousContext->distanceIterations, output.distanceIterations );+			continuousContext->pushBackIterations = b3MaxInt( continuousContext->pushBackIterations, output.pushBackIterations );+			continuousContext->rootIterations = b3MaxInt( continuousContext->rootIterations, output.rootIterations );+		}+	}++	float ms = b3GetMilliseconds( ticks );+	if ( ms > 1000.0f * b3GetStallThreshold() )+	{+		const char* nameFast = b3FindNameWithDefault( &world->names, fastBody->nameId, "NULL" );+		const char* name = b3FindNameWithDefault( &world->names, body->nameId, "NULL" );+		b3Log( "CCD stall: duration %.1f ms for %s versus %s", ms, nameFast, name );+	}++	// 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 );++	uint64_t ticks = b3GetTicks();++	b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet );+	b3BodySim* fastBodySim = b3Array_Get( awakeSet->bodySims, bodySimIndex );+	B3_ASSERT( fastBodySim->flags & b3_isFast );++	// Re-center the sweep on the fast body so the TOI and the swept query stay in float precision+	b3Pos base = fastBodySim->center0;++	b3Sweep sweep = b3MakeRelativeSweep( fastBodySim, base );++	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.base = base;+	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;+		// xf2 is relative to the base, so translate the box back to world space, rounding outward+		b3AABB box2 = b3OffsetAABB( b3ComputeShapeAABB( fastShape, xf2 ), base );++		// 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;++	if ( context.fraction < 1.0f )+	{+		// Handle time of impact event. The sweep is relative to the base, so re-add the base+		// to return the advanced pose to world space.+		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+		b3WorldTransform transform = { b3OffsetPos( base, origin ), q };+		b3Pos center = b3OffsetPos( base, c );+		fastBodySim->transform = transform;+		fastBodySim->center = center;+		fastBodySim->rotation0 = q;+		fastBodySim->center0 = center;++		// The move event was written before CCD, so correct it with the impact pose+		b3BodyMoveEvent* event = b3Array_Get( world->bodyMoveEvents, bodySimIndex );+		event->transform = fastBodySim->transform;++		// 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 = b3ComputeFatShapeAABB( shape, transform, speculativeScalar );+			shape->aabb = aabb;++			if ( b3AABB_Contains( shape->fatAABB, aabb ) == false )+			{+				float marginScalar = shape->aabbMargin;+				b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar };+				shape->fatAABB = (b3AABB){ b3Sub( aabb.lowerBound, aabbMargin ), b3Add( aabb.upperBound, aabbMargin ) };++				shape->flags |= b3_enlargedAABB;+				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 )+			{+				float marginScalar = shape->aabbMargin;+				b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar };+				shape->fatAABB = (b3AABB){+					.lowerBound = b3Sub( shape->aabb.lowerBound, aabbMargin ),+					.upperBound = b3Add( shape->aabb.upperBound, aabbMargin ),+				};++				shape->flags |= b3_enlargedAABB;+				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] );+		}+	}++	taskContext->distanceIterations = b3MaxInt( taskContext->distanceIterations, context.distanceIterations );+	taskContext->pushBackIterations = b3MaxInt( taskContext->pushBackIterations, context.pushBackIterations );+	taskContext->rootIterations = b3MaxInt( taskContext->rootIterations, context.rootIterations );++	float ms = b3GetMilliseconds( ticks );+	if ( ms > 1000.0f * b3GetStallThreshold() )+	{+		const char* nameFast = b3FindNameWithDefault( &world->names, fastBody->nameId, "NULL" );+		b3Vec3 c1 = sweep.c1;+		b3Vec3 c2 = sweep.c2;+		int vc = context.visitCount;+		b3Log( "CCD stall: duration %.1f ms and visit count %d for %s: c1 = (%g, %g, %g), c2 = (%g, %g, %g)", ms, vc, nameFast,+			   c1.x, c1.y, c1.z, c2.x, c2.y, c2.z );+	}++	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;++	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 )+		{+			const char* name = b3FindNameWithDefault( &world->names, bodies[sim->bodyId].nameId, "NULL" );+			b3Log( "unstable: %s", name );+		}++		B3_ASSERT( b3IsValidVec3( v ) );+		B3_ASSERT( b3IsValidVec3( w ) );++		sim->center = b3OffsetPos( 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 = b3OffsetPos( sim->center, b3Neg( b3RotateVector( sim->transform.q, sim->localCenter ) ) );++		// cache miss here, however I need the shape list below+		b3Body* body = bodies + sim->bodyId;+		body->bodyMoveIndex = simIndex;+		body->sleepVelocity = sleepVelocity;++		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;++		// If you hit this then it means you deferred mass computation but never called b3Body_ApplyMassFromShapes+		// or b3Body_SetMassData.+		B3_ASSERT( ( body->flags & b3_dirtyMass ) == 0 );++		body->flags &= ~b3_bodyTransientFlags;+		body->flags |= ( sim->flags & ( b3_isSpeedCapped | b3_hadTimeOfImpact ) );+		body->flags |= ( state->flags & ( b3_isSpeedCapped | b3_hadTimeOfImpact ) );+		sim->flags &= ~b3_bodyTransientFlags;+		state->flags &= ~b3_bodyTransientFlags;++		if ( enableSleep == false || ( body->flags & b3_enableSleep ) == 0 || 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. Track the sleepiest candidate.+			// Break sleep time ties using the island id to ensure determinism. The cross worker reduction+			// breaks ties the same way.+			if ( body->sleepTime > taskContext->splitSleepTime ||+				 ( body->sleepTime == taskContext->splitSleepTime && body->islandId > taskContext->splitIslandId ) )+			{+				// pick the sleepiest candidate+				taskContext->splitIslandId = body->islandId;+				taskContext->splitSleepTime = body->sleepTime;+			}+		}++		// Update shapes AABBs+		b3WorldTransform 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 = b3ComputeFatShapeAABB( shape, transform, speculativeScalar );+				shape->aabb = aabb;++				B3_ASSERT( ( shape->flags & b3_enlargedAABB ) == 0 );++				if ( b3AABB_Contains( shape->fatAABB, aabb ) == false )+				{+					float marginScalar = shape->aabbMargin;+					b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar };+					shape->fatAABB = (b3AABB){ b3Sub( aabb.lowerBound, aabbMargin ), b3Add( aabb.upperBound, aabbMargin ) };+					shape->flags |= b3_enlargedAABB;++					// 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+		for ( int iteration = 0; iteration < B3_RESTITUTION_ITERATIONS; ++iteration )+		{+			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 += B3_RESTITUTION_ITERATIONS * 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, B3_RESTITUTION_ITERATIONS, 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 b3World_Step also enters b3SolverTask 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, 16, 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 );+					b3Pos midCenter = b3LerpPosition( 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 = b3OffsetPos( 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->flags & b3_enlargedAABB )+							{+								b3BroadPhase_EnlargeProxy( broadPhase, shape->proxyKey, shape->fatAABB );+								shape->flags &= ~b3_enlargedAABB;+							}++							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->flags & b3_enlargedAABB ) == 0 )+				{+					shapeId = shape->nextShapeId;+					continue;+				}++				// clear flag+				shape->flags &= ~b3_enlargedAABB;++				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( b3GetBit( &broadPhase->movedProxies[b3_dynamicBody], proxyId ) );++				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 );+	}+}
+ upstream/src/solver.h view
@@ -0,0 +1,306 @@+// 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 <stdint.h>++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 b3World_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;++void b3Solve( b3World* world, b3StepContext* stepContext );++static 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 ( b3Softness ){+		.biasRate = omega / a1,+		.massScale = a2 * a3,+		.impulseScale = a3,+	};+}
+ upstream/src/solver_set.c view
@@ -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 <string.h>++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;+		}+	}+}
+ upstream/src/solver_set.h view
@@ -0,0 +1,67 @@+// 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.+	b3Array( b3BodySim ) bodySims;++	// Body state only exists for active set+	b3Array( b3BodyState ) bodyStates;++	// This holds sleeping/disabled joints. Empty for static/active set.+	b3Array( 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.+	b3Array( 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.+	b3Array( b3IslandSim ) islandSims;++	// Aligns with b3World::solverSetIdPool. Used to create a stable id for body/contact/joint/islands.+	int setIndex;+} b3SolverSet;++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 );
+ upstream/src/sphere.c view
@@ -0,0 +1,247 @@+// 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;++	// Inertia about the center of mass+	out.inertia = b3MakeDiagonalMatrix( ixx, ixx, ixx );+	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.transform = b3InvMulTransforms( shapeTransform, 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.transform = 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 )+{+	float totalRadius = mover->radius + shape->radius;+	b3Vec3 closest = b3PointToSegmentDistance( mover->center1, mover->center2, shape->center );++	// The normal points from the sphere toward the mover.+	float distance;+	b3Vec3 normal = b3GetLengthAndNormalize( &distance, b3Sub( closest, shape->center ) );++	if ( distance > totalRadius )+	{+		return 0;+	}++	float linearSlop = B3_LINEAR_SLOP;+	if ( distance < linearSlop )+	{+		// Deep overlap: the mover axis passes through the sphere center, so no+		// direction is preferred. Push perpendicular to the mover axis.+		float length;+		b3Vec3 axis = b3GetLengthAndNormalize( &length, b3Sub( mover->center2, mover->center1 ) );+		normal = length > linearSlop ? b3Perp( axis ) : b3Vec3_axisY;+		distance = 0.0f;+	}++	b3Plane plane = { normal, totalRadius - distance };+	*result = ( b3PlaneResult ){ plane, shape->center };+	return 1;+}
+ upstream/src/spherical_joint.c view
@@ -0,0 +1,745 @@+// 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"+#include "recording.h"++// needed for dll export+#include "box3d/box3d.h"++void b3SphericalJoint_EnableConeLimit( b3JointId jointId, bool enableLimit )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, SphericalJointEnableConeLimit, jointId, 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 );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, SphericalJointSetConeLimit, jointId, angleRadians );+	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 );+	b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );+	b3WorldTransform 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, SphericalJointEnableTwistLimit, jointId, 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 ) );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, SphericalJointSetTwistLimits, jointId, lowerLimitRadians, upperLimitRadians );++	float lowerAngle = b3MinFloat( lowerLimitRadians, upperLimitRadians );+	float upperAngle = b3MaxFloat( lowerLimitRadians, upperLimitRadians );++	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );+	base->sphericalJoint.lowerTwistAngle = b3ClampFloat( lowerAngle, -0.99f * B3_PI, 0.99f * B3_PI );+	base->sphericalJoint.upperTwistAngle = b3ClampFloat( upperAngle, -0.99f * B3_PI, 0.99f * B3_PI );+}++float b3SphericalJoint_GetTwistAngle( b3JointId jointId )+{+	b3World* world = b3GetWorld( jointId.world0 );+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );+	b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );+	b3WorldTransform 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, SphericalJointEnableSpring, jointId, 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 ) );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, SphericalJointSetTargetRotation, jointId, 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 );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, SphericalJointSetSpringHertz, jointId, hertz );+	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 );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, SphericalJointSetSpringDampingRatio, jointId, dampingRatio );+	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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, SphericalJointEnableMotor, jointId, 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 ) );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, SphericalJointSetMotorVelocity, jointId, 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 );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, SphericalJointSetMaxMotorTorque, jointId, maxForce );+	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 b3SphericalJoint_GetMotorTorque( b3JointId jointId )+{+	b3World* world = b3GetWorld( jointId.world0 );+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );+	return b3MulSV( world->inv_h, base->sphericalJoint.motorImpulse );+}++b3Vec3 b3GetSphericalJointForce( b3World* world, b3JointSim* base )+{+	b3Vec3 force = b3MulSV( world->inv_h, base->sphericalJoint.linearImpulse );+	return force;+}++b3Vec3 b3GetSphericalJointTorque( b3World* world, b3JointSim* base )+{+	b3WorldTransform xfA = b3GetBodyTransform( world, base->bodyIdA );+	b3WorldTransform 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 = b3SubPos( 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, b3WorldTransform transformA, b3WorldTransform transformB, float scale )+{+	b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA );++	float length1 = 0.1f * scale;+	draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisX ) ) ), b3_colorRed,+						  draw->context );+	draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisY ) ) ), b3_colorGreen,+						  draw->context );+	draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisZ ) ) ), b3_colorBlue,+						  draw->context );++	b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB );++	float length2 = 0.2f * scale;+	draw->DrawSegmentFcn( frameB.p, b3OffsetPos( frameB.p, b3MulSV( 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, b3TransformWorldPoint( frameA, vertex1 ), b3_colorCyan, draw->context );+			}++			if ( index == kSliceCount - 1 )+			{+				draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex2 ), frameA.p, b3_colorCyan, draw->context );+			}+			draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex1 ), b3TransformWorldPoint( frameA, vertex2 ), b3_colorCyan,+								  draw->context );+		}++		float twistAngle = b3GetTwistAngle( relQ );+		b3Vec3 p2 = { wedgeRadius * b3Cos( twistAngle ), wedgeRadius * b3Sin( twistAngle ), 0.0f };+		draw->DrawSegmentFcn( frameA.p, b3TransformWorldPoint( 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, b3TransformWorldPoint( frameA, vertex1 ), b3_colorCyan, draw->context );+			draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex1 ), b3TransformWorldPoint( frameA, vertex2 ), b3_colorCyan,+								  draw->context );+		}+	}+}
+ upstream/src/table.c view
@@ -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 <string.h>++#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;+}
+ upstream/src/table.h view
@@ -0,0 +1,53 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "box3d/constants.h"++#include <stdbool.h>+#include <stdint.h>++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 ) );+}++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 );
+ upstream/src/timer.c view
@@ -0,0 +1,701 @@+// 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 <stddef.h>+#include <stdio.h>+#include <string.h>++#define NAME_LENGTH 16++#if defined( _WIN32 )++#ifndef WIN32_LEAN_AND_MEAN+#define WIN32_LEAN_AND_MEAN 1+#endif++// Lower-case windows.h intentionally for cross compiling on mingw.+#include <windows.h>+#include <limits.h>++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();+}++void b3Sleep( int milliseconds )+{+	Sleep( (DWORD)milliseconds );+}++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[NAME_LENGTH];+} 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[NAME_LENGTH];+	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 <sched.h>+#include <time.h>++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();+}++void b3Sleep( int milliseconds )+{+	struct timespec ts;+	ts.tv_sec = milliseconds / 1000;+	ts.tv_nsec = ( milliseconds % 1000 ) * 1000000L;+	nanosleep( &ts, NULL );+}++#include <pthread.h>+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 <semaphore.h>++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[NAME_LENGTH];+} 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[NAME_LENGTH];+	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 <mach/mach_time.h>+#include <sched.h>+#include <sys/time.h>+#include <time.h>++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();+}++void b3Sleep( int milliseconds )+{+	struct timespec ts;+	ts.tv_sec = milliseconds / 1000;+	ts.tv_nsec = ( milliseconds % 1000 ) * 1000000L;+	nanosleep( &ts, NULL );+}++#include <pthread.h>+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 <dispatch/dispatch.h>++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[NAME_LENGTH];+} 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 )+{+}++void b3Sleep( int milliseconds )+{+	( (void)( milliseconds ) );+}++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;+}
+ upstream/src/triangle_manifold.c view
@@ -0,0 +1,1334 @@+// 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 <stdbool.h>+#include <stddef.h>++typedef struct b3TriangleData+{+	b3Vec3 v1, v2, v3;+	b3Vec3 e1, e2, e3;+	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 b3CollideTriangleAndSphere( b3LocalManifold* manifold, int capacity, const b3Vec3* triangleA, const b3Sphere* sphereB )+{+	manifold->pointCount = 0;++	if ( capacity == 0 )+	{+		return;+	}++	b3Vec3 center = sphereB->center;+	b3Vec3 v1 = triangleA[0], v2 = triangleA[1], v3 = triangleA[2];+	b3Plane plane = b3MakePlaneFromPoints( v1, v2, v3 );++	float offset = b3PlaneSeparation( plane, center );+	if ( offset < 0.0f )+	{+		// Cull back side collision+		return;+	}++	float radius = sphereB->radius;++	// 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 = radius + speculativeDistance;+	if ( squaredDistance > maxDistance * maxDistance )+	{+		return;+	}++	float distance = sqrtf( squaredDistance );++	// Normal points from triangle to sphere+	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+	// p = 0.5 * (c + q) - 0.5 * r * n+	b3Vec3 contactPoint = b3MulSV( 0.5f, b3Add( b3Sub( center, b3MulSV( 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 - 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 > 0.0f ) != ( 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 b3SeparatingAxis b3QueryTriangleFaceAndCapsule( b3Plane plane, const b3Capsule* capsule )+{+	float separation1 = b3PlaneSeparation( plane, capsule->center1 );+	float separation2 = b3PlaneSeparation( plane, capsule->center2 );++	if ( separation1 < separation2 )+	{+		return (b3SeparatingAxis){+			.normal = plane.normal,+			.separation = separation1,+			.indexA = 0,+			.indexB = 0,+		};+	}++	return (b3SeparatingAxis){+		.normal = plane.normal,+		.separation = separation2,+		.indexA = 0,+		.indexB = 1,+	};+}++static b3SeparatingAxis b3QueryTriangleAndCapsuleEdges( const b3Vec3* vertices, b3Plane plane, const b3Capsule* capsule )+{+	// Work in the local space of the capsule+	b3Vec3 p1 = capsule->center1;+	b3Vec3 p2 = capsule->center2;+	b3Vec3 capsuleEdge = b3Sub( p2, p1 );++	// Find axis of minimum penetration+	b3Vec3 maxNormal = b3Vec3_zero;+	float maxSeparation = -FLT_MAX;+	int maxIndex1 = B3_NULL_INDEX;+	int maxIndex2 = B3_NULL_INDEX;+	float squaredTolerance = 0.005f * 0.005f;++	int edgeIndex = 2;+	float a = b3Dot( capsuleEdge, plane.normal );+	b3Vec3 v1 = vertices[2];+	for ( int index = 0; index < 3; ++index )+	{+		b3Vec3 v2 = vertices[index];+		b3Vec3 triangleEdge = b3Sub( v2, v1 );+		b3Vec3 sideNormal = b3Normalize( b3Cross( triangleEdge, plane.normal ) );++		// Pretend the triangle edge embeds a zero area face with a side normal. This+		// provides a way to find an edge-edge normal that points outward from+		// the triangle.+		float b = b3Dot( capsuleEdge, sideNormal );++		// Is the capsule edge parallel to the triangle edge? If so, face contact can handle it.+		if ( a * a + b * b < squaredTolerance * b3LengthSquared( capsuleEdge ) )+		{+			v1 = v2;+			edgeIndex = index;+			continue;+		}++		// Similar to hull vs hull (b3QueryEdgeDirections)+		b3Vec3 axis;+		if ( a * b <= 0.0f )+		{+			float t = b / ( b - a );+			axis = b3Lerp( sideNormal, plane.normal, t );+		}+		else+		{+			float t = b / ( a + b );+			axis = b3Lerp( sideNormal, b3Neg( plane.normal ), t );+		}++		B3_VALIDATE( b3LengthSquared( axis ) > 1000.0f * FLT_MIN );+		axis = b3Normalize( axis );+		float separation = b3Dot( axis, b3Sub( p1, v1 ) );++		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.+			maxNormal = axis;+			maxSeparation = separation;+			maxIndex1 = edgeIndex;+			maxIndex2 = 0;+		}++		v1 = v2;+		edgeIndex = index;+	}++	// Save result+	return (b3SeparatingAxis){+		.normal = maxNormal,+		.separation = maxSeparation,+		.indexA = maxIndex1,+		.indexB = 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 radius = capsule->radius;+	float distance1 = b3PlaneSeparation( plane, segment[0].position );+	float distance2 = b3PlaneSeparation( plane, segment[1].position );++	float speculativeDistance = B3_SPECULATIVE_DISTANCE;+	if ( distance1 > speculativeDistance + radius && distance2 > speculativeDistance + radius )+	{+		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, b3Plane plane,+												  const b3Capsule* capsule, b3SeparatingAxis 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 v1 = vs[query.indexA];+	b3Vec3 v2 = vs[( query.indexA + 1 ) % 3];+	b3Vec3 triangleEdge = b3Sub( v2, v1 );++	b3Vec3 sideNormal = b3Normalize( b3Cross( triangleEdge, plane.normal ) );++	// Pretend the triangle edge embeds a zero area face with a side normal. This+	// provides a way to find an edge-edge normal that points outward from+	// the triangle.+	float a = b3Dot( capsuleEdge, plane.normal );+	float b = b3Dot( capsuleEdge, sideNormal );++	// Is the capsule edge parallel to the triangle edge? If so, face contact can handle it.+	float squaredTolerance = 0.005f * 0.005f;+	if ( a * a + b * b < squaredTolerance * b3LengthSquared( capsuleEdge ) )+	{+		return;+	}++	// Similar to hull vs hull (b3QueryEdgeDirections)+	b3Vec3 normal = query.normal;+	b3SegmentDistanceResult result = b3LineDistance( v1, triangleEdge, p1, capsuleEdge );++	if ( result.fraction1 < 0.0f || 1.0f < result.fraction1 || result.fraction2 < 0.0f || 1.0f < result.fraction2 )+	{+		// closest point beyond end points+		return;+	}++	b3Vec3 point = b3Lerp( result.point1, b3MulSub( result.point2, capsule->radius, normal ), 0.5f );+	float separation = b3Dot( normal, b3Sub( p1, v1 ) );++	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 b3CollideTriangleAndCapsule( b3LocalManifold* manifold, int capacity, const b3Vec3* triangleA, const b3Capsule* capsuleB,+								  b3SimplexCache* cache )+{+	manifold->pointCount = 0;++	if ( capacity < 2 )+	{+		return;+	}++	b3Vec3 v1 = triangleA[0], v2 = triangleA[1], v3 = triangleA[2];+	b3Plane plane = b3MakePlaneFromPoints( v1, v2, v3 );+	b3Vec3 capsuleCenter = b3Lerp( capsuleB->center1, capsuleB->center2, 0.5f );++	float offset = b3PlaneSeparation( plane, capsuleCenter );+	if ( offset < 0.0f )+	{+		// Cull back side collision+		return;+	}++	b3DistanceInput distanceInput;+	distanceInput.proxyA = (b3ShapeProxy){ triangleA, 3, 0.0f };+	distanceInput.proxyB = (b3ShapeProxy){ &capsuleB->center1, 2, 0.0f };+	distanceInput.transform = b3Transform_identity;+	distanceInput.useRadii = false;++	b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, cache, NULL, 0 );+	float speculativeDistance = B3_SPECULATIVE_DISTANCE;+	float radius = capsuleB->radius;+	if ( distanceOutput.distance > radius + speculativeDistance )+	{+		// 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 = capsuleB->center1;+			segment[0].separation = 0.0f;+			segment[0].pair = b3MakeFeaturePair( b3_featureShapeA, 0, b3_featureShapeA, 0 );+			segment[1].position = capsuleB->center2;+			segment[1].separation = 0.0f;+			segment[1].pair = b3MakeFeaturePair( b3_featureShapeA, 1, b3_featureShapeA, 1 );++			bool havePoints = b3ClipSegmentToTriangleFace( segment, triangleA, 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 );++				// Normal points from triangle to capsule+				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 = b3Lerp( distanceOutput.pointA, b3MulSub( distanceOutput.pointB, radius, delta ), 0.5f );++		// Normal points from triangle to capsule.+		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++	b3SeparatingAxis faceQuery = b3QueryTriangleFaceAndCapsule( plane, capsuleB );+	if ( faceQuery.separation > radius )+	{+		// Shapes are separated. Should be impossible for a reasonable capsule radius.+		return;+	}++	b3SeparatingAxis edgeQuery = b3QueryTriangleAndCapsuleEdges( triangleA, plane, capsuleB );+	if ( edgeQuery.separation > radius )+	{+		// Shapes are separated. Should be impossible for a reasonable capsule radius.+		return;+	}++	// Create face contact+	float faceSeparation = faceQuery.separation - radius;+	b3BuildTriangleAndCapsuleFaceContact( manifold, triangleA, plane, capsuleB );+	B3_VALIDATE( manifold->pointCount == 0 || manifold->pointCount == 2 );+	if ( manifold->pointCount == 2 )+	{+		// This becomes the clipped separation.+		faceSeparation = b3MinFloat( manifold->points[0].separation, manifold->points[1].separation );+	}++	if ( edgeQuery.indexA == B3_NULL_INDEX )+	{+		// No valid edge contact.+		return;+	}++	// 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!+	float linearSlop = B3_LINEAR_SLOP;+	float edgeSeparation = edgeQuery.separation - radius;+	if ( manifold->pointCount == 0 || edgeSeparation > faceSeparation + linearSlop )+	{+		// Edge contact+		b3BuildTriangleAndCapsuleEdgeContact( manifold, triangleA, plane, capsuleB, 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 b3SeparatingAxis b3QueryTriangleFace( const b3TriangleData* triangle, const b3HullData* hull )+{+	const b3Vec3* hullPoints = b3GetHullPoints( hull );+	b3Plane plane = triangle->plane;+	b3Vec3 normal = b3Neg( plane.normal );+	int vertexIndex = b3FindHullSupportVertex( hull, normal );+	b3Vec3 support = hullPoints[vertexIndex];+	float separation = b3PlaneSeparation( plane, support );++	return (b3SeparatingAxis){+		.normal = plane.normal,+		.separation = separation,+		.indexA = 0,+		.indexB = vertexIndex,+		.type = b3_faceAxisA,+	};+}++static b3SeparatingAxis b3QueryHullFace( const b3TriangleData* triangle, const b3HullData* hull )+{+	const b3Plane* hullPlanes = b3GetHullPlanes( hull );+	int faceCount = hull->faceCount;++	b3Vec3 trianglePoints[] = { triangle->v1, triangle->v2, triangle->v3 };++	b3Vec3 maxNormal = b3Vec3_zero;+	float maxFaceSeparation = -INFINITY;+	int maxFaceIndex = B3_NULL_INDEX;+	int maxVertexIndex = B3_NULL_INDEX;++	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 )+		{+			maxNormal = plane.normal;+			maxFaceSeparation = separation;+			maxFaceIndex = faceIndex;+			maxVertexIndex = vertexIndex;+		}+	}++	// Normal points from triangle to hull+	return (b3SeparatingAxis){+		.normal = b3Neg( maxNormal ),+		.separation = maxFaceSeparation,+		.indexA = maxVertexIndex,+		.indexB = maxFaceIndex,+		.type = b3_faceAxisB,+	};+}++// A: hull, B: triangle+static b3SeparatingAxis b3QueryTriangleAndHullEdges( const b3TriangleData* triangle, const b3HullData* hull )+{+	b3SeparatingAxis result = {+		.normal = b3Vec3_zero,+		.separation = -INFINITY,+		.indexA = B3_NULL_INDEX,+		.indexB = B3_NULL_INDEX,+		.type = b3_edgePairAxis,+	};++	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;+	float squaredTolerance = 0.005f * 0.005f;++	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;+			}++			// Avoid nearly parallel edges that may lead to invalid separation values at the noise floor.+			if ( b3MaxFloat( cab * cab, dab * dab ) < squaredTolerance * b3LengthSquared( triEdge ) )+			{+				continue;+			}++			// Similar to hull vs hull (b3QueryEdgeDirections)+			// dot(hullNormal1 + t * (hullNormal2 - hullNormal1), triEdge) = 0+			// Normal points out of hull by construction.+			float t = cab / ( cab - dab );+			b3Vec3 axis = b3Lerp( hullNormal1, hullNormal2, t );+			B3_VALIDATE( b3LengthSquared( axis ) > 1000.0f * FLT_MIN );+			axis = b3Normalize( axis );+			float separation = b3Dot( axis, b3Sub( trianglePoints[j], hullPoint ) );++			// 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.+				// Flip normal to point from triangle to hull.+				result.normal = b3Neg( axis );+				result.separation = separation;+				result.indexA = j;+				result.indexB = i;+			}+		}+	}++	return result;+}++static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, const b3TriangleData* triangle,+								const b3HullData* hull, b3SeparatingAxis query, b3SATCache* cache, bool enableSpeculative )+{+	B3_VALIDATE( query.type == b3_faceAxisB );+	B3_VALIDATE( 0 <= query.indexA && query.indexA < 3 );+	B3_VALIDATE( 0 <= query.indexB && query.indexB < hull->faceCount );++	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+	b3Plane refPlane = hullPlanes[query.indexB];++	// 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 + query.indexB;+	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;+	int finalPointCount = 0;++	for ( int i = 0; i < pointCount; ++i )+	{+		b3ClipVertex* clipPoint = input + i;+		minSeparation = b3MinFloat( minSeparation, clipPoint->separation );++		if ( enableSpeculative == false && clipPoint->separation > 0.0f )+		{+			continue;+		}++		// Move point onto hull face improved culling+		b3Vec3 point = b3MulSub( clipPoint->position, clipPoint->separation, refPlane.normal );++		b3LocalManifoldPoint* pt = manifold->points + finalPointCount;+		pt->point = point;+		pt->separation = clipPoint->separation;+		pt->pair = b3FlipPair( clipPoint->pair );++		finalPointCount += 1;+	}++	float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f;+	if ( minSeparation > speculativeDistance )+	{+		// This can occur with a stale SAT cache+		manifold->pointCount = 0;+		*cache = (b3SATCache){ 0 };+		return minSeparation;+	}++	manifold->pointCount = finalPointCount;+	manifold->normal = b3Neg( refPlane.normal );+	manifold->feature = b3_featureHullFace;++	// Save cache+	cache->separation = minSeparation;+	cache->type = b3_faceAxisB;+	cache->indexA = (uint8_t)query.indexA;+	cache->indexB = (uint8_t)query.indexB;+	return minSeparation;+}++static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity, const b3TriangleData* triangle,+									const b3HullData* hull, b3SeparatingAxis query, b3SATCache* cache, bool enableSpeculative )+{+	B3_VALIDATE( query.type == b3_faceAxisA );+	B3_VALIDATE( query.indexA == 0 );+	B3_VALIDATE( 0 <= query.indexB && query.indexB < hull->vertexCount );+	B3_VALIDATE( manifold->pointCount == 0 );++	const b3HullFace* hullFaces = b3GetHullFaces( hull );+	const b3HullHalfEdge* hullEdges = b3GetHullEdges( hull );+	const b3Vec3* hullPoints = b3GetHullPoints( hull );++	// Find incident face+	b3Plane refPlane = triangle->plane;++	int incFace = b3FindIncidentFace( hull, refPlane.normal, query.indexB );++	// 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;++	int finalPointCount = 0;+	for ( int i = 0; i < pointCount; ++i )+	{+		b3ClipVertex* clipPoint = input + i;+		minSeparation = b3MinFloat( minSeparation, clipPoint->separation );++		if ( enableSpeculative == false && clipPoint->separation > 0.0f )+		{+			continue;+		}++		// 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 + finalPointCount;+		pt->point = point;+		pt->separation = clipPoint->separation;+		pt->pair = clipPoint->pair;++		finalPointCount += 1;+	}++	float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f;+	if ( minSeparation >= speculativeDistance )+	{+		// This can happens if the objects move a part while re-using a cached axis+		*cache = (b3SATCache){ 0 };+		return minSeparation;+	}++	manifold->pointCount = finalPointCount;+	manifold->normal = refPlane.normal;+	manifold->feature = b3_featureTriangleFace;++	// Save cache+	cache->separation = minSeparation;+	cache->type = b3_faceAxisA;+	cache->indexA = (uint8_t)query.indexA;+	cache->indexB = (uint8_t)query.indexB;+	return minSeparation;+}++static void b3CollideTriangleAndHullEdges( b3LocalManifold* manifold, int capacity, b3Vec3 trianglePoint, b3Vec3 triangleEdge,+										   const b3HullData* hull, b3SeparatingAxis query, b3SATCache* cache )+{+	B3_VALIDATE( query.type == b3_edgePairAxis );+	B3_VALIDATE( 0 <= query.indexA && query.indexA < 3 );+	B3_VALIDATE( 0 <= query.indexB && query.indexB < hull->edgeCount );+	B3_VALIDATE( query.separation <= 2.0f * B3_SPECULATIVE_DISTANCE );++	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 );++	// Get the closest points between the infinite edge lines+	b3SegmentDistanceResult result = b3LineDistance( pA, eA, pB, eB );++	// Is one of the closest points outside of the associated edge segment?+	if ( capacity == 0 || result.fraction1 < 0.0f || 1.0f < result.fraction1 || result.fraction2 < 0.0f ||+		 1.0f < result.fraction2 )+	{+		// Invalid edge pair, no points generated+		B3_ASSERT( manifold->pointCount == 0 );+		*cache = (b3SATCache){ 0 };+		return;+	}++	float separation = b3Dot( query.normal, b3Sub( pB, pA ) );+	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 = query.normal;+	manifold->pointCount = 1;++	b3TriangleFeature edgesFeatures[] = { b3_featureEdge1, b3_featureEdge2, b3_featureEdge3 };+	manifold->feature = edgesFeatures[query.indexA];+}++b3AtomicInt b3_triangleConvexCalls;+b3AtomicInt b3_triangleCacheHits;++// Triangle is in the local space of the hull for efficiency.+void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, int triangleFlags,+							   const b3HullData* hullB, b3SATCache* cache, bool enableSpeculative )+{+	manifold->pointCount = 0;+	manifold->feature = b3_featureNone;++	if ( capacity < 4 )+	{+		return;+	}++	b3Plane trianglePlane = b3MakePlaneFromPoints( v1, v2, v3 );+	float linearSlop = B3_LINEAR_SLOP;++	float offset = b3PlaneSeparation( trianglePlane, hullB->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;+	}++	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],+		.plane = trianglePlane,+		.flags = triangleFlags,+	};++	const b3HullHalfEdge* edges = b3GetHullEdges( hullB );+	const b3Plane* hullPlanes = b3GetHullPlanes( hullB );+	const b3Vec3* hullPoints = b3GetHullPoints( hullB );++	float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f;+	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( hullB, b3Neg( trianglePlane.normal ) );+			b3Vec3 support = hullPoints[vertexIndex];+			float separation = b3PlaneSeparation( trianglePlane, support );+			if ( separation > speculativeDistance )+			{+				// Cache hit, shapes are separated+				return;+			}++			b3SeparatingAxis faceQuery;+			faceQuery.normal = trianglePlane.normal;+			faceQuery.separation = separation;+			faceQuery.indexA = cache->indexA;+			faceQuery.indexB = vertexIndex;+			faceQuery.type = b3_faceAxisA;++			// Read cache but don't modify it+			b3SATCache localCache = *cache;+			float clippedSeparation =+				b3CollideTriangleFace( manifold, capacity, &triangle, hullB, faceQuery, &localCache, enableSpeculative );++			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 < hullB->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+				b3SeparatingAxis faceQuery;+				faceQuery.normal = b3Neg( plane.normal );+				faceQuery.separation = separation;+				faceQuery.indexA = vertexIndex;+				faceQuery.indexB = cache->indexB;+				faceQuery.type = b3_faceAxisB;++				// Read cache but don't modify it+				b3SATCache localCache = *cache;+				float clippedSeparation =+					b3CollideHullFace( manifold, capacity, &triangle, hullB, faceQuery, &localCache, enableSpeculative );++				// 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 < hullB->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.+			// See "Collision Detection of Convex Polyhedra Based on Duality Transformation"+			// Simplified for triangle versus hull.+			float cab = b3Dot( hullNormal1, triEdge );+			float dab = b3Dot( hullNormal2, triEdge );+			float bcd = b3Dot( trianglePlane.normal, hullEdge );++			if ( cab * dab < 0.0f && cab * bcd > 0.0f )+			{+				float squaredTolerance = 0.005f * 0.005f;++				// Avoid nearly parallel edges that may lead to invalid separation values at the noise floor.+				if ( b3MaxFloat( cab * cab, dab * dab ) >= squaredTolerance * b3LengthSquared( triEdge ) )+				{+					// Similar to hull vs hull (b3QueryEdgeDirections)+					// dot(hullNormal1 + t * (hullNormal2 - hullNormal1), triEdge) = 0+					// Normal points out of hull by construction.+					float t = cab / ( cab - dab );+					b3Vec3 axis = b3Lerp( hullNormal1, hullNormal2, t );+					B3_VALIDATE( b3LengthSquared( axis ) > 1000.0f * FLT_MIN );+					axis = b3Normalize( axis );+					float separation = b3Dot( axis, b3Sub( triPoint, hullPoint ) );+					if ( separation > speculativeDistance )+					{+						// Cache hit, shapes are separated+						return;+					}++					if ( b3AbsFloat( cache->separation - separation ) < linearSlop )+					{+						// Try to rebuild contact from last features+						// Flip normal to point from triangle to hull+						b3SeparatingAxis edgeQuery;+						edgeQuery.normal = b3Neg( axis );+						edgeQuery.indexA = indexA;+						edgeQuery.indexB = indexB;+						edgeQuery.separation = separation;+						edgeQuery.type = b3_edgePairAxis;++						// Read cache but don't modify it+						b3SATCache localCache = *cache;+						b3CollideTriangleAndHullEdges( manifold, capacity, triPoint, triEdge, hullB, 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:+		{+			b3SeparatingAxis query = b3QueryTriangleFace( &triangle, hullB );+			b3CollideTriangleFace( manifold, capacity, &triangle, hullB, query, cache, enableSpeculative );+			return;+		}++			// This case is for testing+		case b3_manualFaceAxisB:+		{+			b3SeparatingAxis query = b3QueryHullFace( &triangle, hullB );+			b3CollideHullFace( manifold, capacity, &triangle, hullB, query, cache, enableSpeculative );+			return;+		}++			// This case is for testing+		case b3_manualEdgePairAxis:+		{+			b3SeparatingAxis query = b3QueryTriangleAndHullEdges( &triangle, hullB );+			if ( query.indexA != B3_NULL_INDEX )+			{+				b3Vec3 trianglePoint = trianglePoints[query.indexA];+				b3Vec3 triangleEdge = triangleEdges[query.indexA];+				b3CollideTriangleAndHullEdges( manifold, capacity, trianglePoint, triangleEdge, hullB, query, cache );+			}+			return;+		}++		default:+			B3_ASSERT( cache->type == b3_invalidAxis );+			break;+	}++	// Cache miss+	cache->hit = 0;++	// Find axis of minimum penetration+	b3SeparatingAxis faceQueryA = b3QueryTriangleFace( &triangle, hullB );+	if ( faceQueryA.separation > speculativeDistance )+	{+		// Separating axis found+		cache->separation = faceQueryA.separation;+		cache->type = b3_faceAxisA;+		cache->indexA = (uint8_t)faceQueryA.indexA;+		cache->indexB = (uint8_t)faceQueryA.indexB;+		return;+	}++	b3SeparatingAxis faceQueryB = b3QueryHullFace( &triangle, hullB );+	if ( faceQueryB.separation > speculativeDistance )+	{+		// Separating axis found+		cache->separation = faceQueryB.separation;+		cache->type = b3_faceAxisB;+		cache->indexA = (uint8_t)faceQueryB.indexA;+		cache->indexB = (uint8_t)faceQueryB.indexB;+		return;+	}++	b3SeparatingAxis edgeQuery = b3QueryTriangleAndHullEdges( &triangle, hullB );+	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 clipSeparation;++	// Don't admit a hull face significantly opposed to the triangle face.+	// Need a tolerance to avoid ghost collisions.+	bool pushingDown = b3Dot( faceQueryB.normal, trianglePlane.normal ) < -0.25f;+	if ( faceQueryB.separation >= faceQueryA.separation && pushingDown == false )+	{+		clipSeparation = b3CollideHullFace( manifold, capacity, &triangle, hullB, faceQueryB, cache, enableSpeculative );+	}+	else+	{+		clipSeparation = b3CollideTriangleFace( manifold, capacity, &triangle, hullB, faceQueryA, cache, enableSpeculative );+	}++	// Does an edge axis exist?+	if ( edgeQuery.indexA != B3_NULL_INDEX )+	{+		// When axes are aligned the edge separation can be garbage.+		// If a face axis has positive separation there may be no points.+		float maxFaceSeparation = b3MaxFloat( faceQueryA.separation, faceQueryB.separation );++		if ( ( manifold->pointCount == 0 && edgeQuery.separation > maxFaceSeparation ) ||+			 ( manifold->pointCount == 1 && edgeQuery.separation > clipSeparation + linearSlop ) )+		{+			B3_ASSERT( 0 <= edgeQuery.indexA && edgeQuery.indexA < 3 );+			b3Vec3 trianglePoint = trianglePoints[edgeQuery.indexA];+			b3Vec3 triangleEdge = triangleEdges[edgeQuery.indexA];+			manifold->pointCount = 0;+			b3CollideTriangleAndHullEdges( manifold, capacity, trianglePoint, triangleEdge, hullB, 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 )+	{+		b3Vec3 triangleB[] = { v1, v2, v3 };+		b3DistanceInput input = { 0 };+		input.proxyA = (b3ShapeProxy){+			.points = triangleB,+			.count = 3,+			.radius = 0.0f,+		};+		input.proxyB = (b3ShapeProxy){ .points = hullPoints, .count = hullB->vertexCount, .radius = 0.0f };+		input.transform = 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;+		}++		// No way to cache this scenario+		*cache = (b3SATCache){ 0 };+	}+}
+ upstream/src/types.c view
@@ -0,0 +1,154 @@+// 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.enableContactRecycling = 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, 0, NULL };+	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.enableSpeculativeContact = true;+	def.internalValue = B3_SECRET_COOKIE;+	return def;+}++static void b3EmptyDrawShape( void* userShape, b3WorldTransform transform, b3HexColor color, void* context )+{+	B3_UNUSED( userShape, transform, color, context );+}++static void b3EmptyDrawSegment( b3Pos p1, b3Pos p2, b3HexColor color, void* context )+{+	B3_UNUSED( p1, p2, color, context );+}++static void b3EmptyDrawTransform( b3WorldTransform transform, void* context )+{+	B3_UNUSED( transform, context );+}++static void b3EmptyDrawPoint( b3Pos p, float size, b3HexColor color, void* context )+{+	B3_UNUSED( p, size, color, context );+}++static void b3EmptyDrawSphere( b3Pos p, float radius, b3HexColor color, float alpha, void* context )+{+	B3_UNUSED( p, radius, color, alpha, context );+}++static void b3EmptyDrawCapsule( b3Pos p1, b3Pos p2, float radius, b3HexColor color, float alpha, void* context )+{+	B3_UNUSED( p1, p2, radius, color, alpha, context );+}++static void b3EmptyDrawBounds( b3AABB aabb, b3HexColor color, void* context )+{+	B3_UNUSED( aabb, color, context );+}++static void b3EmptyDrawBox( b3Vec3 extents, b3WorldTransform transform, b3HexColor color, void* context )+{+	B3_UNUSED( extents, transform, color, context );+}++static void b3EmptyDrawString( b3Pos 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.DrawSphereFcn = b3EmptyDrawSphere;+	draw.DrawCapsuleFcn = b3EmptyDrawCapsule;+	draw.DrawBoundsFcn = b3EmptyDrawBounds;+	draw.DrawBoxFcn = b3EmptyDrawBox;+	draw.DrawStringFcn = b3EmptyDrawString;++	// Not too small, not too big.+	float h = 100.0f * b3GetLengthUnitsPerMeter();+	draw.drawingBounds = (b3AABB){+		.lowerBound = { -h, -h, -h },+		.upperBound = { h, h, h },+	};++	draw.jointScale = 1.0f;+	draw.forceScale = 1.0f;++	return draw;+}
+ upstream/src/verstable.h view
@@ -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 <stdio.h>                                      | #include <stdio.h>                                       |+  |                                                         |                                                          |+  | // 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   <your chosen type name>+      #define KEY_TY <type>+      #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 <type>++        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 <function name>++        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 <function name>++        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 <floating point value>++        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 <function name>++        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 <function name>++        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 <type>++        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 <function name>++        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 <function name>++        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 <limits.h>+#include <stddef.h>+#include <stdint.h>+#include <stdlib.h>+#include <stdbool.h>+#include <string.h>++// 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 <intrin.h>+#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
+ upstream/src/weld_joint.c view
@@ -0,0 +1,314 @@+// 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"+#include "recording.h"++// needed for dll export+#include "box3d/box3d.h"++void b3WeldJoint_SetLinearHertz( b3JointId jointId, float hertz )+{+	B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, WeldJointSetLinearHertz, jointId, hertz );+	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 );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, WeldJointSetLinearDampingRatio, jointId, dampingRatio );+	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 );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, WeldJointSetAngularHertz, jointId, hertz );+	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 );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, WeldJointSetAngularDampingRatio, jointId, dampingRatio );+	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 = b3SubPos( 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, b3WorldTransform transformA, b3WorldTransform transformB, float scale )+{+	b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA );+	b3WorldTransform frameB = b3MulWorldTransforms( 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 );+}
+ upstream/src/wheel_joint.c view
@@ -0,0 +1,1105 @@+// 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"+#include "recording.h"++// needed for dll export+#include "box3d/box3d.h"++void b3WheelJoint_EnableSuspension( b3JointId jointId, bool enableSpring )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, WheelJointEnableSuspension, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, WheelJointSetSuspensionHertz, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, WheelJointSetSuspensionDampingRatio, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, WheelJointEnableSuspensionLimit, jointId, 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 );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, WheelJointSetSuspensionLimits, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, WheelJointEnableSpinMotor, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, WheelJointSetSpinMotorSpeed, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, WheelJointSetMaxSpinTorque, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, WheelJointEnableSteering, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, WheelJointSetSteeringHertz, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, WheelJointSetSteeringDampingRatio, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, WheelJointSetMaxSteeringTorque, jointId, 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 )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, WheelJointEnableSteeringLimit, jointId, 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 );+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, WheelJointSetSteeringLimits, jointId, lowerRadians, upperRadians );+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint );+	joint->wheelJoint.lowerSteeringLimit = lowerRadians;+	joint->wheelJoint.upperSteeringLimit = upperRadians;+}++void b3WheelJoint_SetTargetSteeringAngle( b3JointId jointId, float radians )+{+	b3World* world = b3GetWorld( jointId.world0 );+	B3_REC( world, WheelJointSetTargetSteeringAngle, jointId, 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 )+{+	b3WorldTransform 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 = b3SubPos( 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, b3WorldTransform transformA, b3WorldTransform transformB, float scale )+{+	B3_ASSERT( base->type == b3_wheelJoint );++	b3WheelJoint* joint = &base->wheelJoint;++	b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA );+	b3WorldTransform frameB = b3MulWorldTransforms( 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 )+	{+		b3Pos lower = b3OffsetPos( frameA.p, b3MulSV( joint->lowerSuspensionLimit, matrixA.cx ) );+		b3Pos upper = b3OffsetPos( frameA.p, b3MulSV( joint->upperSuspensionLimit, matrixA.cx ) );+		b3Vec3 perp = matrixA.cy;+		draw->DrawSegmentFcn( lower, upper, b3_colorGray, draw->context );+		draw->DrawSegmentFcn( b3OffsetPos( lower, b3MulSV( -0.1f * scale, perp ) ), b3OffsetPos( lower, b3MulSV( 0.1f * scale, perp ) ),+							  b3_colorGreen, draw->context );+		draw->DrawSegmentFcn( b3OffsetPos( upper, b3MulSV( -0.1f * scale, perp ) ), b3OffsetPos( upper, b3MulSV( 0.1f * scale, perp ) ),+							  b3_colorRed, draw->context );+	}+	else+	{+		draw->DrawSegmentFcn( b3OffsetPos( frameA.p, b3MulSV( -1.0f * scale, matrixA.cx ) ),+							  b3OffsetPos( frameA.p, b3MulSV( 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 );++		b3WorldTransform 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 );+		b3Pos vertex1 = b3TransformWorldPoint( 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 );+			b3Pos vertex2 = b3TransformWorldPoint( 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( b3OffsetPos( frameB.p, b3MulSV( -0.5f * scale, matrixB.cz ) ),+						  b3OffsetPos( frameB.p, b3MulSV( 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 );+}
+ upstream/src/world_snapshot.c view
@@ -0,0 +1,1335 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS )+#define _CRT_SECURE_NO_WARNINGS+#endif++#include "world_snapshot.h"++#include "bitset.h"+#include "body.h"+#include "broad_phase.h"+#include "compound.h"+#include "constraint_graph.h"+#include "contact.h"+#include "container.h"+#include "core.h"+#include "id_pool.h"+#include "island.h"+#include "joint.h"+#include "physics_world.h"+#include "recording.h"+#include "sensor.h"+#include "shape.h"+#include "solver_set.h"+#include "table.h"++#include "box3d/box3d.h"+#include "box3d/collision.h"+#include "box3d/types.h"++#include <string.h>++// Snapshot image magic 'BNS3' and version+#define B3_SNAP_MAGIC 0x33534E42u+#define B3_SNAP_VERSION 2u++#define B3_SNAP_FLAG_VALIDATION 0x1u+#define B3_SNAP_FLAG_DOUBLE_PRECISION 0x2u++// Layout hash over all POD-copied structs + key constants.+// Changing a struct size updates this, catching ABI drift early.+static uint32_t b3ComputeLayoutHash( void )+{+	uint32_t h = 2166136261u;+#define MIX( x )                                                                                                                 \+	h ^= (uint32_t)( x );                                                                                                        \+	h *= 16777619u;+	MIX( sizeof( b3Body ) )+	MIX( sizeof( b3BodySim ) )+	MIX( sizeof( b3BodyState ) )+	MIX( sizeof( b3Shape ) )+	MIX( sizeof( b3Contact ) )+	MIX( sizeof( b3Manifold ) )+	MIX( sizeof( b3Joint ) )+	MIX( sizeof( b3JointSim ) )+	MIX( sizeof( b3Island ) )+	MIX( sizeof( b3IslandSim ) )+	MIX( sizeof( b3ContactLink ) )+	MIX( sizeof( b3JointLink ) )+	MIX( sizeof( b3Sensor ) )+	MIX( sizeof( b3Visitor ) )+	MIX( sizeof( b3SolverSet ) )+	MIX( sizeof( b3GraphColor ) )+	MIX( sizeof( b3DynamicTree ) )+	MIX( sizeof( b3TreeNode ) )+	MIX( sizeof( b3SetItem ) )+	MIX( sizeof( b3IdPool ) )+	MIX( sizeof( b3SurfaceMaterial ) )+	MIX( sizeof( b3ContactSpec ) )+	MIX( sizeof( b3TriangleCache ) )+	MIX( B3_GRAPH_COLOR_COUNT )+	MIX( b3_bodyTypeCount )+	MIX( sizeof( void* ) )+#undef MIX+	return h;+}++typedef struct b3SnapHeader+{+	uint32_t magic;+	uint32_t version;+	uint32_t layoutHash;+	uint32_t flags;+} b3SnapHeader;++// Bounds-checked read cursor+typedef struct b3SnapReader+{+	const uint8_t* data;+	int cursor;+	int size;+	bool ok;+} b3SnapReader;++static void b3SnapRCheck( b3SnapReader* r, int need )+{+	if ( need < 0 || (int64_t)r->cursor + (int64_t)need > (int64_t)r->size )+	{+		r->ok = false;+	}+}++static void b3SnapR_Bytes( b3SnapReader* r, void* dst, int n )+{+	b3SnapRCheck( r, n );+	if ( !r->ok )+	{+		return;+	}+	memcpy( dst, r->data + r->cursor, n );+	r->cursor += n;+}++static int b3SnapR_I32( b3SnapReader* r )+{+	int32_t v = 0;+	b3SnapR_Bytes( r, &v, 4 );+	return (int)v;+}++static uint32_t b3SnapR_U32( b3SnapReader* r )+{+	uint32_t v = 0;+	b3SnapR_Bytes( r, &v, 4 );+	return v;+}++static void b3SnapW_I32( b3RecBuffer* buf, int v )+{+	int32_t w = (int32_t)v;+	b3RecBufAppend( buf, &w, 4 );+}++static void b3SnapW_U32( b3RecBuffer* buf, uint32_t v )+{+	b3RecBufAppend( buf, &v, 4 );+}++static void b3SnapW_Bytes( b3RecBuffer* buf, const void* src, int n )+{+	b3RecBufAppend( buf, src, n );+}++// Bounds check before allocating from image+static bool b3SnapCheckCount( const b3SnapReader* r, int count, int memSize, int minStreamBytes )+{+	if ( count < 0 || memSize < 0 || minStreamBytes < 0 )+	{+		return false;+	}+	if ( memSize > 0 && count > 0x7FFFFFFF / memSize )+	{+		return false;+	}+	int64_t remaining = (int64_t)r->size - (int64_t)r->cursor;+	return (int64_t)count * (int64_t)minStreamBytes <= remaining;+}++// POD array: count + raw bytes+#define b3SerPodArray( buf, arr )                                                                                                \+	do                                                                                                                           \+	{                                                                                                                            \+		b3SnapW_I32( buf, ( arr ).count );                                                                                       \+		if ( ( arr ).count > 0 )                                                                                                 \+		{                                                                                                                        \+			b3SnapW_Bytes( buf, ( arr ).data, ( arr ).count * (int)sizeof( *( arr ).data ) );                                    \+		}                                                                                                                        \+	}                                                                                                                            \+	while ( 0 )++#define b3DesPodArray( r, arr )                                                                                                  \+	do                                                                                                                           \+	{                                                                                                                            \+		int cnt = b3SnapR_I32( r );                                                                                              \+		int elemSize = (int)sizeof( *( arr ).data );                                                                             \+		if ( ( r )->ok && b3SnapCheckCount( r, cnt, elemSize, elemSize ) == false )                                              \+		{                                                                                                                        \+			( r )->ok = false;                                                                                                   \+		}                                                                                                                        \+		if ( ( r )->ok && cnt > 0 )                                                                                              \+		{                                                                                                                        \+			b3Array_Resize( arr, cnt );                                                                                          \+			b3SnapR_Bytes( r, ( arr ).data, cnt * elemSize );                                                                    \+		}                                                                                                                        \+		else if ( ( r )->ok )                                                                                                    \+		{                                                                                                                        \+			( arr ).count = 0;                                                                                                   \+		}                                                                                                                        \+	}                                                                                                                            \+	while ( 0 )++// Id pool: nextIndex + freeArray+static void b3SerIdPool( b3RecBuffer* buf, const b3IdPool* pool )+{+	b3SnapW_I32( buf, pool->nextIndex );+	b3SerPodArray( buf, pool->freeArray );+}++static void b3DesIdPool( b3SnapReader* r, b3IdPool* pool )+{+	pool->nextIndex = b3SnapR_I32( r );+	b3DesPodArray( r, pool->freeArray );+}++// BitSet: blockCount + raw words+static void b3SerBitSet( b3RecBuffer* buf, const b3BitSet* bs )+{+	b3SnapW_U32( buf, bs->blockCount );+	if ( bs->blockCount > 0 )+	{+		b3SnapW_Bytes( buf, bs->bits, (int)( bs->blockCount * sizeof( uint64_t ) ) );+	}+}++static void b3DesBitSet( b3SnapReader* r, b3BitSet* bs )+{+	uint32_t blockCount = b3SnapR_U32( r );+	if ( r->ok && b3SnapCheckCount( r, (int)blockCount, (int)sizeof( uint64_t ), (int)sizeof( uint64_t ) ) == false )+	{+		r->ok = false;+	}+	b3DestroyBitSet( bs );+	if ( !r->ok )+	{+		return;+	}+	uint32_t blockCapacity = blockCount > 0 ? blockCount : 1;+	bs->bits = (uint64_t*)b3Alloc( blockCapacity * sizeof( uint64_t ) );+	memset( bs->bits, 0, blockCapacity * sizeof( uint64_t ) );+	bs->blockCapacity = blockCapacity;+	bs->blockCount = blockCount;+	if ( blockCount > 0 )+	{+		b3SnapR_Bytes( r, bs->bits, (int)( blockCount * sizeof( uint64_t ) ) );+	}+}++// HashSet: capacity + count + raw items (probe order depends on layout)+static void b3SerHashSet( b3RecBuffer* buf, const b3HashSet* hs )+{+	b3SnapW_U32( buf, hs->capacity );+	b3SnapW_U32( buf, hs->count );+	if ( hs->capacity > 0 )+	{+		b3SnapW_Bytes( buf, hs->items, (int)( hs->capacity * sizeof( b3SetItem ) ) );+	}+}++static void b3DesHashSet( b3SnapReader* r, b3HashSet* hs )+{+	uint32_t cap = b3SnapR_U32( r );+	uint32_t cnt = b3SnapR_U32( r );+	bool valid = b3SnapCheckCount( r, (int)cap, (int)sizeof( b3SetItem ), (int)sizeof( b3SetItem ) ) &&+				 ( cap & ( cap - 1 ) ) == 0 && cnt <= cap;+	if ( r->ok && valid == false && ( cap != 0 || cnt != 0 ) )+	{+		r->ok = false;+	}+	b3DestroySet( hs );+	if ( !r->ok )+	{+		return;+	}+	if ( cap > 0 )+	{+		hs->items = (b3SetItem*)b3Alloc( cap * sizeof( b3SetItem ) );+		hs->capacity = cap;+		hs->count = cnt;+		b3SnapR_Bytes( r, hs->items, (int)( cap * sizeof( b3SetItem ) ) );+	}+	else+	{+		hs->items = NULL;+		hs->capacity = 0;+		hs->count = 0;+	}+}++// DynamicTree: version, scalars, full nodeCapacity nodes (rebuild scratch excluded)+static void b3SerTree( b3RecBuffer* buf, const b3DynamicTree* tree )+{+	b3SnapW_Bytes( buf, &tree->version, sizeof( uint64_t ) );+	b3SnapW_I32( buf, tree->root );+	b3SnapW_I32( buf, tree->nodeCount );+	b3SnapW_I32( buf, tree->nodeCapacity );+	b3SnapW_I32( buf, tree->freeList );+	b3SnapW_I32( buf, tree->proxyCount );+	if ( tree->nodeCapacity > 0 )+	{+		b3SnapW_Bytes( buf, tree->nodes, tree->nodeCapacity * (int)sizeof( b3TreeNode ) );+	}+}++static void b3DesTree( b3SnapReader* r, b3DynamicTree* tree )+{+	uint64_t version;+	b3SnapR_Bytes( r, &version, sizeof( uint64_t ) );+	int root = b3SnapR_I32( r );+	int nodeCount = b3SnapR_I32( r );+	int nodeCapacity = b3SnapR_I32( r );+	int freeList = b3SnapR_I32( r );+	int proxyCount = b3SnapR_I32( r );++	if ( r->ok && b3SnapCheckCount( r, nodeCapacity, (int)sizeof( b3TreeNode ), (int)sizeof( b3TreeNode ) ) == false )+	{+		r->ok = false;+	}++	// Free existing allocation including any rebuild scratch+	b3Free( tree->nodes, tree->nodeCapacity * (int)sizeof( b3TreeNode ) );+	b3Free( tree->leafIndices, tree->rebuildCapacity * (int)sizeof( int ) );+	b3Free( tree->leafBoxes, tree->rebuildCapacity * (int)sizeof( b3AABB ) );+	b3Free( tree->leafCenters, tree->rebuildCapacity * (int)sizeof( b3Vec3 ) );+	b3Free( tree->binIndices, tree->rebuildCapacity * (int)sizeof( int ) );+	tree->nodes = NULL;+	tree->leafIndices = NULL;+	tree->leafBoxes = NULL;+	tree->leafCenters = NULL;+	tree->binIndices = NULL;+	tree->nodeCapacity = 0;+	tree->rebuildCapacity = 0;++	if ( !r->ok )+	{+		return;+	}++	tree->version = version;+	tree->root = root;+	tree->nodeCount = nodeCount;+	tree->nodeCapacity = nodeCapacity;+	tree->freeList = freeList;+	tree->proxyCount = proxyCount;++	if ( nodeCapacity > 0 )+	{+		tree->nodes = (b3TreeNode*)b3Alloc( nodeCapacity * (int)sizeof( b3TreeNode ) );+		b3SnapR_Bytes( r, tree->nodes, nodeCapacity * (int)sizeof( b3TreeNode ) );+	}+}++// Solver set: setIndex + 4 arrays (note: contactIndices is int array, not contactSims)+static void b3SerSolverSet( b3RecBuffer* buf, const b3SolverSet* set )+{+	b3SnapW_I32( buf, set->setIndex );+	b3SerPodArray( buf, set->bodySims );+	b3SerPodArray( buf, set->bodyStates );+	b3SerPodArray( buf, set->jointSims );+	b3SerPodArray( buf, set->contactIndices );+	b3SerPodArray( buf, set->islandSims );+}++static void b3DesSolverSet( b3SnapReader* r, b3SolverSet* set )+{+	set->setIndex = b3SnapR_I32( r );+	b3DesPodArray( r, set->bodySims );+	b3DesPodArray( r, set->bodyStates );+	b3DesPodArray( r, set->jointSims );+	b3DesPodArray( r, set->contactIndices );+	b3DesPodArray( r, set->islandSims );+}++static void b3SerNames( b3RecBuffer* buf, const b3NameCache* cache )+{+	b3SnapW_I32( buf, cache->entries.count );+	int count = cache->entries.count;+	for ( int i = 0; i < count; ++i )+	{+		const b3NameEntry* entry = cache->entries.data + i;+		b3SnapW_U32( buf, entry->hash );+		b3SnapW_I32( buf, entry->length );+		b3RecBufAppend( buf, entry->name, entry->length );+	}+}++static void b3DesNames( b3SnapReader* r, b3NameCache* cache )+{+	int count = b3SnapR_I32( r );++	if ( r->ok && b3SnapCheckCount( r, count, (int)sizeof( b3NameEntry ), 8 ) == false )+	{+		r->ok = false;+	}++	if ( r->ok == false )+	{+		return;+	}++	b3Array_Reserve( cache->entries, count );+	for ( int i = 0; i < count; ++i )+	{+		uint32_t hash = b3SnapR_U32( r );+		int length = b3SnapR_I32( r );+		if ( r->ok == false || length < 0 || length > r->size - r->cursor )+		{+			r->ok = false;+			return;+		}+		char* name = b3Alloc( length + 1 );+		b3SnapR_Bytes( r, name, length );+		name[length] = 0;+		b3LoadName( cache, hash, name, length );+	}+}++// Graph color: bodySet (non-overflow only) + jointSims + convexContacts + contacts+static void b3SerGraphColor( b3RecBuffer* buf, const b3GraphColor* color, bool isOverflow )+{+	if ( !isOverflow )+	{+		b3SerBitSet( buf, &color->bodySet );+	}+	b3SerPodArray( buf, color->jointSims );+	b3SerPodArray( buf, color->convexContacts );+	b3SerPodArray( buf, color->contacts );+	// wideConstraints / manifoldConstraints / contactConstraints are transient, not serialized+}++static void b3DesGraphColor( b3SnapReader* r, b3GraphColor* color, bool isOverflow )+{+	if ( !isOverflow )+	{+		b3DesBitSet( r, &color->bodySet );+	}+	b3DesPodArray( r, color->jointSims );+	b3DesPodArray( r, color->convexContacts );+	b3DesPodArray( r, color->contacts );+	// Transient pointers left at NULL/0 from shell+}++// World simulation scalars (never host/callback/worker state)+static void b3SerWorldConfig( b3RecBuffer* buf, const b3World* world )+{+	b3SnapW_Bytes( buf, &world->gravity, sizeof( b3Vec3 ) );+	b3SnapW_Bytes( buf, &world->hitEventThreshold, sizeof( float ) );+	b3SnapW_Bytes( buf, &world->restitutionThreshold, sizeof( float ) );+	b3SnapW_Bytes( buf, &world->maxLinearSpeed, sizeof( float ) );+	b3SnapW_Bytes( buf, &world->contactSpeed, sizeof( float ) );+	b3SnapW_Bytes( buf, &world->contactHertz, sizeof( float ) );+	b3SnapW_Bytes( buf, &world->contactDampingRatio, sizeof( float ) );+	b3SnapW_Bytes( buf, &world->contactRecycleDistance, sizeof( float ) );+	b3SnapW_Bytes( buf, &world->stepIndex, sizeof( uint64_t ) );+	b3SnapW_I32( buf, world->splitIslandId );+	b3SnapW_Bytes( buf, &world->inv_h, sizeof( float ) );+	b3SnapW_Bytes( buf, &world->inv_dt, sizeof( float ) );+	b3SnapW_I32( buf, world->endEventArrayIndex );+	b3SnapW_Bytes( buf, &world->maxCapacity, sizeof( b3Capacity ) );+	uint8_t flags = 0;+	flags |= world->enableSleep ? 0x01u : 0u;+	flags |= world->enableWarmStarting ? 0x02u : 0u;+	flags |= world->enableContinuous ? 0x04u : 0u;+	flags |= world->enableSpeculative ? 0x08u : 0u;+	b3RecBufAppend( buf, &flags, 1 );+}++static void b3DesWorldConfig( b3SnapReader* r, b3World* world )+{+	b3SnapR_Bytes( r, &world->gravity, sizeof( b3Vec3 ) );+	b3SnapR_Bytes( r, &world->hitEventThreshold, sizeof( float ) );+	b3SnapR_Bytes( r, &world->restitutionThreshold, sizeof( float ) );+	b3SnapR_Bytes( r, &world->maxLinearSpeed, sizeof( float ) );+	b3SnapR_Bytes( r, &world->contactSpeed, sizeof( float ) );+	b3SnapR_Bytes( r, &world->contactHertz, sizeof( float ) );+	b3SnapR_Bytes( r, &world->contactDampingRatio, sizeof( float ) );+	b3SnapR_Bytes( r, &world->contactRecycleDistance, sizeof( float ) );+	b3SnapR_Bytes( r, &world->stepIndex, sizeof( uint64_t ) );+	world->splitIslandId = b3SnapR_I32( r );+	b3SnapR_Bytes( r, &world->inv_h, sizeof( float ) );+	b3SnapR_Bytes( r, &world->inv_dt, sizeof( float ) );+	world->endEventArrayIndex = b3SnapR_I32( r );+	b3SnapR_Bytes( r, &world->maxCapacity, sizeof( b3Capacity ) );+	uint8_t flags = 0;+	b3SnapR_Bytes( r, &flags, 1 );+	world->enableSleep = ( flags & 0x01u ) != 0;+	world->enableWarmStarting = ( flags & 0x02u ) != 0;+	world->enableContinuous = ( flags & 0x04u ) != 0;+	world->enableSpeculative = ( flags & 0x08u ) != 0;+}++// Shapes carry pointer fields: materials, userData, userShape, and the geometry union.+// Serialize the POD scalars with pointers nulled, then the owned materials array, then geometry.+// A single material lives inline in the struct image.+// Hull/mesh/heightField/compound are interned into the recording registry; sphere/capsule inline.+static void b3SerShapes( b3RecBuffer* buf, b3World* world, b3Recording* rec )+{+	int count = world->shapes.count;+	b3SnapW_I32( buf, count );++	for ( int i = 0; i < count; ++i )+	{+		b3Shape shape = world->shapes.data[i];+		bool isLive = ( shape.id == i );++		// Null out pointer fields before writing the raw struct+		shape.materials = NULL;+		shape.userData = NULL;+		shape.userShape = NULL;+		// Zero the geometry union so free-slot images have deterministic bytes+		if ( !isLive )+		{+			memset( &shape.capsule, 0, sizeof( shape.capsule ) );+		}+		b3SnapW_Bytes( buf, &shape, sizeof( b3Shape ) );++		if ( !isLive )+		{+			// Free slot: no materials or geometry+			b3SnapW_I32( buf, 0 );	// materialCount+			b3SnapW_I32( buf, -1 ); // geometry kind sentinel+			continue;+		}++		// Owned material array. Only multi material meshes and compounds have one. A single material+		// already rode along inline in the struct image, so write a zero length for it.+		const b3Shape* src = world->shapes.data + i;+		if ( src->materials != NULL )+		{+			b3SnapW_I32( buf, src->materialCount );+			b3SnapW_Bytes( buf, src->materials, src->materialCount * (int)sizeof( b3SurfaceMaterial ) );+		}+		else+		{+			b3SnapW_I32( buf, 0 );+		}++		// Geometry+		switch ( src->type )+		{+			case b3_sphereShape:+				b3SnapW_I32( buf, (int)b3_sphereShape );+				b3SnapW_Bytes( buf, &src->sphere, sizeof( b3Sphere ) );+				break;+			case b3_capsuleShape:+				b3SnapW_I32( buf, (int)b3_capsuleShape );+				b3SnapW_Bytes( buf, &src->capsule, sizeof( b3Capsule ) );+				break;+			case b3_hullShape:+			{+				b3SnapW_I32( buf, (int)b3_hullShape );+				uint32_t gid = b3RecInternHull( rec, src->hull );+				b3SnapW_U32( buf, gid );+				break;+			}+			case b3_meshShape:+			{+				b3SnapW_I32( buf, (int)b3_meshShape );+				uint32_t gid = b3RecInternMesh( rec, src->mesh.data );+				b3SnapW_U32( buf, gid );+				b3SnapW_Bytes( buf, &src->mesh.scale, sizeof( b3Vec3 ) );+				break;+			}+			case b3_heightShape:+			{+				b3SnapW_I32( buf, (int)b3_heightShape );+				uint32_t gid = b3RecInternHeightField( rec, src->heightField );+				b3SnapW_U32( buf, gid );+				break;+			}+			case b3_compoundShape:+			{+				b3SnapW_I32( buf, (int)b3_compoundShape );+				uint32_t gid = b3RecInternCompound( rec, src->compound );+				b3SnapW_U32( buf, gid );+				break;+			}+			default:+				// A live shape must have a known geometry type. Fail loudly rather than emit a shape+				// with no geometry that would silently lose its collision on restore.+				B3_ASSERT( false );+				b3SnapW_I32( buf, -1 );+				break;+		}+	}+}++static void b3DesShapes( b3SnapReader* r, b3World* world, b3RecReader* rdr )+{+	int count = b3SnapR_I32( r );+	if ( r->ok && b3SnapCheckCount( r, count, (int)sizeof( b3Shape ), (int)sizeof( b3Shape ) ) == false )+	{+		r->ok = false;+	}+	if ( !r->ok )+	{+		return;+	}++	// Save renderer handles before the array is wiped. A keyframe restore is a deterministic replay+	// state, so a live shape that still occupies the same slot with the same generation is the same+	// shape with the same geometry. Carrying its handle over avoids tearing down and rebuilding every+	// GPU mesh on each seek, which the host (a 3D renderer) would otherwise pay for. Handles that are+	// not reclaimed below belong to shapes that are gone or were replaced, and get released so the host+	// pool does not leak across seeks. Box2D has no such handles, so its restore skips all of this.+	int oldShapeCount = world->shapes.count;+	void** savedUserShape = NULL;+	uint16_t* savedGeneration = NULL;+	if ( oldShapeCount > 0 )+	{+		savedUserShape = (void**)b3Alloc( (size_t)oldShapeCount * sizeof( void* ) );+		savedGeneration = (uint16_t*)b3Alloc( (size_t)oldShapeCount * sizeof( uint16_t ) );+		for ( int i = 0; i < oldShapeCount; ++i )+		{+			b3Shape* old = world->shapes.data + i;+			bool oldLive = ( old->id == i );+			savedUserShape[i] = oldLive ? old->userShape : NULL;+			savedGeneration[i] = old->generation;+		}+	}++	b3Array_Resize( world->shapes, count );+	memset( world->shapes.data, 0, (size_t)count * sizeof( b3Shape ) );++	for ( int i = 0; i < count && r->ok; ++i )+	{+		b3Shape* dst = world->shapes.data + i;+		b3SnapR_Bytes( r, dst, sizeof( b3Shape ) );+		// Pointer fields were written as NULL; set them cleanly+		dst->materials = NULL;+		dst->userData = NULL;+		dst->userShape = NULL;+		memset( &dst->capsule, 0, sizeof( dst->capsule ) );++		bool isLive = ( dst->id == i );++		// Carry the renderer handle over when the same shape still occupies this slot. Consumed+		// handles are nulled so the teardown sweep only releases the ones that vanished.+		if ( isLive && i < oldShapeCount && savedUserShape != NULL && savedUserShape[i] != NULL &&+			 savedGeneration[i] == dst->generation )+		{+			dst->userShape = savedUserShape[i];+			savedUserShape[i] = NULL;+		}++		// Serializer writes: matCount, matData, geoKind, geoData+		int matCount = b3SnapR_I32( r );++		if ( !r->ok )+		{+			break;+		}++		if ( !isLive )+		{+			// Free slot: matCount=0, geoKind=-1+			(void)matCount;+			b3SnapR_I32( r ); // consume the geoKind sentinel+			continue;+		}++		// Owned material array (written before geoKind in serializer). A zero length means the single+		// material is already inline in the restored struct image, so leave materialCount as restored.+		if ( matCount > 0 )+		{+			if ( b3SnapCheckCount( r, matCount, (int)sizeof( b3SurfaceMaterial ), (int)sizeof( b3SurfaceMaterial ) ) == false )+			{+				r->ok = false;+				break;+			}+			dst->materialCount = matCount;+			dst->materials = (b3SurfaceMaterial*)b3Alloc( (size_t)matCount * sizeof( b3SurfaceMaterial ) );+			b3SnapR_Bytes( r, dst->materials, matCount * (int)sizeof( b3SurfaceMaterial ) );+		}+		else+		{+			dst->materials = NULL;+		}++		int geoKind = b3SnapR_I32( r );++		// Geometry+		switch ( (b3ShapeType)geoKind )+		{+			case b3_sphereShape:+				b3SnapR_Bytes( r, &dst->sphere, sizeof( b3Sphere ) );+				break;+			case b3_capsuleShape:+				b3SnapR_Bytes( r, &dst->capsule, sizeof( b3Capsule ) );+				break;+			case b3_hullShape:+			{+				uint32_t gid = b3SnapR_U32( r );+				if ( !r->ok )+				{+					break;+				}+				if ( rdr == NULL || gid >= (uint32_t)rdr->slotCount )+				{+					r->ok = false;+					break;+				}+				// Hull is cloned into the world DB; pass raw bytes directly+				b3RegistrySlot* slot = rdr->slots + gid;+				dst->hull = b3AddHullToDatabase( world, (const b3HullData*)slot->bytes );+				break;+			}+			case b3_meshShape:+			{+				uint32_t gid = b3SnapR_U32( r );+				b3Vec3 scale;+				b3SnapR_Bytes( r, &scale, sizeof( b3Vec3 ) );+				if ( !r->ok )+				{+					break;+				}+				if ( rdr == NULL || gid >= (uint32_t)rdr->slotCount )+				{+					r->ok = false;+					break;+				}+				b3RegistrySlot* slot = rdr->slots + gid;+				// Mesh is a self-contained blob used by reference; point straight at the pristine bytes.+				dst->mesh.data = (const b3MeshData*)slot->bytes;+				dst->mesh.scale = scale;+				break;+			}+			case b3_heightShape:+			{+				uint32_t gid = b3SnapR_U32( r );+				if ( !r->ok )+				{+					break;+				}+				if ( rdr == NULL || gid >= (uint32_t)rdr->slotCount )+				{+					r->ok = false;+					break;+				}+				b3RegistrySlot* slot = rdr->slots + gid;+				// Self-contained blob used by reference; point straight at the pristine bytes.+				dst->heightField = (const b3HeightFieldData*)slot->bytes;+				break;+			}+			case b3_compoundShape:+			{+				uint32_t gid = b3SnapR_U32( r );+				if ( !r->ok )+				{+					break;+				}+				if ( rdr == NULL || gid >= (uint32_t)rdr->slotCount )+				{+					r->ok = false;+					break;+				}+				b3RegistrySlot* slot = rdr->slots + gid;+				if ( slot->live == NULL )+				{+					slot->live = b3Alloc( (size_t)slot->byteCount );+					memcpy( slot->live, slot->bytes, (size_t)slot->byteCount );+					b3ConvertBytesToCompound( (uint8_t*)slot->live, slot->byteCount );+				}+				dst->compound = (const b3CompoundData*)slot->live;+				break;+			}+			default:+				// Unknown geometry kind means a corrupt or unsupported snapshot. Fail the load instead+				// of leaving a shape with no geometry.+				r->ok = false;+				break;+		}+	}++	// Release handles for shapes that are gone or were replaced this restore, so the host pool and any+	// GPU resources they pinned do not leak across seeks.+	if ( savedUserShape != NULL )+	{+		for ( int i = 0; i < oldShapeCount; ++i )+		{+			if ( savedUserShape[i] != NULL && world->destroyDebugShape != NULL )+			{+				world->destroyDebugShape( savedUserShape[i], world->userDebugShapeContext );+			}+		}+		b3Free( savedUserShape, (size_t)oldShapeCount * sizeof( void* ) );+		b3Free( savedGeneration, (size_t)oldShapeCount * sizeof( uint16_t ) );+	}+}++// Contact serialization. b3Contact is not fully POD:+// - manifolds: heap array of b3Manifold, allocated via b3AllocateManifolds+// - meshContact.triangleCache: heap b3Array, active when b3_simMeshContact flag is set+// Serialize: raw struct (with nulled manifolds/triangleCache), then manifolds, then triangleCache.+static void b3SerContacts( b3RecBuffer* buf, b3World* world )+{+	int count = world->contacts.count;+	b3SnapW_I32( buf, count );++	for ( int i = 0; i < count; ++i )+	{+		const b3Contact* c = world->contacts.data + i;+		bool isLive = ( c->contactId == i );++		// Write raw struct with pointer fields zeroed+		b3Contact copy = *c;+		copy.manifolds = NULL;+		copy.bodySimIndexA = B3_NULL_INDEX;+		copy.bodySimIndexB = B3_NULL_INDEX;+		if ( copy.flags & b3_simMeshContact )+		{+			copy.meshContact.triangleCache.data = NULL;+			copy.meshContact.triangleCache.count = 0;+			copy.meshContact.triangleCache.capacity = 0;+		}+		b3SnapW_Bytes( buf, &copy, sizeof( b3Contact ) );++		if ( !isLive )+		{+			// Free slot: no heap data+			b3SnapW_I32( buf, 0 ); // manifoldCount+			// No triangleCache+			continue;+		}++		// Manifolds+		b3SnapW_I32( buf, c->manifoldCount );+		if ( c->manifoldCount > 0 && c->manifolds != NULL )+		{+			b3SnapW_Bytes( buf, c->manifolds, c->manifoldCount * (int)sizeof( b3Manifold ) );+		}++		// Mesh triangleCache+		if ( c->flags & b3_simMeshContact )+		{+			b3SnapW_I32( buf, c->meshContact.triangleCache.count );+			if ( c->meshContact.triangleCache.count > 0 )+			{+				b3SnapW_Bytes( buf, c->meshContact.triangleCache.data,+							   c->meshContact.triangleCache.count * (int)sizeof( b3TriangleCache ) );+			}+		}+	}+}++static void b3DesContacts( b3SnapReader* r, b3World* world )+{+	int count = b3SnapR_I32( r );+	if ( r->ok && b3SnapCheckCount( r, count, (int)sizeof( b3Contact ), (int)sizeof( b3Contact ) ) == false )+	{+		r->ok = false;+	}+	if ( !r->ok )+	{+		return;+	}++	b3Array_Resize( world->contacts, count );+	memset( world->contacts.data, 0, (size_t)count * sizeof( b3Contact ) );++	for ( int i = 0; i < count && r->ok; ++i )+	{+		b3Contact* dst = world->contacts.data + i;+		b3SnapR_Bytes( r, dst, sizeof( b3Contact ) );+		dst->manifolds = NULL;+		dst->bodySimIndexA = B3_NULL_INDEX;+		dst->bodySimIndexB = B3_NULL_INDEX;+		if ( dst->flags & b3_simMeshContact )+		{+			dst->meshContact.triangleCache.data = NULL;+			dst->meshContact.triangleCache.count = 0;+			dst->meshContact.triangleCache.capacity = 0;+		}++		bool isLive = ( dst->contactId == i );++		int manifoldCount = b3SnapR_I32( r );++		if ( !r->ok )+		{+			break;+		}++		if ( isLive && manifoldCount > 0 )+		{+			if ( b3SnapCheckCount( r, manifoldCount, (int)sizeof( b3Manifold ), (int)sizeof( b3Manifold ) ) == false )+			{+				r->ok = false;+				break;+			}+			dst->manifolds = b3AllocateManifolds( world, manifoldCount );+			dst->manifoldCount = manifoldCount;+			b3SnapR_Bytes( r, dst->manifolds, manifoldCount * (int)sizeof( b3Manifold ) );+		}+		else+		{+			dst->manifolds = NULL;+			dst->manifoldCount = 0;+		}++		// Mesh triangleCache+		if ( isLive && ( dst->flags & b3_simMeshContact ) )+		{+			int cacheCount = b3SnapR_I32( r );+			if ( !r->ok )+			{+				break;+			}+			if ( cacheCount > 0 )+			{+				if ( b3SnapCheckCount( r, cacheCount, (int)sizeof( b3TriangleCache ), (int)sizeof( b3TriangleCache ) ) == false )+				{+					r->ok = false;+					break;+				}+				b3Array_Resize( dst->meshContact.triangleCache, cacheCount );+				b3SnapR_Bytes( r, dst->meshContact.triangleCache.data, cacheCount * (int)sizeof( b3TriangleCache ) );+			}+		}+	}+}++// Free per-object heap that b3DeserializeIntoShell will overwrite,+// so restoring over a populated world doesn't leak.+static void b3FreeLiveSimElements( b3World* world )+{+	// Shape heap: materials and hull DB references+	for ( int i = 0; i < world->shapes.count; ++i )+	{+		b3Shape* s = world->shapes.data + i;+		if ( s->id != i )+		{+			continue;+		}+		// A single material lives inline (materials == NULL). Multi material meshes and compounds own+		// the array, so free it exactly as b3DestroyShapeAllocations does.+		if ( s->materials != NULL )+		{+			b3Free( s->materials, (size_t)s->materialCount * sizeof( b3SurfaceMaterial ) );+			s->materials = NULL;+			s->materialCount = 0;+		}+		// Hull is ref-counted in the world DB; release before overwrite so re-adding is ref-neutral.+		if ( s->type == b3_hullShape && s->hull != NULL )+		{+			b3RemoveHullFromDatabase( world, s->hull );+			s->hull = NULL;+		}+		// name / userData / userShape are host-owned; do not free+	}++	// Contact heap: manifolds + mesh triangleCache+	for ( int i = 0; i < world->contacts.count; ++i )+	{+		b3Contact* c = world->contacts.data + i;+		if ( c->contactId == i )+		{+			if ( c->manifolds != NULL )+			{+				b3FreeManifolds( world, c->manifolds, c->manifoldCount );+				c->manifolds = NULL;+				c->manifoldCount = 0;+			}+			if ( c->flags & b3_simMeshContact )+			{+				b3Array_Destroy( c->meshContact.triangleCache );+			}+		}+	}++	// Sensor heap: inner arrays+	for ( int i = 0; i < world->sensors.count; ++i )+	{+		b3Sensor* sensor = world->sensors.data + i;+		b3Array_Destroy( sensor->hits );+		b3Array_Destroy( sensor->overlaps1 );+		b3Array_Destroy( sensor->overlaps2 );+	}++	// Island heap: inner arrays+	for ( int i = 0; i < world->islands.count; ++i )+	{+		b3Island* island = world->islands.data + i;+		b3Array_Destroy( island->bodies );+		b3Array_Destroy( island->contacts );+		b3Array_Destroy( island->joints );+	}+}++int b3SerializeWorld( b3World* world, b3RecBuffer* buf, b3Recording* rec )+{+	int startSize = buf->size;++	// Snapshot header+	b3SnapHeader hdr;+	hdr.magic = B3_SNAP_MAGIC;+	hdr.version = B3_SNAP_VERSION;+	hdr.layoutHash = b3ComputeLayoutHash();+	hdr.flags = B3_ENABLE_VALIDATION ? B3_SNAP_FLAG_VALIDATION : 0u;+#if defined( BOX3D_DOUBLE_PRECISION )+	hdr.flags |= B3_SNAP_FLAG_DOUBLE_PRECISION;+#endif+	b3SnapW_Bytes( buf, &hdr, (int)sizeof( hdr ) );++	// World scalars+	b3SerWorldConfig( buf, world );++	// 6 id pools (Box3D has no chainIdPool)+	b3SerIdPool( buf, &world->bodyIdPool );+	b3SerIdPool( buf, &world->shapeIdPool );+	b3SerIdPool( buf, &world->contactIdPool );+	b3SerIdPool( buf, &world->jointIdPool );+	b3SerIdPool( buf, &world->islandIdPool );+	b3SerIdPool( buf, &world->solverSetIdPool );++	// Solver sets+	int setCount = world->solverSets.count;+	b3SnapW_I32( buf, setCount );+	for ( int i = 0; i < setCount; ++i )+	{+		b3SerSolverSet( buf, world->solverSets.data + i );+	}++	// Sparse body array (userData is host wiring, zero it on the copy)+	{+		int bodyCount = world->bodies.count;+		b3SnapW_I32( buf, bodyCount );+		for ( int i = 0; i < bodyCount; ++i )+		{+			b3Body elem = world->bodies.data[i];+			elem.userData = NULL;+			b3SnapW_Bytes( buf, &elem, sizeof( b3Body ) );+		}+	}++	// Shape sparse array with geometry interning+	b3SerShapes( buf, world, rec );++	// Contact sparse array with manifold and mesh triangleCache+	b3SerContacts( buf, world );++	// Joint sparse array (userData scrubbed)+	{+		int jointCount = world->joints.count;+		b3SnapW_I32( buf, jointCount );+		for ( int i = 0; i < jointCount; ++i )+		{+			b3Joint elem = world->joints.data[i];+			elem.userData = NULL;+			b3SnapW_Bytes( buf, &elem, sizeof( b3Joint ) );+		}+	}++	// Sensors: shapeId + 3 inner arrays each+	{+		int sensorCount = world->sensors.count;+		b3SnapW_I32( buf, sensorCount );+		for ( int i = 0; i < sensorCount; ++i )+		{+			b3Sensor* s = world->sensors.data + i;+			b3SnapW_I32( buf, s->shapeId );+			b3SerPodArray( buf, s->hits );+			b3SerPodArray( buf, s->overlaps1 );+			b3SerPodArray( buf, s->overlaps2 );+		}+	}++	// Islands: 4 scalars + 3 inner arrays each+	{+		int islandCount = world->islands.count;+		b3SnapW_I32( buf, islandCount );+		for ( int i = 0; i < islandCount; ++i )+		{+			b3Island* island = world->islands.data + i;+			b3SnapW_I32( buf, island->setIndex );+			b3SnapW_I32( buf, island->localIndex );+			b3SnapW_I32( buf, island->islandId );+			b3SnapW_I32( buf, island->constraintRemoveCount );+			b3SerPodArray( buf, island->bodies );+			b3SerPodArray( buf, island->contacts );+			b3SerPodArray( buf, island->joints );+		}+	}++	// Broad phase+	b3BroadPhase* bp = &world->broadPhase;+	for ( int t = 0; t < b3_bodyTypeCount; ++t )+	{+		b3SerTree( buf, &bp->trees[t] );+	}+	for ( int t = 0; t < b3_bodyTypeCount; ++t )+	{+		b3SerBitSet( buf, &bp->movedProxies[t] );+	}+	b3SerPodArray( buf, bp->moveArray );+	b3SerHashSet( buf, &bp->pairSet );++	// Constraint graph+	b3ConstraintGraph* graph = &world->constraintGraph;+	for ( int c = 0; c < B3_GRAPH_COLOR_COUNT; ++c )+	{+		b3SerGraphColor( buf, &graph->colors[c], c == B3_OVERFLOW_INDEX );+	}++	b3SerNames( buf, &world->names );++	return buf->size - startSize;+}++bool b3DeserializeIntoShell( const uint8_t* data, int size, b3World* world, b3RecReader* rdr )+{+	if ( data == NULL || size < (int)sizeof( b3SnapHeader ) )+	{+		return false;+	}++	// Validate header+	b3SnapHeader hdr;+	memcpy( &hdr, data, sizeof( hdr ) );+	if ( hdr.magic != B3_SNAP_MAGIC || hdr.version != B3_SNAP_VERSION )+	{+		printf( "b3DeserializeIntoShell: bad magic/version\n" );+		return false;+	}+	bool imageDouble = ( hdr.flags & B3_SNAP_FLAG_DOUBLE_PRECISION ) != 0;+#if defined( BOX3D_DOUBLE_PRECISION )+	bool buildDouble = true;+#else+	bool buildDouble = false;+#endif+	if ( imageDouble != buildDouble )+	{+		printf( "b3DeserializeIntoShell: precision mismatch\n" );+		return false;+	}+	if ( hdr.layoutHash != b3ComputeLayoutHash() )+	{+		printf( "b3DeserializeIntoShell: layout hash mismatch\n" );+		return false;+	}++	b3SnapReader readerStorage;+	b3SnapReader* r = &readerStorage;+	r->data = data;+	r->cursor = (int)sizeof( b3SnapHeader );+	r->size = size;+	r->ok = true;++	// Free existing per-object heap before overwriting+	b3FreeLiveSimElements( world );++	// 1. World scalars+	b3DesWorldConfig( r, world );++	// 2. 6 id pools; destroy the pre-created sets' pool state first+	b3DesIdPool( r, &world->bodyIdPool );+	b3DesIdPool( r, &world->shapeIdPool );+	b3DesIdPool( r, &world->contactIdPool );+	b3DesIdPool( r, &world->jointIdPool );+	b3DesIdPool( r, &world->islandIdPool );+	b3DesIdPool( r, &world->solverSetIdPool );++	// 3. Solver sets: destroy inner arrays of existing sets first+	for ( int i = 0; i < world->solverSets.count; ++i )+	{+		b3SolverSet* set = world->solverSets.data + i;+		b3Array_Destroy( set->bodySims );+		b3Array_Destroy( set->bodyStates );+		b3Array_Destroy( set->jointSims );+		b3Array_Destroy( set->contactIndices );+		b3Array_Destroy( set->islandSims );+	}++	int setCount = b3SnapR_I32( r );+	if ( r->ok && b3SnapCheckCount( r, setCount, (int)sizeof( b3SolverSet ), 6 * (int)sizeof( int ) ) == false )+	{+		r->ok = false;+	}+	if ( r->ok )+	{+		b3Array_Resize( world->solverSets, setCount );+		memset( world->solverSets.data, 0, (size_t)setCount * sizeof( b3SolverSet ) );+		for ( int i = 0; i < setCount; ++i )+		{+			b3DesSolverSet( r, world->solverSets.data + i );+		}+	}++	if ( !r->ok )+	{+		return false;+	}++	// 4. Body sparse array+	{+		int bodyCount = b3SnapR_I32( r );+		if ( r->ok && b3SnapCheckCount( r, bodyCount, (int)sizeof( b3Body ), (int)sizeof( b3Body ) ) == false )+		{+			r->ok = false;+		}+		if ( r->ok )+		{+			b3Array_Resize( world->bodies, bodyCount );+			for ( int i = 0; i < bodyCount; ++i )+			{+				b3SnapR_Bytes( r, world->bodies.data + i, sizeof( b3Body ) );+				world->bodies.data[i].userData = NULL;+			}+		}+	}++	if ( !r->ok )+	{+		return false;+	}++	// 5. Shape sparse array+	b3DesShapes( r, world, rdr );++	if ( !r->ok )+	{+		return false;+	}++	// 6. Contact sparse array+	b3DesContacts( r, world );++	if ( !r->ok )+	{+		return false;+	}++	// 7. Joint sparse array+	{+		int jointCount = b3SnapR_I32( r );+		if ( r->ok && b3SnapCheckCount( r, jointCount, (int)sizeof( b3Joint ), (int)sizeof( b3Joint ) ) == false )+		{+			r->ok = false;+		}+		if ( r->ok )+		{+			b3Array_Resize( world->joints, jointCount );+			for ( int i = 0; i < jointCount; ++i )+			{+				b3SnapR_Bytes( r, world->joints.data + i, sizeof( b3Joint ) );+				world->joints.data[i].userData = NULL;+			}+		}+	}++	// 8. Sensors+	{+		b3Array_Destroy( world->sensors );+		b3Array_Create( world->sensors );++		int sensorCount = b3SnapR_I32( r );+		if ( r->ok && b3SnapCheckCount( r, sensorCount, (int)sizeof( b3Sensor ), 4 * (int)sizeof( int ) ) == false )+		{+			r->ok = false;+		}+		if ( r->ok )+		{+			b3Array_Resize( world->sensors, sensorCount );+			memset( world->sensors.data, 0, (size_t)sensorCount * sizeof( b3Sensor ) );+		}++		for ( int i = 0; i < sensorCount && r->ok; ++i )+		{+			b3Sensor* s = world->sensors.data + i;+			s->shapeId = b3SnapR_I32( r );+			b3Array_Create( s->hits );+			b3Array_Create( s->overlaps1 );+			b3Array_Create( s->overlaps2 );+			b3DesPodArray( r, s->hits );+			b3DesPodArray( r, s->overlaps1 );+			b3DesPodArray( r, s->overlaps2 );+		}+	}++	// 9. Islands+	{+		b3Array_Destroy( world->islands );+		b3Array_Create( world->islands );++		int islandCount = b3SnapR_I32( r );+		if ( r->ok && b3SnapCheckCount( r, islandCount, (int)sizeof( b3Island ), 7 * (int)sizeof( int ) ) == false )+		{+			r->ok = false;+		}+		if ( r->ok )+		{+			b3Array_Resize( world->islands, islandCount );+			memset( world->islands.data, 0, (size_t)islandCount * sizeof( b3Island ) );+		}++		for ( int i = 0; i < islandCount && r->ok; ++i )+		{+			b3Island* island = world->islands.data + i;+			island->setIndex = b3SnapR_I32( r );+			island->localIndex = b3SnapR_I32( r );+			island->islandId = b3SnapR_I32( r );+			island->constraintRemoveCount = b3SnapR_I32( r );+			b3Array_Create( island->bodies );+			b3Array_Create( island->contacts );+			b3Array_Create( island->joints );+			b3DesPodArray( r, island->bodies );+			b3DesPodArray( r, island->contacts );+			b3DesPodArray( r, island->joints );+		}+	}++	// 10. Broad phase+	{+		b3BroadPhase* bp = &world->broadPhase;++		for ( int t = 0; t < b3_bodyTypeCount; ++t )+		{+			b3DesTree( r, &bp->trees[t] );+		}+		for ( int t = 0; t < b3_bodyTypeCount; ++t )+		{+			b3DesBitSet( r, &bp->movedProxies[t] );+		}++		b3Array_Destroy( bp->moveArray );+		b3Array_Create( bp->moveArray );+		b3DesPodArray( r, bp->moveArray );++		b3DesHashSet( r, &bp->pairSet );+		// Transient moveResults/movePairs stay at shell's NULL/0+	}++	// 11. Constraint graph+	{+		b3ConstraintGraph* graph = &world->constraintGraph;+		for ( int c = 0; c < B3_GRAPH_COLOR_COUNT; ++c )+		{+			b3DesGraphColor( r, &graph->colors[c], c == B3_OVERFLOW_INDEX );+		}+	}++	b3DesNames( r, &world->names );++	return r->ok;+}
+ upstream/src/world_snapshot.h view
@@ -0,0 +1,21 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "recording.h"+#include "recording_replay.h"++#include <stdbool.h>+#include <stdint.h>++typedef struct b3World b3World;++// Serialize the live world into buf, interning shape geometry into rec->registry.+// On success buf holds a self-contained snapshot image. Returns the byte count.+int b3SerializeWorld( b3World* world, b3RecBuffer* buf, b3Recording* rec );++// Overwrite a freshly-created (shell) world with the simulation state held in the+// snapshot image [data, size). Geometry references are resolved via the shared+// registry slots in rdr. Returns false on a corrupt or incompatible image.+bool b3DeserializeIntoShell( const uint8_t* data, int size, b3World* world, b3RecReader* rdr );